metawatch/bt_l2cap.c

   1 #include <stdint.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4
   5 #include "mw_uart.h"
   6 #include "mw_bt.h"
   7 #include "bt_hci.h"
   8 #include "bt_l2cap.h"
   9
  10 #include "oswald_main.h"
  11
  12 static bt_l2cap_con_t _l2cap_con;
  13
  14 void init_l2cap(void)
  15 {
  16         memset(&_l2cap_con, 0, sizeof(bt_l2cap_con_t));
  17 }
  18
  19 void bt_l2cap_proc_dyn_channel(const uint16_t channel, const uint16_t handle, const void *mdat, uint16_t mlen)
  20 {
  21         if (channel == _l2cap_con.locCID) {
  22                 debug_uart_tx("L2CAP data rcvd: ");
  23                 debug_dump_ascii(mlen, mdat);
  24                 if (_l2cap_con.PSM == 0x1001) {
  25                         oswald_handle_comm_input(mlen, mdat);
  26                 } else if (_l2cap_con.PSM == 0x0001) { // SDP
  27                         debug_uart_tx("SDP req: ");
  28                         debug_dump_hex(mlen, mdat);
  29                 } else
  30                         debug_uart_tx("L2CAP data on unhandled PSM\n");
  31         } else {
  32                 debug_uart_tx("L2CAP CID does not match local CID\n");
  33         }
  34 }
  35
  36 uint8_t bt_l2cap_get_connected(const uint16_t channel)
  37 {
  38         if (_l2cap_con.locCID == channel)
  39                 return _l2cap_con.cstate;
  40         else
  41                 return BT_L2CAP_CON_IDLE;
  42 }
  43
  44 void bt_l2cap_send_channel(const uint16_t channel, const void *mdat, uint16_t mlen)
  45 {
  46         if (_l2cap_con.cstate == BT_L2CAP_CON_CONNECTED && _l2cap_con.hci_handle != 0 && _l2cap_con.remCID != 0 &&
  47                 mlen != 0 && mdat != NULL) {
  48                 bt_acl_send(_l2cap_con.hci_handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, _l2cap_con.remCID, mlen, mdat);
  49         }
  50 }
  51
  52 typedef struct {
  53         uint8_t resp;
  54         uint8_t ident;
  55         uint16_t length;
  56         uint16_t DCID;
  57         uint16_t SCID;
  58 } __attribute__((packed)) bt_l2cap_disconn_resp_t;
  59
  60 typedef struct {
  61         uint8_t resp;
  62         uint8_t ident;
  63         uint16_t length;
  64         uint16_t SCID;
  65         uint16_t flags;
  66         uint16_t result;
  67         uint16_t config;
  68 } __attribute__((packed)) bt_l2cap_conf_resp_t;
  69
  70
  71 void bt_l2cap_proc_signalling(const uint16_t handle, unsigned char *mdat, uint16_t mlen)
  72 {
  73 #if defined MW_DEVBOARD_V2
  74         char tstr[16];
  75 #endif
  76         debug_uart_tx("ACL L2CAP signalling ");
  77         switch (mdat[0]) {
  78                 case COMMAND_REJECT:
  79                         debug_uart_tx("command reject\n");
  80                         break;
  81                 case CONNECTION_REQUEST: {
  82                         uint8_t ident = mdat[1];
  83                         uint16_t PSM = mdat[4] | (mdat[5] << 8);;
  84                         uint16_t src_CID = mdat[6] | (mdat[7] << 8);;
  85
  86 #if defined MW_DEVBOARD_V2
  87                         debug_uart_tx("connection request on ");
  88                         snprintf(tstr, 16, "PSM 0x%04x ", PSM);
  89                         debug_uart_tx(tstr);
  90                         snprintf(tstr, 16, "srcCID 0x%04x\n", src_CID);
  91                         debug_uart_tx(tstr);
  92 #endif
  93                         bt_l2cap_handle_connection_request(handle, ident, PSM, src_CID);
  94                         } break;
  95                 case CONNECTION_RESPONSE:
  96                         debug_uart_tx("connection response\n");
  97                         break;
  98                 case CONFIGURE_REQUEST: {
  99                         uint8_t ident = mdat[1];
 100                         uint16_t len = mdat[2] | (mdat[3] << 8);
 101                         uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
 102                         uint16_t flags = mdat[6] | (mdat[7] << 8);
 103                         bt_l2cap_conf_resp_t resp;
 104
 105 #if defined MW_DEVBOARD_V2
 106                         debug_uart_tx("configure request ");
 107                         snprintf(tstr, 16, "len 0x%04x ", len);
 108                         debug_uart_tx(tstr);
 109                         snprintf(tstr, 16, "DCID 0x%04x ", dst_CID);
 110                         debug_uart_tx(tstr);
 111                         snprintf(tstr, 16, "flags 0x%04x\n", flags);
 112                         debug_uart_tx(tstr);
 113 #if 0
 114                         for (i=8; i<((len-4)+8); i++) {
 115                                 snprintf(tstr, 16, "0x%02x ", mdat[i]);
 116                                 debug_uart_tx(tstr);
 117                         }
 118                         debug_uart_tx("\n");
 119 #endif
 120                         debug_dump_hex(len-4+8, (mdat+8));
 121 #endif
 122                         resp.resp = CONFIGURE_RESPONSE;
 123                         resp.ident = ident;
 124                         resp.length = 0x06;
 125                         if (dst_CID != _l2cap_con.locCID)
 126                                 debug_uart_tx("warning: DCID does not match\n");
 127                         resp.SCID = _l2cap_con.remCID;
 128                         resp.flags = flags;
 129                         resp.result = 0x0000; // success
 130                         resp.config = 0x0000; // OK?
 131
 132                         bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_resp_t)-2, &resp);
 133
 134                         } break;
 135                 case CONFIGURE_RESPONSE:
 136                         debug_uart_tx("configure response\n");
 137                         break;
 138                 case DISCONNECTION_REQUEST: {
 139                         bt_l2cap_disconn_resp_t resp;
 140                         uint8_t ident = mdat[1];
 141                         uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
 142                         uint16_t src_CID = mdat[6] | (mdat[7] << 8);
 143
 144                         debug_uart_tx("disconnect request\n");
 145
 146                         if (dst_CID != _l2cap_con.locCID || src_CID != _l2cap_con.remCID)
 147                                 debug_uart_tx("warning: discon on unknown CID\n");
 148                         resp.resp = DISCONNECTION_RESPONSE;
 149                         resp.ident = ident;
 150                         resp.length = 0x0004;
 151                         resp.DCID = _l2cap_con.locCID;
 152                         resp.SCID = _l2cap_con.remCID;
 153
 154                         bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_disconn_resp_t), &resp);
 155
 156                         init_l2cap();
 157                         } break;
 158                 case DISCONNECTION_RESPONSE:
 159                         debug_uart_tx("disconnect response\n");
 160                         break;
 161                 case ECHO_REQUEST:
 162                         debug_uart_tx("echo request\n");
 163                         mdat[0] = ECHO_RESPONSE;
 164                         bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, mlen, mdat);
 165                         break;
 166                 case ECHO_RESPONSE:
 167                         debug_uart_tx("echo response\n");
 168                         break;
 169                 case INFORMATION_REQUEST: {
 170                         uint16_t info_type = mdat[4] | (mdat[5] << 8);
 171                         debug_uart_tx("information request ");
 172                         switch (info_type) {
 173                                 case 0x0001:
 174                                         debug_uart_tx("connectionless mtu\n");
 175                                         break;
 176                                 case 0x0002:
 177                                         debug_uart_tx("ext. feat. support\n");
 178                                         mdat[0] = INFORMATION_RESPONSE;
 179                                         mdat[2] = 0x02;
 180                                         mdat[3] = 0x00;
 181                                         mdat[6] = 0x01; // not supported
 182                                         mdat[7] = 0x00;
 183                                         bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, 0x08, mdat);
 184                                         break;
 185                                 case 0x0003:
 186                                         debug_uart_tx("fixed channel support\n");
 187                                         break;
 188                                 default:
 189                                         debug_uart_tx("unknown\n");
 190                                         break;
 191                         };
 192                         } break;
 193                 case INFORMATION_RESPONSE:
 194                         debug_uart_tx("information response\n");
 195                         break;
 196                 default:
 197                         debug_uart_tx("unknown\n");
 198                         break;
 199         }
 200 }
 201
 202 typedef struct {
 203         uint8_t resp;
 204         uint8_t ident;
 205         uint16_t length;
 206         uint16_t DCID;
 207         uint16_t SCID;
 208         uint16_t result;
 209         uint16_t status;
 210 } __attribute__((packed)) bt_l2cap_conn_resp_t;
 211
 212 typedef struct {
 213         uint8_t resp;
 214         uint8_t ident;
 215         uint16_t length;
 216         uint16_t DCID;
 217         uint16_t flags;
 218         uint8_t option;
 219         uint8_t olen;
 220         uint16_t odat;
 221 } __attribute__((packed)) bt_l2cap_conf_req_t;
 222
 223 void bt_l2cap_handle_connection_request(const uint16_t handle, const uint8_t ident, const uint16_t PSM, const uint16_t src_CID)
 224 {
 225         bt_l2cap_conn_resp_t resp;
 226
 227         // for now we only support one connection, only on PSM 0x1001
 228         if (_l2cap_con.cstate == BT_L2CAP_CON_IDLE && PSM == 0x1001) {
 229                 bt_l2cap_conf_req_t req;
 230
 231                 _l2cap_con.cstate = BT_L2CAP_CON_CONNECTED;
 232                 _l2cap_con.hci_handle = handle;
 233                 _l2cap_con.PSM = PSM;
 234
 235                 _l2cap_con.locCID = 0x0040;
 236                 _l2cap_con.remCID = src_CID;
 237
 238                 _l2cap_con.locMTU = 0xf0;
 239                 _l2cap_con.remMTU = 0x00;
 240
 241                 resp.resp = CONNECTION_RESPONSE;
 242                 resp.ident = ident;
 243                 resp.length = 0x0008;
 244                 resp.DCID = _l2cap_con.locCID;
 245                 resp.SCID = _l2cap_con.remCID;
 246                 resp.result = BT_L2CAP_CON_RES_SUCCESS;
 247                 resp.status = BT_L2CAP_CON_STAT_NOINFO;
 248
 249                 debug_uart_tx("l2cap accepting connection\n");
 250                 bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);
 251
 252                 req.resp = CONFIGURE_REQUEST;
 253                 req.ident = ident;
 254                 req.length = 0x08;
 255                 req.DCID = _l2cap_con.remCID;
 256                 req.flags = 0x00;
 257                 req.option = 0x01; // MTU
 258                 req.olen = 0x02;
 259                 req.odat = _l2cap_con.locMTU;
 260
 261                 bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_req_t), &req);
 262
 263                 // max_interval Mandatory Range: 0x0006 to 0x0540
 264                 // min_interval Mandatory Range: 0x0006 to 0x0540
 265                 // sniff_attempt Mandatory Range for Controller: 1 to Tsniff/2
 266                 // sniff_timeout Mandatory Range for Controller: 0 to 0x0028
 267                 bt_hci_set_sniff_mode(handle, 0x10, 0x06, 0x20, 0x10);
 268         } else {
 269                 resp.resp = CONNECTION_RESPONSE;
 270                 resp.ident = ident;
 271                 resp.length = 0x0008;
 272                 resp.DCID = 0x0000;
 273                 resp.SCID = src_CID;
 274                 resp.result = BT_L2CAP_CON_RES_REFUSED_RSRC;
 275                 resp.status = BT_L2CAP_CON_STAT_NOINFO;
 276
 277                 debug_uart_tx("l2cap refusing connection\n");
 278                 bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);
 279         }
 280 }
 281