nimble/host/src/ble_l2cap.c

/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */

#include <string.h>
#include <errno.h>
#include "syscfg/syscfg.h"
#include "os/os.h"
#include "host/ble_l2cap.h"
#include "nimble/ble.h"
#include "nimble/hci_common.h"
#include "ble_hs_priv.h"
#include "ble_l2cap_coc_priv.h"

#if NIMBLE_BLE_CONNECT
_Static_assert(sizeof (struct ble_l2cap_hdr) == BLE_L2CAP_HDR_SZ,
               "struct ble_l2cap_hdr must be 4 bytes");

struct os_mempool ble_l2cap_chan_pool;

static os_membuf_t ble_l2cap_chan_mem[
    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_MAX_CHANS) +
                    MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
                    sizeof (struct ble_l2cap_chan))
];

STATS_SECT_DECL(ble_l2cap_stats) ble_l2cap_stats;
STATS_NAME_START(ble_l2cap_stats)
    STATS_NAME(ble_l2cap_stats, chan_create)
    STATS_NAME(ble_l2cap_stats, chan_delete)
    STATS_NAME(ble_l2cap_stats, update_init)
    STATS_NAME(ble_l2cap_stats, update_rx)
    STATS_NAME(ble_l2cap_stats, update_fail)
    STATS_NAME(ble_l2cap_stats, proc_timeout)
    STATS_NAME(ble_l2cap_stats, sig_tx)
    STATS_NAME(ble_l2cap_stats, sig_rx)
    STATS_NAME(ble_l2cap_stats, sm_tx)
    STATS_NAME(ble_l2cap_stats, sm_rx)
STATS_NAME_END(ble_l2cap_stats)

struct ble_l2cap_chan *
ble_l2cap_chan_alloc(uint16_t conn_handle)
{
    struct ble_l2cap_chan *chan;

    chan = os_memblock_get(&ble_l2cap_chan_pool);
    if (chan == NULL) {
        return NULL;
    }

    memset(chan, 0, sizeof *chan);
    chan->conn_handle = conn_handle;

    STATS_INC(ble_l2cap_stats, chan_create);

    return chan;
}

void
ble_l2cap_chan_free(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
{
    int rc;

    if (chan == NULL) {
        return;
    }

    os_mbuf_free_chain(chan->rx_buf);
    ble_l2cap_coc_cleanup_chan(conn, chan);

#if MYNEWT_VAL(BLE_HS_DEBUG)
    memset(chan, 0xff, sizeof *chan);
#endif
    rc = os_memblock_put(&ble_l2cap_chan_pool, chan);
    BLE_HS_DBG_ASSERT_EVAL(rc == 0);

    STATS_INC(ble_l2cap_stats, chan_delete);
}

bool
ble_l2cap_is_mtu_req_sent(const struct ble_l2cap_chan *chan)
{
    return (chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU);
}

int
ble_l2cap_parse_hdr(struct os_mbuf *om, int off,
                    struct ble_l2cap_hdr *l2cap_hdr)
{
    int rc;

    rc = os_mbuf_copydata(om, off, sizeof *l2cap_hdr, l2cap_hdr);
    if (rc != 0) {
        return BLE_HS_EMSGSIZE;
    }

    l2cap_hdr->len = get_le16(&l2cap_hdr->len);
    l2cap_hdr->cid = get_le16(&l2cap_hdr->cid);

    return 0;
}

struct os_mbuf *
ble_l2cap_prepend_hdr(struct os_mbuf *om, uint16_t cid, uint16_t len)
{
    struct ble_l2cap_hdr hdr;

    put_le16(&hdr.len, len);
    put_le16(&hdr.cid, cid);

    om = os_mbuf_prepend_pullup(om, sizeof hdr);
    if (om == NULL) {
        return NULL;
    }

    memcpy(om->om_data, &hdr, sizeof hdr);

    return om;
}

uint16_t
ble_l2cap_get_conn_handle(struct ble_l2cap_chan *chan)
{
    if (!chan) {
        return BLE_HS_CONN_HANDLE_NONE;
    }

    return chan->conn_handle;
}

int
ble_l2cap_create_server(uint16_t psm, uint16_t mtu,
                        ble_l2cap_event_fn *cb, void *cb_arg)
{
    return ble_l2cap_coc_create_server(psm, mtu, cb, cb_arg);
}

int
ble_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
                  struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb, void *cb_arg)
{
    return ble_l2cap_sig_coc_connect(conn_handle, psm, mtu, sdu_rx, cb, cb_arg);
}

int
ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info *chan_info)
{
    if (!chan || !chan_info) {
        return BLE_HS_EINVAL;
    }

    memset(chan_info, 0, sizeof(*chan_info));
    chan_info->dcid = chan->dcid;
    chan_info->scid = chan->scid;
    chan_info->our_l2cap_mtu = chan->my_mtu;
    chan_info->peer_l2cap_mtu = chan->peer_mtu;

#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
    chan_info->psm = chan->psm;
    chan_info->our_coc_mtu = chan->coc_rx.mtu;
    chan_info->peer_coc_mtu = chan->coc_tx.mtu;
#endif

    return 0;
}

int
ble_l2cap_enhanced_connect(uint16_t conn_handle,
                               uint16_t psm, uint16_t mtu,
                               uint8_t num, struct os_mbuf *sdu_rx[],
                               ble_l2cap_event_fn *cb, void *cb_arg)
{
    return ble_l2cap_sig_ecoc_connect(conn_handle, psm, mtu,
                                      num, sdu_rx, cb, cb_arg);
}

int
ble_l2cap_reconfig(struct ble_l2cap_chan *chans[], uint8_t num, uint16_t new_mtu)
{
    int i;
    uint16_t conn_handle;

    if (num == 0 || !chans) {
        return BLE_HS_EINVAL;
    }

    conn_handle = chans[0]->conn_handle;

    for (i = 1; i < num; i++) {
        if (conn_handle != chans[i]->conn_handle) {
            BLE_HS_LOG(ERROR, "All channels should have same conn handle\n");
            return BLE_HS_EINVAL;
        }
    }

    return ble_l2cap_sig_coc_reconfig(conn_handle, chans, num, new_mtu);
}

int
ble_l2cap_disconnect(struct ble_l2cap_chan *chan)
{
    return ble_l2cap_sig_disconnect(chan);
}

/**
 * Transmits a packet over an L2CAP channel.  This function only consumes the
 * supplied mbuf on success.
 */
int
ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu)
{
    return ble_l2cap_coc_send(chan, sdu);
}

int
ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
{
    return ble_l2cap_coc_recv_ready(chan, sdu_rx);
}

void
ble_l2cap_remove_rx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
{
    conn->bhc_rx_chan = NULL;
    os_mbuf_free_chain(chan->rx_buf);
    chan->rx_buf = NULL;
    chan->rx_len = 0;
}

static void
ble_l2cap_append_rx(struct ble_l2cap_chan *chan, struct os_mbuf *frag)
{
#if MYNEWT_VAL(BLE_L2CAP_JOIN_RX_FRAGS)
    struct os_mbuf *m;

    /* Copy the data from the incoming fragment into the packet in progress. */
    m = os_mbuf_pack_chains(chan->rx_buf, frag);
    assert(m);
#else
    /* Join disabled or append failed due to mbuf shortage.  Just attach the
     * mbuf to the end of the packet.
     */
    os_mbuf_concat(chan->rx_buf, frag);
#endif
}

static int
ble_l2cap_rx_payload(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
                     struct os_mbuf *om,
                     ble_l2cap_rx_fn **out_rx_cb)
{
    int len_diff;
    int rc;

    if (chan->rx_buf == NULL) {
        /* First fragment in packet. */
        chan->rx_buf = om;
    } else {
        /* Continuation of packet in progress. */
        ble_l2cap_append_rx(chan, om);
    }

    /* Determine if packet is fully reassembled. */
    len_diff = OS_MBUF_PKTLEN(chan->rx_buf) - chan->rx_len;
    if (len_diff > 0) {
        /* More data than expected; data corruption. */
        ble_l2cap_remove_rx(conn, chan);
        rc = BLE_HS_EBADDATA;
    } else if (len_diff == 0) {
        /* All fragments received. */
        *out_rx_cb = chan->rx_fn;
        rc = 0;
    } else {
        /* More fragments remain. */
#if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) != 0
        conn->bhc_rx_timeout =
            ble_npl_time_get() + MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT);

        ble_hs_timer_resched();
#endif
        rc = BLE_HS_EAGAIN;
    }

    return rc;
}

static uint16_t
ble_l2cap_get_mtu(struct ble_l2cap_chan *chan)
{
    if (chan->scid == BLE_L2CAP_CID_ATT) {
        /* In case of ATT chan->my_mtu keeps preferred MTU which is later
         * used during exchange MTU procedure. Helper below will gives us actual
         * MTU on the channel, which is 23 or higher if exchange MTU has been
         * done
         */
        return ble_att_chan_mtu(chan);
    }

    return chan->my_mtu;
}

/**
 * Processes an incoming L2CAP fragment.
 *
 * @param conn                  The connection the L2CAP fragment was sent
 *                                  over.
 * @param hci_hdr               The ACL data header that was at the start of
 *                                  the L2CAP fragment.  This header has been
 *                                  stripped from the mbuf parameter.
 * @param om                    An mbuf containing the L2CAP data.  If this is
 *                                  the first fragment, the L2CAP header is at
 *                                  the start of the mbuf.  For subsequent
 *                                  fragments, the mbuf starts with L2CAP
 *                                  payload data.
 * @param out_rx_cb             If a full L2CAP packet has been received, a
 *                                  pointer to the appropriate handler gets
 *                                  written here.  The caller should pass the
 *                                  receive buffer to this callback.
 * @param out_reject_cid        Indicates whether an L2CAP Command Reject
 *                                  command should be sent.  If this equals -1,
 *                                  no reject should get sent.  Otherwise, the
 *                                  value indicates the CID that the outgoing
 *                                  reject should specify.
 *
 * @return                      0 if a complete L2CAP packet has been received.
 *                              BLE_HS_EAGAIN if a partial L2CAP packet has
 *                                  been received; more fragments are expected.
 *                              Other value on error.
 */
int
ble_l2cap_rx(struct ble_hs_conn *conn,
             struct hci_data_hdr *hci_hdr,
             struct os_mbuf *om,
             ble_l2cap_rx_fn **out_rx_cb,
             int *out_reject_cid)
{
    struct ble_l2cap_chan *chan;
    struct ble_l2cap_hdr l2cap_hdr;
    uint8_t pb;
    int rc;

    *out_reject_cid = -1;

    pb = BLE_HCI_DATA_PB(hci_hdr->hdh_handle_pb_bc);
    switch (pb) {
    case BLE_HCI_PB_FIRST_FLUSH:
        /* First fragment. */
        rc = ble_l2cap_parse_hdr(om, 0, &l2cap_hdr);
        if (rc != 0) {
            goto err;
        }

        /* Strip L2CAP header from the front of the mbuf. */
        os_mbuf_adj(om, BLE_L2CAP_HDR_SZ);

        chan = ble_hs_conn_chan_find_by_scid(conn, l2cap_hdr.cid);
        if (chan == NULL) {
            rc = BLE_HS_ENOENT;

            /* Unsupported channel. If the target CID is the black hole
             * channel, quietly drop the packet.  Otherwise, send an invalid
             * CID response.
             */
            if (l2cap_hdr.cid != BLE_L2CAP_CID_BLACK_HOLE) {
                BLE_HS_LOG(DEBUG, "rx on unknown L2CAP channel: %d\n",
                           l2cap_hdr.cid);
                *out_reject_cid = l2cap_hdr.cid;
            }
            goto err;
        }

        /* For CIDs from dynamic range we check if SDU size isn't larger than MPS */
        if (chan->dcid >= 0x0040 && chan->dcid <= 0x007F && l2cap_hdr.len > chan->my_coc_mps) {
            /* Data exceeds MPS */
            BLE_HS_LOG(ERROR, "error: sdu_len > chan->my_coc_mps (%d>%d)\n",
                       l2cap_hdr.len, chan->my_coc_mps);
            ble_l2cap_disconnect(chan);
            rc = BLE_HS_EBADDATA;
            goto err;
        }

        if (chan->rx_buf != NULL) {
            /* Previous data packet never completed.  Discard old packet. */
            ble_l2cap_remove_rx(conn, chan);
        }

        if (l2cap_hdr.len > ble_l2cap_get_mtu(chan)) {
            /* More data than we expected on the channel.
             * Disconnect peer with invalid behaviour
             */
            rc = BLE_HS_EBADDATA;
            ble_l2cap_disconnect(chan);
            goto err;
        }

        /* Remember channel and length of L2CAP data for reassembly. */
        conn->bhc_rx_chan = chan;
        chan->rx_len = l2cap_hdr.len;
        break;

    case BLE_HCI_PB_MIDDLE:
        chan = conn->bhc_rx_chan;
        if (chan == NULL || chan->rx_buf == NULL) {
            /* Middle fragment without the start.  Discard new packet. */
            rc = BLE_HS_EBADDATA;
            goto err;
        }
        break;

    default:
        rc = BLE_HS_EBADDATA;
        goto err;
    }

    rc = ble_l2cap_rx_payload(conn, chan, om, out_rx_cb);
    om = NULL;
    if (rc != 0) {
        goto err;
    }

    return 0;

err:
    os_mbuf_free_chain(om);
    return rc;
}

/**
 * Transmits the L2CAP payload contained in the specified mbuf.  The supplied
 * mbuf is consumed, regardless of the outcome of the function call.
 *
 * @param chan                  The L2CAP channel to transmit over.
 * @param txom                  The data to transmit.
 *
 * @return                      0 on success; nonzero on error.
 */
int
ble_l2cap_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
             struct os_mbuf *txom)
{
    int rc;

    txom = ble_l2cap_prepend_hdr(txom, chan->dcid, OS_MBUF_PKTLEN(txom));
    if (txom == NULL) {
        return BLE_HS_ENOMEM;
    }

    rc = ble_hs_hci_acl_tx(conn, &txom);
    switch (rc) {
    case 0:
        /* Success. */
        return 0;

    case BLE_HS_EAGAIN:
        /* Controller could not accommodate full packet.  Enqueue remainder. */
        STAILQ_INSERT_TAIL(&conn->bhc_tx_q, OS_MBUF_PKTHDR(txom), omp_next);
        return 0;

    default:
        /* Error. */
        return rc;
    }
}

int
ble_l2cap_init(void)
{
    int rc;

    rc = os_mempool_init(&ble_l2cap_chan_pool,
                         MYNEWT_VAL(BLE_L2CAP_MAX_CHANS) +
                         MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
                         sizeof (struct ble_l2cap_chan),
                         ble_l2cap_chan_mem, "ble_l2cap_chan_pool");
    if (rc != 0) {
        return BLE_HS_EOS;
    }

    rc = ble_l2cap_sig_init();
    if (rc != 0) {
        return rc;
    }

    rc = ble_l2cap_coc_init();
    if (rc != 0) {
        return rc;
    }

    rc = ble_sm_init();
    if (rc != 0) {
        return rc;
    }

    rc = stats_init_and_reg(
        STATS_HDR(ble_l2cap_stats), STATS_SIZE_INIT_PARMS(ble_l2cap_stats,
        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_l2cap_stats), "ble_l2cap");
    if (rc != 0) {
        return BLE_HS_EOS;
    }

    return 0;
}

#endif