diff --git a/wolfcrypt/src/port/st/stm32.c b/wolfcrypt/src/port/st/stm32.c
index f72d1872f..82408dc58 100644
--- a/wolfcrypt/src/port/st/stm32.c
+++ b/wolfcrypt/src/port/st/stm32.c
@@ -234,7 +234,7 @@ static void wc_Stm32_Hash_Data(STM32_HASH_Context* stmCtx, word32 len)
     if (len > stmCtx->buffLen)
         len = stmCtx->buffLen;
 
-    /* calculate number of 32-bit blocks */
+    /* calculate number of 32-bit blocks - round up */
     blocks = ((len + STM32_HASH_REG_SIZE-1) / STM32_HASH_REG_SIZE);
 #ifdef DEBUG_STM32_HASH
     printf("STM DIN %d blocks\n", blocks);
@@ -268,17 +268,16 @@ int wc_Stm32_Hash_Update(STM32_HASH_Context* stmCtx, word32 algo,
     byte* local = (byte*)stmCtx->buffer;
     int wroteToFifo = 0;
     const word32 fifoSz = (STM32_HASH_FIFO_SIZE * STM32_HASH_REG_SIZE);
-
-    if (blockSize > fifoSz)
-        blockSize = fifoSz;
+    word32 chunkSz;
 
 #ifdef DEBUG_STM32_HASH
     printf("STM Hash Update: algo %x, len %d, blockSz %d\n",
         algo, len, blockSize);
 #endif
+    (void)blockSize;
 
     /* check that internal buffLen is valid */
-    if (stmCtx->buffLen > blockSize) {
+    if (stmCtx->buffLen > (word32)sizeof(stmCtx->buffer)) {
         return BUFFER_E;
     }
 
@@ -288,37 +287,44 @@ int wc_Stm32_Hash_Update(STM32_HASH_Context* stmCtx, word32 algo,
     /* restore hash context or init as new hash */
     wc_Stm32_Hash_RestoreContext(stmCtx, algo);
 
+    chunkSz = fifoSz;
+#ifdef STM32_HASH_FIFO_WORKAROUND
+    /* if FIFO already has bytes written then fill remainder first */
+    if (stmCtx->fifoBytes > 0) {
+        chunkSz -= stmCtx->fifoBytes;
+        stmCtx->fifoBytes = 0;
+    }
+#endif
+
     /* write blocks to FIFO */
     while (len) {
-        word32 fillBlockSz = blockSize, add;
-
-        /* if FIFO already has bytes written then fill remainder first */
-        if (stmCtx->fifoBytes > 0) {
-            fillBlockSz -= stmCtx->fifoBytes;
-            stmCtx->fifoBytes = 0;
-        }
-
-        add = min(len, fillBlockSz - stmCtx->buffLen);
+        word32 add = min(len, chunkSz - stmCtx->buffLen);
         XMEMCPY(&local[stmCtx->buffLen], data, add);
 
         stmCtx->buffLen += add;
         data            += add;
         len             -= add;
 
-        if (len > 0 && stmCtx->buffLen == fillBlockSz) {
+    #ifdef STM32_HASH_FIFO_WORKAROUND
+        /* We cannot leave the FIFO full and do save/restore
+         * the last must be large enough to flush block from FIFO */
+        if (stmCtx->buffLen + len <= fifoSz * 2) {
+            chunkSz = fifoSz + STM32_HASH_REG_SIZE;
+        }
+    #endif
+
+        if (len >= 0 && stmCtx->buffLen == chunkSz) {
             wc_Stm32_Hash_Data(stmCtx, stmCtx->buffLen);
             wroteToFifo = 1;
+        #ifdef STM32_HASH_FIFO_WORKAROUND
+            if (chunkSz > fifoSz)
+                stmCtx->fifoBytes = chunkSz - fifoSz;
+            chunkSz = fifoSz;
+        #endif
         }
     }
 
     if (wroteToFifo) {
-    #ifdef STM32_HASH_FIFO_WORKAROUND
-        /* If we wrote a block send one more 32-bit to FIFO to trigger start.
-         * The save/restore feature cannot leave 16 deep FIFO filled. */
-        wc_Stm32_Hash_Data(stmCtx, 4);
-        stmCtx->fifoBytes += 4;
-    #endif
-
         /* make sure hash operation is done */
         ret = wc_Stm32_Hash_WaitDone(stmCtx);
 
diff --git a/wolfssl/wolfcrypt/port/st/stm32.h b/wolfssl/wolfcrypt/port/st/stm32.h
index f441072e8..1bcb7749f 100644
--- a/wolfssl/wolfcrypt/port/st/stm32.h
+++ b/wolfssl/wolfcrypt/port/st/stm32.h
@@ -74,10 +74,22 @@
 #if (defined(WOLFSSL_STM32U5) || defined(WOLFSSL_STM32H5) || \
     defined(WOLFSSL_STM32H7)) && !defined(NO_STM32_HASH_FIFO_WORKAROUND)
     /* workaround for hash FIFO to write one extra to finalize */
+    /* RM: Message Data Feeding: Data are entered into the HASH
+     * one 32-bit word at a time, by writing them into the HASH_DIN register.
+     * The current contents of the HASH_DIN register are transferred to the
+     * 16 words input FIFO each time the register is written with new data.
+     * Hence HASH_DIN and the FIFO form a seventeen 32-bit words length FIFO. */
+    #undef  STM32_HASH_BUFFER_SIZE
+    #define STM32_HASH_BUFFER_SIZE 17
+
     #undef  STM32_HASH_FIFO_WORKAROUND
     #define STM32_HASH_FIFO_WORKAROUND
 #endif
 
+#ifndef STM32_HASH_BUFFER_SIZE
+#define STM32_HASH_BUFFER_SIZE STM32_HASH_FIFO_SIZE
+#endif
+
 
 /* STM32 Hash Context */
 typedef struct {
@@ -88,11 +100,13 @@ typedef struct {
     uint32_t HASH_CSR[HASH_CR_SIZE];
 
     /* Hash state / buffers */
-    word32 buffer[STM32_HASH_FIFO_SIZE]; /* partial word buffer */
+    word32 buffer[STM32_HASH_BUFFER_SIZE]; /* partial word buffer */
     word32 buffLen; /* partial word remain */
     word32 loLen;   /* total update bytes
                  (only lsb 6-bits is used for nbr valid bytes in last word) */
+#ifdef STM32_HASH_FIFO_WORKAROUND
     int    fifoBytes; /* number of currently filled FIFO bytes */
+#endif
 } STM32_HASH_Context;