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