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