Norimasa Okamoto
BTstack Bluetooth stack
Embed:
(wiki syntax)
Show/hide line numbers
rfcomm.c
00001 /* 00002 * Copyright (C) 2009-2012 by Matthias Ringwald 00003 * 00004 * Redistribution and use in source and binary forms, with or without 00005 * modification, are permitted provided that the following conditions 00006 * are met: 00007 * 00008 * 1. Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * 2. Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * 3. Neither the name of the copyright holders nor the names of 00014 * contributors may be used to endorse or promote products derived 00015 * from this software without specific prior written permission. 00016 * 4. Any redistribution, use, or modification is done solely for 00017 * personal benefit and not for any commercial purpose or for 00018 * monetary gain. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS 00021 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 00024 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 00027 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 00028 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00029 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 00030 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 00031 * SUCH DAMAGE. 00032 * 00033 * Please inquire about commercial licensing options at btstack@ringwald.ch 00034 * 00035 */ 00036 00037 /* 00038 * rfcomm.c 00039 */ 00040 00041 #include <stdio.h> 00042 #include <stdlib.h> 00043 #include <string.h> // memcpy 00044 #include <stdint.h> 00045 00046 #include <btstack/btstack.h> 00047 #include <btstack/hci_cmds.h> 00048 #include <btstack/utils.h> 00049 00050 #include <btstack/utils.h> 00051 #include "btstack_memory.h" 00052 #include "hci.h" 00053 #include "hci_dump.h" 00054 #include "debug.h" 00055 #include "rfcomm.h" 00056 00057 // workaround for missing PRIxPTR on mspgcc (16/20-bit MCU) 00058 #ifndef PRIxPTR 00059 #if defined(__MSP430X__) && defined(__MSP430X_LARGE__) 00060 #define PRIxPTR "lx" 00061 #else 00062 #define PRIxPTR "x" 00063 #endif 00064 #endif 00065 00066 00067 // Control field values bit no. 1 2 3 4 PF 6 7 8 00068 #define BT_RFCOMM_SABM 0x3F // 1 1 1 1 1 1 0 0 00069 #define BT_RFCOMM_UA 0x73 // 1 1 0 0 1 1 1 0 00070 #define BT_RFCOMM_DM 0x0F // 1 1 1 1 0 0 0 0 00071 #define BT_RFCOMM_DM_PF 0x1F // 1 1 1 1 1 0 0 0 00072 #define BT_RFCOMM_DISC 0x53 // 1 1 0 0 1 0 1 0 00073 #define BT_RFCOMM_UIH 0xEF // 1 1 1 1 0 1 1 1 00074 #define BT_RFCOMM_UIH_PF 0xFF // 1 1 1 1 0 1 1 1 00075 00076 // Multiplexer message types 00077 #define BT_RFCOMM_CLD_CMD 0xC3 00078 #define BT_RFCOMM_FCON_CMD 0xA3 00079 #define BT_RFCOMM_FCON_RSP 0xA1 00080 #define BT_RFCOMM_FCOFF_CMD 0x63 00081 #define BT_RFCOMM_FCOFF_RSP 0x61 00082 #define BT_RFCOMM_MSC_CMD 0xE3 00083 #define BT_RFCOMM_MSC_RSP 0xE1 00084 #define BT_RFCOMM_NSC_RSP 0x11 00085 #define BT_RFCOMM_PN_CMD 0x83 00086 #define BT_RFCOMM_PN_RSP 0x81 00087 #define BT_RFCOMM_RLS_CMD 0x53 00088 #define BT_RFCOMM_RLS_RSP 0x51 00089 #define BT_RFCOMM_RPN_CMD 0x93 00090 #define BT_RFCOMM_RPN_RSP 0x91 00091 #define BT_RFCOMM_TEST_CMD 0x23 00092 #define BT_RFCOMM_TEST_RSP 0x21 00093 00094 #define RFCOMM_MULIPLEXER_TIMEOUT_MS 60000 00095 00096 // FCS calc 00097 #define BT_RFCOMM_CODE_WORD 0xE0 // pol = x8+x2+x1+1 00098 #define BT_RFCOMM_CRC_CHECK_LEN 3 00099 #define BT_RFCOMM_UIHCRC_CHECK_LEN 2 00100 00101 #include "l2cap.h" 00102 00103 // used for debugging 00104 // #define RFCOMM_LOG_CREDITS 00105 00106 // global rfcomm data 00107 static uint16_t rfcomm_client_cid_generator; // used for client channel IDs 00108 00109 // linked lists for all 00110 static linked_list_t rfcomm_multiplexers = NULL; 00111 static linked_list_t rfcomm_channels = NULL; 00112 static linked_list_t rfcomm_services = NULL; 00113 00114 static void (*app_packet_handler)(void * connection, uint8_t packet_type, 00115 uint16_t channel, uint8_t *packet, uint16_t size); 00116 00117 static void rfcomm_run(void); 00118 static void rfcomm_hand_out_credits(void); 00119 static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event); 00120 static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event); 00121 static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel); 00122 static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event); 00123 00124 00125 // MARK: RFCOMM CLIENT EVENTS 00126 00127 // data: event (8), len(8), address(48), channel (8), rfcomm_cid (16) 00128 static void rfcomm_emit_connection_request(rfcomm_channel_t *channel) { 00129 uint8_t event[11]; 00130 event[0] = RFCOMM_EVENT_INCOMING_CONNECTION; 00131 event[1] = sizeof(event) - 2; 00132 bt_flip_addr(&event[2], channel->multiplexer->remote_addr); 00133 event[8] = channel->dlci >> 1; 00134 bt_store_16(event, 9, channel->rfcomm_cid); 00135 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00136 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 00137 } 00138 00139 // API Change: BTstack-0.3.50x uses 00140 // data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16) 00141 // next Cydia release will use SVN version of this 00142 // data: event(8), len(8), status (8), address (48), handle (16), server channel(8), rfcomm_cid(16), max frame size(16) 00143 static void rfcomm_emit_channel_opened(rfcomm_channel_t *channel, uint8_t status) { 00144 uint8_t event[16]; 00145 uint8_t pos = 0; 00146 event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE; 00147 event[pos++] = sizeof(event) - 2; 00148 event[pos++] = status; 00149 bt_flip_addr(&event[pos], channel->multiplexer->remote_addr); pos += 6; 00150 bt_store_16(event, pos, channel->multiplexer->con_handle); pos += 2; 00151 event[pos++] = channel->dlci >> 1; 00152 bt_store_16(event, pos, channel->rfcomm_cid); pos += 2; // channel ID 00153 bt_store_16(event, pos, channel->max_frame_size); pos += 2; // max frame size 00154 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00155 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos); 00156 } 00157 00158 static void rfcomm_emit_channel_open_failed_outgoing_memory(void * connection, bd_addr_t *addr, uint8_t server_channel){ 00159 uint8_t event[16]; 00160 uint8_t pos = 0; 00161 event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE; 00162 event[pos++] = sizeof(event) - 2; 00163 event[pos++] = BTSTACK_MEMORY_ALLOC_FAILED; 00164 bt_flip_addr(&event[pos], *addr); pos += 6; 00165 bt_store_16(event, pos, 0); pos += 2; 00166 event[pos++] = server_channel; 00167 bt_store_16(event, pos, 0); pos += 2; // channel ID 00168 bt_store_16(event, pos, 0); pos += 2; // max frame size 00169 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00170 (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos); 00171 } 00172 00173 // data: event(8), len(8), creidts incoming(8), new credits incoming(8), credits outgoing(8) 00174 static inline void rfcomm_emit_credit_status(rfcomm_channel_t * channel) { 00175 #ifdef RFCOMM_LOG_CREDITS 00176 uint8_t event[5]; 00177 event[0] = 0x88; 00178 event[1] = sizeof(event) - 2; 00179 event[2] = channel->credits_incoming; 00180 event[3] = channel->new_credits_incoming; 00181 event[4] = channel->credits_outgoing; 00182 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00183 #endif 00184 } 00185 00186 // data: event(8), len(8), rfcomm_cid(16) 00187 static void rfcomm_emit_channel_closed(rfcomm_channel_t * channel) { 00188 uint8_t event[4]; 00189 event[0] = RFCOMM_EVENT_CHANNEL_CLOSED; 00190 event[1] = sizeof(event) - 2; 00191 bt_store_16(event, 2, channel->rfcomm_cid); 00192 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00193 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 00194 } 00195 00196 static void rfcomm_emit_credits(rfcomm_channel_t * channel, uint8_t credits) { 00197 uint8_t event[5]; 00198 event[0] = RFCOMM_EVENT_CREDITS; 00199 event[1] = sizeof(event) - 2; 00200 bt_store_16(event, 2, channel->rfcomm_cid); 00201 event[4] = credits; 00202 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 00203 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 00204 } 00205 00206 static void rfcomm_emit_service_registered(void *connection, uint8_t status, uint8_t channel){ 00207 uint8_t event[4]; 00208 event[0] = RFCOMM_EVENT_SERVICE_REGISTERED; 00209 event[1] = sizeof(event) - 2; 00210 event[2] = status; 00211 event[3] = channel; 00212 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00213 (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 00214 } 00215 00216 // MARK: RFCOMM MULTIPLEXER HELPER 00217 00218 static uint16_t rfcomm_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){ 00219 00220 // Assume RFCOMM header with credits and single byte length field 00221 uint16_t max_frame_size = l2cap_mtu - 5; 00222 00223 // single byte can denote len up to 127 00224 if (max_frame_size > 127) { 00225 max_frame_size--; 00226 } 00227 00228 log_info("rfcomm_max_frame_size_for_l2cap_mtu: %u -> %u\n", l2cap_mtu, max_frame_size); 00229 return max_frame_size; 00230 } 00231 00232 static void rfcomm_multiplexer_initialize(rfcomm_multiplexer_t *multiplexer){ 00233 00234 memset(multiplexer, 0, sizeof(rfcomm_multiplexer_t)); 00235 00236 multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED; 00237 multiplexer->l2cap_credits = 0; 00238 multiplexer->send_dm_for_dlci = 0; 00239 multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 00240 } 00241 00242 static rfcomm_multiplexer_t * rfcomm_multiplexer_create_for_addr(bd_addr_t *addr){ 00243 00244 // alloc structure 00245 rfcomm_multiplexer_t * multiplexer = (rfcomm_multiplexer_t*)btstack_memory_rfcomm_multiplexer_get(); 00246 if (!multiplexer) return NULL; 00247 00248 // fill in 00249 rfcomm_multiplexer_initialize(multiplexer); 00250 BD_ADDR_COPY(&multiplexer->remote_addr, addr); 00251 00252 // add to services list 00253 linked_list_add(&rfcomm_multiplexers, (linked_item_t *) multiplexer); 00254 00255 return multiplexer; 00256 } 00257 00258 static rfcomm_multiplexer_t * rfcomm_multiplexer_for_addr(bd_addr_t *addr){ 00259 linked_item_t *it; 00260 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){ 00261 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 00262 if (BD_ADDR_CMP(addr, multiplexer->remote_addr) == 0) { 00263 return multiplexer; 00264 }; 00265 } 00266 return NULL; 00267 } 00268 00269 static rfcomm_multiplexer_t * rfcomm_multiplexer_for_l2cap_cid(uint16_t l2cap_cid) { 00270 linked_item_t *it; 00271 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){ 00272 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 00273 if (multiplexer->l2cap_cid == l2cap_cid) { 00274 return multiplexer; 00275 }; 00276 } 00277 return NULL; 00278 } 00279 00280 static int rfcomm_multiplexer_has_channels(rfcomm_multiplexer_t * multiplexer){ 00281 linked_item_t *it; 00282 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00283 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 00284 if (channel->multiplexer == multiplexer) { 00285 return 1; 00286 } 00287 } 00288 return 0; 00289 } 00290 00291 // MARK: RFCOMM CHANNEL HELPER 00292 00293 static void rfcomm_dump_channels(void){ 00294 #ifndef EMBEDDED 00295 linked_item_t * it; 00296 int channels = 0; 00297 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00298 rfcomm_channel_t * channel = (rfcomm_channel_t *) it; 00299 log_info("Channel #%u: addr %p, state %u\n", channels, channel, channel->state); 00300 channels++; 00301 } 00302 #endif 00303 } 00304 00305 static void rfcomm_channel_initialize(rfcomm_channel_t *channel, rfcomm_multiplexer_t *multiplexer, 00306 rfcomm_service_t *service, uint8_t server_channel){ 00307 00308 // don't use 0 as channel id 00309 if (rfcomm_client_cid_generator == 0) ++rfcomm_client_cid_generator; 00310 00311 // setup channel 00312 memset(channel, 0, sizeof(rfcomm_channel_t)); 00313 00314 channel->state = RFCOMM_CHANNEL_CLOSED; 00315 channel->state_var = RFCOMM_CHANNEL_STATE_VAR_NONE; 00316 00317 channel->multiplexer = multiplexer; 00318 channel->service = service; 00319 channel->rfcomm_cid = rfcomm_client_cid_generator++; 00320 channel->max_frame_size = multiplexer->max_frame_size; 00321 00322 channel->credits_incoming = 0; 00323 channel->credits_outgoing = 0; 00324 channel->packets_granted = 0; 00325 00326 // incoming flow control not active 00327 channel->new_credits_incoming = 0x30; 00328 channel->incoming_flow_control = 0; 00329 00330 if (service) { 00331 // incoming connection 00332 channel->outgoing = 0; 00333 channel->dlci = (server_channel << 1) | multiplexer->outgoing; 00334 if (channel->max_frame_size > service->max_frame_size) { 00335 channel->max_frame_size = service->max_frame_size; 00336 } 00337 channel->incoming_flow_control = service->incoming_flow_control; 00338 channel->new_credits_incoming = service->incoming_initial_credits; 00339 } else { 00340 // outgoing connection 00341 channel->outgoing = 1; 00342 channel->dlci = (server_channel << 1) | (multiplexer->outgoing ^ 1); 00343 } 00344 } 00345 00346 // service == NULL -> outgoing channel 00347 static rfcomm_channel_t * rfcomm_channel_create(rfcomm_multiplexer_t * multiplexer, 00348 rfcomm_service_t * service, uint8_t server_channel){ 00349 00350 log_info("rfcomm_channel_create for service %p, channel %u --- begin\n", service, server_channel); 00351 rfcomm_dump_channels(); 00352 00353 // alloc structure 00354 rfcomm_channel_t * channel = (rfcomm_channel_t*)btstack_memory_rfcomm_channel_get(); 00355 if (!channel) return NULL; 00356 00357 // fill in 00358 rfcomm_channel_initialize(channel, multiplexer, service, server_channel); 00359 00360 // add to services list 00361 linked_list_add(&rfcomm_channels, (linked_item_t *) channel); 00362 00363 return channel; 00364 } 00365 00366 static rfcomm_channel_t * rfcomm_channel_for_rfcomm_cid(uint16_t rfcomm_cid){ 00367 linked_item_t *it; 00368 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00369 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 00370 if (channel->rfcomm_cid == rfcomm_cid) { 00371 return channel; 00372 }; 00373 } 00374 return NULL; 00375 } 00376 00377 static rfcomm_channel_t * rfcomm_channel_for_multiplexer_and_dlci(rfcomm_multiplexer_t * multiplexer, uint8_t dlci){ 00378 linked_item_t *it; 00379 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00380 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 00381 if (channel->dlci == dlci && channel->multiplexer == multiplexer) { 00382 return channel; 00383 }; 00384 } 00385 return NULL; 00386 } 00387 00388 static rfcomm_service_t * rfcomm_service_for_channel(uint8_t server_channel){ 00389 linked_item_t *it; 00390 for (it = (linked_item_t *) rfcomm_services; it ; it = it->next){ 00391 rfcomm_service_t * service = ((rfcomm_service_t *) it); 00392 if ( service->server_channel == server_channel){ 00393 return service; 00394 }; 00395 } 00396 return NULL; 00397 } 00398 00399 // MARK: RFCOMM SEND 00400 00401 /** 00402 * @param credits - only used for RFCOMM flow control in UIH wiht P/F = 1 00403 */ 00404 static int rfcomm_send_packet_for_multiplexer(rfcomm_multiplexer_t *multiplexer, uint8_t address, uint8_t control, uint8_t credits, uint8_t *data, uint16_t len){ 00405 00406 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) return BTSTACK_ACL_BUFFERS_FULL; 00407 00408 uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer(); 00409 00410 uint16_t pos = 0; 00411 uint8_t crc_fields = 3; 00412 00413 rfcomm_out_buffer[pos++] = address; 00414 rfcomm_out_buffer[pos++] = control; 00415 00416 // length field can be 1 or 2 octets 00417 if (len < 128){ 00418 rfcomm_out_buffer[pos++] = (len << 1)| 1; // bits 0-6 00419 } else { 00420 rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6 00421 rfcomm_out_buffer[pos++] = len >> 7; // bits 7-14 00422 crc_fields++; 00423 } 00424 00425 // add credits for UIH frames when PF bit is set 00426 if (control == BT_RFCOMM_UIH_PF){ 00427 rfcomm_out_buffer[pos++] = credits; 00428 } 00429 00430 // copy actual data 00431 if (len) { 00432 memcpy(&rfcomm_out_buffer[pos], data, len); 00433 pos += len; 00434 } 00435 00436 // UIH frames only calc FCS over address + control (5.1.1) 00437 if ((control & 0xef) == BT_RFCOMM_UIH){ 00438 crc_fields = 2; 00439 } 00440 rfcomm_out_buffer[pos++] = crc8_calc(rfcomm_out_buffer, crc_fields); // calc fcs 00441 00442 int credits_taken = 0; 00443 if (multiplexer->l2cap_credits){ 00444 credits_taken++; 00445 multiplexer->l2cap_credits--; 00446 } else { 00447 log_info( "rfcomm_send_packet addr %02x, ctrl %02x size %u without l2cap credits\n", address, control, pos); 00448 } 00449 00450 int err = l2cap_send_prepared(multiplexer->l2cap_cid, pos); 00451 00452 if (err) { 00453 // undo credit counting 00454 multiplexer->l2cap_credits += credits_taken; 00455 } 00456 return err; 00457 } 00458 00459 // C/R Flag in Address 00460 // - terms: initiator = station that creates multiplexer with SABM 00461 // - terms: responder = station that responds to multiplexer setup with UA 00462 // "For SABM, UA, DM and DISC frames C/R bit is set according to Table 1 in GSM 07.10, section 5.2.1.2" 00463 // - command initiator = 1 /response responder = 1 00464 // - command responder = 0 /response initiator = 0 00465 // "For UIH frames, the C/R bit is always set according to section 5.4.3.1 in GSM 07.10. 00466 // This applies independently of what is contained wthin the UIH frames, either data or control messages." 00467 // - c/r = 1 for frames by initiating station, 0 = for frames by responding station 00468 00469 // C/R Flag in Message 00470 // "In the message level, the C/R bit in the command type field is set as stated in section 5.4.6.2 in GSM 07.10." 00471 // - If the C/R bit is set to 1 the message is a command 00472 // - if it is set to 0 the message is a response. 00473 00474 // temp/old messge construction 00475 00476 // new object oriented version 00477 static int rfcomm_send_sabm(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 00478 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command 00479 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_SABM, 0, NULL, 0); 00480 } 00481 00482 static int rfcomm_send_disc(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 00483 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command 00484 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DISC, 0, NULL, 0); 00485 } 00486 00487 static int rfcomm_send_ua(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 00488 uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response 00489 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UA, 0, NULL, 0); 00490 } 00491 00492 static int rfcomm_send_dm_pf(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 00493 uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response 00494 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DM_PF, 0, NULL, 0); 00495 } 00496 00497 static int rfcomm_send_uih_msc_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) { 00498 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 00499 uint8_t payload[4]; 00500 uint8_t pos = 0; 00501 payload[pos++] = BT_RFCOMM_MSC_CMD; 00502 payload[pos++] = 2 << 1 | 1; // len 00503 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 00504 payload[pos++] = signals; 00505 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 00506 } 00507 00508 static int rfcomm_send_uih_msc_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) { 00509 uint8_t address = (1 << 0) | (multiplexer->outgoing<< 1); 00510 uint8_t payload[4]; 00511 uint8_t pos = 0; 00512 payload[pos++] = BT_RFCOMM_MSC_RSP; 00513 payload[pos++] = 2 << 1 | 1; // len 00514 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 00515 payload[pos++] = signals; 00516 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 00517 } 00518 00519 static int rfcomm_send_uih_pn_command(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint16_t max_frame_size){ 00520 uint8_t payload[10]; 00521 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 00522 uint8_t pos = 0; 00523 payload[pos++] = BT_RFCOMM_PN_CMD; 00524 payload[pos++] = 8 << 1 | 1; // len 00525 payload[pos++] = dlci; 00526 payload[pos++] = 0xf0; // pre-defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM 00527 payload[pos++] = 0; // priority 00528 payload[pos++] = 0; // max 60 seconds ack 00529 payload[pos++] = max_frame_size & 0xff; // max framesize low 00530 payload[pos++] = max_frame_size >> 8; // max framesize high 00531 payload[pos++] = 0x00; // number of retransmissions 00532 payload[pos++] = 0x00; // (unused error recovery window) initial number of credits 00533 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 00534 } 00535 00536 // "The response may not change the DLCI, the priority, the convergence layer, or the timer value." RFCOMM-tutorial.pdf 00537 static int rfcomm_send_uih_pn_response(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, 00538 uint8_t priority, uint16_t max_frame_size){ 00539 uint8_t payload[10]; 00540 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 00541 uint8_t pos = 0; 00542 payload[pos++] = BT_RFCOMM_PN_RSP; 00543 payload[pos++] = 8 << 1 | 1; // len 00544 payload[pos++] = dlci; 00545 payload[pos++] = 0xe0; // pre defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM 00546 payload[pos++] = priority; // priority 00547 payload[pos++] = 0; // max 60 seconds ack 00548 payload[pos++] = max_frame_size & 0xff; // max framesize low 00549 payload[pos++] = max_frame_size >> 8; // max framesize high 00550 payload[pos++] = 0x00; // number of retransmissions 00551 payload[pos++] = 0x00; // (unused error recovery window) initial number of credits 00552 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 00553 } 00554 00555 static int rfcomm_send_uih_rpn_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, rfcomm_rpn_data_t *rpn_data) { 00556 uint8_t payload[10]; 00557 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 00558 uint8_t pos = 0; 00559 payload[pos++] = BT_RFCOMM_RPN_RSP; 00560 payload[pos++] = 8 << 1 | 1; // len 00561 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 00562 payload[pos++] = rpn_data->baud_rate; 00563 payload[pos++] = rpn_data->flags; 00564 payload[pos++] = rpn_data->flow_control; 00565 payload[pos++] = rpn_data->xon; 00566 payload[pos++] = rpn_data->xoff; 00567 payload[pos++] = rpn_data->parameter_mask_0; 00568 payload[pos++] = rpn_data->parameter_mask_1; 00569 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 00570 } 00571 00572 static int rfcomm_send_uih_data(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t *data, uint16_t len){ 00573 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); 00574 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, data, len); 00575 } 00576 00577 static void rfcomm_send_uih_credits(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t credits){ 00578 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); 00579 rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH_PF, credits, NULL, 0); 00580 } 00581 00582 // MARK: RFCOMM MULTIPLEXER 00583 00584 static void rfcomm_multiplexer_finalize(rfcomm_multiplexer_t * multiplexer){ 00585 00586 // remove (potential) timer 00587 if (multiplexer->timer_active) { 00588 run_loop_remove_timer(&multiplexer->timer); 00589 multiplexer->timer_active = 0; 00590 } 00591 00592 // close and remove all channels 00593 linked_item_t *it = (linked_item_t *) &rfcomm_channels; 00594 while (it->next){ 00595 rfcomm_channel_t * channel = (rfcomm_channel_t *) it->next; 00596 if (channel->multiplexer == multiplexer) { 00597 // emit appropriate events 00598 if (channel->state == RFCOMM_CHANNEL_OPEN) { 00599 rfcomm_emit_channel_closed(channel); 00600 } else { 00601 rfcomm_emit_channel_opened(channel, RFCOMM_MULTIPLEXER_STOPPED); 00602 } 00603 // remove from list 00604 it->next = it->next->next; 00605 // free channel struct 00606 btstack_memory_rfcomm_channel_free(channel); 00607 } else { 00608 it = it->next; 00609 } 00610 } 00611 00612 // keep reference to l2cap channel 00613 uint16_t l2cap_cid = multiplexer->l2cap_cid; 00614 00615 // remove mutliplexer 00616 linked_list_remove( &rfcomm_multiplexers, (linked_item_t *) multiplexer); 00617 btstack_memory_rfcomm_multiplexer_free(multiplexer); 00618 00619 // close l2cap multiplexer channel, too 00620 l2cap_disconnect_internal(l2cap_cid, 0x13); 00621 } 00622 00623 static void rfcomm_multiplexer_timer_handler(timer_source_t *timer){ 00624 rfcomm_multiplexer_t * multiplexer = (rfcomm_multiplexer_t *) linked_item_get_user( (linked_item_t *) timer); 00625 if (!rfcomm_multiplexer_has_channels(multiplexer)){ 00626 log_info( "rfcomm_multiplexer_timer_handler timeout: shutting down multiplexer!\n"); 00627 rfcomm_multiplexer_finalize(multiplexer); 00628 } 00629 } 00630 00631 static void rfcomm_multiplexer_prepare_idle_timer(rfcomm_multiplexer_t * multiplexer){ 00632 if (multiplexer->timer_active) { 00633 run_loop_remove_timer(&multiplexer->timer); 00634 multiplexer->timer_active = 0; 00635 } 00636 if (!rfcomm_multiplexer_has_channels(multiplexer)){ 00637 // start timer for multiplexer timeout check 00638 run_loop_set_timer(&multiplexer->timer, RFCOMM_MULIPLEXER_TIMEOUT_MS); 00639 multiplexer->timer.process = rfcomm_multiplexer_timer_handler; 00640 linked_item_set_user((linked_item_t*) &multiplexer->timer, multiplexer); 00641 run_loop_add_timer(&multiplexer->timer); 00642 multiplexer->timer_active = 1; 00643 } 00644 } 00645 00646 static void rfcomm_multiplexer_opened(rfcomm_multiplexer_t *multiplexer){ 00647 log_info("Multiplexer up and running\n"); 00648 multiplexer->state = RFCOMM_MULTIPLEXER_OPEN; 00649 00650 rfcomm_channel_event_t event = { CH_EVT_MULTIPLEXER_READY }; 00651 00652 // transition of channels that wait for multiplexer 00653 linked_item_t *it; 00654 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00655 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 00656 if (channel->multiplexer != multiplexer) continue; 00657 rfcomm_channel_state_machine(channel, &event); 00658 } 00659 00660 rfcomm_run(); 00661 rfcomm_multiplexer_prepare_idle_timer(multiplexer); 00662 } 00663 00664 00665 /** 00666 * @return handled packet 00667 */ 00668 static int rfcomm_multiplexer_hci_event_handler(uint8_t *packet, uint16_t size){ 00669 bd_addr_t event_addr; 00670 uint16_t psm; 00671 uint16_t l2cap_cid; 00672 hci_con_handle_t con_handle; 00673 rfcomm_multiplexer_t *multiplexer = NULL; 00674 switch (packet[0]) { 00675 00676 // accept incoming PSM_RFCOMM connection if no multiplexer exists yet 00677 case L2CAP_EVENT_INCOMING_CONNECTION: 00678 // data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) 00679 bt_flip_addr(event_addr, &packet[2]); 00680 con_handle = READ_BT_16(packet, 8); 00681 psm = READ_BT_16(packet, 10); 00682 l2cap_cid = READ_BT_16(packet, 12); 00683 00684 if (psm != PSM_RFCOMM) break; 00685 00686 multiplexer = rfcomm_multiplexer_for_addr(&event_addr); 00687 00688 if (multiplexer) { 00689 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - multiplexer already exists", l2cap_cid); 00690 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 00691 return 1; 00692 } 00693 00694 // create and inititialize new multiplexer instance (incoming) 00695 multiplexer = rfcomm_multiplexer_create_for_addr(&event_addr); 00696 if (!multiplexer){ 00697 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - no memory left", l2cap_cid); 00698 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 00699 return 1; 00700 } 00701 00702 multiplexer->con_handle = con_handle; 00703 multiplexer->l2cap_cid = l2cap_cid; 00704 multiplexer->state = RFCOMM_MULTIPLEXER_W4_SABM_0; 00705 00706 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => accept", l2cap_cid); 00707 l2cap_accept_connection_internal(l2cap_cid); 00708 return 1; 00709 00710 // l2cap connection opened -> store l2cap_cid, remote_addr 00711 case L2CAP_EVENT_CHANNEL_OPENED: 00712 if (READ_BT_16(packet, 11) != PSM_RFCOMM) break; 00713 log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_RFCOMM\n"); 00714 // get multiplexer for remote addr 00715 con_handle = READ_BT_16(packet, 9); 00716 l2cap_cid = READ_BT_16(packet, 13); 00717 bt_flip_addr(event_addr, &packet[3]); 00718 multiplexer = rfcomm_multiplexer_for_addr(&event_addr); 00719 if (!multiplexer) { 00720 log_error("L2CAP_EVENT_CHANNEL_OPENED but no multiplexer prepared\n"); 00721 return 1; 00722 } 00723 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_CONNECT) { 00724 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection\n"); 00725 // wrong remote addr 00726 if (BD_ADDR_CMP(event_addr, multiplexer->remote_addr)) break; 00727 multiplexer->l2cap_cid = l2cap_cid; 00728 multiplexer->con_handle = con_handle; 00729 // send SABM #0 00730 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_SABM_0; 00731 } else { // multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0 00732 00733 // set max frame size based on l2cap MTU 00734 multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17)); 00735 } 00736 return 1; 00737 00738 // l2cap disconnect -> state = RFCOMM_MULTIPLEXER_CLOSED; 00739 00740 case L2CAP_EVENT_CREDITS: 00741 // data: event(8), len(8), local_cid(16), credits(8) 00742 l2cap_cid = READ_BT_16(packet, 2); 00743 multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid); 00744 if (!multiplexer) break; 00745 multiplexer->l2cap_credits += packet[4]; 00746 00747 // log_info("L2CAP_EVENT_CREDITS: %u (now %u)\n", packet[4], multiplexer->l2cap_credits); 00748 00749 // new credits, continue with signaling 00750 rfcomm_run(); 00751 00752 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) break; 00753 rfcomm_hand_out_credits(); 00754 return 1; 00755 00756 case DAEMON_EVENT_HCI_PACKET_SENT: 00757 // testing DMA done code 00758 rfcomm_run(); 00759 break; 00760 00761 case L2CAP_EVENT_CHANNEL_CLOSED: 00762 // data: event (8), len(8), channel (16) 00763 l2cap_cid = READ_BT_16(packet, 2); 00764 multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid); 00765 if (!multiplexer) break; 00766 switch (multiplexer->state) { 00767 case RFCOMM_MULTIPLEXER_W4_SABM_0: 00768 case RFCOMM_MULTIPLEXER_W4_UA_0: 00769 case RFCOMM_MULTIPLEXER_OPEN: 00770 rfcomm_multiplexer_finalize(multiplexer); 00771 return 1; 00772 default: 00773 break; 00774 } 00775 break; 00776 default: 00777 break; 00778 } 00779 return 0; 00780 } 00781 00782 static int rfcomm_multiplexer_l2cap_packet_handler(uint16_t channel, uint8_t *packet, uint16_t size){ 00783 00784 // get or create a multiplexer for a certain device 00785 rfcomm_multiplexer_t *multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 00786 if (!multiplexer) return 0; 00787 00788 // but only care for multiplexer control channel 00789 uint8_t frame_dlci = packet[0] >> 2; 00790 if (frame_dlci) return 0; 00791 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 00792 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 00793 const uint8_t payload_offset = 3 + length_offset + credit_offset; 00794 switch (packet[1]){ 00795 00796 case BT_RFCOMM_SABM: 00797 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0){ 00798 log_info("Received SABM #0\n"); 00799 multiplexer->outgoing = 0; 00800 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0; 00801 return 1; 00802 } 00803 break; 00804 00805 case BT_RFCOMM_UA: 00806 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_UA_0) { 00807 // UA #0 -> send UA #0, state = RFCOMM_MULTIPLEXER_OPEN 00808 log_info("Received UA #0 \n"); 00809 rfcomm_multiplexer_opened(multiplexer); 00810 return 1; 00811 } 00812 break; 00813 00814 case BT_RFCOMM_DISC: 00815 // DISC #0 -> send UA #0, close multiplexer 00816 log_info("Received DISC #0, (ougoing = %u)\n", multiplexer->outgoing); 00817 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC; 00818 return 1; 00819 00820 case BT_RFCOMM_DM: 00821 // DM #0 - we shouldn't get this, just give up 00822 log_info("Received DM #0\n"); 00823 log_info("-> Closing down multiplexer\n"); 00824 rfcomm_multiplexer_finalize(multiplexer); 00825 return 1; 00826 00827 case BT_RFCOMM_UIH: 00828 if (packet[payload_offset] == BT_RFCOMM_CLD_CMD){ 00829 // Multiplexer close down (CLD) -> close mutliplexer 00830 log_info("Received Multiplexer close down command\n"); 00831 log_info("-> Closing down multiplexer\n"); 00832 rfcomm_multiplexer_finalize(multiplexer); 00833 return 1; 00834 } 00835 break; 00836 00837 default: 00838 break; 00839 00840 } 00841 return 0; 00842 } 00843 00844 static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event){ 00845 00846 // process stored DM responses 00847 if (multiplexer->send_dm_for_dlci){ 00848 rfcomm_send_dm_pf(multiplexer, multiplexer->send_dm_for_dlci); 00849 multiplexer->send_dm_for_dlci = 0; 00850 } 00851 00852 switch (multiplexer->state) { 00853 case RFCOMM_MULTIPLEXER_SEND_SABM_0: 00854 switch (event) { 00855 case MULT_EV_READY_TO_SEND: 00856 log_info("Sending SABM #0 - (multi 0x%p)\n", multiplexer); 00857 multiplexer->state = RFCOMM_MULTIPLEXER_W4_UA_0; 00858 rfcomm_send_sabm(multiplexer, 0); 00859 break; 00860 default: 00861 break; 00862 } 00863 break; 00864 case RFCOMM_MULTIPLEXER_SEND_UA_0: 00865 switch (event) { 00866 case MULT_EV_READY_TO_SEND: 00867 log_info("Sending UA #0\n"); 00868 multiplexer->state = RFCOMM_MULTIPLEXER_OPEN; 00869 rfcomm_send_ua(multiplexer, 0); 00870 rfcomm_multiplexer_opened(multiplexer); 00871 break; 00872 default: 00873 break; 00874 } 00875 break; 00876 case RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC: 00877 switch (event) { 00878 case MULT_EV_READY_TO_SEND: 00879 log_info("Sending UA #0\n"); 00880 log_info("Closing down multiplexer\n"); 00881 multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED; 00882 rfcomm_send_ua(multiplexer, 0); 00883 rfcomm_multiplexer_finalize(multiplexer); 00884 // try to detect authentication errors: drop link key if multiplexer closed before first channel got opened 00885 if (!multiplexer->at_least_one_connection){ 00886 log_info("TODO: no connections established - delete link key prophylactically\n"); 00887 // hci_send_cmd(&hci_delete_stored_link_key, multiplexer->remote_addr); 00888 } 00889 default: 00890 break; 00891 } 00892 break; 00893 default: 00894 break; 00895 } 00896 } 00897 00898 // MARK: RFCOMM CHANNEL 00899 00900 static void rfcomm_hand_out_credits(void){ 00901 linked_item_t * it; 00902 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 00903 rfcomm_channel_t * channel = (rfcomm_channel_t *) it; 00904 if (channel->state != RFCOMM_CHANNEL_OPEN) { 00905 // log_info("RFCOMM_EVENT_CREDITS: multiplexer not open\n"); 00906 continue; 00907 } 00908 if (channel->packets_granted) { 00909 // log_info("RFCOMM_EVENT_CREDITS: already packets granted\n"); 00910 continue; 00911 } 00912 if (!channel->credits_outgoing) { 00913 // log_info("RFCOMM_EVENT_CREDITS: no outgoing credits\n"); 00914 continue; 00915 } 00916 if (!channel->multiplexer->l2cap_credits){ 00917 // log_info("RFCOMM_EVENT_CREDITS: no l2cap credits\n"); 00918 continue; 00919 } 00920 // channel open, multiplexer has l2cap credits and we didn't hand out credit before -> go! 00921 // log_info("RFCOMM_EVENT_CREDITS: 1\n"); 00922 channel->packets_granted += 1; 00923 rfcomm_emit_credits(channel, 1); 00924 } 00925 } 00926 00927 static void rfcomm_channel_send_credits(rfcomm_channel_t *channel, uint8_t credits){ 00928 rfcomm_send_uih_credits(channel->multiplexer, channel->dlci, credits); 00929 channel->credits_incoming += credits; 00930 00931 rfcomm_emit_credit_status(channel); 00932 } 00933 00934 static void rfcomm_channel_opened(rfcomm_channel_t *rfChannel){ 00935 00936 log_info("rfcomm_channel_opened!\n"); 00937 00938 rfChannel->state = RFCOMM_CHANNEL_OPEN; 00939 rfcomm_emit_channel_opened(rfChannel, 0); 00940 rfcomm_hand_out_credits(); 00941 00942 // remove (potential) timer 00943 rfcomm_multiplexer_t *multiplexer = rfChannel->multiplexer; 00944 if (multiplexer->timer_active) { 00945 run_loop_remove_timer(&multiplexer->timer); 00946 multiplexer->timer_active = 0; 00947 } 00948 // hack for problem detecting authentication failure 00949 multiplexer->at_least_one_connection = 1; 00950 00951 // start next connection request if pending 00952 rfcomm_run(); 00953 } 00954 00955 static void rfcomm_channel_packet_handler_uih(rfcomm_multiplexer_t *multiplexer, uint8_t * packet, uint16_t size){ 00956 const uint8_t frame_dlci = packet[0] >> 2; 00957 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 00958 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 00959 const uint8_t payload_offset = 3 + length_offset + credit_offset; 00960 00961 rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, frame_dlci); 00962 if (!channel) return; 00963 00964 // handle new outgoing credits 00965 if (packet[1] == BT_RFCOMM_UIH_PF) { 00966 00967 // add them 00968 uint16_t new_credits = packet[3+length_offset]; 00969 channel->credits_outgoing += new_credits; 00970 log_info( "RFCOMM data UIH_PF, new credits: %u, now %u\n", new_credits, channel->credits_outgoing); 00971 00972 // notify channel statemachine 00973 rfcomm_channel_event_t channel_event = { CH_EVT_RCVD_CREDITS }; 00974 rfcomm_channel_state_machine(channel, &channel_event); 00975 } 00976 00977 // contains payload? 00978 if (size - 1 > payload_offset){ 00979 00980 // log_info( "RFCOMM data UIH_PF, size %u, channel %p\n", size-payload_offset-1, rfChannel->connection); 00981 00982 // decrease incoming credit counter 00983 if (channel->credits_incoming > 0){ 00984 channel->credits_incoming--; 00985 } 00986 00987 // deliver payload 00988 (*app_packet_handler)(channel->connection, RFCOMM_DATA_PACKET, channel->rfcomm_cid, 00989 &packet[payload_offset], size-payload_offset-1); 00990 } 00991 00992 // automatically provide new credits to remote device, if no incoming flow control 00993 if (!channel->incoming_flow_control && channel->credits_incoming < 5){ 00994 channel->new_credits_incoming = 0x30; 00995 } 00996 00997 rfcomm_emit_credit_status(channel); 00998 00999 // we received new RFCOMM credits, hand them out if possible 01000 rfcomm_hand_out_credits(); 01001 } 01002 01003 static void rfcomm_channel_accept_pn(rfcomm_channel_t *channel, rfcomm_channel_event_pn_t *event){ 01004 // priority of client request 01005 channel->pn_priority = event->priority; 01006 01007 // new credits 01008 channel->credits_outgoing = event->credits_outgoing; 01009 01010 // negotiate max frame size 01011 if (channel->max_frame_size > channel->multiplexer->max_frame_size) { 01012 channel->max_frame_size = channel->multiplexer->max_frame_size; 01013 } 01014 if (channel->max_frame_size > event->max_frame_size) { 01015 channel->max_frame_size = event->max_frame_size; 01016 } 01017 01018 } 01019 01020 static void rfcomm_channel_finalize(rfcomm_channel_t *channel){ 01021 01022 rfcomm_multiplexer_t *multiplexer = channel->multiplexer; 01023 01024 // remove from list 01025 linked_list_remove( &rfcomm_channels, (linked_item_t *) channel); 01026 01027 // free channel 01028 btstack_memory_rfcomm_channel_free(channel); 01029 01030 // update multiplexer timeout after channel was removed from list 01031 rfcomm_multiplexer_prepare_idle_timer(multiplexer); 01032 } 01033 01034 static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event){ 01035 01036 // TODO: if client max frame size is smaller than RFCOMM_DEFAULT_SIZE, send PN 01037 01038 01039 // lookup existing channel 01040 rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, dlci); 01041 01042 // log_info("rfcomm_channel_state_machine_2 lookup dlci #%u = 0x%08x - event %u\n", dlci, (int) channel, event->type); 01043 01044 if (channel) { 01045 rfcomm_channel_state_machine(channel, event); 01046 return; 01047 } 01048 01049 // service registered? 01050 rfcomm_service_t * service = rfcomm_service_for_channel(dlci >> 1); 01051 // log_info("rfcomm_channel_state_machine_2 service dlci #%u = 0x%08x\n", dlci, (int) service); 01052 if (!service) { 01053 // discard request by sending disconnected mode 01054 multiplexer->send_dm_for_dlci = dlci; 01055 return; 01056 } 01057 01058 // create channel for some events 01059 switch (event->type) { 01060 case CH_EVT_RCVD_SABM: 01061 case CH_EVT_RCVD_PN: 01062 case CH_EVT_RCVD_RPN_REQ: 01063 case CH_EVT_RCVD_RPN_CMD: 01064 // setup incoming channel 01065 channel = rfcomm_channel_create(multiplexer, service, dlci >> 1); 01066 if (!channel){ 01067 // discard request by sending disconnected mode 01068 multiplexer->send_dm_for_dlci = dlci; 01069 } 01070 break; 01071 default: 01072 break; 01073 } 01074 01075 if (!channel) { 01076 // discard request by sending disconnected mode 01077 multiplexer->send_dm_for_dlci = dlci; 01078 return; 01079 } 01080 channel->connection = service->connection; 01081 rfcomm_channel_state_machine(channel, event); 01082 } 01083 01084 void rfcomm_channel_packet_handler(rfcomm_multiplexer_t * multiplexer, uint8_t *packet, uint16_t size){ 01085 01086 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 01087 const uint8_t frame_dlci = packet[0] >> 2; 01088 uint8_t message_dlci; // used by commands in UIH(_PF) packets 01089 uint8_t message_len; // " 01090 01091 // rfcomm: (1) command/control 01092 // -- credits_offset = 1 if command == BT_RFCOMM_UIH_PF 01093 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 01094 // rfcomm: (2) length. if bit 0 is cleared, 2 byte length is used. (little endian) 01095 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 01096 // rfcomm: (3+length_offset) credits if credits_offset == 1 01097 // rfcomm: (3+length_offest+credits_offset) 01098 const uint8_t payload_offset = 3 + length_offset + credit_offset; 01099 01100 rfcomm_channel_event_t event; 01101 rfcomm_channel_event_pn_t event_pn; 01102 rfcomm_channel_event_rpn_t event_rpn; 01103 01104 // switch by rfcomm message type 01105 switch(packet[1]) { 01106 01107 case BT_RFCOMM_SABM: 01108 event.type = CH_EVT_RCVD_SABM; 01109 log_info("Received SABM #%u\n", frame_dlci); 01110 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 01111 break; 01112 01113 case BT_RFCOMM_UA: 01114 event.type = CH_EVT_RCVD_UA; 01115 log_info("Received UA #%u - channel opened\n",frame_dlci); 01116 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 01117 break; 01118 01119 case BT_RFCOMM_DISC: 01120 event.type = CH_EVT_RCVD_DISC; 01121 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 01122 break; 01123 01124 case BT_RFCOMM_DM: 01125 case BT_RFCOMM_DM_PF: 01126 event.type = CH_EVT_RCVD_DM; 01127 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 01128 break; 01129 01130 case BT_RFCOMM_UIH_PF: 01131 case BT_RFCOMM_UIH: 01132 01133 message_len = packet[payload_offset+1] >> 1; 01134 01135 switch (packet[payload_offset]) { 01136 case BT_RFCOMM_PN_CMD: 01137 message_dlci = packet[payload_offset+2]; 01138 event_pn.super.type = CH_EVT_RCVD_PN; 01139 event_pn.priority = packet[payload_offset+4]; 01140 event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6); 01141 event_pn.credits_outgoing = packet[payload_offset+9]; 01142 log_info("Received UIH Parameter Negotiation Command for #%u\n", message_dlci); 01143 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 01144 break; 01145 01146 case BT_RFCOMM_PN_RSP: 01147 message_dlci = packet[payload_offset+2]; 01148 event_pn.super.type = CH_EVT_RCVD_PN_RSP; 01149 event_pn.priority = packet[payload_offset+4]; 01150 event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6); 01151 event_pn.credits_outgoing = packet[payload_offset+9]; 01152 log_info("UIH Parameter Negotiation Response max frame %u, credits %u\n", 01153 event_pn.max_frame_size, event_pn.credits_outgoing); 01154 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 01155 break; 01156 01157 case BT_RFCOMM_MSC_CMD: 01158 message_dlci = packet[payload_offset+2] >> 2; 01159 event.type = CH_EVT_RCVD_MSC_CMD; 01160 log_info("Received MSC CMD for #%u, \n", message_dlci); 01161 rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event); 01162 break; 01163 01164 case BT_RFCOMM_MSC_RSP: 01165 message_dlci = packet[payload_offset+2] >> 2; 01166 event.type = CH_EVT_RCVD_MSC_RSP; 01167 log_info("Received MSC RSP for #%u\n", message_dlci); 01168 rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event); 01169 break; 01170 01171 case BT_RFCOMM_RPN_CMD: 01172 message_dlci = packet[payload_offset+2] >> 2; 01173 switch (message_len){ 01174 case 1: 01175 log_info("Received Remote Port Negotiation for #%u\n", message_dlci); 01176 event.type = CH_EVT_RCVD_RPN_REQ; 01177 rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event); 01178 break; 01179 case 8: 01180 log_info("Received Remote Port Negotiation (Info) for #%u\n", message_dlci); 01181 event_rpn.super.type = CH_EVT_RCVD_RPN_CMD; 01182 event_rpn.data.baud_rate = packet[payload_offset+3]; 01183 event_rpn.data.flags = packet[payload_offset+4]; 01184 event_rpn.data.flow_control = packet[payload_offset+5]; 01185 event_rpn.data.xon = packet[payload_offset+6]; 01186 event_rpn.data.xoff = packet[payload_offset+7]; 01187 event_rpn.data.parameter_mask_0 = packet[payload_offset+8]; 01188 event_rpn.data.parameter_mask_1 = packet[payload_offset+9]; 01189 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rpn); 01190 break; 01191 default: 01192 break; 01193 } 01194 break; 01195 01196 default: 01197 log_error("Received unknown UIH packet - 0x%02x\n", packet[payload_offset]); 01198 break; 01199 } 01200 break; 01201 01202 default: 01203 log_error("Received unknown RFCOMM message type %x\n", packet[1]); 01204 break; 01205 } 01206 01207 // trigger next action - example W4_PN_RSP: transition to SEND_SABM which only depends on "can send" 01208 rfcomm_run(); 01209 } 01210 01211 void rfcomm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 01212 01213 // multiplexer handler 01214 int handled = 0; 01215 switch (packet_type) { 01216 case HCI_EVENT_PACKET: 01217 handled = rfcomm_multiplexer_hci_event_handler(packet, size); 01218 break; 01219 case L2CAP_DATA_PACKET: 01220 handled = rfcomm_multiplexer_l2cap_packet_handler(channel, packet, size); 01221 break; 01222 default: 01223 break; 01224 } 01225 01226 if (handled) { 01227 rfcomm_run(); 01228 return; 01229 } 01230 01231 // we only handle l2cap packet over open multiplexer channel now 01232 if (packet_type != L2CAP_DATA_PACKET) { 01233 (*app_packet_handler)(NULL, packet_type, channel, packet, size); 01234 return; 01235 } 01236 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 01237 if (!multiplexer || multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 01238 (*app_packet_handler)(NULL, packet_type, channel, packet, size); 01239 return; 01240 } 01241 01242 // channel data ? 01243 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 01244 const uint8_t frame_dlci = packet[0] >> 2; 01245 01246 if (frame_dlci && (packet[1] == BT_RFCOMM_UIH || packet[1] == BT_RFCOMM_UIH_PF)) { 01247 rfcomm_channel_packet_handler_uih(multiplexer, packet, size); 01248 rfcomm_run(); 01249 return; 01250 } 01251 01252 rfcomm_channel_packet_handler(multiplexer, packet, size); 01253 } 01254 01255 static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel){ 01256 // log_info("rfcomm_channel_ready_for_open state %u, flags needed %04x, current %04x, rf credits %u, l2cap credits %u \n", channel->state, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP|RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP|RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS, channel->state_var, channel->credits_outgoing, channel->multiplexer->l2cap_credits); 01257 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP) == 0) return 0; 01258 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP) == 0) return 0; 01259 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS) == 0) return 0; 01260 if (channel->credits_outgoing == 0) return 0; 01261 01262 return 1; 01263 } 01264 01265 static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event){ 01266 01267 // log_info("rfcomm_channel_state_machine: state %u, state_var %04x, event %u\n", channel->state, channel->state_var ,event->type); 01268 01269 rfcomm_multiplexer_t *multiplexer = channel->multiplexer; 01270 01271 // TODO: integrate in common switch 01272 if (event->type == CH_EVT_RCVD_DISC){ 01273 rfcomm_emit_channel_closed(channel); 01274 channel->state = RFCOMM_CHANNEL_SEND_UA_AFTER_DISC; 01275 return; 01276 } 01277 01278 // TODO: integrate in common switch 01279 if (event->type == CH_EVT_RCVD_DM){ 01280 log_info("Received DM message for #%u\n", channel->dlci); 01281 log_info("-> Closing channel locally for #%u\n", channel->dlci); 01282 rfcomm_emit_channel_closed(channel); 01283 rfcomm_channel_finalize(channel); 01284 return; 01285 } 01286 01287 // remote port negotiation command - just accept everything for now 01288 // 01289 // "The RPN command can be used before a new DLC is opened and should be used whenever the port settings change." 01290 // "The RPN command is specified as optional in TS 07.10, but it is mandatory to recognize and respond to it in RFCOMM. 01291 // (Although the handling of individual settings are implementation-dependent.)" 01292 // 01293 01294 // TODO: integrate in common switch 01295 if (event->type == CH_EVT_RCVD_RPN_CMD){ 01296 // control port parameters 01297 rfcomm_channel_event_rpn_t *event_rpn = (rfcomm_channel_event_rpn_t*) event; 01298 memcpy(&channel->rpn_data, &event_rpn->data, sizeof(rfcomm_rpn_data_t)); 01299 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP; 01300 return; 01301 } 01302 01303 // TODO: integrate in common switch 01304 if (event->type == CH_EVT_RCVD_RPN_REQ){ 01305 // default rpn rsp 01306 rfcomm_rpn_data_t rpn_data; 01307 rpn_data.baud_rate = 0xa0; /* 9600 bps */ 01308 rpn_data.flags = 0x03; /* 8-n-1 */ 01309 rpn_data.flow_control = 0; /* no flow control */ 01310 rpn_data.xon = 0xd1; /* XON */ 01311 rpn_data.xoff = 0xd3; /* XOFF */ 01312 rpn_data.parameter_mask_0 = 0x7f; /* parameter mask, all values set */ 01313 rpn_data.parameter_mask_1 = 0x3f; /* parameter mask, all values set */ 01314 memcpy(&channel->rpn_data, &rpn_data, sizeof(rfcomm_rpn_data_t)); 01315 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP; 01316 return; 01317 } 01318 01319 // TODO: integrate in common swich 01320 if (event->type == CH_EVT_READY_TO_SEND){ 01321 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP){ 01322 log_info("Sending Remote Port Negotiation RSP for #%u\n", channel->dlci); 01323 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP; 01324 rfcomm_send_uih_rpn_rsp(multiplexer, channel->dlci, &channel->rpn_data); 01325 return; 01326 } 01327 } 01328 01329 rfcomm_channel_event_pn_t * event_pn = (rfcomm_channel_event_pn_t*) event; 01330 01331 switch (channel->state) { 01332 case RFCOMM_CHANNEL_CLOSED: 01333 switch (event->type){ 01334 case CH_EVT_RCVD_SABM: 01335 log_info("-> Inform app\n"); 01336 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM; 01337 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 01338 rfcomm_emit_connection_request(channel); 01339 break; 01340 case CH_EVT_RCVD_PN: 01341 rfcomm_channel_accept_pn(channel, event_pn); 01342 log_info("-> Inform app\n"); 01343 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_PN; 01344 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 01345 rfcomm_emit_connection_request(channel); 01346 break; 01347 default: 01348 break; 01349 } 01350 break; 01351 01352 case RFCOMM_CHANNEL_INCOMING_SETUP: 01353 switch (event->type){ 01354 case CH_EVT_RCVD_SABM: 01355 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM; 01356 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 01357 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_UA; 01358 } 01359 break; 01360 case CH_EVT_RCVD_PN: 01361 rfcomm_channel_accept_pn(channel, event_pn); 01362 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_PN; 01363 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 01364 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP; 01365 } 01366 break; 01367 case CH_EVT_READY_TO_SEND: 01368 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP){ 01369 log_info("Sending UIH Parameter Negotiation Respond for #%u\n", channel->dlci); 01370 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP; 01371 rfcomm_send_uih_pn_response(multiplexer, channel->dlci, channel->pn_priority, channel->max_frame_size); 01372 } 01373 else if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA){ 01374 log_info("Sending UA #%u\n", channel->dlci); 01375 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_UA; 01376 rfcomm_send_ua(multiplexer, channel->dlci); 01377 } 01378 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) && (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM)) { 01379 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD; 01380 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS; 01381 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 01382 } 01383 break; 01384 01385 default: 01386 break; 01387 } 01388 break; 01389 01390 case RFCOMM_CHANNEL_W4_MULTIPLEXER: 01391 switch (event->type) { 01392 case CH_EVT_MULTIPLEXER_READY: 01393 log_info("Muliplexer opened, sending UIH PN next\n"); 01394 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 01395 break; 01396 default: 01397 break; 01398 } 01399 break; 01400 01401 case RFCOMM_CHANNEL_SEND_UIH_PN: 01402 switch (event->type) { 01403 case CH_EVT_READY_TO_SEND: 01404 log_info("Sending UIH Parameter Negotiation Command for #%u (channel 0x%p)\n", channel->dlci, channel ); 01405 channel->state = RFCOMM_CHANNEL_W4_PN_RSP; 01406 rfcomm_send_uih_pn_command(multiplexer, channel->dlci, channel->max_frame_size); 01407 break; 01408 default: 01409 break; 01410 } 01411 break; 01412 01413 case RFCOMM_CHANNEL_W4_PN_RSP: 01414 switch (event->type){ 01415 case CH_EVT_RCVD_PN_RSP: 01416 // update max frame size 01417 if (channel->max_frame_size > event_pn->max_frame_size) { 01418 channel->max_frame_size = event_pn->max_frame_size; 01419 } 01420 // new credits 01421 channel->credits_outgoing = event_pn->credits_outgoing; 01422 channel->state = RFCOMM_CHANNEL_SEND_SABM_W4_UA; 01423 break; 01424 default: 01425 break; 01426 } 01427 break; 01428 01429 case RFCOMM_CHANNEL_SEND_SABM_W4_UA: 01430 switch (event->type) { 01431 case CH_EVT_READY_TO_SEND: 01432 log_info("Sending SABM #%u\n", channel->dlci); 01433 channel->state = RFCOMM_CHANNEL_W4_UA; 01434 rfcomm_send_sabm(multiplexer, channel->dlci); 01435 break; 01436 default: 01437 break; 01438 } 01439 break; 01440 01441 case RFCOMM_CHANNEL_W4_UA: 01442 switch (event->type){ 01443 case CH_EVT_RCVD_UA: 01444 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 01445 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD; 01446 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS; 01447 break; 01448 default: 01449 break; 01450 } 01451 break; 01452 01453 case RFCOMM_CHANNEL_DLC_SETUP: 01454 switch (event->type){ 01455 case CH_EVT_RCVD_MSC_CMD: 01456 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_CMD; 01457 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP; 01458 break; 01459 case CH_EVT_RCVD_MSC_RSP: 01460 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP; 01461 break; 01462 01463 case CH_EVT_READY_TO_SEND: 01464 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD){ 01465 log_info("Sending MSC CMD for #%u\n", channel->dlci); 01466 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD; 01467 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_CMD; 01468 rfcomm_send_uih_msc_cmd(multiplexer, channel->dlci , 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 01469 break; 01470 } 01471 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){ 01472 log_info("Sending MSC RSP for #%u\n", channel->dlci); 01473 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP; 01474 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP; 01475 rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 01476 break; 01477 } 01478 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS){ 01479 log_info("Providing credits for #%u\n", channel->dlci); 01480 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS; 01481 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS; 01482 if (channel->new_credits_incoming) { 01483 uint8_t new_credits = channel->new_credits_incoming; 01484 channel->new_credits_incoming = 0; 01485 rfcomm_channel_send_credits(channel, new_credits); 01486 } 01487 break; 01488 01489 } 01490 break; 01491 default: 01492 break; 01493 } 01494 // finally done? 01495 if (rfcomm_channel_ready_for_open(channel)){ 01496 channel->state = RFCOMM_CHANNEL_OPEN; 01497 rfcomm_channel_opened(channel); 01498 } 01499 break; 01500 01501 case RFCOMM_CHANNEL_OPEN: 01502 switch (event->type){ 01503 case CH_EVT_RCVD_MSC_CMD: 01504 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP; 01505 break; 01506 case CH_EVT_READY_TO_SEND: 01507 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){ 01508 log_info("Sending MSC RSP for #%u\n", channel->dlci); 01509 channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP; 01510 rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 01511 break; 01512 } 01513 if (channel->new_credits_incoming) { 01514 uint8_t new_credits = channel->new_credits_incoming; 01515 channel->new_credits_incoming = 0; 01516 rfcomm_channel_send_credits(channel, new_credits); 01517 break; 01518 } 01519 break; 01520 case CH_EVT_RCVD_CREDITS: { 01521 // notify daemon -> might trigger re-try of parked connections 01522 uint8_t event[1] = { DAEMON_EVENT_NEW_RFCOMM_CREDITS }; 01523 (*app_packet_handler)(channel->connection, DAEMON_EVENT_PACKET, channel->rfcomm_cid, event, sizeof(event)); 01524 break; 01525 } 01526 default: 01527 break; 01528 } 01529 break; 01530 01531 case RFCOMM_CHANNEL_SEND_DM: 01532 switch (event->type) { 01533 case CH_EVT_READY_TO_SEND: 01534 log_info("Sending DM_PF for #%u\n", channel->dlci); 01535 // don't emit channel closed - channel was never open 01536 channel->state = RFCOMM_CHANNEL_CLOSED; 01537 rfcomm_send_dm_pf(multiplexer, channel->dlci); 01538 rfcomm_channel_finalize(channel); 01539 break; 01540 default: 01541 break; 01542 } 01543 break; 01544 01545 case RFCOMM_CHANNEL_SEND_DISC: 01546 switch (event->type) { 01547 case CH_EVT_READY_TO_SEND: 01548 channel->state = RFCOMM_CHANNEL_CLOSED; 01549 rfcomm_send_disc(multiplexer, channel->dlci); 01550 rfcomm_emit_channel_closed(channel); 01551 rfcomm_channel_finalize(channel); 01552 break; 01553 default: 01554 break; 01555 } 01556 break; 01557 01558 case RFCOMM_CHANNEL_SEND_UA_AFTER_DISC: 01559 switch (event->type) { 01560 case CH_EVT_READY_TO_SEND: 01561 log_info("Sending UA after DISC for #%u\n", channel->dlci); 01562 channel->state = RFCOMM_CHANNEL_CLOSED; 01563 rfcomm_send_ua(multiplexer, channel->dlci); 01564 rfcomm_channel_finalize(channel); 01565 break; 01566 default: 01567 break; 01568 } 01569 break; 01570 01571 default: 01572 break; 01573 } 01574 } 01575 01576 01577 // MARK: RFCOMM RUN 01578 // process outstanding signaling tasks 01579 static void rfcomm_run(void){ 01580 01581 linked_item_t *it; 01582 linked_item_t *next; 01583 01584 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = next){ 01585 01586 next = it->next; // be prepared for removal of channel in state machine 01587 01588 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 01589 01590 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) { 01591 // log_info("rfcomm_run cannot send l2cap packet for #%u, credits %u\n", multiplexer->l2cap_cid, multiplexer->l2cap_credits); 01592 continue; 01593 } 01594 // log_info("rfcomm_run: multi 0x%08x, state %u\n", (int) multiplexer, multiplexer->state); 01595 01596 rfcomm_multiplexer_state_machine(multiplexer, MULT_EV_READY_TO_SEND); 01597 } 01598 01599 for (it = (linked_item_t *) rfcomm_channels; it ; it = next){ 01600 01601 next = it->next; // be prepared for removal of channel in state machine 01602 01603 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 01604 rfcomm_multiplexer_t * multiplexer = channel->multiplexer; 01605 01606 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) continue; 01607 01608 rfcomm_channel_event_t event = { CH_EVT_READY_TO_SEND }; 01609 rfcomm_channel_state_machine(channel, &event); 01610 } 01611 } 01612 01613 // MARK: RFCOMM BTstack API 01614 01615 void rfcomm_init(void){ 01616 rfcomm_client_cid_generator = 0; 01617 rfcomm_multiplexers = NULL; 01618 rfcomm_services = NULL; 01619 rfcomm_channels = NULL; 01620 } 01621 01622 // register packet handler 01623 void rfcomm_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, 01624 uint16_t channel, uint8_t *packet, uint16_t size)){ 01625 app_packet_handler = handler; 01626 } 01627 01628 // send packet over specific channel 01629 int rfcomm_send_internal(uint16_t rfcomm_cid, uint8_t *data, uint16_t len){ 01630 01631 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 01632 if (!channel){ 01633 log_error("rfcomm_send_internal cid %u doesn't exist!\n", rfcomm_cid); 01634 return 0; 01635 } 01636 01637 if (!channel->credits_outgoing){ 01638 log_info("rfcomm_send_internal cid %u, no rfcomm outgoing credits!\n", rfcomm_cid); 01639 return RFCOMM_NO_OUTGOING_CREDITS; 01640 } 01641 01642 if (!channel->packets_granted){ 01643 log_info("rfcomm_send_internal cid %u, no rfcomm credits granted!\n", rfcomm_cid); 01644 return RFCOMM_NO_OUTGOING_CREDITS; 01645 } 01646 01647 // log_info("rfcomm_send_internal: len %u... outgoing credits %u, l2cap credit %us, granted %u\n", 01648 // len, channel->credits_outgoing, channel->multiplexer->l2cap_credits, channel->packets_granted); 01649 01650 01651 // send might cause l2cap to emit new credits, update counters first 01652 channel->credits_outgoing--; 01653 int packets_granted_decreased = 0; 01654 if (channel->packets_granted) { 01655 channel->packets_granted--; 01656 packets_granted_decreased++; 01657 } 01658 01659 int result = rfcomm_send_uih_data(channel->multiplexer, channel->dlci, data, len); 01660 01661 if (result != 0) { 01662 channel->credits_outgoing++; 01663 channel->packets_granted += packets_granted_decreased; 01664 log_info("rfcomm_send_internal: error %d\n", result); 01665 return result; 01666 } 01667 01668 // log_info("rfcomm_send_internal: now outgoing credits %u, l2cap credit %us, granted %u\n", 01669 // channel->credits_outgoing, channel->multiplexer->l2cap_credits, channel->packets_granted); 01670 01671 rfcomm_hand_out_credits(); 01672 01673 return result; 01674 } 01675 01676 void rfcomm_create_channel2(void * connection, bd_addr_t *addr, uint8_t server_channel, uint8_t incoming_flow_control, uint8_t initial_credits){ 01677 log_info("rfcomm_create_channel_internal to %s, at channel #%02x, flow control %u, init credits %u\n", bd_addr_to_str(*addr), server_channel, 01678 incoming_flow_control, initial_credits); 01679 01680 // create new multiplexer if necessary 01681 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_addr(addr); 01682 if (!multiplexer) { 01683 multiplexer = rfcomm_multiplexer_create_for_addr(addr); 01684 if (!multiplexer){ 01685 rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel); 01686 return; 01687 } 01688 multiplexer->outgoing = 1; 01689 multiplexer->state = RFCOMM_MULTIPLEXER_W4_CONNECT; 01690 } 01691 01692 // prepare channel 01693 rfcomm_channel_t * channel = rfcomm_channel_create(multiplexer, NULL, server_channel); 01694 if (!channel){ 01695 rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel); 01696 return; 01697 } 01698 channel->connection = connection; 01699 channel->incoming_flow_control = incoming_flow_control; 01700 channel->new_credits_incoming = initial_credits; 01701 01702 // start multiplexer setup 01703 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 01704 01705 channel->state = RFCOMM_CHANNEL_W4_MULTIPLEXER; 01706 01707 l2cap_create_channel_internal(connection, rfcomm_packet_handler, *addr, PSM_RFCOMM, l2cap_max_mtu()); 01708 01709 return; 01710 } 01711 01712 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 01713 01714 // start connecting, if multiplexer is already up and running 01715 rfcomm_run(); 01716 } 01717 01718 void rfcomm_create_channel_with_initial_credits_internal(void * connection, bd_addr_t *addr, uint8_t server_channel, uint8_t initial_credits){ 01719 rfcomm_create_channel2(connection, addr, server_channel, 1, initial_credits); 01720 } 01721 01722 void rfcomm_create_channel_internal(void * connection, bd_addr_t *addr, uint8_t server_channel){ 01723 rfcomm_create_channel2(connection, addr, server_channel, 0, 0x30); 01724 } 01725 01726 void rfcomm_disconnect_internal(uint16_t rfcomm_cid){ 01727 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 01728 if (channel) { 01729 channel->state = RFCOMM_CHANNEL_SEND_DISC; 01730 } 01731 01732 // process 01733 rfcomm_run(); 01734 } 01735 01736 01737 void rfcomm_register_service2(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t incoming_flow_control, uint8_t initial_credits){ 01738 // check if already registered 01739 rfcomm_service_t * service = rfcomm_service_for_channel(channel); 01740 if (service){ 01741 rfcomm_emit_service_registered(connection, RFCOMM_CHANNEL_ALREADY_REGISTERED, channel); 01742 return; 01743 } 01744 01745 // alloc structure 01746 service = (rfcomm_service_t*)btstack_memory_rfcomm_service_get(); 01747 if (!service) { 01748 rfcomm_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, channel); 01749 return; 01750 } 01751 01752 // register with l2cap if not registered before, max MTU 01753 if (linked_list_empty(&rfcomm_services)){ 01754 l2cap_register_service_internal(NULL, rfcomm_packet_handler, PSM_RFCOMM, 0xffff); 01755 } 01756 01757 // fill in 01758 service->connection = connection; 01759 service->server_channel = channel; 01760 service->max_frame_size = max_frame_size; 01761 service->incoming_flow_control = incoming_flow_control; 01762 service->incoming_initial_credits = initial_credits; 01763 01764 // add to services list 01765 linked_list_add(&rfcomm_services, (linked_item_t *) service); 01766 01767 // done 01768 rfcomm_emit_service_registered(connection, 0, channel); 01769 } 01770 01771 void rfcomm_register_service_with_initial_credits_internal(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t initial_credits){ 01772 rfcomm_register_service2(connection, channel, max_frame_size, 1, initial_credits); 01773 } 01774 01775 void rfcomm_register_service_internal(void * connection, uint8_t channel, uint16_t max_frame_size){ 01776 rfcomm_register_service2(connection, channel, max_frame_size, 0, 0x30); 01777 } 01778 01779 void rfcomm_unregister_service_internal(uint8_t service_channel){ 01780 rfcomm_service_t *service = rfcomm_service_for_channel(service_channel); 01781 if (!service) return; 01782 linked_list_remove(&rfcomm_services, (linked_item_t *) service); 01783 btstack_memory_rfcomm_service_free(service); 01784 01785 // unregister if no services active 01786 if (linked_list_empty(&rfcomm_services)){ 01787 // bt_send_cmd(&l2cap_unregister_service, PSM_RFCOMM); 01788 l2cap_unregister_service_internal(NULL, PSM_RFCOMM); 01789 } 01790 } 01791 01792 void rfcomm_accept_connection_internal(uint16_t rfcomm_cid){ 01793 log_info("Received Accept Connction\n"); 01794 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 01795 if (!channel) return; 01796 switch (channel->state) { 01797 case RFCOMM_CHANNEL_INCOMING_SETUP: 01798 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED; 01799 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_PN){ 01800 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP; 01801 } 01802 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM){ 01803 channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_UA; 01804 } 01805 break; 01806 default: 01807 break; 01808 } 01809 01810 // process 01811 rfcomm_run(); 01812 } 01813 01814 void rfcomm_decline_connection_internal(uint16_t rfcomm_cid){ 01815 log_info("Received Decline Connction\n"); 01816 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 01817 if (!channel) return; 01818 switch (channel->state) { 01819 case RFCOMM_CHANNEL_INCOMING_SETUP: 01820 channel->state = RFCOMM_CHANNEL_SEND_DM; 01821 break; 01822 default: 01823 break; 01824 } 01825 01826 // process 01827 rfcomm_run(); 01828 } 01829 01830 void rfcomm_grant_credits(uint16_t rfcomm_cid, uint8_t credits){ 01831 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 01832 if (!channel) return; 01833 if (!channel->incoming_flow_control) return; 01834 channel->new_credits_incoming += credits; 01835 01836 // process 01837 rfcomm_run(); 01838 } 01839 01840 // 01841 void rfcomm_close_connection(void *connection){ 01842 linked_item_t *it; 01843 01844 // close open channels 01845 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 01846 rfcomm_channel_t * channel = (rfcomm_channel_t *)it; 01847 if (channel->connection != connection) continue; 01848 channel->state = RFCOMM_CHANNEL_SEND_DISC; 01849 } 01850 01851 // unregister services 01852 it = (linked_item_t *) &rfcomm_services; 01853 while (it->next) { 01854 rfcomm_service_t * service = (rfcomm_service_t *) it->next; 01855 if (service->connection == connection){ 01856 it->next = it->next->next; 01857 btstack_memory_rfcomm_service_free(service); 01858 } else { 01859 it = it->next; 01860 } 01861 } 01862 01863 // process 01864 rfcomm_run(); 01865 } 01866 01867 01868 01869
Generated on Tue Jul 12 2022 15:55:29 by
1.7.2