diff --git a/src/app/CommandHandlerInterfaceRegistry.cpp b/src/app/CommandHandlerInterfaceRegistry.cpp
index 01436853dd1a55..8345be5dcc2805 100644
--- a/src/app/CommandHandlerInterfaceRegistry.cpp
+++ b/src/app/CommandHandlerInterfaceRegistry.cpp
@@ -68,8 +68,11 @@ void CommandHandlerInterfaceRegistry::UnregisterAllCommandHandlersForEndpoint(En
 {
     CommandHandlerInterface * prev = nullptr;
 
-    for (auto * cur = mCommandHandlerList; cur; cur = cur->GetNext())
+    for (auto * cur = mCommandHandlerList; cur;)
     {
+        // Fetch next node in the list before we remove this one.
+        auto * next = cur->GetNext();
+
         if (cur->MatchesEndpoint(endpointId))
         {
             if (prev == nullptr)
@@ -87,6 +90,8 @@ void CommandHandlerInterfaceRegistry::UnregisterAllCommandHandlersForEndpoint(En
         {
             prev = cur;
         }
+
+        cur = next;
     }
 }
 
diff --git a/src/app/tests/BUILD.gn b/src/app/tests/BUILD.gn
index 1bce08d55e0571..104a57a2fc019a 100644
--- a/src/app/tests/BUILD.gn
+++ b/src/app/tests/BUILD.gn
@@ -194,6 +194,7 @@ chip_test_suite("tests") {
     "TestBasicCommandPathRegistry.cpp",
     "TestBindingTable.cpp",
     "TestBuilderParser.cpp",
+    "TestCommandHandlerInterfaceRegistry.cpp",
     "TestCommandInteraction.cpp",
     "TestCommandPathParams.cpp",
     "TestConcreteAttributePath.cpp",
diff --git a/src/app/tests/TestCommandHandlerInterfaceRegistry.cpp b/src/app/tests/TestCommandHandlerInterfaceRegistry.cpp
new file mode 100644
index 00000000000000..aed017362c83f5
--- /dev/null
+++ b/src/app/tests/TestCommandHandlerInterfaceRegistry.cpp
@@ -0,0 +1,103 @@
+/*
+ *
+ *    Copyright (c) 2021 Project CHIP Authors
+ *    All rights reserved.
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+#include <lib/core/StringBuilderAdapters.h>
+#include <pw_unit_test/framework.h>
+
+#include <app/CommandHandlerInterfaceRegistry.h>
+
+#include <type_traits>
+
+namespace chip {
+namespace app {
+
+namespace {
+
+class TestCommandHandlerInterface : public CommandHandlerInterface
+{
+public:
+    TestCommandHandlerInterface(Optional<EndpointId> endpointId, ClusterId clusterId) :
+        CommandHandlerInterface(endpointId, clusterId)
+    {}
+
+    // Just need this to compile
+    void InvokeCommand(HandlerContext & handlerContext) override {}
+};
+
+} // anonymous namespace
+
+TEST(TestCommandHandlerInterfaceRegistry, TestRegisterUnregister)
+{
+    TestCommandHandlerInterface a(Optional<EndpointId>(1), 1);
+    TestCommandHandlerInterface b(Optional<EndpointId>(1), 2);
+    TestCommandHandlerInterface c(Optional<EndpointId>(2), 1);
+    TestCommandHandlerInterface d(NullOptional, 3);
+
+    CommandHandlerInterfaceRegistry registry;
+    EXPECT_EQ(registry.RegisterCommandHandler(&a), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&b), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&c), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&d), CHIP_NO_ERROR);
+
+    EXPECT_EQ(registry.GetCommandHandler(1, 1), &a);
+    EXPECT_EQ(registry.GetCommandHandler(1, 2), &b);
+    EXPECT_EQ(registry.GetCommandHandler(2, 1), &c);
+    EXPECT_EQ(registry.GetCommandHandler(1, 3), &d);
+    EXPECT_EQ(registry.GetCommandHandler(5, 3), &d);
+
+    EXPECT_EQ(registry.UnregisterCommandHandler(&b), CHIP_NO_ERROR);
+
+    EXPECT_EQ(registry.GetCommandHandler(1, 1), &a);
+    EXPECT_EQ(registry.GetCommandHandler(1, 2), nullptr);
+    EXPECT_EQ(registry.GetCommandHandler(2, 1), &c);
+    EXPECT_EQ(registry.GetCommandHandler(1, 3), &d);
+    EXPECT_EQ(registry.GetCommandHandler(5, 3), &d);
+
+    EXPECT_EQ(registry.UnregisterCommandHandler(&b), CHIP_ERROR_KEY_NOT_FOUND);
+}
+
+TEST(TestCommandHandlerInterfaceRegistry, TestUnregisterAll)
+{
+    TestCommandHandlerInterface a(Optional<EndpointId>(1), 1);
+    TestCommandHandlerInterface b(Optional<EndpointId>(1), 2);
+    TestCommandHandlerInterface c(Optional<EndpointId>(2), 1);
+    TestCommandHandlerInterface d(NullOptional, 3);
+
+    CommandHandlerInterfaceRegistry registry;
+    EXPECT_EQ(registry.RegisterCommandHandler(&a), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&b), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&c), CHIP_NO_ERROR);
+    EXPECT_EQ(registry.RegisterCommandHandler(&d), CHIP_NO_ERROR);
+
+    EXPECT_EQ(registry.GetCommandHandler(1, 1), &a);
+    EXPECT_EQ(registry.GetCommandHandler(1, 2), &b);
+    EXPECT_EQ(registry.GetCommandHandler(2, 1), &c);
+    EXPECT_EQ(registry.GetCommandHandler(1, 3), &d);
+    EXPECT_EQ(registry.GetCommandHandler(5, 3), &d);
+
+    registry.UnregisterAllCommandHandlersForEndpoint(1);
+
+    EXPECT_EQ(registry.GetCommandHandler(1, 1), nullptr);
+    EXPECT_EQ(registry.GetCommandHandler(1, 2), nullptr);
+    EXPECT_EQ(registry.GetCommandHandler(2, 1), &c);
+    EXPECT_EQ(registry.GetCommandHandler(1, 3), &d);
+    EXPECT_EQ(registry.GetCommandHandler(5, 3), &d);
+}
+
+} // namespace app
+} // namespace chip