1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 /* 39 * rfcomm.c 40 */ 41 42 #include <stdio.h> 43 #include <stdlib.h> 44 #include <string.h> // memcpy 45 #include <stdint.h> 46 47 #include "hci_cmds.h" 48 #include "utils.h" 49 50 #include "utils.h" 51 #include "btstack_memory.h" 52 #include "hci.h" 53 #include "hci_dump.h" 54 #include "debug.h" 55 #include "classic/rfcomm.h" 56 57 // workaround for missing PRIxPTR on mspgcc (16/20-bit MCU) 58 #ifndef PRIxPTR 59 #if defined(__MSP430X__) && defined(__MSP430X_LARGE__) 60 #define PRIxPTR "lx" 61 #else 62 #define PRIxPTR "x" 63 #endif 64 #endif 65 66 67 // Control field values bit no. 1 2 3 4 PF 6 7 8 68 #define BT_RFCOMM_SABM 0x3F // 1 1 1 1 1 1 0 0 69 #define BT_RFCOMM_UA 0x73 // 1 1 0 0 1 1 1 0 70 #define BT_RFCOMM_DM 0x0F // 1 1 1 1 0 0 0 0 71 #define BT_RFCOMM_DM_PF 0x1F // 1 1 1 1 1 0 0 0 72 #define BT_RFCOMM_DISC 0x53 // 1 1 0 0 1 0 1 0 73 #define BT_RFCOMM_UIH 0xEF // 1 1 1 1 0 1 1 1 74 #define BT_RFCOMM_UIH_PF 0xFF // 1 1 1 1 0 1 1 1 75 76 // Multiplexer message types 77 #define BT_RFCOMM_CLD_CMD 0xC3 78 #define BT_RFCOMM_FCON_CMD 0xA3 79 #define BT_RFCOMM_FCON_RSP 0xA1 80 #define BT_RFCOMM_FCOFF_CMD 0x63 81 #define BT_RFCOMM_FCOFF_RSP 0x61 82 #define BT_RFCOMM_MSC_CMD 0xE3 83 #define BT_RFCOMM_MSC_RSP 0xE1 84 #define BT_RFCOMM_NSC_RSP 0x11 85 #define BT_RFCOMM_PN_CMD 0x83 86 #define BT_RFCOMM_PN_RSP 0x81 87 #define BT_RFCOMM_RLS_CMD 0x53 88 #define BT_RFCOMM_RLS_RSP 0x51 89 #define BT_RFCOMM_RPN_CMD 0x93 90 #define BT_RFCOMM_RPN_RSP 0x91 91 #define BT_RFCOMM_TEST_CMD 0x23 92 #define BT_RFCOMM_TEST_RSP 0x21 93 94 #define RFCOMM_MULIPLEXER_TIMEOUT_MS 60000 95 96 #define RFCOMM_CREDITS 10 97 98 // FCS calc 99 #define BT_RFCOMM_CODE_WORD 0xE0 // pol = x8+x2+x1+1 100 #define BT_RFCOMM_CRC_CHECK_LEN 3 101 #define BT_RFCOMM_UIHCRC_CHECK_LEN 2 102 103 #include "l2cap.h" 104 105 // used for debugging 106 // #define RFCOMM_LOG_CREDITS 107 108 // global rfcomm data 109 static uint16_t rfcomm_client_cid_generator; // used for client channel IDs 110 111 // linked lists for all 112 static linked_list_t rfcomm_multiplexers = NULL; 113 static linked_list_t rfcomm_channels = NULL; 114 static linked_list_t rfcomm_services = NULL; 115 116 static gap_security_level_t rfcomm_security_level; 117 118 static void (*app_packet_handler)(void * connection, uint8_t packet_type, 119 uint16_t channel, uint8_t *packet, uint16_t size); 120 121 static void rfcomm_run(void); 122 static void rfcomm_hand_out_credits(void); 123 static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event); 124 static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event); 125 static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel); 126 static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event); 127 128 129 // MARK: RFCOMM CLIENT EVENTS 130 131 // data: event (8), len(8), address(48), channel (8), rfcomm_cid (16) 132 static void rfcomm_emit_connection_request(rfcomm_channel_t *channel) { 133 log_info("RFCOMM_EVENT_INCOMING_CONNECTION addr %s channel #%u cid 0x%02x", 134 bd_addr_to_str(channel->multiplexer->remote_addr), channel->dlci>>1, channel->rfcomm_cid); 135 uint8_t event[11]; 136 event[0] = RFCOMM_EVENT_INCOMING_CONNECTION; 137 event[1] = sizeof(event) - 2; 138 bt_flip_addr(&event[2], channel->multiplexer->remote_addr); 139 event[8] = channel->dlci >> 1; 140 bt_store_16(event, 9, channel->rfcomm_cid); 141 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 142 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 143 } 144 145 // API Change: BTstack-0.3.50x uses 146 // data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16) 147 // next Cydia release will use SVN version of this 148 // data: event(8), len(8), status (8), address (48), handle (16), server channel(8), rfcomm_cid(16), max frame size(16) 149 static void rfcomm_emit_channel_opened(rfcomm_channel_t *channel, uint8_t status) { 150 log_info("RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE status 0x%x addr %s handle 0x%x channel #%u cid 0x%02x mtu %u", 151 status, bd_addr_to_str(channel->multiplexer->remote_addr), channel->multiplexer->con_handle, 152 channel->dlci>>1, channel->rfcomm_cid, channel->max_frame_size); 153 uint8_t event[16]; 154 uint8_t pos = 0; 155 event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE; // 0 156 event[pos++] = sizeof(event) - 2; // 1 157 event[pos++] = status; // 2 158 bt_flip_addr(&event[pos], channel->multiplexer->remote_addr); pos += 6; // 3 159 bt_store_16(event, pos, channel->multiplexer->con_handle); pos += 2; // 9 160 event[pos++] = channel->dlci >> 1; // 11 161 bt_store_16(event, pos, channel->rfcomm_cid); pos += 2; // 12 - channel ID 162 bt_store_16(event, pos, channel->max_frame_size); pos += 2; // max frame size 163 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 164 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos); 165 } 166 167 static void rfcomm_emit_channel_open_failed_outgoing_memory(void * connection, bd_addr_t addr, uint8_t server_channel){ 168 log_info("RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE BTSTACK_MEMORY_ALLOC_FAILED addr %s", 169 bd_addr_to_str(addr)); 170 uint8_t event[16]; 171 uint8_t pos = 0; 172 event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE; 173 event[pos++] = sizeof(event) - 2; 174 event[pos++] = BTSTACK_MEMORY_ALLOC_FAILED; 175 bt_flip_addr(&event[pos], addr); pos += 6; 176 bt_store_16(event, pos, 0); pos += 2; 177 event[pos++] = server_channel; 178 bt_store_16(event, pos, 0); pos += 2; // channel ID 179 bt_store_16(event, pos, 0); pos += 2; // max frame size 180 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 181 (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos); 182 } 183 184 // data: event(8), len(8), creidts incoming(8), new credits incoming(8), credits outgoing(8) 185 static inline void rfcomm_emit_credit_status(rfcomm_channel_t * channel) { 186 #ifdef RFCOMM_LOG_CREDITS 187 log_info("RFCOMM_LOG_CREDITS incoming %u new_incoming %u outgoing %u", channel->credits_incoming, channel->new_credits_incoming, channel->credits_outgoing); 188 uint8_t event[5]; 189 event[0] = 0x88; 190 event[1] = sizeof(event) - 2; 191 event[2] = channel->credits_incoming; 192 event[3] = channel->new_credits_incoming; 193 event[4] = channel->credits_outgoing; 194 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 195 #endif 196 } 197 198 // data: event(8), len(8), rfcomm_cid(16) 199 static void rfcomm_emit_channel_closed(rfcomm_channel_t * channel) { 200 log_info("RFCOMM_EVENT_CHANNEL_CLOSED cid 0x%02x", channel->rfcomm_cid); 201 uint8_t event[4]; 202 event[0] = RFCOMM_EVENT_CHANNEL_CLOSED; 203 event[1] = sizeof(event) - 2; 204 bt_store_16(event, 2, channel->rfcomm_cid); 205 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 206 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 207 } 208 209 static void rfcomm_emit_credits(rfcomm_channel_t * channel, uint8_t credits) { 210 log_info("RFCOMM_EVENT_CREDITS cid 0x%02x credits %u", channel->rfcomm_cid, credits); 211 uint8_t event[5]; 212 event[0] = RFCOMM_EVENT_CREDITS; 213 event[1] = sizeof(event) - 2; 214 bt_store_16(event, 2, channel->rfcomm_cid); 215 event[4] = credits; 216 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 217 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 218 } 219 220 static void rfcomm_emit_service_registered(void *connection, uint8_t status, uint8_t channel){ 221 log_info("RFCOMM_EVENT_SERVICE_REGISTERED status 0x%x channel #%u", status, channel); 222 uint8_t event[4]; 223 event[0] = RFCOMM_EVENT_SERVICE_REGISTERED; 224 event[1] = sizeof(event) - 2; 225 event[2] = status; 226 event[3] = channel; 227 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 228 (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 229 } 230 231 static void rfcomm_emit_remote_line_status(rfcomm_channel_t *channel, uint8_t line_status){ 232 log_info("RFCOMM_EVENT_REMOTE_LINE_STATUS cid 0x%02x c, line status 0x%x", channel->rfcomm_cid, line_status); 233 uint8_t event[5]; 234 event[0] = RFCOMM_EVENT_REMOTE_LINE_STATUS; 235 event[1] = sizeof(event) - 2; 236 bt_store_16(event, 2, channel->rfcomm_cid); 237 event[4] = line_status; 238 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 239 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); 240 } 241 242 static void rfcomm_emit_port_configuration(rfcomm_channel_t *channel){ 243 // notify client about new settings 244 uint8_t event[2+sizeof(rfcomm_rpn_data_t)]; 245 event[0] = RFCOMM_EVENT_PORT_CONFIGURATION; 246 event[1] = sizeof(rfcomm_rpn_data_t); 247 memcpy(&event[2], (uint8_t*) &channel->rpn_data, sizeof(rfcomm_rpn_data_t)); 248 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 249 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, channel->rfcomm_cid, (uint8_t*)event, sizeof(event)); 250 } 251 252 // MARK RFCOMM RPN DATA HELPER 253 static void rfcomm_rpn_data_set_defaults(rfcomm_rpn_data_t * rpn_data){ 254 rpn_data->baud_rate = RPN_BAUD_9600; /* 9600 bps */ 255 rpn_data->flags = 0x03; /* 8-n-1 */ 256 rpn_data->flow_control = 0; /* no flow control */ 257 rpn_data->xon = 0xd1; /* XON */ 258 rpn_data->xoff = 0xd3; /* XOFF */ 259 rpn_data->parameter_mask_0 = 0x7f; /* parameter mask, all values set */ 260 rpn_data->parameter_mask_1 = 0x3f; /* parameter mask, all values set */ 261 } 262 263 static void rfcomm_rpn_data_update(rfcomm_rpn_data_t * dest, rfcomm_rpn_data_t * src){ 264 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_BAUD){ 265 dest->baud_rate = src->baud_rate; 266 } 267 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_DATA_BITS){ 268 dest->flags = (dest->flags & 0xfc) | (src->flags & 0x03); 269 } 270 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_STOP_BITS){ 271 dest->flags = (dest->flags & 0xfb) | (src->flags & 0x04); 272 } 273 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_PARITY){ 274 dest->flags = (dest->flags & 0xf7) | (src->flags & 0x08); 275 } 276 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_PARITY_TYPE){ 277 dest->flags = (dest->flags & 0xfc) | (src->flags & 0x30); 278 } 279 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_XON_CHAR){ 280 dest->xon = src->xon; 281 } 282 if (src->parameter_mask_0 & RPN_PARAM_MASK_0_XOFF_CHAR){ 283 dest->xoff = src->xoff; 284 } 285 int i; 286 for (i=0; i < 6 ; i++){ 287 uint8_t mask = 1 << i; 288 if (src->parameter_mask_1 & mask){ 289 dest->flags = (dest->flags & ~mask) | (src->flags & mask); 290 } 291 } 292 // always copy parameter mask, too. informative for client, needed for response 293 dest->parameter_mask_0 = src->parameter_mask_0; 294 dest->parameter_mask_1 = src->parameter_mask_1; 295 } 296 // MARK: RFCOMM MULTIPLEXER HELPER 297 298 static uint16_t rfcomm_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){ 299 // Assume RFCOMM header without credits and 2 byte (14 bit) length field 300 uint16_t max_frame_size = l2cap_mtu - 5; 301 log_info("rfcomm_max_frame_size_for_l2cap_mtu: %u -> %u", l2cap_mtu, max_frame_size); 302 return max_frame_size; 303 } 304 305 static void rfcomm_multiplexer_initialize(rfcomm_multiplexer_t *multiplexer){ 306 307 memset(multiplexer, 0, sizeof(rfcomm_multiplexer_t)); 308 309 multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED; 310 multiplexer->l2cap_credits = 0; 311 multiplexer->fcon = 1; 312 multiplexer->send_dm_for_dlci = 0; 313 multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(l2cap_max_mtu()); 314 multiplexer->test_data_len = 0; 315 multiplexer->nsc_command = 0; 316 } 317 318 static rfcomm_multiplexer_t * rfcomm_multiplexer_create_for_addr(bd_addr_t addr){ 319 320 // alloc structure 321 rfcomm_multiplexer_t * multiplexer = btstack_memory_rfcomm_multiplexer_get(); 322 if (!multiplexer) return NULL; 323 324 // fill in 325 rfcomm_multiplexer_initialize(multiplexer); 326 BD_ADDR_COPY(&multiplexer->remote_addr, addr); 327 328 // add to services list 329 linked_list_add(&rfcomm_multiplexers, (linked_item_t *) multiplexer); 330 331 return multiplexer; 332 } 333 334 static rfcomm_multiplexer_t * rfcomm_multiplexer_for_addr(bd_addr_t addr){ 335 linked_item_t *it; 336 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){ 337 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 338 if (BD_ADDR_CMP(addr, multiplexer->remote_addr) == 0) { 339 return multiplexer; 340 }; 341 } 342 return NULL; 343 } 344 345 static rfcomm_multiplexer_t * rfcomm_multiplexer_for_l2cap_cid(uint16_t l2cap_cid) { 346 linked_item_t *it; 347 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){ 348 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 349 if (multiplexer->l2cap_cid == l2cap_cid) { 350 return multiplexer; 351 }; 352 } 353 return NULL; 354 } 355 356 static int rfcomm_multiplexer_has_channels(rfcomm_multiplexer_t * multiplexer){ 357 linked_item_t *it; 358 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 359 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 360 if (channel->multiplexer == multiplexer) { 361 return 1; 362 } 363 } 364 return 0; 365 } 366 367 // MARK: RFCOMM CHANNEL HELPER 368 369 static void rfcomm_dump_channels(void){ 370 #ifndef EMBEDDED 371 linked_item_t * it; 372 int channels = 0; 373 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 374 rfcomm_channel_t * channel = (rfcomm_channel_t *) it; 375 log_info("Channel #%u: addr %p, state %u", channels, channel, channel->state); 376 channels++; 377 } 378 #endif 379 } 380 381 static void rfcomm_channel_initialize(rfcomm_channel_t *channel, rfcomm_multiplexer_t *multiplexer, 382 rfcomm_service_t *service, uint8_t server_channel){ 383 384 // don't use 0 as channel id 385 if (rfcomm_client_cid_generator == 0) ++rfcomm_client_cid_generator; 386 387 // setup channel 388 memset(channel, 0, sizeof(rfcomm_channel_t)); 389 390 channel->state = RFCOMM_CHANNEL_CLOSED; 391 channel->state_var = RFCOMM_CHANNEL_STATE_VAR_NONE; 392 393 channel->multiplexer = multiplexer; 394 channel->service = service; 395 channel->rfcomm_cid = rfcomm_client_cid_generator++; 396 channel->max_frame_size = multiplexer->max_frame_size; 397 398 channel->credits_incoming = 0; 399 channel->credits_outgoing = 0; 400 channel->packets_granted = 0; 401 402 // set defaults for port configuration (even for services) 403 rfcomm_rpn_data_set_defaults(&channel->rpn_data); 404 405 // incoming flow control not active 406 channel->new_credits_incoming =RFCOMM_CREDITS; 407 channel->incoming_flow_control = 0; 408 409 channel->rls_line_status = RFCOMM_RLS_STATUS_INVALID; 410 411 if (service) { 412 // incoming connection 413 channel->outgoing = 0; 414 channel->dlci = (server_channel << 1) | multiplexer->outgoing; 415 if (channel->max_frame_size > service->max_frame_size) { 416 channel->max_frame_size = service->max_frame_size; 417 } 418 channel->incoming_flow_control = service->incoming_flow_control; 419 channel->new_credits_incoming = service->incoming_initial_credits; 420 } else { 421 // outgoing connection 422 channel->outgoing = 1; 423 channel->dlci = (server_channel << 1) | (multiplexer->outgoing ^ 1); 424 425 } 426 } 427 428 // service == NULL -> outgoing channel 429 static rfcomm_channel_t * rfcomm_channel_create(rfcomm_multiplexer_t * multiplexer, 430 rfcomm_service_t * service, uint8_t server_channel){ 431 432 log_info("rfcomm_channel_create for service %p, channel %u --- list of channels:", service, server_channel); 433 rfcomm_dump_channels(); 434 435 // alloc structure 436 rfcomm_channel_t * channel = btstack_memory_rfcomm_channel_get(); 437 if (!channel) return NULL; 438 439 // fill in 440 rfcomm_channel_initialize(channel, multiplexer, service, server_channel); 441 442 // add to services list 443 linked_list_add(&rfcomm_channels, (linked_item_t *) channel); 444 445 return channel; 446 } 447 448 static rfcomm_channel_t * rfcomm_channel_for_rfcomm_cid(uint16_t rfcomm_cid){ 449 linked_item_t *it; 450 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 451 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 452 if (channel->rfcomm_cid == rfcomm_cid) { 453 return channel; 454 }; 455 } 456 return NULL; 457 } 458 459 static rfcomm_channel_t * rfcomm_channel_for_multiplexer_and_dlci(rfcomm_multiplexer_t * multiplexer, uint8_t dlci){ 460 linked_item_t *it; 461 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 462 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 463 if (channel->dlci == dlci && channel->multiplexer == multiplexer) { 464 return channel; 465 }; 466 } 467 return NULL; 468 } 469 470 static rfcomm_service_t * rfcomm_service_for_channel(uint8_t server_channel){ 471 linked_item_t *it; 472 for (it = (linked_item_t *) rfcomm_services; it ; it = it->next){ 473 rfcomm_service_t * service = ((rfcomm_service_t *) it); 474 if ( service->server_channel == server_channel){ 475 return service; 476 }; 477 } 478 return NULL; 479 } 480 481 // MARK: RFCOMM SEND 482 483 /** 484 * @param credits - only used for RFCOMM flow control in UIH wiht P/F = 1 485 */ 486 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){ 487 488 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) return BTSTACK_ACL_BUFFERS_FULL; 489 490 l2cap_reserve_packet_buffer(); 491 uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer(); 492 493 uint16_t pos = 0; 494 uint8_t crc_fields = 3; 495 496 rfcomm_out_buffer[pos++] = address; 497 rfcomm_out_buffer[pos++] = control; 498 499 // length field can be 1 or 2 octets 500 if (len < 128){ 501 rfcomm_out_buffer[pos++] = (len << 1)| 1; // bits 0-6 502 } else { 503 rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6 504 rfcomm_out_buffer[pos++] = len >> 7; // bits 7-14 505 crc_fields++; 506 } 507 508 // add credits for UIH frames when PF bit is set 509 if (control == BT_RFCOMM_UIH_PF){ 510 rfcomm_out_buffer[pos++] = credits; 511 } 512 513 // copy actual data 514 if (len) { 515 memcpy(&rfcomm_out_buffer[pos], data, len); 516 pos += len; 517 } 518 519 // UIH frames only calc FCS over address + control (5.1.1) 520 if ((control & 0xef) == BT_RFCOMM_UIH){ 521 crc_fields = 2; 522 } 523 rfcomm_out_buffer[pos++] = crc8_calc(rfcomm_out_buffer, crc_fields); // calc fcs 524 525 int credits_taken = 0; 526 if (multiplexer->l2cap_credits){ 527 credits_taken++; 528 multiplexer->l2cap_credits--; 529 } else { 530 log_info( "rfcomm_send_packet addr %02x, ctrl %02x size %u without l2cap credits", address, control, pos); 531 } 532 533 int err = l2cap_send_prepared(multiplexer->l2cap_cid, pos); 534 535 if (err) { 536 // undo credit counting 537 multiplexer->l2cap_credits += credits_taken; 538 } 539 return err; 540 } 541 542 // simplified version of rfcomm_send_packet_for_multiplexer for prepared rfcomm packet (UIH, 2 byte len, no credits) 543 static int rfcomm_send_uih_prepared(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint16_t len){ 544 545 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); 546 uint8_t control = BT_RFCOMM_UIH; 547 548 uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer(); 549 550 uint16_t pos = 0; 551 rfcomm_out_buffer[pos++] = address; 552 rfcomm_out_buffer[pos++] = control; 553 rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6 554 rfcomm_out_buffer[pos++] = len >> 7; // bits 7-14 555 556 // actual data is already in place 557 pos += len; 558 559 // UIH frames only calc FCS over address + control (5.1.1) 560 rfcomm_out_buffer[pos++] = crc8_calc(rfcomm_out_buffer, 2); // calc fcs 561 562 int credits_taken = 0; 563 if (multiplexer->l2cap_credits){ 564 credits_taken++; 565 multiplexer->l2cap_credits--; 566 } else { 567 log_info( "rfcomm_send_uih_prepared addr %02x, ctrl %02x size %u without l2cap credits", address, control, pos); 568 } 569 570 int err = l2cap_send_prepared(multiplexer->l2cap_cid, pos); 571 572 if (err) { 573 // undo credit counting 574 multiplexer->l2cap_credits += credits_taken; 575 } 576 return err; 577 } 578 579 // C/R Flag in Address 580 // - terms: initiator = station that creates multiplexer with SABM 581 // - terms: responder = station that responds to multiplexer setup with UA 582 // "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" 583 // - command initiator = 1 /response responder = 1 584 // - command responder = 0 /response initiator = 0 585 // "For UIH frames, the C/R bit is always set according to section 5.4.3.1 in GSM 07.10. 586 // This applies independently of what is contained wthin the UIH frames, either data or control messages." 587 // - c/r = 1 for frames by initiating station, 0 = for frames by responding station 588 589 // C/R Flag in Message 590 // "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." 591 // - If the C/R bit is set to 1 the message is a command 592 // - if it is set to 0 the message is a response. 593 594 // temp/old messge construction 595 596 // new object oriented version 597 static int rfcomm_send_sabm(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 598 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command 599 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_SABM, 0, NULL, 0); 600 } 601 602 static int rfcomm_send_disc(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 603 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command 604 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DISC, 0, NULL, 0); 605 } 606 607 static int rfcomm_send_ua(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 608 uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response 609 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UA, 0, NULL, 0); 610 } 611 612 static int rfcomm_send_dm_pf(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){ 613 uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response 614 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DM_PF, 0, NULL, 0); 615 } 616 617 static int rfcomm_send_uih_fc_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t fcon) { 618 uint8_t address = (1 << 0) | (multiplexer->outgoing<< 1); 619 uint8_t payload[2]; 620 uint8_t pos = 0; 621 payload[pos++] = fcon ? BT_RFCOMM_FCON_RSP : BT_RFCOMM_FCOFF_RSP; 622 payload[pos++] = (0 << 1) | 1; // len 623 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 624 } 625 626 // static int rfcomm_send_uih_test_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t * data, uint16_t len) { 627 // uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 628 // uint8_t payload[2+len]; 629 // uint8_t pos = 0; 630 // payload[pos++] = BT_RFCOMM_TEST_CMD; 631 // payload[pos++] = (len + 1) << 1 | 1; // len 632 // memcpy(&payload[pos], data, len); 633 // pos += len; 634 // return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 635 // } 636 637 static int rfcomm_send_uih_test_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t * data, uint16_t len) { 638 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 639 uint8_t payload[2+RFCOMM_TEST_DATA_MAX_LEN]; 640 uint8_t pos = 0; 641 payload[pos++] = BT_RFCOMM_TEST_RSP; 642 if (len > RFCOMM_TEST_DATA_MAX_LEN) { 643 len = RFCOMM_TEST_DATA_MAX_LEN; 644 } 645 payload[pos++] = (len << 1) | 1; // len 646 memcpy(&payload[pos], data, len); 647 pos += len; 648 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 649 } 650 651 static int rfcomm_send_uih_msc_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) { 652 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 653 uint8_t payload[4]; 654 uint8_t pos = 0; 655 payload[pos++] = BT_RFCOMM_MSC_CMD; 656 payload[pos++] = (2 << 1) | 1; // len 657 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 658 payload[pos++] = signals; 659 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 660 } 661 662 static int rfcomm_send_uih_msc_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) { 663 uint8_t address = (1 << 0) | (multiplexer->outgoing<< 1); 664 uint8_t payload[4]; 665 uint8_t pos = 0; 666 payload[pos++] = BT_RFCOMM_MSC_RSP; 667 payload[pos++] = (2 << 1) | 1; // len 668 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 669 payload[pos++] = signals; 670 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 671 } 672 673 static int rfcomm_send_uih_nsc_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t command) { 674 uint8_t address = (1 << 0) | (multiplexer->outgoing<< 1); 675 uint8_t payload[3]; 676 uint8_t pos = 0; 677 payload[pos++] = BT_RFCOMM_NSC_RSP; 678 payload[pos++] = (1 << 1) | 1; // len 679 payload[pos++] = command; 680 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 681 } 682 683 static int rfcomm_send_uih_pn_command(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint16_t max_frame_size){ 684 uint8_t payload[10]; 685 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 686 uint8_t pos = 0; 687 payload[pos++] = BT_RFCOMM_PN_CMD; 688 payload[pos++] = (8 << 1) | 1; // len 689 payload[pos++] = dlci; 690 payload[pos++] = 0xf0; // pre-defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM 691 payload[pos++] = 0; // priority 692 payload[pos++] = 0; // max 60 seconds ack 693 payload[pos++] = max_frame_size & 0xff; // max framesize low 694 payload[pos++] = max_frame_size >> 8; // max framesize high 695 payload[pos++] = 0x00; // number of retransmissions 696 payload[pos++] = 0x00; // (unused error recovery window) initial number of credits 697 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 698 } 699 700 // "The response may not change the DLCI, the priority, the convergence layer, or the timer value." RFCOMM-tutorial.pdf 701 static int rfcomm_send_uih_pn_response(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, 702 uint8_t priority, uint16_t max_frame_size){ 703 uint8_t payload[10]; 704 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 705 uint8_t pos = 0; 706 payload[pos++] = BT_RFCOMM_PN_RSP; 707 payload[pos++] = (8 << 1) | 1; // len 708 payload[pos++] = dlci; 709 payload[pos++] = 0xe0; // pre defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM 710 payload[pos++] = priority; // priority 711 payload[pos++] = 0; // max 60 seconds ack 712 payload[pos++] = max_frame_size & 0xff; // max framesize low 713 payload[pos++] = max_frame_size >> 8; // max framesize high 714 payload[pos++] = 0x00; // number of retransmissions 715 payload[pos++] = 0x00; // (unused error recovery window) initial number of credits 716 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 717 } 718 719 static int rfcomm_send_uih_rls_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t line_status) { 720 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 721 uint8_t payload[4]; 722 uint8_t pos = 0; 723 payload[pos++] = BT_RFCOMM_RLS_CMD; 724 payload[pos++] = (2 << 1) | 1; // len 725 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 726 payload[pos++] = line_status; 727 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 728 } 729 730 static int rfcomm_send_uih_rls_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t line_status) { 731 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 732 uint8_t payload[4]; 733 uint8_t pos = 0; 734 payload[pos++] = BT_RFCOMM_RLS_RSP; 735 payload[pos++] = (2 << 1) | 1; // len 736 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 737 payload[pos++] = line_status; 738 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 739 } 740 741 static int rfcomm_send_uih_rpn_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, rfcomm_rpn_data_t *rpn_data) { 742 uint8_t payload[10]; 743 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 744 uint8_t pos = 0; 745 payload[pos++] = BT_RFCOMM_RPN_CMD; 746 payload[pos++] = (8 << 1) | 1; // len 747 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 748 payload[pos++] = rpn_data->baud_rate; 749 payload[pos++] = rpn_data->flags; 750 payload[pos++] = rpn_data->flow_control; 751 payload[pos++] = rpn_data->xon; 752 payload[pos++] = rpn_data->xoff; 753 payload[pos++] = rpn_data->parameter_mask_0; 754 payload[pos++] = rpn_data->parameter_mask_1; 755 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 756 } 757 758 static int rfcomm_send_uih_rpn_req(rfcomm_multiplexer_t *multiplexer, uint8_t dlci) { 759 uint8_t payload[3]; 760 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 761 uint8_t pos = 0; 762 payload[pos++] = BT_RFCOMM_RPN_CMD; 763 payload[pos++] = (1 << 1) | 1; // len 764 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 765 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 766 } 767 768 static int rfcomm_send_uih_rpn_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, rfcomm_rpn_data_t *rpn_data) { 769 uint8_t payload[10]; 770 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1); 771 uint8_t pos = 0; 772 payload[pos++] = BT_RFCOMM_RPN_RSP; 773 payload[pos++] = (8 << 1) | 1; // len 774 payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1 775 payload[pos++] = rpn_data->baud_rate; 776 payload[pos++] = rpn_data->flags; 777 payload[pos++] = rpn_data->flow_control; 778 payload[pos++] = rpn_data->xon; 779 payload[pos++] = rpn_data->xoff; 780 payload[pos++] = rpn_data->parameter_mask_0; 781 payload[pos++] = rpn_data->parameter_mask_1; 782 return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos); 783 } 784 785 static void rfcomm_send_uih_credits(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t credits){ 786 uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); 787 rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH_PF, credits, NULL, 0); 788 } 789 790 // MARK: RFCOMM MULTIPLEXER 791 static void rfcomm_multiplexer_stop_timer(rfcomm_multiplexer_t * multiplexer){ 792 if (multiplexer->timer_active) { 793 run_loop_remove_timer(&multiplexer->timer); 794 multiplexer->timer_active = 0; 795 } 796 } 797 static void rfcomm_multiplexer_free(rfcomm_multiplexer_t * multiplexer){ 798 linked_list_remove( &rfcomm_multiplexers, (linked_item_t *) multiplexer); 799 btstack_memory_rfcomm_multiplexer_free(multiplexer); 800 } 801 802 static void rfcomm_multiplexer_finalize(rfcomm_multiplexer_t * multiplexer){ 803 // remove (potential) timer 804 rfcomm_multiplexer_stop_timer(multiplexer); 805 806 // close and remove all channels 807 linked_item_t *it = (linked_item_t *) &rfcomm_channels; 808 while (it->next){ 809 rfcomm_channel_t * channel = (rfcomm_channel_t *) it->next; 810 if (channel->multiplexer == multiplexer) { 811 // emit appropriate events 812 if (channel->state == RFCOMM_CHANNEL_OPEN) { 813 rfcomm_emit_channel_closed(channel); 814 } else { 815 rfcomm_emit_channel_opened(channel, RFCOMM_MULTIPLEXER_STOPPED); 816 } 817 // remove from list 818 it->next = it->next->next; 819 // free channel struct 820 btstack_memory_rfcomm_channel_free(channel); 821 } else { 822 it = it->next; 823 } 824 } 825 826 // remove mutliplexer 827 rfcomm_multiplexer_free(multiplexer); 828 } 829 830 static void rfcomm_multiplexer_timer_handler(timer_source_t *timer){ 831 rfcomm_multiplexer_t * multiplexer = (rfcomm_multiplexer_t *) linked_item_get_user( (linked_item_t *) timer); 832 if (rfcomm_multiplexer_has_channels(multiplexer)) return; 833 834 log_info("rfcomm_multiplexer_timer_handler timeout: shutting down multiplexer! (no channels)"); 835 uint16_t l2cap_cid = multiplexer->l2cap_cid; 836 rfcomm_multiplexer_finalize(multiplexer); 837 l2cap_disconnect_internal(l2cap_cid, 0x13); 838 } 839 840 static void rfcomm_multiplexer_prepare_idle_timer(rfcomm_multiplexer_t * multiplexer){ 841 if (multiplexer->timer_active) { 842 run_loop_remove_timer(&multiplexer->timer); 843 multiplexer->timer_active = 0; 844 } 845 if (rfcomm_multiplexer_has_channels(multiplexer)) return; 846 847 // start idle timer for multiplexer timeout check as there are no rfcomm channels yet 848 run_loop_set_timer(&multiplexer->timer, RFCOMM_MULIPLEXER_TIMEOUT_MS); 849 multiplexer->timer.process = rfcomm_multiplexer_timer_handler; 850 linked_item_set_user((linked_item_t*) &multiplexer->timer, multiplexer); 851 run_loop_add_timer(&multiplexer->timer); 852 multiplexer->timer_active = 1; 853 } 854 855 static void rfcomm_multiplexer_opened(rfcomm_multiplexer_t *multiplexer){ 856 log_info("Multiplexer up and running"); 857 multiplexer->state = RFCOMM_MULTIPLEXER_OPEN; 858 859 rfcomm_channel_event_t event = { CH_EVT_MULTIPLEXER_READY }; 860 861 // transition of channels that wait for multiplexer 862 linked_item_t *it; 863 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 864 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 865 if (channel->multiplexer != multiplexer) continue; 866 rfcomm_channel_state_machine(channel, &event); 867 } 868 869 rfcomm_run(); 870 rfcomm_multiplexer_prepare_idle_timer(multiplexer); 871 } 872 873 874 /** 875 * @return handled packet 876 */ 877 static int rfcomm_multiplexer_hci_event_handler(uint8_t *packet, uint16_t size){ 878 bd_addr_t event_addr; 879 uint16_t psm; 880 uint16_t l2cap_cid; 881 hci_con_handle_t con_handle; 882 rfcomm_multiplexer_t *multiplexer = NULL; 883 uint8_t status; 884 885 switch (packet[0]) { 886 887 // accept incoming PSM_RFCOMM connection if no multiplexer exists yet 888 case L2CAP_EVENT_INCOMING_CONNECTION: 889 // data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) 890 bt_flip_addr(event_addr, &packet[2]); 891 con_handle = READ_BT_16(packet, 8); 892 psm = READ_BT_16(packet, 10); 893 l2cap_cid = READ_BT_16(packet, 12); 894 895 if (psm != PSM_RFCOMM) break; 896 897 multiplexer = rfcomm_multiplexer_for_addr(event_addr); 898 899 if (multiplexer) { 900 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - multiplexer already exists", l2cap_cid); 901 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 902 return 1; 903 } 904 905 // create and inititialize new multiplexer instance (incoming) 906 multiplexer = rfcomm_multiplexer_create_for_addr(event_addr); 907 if (!multiplexer){ 908 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - no memory left", l2cap_cid); 909 l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available 910 return 1; 911 } 912 913 multiplexer->con_handle = con_handle; 914 multiplexer->l2cap_cid = l2cap_cid; 915 multiplexer->state = RFCOMM_MULTIPLEXER_W4_SABM_0; 916 917 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => accept", l2cap_cid); 918 l2cap_accept_connection_internal(l2cap_cid); 919 return 1; 920 921 // l2cap connection opened -> store l2cap_cid, remote_addr 922 case L2CAP_EVENT_CHANNEL_OPENED: 923 924 if (READ_BT_16(packet, 11) != PSM_RFCOMM) break; 925 926 status = packet[2]; 927 log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_RFCOMM, status %u", status); 928 929 // get multiplexer for remote addr 930 con_handle = READ_BT_16(packet, 9); 931 l2cap_cid = READ_BT_16(packet, 13); 932 bt_flip_addr(event_addr, &packet[3]); 933 multiplexer = rfcomm_multiplexer_for_addr(event_addr); 934 if (!multiplexer) { 935 log_error("L2CAP_EVENT_CHANNEL_OPENED but no multiplexer prepared"); 936 return 1; 937 } 938 939 // on l2cap open error discard everything 940 if (status){ 941 942 // remove (potential) timer 943 rfcomm_multiplexer_stop_timer(multiplexer); 944 945 // emit rfcomm_channel_opened with status and free channel 946 linked_item_t * it = (linked_item_t *) &rfcomm_channels; 947 while (it->next) { 948 rfcomm_channel_t * channel = (rfcomm_channel_t *) it->next; 949 if (channel->multiplexer == multiplexer){ 950 rfcomm_emit_channel_opened(channel, status); 951 it->next = it->next->next; 952 btstack_memory_rfcomm_channel_free(channel); 953 } else { 954 it = it->next; 955 } 956 } 957 958 // free multiplexer 959 rfcomm_multiplexer_free(multiplexer); 960 return 1; 961 } 962 963 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_CONNECT) { 964 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 965 // wrong remote addr 966 if (BD_ADDR_CMP(event_addr, multiplexer->remote_addr)) break; 967 multiplexer->l2cap_cid = l2cap_cid; 968 multiplexer->con_handle = con_handle; 969 // send SABM #0 970 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_SABM_0; 971 } else { // multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0 972 973 // set max frame size based on l2cap MTU 974 multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17)); 975 } 976 return 1; 977 978 // l2cap disconnect -> state = RFCOMM_MULTIPLEXER_CLOSED; 979 980 case L2CAP_EVENT_CREDITS: 981 // data: event(8), len(8), local_cid(16), credits(8) 982 l2cap_cid = READ_BT_16(packet, 2); 983 multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid); 984 if (!multiplexer) break; 985 multiplexer->l2cap_credits += packet[4]; 986 987 // log_info("L2CAP_EVENT_CREDITS: %u (now %u)", packet[4], multiplexer->l2cap_credits); 988 989 // new credits, continue with signaling 990 rfcomm_run(); 991 992 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) break; 993 rfcomm_hand_out_credits(); 994 return 1; 995 996 case DAEMON_EVENT_HCI_PACKET_SENT: 997 // testing DMA done code 998 rfcomm_run(); 999 break; 1000 1001 case L2CAP_EVENT_CHANNEL_CLOSED: 1002 // data: event (8), len(8), channel (16) 1003 l2cap_cid = READ_BT_16(packet, 2); 1004 multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid); 1005 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, mult %p", l2cap_cid, multiplexer); 1006 if (!multiplexer) break; 1007 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", multiplexer->state); 1008 switch (multiplexer->state) { 1009 case RFCOMM_MULTIPLEXER_W4_CONNECT: 1010 case RFCOMM_MULTIPLEXER_SEND_SABM_0: 1011 case RFCOMM_MULTIPLEXER_W4_SABM_0: 1012 case RFCOMM_MULTIPLEXER_SEND_UA_0: 1013 case RFCOMM_MULTIPLEXER_W4_UA_0: 1014 case RFCOMM_MULTIPLEXER_OPEN: 1015 // don't call l2cap_disconnect as it's alreay closed 1016 rfcomm_multiplexer_finalize(multiplexer); 1017 return 1; 1018 default: 1019 break; 1020 } 1021 break; 1022 default: 1023 break; 1024 } 1025 return 0; 1026 } 1027 1028 static int rfcomm_multiplexer_l2cap_packet_handler(uint16_t channel, uint8_t *packet, uint16_t size){ 1029 1030 // get or create a multiplexer for a certain device 1031 rfcomm_multiplexer_t *multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 1032 if (!multiplexer) return 0; 1033 1034 uint16_t l2cap_cid = multiplexer->l2cap_cid; 1035 1036 // but only care for multiplexer control channel 1037 uint8_t frame_dlci = packet[0] >> 2; 1038 if (frame_dlci) return 0; 1039 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 1040 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 1041 const uint8_t payload_offset = 3 + length_offset + credit_offset; 1042 switch (packet[1]){ 1043 1044 case BT_RFCOMM_SABM: 1045 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0){ 1046 log_info("Received SABM #0"); 1047 multiplexer->outgoing = 0; 1048 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0; 1049 return 1; 1050 } 1051 break; 1052 1053 case BT_RFCOMM_UA: 1054 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_UA_0) { 1055 // UA #0 -> send UA #0, state = RFCOMM_MULTIPLEXER_OPEN 1056 log_info("Received UA #0 "); 1057 rfcomm_multiplexer_opened(multiplexer); 1058 return 1; 1059 } 1060 break; 1061 1062 case BT_RFCOMM_DISC: 1063 // DISC #0 -> send UA #0, close multiplexer 1064 log_info("Received DISC #0, (ougoing = %u)", multiplexer->outgoing); 1065 multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC; 1066 return 1; 1067 1068 case BT_RFCOMM_DM: 1069 // DM #0 - we shouldn't get this, just give up 1070 log_info("Received DM #0"); 1071 log_info("-> Closing down multiplexer"); 1072 rfcomm_multiplexer_finalize(multiplexer); 1073 l2cap_disconnect_internal(l2cap_cid, 0x13); 1074 return 1; 1075 1076 case BT_RFCOMM_UIH: 1077 if (packet[payload_offset] == BT_RFCOMM_CLD_CMD){ 1078 // Multiplexer close down (CLD) -> close mutliplexer 1079 log_info("Received Multiplexer close down command"); 1080 log_info("-> Closing down multiplexer"); 1081 rfcomm_multiplexer_finalize(multiplexer); 1082 l2cap_disconnect_internal(l2cap_cid, 0x13); 1083 return 1; 1084 } 1085 switch (packet[payload_offset]){ 1086 case BT_RFCOMM_CLD_CMD: 1087 // Multiplexer close down (CLD) -> close mutliplexer 1088 log_info("Received Multiplexer close down command"); 1089 log_info("-> Closing down multiplexer"); 1090 rfcomm_multiplexer_finalize(multiplexer); 1091 l2cap_disconnect_internal(l2cap_cid, 0x13); 1092 return 1; 1093 1094 case BT_RFCOMM_FCON_CMD: 1095 multiplexer->fcon = 0x81; 1096 break; 1097 1098 case BT_RFCOMM_FCOFF_CMD: 1099 multiplexer->fcon = 0x80; 1100 break; 1101 1102 case BT_RFCOMM_TEST_CMD: { 1103 log_info("Received test command"); 1104 int len = packet[payload_offset+1] >> 1; // length < 125 1105 if (len > RFCOMM_TEST_DATA_MAX_LEN){ 1106 len = RFCOMM_TEST_DATA_MAX_LEN; 1107 } 1108 multiplexer->test_data_len = len; 1109 memcpy(multiplexer->test_data, &packet[payload_offset + 2], len); 1110 return 1; 1111 } 1112 default: 1113 break; 1114 } 1115 break; 1116 1117 default: 1118 break; 1119 1120 } 1121 return 0; 1122 } 1123 1124 static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event){ 1125 1126 uint16_t l2cap_cid = multiplexer->l2cap_cid; 1127 1128 // process stored DM responses 1129 if (multiplexer->send_dm_for_dlci){ 1130 uint8_t dlci = multiplexer->send_dm_for_dlci; 1131 multiplexer->send_dm_for_dlci = 0; 1132 rfcomm_send_dm_pf(multiplexer, dlci); 1133 return; 1134 } 1135 1136 if (multiplexer->nsc_command){ 1137 uint8_t command = multiplexer->nsc_command; 1138 multiplexer->nsc_command = 0; 1139 rfcomm_send_uih_nsc_rsp(multiplexer, command); 1140 return; 1141 } 1142 1143 if (multiplexer->fcon & 0x80){ 1144 multiplexer->fcon &= 0x01; 1145 rfcomm_send_uih_fc_rsp(multiplexer, multiplexer->fcon); 1146 if (multiplexer->fcon == 0) return; 1147 // trigger client to send again after sending FCon Response 1148 uint8_t packet_sent_event[] = { DAEMON_EVENT_HCI_PACKET_SENT, 0}; 1149 linked_item_t *it; 1150 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 1151 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 1152 if (channel->multiplexer != multiplexer) continue; 1153 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) packet_sent_event, sizeof(packet_sent_event)); 1154 } 1155 return; 1156 } 1157 1158 switch (multiplexer->state) { 1159 case RFCOMM_MULTIPLEXER_SEND_SABM_0: 1160 switch (event) { 1161 case MULT_EV_READY_TO_SEND: 1162 log_info("Sending SABM #0 - (multi 0x%p)", multiplexer); 1163 multiplexer->state = RFCOMM_MULTIPLEXER_W4_UA_0; 1164 rfcomm_send_sabm(multiplexer, 0); 1165 break; 1166 default: 1167 break; 1168 } 1169 break; 1170 case RFCOMM_MULTIPLEXER_SEND_UA_0: 1171 switch (event) { 1172 case MULT_EV_READY_TO_SEND: 1173 log_info("Sending UA #0"); 1174 multiplexer->state = RFCOMM_MULTIPLEXER_OPEN; 1175 rfcomm_send_ua(multiplexer, 0); 1176 rfcomm_multiplexer_opened(multiplexer); 1177 break; 1178 default: 1179 break; 1180 } 1181 break; 1182 case RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC: 1183 switch (event) { 1184 case MULT_EV_READY_TO_SEND: 1185 // try to detect authentication errors: drop link key if multiplexer closed before first channel got opened 1186 if (!multiplexer->at_least_one_connection){ 1187 log_info("TODO: no connections established - delete link key prophylactically"); 1188 // hci_send_cmd(&hci_delete_stored_link_key, multiplexer->remote_addr); 1189 } 1190 log_info("Sending UA #0"); 1191 log_info("Closing down multiplexer"); 1192 multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED; 1193 rfcomm_send_ua(multiplexer, 0); 1194 rfcomm_multiplexer_finalize(multiplexer); 1195 l2cap_disconnect_internal(l2cap_cid, 0x13); 1196 default: 1197 break; 1198 } 1199 break; 1200 case RFCOMM_MULTIPLEXER_OPEN: 1201 switch (event) { 1202 case MULT_EV_READY_TO_SEND: 1203 // respond to test command 1204 if (multiplexer->test_data_len){ 1205 int len = multiplexer->test_data_len; 1206 log_info("Sending TEST Response with %u bytes", len); 1207 multiplexer->test_data_len = 0; 1208 rfcomm_send_uih_test_rsp(multiplexer, multiplexer->test_data, len); 1209 return; 1210 } 1211 break; 1212 default: 1213 break; 1214 } 1215 break; 1216 default: 1217 break; 1218 } 1219 } 1220 1221 // MARK: RFCOMM CHANNEL 1222 1223 static void rfcomm_hand_out_credits(void){ 1224 linked_item_t * it; 1225 for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){ 1226 rfcomm_channel_t * channel = (rfcomm_channel_t *) it; 1227 if (channel->state != RFCOMM_CHANNEL_OPEN) { 1228 // log_info("RFCOMM_EVENT_CREDITS: multiplexer not open"); 1229 continue; 1230 } 1231 if (channel->packets_granted) { 1232 // log_info("RFCOMM_EVENT_CREDITS: already packets granted"); 1233 continue; 1234 } 1235 if (!channel->credits_outgoing) { 1236 // log_info("RFCOMM_EVENT_CREDITS: no outgoing credits"); 1237 continue; 1238 } 1239 if (!channel->multiplexer->l2cap_credits){ 1240 // log_info("RFCOMM_EVENT_CREDITS: no l2cap credits"); 1241 continue; 1242 } 1243 // channel open, multiplexer has l2cap credits and we didn't hand out credit before -> go! 1244 // log_info("RFCOMM_EVENT_CREDITS: 1"); 1245 channel->packets_granted += 1; 1246 rfcomm_emit_credits(channel, 1); 1247 } 1248 } 1249 1250 static void rfcomm_channel_send_credits(rfcomm_channel_t *channel, uint8_t credits){ 1251 rfcomm_send_uih_credits(channel->multiplexer, channel->dlci, credits); 1252 channel->credits_incoming += credits; 1253 1254 rfcomm_emit_credit_status(channel); 1255 } 1256 1257 static void rfcomm_channel_opened(rfcomm_channel_t *rfChannel){ 1258 1259 log_info("rfcomm_channel_opened!"); 1260 1261 rfChannel->state = RFCOMM_CHANNEL_OPEN; 1262 rfcomm_emit_channel_opened(rfChannel, 0); 1263 rfcomm_emit_port_configuration(rfChannel); 1264 rfcomm_hand_out_credits(); 1265 1266 // remove (potential) timer 1267 rfcomm_multiplexer_t *multiplexer = rfChannel->multiplexer; 1268 if (multiplexer->timer_active) { 1269 run_loop_remove_timer(&multiplexer->timer); 1270 multiplexer->timer_active = 0; 1271 } 1272 // hack for problem detecting authentication failure 1273 multiplexer->at_least_one_connection = 1; 1274 1275 // start next connection request if pending 1276 rfcomm_run(); 1277 } 1278 1279 static void rfcomm_channel_packet_handler_uih(rfcomm_multiplexer_t *multiplexer, uint8_t * packet, uint16_t size){ 1280 const uint8_t frame_dlci = packet[0] >> 2; 1281 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 1282 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 1283 const uint8_t payload_offset = 3 + length_offset + credit_offset; 1284 1285 rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, frame_dlci); 1286 if (!channel) return; 1287 1288 // handle new outgoing credits 1289 if (packet[1] == BT_RFCOMM_UIH_PF) { 1290 1291 // add them 1292 uint16_t new_credits = packet[3+length_offset]; 1293 channel->credits_outgoing += new_credits; 1294 log_info( "RFCOMM data UIH_PF, new credits: %u, now %u", new_credits, channel->credits_outgoing); 1295 1296 // notify channel statemachine 1297 rfcomm_channel_event_t channel_event = { CH_EVT_RCVD_CREDITS }; 1298 rfcomm_channel_state_machine(channel, &channel_event); 1299 } 1300 1301 // contains payload? 1302 if (size - 1 > payload_offset){ 1303 1304 // log_info( "RFCOMM data UIH_PF, size %u, channel %p", size-payload_offset-1, rfChannel->connection); 1305 1306 // decrease incoming credit counter 1307 if (channel->credits_incoming > 0){ 1308 channel->credits_incoming--; 1309 } 1310 1311 // deliver payload 1312 (*app_packet_handler)(channel->connection, RFCOMM_DATA_PACKET, channel->rfcomm_cid, 1313 &packet[payload_offset], size-payload_offset-1); 1314 } 1315 1316 // automatically provide new credits to remote device, if no incoming flow control 1317 if (!channel->incoming_flow_control && channel->credits_incoming < 5){ 1318 channel->new_credits_incoming =RFCOMM_CREDITS; 1319 } 1320 1321 rfcomm_emit_credit_status(channel); 1322 1323 // we received new RFCOMM credits, hand them out if possible 1324 rfcomm_hand_out_credits(); 1325 } 1326 1327 static void rfcomm_channel_accept_pn(rfcomm_channel_t *channel, rfcomm_channel_event_pn_t *event){ 1328 // priority of client request 1329 channel->pn_priority = event->priority; 1330 1331 // new credits 1332 channel->credits_outgoing = event->credits_outgoing; 1333 1334 // negotiate max frame size 1335 if (channel->max_frame_size > channel->multiplexer->max_frame_size) { 1336 channel->max_frame_size = channel->multiplexer->max_frame_size; 1337 } 1338 if (channel->max_frame_size > event->max_frame_size) { 1339 channel->max_frame_size = event->max_frame_size; 1340 } 1341 1342 } 1343 1344 static void rfcomm_channel_finalize(rfcomm_channel_t *channel){ 1345 1346 rfcomm_multiplexer_t *multiplexer = channel->multiplexer; 1347 1348 // remove from list 1349 linked_list_remove( &rfcomm_channels, (linked_item_t *) channel); 1350 1351 // free channel 1352 btstack_memory_rfcomm_channel_free(channel); 1353 1354 // update multiplexer timeout after channel was removed from list 1355 rfcomm_multiplexer_prepare_idle_timer(multiplexer); 1356 } 1357 1358 static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event){ 1359 1360 // TODO: if client max frame size is smaller than RFCOMM_DEFAULT_SIZE, send PN 1361 1362 1363 // lookup existing channel 1364 rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, dlci); 1365 1366 // log_info("rfcomm_channel_state_machine_2 lookup dlci #%u = 0x%08x - event %u", dlci, (int) channel, event->type); 1367 1368 if (channel) { 1369 rfcomm_channel_state_machine(channel, event); 1370 return; 1371 } 1372 1373 // service registered? 1374 rfcomm_service_t * service = rfcomm_service_for_channel(dlci >> 1); 1375 // log_info("rfcomm_channel_state_machine_2 service dlci #%u = 0x%08x", dlci, (int) service); 1376 if (!service) { 1377 // discard request by sending disconnected mode 1378 multiplexer->send_dm_for_dlci = dlci; 1379 return; 1380 } 1381 1382 // create channel for some events 1383 switch (event->type) { 1384 case CH_EVT_RCVD_SABM: 1385 case CH_EVT_RCVD_PN: 1386 case CH_EVT_RCVD_RPN_REQ: 1387 case CH_EVT_RCVD_RPN_CMD: 1388 // setup incoming channel 1389 channel = rfcomm_channel_create(multiplexer, service, dlci >> 1); 1390 if (!channel){ 1391 // discard request by sending disconnected mode 1392 multiplexer->send_dm_for_dlci = dlci; 1393 } 1394 break; 1395 default: 1396 break; 1397 } 1398 1399 if (!channel) { 1400 // discard request by sending disconnected mode 1401 multiplexer->send_dm_for_dlci = dlci; 1402 return; 1403 } 1404 channel->connection = service->connection; 1405 rfcomm_channel_state_machine(channel, event); 1406 } 1407 1408 static void rfcomm_channel_packet_handler(rfcomm_multiplexer_t * multiplexer, uint8_t *packet, uint16_t size){ 1409 1410 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 1411 const uint8_t frame_dlci = packet[0] >> 2; 1412 uint8_t message_dlci; // used by commands in UIH(_PF) packets 1413 uint8_t message_len; // " 1414 1415 // rfcomm: (1) command/control 1416 // -- credits_offset = 1 if command == BT_RFCOMM_UIH_PF 1417 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 1418 // rfcomm: (2) length. if bit 0 is cleared, 2 byte length is used. (little endian) 1419 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 1420 // rfcomm: (3+length_offset) credits if credits_offset == 1 1421 // rfcomm: (3+length_offest+credits_offset) 1422 const uint8_t payload_offset = 3 + length_offset + credit_offset; 1423 1424 rfcomm_channel_event_t event; 1425 rfcomm_channel_event_pn_t event_pn; 1426 rfcomm_channel_event_rpn_t event_rpn; 1427 rfcomm_channel_event_msc_t event_msc; 1428 1429 // switch by rfcomm message type 1430 switch(packet[1]) { 1431 1432 case BT_RFCOMM_SABM: 1433 event.type = CH_EVT_RCVD_SABM; 1434 log_info("Received SABM #%u", frame_dlci); 1435 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 1436 break; 1437 1438 case BT_RFCOMM_UA: 1439 event.type = CH_EVT_RCVD_UA; 1440 log_info("Received UA #%u",frame_dlci); 1441 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 1442 break; 1443 1444 case BT_RFCOMM_DISC: 1445 event.type = CH_EVT_RCVD_DISC; 1446 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 1447 break; 1448 1449 case BT_RFCOMM_DM: 1450 case BT_RFCOMM_DM_PF: 1451 event.type = CH_EVT_RCVD_DM; 1452 rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event); 1453 break; 1454 1455 case BT_RFCOMM_UIH_PF: 1456 case BT_RFCOMM_UIH: 1457 1458 message_len = packet[payload_offset+1] >> 1; 1459 1460 switch (packet[payload_offset]) { 1461 case BT_RFCOMM_PN_CMD: 1462 message_dlci = packet[payload_offset+2]; 1463 event_pn.super.type = CH_EVT_RCVD_PN; 1464 event_pn.priority = packet[payload_offset+4]; 1465 event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6); 1466 event_pn.credits_outgoing = packet[payload_offset+9]; 1467 log_info("Received UIH Parameter Negotiation Command for #%u, credits %u", 1468 message_dlci, event_pn.credits_outgoing); 1469 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 1470 break; 1471 1472 case BT_RFCOMM_PN_RSP: 1473 message_dlci = packet[payload_offset+2]; 1474 event_pn.super.type = CH_EVT_RCVD_PN_RSP; 1475 event_pn.priority = packet[payload_offset+4]; 1476 event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6); 1477 event_pn.credits_outgoing = packet[payload_offset+9]; 1478 log_info("Received UIH Parameter Negotiation Response max frame %u, credits %u", 1479 event_pn.max_frame_size, event_pn.credits_outgoing); 1480 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 1481 break; 1482 1483 case BT_RFCOMM_MSC_CMD: 1484 message_dlci = packet[payload_offset+2] >> 2; 1485 event_msc.super.type = CH_EVT_RCVD_MSC_CMD; 1486 event_msc.modem_status = packet[payload_offset+3]; 1487 log_info("Received MSC CMD for #%u, ", message_dlci); 1488 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_msc); 1489 break; 1490 1491 case BT_RFCOMM_MSC_RSP: 1492 message_dlci = packet[payload_offset+2] >> 2; 1493 event.type = CH_EVT_RCVD_MSC_RSP; 1494 log_info("Received MSC RSP for #%u", message_dlci); 1495 rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event); 1496 break; 1497 1498 case BT_RFCOMM_RPN_CMD: 1499 message_dlci = packet[payload_offset+2] >> 2; 1500 switch (message_len){ 1501 case 1: 1502 log_info("Received Remote Port Negotiation Request for #%u", message_dlci); 1503 event.type = CH_EVT_RCVD_RPN_REQ; 1504 rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event); 1505 break; 1506 case 8: 1507 log_info("Received Remote Port Negotiation Update for #%u", message_dlci); 1508 event_rpn.super.type = CH_EVT_RCVD_RPN_CMD; 1509 event_rpn.data = *(rfcomm_rpn_data_t*) &packet[payload_offset+3]; 1510 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rpn); 1511 break; 1512 default: 1513 break; 1514 } 1515 break; 1516 1517 case BT_RFCOMM_RPN_RSP: 1518 log_info("Received RPN response"); 1519 break; 1520 1521 case BT_RFCOMM_RLS_CMD: { 1522 log_info("Received RLS command"); 1523 message_dlci = packet[payload_offset+2] >> 2; 1524 rfcomm_channel_event_rls_t event_rls; 1525 event_rls.super.type = CH_EVT_RCVD_RLS_CMD; 1526 event_rls.line_status = packet[payload_offset+3]; 1527 rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rls); 1528 break; 1529 } 1530 1531 case BT_RFCOMM_RLS_RSP: 1532 log_info("Received RLS response"); 1533 break; 1534 1535 // Following commands are handled by rfcomm_multiplexer_l2cap_packet_handler 1536 // case BT_RFCOMM_TEST_CMD: 1537 // case BT_RFCOMM_FCOFF_CMD: 1538 // case BT_RFCOMM_FCON_CMD: 1539 // everything else is an not supported command 1540 default: { 1541 log_error("Received unknown UIH command packet - 0x%02x", packet[payload_offset]); 1542 multiplexer->nsc_command = packet[payload_offset]; 1543 break; 1544 } 1545 } 1546 break; 1547 1548 default: 1549 log_error("Received unknown RFCOMM message type %x", packet[1]); 1550 break; 1551 } 1552 1553 // trigger next action - example W4_PN_RSP: transition to SEND_SABM which only depends on "can send" 1554 rfcomm_run(); 1555 } 1556 1557 void rfcomm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 1558 1559 // multiplexer handler 1560 int handled = 0; 1561 switch (packet_type) { 1562 case HCI_EVENT_PACKET: 1563 handled = rfcomm_multiplexer_hci_event_handler(packet, size); 1564 break; 1565 case L2CAP_DATA_PACKET: 1566 handled = rfcomm_multiplexer_l2cap_packet_handler(channel, packet, size); 1567 break; 1568 default: 1569 break; 1570 } 1571 1572 if (handled) { 1573 rfcomm_run(); 1574 return; 1575 } 1576 1577 // we only handle l2cap packet over open multiplexer channel now 1578 if (packet_type != L2CAP_DATA_PACKET) { 1579 (*app_packet_handler)(NULL, packet_type, channel, packet, size); 1580 return; 1581 } 1582 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 1583 if (!multiplexer || multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 1584 (*app_packet_handler)(NULL, packet_type, channel, packet, size); 1585 return; 1586 } 1587 1588 // channel data ? 1589 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 1590 const uint8_t frame_dlci = packet[0] >> 2; 1591 1592 if (frame_dlci && (packet[1] == BT_RFCOMM_UIH || packet[1] == BT_RFCOMM_UIH_PF)) { 1593 rfcomm_channel_packet_handler_uih(multiplexer, packet, size); 1594 rfcomm_run(); 1595 return; 1596 } 1597 1598 rfcomm_channel_packet_handler(multiplexer, packet, size); 1599 } 1600 1601 static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel){ 1602 // note: exchanging MSC isn't neccessary to consider channel open 1603 // note: having outgoing credits is also not necessary to consider channel open 1604 // log_info("rfcomm_channel_ready_for_open state %u, flags needed %04x, current %04x, rf credits %u, l2cap credits %u ", 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); 1605 // if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP) == 0) return 0; 1606 // if (channel->credits_outgoing == 0) return 0; 1607 log_info("rfcomm_channel_ready_for_open state %u, flags needed %04x, current %04x, rf credits %u, l2cap credits %u ", channel->state, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP, channel->state_var, channel->credits_outgoing, channel->multiplexer->l2cap_credits); 1608 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP) == 0) return 0; 1609 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS) == 0) return 0; 1610 1611 return 1; 1612 } 1613 1614 static int rfcomm_channel_ready_for_incoming_dlc_setup(rfcomm_channel_t * channel){ 1615 log_info("rfcomm_channel_ready_for_incoming_dlc_setup state var %04x", channel->state_var); 1616 // Client accept and SABM/UA is required, PN RSP is needed if PN was received 1617 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) == 0) return 0; 1618 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM ) == 0) return 0; 1619 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA ) != 0) return 0; 1620 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP ) != 0) return 0; 1621 return 1; 1622 } 1623 1624 inline static void rfcomm_channel_state_add(rfcomm_channel_t *channel, RFCOMM_CHANNEL_STATE_VAR event){ 1625 channel->state_var = (RFCOMM_CHANNEL_STATE_VAR) (channel->state_var | event); 1626 } 1627 inline static void rfcomm_channel_state_remove(rfcomm_channel_t *channel, RFCOMM_CHANNEL_STATE_VAR event){ 1628 channel->state_var = (RFCOMM_CHANNEL_STATE_VAR) (channel->state_var & ~event); 1629 } 1630 1631 static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event){ 1632 1633 // log_info("rfcomm_channel_state_machine: state %u, state_var %04x, event %u", channel->state, channel->state_var ,event->type); 1634 1635 rfcomm_multiplexer_t *multiplexer = channel->multiplexer; 1636 1637 // TODO: integrate in common switch 1638 if (event->type == CH_EVT_RCVD_DISC){ 1639 rfcomm_emit_channel_closed(channel); 1640 channel->state = RFCOMM_CHANNEL_SEND_UA_AFTER_DISC; 1641 return; 1642 } 1643 1644 // TODO: integrate in common switch 1645 if (event->type == CH_EVT_RCVD_DM){ 1646 log_info("Received DM message for #%u", channel->dlci); 1647 log_info("-> Closing channel locally for #%u", channel->dlci); 1648 rfcomm_emit_channel_closed(channel); 1649 rfcomm_channel_finalize(channel); 1650 return; 1651 } 1652 1653 // remote port negotiation command - just accept everything for now 1654 // 1655 // "The RPN command can be used before a new DLC is opened and should be used whenever the port settings change." 1656 // "The RPN command is specified as optional in TS 07.10, but it is mandatory to recognize and respond to it in RFCOMM. 1657 // (Although the handling of individual settings are implementation-dependent.)" 1658 // 1659 1660 // TODO: integrate in common switch 1661 if (event->type == CH_EVT_RCVD_RPN_CMD){ 1662 // control port parameters 1663 rfcomm_channel_event_rpn_t *event_rpn = (rfcomm_channel_event_rpn_t*) event; 1664 rfcomm_rpn_data_update(&channel->rpn_data, &event_rpn->data); 1665 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1666 // notify client about new settings 1667 rfcomm_emit_port_configuration(channel); 1668 return; 1669 } 1670 1671 // TODO: integrate in common switch 1672 if (event->type == CH_EVT_RCVD_RPN_REQ){ 1673 // no values got accepted (no values have beens sent) 1674 channel->rpn_data.parameter_mask_0 = 0x00; 1675 channel->rpn_data.parameter_mask_1 = 0x00; 1676 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1677 return; 1678 } 1679 1680 if (event->type == CH_EVT_RCVD_RLS_CMD){ 1681 rfcomm_channel_event_rls_t * event_rls = (rfcomm_channel_event_rls_t*) event; 1682 channel->rls_line_status = event_rls->line_status & 0x0f; 1683 log_info("CH_EVT_RCVD_RLS_CMD setting line status to 0x%0x", channel->rls_line_status); 1684 rfcomm_emit_remote_line_status(channel, event_rls->line_status); 1685 return; 1686 } 1687 1688 // TODO: integrate in common swich 1689 if (event->type == CH_EVT_READY_TO_SEND){ 1690 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP){ 1691 log_info("Sending Remote Port Negotiation RSP for #%u", channel->dlci); 1692 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1693 rfcomm_send_uih_rpn_rsp(multiplexer, channel->dlci, &channel->rpn_data); 1694 return; 1695 } 1696 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){ 1697 log_info("Sending MSC RSP for #%u", channel->dlci); 1698 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1699 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP); 1700 rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 1701 return; 1702 } 1703 if (channel->rls_line_status != RFCOMM_RLS_STATUS_INVALID){ 1704 log_info("Sending RLS RSP 0x%0x", channel->rls_line_status); 1705 uint8_t line_status = channel->rls_line_status; 1706 channel->rls_line_status = RFCOMM_RLS_STATUS_INVALID; 1707 rfcomm_send_uih_rls_rsp(multiplexer, channel->dlci, line_status); 1708 return; 1709 } 1710 } 1711 1712 // emit MSC status to app 1713 if (event->type == CH_EVT_RCVD_MSC_CMD){ 1714 // notify client about new settings 1715 rfcomm_channel_event_msc_t *event_msc = (rfcomm_channel_event_msc_t*) event; 1716 uint8_t modem_status_event[2+1]; 1717 modem_status_event[0] = RFCOMM_EVENT_REMOTE_MODEM_STATUS; 1718 modem_status_event[1] = 1; 1719 modem_status_event[2] = event_msc->modem_status; 1720 (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, channel->rfcomm_cid, (uint8_t*)&modem_status_event, sizeof(modem_status_event)); 1721 // no return, MSC_CMD will be handled by state machine below 1722 } 1723 1724 rfcomm_channel_event_pn_t * event_pn = (rfcomm_channel_event_pn_t*) event; 1725 1726 switch (channel->state) { 1727 case RFCOMM_CHANNEL_CLOSED: 1728 switch (event->type){ 1729 case CH_EVT_RCVD_SABM: 1730 log_info("-> Inform app"); 1731 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM); 1732 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 1733 rfcomm_emit_connection_request(channel); 1734 break; 1735 case CH_EVT_RCVD_PN: 1736 rfcomm_channel_accept_pn(channel, event_pn); 1737 log_info("-> Inform app"); 1738 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_PN); 1739 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 1740 rfcomm_emit_connection_request(channel); 1741 break; 1742 default: 1743 break; 1744 } 1745 break; 1746 1747 case RFCOMM_CHANNEL_INCOMING_SETUP: 1748 switch (event->type){ 1749 case CH_EVT_RCVD_SABM: 1750 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM); 1751 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 1752 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 1753 } 1754 break; 1755 case CH_EVT_RCVD_PN: 1756 rfcomm_channel_accept_pn(channel, event_pn); 1757 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_PN); 1758 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 1759 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 1760 } 1761 break; 1762 case CH_EVT_READY_TO_SEND: 1763 // if / else if is used to check for state transition after sending 1764 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP){ 1765 log_info("Sending UIH Parameter Negotiation Respond for #%u", channel->dlci); 1766 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 1767 rfcomm_send_uih_pn_response(multiplexer, channel->dlci, channel->pn_priority, channel->max_frame_size); 1768 } else if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA){ 1769 log_info("Sending UA #%u", channel->dlci); 1770 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 1771 rfcomm_send_ua(multiplexer, channel->dlci); 1772 } 1773 if (rfcomm_channel_ready_for_incoming_dlc_setup(channel)){ 1774 log_info("Incomping setup done, requesting send MSC CMD and send Credits"); 1775 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1776 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1777 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 1778 } 1779 break; 1780 default: 1781 break; 1782 } 1783 break; 1784 1785 case RFCOMM_CHANNEL_W4_MULTIPLEXER: 1786 switch (event->type) { 1787 case CH_EVT_MULTIPLEXER_READY: 1788 log_info("Muliplexer opened, sending UIH PN next"); 1789 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 1790 break; 1791 default: 1792 break; 1793 } 1794 break; 1795 1796 case RFCOMM_CHANNEL_SEND_UIH_PN: 1797 switch (event->type) { 1798 case CH_EVT_READY_TO_SEND: 1799 log_info("Sending UIH Parameter Negotiation Command for #%u (channel 0x%p)", channel->dlci, channel ); 1800 channel->state = RFCOMM_CHANNEL_W4_PN_RSP; 1801 rfcomm_send_uih_pn_command(multiplexer, channel->dlci, channel->max_frame_size); 1802 break; 1803 default: 1804 break; 1805 } 1806 break; 1807 1808 case RFCOMM_CHANNEL_W4_PN_RSP: 1809 switch (event->type){ 1810 case CH_EVT_RCVD_PN_RSP: 1811 // update max frame size 1812 if (channel->max_frame_size > event_pn->max_frame_size) { 1813 channel->max_frame_size = event_pn->max_frame_size; 1814 } 1815 // new credits 1816 channel->credits_outgoing = event_pn->credits_outgoing; 1817 channel->state = RFCOMM_CHANNEL_SEND_SABM_W4_UA; 1818 break; 1819 default: 1820 break; 1821 } 1822 break; 1823 1824 case RFCOMM_CHANNEL_SEND_SABM_W4_UA: 1825 switch (event->type) { 1826 case CH_EVT_READY_TO_SEND: 1827 log_info("Sending SABM #%u", channel->dlci); 1828 channel->state = RFCOMM_CHANNEL_W4_UA; 1829 rfcomm_send_sabm(multiplexer, channel->dlci); 1830 break; 1831 default: 1832 break; 1833 } 1834 break; 1835 1836 case RFCOMM_CHANNEL_W4_UA: 1837 switch (event->type){ 1838 case CH_EVT_RCVD_UA: 1839 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 1840 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1841 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1842 break; 1843 default: 1844 break; 1845 } 1846 break; 1847 1848 case RFCOMM_CHANNEL_DLC_SETUP: 1849 switch (event->type){ 1850 case CH_EVT_RCVD_MSC_CMD: 1851 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_CMD); 1852 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1853 break; 1854 case CH_EVT_RCVD_MSC_RSP: 1855 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP); 1856 break; 1857 1858 case CH_EVT_READY_TO_SEND: 1859 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD){ 1860 log_info("Sending MSC CMD for #%u", channel->dlci); 1861 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1862 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_CMD); 1863 rfcomm_send_uih_msc_cmd(multiplexer, channel->dlci , 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 1864 break; 1865 } 1866 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS){ 1867 log_info("Providing credits for #%u", channel->dlci); 1868 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1869 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS); 1870 1871 if (channel->new_credits_incoming) { 1872 uint8_t new_credits = channel->new_credits_incoming; 1873 channel->new_credits_incoming = 0; 1874 rfcomm_channel_send_credits(channel, new_credits); 1875 } 1876 break; 1877 1878 } 1879 break; 1880 default: 1881 break; 1882 } 1883 // finally done? 1884 if (rfcomm_channel_ready_for_open(channel)){ 1885 channel->state = RFCOMM_CHANNEL_OPEN; 1886 rfcomm_channel_opened(channel); 1887 } 1888 break; 1889 1890 case RFCOMM_CHANNEL_OPEN: 1891 switch (event->type){ 1892 case CH_EVT_RCVD_MSC_CMD: 1893 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1894 break; 1895 case CH_EVT_READY_TO_SEND: 1896 if (channel->new_credits_incoming) { 1897 uint8_t new_credits = channel->new_credits_incoming; 1898 channel->new_credits_incoming = 0; 1899 rfcomm_channel_send_credits(channel, new_credits); 1900 break; 1901 } 1902 break; 1903 case CH_EVT_RCVD_CREDITS: { 1904 // notify daemon -> might trigger re-try of parked connections 1905 uint8_t credits_event[2] = { DAEMON_EVENT_NEW_RFCOMM_CREDITS, 0 }; 1906 (*app_packet_handler)(channel->connection, DAEMON_EVENT_PACKET, channel->rfcomm_cid, credits_event, sizeof(credits_event)); 1907 break; 1908 } 1909 default: 1910 break; 1911 } 1912 break; 1913 1914 case RFCOMM_CHANNEL_SEND_DM: 1915 switch (event->type) { 1916 case CH_EVT_READY_TO_SEND: 1917 log_info("Sending DM_PF for #%u", channel->dlci); 1918 // don't emit channel closed - channel was never open 1919 channel->state = RFCOMM_CHANNEL_CLOSED; 1920 rfcomm_send_dm_pf(multiplexer, channel->dlci); 1921 rfcomm_channel_finalize(channel); 1922 break; 1923 default: 1924 break; 1925 } 1926 break; 1927 1928 case RFCOMM_CHANNEL_SEND_DISC: 1929 switch (event->type) { 1930 case CH_EVT_READY_TO_SEND: 1931 channel->state = RFCOMM_CHANNEL_W4_UA_AFTER_UA; 1932 rfcomm_send_disc(multiplexer, channel->dlci); 1933 break; 1934 default: 1935 break; 1936 } 1937 break; 1938 1939 case RFCOMM_CHANNEL_W4_UA_AFTER_UA: 1940 switch (event->type){ 1941 case CH_EVT_RCVD_UA: 1942 channel->state = RFCOMM_CHANNEL_CLOSED; 1943 rfcomm_emit_channel_closed(channel); 1944 rfcomm_channel_finalize(channel); 1945 break; 1946 default: 1947 break; 1948 } 1949 break; 1950 1951 case RFCOMM_CHANNEL_SEND_UA_AFTER_DISC: 1952 switch (event->type) { 1953 case CH_EVT_READY_TO_SEND: 1954 log_info("Sending UA after DISC for #%u", channel->dlci); 1955 channel->state = RFCOMM_CHANNEL_CLOSED; 1956 rfcomm_send_ua(multiplexer, channel->dlci); 1957 rfcomm_channel_finalize(channel); 1958 break; 1959 default: 1960 break; 1961 } 1962 break; 1963 1964 default: 1965 break; 1966 } 1967 } 1968 1969 1970 // MARK: RFCOMM RUN 1971 // process outstanding signaling tasks 1972 static void rfcomm_run(void){ 1973 1974 linked_item_t *it; 1975 linked_item_t *next; 1976 1977 for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = next){ 1978 1979 next = it->next; // be prepared for removal of channel in state machine 1980 1981 rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it); 1982 1983 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) { 1984 // log_info("rfcomm_run A cannot send l2cap packet for #%u, credits %u", multiplexer->l2cap_cid, multiplexer->l2cap_credits); 1985 continue; 1986 } 1987 // log_info("rfcomm_run: multi 0x%08x, state %u", (int) multiplexer, multiplexer->state); 1988 1989 rfcomm_multiplexer_state_machine(multiplexer, MULT_EV_READY_TO_SEND); 1990 } 1991 1992 for (it = (linked_item_t *) rfcomm_channels; it ; it = next){ 1993 1994 next = it->next; // be prepared for removal of channel in state machine 1995 1996 rfcomm_channel_t * channel = ((rfcomm_channel_t *) it); 1997 rfcomm_multiplexer_t * multiplexer = channel->multiplexer; 1998 1999 if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) { 2000 // log_info("rfcomm_run B cannot send l2cap packet for #%u, credits %u", multiplexer->l2cap_cid, multiplexer->l2cap_credits); 2001 continue; 2002 } 2003 2004 rfcomm_channel_event_t event = { CH_EVT_READY_TO_SEND }; 2005 rfcomm_channel_state_machine(channel, &event); 2006 } 2007 } 2008 2009 // MARK: RFCOMM BTstack API 2010 2011 void rfcomm_init(void){ 2012 rfcomm_client_cid_generator = 0; 2013 rfcomm_multiplexers = NULL; 2014 rfcomm_services = NULL; 2015 rfcomm_channels = NULL; 2016 rfcomm_security_level = LEVEL_2; 2017 } 2018 2019 void rfcomm_set_required_security_level(gap_security_level_t security_level){ 2020 rfcomm_security_level = security_level; 2021 } 2022 2023 // register packet handler 2024 void rfcomm_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, 2025 uint16_t channel, uint8_t *packet, uint16_t size)){ 2026 app_packet_handler = handler; 2027 } 2028 2029 int rfcomm_can_send_packet_now(uint16_t rfcomm_cid){ 2030 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2031 if (!channel){ 2032 log_error("rfcomm_send_internal cid 0x%02x doesn't exist!", rfcomm_cid); 2033 return 1; 2034 } 2035 if (!channel->credits_outgoing) return 0; 2036 if (!channel->packets_granted) return 0; 2037 if ((channel->multiplexer->fcon & 1) == 0) return 0; 2038 2039 return l2cap_can_send_packet_now(channel->multiplexer->l2cap_cid); 2040 } 2041 2042 static int rfcomm_assert_send_valid(rfcomm_channel_t * channel , uint16_t len){ 2043 if (len > channel->max_frame_size){ 2044 log_error("rfcomm_send_internal cid 0x%02x, rfcomm data lenght exceeds MTU!", channel->rfcomm_cid); 2045 return RFCOMM_DATA_LEN_EXCEEDS_MTU; 2046 } 2047 2048 if (!channel->credits_outgoing){ 2049 log_info("rfcomm_send_internal cid 0x%02x, no rfcomm outgoing credits!", channel->rfcomm_cid); 2050 return RFCOMM_NO_OUTGOING_CREDITS; 2051 } 2052 2053 if (!channel->packets_granted){ 2054 log_info("rfcomm_send_internal cid 0x%02x, no rfcomm credits granted!", channel->rfcomm_cid); 2055 return RFCOMM_NO_OUTGOING_CREDITS; 2056 } 2057 2058 if ((channel->multiplexer->fcon & 1) == 0){ 2059 log_info("rfcomm_send_internal cid 0x%02x, aggregate flow off!", channel->rfcomm_cid); 2060 return RFCOMM_AGGREGATE_FLOW_OFF; 2061 } 2062 return 0; 2063 } 2064 2065 // pre: rfcomm_can_send_packet_now(rfcomm_cid) == true 2066 int rfcomm_reserve_packet_buffer(void){ 2067 return l2cap_reserve_packet_buffer(); 2068 } 2069 2070 void rfcomm_release_packet_buffer(void){ 2071 l2cap_release_packet_buffer(); 2072 } 2073 2074 uint8_t * rfcomm_get_outgoing_buffer(void){ 2075 uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer(); 2076 // address + control + length (16) + no credit field 2077 return &rfcomm_out_buffer[4]; 2078 } 2079 2080 uint16_t rfcomm_get_max_frame_size(uint16_t rfcomm_cid){ 2081 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2082 if (!channel){ 2083 log_error("rfcomm_get_max_frame_size cid 0x%02x doesn't exist!", rfcomm_cid); 2084 return 0; 2085 } 2086 return channel->max_frame_size; 2087 } 2088 int rfcomm_send_prepared(uint16_t rfcomm_cid, uint16_t len){ 2089 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2090 if (!channel){ 2091 log_error("rfcomm_send_prepared cid 0x%02x doesn't exist!", rfcomm_cid); 2092 return 0; 2093 } 2094 2095 int err = rfcomm_assert_send_valid(channel, len); 2096 if (err) return err; 2097 2098 // send might cause l2cap to emit new credits, update counters first 2099 channel->credits_outgoing--; 2100 int packets_granted_decreased = 0; 2101 if (channel->packets_granted) { 2102 channel->packets_granted--; 2103 packets_granted_decreased++; 2104 } 2105 2106 int result = rfcomm_send_uih_prepared(channel->multiplexer, channel->dlci, len); 2107 2108 if (result != 0) { 2109 channel->credits_outgoing++; 2110 channel->packets_granted += packets_granted_decreased; 2111 log_info("rfcomm_send_internal: error %d", result); 2112 return result; 2113 } 2114 2115 rfcomm_hand_out_credits(); 2116 2117 return result; 2118 } 2119 2120 int rfcomm_send_internal(uint16_t rfcomm_cid, uint8_t *data, uint16_t len){ 2121 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2122 if (!channel){ 2123 log_error("rfcomm_send_internal cid 0x%02x doesn't exist!", rfcomm_cid); 2124 return 1; 2125 } 2126 2127 int err = rfcomm_assert_send_valid(channel, len); 2128 if (err) return err; 2129 2130 rfcomm_reserve_packet_buffer(); 2131 uint8_t * rfcomm_payload = rfcomm_get_outgoing_buffer(); 2132 memcpy(rfcomm_payload, data, len); 2133 return rfcomm_send_prepared(rfcomm_cid, len); 2134 } 2135 2136 // Sends Local Lnie Status, see LINE_STATUS_.. 2137 int rfcomm_send_local_line_status(uint16_t rfcomm_cid, uint8_t line_status){ 2138 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2139 if (!channel){ 2140 log_error("rfcomm_send_local_line_status cid 0x%02x doesn't exist!", rfcomm_cid); 2141 return 0; 2142 } 2143 return rfcomm_send_uih_rls_cmd(channel->multiplexer, channel->dlci, line_status); 2144 } 2145 2146 // Sned local modem status. see MODEM_STAUS_.. 2147 int rfcomm_send_modem_status(uint16_t rfcomm_cid, uint8_t modem_status){ 2148 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2149 if (!channel){ 2150 log_error("rfcomm_send_modem_status cid 0x%02x doesn't exist!", rfcomm_cid); 2151 return 0; 2152 } 2153 return rfcomm_send_uih_msc_cmd(channel->multiplexer, channel->dlci, modem_status); 2154 } 2155 2156 // Configure remote port 2157 int rfcomm_send_port_configuration(uint16_t rfcomm_cid, rpn_baud_t baud_rate, rpn_data_bits_t data_bits, rpn_stop_bits_t stop_bits, rpn_parity_t parity, rpn_flow_control_t flow_control){ 2158 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2159 if (!channel){ 2160 log_error("rfcomm_send_port_configuration cid 0x%02x doesn't exist!", rfcomm_cid); 2161 return 0; 2162 } 2163 rfcomm_rpn_data_t rpn_data; 2164 rpn_data.baud_rate = baud_rate; 2165 rpn_data.flags = data_bits | (stop_bits << 2) | (parity << 3); 2166 rpn_data.flow_control = flow_control; 2167 rpn_data.xon = 0; 2168 rpn_data.xoff = 0; 2169 rpn_data.parameter_mask_0 = 0x1f; // all but xon/xoff 2170 rpn_data.parameter_mask_1 = 0x3f; // all flow control options 2171 return rfcomm_send_uih_rpn_cmd(channel->multiplexer, channel->dlci, &rpn_data); 2172 } 2173 2174 // Query remote port 2175 int rfcomm_query_port_configuration(uint16_t rfcomm_cid){ 2176 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2177 if (!channel){ 2178 log_error("rfcomm_query_port_configuration cid 0x%02x doesn't exist!", rfcomm_cid); 2179 return 0; 2180 } 2181 return rfcomm_send_uih_rpn_req(channel->multiplexer, channel->dlci); 2182 } 2183 2184 2185 static void rfcomm_create_channel2(void * connection, bd_addr_t addr, uint8_t server_channel, uint8_t incoming_flow_control, uint8_t initial_credits){ 2186 log_info("RFCOMM_CREATE_CHANNEL addr %s channel #%u flow control %u init credits %u", bd_addr_to_str(addr), server_channel, 2187 incoming_flow_control, initial_credits); 2188 2189 // create new multiplexer if necessary 2190 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_addr(addr); 2191 if (!multiplexer) { 2192 multiplexer = rfcomm_multiplexer_create_for_addr(addr); 2193 if (!multiplexer){ 2194 rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel); 2195 return; 2196 } 2197 multiplexer->outgoing = 1; 2198 multiplexer->state = RFCOMM_MULTIPLEXER_W4_CONNECT; 2199 } 2200 2201 // prepare channel 2202 rfcomm_channel_t * channel = rfcomm_channel_create(multiplexer, NULL, server_channel); 2203 if (!channel){ 2204 rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel); 2205 return; 2206 } 2207 channel->connection = connection; 2208 channel->incoming_flow_control = incoming_flow_control; 2209 channel->new_credits_incoming = initial_credits; 2210 2211 // start multiplexer setup 2212 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 2213 2214 channel->state = RFCOMM_CHANNEL_W4_MULTIPLEXER; 2215 2216 l2cap_create_channel_internal(connection, rfcomm_packet_handler, addr, PSM_RFCOMM, l2cap_max_mtu()); 2217 2218 return; 2219 } 2220 2221 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 2222 2223 // start connecting, if multiplexer is already up and running 2224 rfcomm_run(); 2225 } 2226 2227 void rfcomm_create_channel_with_initial_credits_internal(void * connection, bd_addr_t addr, uint8_t server_channel, uint8_t initial_credits){ 2228 rfcomm_create_channel2(connection, addr, server_channel, 1, initial_credits); 2229 } 2230 2231 void rfcomm_create_channel_internal(void * connection, bd_addr_t addr, uint8_t server_channel){ 2232 rfcomm_create_channel2(connection, addr, server_channel, 0,RFCOMM_CREDITS); 2233 } 2234 2235 2236 static uint8_t rfcomm_create_channel3(bd_addr_t addr, uint8_t server_channel, uint8_t incoming_flow_control, uint8_t initial_credits, uint16_t * out_rfcomm_cid){ 2237 log_info("RFCOMM_CREATE_CHANNEL addr %s channel #%u init credits %u", bd_addr_to_str(addr), server_channel, initial_credits); 2238 2239 // create new multiplexer if necessary 2240 uint8_t status = 0; 2241 int new_multiplexer = 0; 2242 rfcomm_channel_t * channel = NULL; 2243 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_addr(addr); 2244 if (!multiplexer) { 2245 multiplexer = rfcomm_multiplexer_create_for_addr(addr); 2246 if (!multiplexer){ 2247 status = BTSTACK_MEMORY_ALLOC_FAILED; 2248 goto fail; 2249 } 2250 multiplexer->outgoing = 1; 2251 multiplexer->state = RFCOMM_MULTIPLEXER_W4_CONNECT; 2252 new_multiplexer = 1; 2253 } 2254 2255 // prepare channel 2256 channel = rfcomm_channel_create(multiplexer, NULL, server_channel); 2257 if (!channel){ 2258 status = BTSTACK_MEMORY_ALLOC_FAILED; 2259 goto fail; 2260 } 2261 // rfcomm_cid is already assigned by rfcomm_channel_create 2262 channel->incoming_flow_control = incoming_flow_control; 2263 channel->new_credits_incoming = initial_credits; 2264 2265 // return rfcomm_cid 2266 *out_rfcomm_cid = channel->rfcomm_cid; 2267 2268 // start multiplexer setup 2269 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 2270 channel->state = RFCOMM_CHANNEL_W4_MULTIPLEXER; 2271 uint16_t l2cap_cid = 0; 2272 status = l2cap_create_channel(rfcomm_packet_handler, addr, PSM_RFCOMM, l2cap_max_mtu(), &l2cap_cid); 2273 if (status) goto fail; 2274 multiplexer->l2cap_cid = l2cap_cid; 2275 return 0; 2276 } 2277 2278 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 2279 2280 // start connecting, if multiplexer is already up and running 2281 rfcomm_run(); 2282 return 0; 2283 2284 fail: 2285 if (new_multiplexer) btstack_memory_rfcomm_multiplexer_free(multiplexer); 2286 if (channel) btstack_memory_rfcomm_channel_free(channel); 2287 return status; 2288 } 2289 2290 uint8_t rfcomm_create_channel_with_initial_credits(bd_addr_t addr, uint8_t server_channel, uint8_t initial_credits, uint16_t * out_rfcomm_cid){ 2291 return rfcomm_create_channel3(addr, server_channel, 1, initial_credits, out_rfcomm_cid); 2292 } 2293 2294 uint8_t rfcomm_create_channel(bd_addr_t addr, uint8_t server_channel, uint16_t * out_rfcomm_cid){ 2295 return rfcomm_create_channel3(addr, server_channel, 0, RFCOMM_CREDITS, out_rfcomm_cid); 2296 } 2297 2298 void rfcomm_disconnect_internal(uint16_t rfcomm_cid){ 2299 log_info("RFCOMM_DISCONNECT cid 0x%02x", rfcomm_cid); 2300 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2301 if (channel) { 2302 channel->state = RFCOMM_CHANNEL_SEND_DISC; 2303 } 2304 2305 // process 2306 rfcomm_run(); 2307 } 2308 2309 static void rfcomm_register_service2(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t incoming_flow_control, uint8_t initial_credits){ 2310 log_info("RFCOMM_REGISTER_SERVICE channel #%u mtu %u flow_control %u credits %u", 2311 channel, max_frame_size, incoming_flow_control, initial_credits); 2312 // check if already registered 2313 rfcomm_service_t * service = rfcomm_service_for_channel(channel); 2314 if (service){ 2315 rfcomm_emit_service_registered(connection, RFCOMM_CHANNEL_ALREADY_REGISTERED, channel); 2316 return; 2317 } 2318 2319 // alloc structure 2320 service = btstack_memory_rfcomm_service_get(); 2321 if (!service) { 2322 rfcomm_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, channel); 2323 return; 2324 } 2325 2326 // register with l2cap if not registered before, max MTU 2327 if (linked_list_empty(&rfcomm_services)){ 2328 l2cap_register_service_internal(NULL, rfcomm_packet_handler, PSM_RFCOMM, 0xffff, rfcomm_security_level); 2329 } 2330 2331 // fill in 2332 service->connection = connection; 2333 service->server_channel = channel; 2334 service->max_frame_size = max_frame_size; 2335 service->incoming_flow_control = incoming_flow_control; 2336 service->incoming_initial_credits = initial_credits; 2337 2338 // add to services list 2339 linked_list_add(&rfcomm_services, (linked_item_t *) service); 2340 2341 // done 2342 rfcomm_emit_service_registered(connection, 0, channel); 2343 } 2344 2345 void rfcomm_register_service_with_initial_credits_internal(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t initial_credits){ 2346 rfcomm_register_service2(connection, channel, max_frame_size, 1, initial_credits); 2347 } 2348 2349 void rfcomm_register_service_internal(void * connection, uint8_t channel, uint16_t max_frame_size){ 2350 rfcomm_register_service2(connection, channel, max_frame_size, 0,RFCOMM_CREDITS); 2351 } 2352 2353 void rfcomm_unregister_service_internal(uint8_t service_channel){ 2354 log_info("RFCOMM_UNREGISTER_SERVICE #%u", service_channel); 2355 rfcomm_service_t *service = rfcomm_service_for_channel(service_channel); 2356 if (!service) return; 2357 linked_list_remove(&rfcomm_services, (linked_item_t *) service); 2358 btstack_memory_rfcomm_service_free(service); 2359 2360 // unregister if no services active 2361 if (linked_list_empty(&rfcomm_services)){ 2362 // bt_send_cmd(&l2cap_unregister_service, PSM_RFCOMM); 2363 l2cap_unregister_service_internal(NULL, PSM_RFCOMM); 2364 } 2365 } 2366 2367 void rfcomm_accept_connection_internal(uint16_t rfcomm_cid){ 2368 log_info("RFCOMM_ACCEPT_CONNECTION cid 0x%02x", rfcomm_cid); 2369 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2370 if (!channel) return; 2371 switch (channel->state) { 2372 case RFCOMM_CHANNEL_INCOMING_SETUP: 2373 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED); 2374 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_PN){ 2375 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 2376 } 2377 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM){ 2378 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 2379 } 2380 // at least one of { PN RSP, UA } needs to be sent 2381 // state transistion incoming setup -> dlc setup happens in rfcomm_run after these have been sent 2382 break; 2383 default: 2384 break; 2385 } 2386 2387 // process 2388 rfcomm_run(); 2389 } 2390 2391 void rfcomm_decline_connection_internal(uint16_t rfcomm_cid){ 2392 log_info("RFCOMM_DECLINE_CONNECTION cid 0x%02x", rfcomm_cid); 2393 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2394 if (!channel) return; 2395 switch (channel->state) { 2396 case RFCOMM_CHANNEL_INCOMING_SETUP: 2397 channel->state = RFCOMM_CHANNEL_SEND_DM; 2398 break; 2399 default: 2400 break; 2401 } 2402 2403 // process 2404 rfcomm_run(); 2405 } 2406 2407 void rfcomm_grant_credits(uint16_t rfcomm_cid, uint8_t credits){ 2408 log_info("RFCOMM_GRANT_CREDITS cid 0x%02x credits %u", rfcomm_cid, credits); 2409 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2410 if (!channel) return; 2411 if (!channel->incoming_flow_control) return; 2412 channel->new_credits_incoming += credits; 2413 2414 // process 2415 rfcomm_run(); 2416 } 2417 2418 2419 2420 2421