Initial commit

1 files changed, 361 insertions, 0 deletions

diff --git a/zbootloader.c b/zbootloader.c
new file mode 100644
index 0000000..df168df
--- /dev/null
+++ b/zbootloader.c
@@ -0,0 +1,361 @@
+#include <zbootloader.h>
+#include <zeta.h>
+#include <stddef.h>
+#include <string.h>
+
+/* volatile uint8_t rx_head = 0; */
+/* volatile uint8_t rx_tail = 0; */
+/* volatile uint8_t rx_fifo[FIFO_LEN]; */
+
+struct fifo {
+    uint8_t head;
+    uint8_t tail;
+    uint8_t data[FIFO_LEN];
+};
+
+volatile struct fifo rx_fifo = {0, 0, {0}};
+
+uint8_t
+fifo_pop(struct fifo *fifo)
+{
+    uint8_t ret = fifo->data[fifo->head];
+    if (++fifo->head >= LEN(fifo->data))
+        fifo->head = 0;
+    return ret;
+}
+
+void
+fifo_push(struct fifo *fifo, uint8_t v)
+{
+    fifo->data[fifo->tail] = v;
+
+    if (++fifo->tail >= LEN(fifo->data))
+        fifo->tail = 0;
+}
+
+bool
+fifo_empty(const struct fifo *fifo)
+{
+    return (fifo->head == fifo->tail);
+}
+
+void
+fifo_clear(struct fifo *fifo)
+{
+    fifo->head = 0;
+    fifo->tail = 0;
+}
+
+static const uint8_t sio_a_cfg[] = {
+    0b00011000,                      // Reset channel
+    4         ,                      // wr4
+    0b00000100,                      // X1 clock, 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
+};
+
+extern void *rx_isr_ptr;
+extern void *ctc0_isr_ptr;
+extern void *ctc1_isr_ptr;
+extern void *ctc2_isr_ptr;
+extern void *ctc3_isr_ptr;
+
+#define ISR_OFFSET(x) ((unsigned int)&x)
+
+static const unsigned char sio_b_cfg[] = {
+    0b00011000,   // Reset channel
+    2         ,   // load interrupt vector
+    ISR_OFFSET(rx_isr_ptr)          // int_table_rx
+};
+
+/* int_table: */
+/* int_table_rx: */
+/*     dw      rx_isr */
+
+void
+rx_isr(void) __critical __interrupt(0)
+{
+    fifo_push(&rx_fifo, sio_a_data);
+}
+
+static volatile uint32_t millis = 0;
+
+void
+ctc3_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;
+
+uint8_t
+getbyte(void)
+{
+    uint8_t 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
+uint8_t
+encode(uint8_t x)
+{
+    uint8_t y = 0;
+    const uint8_t c[4] = {0x61, 0x52, 0x34, 0x78};
+
+    for (uint8_t i = 0; i < 4; ++i)
+        y ^= ((x >> i) & 1) ? c[i] : 0;
+
+    return y;
+}
+
+// Hamming(7,4) decoding
+uint8_t
+decode(uint8_t x)
+{
+    uint8_t p = 0;
+    const uint8_t 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 < LEN(r); ++i) {
+            if (r[i] == x)
+                break;
+        }
+
+        x ^= (1 << i);
+    }
+
+    return x & 0x0F;
+}
+
+
+int
+read(void *buf, size_t count)
+{
+    uint8_t b;
+    uint8_t *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 uint8_t *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;
+    uint8_t ack;
+    uint16_t 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;
+    uint8_t ack;
+    uint16_t 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;
+    uint8_t ack = NACK;
+    uint16_t 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;
+    uint8_t 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();
+    }
+}
+
+int
+main(void)
+{
+    // Init CTC
+    // 16 prescaler, timer mode
+    ctc_channel_1 = (CTC_CLK_TRG_BIT | CTC_TIME_CONST_BIT | CTC_RST_BIT
+                     | CTC_CTRL_OR_VECTOR_BIT);
+    ctc_channel_1 = (CPU_FREQ / 16 / 9600);
+
+    // 200Hz clock
+    ctc_channel_3 = (CTC_INT_BIT | CTC_PRESCALER_BIT | CTC_CLK_TRG_BIT |
+                     CTC_TIME_CONST_BIT | CTC_RST_BIT | CTC_CTRL_OR_VECTOR_BIT);
+    ctc_channel_3 = (CPU_FREQ / 256 / 36);
+    ctc_channel_0 = ISR_OFFSET(ctc3_isr_ptr) | (3 << 1);
+
+    // Init SIO
+    for (uint8_t i = 0; i < LEN(sio_a_cfg); ++i)
+        sio_a_ctrl = sio_a_cfg[i];
+
+    for (uint8_t i = 0; i < LEN(sio_b_cfg); ++i)
+        sio_b_ctrl = sio_b_cfg[i];
+
+    // Interrupt mode 2
+    IM(2);
+    // Enable interrupts
+    EI;
+
+    loop();
+    return 0;
+}