blinky/src/cdc_wrapper_module.c

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

#include <app_event_manager.h>

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

#include <zephyr/logging/log.h>

#include "protocol_module.h"
#include "usb_cdc_rx_event.h"
#include "usb_cdc_tx_event.h"

LOG_MODULE_REGISTER(MODULE, LOG_LEVEL_INF);

#define CDC_WRAPPER_HEAD1 0xAAU
#define CDC_WRAPPER_HEAD2 0x55U
#define CDC_WRAPPER_MAX_PAYLOAD_LEN 64U
#define CDC_WRAPPER_MAX_FRAME_LEN (2U + 1U + 1U + CDC_WRAPPER_MAX_PAYLOAD_LEN + 1U)

enum frame_parse_state {
	FRAME_PARSE_HEAD1,
	FRAME_PARSE_HEAD2,
	FRAME_PARSE_LEN,
	FRAME_PARSE_TYPE,
	FRAME_PARSE_PAYLOAD,
	FRAME_PARSE_CHECKSUM,
};

struct cdc_frame_parser {
	enum frame_parse_state state;
	uint8_t len;
	uint8_t type;
	uint8_t checksum;
	uint8_t payload[CDC_WRAPPER_MAX_PAYLOAD_LEN];
	size_t payload_pos;
};

static bool initialized;
static bool running;
static struct cdc_frame_parser parser;

static void parser_reset(void)
{
	parser.state = FRAME_PARSE_HEAD1;
	parser.len = 0U;
	parser.type = 0U;
	parser.checksum = 0U;
	parser.payload_pos = 0U;
}

static uint8_t frame_checksum(uint8_t len, uint8_t type,
			      const uint8_t *payload, size_t payload_len)
{
	uint8_t checksum = CDC_WRAPPER_HEAD1 ^ CDC_WRAPPER_HEAD2 ^ len ^ type;

	for (size_t i = 0; i < payload_len; i++) {
		checksum ^= payload[i];
	}

	return checksum;
}

static void submit_tx_frame(uint8_t type, const uint8_t *payload, size_t payload_len)
{
	struct usb_cdc_tx_event *event;
	size_t frame_len = 2U + 1U + 1U + payload_len + 1U;

	event = new_usb_cdc_tx_event(frame_len);
	event->dyndata.data[0] = CDC_WRAPPER_HEAD1;
	event->dyndata.data[1] = CDC_WRAPPER_HEAD2;
	event->dyndata.data[2] = (uint8_t)payload_len;
	event->dyndata.data[3] = type;
	memcpy(&event->dyndata.data[4], payload, payload_len);
	event->dyndata.data[4U + payload_len] =
		frame_checksum((uint8_t)payload_len, type, payload, payload_len);
	APP_EVENT_SUBMIT(event);
}

static void process_complete_frame(void)
{
	uint8_t rsp_type;
	uint8_t rsp_payload[CDC_WRAPPER_MAX_PAYLOAD_LEN];
	size_t rsp_payload_len = 0U;
	int err;

	err = protocol_module_process_cdc_packet(parser.type,
						 parser.payload,
						 parser.payload_pos,
						 &rsp_type,
						 rsp_payload,
						 sizeof(rsp_payload),
						 &rsp_payload_len);
	if (err == -ENOTSUP) {
		LOG_WRN("Ignore unsupported CDC packet type:0x%02x", parser.type);
		return;
	}

	if (err) {
		LOG_WRN("Protocol processing failed (%d)", err);
		return;
	}

	LOG_INF("CDC HelloRsp encoded len:%u", (uint32_t)rsp_payload_len);
	submit_tx_frame(rsp_type, rsp_payload, rsp_payload_len);
}

static void consume_byte(uint8_t byte)
{
	switch (parser.state) {
	case FRAME_PARSE_HEAD1:
		if (byte == CDC_WRAPPER_HEAD1) {
			parser.state = FRAME_PARSE_HEAD2;
		}
		break;

	case FRAME_PARSE_HEAD2:
		if (byte == CDC_WRAPPER_HEAD2) {
			parser.state = FRAME_PARSE_LEN;
		} else if (byte != CDC_WRAPPER_HEAD1) {
			parser.state = FRAME_PARSE_HEAD1;
		}
		break;

	case FRAME_PARSE_LEN:
		if (byte > CDC_WRAPPER_MAX_PAYLOAD_LEN) {
			LOG_WRN("Drop CDC frame with invalid len:%u", byte);
			parser_reset();
			break;
		}

		parser.len = byte;
		parser.payload_pos = 0U;
		parser.state = FRAME_PARSE_TYPE;
		break;

	case FRAME_PARSE_TYPE:
		parser.type = byte;
		parser.state = (parser.len == 0U) ? FRAME_PARSE_CHECKSUM :
						 FRAME_PARSE_PAYLOAD;
		break;

	case FRAME_PARSE_PAYLOAD:
		parser.payload[parser.payload_pos++] = byte;
		if (parser.payload_pos >= parser.len) {
			parser.state = FRAME_PARSE_CHECKSUM;
		}
		break;

	case FRAME_PARSE_CHECKSUM:
		if (byte != frame_checksum(parser.len, parser.type,
					       parser.payload, parser.payload_pos)) {
			LOG_WRN("Drop CDC frame with invalid checksum");
			parser_reset();
			break;
		}

		process_complete_frame();
		parser_reset();
		break;

	default:
		parser_reset();
		break;
	}
}

static bool handle_usb_cdc_rx_event(const struct usb_cdc_rx_event *event)
{
	if (!running) {
		return false;
	}

	for (size_t i = 0; i < event->dyndata.size; i++) {
		consume_byte(event->dyndata.data[i]);
	}

	return false;
}

static int module_init(void)
{
	parser_reset();
	return 0;
}

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

	running = true;
	return 0;
}

static void module_pause(void)
{
	if (!running) {
		return;
	}

	running = false;
	parser_reset();
}

static bool app_event_handler(const struct app_event_header *aeh)
{
	if (is_usb_cdc_rx_event(aeh)) {
		return handle_usb_cdc_rx_event(cast_usb_cdc_rx_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)) {
			int err;

			if (!initialized) {
				err = module_init();
				if (err) {
					module_set_state(MODULE_STATE_ERROR);
					return false;
				}

				initialized = true;
			}

			err = module_start();
			if (err) {
				module_set_state(MODULE_STATE_ERROR);
			} else {
				module_set_state(MODULE_STATE_READY);
			}
		}

		return false;
	}

	if (is_power_down_event(aeh)) {
		if (initialized) {
			module_pause();
			module_set_state(MODULE_STATE_STANDBY);
		}

		return false;
	}

	if (is_wake_up_event(aeh)) {
		if (initialized) {
			int err = module_start();

			if (err) {
				module_set_state(MODULE_STATE_ERROR);
			} else {
				module_set_state(MODULE_STATE_READY);
			}
		}

		return false;
	}

	return false;
}

APP_EVENT_LISTENER(MODULE, app_event_handler);
APP_EVENT_SUBSCRIBE(MODULE, module_state_event);
APP_EVENT_SUBSCRIBE(MODULE, usb_cdc_rx_event);
APP_EVENT_SUBSCRIBE_EARLY(MODULE, power_down_event);
APP_EVENT_SUBSCRIBE(MODULE, wake_up_event);