nica/oswald
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
oswald/metawatch/bt_l2cap.c
Nicole Faerber adbc36ad71 initial commit to new repo
2021-02-14 18:03:13 +01:00

281 lines
7.6 KiB
C
Raw Permalink Blame History

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "mw_uart.h"
#include "mw_bt.h"
#include "bt_hci.h"
#include "bt_l2cap.h"
#include "oswald_main.h"
static bt_l2cap_con_t _l2cap_con;
void init_l2cap(void)
{
memset(&_l2cap_con, 0, sizeof(bt_l2cap_con_t));
}
void bt_l2cap_proc_dyn_channel(const uint16_t channel, const uint16_t handle, const void *mdat, uint16_t mlen)
{
if (channel == _l2cap_con.locCID) {
debug_uart_tx("L2CAP data rcvd: ");
debug_dump_ascii(mlen, mdat);
if (_l2cap_con.PSM == 0x1001) {
oswald_handle_comm_input(mlen, mdat);
} else if (_l2cap_con.PSM == 0x0001) { // SDP
debug_uart_tx("SDP req: ");
debug_dump_hex(mlen, mdat);
} else
debug_uart_tx("L2CAP data on unhandled PSM\n");
} else {
debug_uart_tx("L2CAP CID does not match local CID\n");
}
}
uint8_t bt_l2cap_get_connected(const uint16_t channel)
{
if (_l2cap_con.locCID == channel)
return _l2cap_con.cstate;
else
return BT_L2CAP_CON_IDLE;
}
void bt_l2cap_send_channel(const uint16_t channel, const void *mdat, uint16_t mlen)
{
if (_l2cap_con.cstate == BT_L2CAP_CON_CONNECTED && _l2cap_con.hci_handle != 0 && _l2cap_con.remCID != 0 &&
mlen != 0 && mdat != NULL) {
bt_acl_send(_l2cap_con.hci_handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, _l2cap_con.remCID, mlen, mdat);
}
}
typedef struct {
uint8_t resp;
uint8_t ident;
uint16_t length;
uint16_t DCID;
uint16_t SCID;
} __attribute__((packed)) bt_l2cap_disconn_resp_t;
typedef struct {
uint8_t resp;
uint8_t ident;
uint16_t length;
uint16_t SCID;
uint16_t flags;
uint16_t result;
uint16_t config;
} __attribute__((packed)) bt_l2cap_conf_resp_t;
void bt_l2cap_proc_signalling(const uint16_t handle, unsigned char *mdat, uint16_t mlen)
{
#if defined MW_DEVBOARD_V2
char tstr[16];
#endif
debug_uart_tx("ACL L2CAP signalling ");
switch (mdat[0]) {
case COMMAND_REJECT:
debug_uart_tx("command reject\n");
break;
case CONNECTION_REQUEST: {
uint8_t ident = mdat[1];
uint16_t PSM = mdat[4] | (mdat[5] << 8);;
uint16_t src_CID = mdat[6] | (mdat[7] << 8);;
#if defined MW_DEVBOARD_V2
debug_uart_tx("connection request on ");
snprintf(tstr, 16, "PSM 0x%04x ", PSM);
debug_uart_tx(tstr);
snprintf(tstr, 16, "srcCID 0x%04x\n", src_CID);
debug_uart_tx(tstr);
#endif
bt_l2cap_handle_connection_request(handle, ident, PSM, src_CID);
} break;
case CONNECTION_RESPONSE:
debug_uart_tx("connection response\n");
break;
case CONFIGURE_REQUEST: {
uint8_t ident = mdat[1];
uint16_t len = mdat[2] | (mdat[3] << 8);
uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
uint16_t flags = mdat[6] | (mdat[7] << 8);
bt_l2cap_conf_resp_t resp;
#if defined MW_DEVBOARD_V2
debug_uart_tx("configure request ");
snprintf(tstr, 16, "len 0x%04x ", len);
debug_uart_tx(tstr);
snprintf(tstr, 16, "DCID 0x%04x ", dst_CID);
debug_uart_tx(tstr);
snprintf(tstr, 16, "flags 0x%04x\n", flags);
debug_uart_tx(tstr);
#if 0
for (i=8; i<((len-4)+8); i++) {
snprintf(tstr, 16, "0x%02x ", mdat[i]);
debug_uart_tx(tstr);
}
debug_uart_tx("\n");
#endif
debug_dump_hex(len-4+8, (mdat+8));
#endif
resp.resp = CONFIGURE_RESPONSE;
resp.ident = ident;
resp.length = 0x06;
if (dst_CID != _l2cap_con.locCID)
debug_uart_tx("warning: DCID does not match\n");
resp.SCID = _l2cap_con.remCID;
resp.flags = flags;
resp.result = 0x0000; // success
resp.config = 0x0000; // OK?
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_resp_t)-2, &resp);
} break;
case CONFIGURE_RESPONSE:
debug_uart_tx("configure response\n");
break;
case DISCONNECTION_REQUEST: {
bt_l2cap_disconn_resp_t resp;
uint8_t ident = mdat[1];
uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
uint16_t src_CID = mdat[6] | (mdat[7] << 8);
debug_uart_tx("disconnect request\n");
if (dst_CID != _l2cap_con.locCID || src_CID != _l2cap_con.remCID)
debug_uart_tx("warning: discon on unknown CID\n");
resp.resp = DISCONNECTION_RESPONSE;
resp.ident = ident;
resp.length = 0x0004;
resp.DCID = _l2cap_con.locCID;
resp.SCID = _l2cap_con.remCID;
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_disconn_resp_t), &resp);
init_l2cap();
} break;
case DISCONNECTION_RESPONSE:
debug_uart_tx("disconnect response\n");
break;
case ECHO_REQUEST:
debug_uart_tx("echo request\n");
mdat[0] = ECHO_RESPONSE;
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, mlen, mdat);
break;
case ECHO_RESPONSE:
debug_uart_tx("echo response\n");
break;
case INFORMATION_REQUEST: {
uint16_t info_type = mdat[4] | (mdat[5] << 8);
debug_uart_tx("information request ");
switch (info_type) {
case 0x0001:
debug_uart_tx("connectionless mtu\n");
break;
case 0x0002:
debug_uart_tx("ext. feat. support\n");
mdat[0] = INFORMATION_RESPONSE;
mdat[2] = 0x02;
mdat[3] = 0x00;
mdat[6] = 0x01; // not supported
mdat[7] = 0x00;
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, 0x08, mdat);
break;
case 0x0003:
debug_uart_tx("fixed channel support\n");
break;
default:
debug_uart_tx("unknown\n");
break;
};
} break;
case INFORMATION_RESPONSE:
debug_uart_tx("information response\n");
break;
default:
debug_uart_tx("unknown\n");
break;
}
}
typedef struct {
uint8_t resp;
uint8_t ident;
uint16_t length;
uint16_t DCID;
uint16_t SCID;
uint16_t result;
uint16_t status;
} __attribute__((packed)) bt_l2cap_conn_resp_t;
typedef struct {
uint8_t resp;
uint8_t ident;
uint16_t length;
uint16_t DCID;
uint16_t flags;
uint8_t option;
uint8_t olen;
uint16_t odat;
} __attribute__((packed)) bt_l2cap_conf_req_t;
void bt_l2cap_handle_connection_request(const uint16_t handle, const uint8_t ident, const uint16_t PSM, const uint16_t src_CID)
{
bt_l2cap_conn_resp_t resp;
// for now we only support one connection, only on PSM 0x1001
if (_l2cap_con.cstate == BT_L2CAP_CON_IDLE && PSM == 0x1001) {
bt_l2cap_conf_req_t req;
_l2cap_con.cstate = BT_L2CAP_CON_CONNECTED;
_l2cap_con.hci_handle = handle;
_l2cap_con.PSM = PSM;
_l2cap_con.locCID = 0x0040;
_l2cap_con.remCID = src_CID;
_l2cap_con.locMTU = 0xf0;
_l2cap_con.remMTU = 0x00;
resp.resp = CONNECTION_RESPONSE;
resp.ident = ident;
resp.length = 0x0008;
resp.DCID = _l2cap_con.locCID;
resp.SCID = _l2cap_con.remCID;
resp.result = BT_L2CAP_CON_RES_SUCCESS;
resp.status = BT_L2CAP_CON_STAT_NOINFO;
debug_uart_tx("l2cap accepting connection\n");
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);
req.resp = CONFIGURE_REQUEST;
req.ident = ident;
req.length = 0x08;
req.DCID = _l2cap_con.remCID;
req.flags = 0x00;
req.option = 0x01; // MTU
req.olen = 0x02;
req.odat = _l2cap_con.locMTU;
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_req_t), &req);
// max_interval Mandatory Range: 0x0006 to 0x0540
// min_interval Mandatory Range: 0x0006 to 0x0540
// sniff_attempt Mandatory Range for Controller: 1 to Tsniff/2
// sniff_timeout Mandatory Range for Controller: 0 to 0x0028
bt_hci_set_sniff_mode(handle, 0x10, 0x06, 0x20, 0x10);
} else {
resp.resp = CONNECTION_RESPONSE;
resp.ident = ident;
resp.length = 0x0008;
resp.DCID = 0x0000;
resp.SCID = src_CID;
resp.result = BT_L2CAP_CON_RES_REFUSED_RSRC;
resp.status = BT_L2CAP_CON_STAT_NOINFO;
debug_uart_tx("l2cap refusing connection\n");
bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);
}
}
Reference in a new issue View git blame Copy permalink