diff --git a/lib/tokenize.cpp b/lib/tokenize.cpp
index 9b6a52585..8fc74639f 100644
--- a/lib/tokenize.cpp
+++ b/lib/tokenize.cpp
@@ -4262,9 +4262,18 @@ void Tokenizer::simplifyHeaders()
 
     // We want to remove selected stuff from the headers but not *everything*.
     // The intention here is to not damage the analysis of the source file.
+    // You should get all warnings in the source file.
 
     // TODO: Remove unused types/variables/etc in headers..
 
+    std::set<std::string> functions;
+    for (const Token *tok = list.front(); tok; tok = tok->next()) {
+        if (tok->fileIndex() == 0 && Token::Match(tok, "%name% (") && !Token::simpleMatch(tok->linkAt(1), ") {"))
+            functions.insert(tok->str());
+    }
+
+    const std::set<std::string> functionStart{"static", "const", "unsigned", "signed", "void", "bool", "char", "short", "int", "long", "float", "*"};
+
     for (Token *tok = list.front(); tok; tok = tok->next()) {
         if (tok->fileIndex() == 0)
             // Keep all code in the source file
@@ -4272,6 +4281,7 @@ void Tokenizer::simplifyHeaders()
 
         // Remove executable code
         if (tok->str() == "{") {
+            // TODO: We probably need to keep the executable code if this function is called from the source file.
             const Token *prev = tok->previous();
             while (prev && prev->isName())
                 prev = prev->previous();
@@ -4282,6 +4292,19 @@ void Tokenizer::simplifyHeaders()
                 tok->link(nullptr);
             }
         }
+
+        // Remove unused function declarations
+        if (Token::Match(tok, "[;{}]")) {
+            while (1) {
+                Token *start = tok->next();
+                while (start && functionStart.find(start->str()) != functionStart.end())
+                    start = start->next();
+                if (Token::Match(start, "%name% (") && Token::simpleMatch(start->linkAt(1), ") const| ;") && functions.find(start->str()) == functions.end())
+                    Token::eraseTokens(tok, start->linkAt(1)->tokAt(2));
+                else
+                    break;
+            }
+        }
     }
 }