diff --git a/.gitmodules b/.gitmodules
index f76cc842a0..56038c1026 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,3 +4,6 @@
 [submodule "FreeRTOS-Plus/Test/CMock"]
 	path = FreeRTOS-Plus/Test/CMock
 	url = https://github.com/ThrowTheSwitch/CMock
+[submodule "FreeRTOS/Test/CMock/CMock"]
+	path = FreeRTOS/Test/CMock/CMock
+	url = https://github.com/ThrowTheSwitch/CMock.git
diff --git a/FreeRTOS/Test/CMock/CMakeLists.txt b/FreeRTOS/Test/CMock/CMakeLists.txt
new file mode 100644
index 0000000000..0b1b83694d
--- /dev/null
+++ b/FreeRTOS/Test/CMock/CMakeLists.txt
@@ -0,0 +1,74 @@
+cmake_minimum_required ( VERSION 3.13.0 )
+project ( "FreeRTOS Unit Tests"
+          VERSION 1.0.0
+          LANGUAGES C )
+
+# Allow the project to be organized into folders.
+set_property( GLOBAL PROPERTY USE_FOLDERS ON )
+
+# Use C90.
+set( CMAKE_C_STANDARD 90 )
+set( CMAKE_C_STANDARD_REQUIRED ON )
+
+# Do not allow in-source build.
+if( ${PROJECT_SOURCE_DIR} STREQUAL ${PROJECT_BINARY_DIR} )
+    message( FATAL_ERROR "In-source build is not allowed. Please build in a separate directory, such as ${PROJECT_SOURCE_DIR}/build." )
+endif()
+
+# Set global path variables.
+get_filename_component(__MODULE_ROOT_DIR "${CMAKE_CURRENT_LIST_DIR}/../../.." ABSOLUTE)
+set(MODULE_ROOT_DIR ${__MODULE_ROOT_DIR} CACHE INTERNAL "FreeRTOS root.")
+
+# Configure options to always show in CMake GUI.
+option( BUILD_CLONE_SUBMODULES
+        "Set this to ON to automatically clone any required Git submodules. When OFF, submodules must be manually cloned."
+        ON )
+
+# Set output directories.
+set( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin )
+set( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib )
+set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib )
+
+# Define a CMock resource path.
+set( CMOCK_DIR ${MODULE_ROOT_DIR}/FreeRTOS/Test/CMock/CMock CACHE INTERNAL "CMock library source directory." )
+
+# Include CMock build configuration.
+include( cmock_build.cmake )
+
+# Check if the CMock source directory exists, and if not present, clone the submodule
+# if BUILD_CLONE_SUBMODULES configuration is enabled.
+if( NOT EXISTS ${CMOCK_DIR}/src )
+    # Attempt to clone CMock.
+    if( ${BUILD_CLONE_SUBMODULES} )
+        clone_cmock()
+    else()
+        message( FATAL_ERROR "The required submodule CMock does not exist. Either clone it manually, or set BUILD_CLONE_SUBMODULES to 1 to automatically clone it during build." )
+    endif()
+endif()
+
+# Add unit test and coverage configuration.
+
+# Use CTest utility for managing test runs. This has to be added BEFORE
+# defining test targets with add_test()
+enable_testing()
+
+# Add build targets for CMock and Unit, required for unit testing.
+add_cmock_targets()
+
+# Add function to enable CMock based tests and coverage.
+include( ${MODULE_ROOT_DIR}/tools/cmock/create_test.cmake )
+
+# Include build configuration for unit tests.
+include( queue/queue.cmake )
+
+# List of unit tests
+set( unit_test_list 
+     queue_utest
+)
+
+# Add a target for running coverage on tests.
+add_custom_target( coverage
+    COMMAND ${CMAKE_COMMAND} -P ${MODULE_ROOT_DIR}/tools/cmock/coverage.cmake
+    DEPENDS cmock unity ${unit_test_list}
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+)
diff --git a/FreeRTOS/Test/CMock/CMock b/FreeRTOS/Test/CMock/CMock
new file mode 160000
index 0000000000..afa294982e
--- /dev/null
+++ b/FreeRTOS/Test/CMock/CMock
@@ -0,0 +1 @@
+Subproject commit afa294982e8a28bc06f341cc77fd4276641b42bd
diff --git a/FreeRTOS/Test/CMock/cmock_build.cmake b/FreeRTOS/Test/CMock/cmock_build.cmake
new file mode 100644
index 0000000000..9cd70f7fc3
--- /dev/null
+++ b/FreeRTOS/Test/CMock/cmock_build.cmake
@@ -0,0 +1,59 @@
+# Macro utility to clone the CMock submodule.
+macro( clone_cmock )
+        find_package( Git REQUIRED )
+        message( "Cloning submodule CMock." )
+        execute_process( COMMAND rm -rf ${CMOCK_DIR}
+                         COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive ${MODULE_ROOT_DIR}
+                        WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+                        RESULT_VARIABLE CMOCK_CLONE_RESULT )
+
+        if( NOT ${CMOCK_CLONE_RESULT} STREQUAL "0" )
+                message( FATAL_ERROR "Failed to clone CMock submodule." )
+        endif()
+endmacro()
+
+# Macro utility to add library targets for Unity and CMock to build configuration.
+macro( add_cmock_targets )
+        # Build Configuration for CMock and Unity libraries.
+        list( APPEND CMOCK_INCLUDE_DIRS
+                "${CMOCK_DIR}/vendor/unity/src/"
+                "${CMOCK_DIR}/vendor/unity/extras/fixture/src"
+                "${CMOCK_DIR}/vendor/unity/extras/memory/src"
+                "${CMOCK_DIR}/src"
+        )
+
+        add_library(cmock STATIC
+        "${CMOCK_DIR}/src/cmock.c"
+        )
+
+        set_target_properties(cmock PROPERTIES
+                ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib
+                POSITION_INDEPENDENT_CODE ON
+                COMPILE_FLAGS "-Og"
+                )
+
+        target_include_directories(cmock PUBLIC
+                ${CMOCK_DIR}/src
+                ${CMOCK_DIR}/vendor/unity/src/
+                ${CMOCK_DIR}/examples
+                ${CMOCK_INCLUDE_DIRS}
+                )
+
+        add_library(unity STATIC
+                "${CMOCK_DIR}/vendor/unity/src/unity.c"
+                "${CMOCK_DIR}/vendor/unity/extras/fixture/src/unity_fixture.c"
+                "${CMOCK_DIR}/vendor/unity/extras/memory/src/unity_memory.c"
+                )
+
+        set_target_properties(unity PROPERTIES
+                ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib
+                POSITION_INDEPENDENT_CODE ON
+                )
+
+        target_include_directories(unity PUBLIC
+                ${CMOCK_INCLUDE_DIRS}
+                )
+
+        target_link_libraries(cmock unity)
+endmacro()
+
diff --git a/FreeRTOS/Test/CMock/config/FreeRTOSConfig.h b/FreeRTOS/Test/CMock/config/FreeRTOSConfig.h
new file mode 100644
index 0000000000..8bba808414
--- /dev/null
+++ b/FreeRTOS/Test/CMock/config/FreeRTOSConfig.h
@@ -0,0 +1,125 @@
+/*
+ * FreeRTOS Kernel V10.4.0
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.  See
+ * http://www.freertos.org/a00110.html
+ *----------------------------------------------------------*/
+
+#define configUSE_PREEMPTION					1
+#define configUSE_PORT_OPTIMISED_TASK_SELECTION	1
+#define configUSE_IDLE_HOOK						1
+#define configUSE_TICK_HOOK						1
+#define configUSE_DAEMON_TASK_STARTUP_HOOK		1
+#define configTICK_RATE_HZ						( 1000 ) /* In this non-real time simulated environment the tick frequency has to be at least a multiple of the Win32 tick frequency, and therefore very slow. */
+#define configMINIMAL_STACK_SIZE				( ( unsigned short ) 70 ) /* In this simulated case, the stack only has to hold one small structure as the real stack is part of the win32 thread. */
+#define configTOTAL_HEAP_SIZE					( ( size_t ) ( 52 * 1024 ) )
+#define configMAX_TASK_NAME_LEN					( 12 )
+#define configUSE_TRACE_FACILITY				1
+#define configUSE_16_BIT_TICKS					0
+#define configIDLE_SHOULD_YIELD					1
+#define configUSE_MUTEXES						1
+#define configCHECK_FOR_STACK_OVERFLOW			0
+#define configUSE_RECURSIVE_MUTEXES				1
+#define configQUEUE_REGISTRY_SIZE				20
+#define configUSE_MALLOC_FAILED_HOOK			1
+#define configUSE_APPLICATION_TASK_TAG			1
+#define configUSE_COUNTING_SEMAPHORES			1
+#define configUSE_ALTERNATIVE_API				0
+#define configUSE_QUEUE_SETS					1
+#define configUSE_TASK_NOTIFICATIONS			1
+#define configTASK_NOTIFICATION_ARRAY_ENTRIES		5
+#define configSUPPORT_STATIC_ALLOCATION			1
+#define configINITIAL_TICK_COUNT				( ( TickType_t ) 0 ) /* For test. */
+#define configSTREAM_BUFFER_TRIGGER_LEVEL_TEST_MARGIN 1 /* As there are a lot of tasks running. */
+
+/* Software timer related configuration options. */
+#define configUSE_TIMERS						1
+#define configTIMER_TASK_PRIORITY				( configMAX_PRIORITIES - 1 )
+#define configTIMER_QUEUE_LENGTH				20
+#define configTIMER_TASK_STACK_DEPTH			( configMINIMAL_STACK_SIZE * 2 )
+
+#define configMAX_PRIORITIES					( 7 )
+
+/* Run time stats gathering configuration options. */
+unsigned long ulGetRunTimeCounterValue( void ); /* Prototype of function that returns run time counter. */
+void vConfigureTimerForRunTimeStats( void );	/* Prototype of function that initialises the run time counter. */
+#define configGENERATE_RUN_TIME_STATS			1
+#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() vConfigureTimerForRunTimeStats()
+#define portGET_RUN_TIME_COUNTER_VALUE() ulGetRunTimeCounterValue()
+
+/* Co-routine related configuration options. */
+#define configUSE_CO_ROUTINES 					0
+#define configMAX_CO_ROUTINE_PRIORITIES			( 2 )
+
+/* This demo makes use of one or more example stats formatting functions.  These
+format the raw data provided by the uxTaskGetSystemState() function in to human
+readable ASCII form.  See the notes in the implementation of vTaskList() within
+FreeRTOS/Source/tasks.c for limitations. */
+#define configUSE_STATS_FORMATTING_FUNCTIONS	1
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function.  In most cases the linker will remove unused
+functions anyway. */
+#define INCLUDE_vTaskPrioritySet				1
+#define INCLUDE_uxTaskPriorityGet				1
+#define INCLUDE_vTaskDelete						1
+#define INCLUDE_vTaskCleanUpResources			0
+#define INCLUDE_vTaskSuspend					1
+#define INCLUDE_vTaskDelayUntil					1
+#define INCLUDE_vTaskDelay						1
+#define INCLUDE_uxTaskGetStackHighWaterMark		1
+#define INCLUDE_xTaskGetSchedulerState			1
+#define INCLUDE_xTimerGetTimerDaemonTaskHandle	1
+#define INCLUDE_xTaskGetIdleTaskHandle			1
+#define INCLUDE_xTaskGetHandle					1
+#define INCLUDE_eTaskGetState					1
+#define INCLUDE_xSemaphoreGetMutexHolder		1
+#define INCLUDE_xTimerPendFunctionCall			1
+#define INCLUDE_xTaskAbortDelay					1
+
+/* It is a good idea to define configASSERT() while developing.  configASSERT()
+uses the same semantics as the standard C assert() macro. */
+#define configASSERT( x ) 
+
+#define configINCLUDE_MESSAGE_BUFFER_AMP_DEMO	0
+#if ( configINCLUDE_MESSAGE_BUFFER_AMP_DEMO == 1 )
+	extern void vGenerateCoreBInterrupt( void * xUpdatedMessageBuffer );
+	#define sbSEND_COMPLETED( pxStreamBuffer ) vGenerateCoreBInterrupt( pxStreamBuffer )
+#endif /* configINCLUDE_MESSAGE_BUFFER_AMP_DEMO */
+
+#endif /* FREERTOS_CONFIG_H */
diff --git a/FreeRTOS/Test/CMock/config/portmacro.h b/FreeRTOS/Test/CMock/config/portmacro.h
new file mode 100644
index 0000000000..a2b98a0f7e
--- /dev/null
+++ b/FreeRTOS/Test/CMock/config/portmacro.h
@@ -0,0 +1,149 @@
+/*
+ * FreeRTOS Kernel V10.4.0
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * https://www.FreeRTOS.org
+ * https://github.com/FreeRTOS
+ *
+ * 1 tab == 4 spaces!
+ */
+
+/*
+	Changes from V3.2.3
+
+	+ Modified portENTER_SWITCHING_ISR() to allow use with GCC V4.0.1.
+
+	Changes from V3.2.4
+
+	+ Removed the use of the %0 parameter within the assembler macros and
+	  replaced them with hard coded registers.  This will ensure the
+	  assembler does not select the link register as the temp register as
+	  was occasionally happening previously.
+
+	+ The assembler statements are now included in a single asm block rather
+	  than each line having its own asm block.
+
+	Changes from V4.5.0
+
+	+ Removed the portENTER_SWITCHING_ISR() and portEXIT_SWITCHING_ISR() macros
+	  and replaced them with portYIELD_FROM_ISR() macro.  Application code
+	  should now make use of the portSAVE_CONTEXT() and portRESTORE_CONTEXT()
+	  macros as per the V4.5.1 demo code.
+*/
+
+#ifndef PORTMACRO_H
+#define PORTMACRO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*-----------------------------------------------------------
+ * Port specific definitions.
+ *
+ * The settings in this file configure FreeRTOS correctly for the
+ * given hardware and compiler.
+ *
+ * These settings should not be altered.
+ *-----------------------------------------------------------
+ */
+
+/* Type definitions. */
+#define portCHAR		char
+#define portFLOAT		float
+#define portDOUBLE		double
+#define portLONG		long
+#define portSHORT		short
+#define portSTACK_TYPE	uint32_t
+#define portBASE_TYPE	long
+
+typedef portSTACK_TYPE StackType_t;
+typedef long BaseType_t;
+typedef unsigned long UBaseType_t;
+
+#if( configUSE_16_BIT_TICKS == 1 )
+	typedef uint16_t TickType_t;
+	#define portMAX_DELAY ( TickType_t ) 0xffff
+#else
+	typedef uint32_t TickType_t;
+	#define portMAX_DELAY ( TickType_t ) 0xffffffffUL
+#endif
+/*-----------------------------------------------------------*/
+
+/* Hardware specifics. */
+#define portSTACK_GROWTH			( -1 )
+#define portTICK_PERIOD_MS			( ( TickType_t ) 1000 / configTICK_RATE_HZ )
+#define portBYTE_ALIGNMENT			8
+#define portYIELD()				
+#define portNOP()				
+
+/*
+ * These define the timer to use for generating the tick interrupt.
+ * They are put in this file so they can be shared between "port.c"
+ * and "portisr.c".
+ */
+#define portTIMER_REG_BASE_PTR
+#define portTIMER_CLK_ENABLE_BIT
+#define portTIMER_AIC_CHANNEL
+/*-----------------------------------------------------------*/
+
+/* Task utilities. */
+
+/*
+ * portRESTORE_CONTEXT, portRESTORE_CONTEXT, portENTER_SWITCHING_ISR
+ * and portEXIT_SWITCHING_ISR can only be called from ARM mode, but
+ * are included here for efficiency.  An attempt to call one from
+ * THUMB mode code will result in a compile time error.
+ */
+
+#define portRESTORE_CONTEXT()											
+/*-----------------------------------------------------------*/
+
+#define portSAVE_CONTEXT()										
+
+#define portYIELD_FROM_ISR() 
+
+/* Critical section handling. */
+
+#define portDISABLE_INTERRUPTS()
+
+#define portENABLE_INTERRUPTS()										
+
+
+extern void vPortEnterCritical( void );
+extern void vPortExitCritical( void );
+
+#define portENTER_CRITICAL()		
+#define portEXIT_CRITICAL()			
+
+#undef portUSING_MPU_WRAPPERS
+
+/*-----------------------------------------------------------*/
+
+/* Task function macros as described on the FreeRTOS.org WEB site. */
+#define portTASK_FUNCTION_PROTO( vFunction, pvParameters ) void vFunction( void *pvParameters )
+#define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters )
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PORTMACRO_H */
+
diff --git a/FreeRTOS/Test/CMock/queue/queue.cmake b/FreeRTOS/Test/CMock/queue/queue.cmake
new file mode 100644
index 0000000000..6ad30b9164
--- /dev/null
+++ b/FreeRTOS/Test/CMock/queue/queue.cmake
@@ -0,0 +1,118 @@
+# =============================  Queue unit tests  ===========================
+project( "queue" )
+set(project_name "queue")
+set(utest_name "${project_name}_utest")
+set(utest_source "${project_name}_utest.c")
+set(utest_yml "${project_name}.yml")
+# =====================  Create your mock here  (edit)  ========================
+
+# clear the original variables
+set( mock_list "" )
+set( mock_include_list "" )
+set( mock_define_list "" )
+set( real_source_files "" )
+set( real_include_directories "" )
+set( test_include_directories "" )
+set( utest_link_list "" )
+set( utest_dep_list "" )
+set(mock_dir "wrapper_mocks")
+
+
+# list of files to preprocess
+list(APPEND preprocessed_mock_list
+    "${MODULE_ROOT_DIR}/FreeRTOS/Source/include/task.h"
+    #"${MODULE_ROOT_DIR}/FreeRTOS/Source/include/portable.h"
+    )
+
+set(preprocess_commands "-I ${MODULE_ROOT_DIR}/FreeRTOS/Source/Include -I ${MODULE_ROOT_DIR}/FreeRTOS/Test/CMock/config" )
+
+
+# list the files to mock here
+list(APPEND mock_list
+    "${MODULE_ROOT_DIR}/FreeRTOS/Source/include/task.h"
+    "${MODULE_ROOT_DIR}/FreeRTOS/Source/include/list.h"
+    #"${MODULE_ROOT_DIR}/FreeRTOS/Source/include/portable.h"
+)
+
+# list the directories your mocks need
+list(APPEND mock_include_list
+         "config"
+         "${MODULE_ROOT_DIR}/FreeRTOS/Source/include"
+         #"${CMAKE_CURRENT_LIST_DIR}/${mock_dir}"
+    )
+
+#list the definitions of your mocks to control what to be included
+list(APPEND mock_define_list
+    portUSING_MPU_WRAPPERS=0
+    )
+
+# ================= Create the library under test here (edit) ==================
+
+# list the files you would like to test here
+list(APPEND real_source_files
+            "${MODULE_ROOT_DIR}/FreeRTOS/Source/queue.c"
+    )
+
+# list the directories the module under test includes
+list(APPEND real_include_directories
+            "config"
+            "${MODULE_ROOT_DIR}/FreeRTOS/Source/include"
+            #"${CMAKE_CURRENT_BINARY_DIR}/${mock_dir}"
+    )
+# =====================  Create UnitTest Code here (edit)  =====================
+
+# list the directories your test needs to include
+list(APPEND test_include_directories
+            "config"
+            "${MODULE_ROOT_DIR}/FreeRTOS/Source/include"
+            "${CMAKE_CURRENT_BINARY_DIR}/${mock_dir}"
+    )
+# =============================  (end edit)  ===================================
+
+set(mock_name "${project_name}_mock")
+set(real_name "${project_name}_real")
+set(pre_mock_name "pre_${mock_name}")
+
+create_mock_list("${mock_name}"
+            "${mock_list}"
+            "${CMAKE_CURRENT_LIST_DIR}/${utest_yml}"
+            "${mock_include_list}"
+            "${mock_define_list}"
+            "${mock_dir}"
+    )
+
+separate_arguments(unix_flags UNIX_COMMAND "${preprocess_commands}")
+
+message(STATUS "Unix Flags: ${unix_flags}")
+
+#preprocess_mock_list(${mock_name}
+#            ${preprocessed_mock_list}
+#            "${unix_flags}"
+#        )
+
+#add_dependencies(${mock_name} ${pre_mock_name} )
+
+
+create_real_library(${real_name}
+            "${real_source_files}"
+            "${real_include_directories}"
+            "${mock_name}"
+    )
+
+list(APPEND utest_link_list
+            -l${mock_name}
+            lib${real_name}.a
+    )
+
+list(APPEND utest_dep_list
+            ${real_name}
+    )
+
+create_test(${utest_name}
+            "${CMAKE_CURRENT_LIST_DIR}/${utest_source}"
+            "${utest_link_list}"
+            "${utest_dep_list}"
+            "${test_include_directories}"
+            "${CMAKE_CURRENT_LIST_DIR}/${utest_yml}"
+            "${mock_dir}"
+    )
diff --git a/FreeRTOS/Test/CMock/queue/queue.yml b/FreeRTOS/Test/CMock/queue/queue.yml
new file mode 100644
index 0000000000..e99d6f2a37
--- /dev/null
+++ b/FreeRTOS/Test/CMock/queue/queue.yml
@@ -0,0 +1,33 @@
+:cmock:
+  :mock_prefix: mock_
+  :mock_path: wrapper_mocks
+  :when_no_prototypes: :warn
+  :treat_externs: :include
+  :enforce_strict_ordering: TRUE
+  :plugins:
+    - :ignore
+    - :ignore_arg
+    - :expect_any_args
+    - :array
+    - :callback
+    - :return_thru_ptr
+  :callback_include_count: true # include a count arg when calling the callback
+  :callback_after_arg_check: false # check arguments before calling the callback
+  :treat_as:
+    uint8:    HEX8
+    uint16:   HEX16
+    uint32:   UINT32
+    int8:     INT8
+    bool:     UINT8
+  :includes:        # This will add these includes to each mock.
+    - <stdbool.h>
+    - "FreeRTOS.h"
+  :treat_externs: :exclude  # Now the extern-ed functions will be mocked.
+  :weak: __attribute__((weak))
+  :verbosity: 3
+  :attributes:
+    - PRIVILEGED_FUNCTION
+  :strippables:
+    - PRIVILEGED_FUNCTION
+    - portDONT_DISCARD
+  :treat_externs: :include
diff --git a/FreeRTOS/Test/CMock/queue/queue_utest.c b/FreeRTOS/Test/CMock/queue/queue_utest.c
new file mode 100644
index 0000000000..0f568b58bb
--- /dev/null
+++ b/FreeRTOS/Test/CMock/queue/queue_utest.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://aws.amazon.com/freertos
+ * http://www.FreeRTOS.org
+ */
+
+/* C runtime includes. */
+#include <stdlib.h>
+#include <stdbool.h>
+
+/* Queue includes */
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "queue.h"
+
+/* Test includes. */
+#include "unity.h"
+
+/* Mock includes. */
+#include "mock_task.h"
+
+/* ============================  GLOBAL VARIABLES =========================== */
+static uint16_t usMallocFreeCalls = 0;
+
+/* ==========================  CALLBACK FUNCTIONS =========================== */
+
+void * pvPortMalloc( size_t xSize )
+{
+    return malloc(xSize);
+}
+void vPortFree( void * pv )
+{
+    return free(pv);
+}
+/*******************************************************************************
+ * Unity fixtures
+ ******************************************************************************/
+void setUp( void )
+{
+}
+
+/* called before each testcase */
+void tearDown( void )
+{
+    TEST_ASSERT_EQUAL_INT_MESSAGE( 0, usMallocFreeCalls,
+                                   "free is not called the same number of times as malloc,"
+                                   "you might have a memory leak!!" );
+    usMallocFreeCalls = 0;
+}
+
+/* called at the beginning of the whole suite */
+void suiteSetUp()
+{
+}
+
+/* called at the end of the whole suite */
+int suiteTearDown( int numFailures )
+{
+    return numFailures;
+}
+
+/*!
+ * @brief xQueueCreate happy path.
+ *
+ */
+void test_xQueueCreate_Success( void )
+{
+    QueueHandle_t xQueue = xQueueCreate(1 , 1);
+    TEST_ASSERT_NOT_EQUAL( NULL, xQueue );
+}
diff --git a/scripts/test-run.sh b/scripts/test-run.sh
new file mode 100755
index 0000000000..1173df43a5
--- /dev/null
+++ b/scripts/test-run.sh
@@ -0,0 +1,9 @@
+#!/bin/bash
+
+echo "Running tests..."
+SOURCE_DIR=FreeRTOS/Test/CMock
+BUILD_DIR=FreeRTOS/Test/CMock/build
+cmake -DBUILD_CLONE_SUBMODULES=0 -S ${SOURCE_DIR} -B ${BUILD_DIR} && make -C ${BUILD_DIR} && ${BUILD_DIR}/bin/tests/queue_utest
+TEST_RESULT=$?
+echo "Done"
+exit ${TEST_RESULT}
diff --git a/scripts/test-setup.sh b/scripts/test-setup.sh
new file mode 100755
index 0000000000..be82f26353
--- /dev/null
+++ b/scripts/test-setup.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+echo "Setting up test environment..."
+git -C FreeRTOS submodule update --init --recursive
+RC=$?
+echo "Done"
+exit $RC
diff --git a/tools/cmock/coverage.cmake b/tools/cmock/coverage.cmake
new file mode 100644
index 0000000000..e035ea4c7d
--- /dev/null
+++ b/tools/cmock/coverage.cmake
@@ -0,0 +1,70 @@
+# Taken from amazon-freertos repository
+cmake_minimum_required(VERSION 3.13)
+set(BINARY_DIR ${CMAKE_BINARY_DIR})
+# reset coverage counters
+execute_process(
+            COMMAND lcov --directory ${CMAKE_BINARY_DIR}
+                         --base-directory ${CMAKE_BINARY_DIR}
+                         --zerocounters
+
+            COMMAND mkdir -p  ${CMAKE_BINARY_DIR}/coverage
+        )
+# make the initial/baseline capture a zeroed out files
+execute_process( COMMAND lcov --directory ${CMAKE_BINARY_DIR}
+                         --base-directory ${CMAKE_BINARY_DIR}
+                         --initial
+                         --capture
+                         --rc lcov_branch_coverage=1
+                         --rc genhtml_branch_coverage=1
+                         --output-file=${CMAKE_BINARY_DIR}/base_coverage.info
+        )
+file(GLOB files "${CMAKE_BINARY_DIR}/bin/tests/*")
+
+set(REPORT_FILE ${CMAKE_BINARY_DIR}/utest_report.txt)
+file(WRITE ${REPORT_FILE} "")
+# execute all files in bin directory, gathering the output to show it in CI
+foreach(testname ${files})
+    get_filename_component(test
+                           ${testname}
+                           NAME_WLE
+            )
+    message("Running ${testname}")
+    execute_process(COMMAND ${testname} OUTPUT_FILE ${CMAKE_BINARY_DIR}/${test}_out.txt)
+
+    file(READ ${CMAKE_BINARY_DIR}/${test}_out.txt CONTENTS)
+    file(APPEND ${REPORT_FILE} "${CONTENTS}")
+endforeach()
+
+# generate Junit style xml output
+execute_process(COMMAND ruby
+    ${CMOCK_DIR}/vendor/unity/auto/parse_output.rb
+                    -xml ${REPORT_FILE}
+                    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+            )
+
+# capture data after running the tests
+execute_process(
+            COMMAND lcov --capture
+                         --rc lcov_branch_coverage=1
+                         --rc genhtml_branch_coverage=1
+                         --base-directory ${CMAKE_BINARY_DIR}
+                         --directory ${CMAKE_BINARY_DIR}
+                         --output-file ${CMAKE_BINARY_DIR}/second_coverage.info
+        )
+
+# combile baseline results (zeros) with the one after running the tests
+execute_process(
+            COMMAND lcov --base-directory ${CMAKE_BINARY_DIR}
+                         --directory ${CMAKE_BINARY_DIR}
+                         --add-tracefile ${CMAKE_BINARY_DIR}/base_coverage.info
+                         --add-tracefile ${CMAKE_BINARY_DIR}/second_coverage.info
+                         --output-file ${CMAKE_BINARY_DIR}/coverage.info
+                         --no-external
+                         --rc lcov_branch_coverage=1
+        )
+execute_process(
+            COMMAND genhtml --rc lcov_branch_coverage=1
+                            --branch-coverage
+                            --output-directory ${CMAKE_BINARY_DIR}/coverage
+                                ${CMAKE_BINARY_DIR}/coverage.info
+        )
diff --git a/tools/cmock/create_test.cmake b/tools/cmock/create_test.cmake
new file mode 100644
index 0000000000..53edb09858
--- /dev/null
+++ b/tools/cmock/create_test.cmake
@@ -0,0 +1,171 @@
+# Taken from amazon-freertos repository
+
+#function to create the test executable
+function(create_test test_name
+                     test_src
+                     link_list
+                     dep_list
+                     include_list
+                     config
+                     mock_dir)
+    set(mocks_dir "${CMAKE_CURRENT_BINARY_DIR}/${mock_dir}")
+    include (CTest)
+    get_filename_component(test_src_absolute ${test_src} ABSOLUTE)
+    add_custom_command(OUTPUT ${test_name}_runner.c
+                  COMMAND ruby
+                    ${CMOCK_DIR}/vendor/unity/auto/generate_test_runner.rb
+                    ${config}
+                    ${test_src_absolute}
+                    ${test_name}_runner.c
+                  DEPENDS ${test_src}
+        )
+    add_executable(${test_name} ${test_src} ${test_name}_runner.c)
+    set_target_properties(${test_name} PROPERTIES
+            COMPILE_FLAG "-O0 -ggdb"
+            RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin/tests"
+            INSTALL_RPATH_USE_LINK_PATH TRUE
+            LINK_FLAGS " \
+                -Wl,-rpath,${CMAKE_BINARY_DIR}/lib \
+                -Wl,-rpath,${CMAKE_CURRENT_BINARY_DIR}/lib"
+        )
+    target_include_directories(${test_name} PUBLIC
+                               ${mocks_dir}
+                               ${include_list}
+        )
+
+    target_link_directories(${test_name} PUBLIC
+                            ${CMAKE_CURRENT_BINARY_DIR}
+        )
+
+    # link all libraries sent through parameters
+    foreach(link IN LISTS link_list)
+        target_link_libraries(${test_name} ${link})
+    endforeach()
+
+    # add dependency to all the dep_list parameter
+    foreach(dependency IN LISTS dep_list)
+        add_dependencies(${test_name} ${dependency})
+        target_link_libraries(${test_name} ${dependency})
+    endforeach()
+    target_link_libraries(${test_name} -lgcov unity)
+    target_link_directories(${test_name}  PUBLIC
+                            ${CMAKE_CURRENT_BINARY_DIR}/lib
+            )
+    add_test(NAME ${test_name}
+             COMMAND ${CMAKE_BINARY_DIR}/bin/tests/${test_name}
+             WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+            )
+endfunction()
+
+# Run the C preprocessor on target files.
+# Takes a CMAKE list of arguments to pass to the C compiler
+function(preprocess_mock_list mock_name file_list compiler_args)
+    set_property(GLOBAL PROPERTY ${mock_name}_processed TRUE)
+    foreach (target_file IN LISTS file_list)
+        # Has to be TARGET ALL so the file is pre-processed before CMOCK
+        # is executed on the file.
+        add_custom_command(OUTPUT ${target_file}.backup
+            COMMAND scp ${target_file} ${target_file}.backup
+            VERBATIM COMMAND ${CMAKE_C_COMPILER} -E ${compiler_args} ${target_file} > ${target_file}.out
+        )
+        add_custom_target(pre_${mock_name}
+            COMMAND mv ${target_file}.out ${target_file}
+            DEPENDS ${target_file}.backup
+        )
+    endforeach()
+
+    # Clean up temporary files that were created.
+    # First we test to see if the backup file still exists. If it does we revert
+    # the change made to the original file.
+    foreach (target_file IN LISTS file_list)
+        add_custom_command(TARGET ${mock_name}
+            POST_BUILD
+            COMMAND test ! -e ${target_file}.backup || mv ${target_file}.backup ${target_file}
+        )
+    endforeach()
+endfunction()
+
+# Generates a mock library based on a module's header file
+# places the generated source file in the build directory
+# @param mock_name: name of the target name
+# @param mock_list list of header files to mock
+# @param cmock_config configuration file of the cmock framework
+# @param mock_include_list include list for the target
+# @param mock_define_list special definitions to control compilation
+function(create_mock_list mock_name
+                          mock_list
+                          cmock_config
+                          mock_include_list
+                          mock_define_list
+                          mock_dir)
+    set(mocks_dir "${CMAKE_CURRENT_BINARY_DIR}/${mock_dir}")
+    add_library(${mock_name} SHARED)
+    foreach (mock_file IN LISTS mock_list)
+        get_filename_component(mock_file_abs
+                               ${mock_file}
+                               ABSOLUTE
+                )
+        get_filename_component(mock_file_name
+                               ${mock_file}
+                               NAME_WLE
+                )
+        get_filename_component(mock_file_dir
+                               ${mock_file}
+                               DIRECTORY
+                )
+        add_custom_command (
+                  OUTPUT ${mocks_dir}/mock_${mock_file_name}.c
+                  COMMAND ruby
+                  ${CMOCK_DIR}/lib/cmock.rb
+                  -o${cmock_config} ${mock_file_abs}
+                  WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+                )
+        target_sources(${mock_name} PUBLIC
+                       ${mocks_dir}/mock_${mock_file_name}.c
+        )
+
+        target_include_directories(${mock_name} PUBLIC
+                                   ${mock_file_dir}
+        )
+    endforeach()
+    target_include_directories(${mock_name} PUBLIC
+                               ${mocks_dir}
+                               ${mock_include_list}
+           )
+    set_target_properties(${mock_name} PROPERTIES
+                        LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib
+                        POSITION_INDEPENDENT_CODE ON
+            )
+    target_compile_definitions(${mock_name} PUBLIC
+            ${mock_define_list}
+        )
+    target_link_libraries(${mock_name} cmock unity)
+endfunction()
+
+
+function(create_real_library target
+                             src_file
+                             real_include_list
+                             mock_name)
+    add_library(${target} STATIC
+            ${src_file}
+        )
+    target_include_directories(${target} PUBLIC
+            ${real_include_list}
+        )
+    set_target_properties(${target} PROPERTIES
+                COMPILE_FLAGS "-Wextra -Wpedantic \
+                    -fprofile-arcs -ftest-coverage -fprofile-generate \
+                    -Wno-unused-but-set-variable"
+                LINK_FLAGS "-fprofile-arcs -ftest-coverage \
+                    -fprofile-generate "
+                ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib
+            )
+    if(NOT(mock_name STREQUAL ""))
+        add_dependencies(${target} ${mock_name})
+        target_link_libraries(${target}
+                        -l${mock_name}
+                        -lgcov
+                )
+    endif()
+endfunction()