gadget-roomba-nano33-ble/roomba/gatt-srv.c

/*
 * 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 <hauke.petersen@fu-berlin.de>
 *
 * @}
 */

#include <stdio.h>
#include <stdint.h>

#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;
static int cmd_buf_exp_len = 0;

void order66(void);

static void parse_bt_char(uint8_t c)
{
    uint8_t mode;

    if (cmd_buf_off == 0) {
        switch (c) {
            case 'M':
                cmd_buf_exp_len = 2;
                break;
            case 'D':
                cmd_buf_exp_len = 5;
                break;
            case 'C':
                cmd_buf_exp_len = 4;
                break;
            case 'S':
                motors_stop();
                return;
            default:
                printf("Unknown command %02x\n", cmd_buf[0]);
                return;
        }
    }
    cmd_buf[cmd_buf_off++] = c;
    if (cmd_buf_off >= cmd_buf_exp_len) {
        int len = cmd_buf_exp_len;
        cmd_buf_off = 0;
        cmd_buf_exp_len = 0;
        switch (cmd_buf[0]) {
            case 'M':
                mode = cmd_buf[1];
                if (len != 2)
                    return;
                printf("Mode switch: %d\n", mode);
                if ((mode <= ROOMBA_MODE_DOCK) || (mode == 66)) {
                    switch_mode(mode);
                }
                break;
            case 'D':
                {
                int16_t speed, radius;
                if (len != 5)
                    return;
                memcpy(&speed, cmd_buf + 1, 2);
                memcpy(&radius, cmd_buf + 3, 2);
                printf("Drive %hd %hd\n", speed, radius);
                motors_drive(speed, radius);
                }
                break;
            case 'C':
                {
                    if (len != 4)
                        return;
                    printf("Clean brushes/vac/sidebrush: %d %d %d\n", cmd_buf[1], cmd_buf[2], cmd_buf[3]);
                    motors_clean_set(cmd_buf[1], cmd_buf[2], cmd_buf[3]);
                }
                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();
        }
    }
    return 0;
}