Junichi Katsu
うおーるぼっとをWiiリモコンでコントロールする新しいプログラムです。 以前のものより、Wiiリモコンが早く繋がる様になりました。 It is a program which controls A with the Wii remote. ※ A Bluetooth dongle and a Wii remote control are needed.
Dependencies: USBHost mbed FATFileSystem mbed-rtos
Diff: btstack/rfcomm.c
- Revision:
- 0:fccb789424fc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/btstack/rfcomm.c Mon Jun 10 16:01:50 2013 +0000
@@ -0,0 +1,1869 @@
+/*
+ * Copyright (C) 2009-2012 by Matthias Ringwald
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ * personal benefit and not for any commercial purpose or for
+ * monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
+ * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at btstack@ringwald.ch
+ *
+ */
+
+/*
+ * rfcomm.c
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h> // memcpy
+#include <stdint.h>
+
+#include <btstack/btstack.h>
+#include <btstack/hci_cmds.h>
+#include <btstack/utils.h>
+
+#include <btstack/utils.h>
+#include "btstack_memory.h"
+#include "hci.h"
+#include "hci_dump.h"
+#include "debug.h"
+#include "rfcomm.h"
+
+// workaround for missing PRIxPTR on mspgcc (16/20-bit MCU)
+#ifndef PRIxPTR
+#if defined(__MSP430X__) && defined(__MSP430X_LARGE__)
+#define PRIxPTR "lx"
+#else
+#define PRIxPTR "x"
+#endif
+#endif
+
+
+// Control field values bit no. 1 2 3 4 PF 6 7 8
+#define BT_RFCOMM_SABM 0x3F // 1 1 1 1 1 1 0 0
+#define BT_RFCOMM_UA 0x73 // 1 1 0 0 1 1 1 0
+#define BT_RFCOMM_DM 0x0F // 1 1 1 1 0 0 0 0
+#define BT_RFCOMM_DM_PF 0x1F // 1 1 1 1 1 0 0 0
+#define BT_RFCOMM_DISC 0x53 // 1 1 0 0 1 0 1 0
+#define BT_RFCOMM_UIH 0xEF // 1 1 1 1 0 1 1 1
+#define BT_RFCOMM_UIH_PF 0xFF // 1 1 1 1 0 1 1 1
+
+// Multiplexer message types
+#define BT_RFCOMM_CLD_CMD 0xC3
+#define BT_RFCOMM_FCON_CMD 0xA3
+#define BT_RFCOMM_FCON_RSP 0xA1
+#define BT_RFCOMM_FCOFF_CMD 0x63
+#define BT_RFCOMM_FCOFF_RSP 0x61
+#define BT_RFCOMM_MSC_CMD 0xE3
+#define BT_RFCOMM_MSC_RSP 0xE1
+#define BT_RFCOMM_NSC_RSP 0x11
+#define BT_RFCOMM_PN_CMD 0x83
+#define BT_RFCOMM_PN_RSP 0x81
+#define BT_RFCOMM_RLS_CMD 0x53
+#define BT_RFCOMM_RLS_RSP 0x51
+#define BT_RFCOMM_RPN_CMD 0x93
+#define BT_RFCOMM_RPN_RSP 0x91
+#define BT_RFCOMM_TEST_CMD 0x23
+#define BT_RFCOMM_TEST_RSP 0x21
+
+#define RFCOMM_MULIPLEXER_TIMEOUT_MS 60000
+
+// FCS calc
+#define BT_RFCOMM_CODE_WORD 0xE0 // pol = x8+x2+x1+1
+#define BT_RFCOMM_CRC_CHECK_LEN 3
+#define BT_RFCOMM_UIHCRC_CHECK_LEN 2
+
+#include "l2cap.h"
+
+// used for debugging
+// #define RFCOMM_LOG_CREDITS
+
+// global rfcomm data
+static uint16_t rfcomm_client_cid_generator; // used for client channel IDs
+
+// linked lists for all
+static linked_list_t rfcomm_multiplexers = NULL;
+static linked_list_t rfcomm_channels = NULL;
+static linked_list_t rfcomm_services = NULL;
+
+static void (*app_packet_handler)(void * connection, uint8_t packet_type,
+ uint16_t channel, uint8_t *packet, uint16_t size);
+
+static void rfcomm_run(void);
+static void rfcomm_hand_out_credits(void);
+static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event);
+static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event);
+static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel);
+static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event);
+
+
+// MARK: RFCOMM CLIENT EVENTS
+
+// data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
+static void rfcomm_emit_connection_request(rfcomm_channel_t *channel) {
+ uint8_t event[11];
+ event[0] = RFCOMM_EVENT_INCOMING_CONNECTION;
+ event[1] = sizeof(event) - 2;
+ bt_flip_addr(&event[2], channel->multiplexer->remote_addr);
+ event[8] = channel->dlci >> 1;
+ bt_store_16(event, 9, channel->rfcomm_cid);
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
+}
+
+// API Change: BTstack-0.3.50x uses
+// data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
+// next Cydia release will use SVN version of this
+// data: event(8), len(8), status (8), address (48), handle (16), server channel(8), rfcomm_cid(16), max frame size(16)
+static void rfcomm_emit_channel_opened(rfcomm_channel_t *channel, uint8_t status) {
+ uint8_t event[16];
+ uint8_t pos = 0;
+ event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE;
+ event[pos++] = sizeof(event) - 2;
+ event[pos++] = status;
+ bt_flip_addr(&event[pos], channel->multiplexer->remote_addr); pos += 6;
+ bt_store_16(event, pos, channel->multiplexer->con_handle); pos += 2;
+ event[pos++] = channel->dlci >> 1;
+ bt_store_16(event, pos, channel->rfcomm_cid); pos += 2; // channel ID
+ bt_store_16(event, pos, channel->max_frame_size); pos += 2; // max frame size
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos);
+}
+
+static void rfcomm_emit_channel_open_failed_outgoing_memory(void * connection, bd_addr_t *addr, uint8_t server_channel){
+ uint8_t event[16];
+ uint8_t pos = 0;
+ event[pos++] = RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE;
+ event[pos++] = sizeof(event) - 2;
+ event[pos++] = BTSTACK_MEMORY_ALLOC_FAILED;
+ bt_flip_addr(&event[pos], *addr); pos += 6;
+ bt_store_16(event, pos, 0); pos += 2;
+ event[pos++] = server_channel;
+ bt_store_16(event, pos, 0); pos += 2; // channel ID
+ bt_store_16(event, pos, 0); pos += 2; // max frame size
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, pos);
+}
+
+// data: event(8), len(8), creidts incoming(8), new credits incoming(8), credits outgoing(8)
+static inline void rfcomm_emit_credit_status(rfcomm_channel_t * channel) {
+#ifdef RFCOMM_LOG_CREDITS
+ uint8_t event[5];
+ event[0] = 0x88;
+ event[1] = sizeof(event) - 2;
+ event[2] = channel->credits_incoming;
+ event[3] = channel->new_credits_incoming;
+ event[4] = channel->credits_outgoing;
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+#endif
+}
+
+// data: event(8), len(8), rfcomm_cid(16)
+static void rfcomm_emit_channel_closed(rfcomm_channel_t * channel) {
+ uint8_t event[4];
+ event[0] = RFCOMM_EVENT_CHANNEL_CLOSED;
+ event[1] = sizeof(event) - 2;
+ bt_store_16(event, 2, channel->rfcomm_cid);
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
+}
+
+static void rfcomm_emit_credits(rfcomm_channel_t * channel, uint8_t credits) {
+ uint8_t event[5];
+ event[0] = RFCOMM_EVENT_CREDITS;
+ event[1] = sizeof(event) - 2;
+ bt_store_16(event, 2, channel->rfcomm_cid);
+ event[4] = credits;
+ hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
+}
+
+static void rfcomm_emit_service_registered(void *connection, uint8_t status, uint8_t channel){
+ uint8_t event[4];
+ event[0] = RFCOMM_EVENT_SERVICE_REGISTERED;
+ event[1] = sizeof(event) - 2;
+ event[2] = status;
+ event[3] = channel;
+ hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
+ (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
+}
+
+// MARK: RFCOMM MULTIPLEXER HELPER
+
+static uint16_t rfcomm_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){
+
+ // Assume RFCOMM header with credits and single byte length field
+ uint16_t max_frame_size = l2cap_mtu - 5;
+
+ // single byte can denote len up to 127
+ if (max_frame_size > 127) {
+ max_frame_size--;
+ }
+
+ log_info("rfcomm_max_frame_size_for_l2cap_mtu: %u -> %u\n", l2cap_mtu, max_frame_size);
+ return max_frame_size;
+}
+
+static void rfcomm_multiplexer_initialize(rfcomm_multiplexer_t *multiplexer){
+
+ memset(multiplexer, 0, sizeof(rfcomm_multiplexer_t));
+
+ multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED;
+ multiplexer->l2cap_credits = 0;
+ multiplexer->send_dm_for_dlci = 0;
+ multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(l2cap_max_mtu());
+}
+
+static rfcomm_multiplexer_t * rfcomm_multiplexer_create_for_addr(bd_addr_t *addr){
+
+ // alloc structure
+ rfcomm_multiplexer_t * multiplexer = (rfcomm_multiplexer_t*)btstack_memory_rfcomm_multiplexer_get();
+ if (!multiplexer) return NULL;
+
+ // fill in
+ rfcomm_multiplexer_initialize(multiplexer);
+ BD_ADDR_COPY(&multiplexer->remote_addr, addr);
+
+ // add to services list
+ linked_list_add(&rfcomm_multiplexers, (linked_item_t *) multiplexer);
+
+ return multiplexer;
+}
+
+static rfcomm_multiplexer_t * rfcomm_multiplexer_for_addr(bd_addr_t *addr){
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){
+ rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it);
+ if (BD_ADDR_CMP(addr, multiplexer->remote_addr) == 0) {
+ return multiplexer;
+ };
+ }
+ return NULL;
+}
+
+static rfcomm_multiplexer_t * rfcomm_multiplexer_for_l2cap_cid(uint16_t l2cap_cid) {
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = it->next){
+ rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it);
+ if (multiplexer->l2cap_cid == l2cap_cid) {
+ return multiplexer;
+ };
+ }
+ return NULL;
+}
+
+static int rfcomm_multiplexer_has_channels(rfcomm_multiplexer_t * multiplexer){
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = ((rfcomm_channel_t *) it);
+ if (channel->multiplexer == multiplexer) {
+ return 1;
+ }
+ }
+ return 0;
+}
+
+// MARK: RFCOMM CHANNEL HELPER
+
+static void rfcomm_dump_channels(void){
+#ifndef EMBEDDED
+ linked_item_t * it;
+ int channels = 0;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = (rfcomm_channel_t *) it;
+ log_info("Channel #%u: addr %p, state %u\n", channels, channel, channel->state);
+ channels++;
+ }
+#endif
+}
+
+static void rfcomm_channel_initialize(rfcomm_channel_t *channel, rfcomm_multiplexer_t *multiplexer,
+ rfcomm_service_t *service, uint8_t server_channel){
+
+ // don't use 0 as channel id
+ if (rfcomm_client_cid_generator == 0) ++rfcomm_client_cid_generator;
+
+ // setup channel
+ memset(channel, 0, sizeof(rfcomm_channel_t));
+
+ channel->state = RFCOMM_CHANNEL_CLOSED;
+ channel->state_var = RFCOMM_CHANNEL_STATE_VAR_NONE;
+
+ channel->multiplexer = multiplexer;
+ channel->service = service;
+ channel->rfcomm_cid = rfcomm_client_cid_generator++;
+ channel->max_frame_size = multiplexer->max_frame_size;
+
+ channel->credits_incoming = 0;
+ channel->credits_outgoing = 0;
+ channel->packets_granted = 0;
+
+ // incoming flow control not active
+ channel->new_credits_incoming = 0x30;
+ channel->incoming_flow_control = 0;
+
+ if (service) {
+ // incoming connection
+ channel->outgoing = 0;
+ channel->dlci = (server_channel << 1) | multiplexer->outgoing;
+ if (channel->max_frame_size > service->max_frame_size) {
+ channel->max_frame_size = service->max_frame_size;
+ }
+ channel->incoming_flow_control = service->incoming_flow_control;
+ channel->new_credits_incoming = service->incoming_initial_credits;
+ } else {
+ // outgoing connection
+ channel->outgoing = 1;
+ channel->dlci = (server_channel << 1) | (multiplexer->outgoing ^ 1);
+ }
+}
+
+// service == NULL -> outgoing channel
+static rfcomm_channel_t * rfcomm_channel_create(rfcomm_multiplexer_t * multiplexer,
+ rfcomm_service_t * service, uint8_t server_channel){
+
+ log_info("rfcomm_channel_create for service %p, channel %u --- begin\n", service, server_channel);
+ rfcomm_dump_channels();
+
+ // alloc structure
+ rfcomm_channel_t * channel = (rfcomm_channel_t*)btstack_memory_rfcomm_channel_get();
+ if (!channel) return NULL;
+
+ // fill in
+ rfcomm_channel_initialize(channel, multiplexer, service, server_channel);
+
+ // add to services list
+ linked_list_add(&rfcomm_channels, (linked_item_t *) channel);
+
+ return channel;
+}
+
+static rfcomm_channel_t * rfcomm_channel_for_rfcomm_cid(uint16_t rfcomm_cid){
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = ((rfcomm_channel_t *) it);
+ if (channel->rfcomm_cid == rfcomm_cid) {
+ return channel;
+ };
+ }
+ return NULL;
+}
+
+static rfcomm_channel_t * rfcomm_channel_for_multiplexer_and_dlci(rfcomm_multiplexer_t * multiplexer, uint8_t dlci){
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = ((rfcomm_channel_t *) it);
+ if (channel->dlci == dlci && channel->multiplexer == multiplexer) {
+ return channel;
+ };
+ }
+ return NULL;
+}
+
+static rfcomm_service_t * rfcomm_service_for_channel(uint8_t server_channel){
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_services; it ; it = it->next){
+ rfcomm_service_t * service = ((rfcomm_service_t *) it);
+ if ( service->server_channel == server_channel){
+ return service;
+ };
+ }
+ return NULL;
+}
+
+// MARK: RFCOMM SEND
+
+/**
+ * @param credits - only used for RFCOMM flow control in UIH wiht P/F = 1
+ */
+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){
+
+ if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) return BTSTACK_ACL_BUFFERS_FULL;
+
+ uint8_t * rfcomm_out_buffer = l2cap_get_outgoing_buffer();
+
+ uint16_t pos = 0;
+ uint8_t crc_fields = 3;
+
+ rfcomm_out_buffer[pos++] = address;
+ rfcomm_out_buffer[pos++] = control;
+
+ // length field can be 1 or 2 octets
+ if (len < 128){
+ rfcomm_out_buffer[pos++] = (len << 1)| 1; // bits 0-6
+ } else {
+ rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6
+ rfcomm_out_buffer[pos++] = len >> 7; // bits 7-14
+ crc_fields++;
+ }
+
+ // add credits for UIH frames when PF bit is set
+ if (control == BT_RFCOMM_UIH_PF){
+ rfcomm_out_buffer[pos++] = credits;
+ }
+
+ // copy actual data
+ if (len) {
+ memcpy(&rfcomm_out_buffer[pos], data, len);
+ pos += len;
+ }
+
+ // UIH frames only calc FCS over address + control (5.1.1)
+ if ((control & 0xef) == BT_RFCOMM_UIH){
+ crc_fields = 2;
+ }
+ rfcomm_out_buffer[pos++] = crc8_calc(rfcomm_out_buffer, crc_fields); // calc fcs
+
+ int credits_taken = 0;
+ if (multiplexer->l2cap_credits){
+ credits_taken++;
+ multiplexer->l2cap_credits--;
+ } else {
+ log_info( "rfcomm_send_packet addr %02x, ctrl %02x size %u without l2cap credits\n", address, control, pos);
+ }
+
+ int err = l2cap_send_prepared(multiplexer->l2cap_cid, pos);
+
+ if (err) {
+ // undo credit counting
+ multiplexer->l2cap_credits += credits_taken;
+ }
+ return err;
+}
+
+// C/R Flag in Address
+// - terms: initiator = station that creates multiplexer with SABM
+// - terms: responder = station that responds to multiplexer setup with UA
+// "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"
+// - command initiator = 1 /response responder = 1
+// - command responder = 0 /response initiator = 0
+// "For UIH frames, the C/R bit is always set according to section 5.4.3.1 in GSM 07.10.
+// This applies independently of what is contained wthin the UIH frames, either data or control messages."
+// - c/r = 1 for frames by initiating station, 0 = for frames by responding station
+
+// C/R Flag in Message
+// "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."
+// - If the C/R bit is set to 1 the message is a command
+// - if it is set to 0 the message is a response.
+
+// temp/old messge construction
+
+// new object oriented version
+static int rfcomm_send_sabm(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_SABM, 0, NULL, 0);
+}
+
+static int rfcomm_send_disc(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2); // command
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DISC, 0, NULL, 0);
+}
+
+static int rfcomm_send_ua(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){
+ uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UA, 0, NULL, 0);
+}
+
+static int rfcomm_send_dm_pf(rfcomm_multiplexer_t *multiplexer, uint8_t dlci){
+ uint8_t address = (1 << 0) | ((multiplexer->outgoing ^ 1) << 1) | (dlci << 2); // response
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_DM_PF, 0, NULL, 0);
+}
+
+static int rfcomm_send_uih_msc_cmd(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) {
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1);
+ uint8_t payload[4];
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_MSC_CMD;
+ payload[pos++] = 2 << 1 | 1; // len
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1
+ payload[pos++] = signals;
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+static int rfcomm_send_uih_msc_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t signals) {
+ uint8_t address = (1 << 0) | (multiplexer->outgoing<< 1);
+ uint8_t payload[4];
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_MSC_RSP;
+ payload[pos++] = 2 << 1 | 1; // len
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1
+ payload[pos++] = signals;
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+static int rfcomm_send_uih_pn_command(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint16_t max_frame_size){
+ uint8_t payload[10];
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1);
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_PN_CMD;
+ payload[pos++] = 8 << 1 | 1; // len
+ payload[pos++] = dlci;
+ payload[pos++] = 0xf0; // pre-defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
+ payload[pos++] = 0; // priority
+ payload[pos++] = 0; // max 60 seconds ack
+ payload[pos++] = max_frame_size & 0xff; // max framesize low
+ payload[pos++] = max_frame_size >> 8; // max framesize high
+ payload[pos++] = 0x00; // number of retransmissions
+ payload[pos++] = 0x00; // (unused error recovery window) initial number of credits
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+// "The response may not change the DLCI, the priority, the convergence layer, or the timer value." RFCOMM-tutorial.pdf
+static int rfcomm_send_uih_pn_response(rfcomm_multiplexer_t *multiplexer, uint8_t dlci,
+ uint8_t priority, uint16_t max_frame_size){
+ uint8_t payload[10];
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1);
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_PN_RSP;
+ payload[pos++] = 8 << 1 | 1; // len
+ payload[pos++] = dlci;
+ payload[pos++] = 0xe0; // pre defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
+ payload[pos++] = priority; // priority
+ payload[pos++] = 0; // max 60 seconds ack
+ payload[pos++] = max_frame_size & 0xff; // max framesize low
+ payload[pos++] = max_frame_size >> 8; // max framesize high
+ payload[pos++] = 0x00; // number of retransmissions
+ payload[pos++] = 0x00; // (unused error recovery window) initial number of credits
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+static int rfcomm_send_uih_rpn_rsp(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, rfcomm_rpn_data_t *rpn_data) {
+ uint8_t payload[10];
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1);
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_RPN_RSP;
+ payload[pos++] = 8 << 1 | 1; // len
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); // CMD => C/R = 1
+ payload[pos++] = rpn_data->baud_rate;
+ payload[pos++] = rpn_data->flags;
+ payload[pos++] = rpn_data->flow_control;
+ payload[pos++] = rpn_data->xon;
+ payload[pos++] = rpn_data->xoff;
+ payload[pos++] = rpn_data->parameter_mask_0;
+ payload[pos++] = rpn_data->parameter_mask_1;
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+static int rfcomm_send_uih_data(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t *data, uint16_t len){
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2);
+ return rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH, 0, data, len);
+}
+
+static void rfcomm_send_uih_credits(rfcomm_multiplexer_t *multiplexer, uint8_t dlci, uint8_t credits){
+ uint8_t address = (1 << 0) | (multiplexer->outgoing << 1) | (dlci << 2);
+ rfcomm_send_packet_for_multiplexer(multiplexer, address, BT_RFCOMM_UIH_PF, credits, NULL, 0);
+}
+
+// MARK: RFCOMM MULTIPLEXER
+
+static void rfcomm_multiplexer_finalize(rfcomm_multiplexer_t * multiplexer){
+
+ // remove (potential) timer
+ if (multiplexer->timer_active) {
+ run_loop_remove_timer(&multiplexer->timer);
+ multiplexer->timer_active = 0;
+ }
+
+ // close and remove all channels
+ linked_item_t *it = (linked_item_t *) &rfcomm_channels;
+ while (it->next){
+ rfcomm_channel_t * channel = (rfcomm_channel_t *) it->next;
+ if (channel->multiplexer == multiplexer) {
+ // emit appropriate events
+ if (channel->state == RFCOMM_CHANNEL_OPEN) {
+ rfcomm_emit_channel_closed(channel);
+ } else {
+ rfcomm_emit_channel_opened(channel, RFCOMM_MULTIPLEXER_STOPPED);
+ }
+ // remove from list
+ it->next = it->next->next;
+ // free channel struct
+ btstack_memory_rfcomm_channel_free(channel);
+ } else {
+ it = it->next;
+ }
+ }
+
+ // keep reference to l2cap channel
+ uint16_t l2cap_cid = multiplexer->l2cap_cid;
+
+ // remove mutliplexer
+ linked_list_remove( &rfcomm_multiplexers, (linked_item_t *) multiplexer);
+ btstack_memory_rfcomm_multiplexer_free(multiplexer);
+
+ // close l2cap multiplexer channel, too
+ l2cap_disconnect_internal(l2cap_cid, 0x13);
+}
+
+static void rfcomm_multiplexer_timer_handler(timer_source_t *timer){
+ rfcomm_multiplexer_t * multiplexer = (rfcomm_multiplexer_t *) linked_item_get_user( (linked_item_t *) timer);
+ if (!rfcomm_multiplexer_has_channels(multiplexer)){
+ log_info( "rfcomm_multiplexer_timer_handler timeout: shutting down multiplexer!\n");
+ rfcomm_multiplexer_finalize(multiplexer);
+ }
+}
+
+static void rfcomm_multiplexer_prepare_idle_timer(rfcomm_multiplexer_t * multiplexer){
+ if (multiplexer->timer_active) {
+ run_loop_remove_timer(&multiplexer->timer);
+ multiplexer->timer_active = 0;
+ }
+ if (!rfcomm_multiplexer_has_channels(multiplexer)){
+ // start timer for multiplexer timeout check
+ run_loop_set_timer(&multiplexer->timer, RFCOMM_MULIPLEXER_TIMEOUT_MS);
+ multiplexer->timer.process = rfcomm_multiplexer_timer_handler;
+ linked_item_set_user((linked_item_t*) &multiplexer->timer, multiplexer);
+ run_loop_add_timer(&multiplexer->timer);
+ multiplexer->timer_active = 1;
+ }
+}
+
+static void rfcomm_multiplexer_opened(rfcomm_multiplexer_t *multiplexer){
+ log_info("Multiplexer up and running\n");
+ multiplexer->state = RFCOMM_MULTIPLEXER_OPEN;
+
+ rfcomm_channel_event_t event = { CH_EVT_MULTIPLEXER_READY };
+
+ // transition of channels that wait for multiplexer
+ linked_item_t *it;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = ((rfcomm_channel_t *) it);
+ if (channel->multiplexer != multiplexer) continue;
+ rfcomm_channel_state_machine(channel, &event);
+ }
+
+ rfcomm_run();
+ rfcomm_multiplexer_prepare_idle_timer(multiplexer);
+}
+
+
+/**
+ * @return handled packet
+ */
+static int rfcomm_multiplexer_hci_event_handler(uint8_t *packet, uint16_t size){
+ bd_addr_t event_addr;
+ uint16_t psm;
+ uint16_t l2cap_cid;
+ hci_con_handle_t con_handle;
+ rfcomm_multiplexer_t *multiplexer = NULL;
+ switch (packet[0]) {
+
+ // accept incoming PSM_RFCOMM connection if no multiplexer exists yet
+ case L2CAP_EVENT_INCOMING_CONNECTION:
+ // data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16)
+ bt_flip_addr(event_addr, &packet[2]);
+ con_handle = READ_BT_16(packet, 8);
+ psm = READ_BT_16(packet, 10);
+ l2cap_cid = READ_BT_16(packet, 12);
+
+ if (psm != PSM_RFCOMM) break;
+
+ multiplexer = rfcomm_multiplexer_for_addr(&event_addr);
+
+ if (multiplexer) {
+ log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - multiplexer already exists", l2cap_cid);
+ l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available
+ return 1;
+ }
+
+ // create and inititialize new multiplexer instance (incoming)
+ multiplexer = rfcomm_multiplexer_create_for_addr(&event_addr);
+ if (!multiplexer){
+ log_info("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => decline - no memory left", l2cap_cid);
+ l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available
+ return 1;
+ }
+
+ multiplexer->con_handle = con_handle;
+ multiplexer->l2cap_cid = l2cap_cid;
+ multiplexer->state = RFCOMM_MULTIPLEXER_W4_SABM_0;
+
+ log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_RFCOMM => accept", l2cap_cid);
+ l2cap_accept_connection_internal(l2cap_cid);
+ return 1;
+
+ // l2cap connection opened -> store l2cap_cid, remote_addr
+ case L2CAP_EVENT_CHANNEL_OPENED:
+ if (READ_BT_16(packet, 11) != PSM_RFCOMM) break;
+ log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_RFCOMM\n");
+ // get multiplexer for remote addr
+ con_handle = READ_BT_16(packet, 9);
+ l2cap_cid = READ_BT_16(packet, 13);
+ bt_flip_addr(event_addr, &packet[3]);
+ multiplexer = rfcomm_multiplexer_for_addr(&event_addr);
+ if (!multiplexer) {
+ log_error("L2CAP_EVENT_CHANNEL_OPENED but no multiplexer prepared\n");
+ return 1;
+ }
+ if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_CONNECT) {
+ log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection\n");
+ // wrong remote addr
+ if (BD_ADDR_CMP(event_addr, multiplexer->remote_addr)) break;
+ multiplexer->l2cap_cid = l2cap_cid;
+ multiplexer->con_handle = con_handle;
+ // send SABM #0
+ multiplexer->state = RFCOMM_MULTIPLEXER_SEND_SABM_0;
+ } else { // multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0
+
+ // set max frame size based on l2cap MTU
+ multiplexer->max_frame_size = rfcomm_max_frame_size_for_l2cap_mtu(READ_BT_16(packet, 17));
+ }
+ return 1;
+
+ // l2cap disconnect -> state = RFCOMM_MULTIPLEXER_CLOSED;
+
+ case L2CAP_EVENT_CREDITS:
+ // data: event(8), len(8), local_cid(16), credits(8)
+ l2cap_cid = READ_BT_16(packet, 2);
+ multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid);
+ if (!multiplexer) break;
+ multiplexer->l2cap_credits += packet[4];
+
+ // log_info("L2CAP_EVENT_CREDITS: %u (now %u)\n", packet[4], multiplexer->l2cap_credits);
+
+ // new credits, continue with signaling
+ rfcomm_run();
+
+ if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) break;
+ rfcomm_hand_out_credits();
+ return 1;
+
+ case DAEMON_EVENT_HCI_PACKET_SENT:
+ // testing DMA done code
+ rfcomm_run();
+ break;
+
+ case L2CAP_EVENT_CHANNEL_CLOSED:
+ // data: event (8), len(8), channel (16)
+ l2cap_cid = READ_BT_16(packet, 2);
+ multiplexer = rfcomm_multiplexer_for_l2cap_cid(l2cap_cid);
+ if (!multiplexer) break;
+ switch (multiplexer->state) {
+ case RFCOMM_MULTIPLEXER_W4_SABM_0:
+ case RFCOMM_MULTIPLEXER_W4_UA_0:
+ case RFCOMM_MULTIPLEXER_OPEN:
+ rfcomm_multiplexer_finalize(multiplexer);
+ return 1;
+ default:
+ break;
+ }
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+static int rfcomm_multiplexer_l2cap_packet_handler(uint16_t channel, uint8_t *packet, uint16_t size){
+
+ // get or create a multiplexer for a certain device
+ rfcomm_multiplexer_t *multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel);
+ if (!multiplexer) return 0;
+
+ // but only care for multiplexer control channel
+ uint8_t frame_dlci = packet[0] >> 2;
+ if (frame_dlci) return 0;
+ const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3
+ const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames
+ const uint8_t payload_offset = 3 + length_offset + credit_offset;
+ switch (packet[1]){
+
+ case BT_RFCOMM_SABM:
+ if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_SABM_0){
+ log_info("Received SABM #0\n");
+ multiplexer->outgoing = 0;
+ multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0;
+ return 1;
+ }
+ break;
+
+ case BT_RFCOMM_UA:
+ if (multiplexer->state == RFCOMM_MULTIPLEXER_W4_UA_0) {
+ // UA #0 -> send UA #0, state = RFCOMM_MULTIPLEXER_OPEN
+ log_info("Received UA #0 \n");
+ rfcomm_multiplexer_opened(multiplexer);
+ return 1;
+ }
+ break;
+
+ case BT_RFCOMM_DISC:
+ // DISC #0 -> send UA #0, close multiplexer
+ log_info("Received DISC #0, (ougoing = %u)\n", multiplexer->outgoing);
+ multiplexer->state = RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC;
+ return 1;
+
+ case BT_RFCOMM_DM:
+ // DM #0 - we shouldn't get this, just give up
+ log_info("Received DM #0\n");
+ log_info("-> Closing down multiplexer\n");
+ rfcomm_multiplexer_finalize(multiplexer);
+ return 1;
+
+ case BT_RFCOMM_UIH:
+ if (packet[payload_offset] == BT_RFCOMM_CLD_CMD){
+ // Multiplexer close down (CLD) -> close mutliplexer
+ log_info("Received Multiplexer close down command\n");
+ log_info("-> Closing down multiplexer\n");
+ rfcomm_multiplexer_finalize(multiplexer);
+ return 1;
+ }
+ break;
+
+ default:
+ break;
+
+ }
+ return 0;
+}
+
+static void rfcomm_multiplexer_state_machine(rfcomm_multiplexer_t * multiplexer, RFCOMM_MULTIPLEXER_EVENT event){
+
+ // process stored DM responses
+ if (multiplexer->send_dm_for_dlci){
+ rfcomm_send_dm_pf(multiplexer, multiplexer->send_dm_for_dlci);
+ multiplexer->send_dm_for_dlci = 0;
+ }
+
+ switch (multiplexer->state) {
+ case RFCOMM_MULTIPLEXER_SEND_SABM_0:
+ switch (event) {
+ case MULT_EV_READY_TO_SEND:
+ log_info("Sending SABM #0 - (multi 0x%p)\n", multiplexer);
+ multiplexer->state = RFCOMM_MULTIPLEXER_W4_UA_0;
+ rfcomm_send_sabm(multiplexer, 0);
+ break;
+ default:
+ break;
+ }
+ break;
+ case RFCOMM_MULTIPLEXER_SEND_UA_0:
+ switch (event) {
+ case MULT_EV_READY_TO_SEND:
+ log_info("Sending UA #0\n");
+ multiplexer->state = RFCOMM_MULTIPLEXER_OPEN;
+ rfcomm_send_ua(multiplexer, 0);
+ rfcomm_multiplexer_opened(multiplexer);
+ break;
+ default:
+ break;
+ }
+ break;
+ case RFCOMM_MULTIPLEXER_SEND_UA_0_AND_DISC:
+ switch (event) {
+ case MULT_EV_READY_TO_SEND:
+ log_info("Sending UA #0\n");
+ log_info("Closing down multiplexer\n");
+ multiplexer->state = RFCOMM_MULTIPLEXER_CLOSED;
+ rfcomm_send_ua(multiplexer, 0);
+ rfcomm_multiplexer_finalize(multiplexer);
+ // try to detect authentication errors: drop link key if multiplexer closed before first channel got opened
+ if (!multiplexer->at_least_one_connection){
+ log_info("TODO: no connections established - delete link key prophylactically\n");
+ // hci_send_cmd(&hci_delete_stored_link_key, multiplexer->remote_addr);
+ }
+ default:
+ break;
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+// MARK: RFCOMM CHANNEL
+
+static void rfcomm_hand_out_credits(void){
+ linked_item_t * it;
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = (rfcomm_channel_t *) it;
+ if (channel->state != RFCOMM_CHANNEL_OPEN) {
+ // log_info("RFCOMM_EVENT_CREDITS: multiplexer not open\n");
+ continue;
+ }
+ if (channel->packets_granted) {
+ // log_info("RFCOMM_EVENT_CREDITS: already packets granted\n");
+ continue;
+ }
+ if (!channel->credits_outgoing) {
+ // log_info("RFCOMM_EVENT_CREDITS: no outgoing credits\n");
+ continue;
+ }
+ if (!channel->multiplexer->l2cap_credits){
+ // log_info("RFCOMM_EVENT_CREDITS: no l2cap credits\n");
+ continue;
+ }
+ // channel open, multiplexer has l2cap credits and we didn't hand out credit before -> go!
+ // log_info("RFCOMM_EVENT_CREDITS: 1\n");
+ channel->packets_granted += 1;
+ rfcomm_emit_credits(channel, 1);
+ }
+}
+
+static void rfcomm_channel_send_credits(rfcomm_channel_t *channel, uint8_t credits){
+ rfcomm_send_uih_credits(channel->multiplexer, channel->dlci, credits);
+ channel->credits_incoming += credits;
+
+ rfcomm_emit_credit_status(channel);
+}
+
+static void rfcomm_channel_opened(rfcomm_channel_t *rfChannel){
+
+ log_info("rfcomm_channel_opened!\n");
+
+ rfChannel->state = RFCOMM_CHANNEL_OPEN;
+ rfcomm_emit_channel_opened(rfChannel, 0);
+ rfcomm_hand_out_credits();
+
+ // remove (potential) timer
+ rfcomm_multiplexer_t *multiplexer = rfChannel->multiplexer;
+ if (multiplexer->timer_active) {
+ run_loop_remove_timer(&multiplexer->timer);
+ multiplexer->timer_active = 0;
+ }
+ // hack for problem detecting authentication failure
+ multiplexer->at_least_one_connection = 1;
+
+ // start next connection request if pending
+ rfcomm_run();
+}
+
+static void rfcomm_channel_packet_handler_uih(rfcomm_multiplexer_t *multiplexer, uint8_t * packet, uint16_t size){
+ const uint8_t frame_dlci = packet[0] >> 2;
+ const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3
+ const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames
+ const uint8_t payload_offset = 3 + length_offset + credit_offset;
+
+ rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, frame_dlci);
+ if (!channel) return;
+
+ // handle new outgoing credits
+ if (packet[1] == BT_RFCOMM_UIH_PF) {
+
+ // add them
+ uint16_t new_credits = packet[3+length_offset];
+ channel->credits_outgoing += new_credits;
+ log_info( "RFCOMM data UIH_PF, new credits: %u, now %u\n", new_credits, channel->credits_outgoing);
+
+ // notify channel statemachine
+ rfcomm_channel_event_t channel_event = { CH_EVT_RCVD_CREDITS };
+ rfcomm_channel_state_machine(channel, &channel_event);
+ }
+
+ // contains payload?
+ if (size - 1 > payload_offset){
+
+ // log_info( "RFCOMM data UIH_PF, size %u, channel %p\n", size-payload_offset-1, rfChannel->connection);
+
+ // decrease incoming credit counter
+ if (channel->credits_incoming > 0){
+ channel->credits_incoming--;
+ }
+
+ // deliver payload
+ (*app_packet_handler)(channel->connection, RFCOMM_DATA_PACKET, channel->rfcomm_cid,
+ &packet[payload_offset], size-payload_offset-1);
+ }
+
+ // automatically provide new credits to remote device, if no incoming flow control
+ if (!channel->incoming_flow_control && channel->credits_incoming < 5){
+ channel->new_credits_incoming = 0x30;
+ }
+
+ rfcomm_emit_credit_status(channel);
+
+ // we received new RFCOMM credits, hand them out if possible
+ rfcomm_hand_out_credits();
+}
+
+static void rfcomm_channel_accept_pn(rfcomm_channel_t *channel, rfcomm_channel_event_pn_t *event){
+ // priority of client request
+ channel->pn_priority = event->priority;
+
+ // new credits
+ channel->credits_outgoing = event->credits_outgoing;
+
+ // negotiate max frame size
+ if (channel->max_frame_size > channel->multiplexer->max_frame_size) {
+ channel->max_frame_size = channel->multiplexer->max_frame_size;
+ }
+ if (channel->max_frame_size > event->max_frame_size) {
+ channel->max_frame_size = event->max_frame_size;
+ }
+
+}
+
+static void rfcomm_channel_finalize(rfcomm_channel_t *channel){
+
+ rfcomm_multiplexer_t *multiplexer = channel->multiplexer;
+
+ // remove from list
+ linked_list_remove( &rfcomm_channels, (linked_item_t *) channel);
+
+ // free channel
+ btstack_memory_rfcomm_channel_free(channel);
+
+ // update multiplexer timeout after channel was removed from list
+ rfcomm_multiplexer_prepare_idle_timer(multiplexer);
+}
+
+static void rfcomm_channel_state_machine_2(rfcomm_multiplexer_t * multiplexer, uint8_t dlci, rfcomm_channel_event_t *event){
+
+ // TODO: if client max frame size is smaller than RFCOMM_DEFAULT_SIZE, send PN
+
+
+ // lookup existing channel
+ rfcomm_channel_t * channel = rfcomm_channel_for_multiplexer_and_dlci(multiplexer, dlci);
+
+ // log_info("rfcomm_channel_state_machine_2 lookup dlci #%u = 0x%08x - event %u\n", dlci, (int) channel, event->type);
+
+ if (channel) {
+ rfcomm_channel_state_machine(channel, event);
+ return;
+ }
+
+ // service registered?
+ rfcomm_service_t * service = rfcomm_service_for_channel(dlci >> 1);
+ // log_info("rfcomm_channel_state_machine_2 service dlci #%u = 0x%08x\n", dlci, (int) service);
+ if (!service) {
+ // discard request by sending disconnected mode
+ multiplexer->send_dm_for_dlci = dlci;
+ return;
+ }
+
+ // create channel for some events
+ switch (event->type) {
+ case CH_EVT_RCVD_SABM:
+ case CH_EVT_RCVD_PN:
+ case CH_EVT_RCVD_RPN_REQ:
+ case CH_EVT_RCVD_RPN_CMD:
+ // setup incoming channel
+ channel = rfcomm_channel_create(multiplexer, service, dlci >> 1);
+ if (!channel){
+ // discard request by sending disconnected mode
+ multiplexer->send_dm_for_dlci = dlci;
+ }
+ break;
+ default:
+ break;
+ }
+
+ if (!channel) {
+ // discard request by sending disconnected mode
+ multiplexer->send_dm_for_dlci = dlci;
+ return;
+ }
+ channel->connection = service->connection;
+ rfcomm_channel_state_machine(channel, event);
+}
+
+void rfcomm_channel_packet_handler(rfcomm_multiplexer_t * multiplexer, uint8_t *packet, uint16_t size){
+
+ // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1]
+ const uint8_t frame_dlci = packet[0] >> 2;
+ uint8_t message_dlci; // used by commands in UIH(_PF) packets
+ uint8_t message_len; // "
+
+ // rfcomm: (1) command/control
+ // -- credits_offset = 1 if command == BT_RFCOMM_UIH_PF
+ const uint8_t credit_offset = ((packet[1] & BT_RFCOMM_UIH_PF) == BT_RFCOMM_UIH_PF) ? 1 : 0; // credits for uih_pf frames
+ // rfcomm: (2) length. if bit 0 is cleared, 2 byte length is used. (little endian)
+ const uint8_t length_offset = (packet[2] & 1) ^ 1; // to be used for pos >= 3
+ // rfcomm: (3+length_offset) credits if credits_offset == 1
+ // rfcomm: (3+length_offest+credits_offset)
+ const uint8_t payload_offset = 3 + length_offset + credit_offset;
+
+ rfcomm_channel_event_t event;
+ rfcomm_channel_event_pn_t event_pn;
+ rfcomm_channel_event_rpn_t event_rpn;
+
+ // switch by rfcomm message type
+ switch(packet[1]) {
+
+ case BT_RFCOMM_SABM:
+ event.type = CH_EVT_RCVD_SABM;
+ log_info("Received SABM #%u\n", frame_dlci);
+ rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event);
+ break;
+
+ case BT_RFCOMM_UA:
+ event.type = CH_EVT_RCVD_UA;
+ log_info("Received UA #%u - channel opened\n",frame_dlci);
+ rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event);
+ break;
+
+ case BT_RFCOMM_DISC:
+ event.type = CH_EVT_RCVD_DISC;
+ rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event);
+ break;
+
+ case BT_RFCOMM_DM:
+ case BT_RFCOMM_DM_PF:
+ event.type = CH_EVT_RCVD_DM;
+ rfcomm_channel_state_machine_2(multiplexer, frame_dlci, &event);
+ break;
+
+ case BT_RFCOMM_UIH_PF:
+ case BT_RFCOMM_UIH:
+
+ message_len = packet[payload_offset+1] >> 1;
+
+ switch (packet[payload_offset]) {
+ case BT_RFCOMM_PN_CMD:
+ message_dlci = packet[payload_offset+2];
+ event_pn.super.type = CH_EVT_RCVD_PN;
+ event_pn.priority = packet[payload_offset+4];
+ event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6);
+ event_pn.credits_outgoing = packet[payload_offset+9];
+ log_info("Received UIH Parameter Negotiation Command for #%u\n", message_dlci);
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn);
+ break;
+
+ case BT_RFCOMM_PN_RSP:
+ message_dlci = packet[payload_offset+2];
+ event_pn.super.type = CH_EVT_RCVD_PN_RSP;
+ event_pn.priority = packet[payload_offset+4];
+ event_pn.max_frame_size = READ_BT_16(packet, payload_offset+6);
+ event_pn.credits_outgoing = packet[payload_offset+9];
+ log_info("UIH Parameter Negotiation Response max frame %u, credits %u\n",
+ event_pn.max_frame_size, event_pn.credits_outgoing);
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_pn);
+ break;
+
+ case BT_RFCOMM_MSC_CMD:
+ message_dlci = packet[payload_offset+2] >> 2;
+ event.type = CH_EVT_RCVD_MSC_CMD;
+ log_info("Received MSC CMD for #%u, \n", message_dlci);
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event);
+ break;
+
+ case BT_RFCOMM_MSC_RSP:
+ message_dlci = packet[payload_offset+2] >> 2;
+ event.type = CH_EVT_RCVD_MSC_RSP;
+ log_info("Received MSC RSP for #%u\n", message_dlci);
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event);
+ break;
+
+ case BT_RFCOMM_RPN_CMD:
+ message_dlci = packet[payload_offset+2] >> 2;
+ switch (message_len){
+ case 1:
+ log_info("Received Remote Port Negotiation for #%u\n", message_dlci);
+ event.type = CH_EVT_RCVD_RPN_REQ;
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, &event);
+ break;
+ case 8:
+ log_info("Received Remote Port Negotiation (Info) for #%u\n", message_dlci);
+ event_rpn.super.type = CH_EVT_RCVD_RPN_CMD;
+ event_rpn.data.baud_rate = packet[payload_offset+3];
+ event_rpn.data.flags = packet[payload_offset+4];
+ event_rpn.data.flow_control = packet[payload_offset+5];
+ event_rpn.data.xon = packet[payload_offset+6];
+ event_rpn.data.xoff = packet[payload_offset+7];
+ event_rpn.data.parameter_mask_0 = packet[payload_offset+8];
+ event_rpn.data.parameter_mask_1 = packet[payload_offset+9];
+ rfcomm_channel_state_machine_2(multiplexer, message_dlci, (rfcomm_channel_event_t*) &event_rpn);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ default:
+ log_error("Received unknown UIH packet - 0x%02x\n", packet[payload_offset]);
+ break;
+ }
+ break;
+
+ default:
+ log_error("Received unknown RFCOMM message type %x\n", packet[1]);
+ break;
+ }
+
+ // trigger next action - example W4_PN_RSP: transition to SEND_SABM which only depends on "can send"
+ rfcomm_run();
+}
+
+void rfcomm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
+
+ // multiplexer handler
+ int handled = 0;
+ switch (packet_type) {
+ case HCI_EVENT_PACKET:
+ handled = rfcomm_multiplexer_hci_event_handler(packet, size);
+ break;
+ case L2CAP_DATA_PACKET:
+ handled = rfcomm_multiplexer_l2cap_packet_handler(channel, packet, size);
+ break;
+ default:
+ break;
+ }
+
+ if (handled) {
+ rfcomm_run();
+ return;
+ }
+
+ // we only handle l2cap packet over open multiplexer channel now
+ if (packet_type != L2CAP_DATA_PACKET) {
+ (*app_packet_handler)(NULL, packet_type, channel, packet, size);
+ return;
+ }
+ rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_l2cap_cid(channel);
+ if (!multiplexer || multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) {
+ (*app_packet_handler)(NULL, packet_type, channel, packet, size);
+ return;
+ }
+
+ // channel data ?
+ // rfcomm: (0) addr [76543 server channel] [2 direction: initiator uses 1] [1 C/R: CMD by initiator = 1] [0 EA=1]
+ const uint8_t frame_dlci = packet[0] >> 2;
+
+ if (frame_dlci && (packet[1] == BT_RFCOMM_UIH || packet[1] == BT_RFCOMM_UIH_PF)) {
+ rfcomm_channel_packet_handler_uih(multiplexer, packet, size);
+ rfcomm_run();
+ return;
+ }
+
+ rfcomm_channel_packet_handler(multiplexer, packet, size);
+}
+
+static int rfcomm_channel_ready_for_open(rfcomm_channel_t *channel){
+ // log_info("rfcomm_channel_ready_for_open state %u, flags needed %04x, current %04x, rf credits %u, l2cap credits %u \n", 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);
+ if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP) == 0) return 0;
+ if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP) == 0) return 0;
+ if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS) == 0) return 0;
+ if (channel->credits_outgoing == 0) return 0;
+
+ return 1;
+}
+
+static void rfcomm_channel_state_machine(rfcomm_channel_t *channel, rfcomm_channel_event_t *event){
+
+ // log_info("rfcomm_channel_state_machine: state %u, state_var %04x, event %u\n", channel->state, channel->state_var ,event->type);
+
+ rfcomm_multiplexer_t *multiplexer = channel->multiplexer;
+
+ // TODO: integrate in common switch
+ if (event->type == CH_EVT_RCVD_DISC){
+ rfcomm_emit_channel_closed(channel);
+ channel->state = RFCOMM_CHANNEL_SEND_UA_AFTER_DISC;
+ return;
+ }
+
+ // TODO: integrate in common switch
+ if (event->type == CH_EVT_RCVD_DM){
+ log_info("Received DM message for #%u\n", channel->dlci);
+ log_info("-> Closing channel locally for #%u\n", channel->dlci);
+ rfcomm_emit_channel_closed(channel);
+ rfcomm_channel_finalize(channel);
+ return;
+ }
+
+ // remote port negotiation command - just accept everything for now
+ //
+ // "The RPN command can be used before a new DLC is opened and should be used whenever the port settings change."
+ // "The RPN command is specified as optional in TS 07.10, but it is mandatory to recognize and respond to it in RFCOMM.
+ // (Although the handling of individual settings are implementation-dependent.)"
+ //
+
+ // TODO: integrate in common switch
+ if (event->type == CH_EVT_RCVD_RPN_CMD){
+ // control port parameters
+ rfcomm_channel_event_rpn_t *event_rpn = (rfcomm_channel_event_rpn_t*) event;
+ memcpy(&channel->rpn_data, &event_rpn->data, sizeof(rfcomm_rpn_data_t));
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP;
+ return;
+ }
+
+ // TODO: integrate in common switch
+ if (event->type == CH_EVT_RCVD_RPN_REQ){
+ // default rpn rsp
+ rfcomm_rpn_data_t rpn_data;
+ rpn_data.baud_rate = 0xa0; /* 9600 bps */
+ rpn_data.flags = 0x03; /* 8-n-1 */
+ rpn_data.flow_control = 0; /* no flow control */
+ rpn_data.xon = 0xd1; /* XON */
+ rpn_data.xoff = 0xd3; /* XOFF */
+ rpn_data.parameter_mask_0 = 0x7f; /* parameter mask, all values set */
+ rpn_data.parameter_mask_1 = 0x3f; /* parameter mask, all values set */
+ memcpy(&channel->rpn_data, &rpn_data, sizeof(rfcomm_rpn_data_t));
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP;
+ return;
+ }
+
+ // TODO: integrate in common swich
+ if (event->type == CH_EVT_READY_TO_SEND){
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP){
+ log_info("Sending Remote Port Negotiation RSP for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_RPN_RSP;
+ rfcomm_send_uih_rpn_rsp(multiplexer, channel->dlci, &channel->rpn_data);
+ return;
+ }
+ }
+
+ rfcomm_channel_event_pn_t * event_pn = (rfcomm_channel_event_pn_t*) event;
+
+ switch (channel->state) {
+ case RFCOMM_CHANNEL_CLOSED:
+ switch (event->type){
+ case CH_EVT_RCVD_SABM:
+ log_info("-> Inform app\n");
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM;
+ channel->state = RFCOMM_CHANNEL_INCOMING_SETUP;
+ rfcomm_emit_connection_request(channel);
+ break;
+ case CH_EVT_RCVD_PN:
+ rfcomm_channel_accept_pn(channel, event_pn);
+ log_info("-> Inform app\n");
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_PN;
+ channel->state = RFCOMM_CHANNEL_INCOMING_SETUP;
+ rfcomm_emit_connection_request(channel);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_INCOMING_SETUP:
+ switch (event->type){
+ case CH_EVT_RCVD_SABM:
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM;
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) {
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_UA;
+ }
+ break;
+ case CH_EVT_RCVD_PN:
+ rfcomm_channel_accept_pn(channel, event_pn);
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_PN;
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) {
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP;
+ }
+ break;
+ case CH_EVT_READY_TO_SEND:
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP){
+ log_info("Sending UIH Parameter Negotiation Respond for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP;
+ rfcomm_send_uih_pn_response(multiplexer, channel->dlci, channel->pn_priority, channel->max_frame_size);
+ }
+ else if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_UA){
+ log_info("Sending UA #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_UA;
+ rfcomm_send_ua(multiplexer, channel->dlci);
+ }
+ if ((channel->state_var & RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED) && (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM)) {
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS;
+ channel->state = RFCOMM_CHANNEL_DLC_SETUP;
+ }
+ break;
+
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_W4_MULTIPLEXER:
+ switch (event->type) {
+ case CH_EVT_MULTIPLEXER_READY:
+ log_info("Muliplexer opened, sending UIH PN next\n");
+ channel->state = RFCOMM_CHANNEL_SEND_UIH_PN;
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_SEND_UIH_PN:
+ switch (event->type) {
+ case CH_EVT_READY_TO_SEND:
+ log_info("Sending UIH Parameter Negotiation Command for #%u (channel 0x%p)\n", channel->dlci, channel );
+ channel->state = RFCOMM_CHANNEL_W4_PN_RSP;
+ rfcomm_send_uih_pn_command(multiplexer, channel->dlci, channel->max_frame_size);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_W4_PN_RSP:
+ switch (event->type){
+ case CH_EVT_RCVD_PN_RSP:
+ // update max frame size
+ if (channel->max_frame_size > event_pn->max_frame_size) {
+ channel->max_frame_size = event_pn->max_frame_size;
+ }
+ // new credits
+ channel->credits_outgoing = event_pn->credits_outgoing;
+ channel->state = RFCOMM_CHANNEL_SEND_SABM_W4_UA;
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_SEND_SABM_W4_UA:
+ switch (event->type) {
+ case CH_EVT_READY_TO_SEND:
+ log_info("Sending SABM #%u\n", channel->dlci);
+ channel->state = RFCOMM_CHANNEL_W4_UA;
+ rfcomm_send_sabm(multiplexer, channel->dlci);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_W4_UA:
+ switch (event->type){
+ case CH_EVT_RCVD_UA:
+ channel->state = RFCOMM_CHANNEL_DLC_SETUP;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS;
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_DLC_SETUP:
+ switch (event->type){
+ case CH_EVT_RCVD_MSC_CMD:
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_CMD;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP;
+ break;
+ case CH_EVT_RCVD_MSC_RSP:
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_RCVD_MSC_RSP;
+ break;
+
+ case CH_EVT_READY_TO_SEND:
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD){
+ log_info("Sending MSC CMD for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_CMD;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_CMD;
+ rfcomm_send_uih_msc_cmd(multiplexer, channel->dlci , 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1
+ break;
+ }
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){
+ log_info("Sending MSC RSP for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_MSC_RSP;
+ rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1
+ break;
+ }
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS){
+ log_info("Providing credits for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_CREDITS;
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SENT_CREDITS;
+ if (channel->new_credits_incoming) {
+ uint8_t new_credits = channel->new_credits_incoming;
+ channel->new_credits_incoming = 0;
+ rfcomm_channel_send_credits(channel, new_credits);
+ }
+ break;
+
+ }
+ break;
+ default:
+ break;
+ }
+ // finally done?
+ if (rfcomm_channel_ready_for_open(channel)){
+ channel->state = RFCOMM_CHANNEL_OPEN;
+ rfcomm_channel_opened(channel);
+ }
+ break;
+
+ case RFCOMM_CHANNEL_OPEN:
+ switch (event->type){
+ case CH_EVT_RCVD_MSC_CMD:
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP;
+ break;
+ case CH_EVT_READY_TO_SEND:
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP){
+ log_info("Sending MSC RSP for #%u\n", channel->dlci);
+ channel->state_var &= ~RFCOMM_CHANNEL_STATE_VAR_SEND_MSC_RSP;
+ rfcomm_send_uih_msc_rsp(multiplexer, channel->dlci, 0x8d); // ea=1,fc=0,rtc=1,rtr=1,ic=0,dv=1
+ break;
+ }
+ if (channel->new_credits_incoming) {
+ uint8_t new_credits = channel->new_credits_incoming;
+ channel->new_credits_incoming = 0;
+ rfcomm_channel_send_credits(channel, new_credits);
+ break;
+ }
+ break;
+ case CH_EVT_RCVD_CREDITS: {
+ // notify daemon -> might trigger re-try of parked connections
+ uint8_t event[1] = { DAEMON_EVENT_NEW_RFCOMM_CREDITS };
+ (*app_packet_handler)(channel->connection, DAEMON_EVENT_PACKET, channel->rfcomm_cid, event, sizeof(event));
+ break;
+ }
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_SEND_DM:
+ switch (event->type) {
+ case CH_EVT_READY_TO_SEND:
+ log_info("Sending DM_PF for #%u\n", channel->dlci);
+ // don't emit channel closed - channel was never open
+ channel->state = RFCOMM_CHANNEL_CLOSED;
+ rfcomm_send_dm_pf(multiplexer, channel->dlci);
+ rfcomm_channel_finalize(channel);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_SEND_DISC:
+ switch (event->type) {
+ case CH_EVT_READY_TO_SEND:
+ channel->state = RFCOMM_CHANNEL_CLOSED;
+ rfcomm_send_disc(multiplexer, channel->dlci);
+ rfcomm_emit_channel_closed(channel);
+ rfcomm_channel_finalize(channel);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case RFCOMM_CHANNEL_SEND_UA_AFTER_DISC:
+ switch (event->type) {
+ case CH_EVT_READY_TO_SEND:
+ log_info("Sending UA after DISC for #%u\n", channel->dlci);
+ channel->state = RFCOMM_CHANNEL_CLOSED;
+ rfcomm_send_ua(multiplexer, channel->dlci);
+ rfcomm_channel_finalize(channel);
+ break;
+ default:
+ break;
+ }
+ break;
+
+ default:
+ break;
+ }
+}
+
+
+// MARK: RFCOMM RUN
+// process outstanding signaling tasks
+static void rfcomm_run(void){
+
+ linked_item_t *it;
+ linked_item_t *next;
+
+ for (it = (linked_item_t *) rfcomm_multiplexers; it ; it = next){
+
+ next = it->next; // be prepared for removal of channel in state machine
+
+ rfcomm_multiplexer_t * multiplexer = ((rfcomm_multiplexer_t *) it);
+
+ if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) {
+ // log_info("rfcomm_run cannot send l2cap packet for #%u, credits %u\n", multiplexer->l2cap_cid, multiplexer->l2cap_credits);
+ continue;
+ }
+ // log_info("rfcomm_run: multi 0x%08x, state %u\n", (int) multiplexer, multiplexer->state);
+
+ rfcomm_multiplexer_state_machine(multiplexer, MULT_EV_READY_TO_SEND);
+ }
+
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = next){
+
+ next = it->next; // be prepared for removal of channel in state machine
+
+ rfcomm_channel_t * channel = ((rfcomm_channel_t *) it);
+ rfcomm_multiplexer_t * multiplexer = channel->multiplexer;
+
+ if (!l2cap_can_send_packet_now(multiplexer->l2cap_cid)) continue;
+
+ rfcomm_channel_event_t event = { CH_EVT_READY_TO_SEND };
+ rfcomm_channel_state_machine(channel, &event);
+ }
+}
+
+// MARK: RFCOMM BTstack API
+
+void rfcomm_init(void){
+ rfcomm_client_cid_generator = 0;
+ rfcomm_multiplexers = NULL;
+ rfcomm_services = NULL;
+ rfcomm_channels = NULL;
+}
+
+// register packet handler
+void rfcomm_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type,
+ uint16_t channel, uint8_t *packet, uint16_t size)){
+ app_packet_handler = handler;
+}
+
+// send packet over specific channel
+int rfcomm_send_internal(uint16_t rfcomm_cid, uint8_t *data, uint16_t len){
+
+ rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid);
+ if (!channel){
+ log_error("rfcomm_send_internal cid %u doesn't exist!\n", rfcomm_cid);
+ return 0;
+ }
+
+ if (!channel->credits_outgoing){
+ log_info("rfcomm_send_internal cid %u, no rfcomm outgoing credits!\n", rfcomm_cid);
+ return RFCOMM_NO_OUTGOING_CREDITS;
+ }
+
+ if (!channel->packets_granted){
+ log_info("rfcomm_send_internal cid %u, no rfcomm credits granted!\n", rfcomm_cid);
+ return RFCOMM_NO_OUTGOING_CREDITS;
+ }
+
+ // log_info("rfcomm_send_internal: len %u... outgoing credits %u, l2cap credit %us, granted %u\n",
+ // len, channel->credits_outgoing, channel->multiplexer->l2cap_credits, channel->packets_granted);
+
+
+ // send might cause l2cap to emit new credits, update counters first
+ channel->credits_outgoing--;
+ int packets_granted_decreased = 0;
+ if (channel->packets_granted) {
+ channel->packets_granted--;
+ packets_granted_decreased++;
+ }
+
+ int result = rfcomm_send_uih_data(channel->multiplexer, channel->dlci, data, len);
+
+ if (result != 0) {
+ channel->credits_outgoing++;
+ channel->packets_granted += packets_granted_decreased;
+ log_info("rfcomm_send_internal: error %d\n", result);
+ return result;
+ }
+
+ // log_info("rfcomm_send_internal: now outgoing credits %u, l2cap credit %us, granted %u\n",
+ // channel->credits_outgoing, channel->multiplexer->l2cap_credits, channel->packets_granted);
+
+ rfcomm_hand_out_credits();
+
+ return result;
+}
+
+void rfcomm_create_channel2(void * connection, bd_addr_t *addr, uint8_t server_channel, uint8_t incoming_flow_control, uint8_t initial_credits){
+ log_info("rfcomm_create_channel_internal to %s, at channel #%02x, flow control %u, init credits %u\n", bd_addr_to_str(*addr), server_channel,
+ incoming_flow_control, initial_credits);
+
+ // create new multiplexer if necessary
+ rfcomm_multiplexer_t * multiplexer = rfcomm_multiplexer_for_addr(addr);
+ if (!multiplexer) {
+ multiplexer = rfcomm_multiplexer_create_for_addr(addr);
+ if (!multiplexer){
+ rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel);
+ return;
+ }
+ multiplexer->outgoing = 1;
+ multiplexer->state = RFCOMM_MULTIPLEXER_W4_CONNECT;
+ }
+
+ // prepare channel
+ rfcomm_channel_t * channel = rfcomm_channel_create(multiplexer, NULL, server_channel);
+ if (!channel){
+ rfcomm_emit_channel_open_failed_outgoing_memory(connection, addr, server_channel);
+ return;
+ }
+ channel->connection = connection;
+ channel->incoming_flow_control = incoming_flow_control;
+ channel->new_credits_incoming = initial_credits;
+
+ // start multiplexer setup
+ if (multiplexer->state != RFCOMM_MULTIPLEXER_OPEN) {
+
+ channel->state = RFCOMM_CHANNEL_W4_MULTIPLEXER;
+
+ l2cap_create_channel_internal(connection, rfcomm_packet_handler, *addr, PSM_RFCOMM, l2cap_max_mtu());
+
+ return;
+ }
+
+ channel->state = RFCOMM_CHANNEL_SEND_UIH_PN;
+
+ // start connecting, if multiplexer is already up and running
+ rfcomm_run();
+}
+
+void rfcomm_create_channel_with_initial_credits_internal(void * connection, bd_addr_t *addr, uint8_t server_channel, uint8_t initial_credits){
+ rfcomm_create_channel2(connection, addr, server_channel, 1, initial_credits);
+}
+
+void rfcomm_create_channel_internal(void * connection, bd_addr_t *addr, uint8_t server_channel){
+ rfcomm_create_channel2(connection, addr, server_channel, 0, 0x30);
+}
+
+void rfcomm_disconnect_internal(uint16_t rfcomm_cid){
+ rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid);
+ if (channel) {
+ channel->state = RFCOMM_CHANNEL_SEND_DISC;
+ }
+
+ // process
+ rfcomm_run();
+}
+
+
+void rfcomm_register_service2(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t incoming_flow_control, uint8_t initial_credits){
+ // check if already registered
+ rfcomm_service_t * service = rfcomm_service_for_channel(channel);
+ if (service){
+ rfcomm_emit_service_registered(connection, RFCOMM_CHANNEL_ALREADY_REGISTERED, channel);
+ return;
+ }
+
+ // alloc structure
+ service = (rfcomm_service_t*)btstack_memory_rfcomm_service_get();
+ if (!service) {
+ rfcomm_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, channel);
+ return;
+ }
+
+ // register with l2cap if not registered before, max MTU
+ if (linked_list_empty(&rfcomm_services)){
+ l2cap_register_service_internal(NULL, rfcomm_packet_handler, PSM_RFCOMM, 0xffff);
+ }
+
+ // fill in
+ service->connection = connection;
+ service->server_channel = channel;
+ service->max_frame_size = max_frame_size;
+ service->incoming_flow_control = incoming_flow_control;
+ service->incoming_initial_credits = initial_credits;
+
+ // add to services list
+ linked_list_add(&rfcomm_services, (linked_item_t *) service);
+
+ // done
+ rfcomm_emit_service_registered(connection, 0, channel);
+}
+
+void rfcomm_register_service_with_initial_credits_internal(void * connection, uint8_t channel, uint16_t max_frame_size, uint8_t initial_credits){
+ rfcomm_register_service2(connection, channel, max_frame_size, 1, initial_credits);
+}
+
+void rfcomm_register_service_internal(void * connection, uint8_t channel, uint16_t max_frame_size){
+ rfcomm_register_service2(connection, channel, max_frame_size, 0, 0x30);
+}
+
+void rfcomm_unregister_service_internal(uint8_t service_channel){
+ rfcomm_service_t *service = rfcomm_service_for_channel(service_channel);
+ if (!service) return;
+ linked_list_remove(&rfcomm_services, (linked_item_t *) service);
+ btstack_memory_rfcomm_service_free(service);
+
+ // unregister if no services active
+ if (linked_list_empty(&rfcomm_services)){
+ // bt_send_cmd(&l2cap_unregister_service, PSM_RFCOMM);
+ l2cap_unregister_service_internal(NULL, PSM_RFCOMM);
+ }
+}
+
+void rfcomm_accept_connection_internal(uint16_t rfcomm_cid){
+ log_info("Received Accept Connction\n");
+ rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid);
+ if (!channel) return;
+ switch (channel->state) {
+ case RFCOMM_CHANNEL_INCOMING_SETUP:
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_CLIENT_ACCEPTED;
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_PN){
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_PN_RSP;
+ }
+ if (channel->state_var & RFCOMM_CHANNEL_STATE_VAR_RCVD_SABM){
+ channel->state_var |= RFCOMM_CHANNEL_STATE_VAR_SEND_UA;
+ }
+ break;
+ default:
+ break;
+ }
+
+ // process
+ rfcomm_run();
+}
+
+void rfcomm_decline_connection_internal(uint16_t rfcomm_cid){
+ log_info("Received Decline Connction\n");
+ rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid);
+ if (!channel) return;
+ switch (channel->state) {
+ case RFCOMM_CHANNEL_INCOMING_SETUP:
+ channel->state = RFCOMM_CHANNEL_SEND_DM;
+ break;
+ default:
+ break;
+ }
+
+ // process
+ rfcomm_run();
+}
+
+void rfcomm_grant_credits(uint16_t rfcomm_cid, uint8_t credits){
+ rfcomm_channel_t * channel = rfcomm_channel_for_rfcomm_cid(rfcomm_cid);
+ if (!channel) return;
+ if (!channel->incoming_flow_control) return;
+ channel->new_credits_incoming += credits;
+
+ // process
+ rfcomm_run();
+}
+
+//
+void rfcomm_close_connection(void *connection){
+ linked_item_t *it;
+
+ // close open channels
+ for (it = (linked_item_t *) rfcomm_channels; it ; it = it->next){
+ rfcomm_channel_t * channel = (rfcomm_channel_t *)it;
+ if (channel->connection != connection) continue;
+ channel->state = RFCOMM_CHANNEL_SEND_DISC;
+ }
+
+ // unregister services
+ it = (linked_item_t *) &rfcomm_services;
+ while (it->next) {
+ rfcomm_service_t * service = (rfcomm_service_t *) it->next;
+ if (service->connection == connection){
+ it->next = it->next->next;
+ btstack_memory_rfcomm_service_free(service);
+ } else {
+ it = it->next;
+ }
+ }
+
+ // process
+ rfcomm_run();
+}
+
+
+
+