1 files changed, 9 insertions, 2 deletions
diff --git a/os/common/oslib/include/chmempools.h b/os/common/oslib/include/chmempools.h
index 5c7666f4e..e2bd4277e 100644
--- a/os/common/oslib/include/chmempools.h
+++ b/os/common/oslib/include/chmempools.h
@@ -304,8 +304,15 @@ static inline void chGuardedPoolAddI(guarded_memory_pool_t *gmp, void *objp) {
  * @iclass
  */
 static inline void *chGuardedPoolAllocI(guarded_memory_pool_t *gmp) {
-
-  return chPoolAllocI(&gmp->pool);
+  void *p;
+
+  p = chPoolAllocI(&gmp->pool);
+  if (p != NULL) {
+    chSemFastWaitI(&gmp->sem);
+    chDbgAssert(chSemGetCounterI(&gmp->sem) >= (cnt_t)0,
+                "semaphore out of sync");
+  }
+  return p;
 }
 #endif /* CH_CFG_USE_SEMAPHORES == TRUE */