/**
* Called when a data packet is received from the controller. This function
* consumes the supplied mbuf, regardless of the outcome.
*
* @param om The incoming data packet, beginning with the
* HCI ACL data header.
*
* @return 0 on success; nonzero on failure.
*/
int
host_hci_data_rx(struct os_mbuf *om)
{
struct hci_data_hdr hci_hdr;
struct ble_hs_conn *conn;
ble_l2cap_rx_fn *rx_cb;
struct os_mbuf *rx_buf;
uint16_t handle;
int rc;
rc = host_hci_data_hdr_strip(om, &hci_hdr);
if (rc == 0) {
BLE_HS_LOG(DEBUG, "host_hci_data_rx(): handle=%u pb=%x len=%u data=",
BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc),
BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc),
hci_hdr.hdh_len);
ble_hs_misc_log_mbuf(om);
if (hci_hdr.hdh_len != OS_MBUF_PKTHDR(om)->omp_len) {
rc = BLE_HS_EBADDATA;
} else {
handle = BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc);
ble_hs_conn_lock();
conn = ble_hs_conn_find(handle);
if (conn == NULL) {
rc = BLE_HS_ENOTCONN;
} else {
rc = ble_l2cap_rx(conn, &hci_hdr, om, &rx_cb, &rx_buf);
om = NULL;
}
ble_hs_conn_unlock();
}
}
os_mbuf_free_chain(om);
if (rc == 0) {
assert(rx_cb != NULL);
assert(rx_buf != NULL);
rc = rx_cb(handle, &rx_buf);
os_mbuf_free_chain(rx_buf);
} else if (rc == BLE_HS_EAGAIN) {
/* More fragments on the way. */
rc = 0;
}
return rc;
}
static int
ble_att_clt_build_indicate_req(uint16_t conn_handle,
struct ble_att_indicate_req *req,
void *value, uint16_t value_len,
struct os_mbuf **out_txom)
{
struct os_mbuf *txom;
int rc;
txom = NULL;
rc = ble_att_clt_init_req(BLE_ATT_INDICATE_REQ_BASE_SZ, &txom);
if (rc != 0) {
goto done;
}
ble_att_indicate_req_write(txom->om_data, txom->om_len, req);
rc = ble_att_clt_append_blob(conn_handle, txom, value, value_len);
if (rc != 0) {
goto done;
}
done:
if (rc != 0) {
os_mbuf_free_chain(txom);
txom = NULL;
}
*out_txom = txom;
return rc;
}
static int
ble_att_clt_build_read_group_type_req(struct ble_att_read_group_type_req *req,
void *uuid128, struct os_mbuf **out_txom)
{
struct os_mbuf *txom;
int rc;
txom = NULL;
rc = ble_att_clt_init_req(BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, &txom);
if (rc != 0) {
goto done;
}
ble_att_read_group_type_req_write(txom->om_data, txom->om_len, req);
rc = ble_uuid_append(txom, uuid128);
if (rc != 0) {
goto done;
}
done:
if (rc != 0) {
os_mbuf_free_chain(txom);
txom = NULL;
}
*out_txom = txom;
return rc;
}
static int
ble_att_clt_build_read_blob_req(struct ble_att_read_blob_req *req,
struct os_mbuf **out_txom)
{
struct os_mbuf *txom;
int rc;
txom = NULL;
rc = ble_att_clt_init_req(BLE_ATT_READ_BLOB_REQ_SZ, &txom);
if (rc != 0) {
goto done;
}
ble_att_read_blob_req_write(txom->om_data, txom->om_len, req);
done:
if (rc != 0) {
os_mbuf_free_chain(txom);
txom = NULL;
}
*out_txom = txom;
return rc;
}
static int
ble_sm_init_req(uint16_t initial_sz, struct os_mbuf **out_txom)
{
void *buf;
int rc;
*out_txom = ble_hs_mbuf_l2cap_pkt();
if (*out_txom == NULL) {
rc = BLE_HS_ENOMEM;
goto err;
}
buf = os_mbuf_extend(*out_txom, BLE_SM_HDR_SZ + initial_sz);
if (buf == NULL) {
rc = BLE_HS_ENOMEM;
goto err;
}
return BLE_HS_ENONE;
err:
os_mbuf_free_chain(*out_txom);
*out_txom = NULL;
return rc;
}
static int
ble_att_clt_init_req(uint16_t initial_sz, struct os_mbuf **out_txom)
{
void *buf;
int rc;
*out_txom = ble_hs_misc_pkthdr();
if (*out_txom == NULL) {
rc = BLE_HS_ENOMEM;
goto err;
}
buf = os_mbuf_extend(*out_txom, initial_sz);
if (buf == NULL) {
rc = BLE_HS_ENOMEM;
goto err;
}
/* The caller expects the initial buffer to be at the start of the mbuf. */
BLE_HS_DBG_ASSERT(buf == (*out_txom)->om_data);
return 0;
err:
os_mbuf_free_chain(*out_txom);
*out_txom = NULL;
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;
}
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;
}
/*
* 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;
}
/**
* 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;
}
/**
* ll rx pkt in
*
* Process received packet from PHY.
*
* Context: Link layer task
*
*/
static void
ble_ll_rx_pkt_in(void)
{
os_sr_t sr;
uint8_t pdu_type;
uint8_t *rxbuf;
struct os_mbuf_pkthdr *pkthdr;
struct ble_mbuf_hdr *ble_hdr;
struct os_mbuf *m;
/* Drain all packets off the queue */
while (STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q)) {
/* Get mbuf pointer from packet header pointer */
pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q);
m = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
/* Remove from queue */
OS_ENTER_CRITICAL(sr);
STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_rx_pkt_q, omp_next);
OS_EXIT_CRITICAL(sr);
/* Note: pdu type wont get used unless this is an advertising pdu */
ble_hdr = BLE_MBUF_HDR_PTR(m);
rxbuf = m->om_data;
pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
ble_ll_count_rx_stats(ble_hdr, pkthdr->omp_len, pdu_type);
/* Process the data or advertising pdu */
if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) {
ble_ll_conn_rx_data_pdu(m, ble_hdr);
} else {
/* Process the PDU */
switch (BLE_MBUF_HDR_RX_STATE(ble_hdr)) {
case BLE_LL_STATE_ADV:
ble_ll_adv_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
break;
case BLE_LL_STATE_SCANNING:
ble_ll_scan_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
break;
case BLE_LL_STATE_INITIATING:
ble_ll_init_rx_pkt_in(rxbuf, ble_hdr);
break;
default:
/* Any other state should never occur */
STATS_INC(ble_ll_stats, bad_ll_state);
break;
}
/* Free the packet buffer */
os_mbuf_free_chain(m);
}
}
}
/**
* Transfers an incoming request to the newtmgr task. The caller relinquishes
* ownership of the supplied mbuf upon calling this function, whether this
* function succeeds or fails.
*
* @param nt The transport that the request was received
* over.
* @param req An mbuf containing the newtmgr request.
*
* @return 0 on success; nonzero on failure.
*/
int
nmgr_rx_req(struct nmgr_transport *nt, struct os_mbuf *req)
{
int rc;
rc = os_mqueue_put(&nt->nt_imq, mgmt_evq_get(), req);
if (rc != 0) {
os_mbuf_free_chain(req);
}
return rc;
}
/**
* Reset the advertising state machine.
*
* Context: Link Layer task
*
*/
void
ble_ll_adv_reset(void)
{
struct ble_ll_adv_sm *advsm;
advsm = &g_ble_ll_adv_sm;
/* Stop advertising state machine */
ble_ll_adv_sm_stop(advsm);
/* Free scan pdu's */
os_mbuf_free_chain(advsm->scan_rsp_pdu);
/* re-initialize the advertiser state machine */
ble_ll_adv_init();
}
static int
nmgr_ble_out(struct nmgr_transport *nt, struct os_mbuf *om)
{
int rc;
rc = os_mqueue_put(&nmgr_ble_mq, mgmt_evq_get(), om);
if (rc != 0) {
goto err;
}
return (0);
err:
os_mbuf_free_chain(om);
return (rc);
}
static int
nmgr_shell_out(struct nmgr_transport *nt, struct os_mbuf *m)
{
int rc;
rc = shell_nlip_output(m);
if (rc != 0) {
goto err;
}
return (0);
err:
os_mbuf_free_chain(m);
return (rc);
}
/**
* Flush a link layer packet queue.
*
* @param pktq
*/
static void
ble_ll_flush_pkt_queue(struct ble_ll_pkt_q *pktq)
{
struct os_mbuf_pkthdr *pkthdr;
struct os_mbuf *om;
/* FLush all packets from Link layer queues */
while (STAILQ_FIRST(pktq)) {
/* Get mbuf pointer from packet header pointer */
pkthdr = STAILQ_FIRST(pktq);
om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
/* Remove from queue and free the mbuf */
STAILQ_REMOVE_HEAD(pktq, omp_next);
os_mbuf_free_chain(om);
}
}
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;
}
static int
ble_att_clt_tx_req_flags(uint16_t conn_handle, struct os_mbuf *txom,
ble_hs_conn_flags_t flags_on_success)
{
struct ble_l2cap_chan *chan;
struct ble_hs_conn *conn;
uint16_t total_len;
uint16_t mtu;
int extra_len;
int rc;
BLE_HS_DBG_ASSERT_EVAL(txom->om_len >= 1);
ble_att_inc_tx_stat(txom->om_data[0]);
ble_hs_lock();
rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
if (rc == 0) {
/* Reduce the size of the transmission to fit the connection's ATT
* MTU.
*/
total_len = OS_MBUF_PKTLEN(txom);
mtu = ble_l2cap_chan_mtu(chan);
extra_len = total_len - mtu;
if (extra_len > 0) {
os_mbuf_adj(txom, -extra_len);
}
rc = ble_l2cap_tx(conn, chan, txom);
txom = NULL;
if (rc == 0) {
conn->bhc_flags |= flags_on_success;
}
}
ble_hs_unlock();
os_mbuf_free_chain(txom);
return rc;
}
int
ble_hs_test_util_l2cap_rx(struct ble_hs_conn *conn,
struct hci_data_hdr *hci_hdr,
struct os_mbuf *om)
{
ble_l2cap_rx_fn *rx_cb;
struct os_mbuf *rx_buf;
int rc;
rc = ble_l2cap_rx(conn, hci_hdr, om, &rx_cb, &rx_buf);
if (rc == 0) {
assert(rx_cb != NULL);
assert(rx_buf != NULL);
rc = rx_cb(conn->bhc_handle, &rx_buf);
os_mbuf_free_chain(rx_buf);
} else if (rc == BLE_HS_EAGAIN) {
/* More fragments on the way. */
rc = 0;
}
return rc;
}
static int
ble_att_clt_build_read_mult_req(uint16_t *att_handles, int num_att_handles,
struct os_mbuf **out_txom)
{
struct os_mbuf *txom;
void *buf;
int rc;
int i;
txom = NULL;
rc = ble_att_clt_init_req(BLE_ATT_READ_MULT_REQ_BASE_SZ, &txom);
if (rc != 0) {
goto done;
}
ble_att_read_mult_req_write(txom->om_data, txom->om_len);
for (i = 0; i < num_att_handles; i++) {
buf = os_mbuf_extend(txom, 2);
if (buf == NULL) {
rc = BLE_HS_ENOMEM;
goto done;
}
htole16(buf, att_handles[i]);
}
rc = 0;
done:
if (rc != 0) {
os_mbuf_free_chain(txom);
txom = NULL;
}
*out_txom = txom;
return rc;
}
static int
ble_att_clt_build_find_type_value_req(struct ble_att_find_type_value_req *req,
void *attribute_value, int value_len,
struct os_mbuf **out_txom)
{
int rc;
rc = ble_att_clt_init_req(BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, out_txom);
if (rc != 0) {
return rc;
}
ble_att_find_type_value_req_write((*out_txom)->om_data,
(*out_txom)->om_len,
req);
rc = os_mbuf_append(*out_txom, attribute_value, value_len);
if (rc != 0) {
os_mbuf_free_chain(*out_txom);
return BLE_HS_ENOMEM;
}
return 0;
}
/**
* ll tx pkt in proc
*
* Process ACL data packet input from host
*
* Context: Link layer task
*
*/
static void
ble_ll_tx_pkt_in(void)
{
uint16_t handle;
uint16_t length;
uint16_t pb;
struct os_mbuf_pkthdr *pkthdr;
struct os_mbuf *om;
/* Drain all packets off the queue */
while (STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q)) {
/* Get mbuf pointer from packet header pointer */
pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q);
om = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
/* Remove from queue */
STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_tx_pkt_q, omp_next);
/* Strip HCI ACL header to get handle and length */
handle = le16toh(om->om_data);
length = le16toh(om->om_data + 2);
os_mbuf_adj(om, sizeof(struct hci_data_hdr));
/* Do some basic error checking */
pb = handle & 0x3000;
if ((pkthdr->omp_len != length) || (pb > 0x1000) || (length == 0)) {
/* This is a bad ACL packet. Count a stat and free it */
STATS_INC(ble_ll_stats, bad_acl_hdr);
os_mbuf_free_chain(om);
continue;
}
/* Hand to connection state machine */
ble_ll_conn_tx_pkt_in(om, handle, length);
}
}
/**
* Called when the LL receives a scan request or connection request
*
* Context: Called from interrupt context.
*
* @param rxbuf
*
* @return -1: request not for us or is a connect request.
* 0: request (scan) is for us and we successfully went from rx to tx.
* > 0: PHY error attempting to go from rx to tx.
*/
static int
ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu)
{
int rc;
int resolved;
uint8_t chk_wl;
uint8_t txadd;
uint8_t peer_addr_type;
uint8_t *rxbuf;
uint8_t *adva;
uint8_t *peer;
struct ble_mbuf_hdr *ble_hdr;
struct ble_ll_adv_sm *advsm;
struct os_mbuf *scan_rsp;
/* See if adva in the request (scan or connect) matches what we sent */
advsm = &g_ble_ll_adv_sm;
rxbuf = rxpdu->om_data;
adva = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN;
if (memcmp(advsm->adva, adva, BLE_DEV_ADDR_LEN)) {
return -1;
}
/* Set device match bit if we are whitelisting */
if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) {
chk_wl = advsm->adv_filter_policy & 1;
} else {
chk_wl = advsm->adv_filter_policy & 2;
}
/* Get the peer address type */
if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) {
txadd = BLE_ADDR_TYPE_RANDOM;
} else {
txadd = BLE_ADDR_TYPE_PUBLIC;
}
ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
peer = rxbuf + BLE_LL_PDU_HDR_LEN;
peer_addr_type = txadd;
resolved = 0;
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
if (ble_ll_is_rpa(peer, txadd) && ble_ll_resolv_enabled()) {
advsm->adv_rpa_index = ble_hw_resolv_list_match();
if (advsm->adv_rpa_index >= 0) {
ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED;
if (chk_wl) {
peer = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_identity_addr;
peer_addr_type = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_addr_type;
resolved = 1;
}
} else {
if (chk_wl) {
return -1;
}
}
}
#endif
/* Set device match bit if we are whitelisting */
if (chk_wl && !ble_ll_whitelist_match(peer, peer_addr_type, resolved)) {
return -1;
}
/*
* We set the device match bit to tell the upper layer that we will
* accept the request
*/
ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH;
/* Setup to transmit the scan response if appropriate */
rc = -1;
if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) {
scan_rsp = ble_ll_adv_scan_rsp_pdu_make(advsm);
if (scan_rsp) {
ble_phy_set_txend_cb(ble_ll_adv_tx_done, &g_ble_ll_adv_sm);
rc = ble_phy_tx(scan_rsp, BLE_PHY_TRANSITION_NONE);
if (!rc) {
ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD;
STATS_INC(ble_ll_stats, scan_rsp_txg);
}
os_mbuf_free_chain(scan_rsp);
}
}
return rc;
}
/**
* 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;
}
/*
* Check for full packet. If frame is not right, free the mbuf.
*/
static void
nmgr_uart_rx_pkt(struct nmgr_uart_state *nus, struct os_mbuf_pkthdr *rxm)
{
struct os_mbuf *m;
struct nmgr_ser_hdr *nsh;
uint16_t crc;
int rc;
m = OS_MBUF_PKTHDR_TO_MBUF(rxm);
if (rxm->omp_len <= sizeof(uint16_t) + sizeof(crc)) {
goto err;
}
nsh = (struct nmgr_ser_hdr *)m->om_data;
switch (nsh->nsh_seq) {
case htons(SHELL_NLIP_PKT):
if (nus->nus_rx_pkt) {
os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(nus->nus_rx_pkt));
nus->nus_rx_pkt = NULL;
}
break;
case htons(SHELL_NLIP_DATA):
if (!nus->nus_rx_pkt) {
goto err;
}
break;
default:
goto err;
}
if (os_mbuf_append(m, "\0", 1)) {
/*
* Null-terminate the line for base64_decode's sake.
*/
goto err;
}
m = os_mbuf_pullup(m, rxm->omp_len);
if (!m) {
/*
* Make data contiguous for base64_decode's sake.
*/
goto err;
}
rxm = OS_MBUF_PKTHDR(m);
rc = base64_decode((char *)m->om_data + 2, (char *)m->om_data + 2);
if (rc < 0) {
goto err;
}
rxm->omp_len = m->om_len = rc + 2;
if (nus->nus_rx_pkt) {
os_mbuf_adj(m, 2);
os_mbuf_concat(OS_MBUF_PKTHDR_TO_MBUF(nus->nus_rx_pkt), m);
} else {
nus->nus_rx_pkt = rxm;
}
m = OS_MBUF_PKTHDR_TO_MBUF(nus->nus_rx_pkt);
nsh = (struct nmgr_ser_hdr *)m->om_data;
if (nus->nus_rx_pkt->omp_len - sizeof(*nsh) == ntohs(nsh->nsh_len)) {
os_mbuf_adj(m, 4);
os_mbuf_adj(m, -2);
nmgr_rx_req(&nus->nus_transport, m);
nus->nus_rx_pkt = NULL;
}
return;
err:
os_mbuf_free_chain(m);
}
/*
* Called by mgmt to queue packet out to UART.
*/
static int
nmgr_uart_out(struct nmgr_transport *nt, struct os_mbuf *m)
{
struct nmgr_uart_state *nus = (struct nmgr_uart_state *)nt;
struct os_mbuf_pkthdr *mpkt;
struct os_mbuf *n;
char tmp_buf[12];
uint16_t crc;
char *dst;
int off;
int boff;
int slen;
int sr;
int rc;
int last;
int tx_sz;
assert(OS_MBUF_IS_PKTHDR(m));
mpkt = OS_MBUF_PKTHDR(m);
off = 0;
crc = CRC16_INITIAL_CRC;
for (n = m; n; n = SLIST_NEXT(n, om_next)) {
crc = crc16_ccitt(crc, n->om_data, n->om_len);
}
crc = htons(crc);
dst = os_mbuf_extend(m, sizeof(crc));
if (!dst) {
goto err;
}
memcpy(dst, &crc, sizeof(crc));
n = os_msys_get(SHELL_NLIP_MAX_FRAME, 0);
if (!n || OS_MBUF_TRAILINGSPACE(n) < 32) {
goto err;
}
while (off < mpkt->omp_len) {
tx_sz = 2;
dst = os_mbuf_extend(n, 2);
if (off == 0) {
*(uint16_t *)dst = htons(SHELL_NLIP_PKT);
*(uint16_t *)tmp_buf = htons(mpkt->omp_len);
boff = 2;
} else {
*(uint16_t *)dst = htons(SHELL_NLIP_DATA);
boff = 0;
}
while (off < mpkt->omp_len) {
slen = mpkt->omp_len - off;
last = 1;
if (slen > sizeof(tmp_buf) + boff) {
slen = sizeof(tmp_buf) - boff;
last = 0;
}
if (tx_sz + BASE64_ENCODE_SIZE(slen + boff) >= 124) {
break;
}
rc = os_mbuf_copydata(m, off, slen, tmp_buf + boff);
assert(rc == 0);
off += slen;
slen += boff;
dst = os_mbuf_extend(n, BASE64_ENCODE_SIZE(slen));
if (!dst) {
goto err;
}
tx_sz += base64_encode(tmp_buf, slen, dst, last);
boff = 0;
}
if (os_mbuf_append(n, "\n", 1)) {
goto err;
}
}
os_mbuf_free_chain(m);
OS_ENTER_CRITICAL(sr);
if (!nus->nus_tx) {
nus->nus_tx = n;
uart_start_tx(nus->nus_dev);
} else {
os_mbuf_concat(nus->nus_tx, n);
}
OS_EXIT_CRITICAL(sr);
return 0;
err:
os_mbuf_free_chain(m);
os_mbuf_free_chain(n);
return -1;
}
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;
}
/**
* Called when a data packet is received from the controller. This function
* consumes the supplied mbuf, regardless of the outcome.
*
* @param om The incoming data packet, beginning with the
* HCI ACL data header.
*
* @return 0 on success; nonzero on failure.
*/
int
ble_hs_hci_evt_acl_process(struct os_mbuf *om)
{
struct hci_data_hdr hci_hdr;
struct ble_hs_conn *conn;
ble_l2cap_rx_fn *rx_cb;
uint16_t conn_handle;
int reject_cid;
int rc;
rc = ble_hs_hci_util_data_hdr_strip(om, &hci_hdr);
if (rc != 0) {
goto err;
}
#if (BLETEST_THROUGHPUT_TEST == 0)
BLE_HS_LOG(DEBUG, "ble_hs_hci_evt_acl_process(): conn_handle=%u pb=%x "
"len=%u data=",
BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc),
BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc),
hci_hdr.hdh_len);
ble_hs_log_mbuf(om);
BLE_HS_LOG(DEBUG, "\n");
#endif
if (hci_hdr.hdh_len != OS_MBUF_PKTHDR(om)->omp_len) {
rc = BLE_HS_EBADDATA;
goto err;
}
conn_handle = BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc);
ble_hs_lock();
conn = ble_hs_conn_find(conn_handle);
if (conn == NULL) {
/* Peer not connected; quietly discard packet. */
rc = BLE_HS_ENOTCONN;
reject_cid = -1;
} else {
/* Forward ACL data to L2CAP. */
rc = ble_l2cap_rx(conn, &hci_hdr, om, &rx_cb, &reject_cid);
om = NULL;
}
ble_hs_unlock();
switch (rc) {
case 0:
/* Final fragment received. */
BLE_HS_DBG_ASSERT(rx_cb != NULL);
rc = rx_cb(conn->bhc_rx_chan);
ble_l2cap_forget_rx(conn, conn->bhc_rx_chan);
break;
case BLE_HS_EAGAIN:
/* More fragments on the way. */
break;
default:
if (reject_cid != -1) {
ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, 0, 0, reject_cid);
}
goto err;
}
return 0;
err:
os_mbuf_free_chain(om);
return rc;
}