123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474 |
- /****************************************************************************
- * wireless/bluetooth/bt_l2cap.c
- * L2CAP handling
- *
- * Copyright (C) 2018 Gregory Nutt. All rights reserved.
- * Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * Ported from the Intel/Zephyr arduino101_firmware_source-v1.tar package
- * where the code was released with a compatible 3-clause BSD license:
- *
- * Copyright (c) 2016, Intel Corporation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are
- * met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
- * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS
- * ; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
- * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
- * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
- /****************************************************************************
- * Included Files
- ****************************************************************************/
- #include <nuttx/config.h>
- #include <string.h>
- #include <errno.h>
- #include <debug.h>
- #include <nuttx/wireless/bluetooth/bt_hci.h>
- #include <nuttx/wireless/bluetooth/bt_core.h>
- #include "bt_hcicore.h"
- #include "bt_conn.h"
- #include "bt_l2cap.h"
- #include "bt_att.h"
- #include "bt_smp.h"
- /****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
- #define LE_CONN_MIN_INTERVAL 0x0028
- #define LE_CONN_MAX_INTERVAL 0x0038
- #define LE_CONN_LATENCY 0x0000
- #define LE_CONN_TIMEOUT 0x002a
- #define BT_L2CAP_CONN_PARAM_ACCEPTED 0
- #define BT_L2CAP_CONN_PARAM_REJECTED 1
- /****************************************************************************
- * Private Data
- ****************************************************************************/
- static FAR struct bt_l2cap_chan_s *g_channels;
- static FAR struct bt_l2cap_chan_s *g_default;
- /****************************************************************************
- * Private Functions
- ****************************************************************************/
- static uint8_t get_ident(FAR struct bt_conn_s *conn)
- {
- conn->l2cap.ident++;
- /* Handle integer overflow (0 is not valid) */
- if (!conn->l2cap.ident)
- {
- conn->l2cap.ident++;
- }
- return conn->l2cap.ident;
- }
- void bt_l2cap_chan_register(FAR struct bt_l2cap_chan_s *chan)
- {
- wlinfo("CID 0x%04x\n", chan->cid);
- chan->flink = g_channels;
- g_channels = chan;
- }
- void bt_l2cap_chan_default(FAR struct bt_l2cap_chan_s *chan)
- {
- g_default = chan;
- }
- void bt_l2cap_connected(FAR struct bt_conn_s *conn)
- {
- FAR struct bt_l2cap_chan_s *chan;
- /* Notify all registered channels of the connection event */
- for (chan = g_channels; chan; chan = chan->flink)
- {
- if (chan->connected != NULL)
- {
- chan->connected(conn, chan->context, chan->cid);
- }
- }
- /* Notify any default listener of the connection event */
- chan = g_default;
- if (chan != NULL && chan->connected != NULL)
- {
- chan->connected(conn, chan->context, chan->cid);
- }
- }
- void bt_l2cap_disconnected(FAR struct bt_conn_s *conn)
- {
- FAR struct bt_l2cap_chan_s *chan;
- /* Notify all registered channels of the disconnection event */
- for (chan = g_channels; chan; chan = chan->flink)
- {
- if (chan->disconnected != NULL)
- {
- chan->disconnected(conn, chan->context, chan->cid);
- }
- }
- /* Notify any default listener of the disconnection event */
- chan = g_default;
- if (chan != NULL && chan->disconnected != NULL)
- {
- chan->disconnected(conn, chan->context, chan->cid);
- }
- }
- void bt_l2cap_encrypt_change(FAR struct bt_conn_s *conn)
- {
- FAR struct bt_l2cap_chan_s *chan;
- /* Notify all registered channels of the encryption change event */
- for (chan = g_channels; chan; chan = chan->flink)
- {
- if (chan->encrypt_change != NULL)
- {
- chan->encrypt_change(conn, chan->context, chan->cid);
- }
- }
- /* Notify any default listener of the encryption change event */
- chan = g_default;
- if (chan != NULL && chan->encrypt_change != NULL)
- {
- chan->encrypt_change(conn, chan->context, chan->cid);
- }
- }
- struct bt_buf_s *bt_l2cap_create_pdu(FAR struct bt_conn_s *conn)
- {
- size_t head_reserve = sizeof(struct bt_l2cap_hdr_s) +
- sizeof(struct bt_hci_acl_hdr_s) + g_btdev.btdev->head_reserve;
- return bt_buf_alloc(BT_ACL_OUT, NULL, head_reserve);
- }
- void bt_l2cap_send(FAR struct bt_conn_s *conn, uint16_t cid,
- FAR struct bt_buf_s *buf)
- {
- FAR struct bt_l2cap_hdr_s *hdr;
- hdr = bt_buf_provide(buf, sizeof(*hdr));
- hdr->len = BT_HOST2LE16(buf->len - sizeof(*hdr));
- hdr->cid = BT_HOST2LE16(cid);
- bt_conn_send(conn, buf);
- }
- static void rej_not_understood(FAR struct bt_conn_s *conn, uint8_t ident)
- {
- FAR struct bt_l2cap_cmd_reject_s *rej;
- FAR struct bt_l2cap_sig_hdr_s *hdr;
- FAR struct bt_buf_s *buf;
- buf = bt_l2cap_create_pdu(conn);
- if (!buf)
- {
- return;
- }
- hdr = bt_buf_extend(buf, sizeof(*hdr));
- hdr->code = BT_L2CAP_CMD_REJECT;
- hdr->ident = ident;
- hdr->len = BT_HOST2LE16(sizeof(*rej));
- rej = bt_buf_extend(buf, sizeof(*rej));
- rej->reason = BT_HOST2LE16(BT_L2CAP_REJ_NOT_UNDERSTOOD);
- bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
- }
- static void le_conn_param_rsp(FAR struct bt_conn_s *conn,
- FAR struct bt_buf_s *buf)
- {
- struct bt_l2cap_conn_param_rsp_s *rsp = (void *)buf->data;
- if (buf->len < sizeof(*rsp))
- {
- wlerr("ERROR: Too small LE conn param rsp\n");
- return;
- }
- wlinfo("LE conn param rsp result %u\n", BT_LE162HOST(rsp->result));
- }
- static uint16_t le_validate_conn_params(uint16_t min, uint16_t max,
- uint16_t latency, uint16_t timeout)
- {
- uint16_t max_latency;
- if (min > max || min < 6 || max > 3200)
- {
- return BT_L2CAP_CONN_PARAM_REJECTED;
- }
- if (timeout < 10 || timeout > 3200)
- {
- return BT_L2CAP_CONN_PARAM_REJECTED;
- }
- /* Calculation based on BT spec 4.2 [Vol3, PartA, 4.20] max_latency =
- * ((timeout * 10)/(max * 1.25 * 2)) - 1;
- */
- max_latency = (timeout * 4 / max) - 1;
- if (latency > 499 || latency > max_latency)
- {
- return BT_L2CAP_CONN_PARAM_REJECTED;
- }
- return BT_L2CAP_CONN_PARAM_ACCEPTED;
- }
- static void le_conn_param_update_req(FAR struct bt_conn_s *conn,
- uint8_t ident,
- FAR struct bt_buf_s *buf)
- {
- FAR struct bt_l2cap_sig_hdr_s *hdr;
- FAR struct bt_l2cap_conn_param_rsp_s *rsp;
- FAR struct bt_l2cap_conn_param_req_s *req = (void *)buf->data;
- uint16_t min;
- uint16_t max;
- uint16_t latency;
- uint16_t timeout;
- uint16_t result;
- if (buf->len < sizeof(*req))
- {
- wlerr("ERROR: Too small LE conn update param req\n");
- return;
- }
- if (conn->role != BT_HCI_ROLE_MASTER)
- {
- return;
- }
- min = BT_LE162HOST(req->min_interval);
- max = BT_LE162HOST(req->max_interval);
- latency = BT_LE162HOST(req->latency);
- timeout = BT_LE162HOST(req->timeout);
- wlinfo("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x timeout: 0x%4.4x",
- min, max, latency, timeout);
- buf = bt_l2cap_create_pdu(conn);
- if (!buf)
- {
- return;
- }
- result = le_validate_conn_params(min, max, latency, timeout);
- hdr = bt_buf_extend(buf, sizeof(*hdr));
- hdr->code = BT_L2CAP_CONN_PARAM_RSP;
- hdr->ident = ident;
- hdr->len = BT_HOST2LE16(sizeof(*rsp));
- rsp = bt_buf_extend(buf, sizeof(*rsp));
- memset(rsp, 0, sizeof(*rsp));
- rsp->result = BT_HOST2LE16(result);
- bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
- if (result == BT_L2CAP_CONN_PARAM_ACCEPTED)
- {
- bt_conn_le_conn_update(conn, min, max, latency, timeout);
- }
- }
- static void le_sig(FAR struct bt_conn_s *conn, FAR struct bt_buf_s *buf,
- FAR void *context, uint16_t cid)
- {
- struct bt_l2cap_sig_hdr_s *hdr = (FAR void *)buf->data;
- uint16_t len;
- if (buf->len < sizeof(*hdr))
- {
- wlerr("ERROR: Too small L2CAP LE signaling PDU\n");
- goto drop;
- }
- len = BT_LE162HOST(hdr->len);
- bt_buf_consume(buf, sizeof(*hdr));
- wlinfo("LE signaling code 0x%02x ident %u len %u\n", hdr->code,
- hdr->ident, len);
- if (buf->len != len)
- {
- wlerr("ERROR: L2CAP length mismatch (%u != %u)\n", buf->len, len);
- goto drop;
- }
- if (!hdr->ident)
- {
- wlerr("ERROR: Invalid ident value in L2CAP PDU\n");
- goto drop;
- }
- switch (hdr->code)
- {
- case BT_L2CAP_CONN_PARAM_RSP:
- le_conn_param_rsp(conn, buf);
- break;
- case BT_L2CAP_CONN_PARAM_REQ:
- le_conn_param_update_req(conn, hdr->ident, buf);
- break;
- default:
- wlwarn("Unknown L2CAP PDU code 0x%02x\n", hdr->code);
- rej_not_understood(conn, hdr->ident);
- break;
- }
- drop:
- bt_buf_release(buf);
- }
- void bt_l2cap_receive(FAR struct bt_conn_s *conn, FAR struct bt_buf_s *buf)
- {
- FAR struct bt_l2cap_hdr_s *hdr = (FAR void *)buf->data;
- FAR struct bt_l2cap_chan_s *chan;
- uint16_t cid;
- if (buf->len < sizeof(*hdr))
- {
- wlerr("ERROR: Too small L2CAP PDU received\n");
- bt_buf_release(buf);
- return;
- }
- cid = BT_LE162HOST(hdr->cid);
- bt_buf_consume(buf, sizeof(*hdr));
- wlinfo("Packet for CID %u len %u\n", cid, buf->len);
- /* Search for a subscriber to this channel */
- for (chan = g_channels; chan != NULL; chan = chan->flink)
- {
- if (chan->cid == cid)
- {
- break;
- }
- }
- /* If there is no subscriber, then send all received frames to the default
- * listener (if one is registered).
- */
- if (chan == NULL)
- {
- chan = g_default;
- }
- if (chan == NULL)
- {
- wlwarn("WARNING: No subscriber to CID 0x%04x\n", cid);
- bt_buf_release(buf);
- return;
- }
- chan->receive(conn, buf, chan->context, cid);
- }
- void bt_l2cap_update_conn_param(FAR struct bt_conn_s *conn)
- {
- FAR struct bt_l2cap_sig_hdr_s *hdr;
- FAR struct bt_l2cap_conn_param_req_s *req;
- FAR struct bt_buf_s *buf;
- /* Check if we need to update anything */
- if (conn->le_conn_interval >= LE_CONN_MIN_INTERVAL &&
- conn->le_conn_interval <= LE_CONN_MAX_INTERVAL)
- {
- return;
- }
- buf = bt_l2cap_create_pdu(conn);
- if (!buf)
- {
- return;
- }
- hdr = bt_buf_extend(buf, sizeof(*hdr));
- hdr->code = BT_L2CAP_CONN_PARAM_REQ;
- hdr->ident = get_ident(conn);
- hdr->len = BT_HOST2LE16(sizeof(*req));
- req = bt_buf_extend(buf, sizeof(*req));
- req->min_interval = BT_HOST2LE16(LE_CONN_MIN_INTERVAL);
- req->max_interval = BT_HOST2LE16(LE_CONN_MAX_INTERVAL);
- req->latency = BT_HOST2LE16(LE_CONN_LATENCY);
- req->timeout = BT_HOST2LE16(LE_CONN_TIMEOUT);
- bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
- }
- int bt_l2cap_init(void)
- {
- int ret;
- static struct bt_l2cap_chan_s chan =
- {
- .cid = BT_L2CAP_CID_LE_SIG,
- .receive = le_sig,
- };
- bt_att_initialize();
- ret = bt_smp_initialize();
- if (ret < 0)
- {
- return ret;
- }
- bt_l2cap_chan_register(&chan);
- return ret;
- }
|