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