static int
ble_gatts_read_test_util_access_2(uint16_t conn_handle,
uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt,
void *arg)
{
uint8_t *buf;
TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_2_def_handle + 1);
TEST_ASSERT(ctxt->chr ==
&ble_gatts_read_test_svcs[0].characteristics[1]);
buf = os_mbuf_extend(ctxt->om, 6);
TEST_ASSERT_FATAL(buf != NULL);
buf[0] = 0;
buf[1] = 10;
buf[2] = 20;
buf[3] = 30;
buf[4] = 40;
buf[5] = 50;
return 0;
}
int
ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len,
struct os_mbuf **out_om, void **out_payload_buf)
{
struct ble_l2cap_sig_hdr hdr;
struct os_mbuf *txom;
void *v;
*out_om = NULL;
*out_payload_buf = NULL;
txom = ble_hs_misc_pkthdr();
if (txom == NULL) {
return BLE_HS_ENOMEM;
}
v = os_mbuf_extend(txom, BLE_L2CAP_SIG_HDR_SZ + payload_len);
if (v == NULL) {
return BLE_HS_ENOMEM;
}
hdr.op = op;
hdr.identifier = id;
hdr.length = TOFROMLE16(payload_len);
ble_l2cap_sig_hdr_write(v, BLE_L2CAP_SIG_HDR_SZ, &hdr);
*out_om = txom;
*out_payload_buf = (uint8_t *)v + BLE_L2CAP_SIG_HDR_SZ;
return 0;
}
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_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
uint8_t *u8;
/* The only operation allowed for this characteristic is indicate. This
* access callback gets called by the stack when it needs to read the
* characteristic value to populate the outgoing indication command.
* Therefore, this callback should only get called during an attempt to
* read the characteristic.
*/
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
assert(ctxt->chr == &ble_svc_gatt_defs[0].characteristics[0]);
/* XXX: For now, always respond with 0 (unchanged). */
u8 = os_mbuf_extend(ctxt->om, 1);
if (u8 == NULL) {
return BLE_ATT_ERR_INSUFFICIENT_RES;
}
*u8 = 0;
return 0;
}
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
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;
}
int
ble_uuid_append(struct os_mbuf *om, void *uuid128)
{
uint16_t uuid16;
void *buf;
int rc;
uuid16 = ble_uuid_128_to_16(uuid128);
if (uuid16 != 0) {
buf = os_mbuf_extend(om, 2);
if (buf == NULL) {
return BLE_HS_ENOMEM;
}
htole16(buf, uuid16);
} else {
rc = os_mbuf_append(om, uuid128, 16);
if (rc != 0) {
return BLE_HS_ENOMEM;
}
}
return 0;
}
static struct nmgr_hdr *
nmgr_init_rsp(struct os_mbuf *m, struct nmgr_hdr *src)
{
struct nmgr_hdr *hdr;
hdr = (struct nmgr_hdr *) os_mbuf_extend(m, sizeof(struct nmgr_hdr));
if (!hdr) {
return NULL;
}
memcpy(hdr, src, sizeof(*hdr));
hdr->nh_len = 0;
hdr->nh_flags = 0;
hdr->nh_op = (src->nh_op == NMGR_OP_READ) ? NMGR_OP_READ_RSP :
NMGR_OP_WRITE_RSP;
hdr->nh_group = src->nh_group;
hdr->nh_seq = src->nh_seq;
hdr->nh_id = src->nh_id;
/* setup state for cbor encoding */
cbor_mbuf_writer_init(&nmgr_task_cbuf.writer, m);
cbor_encoder_init(&nmgr_task_cbuf.n_b.encoder, &nmgr_task_cbuf.writer.enc, 0);
nmgr_task_cbuf.n_out_m = m;
return hdr;
}
/*
* 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;
}
