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