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; }