1 files changed, 102 insertions, 0 deletions
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);
+}