path: root/boot/devmode/bootloader.c

#include "devmode.h"

#include <hardware.h>

#include <fifo.h>

#include <assert.h>
#include <stddef.h>
#include <string.h>

u16
crc16(const void *buf, size_t len);

volatile struct fifo rx_fifo = {0, 0, {0}};

enum ack {
    ACK = 0x00,
    NACK = 0xFF
};

void
rx_isr(void) __critical __interrupt(0)
{
    fifo_push(&rx_fifo, sio_a_data);
}

static volatile uint32_t millis = 0;

void
ctc1_isr(void) __critical __interrupt(1)
{
    millis += 5;
}

uint32_t
clock(void)
{
    volatile uint32_t ret;
    DI;
    ret = millis;
    EI;
    return ret;
}

void
putbyte(unsigned char b)
{
    unsigned char ctrl = 0;

    sio_a_data = b;

    while (!(ctrl & 0x04)) {
	sio_a_ctrl = 0;
	ctrl = sio_a_ctrl;
    }
}

static volatile int32_t errno = 0;

u8
getbyte(void)
{
    u8 b;
    uint32_t ms = clock();
    errno = 0;
    while (fifo_empty(&rx_fifo)) {
	if (clock() - ms > TIMEOUT_MS) {
	    errno = ERR_TIMEOUT;
	    return 0;
	}
    }
    DI;
    b = fifo_pop(&rx_fifo);
    EI;
    return b;
}

void
flush(void)
{
    DI;
    fifo_clear(&rx_fifo);
    EI;
}


// Hamming(7,4) encoding
u8
encode(u8 x)
{
    u8 y = 0;
    const u8 c[4] = {0x61, 0x52, 0x34, 0x78};

    for (u8 i = 0; i < 4; ++i)
	y ^= ((x >> i) & 1) ? c[i] : 0;

    return y;
}

// Hamming(7,4) decoding
u8
decode(u8 x)
{
    u8 p = 0;
    const u8 r[7] = {6, 5, 3, 7, 1, 2, 4};

    for (int i = 0; i < 7; ++i)
	p ^= ((x >> i) & 1) ? r[i] : 0;

    // Assume simple error, attempt correction
    if (p) {
	size_t i = 0;

	for (i = 0; i < LENGTH(r); ++i) {
	    if (r[i] == x)
		break;
	}

	x ^= (1 << i);
    }

    return x & 0x0F;
}


int
read(void *buf, size_t count)
{
    u8 b;
    u8 *p = buf;

    for (int n = 0; n < count; ++n) {
	b = decode(getbyte());
	if (errno)
	    return errno;

	b |= (decode(getbyte()) << 4);
	if (errno)
	    return errno;

	p[n] = b;
    }

    return 0;
}

int
write(const void *buf, size_t count)
{
    const u8 *p = buf;

    for (size_t i = 0; i < count; ++i) {
	putbyte(encode(p[i] & 0x0F));
	putbyte(encode((p[i] >> 4) & 0x0F));
    }

    return 0;
}

int
read_header(struct header *header)
{
    int err;
    u8 ack;
    u16 checksum;

    while (1) {
	if ((err = read(header, sizeof(*header))))
	    return err;

	checksum = header->checksum;
	header->checksum = 0;

	if (checksum == crc16(header, sizeof(*header))) {
	    header->checksum = checksum;
	    ack = ACK;
	    write(&ack, sizeof(ack));
	    return 0;
	} else {
	    ack = NACK;
	    write(&ack, sizeof(ack));
	}
    }
}

int
read_buf(size_t len, void *buf)
{
    int err;
    u8 ack;
    u16 checksum;

    for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i) {
	// TODO: reduce code?
	if ((err = read(&checksum, sizeof(checksum)))
	    || (err = read(buf, len)))
	    break;

	if (checksum == crc16(buf, len)) {
	    ack = ACK;
	    write(&ack, sizeof(ack));
	    return 0;
	} else {
	    ack = NACK;
	    write(&ack, sizeof(ack));
	}
    }

    return -1;
}

int
write_buf(size_t len, const void *buf)
{
    int err;
    u8 ack = NACK;
    u16 checksum = crc16(buf, len);

    for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i){
	write(&checksum, sizeof(checksum));
	write(buf, len);

	// If TIMEOUT sending just give up
	if ((err = read(&ack, sizeof(ack))))
	    return err;

	if (ack == ACK)
	    return 0;
    }

    return -1;
}

// TODO: Restart after timeouts
void
loop(void)
{
    struct header header;
    u8 buf[MAX_PACKET_SIZE];

    while (1) {
	if (read_header(&header)) {
	    flush();
	    continue;
	}

	switch (header.type) {
	case CMD_BOOT:
	    ((void (*)(void))header.address)();
	    break;

	case CMD_READ:
	    write_buf(header.length, (const void *)header.address);
	    break;

	case CMD_WRITE:
	    if (!read_buf(header.length, buf))
		memcpy((void *)header.address, buf, header.length);
	    break;

	case CMD_ECHO:
	    if (!read_buf(header.length, buf))
		write_buf(header.length, buf);
	    break;

	default:
	    break;
	}

	flush();
    }
}

static inline void
init_ctc(void)
{
    ctc_channel_0 = CTC_CTRL(0);

    /* 200Hz clock */
    ctc_channel_1 = CTC_CTRL(CTC_INT_BIT | CTC_PRESCALER_BIT
			     | CTC_TIME_CONST_BIT | CTC_RST_BIT);
    ctc_channel_1 = 0;

    ctc_channel_2 = CTC_CTRL(0);
    ctc_channel_3 = CTC_CTRL(0);

    // Interrupt table for CTC
    // Final address is (Ireg << 8) | ctc_isr_ptr | {00/01/10/11} | 0
    ctc_channel_0 = CTC_INT_VEC(0x10);
}

static inline void
init_pio(void)
{
    // Disable PIO interrupts
    port_a_ctrl = PIO_INT_CTRL(0);
    port_b_ctrl = PIO_INT_CTRL(0);
}

static inline void
init_sio(void)
{
    static const u8 sio_a_cfg[] = {
	0b00011000,			 // Reset channel
	4	  ,			 // wr4
	0b01000100,			 // X16 clock (115200), one stop bit, no parity
	1	  ,			 // wr1
	SIO_RX_INT_MD0 | SIO_RX_INT_MD1, // interrupt on every Rx, no wait function
	3	  ,			 // wr3
	0b11000001,			 // enable Rx - 8 bit char
	5	  ,			 // wr5
	0b01101000			 // enable Tx - 8 bit char
    };

    static const u8 sio_b_cfg[] = {
	0b00011000,			// Reset channel
	2	  ,			// load interrupt vector
	0x1C				// int_table_rx
    };

    for (u8 i = 0; i < LENGTH(sio_a_cfg); ++i)
	sio_a_ctrl = sio_a_cfg[i];

    for (u8 i = 0; i < LENGTH(sio_b_cfg); ++i)
	sio_b_ctrl = sio_b_cfg[i];
}

void
main(void)
{
    init_ctc();
    init_pio();
    init_sio();

    IM(2);
    EI;

    loop();
}