quack/main/blue.c

/* This example code is in the Public Domain (or CC0 licensed, at your option.)
   Unless required by applicable law or agreed to in writing, this software is
   distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"
#include "esp_bt_defs.h"
#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_gatt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"

#include "esp_hidh.h"
#include "esp_hid_gap.h"

#include "adb.h"
#include "blue.h"
#include "gpio.h"
#include "led.h"
#include "m4848.h"

/* debug tag */
static const char *TAG = "blue";

/* globals */
extern TaskHandle_t t_click, t_qx, t_qy;

/* private functions */
void blue_h_callback(void *handler_args, esp_event_base_t base, int32_t id, void *event_data);
static void	blue_handle_button(uint8_t buttons);
static esp_hid_raw_report_map_t	*blue_hid_rm_get(esp_hidh_dev_t *dev);
static esp_hid_report_item_t	blue_h_ri_find(esp_hidh_dev_t *d, esp_hid_usage_t u, uint8_t t, uint8_t p);
static void blue_set_boot_protocol(esp_hidh_dev_t *dev);
static bool blue_support_boot(esp_hidh_dev_t *dev);

/* direct calls to bluedroid */
extern void BTA_HhSetProtoMode(uint8_t handle, uint8_t t_type);

void blue_h_callback(void *handler_args, esp_event_base_t base, int32_t id, void *event_data)
{
	esp_hidh_event_t event = (esp_hidh_event_t)id;
	esp_hidh_event_data_t *param = (esp_hidh_event_data_t *)event_data;

	const uint8_t *bda = NULL;

	/* union described in include/esp_hidh.h */
	switch (event) {
		case ESP_HIDH_OPEN_EVENT: {
			esp_hidh_dev_dump(param->open.dev, stdout);
			blue_h_open(param);
			break;
		}
		case ESP_HIDH_BATTERY_EVENT: {
			bda = esp_hidh_dev_bda_get(param->battery.dev);
			ESP_LOGI(TAG, ESP_BD_ADDR_STR " battery: %d%%", ESP_BD_ADDR_HEX(bda), param->battery.level);
			break;
		}
		case ESP_HIDH_INPUT_EVENT: {
			bda = esp_hidh_dev_bda_get(param->input.dev);
						   //ESP_LOGD(TAG, ESP_BD_ADDR_STR " INPUT: %8s, MAP: %2u, ID: %3u, Len: %d, Data:", ESP_BD_ADDR_HEX(bda), esp_hid_usage_str(param->input.usage), param->input.map_index, param->input.report_id, param->input.length);
						   //ESP_LOG_BUFFER_HEX(TAG, param->input.data, param->input.length);
						   xTaskNotify(t_yellow, LED_ONCE, eSetValueWithOverwrite);
			blue_h_input(param->input.dev, param->input.data, param->input.length);
			break;
					   }
		case ESP_HIDH_FEATURE_EVENT: {
			bda = esp_hidh_dev_bda_get(param->feature.dev);
							 ESP_LOGI(TAG, ESP_BD_ADDR_STR " FEATURE: %8s, MAP: %2u, ID: %3u, Len: %d", ESP_BD_ADDR_HEX(bda), esp_hid_usage_str(param->feature.usage), param->feature.map_index, param->feature.report_id, param->feature.length);
							 ESP_LOG_BUFFER_HEX(TAG, param->feature.data, param->feature.length);
							 break;
						 }
		case ESP_HIDH_CLOSE_EVENT: {
			blue_h_close(param);
			break;
		}
		default:
			ESP_LOGI(TAG, "Unknwown event: %d", event);
	}
}

void blue_h_close(esp_hidh_event_data_t *p) {
	const uint8_t *bda = NULL;

	configASSERT(p != NULL);
	bda = esp_hidh_dev_bda_get(p->close.dev);
	ESP_LOGI(TAG, "closed connection with device " ESP_BD_ADDR_STR, ESP_BD_ADDR_HEX(bda));
	esp_hidh_dev_free(p->close.dev);

	xTaskNotify(t_blue, LED_SLOW, eSetValueWithOverwrite);
}

static void	blue_handle_button(uint8_t buttons) {
	static bool	status = BLUE_BUTTON_N;	// Keep state

	buttons = buttons & (BLUE_BUTTON_1 | BLUE_BUTTON_2 | BLUE_BUTTON_3);

	if (status && buttons)
		return ;
	if (status == BLUE_BUTTON_N && buttons == BLUE_BUTTON_N)
		return ;

	/* release button */
	if (status && buttons == BLUE_BUTTON_N) {
		xTaskNotify(t_click, 0, eSetValueWithOverwrite);
		status = 0;
		ESP_LOGD(TAG, "button released");
		return ;
	}

	/* press button */
	if (status == BLUE_BUTTON_N && buttons) {
		xTaskNotify(t_click, 1, eSetValueWithOverwrite);
		status = 1;
		ESP_LOGD(TAG, "button pressed");
		return ;
	}

}

void blue_init(void)
{
	esp_err_t	ret;

	ret = nvs_flash_init();
	if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
		ESP_ERROR_CHECK(nvs_flash_erase());
		ret = nvs_flash_init();
	}

	ESP_LOGD(TAG, "Starting Bluetooth init on core %d", xPortGetCoreID());
	ESP_ERROR_CHECK(ret);
	/* ESP_BT_MODE_CLASSIC_BT doesn't work, it freezes esp_hidh_init */
	ESP_ERROR_CHECK(esp_hid_gap_init(ESP_BT_MODE_BTDM));
	ESP_ERROR_CHECK(esp_ble_gattc_register_callback(esp_hidh_gattc_event_handler));

	esp_hidh_config_t config = {
		.callback = blue_h_callback,
		.event_stack_size = 4096
	};

	ESP_ERROR_CHECK(esp_hidh_init(&config));
	esp_log_level_set("event", ESP_LOG_INFO);

	/* complains about wrong data len on BOOT mode and CCONTROL */
	esp_log_level_set("BT_HIDH", ESP_LOG_ERROR);

    /*
     * at this point, everything but bluetooth is started.
     * put green steady, start blinking blue and keep scanning until a device is found
     */

    xTaskNotify(t_green, LED_ON, eSetValueWithOverwrite);
    xTaskNotify(t_blue, LED_FAST, eSetValueWithOverwrite);
	xTaskCreatePinnedToCore(blue_scan, "blue_scan", 6 * 1024, NULL, 2, NULL, 0);
}

/*
 * data follows HID Mouse boot protocol format
 *
 * Byte | Bits | Description
 * -----+------+---------------------------------------------------------------
 * 0    | 0    | Button 1 (left)
 * 0    | 1    | Button 2 (right)
 * 0    | 2    | Button 3 (middle)
 * 0    | 3-7  | Device specific, usually unused (0)
 * 1    | 0-7  | X Displacement
 * 2    | 0-7  | Y Displacement
 * 3+   | 0-7  | Device specific, usually 3 is scroll wheel, usually unused (0)
 */

void blue_h_input(esp_hidh_dev_t *dev, uint8_t *data, uint16_t length) {
	uint8_t buttons;
	uint8_t	i;
	int8_t x, y;

	buttons = data[0];

	/*
	 * Do some checks before parsing data. We sould't get anything wrong in theory,
	 * but we sometimes to get shit from either mouse or bluetooth stack
	 *
	 * Stack issue: https://github.com/espressif/esp-idf/issues/6985
	 */

	if (length < 3) {
		xTaskNotify(t_red, LED_ONCE, eSetValueWithOverwrite);
		return;
	}
	if (buttons & BLUE_BUTTON_E) {
		xTaskNotify(t_red, LED_ONCE, eSetValueWithOverwrite);
		return;
	}

	/*
	 * we also want to ignore anything that contains scroll wheel & co for now
	 * statistically theses are corrupted packets
	 */

	for (i = 3; i < length; i++) {
		if (data[i]) {
			xTaskNotify(t_red, LED_ONCE, eSetValueWithOverwrite);
			return;
		}
	}

	blue_handle_button(buttons);

	/* quadrature use 7 bits x/y offsets, bluetooth BOOT uses 8 bits */
	x = data[1];
	y = data[2];

	if (x != 0) {
		if (x == 1 || x == -1)
			xTaskNotify(t_qx, x, eSetValueWithOverwrite);
		else
			xTaskNotify(t_qx, x / 2, eSetValueWithOverwrite);
	}
	if (y != 0) {
		if (y == 1 || y == -1)
			xTaskNotify(t_qy, y, eSetValueWithOverwrite);
		else
			xTaskNotify(t_qy, y / 2, eSetValueWithOverwrite);
	}
}

void blue_h_open(esp_hidh_event_data_t *p) {
    const uint8_t *bda = NULL;

	configASSERT(p != NULL);
    bda = esp_hidh_dev_bda_get(p->open.dev);
    ESP_LOGI(TAG, "opened connection with " ESP_BD_ADDR_STR, ESP_BD_ADDR_HEX(bda));

	/* Dump various info on console */
	blue_hid_rm_get(p->open.dev);
	blue_set_boot_protocol(p->open.dev);
	if (blue_support_boot(p->open.dev) == false) {
		xTaskNotify(t_red, LED_ONCE, eSetValueWithOverwrite);
		ESP_LOGE(TAG, "Mouse does not support boot protocol !");
	}

    xTaskNotify(t_blue, LED_ON, eSetValueWithOverwrite);
    gpio_output_enable();
}

/* get specific report from report map matching specified usage + type + protocol */
static esp_hid_report_item_t blue_h_ri_find(esp_hidh_dev_t *d, esp_hid_usage_t u, uint8_t t, uint8_t p) {
	size_t num_reports = 0;
	esp_hid_report_item_t *reports;
	esp_hid_report_item_t search;

	configASSERT(d != NULL);
	configASSERT(p <= ESP_HID_REPORT_TYPE_FEATURE);
	configASSERT(t > 0);

	esp_hidh_dev_reports_get(d, &num_reports, &reports);
	memset(&search, 0, sizeof(esp_hid_report_item_t));

	for (uint8_t i = 0; i < num_reports; i++) {
		if (reports[i].protocol_mode == p && reports[i].report_type == t &&	reports[i].usage == u) {
			memcpy(&search, &reports[i], sizeof(esp_hid_report_item_t));
			break;
		}
	}

	free(reports);
	if (search.value_len == 0) {
		ESP_LOGW(TAG, "%s %s %s not found",
				 esp_hid_usage_str(u),
				 esp_hid_report_type_str(t),
				 esp_hid_protocol_mode_str(p));
	}
	else {
		ESP_LOGD(TAG, "%s %s %s is id %i size %i byte(s)",
				 esp_hid_usage_str(u),
				 esp_hid_report_type_str(t),
				 esp_hid_protocol_mode_str(p),
				 search.report_id,
				 search.value_len);
	}

	return search;
}

/* get report map raw pointer. Also dump it on JTAG */
static esp_hid_raw_report_map_t *blue_hid_rm_get(esp_hidh_dev_t *dev) {
	size_t num_maps = 0;
	esp_hid_raw_report_map_t *maps;

	configASSERT(dev != NULL);
	ESP_LOGI(TAG, BLUE_SNIP);
	esp_hidh_dev_report_maps_get(dev, &num_maps, &maps);
	ESP_LOG_BUFFER_HEX(TAG, maps[0].data, maps[0].len);
	ESP_LOGI(TAG, BLUE_SNIP);

	/* looking at 4.2 SDK source, seems there is always only one map */
	configASSERT(num_maps == 1);
	return maps;
}

void blue_scan(void *pvParameters) {
    size_t len = 0;
    esp_hid_scan_result_t *mouse = NULL;
    esp_hid_scan_result_t *results = NULL;

    ESP_LOGI(TAG, "starting scan on core %d…", xPortGetCoreID());
    esp_hid_scan(BLUE_SCAN_DURATION, &len, &results);
    ESP_LOGI(TAG, "scan returned %u result(s)", len);
	xTaskNotify(t_blue, LED_SLOW, eSetValueWithOverwrite);

    if (len) {
        esp_hid_scan_result_t *r = results;

        while (r) {
            ESP_LOGI(TAG, "found %s %s device: " ESP_BD_ADDR_STR ", RSSI: %d, NAME: %s",
					 (r->transport == ESP_HID_TRANSPORT_BLE) ? "BLE" : "BT",
					 esp_hid_cod_major_str(r->bt.cod.major),
					 ESP_BD_ADDR_HEX(r->bda), r->rssi, r->name ? r->name : "");

			/* search for something that looks like Bluetooth Classic mouse */
            if (r->transport == ESP_HID_TRANSPORT_BT &&
				strcmp("PERIPHERAL", esp_hid_cod_major_str(r->bt.cod.major)) == 0
				&& (r->bt.cod.minor & ESP_HID_COD_MIN_MOUSE)) {
                    mouse = r;
            }
            r = r->next;
        }

        /* try to connect to the last candidate found */
        if (mouse)
            esp_hidh_dev_open(mouse->bda, mouse->transport, mouse->ble.addr_type);
        else
            ESP_LOGI(TAG, "devices found but no mouse detected");

        esp_hid_scan_results_free(results);
    }

    vTaskDelete(NULL);
}

static void blue_set_boot_protocol(esp_hidh_dev_t *dev) {
	configASSERT(dev != NULL);

	/*
	 * /!\ Disclaimer /!\
	 * This is ugly. We are accessing directly bluedroid and need the hidden handle to do that
	 * Extract it from a private esp_hidh_dev_s struct and call BTA_HhSetProtoMode directly.
	 */

	struct decoy_dev_s {
		struct esp_hidh_dev_s   *next;

		esp_hid_device_config_t config;
		esp_hid_usage_t         usage;
		esp_hid_transport_t     transport;
		bool                    connected;
		bool                    opened;
		int                     status;

		size_t                  reports_len;
		void                    *reports;

		void                    *tmp;
		size_t                  tmp_len;

		xSemaphoreHandle        semaphore;

		esp_err_t               (*close)        (esp_hidh_dev_t *dev);
		esp_err_t               (*report_write) (esp_hidh_dev_t *dev, size_t map_index, size_t report_id, int report_type, uint8_t *data, size_t len);
		esp_err_t               (*report_read)  (esp_hidh_dev_t *dev, size_t map_index, size_t report_id, int report_type, size_t max_length, uint8_t *value, size_t *value_len);
		void                    (*dump)         (esp_hidh_dev_t *dev, FILE *fp);

		esp_bd_addr_t bda;

		struct {
			esp_bt_cod_t cod;
			int handle;
			uint8_t sub_class;
			uint8_t app_id;
			uint16_t attr_mask;
		} bt;
		TAILQ_ENTRY(esp_hidh_dev_s) devices;
	};

	struct decoy_dev_s *pass_that_handle;
	pass_that_handle = (struct decoy_dev_s *)dev;

	ESP_LOGI(TAG, "switching " ESP_BD_ADDR_STR " (%i) to protocol mode boot" ,
			 ESP_BD_ADDR_HEX(pass_that_handle->bda), pass_that_handle->bt.handle);

	//ESP_LOG_BUFFER_HEX(TAG, dev, sizeof(struct decoy_dev_s));
	/* bluedroid/bta/include/bta_hh_api.h */
	BTA_HhSetProtoMode(pass_that_handle->bt.handle, 0x01);
}

static bool blue_support_boot(esp_hidh_dev_t *dev) {
	esp_hid_report_item_t	mib;

	configASSERT(dev != NULL);
	mib = blue_h_ri_find(dev, ESP_HID_USAGE_MOUSE, ESP_HID_REPORT_TYPE_INPUT, ESP_HID_PROTOCOL_MODE_BOOT);
	if (mib.value_len == 0)
		return false;
	return true;
}