blinky/src/protocol_module.c

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

#include <zephyr/logging/log.h>

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

#include <proto/device_comm.pb.h>

#include "protocol_module.h"

LOG_MODULE_REGISTER(protocol_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

static bool type_matches_body(uint8_t type, const CdcPacketBody *body)
{
	switch (type) {
	case CDC_PROTO_TYPE_HELLO_REQ:
		return body->which_body == CdcPacketBody_hello_req_tag;
	case CDC_PROTO_TYPE_HELLO_RSP:
		return body->which_body == CdcPacketBody_hello_rsp_tag;
	default:
		return false;
	}
}

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_hello_rsp(uint8_t *rsp_payload, size_t rsp_payload_buf_size,
			    size_t *rsp_payload_len)
{
	CdcPacketBody body = CdcPacketBody_init_zero;
	pb_ostream_t stream;

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

	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;

	stream = pb_ostream_from_buffer(rsp_payload, rsp_payload_buf_size);

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

	*rsp_payload_len = stream.bytes_written;
	return 0;
}

int protocol_module_process_cdc_packet(uint8_t req_type,
				       const uint8_t *req_payload,
				       size_t req_payload_len,
				       uint8_t *rsp_type,
				       uint8_t *rsp_payload,
				       size_t rsp_payload_buf_size,
				       size_t *rsp_payload_len)
{
	CdcPacketBody body;
	int err;

	if ((rsp_type == NULL) || (rsp_payload == NULL) || (rsp_payload_len == NULL)) {
		return -EINVAL;
	}

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

	if (!type_matches_body(req_type, &body)) {
		LOG_WRN("CDC type/body mismatch type:0x%02x body_case:%d",
			req_type, body.which_body);
		return -EBADMSG;
	}

	switch (req_type) {
	case CDC_PROTO_TYPE_HELLO_REQ:
		LOG_INF("HelloReq protocol_version:%u",
			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);
		}

		err = encode_hello_rsp(rsp_payload, rsp_payload_buf_size, rsp_payload_len);
		if (err) {
			return err;
		}

		*rsp_type = CDC_PROTO_TYPE_HELLO_RSP;
		return 0;

	default:
		LOG_WRN("Unsupported CDC protocol type:0x%02x", req_type);
		return -ENOTSUP;
	}
}