blinky/src/keyboard_core_module.c

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include <app_event_manager.h>

#define MODULE keyboard_core_module
#include <caf/events/button_event.h>
#include <caf/events/module_state_event.h>
#include <caf/events/power_event.h>

#include <caf/key_id.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/util.h>

#include "encoder_event.h"
#include "function_bitmap_state_event.h"
#include "function_bitmap_update_event.h"
#include "keyboard_core.h"
#include "keyboard_hid_report_event.h"
#include "module_lifecycle.h"
#include "mode_switch_event.h"
#include "set_protocol_event.h"

LOG_MODULE_REGISTER(MODULE, LOG_LEVEL_INF);

#define KEYBOARD_USAGE_FIRST_MODIFIER 0xE0U
#define KEYBOARD_USAGE_LAST_MODIFIER 0xE7U
#define KEYBOARD_USAGE_ERROR_ROLLOVER 0x01U
#define KEYBOARD_BOOT_RESERVED_BYTE 0x00U

enum key_usage_type {
	KEY_USAGE_TYPE_KEYBOARD,
	KEY_USAGE_TYPE_CONSUMER,
};

struct keymap_entry {
	uint16_t key_id;
	uint8_t usage_type;
	uint16_t usage_id;
};

struct keyboard_state {
	uint8_t pressed_usage_bitmap[KEYBOARD_PROTOCOL_BITMAP_BYTES];
	uint8_t function_pressed_bitmap[KEYBOARD_PROTOCOL_BITMAP_BYTES];
	uint32_t consumer_bits;
};

struct keyboard_reports_cache {
	uint8_t boot_report[KEYBOARD_BOOT_REPORT_SIZE];
	uint8_t nkro_report[KEYBOARD_NKRO_REPORT_SIZE];
	uint8_t consumer_report[KEYBOARD_CONSUMER_REPORT_SIZE];
	bool boot_valid;
	bool nkro_valid;
	bool consumer_valid;
};

static const struct keymap_entry keymap[] = {
	{ KEY_ID(0, 1), KEY_USAGE_TYPE_KEYBOARD, 0x0053 }, /* num lock */
	{ KEY_ID(0, 2), KEY_USAGE_TYPE_KEYBOARD, 0x005F }, /* keypad 7 */
	{ KEY_ID(0, 3), KEY_USAGE_TYPE_KEYBOARD, 0x005C }, /* keypad 4 */
	{ KEY_ID(0, 4), KEY_USAGE_TYPE_KEYBOARD, 0x0059 }, /* keypad 1 */
	{ KEY_ID(0, 5), KEY_USAGE_TYPE_KEYBOARD, 0x0062 }, /* keypad 0 */
	{ KEY_ID(1, 1), KEY_USAGE_TYPE_KEYBOARD, 0x0054 }, /* keypad / */
	{ KEY_ID(1, 2), KEY_USAGE_TYPE_KEYBOARD, 0x0060 }, /* keypad 8 */
	{ KEY_ID(1, 3), KEY_USAGE_TYPE_KEYBOARD, 0x005D }, /* keypad 5 */
	{ KEY_ID(1, 4), KEY_USAGE_TYPE_KEYBOARD, 0x005A }, /* keypad 2 */
	{ KEY_ID(1, 5), KEY_USAGE_TYPE_KEYBOARD, 0x0063 }, /* keypad . */
	{ KEY_ID(2, 1), KEY_USAGE_TYPE_KEYBOARD, 0x0055 }, /* keypad * */
	{ KEY_ID(2, 2), KEY_USAGE_TYPE_KEYBOARD, 0x0061 }, /* keypad 9 */
	{ KEY_ID(2, 3), KEY_USAGE_TYPE_KEYBOARD, 0x005E }, /* keypad 6 */
	{ KEY_ID(2, 4), KEY_USAGE_TYPE_KEYBOARD, 0x005B }, /* keypad 3 */
	{ KEY_ID(3, 0), KEY_USAGE_TYPE_CONSUMER, KEYBOARD_CONSUMER_CTRL_MUTE },
	{ KEY_ID(3, 1), KEY_USAGE_TYPE_KEYBOARD, 0x0056 }, /* keypad - */
	{ KEY_ID(3, 3), KEY_USAGE_TYPE_KEYBOARD, 0x0057 }, /* keypad + */
	{ KEY_ID(3, 5), KEY_USAGE_TYPE_KEYBOARD, 0x0058 }, /* keypad enter */
};

static const uint16_t consumer_usage_map[KEYBOARD_CONSUMER_CTRL_COUNT] = {
	[KEYBOARD_CONSUMER_CTRL_MUTE] = 0x00E2,
	[KEYBOARD_CONSUMER_CTRL_VOLUME_UP] = 0x00E9,
	[KEYBOARD_CONSUMER_CTRL_VOLUME_DOWN] = 0x00EA,
	[KEYBOARD_CONSUMER_CTRL_PLAY_PAUSE] = 0x00CD,
	[KEYBOARD_CONSUMER_CTRL_NEXT_TRACK] = 0x00B5,
	[KEYBOARD_CONSUMER_CTRL_PREV_TRACK] = 0x00B6,
};

struct keyboard_core_module_ctx {
	struct module_lifecycle_ctx lc;
	struct keyboard_state keyboard_state;
	struct keyboard_reports_cache reports_cache;
	uint8_t function_usage_mask[KEYBOARD_PROTOCOL_BITMAP_BYTES];
	enum keyboard_protocol_mode transport_protocol_modes[HID_TRANSPORT_COUNT];
	enum mode_switch_mode current_mode;
	bool mode_valid;
};

static int do_init(void);
static int do_start(void);
static int do_stop(void);

static const struct module_lifecycle_cfg lifecycle_cfg = {
	.mode = ML_MODE_POWER,
	.stopped_state = MODULE_STATE_STANDBY,
};

static const struct module_lifecycle_ops lifecycle_ops = {
	.do_init = do_init,
	.do_start = do_start,
	.do_stop = do_stop,
};

static struct keyboard_core_module_ctx ctx = {
	.lc = {
		.state = LC_UNINIT,
		.cfg = &lifecycle_cfg,
		.ops = &lifecycle_ops,
	},
	.transport_protocol_modes = {
		[HID_TRANSPORT_USB] = KEYBOARD_PROTOCOL_MODE_REPORT,
		[HID_TRANSPORT_BLE] = KEYBOARD_PROTOCOL_MODE_REPORT,
	},
};

#define lifecycle ctx.lc
#define keyboard_state ctx.keyboard_state
#define reports_cache ctx.reports_cache
#define function_usage_mask ctx.function_usage_mask
#define transport_protocol_modes ctx.transport_protocol_modes
#define current_mode ctx.current_mode
#define mode_valid ctx.mode_valid
#define running module_lifecycle_is_running(&ctx.lc)

static bool mode_to_transport(enum mode_switch_mode mode, enum hid_transport *transport)
{
	switch (mode) {
	case MODE_SWITCH_USB:
		*transport = HID_TRANSPORT_USB;
		return true;

	case MODE_SWITCH_BLE:
		*transport = HID_TRANSPORT_BLE;
		return true;

	default:
		return false;
	}
}

static enum keyboard_protocol_mode active_protocol_mode_get(void)
{
	enum hid_transport transport;

	if (mode_valid && mode_to_transport(current_mode, &transport)) {
		return transport_protocol_modes[transport];
	}

	return KEYBOARD_PROTOCOL_MODE_REPORT;
}

static const struct keymap_entry *keymap_get(uint16_t key_id)
{
	size_t left = 0;
	size_t right = ARRAY_SIZE(keymap);

	while (left < right) {
		size_t mid = left + ((right - left) / 2U);

		if (keymap[mid].key_id == key_id) {
			return &keymap[mid];
		}

		if (keymap[mid].key_id < key_id) {
			left = mid + 1U;
		} else {
			right = mid;
		}
	}

	return NULL;
}

static bool usage_to_bitmap_pos(uint16_t usage_id, uint8_t *byte_idx,
				uint8_t *bit_idx)
{
	if ((byte_idx == NULL) || (bit_idx == NULL)) {
		return false;
	}

	if ((usage_id >= KEYBOARD_USAGE_FIRST_MODIFIER) &&
	    (usage_id <= KEYBOARD_USAGE_LAST_MODIFIER)) {
		*byte_idx = 0U;
		*bit_idx = (uint8_t)(usage_id - KEYBOARD_USAGE_FIRST_MODIFIER);
		return true;
	}

	if (usage_id <= KEYBOARD_NKRO_USAGE_MAX) {
		*byte_idx = (uint8_t)(1U + (usage_id / 8U));
		*bit_idx = (uint8_t)(usage_id % 8U);
		return true;
	}

	return false;
}

static bool usage_bitmap_test(const uint8_t *bitmap, uint16_t usage_id)
{
	uint8_t byte_idx;
	uint8_t bit_idx;

	if ((bitmap == NULL) ||
	    !usage_to_bitmap_pos(usage_id, &byte_idx, &bit_idx)) {
		return false;
	}

	return (bitmap[byte_idx] & BIT(bit_idx)) != 0U;
}

static bool usage_bitmap_write(uint8_t *bitmap, uint16_t usage_id, bool pressed)
{
	uint8_t byte_idx;
	uint8_t bit_idx;
	bool was_pressed;

	if ((bitmap == NULL) ||
	    !usage_to_bitmap_pos(usage_id, &byte_idx, &bit_idx)) {
		LOG_WRN("Unsupported keyboard usage 0x%04x", usage_id);
		return false;
	}

	was_pressed = (bitmap[byte_idx] & BIT(bit_idx)) != 0U;

	if (was_pressed == pressed) {
		return false;
	}

	WRITE_BIT(bitmap[byte_idx], bit_idx, pressed);
	return true;
}

static bool consumer_key_update(uint16_t consumer_id, bool pressed)
{
	if (consumer_id >= KEYBOARD_CONSUMER_CTRL_COUNT) {
		LOG_WRN("Unsupported consumer id %u", consumer_id);
		return false;
	}

	bool was_pressed = (keyboard_state.consumer_bits & BIT(consumer_id)) != 0U;

	if (was_pressed == pressed) {
		return false;
	}

	WRITE_BIT(keyboard_state.consumer_bits, consumer_id, pressed);
	return true;
}

static void keyboard_state_clear(void)
{
	memset(&keyboard_state, 0, sizeof(keyboard_state));
}

static void function_usage_mask_clear(void)
{
	memset(function_usage_mask, 0, sizeof(function_usage_mask));
}

static void reports_cache_invalidate(void)
{
	reports_cache.boot_valid = false;
	reports_cache.nkro_valid = false;
	reports_cache.consumer_valid = false;
}

static void build_effective_hid_bitmap(uint8_t bitmap[KEYBOARD_PROTOCOL_BITMAP_BYTES])
{
	for (size_t i = 0; i < KEYBOARD_PROTOCOL_BITMAP_BYTES; i++) {
		bitmap[i] = keyboard_state.pressed_usage_bitmap[i] &
			    (uint8_t)~keyboard_state.function_pressed_bitmap[i];
	}
}

static void build_boot_report(uint8_t report[KEYBOARD_BOOT_REPORT_SIZE])
{
	uint8_t effective_hid_bitmap[KEYBOARD_PROTOCOL_BITMAP_BYTES];
	size_t key_count = 0;

	build_effective_hid_bitmap(effective_hid_bitmap);
	memset(report, 0, KEYBOARD_BOOT_REPORT_SIZE);
	report[0] = effective_hid_bitmap[0];
	report[1] = KEYBOARD_BOOT_RESERVED_BYTE;

	for (uint16_t usage_id = 0; usage_id <= KEYBOARD_NKRO_USAGE_MAX; usage_id++) {
		uint8_t byte_idx;
		uint8_t bit_idx;

		(void)usage_to_bitmap_pos(usage_id, &byte_idx, &bit_idx);

		if ((effective_hid_bitmap[byte_idx] & BIT(bit_idx)) == 0U) {
			continue;
		}

		if (key_count == (KEYBOARD_BOOT_REPORT_SIZE - 2U)) {
			memset(&report[2], KEYBOARD_USAGE_ERROR_ROLLOVER,
			       KEYBOARD_BOOT_REPORT_SIZE - 2U);
			return;
		}

		report[2U + key_count] = (uint8_t)usage_id;
		key_count++;
	}
}

static void build_nkro_report(uint8_t report[KEYBOARD_NKRO_REPORT_SIZE])
{
	uint8_t effective_hid_bitmap[KEYBOARD_PROTOCOL_BITMAP_BYTES];

	build_effective_hid_bitmap(effective_hid_bitmap);
	report[0] = effective_hid_bitmap[0];
	memcpy(&report[1], &effective_hid_bitmap[1], KEYBOARD_NKRO_BITMAP_BYTES);
}

static uint16_t active_consumer_usage_get(void)
{
	for (uint8_t consumer_id = 0; consumer_id < KEYBOARD_CONSUMER_CTRL_COUNT; consumer_id++) {
		if ((keyboard_state.consumer_bits & BIT(consumer_id)) != 0U) {
			return consumer_usage_map[consumer_id];
		}
	}

	return 0U;
}

static void build_consumer_report(uint8_t report[KEYBOARD_CONSUMER_REPORT_SIZE])
{
	sys_put_le16(active_consumer_usage_get(), report);
}

static void submit_consumer_fifo_frame(uint16_t usage_id)
{
	uint8_t report_buf[KEYBOARD_CONSUMER_REPORT_SIZE];
	enum keyboard_protocol_mode protocol_mode = active_protocol_mode_get();

	if (!running || !mode_valid ||
	    (protocol_mode == KEYBOARD_PROTOCOL_MODE_BOOT)) {
		return;
	}

	sys_put_le16(usage_id, report_buf);
	(void)submit_keyboard_hid_report_event(
		current_mode, KEYBOARD_REPORT_TYPE_CONSUMER, protocol_mode,
		HID_QUEUE_POLICY_FIFO, report_buf, KEYBOARD_CONSUMER_REPORT_SIZE);
}

static void submit_consumer_pulse_frames(enum keyboard_consumer_control control_id,
					 uint8_t pulse_count)
{
	uint16_t usage_id;

	if (active_protocol_mode_get() == KEYBOARD_PROTOCOL_MODE_BOOT) {
		return;
	}

	if (control_id >= KEYBOARD_CONSUMER_CTRL_COUNT) {
		LOG_WRN("Unsupported consumer control id %u", control_id);
		return;
	}

	usage_id = consumer_usage_map[control_id];
	if (usage_id == 0U) {
		LOG_WRN("Unmapped consumer control id %u", control_id);
		return;
	}

	for (uint8_t i = 0; i < pulse_count; i++) {
		submit_consumer_fifo_frame(usage_id);
		submit_consumer_fifo_frame(0U);
	}
}

static void emit_keys_report(bool force)
{
	uint8_t report_buf[KEYBOARD_NKRO_REPORT_SIZE];
	uint8_t report_size;
	uint8_t *cache_buf;
	bool *cache_valid;
	enum keyboard_protocol_mode protocol_mode = active_protocol_mode_get();

	if (!mode_valid) {
		return;
	}

	if (protocol_mode == KEYBOARD_PROTOCOL_MODE_BOOT) {
		build_boot_report(report_buf);
		report_size = KEYBOARD_BOOT_REPORT_SIZE;
		cache_buf = reports_cache.boot_report;
		cache_valid = &reports_cache.boot_valid;
	} else {
		build_nkro_report(report_buf);
		report_size = KEYBOARD_NKRO_REPORT_SIZE;
		cache_buf = reports_cache.nkro_report;
		cache_valid = &reports_cache.nkro_valid;
	}

	if (!force && *cache_valid && (memcmp(cache_buf, report_buf, report_size) == 0)) {
		return;
	}

	memcpy(cache_buf, report_buf, report_size);
	*cache_valid = true;

	(void)submit_keyboard_hid_report_event(
		current_mode, KEYBOARD_REPORT_TYPE_KEYS, protocol_mode,
		HID_QUEUE_POLICY_LATEST, report_buf, report_size);
}

static void emit_consumer_report(bool force)
{
	uint8_t report_buf[KEYBOARD_CONSUMER_REPORT_SIZE];
	enum keyboard_protocol_mode protocol_mode = active_protocol_mode_get();

	if (!mode_valid || (protocol_mode == KEYBOARD_PROTOCOL_MODE_BOOT)) {
		return;
	}

	build_consumer_report(report_buf);
	if (!force && reports_cache.consumer_valid &&
	    (memcmp(reports_cache.consumer_report, report_buf,
		    KEYBOARD_CONSUMER_REPORT_SIZE) == 0)) {
		return;
	}

	memcpy(reports_cache.consumer_report, report_buf, KEYBOARD_CONSUMER_REPORT_SIZE);
	reports_cache.consumer_valid = true;

	(void)submit_keyboard_hid_report_event(
		current_mode, KEYBOARD_REPORT_TYPE_CONSUMER, protocol_mode,
		HID_QUEUE_POLICY_LATEST, report_buf,
		KEYBOARD_CONSUMER_REPORT_SIZE);
}

static void emit_all_reports(bool force)
{
	emit_keys_report(force);

	if (active_protocol_mode_get() != KEYBOARD_PROTOCOL_MODE_BOOT) {
		emit_consumer_report(force);
	}
}

static void emit_function_state_event(void)
{
	(void)submit_function_bitmap_state_event(keyboard_state.pressed_usage_bitmap);
}

static void emit_release_reports(enum mode_switch_mode mode)
{
	uint8_t keys_report[KEYBOARD_NKRO_REPORT_SIZE] = { 0 };
	uint8_t consumer_report[KEYBOARD_CONSUMER_REPORT_SIZE] = { 0 };
	enum keyboard_protocol_mode protocol_mode = active_protocol_mode_get();
	size_t keys_report_size =
		(protocol_mode == KEYBOARD_PROTOCOL_MODE_BOOT) ?
			KEYBOARD_BOOT_REPORT_SIZE : KEYBOARD_NKRO_REPORT_SIZE;

	(void)submit_keyboard_hid_report_event(
		mode, KEYBOARD_REPORT_TYPE_KEYS, protocol_mode,
		HID_QUEUE_POLICY_LATEST, keys_report, keys_report_size);

	if (protocol_mode != KEYBOARD_PROTOCOL_MODE_BOOT) {
		(void)submit_keyboard_hid_report_event(
			mode, KEYBOARD_REPORT_TYPE_CONSUMER, protocol_mode,
			HID_QUEUE_POLICY_LATEST, consumer_report,
			KEYBOARD_CONSUMER_REPORT_SIZE);
	}
}

static int do_init(void)
{
	keyboard_state_clear();
	reports_cache_invalidate();
	function_usage_mask_clear();
	mode_valid = false;
	transport_protocol_modes[HID_TRANSPORT_USB] =
		KEYBOARD_PROTOCOL_MODE_REPORT;
	transport_protocol_modes[HID_TRANSPORT_BLE] =
		KEYBOARD_PROTOCOL_MODE_REPORT;

	return 0;
}

static int do_start(void)
{
	if (running) {
		return 0;
	}

	return 0;
}

static int do_stop(void)
{
	if (!running) {
		return 0;
	}

	if (mode_valid) {
		emit_release_reports(current_mode);
	}
	emit_function_state_event();

	keyboard_state_clear();
	reports_cache_invalidate();
	mode_valid = false;

	return 0;
}

static bool handle_button_event(const struct button_event *event)
{
	const struct keymap_entry *entry;
	bool changed;

	if (!running) {
		return false;
	}

	entry = keymap_get(event->key_id);
	if (!entry) {
		LOG_WRN("Unmapped key id 0x%04x", event->key_id);
		return false;
	}

	if (entry->usage_type == KEY_USAGE_TYPE_KEYBOARD) {
		bool routed_to_function;

		changed = usage_bitmap_write(keyboard_state.pressed_usage_bitmap,
						entry->usage_id, event->pressed);
		if (!changed) {
			return false;
		}

		if (event->pressed) {
			routed_to_function =
				usage_bitmap_test(function_usage_mask, entry->usage_id);
			(void)usage_bitmap_write(keyboard_state.function_pressed_bitmap,
						 entry->usage_id, routed_to_function);
		} else {
			routed_to_function =
				usage_bitmap_test(keyboard_state.function_pressed_bitmap,
						  entry->usage_id);
			(void)usage_bitmap_write(keyboard_state.function_pressed_bitmap,
						 entry->usage_id, false);
		}

		if (routed_to_function) {
			emit_function_state_event();
		} else {
			emit_keys_report(false);
		}
	} else {
		changed = consumer_key_update(entry->usage_id, event->pressed);
		if (changed) {
			emit_consumer_report(false);
		}
	}

	return false;
}

static bool handle_mode_switch_event(const struct mode_switch_event *event)
{
	bool mode_changed;

	if (!running) {
		current_mode = event->mode;
		return false;
	}

	mode_changed = mode_valid && (current_mode != event->mode);
	if (mode_changed) {
		emit_release_reports(current_mode);
		emit_function_state_event();
		keyboard_state_clear();
		reports_cache_invalidate();
	}

	current_mode = event->mode;
	mode_valid = true;
	emit_all_reports(true);

	return false;
}

static bool handle_encoder_event(const struct encoder_event *event)
{
	if (!running || !mode_valid) {
		return false;
	}

	if (event->detents > 0) {
		submit_consumer_pulse_frames(KEYBOARD_CONSUMER_CTRL_VOLUME_UP,
					     (uint8_t)event->detents);
	} else if (event->detents < 0) {
		submit_consumer_pulse_frames(KEYBOARD_CONSUMER_CTRL_VOLUME_DOWN,
					     (uint8_t)(-event->detents));
	}

	return false;
}

static bool handle_function_bitmap_update_event(
	const struct function_bitmap_update_event *event)
{
	memcpy(function_usage_mask, event->bitmap, sizeof(function_usage_mask));
	return false;
}

static bool app_event_handler(const struct app_event_header *aeh)
{
	if (is_button_event(aeh)) {
		return handle_button_event(cast_button_event(aeh));
	}

	if (is_encoder_event(aeh)) {
		return handle_encoder_event(cast_encoder_event(aeh));
	}

	if (is_function_bitmap_update_event(aeh)) {
		return handle_function_bitmap_update_event(
			cast_function_bitmap_update_event(aeh));
	}

	if (is_set_protocol_event(aeh)) {
		const struct set_protocol_event *event = cast_set_protocol_event(aeh);
		enum hid_transport active_transport;

		if (event->transport >= HID_TRANSPORT_COUNT) {
			return false;
		}

		if (transport_protocol_modes[event->transport] != event->protocol_mode) {
			transport_protocol_modes[event->transport] = event->protocol_mode;

			if (running && mode_valid &&
			    mode_to_transport(current_mode, &active_transport) &&
			    (active_transport == event->transport)) {
				reports_cache_invalidate();
				emit_keys_report(true);

				if (event->protocol_mode != KEYBOARD_PROTOCOL_MODE_BOOT) {
					emit_consumer_report(true);
				}
			}
		}

		return false;
	}

	if (is_mode_switch_event(aeh)) {
		return handle_mode_switch_event(cast_mode_switch_event(aeh));
	}

	if (is_module_state_event(aeh)) {
		const struct module_state_event *event = cast_module_state_event(aeh);

		if (check_state(event, MODULE_ID(main), MODULE_STATE_READY)) {
			(void)module_set_lifecycle(&lifecycle, LC_RUNNING);
		}

		return false;
	}

	if (is_power_down_event(aeh)) {
		if (module_lifecycle_is_initialized(&lifecycle)) {
			(void)module_set_lifecycle(&lifecycle, LC_STOPPED);
		}

		return false;
	}

	if (is_wake_up_event(aeh)) {
		if (module_lifecycle_is_initialized(&lifecycle)) {
			(void)module_set_lifecycle(&lifecycle, LC_RUNNING);
		}

		return false;
	}

	__ASSERT_NO_MSG(false);
	return false;
}

APP_EVENT_LISTENER(MODULE, app_event_handler);
APP_EVENT_SUBSCRIBE(MODULE, button_event);
APP_EVENT_SUBSCRIBE(MODULE, encoder_event);
APP_EVENT_SUBSCRIBE(MODULE, function_bitmap_update_event);
APP_EVENT_SUBSCRIBE(MODULE, set_protocol_event);
APP_EVENT_SUBSCRIBE(MODULE, mode_switch_event);
APP_EVENT_SUBSCRIBE(MODULE, module_state_event);
APP_EVENT_SUBSCRIBE_EARLY(MODULE, power_down_event);
APP_EVENT_SUBSCRIBE(MODULE, wake_up_event);