microchip-application-bmd380-bmd380_central/host_tasks.c

#include <string.h>

#ifdef __cplusplus
extern "C"
{
#endif

#include "ble_advdata.h"

#include "nrf_ble_gatt.h"
#include "nrf_ble_scan.h"

#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_freertos.h"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "FreeRTOS.h"
#include "message_buffer.h"
#include "semphr.h"
#include "task.h"

#ifdef __cplusplus
}
#endif

extern uint32_t uart_drv_rx(void *, uint32_t);
extern uint32_t uart_drv_tx(void *, uint32_t);
typedef struct
{
    uint8_t opcode;
    uint8_t len;
} ins_survive_t;

typedef struct
{
    uint8_t opcode;
    uint8_t len;
} ins_scan_t;

typedef struct
{
    uint8_t opcode;
    uint8_t len;
    uint8_t addr_type;
    uint8_t addr[6];
} ins_connect_t;

typedef struct
{
    uint8_t opcode;
    uint8_t len;
} ins_disconnect_t;

typedef struct
{
    uint8_t opcode;
    uint8_t len;
    uint8_t handle;
    uint8_t write_data[255];
} ins_write_char_t;

typedef struct
{
    uint8_t opcode;
    uint8_t len;
    uint8_t handle;
} ins_read_char_t;

typedef union
{
    struct
    {
        uint8_t opcode;
        union
        {
            uint8_t len;
            uint8_t payload[255];
        };
    } raw;
    ins_survive_t survive;
    ins_scan_t scan;
    ins_connect_t connect;
    ins_disconnect_t disconnect;
    ins_write_char_t write_char;
    ins_read_char_t read_char;
} host_ins_t;

#define EVT_ALL                   0xFFFF
#define EVT_MEM_RETURN_DATA       0x0010
#define EVT_MEM_NOTIFY_HANDLE     0x0040
#define EVT_MEM_UART_ROUTINE      0x0080
#define EVT_MEM_INS_SCAN          0x0100
#define EVT_MEM_INS_CONNECT       0x0200
#define EVT_MEM_INS_WRITE         0x0400
#define EVT_MEM_INS_READ          0x0800
#define EVT_MEM_INS_DISCONNECT    0x1000
#define EVT_MEM_INS_CHECK_SURVIVE 0x2000

typedef enum
{
    INS_IDLE            = 0x00,
    INS_RESET           = 0x01,
    INS_KEY             = 0x02,
    INS_SCAN            = 0x03,
    INS_SCAN_RESPONSE   = 0x04,
    INS_CONNECT         = 0x05,
    INS_WRITE_CHAR      = 0x06,
    INS_READ_CHAR       = 0x07,
    INS_DISCONNECT      = 0x08,
    INS_PREPARE_CONNECT = 0x09,
    INS_CHECK_SURVIVE   = 0x0A
} ctrl_instr_t;

#define INST_SUFFIX 0xF1

static MessageBufferHandle_t host_cmd_msg;

static void success_ack(void)
{
    uint8_t ack[] = { 0x04, 0x00, 0x01, 0x03 };
    uart_drv_tx(ack, sizeof(ack));
}

static void disconnect_and_timeout_ack(void)
{
    uint8_t ack[] = { 0x04, 0x00, 0x01, 0xFF };
    uart_drv_tx(ack, sizeof(ack));
}

static void on_ins_scan(ins_scan_t *p_ins)
{
    extern void le_scan_start(void);
    le_scan_start();
}

static void on_ins_connect(ins_connect_t *p_ins)
{
    ble_gap_addr_t peer_addr;
    peer_addr.addr_id_peer = 0;
    peer_addr.addr_type    = BLE_GAP_ADDR_TYPE_ANONYMOUS;
    peer_addr.addr[0]      = p_ins->addr[5];
    peer_addr.addr[1]      = p_ins->addr[4];
    peer_addr.addr[2]      = p_ins->addr[3];
    peer_addr.addr[3]      = p_ins->addr[2];
    peer_addr.addr[4]      = p_ins->addr[1];
    peer_addr.addr[5]      = p_ins->addr[0];

    extern void le_scan_stop(void);
    le_scan_stop();

    vTaskDelay(pdMS_TO_TICKS(3));

    extern void le_scan_get_peer_addr(ble_gap_addr_t * p_peer_addr);
    le_scan_get_peer_addr(&peer_addr);

    if (peer_addr.addr_type == BLE_GAP_ADDR_TYPE_ANONYMOUS)
    {
        extern void le_scan_start(void);
        le_scan_start();
        return;
    }

    extern void le_gap_connet(ble_gap_addr_t * p_peer_addr);
    le_gap_connet(&peer_addr);

    char rsp_buf[64];
    sprintf(rsp_buf, "%s: %02X:%02X:%02X:%02X:%02X:%02X", "Connect to", peer_addr.addr[5], peer_addr.addr[4], peer_addr.addr[3], peer_addr.addr[2], peer_addr.addr[1], peer_addr.addr[0]);
    NRF_LOG_INFO("%s", rsp_buf);
}

static void on_ins_write_char(ins_write_char_t *p_ins)
{
    uint16_t write_size = p_ins->len - sizeof(p_ins->handle) - 1;
    NRF_LOG_INFO("write handle: 0x%02X", p_ins->handle);
    {
        uint8_t *p_start = (void *)p_ins->write_data;
        uint32_t loop    = write_size / 8;
        uint32_t remain  = write_size % 8;
        for (int i = 0; i < loop; i++)
        {
            NRF_LOG_HEXDUMP_INFO(p_start, 8);
            p_start += 8;
        }
        if (remain)
        {
            NRF_LOG_HEXDUMP_INFO(p_start, remain);
        }
    }

    extern ret_code_t le_gatt_c_write_req(uint32_t handle, void *, uint16_t);
    ret_code_t err_code = le_gatt_c_write_req(p_ins->handle, p_ins->write_data, write_size);
    APP_ERROR_CHECK(err_code);

    success_ack();
}

static void on_ins_read_char(ins_read_char_t *p_ins)
{
    NRF_LOG_INFO("read handle: 0x%02X", p_ins->handle);
    extern ret_code_t le_gatt_c_read_char_req(uint32_t handle);
    ret_code_t ret_code = le_gatt_c_read_char_req(p_ins->handle);
}

static void on_ins_disconnect(ins_disconnect_t *p_ins)
{
    extern void le_gap_disconnet(uint16_t);
    le_gap_disconnet(0);
}

static void on_ins_survive(ins_survive_t *p_ins)
{
    success_ack();
}

void host_read_char_cb(uint8_t *p_data, uint16_t len)
{
    static union
    {
        struct
        {
            uint16_t rsp_code;
            uint8_t payload[];
        };
        uint8_t arr[256];
    } read_char_rsp = {
        .rsp_code = 0x0004
    };

    memcpy(read_char_rsp.payload, p_data, len);

    uart_drv_tx(&read_char_rsp, sizeof(read_char_rsp.rsp_code) + len);

    {
        uint8_t *p_start = p_data;
        uint32_t loop    = len / 8;
        uint32_t remain  = len % 8;
        for (int i = 0; i < loop; i++)
        {
            NRF_LOG_HEXDUMP_INFO(p_start, 8);
            p_start += 8;
        }
        if (remain)
        {
            NRF_LOG_HEXDUMP_INFO(p_start, remain);
        }
    }
}

typedef struct
{
    const uint16_t company_identifier;
    struct
    {
        const uint8_t company_code[5];
        const uint8_t hw_ver[4];
        const uint16_t battery_level;
    } __PACKED data;
} manu_spec_data_t;

void host_scan_filter_match_cb(
    void *p_manu_spec_data,
    uint32_t manu_spec_data_len,
    char *p_device_name,
    uint32_t device_name_len,
    ble_gap_addr_t const *peer_addr,
    int16_t rssi)
{
    typedef struct
    {
        uint16_t hci_packet_event;
        uint8_t len;
        uint8_t addr[6];
        uint8_t manu_spec_data[15];
        uint8_t dev_name[21];
    } __PACKED host_scan_rsp_t;

    static host_scan_rsp_t host_scan_rsp;

    memset(&host_scan_rsp, 0x00, sizeof(host_scan_rsp));

    host_scan_rsp.hci_packet_event = 0x0004;

    host_scan_rsp.len = sizeof(host_scan_rsp_t) - offsetof(host_scan_rsp_t, addr);

    memcpy(host_scan_rsp.addr, peer_addr->addr, sizeof(host_scan_rsp.addr));

    memcpy((void *)&host_scan_rsp.manu_spec_data, p_manu_spec_data, sizeof(host_scan_rsp.manu_spec_data));

    memcpy((void *)host_scan_rsp.dev_name, p_device_name, device_name_len);

    uart_drv_tx(&host_scan_rsp, sizeof(host_scan_rsp));

    char peer_addr_str[(2 + 1) * BLE_GAP_ADDR_LEN + 1] = { 0 };
    sprintf(peer_addr_str, "%02X:%02X:%02X:%02X:%02X:%02X", host_scan_rsp.addr[5], host_scan_rsp.addr[4], host_scan_rsp.addr[3], host_scan_rsp.addr[2], host_scan_rsp.addr[1], host_scan_rsp.addr[0]);

    NRF_LOG_INFO("Found: %s [%s, %ddb]",
                 host_scan_rsp.dev_name,
                 peer_addr_str,
                 rssi);
}

void host_connected_cb(void)
{
    success_ack();
}

static void host_cmd_exec_task(void *p_arg)
{
    for (;;)
    {
        static uint8_t buf[256] = { 0 };
        host_ins_t *p_ins       = (void *)buf;

        p_ins->raw.opcode = INS_IDLE;
        p_ins->raw.len    = 0;

        size_t recv = xMessageBufferReceive(host_cmd_msg, buf, sizeof(buf), portMAX_DELAY);

        switch (p_ins->raw.opcode)
        {
            case INS_SCAN:
                on_ins_scan(&p_ins->scan);
                break;
            case INS_CONNECT:
                on_ins_connect(&p_ins->connect);
                break;
            case INS_WRITE_CHAR:
                on_ins_write_char(&p_ins->write_char);
                break;
            case INS_READ_CHAR:
                on_ins_read_char(&p_ins->read_char);
                break;
            case INS_DISCONNECT:
                on_ins_disconnect(&p_ins->disconnect);
                break;
            case INS_CHECK_SURVIVE:
                on_ins_survive(&p_ins->survive);
                break;
            default:
                break;
        }
    }
}

static void host_cmd_recv_task(void *p_arg)
{
    for (;;)
    {
        static uint8_t buf[256];

        uint32_t recv   = uart_drv_rx(buf, sizeof(buf));
        uint32_t offset = 0;

        while (offset < recv)
        {
            host_ins_t *p_ins = (void *)&buf[offset];

            taskENTER_CRITICAL();

            if (p_ins->raw.payload[p_ins->raw.len] == INST_SUFFIX)
            {
                switch (p_ins->raw.opcode)
                {
                    case INS_CHECK_SURVIVE:
                        xMessageBufferSend(host_cmd_msg, p_ins, sizeof(ins_survive_t), pdMS_TO_TICKS(0));
                        offset += sizeof(ins_survive_t);
                        break;
                    case INS_CONNECT:
                        xMessageBufferSend(host_cmd_msg, p_ins, sizeof(ins_connect_t), pdMS_TO_TICKS(0));
                        offset += sizeof(ins_connect_t);
                        break;
                    case INS_DISCONNECT:
                        xMessageBufferSend(host_cmd_msg, p_ins, sizeof(ins_disconnect_t), pdMS_TO_TICKS(0));
                        offset += sizeof(ins_disconnect_t);
                        break;
                    case INS_SCAN:
                        xMessageBufferSend(host_cmd_msg, p_ins, sizeof(ins_scan_t), pdMS_TO_TICKS(0));
                        offset += sizeof(ins_scan_t);
                        break;
                    case INS_READ_CHAR:
                        xMessageBufferSend(host_cmd_msg, p_ins, sizeof(ins_read_char_t), pdMS_TO_TICKS(0));
                        offset += sizeof(ins_read_char_t);
                        break;
                    case INS_WRITE_CHAR:
                        xMessageBufferSend(host_cmd_msg, p_ins, offsetof(ins_write_char_t, handle) + p_ins->write_char.len, pdMS_TO_TICKS(0));
                        offset += offsetof(ins_write_char_t, handle) + p_ins->write_char.len;
                        break;
                    default:
                        offset++;
                        break;
                }
            }
            else
            {
                offset++;
            }
            taskEXIT_CRITICAL();
        }
    }
}

static void host_tasks_handler(ble_evt_t const *p_ble_evt, void *p_context)
{
    ret_code_t err_code;
    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            break;
        case BLE_GAP_EVT_DISCONNECTED:
        case BLE_GAP_EVT_TIMEOUT:
            disconnect_and_timeout_ack();
            break;
        default:
            break;
    }
}

void host_tasks_init(void)
{
    host_cmd_msg = xMessageBufferCreate(1024);
    xTaskCreate(host_cmd_recv_task, "cmd_recv", 128, NULL, 3, NULL);
    xTaskCreate(host_cmd_exec_task, "cmd_exec", 512, NULL, 3, NULL);
    // Register a handler for BLE events.
    NRF_SDH_BLE_OBSERVER(m_tasks_observer, 3, host_tasks_handler, NULL);
}