| /* |
| * 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 "syscfg/syscfg.h" |
| #define BLE_NPL_LOG_MODULE BLE_EATT_LOG |
| #include <nimble/nimble_npl_log.h> |
| |
| #if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0 |
| |
| #include "os/mynewt.h" |
| #include "mem/mem.h" |
| #include "host/ble_hs_log.h" |
| #include "ble_att_cmd_priv.h" |
| #include "ble_hs_priv.h" |
| #include "ble_l2cap_priv.h" |
| #include "ble_eatt_priv.h" |
| #include "services/gatt/ble_svc_gatt.h" |
| |
| struct ble_eatt { |
| SLIST_ENTRY(ble_eatt) next; |
| uint16_t conn_handle; |
| struct ble_l2cap_chan *chan; |
| uint8_t client_op; |
| |
| /* Packet transmit queue */ |
| STAILQ_HEAD(, os_mbuf_pkthdr) eatt_tx_q; |
| |
| struct ble_npl_event setup_ev; |
| struct ble_npl_event wakeup_ev; |
| }; |
| |
| SLIST_HEAD(ble_eatt_list, ble_eatt); |
| |
| static struct ble_eatt_list g_ble_eatt_list; |
| static ble_eatt_att_rx_fn ble_eatt_att_rx_cb; |
| |
| #define BLE_EATT_DATABUF_SIZE ( \ |
| MYNEWT_VAL(BLE_EATT_MTU) + \ |
| 2 + \ |
| sizeof (struct os_mbuf_pkthdr) + \ |
| sizeof (struct os_mbuf)) |
| |
| #define BLE_EATT_MEMBLOCK_SIZE \ |
| (OS_ALIGN(BLE_EATT_DATABUF_SIZE, 4)) |
| |
| #define BLE_EATT_MEMPOOL_SIZE \ |
| OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_EATT_CHAN_NUM) + 1, BLE_EATT_MEMBLOCK_SIZE) |
| static os_membuf_t ble_eatt_conn_mem[ |
| OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_EATT_CHAN_NUM), |
| sizeof(struct ble_eatt)) |
| ]; |
| static struct os_mempool ble_eatt_conn_pool; |
| static os_membuf_t ble_eatt_sdu_coc_mem[BLE_EATT_MEMPOOL_SIZE]; |
| struct os_mbuf_pool ble_eatt_sdu_os_mbuf_pool; |
| static struct os_mempool ble_eatt_sdu_mbuf_mempool; |
| |
| static struct ble_gap_event_listener ble_eatt_listener; |
| |
| static struct ble_npl_event g_read_sup_cl_feat_ev; |
| static struct ble_npl_event g_read_sup_srv_feat_ev; |
| |
| static void ble_eatt_setup_cb(struct ble_npl_event *ev); |
| static void ble_eatt_start(uint16_t conn_handle); |
| |
| static struct ble_eatt * |
| ble_eatt_find_not_busy(uint16_t conn_handle) |
| { |
| struct ble_eatt *eatt; |
| |
| SLIST_FOREACH(eatt, &g_ble_eatt_list, next) { |
| if ((eatt->conn_handle == conn_handle) && !eatt->client_op) { |
| return eatt; |
| } |
| } |
| return NULL; |
| } |
| |
| static struct ble_eatt * |
| ble_eatt_find_by_conn_handle(uint16_t conn_handle) |
| { |
| struct ble_eatt *eatt; |
| |
| SLIST_FOREACH(eatt, &g_ble_eatt_list, next) { |
| if (eatt->conn_handle == conn_handle) { |
| return eatt; |
| } |
| } |
| return NULL; |
| |
| } |
| |
| static struct ble_eatt * |
| ble_eatt_find_by_conn_handle_and_busy_op(uint16_t conn_handle, uint8_t op) |
| { |
| struct ble_eatt *eatt; |
| |
| SLIST_FOREACH(eatt, &g_ble_eatt_list, next) { |
| if (eatt->conn_handle == conn_handle && eatt->client_op == op) { |
| return eatt; |
| } |
| } |
| return NULL; |
| |
| } |
| |
| static struct ble_eatt * |
| ble_eatt_find(uint16_t conn_handle, uint16_t cid) |
| { |
| struct ble_eatt *eatt; |
| |
| SLIST_FOREACH(eatt, &g_ble_eatt_list, next) { |
| if ((eatt->conn_handle == conn_handle) && |
| (eatt->chan) && |
| (eatt->chan->scid == cid)) { |
| return eatt; |
| } |
| } |
| return NULL; |
| |
| } |
| |
| static int |
| ble_eatt_prepare_rx_sdu(struct ble_l2cap_chan *chan) |
| { |
| int rc; |
| struct os_mbuf *om; |
| |
| om = os_mbuf_get_pkthdr(&ble_eatt_sdu_os_mbuf_pool, 0); |
| if (!om) { |
| BLE_EATT_LOG_ERROR("eatt: no memory for sdu\n"); |
| return BLE_HS_ENOMEM; |
| } |
| |
| rc = ble_l2cap_recv_ready(chan, om); |
| return rc; |
| } |
| |
| static void |
| ble_eatt_wakeup_cb(struct ble_npl_event *ev) |
| { |
| struct ble_eatt *eatt; |
| struct os_mbuf *txom; |
| struct os_mbuf_pkthdr *omp; |
| struct ble_l2cap_chan_info info; |
| |
| eatt = ble_npl_event_get_arg(ev); |
| assert(eatt); |
| |
| omp = STAILQ_FIRST(&eatt->eatt_tx_q); |
| if (omp != NULL) { |
| STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next); |
| |
| txom = OS_MBUF_PKTHDR_TO_MBUF(omp); |
| ble_l2cap_get_chan_info(eatt->chan, &info); |
| ble_eatt_tx(eatt->conn_handle, info.dcid, txom); |
| } |
| } |
| |
| static struct ble_eatt * |
| ble_eatt_alloc(void) |
| { |
| struct ble_eatt *eatt; |
| |
| eatt = os_memblock_get(&ble_eatt_conn_pool); |
| if (!eatt) { |
| BLE_EATT_LOG_WARN("eatt: Failed to allocate new eatt context\n"); |
| return NULL; |
| } |
| |
| SLIST_INSERT_HEAD(&g_ble_eatt_list, eatt, next); |
| |
| eatt->conn_handle = BLE_HS_CONN_HANDLE_NONE; |
| eatt->chan = NULL; |
| eatt->client_op = 0; |
| |
| STAILQ_INIT(&eatt->eatt_tx_q); |
| ble_npl_event_init(&eatt->setup_ev, ble_eatt_setup_cb, eatt); |
| ble_npl_event_init(&eatt->wakeup_ev, ble_eatt_wakeup_cb, eatt); |
| return eatt; |
| } |
| |
| static void |
| ble_eatt_free(struct ble_eatt *eatt) |
| { |
| struct os_mbuf_pkthdr *omp; |
| |
| while ((omp = STAILQ_FIRST(&eatt->eatt_tx_q)) != NULL) { |
| STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next); |
| os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); |
| } |
| |
| SLIST_REMOVE(&g_ble_eatt_list, eatt, ble_eatt, next); |
| os_memblock_put(&ble_eatt_conn_pool, eatt); |
| } |
| |
| static int |
| ble_eatt_l2cap_event_fn(struct ble_l2cap_event *event, void *arg) |
| { |
| struct ble_eatt *eatt = arg; |
| struct ble_gap_conn_desc desc; |
| uint8_t opcode; |
| int rc; |
| |
| switch (event->type) { |
| case BLE_L2CAP_EVENT_COC_CONNECTED: |
| BLE_EATT_LOG_DEBUG("eatt: Connected \n"); |
| if (event->connect.status) { |
| ble_eatt_free(eatt); |
| return 0; |
| } |
| eatt->chan = event->connect.chan; |
| break; |
| case BLE_L2CAP_EVENT_COC_DISCONNECTED: |
| BLE_EATT_LOG_DEBUG("eatt: Disconnected \n"); |
| ble_eatt_free(eatt); |
| break; |
| case BLE_L2CAP_EVENT_COC_ACCEPT: |
| BLE_EATT_LOG_DEBUG("eatt: Accept request\n"); |
| eatt = ble_eatt_find_by_conn_handle(event->accept.conn_handle); |
| if (eatt) { |
| /* For now we accept only one additional coc channel per ACL |
| * TODO: improve it |
| */ |
| return BLE_HS_ENOMEM; |
| } |
| |
| eatt = ble_eatt_alloc(); |
| if (!eatt) { |
| return BLE_HS_ENOMEM; |
| } |
| eatt->conn_handle = event->accept.conn_handle; |
| event->accept.chan->cb_arg = eatt; |
| |
| rc = ble_eatt_prepare_rx_sdu(event->accept.chan); |
| if (rc) { |
| ble_eatt_free(eatt); |
| } |
| assert(rc == 0); |
| return 0; |
| case BLE_L2CAP_EVENT_COC_TX_UNSTALLED: |
| ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); |
| break; |
| case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: |
| assert(eatt->chan == event->receive.chan); |
| opcode = event->receive.sdu_rx->om_data[0]; |
| if (ble_att_is_response_op(opcode)) { |
| ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); |
| } else if (!ble_att_is_request_op(opcode)) { |
| /* As per BLE 5.4 Standard, Vol. 3, Part F, section 5.3.2 |
| * (ENHANCED ATT BEARER L2CAP INTEROPERABILITY REQUIREMENTS: |
| * Channel Requirements): |
| * All packets sent on this L2CAP channel shall be Attribute PDUs. |
| * |
| * Disconnect peer with invalid behavior. |
| */ |
| ble_l2cap_disconnect(eatt->chan); |
| return BLE_HS_EREJECT; |
| } |
| |
| assert (!ble_gap_conn_find(event->receive.conn_handle, &desc)); |
| /* As per BLE 5.4 Standard, Vol. 3, Part F, section 5.3.2 |
| * (ENHANCED ATT BEARER L2CAP INTEROPERABILITY REQUIREMENTS: |
| * Channel Requirements): |
| * The channel shall be encrypted. |
| * |
| * Disconnect peer with invalid behavior - ATT PDU received before |
| * encryption. |
| */ |
| if (!desc.sec_state.encrypted) { |
| ble_l2cap_disconnect(eatt->chan); |
| return BLE_HS_EREJECT; |
| } |
| |
| ble_eatt_att_rx_cb(event->receive.conn_handle, eatt->chan->scid, &event->receive.sdu_rx); |
| if (event->receive.sdu_rx) { |
| os_mbuf_free_chain(event->receive.sdu_rx); |
| event->receive.sdu_rx = NULL; |
| } |
| |
| /* Receiving L2CAP data is no longer possible, terminate connection */ |
| rc = ble_eatt_prepare_rx_sdu(event->receive.chan); |
| if (rc) { |
| ble_l2cap_disconnect(eatt->chan); |
| return BLE_HS_ENOMEM; |
| } |
| break; |
| default: |
| break; |
| } |
| |
| return 0; |
| } |
| |
| static void |
| ble_eatt_setup_cb(struct ble_npl_event *ev) |
| { |
| struct ble_eatt *eatt; |
| struct os_mbuf *om; |
| |
| eatt = ble_npl_event_get_arg(ev); |
| assert(eatt); |
| |
| om = os_mbuf_get_pkthdr(&ble_eatt_sdu_os_mbuf_pool, 0); |
| if (!om) { |
| ble_eatt_free(eatt); |
| BLE_EATT_LOG_ERROR("eatt: no memory for sdu\n"); |
| return; |
| } |
| |
| BLE_EATT_LOG_DEBUG("eatt: connecting eatt on conn_handle 0x%04x\n", eatt->conn_handle); |
| ble_l2cap_enhanced_connect(eatt->conn_handle, BLE_EATT_PSM, |
| MYNEWT_VAL(BLE_EATT_MTU), 1, &om, ble_eatt_l2cap_event_fn, eatt); |
| } |
| |
| static int |
| ble_gatt_eatt_write_cl_cb(uint16_t conn_handle, |
| const struct ble_gatt_error *error, |
| struct ble_gatt_attr *attr, void *arg) |
| { |
| if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) { |
| BLE_EATT_LOG_DEBUG("eatt: Cannot write to Client Supported features on peer device\n"); |
| return 0; |
| } |
| |
| ble_eatt_start(conn_handle); |
| return 0; |
| } |
| |
| static int |
| ble_gatt_eatt_read_cl_uuid_cb(uint16_t conn_handle, |
| const struct ble_gatt_error *error, |
| struct ble_gatt_attr *attr, void *arg) |
| { |
| uint8_t client_supported_feat; |
| int rc; |
| |
| if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) { |
| BLE_EATT_LOG_DEBUG("eatt: Cannot find Client Supported features on peer device\n"); |
| return BLE_HS_EDONE; |
| } |
| |
| if (attr == NULL) { |
| BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); |
| return BLE_HS_EDONE; |
| } |
| |
| if (error->status == 0) { |
| client_supported_feat = ble_svc_gatt_get_local_cl_supported_feat(); |
| rc = ble_gattc_write_flat(conn_handle, attr->handle, &client_supported_feat, 1, |
| ble_gatt_eatt_write_cl_cb, NULL); |
| BLE_EATT_LOG_DEBUG("eatt: %s , write rc = %d \n", __func__, rc); |
| assert(rc == 0); |
| return 0; |
| } |
| |
| return BLE_HS_EDONE; |
| } |
| |
| static int |
| ble_gatt_eatt_read_uuid_cb(uint16_t conn_handle, |
| const struct ble_gatt_error *error, |
| struct ble_gatt_attr *attr, void *arg) |
| { |
| uint8_t supported_features; |
| int rc; |
| |
| if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) { |
| BLE_EATT_LOG_DEBUG("eatt: Cannot find Server Supported features on peer device\n"); |
| return BLE_HS_EDONE; |
| } |
| |
| if (attr == NULL) { |
| BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); |
| return BLE_HS_EDONE; |
| } |
| |
| rc = os_mbuf_copydata(attr->om, 0, 1, &supported_features); |
| if (rc) { |
| BLE_EATT_LOG_ERROR("eatt: Cannot read srv supported features \n"); |
| return BLE_HS_EDONE; |
| } |
| |
| if (supported_features & 0x01) { |
| ble_npl_event_set_arg(&g_read_sup_cl_feat_ev, UINT_TO_POINTER(conn_handle)); |
| ble_npl_eventq_put(ble_hs_evq_get(), &g_read_sup_cl_feat_ev); |
| } |
| return BLE_HS_EDONE; |
| } |
| |
| static void |
| ble_gatt_eatt_read_svr_uuid(struct ble_npl_event *ev) |
| { |
| uint16_t conn_handle; |
| |
| conn_handle = POINTER_TO_UINT(ble_npl_event_get_arg(ev)); |
| |
| ble_gattc_read_by_uuid(conn_handle, 1, 0xffff, |
| BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_SERVER_SUPPORTED_FEAT_UUID16), |
| ble_gatt_eatt_read_uuid_cb, NULL); |
| } |
| |
| static void |
| ble_gatt_eatt_read_cl_uuid(struct ble_npl_event *ev) |
| { |
| uint16_t conn_handle; |
| |
| conn_handle = POINTER_TO_UINT(ble_npl_event_get_arg(ev)); |
| |
| ble_gattc_read_by_uuid(conn_handle, 1, 0xffff, |
| BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_CLIENT_SUPPORTED_FEAT_UUID16), |
| ble_gatt_eatt_read_cl_uuid_cb, NULL); |
| } |
| |
| static int |
| ble_eatt_gap_event(struct ble_gap_event *event, void *arg) |
| { |
| struct ble_eatt *eatt; |
| |
| switch (event->type) { |
| case BLE_GAP_EVENT_CONNECT: |
| if (event->connect.status != 0) { |
| return 0; |
| } |
| |
| BLE_EATT_LOG_DEBUG("eatt: Device connected, waiting to write into " |
| "client supported features\n"); |
| |
| ble_npl_event_set_arg(&g_read_sup_srv_feat_ev, UINT_TO_POINTER(event->connect.conn_handle)); |
| ble_npl_eventq_put(ble_hs_evq_get(), &g_read_sup_srv_feat_ev); |
| |
| break; |
| case BLE_GAP_EVENT_DISCONNECT: |
| eatt = ble_eatt_find_by_conn_handle(event->disconnect.conn.conn_handle); |
| assert(eatt == NULL); |
| break; |
| default: |
| break; |
| } |
| return 0; |
| } |
| |
| uint16_t |
| ble_eatt_get_available_chan_cid(uint16_t conn_handle, uint8_t op) |
| { |
| struct ble_eatt * eatt; |
| |
| eatt = ble_eatt_find_not_busy(conn_handle); |
| if (!eatt) { |
| return BLE_L2CAP_CID_ATT; |
| } |
| |
| eatt->client_op = op; |
| return eatt->chan->scid; |
| } |
| |
| void |
| ble_eatt_release_chan(uint16_t conn_handle, uint8_t op) |
| { |
| struct ble_eatt * eatt; |
| |
| eatt = ble_eatt_find_by_conn_handle_and_busy_op(conn_handle, op); |
| if (!eatt) { |
| BLE_EATT_LOG_WARN("ble_eatt_release_chan:" |
| "EATT not found for conn_handle 0x%04x, operation 0x%02\n", conn_handle, op); |
| return; |
| } |
| |
| eatt->client_op = 0; |
| } |
| |
| int |
| ble_eatt_tx(uint16_t conn_handle, uint16_t cid, struct os_mbuf *txom) |
| { |
| struct ble_eatt *eatt; |
| int rc; |
| |
| BLE_EATT_LOG_DEBUG("eatt: %s, size %d ", __func__, OS_MBUF_PKTLEN(txom)); |
| eatt = ble_eatt_find(conn_handle, cid); |
| if (!eatt || !eatt->chan) { |
| BLE_EATT_LOG_ERROR("Eatt not available"); |
| rc = BLE_HS_ENOENT; |
| goto error; |
| } |
| |
| rc = ble_l2cap_send(eatt->chan, txom); |
| if (rc == 0) { |
| goto done; |
| } |
| |
| if (rc == BLE_HS_ESTALLED) { |
| BLE_EATT_LOG_DEBUG("ble_eatt_tx: Eatt stalled"); |
| } else if (rc == BLE_HS_EBUSY) { |
| BLE_EATT_LOG_DEBUG("ble_eatt_tx: Message queued"); |
| STAILQ_INSERT_HEAD(&eatt->eatt_tx_q, OS_MBUF_PKTHDR(txom), omp_next); |
| ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); |
| } else { |
| BLE_EATT_LOG_ERROR("eatt: %s, ERROR %d ", __func__, rc); |
| assert(0); |
| } |
| done: |
| return 0; |
| |
| error: |
| os_mbuf_free_chain(txom); |
| return rc; |
| } |
| |
| static void |
| ble_eatt_start(uint16_t conn_handle) |
| { |
| struct ble_gap_conn_desc desc; |
| struct ble_eatt *eatt; |
| int rc; |
| |
| rc = ble_gap_conn_find(conn_handle, &desc); |
| assert(rc == 0); |
| if (desc.role != BLE_GAP_ROLE_MASTER) { |
| /* Let master to create ecoc. |
| * TODO: Slave could setup after some timeout |
| */ |
| return; |
| } |
| |
| eatt = ble_eatt_alloc(); |
| if (!eatt) { |
| return; |
| } |
| |
| eatt->conn_handle = conn_handle; |
| |
| /* Setup EATT */ |
| ble_npl_eventq_put(ble_hs_evq_get(), &eatt->setup_ev); |
| } |
| |
| void |
| ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb) |
| { |
| int rc; |
| |
| rc = mem_init_mbuf_pool(ble_eatt_sdu_coc_mem, |
| &ble_eatt_sdu_mbuf_mempool, |
| &ble_eatt_sdu_os_mbuf_pool, |
| MYNEWT_VAL(BLE_EATT_CHAN_NUM) + 1, |
| BLE_EATT_MEMBLOCK_SIZE, |
| "ble_eatt_sdu"); |
| BLE_HS_DBG_ASSERT_EVAL(rc == 0); |
| |
| rc = os_mempool_init(&ble_eatt_conn_pool, MYNEWT_VAL(BLE_EATT_CHAN_NUM), |
| sizeof (struct ble_eatt), |
| ble_eatt_conn_mem, "ble_eatt_conn_pool"); |
| BLE_HS_DBG_ASSERT_EVAL(rc == 0); |
| |
| ble_gap_event_listener_register(&ble_eatt_listener, ble_eatt_gap_event, NULL); |
| ble_l2cap_create_server(BLE_EATT_PSM, MYNEWT_VAL(BLE_EATT_MTU), ble_eatt_l2cap_event_fn, NULL); |
| |
| ble_npl_event_init(&g_read_sup_srv_feat_ev, ble_gatt_eatt_read_svr_uuid, NULL); |
| ble_npl_event_init(&g_read_sup_cl_feat_ev, ble_gatt_eatt_read_cl_uuid, NULL); |
| |
| ble_eatt_att_rx_cb = att_rx_cb; |
| } |
| #endif |