[PATCH BlueZ 2/4] mesh: Reply to HB pub set with same fields

[Isak Westin <isak.westin@loytec.com>]

If a Config Heartbeat Publication Set message is unsuccessfully
processed, the fields in the status reply should be the same as in the
original message. See MshPRFv1.0.1 section 4.4.1.2.15.
---
 mesh/cfgmod-server.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/mesh/cfgmod-server.c b/mesh/cfgmod-server.c
index 33796d05a..9c5edf551 100644
--- a/mesh/cfgmod-server.c
+++ b/mesh/cfgmod-server.c
@@ -575,7 +575,17 @@ static uint16_t hb_publication_set(struct mesh_node *node, const uint8_t *pkt)
 	status = mesh_net_set_heartbeat_pub(net, dst, features, net_idx, ttl,
 						count_log, period_log);
 
-	return hb_publication_get(node, status);
+	if (status != MESH_STATUS_SUCCESS) {
+		uint16_t n;
+
+		n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS, msg);
+		msg[n++] = status;
+		memcpy(msg + n, pkt, 9);
+		n += 9;
+
+		return n;
+	} else
+		return hb_publication_get(node, status);
 }
 
 static void node_reset(void *user_data)
-- 
2.20.1