int
nm_rx_rsp(uint8_t *attr_val, uint16_t attr_len)
{
struct os_mbuf *om;
int rc;
om = os_msys_get_pkthdr(attr_len, 0);
if (om == NULL) {
rc = 1;
goto err;
}
rc = os_mbuf_append(om, attr_val, attr_len);
if (rc != 0) {
goto err;
}
bleshell_printf("received nmgr rsp: ");
rc = shell_nlip_output(om);
if (rc != 0) {
goto err;
}
return 0;
err:
os_mbuf_free_chain(om);
return rc;
}
static int
nmgr_send_rspfrag(struct nmgr_transport *nt, struct os_mbuf *rsp,
struct os_mbuf *req, uint16_t len)
{
struct os_mbuf *rspfrag;
int rc;
rspfrag = NULL;
rspfrag = os_msys_get_pkthdr(len, OS_MBUF_USRHDR_LEN(req));
if (!rspfrag) {
rc = MGMT_ERR_ENOMEM;
goto err;
}
/* Copy the request packet header into the response. */
memcpy(OS_MBUF_USRHDR(rspfrag), OS_MBUF_USRHDR(req),
OS_MBUF_USRHDR_LEN(req));
if (os_mbuf_appendfrom(rspfrag, rsp, 0, len)) {
rc = MGMT_ERR_ENOMEM;
goto err;
}
nt->nt_output(nt, rspfrag);
return MGMT_ERR_EOK;
err:
if (rspfrag) {
os_mbuf_free_chain(rspfrag);
}
return rc;
}
/**
* Allocate a pdu (chain) for reception.
*
* @param len
*
* @return struct os_mbuf*
*/
struct os_mbuf *
ble_ll_rxpdu_alloc(uint16_t len)
{
uint16_t mb_bytes;
struct os_mbuf *m;
struct os_mbuf *n;
struct os_mbuf *p;
struct os_mbuf_pkthdr *pkthdr;
p = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
if (!p) {
goto rxpdu_alloc_exit;
}
/* Set packet length */
pkthdr = OS_MBUF_PKTHDR(p);
pkthdr->omp_len = len;
/*
* NOTE: first mbuf in chain will have data pre-pended to it so we adjust
* m_data by a word.
*/
p->om_data += 4;
mb_bytes = (p->om_omp->omp_databuf_len - p->om_pkthdr_len - 4);
if (mb_bytes < len) {
n = p;
len -= mb_bytes;
while (len) {
m = os_msys_get(len, 0);
if (!m) {
os_mbuf_free_chain(p);
p = NULL;
goto rxpdu_alloc_exit;
}
/* Chain new mbuf to existing chain */
SLIST_NEXT(n, om_next) = m;
n = m;
mb_bytes = m->om_omp->omp_databuf_len;
if (mb_bytes >= len) {
len = 0;
} else {
len -= mb_bytes;
}
}
}
rxpdu_alloc_exit:
if (!p) {
STATS_INC(ble_ll_stats, no_bufs);
}
return p;
}
/*
* Test wrapper to get packets. Only get a packet if we have more than half
* left
*/
static struct os_mbuf *
bletest_get_packet(void)
{
struct os_mbuf *om;
om = NULL;
if (os_msys_num_free() >= 5) {
om = os_msys_get_pkthdr(0, sizeof(struct ble_mbuf_hdr));
}
return om;
}
/**
* Create a scan response PDU
*
* @param advsm
*/
static struct os_mbuf *
ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm)
{
uint8_t scan_rsp_len;
uint8_t *dptr;
uint8_t pdulen;
uint8_t hdr;
struct os_mbuf *m;
/* Obtain scan response buffer */
m = os_msys_get_pkthdr(BLE_SCAN_RSP_DATA_MAX_LEN + BLE_DEV_ADDR_LEN,
sizeof(struct ble_mbuf_hdr));
if (!m) {
return NULL;
}
/* Make sure that the length is valid */
scan_rsp_len = advsm->scan_rsp_len;
assert(scan_rsp_len <= BLE_SCAN_RSP_DATA_MAX_LEN);
/* Set BLE transmit header */
pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len;
hdr = BLE_ADV_PDU_TYPE_SCAN_RSP;
if (advsm->adv_txadd) {
hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
}
ble_ll_mbuf_init(m, pdulen, hdr);
/*
* XXX: Am I sure this is correct? The adva in this packet will be the
* same one that was being advertised and is based on the peer identity
* address in the set advertising parameters. If a different peer sends
* us a scan request (for some reason) we will reply with an adva that
* was not generated based on the local irk of the peer sending the scan
* request.
*/
/* Construct scan response */
dptr = m->om_data;
memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
if (scan_rsp_len != 0) {
memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len);
}
return m;
}
/*
* Receive a character from UART.
*/
static int
nmgr_uart_rx_char(void *arg, uint8_t data)
{
struct nmgr_uart_state *nus = (struct nmgr_uart_state *)arg;
struct os_mbuf *m;
int rc;
if (!nus->nus_rx) {
m = os_msys_get_pkthdr(SHELL_NLIP_MAX_FRAME, 0);
if (!m) {
return 0;
}
nus->nus_rx = OS_MBUF_PKTHDR(m);
if (OS_MBUF_TRAILINGSPACE(m) < SHELL_NLIP_MAX_FRAME) {
/*
* mbuf is too small.
*/
os_mbuf_free_chain(m);
nus->nus_rx = NULL;
return 0;
}
}
m = OS_MBUF_PKTHDR_TO_MBUF(nus->nus_rx);
if (data == '\n') {
/*
* Full line of input. Process it outside interrupt context.
*/
assert(!nus->nus_rx_q);
nus->nus_rx_q = nus->nus_rx;
nus->nus_rx = NULL;
os_eventq_put(g_mgmt_evq, &nus->nus_cb_ev);
return 0;
} else {
rc = os_mbuf_append(m, &data, 1);
if (rc == 0) {
return 0;
}
}
/* failed */
nus->nus_rx->omp_len = 0;
m->om_len = 0;
os_mbuf_free_chain(SLIST_NEXT(m, om_next));
SLIST_NEXT(m, om_next) = NULL;
return 0;
}
int
nm_chr_access(uint16_t conn_handle, uint16_t attr_handle,
uint8_t op, union ble_gatt_access_ctxt *ctxt,
void *arg)
{
struct nmgr_transport *nt;
struct os_mbuf *om;
int rc;
assert(attr_handle == nm_attr_val_handle);
om = NULL;
if (op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
rc = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
goto err;
}
om = os_msys_get_pkthdr(ctxt->chr_access.len, 2);
if (om == NULL) {
rc = 1;
goto err;
}
/* Put the connection handle in the request packet header. */
memcpy(OS_MBUF_USRHDR(om), &conn_handle, sizeof conn_handle);
rc = os_mbuf_append(om, ctxt->chr_access.data, ctxt->chr_access.len);
if (rc != 0) {
goto err;
}
nt = arg;
rc = nmgr_rx_req(nt, om);
return rc;
err:
os_mbuf_free_chain(om);
return rc;
}
/**
* Initialize the advertising functionality of a BLE device. This should
* be called once on initialization
*/
void
ble_ll_adv_init(void)
{
struct ble_ll_adv_sm *advsm;
/* Set default advertising parameters */
advsm = &g_ble_ll_adv_sm;
memset(advsm, 0, sizeof(struct ble_ll_adv_sm));
advsm->adv_itvl_min = BLE_HCI_ADV_ITVL_DEF;
advsm->adv_itvl_max = BLE_HCI_ADV_ITVL_DEF;
advsm->adv_chanmask = BLE_HCI_ADV_CHANMASK_DEF;
/* Initialize advertising tx done event */
advsm->adv_txdone_ev.ev_type = BLE_LL_EVENT_ADV_EV_DONE;
advsm->adv_txdone_ev.ev_arg = advsm;
/* Get a scan response mbuf (packet header) and attach to state machine */
advsm->scan_rsp_pdu = os_msys_get_pkthdr(BLE_MBUF_PAYLOAD_SIZE,
sizeof(struct ble_mbuf_hdr));
assert(advsm->scan_rsp_pdu != NULL);
}
/**
* This is the scheduler callback (called from interrupt context) which
* transmits an advertisement.
*
* Context: Interrupt (scheduler)
*
* @param sch
*
* @return int
*/
static int
ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch)
{
int rc;
uint8_t end_trans;
uint32_t txstart;
struct ble_ll_adv_sm *advsm;
struct os_mbuf *adv_pdu;
/* Get the state machine for the event */
advsm = (struct ble_ll_adv_sm *)sch->cb_arg;
/* Set channel */
rc = ble_phy_setchan(advsm->adv_chan, 0, 0);
assert(rc == 0);
/* Set transmit start time. */
txstart = sch->start_time + XCVR_PROC_DELAY_USECS;
rc = ble_phy_tx_set_start_time(txstart);
if (rc) {
STATS_INC(ble_ll_stats, adv_late_starts);
goto adv_tx_done;
}
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
/* XXX: automatically do this in the phy based on channel? */
ble_phy_encrypt_disable();
#endif
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
advsm->adv_rpa_index = -1;
if (ble_ll_resolv_enabled()) {
ble_phy_resolv_list_enable();
} else {
ble_phy_resolv_list_disable();
}
#endif
/* Set phy mode based on type of advertisement */
if (advsm->adv_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
end_trans = BLE_PHY_TRANSITION_NONE;
ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
} else {
end_trans = BLE_PHY_TRANSITION_TX_RX;
ble_phy_set_txend_cb(NULL, NULL);
}
/* Get an advertising mbuf (packet header) */
adv_pdu = os_msys_get_pkthdr(BLE_ADV_MAX_PKT_LEN,
sizeof(struct ble_mbuf_hdr));
if (!adv_pdu) {
ble_phy_disable();
goto adv_tx_done;
}
ble_ll_adv_pdu_make(advsm, adv_pdu);
/* Transmit advertisement */
rc = ble_phy_tx(adv_pdu, end_trans);
os_mbuf_free_chain(adv_pdu);
if (rc) {
goto adv_tx_done;
}
/* Enable/disable whitelisting based on filter policy */
if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) {
ble_ll_whitelist_enable();
} else {
ble_ll_whitelist_disable();
}
/* Set link layer state to advertising */
ble_ll_state_set(BLE_LL_STATE_ADV);
/* Count # of adv. sent */
STATS_INC(ble_ll_stats, adv_txg);
return BLE_LL_SCHED_STATE_RUNNING;
adv_tx_done:
ble_ll_adv_tx_done(advsm);
return BLE_LL_SCHED_STATE_DONE;
}
static int
gatt_svr_chr_access_newtmgr(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
int rc;
struct os_mbuf *m_req;
switch (ctxt->op) {
case BLE_GATT_ACCESS_OP_WRITE_CHR:
/* Try to reuse the BLE packet mbuf as the newtmgr request. This
* requires a two-byte usrhdr to hold the BLE connection handle so
* that the newtmgr response can be sent to the correct peer. If
* it is not possible to reuse the mbuf, then allocate a new one
* and copy the request contents.
*/
if (OS_MBUF_USRHDR_LEN(ctxt->om) >= sizeof (conn_handle)) {
/* Sufficient usrhdr space already present. */
m_req = ctxt->om;
ctxt->om = NULL;
} else if (OS_MBUF_LEADINGSPACE(ctxt->om) >=
sizeof (conn_handle)) {
/* Usrhdr isn't present, but there is enough leading space to
* add one.
*/
m_req = ctxt->om;
ctxt->om = NULL;
m_req->om_pkthdr_len += sizeof (conn_handle);
} else {
/* The mbuf can't be reused. Allocate a new one and perform a
* copy. Don't set ctxt->om to NULL; let the NimBLE host free
* it.
*/
m_req = os_msys_get_pkthdr(OS_MBUF_PKTLEN(ctxt->om),
sizeof (conn_handle));
if (!m_req) {
return BLE_ATT_ERR_INSUFFICIENT_RES;
}
rc = os_mbuf_appendfrom(m_req, ctxt->om, 0,
OS_MBUF_PKTLEN(ctxt->om));
if (rc) {
return BLE_ATT_ERR_INSUFFICIENT_RES;
}
}
/* Write the connection handle to the newtmgr request usrhdr. This
* is necessary so that we later know who to send the newtmgr
* response to.
*/
memcpy(OS_MBUF_USRHDR(m_req), &conn_handle, sizeof(conn_handle));
rc = nmgr_rx_req(&ble_nt, m_req);
if (rc != 0) {
return BLE_ATT_ERR_UNLIKELY;
}
return 0;
default:
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
}
static void
nmgr_handle_req(struct nmgr_transport *nt, struct os_mbuf *req)
{
struct os_mbuf *rsp;
const struct mgmt_handler *handler;
struct nmgr_hdr *rsp_hdr;
struct nmgr_hdr hdr;
int off;
uint16_t len;
int rc;
rsp_hdr = NULL;
rsp = os_msys_get_pkthdr(512, OS_MBUF_USRHDR_LEN(req));
if (!rsp) {
rc = os_mbuf_copydata(req, 0, sizeof(hdr), &hdr);
if (rc < 0) {
goto err_norsp;
}
rsp = req;
req = NULL;
goto err;
}
/* Copy the request packet header into the response. */
memcpy(OS_MBUF_USRHDR(rsp), OS_MBUF_USRHDR(req), OS_MBUF_USRHDR_LEN(req));
off = 0;
len = OS_MBUF_PKTHDR(req)->omp_len;
while (off < len) {
rc = os_mbuf_copydata(req, off, sizeof(hdr), &hdr);
if (rc < 0) {
rc = MGMT_ERR_EINVAL;
goto err_norsp;
}
hdr.nh_len = ntohs(hdr.nh_len);
handler = mgmt_find_handler(ntohs(hdr.nh_group), hdr.nh_id);
if (!handler) {
rc = MGMT_ERR_ENOENT;
goto err;
}
/* Build response header apriori. Then pass to the handlers
* to fill out the response data, and adjust length & flags.
*/
rsp_hdr = nmgr_init_rsp(rsp, &hdr);
if (!rsp_hdr) {
rc = MGMT_ERR_ENOMEM;
goto err_norsp;
}
cbor_mbuf_reader_init(&nmgr_task_cbuf.reader, req, sizeof(hdr));
cbor_parser_init(&nmgr_task_cbuf.reader.r, 0,
&nmgr_task_cbuf.n_b.parser, &nmgr_task_cbuf.n_b.it);
if (hdr.nh_op == NMGR_OP_READ) {
if (handler->mh_read) {
rc = handler->mh_read(&nmgr_task_cbuf.n_b);
} else {
rc = MGMT_ERR_ENOENT;
}
} else if (hdr.nh_op == NMGR_OP_WRITE) {
if (handler->mh_write) {
rc = handler->mh_write(&nmgr_task_cbuf.n_b);
} else {
rc = MGMT_ERR_ENOENT;
}
} else {
rc = MGMT_ERR_EINVAL;
}
if (rc != 0) {
goto err;
}
rsp_hdr->nh_len +=
cbor_encode_bytes_written(&nmgr_task_cbuf.n_b.encoder);
off += sizeof(hdr) + OS_ALIGN(hdr.nh_len, 4);
rsp_hdr->nh_len = htons(rsp_hdr->nh_len);
rc = nmgr_rsp_fragment(nt, rsp_hdr, rsp, req);
if (rc) {
goto err;
}
}
os_mbuf_free_chain(rsp);
os_mbuf_free_chain(req);
return;
err:
OS_MBUF_PKTHDR(rsp)->omp_len = rsp->om_len = 0;
nmgr_send_err_rsp(nt, rsp, &hdr, rc);
os_mbuf_free_chain(req);
return;
err_norsp:
os_mbuf_free_chain(rsp);
os_mbuf_free_chain(req);
return;
}
