add bootloader

diff --git a/bootloader/bootloader.c b/bootloader/bootloader.c
new file mode 100644 (file)
index 0000000..deb160e
--- /dev/null
+++ b/bootloader/bootloader.c
@@ -0,0 +1,322 @@
+/* SPDX-License-Identifier: BSD-3-Clause
+ * Copyright 2019, Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <avr/io.h>
+#include <util/delay.h>
+#include <avr/boot.h>
+#include <avr/pgmspace.h>
+#include <avr/wdt.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <string.h>
+
+#define NOECHO
+
+#ifdef NOECHO
+#define echo(c) do {} while(0)
+#else
+#define echo(c) uart_send(c)
+#endif
+
+/* disable watchdog early at init */
+__attribute__((naked)) __attribute__((section(".init3")))
+void wdt_init(void)
+{
+    MCUSR = 0;
+    wdt_disable();
+}
+
+static void uart_send(char c)
+{
+       while (!(UCSR0A & (1 << UDRE0)))
+               ;
+       UDR0 = c;
+}
+
+static void uart_puts(const char *buf)
+{
+       while (*buf)
+               uart_send(*(buf++));
+}
+
+static int8_t uart_recv(char *c)
+{
+       if (!(UCSR0A & (1 << RXC0)))
+               return -1;
+
+       *c = UDR0;
+       return 0;
+}
+
+static char uart_recv_wait(void)
+{
+       char c;
+
+       while (uart_recv(&c) < 0)
+               ;
+
+       return c;
+}
+
+static void uart_init(void)
+{
+       // 57600 bauds
+       UBRR0H = 0;
+       UBRR0L = 16;
+       UCSR0A = (1 << U2X0);
+       UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);
+       UCSR0B = (1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0);
+}
+
+static void led_debug_init(void)
+{
+       DDRB |= (1 << 6); /* debug led as output */
+}
+
+static void led_debug_on(void)
+{
+       PORTB |= (1 << 6);
+}
+
+__attribute__((unused))
+static void led_debug_off(void)
+{
+       PORTB &= (~(1 << 6));
+}
+
+static int8_t bootloader_query_hex(uint32_t *val)
+{
+       uint32_t tmp = 0;
+       int c;
+
+       while (1) {
+               c = uart_recv_wait();
+               echo(c);
+
+               if (c == '\n' || c == '\r') {
+                       *val = tmp;
+                       return 0;
+               }
+               else if (c >= '0' && c <= '9') {
+                       tmp <<= 4;
+                       tmp += (c - '0');
+               }
+               else if (c >= 'a' && c <= 'f') {
+                       tmp <<= 4;
+                       tmp += (c - 'a' + 10);
+               }
+               else if (c >= 'A' && c <= 'F') {
+                       tmp <<= 4;
+                       tmp += (c - 'A' + 10);
+               }
+               else
+                       return -1;
+       }
+       return 0;
+}
+
+/* launch application */
+static void launch_app(void)
+{
+       uint32_t w;
+
+       w = pgm_read_dword_near(0);
+       if (w == 0xffffffff) {
+               uart_puts("no app\r\n");
+               return;
+       }
+
+       uart_puts("Boot...");
+       MCUCR = (1 << IVCE);
+       MCUCR = 0; /* IVSEL = 0 */
+       __asm__ __volatile__ ("ldi r30,0x00\n");
+       __asm__ __volatile__ ("ldi r31,0x00\n");
+       __asm__ __volatile__ ("ijmp\n");
+}
+
+static void disp_digit(uint8_t x)
+{
+       if (x < 10)
+               x += '0';
+       else
+               x += 'a' - 10 ;
+       uart_send(x);
+}
+
+static void disp_hex8(uint8_t x)
+{
+       disp_digit(x>>4);
+       disp_digit(x&0xf);
+}
+
+static void disp_hex16(uint16_t x)
+{
+       disp_hex8(x>>8);
+       disp_hex8(x);
+}
+
+static void disp_hex32(uint32_t x)
+{
+       disp_hex16(x>>16);
+       disp_hex16(x);
+}
+
+#define DJB_HASH_INIT 5381
+static void djb_hash_add(uint32_t *h, uint8_t x)
+{
+      *h = ((*h << 5) + *h) + x;
+}
+
+static void hash_area(void)
+{
+       uint32_t start_addr, addr, size;
+       uint32_t hash = DJB_HASH_INIT;
+       uint8_t val;
+
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&start_addr))
+               goto fail;
+       if (start_addr > FLASHEND)
+               goto fail;
+       uart_puts("size?\r\n");
+       if (bootloader_query_hex(&size))
+               goto fail;
+       if (start_addr + size > FLASHEND)
+               goto fail;
+       for (addr = start_addr; addr < start_addr + size; addr++) {
+               val = pgm_read_byte_near(addr);
+               djb_hash_add(&hash, val);
+       }
+       disp_hex32(hash);
+
+       return;
+
+ fail:
+       uart_puts("KO");
+}
+
+static void read32(void)
+{
+       uint32_t start_addr, val = 0;
+       uint8_t c, i;
+
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&start_addr))
+               goto fail;
+       if (start_addr > FLASHEND)
+               goto fail;
+       for (i = 0; i < 4; i++) {
+               c = pgm_read_byte_near(start_addr + i);
+               val <<= 8;
+               val |= c;
+       }
+       disp_hex32(val);
+       return;
+ fail:
+       uart_puts("KO");
+}
+
+static void prog_page(void)
+{
+       uint8_t c;
+       uint32_t addr;
+       uint16_t i;
+       uint32_t hash = DJB_HASH_INIT;
+       uint8_t buf[SPM_PAGESIZE];
+
+#define SPM_PAGEMASK ((uint32_t)SPM_PAGESIZE-1)
+       uart_puts("addr?\r\n");
+       if (bootloader_query_hex(&addr))
+               goto fail;
+       if (addr > FLASHEND)
+               goto fail;
+       /* start_addr must be page aligned */
+       if (addr & SPM_PAGEMASK)
+               goto fail;
+
+       uart_puts("ok\r\n");
+
+       /* data is received like the .bin format (which is already in
+        * little endian) */
+       for (i = 0; i < SPM_PAGESIZE; i++) {
+               c = uart_recv_wait();
+               djb_hash_add(&hash, c);
+               buf[i] = c;
+       }
+       disp_hex32(hash);
+       uart_puts(" (y?)\r\n");
+       c = uart_recv_wait();
+       if (c != 'y')
+               goto fail;
+
+       /* erase page */
+       eeprom_busy_wait();
+       boot_page_erase(addr);
+       boot_spm_busy_wait();
+
+       /* write data in flash */
+       for (i = 0; i < SPM_PAGESIZE; i += 2) {
+               uint16_t w = buf[i] + ((uint16_t)(buf[i+1]) << 8);
+               boot_page_fill(addr + i, w);
+       }
+       boot_page_write(addr);
+       boot_spm_busy_wait();
+
+       /* Reenable RWW-section again. We need this if we want to jump
+        * back to the application after bootloading. */
+       boot_rww_enable();
+
+       uart_puts("OK");
+       return;
+ fail:
+       uart_puts("KO");
+}
+
+int main(void)
+{
+       uint8_t i;
+       char c;
+
+       /* LEDs as output pin */
+       led_debug_init();
+       led_debug_on();
+
+       /* uart and stdout initialization */
+       uart_init();
+
+       //XXX check button ?
+
+       uart_puts("\r\nboot> ");
+
+       /* timeout */
+       i = 0;
+       while ( !(UCSR0A & (1 << RXC0)) ) {
+               i++;
+               _delay_ms(10);
+               if (i > 100) {
+                       launch_app();
+                       break;
+               }
+       }
+
+       while (1) {
+               c = uart_recv_wait();
+               uart_puts("\r\n");
+               if (c == 'x')
+                       launch_app();
+               else if (c == 'h')
+                       hash_area();
+               else if (c == 'p')
+                       prog_page();
+               else if (c == 'd')
+                       read32();
+               else
+                       uart_puts("p:prog_page h:hash x:exec d:dump32");
+
+               uart_puts("\r\nboot> ");
+       }
+
+       return 0;
+}