net: lwm2m: Fix blockwise response code

In CoAP blockwise the client is supposed to
respond with 2.31 Continue code on Ack. This was recently
broken when Block1 parsing was moved after the initialization
of reponse packet. We need separate CoAP API to modify the code
of existing CoAP packet.

Also Ack packet should contain the Block1 options, even the
last one.

Signed-off-by: Seppo Takalo <seppo.takalo@nordicsemi.no>

include/zephyr/net/coap.h

@@ -373,6 +373,15 @@ uint8_t coap_header_get_token(const struct coap_packet *cpkt, uint8_t *token);
  */
 uint8_t coap_header_get_code(const struct coap_packet *cpkt);
 
+/**
+ * @brief Modifies the code of the CoAP packet.
+ *
+ * @param cpkt CoAP packet representation
+ * @param code CoAP code
+ * @return 0 on success, -EINVAL on failure
+ */
+int coap_header_set_code(const struct coap_packet *cpkt, uint8_t code);
+
 /**
  * @brief Returns the message id associated with the CoAP packet.
  *

subsys/net/lib/coap/coap.c

@@ -950,6 +950,16 @@ static uint8_t __coap_header_get_code(const struct coap_packet *cpkt)
 	return cpkt->data[1];
 }
 
+int coap_header_set_code(const struct coap_packet *cpkt, uint8_t code)
+{
+	if (!cpkt || !cpkt->data) {
+		return -EINVAL;
+	}
+
+	cpkt->data[1] = code;
+	return 0;
+}
+
 uint8_t coap_header_get_token(const struct coap_packet *cpkt, uint8_t *token)
 {
 	uint8_t tkl;

subsys/net/lib/lwm2m/lwm2m_message_handling.c

@@ -2065,6 +2065,13 @@ static int parse_write_op(struct lwm2m_message *msg, uint16_t format)
 		if (block_num < block_ctx->expected) {
 			LOG_WRN("Block already handled %d, expected %d", block_num,
 				block_ctx->expected);
+			(void)coap_header_set_code(msg->out.out_cpkt, COAP_RESPONSE_CODE_CONTINUE);
+			/* Respond with the original Block1 header, original Ack migh have been
+			 * lost, and this is a retry. We don't know the original response, but
+			 * since it is handled, just assume we can continue.
+			 */
+			(void)coap_append_option_int(msg->out.out_cpkt, COAP_OPTION_BLOCK1,
+						     block_opt);
 			return 0;
 		}
 		if (block_num > block_ctx->expected) {
@@ -2087,30 +2094,35 @@ static int parse_write_op(struct lwm2m_message *msg, uint16_t format)
 		 * number.
 		 */
 		block_ctx->expected += GET_BLOCK_SIZE(block_opt) - block_ctx->ctx.block_size + 1;
-
-		/* Handle blockwise 1 (Part 1): Set response code */
-		if (!last_block) {
-			msg->code = COAP_RESPONSE_CODE_CONTINUE;
-		}
 	}
 
 	r = do_write_op(msg, format);
 
 	/* Handle blockwise 1 (Part 2): Append BLOCK1 option / free context */
 	if (block_ctx) {
-		if (r >= 0 && !last_block) {
+		if (r >= 0) {
-			/* More to come, ack with correspond block # */
+			/* Add block1 option to response.
+			 * As RFC7959 Section-2.3, More flag is off, because we have already
+			 * written the data.
+			 */
 			r = coap_append_block1_option(msg->out.out_cpkt, &block_ctx->ctx);
 			if (r < 0) {
 				/* report as internal server error */
-				LOG_ERR("Fail adding block1 option: %d", r);
+				LOG_DBG("Fail adding block1 option: %d", r);
 				r = -EINVAL;
 			}
+			if (!last_block) {
+				r = coap_header_set_code(msg->out.out_cpkt,
+							 COAP_RESPONSE_CODE_CONTINUE);
+				if (r < 0) {
+					LOG_DBG("Failed to modify response code");
+					r = -EINVAL;
+				}
+			}
 		}
 		if (r < 0 || last_block) {
 			/* Free context when finished or when there is error */
 			free_block_ctx(block_ctx);
-
 		}
 	}