int
ble_hs_test_util_l2cap_rx_payload_flat(struct ble_hs_conn *conn,
struct ble_l2cap_chan *chan,
const void *data, int len)
{
struct hci_data_hdr hci_hdr;
struct os_mbuf *om;
int rc;
om = ble_hs_misc_pkthdr();
TEST_ASSERT_FATAL(om != NULL);
om->om_data += BLE_L2CAP_HDR_SZ;
rc = os_mbuf_append(om, data, len);
TEST_ASSERT_FATAL(rc == 0);
om = ble_l2cap_prepend_hdr(om, chan->blc_cid, OS_MBUF_PKTLEN(om));
TEST_ASSERT_FATAL(om != NULL);
hci_hdr.hdh_handle_pb_bc =
host_hci_handle_pb_bc_join(conn->bhc_handle,
BLE_HCI_PB_FIRST_FLUSH, 0);
hci_hdr.hdh_len = OS_MBUF_PKTHDR(om)->omp_len;
rc = ble_hs_test_util_l2cap_rx(conn, &hci_hdr, om);
return rc;
}
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 to put a packet on the Link Layer receive packet queue.
*
* @param rxpdu Pointer to received PDU
*/
void
ble_ll_rx_pdu_in(struct os_mbuf *rxpdu)
{
struct os_mbuf_pkthdr *pkthdr;
pkthdr = OS_MBUF_PKTHDR(rxpdu);
STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_rx_pkt_q, pkthdr, omp_next);
os_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_rx_pkt_ev);
}
/**
* 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;
}
/**
* Called to put a packet on the Link Layer transmit packet queue.
*
* @param txpdu Pointer to transmit packet
*/
void
ble_ll_acl_data_in(struct os_mbuf *txpkt)
{
os_sr_t sr;
struct os_mbuf_pkthdr *pkthdr;
pkthdr = OS_MBUF_PKTHDR(txpkt);
OS_ENTER_CRITICAL(sr);
STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_tx_pkt_q, pkthdr, omp_next);
OS_EXIT_CRITICAL(sr);
os_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_tx_pkt_ev);
}
/**
* 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;
}
/**
* Called to initialize a mbuf used by the controller
*
* NOTE: this is only used when the mbuf is created by the controller;
* it should not be used for data packets (ACL data packets) that come from
* the host. This routine assumes that the entire pdu length can fit in
* one mbuf contiguously.
*
* @param m
* @param pdulen
* @param hdr
*/
void
ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr)
{
struct ble_mbuf_hdr *ble_hdr;
/* Set mbuf length and packet length */
m->om_len = pdulen;
OS_MBUF_PKTHDR(m)->omp_len = pdulen;
/* Set BLE transmit header */
ble_hdr = BLE_MBUF_HDR_PTR(m);
ble_hdr->txinfo.flags = 0;
ble_hdr->txinfo.offset = 0;
ble_hdr->txinfo.pyld_len = pdulen;
ble_hdr->txinfo.hdr_byte = hdr;
}
/*
* 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;
}
static struct os_mbuf *
bletest_send_packet(uint16_t handle)
{
int j;
uint8_t val;
struct os_mbuf *om;
uint16_t pktlen;
om = bletest_get_packet();
if (om) {
/* set payload length */
#if BLETEST_THROUGHPUT_TEST
pktlen = BLETEST_PKT_SIZE;
#else
#if (BLETEST_CFG_RAND_PKT_SIZE == 1)
pktlen = rand() % (BLETEST_MAX_PKT_SIZE + 1);
#else
pktlen = BLETEST_PKT_SIZE;
#endif
#endif
/* Put the HCI header in the mbuf */
put_le16(om->om_data, handle);
put_le16(om->om_data + 2, pktlen + 4);
/* Place L2CAP header in packet */
put_le16(om->om_data + 4, pktlen);
om->om_data[6] = 0;
om->om_data[7] = 0;
om->om_len = 8;
OS_MBUF_PKTHDR(om)->omp_len = 8;
/* Fill with incrementing pattern (starting from 1) */
for (j = 0; j < pktlen; ++j) {
val = j + 1;
os_mbuf_append(om, &val, 1);
}
/* Transmit it */
ble_hci_trans_hs_acl_tx(om);
}
return om;
}
static struct os_mbuf *
host_hci_data_hdr_prepend(struct os_mbuf *om, uint16_t handle, uint8_t pb_flag)
{
struct hci_data_hdr hci_hdr;
hci_hdr.hdh_handle_pb_bc = host_hci_handle_pb_bc_join(handle, pb_flag, 0);
htole16(&hci_hdr.hdh_len, OS_MBUF_PKTHDR(om)->omp_len);
om = os_mbuf_prepend(om, sizeof hci_hdr);
if (om == NULL) {
return NULL;
}
memcpy(om->om_data, &hci_hdr, sizeof hci_hdr);
BLE_HS_LOG(DEBUG, "host tx hci data; handle=%d length=%d\n", handle,
le16toh(&hci_hdr.hdh_len));
return om;
}
/**
* 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;
}
/**
* Copies the data from the phy receive buffer into a mbuf chain.
*
* @param dptr Pointer to receive buffer
* @param rxpdu Pointer to already allocated mbuf chain
*
* NOTE: the packet header already has the total mbuf length in it. The
* lengths of the individual mbufs are not set prior to calling.
*
*/
void
ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
{
uint16_t rem_bytes;
uint16_t mb_bytes;
uint16_t copylen;
uint32_t *dst;
uint32_t *src;
struct os_mbuf *m;
struct ble_mbuf_hdr *ble_hdr;
struct os_mbuf_pkthdr *pkthdr;
/* Better be aligned */
assert(((uint32_t)dptr & 3) == 0);
pkthdr = OS_MBUF_PKTHDR(rxpdu);
rem_bytes = pkthdr->omp_len;
/* Fill in the mbuf pkthdr first. */
dst = (uint32_t *)(rxpdu->om_data);
src = (uint32_t *)dptr;
mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
copylen = min(mb_bytes, rem_bytes);
copylen &= 0xFFFC;
rem_bytes -= copylen;
mb_bytes -= copylen;
rxpdu->om_len = copylen;
while (copylen > 0) {
*dst = *src;
++dst;
++src;
copylen -= 4;
}
/* Copy remaining bytes */
m = rxpdu;
while (rem_bytes > 0) {
/* If there are enough bytes in the mbuf, copy them and leave */
if (rem_bytes <= mb_bytes) {
memcpy(m->om_data + m->om_len, src, rem_bytes);
m->om_len += rem_bytes;
break;
}
m = SLIST_NEXT(m, om_next);
assert(m != NULL);
mb_bytes = m->om_omp->omp_databuf_len;
copylen = min(mb_bytes, rem_bytes);
copylen &= 0xFFFC;
rem_bytes -= copylen;
mb_bytes -= copylen;
m->om_len = copylen;
dst = (uint32_t *)m->om_data;
while (copylen > 0) {
*dst = *src;
++dst;
++src;
copylen -= 4;
}
}
/* Copy ble header */
ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
}
static void
bletest_execute_advertiser(void)
{
int i,j;
int rc;
uint16_t handle;
uint16_t pktlen;
struct os_mbuf *om;
/* See if we should start advertising again */
if (g_bletest_current_conns < BLETEST_CFG_CONCURRENT_CONNS) {
handle = g_bletest_current_conns + 1;
if (ble_ll_conn_find_active_conn(handle)) {
/* Set LED to slower blink rate */
g_bletest_led_rate = OS_TICKS_PER_SEC;
/* advertising better be stopped! */
assert(ble_ll_adv_enabled() == 0);
/* Send the remote used features command */
rc = host_hci_cmd_le_read_rem_used_feat(handle);
host_hci_outstanding_opcode = 0;
assert(rc == 0);
/* Send the remote used features command */
rc = host_hci_cmd_rd_rem_version(handle);
host_hci_outstanding_opcode = 0;
assert(rc == 0);
/* set conn update time */
g_bletest_conn_upd_time = os_time_get() + (OS_TICKS_PER_SEC * 5);
g_bletest_start_update = 1;
/* Add to current connections */
++g_bletest_current_conns;
/* Move to next connection */
if (g_bletest_current_conns < BLETEST_CFG_CONCURRENT_CONNS) {
/* restart initiating */
g_bletest_cur_peer_addr[5] += 1;
g_dev_addr[5] += 1;
bletest_init_advertising();
rc = host_hci_cmd_le_set_adv_enable(1);
host_hci_outstanding_opcode = 0;
assert(rc == 0);
}
}
}
#if 0
if (g_bletest_start_update) {
if ((int32_t)(os_time_get() - g_bletest_conn_upd_time) >= 0) {
bletest_send_conn_update(1);
g_bletest_start_update = 0;
}
}
#endif
/* See if it is time to hand a data packet to the connection */
if ((int32_t)(os_time_get() - g_next_os_time) >= 0) {
if (g_bletest_current_conns) {
for (i = 0; i < g_bletest_current_conns; ++i) {
if ((g_last_handle_used == 0) ||
(g_last_handle_used > g_bletest_current_conns)) {
g_last_handle_used = 1;
}
handle = g_last_handle_used;
if (ble_ll_conn_find_active_conn(handle)) {
om = bletest_get_packet();
if (om) {
/* set payload length */
pktlen = BLETEST_PKT_SIZE;
om->om_len = BLETEST_PKT_SIZE + 4;
/* Put the HCI header in the mbuf */
htole16(om->om_data, handle);
htole16(om->om_data + 2, om->om_len);
/* Place L2CAP header in packet */
htole16(om->om_data + 4, pktlen);
om->om_data[6] = 0;
om->om_data[7] = 0;
/* Fill with incrementing pattern (starting from 1) */
for (j = 0; j < pktlen; ++j) {
om->om_data[8 + j] = (uint8_t)(j + 1);
}
/* Add length */
om->om_len += 4;
OS_MBUF_PKTHDR(om)->omp_len = om->om_len;
ble_hci_transport_host_acl_data_send(om);
/* Increment last handle used */
++g_last_handle_used;
}
}
}
}
g_next_os_time += OS_TICKS_PER_SEC;
}
}
void
bletest_execute(void)
{
int rc;
int i;
uint16_t pktlen;
uint16_t handle;
struct os_mbuf *om;
struct ble_ll_conn_sm *connsm;
handle = 1;
if ((int32_t)(os_time_get() - g_next_os_time) >= 0) {
if (g_bletest_state == 0) {
rc = host_hci_cmd_le_set_adv_enable(1);
host_hci_outstanding_opcode = 0;
assert(rc == 0);
g_bletest_state = 1;
} else if (g_bletest_state == 1) {
/* See if handle 1 has been created. If so, send packets */
connsm = ble_ll_conn_find_active_conn(handle);
if (connsm) {
/* Set connection end time */
g_bletest_conn_end = os_time_get() +
(OS_TICKS_PER_SEC * (60 * 15));
g_bletest_state = 2;
}
} else if (g_bletest_state == 2) {
if ((int32_t)(os_time_get() - g_bletest_conn_end) >= 0) {
g_bletest_state = 3;
host_hci_cmd_disconnect(handle, BLE_ERR_REM_USER_CONN_TERM);
} else {
om = bletest_get_packet();
if (om) {
/* set payload length */
pktlen = BLETEST_PKT_SIZE;
om->om_len = BLETEST_PKT_SIZE + 4;
/* Put the HCI header in the mbuf */
htole16(om->om_data, handle);
htole16(om->om_data + 2, om->om_len);
/* Place L2CAP header in packet */
htole16(om->om_data + 4, pktlen);
om->om_data[6] = 0;
om->om_data[7] = 0;
/* Fill with incrementing pattern (starting from 1) */
for (i = 0; i < pktlen; ++i) {
om->om_data[8 + i] = (uint8_t)(i + 1);
}
/* Add length */
om->om_len += 4;
OS_MBUF_PKTHDR(om)->omp_len = om->om_len;
ble_hci_transport_host_acl_data_send(om);
}
g_next_os_time += OS_TICKS_PER_SEC / 10;
return;
}
} else if (g_bletest_state == 3) {
/* We should be waiting for disconnect */
connsm = ble_ll_conn_find_active_conn(handle);
if (!connsm) {
/* Set to 0 if you want to restart advertising */
//g_bletest_state = 0;
g_bletest_state = 4;
}
}
g_next_os_time += OS_TICKS_PER_SEC;
}
}
/*
* 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;
}
/*
* 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);
}
