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 916 UNUSED(size); // ok: handling own l2cap events 917 918 bd_addr_t event_addr; 919 uint16_t psm; 920 uint16_t l2cap_cid; 921 hci_con_handle_t con_handle; 922 rfcomm_multiplexer_t *multiplexer = NULL; 923 uint8_t status; 924 925 switch (hci_event_packet_get_type(packet)) { 926 927 // accept incoming rfcomm connection if no multiplexer exists yet 928 case L2CAP_EVENT_INCOMING_CONNECTION: 929 // data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) 930 reverse_bd_addr(&packet[2], event_addr); 931 con_handle = little_endian_read_16(packet, 8); 932 psm = little_endian_read_16(packet, 10); 933 l2cap_cid = little_endian_read_16(packet, 12); 934 935 if (psm != BLUETOOTH_PROTOCOL_RFCOMM) break; 936 937 multiplexer = rfcomm_multiplexer_for_addr(event_addr); 938 939 if (multiplexer) { 940 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_RFCOMM => decline - multiplexer already exists", l2cap_cid); 941 l2cap_decline_connection(l2cap_cid); 942 return 1; 943 } 944 945 // create and inititialize new multiplexer instance (incoming) 946 multiplexer = rfcomm_multiplexer_create_for_addr(event_addr); 947 if (!multiplexer){ 948 log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_RFCOMM => decline - no memory left", l2cap_cid); 949 l2cap_decline_connection(l2cap_cid); 950 return 1; 951 } 952 953 multiplexer->con_handle = con_handle; 954 multiplexer->l2cap_cid = l2cap_cid; 955 // 956 multiplexer->state = RFCOMM_MULTIPLEXER_W4_SABM_0; 957 log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for BLUETOOTH_PROTOCOL_RFCOMM => accept", l2cap_cid); 958 l2cap_accept_connection(l2cap_cid); 959 return 1; 960 961 // l2cap connection opened -> store l2cap_cid, remote_addr 962 case L2CAP_EVENT_CHANNEL_OPENED: 963 964 if (little_endian_read_16(packet, 11) != BLUETOOTH_PROTOCOL_RFCOMM) break; 965 966 status = packet[2]; 967 log_info("L2CAP_EVENT_CHANNEL_OPENED for BLUETOOTH_PROTOCOL_RFCOMM, status %u", status); 968 969 // get multiplexer for remote addr 970 con_handle = little_endian_read_16(packet, 9); 971 l2cap_cid = little_endian_read_16(packet, 13); 972 reverse_bd_addr(&packet[3], event_addr); 973 multiplexer = rfcomm_multiplexer_for_addr(event_addr); 974 if (!multiplexer) { 975 log_error("L2CAP_EVENT_CHANNEL_OPENED but no multiplexer prepared"); 976 return 1; 977 } 978 979 // on l2cap open error discard everything 980 if (status){ 981 982 // remove (potential) timer 983 rfcomm_multiplexer_stop_timer(multiplexer); 984 985 // emit rfcomm_channel_opened with status and free channel 986 btstack_linked_item_t * it = (btstack_linked_item_t *) &rfcomm_channels; 987 while (it->next) { 988 rfcomm_channel_t * channel = (rfcomm_channel_t *) it->next; 989 if (channel->multiplexer == multiplexer){ 990 rfcomm_emit_channel_opened(channel, status); 991 it->next = it->next->next; 992 btstack_memory_rfcomm_channel_free(channel); 993 } else { 994 it = it->next; 995 } 996 } 997 998 // free multiplexer 999 rfcomm_multiplexer_free(multiplexer); 1000 return 1; 1001 } 1002 1003 // following could be: rfcom_multiplexer_state_machein(..., EVENT_L2CAP_OPENED) 1004 1005 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_CONNECT) { 1006 log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection"); 1007 // wrong remote addr 1008 if (bd_addr_cmp(event_addr, multiplexer->remote_addr)) break; 1009 multiplexer->l2cap_cid = l2cap_cid; 1010 multiplexer->con_handle = con_handle; 1011 // send SABM #0 1012 rfcomm_multiplexer_set_state_and_request_can_send_now_event(multiplexer, RFCOMM_MULTIPLEXER_SEND_SABM_0); 1013 1014 } else { // multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0 1015 1016 // set max frame size based on l2cap MTU 1017 multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17)); 1018 } 1019 return 1; 1020 1021 // l2cap disconnect -> state = RFCOMM_MULTIPLEXER_CLOSED; 1022 1023 // Notify channel packet handler if they can send now 1024 case L2CAP_EVENT_CAN_SEND_NOW: 1025 l2cap_cid = l2cap_event_can_send_now_get_local_cid(packet); 1026 rfcomm_handle_can_send_now(l2cap_cid); 1027 return 1; 1028 1029 case L2CAP_EVENT_CHANNEL_CLOSED: 1030 // data: event (8), len(8), channel (16) 1031 l2cap_cid = little_endian_read_16(packet, 2); 1032 multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid); 1033 log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, mult %p", l2cap_cid, multiplexer); 1034 if (!multiplexer) break; 1035 log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", multiplexer->state); 1036 // no need to call l2cap_disconnect here, as it's already closed 1037 rfcomm_multiplexer_finalize(multiplexer); 1038 return 1; 1039 1040 default: 1041 break; 1042 } 1043 return 0; 1044 } 1045 1046 static int rfcomm_multiplexer_l2cap_packet_handler(uint16_t channel, uint8_t *packet, uint16_t size){ 1047 // get or create a multiplexer for a certain device 1048 rfcomm_multiplexer_t *multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 1049 if (!multiplexer) return 0; 1050 1051 uint16_t l2cap_cid = multiplexer->l2cap_cid; 1052 1053 // but only care for multiplexer control channel 1054 uint8_t frame_dlci = packet[0] >> 2; 1055 if (frame_dlci) return 0; 1056 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 1057 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 1058 const uint8_t payload_offset = 3 + length_offset + credit_offset; 1059 switch (packet[1]){ 1060 1061 case BT_RFCOMM_SABM: 1062 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0){ 1063 log_info("Received SABM #0"); 1064 multiplexer->outgoing = 0; 1065 rfcomm_multiplexer_set_state_and_request_can_send_now_event(multiplexer, RFCOMM_MULTIPLEXER_SEND_UA_0); 1066 return 1; 1067 } 1068 break; 1069 1070 case BT_RFCOMM_UA: 1071 if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_UA_0) { 1072 // UA #0 -> send UA #0, state = RFCOMM_MULTIPLEXER_OPEN 1073 log_info("Received UA #0 "); 1074 rfcomm_multiplexer_opened(multiplexer); 1075 return 1; 1076 } 1077 break; 1078 1079 case BT_RFCOMM_DISC: 1080 // DISC #0 -> send UA #0, close multiplexer 1081 log_info("Received DISC #0, (ougoing = %u)", multiplexer->outgoing); 1082 rfcomm_multiplexer_set_state_and_request_can_send_now_event(multiplexer, RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC); 1083 return 1; 1084 1085 case BT_RFCOMM_DM: 1086 // DM #0 - we shouldn't get this, just give up 1087 log_info("Received DM #0"); 1088 log_info("-> Closing down multiplexer"); 1089 rfcomm_multiplexer_finalize(multiplexer); 1090 l2cap_disconnect(l2cap_cid, 0x13); 1091 return 1; 1092 1093 case BT_RFCOMM_UIH: 1094 if (packet[payload_offset] == BT_RFCOMM_CLD_CMD){ 1095 // Multiplexer close down (CLD) -> close mutliplexer 1096 log_info("Received Multiplexer close down command"); 1097 log_info("-> Closing down multiplexer"); 1098 rfcomm_multiplexer_finalize(multiplexer); 1099 l2cap_disconnect(l2cap_cid, 0x13); 1100 return 1; 1101 } 1102 switch (packet[payload_offset]){ 1103 case BT_RFCOMM_CLD_CMD: 1104 // Multiplexer close down (CLD) -> close mutliplexer 1105 log_info("Received Multiplexer close down command"); 1106 log_info("-> Closing down multiplexer"); 1107 rfcomm_multiplexer_finalize(multiplexer); 1108 l2cap_disconnect(l2cap_cid, 0x13); 1109 return 1; 1110 1111 case BT_RFCOMM_FCON_CMD: 1112 multiplexer->fcon = 0x81; 1113 l2cap_request_can_send_now_event(multiplexer->l2cap_cid); 1114 return 1; 1115 1116 case BT_RFCOMM_FCOFF_CMD: 1117 multiplexer->fcon = 0x80; 1118 l2cap_request_can_send_now_event(multiplexer->l2cap_cid); 1119 return 1; 1120 1121 case BT_RFCOMM_TEST_CMD: { 1122 log_info("Received test command"); 1123 int len = packet[payload_offset+1] >> 1; // length < 125 1124 if (len > RFCOMM_TEST_DATA_MAX_LEN){ 1125 len = RFCOMM_TEST_DATA_MAX_LEN; 1126 } 1127 len = btstack_min(len, size - 1 - payload_offset); // avoid information leak 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 1413 UNUSED(size); // ok: fixed format messages 1414 1415 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 1416 const uint8_t frame_dlci = packet[0] >> 2; 1417 uint8_t message_dlci; // used by commands in UIH(_PF) packets 1418 uint8_t message_len; // " 1419 1420 // rfcomm: (1) command/control 1421 // -- credits_offset = 1 if command == BT_RFCOMM_UIH_PF 1422 const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames 1423 // rfcomm: (2) length. if bit 0 is cleared, 2 byte length is used. (little endian) 1424 const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3 1425 // rfcomm: (3+length_offset) credits if credits_offset == 1 1426 // rfcomm: (3+length_offest+credits_offset) 1427 const uint8_t payload_offset = 3 + length_offset + credit_offset; 1428 1429 rfcomm_channel_event_t event; 1430 rfcomm_channel_event_pn_t event_pn; 1431 rfcomm_channel_event_rpn_t event_rpn; 1432 rfcomm_channel_event_msc_t event_msc; 1433 1434 // switch by rfcomm message type 1435 switch(packet[1]) { 1436 1437 case BT_RFCOMM_SABM: 1438 event.type = CH_EVT_RCVD_SABM; 1439 log_info("Received SABM #%u", frame_dlci); 1440 rfcomm_channel_state_machine_with_dlci(multiplexer, frame_dlci, &event); 1441 break; 1442 1443 case BT_RFCOMM_UA: 1444 event.type = CH_EVT_RCVD_UA; 1445 log_info("Received UA #%u",frame_dlci); 1446 rfcomm_channel_state_machine_with_dlci(multiplexer, frame_dlci, &event); 1447 break; 1448 1449 case BT_RFCOMM_DISC: 1450 event.type = CH_EVT_RCVD_DISC; 1451 rfcomm_channel_state_machine_with_dlci(multiplexer, frame_dlci, &event); 1452 break; 1453 1454 case BT_RFCOMM_DM: 1455 case BT_RFCOMM_DM_PF: 1456 event.type = CH_EVT_RCVD_DM; 1457 rfcomm_channel_state_machine_with_dlci(multiplexer, frame_dlci, &event); 1458 break; 1459 1460 case BT_RFCOMM_UIH_PF: 1461 case BT_RFCOMM_UIH: 1462 1463 message_len = packet[payload_offset+1] >> 1; 1464 1465 switch (packet[payload_offset]) { 1466 case BT_RFCOMM_PN_CMD: 1467 message_dlci = packet[payload_offset+2]; 1468 event_pn.super.type = CH_EVT_RCVD_PN; 1469 event_pn.priority = packet[payload_offset+4]; 1470 event_pn.max_frame_size = little_endian_read_16(packet, payload_offset+6); 1471 event_pn.credits_outgoing = packet[payload_offset+9]; 1472 log_info("Received UIH Parameter Negotiation Command for #%u, credits %u", 1473 message_dlci, event_pn.credits_outgoing); 1474 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 1475 break; 1476 1477 case BT_RFCOMM_PN_RSP: 1478 message_dlci = packet[payload_offset+2]; 1479 event_pn.super.type = CH_EVT_RCVD_PN_RSP; 1480 event_pn.priority = packet[payload_offset+4]; 1481 event_pn.max_frame_size = little_endian_read_16(packet, payload_offset+6); 1482 event_pn.credits_outgoing = packet[payload_offset+9]; 1483 log_info("Received UIH Parameter Negotiation Response max frame %u, credits %u", 1484 event_pn.max_frame_size, event_pn.credits_outgoing); 1485 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn); 1486 break; 1487 1488 case BT_RFCOMM_MSC_CMD: 1489 message_dlci = packet[payload_offset+2] >> 2; 1490 event_msc.super.type = CH_EVT_RCVD_MSC_CMD; 1491 event_msc.modem_status = packet[payload_offset+3]; 1492 log_info("Received MSC CMD for #%u, ", message_dlci); 1493 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_msc); 1494 break; 1495 1496 case BT_RFCOMM_MSC_RSP: 1497 message_dlci = packet[payload_offset+2] >> 2; 1498 event.type = CH_EVT_RCVD_MSC_RSP; 1499 log_info("Received MSC RSP for #%u", message_dlci); 1500 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, &event); 1501 break; 1502 1503 case BT_RFCOMM_RPN_CMD: 1504 message_dlci = packet[payload_offset+2] >> 2; 1505 switch (message_len){ 1506 case 1: 1507 log_info("Received Remote Port Negotiation Request for #%u", message_dlci); 1508 event.type = CH_EVT_RCVD_RPN_REQ; 1509 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, &event); 1510 break; 1511 case 8: 1512 log_info("Received Remote Port Negotiation Update for #%u", message_dlci); 1513 event_rpn.super.type = CH_EVT_RCVD_RPN_CMD; 1514 event_rpn.data = *(rfcomm_rpn_data_t*) &packet[payload_offset+3]; 1515 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rpn); 1516 break; 1517 default: 1518 break; 1519 } 1520 break; 1521 1522 case BT_RFCOMM_RPN_RSP: 1523 log_info("Received RPN response"); 1524 break; 1525 1526 case BT_RFCOMM_RLS_CMD: { 1527 log_info("Received RLS command"); 1528 message_dlci = packet[payload_offset+2] >> 2; 1529 rfcomm_channel_event_rls_t event_rls; 1530 event_rls.super.type = CH_EVT_RCVD_RLS_CMD; 1531 event_rls.line_status = packet[payload_offset+3]; 1532 rfcomm_channel_state_machine_with_dlci(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rls); 1533 break; 1534 } 1535 1536 case BT_RFCOMM_RLS_RSP: 1537 log_info("Received RLS response"); 1538 break; 1539 1540 // Following commands are handled by rfcomm_multiplexer_l2cap_packet_handler 1541 // case BT_RFCOMM_TEST_CMD: 1542 // case BT_RFCOMM_FCOFF_CMD: 1543 // case BT_RFCOMM_FCON_CMD: 1544 // everything else is an not supported command 1545 default: { 1546 log_error("Received unknown UIH command packet - 0x%02x", packet[payload_offset]); 1547 multiplexer->nsc_command = packet[payload_offset]; 1548 break; 1549 } 1550 } 1551 break; 1552 1553 default: 1554 log_error("Received unknown RFCOMM message type %x", packet[1]); 1555 break; 1556 } 1557 1558 // trigger next action - example W4_PN_RSP: transition to SEND_SABM which only depends on "can send" 1559 if (rfcomm_multiplexer_ready_to_send(multiplexer)){ 1560 l2cap_request_can_send_now_event(multiplexer->l2cap_cid); 1561 } 1562 } 1563 1564 static void rfcomm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 1565 1566 if (packet_type == HCI_EVENT_PACKET){ 1567 rfcomm_hci_event_handler(packet, size); 1568 return; 1569 } 1570 1571 // we only handle l2cap packets for: 1572 if (packet_type != L2CAP_DATA_PACKET) return; 1573 1574 // - multiplexer itself 1575 int handled = rfcomm_multiplexer_l2cap_packet_handler(channel, packet, size); 1576 1577 if (handled) return; 1578 1579 // - channel over open mutliplexer 1580 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel); 1581 if (!multiplexer || multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) return; 1582 1583 // channel data ? 1584 // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1] 1585 const uint8_t frame_dlci = packet[0] >> 2; 1586 1587 if (frame_dlci && (packet[1] == BT_RFCOMM_UIH || packet[1] == BT_RFCOMM_UIH_PF)) { 1588 rfcomm_channel_packet_handler_uih(multiplexer, packet, size); 1589 return; 1590 } 1591 1592 rfcomm_channel_packet_handler(multiplexer, packet, size); 1593 } 1594 1595 static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel){ 1596 // note: exchanging MSC isn't neccessary to consider channel open 1597 // note: having outgoing credits is also not necessary to consider channel open 1598 // 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); 1599 // if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP) == 0) return 0; 1600 // if (channel->credits_outgoing == 0) return 0; 1601 log_info("rfcomm_channel_ready_for_open state %u, flags needed %04x, current %04x, rf credits %u", 1602 channel->state, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP, channel->state_var, channel->credits_outgoing); 1603 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP) == 0) return 0; 1604 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS) == 0) return 0; 1605 1606 return 1; 1607 } 1608 1609 static int rfcomm_channel_ready_for_incoming_dlc_setup(rfcomm_channel_t * channel){ 1610 log_info("rfcomm_channel_ready_for_incoming_dlc_setup state var %04x", channel->state_var); 1611 // Client accept and SABM/UA is required, PN RSP is needed if PN was received 1612 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) == 0) return 0; 1613 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM ) == 0) return 0; 1614 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA ) != 0) return 0; 1615 if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP ) != 0) return 0; 1616 return 1; 1617 } 1618 1619 inline static void rfcomm_channel_state_add(rfcomm_channel_t *channel, RFCOMM_CHANNEL_STATE_VAR event){ 1620 channel->state_var = (RFCOMM_CHANNEL_STATE_VAR) (channel->state_var | event); 1621 } 1622 inline static void rfcomm_channel_state_remove(rfcomm_channel_t *channel, RFCOMM_CHANNEL_STATE_VAR event){ 1623 channel->state_var = (RFCOMM_CHANNEL_STATE_VAR) (channel->state_var & ~event); 1624 } 1625 1626 static int rfcomm_channel_ready_to_send(rfcomm_channel_t * channel){ 1627 switch (channel->state){ 1628 case RFCOMM_CHANNEL_SEND_UIH_PN: 1629 log_debug("ch-ready: state %u", channel->state); 1630 return 1; 1631 case RFCOMM_CHANNEL_SEND_SABM_W4_UA: 1632 log_debug("ch-ready: state %u", channel->state); 1633 return 1; 1634 case RFCOMM_CHANNEL_SEND_UA_AFTER_DISC: 1635 log_debug("ch-ready: state %u", channel->state); 1636 return 1; 1637 case RFCOMM_CHANNEL_SEND_DISC: 1638 log_debug("ch-ready: state %u", channel->state); 1639 return 1; 1640 case RFCOMM_CHANNEL_SEND_DM: 1641 log_debug("ch-ready: state %u", channel->state); 1642 return 1; 1643 case RFCOMM_CHANNEL_OPEN: 1644 if (channel->new_credits_incoming) { 1645 log_debug("ch-ready: channel open & new_credits_incoming") ; 1646 return 1; 1647 } 1648 break; 1649 case RFCOMM_CHANNEL_DLC_SETUP: 1650 if (channel->state_var & ( 1651 RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD | 1652 RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS 1653 )) { 1654 log_debug("ch-ready: channel dlc setup & send msc cmd or send credits") ; 1655 return 1; 1656 } 1657 break; 1658 1659 default: 1660 break; 1661 } 1662 1663 if (channel->state_var & ( 1664 RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP | 1665 RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_INFO | 1666 RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP | 1667 RFCOMM_CHANNEL_STATE_VAR_SEND_UA | 1668 RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP 1669 )){ 1670 log_debug("ch-ready: state %x, state var %x", channel->state, channel->state_var); 1671 return 1; 1672 } 1673 1674 if (channel->rls_line_status != RFCOMM_RLS_STATUS_INVALID) { 1675 log_debug("ch-ready: rls_line_status"); 1676 return 1; 1677 } 1678 1679 return 0; 1680 } 1681 1682 1683 static void rfcomm_channel_state_machine_with_channel(rfcomm_channel_t *channel, const rfcomm_channel_event_t *event){ 1684 1685 // log_info("rfcomm_channel_state_machine_with_channel: state %u, state_var %04x, event %u", channel->state, channel->state_var ,event->type); 1686 1687 rfcomm_multiplexer_t *multiplexer = channel->multiplexer; 1688 1689 // TODO: integrate in common switch 1690 if (event->type == CH_EVT_RCVD_DISC){ 1691 rfcomm_emit_channel_closed(channel); 1692 channel->state = RFCOMM_CHANNEL_SEND_UA_AFTER_DISC; 1693 return; 1694 } 1695 1696 // TODO: integrate in common switch 1697 if (event->type == CH_EVT_RCVD_DM){ 1698 log_info("Received DM message for #%u", channel->dlci); 1699 log_info("-> Closing channel locally for #%u", channel->dlci); 1700 rfcomm_emit_channel_closed(channel); 1701 rfcomm_channel_finalize(channel); 1702 return; 1703 } 1704 1705 // remote port negotiation command - just accept everything for now 1706 // 1707 // "The RPN command can be used before a new DLC is opened and should be used whenever the port settings change." 1708 // "The RPN command is specified as optional in TS 07.10, but it is mandatory to recognize and respond to it in RFCOMM. 1709 // (Although the handling of individual settings are implementation-dependent.)" 1710 // 1711 1712 // TODO: integrate in common switch 1713 if (event->type == CH_EVT_RCVD_RPN_CMD){ 1714 // control port parameters 1715 rfcomm_channel_event_rpn_t *event_rpn = (rfcomm_channel_event_rpn_t*) event; 1716 rfcomm_rpn_data_update(&channel->rpn_data, &event_rpn->data); 1717 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1718 // notify client about new settings 1719 rfcomm_emit_port_configuration(channel); 1720 return; 1721 } 1722 1723 // TODO: integrate in common switch 1724 if (event->type == CH_EVT_RCVD_RPN_REQ){ 1725 // no values got accepted (no values have beens sent) 1726 channel->rpn_data.parameter_mask_0 = 0x00; 1727 channel->rpn_data.parameter_mask_1 = 0x00; 1728 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1729 return; 1730 } 1731 1732 if (event->type == CH_EVT_RCVD_RLS_CMD){ 1733 rfcomm_channel_event_rls_t * event_rls = (rfcomm_channel_event_rls_t*) event; 1734 channel->rls_line_status = event_rls->line_status & 0x0f; 1735 log_info("CH_EVT_RCVD_RLS_CMD setting line status to 0x%0x", channel->rls_line_status); 1736 rfcomm_emit_remote_line_status(channel, event_rls->line_status); 1737 return; 1738 } 1739 1740 // TODO: integrate in common switch 1741 if (event->type == CH_EVT_READY_TO_SEND){ 1742 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP){ 1743 log_info("Sending Remote Port Negotiation RSP for #%u", channel->dlci); 1744 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP); 1745 rfcomm_send_uih_rpn_rsp(multiplexer, channel->dlci, &channel->rpn_data); 1746 return; 1747 } 1748 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){ 1749 log_info("Sending MSC RSP for #%u", channel->dlci); 1750 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1751 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP); 1752 rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 1753 return; 1754 } 1755 if (channel->rls_line_status != RFCOMM_RLS_STATUS_INVALID){ 1756 log_info("Sending RLS RSP 0x%0x", channel->rls_line_status); 1757 uint8_t line_status = channel->rls_line_status; 1758 channel->rls_line_status = RFCOMM_RLS_STATUS_INVALID; 1759 rfcomm_send_uih_rls_rsp(multiplexer, channel->dlci, line_status); 1760 return; 1761 } 1762 } 1763 1764 // emit MSC status to app 1765 if (event->type == CH_EVT_RCVD_MSC_CMD){ 1766 // notify client about new settings 1767 rfcomm_channel_event_msc_t *event_msc = (rfcomm_channel_event_msc_t*) event; 1768 uint8_t modem_status_event[2+1]; 1769 modem_status_event[0] = RFCOMM_EVENT_REMOTE_MODEM_STATUS; 1770 modem_status_event[1] = 1; 1771 modem_status_event[2] = event_msc->modem_status; 1772 (channel->packet_handler)(HCI_EVENT_PACKET, channel->rfcomm_cid, (uint8_t*)&modem_status_event, sizeof(modem_status_event)); 1773 // no return, MSC_CMD will be handled by state machine below 1774 } 1775 1776 rfcomm_channel_event_pn_t * event_pn = (rfcomm_channel_event_pn_t*) event; 1777 1778 switch (channel->state) { 1779 case RFCOMM_CHANNEL_CLOSED: 1780 switch (event->type){ 1781 case CH_EVT_RCVD_SABM: 1782 log_info("-> Inform app"); 1783 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM); 1784 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 1785 rfcomm_emit_connection_request(channel); 1786 break; 1787 case CH_EVT_RCVD_PN: 1788 rfcomm_channel_accept_pn(channel, event_pn); 1789 log_info("-> Inform app"); 1790 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_PN); 1791 channel->state = RFCOMM_CHANNEL_INCOMING_SETUP; 1792 rfcomm_emit_connection_request(channel); 1793 break; 1794 default: 1795 break; 1796 } 1797 break; 1798 1799 case RFCOMM_CHANNEL_INCOMING_SETUP: 1800 switch (event->type){ 1801 case CH_EVT_RCVD_SABM: 1802 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM); 1803 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 1804 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 1805 } 1806 break; 1807 case CH_EVT_RCVD_PN: 1808 rfcomm_channel_accept_pn(channel, event_pn); 1809 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_PN); 1810 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) { 1811 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 1812 } 1813 break; 1814 case CH_EVT_READY_TO_SEND: 1815 // if / else if is used to check for state transition after sending 1816 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP){ 1817 log_info("Sending UIH Parameter Negotiation Respond for #%u", channel->dlci); 1818 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 1819 rfcomm_send_uih_pn_response(multiplexer, channel->dlci, channel->pn_priority, channel->max_frame_size); 1820 } else if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA){ 1821 log_info("Sending UA #%u", channel->dlci); 1822 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 1823 rfcomm_send_ua(multiplexer, channel->dlci); 1824 } 1825 if (rfcomm_channel_ready_for_incoming_dlc_setup(channel)){ 1826 log_info("Incomping setup done, requesting send MSC CMD and send Credits"); 1827 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1828 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1829 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 1830 } 1831 break; 1832 default: 1833 break; 1834 } 1835 break; 1836 1837 case RFCOMM_CHANNEL_W4_MULTIPLEXER: 1838 switch (event->type) { 1839 case CH_EVT_MULTIPLEXER_READY: 1840 log_info("Muliplexer opened, sending UIH PN next"); 1841 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 1842 break; 1843 default: 1844 break; 1845 } 1846 break; 1847 1848 case RFCOMM_CHANNEL_SEND_UIH_PN: 1849 switch (event->type) { 1850 case CH_EVT_READY_TO_SEND: 1851 log_info("Sending UIH Parameter Negotiation Command for #%u (channel 0x%p)", channel->dlci, channel ); 1852 channel->state = RFCOMM_CHANNEL_W4_PN_RSP; 1853 rfcomm_send_uih_pn_command(multiplexer, channel->dlci, channel->max_frame_size); 1854 break; 1855 default: 1856 break; 1857 } 1858 break; 1859 1860 case RFCOMM_CHANNEL_W4_PN_RSP: 1861 switch (event->type){ 1862 case CH_EVT_RCVD_PN_RSP: 1863 // update max frame size 1864 if (channel->max_frame_size > event_pn->max_frame_size) { 1865 channel->max_frame_size = event_pn->max_frame_size; 1866 } 1867 // new credits 1868 channel->credits_outgoing = event_pn->credits_outgoing; 1869 channel->state = RFCOMM_CHANNEL_SEND_SABM_W4_UA; 1870 break; 1871 default: 1872 break; 1873 } 1874 break; 1875 1876 case RFCOMM_CHANNEL_SEND_SABM_W4_UA: 1877 switch (event->type) { 1878 case CH_EVT_READY_TO_SEND: 1879 log_info("Sending SABM #%u", channel->dlci); 1880 channel->state = RFCOMM_CHANNEL_W4_UA; 1881 rfcomm_send_sabm(multiplexer, channel->dlci); 1882 break; 1883 default: 1884 break; 1885 } 1886 break; 1887 1888 case RFCOMM_CHANNEL_W4_UA: 1889 switch (event->type){ 1890 case CH_EVT_RCVD_UA: 1891 channel->state = RFCOMM_CHANNEL_DLC_SETUP; 1892 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1893 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1894 break; 1895 default: 1896 break; 1897 } 1898 break; 1899 1900 case RFCOMM_CHANNEL_DLC_SETUP: 1901 switch (event->type){ 1902 case CH_EVT_RCVD_MSC_CMD: 1903 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_CMD); 1904 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1905 break; 1906 case CH_EVT_RCVD_MSC_RSP: 1907 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP); 1908 break; 1909 1910 case CH_EVT_READY_TO_SEND: 1911 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD){ 1912 log_info("Sending MSC CMD for #%u", channel->dlci); 1913 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD); 1914 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_CMD); 1915 rfcomm_send_uih_msc_cmd(multiplexer, channel->dlci , 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1 1916 break; 1917 } 1918 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS){ 1919 log_info("Providing credits for #%u", channel->dlci); 1920 rfcomm_channel_state_remove(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS); 1921 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS); 1922 1923 if (channel->new_credits_incoming) { 1924 uint8_t new_credits = channel->new_credits_incoming; 1925 channel->new_credits_incoming = 0; 1926 rfcomm_channel_send_credits(channel, new_credits); 1927 } 1928 break; 1929 1930 } 1931 break; 1932 default: 1933 break; 1934 } 1935 // finally done? 1936 if (rfcomm_channel_ready_for_open(channel)){ 1937 channel->state = RFCOMM_CHANNEL_OPEN; 1938 rfcomm_channel_opened(channel); 1939 } 1940 break; 1941 1942 case RFCOMM_CHANNEL_OPEN: 1943 switch (event->type){ 1944 case CH_EVT_RCVD_MSC_CMD: 1945 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP); 1946 break; 1947 case CH_EVT_READY_TO_SEND: 1948 if (channel->new_credits_incoming) { 1949 uint8_t new_credits = channel->new_credits_incoming; 1950 channel->new_credits_incoming = 0; 1951 rfcomm_channel_send_credits(channel, new_credits); 1952 break; 1953 } 1954 break; 1955 case CH_EVT_RCVD_CREDITS: 1956 rfcomm_notify_channel_can_send(); 1957 break; 1958 default: 1959 break; 1960 } 1961 break; 1962 1963 case RFCOMM_CHANNEL_SEND_DM: 1964 switch (event->type) { 1965 case CH_EVT_READY_TO_SEND: 1966 log_info("Sending DM_PF for #%u", channel->dlci); 1967 // don't emit channel closed - channel was never open 1968 channel->state = RFCOMM_CHANNEL_CLOSED; 1969 rfcomm_send_dm_pf(multiplexer, channel->dlci); 1970 rfcomm_channel_finalize(channel); 1971 break; 1972 default: 1973 break; 1974 } 1975 break; 1976 1977 case RFCOMM_CHANNEL_SEND_DISC: 1978 switch (event->type) { 1979 case CH_EVT_READY_TO_SEND: 1980 channel->state = RFCOMM_CHANNEL_W4_UA_AFTER_UA; 1981 rfcomm_send_disc(multiplexer, channel->dlci); 1982 break; 1983 default: 1984 break; 1985 } 1986 break; 1987 1988 case RFCOMM_CHANNEL_W4_UA_AFTER_UA: 1989 switch (event->type){ 1990 case CH_EVT_RCVD_UA: 1991 channel->state = RFCOMM_CHANNEL_CLOSED; 1992 rfcomm_emit_channel_closed(channel); 1993 rfcomm_channel_finalize(channel); 1994 break; 1995 default: 1996 break; 1997 } 1998 break; 1999 2000 case RFCOMM_CHANNEL_SEND_UA_AFTER_DISC: 2001 switch (event->type) { 2002 case CH_EVT_READY_TO_SEND: 2003 log_info("Sending UA after DISC for #%u", channel->dlci); 2004 channel->state = RFCOMM_CHANNEL_CLOSED; 2005 rfcomm_send_ua(multiplexer, channel->dlci); 2006 rfcomm_channel_finalize(channel); 2007 break; 2008 default: 2009 break; 2010 } 2011 break; 2012 2013 default: 2014 break; 2015 } 2016 } 2017 2018 // MARK: RFCOMM BTstack API 2019 2020 void rfcomm_init(void){ 2021 rfcomm_client_cid_generator = 0; 2022 rfcomm_multiplexers = NULL; 2023 rfcomm_services = NULL; 2024 rfcomm_channels = NULL; 2025 rfcomm_security_level = LEVEL_2; 2026 } 2027 2028 void rfcomm_set_required_security_level(gap_security_level_t security_level){ 2029 rfcomm_security_level = security_level; 2030 } 2031 2032 int rfcomm_can_send_packet_now(uint16_t rfcomm_cid){ 2033 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2034 if (!channel){ 2035 log_error("rfcomm_send cid 0x%02x doesn't exist!", rfcomm_cid); 2036 return 0; 2037 } 2038 return rfcomm_channel_can_send(channel); 2039 } 2040 2041 void rfcomm_request_can_send_now_event(uint16_t rfcomm_cid){ 2042 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2043 if (!channel){ 2044 log_error("rfcomm_send cid 0x%02x doesn't exist!", rfcomm_cid); 2045 return; 2046 } 2047 channel->waiting_for_can_send_now = 1; 2048 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2049 } 2050 2051 static int rfcomm_assert_send_valid(rfcomm_channel_t * channel , uint16_t len){ 2052 if (len > channel->max_frame_size){ 2053 log_error("rfcomm_send cid 0x%02x, rfcomm data lenght exceeds MTU!", channel->rfcomm_cid); 2054 return RFCOMM_DATA_LEN_EXCEEDS_MTU; 2055 } 2056 2057 if (!channel->credits_outgoing){ 2058 log_info("rfcomm_send cid 0x%02x, no rfcomm outgoing credits!", channel->rfcomm_cid); 2059 return RFCOMM_NO_OUTGOING_CREDITS; 2060 } 2061 2062 if ((channel->multiplexer->fcon & 1) == 0){ 2063 log_info("rfcomm_send cid 0x%02x, aggregate flow off!", channel->rfcomm_cid); 2064 return RFCOMM_AGGREGATE_FLOW_OFF; 2065 } 2066 return 0; 2067 } 2068 2069 // pre: rfcomm_can_send_packet_now(rfcomm_cid) == true 2070 int rfcomm_reserve_packet_buffer(void){ 2071 return l2cap_reserve_packet_buffer(); 2072 } 2073 2074 void rfcomm_release_packet_buffer(void){ 2075 l2cap_release_packet_buffer(); 2076 } 2077 2078 uint8_t * rfcomm_get_outgoing_buffer(void){ 2079 uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer(); 2080 // address + control + length (16) + no credit field 2081 return &rfcomm_out_buffer[4]; 2082 } 2083 2084 uint16_t rfcomm_get_max_frame_size(uint16_t rfcomm_cid){ 2085 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2086 if (!channel){ 2087 log_error("rfcomm_get_max_frame_size cid 0x%02x doesn't exist!", rfcomm_cid); 2088 return 0; 2089 } 2090 return channel->max_frame_size; 2091 } 2092 2093 int rfcomm_send_prepared(uint16_t rfcomm_cid, uint16_t len){ 2094 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2095 if (!channel){ 2096 log_error("rfcomm_send_prepared cid 0x%02x doesn't exist!", rfcomm_cid); 2097 return 0; 2098 } 2099 2100 int err = rfcomm_assert_send_valid(channel, len); 2101 if (err) return err; 2102 if (!l2cap_can_send_prepared_packet_now(channel->multiplexer->l2cap_cid)){ 2103 log_error("rfcomm_send_prepared: l2cap cannot send now"); 2104 return BTSTACK_ACL_BUFFERS_FULL; 2105 } 2106 2107 // send might cause l2cap to emit new credits, update counters first 2108 if (len){ 2109 channel->credits_outgoing--; 2110 } else { 2111 log_info("sending empty RFCOMM packet for cid %02x", rfcomm_cid); 2112 } 2113 2114 int result = rfcomm_send_uih_prepared(channel->multiplexer, channel->dlci, len); 2115 2116 if (result != 0) { 2117 if (len) { 2118 channel->credits_outgoing++; 2119 } 2120 log_error("rfcomm_send_prepared: error %d", result); 2121 return result; 2122 } 2123 2124 return result; 2125 } 2126 2127 int rfcomm_send(uint16_t rfcomm_cid, uint8_t *data, uint16_t len){ 2128 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2129 if (!channel){ 2130 log_error("cid 0x%02x doesn't exist!", rfcomm_cid); 2131 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 2132 } 2133 2134 int err = rfcomm_assert_send_valid(channel, len); 2135 if (err) return err; 2136 if (!l2cap_can_send_packet_now(channel->multiplexer->l2cap_cid)){ 2137 log_error("rfcomm_send_internal: l2cap cannot send now"); 2138 return BTSTACK_ACL_BUFFERS_FULL; 2139 } 2140 2141 rfcomm_reserve_packet_buffer(); 2142 uint8_t * rfcomm_payload = rfcomm_get_outgoing_buffer(); 2143 memcpy(rfcomm_payload, data, len); 2144 err = rfcomm_send_prepared(rfcomm_cid, len); 2145 if (err){ 2146 rfcomm_release_packet_buffer(); 2147 } 2148 return err; 2149 } 2150 2151 // Sends Local Lnie Status, see LINE_STATUS_.. 2152 int rfcomm_send_local_line_status(uint16_t rfcomm_cid, uint8_t line_status){ 2153 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2154 if (!channel){ 2155 log_error("rfcomm_send_local_line_status cid 0x%02x doesn't exist!", rfcomm_cid); 2156 return 0; 2157 } 2158 return rfcomm_send_uih_rls_cmd(channel->multiplexer, channel->dlci, line_status); 2159 } 2160 2161 // Sned local modem status. see MODEM_STAUS_.. 2162 int rfcomm_send_modem_status(uint16_t rfcomm_cid, uint8_t modem_status){ 2163 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2164 if (!channel){ 2165 log_error("rfcomm_send_modem_status cid 0x%02x doesn't exist!", rfcomm_cid); 2166 return 0; 2167 } 2168 return rfcomm_send_uih_msc_cmd(channel->multiplexer, channel->dlci, modem_status); 2169 } 2170 2171 // Configure remote port 2172 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){ 2173 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2174 if (!channel){ 2175 log_error("rfcomm_send_port_configuration cid 0x%02x doesn't exist!", rfcomm_cid); 2176 return 0; 2177 } 2178 rfcomm_rpn_data_t rpn_data; 2179 rpn_data.baud_rate = baud_rate; 2180 rpn_data.flags = data_bits | (stop_bits << 2) | (parity << 3); 2181 rpn_data.flow_control = flow_control; 2182 rpn_data.xon = 0; 2183 rpn_data.xoff = 0; 2184 rpn_data.parameter_mask_0 = 0x1f; // all but xon/xoff 2185 rpn_data.parameter_mask_1 = 0x3f; // all flow control options 2186 return rfcomm_send_uih_rpn_cmd(channel->multiplexer, channel->dlci, &rpn_data); 2187 } 2188 2189 // Query remote port 2190 int rfcomm_query_port_configuration(uint16_t rfcomm_cid){ 2191 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2192 if (!channel){ 2193 log_error("rfcomm_query_port_configuration cid 0x%02x doesn't exist!", rfcomm_cid); 2194 return 0; 2195 } 2196 return rfcomm_send_uih_rpn_req(channel->multiplexer, channel->dlci); 2197 } 2198 2199 2200 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){ 2201 log_info("RFCOMM_CREATE_CHANNEL addr %s channel #%u init credits %u", bd_addr_to_str(addr), server_channel, initial_credits); 2202 2203 // create new multiplexer if necessary 2204 uint8_t status = 0; 2205 uint8_t dlci = 0; 2206 int new_multiplexer = 0; 2207 rfcomm_channel_t * channel = NULL; 2208 rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_addr(addr); 2209 if (!multiplexer) { 2210 multiplexer = rfcomm_multiplexer_create_for_addr(addr); 2211 if (!multiplexer){ 2212 status = BTSTACK_MEMORY_ALLOC_FAILED; 2213 goto fail; 2214 } 2215 multiplexer->outgoing = 1; 2216 multiplexer->state = RFCOMM_MULTIPLEXER_W4_CONNECT; 2217 new_multiplexer = 1; 2218 } 2219 2220 // check if channel for this remote service already exists 2221 dlci = (server_channel << 1) | (multiplexer->outgoing ^ 1); 2222 channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, dlci); 2223 if (channel){ 2224 status = RFCOMM_CHANNEL_ALREADY_REGISTERED; 2225 goto fail; 2226 } 2227 2228 // prepare channel 2229 channel = rfcomm_channel_create(multiplexer, NULL, server_channel); 2230 if (!channel){ 2231 status = BTSTACK_MEMORY_ALLOC_FAILED; 2232 goto fail; 2233 } 2234 2235 // rfcomm_cid is already assigned by rfcomm_channel_create 2236 channel->incoming_flow_control = incoming_flow_control; 2237 channel->new_credits_incoming = initial_credits; 2238 channel->packet_handler = packet_handler; 2239 2240 // return rfcomm_cid 2241 if (out_rfcomm_cid){ 2242 *out_rfcomm_cid = channel->rfcomm_cid; 2243 } 2244 2245 // start multiplexer setup 2246 if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) { 2247 channel->state = RFCOMM_CHANNEL_W4_MULTIPLEXER; 2248 uint16_t l2cap_cid = 0; 2249 status = l2cap_create_channel(rfcomm_packet_handler, addr, BLUETOOTH_PROTOCOL_RFCOMM, l2cap_max_mtu(), &l2cap_cid); 2250 if (status) goto fail; 2251 multiplexer->l2cap_cid = l2cap_cid; 2252 return 0; 2253 } 2254 2255 channel->state = RFCOMM_CHANNEL_SEND_UIH_PN; 2256 2257 // start connecting, if multiplexer is already up and running 2258 l2cap_request_can_send_now_event(multiplexer->l2cap_cid); 2259 return 0; 2260 2261 fail: 2262 if (new_multiplexer) btstack_memory_rfcomm_multiplexer_free(multiplexer); 2263 if (channel) btstack_memory_rfcomm_channel_free(channel); 2264 return status; 2265 } 2266 2267 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){ 2268 return rfcomm_channel_create_internal(packet_handler, addr, server_channel, 1, initial_credits, out_rfcomm_cid); 2269 } 2270 2271 uint8_t rfcomm_create_channel(btstack_packet_handler_t packet_handler, bd_addr_t addr, uint8_t server_channel, uint16_t * out_rfcomm_cid){ 2272 return rfcomm_channel_create_internal(packet_handler, addr, server_channel, 0, RFCOMM_CREDITS, out_rfcomm_cid); 2273 } 2274 2275 void rfcomm_disconnect(uint16_t rfcomm_cid){ 2276 log_info("RFCOMM_DISCONNECT cid 0x%02x", rfcomm_cid); 2277 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2278 if (!channel) return; 2279 2280 channel->state = RFCOMM_CHANNEL_SEND_DISC; 2281 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2282 } 2283 2284 static uint8_t rfcomm_register_service_internal(btstack_packet_handler_t packet_handler, 2285 uint8_t channel, uint16_t max_frame_size, uint8_t incoming_flow_control, uint8_t initial_credits){ 2286 2287 log_info("RFCOMM_REGISTER_SERVICE channel #%u mtu %u flow_control %u credits %u", 2288 channel, max_frame_size, incoming_flow_control, initial_credits); 2289 2290 // check if already registered 2291 rfcomm_service_t * service = rfcomm_service_for_channel(channel); 2292 if (service){ 2293 return RFCOMM_CHANNEL_ALREADY_REGISTERED; 2294 } 2295 2296 // alloc structure 2297 service = btstack_memory_rfcomm_service_get(); 2298 if (!service) { 2299 return BTSTACK_MEMORY_ALLOC_FAILED; 2300 } 2301 2302 // register with l2cap if not registered before, max MTU 2303 if (btstack_linked_list_empty(&rfcomm_services)){ 2304 l2cap_register_service(rfcomm_packet_handler, BLUETOOTH_PROTOCOL_RFCOMM, 0xffff, rfcomm_security_level); 2305 } 2306 2307 // fill in 2308 service->packet_handler = packet_handler; 2309 service->server_channel = channel; 2310 service->max_frame_size = max_frame_size; 2311 service->incoming_flow_control = incoming_flow_control; 2312 service->incoming_initial_credits = initial_credits; 2313 2314 // add to services list 2315 btstack_linked_list_add(&rfcomm_services, (btstack_linked_item_t *) service); 2316 2317 return 0; 2318 } 2319 2320 uint8_t rfcomm_register_service_with_initial_credits(btstack_packet_handler_t packet_handler, 2321 uint8_t channel, uint16_t max_frame_size, uint8_t initial_credits){ 2322 2323 return rfcomm_register_service_internal(packet_handler, channel, max_frame_size, 1, initial_credits); 2324 } 2325 2326 uint8_t rfcomm_register_service(btstack_packet_handler_t packet_handler, uint8_t channel, 2327 uint16_t max_frame_size){ 2328 2329 return rfcomm_register_service_internal(packet_handler, channel, max_frame_size, 0,RFCOMM_CREDITS); 2330 } 2331 2332 void rfcomm_unregister_service(uint8_t service_channel){ 2333 log_info("RFCOMM_UNREGISTER_SERVICE #%u", service_channel); 2334 rfcomm_service_t *service = rfcomm_service_for_channel(service_channel); 2335 if (!service) return; 2336 btstack_linked_list_remove(&rfcomm_services, (btstack_linked_item_t *) service); 2337 btstack_memory_rfcomm_service_free(service); 2338 2339 // unregister if no services active 2340 if (btstack_linked_list_empty(&rfcomm_services)){ 2341 // bt_send_cmd(&l2cap_unregister_service, BLUETOOTH_PROTOCOL_RFCOMM); 2342 l2cap_unregister_service(BLUETOOTH_PROTOCOL_RFCOMM); 2343 } 2344 } 2345 2346 void rfcomm_accept_connection(uint16_t rfcomm_cid){ 2347 log_info("RFCOMM_ACCEPT_CONNECTION cid 0x%02x", rfcomm_cid); 2348 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2349 if (!channel) return; 2350 switch (channel->state) { 2351 case RFCOMM_CHANNEL_INCOMING_SETUP: 2352 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED); 2353 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_PN){ 2354 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP); 2355 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2356 } 2357 if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM){ 2358 rfcomm_channel_state_add(channel, RFCOMM_CHANNEL_STATE_VAR_SEND_UA); 2359 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2360 } 2361 // at least one of { PN RSP, UA } needs to be sent 2362 // state transistion incoming setup -> dlc setup happens in rfcomm_run after these have been sent 2363 break; 2364 default: 2365 break; 2366 } 2367 2368 } 2369 2370 void rfcomm_decline_connection(uint16_t rfcomm_cid){ 2371 log_info("RFCOMM_DECLINE_CONNECTION cid 0x%02x", rfcomm_cid); 2372 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2373 if (!channel) return; 2374 switch (channel->state) { 2375 case RFCOMM_CHANNEL_INCOMING_SETUP: 2376 channel->state = RFCOMM_CHANNEL_SEND_DM; 2377 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2378 break; 2379 default: 2380 break; 2381 } 2382 } 2383 2384 void rfcomm_grant_credits(uint16_t rfcomm_cid, uint8_t credits){ 2385 log_info("RFCOMM_GRANT_CREDITS cid 0x%02x credits %u", rfcomm_cid, credits); 2386 rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid); 2387 if (!channel) return; 2388 if (!channel->incoming_flow_control) return; 2389 channel->new_credits_incoming += credits; 2390 2391 // process 2392 l2cap_request_can_send_now_event(channel->multiplexer->l2cap_cid); 2393 } 2394 2395 2396 /* 2397 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 2398 */ 2399 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 2400 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 2401 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 2402 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 2403 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 2404 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 2405 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 2406 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 2407 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 2408 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 2409 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 2410 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 2411 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 2412 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 2413 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 2414 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 2415 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 2416 }; 2417 2418 #define CRC8_INIT 0xFF // Initial FCS value 2419 #define CRC8_OK 0xCF // Good final FCS value 2420 /*-----------------------------------------------------------------------------------*/ 2421 static uint8_t crc8(uint8_t *data, uint16_t len) 2422 { 2423 uint16_t count; 2424 uint8_t crc = CRC8_INIT; 2425 for (count = 0; count < len; count++) 2426 crc = crc8table[crc ^ data[count]]; 2427 return crc; 2428 } 2429 2430 /*-----------------------------------------------------------------------------------*/ 2431 uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) 2432 { 2433 uint8_t crc; 2434 2435 crc = crc8(data, len); 2436 2437 crc = crc8table[crc ^ check_sum]; 2438 if (crc == CRC8_OK) 2439 return 0; /* Valid */ 2440 else 2441 return 1; /* Failed */ 2442 2443 } 2444 2445 /*-----------------------------------------------------------------------------------*/ 2446 uint8_t crc8_calc(uint8_t *data, uint16_t len) 2447 { 2448 /* Ones complement */ 2449 return 0xFF - crc8(data, len); 2450 } 2451 2452 2453 2454 2455