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 by UART driver to send out next character. * * Interrupts disabled when nmgr_uart_tx_char/nmgr_uart_rx_char are called. */ static int nmgr_uart_tx_char(void *arg) { struct nmgr_uart_state *nus = (struct nmgr_uart_state *)arg; struct os_mbuf *m; uint8_t ch; if (!nus->nus_tx) { /* * Out of data. Return -1 makes UART stop asking for more. */ return -1; } while (nus->nus_tx->om_len == nus->nus_tx_off) { /* * If no active mbuf, move to next one. */ m = SLIST_NEXT(nus->nus_tx, om_next); os_mbuf_free(nus->nus_tx); nus->nus_tx = m; nus->nus_tx_off = 0; if (!nus->nus_tx) { return -1; } } os_mbuf_copydata(nus->nus_tx, nus->nus_tx_off++, 1, &ch); return ch; }
static int ble_att_rx(uint16_t conn_handle, struct os_mbuf **om) { const struct ble_att_rx_dispatch_entry *entry; uint8_t op; int rc; rc = os_mbuf_copydata(*om, 0, 1, &op); if (rc != 0) { return BLE_HS_EMSGSIZE; } entry = ble_att_rx_dispatch_entry_find(op); if (entry == NULL) { return BLE_HS_EINVAL; } ble_att_inc_rx_stat(op); rc = entry->bde_fn(conn_handle, om); if (rc != 0) { return rc; } return 0; }
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 int ble_att_clt_parse_find_info_entry(struct os_mbuf **rxom, uint8_t rsp_format, struct ble_att_find_info_idata *idata) { uint16_t uuid16; int entry_len; int rc; switch (rsp_format) { case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT: entry_len = 2 + 2; break; case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT: entry_len = 2 + 16; break; default: return BLE_HS_EBADDATA; } rc = ble_hs_misc_pullup_base(rxom, entry_len); if (rc != 0) { return rc; } idata->attr_handle = le16toh((*rxom)->om_data); switch (rsp_format) { case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT: uuid16 = le16toh((*rxom)->om_data + 2); rc = ble_uuid_16_to_128(uuid16, idata->uuid128); if (rc != 0) { return BLE_HS_EBADDATA; } break; case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT: rc = os_mbuf_copydata(*rxom, 2, 16, idata->uuid128); if (rc != 0) { return BLE_HS_EBADDATA; } break; default: BLE_HS_DBG_ASSERT(0); break; } os_mbuf_adj(*rxom, entry_len); return 0; }
static int ble_att_clt_parse_find_type_value_hinfo( struct os_mbuf **om, struct ble_att_find_type_value_hinfo *dst) { int rc; rc = os_mbuf_copydata(*om, 0, BLE_ATT_FIND_TYPE_VALUE_HINFO_BASE_SZ, dst); if (rc != 0) { return BLE_HS_EBADDATA; } dst->attr_handle = TOFROMLE16(dst->attr_handle); dst->group_end_handle = TOFROMLE16(dst->group_end_handle); return 0; }
static int host_hci_data_hdr_strip(struct os_mbuf *om, struct hci_data_hdr *hdr) { int rc; rc = os_mbuf_copydata(om, 0, BLE_HCI_DATA_HDR_SZ, hdr); if (rc != 0) { return BLE_HS_EMSGSIZE; } /* Strip HCI ACL data header from the front of the packet. */ os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ); hdr->hdh_handle_pb_bc = le16toh(&hdr->hdh_handle_pb_bc); hdr->hdh_len = le16toh(&hdr->hdh_len); return 0; }
static int ble_att_clt_copy_attr_to_flatbuf(struct os_mbuf *om, void **out_attr_val, uint16_t *out_attr_len) { uint8_t *flat_buf; uint16_t attr_len; /* Make sure the attribute value isn't too big. */ attr_len = OS_MBUF_PKTLEN(om); if (attr_len > BLE_ATT_ATTR_MAX_LEN) { *out_attr_len = 0; *out_attr_val = NULL; return BLE_HS_EBADDATA; } /* Copy the attribute data into the global ATT flat buffer. */ flat_buf = ble_att_get_flat_buf(); os_mbuf_copydata(om, 0, attr_len, flat_buf); *out_attr_val = flat_buf; *out_attr_len = attr_len; return 0; }
int ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) { int rc; uint8_t *dptr; uint8_t *pktptr; uint8_t payload_len; uint32_t state; uint32_t shortcuts; struct ble_mbuf_hdr *ble_hdr; /* Better have a pdu! */ assert(txpdu != NULL); /* * This check is to make sure that the radio is not in a state where * it is moving to disabled state. If so, let it get there. */ nrf_wait_disabled(); ble_hdr = BLE_MBUF_HDR_PTR(txpdu); payload_len = ble_hdr->txinfo.pyld_len; #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1) if (g_ble_phy_data.phy_encrypted) { dptr = (uint8_t *)&g_ble_phy_enc_buf[0]; ++dptr; pktptr = (uint8_t *)&g_ble_phy_tx_buf[0]; NRF_CCM->SHORTS = 1; NRF_CCM->INPTR = (uint32_t)dptr; NRF_CCM->OUTPTR = (uint32_t)pktptr; NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0]; NRF_CCM->EVENTS_ERROR = 0; NRF_CCM->MODE = CCM_MODE_LENGTH_Msk; NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data; NRF_PPI->CHENCLR = PPI_CHEN_CH25_Msk | PPI_CHEN_CH23_Msk; NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk; } else { #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1) NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk; NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0]; #endif dptr = (uint8_t *)&g_ble_phy_tx_buf[0]; ++dptr; pktptr = dptr; } #else #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1) NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk; #endif dptr = (uint8_t *)&g_ble_phy_tx_buf[0]; ++dptr; pktptr = dptr; #endif /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */ dptr[0] = ble_hdr->txinfo.hdr_byte; dptr[1] = payload_len; dptr[2] = 0; dptr += 3; NRF_RADIO->PACKETPTR = (uint32_t)pktptr; /* Clear the ready, end and disabled events */ NRF_RADIO->EVENTS_READY = 0; NRF_RADIO->EVENTS_END = 0; NRF_RADIO->EVENTS_DISABLED = 0; /* Enable shortcuts for transmit start/end. */ shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk; if (end_trans == BLE_PHY_TRANSITION_TX_RX) { shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk; } NRF_RADIO->SHORTS = shortcuts; NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk; /* Set transmitted payload length */ g_ble_phy_data.phy_tx_pyld_len = payload_len; /* Set the PHY transition */ g_ble_phy_data.phy_transition = end_trans; /* If we already started transmitting, abort it! */ state = NRF_RADIO->STATE; if (state != RADIO_STATE_STATE_Tx) { /* Copy data from mbuf into transmit buffer */ os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, payload_len, dptr); /* Set phy state to transmitting and count packet statistics */ g_ble_phy_data.phy_state = BLE_PHY_STATE_TX; STATS_INC(ble_phy_stats, tx_good); STATS_INCN(ble_phy_stats, tx_bytes, payload_len + BLE_LL_PDU_HDR_LEN); rc = BLE_ERR_SUCCESS; } else { ble_phy_disable(); STATS_INC(ble_phy_stats, tx_late); rc = BLE_PHY_ERR_RADIO_STATE; } return rc; }
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; }
int ble_phy_tx(struct os_mbuf *txpdu, uint8_t beg_trans, uint8_t end_trans) { int rc; uint8_t *dptr; uint32_t state; uint32_t shortcuts; struct ble_mbuf_hdr *ble_hdr; /* Better have a pdu! */ assert(txpdu != NULL); /* If radio is not disabled, */ nrf52_wait_disabled(); if (beg_trans == BLE_PHY_TRANSITION_RX_TX) { if ((NRF_RADIO->SHORTS & RADIO_SHORTS_DISABLED_TXEN_Msk) == 0) { assert(0); } /* Radio better be in TXRU state or we are in bad shape */ state = RADIO_STATE_STATE_TxRu; } else { /* Radio should be in disabled state */ state = RADIO_STATE_STATE_Disabled; } if (NRF_RADIO->STATE != state) { ble_phy_disable(); ++g_ble_phy_stats.radio_state_errs; return BLE_PHY_ERR_RADIO_STATE; } /* Write LL header first */ ble_hdr = BLE_MBUF_HDR_PTR(txpdu); dptr = (uint8_t *)&g_ble_phy_txrx_buf[0]; dptr[0] = ble_hdr->txinfo.hdr_byte; dptr[1] = ble_hdr->txinfo.pyld_len; dptr += 2; /* Set radio transmit data pointer */ NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0]; /* Clear the ready, end and disabled events */ NRF_RADIO->EVENTS_READY = 0; NRF_RADIO->EVENTS_END = 0; NRF_RADIO->EVENTS_DISABLED = 0; /* Enable shortcuts for transmit start/end. */ shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk; if (end_trans == BLE_PHY_TRANSITION_TX_RX) { /* If we are going into receive after this, try to get a buffer. */ if (ble_phy_rxpdu_get()) { shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk; } } NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk; NRF_RADIO->SHORTS = shortcuts; /* Trigger transmit if our state was disabled */ if (state == RADIO_STATE_STATE_Disabled) { NRF_RADIO->TASKS_TXEN = 1; } /* Set the PHY transition */ g_ble_phy_data.phy_transition = end_trans; /* Read back radio state. If in TXRU, we are fine */ state = NRF_RADIO->STATE; if (state == RADIO_STATE_STATE_TxRu) { /* Copy data from mbuf into transmit buffer */ os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, ble_hdr->txinfo.pyld_len, dptr); /* Set phy state to transmitting and count packet statistics */ g_ble_phy_data.phy_state = BLE_PHY_STATE_TX; ++g_ble_phy_stats.tx_good; g_ble_phy_stats.tx_bytes += ble_hdr->txinfo.pyld_len + BLE_LL_PDU_HDR_LEN; rc = BLE_ERR_SUCCESS; } else { if (state == RADIO_STATE_STATE_Tx) { ++g_ble_phy_stats.tx_late; } else { ++g_ble_phy_stats.tx_fail; } /* Frame failed to transmit */ ble_phy_disable(); rc = BLE_PHY_ERR_RADIO_STATE; } return rc; }
/* * 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; }