diff --git a/src/preprocessor.cpp b/src/preprocessor.cpp
index 3112ac386..d2b71d046 100644
--- a/src/preprocessor.cpp
+++ b/src/preprocessor.cpp
@@ -445,5 +445,170 @@ std::string Preprocessor::getcode(const std::string &filedata, std::string cfg)
 
 
 
+std::string Preprocessor::expandMacros(std::string code)
+{
+    // Bail out if there are "#undef" it can cause cppcheck to hang
+    if (code.find("#undef") != std::string::npos)
+        return code;
 
+    // Search for macros and expand them..
+    std::string::size_type defpos = 0;
+    while ((defpos = code.find("#define ", defpos)) != std::string::npos)
+    {
+        if (defpos > 0 && code[defpos-1] != '\n')
+        {
+            defpos += 6;
+            continue;
+        }
+
+        // Get macro..
+        std::string::size_type endpos = code.find("\n", defpos + 6);
+        if (endpos == std::string::npos)
+        {
+            code.erase(defpos);
+            break;
+        }
+
+        // Extract the whole macro into a separate variable "macro" and then erase it from "code"
+        const std::string macro(code.substr(defpos + 8, endpos - defpos - 7));
+        code.erase(defpos, endpos - defpos);
+
+        // Tokenize the macro to make it easier to handle
+        Tokenizer tokenizer;
+        std::istringstream istr(macro.c_str());
+        tokenizer.tokenize(istr, "");
+        if (! tokenizer.tokens())
+            continue;
+        if (! tokenizer.tokens()->isName())
+            continue;
+
+        // Extract macro parameters
+        std::vector<std::string> macroparams;
+        if (Token::Match(tokenizer.tokens(), "%var% ( %var%"))
+        {
+            for (const Token *tok = tokenizer.tokens()->tokAt(2); tok; tok = tok->next())
+            {
+                if (tok->str() == ")")
+                    break;
+                if (tok->isName())
+                    macroparams.push_back(tok->str());
+            }
+        }
+
+        // Expand all macros in the code..
+        const std::string macroname(tokenizer.tokens()->str());
+        std::string::size_type pos1 = defpos;
+        while ((pos1 = code.find(macroname, pos1 + 1)) != std::string::npos)
+        {
+            // Previous char must not be alphanumeric or '_'
+            if (pos1 != 0 && (isalnum(code[pos1-1]) || code[pos1-1] == '_'))
+                continue;
+
+            // The char after the macroname must not be alphanumeric or '_'
+            if (pos1 + macroname.length() < code.length())
+            {
+                std::string::size_type pos2 = pos1 + macroname.length();
+                if (isalnum(code[pos2]) || code[pos2] == '_')
+                    continue;
+            }
+
+            std::vector<std::string> params;
+            std::string::size_type pos2 = pos1 + macroname.length();
+            if (macroparams.size() && pos2 >= code.length())
+                continue;
+            if (macroparams.size())
+            {
+                if (code[pos2] != '(')
+                    continue;
+
+                int parlevel = 0;
+                std::string par;
+                for (; pos2 < code.length(); ++pos2)
+                {
+                    if (code[pos2] == '(')
+                    {
+                        ++parlevel;
+                        if (parlevel == 1)
+                            continue;
+                    }
+                    else if (code[pos2] == ')')
+                    {
+                        --parlevel;
+                        if (parlevel <= 0)
+                        {
+                            params.push_back(par);
+                            break;
+                        }
+                    }
+
+                    if (parlevel == 1 && code[pos2] == ',')
+                    {
+                        params.push_back(par);
+                        par = "";
+                    }
+                    else if (parlevel >= 1)
+                    {
+                        par += std::string(1, code[pos2]);
+                    }
+                }
+            }
+
+            // Same number of parameters..
+            if (params.size() != macroparams.size())
+                continue;
+
+            // Create macro code..
+            std::string macrocode;
+            if (macroparams.empty())
+            {
+                std::string::size_type pos = macro.find(" ");
+                if (pos == std::string::npos)
+                    macrocode = "";
+                else
+                {
+                    macrocode = macro.substr(pos + 1);
+                    if ((pos = macrocode.find_first_of("\r\n")) != std::string::npos)
+                        macrocode.erase(pos);
+                }
+            }
+            else
+            {
+                const Token *tok = tokenizer.tokens();
+                while (tok && tok->str() != ")")
+                    tok = tok->next();
+                if (tok)
+                {
+                    while ((tok = tok->next()) != NULL)
+                    {
+                        std::string str = tok->str();
+                        if (tok->isName())
+                        {
+                            for (unsigned int i = 0; i < macroparams.size(); ++i)
+                            {
+                                if (str == macroparams[i])
+                                {
+                                    str = params[i];
+                                    break;
+                                }
+                            }
+                        }
+                        macrocode += str;
+                        if (Token::Match(tok, "%type% %var%"))
+                            macrocode += " ";
+                    }
+                }
+            }
+
+            // Insert macro code..
+            if (!macroparams.empty())
+                ++pos2;
+
+            code.erase(pos1, pos2 - pos1);
+            code.insert(pos1, macrocode);
+            pos1 += macrocode.length();
+        }
+    }
+
+    return code;
+}
 
diff --git a/src/preprocessor.h b/src/preprocessor.h
index fe031def3..8de2495d0 100644
--- a/src/preprocessor.h
+++ b/src/preprocessor.h
@@ -88,6 +88,8 @@ private:
     static std::string getdef(std::string line, bool def);
 
     static bool match_cfg_def(std::string cfg, const std::string &def);
+
+    static std::string expandMacros(std::string code);
 };
 
 //---------------------------------------------------------------------------
diff --git a/test/testpreprocessor.cpp b/test/testpreprocessor.cpp
index 91d43be74..aee74e6dc 100644
--- a/test/testpreprocessor.cpp
+++ b/test/testpreprocessor.cpp
@@ -70,7 +70,6 @@ private:
         TEST_CASE(if_defined);      // "#if defined(AAA)" => "#ifdef AAA"
 
         // Macros..
-        /*
         TEST_CASE(macro_simple1);
         TEST_CASE(macro_simple2);
         TEST_CASE(macro_simple3);
@@ -79,7 +78,6 @@ private:
         TEST_CASE(macro_mismatch);
         TEST_CASE(preprocessor_inside_string);
         // TODO TEST_CASE(preprocessor_undef);
-        */
     }
 
 
@@ -447,7 +445,7 @@ private:
         ASSERT_EQUALS(expected, Preprocessor::replaceIfDefined(filedata));
     }
 
-/*
+
     void macro_simple1()
     {
         const char filedata[] = "#define AAA(aa) f(aa)\n"
@@ -530,7 +528,6 @@ private:
         ASSERT_EQUALS(1, actual.size());
         ASSERT_EQUALS("\n\n\nchar b=0;\n", actual[""]);
     }
-*/
 };
 
 REGISTER_TEST(TestPreprocessor)