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;
}
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 void
nmgr_ble_event_data_in(struct os_event *ev)
{
struct os_mbuf *m_resp;
uint16_t conn_handle;
while ((m_resp = os_mqueue_get(&nmgr_ble_mq)) != NULL) {
assert(OS_MBUF_USRHDR_LEN(m_resp) >= sizeof (conn_handle));
memcpy(&conn_handle, OS_MBUF_USRHDR(m_resp), sizeof (conn_handle));
ble_gattc_notify_custom(conn_handle, g_ble_nmgr_attr_handle,
m_resp);
}
}
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;
}
