diff --git a/platformio/builder/tools/piolib.py b/platformio/builder/tools/piolib.py
index f0b86794..ca1a2bb9 100644
--- a/platformio/builder/tools/piolib.py
+++ b/platformio/builder/tools/piolib.py
@@ -334,8 +334,20 @@ class LibBuilderBase(object):
                     sys.stderr.write(
                         "Warning! Classic Pre Processor is used for `%s`, "
                         "advanced has failed with `%s`\n" % (path, e))
-                incs = self.env.File(path).get_found_includes(
+                _incs = self.env.File(path).get_found_includes(
                     self.env, LibBuilderBase.CLASSIC_SCANNER, tuple(inc_dirs))
+                incs = []
+                for inc in _incs:
+                    incs.append(inc)
+                    _h_path = inc.get_abspath()
+                    if not self.env.IsFileWithExt(_h_path,
+                                                  piotool.SRC_HEADER_EXT):
+                        continue
+                    _f_part = _h_path[:_h_path.rindex(".")]
+                    for ext in piotool.SRC_C_EXT:
+                        if isfile("%s.%s" % (_f_part, ext)):
+                            incs.append(
+                                self.env.File("%s.%s" % (_f_part, ext)))
             # print path, map(lambda n: n.get_abspath(), incs)
             for inc in incs:
                 if inc not in result:
diff --git a/platformio/builder/tools/platformio.py b/platformio/builder/tools/platformio.py
index 67b1d43f..5d905b0a 100644
--- a/platformio/builder/tools/platformio.py
+++ b/platformio/builder/tools/platformio.py
@@ -27,8 +27,9 @@ from SCons.Util import case_sensitive_suffixes, is_Sequence
 
 from platformio.util import glob_escape, pioversion_to_intstr
 
-SRC_BUILD_EXT = ["c", "cc", "cpp", "S", "spp", "SPP", "sx", "s", "asm", "ASM"]
 SRC_HEADER_EXT = ["h", "hpp"]
+SRC_C_EXT = ["c", "cc", "cpp"]
+SRC_BUILD_EXT = SRC_C_EXT + ["S", "spp", "SPP", "sx", "s", "asm", "ASM"]
 SRC_FILTER_DEFAULT = ["+<*>", "-<.git%s>" % sep, "-<svn%s>" % sep]