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