Parse data field of incoming advertisement report.
Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/repo Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/commit/2b2118b3 Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/tree/2b2118b3 Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/diff/2b2118b3 Branch: refs/heads/master Commit: 2b2118b37ebe732df984d88c530d5f8e3ddb01a1 Parents: 1ce6fd0 Author: Christopher Collins <[email protected]> Authored: Mon Dec 21 17:50:37 2015 -0800 Committer: Christopher Collins <[email protected]> Committed: Mon Dec 21 17:51:29 2015 -0800 ---------------------------------------------------------------------- net/nimble/host/include/host/ble_gap.h | 4 +- net/nimble/host/include/host/ble_hs.h | 4 +- net/nimble/host/src/ble_gap_conn.c | 35 ++++++++++++----- net/nimble/host/src/ble_gap_conn.h | 3 +- net/nimble/host/src/ble_hs_adv.c | 61 +++++++++++++++++++++++++++++ net/nimble/host/src/ble_hs_adv.h | 10 +++++ net/nimble/host/src/host_hci.c | 19 ++++----- project/hostctlrtest/src/main.c | 58 ++++++++++++++++++++------- 8 files changed, 159 insertions(+), 35 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/include/host/ble_gap.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/include/host/ble_gap.h b/net/nimble/host/include/host/ble_gap.h index 48fb278..d81f1d1 100644 --- a/net/nimble/host/include/host/ble_gap.h +++ b/net/nimble/host/include/host/ble_gap.h @@ -18,8 +18,8 @@ #define H_BLE_GAP_ #include <inttypes.h> +#include "host/ble_hs.h" struct hci_le_conn_complete; -struct ble_hs_adv_fields; #define BLE_GAP_CONN_EVENT_TYPE_CONNECT 1 #define BLE_GAP_CONN_EVENT_TYPE_ADV_RPT 2 @@ -39,6 +39,8 @@ struct ble_gap_conn_adv_rpt { int8_t rssi; uint8_t addr[6]; uint8_t *data; + + struct ble_hs_adv_fields fields; }; struct ble_gap_conn_terminate_rpt { http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/include/host/ble_hs.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/include/host/ble_hs.h b/net/nimble/host/include/host/ble_hs.h index 32c34fd..6081e4a 100644 --- a/net/nimble/host/include/host/ble_hs.h +++ b/net/nimble/host/include/host/ble_hs.h @@ -39,7 +39,9 @@ struct ble_hs_cfg { extern struct ble_hs_cfg ble_hs_cfg; struct ble_hs_adv_fields { - char *name; + uint8_t flags; + + uint8_t *name; uint8_t name_len; unsigned name_is_complete:1; }; http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/src/ble_gap_conn.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_gap_conn.c b/net/nimble/host/src/ble_gap_conn.c index ec6fa39..cb64e1f 100644 --- a/net/nimble/host/src/ble_gap_conn.c +++ b/net/nimble/host/src/ble_gap_conn.c @@ -368,20 +368,37 @@ ble_gap_conn_accept_slave_conn(uint8_t addr_type, uint8_t *addr) } void -ble_gap_conn_rx_adv_report(struct ble_gap_conn_adv_rpt *rpt) +ble_gap_conn_rx_adv_report(struct ble_hs_adv *adv) { struct ble_gap_conn_event event; + int rc; - switch (ble_gap_conn_master_state) { - case BLE_GAP_CONN_M_STATE_DISC_ACKED: - event.type = BLE_GAP_CONN_EVENT_TYPE_ADV_RPT; - event.adv = *rpt; - ble_gap_conn_call_cb(&event); - break; + if (ble_gap_conn_master_state != BLE_GAP_CONN_M_STATE_DISC_ACKED) { + return; + } - default: - break; + rc = ble_hs_adv_parse_fields(&event.adv.fields, adv->data, + adv->length_data); + if (rc != 0) { + /* XXX: Increment stat. */ + return; } + + if (ble_gap_conn_m_disc_mode == BLE_GAP_DISC_MODE_LTD && + !(event.adv.fields.flags & BLE_HS_ADV_F_DISC_LTD)) { + + return; + } + + event.type = BLE_GAP_CONN_EVENT_TYPE_ADV_RPT; + event.adv.event_type = adv->event_type; + event.adv.addr_type = adv->addr_type; + event.adv.length_data = adv->length_data; + event.adv.rssi = adv->rssi; + memcpy(event.adv.addr, adv->addr, sizeof event.adv.addr); + event.adv.data = adv->data; + + ble_gap_conn_call_cb(&event); } /** http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/src/ble_gap_conn.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_gap_conn.h b/net/nimble/host/src/ble_gap_conn.h index a9ae5b8..cb1b52b 100644 --- a/net/nimble/host/src/ble_gap_conn.h +++ b/net/nimble/host/src/ble_gap_conn.h @@ -22,8 +22,9 @@ struct hci_le_conn_complete; struct hci_disconn_complete; struct ble_hci_ack; +struct ble_hs_adv; -void ble_gap_conn_rx_adv_report(struct ble_gap_conn_adv_rpt *rpt); +void ble_gap_conn_rx_adv_report(struct ble_hs_adv *adv); int ble_gap_conn_rx_conn_complete(struct hci_le_conn_complete *evt); void ble_gap_conn_rx_disconn_complete(struct hci_disconn_complete *evt); int ble_gap_conn_master_in_progress(void); http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/src/ble_hs_adv.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_hs_adv.c b/net/nimble/host/src/ble_hs_adv.c index a3366ab..d25d354 100644 --- a/net/nimble/host/src/ble_hs_adv.c +++ b/net/nimble/host/src/ble_hs_adv.c @@ -71,11 +71,72 @@ ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields, return 0; } +static int +ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, + uint8_t *total_len, uint8_t *src, uint8_t src_len) +{ + uint8_t data_len; + uint8_t type; + uint8_t *data; + + if (src_len < 1) { + return BLE_HS_EMSGSIZE; + } + *total_len = src[0] + 1; + + if (src_len < *total_len) { + return BLE_HS_EMSGSIZE; + } + + type = src[1]; + data = src + 2; + data_len = *total_len - 2; + + switch (type) { + case BLE_HS_ADV_TYPE_FLAGS: + if (data_len != BLE_HS_ADV_FLAGS_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->flags = *data; + break; + + case BLE_HS_ADV_TYPE_INCOMP_NAME: + adv_fields->name = data; + adv_fields->name_len = data_len; + adv_fields->name_is_complete = 0; + break; + + case BLE_HS_ADV_TYPE_COMP_NAME: + adv_fields->name = data; + adv_fields->name_len = data_len; + adv_fields->name_is_complete = 1; + break; + + default: + break; + } + + return 0; +} + int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src, uint8_t src_len) { + uint8_t field_len; + int rc; + memset(adv_fields, 0, sizeof *adv_fields); + while (src_len > 0) { + rc = ble_hs_adv_parse_one_field(adv_fields, &field_len, src, src_len); + if (rc != 0) { + return rc; + } + + src += field_len; + src_len -= field_len; + } + return 0; } http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/src/ble_hs_adv.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_hs_adv.h b/net/nimble/host/src/ble_hs_adv.h index 08e0a0b..3cb5231 100644 --- a/net/nimble/host/src/ble_hs_adv.h +++ b/net/nimble/host/src/ble_hs_adv.h @@ -20,6 +20,15 @@ #include <inttypes.h> struct ble_hs_adv_fields; +struct ble_hs_adv { + uint8_t event_type; + uint8_t addr_type; + uint8_t length_data; + int8_t rssi; + uint8_t addr[6]; + uint8_t *data; +}; + #define BLE_HS_ADV_TYPE_FLAGS 0x01 #define BLE_HS_ADV_TYPE_INCOMP_16BIT_UUIDS 0x02 #define BLE_HS_ADV_TYPE_COMP_16BIT_UUIDS 0x03 @@ -32,6 +41,7 @@ struct ble_hs_adv_fields; #define BLE_HS_ADV_TYPE_TX_PWR_LEVEL 0x0a #define BLE_HS_ADV_TYPE_DEVICE_CLASS 0x0b +#define BLE_HS_ADV_FLAGS_LEN 1 #define BLE_HS_ADV_F_DISC_LTD 0x01 #define BLE_HS_ADV_F_DISC_GEN 0x02 http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/net/nimble/host/src/host_hci.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/host_hci.c b/net/nimble/host/src/host_hci.c index 5ff4726..964d9d9 100644 --- a/net/nimble/host/src/host_hci.c +++ b/net/nimble/host/src/host_hci.c @@ -29,6 +29,7 @@ #include "ble_l2cap.h" #include "ble_hci_ack.h" #include "ble_gap_conn.h" +#include "ble_hs_adv.h" _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ, "struct hci_data_hdr must be 4 bytes"); @@ -415,7 +416,7 @@ host_hci_le_adv_rpt_first_pass(uint8_t *data, int len, static int host_hci_rx_le_adv_rpt(uint8_t subevent, uint8_t *data, int len) { - struct ble_gap_conn_adv_rpt rpt; + struct ble_hs_adv adv; uint8_t num_reports; int rssi_off; int data_off; @@ -431,25 +432,25 @@ host_hci_rx_le_adv_rpt(uint8_t subevent, uint8_t *data, int len) data_off = 0; for (i = 0; i < num_reports; i++) { off = 2 + 0 * num_reports + i; - rpt.event_type = data[2 + 0 * num_reports + i]; + adv.event_type = data[2 + 0 * num_reports + i]; off = 2 + 1 * num_reports + i; - rpt.addr_type = data[2 + 1 * num_reports + i]; + adv.addr_type = data[2 + 1 * num_reports + i]; off = 2 + 2 * num_reports + i * 6; - memcpy(rpt.addr, data + off, 6); + memcpy(adv.addr, data + off, 6); off = 2 + 8 * num_reports + i; - rpt.length_data = data[off]; + adv.length_data = data[off]; off = 2 + 9 * num_reports + data_off; - rpt.data = data + off; - data_off += rpt.length_data; + adv.data = data + off; + data_off += adv.length_data; off = rssi_off + 1 * i; - rpt.rssi = data[off]; + adv.rssi = data[off]; - ble_gap_conn_rx_adv_report(&rpt); + ble_gap_conn_rx_adv_report(&adv); } return 0; http://git-wip-us.apache.org/repos/asf/incubator-mynewt-larva/blob/2b2118b3/project/hostctlrtest/src/main.c ---------------------------------------------------------------------- diff --git a/project/hostctlrtest/src/main.c b/project/hostctlrtest/src/main.c index dc8c8e2..4382323 100755 --- a/project/hostctlrtest/src/main.c +++ b/project/hostctlrtest/src/main.c @@ -57,8 +57,10 @@ uint8_t g_random_addr[BLE_DEV_ADDR_LEN]; uint8_t g_host_adv_data[BLE_HCI_MAX_ADV_DATA_LEN]; uint8_t g_host_adv_len; +#if HOSTCTLRTEST_CFG_ROLE == HOSTCTLRTEST_ROLE_ADVERTISER static uint8_t hostctlrtest_slv_addr[6] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06}; //static uint8_t hostctlrtest_slv_addr[6] = {0x82, 0x6a, 0xd0, 0x48, 0xb4, 0xb0}; +#endif #if HOSTCTLRTEST_CFG_ROLE == HOSTCTLRTEST_ROLE_INITIATOR static uint8_t hostctlrtest_mst_addr[6] = {0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a}; #endif @@ -184,6 +186,20 @@ hostctlrtest_on_disc_s(uint16_t conn_handle, uint8_t ble_hs_status, return 0; } + +static void +hostctlrtest_print_adv_rpt(struct ble_gap_conn_adv_rpt *adv) +{ + console_printf("Received advertisement report:\n"); + console_printf(" addr=%02x:%02x:%02x:%02x:%02x:%02x\n", + adv->addr[0], adv->addr[1], adv->addr[2], + adv->addr[3], adv->addr[4], adv->addr[5]); + console_printf(" flags=0x%02x\n", adv->fields.flags); + console_printf(" name="); + console_write((char *)adv->fields.name, adv->fields.name_len); + console_printf("%s\n"); +} + #endif #if HOSTCTLRTEST_CFG_ROLE == HOSTCTLRTEST_ROLE_ADVERTISER @@ -295,17 +311,29 @@ hostctlrtest_register_attrs(void) static void hostctlrtest_on_connect(struct ble_gap_conn_event *event, void *arg) { - console_printf("connection complete; handle=%d status=%d " - "peer_addr=%02x:%02x:%02x:%02x:%02x:%02x\n", - event->conn.handle, event->conn.status, - event->conn.peer_addr[0], event->conn.peer_addr[1], - event->conn.peer_addr[2], event->conn.peer_addr[3], - event->conn.peer_addr[4], event->conn.peer_addr[5]); + switch (event->type) { + case BLE_GAP_CONN_EVENT_TYPE_CONNECT: + console_printf("connection complete; handle=%d status=%d " + "peer_addr=%02x:%02x:%02x:%02x:%02x:%02x\n", + event->conn.handle, event->conn.status, + event->conn.peer_addr[0], event->conn.peer_addr[1], + event->conn.peer_addr[2], event->conn.peer_addr[3], + event->conn.peer_addr[4], event->conn.peer_addr[5]); #if HOSTCTLRTEST_CFG_ROLE == HOSTCTLRTEST_ROLE_INITIATOR - ble_gatt_disc_all_services(event->conn.handle, hostctlrtest_on_disc_s, - NULL); + ble_gatt_disc_all_services(event->conn.handle, hostctlrtest_on_disc_s, + NULL); + break; + + case BLE_GAP_CONN_EVENT_TYPE_ADV_RPT: + hostctlrtest_print_adv_rpt(&event->adv); + break; + + case BLE_GAP_CONN_EVENT_TYPE_SCAN_DONE: + console_printf("scan complete\n"); + break; #endif + } } /** @@ -333,22 +361,24 @@ hostctlrtest_task_handler(void *arg) ble_gap_conn_set_cb(hostctlrtest_on_connect, NULL); #if HOSTCTLRTEST_CFG_ROLE == HOSTCTLRTEST_ROLE_ADVERTISER - struct ble_gap_conn_adv_fields ad_fields; + struct ble_hs_adv_fields fields; hostctlrtest_register_attrs(); console_printf("ADVERTISER\n"); - ad_fields.name = "blahblah"; - ad_fields.name_is_complete = 1; - rc = ble_gap_conn_set_adv_fields(&ad_fields); + fields.name = (uint8_t *)"nimble"; + fields.name_len = 6; + fields.name_is_complete = 1; + rc = ble_gap_conn_set_adv_fields(&fields); assert(rc == 0); rc = ble_gap_conn_advertise(BLE_GAP_DISC_MODE_NON, BLE_GAP_CONN_MODE_UND, NULL, 0); #else console_printf("INITIATOR\n"); - rc = ble_gap_conn_direct_connect(BLE_HCI_ADV_PEER_ADDR_PUBLIC, - hostctlrtest_slv_addr); + rc = ble_gap_conn_disc(20000, BLE_GAP_DISC_MODE_GEN); + //rc = ble_gap_conn_direct_connect(BLE_HCI_ADV_PEER_ADDR_PUBLIC, + // hostctlrtest_slv_addr); #endif assert(rc == 0);
