From e87d14721ef11546b068e4d78a0bd7c1005ba83e Mon Sep 17 00:00:00 2001
From: Masayuki Ishikawa <masayuki.ishikawa@gmail.com>
Date: Fri, 19 Feb 2021 08:26:13 +0900
Subject: [PATCH] arch: xtensa: Fix stack coloring

Summary:
- Call up_stack_color() correctly in the up_create_stack()
- Fix nwords calculation in up_stack_color()
- Also, refactor up_stack_color()
- Fix do_stackcheck() to consider stack alignment

Impact:
- Only for CONFIG_STACK_COLORATION=y

Testing:
- Tested with esp32-devkitc:wapi_smp

Signed-off-by: Masayuki Ishikawa <Masayuki.Ishikawa@jp.sony.com>
---
 arch/xtensa/src/common/xtensa_checkstack.c  | 12 +++++++--
 arch/xtensa/src/common/xtensa_createstack.c | 27 ++++++++++++++-------
 2 files changed, 28 insertions(+), 11 deletions(-)

diff --git a/arch/xtensa/src/common/xtensa_checkstack.c b/arch/xtensa/src/common/xtensa_checkstack.c
index 6728309448..b666b33355 100644
--- a/arch/xtensa/src/common/xtensa_checkstack.c
+++ b/arch/xtensa/src/common/xtensa_checkstack.c
@@ -55,6 +55,14 @@
 
 #ifdef CONFIG_STACK_COLORATION
 
+#define STACK_ALIGNMENT     16
+
+/* Stack alignment macros */
+
+#define STACK_ALIGN_MASK    (STACK_ALIGNMENT - 1)
+#define STACK_ALIGN_DOWN(a) ((a) & ~STACK_ALIGN_MASK)
+#define STACK_ALIGN_UP(a)   (((a) + STACK_ALIGN_MASK) & ~STACK_ALIGN_MASK)
+
 /****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/
@@ -97,8 +105,8 @@ static size_t do_stackcheck(uintptr_t alloc, size_t size)
 #ifdef CONFIG_TLS_ALIGNED
   DEBUGASSERT((alloc & TLS_STACK_MASK) == 0);
 #endif
-  start = alloc + sizeof(struct tls_info_s);
-  end   = (alloc + size + 15) & ~15;
+  start = STACK_ALIGN_UP(alloc + sizeof(struct tls_info_s));
+  end   = STACK_ALIGN_DOWN(alloc + size);
 
   /* Get the adjusted size based on the top and bottom of the stack */
 
diff --git a/arch/xtensa/src/common/xtensa_createstack.c b/arch/xtensa/src/common/xtensa_createstack.c
index 92fec43e71..19a6cbd86f 100644
--- a/arch/xtensa/src/common/xtensa_createstack.c
+++ b/arch/xtensa/src/common/xtensa_createstack.c
@@ -269,10 +269,6 @@ int up_create_stack(FAR struct tcb_s *tcb, size_t stack_size, uint8_t ttype)
 
       memset(tcb->stack_alloc_ptr, 0, sizeof(struct tls_info_s));
 
-      board_autoled_on(LED_STACKCREATED);
-      return OK;
-    }
-
 #ifdef CONFIG_STACK_COLORATION
       /* If stack debug is enabled, then fill the stack with a
        * recognizable value that we can use later to test for high
@@ -284,6 +280,10 @@ int up_create_stack(FAR struct tcb_s *tcb, size_t stack_size, uint8_t ttype)
                      tcb->adj_stack_size - sizeof(struct tls_info_s));
 #endif
 
+      board_autoled_on(LED_STACKCREATED);
+      return OK;
+    }
+
   return ERROR;
 }
 
@@ -298,17 +298,26 @@ int up_create_stack(FAR struct tcb_s *tcb, size_t stack_size, uint8_t ttype)
 #ifdef CONFIG_STACK_COLORATION
 void up_stack_color(FAR void *stackbase, size_t nbytes)
 {
-  /* Take extra care that we do not write outsize the stack boundaries */
+  uintptr_t start;
+  uintptr_t end;
+  size_t nwords;
+  FAR uint32_t *ptr;
 
-  uint32_t *stkptr = (uint32_t *)(((uintptr_t)stackbase + 15) & ~15);
-  uintptr_t stkend = (((uintptr_t)stackbase + nbytes) & ~15);
-  size_t    nwords = (stkend - (uintptr_t)stackbase) >> 2;
+  /* Take extra care that we do not write outside the stack boundaries */
+
+  start = STACK_ALIGN_UP((uintptr_t)stackbase);
+  end   = STACK_ALIGN_DOWN((uintptr_t)stackbase + nbytes);
+
+  /* Get the adjusted size based on the top and bottom of the stack */
+
+  nwords = (end - start) >> 2;
+  ptr  = (FAR uint32_t *)start;
 
   /* Set the entire stack to the coloration value */
 
   while (nwords-- > 0)
     {
-      *stkptr++ = STACK_COLOR;
+      *ptr++ = STACK_COLOR;
     }
 }
 #endif