diff --git a/tests/scripts/generate_code.py b/tests/scripts/generate_code.py
index f59eb7683..c6fc03f53 100644
--- a/tests/scripts/generate_code.py
+++ b/tests/scripts/generate_code.py
@@ -328,8 +328,8 @@ def parse_functions(funcs_f):
             function_idx += 1
 
     ifdef, endif = gen_deps(suite_deps)
-    func_code = ifdef + suite_functions + endif
-    return dispatch_code, suite_headers, func_code, func_info
+    func_code = ifdef + suite_headers + suite_functions + endif
+    return suite_deps, dispatch_code, func_code, func_info
 
 
 def escaped_split(str, ch):
@@ -443,13 +443,14 @@ else
     return exp_code
 
 
-def gen_from_test_data(data_f, out_data_f, func_info):
+def gen_from_test_data(data_f, out_data_f, func_info, suite_deps):
     """
     Generates dependency checks, expression code and intermediate data file from test data file.
     
     :param data_f: 
     :param out_data_f:
     :param func_info: 
+    :param suite_deps:
     :return: 
     """
     unique_deps = []
@@ -500,7 +501,23 @@ def gen_from_test_data(data_f, out_data_f, func_info):
     if len(expression_code) == 0:
         expression_code = '(void) exp_id;\n'
         expression_code += '(void) out_value;\n'
-
+    ifdef = gen_deps_one_line(suite_deps)
+    if len(suite_deps):
+        dep_check_code = '''
+{ifdef}
+{code}
+#else
+(void) dep_id;
+#endif
+'''.format(ifdef=ifdef, code=dep_check_code)
+        expression_code = '''
+{ifdef}
+{code}
+#else
+(void) exp_id;
+(void) out_value;
+#endif
+'''.format(ifdef=ifdef, code=expression_code)
     return dep_check_code, expression_code
 
 
@@ -539,11 +556,10 @@ def generate_code(funcs_file, data_file, template_file, platform_file, help_file
 
     # Function code
     with open(funcs_file, 'r') as funcs_f, open(data_file, 'r') as data_f, open(out_data_file, 'w') as out_data_f:
-        dispatch_code, func_headers, func_code, func_info = parse_functions(funcs_f)
-        snippets['function_headers'] = func_headers
+        suite_deps, dispatch_code, func_code, func_info = parse_functions(funcs_f)
         snippets['functions_code'] = func_code
         snippets['dispatch_code'] = dispatch_code
-        dep_check_code, expression_code = gen_from_test_data(data_f, out_data_f, func_info)
+        dep_check_code, expression_code = gen_from_test_data(data_f, out_data_f, func_info, suite_deps)
         snippets['dep_check_code'] = dep_check_code
         snippets['expression_code'] = expression_code
 
diff --git a/tests/suites/desktop_test.function b/tests/suites/desktop_test.function
index 9c9a0b2d4..4c790a85e 100644
--- a/tests/suites/desktop_test.function
+++ b/tests/suites/desktop_test.function
@@ -389,6 +389,7 @@ int execute_tests( int argc , const char ** argv )
     const char **test_files = NULL;
     int testfile_count = 0;
     int option_verbose = 0;
+    int function_id = 0;
 
     /* Other Local variables */
     int arg_index = 1;
@@ -562,11 +563,14 @@ int execute_tests( int argc , const char ** argv )
                 }
 #endif /* __unix__ || __APPLE__ __MACH__ */
 
-                ret = convert_params( cnt - 1, params + 1, int_params );
-                if ( DISPATCH_TEST_SUCCESS == ret )
+                function_id = strtol( params[0], NULL, 10 );
+                if ( (ret = check_test( function_id )) == DISPATCH_TEST_SUCCESS )
                 {
-                    int function_id = strtol( params[0], NULL, 10 );
-                    ret = dispatch_test( function_id, (void **)( params + 1 ) );
+                    ret = convert_params( cnt - 1, params + 1, int_params );
+                    if ( DISPATCH_TEST_SUCCESS == ret )
+                    {
+                        ret = dispatch_test( function_id, (void **)( params + 1 ) );
+                    }
                 }
 
 #if defined(__unix__) || (defined(__APPLE__) && defined(__MACH__))
diff --git a/tests/suites/embedded_test.function b/tests/suites/embedded_test.function
index 4436ccbdd..312cf9125 100644
--- a/tests/suites/embedded_test.function
+++ b/tests/suites/embedded_test.function
@@ -371,6 +371,8 @@ int execute_tests( int args, const char ** argv )
             /* Read function id */
             function_id = *p;
             INCR_ASSERT( p, data, data_len, sizeof( uint8_t ) );
+            if ( ( ret = check_test( function_id ) ) != DISPATCH_TEST_SUCCESS )
+                break;
 
             /* Read number of parameters */
             count = *p;
diff --git a/tests/suites/main_test.function b/tests/suites/main_test.function
index 0dcab7d69..e294e3621 100644
--- a/tests/suites/main_test.function
+++ b/tests/suites/main_test.function
@@ -40,8 +40,6 @@
 
 #define TEST_SUITE_ACTIVE
 
-{function_headers}
-
 {functions_code}
 
 #line {line_no} "suites/main_test.function"
@@ -151,6 +149,35 @@ int dispatch_test( int func_idx, void ** params )
 }}
 
 
+/**
+ * \brief       Checks if test function is supported
+ *
+ * \param exp_id    Test function index.
+ *
+ * \return       DISPATCH_TEST_SUCCESS if found
+ *               DISPATCH_TEST_FN_NOT_FOUND if not found
+ *               DISPATCH_UNSUPPORTED_SUITE if not compile time enabled.
+ */
+int check_test( int func_idx )
+{{
+    int ret = DISPATCH_TEST_SUCCESS;
+    TestWrapper_t fp = NULL;
+
+    if ( func_idx < (int)( sizeof(test_funcs)/sizeof( TestWrapper_t ) ) )
+    {{
+        fp = test_funcs[func_idx];
+        if ( fp == NULL )
+            ret = ( DISPATCH_UNSUPPORTED_SUITE );
+    }}
+    else
+    {{
+        ret = ( DISPATCH_TEST_FN_NOT_FOUND );
+    }}
+
+    return( ret );
+}}
+
+
 {platform_code}
 
 #line {line_no} "suites/main_test.function"