qemu/hw/bt/l2cap.c
<<
>>
Prefs
   1/*
   2 * QEMU Bluetooth L2CAP logic.
   3 *
   4 * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   5 *
   6 * This program is free software; you can redistribute it and/or
   7 * modify it under the terms of the GNU General Public License as
   8 * published by the Free Software Foundation; either version 2 of
   9 * the License, or (at your option) any later version.
  10 *
  11 * This program is distributed in the hope that it will be useful,
  12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 * GNU General Public License for more details.
  15 *
  16 * You should have received a copy of the GNU General Public License
  17 * along with this program; if not, see <http://www.gnu.org/licenses/>.
  18 */
  19
  20#include "qemu/osdep.h"
  21#include "qemu-common.h"
  22#include "qemu/timer.h"
  23#include "qemu/bswap.h"
  24#include "hw/bt.h"
  25
  26#define L2CAP_CID_MAX   0x100   /* Between 0x40 and 0x10000 */
  27
  28struct 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
  94struct slave_l2cap_instance_s {
  95    struct bt_link_s link;      /* Underlying logical link (ACL) */
  96    struct l2cap_instance_s l2cap;
  97};
  98
  99struct 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
 107static 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
 142static 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
 167static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
 168{
 169#if 0
 170    if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit)
 171        timer_mod(ch->retransmission_timer);
 172    else
 173        timer_del(ch->retransmission_timer);
 174#endif
 175}
 176
 177static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
 178{
 179#if 0
 180    if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit)
 181        timer_mod(ch->monitor_timer);
 182    else
 183        timer_del(ch->monitor_timer);
 184#endif
 185}
 186
 187static 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
 213static 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
 224static 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
 248static 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
 276static 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
 301static 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
 323static 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
 344static 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
 368static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len);
 369static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms);
 370#if 0
 371static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len);
 372static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm);
 373#endif
 374static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 375                const l2cap_hdr *hdr, int len);
 376static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 377                const l2cap_hdr *hdr, int len);
 378
 379static 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
 390static 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
 401static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
 402                int psm, int source_cid)
 403{
 404    struct l2cap_chan_s *ch = NULL;
 405    struct bt_l2cap_psm_s *psm_info;
 406    int result, status;
 407    int cid = l2cap_cid_new(l2cap);
 408
 409    if (cid) {
 410        /* See what the channel is to be used for.. */
 411        psm_info = l2cap_psm(l2cap->dev, psm);
 412
 413        if (psm_info) {
 414            /* Device supports this use-case.  */
 415            ch = g_malloc0(sizeof(*ch));
 416            ch->params.sdu_out = l2cap_bframe_out;
 417            ch->params.sdu_submit = l2cap_bframe_submit;
 418            ch->frame_in = l2cap_bframe_in;
 419            ch->mps = 65536;
 420            ch->min_mtu = MAX(48, psm_info->min_mtu);
 421            ch->params.remote_mtu = MAX(672, ch->min_mtu);
 422            ch->remote_cid = source_cid;
 423            ch->mode = L2CAP_MODE_BASIC;
 424            ch->l2cap = l2cap;
 425
 426            /* Does it feel like opening yet another channel though?  */
 427            if (!psm_info->new_channel(l2cap->dev, &ch->params)) {
 428                l2cap->cid[cid] = ch;
 429
 430                result = L2CAP_CR_SUCCESS;
 431                status = L2CAP_CS_NO_INFO;
 432            } else {
 433                g_free(ch);
 434                ch = NULL;
 435                result = L2CAP_CR_NO_MEM;
 436                status = L2CAP_CS_NO_INFO;
 437            }
 438        } else {
 439            result = L2CAP_CR_BAD_PSM;
 440            status = L2CAP_CS_NO_INFO;
 441        }
 442    } else {
 443        result = L2CAP_CR_NO_MEM;
 444        status = L2CAP_CS_NO_INFO;
 445    }
 446
 447    l2cap_connection_response(l2cap, cid, source_cid, result, status);
 448
 449    return ch;
 450}
 451
 452static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
 453                int cid, int source_cid)
 454{
 455    struct l2cap_chan_s *ch = NULL;
 456
 457    /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a
 458     * connection in CLOSED state still responds with a L2CAP_DisconnectRsp
 459     * message on an L2CAP_DisconnectReq event.  */
 460    if (unlikely(cid < L2CAP_CID_ALLOC)) {
 461        l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
 462                        cid, source_cid);
 463        return;
 464    }
 465    if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
 466        ch = l2cap->cid[cid];
 467
 468    if (likely(ch)) {
 469        if (ch->remote_cid != source_cid) {
 470            fprintf(stderr, "%s: Ignoring a Disconnection Request with the "
 471                            "invalid SCID %04x.\n", __FUNCTION__, source_cid);
 472            return;
 473        }
 474
 475        l2cap->cid[cid] = NULL;
 476
 477        ch->params.close(ch->params.opaque);
 478        g_free(ch);
 479    }
 480
 481    l2cap_disconnection_response(l2cap, cid, source_cid);
 482}
 483
 484static 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
 492static 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
 499static 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 = lduw_le_p(opt->val);
 530            if (val < ch->min_mtu) {
 531                stw_le_p(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 = lduw_le_p(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
 685static 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
 717static 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
 745static 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
 758static 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
 785static 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
 932static 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 */
 941static 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 */
 969static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
 970{
 971}
 972
 973/* Supervisory frame */
 974static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
 975{
 976}
 977
 978/* Basic L2CAP mode Information frame */
 979static 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 */
 987static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
 988                const l2cap_hdr *hdr, int len)
 989{
 990    uint16_t fcs = lduw_le_p(hdr->data + len - 2);
 991
 992    if (len < 4)
 993        goto len_error;
 994    if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs)
 995        goto fcs_error;
 996
 997    if ((hdr->data[0] >> 7) == ch->rexmit)
 998        l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
 999
1000    if (hdr->data[0] & 1) {
1001        if (len != 4) {
1002            /* TODO: Signal an error? */
1003            return;
1004        }
1005        l2cap_sframe_in(ch, lduw_le_p(hdr->data));
1006        return;
1007    }
1008
1009    switch (hdr->data[1] >> 6) {        /* SAR */
1010    case L2CAP_SAR_NO_SEG:
1011        if (ch->len_total)
1012            goto seg_error;
1013        if (len - 4 > ch->mps)
1014            goto len_error;
1015
1016        ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
1017        break;
1018
1019    case L2CAP_SAR_START:
1020        if (ch->len_total || len < 6)
1021            goto seg_error;
1022        if (len - 6 > ch->mps)
1023            goto len_error;
1024
1025        ch->len_total = lduw_le_p(hdr->data + 2);
1026        if (len >= 6 + ch->len_total)
1027            goto seg_error;
1028
1029        ch->len_cur = len - 6;
1030        memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
1031        break;
1032
1033    case L2CAP_SAR_END:
1034        if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
1035            goto seg_error;
1036        if (len - 4 > ch->mps)
1037            goto len_error;
1038
1039        memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1040        ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
1041        break;
1042
1043    case L2CAP_SAR_CONT:
1044        if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
1045            goto seg_error;
1046        if (len - 4 > ch->mps)
1047            goto len_error;
1048
1049        memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1050        ch->len_cur += len - 4;
1051        break;
1052
1053    seg_error:
1054    len_error:  /* TODO */
1055    fcs_error:  /* TODO */
1056        ch->len_cur = 0;
1057        ch->len_total = 0;
1058        break;
1059    }
1060}
1061
1062static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
1063                const l2cap_hdr *frame)
1064{
1065    uint16_t cid = le16_to_cpu(frame->cid);
1066    uint16_t len = le16_to_cpu(frame->len);
1067
1068    if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
1069        fprintf(stderr, "%s: frame addressed to a non-existent L2CAP "
1070                        "channel %04x received.\n", __FUNCTION__, cid);
1071        return;
1072    }
1073
1074    l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
1075}
1076
1077/* "Recombination" */
1078static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
1079                const uint8_t *data, int len)
1080{
1081    const l2cap_hdr *hdr = (void *) l2cap->frame_in;
1082
1083    if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
1084        if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
1085            memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
1086                            sizeof(l2cap->frame_in) - l2cap->frame_in_len);
1087            l2cap->frame_in_len = sizeof(l2cap->frame_in);
1088            /* TODO: truncate */
1089            l2cap_frame_in(l2cap, hdr);
1090        }
1091
1092        return;
1093    }
1094
1095    memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
1096    l2cap->frame_in_len += len;
1097
1098    if (len >= L2CAP_HDR_SIZE)
1099        if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
1100            l2cap_frame_in(l2cap, hdr);
1101            /* There is never a start of a new PDU in the same ACL packet, so
1102             * no need to memmove the remaining payload and loop.  */
1103}
1104
1105static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
1106                uint16_t cid, uint16_t len)
1107{
1108    l2cap_hdr *hdr = (void *) l2cap->frame_out;
1109
1110    l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
1111
1112    hdr->cid = cpu_to_le16(cid);
1113    hdr->len = cpu_to_le16(len);
1114
1115    return l2cap->frame_out + L2CAP_HDR_SIZE;
1116}
1117
1118static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
1119{
1120    /* TODO: Fragmentation */
1121    (l2cap->role ?
1122     l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
1123            (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
1124}
1125
1126static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1127{
1128    struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1129
1130    if (len > chan->params.remote_mtu) {
1131        fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n",
1132                        __FUNCTION__,
1133                        chan->remote_cid, chan->params.remote_mtu);
1134        exit(-1);
1135    }
1136
1137    return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
1138}
1139
1140static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
1141{
1142    struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
1143
1144    l2cap_pdu_submit(chan->l2cap);
1145}
1146
1147#if 0
1148/* Stub: Only used if an emulated device requests outgoing flow control */
1149static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1150{
1151    struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1152
1153    if (len > chan->params.remote_mtu) {
1154        /* TODO: slice into segments and queue each segment as a separate
1155         * I-Frame in a FIFO of I-Frames, local to the CID.  */
1156    } else {
1157        /* TODO: add to the FIFO of I-Frames, local to the CID.  */
1158        /* Possibly we need to return a pointer to a contiguous buffer
1159         * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
1160         * while segmenting at the same time.  */
1161    }
1162    return 0;
1163}
1164
1165static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
1166{
1167    /* TODO: If flow control indicates clear to send, start submitting the
1168     * invidual I-Frames from the FIFO, but don't remove them from there.
1169     * Kick the appropriate timer until we get an S-Frame, and only then
1170     * remove from FIFO or resubmit and re-kick the timer if the timer
1171     * expired.  */
1172}
1173#endif
1174
1175static void l2cap_init(struct l2cap_instance_s *l2cap,
1176                struct bt_link_s *link, int role)
1177{
1178    l2cap->link = link;
1179    l2cap->role = role;
1180    l2cap->dev = (struct bt_l2cap_device_s *)
1181            (role ? link->host : link->slave);
1182
1183    l2cap->next_id = 1;
1184
1185    /* Establish the signalling channel */
1186    l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
1187    l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
1188    l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
1189    l2cap->signalling_ch.params.opaque = l2cap;
1190    l2cap->signalling_ch.params.remote_mtu = 48;
1191    l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
1192    l2cap->signalling_ch.frame_in = l2cap_bframe_in;
1193    l2cap->signalling_ch.mps = 65536;
1194    l2cap->signalling_ch.min_mtu = 48;
1195    l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
1196    l2cap->signalling_ch.l2cap = l2cap;
1197    l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
1198
1199    /* Establish the connection-less data channel */
1200    l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
1201    l2cap->group_ch.params.opaque = l2cap;
1202    l2cap->group_ch.frame_in = l2cap_bframe_in;
1203    l2cap->group_ch.mps = 65533;
1204    l2cap->group_ch.l2cap = l2cap;
1205    l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
1206    l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
1207}
1208
1209static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
1210{
1211    int cid;
1212
1213    /* Don't send DISCONNECT if we are currently handling a DISCONNECT
1214     * sent from the other side.  */
1215    if (send_disconnect) {
1216        if (l2cap->role)
1217            l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
1218            /* l2cap->link is invalid from now on.  */
1219        else
1220            l2cap->dev->device.lmp_disconnect_master(l2cap->link);
1221    }
1222
1223    for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
1224        if (l2cap->cid[cid]) {
1225            l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
1226            g_free(l2cap->cid[cid]);
1227        }
1228
1229    if (l2cap->role)
1230        g_free(l2cap);
1231    else
1232        g_free(l2cap->link);
1233}
1234
1235/* L2CAP glue to lower layers in bluetooth stack (LMP) */
1236
1237static void l2cap_lmp_connection_request(struct bt_link_s *link)
1238{
1239    struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
1240    struct slave_l2cap_instance_s *l2cap;
1241
1242    /* Always accept - we only get called if (dev->device->page_scan).  */
1243
1244    l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s));
1245    l2cap->link.slave = &dev->device;
1246    l2cap->link.host = link->host;
1247    l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
1248
1249    /* Always at the end */
1250    link->host->reject_reason = 0;
1251    link->host->lmp_connection_complete(&l2cap->link);
1252}
1253
1254/* Stub */
1255static void l2cap_lmp_connection_complete(struct bt_link_s *link)
1256{
1257    struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1258    struct l2cap_instance_s *l2cap;
1259
1260    if (dev->device.reject_reason) {
1261        /* Signal to upper layer */
1262        return;
1263    }
1264
1265    l2cap = g_malloc0(sizeof(struct l2cap_instance_s));
1266    l2cap_init(l2cap, link, 1);
1267
1268    link->acl_mode = acl_active;
1269
1270    /* Signal to upper layer */
1271}
1272
1273/* Stub */
1274static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
1275{
1276    struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1277    struct l2cap_instance_s *l2cap =
1278            /* TODO: Retrieve from upper layer */ (void *) dev;
1279
1280    /* Signal to upper layer */
1281
1282    l2cap_teardown(l2cap, 0);
1283}
1284
1285static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
1286{
1287    struct slave_l2cap_instance_s *l2cap =
1288            (struct slave_l2cap_instance_s *) link;
1289
1290    l2cap_teardown(&l2cap->l2cap, 0);
1291}
1292
1293static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
1294                const uint8_t *data, int start, int len)
1295{
1296    struct slave_l2cap_instance_s *l2cap =
1297            (struct slave_l2cap_instance_s *) link;
1298
1299    if (start)
1300        l2cap->l2cap.frame_in_len = 0;
1301
1302    l2cap_pdu_in(&l2cap->l2cap, data, len);
1303}
1304
1305/* Stub */
1306static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
1307                const uint8_t *data, int start, int len)
1308{
1309    struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1310    struct l2cap_instance_s *l2cap =
1311            /* TODO: Retrieve from upper layer */ (void *) dev;
1312
1313    if (start)
1314        l2cap->frame_in_len = 0;
1315
1316    l2cap_pdu_in(l2cap, data, len);
1317}
1318
1319static void l2cap_dummy_destroy(struct bt_device_s *dev)
1320{
1321    struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
1322
1323    bt_l2cap_device_done(l2cap_dev);
1324}
1325
1326void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
1327                struct bt_scatternet_s *net)
1328{
1329    bt_device_init(&dev->device, net);
1330
1331    dev->device.lmp_connection_request = l2cap_lmp_connection_request;
1332    dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
1333    dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
1334    dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
1335    dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
1336    dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
1337
1338    dev->device.handle_destroy = l2cap_dummy_destroy;
1339}
1340
1341void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
1342{
1343    bt_device_done(&dev->device);
1344
1345    /* Should keep a list of all instances and go through it and
1346     * invoke l2cap_teardown() for each.  */
1347}
1348
1349void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
1350                int (*new_channel)(struct bt_l2cap_device_s *dev,
1351                        struct bt_l2cap_conn_params_s *params))
1352{
1353    struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
1354
1355    if (new_psm) {
1356        fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n",
1357                        __FUNCTION__, psm, dev->device.lmp_name);
1358        exit(-1);
1359    }
1360
1361    new_psm = g_malloc0(sizeof(*new_psm));
1362    new_psm->psm = psm;
1363    new_psm->min_mtu = min_mtu;
1364    new_psm->new_channel = new_channel;
1365    new_psm->next = dev->first_psm;
1366    dev->first_psm = new_psm;
1367}
1368