mgmt: mcumgr: smp: Fix custom payload size

Fixes an issue when using custom payloads where the size was still
using the zcbor buffer instead of the size of the network buffer

Signed-off-by: Jamie McCrae <[email protected]>

Author: nordicjm

subsys/mgmt/mcumgr/smp/src/smp.c

@@ -310,9 +310,15 @@ static int smp_handle_single_req(struct smp_streamer *streamer, const struct smp
 	}
 #endif
 
-	smp_make_rsp_hdr(req_hdr, &rsp_hdr,
-			 zsp->payload_mut - nbw->nb->data - MGMT_HDR_SIZE);
-	nbw->nb->len = zsp->payload_mut - nbw->nb->data;
+#if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
+	if (!mgmt_find_group(req_hdr->nh_group)->custom_payload) {
+#endif
+		nbw->nb->len = zsp->payload_mut - nbw->nb->data;
+#if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
+	}
+#endif
+
+	smp_make_rsp_hdr(req_hdr, &rsp_hdr, (nbw->nb->len - MGMT_HDR_SIZE));
 	smp_write_hdr(streamer, &rsp_hdr);
 
 	return 0;