From 285d5d6a6f7a1e2b30956a310d465b82b167994e Mon Sep 17 00:00:00 2001
From: Mateusz Faderewski <sc@mateuszfaderewski.pl>
Date: Mon, 30 Jan 2023 06:15:08 +0100
Subject: [PATCH] [SC64][SW] Added controller stack usage diagnostics

---
 docs/02_usb_commands.md  |  1 +
 sw/controller/common.mk  |  5 +----
 sw/controller/src/app.c  |  9 ++++++++-
 sw/controller/src/app.h  | 11 +++++++++++
 sw/controller/src/task.c | 17 ++++++++++++++++-
 sw/controller/src/task.h |  1 +
 sw/controller/src/usb.c  |  8 ++++++++
 7 files changed, 46 insertions(+), 6 deletions(-)
 create mode 100644 sw/controller/src/app.h

diff --git a/docs/02_usb_commands.md b/docs/02_usb_commands.md
index 90ea86e..f104672 100644
--- a/docs/02_usb_commands.md
+++ b/docs/02_usb_commands.md
@@ -22,3 +22,4 @@
 | `p` | **FLASH_WAIT_BUSY**    | wait         | ---          | ---  | erase_block_size | Wait until flash ready / get flash block erase size |
 | `P` | **FLASH_ERASE_BLOCK**  | address      | ---          | ---  | ---              | Start flash block erase                             |
 | `?` | **DEBUG_GET**          | ---          | ---          | ---  | debug_data       | Get internal FPGA debug info                        |
+| `%` | **STACK_USAGE_GET**    | ---          | ---          | ---  | stack_usage      | Get per task stack usage                            |
diff --git a/sw/controller/common.mk b/sw/controller/common.mk
index b76e5ee..8dbbcf1 100644
--- a/sw/controller/common.mk
+++ b/sw/controller/common.mk
@@ -31,16 +31,13 @@ $(BUILD_DIR)/$(EXE_NAME).elf: $(OBJS) $(LD_SCRIPT)
 $(BUILD_DIR)/$(EXE_NAME).bin: $(BUILD_DIR)/$(EXE_NAME).elf
 	$(OBJCOPY) -O binary --gap-fill 0xFF $< $@
 
-$(BUILD_DIR)/$(EXE_NAME).hex: $(BUILD_DIR)/$(EXE_NAME).bin
-	@$(OBJCOPY) -I binary -O ihex $< $@
-
 print_size: $(BUILD_DIR)/$(EXE_NAME).elf
 	@echo 'Size of modules:'
 	@$(SIZE) -B -d -t --common $(OBJS)
 	@echo 'Size of $(EXE_NAME):'
 	@$(SIZE) -B -d $<
 
-all: $(BUILD_DIR)/$(EXE_NAME).hex print_size
+all: $(BUILD_DIR)/$(EXE_NAME).bin print_size
 
 clean:
 	@rm -rf ./$(BUILD_DIR)/*
diff --git a/sw/controller/src/app.c b/sw/controller/src/app.c
index 9bceeb5..1988a88 100644
--- a/sw/controller/src/app.c
+++ b/sw/controller/src/app.c
@@ -1,4 +1,4 @@
-#include <stdint.h>
+#include "app.h"
 #include "cic.h"
 #include "gvr.h"
 #include "hw.h"
@@ -19,6 +19,13 @@ uint8_t led_stack[LED_STACK_SIZE] __attribute__((aligned(8)));
 uint8_t gvr_stack[GVR_STACK_SIZE] __attribute__((aligned(8)));
 
 
+void app_get_stack_usage (uint32_t *usage) {
+    *usage++ = task_get_stack_usage(cic_stack, CIC_STACK_SIZE);
+    *usage++ = task_get_stack_usage(rtc_stack, RTC_STACK_SIZE);
+    *usage++ = task_get_stack_usage(led_stack, LED_STACK_SIZE);
+    *usage++ = task_get_stack_usage(gvr_stack, GVR_STACK_SIZE);
+}
+
 void app (void) {
     hw_init();
     cic_hw_init();
diff --git a/sw/controller/src/app.h b/sw/controller/src/app.h
new file mode 100644
index 0000000..e0d24ca
--- /dev/null
+++ b/sw/controller/src/app.h
@@ -0,0 +1,11 @@
+#ifndef APP_H__
+#define APP_H__
+
+
+#include <stdint.h>
+
+
+void app_get_stack_usage (uint32_t *usage);
+
+
+#endif
diff --git a/sw/controller/src/task.c b/sw/controller/src/task.c
index 7731e5f..b2ddac5 100644
--- a/sw/controller/src/task.c
+++ b/sw/controller/src/task.c
@@ -5,6 +5,7 @@
 
 #define TASK_INITIAL_XPSR       (0x21000000UL)
 #define TASK_CONTEXT_SWITCH()   { SCB->ICSR = (1 << SCB_ICSR_PENDSVSET_Pos); }
+#define TASK_STACK_FILL_VALUE   (0xDEADBEEF)
 
 
 typedef enum {
@@ -38,7 +39,9 @@ static void task_initialize (task_id_t id) {
     *--sp = TASK_INITIAL_XPSR;
     *--sp = task->initial_pc;
     *--sp = ((uint32_t) (task_exit));
-    sp -= 13;
+    for (int i = 0; i < 13; i++) {
+        *--sp = 0;
+    }
     task->sp = ((uint32_t) (sp));
 }
 
@@ -67,6 +70,9 @@ static uint32_t task_switch_context (uint32_t sp) {
 
 void task_create (task_id_t id, void (*code)(void), void *stack, size_t stack_size) {
     if (id < __TASK_ID_MAX) {
+        for (size_t i = 0; i < stack_size; i += sizeof(uint32_t)) {
+            (*(uint32_t *) (stack + i)) = TASK_STACK_FILL_VALUE;
+        }
         task_t *task = &task_table[id];
         task->initial_pc = (uint32_t) (code);
         task->initial_sp = (((uint32_t) (stack)) + stack_size);
@@ -90,6 +96,15 @@ void task_set_ready_and_reset (task_id_t id) {
     TASK_CONTEXT_SWITCH();
 }
 
+size_t task_get_stack_usage (void *stack, size_t stack_size) {
+    for (size_t i = 0; i < stack_size; i += sizeof(uint32_t)) {
+        if ((*(uint32_t *) (stack + i)) != TASK_STACK_FILL_VALUE) {
+            return (stack_size - i);
+        }
+    }
+    return 0;
+}
+
 __attribute__((naked)) void task_scheduler_start (void) {
     uint32_t sp = task_table[task_current].sp;
 
diff --git a/sw/controller/src/task.h b/sw/controller/src/task.h
index 5185c34..684f5d7 100644
--- a/sw/controller/src/task.h
+++ b/sw/controller/src/task.h
@@ -18,6 +18,7 @@ void task_create (task_id_t id, void (*code)(void), void *stack, size_t stack_si
 void task_yield (void);
 void task_set_ready (task_id_t id);
 void task_set_ready_and_reset (task_id_t id);
+size_t task_get_stack_usage (void *stack, size_t stack_size);
 void task_scheduler_start (void);
 
 
diff --git a/sw/controller/src/usb.c b/sw/controller/src/usb.c
index ab59986..008ae80 100644
--- a/sw/controller/src/usb.c
+++ b/sw/controller/src/usb.c
@@ -1,3 +1,4 @@
+#include "app.h"
 #include "cfg.h"
 #include "cic.h"
 #include "dd.h"
@@ -295,6 +296,13 @@ static void usb_rx_process (void) {
                 p.response_info.data[1] = fpga_reg_get(REG_DEBUG_1);
                 break;
 
+            case '%':
+                p.rx_state = RX_STATE_IDLE;
+                p.response_pending = true;
+                p.response_info.data_length = 16;
+                app_get_stack_usage(p.response_info.data);
+                break;
+
             default:
                 p.rx_state = RX_STATE_IDLE;
                 p.response_pending = true;