From 3c1c8415713f80cd0919f7bd5244d292f1c3cfe1 Mon Sep 17 00:00:00 2001 From: Thomas Albers Date: Tue, 8 Aug 2023 18:52:32 +0200 Subject: Initial commit --- zbootloader.c | 361 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 361 insertions(+) create mode 100644 zbootloader.c (limited to 'zbootloader.c') 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 +#include +#include +#include + +/* 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; +} -- cgit v1.2.3