Send status report in IM server side when server encounter the error (#11667)

diff --git a/src/app/BUILD.gn b/src/app/BUILD.gn
index 673dd00..269df93 100644
--- a/src/app/BUILD.gn
+++ b/src/app/BUILD.gn
@@ -117,6 +117,8 @@
     "OperationalDeviceProxy.h",
     "ReadClient.cpp",
     "ReadHandler.cpp",
+    "StatusResponse.cpp",
+    "StatusResponse.h",
     "WriteClient.cpp",
     "WriteHandler.cpp",
     "decoder.cpp",
diff --git a/src/app/CommandSender.cpp b/src/app/CommandSender.cpp
index b99ce2e..9e3da0b 100644
--- a/src/app/CommandSender.cpp
+++ b/src/app/CommandSender.cpp
@@ -26,6 +26,7 @@
 #include "Command.h"
 #include "CommandHandler.h"
 #include "InteractionModelEngine.h"
+#include "StatusResponse.h"
 #include <protocols/Protocols.h>
 #include <protocols/interaction_model/Constants.h>
 
@@ -91,20 +92,29 @@
                                             System::PacketBufferHandle && aPayload)
 {
     CHIP_ERROR err = CHIP_NO_ERROR;
-
+    StatusIB status(Protocols::InteractionModel::Status::Failure);
     VerifyOrExit(apExchangeContext == mpExchangeCtx, err = CHIP_ERROR_INCORRECT_STATE);
-    VerifyOrExit(aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::InvokeCommandResponse),
-                 err = CHIP_ERROR_INVALID_MESSAGE_TYPE);
-
-    err = ProcessInvokeResponse(std::move(aPayload));
+    if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::InvokeCommandResponse))
+    {
+        err = ProcessInvokeResponse(std::move(aPayload));
+        SuccessOrExit(err);
+        status.mStatus = Protocols::InteractionModel::Status::Success;
+    }
+    else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::StatusResponse))
+    {
+        err = StatusResponse::ProcessStatusResponse(std::move(aPayload), status);
+        SuccessOrExit(err);
+    }
+    else
+    {
+        err = CHIP_ERROR_INVALID_MESSAGE_TYPE;
+    }
 
 exit:
     if (mpCallback != nullptr)
     {
         if (err != CHIP_NO_ERROR)
         {
-            StatusIB status;
-            status.mStatus = Protocols::InteractionModel::Status::Failure;
             mpCallback->OnError(this, status, err);
         }
     }
diff --git a/src/app/InteractionModelEngine.cpp b/src/app/InteractionModelEngine.cpp
index 9aee8b3..b0d6ccb 100644
--- a/src/app/InteractionModelEngine.cpp
+++ b/src/app/InteractionModelEngine.cpp
@@ -156,7 +156,6 @@
             return err;
         }
     }
-
     return CHIP_ERROR_NO_MEMORY;
 }
 
@@ -263,7 +262,6 @@
         {
             continue;
         }
-
         ReturnLogErrorOnFailure(writeClient.Init(mpExchangeMgr, apCallback));
         apWriteClient.SetWriteClient(&writeClient);
         return CHIP_NO_ERROR;
@@ -272,27 +270,6 @@
     return CHIP_ERROR_NO_MEMORY;
 }
 
-CHIP_ERROR InteractionModelEngine::OnUnknownMsgType(Messaging::ExchangeContext * apExchangeContext,
-                                                    const PayloadHeader & aPayloadHeader, System::PacketBufferHandle && aPayload)
-{
-    CHIP_ERROR err = CHIP_NO_ERROR;
-
-    ChipLogDetail(InteractionModel, "Msg type %d not supported", aPayloadHeader.GetMessageType());
-
-    // Todo: Add status report
-    // err = SendStatusReport(ec, kChipProfile_Common, kStatus_UnsupportedMessage);
-    // SuccessOrExit(err);
-
-    apExchangeContext = nullptr;
-
-    // Todo: Fix the below check after the above status report is implemented.
-    if (nullptr != apExchangeContext)
-    {
-        apExchangeContext->Abort();
-    }
-    return err;
-}
-
 void InteractionModelEngine::OnDone(CommandHandler & apCommandObj)
 {
     mCommandHandlerObjs.ReleaseObject(&apCommandObj);
@@ -300,23 +277,27 @@
 
 CHIP_ERROR InteractionModelEngine::OnInvokeCommandRequest(Messaging::ExchangeContext * apExchangeContext,
                                                           const PayloadHeader & aPayloadHeader,
-                                                          System::PacketBufferHandle && aPayload)
+                                                          System::PacketBufferHandle && aPayload,
+                                                          Protocols::InteractionModel::Status & aStatus)
 {
     CommandHandler * commandHandler = mCommandHandlerObjs.CreateObject(this);
     if (commandHandler == nullptr)
     {
+        ChipLogProgress(InteractionModel, "no resource for Invoke interaction");
+        aStatus = Protocols::InteractionModel::Status::Busy;
         return CHIP_ERROR_NO_MEMORY;
     }
-    return commandHandler->OnInvokeCommandRequest(apExchangeContext, aPayloadHeader, std::move(aPayload));
+    ReturnErrorOnFailure(commandHandler->OnInvokeCommandRequest(apExchangeContext, aPayloadHeader, std::move(aPayload)));
+    aStatus = Protocols::InteractionModel::Status::Success;
+    return CHIP_NO_ERROR;
 }
 
 CHIP_ERROR InteractionModelEngine::OnReadInitialRequest(Messaging::ExchangeContext * apExchangeContext,
                                                         const PayloadHeader & aPayloadHeader,
                                                         System::PacketBufferHandle && aPayload,
-                                                        ReadHandler::InteractionType aInteractionType)
+                                                        ReadHandler::InteractionType aInteractionType,
+                                                        Protocols::InteractionModel::Status & aStatus)
 {
-    CHIP_ERROR err = CHIP_NO_ERROR;
-
     ChipLogDetail(InteractionModel, "Received %s request",
                   aInteractionType == ReadHandler::InteractionType::Subscribe ? "Subscribe" : "Read");
 
@@ -329,10 +310,10 @@
             bool keepSubscriptions = true;
             System::PacketBufferTLVReader reader;
             reader.Init(aPayload.Retain());
-            SuccessOrExit(err = reader.Next());
+            ReturnErrorOnFailure(reader.Next());
             SubscribeRequestMessage::Parser subscribeRequestParser;
-            SuccessOrExit(err = subscribeRequestParser.Init(reader));
-            err = subscribeRequestParser.GetKeepSubscriptions(&keepSubscriptions);
+            ReturnErrorOnFailure(subscribeRequestParser.Init(reader));
+            CHIP_ERROR err = subscribeRequestParser.GetKeepSubscriptions(&keepSubscriptions);
             if (err == CHIP_NO_ERROR && !keepSubscriptions)
             {
                 readHandler.Shutdown(ReadHandler::ShutdownOptions::AbortCurrentExchange);
@@ -344,49 +325,39 @@
     {
         if (readHandler.IsFree())
         {
-            err = readHandler.Init(mpExchangeMgr, mpDelegate, apExchangeContext, aInteractionType);
-            SuccessOrExit(err);
-            err               = readHandler.OnReadInitialRequest(std::move(aPayload));
-            apExchangeContext = nullptr;
-            break;
+            ReturnErrorOnFailure(readHandler.Init(mpExchangeMgr, mpDelegate, apExchangeContext, aInteractionType));
+            ReturnErrorOnFailure(readHandler.OnReadInitialRequest(std::move(aPayload)));
+            aStatus = Protocols::InteractionModel::Status::Success;
+            return CHIP_NO_ERROR;
         }
     }
 
-exit:
+    ChipLogProgress(InteractionModel, "no resource for %s interaction",
+                    aInteractionType == ReadHandler::InteractionType::Subscribe ? "Subscribe" : "Read");
+    aStatus = Protocols::InteractionModel::Status::ResourceExhausted;
 
-    if (nullptr != apExchangeContext)
-    {
-        apExchangeContext->Abort();
-    }
-    return err;
+    return CHIP_NO_ERROR;
 }
 
 CHIP_ERROR InteractionModelEngine::OnWriteRequest(Messaging::ExchangeContext * apExchangeContext,
-                                                  const PayloadHeader & aPayloadHeader, System::PacketBufferHandle && aPayload)
+                                                  const PayloadHeader & aPayloadHeader, System::PacketBufferHandle && aPayload,
+                                                  Protocols::InteractionModel::Status & aStatus)
 {
-    CHIP_ERROR err = CHIP_NO_ERROR;
-
     ChipLogDetail(InteractionModel, "Received Write request");
 
     for (auto & writeHandler : mWriteHandlers)
     {
         if (writeHandler.IsFree())
         {
-            err = writeHandler.Init(mpDelegate);
-            SuccessOrExit(err);
-            err               = writeHandler.OnWriteRequest(apExchangeContext, std::move(aPayload));
-            apExchangeContext = nullptr;
-            break;
+            ReturnErrorOnFailure(writeHandler.Init(mpDelegate));
+            ReturnErrorOnFailure(writeHandler.OnWriteRequest(apExchangeContext, std::move(aPayload)));
+            aStatus = Protocols::InteractionModel::Status::Success;
+            return CHIP_NO_ERROR;
         }
     }
-
-exit:
-
-    if (nullptr != apExchangeContext)
-    {
-        apExchangeContext->Abort();
-    }
-    return err;
+    ChipLogProgress(InteractionModel, "no resource for write interaction");
+    aStatus = Protocols::InteractionModel::Status::Busy;
+    return CHIP_NO_ERROR;
 }
 
 CHIP_ERROR InteractionModelEngine::OnUnsolicitedReportData(Messaging::ExchangeContext * apExchangeContext,
@@ -413,7 +384,6 @@
         {
             continue;
         }
-
         return readClient.OnUnsolicitedReportData(apExchangeContext, std::move(aPayload));
     }
     return CHIP_NO_ERROR;
@@ -422,31 +392,43 @@
 CHIP_ERROR InteractionModelEngine::OnMessageReceived(Messaging::ExchangeContext * apExchangeContext,
                                                      const PayloadHeader & aPayloadHeader, System::PacketBufferHandle && aPayload)
 {
+    CHIP_ERROR err                             = CHIP_NO_ERROR;
+    Protocols::InteractionModel::Status status = Protocols::InteractionModel::Status::Failure;
+
     if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::InvokeCommandRequest))
     {
-        return OnInvokeCommandRequest(apExchangeContext, aPayloadHeader, std::move(aPayload));
+        SuccessOrExit(OnInvokeCommandRequest(apExchangeContext, aPayloadHeader, std::move(aPayload), status));
     }
     else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::ReadRequest))
     {
-        return OnReadInitialRequest(apExchangeContext, aPayloadHeader, std::move(aPayload), ReadHandler::InteractionType::Read);
+        SuccessOrExit(OnReadInitialRequest(apExchangeContext, aPayloadHeader, std::move(aPayload),
+                                           ReadHandler::InteractionType::Read, status));
     }
     else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::WriteRequest))
     {
-        return OnWriteRequest(apExchangeContext, aPayloadHeader, std::move(aPayload));
+        SuccessOrExit(OnWriteRequest(apExchangeContext, aPayloadHeader, std::move(aPayload), status));
     }
     else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::SubscribeRequest))
     {
-        return OnReadInitialRequest(apExchangeContext, aPayloadHeader, std::move(aPayload),
-                                    ReadHandler::InteractionType::Subscribe);
+        SuccessOrExit(OnReadInitialRequest(apExchangeContext, aPayloadHeader, std::move(aPayload),
+                                           ReadHandler::InteractionType::Subscribe, status));
     }
     else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::ReportData))
     {
-        return OnUnsolicitedReportData(apExchangeContext, aPayloadHeader, std::move(aPayload));
+        ReturnErrorOnFailure(OnUnsolicitedReportData(apExchangeContext, aPayloadHeader, std::move(aPayload)));
+        status = Protocols::InteractionModel::Status::Success;
     }
     else
     {
-        return OnUnknownMsgType(apExchangeContext, aPayloadHeader, std::move(aPayload));
+        ChipLogProgress(InteractionModel, "Msg type %d not supported", aPayloadHeader.GetMessageType());
     }
+
+exit:
+    if (status != Protocols::InteractionModel::Status::Success)
+    {
+        err = StatusResponse::SendStatusResponse(status, apExchangeContext, false /*aExpectResponse*/);
+    }
+    return err;
 }
 
 void InteractionModelEngine::OnResponseTimeout(Messaging::ExchangeContext * ec)
diff --git a/src/app/InteractionModelEngine.h b/src/app/InteractionModelEngine.h
index 05bdd4b..5810bae 100644
--- a/src/app/InteractionModelEngine.h
+++ b/src/app/InteractionModelEngine.h
@@ -47,6 +47,7 @@
 #include <app/InteractionModelDelegate.h>
 #include <app/ReadClient.h>
 #include <app/ReadHandler.h>
+#include <app/StatusResponse.h>
 #include <app/WriteClient.h>
 #include <app/WriteHandler.h>
 #include <app/reporting/Engine.h>
@@ -54,9 +55,6 @@
 
 namespace chip {
 namespace app {
-
-static constexpr size_t kMaxSecureSduLengthBytes = 1024;
-
 /**
  * @class InteractionModelEngine
  *
@@ -202,28 +200,34 @@
 
     void OnDone(CommandHandler & apCommandObj) override;
 
-    CHIP_ERROR OnUnknownMsgType(Messaging::ExchangeContext * apExchangeContext, const PayloadHeader & aPayloadHeader,
-                                System::PacketBufferHandle && aPayload);
+    /**
+     * Called when Interaction Model receives a Command Request message.  Errors processing
+     * the Command Request are handled entirely within this function. The caller pre-sets status to failure and the callee is
+     * expected to set it to success if it does not want an automatic status response message to be sent.
+     */
     CHIP_ERROR OnInvokeCommandRequest(Messaging::ExchangeContext * apExchangeContext, const PayloadHeader & aPayloadHeader,
-                                      System::PacketBufferHandle && aPayload);
+                                      System::PacketBufferHandle && aPayload, Protocols::InteractionModel::Status & aStatus);
     CHIP_ERROR OnMessageReceived(Messaging::ExchangeContext * apExchangeContext, const PayloadHeader & aPayloadHeader,
                                  System::PacketBufferHandle && aPayload) override;
     void OnResponseTimeout(Messaging::ExchangeContext * ec) override;
 
     /**
      * Called when Interaction Model receives a Read Request message.  Errors processing
-     * the Read Request are handled entirely within this function.
+     * the Read Request are handled entirely within this function. The caller pre-sets status to failure and the callee is
+     * expected to set it to success if it does not want an automatic status response message to be sent.
      */
 
     CHIP_ERROR OnReadInitialRequest(Messaging::ExchangeContext * apExchangeContext, const PayloadHeader & aPayloadHeader,
-                                    System::PacketBufferHandle && aPayload, ReadHandler::InteractionType aInteractionType);
+                                    System::PacketBufferHandle && aPayload, ReadHandler::InteractionType aInteractionType,
+                                    Protocols::InteractionModel::Status & aStatus);
 
     /**
      * Called when Interaction Model receives a Write Request message.  Errors processing
-     * the Write Request are handled entirely within this function.
+     * the Write Request are handled entirely within this function. The caller pre-sets status to failure and the callee is
+     * expected to set it to success if it does not want an automatic status response message to be sent.
      */
     CHIP_ERROR OnWriteRequest(Messaging::ExchangeContext * apExchangeContext, const PayloadHeader & aPayloadHeader,
-                              System::PacketBufferHandle && aPayload);
+                              System::PacketBufferHandle && aPayload, Protocols::InteractionModel::Status & aStatus);
 
     /**This function handles processing of un-solicited ReportData messages on the client, which can
      * only occur post subscription establishment
diff --git a/src/app/ReadClient.cpp b/src/app/ReadClient.cpp
index 613057d..e6321a0 100644
--- a/src/app/ReadClient.cpp
+++ b/src/app/ReadClient.cpp
@@ -25,6 +25,7 @@
 #include <app/AppBuildConfig.h>
 #include <app/InteractionModelEngine.h>
 #include <app/ReadClient.h>
+#include <app/StatusResponse.h>
 
 namespace chip {
 namespace app {
@@ -189,46 +190,6 @@
     return err;
 }
 
-CHIP_ERROR ReadClient::SendStatusResponse(CHIP_ERROR aError)
-{
-    using Protocols::InteractionModel::Status;
-
-    System::PacketBufferHandle msgBuf = System::PacketBufferHandle::New(kMaxSecureSduLengthBytes);
-    VerifyOrReturnLogError(!msgBuf.IsNull(), CHIP_ERROR_NO_MEMORY);
-
-    System::PacketBufferTLVWriter writer;
-    writer.Init(std::move(msgBuf));
-
-    StatusResponseMessage::Builder response;
-    ReturnLogErrorOnFailure(response.Init(&writer));
-    Status statusCode = Status::Success;
-    if (aError != CHIP_NO_ERROR)
-    {
-        statusCode = Status::InvalidSubscription;
-    }
-    response.Status(statusCode);
-    ReturnLogErrorOnFailure(response.GetError());
-    ReturnLogErrorOnFailure(writer.Finalize(&msgBuf));
-    VerifyOrReturnLogError(mpExchangeCtx != nullptr, CHIP_ERROR_INCORRECT_STATE);
-
-    if (IsSubscriptionType())
-    {
-        if (IsAwaitingInitialReport())
-        {
-            MoveToState(ClientState::AwaitingSubscribeResponse);
-        }
-        else
-        {
-            RefreshLivenessCheckTimer();
-        }
-    }
-    ReturnLogErrorOnFailure(
-        mpExchangeCtx->SendMessage(Protocols::InteractionModel::MsgType::StatusResponse, std::move(msgBuf),
-                                   Messaging::SendFlags(IsAwaitingSubscribeResponse() ? Messaging::SendMessageFlags::kExpectResponse
-                                                                                      : Messaging::SendMessageFlags::kNone)));
-    return CHIP_NO_ERROR;
-}
-
 CHIP_ERROR ReadClient::GenerateEventPaths(EventPaths::Builder & aEventPathsBuilder, EventPathParams * apEventPathParamsList,
                                           size_t aEventPathParamsListSize)
 {
@@ -287,6 +248,13 @@
         mpExchangeCtx = nullptr;
         SuccessOrExit(err);
     }
+    else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::StatusResponse))
+    {
+        StatusIB status;
+        VerifyOrExit(apExchangeContext == mpExchangeCtx, err = CHIP_ERROR_INCORRECT_STATE);
+        err = StatusResponse::ProcessStatusResponse(std::move(aPayload), status);
+        SuccessOrExit(err);
+    }
     else
     {
         err = CHIP_ERROR_INVALID_MESSAGE_TYPE;
@@ -425,7 +393,22 @@
     }
 
 exit:
-    SendStatusResponse(err);
+    if (IsSubscriptionType())
+    {
+        if (IsAwaitingInitialReport())
+        {
+            MoveToState(ClientState::AwaitingSubscribeResponse);
+        }
+        else
+        {
+            RefreshLivenessCheckTimer();
+        }
+    }
+
+    StatusResponse::SendStatusResponse(err == CHIP_NO_ERROR ? Protocols::InteractionModel::Status::Success
+                                                            : Protocols::InteractionModel::Status::InvalidSubscription,
+                                       mpExchangeCtx, IsAwaitingSubscribeResponse());
+
     if (!mInitialReport)
     {
         mpExchangeCtx = nullptr;
diff --git a/src/app/ReadClient.h b/src/app/ReadClient.h
index 49b2587..d3c9837 100644
--- a/src/app/ReadClient.h
+++ b/src/app/ReadClient.h
@@ -179,7 +179,6 @@
     NodeId GetPeerNodeId() const { return mPeerNodeId; }
     bool IsReadType() { return mInteractionType == InteractionType::Read; }
     bool IsSubscriptionType() const { return mInteractionType == InteractionType::Subscribe; };
-    CHIP_ERROR SendStatusResponse(CHIP_ERROR aError);
 
 private:
     friend class TestReadInteraction;
diff --git a/src/app/ReadHandler.cpp b/src/app/ReadHandler.cpp
index 50ad988..b5f8dbb 100644
--- a/src/app/ReadHandler.cpp
+++ b/src/app/ReadHandler.cpp
@@ -134,25 +134,9 @@
 CHIP_ERROR ReadHandler::OnStatusResponse(Messaging::ExchangeContext * apExchangeContext, System::PacketBufferHandle && aPayload)
 {
     CHIP_ERROR err = CHIP_NO_ERROR;
-    Protocols::InteractionModel::Status statusCode;
-    StatusResponseMessage::Parser response;
-    System::PacketBufferTLVReader reader;
-    reader.Init(std::move(aPayload));
-    reader.Next();
-    err = response.Init(reader);
+    StatusIB status;
+    err = StatusResponse::ProcessStatusResponse(std::move(aPayload), status);
     SuccessOrExit(err);
-
-#if CHIP_CONFIG_IM_ENABLE_SCHEMA_CHECK
-    err = response.CheckSchemaValidity();
-    SuccessOrExit(err);
-#endif
-
-    err = response.GetStatus(statusCode);
-    SuccessOrExit(err);
-
-    ChipLogProgress(DataManagement, "In state %s, receive status response, status code is %" PRIu16, GetStateStr(),
-                    to_underlying(statusCode));
-    VerifyOrExit((statusCode == Protocols::InteractionModel::Status::Success), err = CHIP_ERROR_INVALID_ARGUMENT);
     switch (mState)
     {
     case HandlerState::AwaitingReportResponse:
diff --git a/src/app/StatusResponse.cpp b/src/app/StatusResponse.cpp
new file mode 100644
index 0000000..dbc5afc
--- /dev/null
+++ b/src/app/StatusResponse.cpp
@@ -0,0 +1,74 @@
+/*
+ *
+ *    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 <app/MessageDef/StatusResponseMessage.h>
+#include <app/StatusResponse.h>
+
+namespace chip {
+namespace app {
+CHIP_ERROR StatusResponse::SendStatusResponse(Protocols::InteractionModel::Status aStatus,
+                                              Messaging::ExchangeContext * apExchangeContext, bool aExpectResponse)
+{
+    VerifyOrReturnError(apExchangeContext != nullptr, CHIP_ERROR_INCORRECT_STATE);
+    System::PacketBufferHandle msgBuf = System::PacketBufferHandle::New(kMaxSecureSduLengthBytes);
+    VerifyOrReturnError(!msgBuf.IsNull(), CHIP_ERROR_NO_MEMORY);
+
+    System::PacketBufferTLVWriter writer;
+    writer.Init(std::move(msgBuf));
+
+    StatusResponseMessage::Builder response;
+    ReturnErrorOnFailure(response.Init(&writer));
+    response.Status(aStatus);
+    ReturnErrorOnFailure(response.GetError());
+    ReturnErrorOnFailure(writer.Finalize(&msgBuf));
+    ReturnErrorOnFailure(apExchangeContext->SendMessage(Protocols::InteractionModel::MsgType::StatusResponse, std::move(msgBuf),
+                                                        aExpectResponse ? Messaging::SendMessageFlags::kExpectResponse
+                                                                        : Messaging::SendMessageFlags::kNone));
+    return CHIP_NO_ERROR;
+}
+
+CHIP_ERROR StatusResponse::ProcessStatusResponse(System::PacketBufferHandle && aPayload, StatusIB & aStatus)
+{
+    CHIP_ERROR err = CHIP_NO_ERROR;
+    StatusResponseMessage::Parser response;
+    System::PacketBufferTLVReader reader;
+    reader.Init(std::move(aPayload));
+    ReturnErrorOnFailure(reader.Next());
+    ReturnErrorOnFailure(response.Init(reader));
+#if CHIP_CONFIG_IM_ENABLE_SCHEMA_CHECK
+    ReturnErrorOnFailure(response.CheckSchemaValidity());
+#endif
+    ReturnErrorOnFailure(response.GetStatus(aStatus.mStatus));
+    ChipLogProgress(InteractionModel, "Received status response, status is %" PRIu16, to_underlying(aStatus.mStatus));
+
+    if (aStatus.mStatus == Protocols::InteractionModel::Status::Success)
+    {
+        err = CHIP_NO_ERROR;
+    }
+    else if (aStatus.mStatus == Protocols::InteractionModel::Status::ResourceExhausted)
+    {
+        err = CHIP_ERROR_NO_MEMORY;
+    }
+    else
+    {
+        err = CHIP_ERROR_INCORRECT_STATE;
+    }
+    return err;
+}
+} // namespace app
+} // namespace chip
diff --git a/src/app/StatusResponse.h b/src/app/StatusResponse.h
new file mode 100644
index 0000000..03114de
--- /dev/null
+++ b/src/app/StatusResponse.h
@@ -0,0 +1,38 @@
+/*
+ *
+ *    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.
+ */
+
+#pragma once
+
+#include <app/MessageDef/StatusIB.h>
+#include <messaging/ExchangeContext.h>
+#include <protocols/interaction_model/Constants.h>
+#include <system/TLVPacketBufferBackingStore.h>
+
+namespace chip {
+namespace app {
+static constexpr size_t kMaxSecureSduLengthBytes = 1024;
+
+class StatusResponse
+{
+public:
+    static CHIP_ERROR SendStatusResponse(Protocols::InteractionModel::Status aStatus,
+                                         Messaging::ExchangeContext * apExchangeContext, bool aExpectResponse);
+    static CHIP_ERROR ProcessStatusResponse(System::PacketBufferHandle && aPayload, StatusIB & aStatus);
+};
+} // namespace app
+} // namespace chip
diff --git a/src/app/WriteClient.cpp b/src/app/WriteClient.cpp
index 8f2acc3..60d56a7 100644
--- a/src/app/WriteClient.cpp
+++ b/src/app/WriteClient.cpp
@@ -277,15 +277,23 @@
     // This should never fail because even if SendWriteRequest is called
     // back-to-back, the second call will call Close() on the first exchange,
     // which clears the OnMessageReceived callback.
+    VerifyOrExit(apExchangeContext == mpExchangeCtx, err = CHIP_ERROR_INCORRECT_STATE);
 
-    VerifyOrDie(apExchangeContext == mpExchangeCtx);
-
-    // Verify that the message is an Write Response. If not, this is an unexpected message.
-    // Signal the error through the error callback and shutdown the client.
-    VerifyOrExit(aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::WriteResponse),
-                 err = CHIP_ERROR_INVALID_MESSAGE_TYPE);
-
-    err = ProcessWriteResponseMessage(std::move(aPayload));
+    if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::WriteResponse))
+    {
+        err = ProcessWriteResponseMessage(std::move(aPayload));
+        SuccessOrExit(err);
+    }
+    else if (aPayloadHeader.HasMessageType(Protocols::InteractionModel::MsgType::StatusResponse))
+    {
+        StatusIB status;
+        err = StatusResponse::ProcessStatusResponse(std::move(aPayload), status);
+        SuccessOrExit(err);
+    }
+    else
+    {
+        err = CHIP_ERROR_INVALID_MESSAGE_TYPE;
+    }
 
 exit:
     if (mpCallback != nullptr)
diff --git a/src/app/WriteHandler.cpp b/src/app/WriteHandler.cpp
index 200946f..c6533f2 100644
--- a/src/app/WriteHandler.cpp
+++ b/src/app/WriteHandler.cpp
@@ -269,7 +269,7 @@
 void WriteHandler::MoveToState(const State aTargetState)
 {
     mState = aTargetState;
-    ChipLogDetail(DataManagement, "IM RH moving to [%s]", GetStateStr());
+    ChipLogDetail(DataManagement, "IM WH moving to [%s]", GetStateStr());
 }
 
 void WriteHandler::ClearState()
diff --git a/src/app/tests/TestReadInteraction.cpp b/src/app/tests/TestReadInteraction.cpp
index 64af737..e0bc4a0 100644
--- a/src/app/tests/TestReadInteraction.cpp
+++ b/src/app/tests/TestReadInteraction.cpp
@@ -879,7 +879,7 @@
 
     readPrepareParams.mMinIntervalFloorSeconds   = 2;
     readPrepareParams.mMaxIntervalCeilingSeconds = 5;
-    printf("\nSend subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
+    printf("\nSend first subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
 
     err = engine->SendSubscribeRequest(readPrepareParams, &delegate);
     NL_TEST_ASSERT(apSuite, err == CHIP_NO_ERROR);
@@ -994,7 +994,7 @@
     readPrepareParams1.mMinIntervalFloorSeconds                  = 2;
     readPrepareParams1.mMaxIntervalCeilingSeconds                = 5;
 
-    printf("\nSend another subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
+    printf("\nSend 2nd subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
 
     err = engine->SendSubscribeRequest(readPrepareParams1, &delegate);
     NL_TEST_ASSERT(apSuite, err == CHIP_NO_ERROR);
@@ -1016,6 +1016,74 @@
     engine->GetReportingEngine().Run();
     NL_TEST_ASSERT(apSuite, delegate.mGotReport);
     NL_TEST_ASSERT(apSuite, delegate.mNumAttributeResponse == 1);
+
+    delegate.mNumAttributeResponse = 0;
+    delegate.mGotReport            = false;
+    ReadPrepareParams readPrepareParams2(ctx.GetSessionBobToAlice());
+    chip::app::AttributePathParams attributePathParams2[1];
+    readPrepareParams2.mpAttributePathParamsList                 = attributePathParams2;
+    readPrepareParams2.mpAttributePathParamsList[0].mEndpointId  = kTestEndpointId;
+    readPrepareParams2.mpAttributePathParamsList[0].mClusterId   = kTestClusterId;
+    readPrepareParams2.mpAttributePathParamsList[0].mAttributeId = 1;
+    readPrepareParams2.mAttributePathParamsListSize              = 1;
+    readPrepareParams2.mMinIntervalFloorSeconds                  = 2;
+    readPrepareParams2.mMaxIntervalCeilingSeconds                = 5;
+
+    printf("\nSend 3rd subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
+
+    err = engine->SendSubscribeRequest(readPrepareParams2, &delegate);
+    NL_TEST_ASSERT(apSuite, err == CHIP_NO_ERROR);
+    engine->GetReportingEngine().Run();
+    NL_TEST_ASSERT(apSuite, delegate.mGotReport);
+    NL_TEST_ASSERT(apSuite, delegate.mNumAttributeResponse == 1);
+    NL_TEST_ASSERT(apSuite, !delegate.mReadError);
+
+    delegate.mNumAttributeResponse = 0;
+    delegate.mGotReport            = false;
+    ReadPrepareParams readPrepareParams3(ctx.GetSessionBobToAlice());
+    chip::app::AttributePathParams attributePathParams3[1];
+    readPrepareParams3.mpAttributePathParamsList                 = attributePathParams3;
+    readPrepareParams3.mpAttributePathParamsList[0].mEndpointId  = kTestEndpointId;
+    readPrepareParams3.mpAttributePathParamsList[0].mClusterId   = kTestClusterId;
+    readPrepareParams3.mpAttributePathParamsList[0].mAttributeId = 1;
+    readPrepareParams3.mAttributePathParamsListSize              = 1;
+    readPrepareParams3.mMinIntervalFloorSeconds                  = 2;
+    readPrepareParams3.mMaxIntervalCeilingSeconds                = 5;
+
+    printf("\nSend 4th subscribe request message to Node: %" PRIu64 "\n", chip::kTestDeviceNodeId);
+
+    ReadClient readClient3;
+    readClient3.Init(&ctx.GetExchangeManager(), &delegate, ReadClient::InteractionType::Subscribe);
+    err = readClient3.SendSubscribeRequest(readPrepareParams3);
+    NL_TEST_ASSERT(apSuite, err == CHIP_NO_ERROR);
+    engine->GetReportingEngine().Run();
+    NL_TEST_ASSERT(apSuite, delegate.mGotReport);
+    NL_TEST_ASSERT(apSuite, delegate.mNumAttributeResponse == 1);
+    NL_TEST_ASSERT(apSuite, !delegate.mReadError);
+
+    delegate.mNumAttributeResponse = 0;
+    delegate.mGotReport            = false;
+    ReadPrepareParams readPrepareParams4(ctx.GetSessionBobToAlice());
+    chip::app::AttributePathParams attributePathParams4[1];
+    readPrepareParams4.mpAttributePathParamsList                 = attributePathParams4;
+    readPrepareParams4.mpAttributePathParamsList[0].mEndpointId  = kTestEndpointId;
+    readPrepareParams4.mpAttributePathParamsList[0].mClusterId   = kTestClusterId;
+    readPrepareParams4.mpAttributePathParamsList[0].mAttributeId = 1;
+    readPrepareParams4.mAttributePathParamsListSize              = 1;
+    readPrepareParams4.mMinIntervalFloorSeconds                  = 2;
+    readPrepareParams4.mMaxIntervalCeilingSeconds                = 5;
+
+    printf("\nSend 5th subscribe request message to Node: %" PRIu64 ",  resource exhausted\n", chip::kTestDeviceNodeId);
+
+    ReadClient readClient4;
+    readClient4.Init(&ctx.GetExchangeManager(), &delegate, ReadClient::InteractionType::Subscribe);
+    err = readClient4.SendSubscribeRequest(readPrepareParams4);
+    NL_TEST_ASSERT(apSuite, err == CHIP_NO_ERROR);
+    engine->GetReportingEngine().Run();
+    NL_TEST_ASSERT(apSuite, !delegate.mGotReport);
+    NL_TEST_ASSERT(apSuite, delegate.mNumAttributeResponse == 0);
+    NL_TEST_ASSERT(apSuite, delegate.mReadError);
+
     // By now we should have closed all exchanges and sent all pending acks, so
     // there should be no queued-up things in the retransmit table.
     NL_TEST_ASSERT(apSuite, rm->TestGetCountRetransTable() == 0);