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