int
ble_uuid_extract(struct os_mbuf *om, int off, void *uuid128)
{
uint16_t uuid16;
int remlen;
int rc;
remlen = OS_MBUF_PKTHDR(om)->omp_len - off;
switch (remlen) {
case 2:
rc = os_mbuf_copydata(om, off, 2, &uuid16);
BLE_HS_DBG_ASSERT_EVAL(rc == 0);
uuid16 = le16toh(&uuid16);
rc = ble_uuid_16_to_128(uuid16, uuid128);
if (rc != 0) {
return rc;
}
return 0;
case 16:
rc = os_mbuf_copydata(om, off, 16, uuid128);
BLE_HS_DBG_ASSERT_EVAL(rc == 0);
return 0;
default:
return BLE_HS_EMSGSIZE;
}
}
/*
* Called by UART driver to send out next character.
*
* Interrupts disabled when nmgr_uart_tx_char/nmgr_uart_rx_char are called.
*/
static int
nmgr_uart_tx_char(void *arg)
{
struct nmgr_uart_state *nus = (struct nmgr_uart_state *)arg;
struct os_mbuf *m;
uint8_t ch;
if (!nus->nus_tx) {
/*
* Out of data. Return -1 makes UART stop asking for more.
*/
return -1;
}
while (nus->nus_tx->om_len == nus->nus_tx_off) {
/*
* If no active mbuf, move to next one.
*/
m = SLIST_NEXT(nus->nus_tx, om_next);
os_mbuf_free(nus->nus_tx);
nus->nus_tx = m;
nus->nus_tx_off = 0;
if (!nus->nus_tx) {
return -1;
}
}
os_mbuf_copydata(nus->nus_tx, nus->nus_tx_off++, 1, &ch);
return ch;
}
static int
ble_att_rx(uint16_t conn_handle, struct os_mbuf **om)
{
const struct ble_att_rx_dispatch_entry *entry;
uint8_t op;
int rc;
rc = os_mbuf_copydata(*om, 0, 1, &op);
if (rc != 0) {
return BLE_HS_EMSGSIZE;
}
entry = ble_att_rx_dispatch_entry_find(op);
if (entry == NULL) {
return BLE_HS_EINVAL;
}
ble_att_inc_rx_stat(op);
rc = entry->bde_fn(conn_handle, om);
if (rc != 0) {
return rc;
}
return 0;
}
static int
nm_ble_out(struct nmgr_transport *nt, struct os_mbuf *om)
{
struct ble_gatt_attr attr;
uint16_t conn_handle;
int rc;
assert(OS_MBUF_USRHDR_LEN(om) == 2);
memcpy(&conn_handle, OS_MBUF_USRHDR(om), 2);
assert(OS_MBUF_PKTLEN(om) <= sizeof nm_buf);
rc = os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), nm_buf);
assert(rc == 0);
attr.handle = nm_attr_val_handle;
attr.offset = 0;
attr.value_len = OS_MBUF_PKTLEN(om);
attr.value = nm_buf;
rc = ble_gattc_notify_custom(conn_handle, &attr);
console_printf("nm_ble_out(); conn_handle = %d notify-rc=%d\n",
conn_handle, rc);
return rc;
}
static int
ble_att_clt_parse_find_info_entry(struct os_mbuf **rxom, uint8_t rsp_format,
struct ble_att_find_info_idata *idata)
{
uint16_t uuid16;
int entry_len;
int rc;
switch (rsp_format) {
case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT:
entry_len = 2 + 2;
break;
case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT:
entry_len = 2 + 16;
break;
default:
return BLE_HS_EBADDATA;
}
rc = ble_hs_misc_pullup_base(rxom, entry_len);
if (rc != 0) {
return rc;
}
idata->attr_handle = le16toh((*rxom)->om_data);
switch (rsp_format) {
case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT:
uuid16 = le16toh((*rxom)->om_data + 2);
rc = ble_uuid_16_to_128(uuid16, idata->uuid128);
if (rc != 0) {
return BLE_HS_EBADDATA;
}
break;
case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT:
rc = os_mbuf_copydata(*rxom, 2, 16, idata->uuid128);
if (rc != 0) {
return BLE_HS_EBADDATA;
}
break;
default:
BLE_HS_DBG_ASSERT(0);
break;
}
os_mbuf_adj(*rxom, entry_len);
return 0;
}
static int
ble_att_clt_parse_find_type_value_hinfo(
struct os_mbuf **om, struct ble_att_find_type_value_hinfo *dst)
{
int rc;
rc = os_mbuf_copydata(*om, 0, BLE_ATT_FIND_TYPE_VALUE_HINFO_BASE_SZ, dst);
if (rc != 0) {
return BLE_HS_EBADDATA;
}
dst->attr_handle = TOFROMLE16(dst->attr_handle);
dst->group_end_handle = TOFROMLE16(dst->group_end_handle);
return 0;
}
static int
host_hci_data_hdr_strip(struct os_mbuf *om, struct hci_data_hdr *hdr)
{
int rc;
rc = os_mbuf_copydata(om, 0, BLE_HCI_DATA_HDR_SZ, hdr);
if (rc != 0) {
return BLE_HS_EMSGSIZE;
}
/* Strip HCI ACL data header from the front of the packet. */
os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ);
hdr->hdh_handle_pb_bc = le16toh(&hdr->hdh_handle_pb_bc);
hdr->hdh_len = le16toh(&hdr->hdh_len);
return 0;
}
static int
ble_att_clt_copy_attr_to_flatbuf(struct os_mbuf *om, void **out_attr_val,
uint16_t *out_attr_len)
{
uint8_t *flat_buf;
uint16_t attr_len;
/* Make sure the attribute value isn't too big. */
attr_len = OS_MBUF_PKTLEN(om);
if (attr_len > BLE_ATT_ATTR_MAX_LEN) {
*out_attr_len = 0;
*out_attr_val = NULL;
return BLE_HS_EBADDATA;
}
/* Copy the attribute data into the global ATT flat buffer. */
flat_buf = ble_att_get_flat_buf();
os_mbuf_copydata(om, 0, attr_len, flat_buf);
*out_attr_val = flat_buf;
*out_attr_len = attr_len;
return 0;
}
int
ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
{
int rc;
uint8_t *dptr;
uint8_t *pktptr;
uint8_t payload_len;
uint32_t state;
uint32_t shortcuts;
struct ble_mbuf_hdr *ble_hdr;
/* Better have a pdu! */
assert(txpdu != NULL);
/*
* This check is to make sure that the radio is not in a state where
* it is moving to disabled state. If so, let it get there.
*/
nrf_wait_disabled();
ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
payload_len = ble_hdr->txinfo.pyld_len;
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
if (g_ble_phy_data.phy_encrypted) {
dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
++dptr;
pktptr = (uint8_t *)&g_ble_phy_tx_buf[0];
NRF_CCM->SHORTS = 1;
NRF_CCM->INPTR = (uint32_t)dptr;
NRF_CCM->OUTPTR = (uint32_t)pktptr;
NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
NRF_CCM->EVENTS_ERROR = 0;
NRF_CCM->MODE = CCM_MODE_LENGTH_Msk;
NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
NRF_PPI->CHENCLR = PPI_CHEN_CH25_Msk | PPI_CHEN_CH23_Msk;
NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk;
} else {
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk;
NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
#endif
dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
++dptr;
pktptr = dptr;
}
#else
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk;
#endif
dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
++dptr;
pktptr = dptr;
#endif
/* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
dptr[0] = ble_hdr->txinfo.hdr_byte;
dptr[1] = payload_len;
dptr[2] = 0;
dptr += 3;
NRF_RADIO->PACKETPTR = (uint32_t)pktptr;
/* Clear the ready, end and disabled events */
NRF_RADIO->EVENTS_READY = 0;
NRF_RADIO->EVENTS_END = 0;
NRF_RADIO->EVENTS_DISABLED = 0;
/* Enable shortcuts for transmit start/end. */
shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
}
NRF_RADIO->SHORTS = shortcuts;
NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
/* Set transmitted payload length */
g_ble_phy_data.phy_tx_pyld_len = payload_len;
/* Set the PHY transition */
g_ble_phy_data.phy_transition = end_trans;
/* If we already started transmitting, abort it! */
state = NRF_RADIO->STATE;
if (state != RADIO_STATE_STATE_Tx) {
/* Copy data from mbuf into transmit buffer */
os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, payload_len, dptr);
/* Set phy state to transmitting and count packet statistics */
g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
STATS_INC(ble_phy_stats, tx_good);
STATS_INCN(ble_phy_stats, tx_bytes, payload_len + BLE_LL_PDU_HDR_LEN);
rc = BLE_ERR_SUCCESS;
} else {
ble_phy_disable();
STATS_INC(ble_phy_stats, tx_late);
rc = BLE_PHY_ERR_RADIO_STATE;
}
return rc;
}
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;
}
int
ble_phy_tx(struct os_mbuf *txpdu, uint8_t beg_trans, uint8_t end_trans)
{
int rc;
uint8_t *dptr;
uint32_t state;
uint32_t shortcuts;
struct ble_mbuf_hdr *ble_hdr;
/* Better have a pdu! */
assert(txpdu != NULL);
/* If radio is not disabled, */
nrf52_wait_disabled();
if (beg_trans == BLE_PHY_TRANSITION_RX_TX) {
if ((NRF_RADIO->SHORTS & RADIO_SHORTS_DISABLED_TXEN_Msk) == 0) {
assert(0);
}
/* Radio better be in TXRU state or we are in bad shape */
state = RADIO_STATE_STATE_TxRu;
} else {
/* Radio should be in disabled state */
state = RADIO_STATE_STATE_Disabled;
}
if (NRF_RADIO->STATE != state) {
ble_phy_disable();
++g_ble_phy_stats.radio_state_errs;
return BLE_PHY_ERR_RADIO_STATE;
}
/* Write LL header first */
ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
dptr[0] = ble_hdr->txinfo.hdr_byte;
dptr[1] = ble_hdr->txinfo.pyld_len;
dptr += 2;
/* Set radio transmit data pointer */
NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
/* Clear the ready, end and disabled events */
NRF_RADIO->EVENTS_READY = 0;
NRF_RADIO->EVENTS_END = 0;
NRF_RADIO->EVENTS_DISABLED = 0;
/* Enable shortcuts for transmit start/end. */
shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
/* If we are going into receive after this, try to get a buffer. */
if (ble_phy_rxpdu_get()) {
shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
}
}
NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
NRF_RADIO->SHORTS = shortcuts;
/* Trigger transmit if our state was disabled */
if (state == RADIO_STATE_STATE_Disabled) {
NRF_RADIO->TASKS_TXEN = 1;
}
/* Set the PHY transition */
g_ble_phy_data.phy_transition = end_trans;
/* Read back radio state. If in TXRU, we are fine */
state = NRF_RADIO->STATE;
if (state == RADIO_STATE_STATE_TxRu) {
/* Copy data from mbuf into transmit buffer */
os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset,
ble_hdr->txinfo.pyld_len, dptr);
/* Set phy state to transmitting and count packet statistics */
g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
++g_ble_phy_stats.tx_good;
g_ble_phy_stats.tx_bytes += ble_hdr->txinfo.pyld_len +
BLE_LL_PDU_HDR_LEN;
rc = BLE_ERR_SUCCESS;
} else {
if (state == RADIO_STATE_STATE_Tx) {
++g_ble_phy_stats.tx_late;
} else {
++g_ble_phy_stats.tx_fail;
}
/* Frame failed to transmit */
ble_phy_disable();
rc = BLE_PHY_ERR_RADIO_STATE;
}
return rc;
}
/*
* 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;
}
