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