diff --git a/os/oslib/include/chmemheaps.h b/os/oslib/include/chmemheaps.h
index 359774956..38e7a7574 100644
--- a/os/oslib/include/chmemheaps.h
+++ b/os/oslib/include/chmemheaps.h
@@ -160,6 +160,7 @@ extern "C" {
   void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align);
   void chHeapFree(void *p);
   size_t chHeapStatus(memory_heap_t *heapp, size_t *totalp, size_t *largestp);
+  bool chHeapIntegrityCheck(memory_heap_t *heapp);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/oslib/src/chmemheaps.c b/os/oslib/src/chmemheaps.c
index e61f4e40e..069cefae1 100644
--- a/os/oslib/src/chmemheaps.c
+++ b/os/oslib/src/chmemheaps.c
@@ -415,7 +415,7 @@ size_t chHeapStatus(memory_heap_t *heapp, size_t *totalp, size_t *largestp) {
  */
 bool chHeapIntegrityCheck(memory_heap_t *heapp) {
   bool result = false;
-  heap_header_t *hp;
+  heap_header_t *hp, *prevhp;
 
   /* If an heap is not specified then the default system header is used.*/
   if (heapp == NULL) {
@@ -430,9 +430,22 @@ bool chHeapIntegrityCheck(memory_heap_t *heapp) {
   /* Taking heap mutex.*/
   H_LOCK(heapp);
 
+  prevhp = NULL;
   hp = &heapp->header;
   while ((hp = H_FREE_NEXT(hp)) != NULL) {
 
+    /* Order violation or loop.*/
+    if (hp <= prevhp) {
+      result = true;
+      break;
+    }
+
+    /* Checking pointer alignment.*/
+    if (!MEM_IS_ALIGNED(hp, CH_HEAP_ALIGNMENT)) {
+      result = true;
+      break;
+    }
+
     /* Validating the found free block.*/
     if (!chMemIsAreaWithinX(&heapp->region,
                             (void *)hp,
@@ -440,6 +453,8 @@ bool chHeapIntegrityCheck(memory_heap_t *heapp) {
       result = true;
       break;
     }
+
+    prevhp = hp;
   }
 
   /* Releasing the heap mutex.*/
diff --git a/readme.txt b/readme.txt
index 5f60ec17d..1f1a1d1e0 100644
--- a/readme.txt
+++ b/readme.txt
@@ -74,6 +74,7 @@
 *****************************************************************************
 
 *** Next ***
+- NEW: Added new function chHeapIntegrityCheck().
 - NEW: Added EFL driver implementation for STM32G4xx.
 - NEW: Function chCoreGetStatusX() changed to return a memory region object
        instead of a simple size.