diff --git a/os/rt/include/chthreads.h b/os/rt/include/chthreads.h
index a25fc0e91..84b61a870 100644
--- a/os/rt/include/chthreads.h
+++ b/os/rt/include/chthreads.h
@@ -291,7 +291,7 @@ extern "C" {
                                const char *name,
                                tprio_t prio);
 #if CH_DBG_FILL_THREADS == TRUE
-  void __thd_memfill(uint8_t *startp, uint8_t *endp, uint8_t v);
+  void __thd_stackfill(uint8_t *startp, uint8_t *endp);
 #endif
   thread_t *chThdCreateSuspendedI(const thread_descriptor_t *tdp);
   thread_t *chThdCreateSuspended(const thread_descriptor_t *tdp);
diff --git a/os/rt/src/chdynamic.c b/os/rt/src/chdynamic.c
index 075666b7b..b37e47491 100644
--- a/os/rt/src/chdynamic.c
+++ b/os/rt/src/chdynamic.c
@@ -94,9 +94,7 @@ thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size,
   thread_descriptor_t td = THD_DESCRIPTOR(name, wbase, wend, prio, pf, arg);
 
 #if CH_DBG_FILL_THREADS == TRUE
-  __thd_memfill((uint8_t *)wbase,
-                (uint8_t *)wbase + size,
-                CH_DBG_STACK_FILL_VALUE);
+  __thd_stackfill((uint8_t *)wbase, (uint8_t *)wend);
 #endif
 
   chSysLock();
@@ -152,9 +150,7 @@ thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, const char *name,
   thread_descriptor_t td = THD_DESCRIPTOR(name, wbase, wend, prio, pf, arg);
 
 #if CH_DBG_FILL_THREADS == TRUE
-  __thd_memfill((uint8_t *)wbase,
-                (uint8_t *)wbase + mp->object_size,
-                CH_DBG_STACK_FILL_VALUE);
+  __thd_stackfill((uint8_t *)wbase, (uint8_t *)wend);
 #endif
 
   chSysLock();
diff --git a/os/rt/src/chinstances.c b/os/rt/src/chinstances.c
index 55248b92c..e0417d99c 100644
--- a/os/rt/src/chinstances.c
+++ b/os/rt/src/chinstances.c
@@ -174,9 +174,8 @@ void chInstanceObjectInit(os_instance_t *oip,
     };
 
 #if CH_DBG_FILL_THREADS == TRUE
-    __thd_memfill((uint8_t *)idle_descriptor.wbase,
-                  (uint8_t *)idle_descriptor.wend,
-                  CH_DBG_STACK_FILL_VALUE);
+    __thd_stackfill((uint8_t *)idle_descriptor.wbase,
+                    (uint8_t *)idle_descriptor.wend);
 #endif
 
     /* This thread has the lowest priority in the system, its role is just to
diff --git a/os/rt/src/chthreads.c b/os/rt/src/chthreads.c
index 7bf14073a..f90ffca96 100644
--- a/os/rt/src/chthreads.c
+++ b/os/rt/src/chthreads.c
@@ -130,20 +130,18 @@ thread_t *__thd_object_init(os_instance_t *oip,
 
 #if (CH_DBG_FILL_THREADS == TRUE) || defined(__DOXYGEN__)
 /**
- * @brief   Memory fill utility.
- * @todo    Optimize using the stack align type instead of uint8_t.
+ * @brief   Stack fill utility.
  *
  * @param[in] startp    first address to fill
  * @param[in] endp      last address to fill +1
- * @param[in] v         filler value
  *
  * @notapi
  */
-void __thd_memfill(uint8_t *startp, uint8_t *endp, uint8_t v) {
+void __thd_stackfill(uint8_t *startp, uint8_t *endp) {
 
-  while (startp < endp) {
-    *startp++ = v;
-  }
+  do {
+    *startp++ = CH_DBG_STACK_FILL_VALUE;
+  } while (likely(startp < endp));
 }
 #endif /* CH_DBG_FILL_THREADS */
 
@@ -234,9 +232,7 @@ thread_t *chThdCreateSuspended(const thread_descriptor_t *tdp) {
 #endif
 
 #if CH_DBG_FILL_THREADS == TRUE
-  __thd_memfill((uint8_t *)tdp->wbase,
-                (uint8_t *)tdp->wend,
-                CH_DBG_STACK_FILL_VALUE);
+  __thd_stackfill((uint8_t *)tdp->wbase, (uint8_t *)tdp->wend);
 #endif
 
   chSysLock();
@@ -299,9 +295,7 @@ thread_t *chThdCreate(const thread_descriptor_t *tdp) {
 #endif
 
 #if CH_DBG_FILL_THREADS == TRUE
-  __thd_memfill((uint8_t *)tdp->wbase,
-                (uint8_t *)tdp->wend,
-                CH_DBG_STACK_FILL_VALUE);
+  __thd_stackfill((uint8_t *)tdp->wbase, (uint8_t *)tdp->wend);
 #endif
 
   chSysLock();
@@ -349,9 +343,7 @@ thread_t *chThdCreateStatic(void *wsp, size_t size,
 #endif
 
 #if CH_DBG_FILL_THREADS == TRUE
-  __thd_memfill((uint8_t *)wsp,
-                (uint8_t *)wsp + size,
-                CH_DBG_STACK_FILL_VALUE);
+  __thd_stackfill((uint8_t *)wsp, (uint8_t *)wsp + size);
 #endif
 
   chSysLock();