/* * Copyright (C) 2019 Freie Universität Berlin * * This file is subject to the terms and conditions of the GNU Lesser * General Public License v2.1. See the file LICENSE in the top level * directory for more details. */ /** * @ingroup examples * @{ * * @file * @brief (Mock-up) BLE heart rate sensor example * * @author Hauke Petersen * * @} */ #include #include #include "assert.h" #include "event/timeout.h" #include "nimble_riot.h" #include "net/bluetil/ad.h" #include "periph/gpio.h" #include "timex.h" #include "roomba.h" #include "board.h" #include "host/ble_hs.h" #include "host/ble_gatt.h" #include "services/gap/ble_svc_gap.h" #include "services/gatt/ble_svc_gatt.h" #define BLE_GATT_SVC_IPS 0x1821 #define BLE_GATT_CHAR_IPS_CONFIG 0x2AAD #define BLE_GATT_CHAR_LATITUDE 0x2AAE #define BLE_GATT_CHAR_LONGITUDE 0x2AAF #define BLE_GATT_CHAR_LOCAL_NORTH 0x2AB0 #define BLE_GATT_CHAR_LOCAL_EAST 0x2AB1 #define BLE_GATT_CHAR_FLOOR_N 0x2AB2 #define BLE_GATT_CHAR_ALTITUDE 0x2AB3 #define BLE_GATT_CHAR_UNCERTAIN 0x2AB4 #define BLE_GATT_CHAR_LOC_NAME 0x2AB4 #define IPS_CONFIG_COORD_PRESENT (1 << 0) #define IPS_CONFIG_COORD_IN_ADV (1 << 1) #define IPS_CONFIG_TXP_IN_ADV (1 << 2) #define IPS_CONFIG_ALT_IN_ADV (1 << 3) #define IPS_CONFIG_FLOOR_IN_ADV (1 << 4) #define IPS_CONFIG_UNCE_IN_ADV (1 << 5) #define IPS_CONFIG_LOC_NAME_AVAIL (1 << 6) static const char *_device_name = "ROOMBAHAHAHACKED"; static const char *_manufacturer_name = "Insane adding machines"; static const char *_model_number = "2X"; static const char *_serial_number = "a8b302c7f3-29183-x8"; static const char *_fw_ver = "1.0.0"; static const char *_hw_ver = "1.0"; static event_queue_t _eq; static event_t _update_evt; static event_timeout_t _update_timeout_evt; static uint16_t _conn_handle; static uint16_t _ips_val_handle; static uint16_t _sensors_val_handle; static int _ips_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); static int _devinfo_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); static int _roomba_ctrl_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); static int _roomba_sensors_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); static int _bas_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); static void _start_advertising(void); /* UUID = 35f28386-3070-4f3b-ba38-27507e991760 */ static const ble_uuid128_t gatt_svr_svc_roomba = BLE_UUID128_INIT( 0x60, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba, 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35 ); /* UUID = 35f28386-3070-4f3b-ba38-27507e991762 */ static const ble_uuid128_t gatt_svr_chr_roomba_ctl = BLE_UUID128_INIT( 0x62, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba, 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35 ); /* UUID = 35f28386-3070-4f3b-ba38-27507e991764 */ static const ble_uuid128_t gatt_svr_chr_roomba_sensors = BLE_UUID128_INIT( 0x64, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba, 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35 ); /* UUID = 35f28386-3070-4f3b-ba38-27507e991766 */ static const ble_uuid128_t gatt_svr_chr_roomba_sensors_notify = BLE_UUID128_INIT( 0x66, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba, 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35 ); /* GATT service definitions */ static const struct ble_gatt_svc_def gatt_svr_svcs[] = { { /* Indoor positioning service */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_IPS), .characteristics = (struct ble_gatt_chr_def[]) { { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_IPS_CONFIG), .access_cb = _ips_handler, .val_handle = &_ips_val_handle, .flags = BLE_GATT_CHR_F_NOTIFY, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_LATITUDE), .access_cb = _ips_handler, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_LONGITUDE), .access_cb = _ips_handler, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE, }, { 0, /* no more characteristics in this service */ }, } }, { /* Roomba controls */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = (ble_uuid_t*) &gatt_svr_svc_roomba.u, .characteristics = (struct ble_gatt_chr_def[]) { { /* Characteristic: ROOMBA CTRL */ .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_ctl.u, .access_cb = _roomba_ctrl_handler, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE, }, { /* Characteristic: ROOMBA sensors read/notify */ .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors.u, .access_cb = _roomba_sensors_handler, .flags = BLE_GATT_CHR_F_READ, }, { .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors_notify.u, .access_cb = _roomba_sensors_handler, .val_handle = &_sensors_val_handle, .flags = BLE_GATT_CHR_F_NOTIFY, }, { 0, /* no more characteristics in this service */ }, } }, { /* Device Information Service */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_DEVINFO), .characteristics = (struct ble_gatt_chr_def[]) { { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_MANUFACTURER_NAME), .access_cb = _devinfo_handler, .flags = BLE_GATT_CHR_F_READ, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_MODEL_NUMBER_STR), .access_cb = _devinfo_handler, .flags = BLE_GATT_CHR_F_READ, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_SERIAL_NUMBER_STR), .access_cb = _devinfo_handler, .flags = BLE_GATT_CHR_F_READ, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_FW_REV_STR), .access_cb = _devinfo_handler, .flags = BLE_GATT_CHR_F_READ, }, { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_HW_REV_STR), .access_cb = _devinfo_handler, .flags = BLE_GATT_CHR_F_READ, }, { 0, /* no more characteristics in this service */ }, } }, { /* Battery Level Service */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_BAS), .characteristics = (struct ble_gatt_chr_def[]) { { .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_BATTERY_LEVEL), .access_cb = _bas_handler, .flags = BLE_GATT_CHR_F_READ, }, { 0, /* no more characteristics in this service */ }, } }, { 0, /* no more services */ }, }; int cmd_mode(int argc, char **argv); uint8_t get_mode(void); int switch_mode(int newmode); static uint8_t cmd_buf[20]; static int cmd_buf_off = 0; void order66(void); static void parse_bt_char(uint8_t c) { uint8_t mode; cmd_buf[cmd_buf_off++] = c; if (c == 0xFF) { int len = cmd_buf_off - 1; cmd_buf_off = 0; if (len == 0) return; switch (cmd_buf[0]) { case 'M': mode = cmd_buf[1]; if (len < 1) return; printf("Mode switch: %d\n", mode); if (mode <= ROOMBA_MODE_DOCK) { switch_mode(mode); } break; case 'B': printf("Order 66\n"); order66(); break; default: printf("Unknown command %02x\n", cmd_buf[0]); } } } static int _roomba_sensors_handler( uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { int rc = 0; uint16_t om_len; ble_uuid_t* read_uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors.u; static uint8_t ch; struct roomba_sensors_data *bt_sensors; puts("[BT] Sensor command"); (void) conn_handle; (void) attr_handle; (void) arg; bt_sensors = roomba_sensors(); printf("bt_sensors.valid = %s\n", bt_sensors->valid?"yes":"no"); if (ble_uuid_cmp(ctxt->chr->uuid, read_uuid) == 0) { puts("access to characteristic 'roomba sensors'"); rc = os_mbuf_append(ctxt->om, bt_sensors, sizeof(struct roomba_sensors_data)); puts(""); return rc; } puts("unhandled uuid!"); return 1; } static int _roomba_ctrl_handler( uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { int rc = 0; uint16_t om_len; ble_uuid_t* write_uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_ctl.u; static uint8_t ch; puts("[BT] Command"); (void) conn_handle; (void) attr_handle; (void) arg; int mode = get_mode(); if (ble_uuid_cmp(ctxt->chr->uuid, write_uuid) == 0) { puts("access to characteristic 'roomba ctl'"); switch (ctxt->op) { case BLE_GATT_ACCESS_OP_READ_CHR: puts("read mode"); /* send given data to the client */ rc = os_mbuf_append(ctxt->om, &mode, sizeof(unsigned int)); break; case BLE_GATT_ACCESS_OP_WRITE_CHR: rc = ble_hs_mbuf_to_flat(ctxt->om, &ch, 1, &om_len); printf("om_len = %d, char: %02x\n", om_len, ch); parse_bt_char(ch); 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; } puts("unhandled uuid!"); return 1; } static int _ips_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { (void)conn_handle; (void)attr_handle; (void)arg; int res; static uint32_t lat = 0x01020304, lon = 0x05060708; puts("[ACCESS] indoor positioning value"); if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { printf("[READ] "); if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LATITUDE) { printf("LAT\n"); res = os_mbuf_append(ctxt->om, &lat, sizeof(lat)); return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES; } if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LONGITUDE) { printf("LON\n"); res = os_mbuf_append(ctxt->om, &lon, sizeof(lon)); return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES; } } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { printf("[WRITE] "); if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LATITUDE) { printf("LAT\n"); memcpy(&lat, ctxt->om->om_databuf, sizeof(lat)); res = os_mbuf_append(ctxt->om, &lat, sizeof(lat)); return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES; } if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LONGITUDE) { printf("LON\n"); printf("%08x\n", *((unsigned int *)(ctxt->om->om_databuf))); printf("%08x\n", *((unsigned int *)(ctxt->om->om_databuf + 4))); memcpy(&lon, ctxt->om->om_databuf, sizeof(lon)); res = os_mbuf_append(ctxt->om, &lon, sizeof(lon)); return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES; } } return BLE_ATT_ERR_INSUFFICIENT_RES; } static int _devinfo_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { (void)conn_handle; (void)attr_handle; (void)arg; const char *str; switch (ble_uuid_u16(ctxt->chr->uuid)) { case BLE_GATT_CHAR_MANUFACTURER_NAME: puts("[READ] device information service: manufacturer name value"); str = _manufacturer_name; break; case BLE_GATT_CHAR_MODEL_NUMBER_STR: puts("[READ] device information service: model number value"); str = _model_number; break; case BLE_GATT_CHAR_SERIAL_NUMBER_STR: puts("[READ] device information service: serial number value"); str = _serial_number; break; case BLE_GATT_CHAR_FW_REV_STR: puts("[READ] device information service: firmware revision value"); str = _fw_ver; break; case BLE_GATT_CHAR_HW_REV_STR: puts("[READ] device information service: hardware revision value"); str = _hw_ver; break; default: return BLE_ATT_ERR_UNLIKELY; } int res = os_mbuf_append(ctxt->om, str, strlen(str)); return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } static int _bas_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { (void)conn_handle; (void)attr_handle; (void)arg; puts("[READ] battery level service: battery level value"); uint8_t level = 50; /* this battery will never drain :-) */ int res = os_mbuf_append(ctxt->om, &level, sizeof(level)); return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } static int gap_event_cb(struct ble_gap_event *event, void *arg) { (void)arg; switch (event->type) { case BLE_GAP_EVENT_CONNECT: printf("Ev: connect, Status: %d\n", event->connect.status); if (event->connect.status) { _start_advertising(); LED2_OFF; return 0; } LED2_ON; _conn_handle = event->connect.conn_handle; break; case BLE_GAP_EVENT_DISCONNECT: _start_advertising(); break; case BLE_GAP_EVENT_SUBSCRIBE: if (event->subscribe.attr_handle == _ips_val_handle) { if (event->subscribe.cur_notify == 1) { } else { } } break; } return 0; } static void _start_advertising(void) { struct ble_gap_adv_params advp; int res; memset(&advp, 0, sizeof advp); advp.conn_mode = BLE_GAP_CONN_MODE_UND; advp.disc_mode = BLE_GAP_DISC_MODE_GEN; advp.itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN; advp.itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX; res = ble_gap_adv_start(nimble_riot_own_addr_type, NULL, BLE_HS_FOREVER, &advp, gap_event_cb, NULL); assert(res == 0); (void)res; } static void update_sensors(void) { struct os_mbuf *om; struct roomba_sensors_data *sens; printf("[GATT] Sensor values changed\n"); sens = roomba_sensors(); /* send data notification to GATT client */ om = ble_hs_mbuf_from_flat(sens, sizeof(struct roomba_sensors_data)); if (!om) { printf("[GATT] Error incapsulating sensor data in frame \n"); return; } ble_gattc_notify_custom(_conn_handle, _sensors_val_handle, om); } void *gatt_srv(void *arg) { puts("NimBLE GATT server starting"); int res = 0; msg_t msg; (void)res; /* setup local event queue (for handling heart rate updates) */ /* verify and add our custom services */ res = ble_gatts_count_cfg(gatt_svr_svcs); assert(res == 0); res = ble_gatts_add_svcs(gatt_svr_svcs); assert(res == 0); /* set the device name */ ble_svc_gap_device_name_set(_device_name); /* reload the GATT server to link our added services */ ble_gatts_start(); /* configure and set the advertising data */ uint8_t buf[BLE_HS_ADV_MAX_SZ]; bluetil_ad_t ad; bluetil_ad_init_with_flags(&ad, buf, sizeof(buf), BLUETIL_AD_FLAGS_DEFAULT); uint16_t ips_uuid = BLE_GATT_SVC_IPS; bluetil_ad_add(&ad, BLE_GAP_AD_UUID16_INCOMP, &ips_uuid, sizeof(ips_uuid)); bluetil_ad_add_name(&ad, _device_name); ble_gap_adv_set_data(ad.buf, ad.pos); /* start to advertise this node */ _start_advertising(); while (1) { if (msg_receive(&msg) >= 0) { (void)msg; update_sensors(); } } /* run an event loop for handling the heart rate update events */ //event_loop(&_eq); return 0; }