diff --git a/ChangeLog.txt b/ChangeLog.txt
index 510af8db..1c8b9778 100644
--- a/ChangeLog.txt
+++ b/ChangeLog.txt
@@ -18,6 +18,8 @@ Client library:
 - Initialise "result" variable as soon as possible in
   mosquitto_topic_matches_sub. Closes #654.
 - No need to close socket again if setting non-blocking failed. Closes #649.
+- Fix mosquitto_topic_matches_sub() not correctly matching foo/bar against
+  foo/+/#. Closes #670.
 
 Build:
 - Don't run TLS-PSK tests if TLS-PSK disabled at compile time. Closes #636.
diff --git a/lib/util_mosq.c b/lib/util_mosq.c
index 6469dfad..edef7188 100644
--- a/lib/util_mosq.c
+++ b/lib/util_mosq.c
@@ -308,6 +308,18 @@ int mosquitto_topic_matches_sub(const char *sub, const char *topic, bool *result
 					return MOSQ_ERR_SUCCESS;
 				}
 			}else{
+				/* Check for e.g. foo/bar matching foo/+/# */
+				if(spos > 0
+						&& spos+2 == slen
+						&& tpos == tlen
+						&& sub[spos-1] == '+'
+						&& sub[spos] == '/'
+						&& sub[spos+1] == '#')
+				{
+					*result = true;
+					multilevel_wildcard = true;
+					return MOSQ_ERR_SUCCESS;
+				}
 				return MOSQ_ERR_SUCCESS;
 			}
 		}
diff --git a/test/lib/c/09-util-topic-matching.c b/test/lib/c/09-util-topic-matching.c
index 8b59a633..12c512fd 100644
--- a/test/lib/c/09-util-topic-matching.c
+++ b/test/lib/c/09-util-topic-matching.c
@@ -2,6 +2,9 @@
 #include <stdlib.h>
 #include <mosquitto.h>
 
+#define EXPECT_MATCH(A, B) do_check((A), (B), false)
+#define EXPECT_NOMATCH(A, B) do_check((A), (B), true)
+
 void do_check(const char *sub, const char *topic, bool bad_res)
 {
 	bool match;
@@ -16,42 +19,44 @@ void do_check(const char *sub, const char *topic, bool bad_res)
 
 int main(int argc, char *argv[])
 {
-	do_check("foo/#", "foo/", false);
-	do_check("foo#", "foo", true);
-	do_check("fo#o/", "foo", true);
-	do_check("foo#", "fooa", true);
-	do_check("foo+", "foo", true);
-	do_check("foo+", "fooa", true);
+	EXPECT_MATCH("foo/#", "foo/");
+	EXPECT_NOMATCH("foo#", "foo");
+	EXPECT_NOMATCH("fo#o/", "foo");
+	EXPECT_NOMATCH("foo#", "fooa");
+	EXPECT_NOMATCH("foo+", "foo");
+	EXPECT_NOMATCH("foo+", "fooa");
 
-	do_check("test/6/#", "test/3", true);
-	do_check("foo/bar", "foo/bar", false);
-	do_check("foo/+", "foo/bar", false);
-	do_check("foo/+/baz", "foo/bar/baz", false);
+	EXPECT_NOMATCH("test/6/#", "test/3");
+	EXPECT_MATCH("foo/bar", "foo/bar");
+	EXPECT_MATCH("foo/+", "foo/bar");
+	EXPECT_MATCH("foo/+/baz", "foo/bar/baz");
 
-	do_check("A/B/+/#", "A/B/B/C", false);
+	EXPECT_MATCH("A/B/+/#", "A/B/B/C");
 
-	do_check("foo/+/#", "foo/bar/baz", false);
-	do_check("#", "foo/bar/baz", false);
+	EXPECT_MATCH("foo/+/#", "foo/bar/baz");
+	EXPECT_MATCH("foo/+/#", "foo/bar");
+	EXPECT_MATCH("#", "foo/bar/baz");
+	EXPECT_MATCH("#", "foo/bar/baz");
 
-	do_check("foo/bar", "foo", true);
-	do_check("foo/+", "foo/bar/baz", true);
-	do_check("foo/+/baz", "foo/bar/bar", true);
+	EXPECT_NOMATCH("foo/bar", "foo");
+	EXPECT_NOMATCH("foo/+", "foo/bar/baz");
+	EXPECT_NOMATCH("foo/+/baz", "foo/bar/bar");
 
-	do_check("foo/+/#", "fo2/bar/baz", true);
+	EXPECT_NOMATCH("foo/+/#", "fo2/bar/baz");
 
-	do_check("#", "/foo/bar", false);
-	do_check("/#", "/foo/bar", false);
-	do_check("/#", "foo/bar", true);
+	EXPECT_MATCH("#", "/foo/bar");
+	EXPECT_MATCH("/#", "/foo/bar");
+	EXPECT_NOMATCH("/#", "foo/bar");
 
 
-	do_check("foo//bar", "foo//bar", false);
-	do_check("foo//+", "foo//bar", false);
-	do_check("foo/+/+/baz", "foo///baz", false);
-	do_check("foo/bar/+", "foo/bar/", false);
+	EXPECT_MATCH("foo//bar", "foo//bar");
+	EXPECT_MATCH("foo//+", "foo//bar");
+	EXPECT_MATCH("foo/+/+/baz", "foo///baz");
+	EXPECT_MATCH("foo/bar/+", "foo/bar/");
 
-	do_check("$SYS/bar", "$SYS/bar", false);
-	do_check("#", "$SYS/bar", true);
-	do_check("$BOB/bar", "$SYS/bar", true);
+	EXPECT_MATCH("$SYS/bar", "$SYS/bar");
+	EXPECT_NOMATCH("#", "$SYS/bar");
+	EXPECT_NOMATCH("$BOB/bar", "$SYS/bar");
 
 	return 0;
 }