blinky/src/protocol_module.c

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

#include <app_event_manager.h>

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

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

#include <pb_decode.h>
#include <pb_encode.h>

#include <proto/device_comm.pb.h>

#include "proto_rx_event.h"
#include "proto_tx_event.h"
#include "protocol_module.h"

LOG_MODULE_REGISTER(MODULE, LOG_LEVEL_INF);

#define PROTOCOL_VERSION 1U
#define PROTOCOL_VENDOR_ID 0x1915U
#define PROTOCOL_PRODUCT_ID 0x52F0U
#define PROTOCOL_FIRMWARE_MAJOR 0U
#define PROTOCOL_FIRMWARE_MINOR 0U
#define PROTOCOL_CAPABILITY_FLAGS 0U
#define PROTOCOL_MAX_MSG_LEN 128U

static bool initialized;
static bool running;
static bool hello_done[PROTO_TRANSPORT_COUNT];

static int decode_body(const uint8_t *payload, size_t payload_len,
		       CdcPacketBody *body)
{
	pb_istream_t stream;

	if ((payload == NULL) || (body == NULL)) {
		return -EINVAL;
	}

	*body = (CdcPacketBody)CdcPacketBody_init_zero;
	stream = pb_istream_from_buffer(payload, payload_len);

	if (!pb_decode(&stream, CdcPacketBody_fields, body)) {
		LOG_WRN("pb_decode failed: %s", PB_GET_ERROR(&stream));
		return -EBADMSG;
	}

	return 0;
}

static int encode_body(const CdcPacketBody *body, uint8_t *payload,
		       size_t payload_buf_size, size_t *payload_len)
{
	pb_ostream_t stream;

	if ((body == NULL) || (payload == NULL) || (payload_len == NULL)) {
		return -EINVAL;
	}

	stream = pb_ostream_from_buffer(payload, payload_buf_size);
	if (!pb_encode(&stream, CdcPacketBody_fields, body)) {
		LOG_WRN("pb_encode failed: %s", PB_GET_ERROR(&stream));
		return -EIO;
	}

	*payload_len = stream.bytes_written;
	return 0;
}

static int encode_hello_rsp(uint8_t *rsp_payload, size_t rsp_payload_buf_size,
			    size_t *rsp_payload_len)
{
	CdcPacketBody body = CdcPacketBody_init_zero;

	body.which_body = CdcPacketBody_hello_rsp_tag;
	body.body.hello_rsp.protocol_version = PROTOCOL_VERSION;
	body.body.hello_rsp.vendor_id = PROTOCOL_VENDOR_ID;
	body.body.hello_rsp.product_id = PROTOCOL_PRODUCT_ID;
	body.body.hello_rsp.firmware_major = PROTOCOL_FIRMWARE_MAJOR;
	body.body.hello_rsp.firmware_minor = PROTOCOL_FIRMWARE_MINOR;
	body.body.hello_rsp.capability_flags = PROTOCOL_CAPABILITY_FLAGS;

	return encode_body(&body, rsp_payload, rsp_payload_buf_size, rsp_payload_len);
}

static int module_init(void)
{
	for (size_t i = 0; i < ARRAY_SIZE(hello_done); i++) {
		hello_done[i] = false;
	}

	return 0;
}

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

	running = true;
	return 0;
}

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

	for (size_t i = 0; i < ARRAY_SIZE(hello_done); i++) {
		hello_done[i] = false;
	}

	running = false;
}

void protocol_module_reset_transport_state(enum proto_transport transport)
{
	if (transport >= PROTO_TRANSPORT_COUNT) {
		return;
	}

	hello_done[transport] = false;
}

int protocol_module_process_message(enum proto_transport transport,
				    const uint8_t *req_payload,
				    size_t req_payload_len,
				    uint8_t *rsp_payload,
				    size_t rsp_payload_buf_size,
				    size_t *rsp_payload_len)
{
	CdcPacketBody body;
	int err;

	if ((transport >= PROTO_TRANSPORT_COUNT) ||
	    (rsp_payload == NULL) || (rsp_payload_len == NULL)) {
		return -EINVAL;
	}

	if (!running) {
		return -EAGAIN;
	}

	err = decode_body(req_payload, req_payload_len, &body);
	if (err) {
		return err;
	}

	if (body.which_body != CdcPacketBody_hello_req_tag) {
		LOG_WRN("Unsupported protobuf body case %d", body.which_body);
		return -ENOTSUP;
	}

	LOG_INF("HelloReq transport:%u protocol_version:%u",
		transport, body.body.hello_req.protocol_version);

	if (body.body.hello_req.protocol_version != PROTOCOL_VERSION) {
		LOG_WRN("Unexpected protocol version:%u",
			body.body.hello_req.protocol_version);
	}

	hello_done[transport] = true;
	return encode_hello_rsp(rsp_payload, rsp_payload_buf_size, rsp_payload_len);
}

static bool handle_proto_rx_event(const struct proto_rx_event *event)
{
	uint8_t rsp_payload[PROTOCOL_MAX_MSG_LEN];
	size_t rsp_payload_len = 0U;
	int err;

	if (!running) {
		return false;
	}

	err = protocol_module_process_message(event->transport,
					      event->dyndata.data,
					      event->dyndata.size,
					      rsp_payload,
					      sizeof(rsp_payload),
					      &rsp_payload_len);
	if (err) {
		if (err != -ENOTSUP) {
			LOG_WRN("Protocol processing failed (%d)", err);
		}

		return false;
	}

	err = submit_proto_tx_event(event->transport, rsp_payload, rsp_payload_len);
	if (err) {
		LOG_WRN("Proto TX submit failed (%d)", err);
	}

	return false;
}

static bool app_event_handler(const struct app_event_header *aeh)
{
	if (is_proto_rx_event(aeh)) {
		return handle_proto_rx_event(cast_proto_rx_event(aeh));
	}

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

		if (check_state(event, MODULE_ID(main), MODULE_STATE_READY)) {
			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;
		}

		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, proto_rx_event);
APP_EVENT_SUBSCRIBE_EARLY(MODULE, power_down_event);
APP_EVENT_SUBSCRIBE(MODULE, wake_up_event);