poly-1-fw/sysex.cpp

// intellectual property is bullshit bgdc

#include "sysex.hpp"
#include "midix.hpp"
#include "stor.hpp"
#include "board.hpp"

extern "C" {
#include "base64.h"
}

#include <stdbool.h>
#include <stddef.h>
#include <inttypes.h>
#include <string.h>

// Send a status code in response to a message
static void _send_status(struct midi_message const * msg, uint8_t status);

// Handle Storage group commands
static void _handle_storage(struct midi_message const * msg, size_t len);

// Handle Status group commands
static void _handle_status(struct midi_message const * msg, size_t len);

// Validate that the message's length is at least n. Returns false if not.
static bool _check_len(struct midi_message const * msg, size_t len, size_t n);

// Parse 8 hex characters and return the integer. Undefined results if invalid.
static uint32_t _parse_8hex(uint8_t const * s);

// Send a block of data, up to 32 bytes, as a SYSEX_STORAGE_RESPONSE
static void _send_storage_response(
	struct midi_message const * msg, uint8_t const * data, uint8_t n
);

// Read in some data from a message as base64. Returns the number of bytes
// read. On checksum fail, returns SIZE_MAX.
static size_t _convert_message_data(uint8_t * dest, size_t dest_len,
	struct midi_message const * msg, size_t msg_len
);

void sysex_handle(struct midi_message const * msg)
{
	PIN_SET(PIN_DEBUG0);

	size_t len = strlen((char const *) msg->content);
	PIN_SET(PIN_DEBUG1);

	// XXX
	msg->source->send_sysex((const uint8_t *) "Hello");
	PIN_SET(PIN_DEBUG2);
	PIN_CLR(PIN_DEBUG0 | PIN_DEBUG1 | PIN_DEBUG2);

	if (msg->content[0] != MIDI_MANUF_ID)
		// Not for us - return
		return;

	if (!_check_len(msg, len, 2))
		return;

	switch (msg->content[1])
	{
	case MIDI_SYSEX_STORAGE_GRP:
		_handle_storage(msg, len);
		break;

	case MIDI_SYSEX_STATUS_GRP:
		_handle_status(msg, len);
		break;

	default:
		_send_status(msg, MIDI_SYSEX_STATUS_UNKNOWN);
		return;
	}
}

static void _send_status(struct midi_message const * msg, uint8_t status)
{
	uint8_t message[] = {MIDI_MANUF_ID, MIDI_SYSEX_STATUS_GRP, status, 0};
	msg->source->send_sysex(message);
}

static void _send_storage_response(
	struct midi_message const * msg, uint8_t const * data, uint8_t n
)
{
	uint8_t message[5 + 44] = {MIDI_MANUF_ID, MIDI_SYSEX_STORAGE_GRP,
		MIDI_SYSEX_STORAGE_RESPONSE, 0};

	// checksum
	for (uint8_t i = 0; i < n; i++)
		message[3] += data[n];
	message[3] = (message[3] & 0x7F) ^ 0x7F;

	base64_encode(&message[4], data, n);
	msg->source->send_sysex(message);
}

static void _handle_storage(struct midi_message const * msg, size_t len)
{
	if (!_check_len(msg, len, 3))
		return;

	switch (msg->content[3])
	{
	case MIDI_SYSEX_STORAGE_READ:
		if (!_check_len(msg, len, 11))
			return;

		if (!stor_try_lock())
		{
			_send_status(msg, MIDI_SYSEX_STATUS_BUSY);
			return;
		}

		if (!stor_read(_parse_8hex(&msg->content[4])))
		{
			_send_status(msg, MIDI_SYSEX_STATUS_IOERR);
			stor_unlock();
			return;
		}

		for (size_t curs = 0; curs < STOR_SIZE; curs += 32)
		{
			_send_storage_response(msg, &stor_buffer[curs], 32);
		}

		stor_unlock();
		_send_status(msg, MIDI_SYSEX_STATUS_ACK);
		break;

	case MIDI_SYSEX_STORAGE_OPENWR:
		if (!_check_len(msg, len, 11))
			return;

		if (stor_try_lock())
		{
			_send_status(msg, MIDI_SYSEX_STATUS_ACK);
			stor_cursor = 0;
			stor_block_memo = _parse_8hex(&msg->content[4]);
		}
		else
			_send_status(msg, MIDI_SYSEX_STATUS_BUSY);
		break;

	case MIDI_SYSEX_STORAGE_WRDATA: {
		size_t n = _convert_message_data(
			&stor_buffer[stor_cursor],
			512 - stor_cursor,
			msg, len
		);
		if (n == SIZE_MAX)
			_send_status(msg, MIDI_SYSEX_STATUS_CKSUM);
		else
		{
			stor_cursor += n;
			_send_status(msg, MIDI_SYSEX_STATUS_ACK);
		}
		break;
	}

	case MIDI_SYSEX_STORAGE_COMMIT:
		if (!_check_len(msg, len, 11))
			return;

		if (stor_write(_parse_8hex(&msg->content[4])))
			_send_status(msg, MIDI_SYSEX_STATUS_ACK);
		else
			_send_status(msg, MIDI_SYSEX_STATUS_IOERR);
		stor_unlock();
		break;

	case MIDI_SYSEX_STORAGE_CANCEL:
		stor_unlock();
		_send_status(msg, MIDI_SYSEX_STATUS_ACK);
		break;

	default:
		_send_status(msg, MIDI_SYSEX_STATUS_UNKNOWN);
		break;
	}
}

static void _handle_status(struct midi_message const * msg, size_t len)
{
	if (!_check_len(msg, len, 3))
		return;

	switch (msg->content[2])
	{
	case MIDI_SYSEX_STATUS_ACK:
	case MIDI_SYSEX_STATUS_FMT_ERR:
	case MIDI_SYSEX_STATUS_UNKNOWN:
		// Do not respond to recognized status messages.
		break;

	default:
		_send_status(msg, MIDI_SYSEX_STATUS_UNKNOWN);
		break;
	}
}

static bool _check_len(struct midi_message const * msg, size_t len, size_t n)
{
	if (len < n)
	{
		_send_status(msg, MIDI_SYSEX_STATUS_FMT_ERR);
		return false;
	}
	else
		return true;
}

static uint32_t _parse_8hex(uint8_t const * s)
{
	uint32_t x = 0;

	for (uint8_t i = 0; i < 8; i++)
	{
		char c = s[i];
		x <<= 4;
		if (c >= '0' && c <= '9')
			x |= c - '0';
		else if (c >= 'A' && c <= 'F')
			x |= c - 'A' + 10;
		else if (c >= 'a' && c <= 'f')
			x |= c - 'a' + 10;
	}

	return x;
}

static size_t _convert_message_data(uint8_t * dest, size_t dest_len,
	struct midi_message const * msg, size_t msg_len
)
{
	// Compute the length of base64 data
	size_t src_len_base64 = msg_len - 4;

	// Compute the destination length required to hold it
	size_t len_needed = (4 * src_len_base64 + 2) / 3;

	if (len_needed < dest_len)
		return -1;

	size_t n = base64_decode(dest, &msg->content[4], dest_len);

	uint8_t checksum = 0;
	for (size_t i = 0; i < n; i++)
		checksum += dest[i];
	checksum &= 0x7F;
	checksum ^= 0x7F;

	if (checksum == msg->content[3])
		return n;
	else
		return -1;
}