diff --git a/microros_olimex_e407_extensions/Src/allocators.c b/microros_olimex_e407_extensions/Src/allocators.c
index 9ecf48db..9d41e202 100644
--- a/microros_olimex_e407_extensions/Src/allocators.c
+++ b/microros_olimex_e407_extensions/Src/allocators.c
@@ -14,8 +14,10 @@ void * __freertos_allocate(size_t size, void * state){
 void __freertos_deallocate(void * pointer, void * state){
   (void) state;
   // printf("-- Free %d (prev: %d B)\n",getBlockSize(pointer), xPortGetFreeHeapSize());
-  usedMemory -= getBlockSize(pointer);
-  vPortFree(pointer);
+  if (NULL != pointer){
+    usedMemory -= getBlockSize(pointer);
+    vPortFree(pointer);
+  }
 }
 
 void * __freertos_reallocate(void * pointer, size_t size, void * state){
@@ -23,8 +25,12 @@ void * __freertos_reallocate(void * pointer, size_t size, void * state){
   // printf("-- Realloc %d -> %d (prev: %d B)\n",getBlockSize(pointer),size, xPortGetFreeHeapSize());
   absoluteUsedMemory += size;
   usedMemory += size;
-  usedMemory -= getBlockSize(pointer);
-  return pvPortRealloc(pointer,size);
+  if (NULL != pointer){
+    usedMemory -= getBlockSize(pointer);
+    return pvPortRealloc(pointer,size);
+  } else {
+    return pvPortMalloc(size);
+  }
 }
 
 void * __freertos_zero_allocate(size_t number_of_elements, size_t size_of_element, void * state){