hw/bt/l2cap.c

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