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