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