diff --git a/TESTS/mbed_drivers/stats/main.cpp b/TESTS/mbed_drivers/stats/main.cpp
new file mode 100644
index 00000000000..f1cb47830b1
--- /dev/null
+++ b/TESTS/mbed_drivers/stats/main.cpp
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2013-2016, ARM Limited, All Rights Reserved
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "mbed.h"
+#include "greentea-client/test_env.h"
+#include "unity/unity.h"
+#include "utest/utest.h"
+#include "mbed_stats.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#if !defined(MBED_HEAP_STATS_ENABLED) || !MBED_HEAP_STATS_ENABLED || defined(__ICCARM__)
+  #error [NOT_SUPPORTED] test not supported
+#endif
+
+using namespace utest::v1;
+
+#define ALLOCATION_SIZE_DEFAULT 564
+#define ALLOCATION_SIZE_SMALL   124
+#define ALLOCATION_SIZE_LARGE   790
+#define ALLOCATION_SIZE_FAIL   (1024 * 1024 *1024)
+
+typedef void* (*malloc_cb_t) (uint32_t size);
+
+static void* thunk_malloc(uint32_t size);
+static void* thunk_calloc_1(uint32_t size);
+static void* thunk_calloc_4(uint32_t size);
+static void* thunk_realloc(uint32_t size);
+
+malloc_cb_t malloc_thunk_array[] = {
+    thunk_malloc,
+    thunk_calloc_1,
+    thunk_calloc_4,
+    thunk_realloc,
+};
+
+void test_case_malloc_free_size()
+{
+    printf("Initial print to setup stdio buffers\n");
+    mbed_stats_heap_t stats_start;
+    mbed_stats_heap_t stats_current;
+    void *data;
+
+    mbed_stats_heap_get(&stats_start);
+
+    for (uint32_t i = 0; i < sizeof(malloc_thunk_array) / sizeof(malloc_cb_t); i++) {
+
+        // Allocate memory and assert size change
+        data = malloc_thunk_array[i](ALLOCATION_SIZE_DEFAULT);
+        TEST_ASSERT(data != NULL);
+        mbed_stats_heap_get(&stats_current);
+        uint32_t increase = stats_current.current_size - stats_start.current_size;
+        TEST_ASSERT_EQUAL_UINT32(ALLOCATION_SIZE_DEFAULT, increase);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.total_size + ALLOCATION_SIZE_DEFAULT * (i + 1), stats_current.total_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_cnt + 1, stats_current.alloc_cnt);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_fail_cnt, stats_current.alloc_fail_cnt);
+
+        // Free memory and assert back to starting size
+        free(data);
+        mbed_stats_heap_get(&stats_current);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.current_size, stats_current.current_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_cnt, stats_current.alloc_cnt);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_fail_cnt, stats_current.alloc_fail_cnt);
+    }
+}
+
+void test_case_allocate_zero()
+{
+    mbed_stats_heap_t stats_start;
+    mbed_stats_heap_t stats_current;
+    void *data;
+
+    mbed_stats_heap_get(&stats_start);
+
+    for (uint32_t i = 0; i < sizeof(malloc_thunk_array) / sizeof(malloc_cb_t); i++) {
+
+        // Allocate memory and assert size change
+        data = malloc_thunk_array[i](0);
+        // Return can be NULL
+        mbed_stats_heap_get(&stats_current);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.current_size, stats_current.current_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.total_size, stats_current.total_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_fail_cnt, stats_current.alloc_fail_cnt);
+
+        // Free memory and assert back to starting size
+        free(data);
+        mbed_stats_heap_get(&stats_current);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.current_size, stats_current.current_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_cnt, stats_current.alloc_cnt);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_fail_cnt, stats_current.alloc_fail_cnt);
+    }
+}
+
+void test_case_allocate_fail()
+{
+    mbed_stats_heap_t stats_start;
+    mbed_stats_heap_t stats_current;
+    void *data;
+
+    mbed_stats_heap_get(&stats_start);
+
+    for (uint32_t i = 0; i < sizeof(malloc_thunk_array) / sizeof(malloc_cb_t); i++) {
+
+        // Trigger a failure by trying to allocate a buffer that won't fit
+        data = malloc_thunk_array[i](ALLOCATION_SIZE_FAIL);
+        TEST_ASSERT(data == NULL);
+        mbed_stats_heap_get(&stats_current);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.current_size, stats_current.current_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.total_size, stats_current.total_size);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_cnt, stats_current.alloc_cnt);
+        TEST_ASSERT_EQUAL_UINT32(stats_start.alloc_fail_cnt + i + 1, stats_current.alloc_fail_cnt);
+    }
+}
+
+static void* thunk_malloc(uint32_t size)
+{
+    printf("Malloc thunk\n");
+    return malloc(size);
+}
+
+static void* thunk_calloc_1(uint32_t size)
+{
+    printf("Calloc thunk 1 byte\n");
+    return calloc(size / 1, 1);
+}
+
+static void* thunk_calloc_4(uint32_t size)
+{
+    printf("Calloc thunk 4 bytes\n");
+    return calloc(size / 4, 4);
+}
+
+
+static void* thunk_realloc(uint32_t size)
+{
+    printf("Realloc thunk\n");
+    return realloc(NULL, size);
+}
+
+void test_case_realloc_size()
+{
+    mbed_stats_heap_t stats_start;
+    mbed_stats_heap_t stats_current;
+    uint32_t increase;
+    void *data;
+
+    mbed_stats_heap_get(&stats_start);
+
+    // Allocate memory and assert size change
+    data = realloc(NULL, ALLOCATION_SIZE_DEFAULT);
+    TEST_ASSERT(data != NULL);
+    mbed_stats_heap_get(&stats_current);
+    increase = stats_current.current_size - stats_start.current_size;
+    TEST_ASSERT_EQUAL_UINT32(increase, ALLOCATION_SIZE_DEFAULT);
+
+    // Decrease size and assert size change
+    data = realloc(data, ALLOCATION_SIZE_SMALL);
+    TEST_ASSERT(data != NULL);
+    mbed_stats_heap_get(&stats_current);
+    increase = stats_current.current_size - stats_start.current_size;
+    TEST_ASSERT_EQUAL_UINT32(increase, ALLOCATION_SIZE_SMALL);
+
+    // Increase size and assert size change
+    data = realloc(data, ALLOCATION_SIZE_LARGE);
+    TEST_ASSERT(data != NULL);
+    mbed_stats_heap_get(&stats_current);
+    increase = stats_current.current_size - stats_start.current_size;
+    TEST_ASSERT_EQUAL_UINT32(increase, ALLOCATION_SIZE_LARGE);
+
+    // Free memory and assert back to starting size
+    free(data);
+    mbed_stats_heap_get(&stats_current);
+    TEST_ASSERT_EQUAL_UINT32(stats_start.current_size, stats_current.current_size);
+}
+
+Case cases[] = {
+    Case("malloc and free size", test_case_malloc_free_size),
+    Case("allocate size zero", test_case_allocate_zero),
+    Case("allocation failure", test_case_allocate_fail),
+    Case("realloc size", test_case_realloc_size),
+};
+
+utest::v1::status_t greentea_test_setup(const size_t number_of_cases)
+{
+    GREENTEA_SETUP(20, "default_auto");
+    return greentea_test_setup_handler(number_of_cases);
+}
+
+Specification specification(greentea_test_setup, cases, greentea_test_teardown_handler);
+
+int main()
+{
+    Harness::run(specification);
+}
diff --git a/features/frameworks/utest/source/utest_greentea_handlers.cpp b/features/frameworks/utest/source/utest_greentea_handlers.cpp
index b55407d098f..bfc6858221d 100644
--- a/features/frameworks/utest/source/utest_greentea_handlers.cpp
+++ b/features/frameworks/utest/source/utest_greentea_handlers.cpp
@@ -21,6 +21,7 @@
 #include "greentea-client/test_env.h"
 #include "utest/utest_stack_trace.h"
 #include "utest/utest_serial.h"
+#include "mbed_stats.h"
 
 using namespace utest::v1;
 
@@ -105,7 +106,10 @@ utest::v1::status_t utest::v1::greentea_test_setup_handler(const size_t number_o
 void utest::v1::greentea_test_teardown_handler(const size_t passed, const size_t failed, const failure_t failure)
 {
     UTEST_LOG_FUNCTION();
+    mbed_stats_heap_t heap_stats;
     verbose_test_teardown_handler(passed, failed, failure);
+    mbed_stats_heap_get(&heap_stats);
+    greentea_send_kv("max_heap_usage",heap_stats.max_size);
     greentea_send_kv(TEST_ENV_TESTCASE_SUMMARY, passed, failed);
     int result = !(failed || (failure.reason && !(failure.reason & REASON_IGNORE)));
     GREENTEA_TESTSUITE_RESULT(result);
diff --git a/features/frameworks/utest/source/utest_harness.cpp b/features/frameworks/utest/source/utest_harness.cpp
index 4246d59726f..a796c020f0b 100644
--- a/features/frameworks/utest/source/utest_harness.cpp
+++ b/features/frameworks/utest/source/utest_harness.cpp
@@ -167,11 +167,12 @@ void Harness::raise_failure(const failure_reason_t reason)
     if (test_cases == NULL) return;
 
     utest::v1::status_t fail_status = STATUS_ABORT;
+    if (handlers.test_failure) handlers.test_failure(failure_t(reason, location));
+    if (handlers.case_failure) fail_status = handlers.case_failure(case_current, failure_t(reason, location));
+
     {
         UTEST_ENTER_CRITICAL_SECTION;
 
-        if (handlers.test_failure) handlers.test_failure(failure_t(reason, location));
-        if (handlers.case_failure) fail_status = handlers.case_failure(case_current, failure_t(reason, location));
         if (fail_status != STATUS_IGNORE) case_failed++;
 
         if ((fail_status == STATUS_ABORT) && case_timeout_handle)
diff --git a/features/storage/FEATURE_STORAGE/cfstore/source/cfstore_test.c b/features/storage/FEATURE_STORAGE/cfstore/source/cfstore_test.c
index 700e4e4b253..13aae8b1d0e 100644
--- a/features/storage/FEATURE_STORAGE/cfstore/source/cfstore_test.c
+++ b/features/storage/FEATURE_STORAGE/cfstore/source/cfstore_test.c
@@ -374,7 +374,11 @@ int32_t cfstore_test_delete_all(void)
             CFSTORE_ERRLOG("%s:Error: failed to delete key_name=%s, len=%d\r\n", __func__, key_name, (int) len);
             return ret;
         }
-        CFSTORE_HANDLE_SWAP(prev, next);
+        ret = drv->Close(next);
+        if(ret < ARM_DRIVER_OK){
+            CFSTORE_ERRLOG("%s:Error: failed to close key_name=%s, len=%d\r\n", __func__, key_name, (int) len);
+            return ret;
+        }
     }
     if(ret == ARM_CFSTORE_DRIVER_ERROR_KEY_NOT_FOUND) {
         /* as expected, no more keys have been found by the Find()*/
diff --git a/hal/api/mbed_stats.h b/hal/api/mbed_stats.h
new file mode 100644
index 00000000000..9a0cae89933
--- /dev/null
+++ b/hal/api/mbed_stats.h
@@ -0,0 +1,40 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2016-2016 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_STATS_H
+#define MBED_STATS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    uint32_t current_size;      /**< Bytes allocated currently. */
+    uint32_t max_size;          /**< Max bytes allocated at a given time. */
+    uint32_t total_size;        /**< Cumulative sum of bytes ever allocated. */
+    uint32_t alloc_cnt;         /**< Current number of allocations. */
+    uint32_t alloc_fail_cnt;    /**< Number of failed allocations. */
+} mbed_stats_heap_t;
+
+/**
+ * Fill the passed in structure with heap stats.
+ */
+void mbed_stats_heap_get(mbed_stats_heap_t *stats);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/hal/common/retarget.cpp b/hal/common/retarget.cpp
index a09854fec57..2dea179b398 100644
--- a/hal/common/retarget.cpp
+++ b/hal/common/retarget.cpp
@@ -24,7 +24,9 @@
 #include "SingletonPtr.h"
 #include "PlatformMutex.h"
 #include "mbed_error.h"
+#include "mbed_stats.h"
 #include <stdlib.h>
+#include <string.h>
 #if DEVICE_STDIO_MESSAGES
 #include <stdio.h>
 #endif
@@ -477,6 +479,22 @@ extern "C" WEAK void __cxa_pure_virtual(void) {
 
 #endif
 
+/* Size must be a multiple of 8 to keep alignment */
+typedef struct {
+    uint32_t size;
+    uint32_t pad;
+} alloc_info_t;
+
+static SingletonPtr<PlatformMutex> malloc_stats_mutex;
+static mbed_stats_heap_t heap_stats = {0, 0, 0, 0, 0};
+
+void mbed_stats_heap_get(mbed_stats_heap_t *stats)
+{
+    malloc_stats_mutex->lock();
+    memcpy(stats, &heap_stats, sizeof(mbed_stats_heap_t));
+    malloc_stats_mutex->unlock();
+}
+
 #if defined(TOOLCHAIN_GCC)
 #ifdef   FEATURE_UVISOR
 #include "uvisor-lib/uvisor-lib.h"
@@ -484,17 +502,105 @@ extern "C" WEAK void __cxa_pure_virtual(void) {
 
 #ifndef  FEATURE_UVISOR
 extern "C" {
+
+extern "C" void __malloc_lock( struct _reent *_r );
+extern "C" void __malloc_unlock( struct _reent *_r );
+
 void * __wrap__malloc_r(struct _reent * r, size_t size) {
     extern void * __real__malloc_r(struct _reent * r, size_t size);
+#if !defined(MBED_HEAP_STATS_ENABLED ) || !MBED_HEAP_STATS_ENABLED
     return __real__malloc_r(r, size);
+#else
+
+    malloc_stats_mutex->lock();
+    alloc_info_t *alloc_info = (alloc_info_t*)__real__malloc_r(r, size + sizeof(alloc_info_t));
+    void *ptr = NULL;
+    if (alloc_info != NULL) {
+        alloc_info->size = size;
+        ptr = (void*)(alloc_info + 1);
+        heap_stats.current_size += size;
+        heap_stats.total_size += size;
+        heap_stats.alloc_cnt += 1;
+        if (heap_stats.current_size > heap_stats.max_size) {
+            heap_stats.max_size = heap_stats.current_size;
+        }
+    } else {
+        heap_stats.alloc_fail_cnt += 1;
+    }
+    malloc_stats_mutex->unlock();
+
+    return ptr;
+#endif
 }
 void * __wrap__realloc_r(struct _reent * r, void * ptr, size_t size) {
+#if !defined(MBED_HEAP_STATS_ENABLED ) || !MBED_HEAP_STATS_ENABLED
     extern void * __real__realloc_r(struct _reent * r, void * ptr, size_t size);
     return __real__realloc_r(r, ptr, size);
+#else
+
+    // Implement realloc_r with malloc and free.
+    // The function realloc_r can't be used here directly since
+    // it can call into __wrap__malloc_r (returns ptr + 4) or
+    // resize memory directly (returns ptr + 0).
+
+    // Note - no lock needed since malloc and free are thread safe
+
+    // Get old size
+    uint32_t old_size = 0;
+    if (ptr != NULL) {
+        alloc_info_t *alloc_info = ((alloc_info_t*)ptr) - 1;
+        old_size = alloc_info->size;
+    }
+
+    // Allocate space
+    void *new_ptr = NULL;
+    if (size != 0) {
+        new_ptr = malloc(size);
+    }
+
+    // If the new buffer has been allocated copy the data to it
+    // and free the old buffer
+    if (new_ptr != NULL) {
+        uint32_t copy_size = (old_size < size) ? old_size : size;
+        memcpy(new_ptr, (void*)ptr, copy_size);
+        free(ptr);
+    }
+
+    return new_ptr;
+#endif
 }
 void __wrap__free_r(struct _reent * r, void * ptr) {
     extern void __real__free_r(struct _reent * r, void * ptr);
+#if !defined(MBED_HEAP_STATS_ENABLED ) || !MBED_HEAP_STATS_ENABLED
     __real__free_r(r, ptr);
+#else
+
+    malloc_stats_mutex->lock();
+    alloc_info_t *alloc_info = NULL;
+    if (ptr != NULL) {
+        alloc_info = ((alloc_info_t*)ptr) - 1;
+        heap_stats.current_size -= alloc_info->size;
+        heap_stats.alloc_cnt -= 1;
+    }
+    __real__free_r(r, (void*)alloc_info);
+    malloc_stats_mutex->unlock();
+#endif
+}
+void* __wrap__calloc_r(struct _reent * r, size_t num, size_t size) {
+#if !defined(MBED_HEAP_STATS_ENABLED ) || !MBED_HEAP_STATS_ENABLED
+    extern void* __real__calloc_r(struct _reent * r, size_t num, size_t size);
+    return __real__calloc_r(r, num, size);
+#else
+
+    // Note - no lock needed since malloc is thread safe
+
+    void *ptr = malloc(num * size);
+    if (ptr != NULL) {
+        memset(ptr, 0, num * size);
+    }
+
+    return ptr;
+#endif
 }
 }
 #endif/* FEATURE_UVISOR */
@@ -519,6 +625,85 @@ extern "C" void software_init_hook(void)
 }
 #endif
 
+#if defined(TOOLCHAIN_ARM) && (defined(MBED_HEAP_STATS_ENABLED ) && MBED_HEAP_STATS_ENABLED )
+
+extern "C" void *$Super$$malloc(size_t size);
+extern "C" void *$Super$$realloc(void * ptr, size_t size);
+extern "C" void $Super$$free(void * ptr);
+
+extern "C" void *$Sub$$malloc(size_t size)
+{
+    malloc_stats_mutex->lock();
+    alloc_info_t *alloc_info = (alloc_info_t*)$Super$$malloc(size + sizeof(alloc_info_t));
+    void *ptr = NULL;
+    if (alloc_info != NULL) {
+        alloc_info->size = size;
+        ptr = (void*)(alloc_info + 1);
+        heap_stats.current_size += size;
+        heap_stats.total_size += size;
+        heap_stats.alloc_cnt += 1;
+        if (heap_stats.current_size > heap_stats.max_size) {
+            heap_stats.max_size = heap_stats.current_size;
+        }
+    } else {
+        heap_stats.alloc_fail_cnt += 1;
+    }
+    malloc_stats_mutex->unlock();
+
+    return ptr;
+}
+
+extern "C" void *$Sub$$realloc(void * ptr, size_t size)
+{
+    // Note - no lock needed since malloc and free are thread safe
+
+    // Get old size
+    uint32_t old_size = 0;
+    if (ptr != NULL) {
+        alloc_info_t *alloc_info = ((alloc_info_t*)ptr) - 1;
+        old_size = alloc_info->size;
+    }
+
+    // Allocate space
+    void *new_ptr = NULL;
+    if (size != 0) {
+        new_ptr = malloc(size);
+    }
+
+    // If the new buffer has been allocated copy the data to it
+    // and free the old buffer
+    if (new_ptr != NULL) {
+        uint32_t copy_size = (old_size < size) ? old_size : size;
+        memcpy(new_ptr, (void*)ptr, copy_size);
+        free(ptr);
+    }
+
+    return new_ptr;
+}
+extern "C" void $Sub$$free(void * ptr)
+{
+    malloc_stats_mutex->lock();
+    alloc_info_t *alloc_info = NULL;
+    if (ptr != NULL) {
+        alloc_info = ((alloc_info_t*)ptr) - 1;
+        heap_stats.current_size -= alloc_info->size;
+        heap_stats.alloc_cnt -= 1;
+    }
+    $Super$$free((void*)alloc_info);
+    malloc_stats_mutex->unlock();
+}
+extern "C" void *$Sub$$calloc(size_t num, size_t size)
+{
+    // Note - no lock needed since malloc is thread safe
+    void *ptr = malloc(num * size);
+    if (ptr != NULL) {
+        memset(ptr, 0, num * size);
+    }
+    return ptr;
+}
+
+#endif /* defined(TOOLCHAIN_ARM) && (defined(MBED_HEAP_STATS_ENABLED ) && MBED_HEAP_STATS_ENABLED ) */
+
 // ****************************************************************************
 // mbed_main is a function that is called before main()
 // mbed_sdk_init() is also a function that is called before main(), but unlike
@@ -684,6 +869,8 @@ char* mbed_gets(char*s, int size, FILE *_file){
 #endif
 }
 
+} // namespace mbed
+
 #if defined (__ICCARM__)
 // Stub out locks when an rtos is not present
 extern "C" WEAK void __iar_system_Mtxinit(__iar_Rmtx *mutex) {}
@@ -725,8 +912,6 @@ extern "C" void __env_unlock( struct _reent *_r )
 }
 #endif
 
-} // namespace mbed
-
 void *operator new(std::size_t count)
 {
     void *buffer = malloc(count);
diff --git a/tools/toolchains/gcc.py b/tools/toolchains/gcc.py
index 125715d3d17..5eb11cab3b8 100644
--- a/tools/toolchains/gcc.py
+++ b/tools/toolchains/gcc.py
@@ -39,7 +39,7 @@ class GCC(mbedToolchain):
         'c': ["-std=gnu99"],
         'cxx': ["-std=gnu++98", "-fno-rtti", "-Wvla"],
         'ld': ["-Wl,--gc-sections", "-Wl,--wrap,main",
-            "-Wl,--wrap,_malloc_r", "-Wl,--wrap,_free_r", "-Wl,--wrap,_realloc_r"],
+            "-Wl,--wrap,_malloc_r", "-Wl,--wrap,_free_r", "-Wl,--wrap,_realloc_r", "-Wl,--wrap,_calloc_r"],
     }
 
     def __init__(self, target, options=None, notify=None, macros=None, silent=False, tool_path="", extra_verbose=False):