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