Add panic function

2 files changed, 113 insertions, 44 deletions

diff --git a/src/main.c b/src/main.c
index ae7dab5..c2796a9 100644
--- a/src/main.c
+++ b/src/main.c
@@ -6,6 +6,7 @@
 #include "ch573/pwr.h"
 #include "ch573/uart.h"
 #include "isr_vector.h"
+#include "panic.h"
 
 #define GPIO_PORT_A ch573_gpio__gpio_port_a
 #define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF
@@ -49,8 +50,6 @@ FILE _uart1_FILE = (FILE){
 
 FILE* const stdout = &_uart1_FILE;
 
-int my_puts(const char* str);
-
 // FILE* const stdout = &_uart1_FILE;
 
 /*
@@ -127,6 +126,8 @@ int main(void)
   char buf[32] = { 0 };
   volatile uint32_t i = 0xdeadbeef;
 
+  panic(0xdeadbeef);
+
   stack_dump(&i);
 
   print_hex(i);
@@ -134,7 +135,6 @@ int main(void)
   stdout->put('a', NULL);
   stdout->put('\n', NULL);
   fputs("bulitin fputs\n", stdout);
-  my_puts("my_puts\n");
   puts("bulitin puts\n");
 
   while (1) {
@@ -186,56 +186,23 @@ void print_hex(uint32_t x)
   }
 }
 
-void stack_dump(uint32_t* sp)
-{
-  fputs("\n\nstack_dump:\n\n", &_uart1_FILE);
-  while ((uint32_t) sp < 0x20008000) {
-    fputs("   0x", &_uart1_FILE);
-    print_hex((uint32_t)sp);
-    fputs("  ->  0x", &_uart1_FILE);
-    print_hex(*sp);
-    fputs("\n", &_uart1_FILE);
-    sp += 1;
-  }
-}
 
 IRQ(exc)
 {
   uint32_t mcause, mepc, mtval, *sp;
+
   asm volatile("csrr %0, mcause" : "=r"(mcause));
   asm volatile("csrr %0, mepc" : "=r"(mepc));
   asm volatile("csrr %0, mtval" : "=r"(mtval));
 
-  fputs("NMI Detected:\n", &_uart1_FILE);
-  fputs("  mcause: 0b", &_uart1_FILE);
-  print_binary(mcause);
-  fputs("\n  mepc: 0b", &_uart1_FILE);
-  print_binary(mepc);
-  fputs("\n        0x", &_uart1_FILE);
-  print_hex(mepc);
-  fputs("\n  mtval: 0b", &_uart1_FILE);
-  print_binary(mtval);
-  fputs("\n  deadbeef: 0b", &_uart1_FILE);
-  print_binary(0xdeadbeef);
-  fputs("\n", &_uart1_FILE);
-
-  stack_dump(__builtin_return_address(0));
+  printf("Hardware Exception Caught:\n");
+  printf("  mcause: 0x%80x\n", mcause);
+  printf("    mepc: 0x%80x\n", mepc);
+  printf("   mtval: 0x%80x\n", mtval);
 
-  while (1) {
-    GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
-    delay();
-    delay();
-    delay();
-    delay();
-    delay();
-    delay();
-    delay();
-    delay();
-    GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
-    delay();
-    delay();
-    delay();
-  }
+  panic(mcause);
+
+  while (1);
 }
 
 IRQ(nmi)
diff --git a/src/panic.c b/src/panic.c
new file mode 100644
index 0000000..c64aacd
--- /dev/null
+++ b/src/panic.c
@@ -0,0 +1,102 @@
+#include <stdint.h>
+#include <stdio.h>
+
+#include "ch573/gpio.h"
+
+#define GPIO_PORT_A ch573_gpio__gpio_port_a
+#define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF
+
+#define FLASH_1 12
+#define DOWN_TIME 6
+#define FLASH_0 1
+
+static void delay(void)
+{
+  for (volatile uint32_t i = 0; i < 10000; ++i) {
+    asm volatile("");
+  }
+}
+
+static void flash_1()
+{
+  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+  for (int i = 0; i < FLASH_1; ++i) {
+    delay();
+  }
+  GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
+  for (int i = 0; i < DOWN_TIME; ++i) {
+    delay();
+  }
+}
+
+static void reset_flash()
+{
+  for (int i = 0; i < 20; ++i) {
+    GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+    delay();
+    GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
+    delay();
+  }
+
+  delay();
+  delay();
+  delay();
+  delay();
+  delay();
+  delay();
+  delay();
+  delay();
+  delay();
+}
+
+static void flash_0()
+{
+  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+  for (int i = 0; i < FLASH_0; ++i) {
+    delay();
+  }
+  GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
+  for (int i = 0; i < (DOWN_TIME + (FLASH_1 - FLASH_0)); ++i) {
+    delay();
+  }
+}
+
+void stack_dump(uint32_t* sp, size_t n)
+{
+  puts("\nStack Dump:\n");
+  int i = 0;
+  while ((uint32_t)sp < 0x20008000 && i < n) {
+    printf("   %p -> %08x\n", sp, *sp);
+    sp++;
+    i++;
+  }
+}
+
+void flash_code(uint32_t code)
+{
+  while (1) {
+    reset_flash();
+    uint32_t c = code;
+    for (int i = 0; i < 32; ++i, c <<= 1) {
+      int msb = (c & 0x80000000) >> 31;
+      if (msb) {
+        flash_1();
+      } else {
+        flash_0();
+      }
+    }
+  }
+}
+
+void panic(uint32_t code)
+{
+  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 8);
+  GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 8);
+
+  printf("Panic called. Code 0x%08x\n", code);
+  uint32_t* sp;
+  asm volatile("add %0, sp, x0" : "=r"(sp));
+  stack_dump(sp, 128);
+
+  flash_code(code);
+}