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