diff --git a/TESTS/mbedmicro-rtos-mbed/heap_and_stack/main.cpp b/TESTS/mbedmicro-rtos-mbed/heap_and_stack/main.cpp
index a9766f429a5..b2e253a13f8 100644
--- a/TESTS/mbedmicro-rtos-mbed/heap_and_stack/main.cpp
+++ b/TESTS/mbedmicro-rtos-mbed/heap_and_stack/main.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2016, ARM Limited, All Rights Reserved
+ * Copyright (c) 2016-2017, ARM Limited, All Rights Reserved
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License"); you may
@@ -22,11 +22,17 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include "greentea-client/test_env.h"
-#include "cmsis.h"
+
 #include "mbed.h"
-#include "rtos.h"
-#include "mbed_assert.h"
+#include "cmsis.h"
+#include "greentea-client/test_env.h"
+#include "utest/utest.h"
+#include "unity/unity.h"
+
+using utest::v1::Case;
+
+static const int test_timeout = 30;
+
 
 // Amount to malloc for each iteration
 #define MALLOC_TEST_SIZE            256
@@ -38,58 +44,29 @@ extern uint32_t mbed_heap_size;
 extern uint32_t mbed_stack_isr_start;
 extern uint32_t mbed_stack_isr_size;
 
-static uint32_t max_allocation_size = 0;
-
-static bool inrange(uint32_t addr, uint32_t start, uint32_t size);
-static bool rangeinrange(uint32_t addr, uint32_t size, uint32_t start, uint32_t len);
-static bool valid_fill(uint8_t * data, uint32_t size, uint8_t fill);
-static bool allocate_and_fill_heap(void);
-static bool check_and_free_heap(void);
-
-int main (void) {
-    GREENTEA_SETUP(30, "default_auto");
 
-    char c;
-    char * initial_stack = &c;
-    char *initial_heap;
-
-    // Sanity check malloc
-    initial_heap = (char*)malloc(1);
-    if (initial_heap == NULL) {
-        printf("Unable to malloc a single byte\n");
-        GREENTEA_TESTSUITE_RESULT(false);
-    }
+struct linked_list {
+    linked_list * next;
+    uint8_t data[MALLOC_TEST_SIZE];
+};
 
-    if (!inrange((uint32_t)initial_heap, mbed_heap_start, mbed_heap_size)) {
-        printf("Heap in wrong location\n");
-        GREENTEA_TESTSUITE_RESULT(false);
-    }
-    // MSP stack should be very near end (test using within 128 bytes)
-    uint32_t msp = __get_MSP();
-    if (!inrange(msp, mbed_stack_isr_start + mbed_stack_isr_size - 128, 128)) {
-        printf("Interrupt stack in wrong location\n");
-        GREENTEA_TESTSUITE_RESULT(false);
-    }
 
-    // Fully allocate the heap and stack
-    bool ret = true;
-    ret = ret && allocate_and_fill_heap();
-    ret = ret && check_and_free_heap();
 
-    // Force a task switch so a stack check is performed
-    Thread::wait(10);
-
-    printf("Total size dynamically allocated: %lu\n", max_allocation_size);
+/* TODO: add memory layout test.
+ *
+ * The test was skipped for now since not all devices seems to comply with Mbed OS memory.
+ *
+ * @note Mbed OS memory model: https://os.mbed.com/docs/latest/reference/memory.html
+ *
+ */
 
-    GREENTEA_TESTSUITE_RESULT(ret);
-}
 
 /*
  * Return true if addr is in range [start:start+size)
  */
 static bool inrange(uint32_t addr, uint32_t start, uint32_t size)
 {
-    return (addr >= start) && (addr < start + size) ? true : false;
+    return (addr >= start) && (addr < (start + size));
 }
 
 /*
@@ -97,7 +74,7 @@ static bool inrange(uint32_t addr, uint32_t start, uint32_t size)
  */
 static bool rangeinrange(uint32_t addr, uint32_t size, uint32_t start, uint32_t len)
 {
-    if (addr + size > start + len) {
+    if ((addr + size) > (start + len)) {
         return false;
     }
     if (addr < start) {
@@ -107,7 +84,7 @@ static bool rangeinrange(uint32_t addr, uint32_t size, uint32_t start, uint32_t
 }
 
 /*
- * Return true of the region is filled only the the specified fill value
+ * Return true if the region is filled only with the specified value
  */
 static bool valid_fill(uint8_t * data, uint32_t size, uint8_t fill)
 {
@@ -119,66 +96,155 @@ static bool valid_fill(uint8_t * data, uint32_t size, uint8_t fill)
     return true;
 }
 
-struct linked_list {
-    linked_list * next;
-    uint8_t data[MALLOC_TEST_SIZE];
-};
-
-static linked_list *head = NULL;
-static bool allocate_and_fill_heap()
+static void allocate_and_fill_heap(linked_list *&head)
 {
-
     linked_list *current;
 
-    current = (linked_list*)malloc(sizeof(linked_list));
-    if (0 == current) {
-        return false;
-    }
+    current = (linked_list*) malloc(sizeof(linked_list));
+    TEST_ASSERT_NOT_NULL(current);
+
     current->next = NULL;
-    memset((void*)current->data, MALLOC_FILL, sizeof(current->data));
+    memset((void*) current->data, MALLOC_FILL, sizeof(current->data));
 
     // Allocate until malloc returns NULL
-    bool pass = true;
     head = current;
     while (true) {
 
         // Allocate
-        linked_list *temp = (linked_list*)malloc(sizeof(linked_list));
+        linked_list *temp = (linked_list*) malloc(sizeof(linked_list));
+
         if (NULL == temp) {
             break;
         }
-        if (!rangeinrange((uint32_t)temp, sizeof(linked_list), mbed_heap_start, mbed_heap_size)) {
-            printf("Memory allocation out of range\n");
-            pass = false;
-            break;
-        }
+        bool result = rangeinrange((uint32_t) temp, sizeof(linked_list), mbed_heap_start, mbed_heap_size);
+
+        TEST_ASSERT_TRUE_MESSAGE(result, "Memory allocation out of range");
 
         // Init
         temp->next = NULL;
-        memset((void*)temp->data, MALLOC_FILL, sizeof(current->data));
+        memset((void*) temp->data, MALLOC_FILL, sizeof(current->data));
 
         // Add to list
         current->next = temp;
         current = temp;
     }
-    return pass;
 }
 
-static bool check_and_free_heap()
+static void check_and_free_heap(linked_list *head, uint32_t &max_allocation_size)
 {
     uint32_t total_size = 0;
     linked_list * current = head;
-    bool pass = true;
+
     while (current != NULL) {
         total_size += sizeof(linked_list);
-        if (!valid_fill(current->data, sizeof(current->data), MALLOC_FILL)) {
-            pass = false;
-        }
+        bool result = valid_fill(current->data, sizeof(current->data), MALLOC_FILL);
+
+        TEST_ASSERT_TRUE_MESSAGE(result, "Memory fill check failed");
+
         linked_list * next = current->next;
         free(current);
         current = next;
     }
 
     max_allocation_size = total_size;
-    return pass;
+}
+
+/** Test heap allocation
+
+    Given a heap
+    When memory is allocated from heap
+    Then the memory is within heap boundary
+
+ */
+void test_heap_in_range(void)
+{
+    char *initial_heap;
+
+    // Sanity check malloc
+    initial_heap = (char*) malloc(1);
+    TEST_ASSERT_NOT_NULL(initial_heap);
+
+    bool result = inrange((uint32_t) initial_heap, mbed_heap_start, mbed_heap_size);
+
+    TEST_ASSERT_TRUE_MESSAGE(result, "Heap in wrong location");
+    free(initial_heap);
+}
+
+/** Test for Main thread stack
+
+    Given a Main thread and its stack
+    When check Main thread stack pointer
+    Then the SP is within Main stack boundary
+ */
+void test_main_stack_in_range(void)
+{
+    os_thread_t *thread = (os_thread_t*) osThreadGetId();
+
+    uint32_t psp = __get_PSP();
+    uint8_t *stack_mem = (uint8_t*) thread->stack_mem;
+    uint32_t stack_size = thread->stack_size;
+
+    // PSP stack should be somewhere in the middle
+    bool result = inrange(psp, (uint32_t) stack_mem, stack_size);
+
+    TEST_ASSERT_TRUE_MESSAGE(result, "Main stack in wrong location");
+}
+
+/** Test for Scheduler/ISR thread stack
+
+    Given a Scheduler/ISR thread and its stack
+    When check Scheduler/ISR thread stack pointer
+    Then the SP is within Scheduler/ISR stack boundary
+ */
+void test_isr_stack_in_range(void)
+{
+    // MSP stack should be very near end (test using within 128 bytes)
+    uint32_t msp = __get_MSP();
+    bool result = inrange(msp, mbed_stack_isr_start + mbed_stack_isr_size - 128, 128);
+
+    TEST_ASSERT_TRUE_MESSAGE(result, "Interrupt stack in wrong location");
+}
+
+/** Test full heap allocation
+
+    Given a heap and linked_list data structure
+    When linked_list is filled till run out of heap memory
+    Then the memory is properly initialised and freed
+ */
+void test_heap_allocation_free(void)
+{
+    linked_list *head = NULL;
+    uint32_t max_allocation_size = 0;
+
+    // Fully allocate the heap and stack
+    allocate_and_fill_heap(head);
+
+    check_and_free_heap(head, max_allocation_size);
+
+    // Force a task switch so a stack check is performed
+    Thread::wait(10);
+
+    printf("Total size dynamically allocated: %luB\n", max_allocation_size);
+}
+
+
+// Test cases
+Case cases[] = {
+    Case("Test heap in range", test_heap_in_range),
+    Case("Test main stack in range", test_main_stack_in_range),
+    Case("Test isr stack in range", test_isr_stack_in_range),
+    Case("Test heap allocation and free", test_heap_allocation_free)
+};
+
+utest::v1::status_t greentea_test_setup(const size_t number_of_cases)
+{
+    GREENTEA_SETUP(test_timeout, "default_auto");
+    return utest::v1::greentea_test_setup_handler(number_of_cases);
+}
+
+utest::v1::Specification specification(greentea_test_setup, cases);
+
+int main()
+{
+    return !utest::v1::Harness::run(specification);
 }