370 lines
11 KiB
C
370 lines
11 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include "pico/stdlib.h"
|
|
|
|
#include "ble/gatt-service/hids_device.h"
|
|
#include "btstack_ring_buffer.h"
|
|
#include "btstack_defines.h"
|
|
|
|
#include "usb_device.h"
|
|
#include "usb_host.h"
|
|
|
|
#include "hid_report.h"
|
|
|
|
static uint8_t buffer_storage[REPORT_BUF_SIZE];
|
|
static btstack_ring_buffer_t report_buf;
|
|
|
|
static uint8_t desc_hid_report[HID_DESCRIPTOR_SIZE];
|
|
static uint16_t desc_hid_report_len=0;
|
|
static struct report_desc *descriptors;
|
|
|
|
static struct report_desc* report_desc_alloc(void);
|
|
static void report_desc_init(struct report_desc *descriptor);
|
|
static void report_desc_free(struct report_desc *descriptor);
|
|
static struct report_desc* report_desc_find(uint8_t dev_addr, uint8_t instance);
|
|
static struct report_dict* report_dict_alloc(struct report_desc *descriptor);
|
|
static void report_dict_init(struct report_dict *mapping);
|
|
static void report_dict_free(struct report_dict *mapping, struct report_desc *descriptor);
|
|
static struct report_dict* find_mapping(uint8_t dev_addr, uint8_t instance, uint8_t report_id);
|
|
|
|
extern hci_con_handle_t con_handle;
|
|
|
|
// start listening to HID input reports on all mounted devices
|
|
bool request_hid_reports_all(void) {
|
|
// send request to receive reports on all mounted devices
|
|
struct report_desc * current;
|
|
for (current=descriptors; current != NULL; current=current->next) {
|
|
if (! current->listening) {
|
|
if(request_hid_report(current->dev_addr, current->instance)) {
|
|
char tempbuf[40];
|
|
size_t count = sprintf(tempbuf, "Listening to input reports on [%u:%u]\n", current->dev_addr, current->instance);
|
|
cdc_print_str(tempbuf, count);
|
|
current->listening = true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// stop listening to HID input reports on all mounted devices
|
|
bool stop_hid_reports_all(void) {
|
|
// send request to stop reports on all mounted devices
|
|
struct report_desc * current;
|
|
for (current=descriptors; current != NULL; current=current->next) {
|
|
if (current->listening) {
|
|
if(stop_hid_report(current->dev_addr, current->instance)) {
|
|
char tempbuf[40];
|
|
size_t count = sprintf(tempbuf, "Stopping input reports on [%u:%u]\n", current->dev_addr, current->instance);
|
|
cdc_print_str(tempbuf, count);
|
|
current->listening = false;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// used to send next HID input report on the BLE interface
|
|
void send_report(){
|
|
// process queue and send next report
|
|
if (btstack_ring_buffer_bytes_available(&report_buf)) {
|
|
uint8_t dev_addr;
|
|
uint8_t instance;
|
|
uint8_t len8[2];
|
|
uint16_t len;
|
|
uint32_t num_bytes_read;
|
|
|
|
// retrieve dev_addr from ring_buffer
|
|
btstack_ring_buffer_read(&report_buf, &dev_addr, 1, &num_bytes_read);
|
|
|
|
// retrieve instance from ring buffer
|
|
btstack_ring_buffer_read(&report_buf, &instance, 1, &num_bytes_read);
|
|
|
|
// retrieve length as two uint8_t and turn into uint16_t
|
|
btstack_ring_buffer_read(&report_buf, len8, 2, &num_bytes_read);
|
|
memcpy(&len, len8, 2);
|
|
|
|
// retrieve report from ring buffer
|
|
uint8_t report[len];
|
|
btstack_ring_buffer_read(&report_buf, report, len, &num_bytes_read);
|
|
|
|
// find report id mapping
|
|
struct report_dict * mapping = find_mapping(dev_addr, instance, report[0]);
|
|
|
|
char tempbuf[16];
|
|
size_t count=sprintf(tempbuf, "[%04x](%u) ", con_handle, mapping->ble_id);
|
|
cdc_print_str(tempbuf, count);
|
|
|
|
// send hid report to ble interface
|
|
if (mapping != NULL) {
|
|
if (mapping->report_id==0) {
|
|
hids_device_send_input_report_for_id(con_handle, mapping->ble_id, report, len);
|
|
cdc_print_hex(report, len);
|
|
} else {
|
|
// replace report ID in original report before sending
|
|
hids_device_send_input_report_for_id(con_handle, mapping->ble_id, &report[1], len-1);
|
|
cdc_print_hex(&report[1], len-1);
|
|
}
|
|
}
|
|
|
|
// request send of next report
|
|
hids_device_request_can_send_now_event(con_handle);
|
|
}
|
|
}
|
|
|
|
// add report to the BTstack ring buffer for sending
|
|
void queue_report(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len) {
|
|
if (con_handle != HCI_CON_HANDLE_INVALID) {
|
|
// convert len to array of two uint8_t from single uint16_t
|
|
uint8_t len8[2];
|
|
memcpy(len8, &len, 2);
|
|
|
|
if (btstack_ring_buffer_bytes_free(&report_buf) >= len+4) {
|
|
// put instance, length, and report into ring buffer if space available
|
|
btstack_ring_buffer_write(&report_buf, &dev_addr, 1);
|
|
btstack_ring_buffer_write(&report_buf, &instance, 1);
|
|
btstack_ring_buffer_write(&report_buf, len8, 2);
|
|
btstack_ring_buffer_write(&report_buf, report, len);
|
|
}
|
|
|
|
// request send on BLE HID interface
|
|
hids_device_request_can_send_now_event(con_handle);
|
|
}
|
|
}
|
|
|
|
// allocate memory for HID report ring buffer
|
|
void init_report_buf(void) {
|
|
btstack_ring_buffer_init(&report_buf, buffer_storage, sizeof(buffer_storage));
|
|
}
|
|
|
|
// allocate memory for USB interface report descriptor
|
|
static struct report_desc * report_desc_alloc(void) {
|
|
struct report_desc *ret = REPORT_DESC_ALLOC();
|
|
|
|
if (ret != NULL) {
|
|
report_desc_init(ret);
|
|
if (descriptors == NULL) {
|
|
descriptors = ret;
|
|
} else {
|
|
struct report_desc *last;
|
|
for (last = descriptors; last->next != NULL; last=last->next);
|
|
last->next = ret;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
// initialize report descriptor struct
|
|
static void report_desc_init(struct report_desc *descriptor) {
|
|
memset(descriptor, 0, sizeof(struct report_desc));
|
|
descriptor->next = NULL;
|
|
descriptor->mappings = NULL;
|
|
}
|
|
|
|
// free memory and teardown usb->bt report ID mappings for report descriptor struct
|
|
static void report_desc_free(struct report_desc *descriptor) {
|
|
if (descriptor != NULL) {
|
|
if (descriptors == descriptor) {
|
|
descriptors = descriptor->next;
|
|
} else {
|
|
struct report_desc *last;
|
|
for (last = descriptors; last->next != NULL; last = last->next) {
|
|
if ((last->next) == descriptor) {
|
|
last->next = descriptor->next;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
while (descriptor->mappings != NULL) {
|
|
report_dict_free(descriptor->mappings, descriptor);
|
|
}
|
|
free(descriptor);
|
|
}
|
|
}
|
|
|
|
// add report descriptor for new HID interface
|
|
bool add_descriptor(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len) {
|
|
struct report_desc * descriptor = report_desc_alloc();
|
|
if (descriptor == NULL) {
|
|
return false;
|
|
}
|
|
|
|
memcpy(descriptor->descriptor, desc_report, desc_len);
|
|
descriptor->desc_len = desc_len;
|
|
descriptor->dev_addr = dev_addr;
|
|
descriptor->instance = instance;
|
|
descriptor->listening = false;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
// generate new report descriptor for BLE device and save mappings for report IDs
|
|
bool generate_report_descriptor(void) {
|
|
memset(desc_hid_report, 0, sizeof(desc_hid_report));
|
|
desc_hid_report_len=0;
|
|
|
|
uint8_t num_mounted = 0;
|
|
|
|
struct report_desc * current;
|
|
for (current=descriptors; current != NULL; current=current->next) {
|
|
if (desc_hid_report_len + (current->desc_len) + 2 < HID_DESCRIPTOR_SIZE) {
|
|
memcpy(&desc_hid_report[desc_hid_report_len], current->descriptor, current->desc_len);
|
|
} else {
|
|
cdc_print_msg("Err: HID report descriptor too long\n");
|
|
return false;
|
|
}
|
|
|
|
bool no_id = true;
|
|
|
|
// search for report ID (0x85) in report descriptor
|
|
for (int i=0; i<(current->desc_len); i++) {
|
|
if(current->descriptor[i] == 0x85) {
|
|
// use report ID found in descriptor
|
|
no_id=false;
|
|
i++;
|
|
|
|
// modify report ID and save mapping
|
|
struct report_dict * report_map = report_dict_alloc(current);
|
|
if (report_map == NULL) {
|
|
return false;
|
|
}
|
|
num_mounted++;
|
|
report_map->report_id = current->descriptor[i];
|
|
report_map->ble_id = num_mounted;
|
|
|
|
desc_hid_report[desc_hid_report_len+i]=num_mounted;
|
|
}
|
|
}
|
|
|
|
// no report ID found in descriptor for the interface
|
|
if (no_id) {
|
|
// add report id field to beginning of descriptor after Collection (0xa1) field
|
|
uint8_t col_pos=0;
|
|
for (int i=0; i<(current->desc_len); i++) {
|
|
if(current->descriptor[i] == 0xa1) {
|
|
col_pos=i;
|
|
break;
|
|
}
|
|
}
|
|
desc_hid_report[desc_hid_report_len+col_pos+2] = 0x85;
|
|
desc_hid_report[desc_hid_report_len+col_pos+3] = num_mounted+1;
|
|
memcpy(&desc_hid_report[desc_hid_report_len+col_pos+4], ¤t->descriptor[col_pos+2], current->desc_len-col_pos);
|
|
desc_hid_report_len += current->desc_len+2;
|
|
|
|
// store mapping with report ID of 0
|
|
struct report_dict * report_map = report_dict_alloc(current);
|
|
num_mounted++;
|
|
report_map->report_id = 0;
|
|
report_map->ble_id = num_mounted;
|
|
} else {
|
|
desc_hid_report_len += current->desc_len;
|
|
}
|
|
}
|
|
|
|
if (num_mounted > NUM_REPORT_IDS) {
|
|
cdc_print_msg("Error: too many report IDs\n");
|
|
return false;
|
|
}
|
|
|
|
btstack_ring_buffer_reset(&report_buf);
|
|
|
|
return true;
|
|
}
|
|
|
|
// remove report descriptor for HID interface
|
|
void remove_instance(uint8_t dev_addr, uint8_t instance) {
|
|
struct report_desc *descriptor = report_desc_find(dev_addr, instance);
|
|
|
|
if (descriptor != NULL) {
|
|
report_desc_free(descriptor);
|
|
}
|
|
}
|
|
|
|
// find report descriptor by device address and instance
|
|
static struct report_desc * report_desc_find(uint8_t dev_addr, uint8_t instance) {
|
|
struct report_desc *descriptor;
|
|
for (descriptor = descriptors; descriptor != NULL; descriptor = descriptor->next) {
|
|
if (descriptor->dev_addr==dev_addr && descriptor->instance==instance) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
return descriptor;
|
|
}
|
|
|
|
// allocate memory for usb->bt report id mapping
|
|
static struct report_dict * report_dict_alloc(struct report_desc * descriptor) {
|
|
struct report_dict *ret = REPORT_DICT_ALLOC();
|
|
|
|
if (ret != NULL) {
|
|
report_dict_init(ret);
|
|
if (descriptor->mappings == NULL) {
|
|
descriptor->mappings = ret;
|
|
} else {
|
|
struct report_dict *last;
|
|
for (last = descriptor->mappings; last->next != NULL; last=last->next);
|
|
last->next = ret;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
// initialize usb->bt report id mapping struct
|
|
static void report_dict_init(struct report_dict *mapping) {
|
|
memset(mapping, 0, sizeof(struct report_dict));
|
|
mapping->next = NULL;
|
|
}
|
|
|
|
// free memory from report id mapping
|
|
static void report_dict_free(struct report_dict *mapping, struct report_desc *descriptor) {
|
|
if (mapping != NULL) {
|
|
if (descriptor->mappings == mapping) {
|
|
descriptor->mappings = mapping->next;
|
|
} else {
|
|
struct report_dict *last;
|
|
for (last = descriptor->mappings; last->next != NULL; last = last->next) {
|
|
if ((last->next) == mapping) {
|
|
last->next = mapping->next;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
free(mapping);
|
|
}
|
|
}
|
|
|
|
// find the usb->bt report id mapping struct for given device, instance, and usb report id
|
|
static struct report_dict * find_mapping(uint8_t dev_addr, uint8_t instance, uint8_t report_id) {
|
|
struct report_desc * descriptor;
|
|
descriptor = report_desc_find(dev_addr, instance);
|
|
|
|
if (descriptor != NULL) {
|
|
struct report_dict * mapping;
|
|
for (mapping = descriptor->mappings; mapping != NULL; mapping = mapping->next) {
|
|
if (mapping->report_id ==0 || mapping->report_id == report_id) {
|
|
return mapping;
|
|
}
|
|
}
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
uint16_t get_desc_hid_report_len(void) {
|
|
return desc_hid_report_len;
|
|
}
|
|
|
|
|
|
uint8_t const* get_desc_hid_report(void) {
|
|
return desc_hid_report;
|
|
}
|