diff --git a/arch/risc-v/src/c906/c906_head.S b/arch/risc-v/src/c906/c906_head.S
index 5e6032906c1..7880e3510ff 100644
--- a/arch/risc-v/src/c906/c906_head.S
+++ b/arch/risc-v/src/c906/c906_head.S
@@ -27,6 +27,8 @@
 #include <arch/rv64gc/irq.h>
 
 #include "chip.h"
+#include "c906_memorymap.h"
+#include "riscv_internal.h"
 
 /****************************************************************************
  * Public Symbols
@@ -54,7 +56,7 @@ __start:
 
   /* enable FPU if CFLAGS has 'f' or 'd' in -march */
 
-#ifdef __riscv_xlen
+#ifdef __riscv_flen
     li      a0, MSTATUS_FS_INIT
     csrs    mstatus, a0
 #endif
@@ -79,7 +81,7 @@ __start:
 
   /* Set stack pointer to the idle thread stack */
 
-  la   sp, C906_IDLESTACK0_TOP
+  la   sp, C906_IDLESTACK_TOP
 
   /* Disable all interrupts (i.e. timer, external) in mie */
 
@@ -91,6 +93,33 @@ __start:
   la   t0, __trap_vec
   csrw mtvec, t0
 
+#ifdef CONFIG_STACK_COLORATION
+
+  /* Write a known value to the IDLE thread stack to support stack
+   * monitoring logic
+   */
+
+  lui  t0, %hi(C906_IDLESTACK_BASE)
+  addi t0, t0, %lo(C906_IDLESTACK_BASE)
+
+  lui  t1, %hi(C906_IDLESTACK_TOP)
+  addi t1, t1, %lo(C906_IDLESTACK_TOP)
+
+  lui  t2, %hi(STACK_COLOR)
+  addi t2, t2, %lo(STACK_COLOR)
+
+  bgeu t0, t1, 2f
+
+  /* t0 = start of IDLE stack; t1 = Size of tack; t2 = coloration */
+
+1:
+  sw   t2, 0(t0)
+  addi t0, t0, 4
+  bne  t0, t1, 1b
+2:
+
+#endif
+
   /* Jump to __c906_start with mhartid */
 
   j    __c906_start