/*
|
* 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 <stdlib.h>
|
#include <string.h>
|
#include <errno.h>
|
#include "os/os.h"
|
#include "host/ble_att.h"
|
#include "nimble/ble.h"
|
#include "host/ble_uuid.h"
|
#include "ble_hs_priv.h"
|
#include "os/panchip_mempool.h"
|
|
#if NIMBLE_BLE_CONNECT
|
/**
|
* ATT server - Attribute Protocol
|
*
|
* Notes on buffer reuse:
|
* Most request handlers reuse the request buffer for the response. This is
|
* done to prevent out-of-memory conditions. However, there are two handlers
|
* which do not reuse the request buffer:
|
* 1. Write request.
|
* 2. Indicate request.
|
*
|
* Both of these handlers attempt to allocate a new buffer for the response
|
* prior to processing the request. If allocation fails, the request is not
|
* processed, and the request buffer is reused for the transmission of an
|
* "insufficient resources" ATT error response. These handlers don't reuse the
|
* request mbuf for an affirmative response because the buffer contains the
|
* attribute data that gets passed to the application callback. The
|
* application may choose to retain the mbuf during the callback, so the stack
|
*/
|
|
STAILQ_HEAD(ble_att_svr_entry_list, ble_att_svr_entry);
|
static struct ble_att_svr_entry_list ble_att_svr_list;
|
static struct ble_att_svr_entry_list ble_att_svr_hidden_list;
|
|
static uint16_t ble_att_svr_id;
|
|
static void *ble_att_svr_entry_mem;
|
static struct os_mempool ble_att_svr_entry_pool;
|
|
#ifdef IP_101x
|
static os_membuf_t *ble_att_svr_prep_entry_mem;
|
#else
|
static os_membuf_t ble_att_svr_prep_entry_mem[
|
OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
|
sizeof (struct ble_att_prep_entry))
|
];
|
#endif
|
|
static struct os_mempool ble_att_svr_prep_entry_pool;
|
|
static struct ble_att_svr_entry *
|
ble_att_svr_entry_alloc(void)
|
{
|
struct ble_att_svr_entry *entry;
|
|
entry = os_memblock_get(&ble_att_svr_entry_pool);
|
if (entry != NULL) {
|
memset(entry, 0, sizeof *entry);
|
}
|
|
return entry;
|
}
|
|
static void
|
ble_att_svr_entry_free(struct ble_att_svr_entry *entry)
|
{
|
os_memblock_put(&ble_att_svr_entry_pool, entry);
|
}
|
|
/**
|
* Allocate the next handle id and return it.
|
*
|
* @return A new 16-bit handle ID.
|
*/
|
static uint16_t
|
ble_att_svr_next_id(void)
|
{
|
/* Rollover is fatal. */
|
BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX);
|
return ++ble_att_svr_id;
|
}
|
|
/**
|
* Register a host attribute with the BLE stack.
|
*
|
* @param ha A filled out ble_att structure to register
|
* @param handle_id A pointer to a 16-bit handle ID, which will be
|
* the handle that is allocated.
|
* @param fn The callback function that gets executed when
|
* the attribute is operated on.
|
*
|
* @return 0 on success, non-zero error code on failure.
|
*/
|
int
|
ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
|
uint8_t min_key_size, uint16_t *handle_id,
|
ble_att_svr_access_fn *cb, void *cb_arg)
|
{
|
struct ble_att_svr_entry *entry;
|
|
entry = ble_att_svr_entry_alloc();
|
if (entry == NULL) {
|
return BLE_HS_ENOMEM;
|
}
|
|
entry->ha_uuid = uuid;
|
entry->ha_flags = flags;
|
entry->ha_min_key_size = min_key_size;
|
entry->ha_handle_id = ble_att_svr_next_id();
|
entry->ha_cb = cb;
|
entry->ha_cb_arg = cb_arg;
|
|
STAILQ_INSERT_TAIL(&ble_att_svr_list, entry, ha_next);
|
|
if (handle_id != NULL) {
|
*handle_id = entry->ha_handle_id;
|
}
|
|
return 0;
|
}
|
|
uint16_t
|
ble_att_svr_prev_handle(void)
|
{
|
return ble_att_svr_id;
|
}
|
|
/**
|
* Find a host attribute by handle id.
|
*
|
* @param handle_id The handle_id to search for
|
* @param ha_ptr On input: Indicates the starting point of the
|
* walk; null means start at the beginning of
|
* the list, non-null means start at the
|
* following entry.
|
* On output: Indicates the last ble_att element
|
* processed, or NULL if the entire list has
|
* been processed.
|
*
|
* @return 0 on success; BLE_HS_ENOENT on not found.
|
*/
|
struct ble_att_svr_entry *
|
ble_att_svr_find_by_handle(uint16_t handle_id)
|
{
|
struct ble_att_svr_entry *entry;
|
|
for (entry = STAILQ_FIRST(&ble_att_svr_list);
|
entry != NULL;
|
entry = STAILQ_NEXT(entry, ha_next)) {
|
|
if (entry->ha_handle_id == handle_id) {
|
return entry;
|
}
|
}
|
|
return NULL;
|
}
|
|
/**
|
* Find a host attribute by UUID.
|
*
|
* @param uuid The ble_uuid_t to search for; null means
|
* find any type of attribute.
|
* @param prev On input: Indicates the starting point of the
|
* walk; null means start at the beginning of
|
* the list, non-null means start at the
|
* following entry.
|
* On output: Indicates the last ble_att element
|
* processed, or NULL if the entire list has
|
* been processed.
|
*
|
* @return 0 on success; BLE_HS_ENOENT on not found.
|
*/
|
struct ble_att_svr_entry *
|
ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const ble_uuid_t *uuid,
|
uint16_t end_handle)
|
{
|
struct ble_att_svr_entry *entry;
|
|
if (prev == NULL) {
|
entry = STAILQ_FIRST(&ble_att_svr_list);
|
} else {
|
entry = STAILQ_NEXT(prev, ha_next);
|
}
|
|
for (;
|
entry != NULL && entry->ha_handle_id <= end_handle;
|
entry = STAILQ_NEXT(entry, ha_next)) {
|
|
if (uuid == NULL || ble_uuid_cmp(entry->ha_uuid, uuid) == 0) {
|
return entry;
|
}
|
}
|
|
return NULL;
|
}
|
|
static int
|
ble_att_svr_pullup_req_base(struct os_mbuf **om, int base_len,
|
uint8_t *out_att_err)
|
{
|
uint8_t att_err;
|
int rc;
|
|
rc = ble_hs_mbuf_pullup_base(om, base_len);
|
if (rc == BLE_HS_ENOMEM) {
|
att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
} else {
|
att_err = 0;
|
}
|
|
if (out_att_err != NULL) {
|
*out_att_err = att_err;
|
}
|
|
return rc;
|
}
|
|
static void
|
ble_att_svr_get_sec_state(uint16_t conn_handle,
|
struct ble_gap_sec_state *out_sec_state)
|
{
|
struct ble_hs_conn *conn;
|
|
ble_hs_lock();
|
|
conn = ble_hs_conn_find_assert(conn_handle);
|
*out_sec_state = conn->bhc_sec_state;
|
|
ble_hs_unlock();
|
}
|
|
static int
|
ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
|
struct ble_att_svr_entry *entry,
|
uint8_t *out_att_err)
|
{
|
struct ble_gap_sec_state sec_state;
|
struct ble_store_value_sec value_sec;
|
struct ble_store_key_sec key_sec;
|
struct ble_hs_conn_addrs addrs;
|
struct ble_hs_conn *conn;
|
int author;
|
int authen;
|
int enc;
|
int rc;
|
|
if (is_read) {
|
if (!(entry->ha_flags & BLE_ATT_F_READ)) {
|
*out_att_err = BLE_ATT_ERR_READ_NOT_PERMITTED;
|
return BLE_HS_EREJECT;
|
}
|
|
enc = entry->ha_flags & BLE_ATT_F_READ_ENC;
|
authen = entry->ha_flags & BLE_ATT_F_READ_AUTHEN;
|
author = entry->ha_flags & BLE_ATT_F_READ_AUTHOR;
|
} else {
|
if (!(entry->ha_flags & BLE_ATT_F_WRITE)) {
|
*out_att_err = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
|
return BLE_HS_EREJECT;
|
}
|
|
enc = entry->ha_flags & BLE_ATT_F_WRITE_ENC;
|
authen = entry->ha_flags & BLE_ATT_F_WRITE_AUTHEN;
|
author = entry->ha_flags & BLE_ATT_F_WRITE_AUTHOR;
|
}
|
|
/* Bail early if this operation doesn't require security. */
|
if (!enc && !authen && !author) {
|
return 0;
|
}
|
|
ble_att_svr_get_sec_state(conn_handle, &sec_state);
|
/* In SC Only mode all characteristics requiring security
|
* require it on level 4
|
*/
|
if (MYNEWT_VAL(BLE_SM_SC_ONLY)) {
|
if (sec_state.key_size != 16 ||
|
!sec_state.authenticated ||
|
!sec_state.encrypted) {
|
return BLE_ATT_ERR_INSUFFICIENT_KEY_SZ;
|
}
|
}
|
if ((enc || authen) && !sec_state.encrypted) {
|
ble_hs_lock();
|
conn = ble_hs_conn_find(conn_handle);
|
if (conn != NULL) {
|
ble_hs_conn_addrs(conn, &addrs);
|
|
memset(&key_sec, 0, sizeof key_sec);
|
key_sec.peer_addr = addrs.peer_id_addr;
|
}
|
ble_hs_unlock();
|
|
rc = ble_store_read_peer_sec(&key_sec, &value_sec);
|
if (rc == 0 && value_sec.ltk_present) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_ENC;
|
} else {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
|
}
|
|
return BLE_HS_ATT_ERR(*out_att_err);
|
}
|
|
if (authen && !sec_state.authenticated) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
|
return BLE_HS_ATT_ERR(*out_att_err);
|
}
|
|
if (entry->ha_min_key_size > sec_state.key_size) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_KEY_SZ;
|
return BLE_HS_ATT_ERR(*out_att_err);
|
}
|
|
if (author) {
|
/* XXX: Prompt user for authorization. */
|
}
|
|
return 0;
|
}
|
|
/**
|
* Calculates the number of ticks until a queued write times out on the
|
* specified ATT server. If this server is not in the process of receiving a
|
* queued write, then BLE_HS_FOREVER is returned. If a timeout just occurred,
|
* 0 is returned.
|
*
|
* @param svr The ATT server to check.
|
* @param now The current OS time.
|
*
|
* @return The number of ticks until the current queued
|
* write times out.
|
*/
|
int32_t
|
ble_att_svr_ticks_until_tmo(const struct ble_att_svr_conn *svr, ble_npl_time_t now)
|
{
|
#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO == 0
|
return BLE_HS_FOREVER;
|
#endif
|
|
int32_t time_diff;
|
|
if (SLIST_EMPTY(&svr->basc_prep_list)) {
|
return BLE_HS_FOREVER;
|
}
|
|
time_diff = svr->basc_prep_timeout_at - now;
|
if (time_diff < 0) {
|
return 0;
|
}
|
|
return time_diff;
|
}
|
|
/**
|
* Allocates an mbuf to be used for an ATT response. If an mbuf cannot be
|
* allocated, the received request mbuf is reused for the error response.
|
*/
|
static int
|
ble_att_svr_pkt(struct os_mbuf **rxom, struct os_mbuf **out_txom,
|
uint8_t *out_att_err)
|
{
|
*out_txom = ble_hs_mbuf_l2cap_pkt();
|
if (*out_txom != NULL) {
|
return 0;
|
}
|
|
/* Allocation failure. Reuse receive buffer for response. */
|
*out_txom = *rxom;
|
*rxom = NULL;
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
return BLE_HS_ENOMEM;
|
}
|
|
static int
|
ble_att_svr_read(uint16_t conn_handle,
|
struct ble_att_svr_entry *entry,
|
uint16_t offset,
|
struct os_mbuf *om,
|
uint8_t *out_att_err)
|
{
|
uint8_t att_err;
|
int rc;
|
|
att_err = 0; /* Silence gcc warning. */
|
|
if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
|
rc = ble_att_svr_check_perms(conn_handle, 1, entry, &att_err);
|
if (rc != 0) {
|
goto err;
|
}
|
}
|
|
BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
|
rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
|
BLE_ATT_ACCESS_OP_READ, offset, &om, entry->ha_cb_arg);
|
if (rc != 0) {
|
att_err = rc;
|
rc = BLE_HS_EAPP;
|
goto err;
|
}
|
|
return 0;
|
|
err:
|
if (out_att_err != NULL) {
|
*out_att_err = att_err;
|
}
|
return rc;
|
}
|
|
static int
|
ble_att_svr_read_flat(uint16_t conn_handle,
|
struct ble_att_svr_entry *entry,
|
uint16_t offset,
|
uint16_t max_len,
|
void *dst,
|
uint16_t *out_len,
|
uint8_t *out_att_err)
|
{
|
struct os_mbuf *om;
|
uint16_t len;
|
int rc;
|
|
om = ble_hs_mbuf_l2cap_pkt();
|
if (om == NULL) {
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
len = OS_MBUF_PKTLEN(om);
|
if (len > max_len) {
|
rc = BLE_HS_EMSGSIZE;
|
*out_att_err = BLE_ATT_ERR_UNLIKELY;
|
goto done;
|
}
|
|
rc = os_mbuf_copydata(om, 0, len, dst);
|
BLE_HS_DBG_ASSERT(rc == 0);
|
|
*out_len = len;
|
rc = 0;
|
|
done:
|
os_mbuf_free_chain(om);
|
return rc;
|
}
|
|
int
|
ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
|
uint16_t offset, struct os_mbuf *om,
|
uint8_t *out_att_err)
|
{
|
struct ble_att_svr_entry *entry;
|
int rc;
|
|
entry = ble_att_svr_find_by_handle(attr_handle);
|
if (entry == NULL) {
|
if (out_att_err != NULL) {
|
*out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
}
|
return BLE_HS_ENOENT;
|
}
|
|
rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
|
if (rc != 0) {
|
return rc;
|
}
|
|
return 0;
|
}
|
|
int
|
ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
|
{
|
struct os_mbuf *om;
|
int rc;
|
|
om = ble_hs_mbuf_bare_pkt();
|
if (om == NULL) {
|
rc = BLE_HS_ENOMEM;
|
goto err;
|
}
|
|
rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, om,
|
NULL);
|
if (rc != 0) {
|
goto err;
|
}
|
|
*out_om = om;
|
return 0;
|
|
err:
|
os_mbuf_free_chain(om);
|
return rc;
|
}
|
|
static int
|
ble_att_svr_write(uint16_t conn_handle, struct ble_att_svr_entry *entry,
|
uint16_t offset, struct os_mbuf **om, uint8_t *out_att_err)
|
{
|
uint8_t att_err = 0;
|
int rc;
|
|
BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
|
|
if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
|
rc = ble_att_svr_check_perms(conn_handle, 0, entry, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
}
|
|
BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
|
rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
|
BLE_ATT_ACCESS_OP_WRITE, offset, om, entry->ha_cb_arg);
|
if (rc != 0) {
|
att_err = rc;
|
rc = BLE_HS_EAPP;
|
goto done;
|
}
|
|
done:
|
if (out_att_err != NULL) {
|
*out_att_err = att_err;
|
}
|
return rc;
|
}
|
|
static int
|
ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
|
uint16_t offset, struct os_mbuf **om,
|
uint8_t *out_att_err)
|
{
|
struct ble_att_svr_entry *entry;
|
int rc;
|
|
entry = ble_att_svr_find_by_handle(attr_handle);
|
if (entry == NULL) {
|
if (out_att_err != NULL) {
|
*out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
}
|
return BLE_HS_ENOENT;
|
}
|
|
rc = ble_att_svr_write(conn_handle, entry, offset, om, out_att_err);
|
if (rc != 0) {
|
return rc;
|
}
|
|
return 0;
|
}
|
|
int
|
ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
|
uint8_t req_op, uint16_t handle, uint8_t error_code)
|
{
|
struct ble_att_error_rsp *rsp;
|
|
BLE_HS_DBG_ASSERT(error_code != 0);
|
BLE_HS_DBG_ASSERT(OS_MBUF_PKTLEN(txom) == 0);
|
|
rsp = ble_att_cmd_prepare(BLE_ATT_OP_ERROR_RSP, sizeof(*rsp), txom);
|
if (rsp == NULL) {
|
return BLE_HS_ENOMEM;
|
}
|
|
rsp->baep_req_op = req_op;
|
rsp->baep_handle = htole16(handle);
|
rsp->baep_error_code = error_code;
|
|
return ble_att_tx(conn_handle, txom);
|
}
|
|
/**
|
* Transmits a response or error message over the specified connection.
|
*
|
* The specified rc and err_status values control what gets sent as follows:
|
* o If rc == 0: tx an affirmative response.
|
* o Else if err_status != 0: tx an error response.
|
* o Else: tx nothing.
|
*
|
* In addition, if transmission of an affirmative response fails, an error is
|
* sent instead.
|
*
|
* @param conn_handle The handle of the connection to send over.
|
* @param hs_status The status indicating whether to transmit an
|
* affirmative response or an error.
|
* @param txom Contains the affirmative response payload.
|
* @param att_op If an error is transmitted, this is the value
|
* of the error message's op field.
|
* @param err_status If an error is transmitted, this is the value
|
* of the error message's status field.
|
* @param err_handle If an error is transmitted, this is the value
|
* of the error message's attribute handle
|
* field.
|
*/
|
static int
|
ble_att_svr_tx_rsp(uint16_t conn_handle, int hs_status, struct os_mbuf *om,
|
uint8_t att_op, uint8_t err_status, uint16_t err_handle)
|
{
|
struct ble_l2cap_chan *chan;
|
struct ble_hs_conn *conn;
|
int do_tx;
|
int rc;
|
|
if (hs_status != 0 && err_status == 0) {
|
/* Processing failed, but err_status of 0 means don't send error. */
|
do_tx = 0;
|
} else {
|
do_tx = 1;
|
}
|
|
if (do_tx) {
|
ble_hs_lock();
|
|
rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
|
if (rc != 0) {
|
/* No longer connected. */
|
hs_status = rc;
|
} else {
|
if (hs_status == 0) {
|
BLE_HS_DBG_ASSERT(om != NULL);
|
|
ble_att_inc_tx_stat(om->om_data[0]);
|
ble_att_truncate_to_mtu(chan, om);
|
hs_status = ble_l2cap_tx(conn, chan, om);
|
om = NULL;
|
if (hs_status != 0) {
|
err_status = BLE_ATT_ERR_UNLIKELY;
|
}
|
}
|
}
|
|
ble_hs_unlock();
|
|
if (hs_status != 0) {
|
STATS_INC(ble_att_stats, error_rsp_tx);
|
|
/* Reuse om for error response. */
|
if (om == NULL) {
|
om = ble_hs_mbuf_l2cap_pkt();
|
} else {
|
os_mbuf_adj(om, OS_MBUF_PKTLEN(om));
|
}
|
if (om != NULL) {
|
ble_att_svr_tx_error_rsp(conn_handle, om, att_op,
|
err_handle, err_status);
|
om = NULL;
|
}
|
}
|
}
|
|
/* Free mbuf if it was not consumed (i.e., if the send failed). */
|
os_mbuf_free_chain(om);
|
|
return hs_status;
|
}
|
|
static int
|
ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **rxom,
|
struct os_mbuf **out_txom, uint8_t *att_err)
|
{
|
struct ble_att_mtu_cmd *cmd;
|
struct ble_l2cap_chan *chan;
|
struct os_mbuf *txom;
|
uint16_t mtu;
|
int rc;
|
|
*att_err = 0; /* Silence unnecessary warning. */
|
txom = NULL;
|
|
ble_hs_lock();
|
rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
|
if (rc == 0) {
|
mtu = chan->my_mtu;
|
}
|
ble_hs_unlock();
|
|
if (rc != 0) {
|
goto done;
|
}
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
cmd = ble_att_cmd_prepare(BLE_ATT_OP_MTU_RSP, sizeof(*cmd), txom);
|
if (cmd == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
cmd->bamc_mtu = htole16(mtu);
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
struct ble_att_mtu_cmd *cmd;
|
struct ble_l2cap_chan *chan;
|
struct ble_hs_conn *conn;
|
struct os_mbuf *txom;
|
uint16_t mtu;
|
uint8_t att_err;
|
int rc;
|
|
txom = NULL;
|
mtu = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*cmd), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
cmd = (struct ble_att_mtu_cmd *)(*rxom)->om_data;
|
|
mtu = le16toh(cmd->bamc_mtu);
|
|
rc = ble_att_svr_build_mtu_rsp(conn_handle, rxom, &txom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_MTU_REQ,
|
att_err, 0);
|
if (rc == 0) {
|
ble_hs_lock();
|
|
rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
|
if (rc == 0) {
|
ble_att_set_peer_mtu(chan, mtu);
|
chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
|
mtu = ble_att_chan_mtu(chan);
|
}
|
|
ble_hs_unlock();
|
|
if (rc == 0) {
|
ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
|
}
|
}
|
return rc;
|
}
|
|
/**
|
* Fills the supplied mbuf with the variable length Information Data field of a
|
* Find Information ATT response.
|
*
|
* @param req The Find Information request being responded
|
* to.
|
* @param om The destination mbuf where the Information
|
* Data field gets written.
|
* @param mtu The ATT L2CAP channel MTU.
|
* @param format On success, the format field of the response
|
* gets stored here. One of:
|
* o BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT
|
* o BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT
|
*
|
* @return 0 on success; nonzero on failure.
|
*/
|
static int
|
ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
|
struct os_mbuf *om, uint16_t mtu, uint8_t *format)
|
{
|
struct ble_att_svr_entry *ha;
|
uint8_t *buf;
|
int num_entries;
|
int entry_sz;
|
int rc;
|
|
*format = 0;
|
num_entries = 0;
|
rc = 0;
|
|
STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
|
if (ha->ha_handle_id > end_handle) {
|
rc = 0;
|
goto done;
|
}
|
if (ha->ha_handle_id >= start_handle) {
|
if (ha->ha_uuid->type == BLE_UUID_TYPE_16) {
|
if (*format == 0) {
|
*format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
|
} else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT) {
|
rc = 0;
|
goto done;
|
}
|
|
entry_sz = 4;
|
} else {
|
if (*format == 0) {
|
*format = BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT;
|
} else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT) {
|
rc = 0;
|
goto done;
|
}
|
entry_sz = 18;
|
}
|
|
if (OS_MBUF_PKTLEN(om) + entry_sz > mtu) {
|
rc = 0;
|
goto done;
|
}
|
|
buf = os_mbuf_extend(om, entry_sz);
|
if (buf == NULL) {
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
put_le16(buf + 0, ha->ha_handle_id);
|
|
ble_uuid_flat(ha->ha_uuid, buf + 2);
|
|
num_entries++;
|
}
|
}
|
|
done:
|
if (rc == 0 && num_entries == 0) {
|
return BLE_HS_ENOENT;
|
} else {
|
return rc;
|
}
|
}
|
|
static int
|
ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
|
uint16_t start_handle, uint16_t end_handle,
|
struct os_mbuf **rxom,
|
struct os_mbuf **out_txom,
|
uint8_t *att_err)
|
{
|
struct ble_att_find_info_rsp *rsp;
|
struct os_mbuf *txom;
|
uint16_t mtu;
|
int rc;
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
/* Write the response base at the start of the buffer. The format field is
|
* unknown at this point; it will be filled in later.
|
*/
|
rsp = ble_att_cmd_prepare(BLE_ATT_OP_FIND_INFO_RSP, sizeof(*rsp), txom);
|
if (rsp == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
/* Write the variable length Information Data field, populating the format
|
* field as appropriate.
|
*/
|
mtu = ble_att_mtu(conn_handle);
|
rc = ble_att_svr_fill_info(start_handle, end_handle, txom, mtu,
|
&rsp->bafp_format);
|
if (rc != 0) {
|
*att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
|
rc = BLE_HS_ENOENT;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_FIND_INFO)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_find_info_req *req;
|
struct os_mbuf *txom;
|
uint16_t err_handle, start_handle, end_handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
err_handle = 0;
|
goto done;
|
}
|
|
req = (struct ble_att_find_info_req *)(*rxom)->om_data;
|
start_handle = le16toh(req->bafq_start_handle);
|
end_handle = le16toh(req->bafq_end_handle);
|
|
/* Tx error response if start handle is greater than end handle or is equal
|
* to 0 (Vol. 3, Part F, 3.4.3.1).
|
*/
|
if (start_handle > end_handle || start_handle == 0) {
|
att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
err_handle = start_handle;
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
rc = ble_att_svr_build_find_info_rsp(conn_handle,
|
start_handle, end_handle,
|
rxom, &txom, &att_err);
|
if (rc != 0) {
|
err_handle = start_handle;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_FIND_INFO_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
/**
|
* Fills a Find-By-Type-Value-Response with single entry.
|
*
|
* @param om The response mbuf.
|
* @param first First handle ID in the current group of IDs.
|
* @param last Last handle ID in the current group of ID.
|
* @param mtu The ATT L2CAP channel MTU.
|
*
|
* @return 0 if the response should be sent;
|
* BLE_HS_EAGAIN if the entry was successfully
|
* processed and subsequent entries can be
|
* inspected.
|
* Other nonzero on error.
|
*/
|
static int
|
ble_att_svr_fill_type_value_entry(struct os_mbuf *om, uint16_t first,
|
uint16_t last, int mtu,
|
uint8_t *out_att_err)
|
{
|
uint16_t u16;
|
int rsp_sz;
|
int rc;
|
|
rsp_sz = OS_MBUF_PKTHDR(om)->omp_len + 4;
|
if (rsp_sz > mtu) {
|
return 0;
|
}
|
|
put_le16(&u16, first);
|
rc = os_mbuf_append(om, &u16, 2);
|
if (rc != 0) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
return BLE_HS_ENOMEM;
|
}
|
|
put_le16(&u16, last);
|
rc = os_mbuf_append(om, &u16, 2);
|
if (rc != 0) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
return BLE_HS_ENOMEM;
|
}
|
|
return BLE_HS_EAGAIN;
|
}
|
|
static int
|
ble_att_svr_is_valid_find_group_type(const ble_uuid_t *uuid)
|
{
|
uint16_t uuid16;
|
|
uuid16 = ble_uuid_u16(uuid);
|
|
return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
|
uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE ||
|
uuid16 == BLE_ATT_UUID_CHARACTERISTIC;
|
}
|
|
static int
|
ble_att_svr_is_valid_group_end(const ble_uuid_t *uuid_group,
|
const ble_uuid_t *uuid)
|
{
|
uint16_t uuid16;
|
|
/* Grouping is defined only for 16-bit UUIDs, so any attribute ends group
|
* for non-16-bit UUIDs.
|
*/
|
if (uuid_group->type != BLE_UUID_TYPE_16) {
|
return 1;
|
}
|
|
/* Grouping is defined only for 16-bit UUIDs, so non-16-bit UUID attribute
|
* cannot end group.
|
*/
|
if (uuid->type != BLE_UUID_TYPE_16) {
|
return 0;
|
}
|
|
switch (ble_uuid_u16(uuid_group)) {
|
case BLE_ATT_UUID_PRIMARY_SERVICE:
|
case BLE_ATT_UUID_SECONDARY_SERVICE:
|
uuid16 = ble_uuid_u16(uuid);
|
|
/* Only Primary or Secondary Service types end service group. */
|
return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
|
uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
|
case BLE_ATT_UUID_CHARACTERISTIC:
|
/* Any valid grouping type ends characteristic group */
|
return ble_att_svr_is_valid_find_group_type(uuid);
|
default:
|
/* Any attribute type ends group of non-grouping type */
|
return 1;
|
}
|
}
|
|
/**
|
* Fills the supplied mbuf with the variable length Handles-Information-List
|
* field of a Find-By-Type-Value ATT response.
|
*
|
* @param req The Find-By-Type-Value-Request being responded
|
* to.
|
* @param rxom The mbuf containing the received request.
|
* @param txom The destination mbuf where the
|
* Handles-Information-List field gets
|
* written.
|
* @param mtu The ATT L2CAP channel MTU.
|
*
|
* @return 0 on success;
|
* BLE_HS_ENOENT if attribute not found;
|
* BLE_HS_EAPP on other error.
|
*/
|
static int
|
ble_att_svr_fill_type_value(uint16_t conn_handle,
|
uint16_t start_handle, uint16_t end_handle,
|
ble_uuid16_t attr_type,
|
struct os_mbuf *rxom, struct os_mbuf *txom,
|
uint16_t mtu, uint8_t *out_att_err)
|
{
|
struct ble_att_svr_entry *ha;
|
uint8_t buf[16];
|
uint16_t attr_len;
|
uint16_t first;
|
uint16_t prev;
|
int any_entries;
|
int rc;
|
|
first = 0;
|
prev = 0;
|
rc = 0;
|
|
/* Iterate through the attribute list, keeping track of the current
|
* matching group. For each attribute entry, determine if data needs to be
|
* written to the response.
|
*/
|
STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
|
if (ha->ha_handle_id < start_handle) {
|
continue;
|
}
|
|
/* Continue to look for end of group in case group is in progress. */
|
if (!first && ha->ha_handle_id > end_handle) {
|
break;
|
}
|
|
/* With group in progress, check if current attribute ends it. */
|
if (first) {
|
if (!ble_att_svr_is_valid_group_end(&attr_type.u, ha->ha_uuid)) {
|
prev = ha->ha_handle_id;
|
continue;
|
}
|
|
rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
|
out_att_err);
|
if (rc != BLE_HS_EAGAIN) {
|
goto done;
|
}
|
|
first = 0;
|
prev = 0;
|
|
/* Break in case we were just looking for end of group past the end
|
* handle ID. */
|
if (ha->ha_handle_id > end_handle) {
|
break;
|
}
|
}
|
|
/* Compare the attribute type and value to the request fields to
|
* determine if this attribute matches.
|
*/
|
if (ble_uuid_cmp(ha->ha_uuid, &attr_type.u) == 0) {
|
rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf,
|
&attr_len, out_att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
/* value is at the end of req */
|
rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req),
|
buf, attr_len);
|
if (rc == 0) {
|
first = ha->ha_handle_id;
|
prev = ha->ha_handle_id;
|
}
|
}
|
}
|
|
/* Process last group in case a group was in progress when the end of the
|
* attribute list was reached.
|
*/
|
if (first) {
|
rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
|
out_att_err);
|
if (rc == BLE_HS_EAGAIN) {
|
rc = 0;
|
}
|
} else {
|
rc = 0;
|
}
|
|
done:
|
any_entries = OS_MBUF_PKTHDR(txom)->omp_len >
|
BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ;
|
if (rc == 0 && !any_entries) {
|
*out_att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
|
return BLE_HS_ENOENT;
|
} else {
|
return rc;
|
}
|
}
|
|
static int
|
ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,
|
uint16_t start_handle,
|
uint16_t end_handle,
|
ble_uuid16_t attr_type,
|
struct os_mbuf **rxom,
|
struct os_mbuf **out_txom,
|
uint8_t *out_att_err)
|
{
|
struct os_mbuf *txom;
|
uint16_t mtu;
|
uint8_t *buf;
|
int rc;
|
|
rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
/* info list is filled later on */
|
buf = ble_att_cmd_prepare(BLE_ATT_OP_FIND_TYPE_VALUE_RSP, 0, txom);
|
if (buf == NULL) {
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
/* Write the variable length Information Data field. */
|
mtu = ble_att_mtu(conn_handle);
|
|
rc = ble_att_svr_fill_type_value(conn_handle, start_handle, end_handle,
|
attr_type, *rxom, txom, mtu,
|
out_att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_find_type_value(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_FIND_TYPE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_find_type_value_req *req;
|
uint16_t start_handle, end_handle;
|
ble_uuid16_t attr_type;
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_find_type_value_req *)(*rxom)->om_data;
|
start_handle = le16toh(req->bavq_start_handle);
|
end_handle = le16toh(req->bavq_end_handle);
|
attr_type = (ble_uuid16_t) BLE_UUID16_INIT(le16toh(req->bavq_attr_type));
|
|
/* Tx error response if start handle is greater than end handle or is equal
|
* to 0 (Vol. 3, Part F, 3.4.3.3).
|
*/
|
if (start_handle > end_handle || start_handle == 0) {
|
att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
err_handle = start_handle;
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
rc = ble_att_svr_build_find_type_value_rsp(conn_handle, start_handle,
|
end_handle, attr_type, rxom,
|
&txom, &att_err);
|
if (rc != 0) {
|
err_handle = start_handle;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
|
BLE_ATT_OP_FIND_TYPE_VALUE_REQ, att_err,
|
err_handle);
|
return rc;
|
}
|
|
static int
|
ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
|
uint16_t start_handle, uint16_t end_handle,
|
const ble_uuid_t *uuid,
|
struct os_mbuf **rxom,
|
struct os_mbuf **out_txom,
|
uint8_t *att_err,
|
uint16_t *err_handle)
|
{
|
struct ble_att_attr_data_list *data;
|
struct ble_att_read_type_rsp *rsp;
|
struct ble_att_svr_entry *entry;
|
struct os_mbuf *txom;
|
uint16_t attr_len;
|
uint16_t mtu;
|
uint8_t buf[19];
|
int entry_written;
|
int txomlen;
|
int prev_attr_len;
|
int rc;
|
|
*att_err = 0; /* Silence unnecessary warning. */
|
|
*err_handle = start_handle;
|
entry_written = 0;
|
prev_attr_len = 0;
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
/* Allocate space for the respose base, but don't fill in the fields. They
|
* get filled in at the end, when we know the value of the length field.
|
*/
|
|
rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_TYPE_RSP, sizeof(*rsp), txom);
|
if (rsp == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
*err_handle = 0;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
mtu = ble_att_mtu(conn_handle);
|
|
/* Find all matching attributes, writing a record for each. */
|
entry = NULL;
|
while (1) {
|
entry = ble_att_svr_find_by_uuid(entry, uuid, end_handle);
|
if (entry == NULL) {
|
rc = BLE_HS_ENOENT;
|
break;
|
}
|
|
if (entry->ha_handle_id >= start_handle) {
|
rc = ble_att_svr_read_flat(conn_handle, entry, 0, sizeof buf, buf,
|
&attr_len, att_err);
|
if (rc != 0) {
|
*err_handle = entry->ha_handle_id;
|
goto done;
|
}
|
|
if (attr_len > mtu - 4) {
|
attr_len = mtu - 4;
|
}
|
|
if (prev_attr_len == 0) {
|
prev_attr_len = attr_len;
|
} else if (prev_attr_len != attr_len) {
|
break;
|
}
|
|
txomlen = OS_MBUF_PKTHDR(txom)->omp_len + 2 + attr_len;
|
if (txomlen > mtu) {
|
break;
|
}
|
|
data = os_mbuf_extend(txom, 2 + attr_len);
|
if (data == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
*err_handle = entry->ha_handle_id;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
data->handle = htole16(entry->ha_handle_id);
|
memcpy(data->value, buf, attr_len);
|
entry_written = 1;
|
}
|
}
|
|
done:
|
if (!entry_written) {
|
/* No matching attributes. */
|
if (*att_err == 0) {
|
*att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
|
}
|
if (rc == 0) {
|
rc = BLE_HS_ENOENT;
|
}
|
} else {
|
/* Send what we can, even if an error was encountered. */
|
rc = 0;
|
*att_err = 0;
|
|
/* Fill the response base. */
|
rsp->batp_length = sizeof(*data) + prev_attr_len;
|
}
|
|
*out_txom = txom;
|
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_READ_TYPE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_read_type_req *req;
|
uint16_t start_handle, end_handle;
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint16_t pktlen;
|
ble_uuid_any_t uuid;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
err_handle = 0;
|
att_err = 0;
|
|
pktlen = OS_MBUF_PKTLEN(*rxom);
|
if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
|
/* Malformed packet */
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_read_type_req *)(*rxom)->om_data;
|
|
start_handle = le16toh(req->batq_start_handle);
|
end_handle = le16toh(req->batq_end_handle);
|
|
if (start_handle > end_handle || start_handle == 0) {
|
att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
err_handle = start_handle;
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req),
|
pktlen - sizeof(*req));
|
if (rc != 0) {
|
att_err = BLE_ATT_ERR_INVALID_PDU;
|
rc = BLE_HS_EMSGSIZE;
|
goto done;
|
}
|
|
rc = ble_att_svr_build_read_type_rsp(conn_handle, start_handle, end_handle,
|
&uuid.u, rxom, &txom, &att_err,
|
&err_handle);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_TYPE_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_READ)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_read_req *req;
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_read_req *)(*rxom)->om_data;
|
|
err_handle = le16toh(req->barq_handle);
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) {
|
att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
rc = ble_att_svr_read_handle(conn_handle, err_handle, 0, txom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_READ_BLOB)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_read_blob_req *req;
|
struct os_mbuf *txom;
|
uint16_t err_handle, offset;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_read_blob_req *)(*rxom)->om_data;
|
|
err_handle = le16toh(req->babq_handle);
|
offset = le16toh(req->babq_offset);
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_READ_BLOB_RSP, 0, txom) == NULL) {
|
att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
rc = ble_att_svr_read_handle(conn_handle, err_handle, offset,
|
txom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_BLOB_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
static int
|
ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
|
struct os_mbuf **rxom,
|
struct os_mbuf **out_txom,
|
uint8_t *att_err,
|
uint16_t *err_handle)
|
{
|
struct os_mbuf *txom;
|
uint16_t handle;
|
uint16_t mtu;
|
int rc;
|
|
mtu = ble_att_mtu(conn_handle);
|
|
rc = ble_att_svr_pkt(rxom, &txom, att_err);
|
if (rc != 0) {
|
*err_handle = 0;
|
goto done;
|
}
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_READ_MULT_RSP, 0, txom) == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
*err_handle = 0;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
/* Iterate through requested handles, reading the corresponding attribute
|
* for each. Stop when there are no more handles to process, or the
|
* response is full.
|
*/
|
while (OS_MBUF_PKTLEN(*rxom) >= 2 && OS_MBUF_PKTLEN(txom) < mtu) {
|
/* Ensure the full 16-bit handle is contiguous at the start of the
|
* mbuf.
|
*/
|
rc = ble_att_svr_pullup_req_base(rxom, 2, att_err);
|
if (rc != 0) {
|
*err_handle = 0;
|
goto done;
|
}
|
|
/* Extract the 16-bit handle and strip it from the front of the
|
* mbuf.
|
*/
|
handle = get_le16((*rxom)->om_data);
|
os_mbuf_adj(*rxom, 2);
|
|
rc = ble_att_svr_read_handle(conn_handle, handle, 0, txom, att_err);
|
if (rc != 0) {
|
*err_handle = handle;
|
goto done;
|
}
|
}
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_READ_MULT)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
err_handle = 0;
|
att_err = 0;
|
|
rc = ble_att_svr_build_read_mult_rsp(conn_handle, rxom, &txom, &att_err,
|
&err_handle);
|
|
return ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_MULT_REQ,
|
att_err, err_handle);
|
}
|
|
static int
|
ble_att_svr_is_valid_read_group_type(const ble_uuid_t *uuid)
|
{
|
uint16_t uuid16;
|
|
uuid16 = ble_uuid_u16(uuid);
|
|
return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
|
uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
|
}
|
|
static int
|
ble_att_svr_service_uuid(struct ble_att_svr_entry *entry,
|
ble_uuid_any_t *uuid, uint8_t *out_att_err)
|
{
|
uint8_t val[16];
|
uint16_t attr_len;
|
int rc;
|
|
rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, entry, 0, sizeof(val), val,
|
&attr_len, out_att_err);
|
if (rc != 0) {
|
return rc;
|
}
|
|
rc = ble_uuid_init_from_buf(uuid, val, attr_len);
|
|
return rc;
|
}
|
|
static int
|
ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu,
|
uint16_t start_group_handle,
|
uint16_t end_group_handle,
|
const ble_uuid_t *service_uuid)
|
{
|
uint8_t *buf;
|
int len;
|
|
if (service_uuid->type == BLE_UUID_TYPE_16) {
|
len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
|
} else {
|
len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
|
}
|
if (OS_MBUF_PKTLEN(om) + len > mtu) {
|
return BLE_HS_EMSGSIZE;
|
}
|
|
buf = os_mbuf_extend(om, len);
|
if (buf == NULL) {
|
return BLE_HS_ENOMEM;
|
}
|
|
put_le16(buf + 0, start_group_handle);
|
put_le16(buf + 2, end_group_handle);
|
ble_uuid_flat(service_uuid, buf + 4);
|
|
return 0;
|
}
|
|
/**
|
* @return 0 on success; BLE_HS error code on failure.
|
*/
|
static int
|
ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle,
|
uint16_t start_handle,
|
uint16_t end_handle,
|
const ble_uuid_t *group_uuid,
|
struct os_mbuf **rxom,
|
struct os_mbuf **out_txom,
|
uint8_t *att_err,
|
uint16_t *err_handle)
|
{
|
struct ble_att_read_group_type_rsp *rsp;
|
struct ble_att_svr_entry *entry;
|
struct os_mbuf *txom;
|
uint16_t start_group_handle;
|
uint16_t end_group_handle;
|
uint16_t mtu;
|
ble_uuid_any_t service_uuid;
|
int rc;
|
|
/* Silence warnings. */
|
end_group_handle = 0;
|
|
*att_err = 0;
|
*err_handle = start_handle;
|
|
mtu = ble_att_mtu(conn_handle);
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
/* Reserve space for the response base. */
|
rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_GROUP_TYPE_RSP, sizeof(*rsp),
|
txom);
|
if (rsp == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
start_group_handle = 0;
|
rsp->bagp_length = 0;
|
STAILQ_FOREACH(entry, &ble_att_svr_list, ha_next) {
|
if (entry->ha_handle_id < start_handle) {
|
continue;
|
}
|
if (entry->ha_handle_id > end_handle) {
|
/* The full input range has been searched. */
|
rc = 0;
|
goto done;
|
}
|
|
if (start_group_handle != 0) {
|
/* We have already found the start of a group. */
|
if (!ble_att_svr_is_valid_read_group_type(entry->ha_uuid)) {
|
/* This attribute is part of the current group. */
|
end_group_handle = entry->ha_handle_id;
|
} else {
|
/* This attribute marks the end of the group. Write an entry
|
* representing the group to the response.
|
*/
|
rc = ble_att_svr_read_group_type_entry_write(
|
txom, mtu, start_group_handle, end_group_handle,
|
&service_uuid.u);
|
start_group_handle = 0;
|
end_group_handle = 0;
|
if (rc != 0) {
|
*err_handle = entry->ha_handle_id;
|
if (rc == BLE_HS_ENOMEM) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
} else {
|
BLE_HS_DBG_ASSERT(rc == BLE_HS_EMSGSIZE);
|
}
|
goto done;
|
}
|
}
|
}
|
|
if (start_group_handle == 0) {
|
/* We are looking for the start of a group. */
|
if (ble_uuid_cmp(entry->ha_uuid, group_uuid) == 0) {
|
/* Found a group start. Read the group UUID. */
|
rc = ble_att_svr_service_uuid(entry, &service_uuid, att_err);
|
if (rc != 0) {
|
*err_handle = entry->ha_handle_id;
|
goto done;
|
}
|
|
/* Make sure the group UUID lengths are consistent. If this
|
* group has a different length UUID, then cut the response
|
* short.
|
*/
|
switch (rsp->bagp_length) {
|
case 0:
|
if (service_uuid.u.type == BLE_UUID_TYPE_16) {
|
rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
|
} else {
|
rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
|
}
|
break;
|
|
case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16:
|
if (service_uuid.u.type != BLE_UUID_TYPE_16) {
|
rc = 0;
|
goto done;
|
}
|
break;
|
|
case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128:
|
if (service_uuid.u.type == BLE_UUID_TYPE_16) {
|
rc = 0;
|
goto done;
|
}
|
break;
|
|
default:
|
BLE_HS_DBG_ASSERT(0);
|
goto done;
|
}
|
|
start_group_handle = entry->ha_handle_id;
|
end_group_handle = entry->ha_handle_id;
|
}
|
}
|
}
|
|
rc = 0;
|
|
done:
|
if (rc == 0) {
|
if (start_group_handle != 0) {
|
/* A group was being processed. Add its corresponding entry to the
|
* response.
|
*/
|
|
if (entry == NULL) {
|
/* We have reached the end of the attribute list. Indicate an
|
* end handle of 0xffff so that the client knows there are no
|
* more attributes without needing to send a follow-up request.
|
*/
|
end_group_handle = 0xffff;
|
}
|
|
rc = ble_att_svr_read_group_type_entry_write(txom, mtu,
|
start_group_handle,
|
end_group_handle,
|
&service_uuid.u);
|
if (rc == BLE_HS_ENOMEM) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
}
|
}
|
|
if (OS_MBUF_PKTLEN(txom) <= BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ) {
|
*att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
|
rc = BLE_HS_ENOENT;
|
}
|
}
|
|
if (rc == 0 || rc == BLE_HS_EMSGSIZE) {
|
rc = 0;
|
}
|
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_read_group_type(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_READ_GROUP_TYPE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_read_group_type_req *req;
|
struct os_mbuf *txom;
|
ble_uuid_any_t uuid;
|
uint16_t err_handle, start_handle, end_handle;
|
uint16_t pktlen;
|
uint8_t att_err;
|
int om_uuid_len;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
err_handle = 0;
|
att_err = 0;
|
|
pktlen = OS_MBUF_PKTLEN(*rxom);
|
if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
|
/* Malformed packet */
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_read_group_type_req *)(*rxom)->om_data;
|
|
start_handle = le16toh(req->bagq_start_handle);
|
end_handle = le16toh(req->bagq_end_handle);
|
|
if (start_handle > end_handle || start_handle == 0) {
|
att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
err_handle = start_handle;
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
om_uuid_len = OS_MBUF_PKTHDR(*rxom)->omp_len - sizeof(*req);
|
rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req), om_uuid_len);
|
if (rc != 0) {
|
att_err = BLE_ATT_ERR_INVALID_PDU;
|
err_handle = start_handle;
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
if (!ble_att_svr_is_valid_read_group_type(&uuid.u)) {
|
att_err = BLE_ATT_ERR_UNSUPPORTED_GROUP;
|
err_handle = start_handle;
|
rc = BLE_HS_EREJECT;
|
goto done;
|
}
|
|
rc = ble_att_svr_build_read_group_type_rsp(conn_handle, start_handle,
|
end_handle, &uuid.u,
|
rxom, &txom, &att_err,
|
&err_handle);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
|
BLE_ATT_OP_READ_GROUP_TYPE_REQ, att_err,
|
err_handle);
|
return rc;
|
}
|
|
static int
|
ble_att_svr_build_write_rsp(struct os_mbuf **rxom, struct os_mbuf **out_txom,
|
uint8_t *att_err)
|
{
|
struct os_mbuf *txom;
|
int rc;
|
|
/* Allocate a new buffer for the response. A write response never reuses
|
* the request buffer. See the note at the top of this file for details.
|
*/
|
rc = ble_att_svr_pkt(rxom, &txom, att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_WRITE_RSP, 0, txom) == NULL) {
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_WRITE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_write_req *req;
|
struct os_mbuf *txom;
|
uint16_t handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_write_req *)(*rxom)->om_data;
|
|
handle = le16toh(req->bawq_handle);
|
|
/* Allocate the write response. This must be done prior to processing the
|
* request. See the note at the top of this file for details.
|
*/
|
rc = ble_att_svr_build_write_rsp(rxom, &txom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
/* Strip the request base from the front of the mbuf. */
|
os_mbuf_adj(*rxom, sizeof(*req));
|
|
rc = ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_WRITE_REQ,
|
att_err, handle);
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_write_req *req;
|
uint8_t att_err;
|
uint16_t handle;
|
int rc;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
return rc;
|
}
|
|
req = (struct ble_att_write_req *)(*rxom)->om_data;
|
|
handle = le16toh(req->bawq_handle);
|
|
/* Strip the request base from the front of the mbuf. */
|
os_mbuf_adj(*rxom, sizeof(*req));
|
|
return ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
|
}
|
|
int
|
ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
|
{
|
int rc;
|
|
rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0,
|
&om, NULL);
|
|
/* Free the mbuf if it wasn't relinquished to the application. */
|
os_mbuf_free_chain(om);
|
|
return rc;
|
}
|
|
static void
|
ble_att_svr_prep_free(struct ble_att_prep_entry *entry)
|
{
|
if (entry != NULL) {
|
os_mbuf_free_chain(entry->bape_value);
|
#if MYNEWT_VAL(BLE_HS_DEBUG)
|
memset(entry, 0xff, sizeof *entry);
|
#endif
|
os_memblock_put(&ble_att_svr_prep_entry_pool, entry);
|
}
|
}
|
|
static struct ble_att_prep_entry *
|
ble_att_svr_prep_alloc(uint8_t *att_err)
|
{
|
struct ble_att_prep_entry *entry;
|
|
entry = os_memblock_get(&ble_att_svr_prep_entry_pool);
|
if (entry == NULL) {
|
*att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
|
return NULL;
|
}
|
|
memset(entry, 0, sizeof *entry);
|
entry->bape_value = ble_hs_mbuf_l2cap_pkt();
|
if (entry->bape_value == NULL) {
|
ble_att_svr_prep_free(entry);
|
*att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
return NULL;
|
}
|
|
return entry;
|
}
|
|
static struct ble_att_prep_entry *
|
ble_att_svr_prep_find_prev(struct ble_att_svr_conn *basc, uint16_t handle,
|
uint16_t offset)
|
{
|
struct ble_att_prep_entry *entry;
|
struct ble_att_prep_entry *prev;
|
|
prev = NULL;
|
SLIST_FOREACH(entry, &basc->basc_prep_list, bape_next) {
|
if (entry->bape_handle > handle) {
|
break;
|
}
|
|
if (entry->bape_handle == handle && entry->bape_offset > offset) {
|
break;
|
}
|
|
prev = entry;
|
}
|
|
return prev;
|
}
|
|
void
|
ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list)
|
{
|
struct ble_att_prep_entry *entry;
|
|
while ((entry = SLIST_FIRST(prep_list)) != NULL) {
|
SLIST_REMOVE_HEAD(prep_list, bape_next);
|
ble_att_svr_prep_free(entry);
|
}
|
}
|
|
/**
|
* @return 0 on success; ATT error code on failure.
|
*/
|
static int
|
ble_att_svr_prep_validate(struct ble_att_prep_entry_list *prep_list,
|
uint16_t *err_handle)
|
{
|
struct ble_att_prep_entry *entry;
|
struct ble_att_prep_entry *prev;
|
int cur_len;
|
|
prev = NULL;
|
SLIST_FOREACH(entry, prep_list, bape_next) {
|
if (prev == NULL || prev->bape_handle != entry->bape_handle) {
|
/* Ensure attribute write starts at offset 0. */
|
if (entry->bape_offset != 0) {
|
*err_handle = entry->bape_handle;
|
return BLE_ATT_ERR_INVALID_OFFSET;
|
}
|
} else {
|
/* Ensure entry continues where previous left off. */
|
if (prev->bape_offset + OS_MBUF_PKTLEN(prev->bape_value) !=
|
entry->bape_offset) {
|
|
*err_handle = entry->bape_handle;
|
return BLE_ATT_ERR_INVALID_OFFSET;
|
}
|
}
|
|
cur_len = entry->bape_offset + OS_MBUF_PKTLEN(entry->bape_value);
|
if (cur_len > BLE_ATT_ATTR_MAX_LEN) {
|
*err_handle = entry->bape_handle;
|
return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
|
}
|
|
prev = entry;
|
}
|
|
return 0;
|
}
|
|
static void
|
ble_att_svr_prep_extract(struct ble_att_prep_entry_list *prep_list,
|
uint16_t *out_attr_handle,
|
struct os_mbuf **out_om)
|
{
|
struct ble_att_prep_entry *entry;
|
struct ble_att_prep_entry *first;
|
struct os_mbuf *om;
|
uint16_t attr_handle;
|
|
BLE_HS_DBG_ASSERT(!SLIST_EMPTY(prep_list));
|
|
first = SLIST_FIRST(prep_list);
|
attr_handle = first->bape_handle;
|
om = NULL;
|
|
while ((entry = SLIST_FIRST(prep_list)) != NULL) {
|
if (entry->bape_handle != attr_handle) {
|
break;
|
}
|
|
if (om == NULL) {
|
om = entry->bape_value;
|
} else {
|
os_mbuf_concat(om, entry->bape_value);
|
}
|
entry->bape_value = NULL;
|
|
SLIST_REMOVE_HEAD(prep_list, bape_next);
|
ble_att_svr_prep_free(entry);
|
}
|
|
*out_attr_handle = attr_handle;
|
*out_om = om;
|
}
|
|
/**
|
* @return 0 on success; ATT error code on failure.
|
*/
|
static int
|
ble_att_svr_prep_write(uint16_t conn_handle,
|
struct ble_att_prep_entry_list *prep_list,
|
uint16_t *err_handle)
|
{
|
struct ble_att_svr_entry *attr;
|
struct os_mbuf *om;
|
uint16_t attr_handle;
|
uint8_t att_err;
|
int rc;
|
|
*err_handle = 0; /* Silence unnecessary warning. */
|
|
/* First, validate the contents of the prepare queue. */
|
rc = ble_att_svr_prep_validate(prep_list, err_handle);
|
if (rc != 0) {
|
return rc;
|
}
|
|
/* Contents are valid; perform the writes. */
|
while (!SLIST_EMPTY(prep_list)) {
|
ble_att_svr_prep_extract(prep_list, &attr_handle, &om);
|
|
/* Attribute existence was verified during prepare-write request
|
* processing.
|
*/
|
attr = ble_att_svr_find_by_handle(attr_handle);
|
BLE_HS_DBG_ASSERT(attr != NULL);
|
|
rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err);
|
os_mbuf_free_chain(om);
|
if (rc != 0) {
|
*err_handle = attr_handle;
|
return att_err;
|
}
|
}
|
|
return 0;
|
}
|
|
static int
|
ble_att_svr_insert_prep_entry(uint16_t conn_handle,
|
uint16_t handle, uint16_t offset,
|
const struct os_mbuf *rxom,
|
uint8_t *out_att_err)
|
{
|
struct ble_att_prep_entry *prep_entry;
|
struct ble_att_prep_entry *prep_prev;
|
struct ble_hs_conn *conn;
|
int rc;
|
|
conn = ble_hs_conn_find_assert(conn_handle);
|
|
prep_entry = ble_att_svr_prep_alloc(out_att_err);
|
if (prep_entry == NULL) {
|
return BLE_HS_ENOMEM;
|
}
|
prep_entry->bape_handle = handle;
|
prep_entry->bape_offset = offset;
|
|
/* Append attribute value from request onto prep mbuf. */
|
rc = os_mbuf_appendfrom(
|
prep_entry->bape_value,
|
rxom,
|
sizeof(struct ble_att_prep_write_cmd),
|
OS_MBUF_PKTLEN(rxom) - sizeof(struct ble_att_prep_write_cmd));
|
if (rc != 0) {
|
/* Failed to allocate an mbuf to hold the additional data. */
|
ble_att_svr_prep_free(prep_entry);
|
|
/* XXX: We need to differentiate between "prepare queue full" and
|
* "insufficient resources." Currently, we always indicate prepare
|
* queue full.
|
*/
|
*out_att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
|
return rc;
|
}
|
|
prep_prev = ble_att_svr_prep_find_prev(&conn->bhc_att_svr,
|
handle, offset);
|
if (prep_prev == NULL) {
|
SLIST_INSERT_HEAD(&conn->bhc_att_svr.basc_prep_list, prep_entry,
|
bape_next);
|
} else {
|
SLIST_INSERT_AFTER(prep_prev, prep_entry, bape_next);
|
}
|
|
#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO != 0
|
conn->bhc_att_svr.basc_prep_timeout_at =
|
ble_npl_time_get() + BLE_HS_ATT_SVR_QUEUED_WRITE_TMO;
|
|
ble_hs_timer_resched();
|
#endif
|
|
return 0;
|
}
|
|
int
|
ble_att_svr_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_prep_write_cmd *req;
|
struct ble_att_svr_entry *attr_entry;
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_prep_write_cmd *)(*rxom)->om_data;
|
|
err_handle = le16toh(req->bapc_handle);
|
|
attr_entry = ble_att_svr_find_by_handle(le16toh(req->bapc_handle));
|
|
/* A prepare write request gets rejected for the following reasons:
|
* 1. Insufficient authorization.
|
* 2. Insufficient authentication.
|
* 3. Insufficient encryption key size (XXX: Not checked).
|
* 4. Insufficient encryption (XXX: Not checked).
|
* 5. Invalid handle.
|
* 6. Write not permitted.
|
*/
|
|
/* <5> */
|
if (attr_entry == NULL) {
|
rc = BLE_HS_ENOENT;
|
att_err = BLE_ATT_ERR_INVALID_HANDLE;
|
goto done;
|
}
|
|
/* <1>, <2>, <4>, <6> */
|
rc = ble_att_svr_check_perms(conn_handle, 0, attr_entry, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
ble_hs_lock();
|
rc = ble_att_svr_insert_prep_entry(conn_handle, le16toh(req->bapc_handle),
|
le16toh(req->bapc_offset), *rxom,
|
&att_err);
|
ble_hs_unlock();
|
|
/* Reuse rxom for response. On success, the response is identical to
|
* request except for op code. On error, the buffer contents will get
|
* cleared before the error gets written.
|
*/
|
txom = *rxom;
|
*rxom = NULL;
|
|
if (rc != 0) {
|
goto done;
|
}
|
|
/* adjust for ATT header */
|
os_mbuf_prepend(txom, 1);
|
txom->om_data[0] = BLE_ATT_OP_PREP_WRITE_RSP;
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_PREP_WRITE_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_prep_entry_list prep_list;
|
struct ble_att_exec_write_req *req;
|
struct ble_hs_conn *conn;
|
struct os_mbuf *txom;
|
uint16_t err_handle;
|
uint8_t att_err;
|
uint8_t flags;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
err_handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
|
if (rc != 0) {
|
flags = 0;
|
goto done;
|
}
|
|
req = (struct ble_att_exec_write_req *)(*rxom)->om_data;
|
|
flags = req->baeq_flags;
|
|
/* Just reuse the request buffer for the response. */
|
txom = *rxom;
|
*rxom = NULL;
|
os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_EXEC_WRITE_RSP, 0, txom) == NULL) {
|
att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
rc = BLE_HS_ENOMEM;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
if (rc == 0) {
|
ble_hs_lock();
|
conn = ble_hs_conn_find_assert(conn_handle);
|
|
/* Extract the list of prepared writes from the connection so
|
* that they can be processed after the mutex is unlocked. They
|
* aren't processed now because attribute writes involve executing
|
* an application callback.
|
*/
|
prep_list = conn->bhc_att_svr.basc_prep_list;
|
SLIST_INIT(&conn->bhc_att_svr.basc_prep_list);
|
ble_hs_unlock();
|
|
if (flags) {
|
/* Perform attribute writes. */
|
att_err = ble_att_svr_prep_write(conn_handle, &prep_list,
|
&err_handle);
|
if (att_err != 0) {
|
rc = BLE_HS_EAPP;
|
}
|
}
|
|
/* Free the prep entries. */
|
ble_att_svr_prep_clear(&prep_list);
|
}
|
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_EXEC_WRITE_REQ,
|
att_err, err_handle);
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_notify(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_NOTIFY)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_notify_req *req;
|
struct ble_gap_sec_state sec_state;
|
uint16_t handle;
|
int rc;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
|
if (rc != 0) {
|
return BLE_HS_ENOMEM;
|
}
|
|
req = (struct ble_att_notify_req *)(*rxom)->om_data;
|
|
handle = le16toh(req->banq_handle);
|
|
if (handle == 0) {
|
return BLE_HS_EBADDATA;
|
}
|
|
ble_att_svr_get_sec_state(conn_handle, &sec_state);
|
|
/* All indications shall be confirmed, but only these with required
|
* security established shall be pass to application
|
*/
|
/* mask this condition temporary
|
* todo in future @chao
|
*/
|
#if 0
|
if (MYNEWT_VAL(BLE_SM_SC_LVL) >= 2 && !sec_state.encrypted) {
|
return 0;
|
}
|
#endif
|
/* Strip the request base from the front of the mbuf. */
|
os_mbuf_adj(*rxom, sizeof(*req));
|
|
ble_gap_notify_rx_event(conn_handle, handle, *rxom, 0);
|
*rxom = NULL;
|
|
return 0;
|
}
|
|
/**
|
* @return 0 on success; nonzero on failure.
|
*/
|
static int
|
ble_att_svr_build_indicate_rsp(struct os_mbuf **rxom,
|
struct os_mbuf **out_txom, uint8_t *out_att_err)
|
{
|
struct os_mbuf *txom;
|
int rc;
|
|
/* Allocate a new buffer for the response. An indicate response never
|
* reuses the request buffer. See the note at the top of this file for
|
* details.
|
*/
|
rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
if (ble_att_cmd_prepare(BLE_ATT_OP_INDICATE_RSP, 0, txom) == NULL) {
|
rc = BLE_HS_ENOMEM;
|
*out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
|
goto done;
|
}
|
|
rc = 0;
|
|
done:
|
*out_txom = txom;
|
return rc;
|
}
|
|
int
|
ble_att_svr_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
|
{
|
#if !MYNEWT_VAL(BLE_ATT_SVR_INDICATE)
|
return BLE_HS_ENOTSUP;
|
#endif
|
|
struct ble_att_indicate_req *req;
|
struct ble_gap_sec_state sec_state;
|
struct os_mbuf *txom;
|
uint16_t handle;
|
uint8_t att_err;
|
int rc;
|
|
/* Initialize some values in case of early error. */
|
txom = NULL;
|
att_err = 0;
|
handle = 0;
|
|
rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
|
if (rc != 0) {
|
goto done;
|
}
|
|
req = (struct ble_att_indicate_req *)(*rxom)->om_data;
|
|
handle = le16toh(req->baiq_handle);
|
|
if (handle == 0) {
|
rc = BLE_HS_EBADDATA;
|
goto done;
|
}
|
|
/* Allocate the indicate response. This must be done prior to processing
|
* the request. See the note at the top of this file for details.
|
*/
|
rc = ble_att_svr_build_indicate_rsp(rxom, &txom, &att_err);
|
if (rc != 0) {
|
goto done;
|
}
|
|
ble_att_svr_get_sec_state(conn_handle, &sec_state);
|
|
/* All indications shall be confirmed, but only these with required
|
* security established shall be pass to application
|
*/
|
|
/* mask this condition temporary
|
* todo in future @chao
|
*/
|
#if 0
|
if (MYNEWT_VAL(BLE_SM_SC_LVL) >= 2 && !sec_state.encrypted) {
|
goto done;
|
}
|
#endif
|
|
/* Strip the request base from the front of the mbuf. */
|
os_mbuf_adj(*rxom, sizeof(*req));
|
|
ble_gap_notify_rx_event(conn_handle, handle, *rxom, 1);
|
*rxom = NULL;
|
|
rc = 0;
|
|
done:
|
rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_INDICATE_REQ,
|
att_err, handle);
|
return rc;
|
}
|
|
static void
|
ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
|
struct ble_att_svr_entry_list *dst,
|
uint16_t start_handle, uint16_t end_handle)
|
{
|
|
struct ble_att_svr_entry *entry;
|
struct ble_att_svr_entry *prev;
|
struct ble_att_svr_entry *remove;
|
struct ble_att_svr_entry *insert;
|
|
/* Find first matching element to move */
|
remove = NULL;
|
entry = STAILQ_FIRST(src);
|
while (entry && entry->ha_handle_id < start_handle) {
|
remove = entry;
|
entry = STAILQ_NEXT(entry, ha_next);
|
}
|
|
/* Nothing to remove? */
|
if (!entry) {
|
return;
|
}
|
|
/* Find element after which we'll put moved elements */
|
prev = NULL;
|
insert = STAILQ_FIRST(dst);
|
while (insert && insert->ha_handle_id < start_handle) {
|
prev = insert;
|
insert = STAILQ_NEXT(insert, ha_next);
|
}
|
insert = prev;
|
|
/* Move elements */
|
while (entry && entry->ha_handle_id <= end_handle) {
|
/* Remove either from head or after prev (which is current one) */
|
if (remove == NULL) {
|
STAILQ_REMOVE_HEAD(src, ha_next);
|
} else {
|
STAILQ_REMOVE_AFTER(src, remove, ha_next);
|
}
|
|
/* Insert current element */
|
if (insert == NULL) {
|
STAILQ_INSERT_HEAD(dst, entry, ha_next);
|
insert = STAILQ_FIRST(dst);
|
} else {
|
STAILQ_INSERT_AFTER(dst, insert, entry, ha_next);
|
insert = entry;
|
}
|
|
/* Calculate next candidate to remove */
|
if (remove == NULL) {
|
entry = STAILQ_FIRST(src);
|
} else {
|
entry = STAILQ_NEXT(remove, ha_next);
|
}
|
}
|
}
|
|
void
|
ble_att_svr_hide_range(uint16_t start_handle, uint16_t end_handle)
|
{
|
ble_att_svr_move_entries(&ble_att_svr_list, &ble_att_svr_hidden_list,
|
start_handle, end_handle);
|
}
|
|
void
|
ble_att_svr_restore_range(uint16_t start_handle, uint16_t end_handle)
|
{
|
ble_att_svr_move_entries(&ble_att_svr_hidden_list, &ble_att_svr_list,
|
start_handle, end_handle);
|
}
|
|
void
|
ble_att_svr_reset(void)
|
{
|
struct ble_att_svr_entry *entry;
|
|
while ((entry = STAILQ_FIRST(&ble_att_svr_list)) != NULL) {
|
STAILQ_REMOVE_HEAD(&ble_att_svr_list, ha_next);
|
ble_att_svr_entry_free(entry);
|
}
|
|
while ((entry = STAILQ_FIRST(&ble_att_svr_hidden_list)) != NULL) {
|
STAILQ_REMOVE_HEAD(&ble_att_svr_hidden_list, ha_next);
|
ble_att_svr_entry_free(entry);
|
}
|
|
ble_att_svr_id = 0;
|
|
/* Note: prep entries do not get freed here because it is assumed there are
|
* no established connections.
|
*/
|
}
|
|
static void
|
ble_att_svr_free_start_mem(void)
|
{
|
free(ble_att_svr_entry_mem);
|
ble_att_svr_entry_mem = NULL;
|
}
|
|
int
|
ble_att_svr_start(void)
|
{
|
int rc;
|
|
ble_att_svr_free_start_mem();
|
|
if (ble_hs_max_attrs > 0) {
|
ble_att_svr_entry_mem = pvPortMalloc(
|
OS_MEMPOOL_BYTES(ble_hs_max_attrs,
|
sizeof (struct ble_att_svr_entry)));
|
if (ble_att_svr_entry_mem == NULL) {
|
rc = BLE_HS_ENOMEM;
|
goto err;
|
}
|
|
rc = os_mempool_init(&ble_att_svr_entry_pool, ble_hs_max_attrs,
|
sizeof (struct ble_att_svr_entry),
|
ble_att_svr_entry_mem, "ble_att_svr_entry_pool");
|
if (rc != 0) {
|
rc = BLE_HS_EOS;
|
goto err;
|
}
|
}
|
|
return 0;
|
|
err:
|
ble_att_svr_free_start_mem();
|
return rc;
|
}
|
|
int
|
ble_att_svr_init(void)
|
{
|
int rc;
|
|
#ifdef IP_101x
|
ble_att_svr_prep_entry_mem = panchip_mem_get((OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES), sizeof (struct ble_att_prep_entry))) * 4, 4);
|
#endif
|
|
if (MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES) > 0) {
|
rc = os_mempool_init(&ble_att_svr_prep_entry_pool,
|
MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
|
sizeof (struct ble_att_prep_entry),
|
ble_att_svr_prep_entry_mem,
|
"ble_att_svr_prep_entry_pool");
|
if (rc != 0) {
|
return BLE_HS_EOS;
|
}
|
}
|
|
STAILQ_INIT(&ble_att_svr_list);
|
STAILQ_INIT(&ble_att_svr_hidden_list);
|
|
ble_att_svr_id = 0;
|
|
return 0;
|
}
|
|
#endif
|
