qemu/hw/bt/l2cap.c

   1 /*
   2  * QEMU Bluetooth L2CAP logic.
   3  *
   4  * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License as
   8  * published by the Free Software Foundation; either version 2 of
   9  * the License, or (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include "qemu/osdep.h"
  21 #include "qemu-common.h"
  22 #include "qemu/timer.h"
  23 #include "hw/bt.h"
  24
  25 #define L2CAP_CID_MAX   0x100   /* Between 0x40 and 0x10000 */
  26
  27 struct l2cap_instance_s {
  28     struct bt_link_s *link;
  29     struct bt_l2cap_device_s *dev;
  30     int role;
  31
  32     uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
  33     int frame_in_len;
  34
  35     uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
  36     int frame_out_len;
  37
  38     /* Signalling channel timers.  They exist per-request but we can make
  39      * sure we have no more than one outstanding request at any time.  */
  40     QEMUTimer *rtx;
  41     QEMUTimer *ertx;
  42
  43     int last_id;
  44     int next_id;
  45
  46     struct l2cap_chan_s {
  47         struct bt_l2cap_conn_params_s params;
  48
  49         void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid,
  50                         const l2cap_hdr *hdr, int len);
  51         int mps;
  52         int min_mtu;
  53
  54         struct l2cap_instance_s *l2cap;
  55
  56         /* Only allocated channels */
  57         uint16_t remote_cid;
  58 #define L2CAP_CFG_INIT  2
  59 #define L2CAP_CFG_ACC   1
  60         int config_req_id; /* TODO: handle outgoing requests generically */
  61         int config;
  62
  63         /* Only connection-oriented channels.  Note: if we allow the tx and
  64          * rx traffic to be in different modes at any time, we need two.  */
  65         int mode;
  66
  67         /* Only flow-controlled, connection-oriented channels */
  68         uint8_t sdu[65536]; /* TODO: dynamically allocate */
  69         int len_cur, len_total;
  70         int rexmit;
  71         int monitor_timeout;
  72         QEMUTimer *monitor_timer;
  73         QEMUTimer *retransmission_timer;
  74     } *cid[L2CAP_CID_MAX];
  75     /* The channel state machine states map as following:
  76      * CLOSED           -> !cid[N]
  77      * WAIT_CONNECT     -> never occurs
  78      * WAIT_CONNECT_RSP -> never occurs
  79      * CONFIG           -> cid[N] && config < 3
  80      *   WAIT_CONFIG         -> never occurs, cid[N] && config == 0 && !config_r
  81      *   WAIT_SEND_CONFIG    -> never occurs, cid[N] && config == 1 && !config_r
  82      *   WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id
  83      *   WAIT_CONFIG_RSP     -> cid[N] && config == 1 && config_req_id
  84      *   WAIT_CONFIG_REQ     -> cid[N] && config == 2
  85      * OPEN             -> cid[N] && config == 3
  86      * WAIT_DISCONNECT  -> never occurs
  87      */
  88
  89     struct l2cap_chan_s signalling_ch;
  90     struct l2cap_chan_s group_ch;
  91 };
  92
  93 struct slave_l2cap_instance_s {
  94     struct bt_link_s link;      /* Underlying logical link (ACL) */
  95     struct l2cap_instance_s l2cap;
  96 };
  97
  98 struct bt_l2cap_psm_s {
  99     int psm;
 100     int min_mtu;
 101     int (*new_channel)(struct bt_l2cap_device_s *device,
 102                     struct bt_l2cap_conn_params_s *params);
 103     struct bt_l2cap_psm_s *next;
 104 };
 105
 106 static const uint16_t l2cap_fcs16_table[256] = {
 107     0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
 108     0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
 109     0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
 110     0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
 111     0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
 112     0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
 113     0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
 114     0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
 115     0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
 116     0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
 117     0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
 118     0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
 119     0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
 120     0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
 121     0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
 122     0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
 123     0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
 124     0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
 125     0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
 126     0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
 127     0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
 128     0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
 129     0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
 130     0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
 131     0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
 132     0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
 133     0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
 134     0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
 135     0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
 136     0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
 137     0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
 138     0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
 139 };
 140
 141 static uint16_t l2cap_fcs16(const uint8_t *message, int len)
 142 {
 143     uint16_t fcs = 0x0000;
 144
 145     while (len --)
 146 #if 0
 147     {
 148         int i;
 149
 150         fcs ^= *message ++;
 151         for (i = 8; i; -- i)
 152             if (fcs & 1)
 153                 fcs = (fcs >> 1) ^ 0xa001;
 154             else
 155                 fcs = (fcs >> 1);
 156     }
 157 #else
 158         fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff];
 159 #endif
 160
 161     return fcs;
 162 }
 163
 164 /* L2CAP layer logic (protocol) */
 165
 166 static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
 167 {
 168 #if 0
 169     if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit)
 170         timer_mod(ch->retransmission_timer);
 171     else
 172         timer_del(ch->retransmission_timer);
 173 #endif
 174 }
 175
 176 static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
 177 {
 178 #if 0
 179     if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit)
 180         timer_mod(ch->monitor_timer);
 181     else
 182         timer_del(ch->monitor_timer);
 183 #endif
 184 }
 185
 186 static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id,
 187                 uint16_t reason, const void *data, int plen)
 188 {
 189     uint8_t *pkt;
 190     l2cap_cmd_hdr *hdr;
 191     l2cap_cmd_rej *params;
 192     uint16_t len;
 193
 194     reason = cpu_to_le16(reason);
 195     len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen);
 196
 197     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 198                     L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen);
 199     hdr = (void *) (pkt + 0);
 200     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 201
 202     hdr->code = L2CAP_COMMAND_REJ;
 203     hdr->ident = id;
 204     memcpy(&hdr->len, &len, sizeof(hdr->len));
 205     memcpy(&params->reason, &reason, sizeof(reason));
 206     if (plen)
 207        memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen);
 208
 209     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 210 }
 211
 212 static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id,
 213                 uint16_t reason, uint16_t dcid, uint16_t scid)
 214 {
 215     l2cap_cmd_rej_cid params = {
 216         .dcid = dcid,
 217         .scid = scid,
 218     };
 219
 220     l2cap_command_reject(l2cap, id, reason, &params, L2CAP_CMD_REJ_CID_SIZE);
 221 }
 222
 223 static void l2cap_connection_response(struct l2cap_instance_s *l2cap,
 224                 int dcid, int scid, int result, int status)
 225 {
 226     uint8_t *pkt;
 227     l2cap_cmd_hdr *hdr;
 228     l2cap_conn_rsp *params;
 229
 230     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 231                     L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE);
 232     hdr = (void *) (pkt + 0);
 233     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 234
 235     hdr->code = L2CAP_CONN_RSP;
 236     hdr->ident = l2cap->last_id;
 237     hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE);
 238
 239     params->dcid = cpu_to_le16(dcid);
 240     params->scid = cpu_to_le16(scid);
 241     params->result = cpu_to_le16(result);
 242     params->status = cpu_to_le16(status);
 243
 244     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 245 }
 246
 247 static void l2cap_configuration_request(struct l2cap_instance_s *l2cap,
 248                 int dcid, int flag, const uint8_t *data, int len)
 249 {
 250     uint8_t *pkt;
 251     l2cap_cmd_hdr *hdr;
 252     l2cap_conf_req *params;
 253
 254     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 255                     L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len));
 256     hdr = (void *) (pkt + 0);
 257     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 258
 259     /* TODO: unify the id sequencing */
 260     l2cap->last_id = l2cap->next_id;
 261     l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
 262
 263     hdr->code = L2CAP_CONF_REQ;
 264     hdr->ident = l2cap->last_id;
 265     hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len));
 266
 267     params->dcid = cpu_to_le16(dcid);
 268     params->flags = cpu_to_le16(flag);
 269     if (len)
 270         memcpy(params->data, data, len);
 271
 272     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 273 }
 274
 275 static void l2cap_configuration_response(struct l2cap_instance_s *l2cap,
 276                 int scid, int flag, int result, const uint8_t *data, int len)
 277 {
 278     uint8_t *pkt;
 279     l2cap_cmd_hdr *hdr;
 280     l2cap_conf_rsp *params;
 281
 282     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 283                     L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len));
 284     hdr = (void *) (pkt + 0);
 285     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 286
 287     hdr->code = L2CAP_CONF_RSP;
 288     hdr->ident = l2cap->last_id;
 289     hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len));
 290
 291     params->scid = cpu_to_le16(scid);
 292     params->flags = cpu_to_le16(flag);
 293     params->result = cpu_to_le16(result);
 294     if (len)
 295         memcpy(params->data, data, len);
 296
 297     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 298 }
 299
 300 static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap,
 301                 int dcid, int scid)
 302 {
 303     uint8_t *pkt;
 304     l2cap_cmd_hdr *hdr;
 305     l2cap_disconn_rsp *params;
 306
 307     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 308                     L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE);
 309     hdr = (void *) (pkt + 0);
 310     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 311
 312     hdr->code = L2CAP_DISCONN_RSP;
 313     hdr->ident = l2cap->last_id;
 314     hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE);
 315
 316     params->dcid = cpu_to_le16(dcid);
 317     params->scid = cpu_to_le16(scid);
 318
 319     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 320 }
 321
 322 static void l2cap_echo_response(struct l2cap_instance_s *l2cap,
 323                 const uint8_t *data, int len)
 324 {
 325     uint8_t *pkt;
 326     l2cap_cmd_hdr *hdr;
 327     uint8_t *params;
 328
 329     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 330                     L2CAP_CMD_HDR_SIZE + len);
 331     hdr = (void *) (pkt + 0);
 332     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 333
 334     hdr->code = L2CAP_ECHO_RSP;
 335     hdr->ident = l2cap->last_id;
 336     hdr->len = cpu_to_le16(len);
 337
 338     memcpy(params, data, len);
 339
 340     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 341 }
 342
 343 static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type,
 344                 int result, const uint8_t *data, int len)
 345 {
 346     uint8_t *pkt;
 347     l2cap_cmd_hdr *hdr;
 348     l2cap_info_rsp *params;
 349
 350     pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
 351                     L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len);
 352     hdr = (void *) (pkt + 0);
 353     params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
 354
 355     hdr->code = L2CAP_INFO_RSP;
 356     hdr->ident = l2cap->last_id;
 357     hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len);
 358
 359     params->type = cpu_to_le16(type);
 360     params->result = cpu_to_le16(result);
 361     if (len)
 362        memcpy(params->data, data, len);
 363
 364     l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
 365 }
 366
 367 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len);
 368 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms);
 369 #if 0
 370 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len);
 371 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm);
 372 #endif
 373 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 374                 const l2cap_hdr *hdr, int len);
 375 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 376                 const l2cap_hdr *hdr, int len);
 377
 378 static int l2cap_cid_new(struct l2cap_instance_s *l2cap)
 379 {
 380     int i;
 381
 382     for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++)
 383         if (!l2cap->cid[i])
 384             return i;
 385
 386     return L2CAP_CID_INVALID;
 387 }
 388
 389 static inline struct bt_l2cap_psm_s *l2cap_psm(
 390                 struct bt_l2cap_device_s *device, int psm)
 391 {
 392     struct bt_l2cap_psm_s *ret = device->first_psm;
 393
 394     while (ret && ret->psm != psm)
 395         ret = ret->next;
 396
 397     return ret;
 398 }
 399
 400 static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
 401                 int psm, int source_cid)
 402 {
 403     struct l2cap_chan_s *ch = NULL;
 404     struct bt_l2cap_psm_s *psm_info;
 405     int result, status;
 406     int cid = l2cap_cid_new(l2cap);
 407
 408     if (cid) {
 409         /* See what the channel is to be used for.. */
 410         psm_info = l2cap_psm(l2cap->dev, psm);
 411
 412         if (psm_info) {
 413             /* Device supports this use-case.  */
 414             ch = g_malloc0(sizeof(*ch));
 415             ch->params.sdu_out = l2cap_bframe_out;
 416             ch->params.sdu_submit = l2cap_bframe_submit;
 417             ch->frame_in = l2cap_bframe_in;
 418             ch->mps = 65536;
 419             ch->min_mtu = MAX(48, psm_info->min_mtu);
 420             ch->params.remote_mtu = MAX(672, ch->min_mtu);
 421             ch->remote_cid = source_cid;
 422             ch->mode = L2CAP_MODE_BASIC;
 423             ch->l2cap = l2cap;
 424
 425             /* Does it feel like opening yet another channel though?  */
 426             if (!psm_info->new_channel(l2cap->dev, &ch->params)) {
 427                 l2cap->cid[cid] = ch;
 428
 429                 result = L2CAP_CR_SUCCESS;
 430                 status = L2CAP_CS_NO_INFO;
 431             } else {
 432                 g_free(ch);
 433                 ch = NULL;
 434                 result = L2CAP_CR_NO_MEM;
 435                 status = L2CAP_CS_NO_INFO;
 436             }
 437         } else {
 438             result = L2CAP_CR_BAD_PSM;
 439             status = L2CAP_CS_NO_INFO;
 440         }
 441     } else {
 442         result = L2CAP_CR_NO_MEM;
 443         status = L2CAP_CS_NO_INFO;
 444     }
 445
 446     l2cap_connection_response(l2cap, cid, source_cid, result, status);
 447
 448     return ch;
 449 }
 450
 451 static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
 452                 int cid, int source_cid)
 453 {
 454     struct l2cap_chan_s *ch = NULL;
 455
 456     /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a
 457      * connection in CLOSED state still responds with a L2CAP_DisconnectRsp
 458      * message on an L2CAP_DisconnectReq event.  */
 459     if (unlikely(cid < L2CAP_CID_ALLOC)) {
 460         l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
 461                         cid, source_cid);
 462         return;
 463     }
 464     if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
 465         ch = l2cap->cid[cid];
 466
 467     if (likely(ch)) {
 468         if (ch->remote_cid != source_cid) {
 469             fprintf(stderr, "%s: Ignoring a Disconnection Request with the "
 470                             "invalid SCID %04x.\n", __FUNCTION__, source_cid);
 471             return;
 472         }
 473
 474         l2cap->cid[cid] = NULL;
 475
 476         ch->params.close(ch->params.opaque);
 477         g_free(ch);
 478     }
 479
 480     l2cap_disconnection_response(l2cap, cid, source_cid);
 481 }
 482
 483 static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap,
 484                 struct l2cap_chan_s *ch)
 485 {
 486     l2cap_configuration_request(l2cap, ch->remote_cid, 0, NULL, 0);
 487     ch->config_req_id = l2cap->last_id;
 488     ch->config &= ~L2CAP_CFG_INIT;
 489 }
 490
 491 static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap,
 492                 struct l2cap_chan_s *ch)
 493 {
 494     /* Use all default channel options and terminate negotiation.  */
 495     l2cap_channel_config_null(l2cap, ch);
 496 }
 497
 498 static int l2cap_channel_config(struct l2cap_instance_s *l2cap,
 499                 struct l2cap_chan_s *ch, int flag,
 500                 const uint8_t *data, int len)
 501 {
 502     l2cap_conf_opt *opt;
 503     l2cap_conf_opt_qos *qos;
 504     uint32_t val;
 505     uint8_t rsp[len];
 506     int result = L2CAP_CONF_SUCCESS;
 507
 508     data = memcpy(rsp, data, len);
 509     while (len) {
 510         opt = (void *) data;
 511
 512         if (len < L2CAP_CONF_OPT_SIZE ||
 513                         len < L2CAP_CONF_OPT_SIZE + opt->len) {
 514             result = L2CAP_CONF_REJECT;
 515             break;
 516         }
 517         data += L2CAP_CONF_OPT_SIZE + opt->len;
 518         len -= L2CAP_CONF_OPT_SIZE + opt->len;
 519
 520         switch (opt->type & 0x7f) {
 521         case L2CAP_CONF_MTU:
 522             if (opt->len != 2) {
 523                 result = L2CAP_CONF_REJECT;
 524                 break;
 525             }
 526
 527             /* MTU */
 528             val = le16_to_cpup((void *) opt->val);
 529             if (val < ch->min_mtu) {
 530                 cpu_to_le16w((void *) opt->val, ch->min_mtu);
 531                 result = L2CAP_CONF_UNACCEPT;
 532                 break;
 533             }
 534
 535             ch->params.remote_mtu = val;
 536             break;
 537
 538         case L2CAP_CONF_FLUSH_TO:
 539             if (opt->len != 2) {
 540                 result = L2CAP_CONF_REJECT;
 541                 break;
 542             }
 543
 544             /* Flush Timeout */
 545             val = le16_to_cpup((void *) opt->val);
 546             if (val < 0x0001) {
 547                 opt->val[0] = 0xff;
 548                 opt->val[1] = 0xff;
 549                 result = L2CAP_CONF_UNACCEPT;
 550                 break;
 551             }
 552             break;
 553
 554         case L2CAP_CONF_QOS:
 555             if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) {
 556                 result = L2CAP_CONF_REJECT;
 557                 break;
 558             }
 559             qos = (void *) opt->val;
 560
 561             /* Flags */
 562             val = qos->flags;
 563             if (val) {
 564                 qos->flags = 0;
 565                 result = L2CAP_CONF_UNACCEPT;
 566             }
 567
 568             /* Service type */
 569             val = qos->service_type;
 570             if (val != L2CAP_CONF_QOS_BEST_EFFORT &&
 571                             val != L2CAP_CONF_QOS_NO_TRAFFIC) {
 572                 qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT;
 573                 result = L2CAP_CONF_UNACCEPT;
 574             }
 575
 576             if (val != L2CAP_CONF_QOS_NO_TRAFFIC) {
 577                 /* XXX: These values should possibly be calculated
 578                  * based on LM / baseband properties also.  */
 579
 580                 /* Token rate */
 581                 val = le32_to_cpu(qos->token_rate);
 582                 if (val == L2CAP_CONF_QOS_WILDCARD)
 583                     qos->token_rate = cpu_to_le32(0x100000);
 584
 585                 /* Token bucket size */
 586                 val = le32_to_cpu(qos->token_bucket_size);
 587                 if (val == L2CAP_CONF_QOS_WILDCARD)
 588                     qos->token_bucket_size = cpu_to_le32(65500);
 589
 590                 /* Any Peak bandwidth value is correct to return as-is */
 591                 /* Any Access latency value is correct to return as-is */
 592                 /* Any Delay variation value is correct to return as-is */
 593             }
 594             break;
 595
 596         case L2CAP_CONF_RFC:
 597             if (opt->len != 9) {
 598                 result = L2CAP_CONF_REJECT;
 599                 break;
 600             }
 601
 602             /* Mode */
 603             val = opt->val[0];
 604             switch (val) {
 605             case L2CAP_MODE_BASIC:
 606                 ch->mode = val;
 607                 ch->frame_in = l2cap_bframe_in;
 608
 609                 /* All other parameters shall be ignored */
 610                 break;
 611
 612             case L2CAP_MODE_RETRANS:
 613             case L2CAP_MODE_FLOWCTL:
 614                 ch->mode = val;
 615                 ch->frame_in = l2cap_iframe_in;
 616                 /* Note: most of these parameters refer to incoming traffic
 617                  * so we don't need to save them as long as we can accept
 618                  * incoming PDUs at any values of the parameters.  */
 619
 620                 /* TxWindow size */
 621                 val = opt->val[1];
 622                 if (val < 1 || val > 32) {
 623                     opt->val[1] = 32;
 624                     result = L2CAP_CONF_UNACCEPT;
 625                     break;
 626                 }
 627
 628                 /* MaxTransmit */
 629                 val = opt->val[2];
 630                 if (val < 1) {
 631                     opt->val[2] = 1;
 632                     result = L2CAP_CONF_UNACCEPT;
 633                     break;
 634                 }
 635
 636                 /* Remote Retransmission time-out shouldn't affect local
 637                  * operation (?) */
 638
 639                 /* The Monitor time-out drives the local Monitor timer (?),
 640                  * so save the value.  */
 641                 val = (opt->val[6] << 8) | opt->val[5];
 642                 if (val < 30) {
 643                     opt->val[5] = 100 & 0xff;
 644                     opt->val[6] = 100 >> 8;
 645                     result = L2CAP_CONF_UNACCEPT;
 646                     break;
 647                 }
 648                 ch->monitor_timeout = val;
 649                 l2cap_monitor_timer_update(ch);
 650
 651                 /* MPS */
 652                 val = (opt->val[8] << 8) | opt->val[7];
 653                 if (val < ch->min_mtu) {
 654                     opt->val[7] = ch->min_mtu & 0xff;
 655                     opt->val[8] = ch->min_mtu >> 8;
 656                     result = L2CAP_CONF_UNACCEPT;
 657                     break;
 658                 }
 659                 ch->mps = val;
 660                 break;
 661
 662             default:
 663                 result = L2CAP_CONF_UNACCEPT;
 664                 break;
 665             }
 666             break;
 667
 668         default:
 669             if (!(opt->type >> 7))
 670                 result = L2CAP_CONF_UNKNOWN;
 671             break;
 672         }
 673
 674         if (result != L2CAP_CONF_SUCCESS)
 675             break;      /* XXX: should continue? */
 676     }
 677
 678     l2cap_configuration_response(l2cap, ch->remote_cid,
 679                     flag, result, rsp, len);
 680
 681     return result == L2CAP_CONF_SUCCESS && !flag;
 682 }
 683
 684 static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap,
 685                 int flag, int cid, const uint8_t *data, int len)
 686 {
 687     struct l2cap_chan_s *ch;
 688
 689     if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
 690         l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
 691                         cid, 0x0000);
 692         return;
 693     }
 694     ch = l2cap->cid[cid];
 695
 696     /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to
 697      * WAIT_CONFIG_REQ_RSP.  This is assuming the transition chart for OPEN
 698      * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake
 699      * and on options-acceptable we go back to OPEN and otherwise to
 700      * WAIT_CONFIG_REQ and not the other way.  */
 701     ch->config &= ~L2CAP_CFG_ACC;
 702
 703     if (l2cap_channel_config(l2cap, ch, flag, data, len))
 704         /* Go to OPEN or WAIT_CONFIG_RSP */
 705         ch->config |= L2CAP_CFG_ACC;
 706
 707     /* TODO: if the incoming traffic flow control or retransmission mode
 708      * changed then we probably need to also generate the
 709      * ConfigureChannel_Req event and set the outgoing traffic to the same
 710      * mode.  */
 711     if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) &&
 712                     !ch->config_req_id)
 713         l2cap_channel_config_req_event(l2cap, ch);
 714 }
 715
 716 static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap,
 717                 int result, int flag, int cid, const uint8_t *data, int len)
 718 {
 719     struct l2cap_chan_s *ch;
 720
 721     if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
 722         l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
 723                         cid, 0x0000);
 724         return 0;
 725     }
 726     ch = l2cap->cid[cid];
 727
 728     if (ch->config_req_id != l2cap->last_id)
 729         return 1;
 730     ch->config_req_id = 0;
 731
 732     if (result == L2CAP_CONF_SUCCESS) {
 733         if (!flag)
 734             ch->config |= L2CAP_CFG_INIT;
 735         else
 736             l2cap_channel_config_null(l2cap, ch);
 737     } else
 738         /* Retry until we succeed */
 739         l2cap_channel_config_req_event(l2cap, ch);
 740
 741     return 0;
 742 }
 743
 744 static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap,
 745                 int psm, int source_cid)
 746 {
 747     struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid);
 748
 749     if (!ch)
 750         return;
 751
 752     /* Optional */
 753     if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id)
 754         l2cap_channel_config_req_event(l2cap, ch);
 755 }
 756
 757 static void l2cap_info(struct l2cap_instance_s *l2cap, int type)
 758 {
 759     uint8_t data[4];
 760     int len = 0;
 761     int result = L2CAP_IR_SUCCESS;
 762
 763     switch (type) {
 764     case L2CAP_IT_CL_MTU:
 765         data[len ++] = l2cap->group_ch.mps & 0xff;
 766         data[len ++] = l2cap->group_ch.mps >> 8;
 767         break;
 768
 769     case L2CAP_IT_FEAT_MASK:
 770         /* (Prematurely) report Flow control and Retransmission modes.  */
 771         data[len ++] = 0x03;
 772         data[len ++] = 0x00;
 773         data[len ++] = 0x00;
 774         data[len ++] = 0x00;
 775         break;
 776
 777     default:
 778         result = L2CAP_IR_NOTSUPP;
 779     }
 780
 781     l2cap_info_response(l2cap, type, result, data, len);
 782 }
 783
 784 static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id,
 785                 const uint8_t *params, int len)
 786 {
 787     int err;
 788
 789 #if 0
 790     /* TODO: do the IDs really have to be in sequence?  */
 791     if (!id || (id != l2cap->last_id && id != l2cap->next_id)) {
 792         fprintf(stderr, "%s: out of sequence command packet ignored.\n",
 793                         __FUNCTION__);
 794         return;
 795     }
 796 #else
 797     l2cap->next_id = id;
 798 #endif
 799     if (id == l2cap->next_id) {
 800         l2cap->last_id = l2cap->next_id;
 801         l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
 802     } else {
 803         /* TODO: Need to re-send the same response, without re-executing
 804          * the corresponding command!  */
 805     }
 806
 807     switch (code) {
 808     case L2CAP_COMMAND_REJ:
 809         if (unlikely(len != 2 && len != 4 && len != 6)) {
 810             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 811             goto reject;
 812         }
 813
 814         /* We never issue commands other than Command Reject currently.  */
 815         fprintf(stderr, "%s: stray Command Reject (%02x, %04x) "
 816                         "packet, ignoring.\n", __FUNCTION__, id,
 817                         le16_to_cpu(((l2cap_cmd_rej *) params)->reason));
 818         break;
 819
 820     case L2CAP_CONN_REQ:
 821         if (unlikely(len != L2CAP_CONN_REQ_SIZE)) {
 822             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 823             goto reject;
 824         }
 825
 826         l2cap_channel_open_req_msg(l2cap,
 827                         le16_to_cpu(((l2cap_conn_req *) params)->psm),
 828                         le16_to_cpu(((l2cap_conn_req *) params)->scid));
 829         break;
 830
 831     case L2CAP_CONN_RSP:
 832         if (unlikely(len != L2CAP_CONN_RSP_SIZE)) {
 833             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 834             goto reject;
 835         }
 836
 837         /* We never issue Connection Requests currently. TODO  */
 838         fprintf(stderr, "%s: unexpected Connection Response (%02x) "
 839                         "packet, ignoring.\n", __FUNCTION__, id);
 840         break;
 841
 842     case L2CAP_CONF_REQ:
 843         if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) {
 844             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 845             goto reject;
 846         }
 847
 848         l2cap_channel_config_req_msg(l2cap,
 849                         le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1,
 850                         le16_to_cpu(((l2cap_conf_req *) params)->dcid),
 851                         ((l2cap_conf_req *) params)->data,
 852                         len - L2CAP_CONF_REQ_SIZE(0));
 853         break;
 854
 855     case L2CAP_CONF_RSP:
 856         if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) {
 857             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 858             goto reject;
 859         }
 860
 861         if (l2cap_channel_config_rsp_msg(l2cap,
 862                         le16_to_cpu(((l2cap_conf_rsp *) params)->result),
 863                         le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1,
 864                         le16_to_cpu(((l2cap_conf_rsp *) params)->scid),
 865                         ((l2cap_conf_rsp *) params)->data,
 866                         len - L2CAP_CONF_RSP_SIZE(0)))
 867             fprintf(stderr, "%s: unexpected Configure Response (%02x) "
 868                             "packet, ignoring.\n", __FUNCTION__, id);
 869         break;
 870
 871     case L2CAP_DISCONN_REQ:
 872         if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) {
 873             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 874             goto reject;
 875         }
 876
 877         l2cap_channel_close(l2cap,
 878                         le16_to_cpu(((l2cap_disconn_req *) params)->dcid),
 879                         le16_to_cpu(((l2cap_disconn_req *) params)->scid));
 880         break;
 881
 882     case L2CAP_DISCONN_RSP:
 883         if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) {
 884             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 885             goto reject;
 886         }
 887
 888         /* We never issue Disconnection Requests currently. TODO  */
 889         fprintf(stderr, "%s: unexpected Disconnection Response (%02x) "
 890                         "packet, ignoring.\n", __FUNCTION__, id);
 891         break;
 892
 893     case L2CAP_ECHO_REQ:
 894         l2cap_echo_response(l2cap, params, len);
 895         break;
 896
 897     case L2CAP_ECHO_RSP:
 898         /* We never issue Echo Requests currently. TODO  */
 899         fprintf(stderr, "%s: unexpected Echo Response (%02x) "
 900                         "packet, ignoring.\n", __FUNCTION__, id);
 901         break;
 902
 903     case L2CAP_INFO_REQ:
 904         if (unlikely(len != L2CAP_INFO_REQ_SIZE)) {
 905             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 906             goto reject;
 907         }
 908
 909         l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type));
 910         break;
 911
 912     case L2CAP_INFO_RSP:
 913         if (unlikely(len != L2CAP_INFO_RSP_SIZE)) {
 914             err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 915             goto reject;
 916         }
 917
 918         /* We never issue Information Requests currently. TODO  */
 919         fprintf(stderr, "%s: unexpected Information Response (%02x) "
 920                         "packet, ignoring.\n", __FUNCTION__, id);
 921         break;
 922
 923     default:
 924         err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
 925     reject:
 926         l2cap_command_reject(l2cap, id, err, 0, 0);
 927         break;
 928     }
 929 }
 930
 931 static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable)
 932 {
 933     ch->rexmit = enable;
 934
 935     l2cap_retransmission_timer_update(ch);
 936     l2cap_monitor_timer_update(ch);
 937 }
 938
 939 /* Command frame SDU */
 940 static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len)
 941 {
 942     struct l2cap_instance_s *l2cap = opaque;
 943     const l2cap_cmd_hdr *hdr;
 944     int clen;
 945
 946     while (len) {
 947         hdr = (void *) data;
 948         if (len < L2CAP_CMD_HDR_SIZE)
 949             /* TODO: signal an error */
 950             return;
 951         len -= L2CAP_CMD_HDR_SIZE;
 952         data += L2CAP_CMD_HDR_SIZE;
 953
 954         clen = le16_to_cpu(hdr->len);
 955         if (len < clen) {
 956             l2cap_command_reject(l2cap, hdr->ident,
 957                             L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0);
 958             break;
 959         }
 960
 961         l2cap_command(l2cap, hdr->code, hdr->ident, data, clen);
 962         len -= clen;
 963         data += clen;
 964     }
 965 }
 966
 967 /* Group frame SDU */
 968 static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
 969 {
 970 }
 971
 972 /* Supervisory frame */
 973 static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
 974 {
 975 }
 976
 977 /* Basic L2CAP mode Information frame */
 978 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 979                 const l2cap_hdr *hdr, int len)
 980 {
 981     /* We have a full SDU, no further processing */
 982     ch->params.sdu_in(ch->params.opaque, hdr->data, len);
 983 }
 984
 985 /* Flow Control and Retransmission mode frame */
 986 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 987                 const l2cap_hdr *hdr, int len)
 988 {
 989     uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2));
 990
 991     if (len < 4)
 992         goto len_error;
 993     if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs)
 994         goto fcs_error;
 995
 996     if ((hdr->data[0] >> 7) == ch->rexmit)
 997         l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
 998
 999     if (hdr->data[0] & 1) {
1000         if (len != 4) {
1001             /* TODO: Signal an error? */
1002             return;
1003         }
1004         l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data));
1005         return;
1006     }
1007
1008     switch (hdr->data[1] >> 6) {        /* SAR */
1009     case L2CAP_SAR_NO_SEG:
1010         if (ch->len_total)
1011             goto seg_error;
1012         if (len - 4 > ch->mps)
1013             goto len_error;
1014
1015         ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
1016         break;
1017
1018     case L2CAP_SAR_START:
1019         if (ch->len_total || len < 6)
1020             goto seg_error;
1021         if (len - 6 > ch->mps)
1022             goto len_error;
1023
1024         ch->len_total = le16_to_cpup((void *) (hdr->data + 2));
1025         if (len >= 6 + ch->len_total)
1026             goto seg_error;
1027
1028         ch->len_cur = len - 6;
1029         memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
1030         break;
1031
1032     case L2CAP_SAR_END:
1033         if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
1034             goto seg_error;
1035         if (len - 4 > ch->mps)
1036             goto len_error;
1037
1038         memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1039         ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
1040         break;
1041
1042     case L2CAP_SAR_CONT:
1043         if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
1044             goto seg_error;
1045         if (len - 4 > ch->mps)
1046             goto len_error;
1047
1048         memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1049         ch->len_cur += len - 4;
1050         break;
1051
1052     seg_error:
1053     len_error:  /* TODO */
1054     fcs_error:  /* TODO */
1055         ch->len_cur = 0;
1056         ch->len_total = 0;
1057         break;
1058     }
1059 }
1060
1061 static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
1062                 const l2cap_hdr *frame)
1063 {
1064     uint16_t cid = le16_to_cpu(frame->cid);
1065     uint16_t len = le16_to_cpu(frame->len);
1066
1067     if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
1068         fprintf(stderr, "%s: frame addressed to a non-existent L2CAP "
1069                         "channel %04x received.\n", __FUNCTION__, cid);
1070         return;
1071     }
1072
1073     l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
1074 }
1075
1076 /* "Recombination" */
1077 static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
1078                 const uint8_t *data, int len)
1079 {
1080     const l2cap_hdr *hdr = (void *) l2cap->frame_in;
1081
1082     if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
1083         if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
1084             memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
1085                             sizeof(l2cap->frame_in) - l2cap->frame_in_len);
1086             l2cap->frame_in_len = sizeof(l2cap->frame_in);
1087             /* TODO: truncate */
1088             l2cap_frame_in(l2cap, hdr);
1089         }
1090
1091         return;
1092     }
1093
1094     memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
1095     l2cap->frame_in_len += len;
1096
1097     if (len >= L2CAP_HDR_SIZE)
1098         if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
1099             l2cap_frame_in(l2cap, hdr);
1100             /* There is never a start of a new PDU in the same ACL packet, so
1101              * no need to memmove the remaining payload and loop.  */
1102 }
1103
1104 static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
1105                 uint16_t cid, uint16_t len)
1106 {
1107     l2cap_hdr *hdr = (void *) l2cap->frame_out;
1108
1109     l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
1110
1111     hdr->cid = cpu_to_le16(cid);
1112     hdr->len = cpu_to_le16(len);
1113
1114     return l2cap->frame_out + L2CAP_HDR_SIZE;
1115 }
1116
1117 static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
1118 {
1119     /* TODO: Fragmentation */
1120     (l2cap->role ?
1121      l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
1122             (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
1123 }
1124
1125 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1126 {
1127     struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1128
1129     if (len > chan->params.remote_mtu) {
1130         fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n",
1131                         __FUNCTION__,
1132                         chan->remote_cid, chan->params.remote_mtu);
1133         exit(-1);
1134     }
1135
1136     return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
1137 }
1138
1139 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
1140 {
1141     struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
1142
1143     l2cap_pdu_submit(chan->l2cap);
1144 }
1145
1146 #if 0
1147 /* Stub: Only used if an emulated device requests outgoing flow control */
1148 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1149 {
1150     struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1151
1152     if (len > chan->params.remote_mtu) {
1153         /* TODO: slice into segments and queue each segment as a separate
1154          * I-Frame in a FIFO of I-Frames, local to the CID.  */
1155     } else {
1156         /* TODO: add to the FIFO of I-Frames, local to the CID.  */
1157         /* Possibly we need to return a pointer to a contiguous buffer
1158          * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
1159          * while segmenting at the same time.  */
1160     }
1161     return 0;
1162 }
1163
1164 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
1165 {
1166     /* TODO: If flow control indicates clear to send, start submitting the
1167      * invidual I-Frames from the FIFO, but don't remove them from there.
1168      * Kick the appropriate timer until we get an S-Frame, and only then
1169      * remove from FIFO or resubmit and re-kick the timer if the timer
1170      * expired.  */
1171 }
1172 #endif
1173
1174 static void l2cap_init(struct l2cap_instance_s *l2cap,
1175                 struct bt_link_s *link, int role)
1176 {
1177     l2cap->link = link;
1178     l2cap->role = role;
1179     l2cap->dev = (struct bt_l2cap_device_s *)
1180             (role ? link->host : link->slave);
1181
1182     l2cap->next_id = 1;
1183
1184     /* Establish the signalling channel */
1185     l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
1186     l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
1187     l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
1188     l2cap->signalling_ch.params.opaque = l2cap;
1189     l2cap->signalling_ch.params.remote_mtu = 48;
1190     l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
1191     l2cap->signalling_ch.frame_in = l2cap_bframe_in;
1192     l2cap->signalling_ch.mps = 65536;
1193     l2cap->signalling_ch.min_mtu = 48;
1194     l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
1195     l2cap->signalling_ch.l2cap = l2cap;
1196     l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
1197
1198     /* Establish the connection-less data channel */
1199     l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
1200     l2cap->group_ch.params.opaque = l2cap;
1201     l2cap->group_ch.frame_in = l2cap_bframe_in;
1202     l2cap->group_ch.mps = 65533;
1203     l2cap->group_ch.l2cap = l2cap;
1204     l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
1205     l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
1206 }
1207
1208 static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
1209 {
1210     int cid;
1211
1212     /* Don't send DISCONNECT if we are currently handling a DISCONNECT
1213      * sent from the other side.  */
1214     if (send_disconnect) {
1215         if (l2cap->role)
1216             l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
1217             /* l2cap->link is invalid from now on.  */
1218         else
1219             l2cap->dev->device.lmp_disconnect_master(l2cap->link);
1220     }
1221
1222     for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
1223         if (l2cap->cid[cid]) {
1224             l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
1225             g_free(l2cap->cid[cid]);
1226         }
1227
1228     if (l2cap->role)
1229         g_free(l2cap);
1230     else
1231         g_free(l2cap->link);
1232 }
1233
1234 /* L2CAP glue to lower layers in bluetooth stack (LMP) */
1235
1236 static void l2cap_lmp_connection_request(struct bt_link_s *link)
1237 {
1238     struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
1239     struct slave_l2cap_instance_s *l2cap;
1240
1241     /* Always accept - we only get called if (dev->device->page_scan).  */
1242
1243     l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s));
1244     l2cap->link.slave = &dev->device;
1245     l2cap->link.host = link->host;
1246     l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
1247
1248     /* Always at the end */
1249     link->host->reject_reason = 0;
1250     link->host->lmp_connection_complete(&l2cap->link);
1251 }
1252
1253 /* Stub */
1254 static void l2cap_lmp_connection_complete(struct bt_link_s *link)
1255 {
1256     struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1257     struct l2cap_instance_s *l2cap;
1258
1259     if (dev->device.reject_reason) {
1260         /* Signal to upper layer */
1261         return;
1262     }
1263
1264     l2cap = g_malloc0(sizeof(struct l2cap_instance_s));
1265     l2cap_init(l2cap, link, 1);
1266
1267     link->acl_mode = acl_active;
1268
1269     /* Signal to upper layer */
1270 }
1271
1272 /* Stub */
1273 static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
1274 {
1275     struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1276     struct l2cap_instance_s *l2cap =
1277             /* TODO: Retrieve from upper layer */ (void *) dev;
1278
1279     /* Signal to upper layer */
1280
1281     l2cap_teardown(l2cap, 0);
1282 }
1283
1284 static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
1285 {
1286     struct slave_l2cap_instance_s *l2cap =
1287             (struct slave_l2cap_instance_s *) link;
1288
1289     l2cap_teardown(&l2cap->l2cap, 0);
1290 }
1291
1292 static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
1293                 const uint8_t *data, int start, int len)
1294 {
1295     struct slave_l2cap_instance_s *l2cap =
1296             (struct slave_l2cap_instance_s *) link;
1297
1298     if (start)
1299         l2cap->l2cap.frame_in_len = 0;
1300
1301     l2cap_pdu_in(&l2cap->l2cap, data, len);
1302 }
1303
1304 /* Stub */
1305 static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
1306                 const uint8_t *data, int start, int len)
1307 {
1308     struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1309     struct l2cap_instance_s *l2cap =
1310             /* TODO: Retrieve from upper layer */ (void *) dev;
1311
1312     if (start)
1313         l2cap->frame_in_len = 0;
1314
1315     l2cap_pdu_in(l2cap, data, len);
1316 }
1317
1318 static void l2cap_dummy_destroy(struct bt_device_s *dev)
1319 {
1320     struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
1321
1322     bt_l2cap_device_done(l2cap_dev);
1323 }
1324
1325 void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
1326                 struct bt_scatternet_s *net)
1327 {
1328     bt_device_init(&dev->device, net);
1329
1330     dev->device.lmp_connection_request = l2cap_lmp_connection_request;
1331     dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
1332     dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
1333     dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
1334     dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
1335     dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
1336
1337     dev->device.handle_destroy = l2cap_dummy_destroy;
1338 }
1339
1340 void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
1341 {
1342     bt_device_done(&dev->device);
1343
1344     /* Should keep a list of all instances and go through it and
1345      * invoke l2cap_teardown() for each.  */
1346 }
1347
1348 void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
1349                 int (*new_channel)(struct bt_l2cap_device_s *dev,
1350                         struct bt_l2cap_conn_params_s *params))
1351 {
1352     struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
1353
1354     if (new_psm) {
1355         fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n",
1356                         __FUNCTION__, psm, dev->device.lmp_name);
1357         exit(-1);
1358     }
1359
1360     new_psm = g_malloc0(sizeof(*new_psm));
1361     new_psm->psm = psm;
1362     new_psm->min_mtu = min_mtu;
1363     new_psm->new_channel = new_channel;
1364     new_psm->next = dev->first_psm;
1365     dev->first_psm = new_psm;
1366 }