static int
gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt,
void *arg)
{
const void *uuid128;
int rand_num;
int rc;
uuid128 = ctxt->chr->uuid128;
/* Determine which characteristic is being accessed by examining its
* 128-bit UUID.
*/
if (memcmp(uuid128, gatt_svr_chr_sec_test_rand_uuid, 16) == 0) {
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
/* Respond with a 32-bit random number. */
rand_num = rand();
rc = os_mbuf_append(ctxt->om, &rand_num, sizeof rand_num);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}
if (memcmp(uuid128, gatt_svr_chr_sec_test_static_uuid, 16) == 0) {
switch (ctxt->op) {
case BLE_GATT_ACCESS_OP_READ_CHR:
rc = os_mbuf_append(ctxt->om, &gatt_svr_sec_test_static_val,
sizeof gatt_svr_sec_test_static_val);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case BLE_GATT_ACCESS_OP_WRITE_CHR:
rc = gatt_svr_chr_write(ctxt->om,
sizeof gatt_svr_sec_test_static_val,
sizeof gatt_svr_sec_test_static_val,
&gatt_svr_sec_test_static_val, NULL);
return rc;
default:
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
}
/* Unknown characteristic; the nimble stack should not have called this
* function.
*/
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
static int
ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
uint16_t uuid16;
int rc;
uuid16 = ble_uuid_128_to_16(ctxt->chr->uuid128);
assert(uuid16 != 0);
switch (uuid16) {
case BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, ble_svc_gap_name,
strlen(ble_svc_gap_name));
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case BLE_SVC_GAP_CHR_UUID16_APPEARANCE:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, &ble_svc_gap_appearance,
sizeof ble_svc_gap_appearance);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case BLE_SVC_GAP_CHR_UUID16_PERIPH_PRIV_FLAG:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, &ble_svc_gap_privacy_flag,
sizeof ble_svc_gap_privacy_flag);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case BLE_SVC_GAP_CHR_UUID16_RECONNECT_ADDR:
assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR);
if (OS_MBUF_PKTLEN(ctxt->om) != sizeof ble_svc_gap_reconnect_addr) {
return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
}
ble_hs_mbuf_to_flat(ctxt->om, ble_svc_gap_reconnect_addr,
sizeof ble_svc_gap_reconnect_addr, NULL);
return 0;
case BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, &ble_svc_gap_pref_conn_params,
sizeof ble_svc_gap_pref_conn_params);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
default:
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
}
int
nm_rx_rsp(uint8_t *attr_val, uint16_t attr_len)
{
struct os_mbuf *om;
int rc;
om = os_msys_get_pkthdr(attr_len, 0);
if (om == NULL) {
rc = 1;
goto err;
}
rc = os_mbuf_append(om, attr_val, attr_len);
if (rc != 0) {
goto err;
}
bleshell_printf("received nmgr rsp: ");
rc = shell_nlip_output(om);
if (rc != 0) {
goto err;
}
return 0;
err:
os_mbuf_free_chain(om);
return rc;
}
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;
}
/**
* Simple read access callback for the tx power level
* characteristic.
*/
static int
ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
int rc;
assert(ctxt->chr == &ble_svc_tps_defs[0].characteristics[0]);
switch (ctxt->op) {
case BLE_GATT_ACCESS_OP_READ_CHR:
rc = ble_hs_hci_util_read_adv_tx_pwr(&ble_svc_tps_tx_power_level);
if (rc != 0) {
return BLE_ATT_ERR_UNLIKELY;
}
rc = os_mbuf_append(ctxt->om, &ble_svc_tps_tx_power_level,
sizeof ble_svc_tps_tx_power_level);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
default:
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
return 0;
}
/*
* 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 int
ble_att_clt_append_blob(uint16_t conn_handle, struct os_mbuf *txom,
void *blob, int blob_len)
{
int rc;
if (blob_len < 0) {
return BLE_HS_EINVAL;
}
if (blob_len == 0) {
return 0;
}
rc = os_mbuf_append(txom, blob, blob_len);
if (rc != 0) {
return rc;
}
return 0;
}
static int gatt_svr_chr_access_device_info_manufacturer(
uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
puts("service 'device info: manufacturer' callback triggered");
(void) conn_handle;
(void) attr_handle;
(void) arg;
snprintf(str_answer, STR_ANSWER_BUFFER_SIZE,
"This is RIOT! (Version: %s)\n", RIOT_VERSION);
puts(str_answer);
int rc = os_mbuf_append(ctxt->om, str_answer, strlen(str_answer));
puts("");
return rc;
}
static int
ble_gatts_read_test_util_access_1(uint16_t conn_handle,
uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt,
void *arg)
{
int rc;
TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_1_val_handle);
TEST_ASSERT(ctxt->chr ==
&ble_gatts_read_test_svcs[0].characteristics[0]);
rc = os_mbuf_append(ctxt->om, ble_gatts_read_test_chr_1_val,
ble_gatts_read_test_chr_1_len);
TEST_ASSERT(rc == 0);
return 0;
}
int
nm_chr_access(uint16_t conn_handle, uint16_t attr_handle,
uint8_t op, union ble_gatt_access_ctxt *ctxt,
void *arg)
{
struct nmgr_transport *nt;
struct os_mbuf *om;
int rc;
assert(attr_handle == nm_attr_val_handle);
om = NULL;
if (op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
rc = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
goto err;
}
om = os_msys_get_pkthdr(ctxt->chr_access.len, 2);
if (om == NULL) {
rc = 1;
goto err;
}
/* Put the connection handle in the request packet header. */
memcpy(OS_MBUF_USRHDR(om), &conn_handle, sizeof conn_handle);
rc = os_mbuf_append(om, ctxt->chr_access.data, ctxt->chr_access.len);
if (rc != 0) {
goto err;
}
nt = arg;
rc = nmgr_rx_req(nt, om);
return rc;
err:
os_mbuf_free_chain(om);
return rc;
}
static int gatt_svr_chr_access_device_info_model(
uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
puts("service 'device info: model' callback triggered");
(void) conn_handle;
(void) attr_handle;
(void) arg;
snprintf(str_answer, STR_ANSWER_BUFFER_SIZE,
"You are running RIOT on a(n) %s board, "
"which features a(n) %s MCU.", RIOT_BOARD, RIOT_MCU);
puts(str_answer);
int rc = os_mbuf_append(ctxt->om, str_answer, strlen(str_answer));
puts("");
return rc;
}
static int
ble_att_clt_build_find_type_value_req(struct ble_att_find_type_value_req *req,
void *attribute_value, int value_len,
struct os_mbuf **out_txom)
{
int rc;
rc = ble_att_clt_init_req(BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, out_txom);
if (rc != 0) {
return rc;
}
ble_att_find_type_value_req_write((*out_txom)->om_data,
(*out_txom)->om_len,
req);
rc = os_mbuf_append(*out_txom, attribute_value, value_len);
if (rc != 0) {
os_mbuf_free_chain(*out_txom);
return BLE_HS_ENOMEM;
}
return 0;
}
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 int gatt_svr_chr_access_rw_demo(
uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt, void *arg)
{
puts("service 'rw demo' callback triggered");
(void) conn_handle;
(void) attr_handle;
(void) arg;
int rc = 0;
ble_uuid_t* write_uuid = (ble_uuid_t*) &gatt_svr_chr_rw_demo_write_uuid.u;
ble_uuid_t* readonly_uuid = (ble_uuid_t*) &gatt_svr_chr_rw_demo_readonly_uuid.u;
if (ble_uuid_cmp(ctxt->chr->uuid, write_uuid) == 0) {
puts("access to characteristic 'rw demo (write)'");
switch (ctxt->op) {
case BLE_GATT_ACCESS_OP_READ_CHR:
puts("read from characteristic");
printf("current value of rm_demo_write_data: '%s'\n",
rm_demo_write_data);
/* send given data to the client */
rc = os_mbuf_append(ctxt->om, &rm_demo_write_data,
strlen(rm_demo_write_data));
break;
case BLE_GATT_ACCESS_OP_WRITE_CHR:
puts("write to characteristic");
printf("old value of rm_demo_write_data: '%s'\n",
rm_demo_write_data);
uint16_t om_len;
om_len = OS_MBUF_PKTLEN(ctxt->om);
/* read sent data */
rc = ble_hs_mbuf_to_flat(ctxt->om, &rm_demo_write_data,
sizeof rm_demo_write_data, &om_len);
/* we need to null-terminate the received string */
rm_demo_write_data[om_len] = '\0';
printf("new value of rm_demo_write_data: '%s'\n",
rm_demo_write_data);
break;
case BLE_GATT_ACCESS_OP_READ_DSC:
puts("read from descriptor");
break;
case BLE_GATT_ACCESS_OP_WRITE_DSC:
puts("write to descriptor");
break;
default:
puts("unhandled operation!");
rc = 1;
break;
}
puts("");
return rc;
}
else if (ble_uuid_cmp(ctxt->chr->uuid, readonly_uuid) == 0) {
puts("access to characteristic 'rw demo (read-only)'");
if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
char random_digit;
/* get random char between '0' and '9' */
random_digit = 48 + (rand() % 10);
snprintf(str_answer, STR_ANSWER_BUFFER_SIZE,
"new random number: %c", random_digit);
puts(str_answer);
rc = os_mbuf_append(ctxt->om, &str_answer, strlen(str_answer));
puts("");
return rc;
}
return 0;
}
puts("unhandled uuid!");
return 1;
}
static int
gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
struct ble_gatt_access_ctxt *ctxt,
void *arg)
{
uint16_t uuid16;
int rc;
uuid16 = ble_uuid_128_to_16(ctxt->chr->uuid128);
assert(uuid16 != 0);
switch (uuid16) {
case GATT_SVR_CHR_SUP_NEW_ALERT_CAT_UUID:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_cat,
sizeof gatt_svr_new_alert_cat);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case GATT_SVR_CHR_NEW_ALERT:
if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
rc = gatt_svr_chr_write(ctxt->om, 0,
sizeof gatt_svr_new_alert_val,
gatt_svr_new_alert_val,
&gatt_svr_new_alert_val_len);
return rc;
} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_val,
sizeof gatt_svr_new_alert_val);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}
case GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID:
assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_cat,
sizeof gatt_svr_unr_alert_cat);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
case GATT_SVR_CHR_UNR_ALERT_STAT_UUID:
if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
rc = gatt_svr_chr_write(ctxt->om, 2, 2, &gatt_svr_unr_alert_stat,
NULL);
return rc;
} else {
rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_stat,
sizeof gatt_svr_unr_alert_stat);
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}
case GATT_SVR_CHR_ALERT_NOT_CTRL_PT:
if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
rc = gatt_svr_chr_write(ctxt->om, 2, 2,
&gatt_svr_alert_not_ctrl_pt, NULL);
} else {
rc = BLE_ATT_ERR_UNLIKELY;
}
return rc;
default:
assert(0);
return BLE_ATT_ERR_UNLIKELY;
}
}
/*
* 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);
}
/*
* 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;
}
