net: lwm2m: Refactor blockwise SEND to support GET and FETCH

Allow blockwise-send buffers to be used with GET and FETCH
queries as well.
When outgoing packet is split into multiple blocks, don't free
it when first block is send. Keep it in memory until some other requests
come.

Following queries to next block are matched using CoAP token.
However, this required Leshan to use COAP.BLOCKWISE_REUSE_TOKEN=true
option from Californium.

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

include/zephyr/net/coap.h

@@ -713,6 +713,20 @@ int coap_get_option_int(const struct coap_packet *cpkt, uint16_t code);
  */
 int coap_get_block1_option(const struct coap_packet *cpkt, bool *has_more, uint8_t *block_number);
 
+/**
+ * @brief Get values from CoAP block2 option.
+ *
+ * Decode block number and block size from option. Ignore the has_more flag
+ * as it should always be zero on queries.
+ *
+ * @param cpkt Packet to be inspected
+ * @param block_number Is set to the number of the block
+ *
+ * @return Integer value of the block size in case of success
+ * or negative in case of error.
+ */
+int coap_get_block2_option(const struct coap_packet *cpkt, uint8_t *block_number);
+
 /**
  * @brief Retrieves BLOCK{1,2} and SIZE{1,2} from @a cpkt and updates
  * @a ctx accordingly.

subsys/net/lib/coap/coap.c

@@ -1303,6 +1303,19 @@ int coap_get_block1_option(const struct coap_packet *cpkt, bool *has_more, uint8
 	return ret;
 }
 
+int coap_get_block2_option(const struct coap_packet *cpkt, uint8_t *block_number)
+{
+	int ret = coap_get_option_int(cpkt, COAP_OPTION_BLOCK2);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	*block_number = GET_NUM(ret);
+	ret = 1 << (GET_BLOCK_SIZE(ret) + 4);
+	return ret;
+}
+
 int insert_option(struct coap_packet *cpkt, uint16_t code, const uint8_t *value, uint16_t len)
 {
 	uint16_t offset = cpkt->hdr_len;

subsys/net/lib/lwm2m/lwm2m_engine.c

@@ -675,7 +675,9 @@ static int socket_send_message(struct lwm2m_ctx *client_ctx)
 	}
 
 	if (msg->type != COAP_TYPE_CON) {
-		lwm2m_reset_message(msg, true);
+		if (!lwm2m_outgoing_is_part_of_blockwise(msg)) {
+			lwm2m_reset_message(msg, true);
+		}
 	}
 
 	return rc;

subsys/net/lib/lwm2m/lwm2m_message_handling.c

@@ -81,6 +81,7 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME);
 /* Shared set of in-flight LwM2M messages */
 static struct lwm2m_message messages[CONFIG_LWM2M_ENGINE_MAX_MESSAGES];
 static struct lwm2m_block_context block1_contexts[NUM_BLOCK1_CONTEXT];
+static sys_slist_t ongoing_block_sends;
 
 #if defined(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)
 /* we need 1 more buffer as the payload is encoded in that buffer first even if
@@ -99,6 +100,7 @@ sys_slist_t *lwm2m_engine_obj_inst_list(void);
 
 static int handle_request(struct coap_packet *request, struct lwm2m_message *msg);
 #if defined(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)
+STATIC int build_msg_block_for_send(struct lwm2m_message *msg, uint16_t block_num);
 struct coap_block_context *lwm2m_output_block_context(void);
 #endif
 
@@ -304,7 +306,11 @@ STATIC int build_msg_block_for_send(struct lwm2m_message *msg, uint16_t block_nu
 		/* reuse message for next block */
 		tkl = coap_header_get_token(&msg->cpkt, token);
 		lwm2m_reset_message(msg, false);
-		msg->mid = coap_next_id();
+		if (msg->type == COAP_TYPE_ACK) {
+			msg->mid = coap_header_get_id(msg->in.in_cpkt);
+		} else {
+			msg->mid = coap_next_id();
+		}
 		msg->token = token;
 		msg->tkl = tkl;
 		ret = lwm2m_init_message(msg);
@@ -333,10 +339,12 @@ STATIC int build_msg_block_for_send(struct lwm2m_message *msg, uint16_t block_nu
 			return ret;
 		}
 		ret = coap_block_transfer_init(msg->out.block_ctx, lwm2m_default_block_size(),
-					       msg->body_encode_buffer.offset);
+					       complete_payload_len);
 		if (ret < 0) {
 			return ret;
 		}
+		sys_slist_append(&ongoing_block_sends, &msg->node);
+		msg->block_send = true;
 	} else {
 		/*  update block context */
 		msg->out.block_ctx->current = block_num * block_size_bytes;
@@ -402,8 +410,15 @@ STATIC int prepare_msg_for_send(struct lwm2m_message *msg)
 
 	return 0;
 }
+
 #endif
 
+bool lwm2m_outgoing_is_part_of_blockwise(struct lwm2m_message *msg)
+{
+	return msg->block_send;
+}
+
+
 void lwm2m_engine_context_close(struct lwm2m_ctx *client_ctx)
 {
 	struct lwm2m_message *msg;
@@ -545,6 +560,7 @@ void lwm2m_reset_message(struct lwm2m_message *msg, bool release)
 #if defined(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)
 		release_output_block_ctx(&msg->out.block_ctx);
 		release_body_encode_buffer(&msg->body_encode_buffer.data);
+		sys_slist_find_and_remove(&ongoing_block_sends, &msg->node);
 #endif
 		(void)memset(msg, 0, sizeof(*msg));
 	} else {
@@ -2552,6 +2568,63 @@ static int lwm2m_response_promote_to_con(struct lwm2m_message *msg)
 	return ret;
 }
 
+static struct lwm2m_message *find_ongoing_block_tx(uint8_t *token, uint8_t tkl)
+{
+	uint8_t msg_token[8];
+	uint8_t msg_tkl;
+	struct lwm2m_message *msg;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&ongoing_block_sends, msg, node) {
+		msg_tkl = coap_header_get_token(&msg->cpkt, msg_token);
+
+		if (msg_tkl == tkl && memcmp(msg_token, token, tkl) == 0) {
+			return msg;
+		}
+	}
+	return NULL;
+}
+
+static void clear_ongoing_block_tx(void)
+{
+	sys_snode_t *node;
+
+	while ((node = sys_slist_get(&ongoing_block_sends)) != NULL) {
+		struct lwm2m_message *msg = SYS_SLIST_CONTAINER(node, msg, node);
+
+		lwm2m_reset_message(msg, true);
+	}
+}
+
+static void handle_ongoing_block_tx(struct lwm2m_message *msg, struct coap_packet *cpkt)
+{
+#if defined(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)
+	int r;
+	uint8_t block;
+
+	r = coap_get_block2_option(cpkt, &block);
+	if (r < 0) {
+		LOG_ERR("Failed to parse BLOCK2");
+		return;
+	}
+
+	msg->in.in_cpkt = cpkt;
+
+	r = build_msg_block_for_send(msg, block);
+	if (r < 0) {
+		clear_ongoing_block_tx();
+		LOG_ERR("Unable to build next block of lwm2m message! r=%d", r);
+		return;
+	}
+
+	r = lwm2m_send_message_async(msg);
+	if (r < 0) {
+		clear_ongoing_block_tx();
+		LOG_ERR("Unable to send next block of lwm2m message!");
+		return;
+	}
+#endif
+}
+
 void lwm2m_udp_receive(struct lwm2m_ctx *client_ctx, uint8_t *buf, uint16_t buf_len,
 		       struct sockaddr *from_addr)
 {
@@ -2561,19 +2634,24 @@ void lwm2m_udp_receive(struct lwm2m_ctx *client_ctx, uint8_t *buf, uint16_t buf_
 	struct coap_packet response;
 	int r;
 	uint8_t token[8];
+	uint8_t tkl;
 #if defined(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)
 	bool more_blocks = false;
 	uint8_t block_num;
 	uint8_t last_block_num;
 #endif
+	bool has_block1;
+	bool has_block2;
 
 	r = coap_packet_parse(&response, buf, buf_len, NULL, 0);
 	if (r < 0) {
 		LOG_ERR("Invalid data received (err:%d)", r);
 		return;
 	}
 
-	(void)coap_header_get_token(&response, token);
+	tkl = coap_header_get_token(&response, token);
+	has_block1 = coap_get_option_int(&response, COAP_OPTION_BLOCK1) > 0 ? true : false;
+	has_block2 = coap_get_option_int(&response, COAP_OPTION_BLOCK2) > 0 ? true : false;
 	pending = coap_pending_received(&response, client_ctx->pendings,
 					ARRAY_SIZE(client_ctx->pendings));
 	if (pending && coap_header_get_type(&response) == COAP_TYPE_ACK) {
@@ -2676,11 +2754,23 @@ void lwm2m_udp_receive(struct lwm2m_ctx *client_ctx, uint8_t *buf, uint16_t buf_
 			lwm2m_reset_message(msg, true);
 		}
 
+		clear_ongoing_block_tx();
+
 		LOG_DBG("reply %p handled and removed", reply);
 		return;
 	}
 
 	if (coap_header_get_type(&response) == COAP_TYPE_CON) {
+		if (has_block2 && IS_ENABLED(CONFIG_LWM2M_COAP_BLOCK_TRANSFER)) {
+			msg = find_ongoing_block_tx(token, tkl);
+			if (msg) {
+				return handle_ongoing_block_tx(msg, &response);
+			}
+		}
+
+		/* Clear out existing block transfers when new requests come */
+		clear_ongoing_block_tx();
+
 		msg = lwm2m_get_message(client_ctx);
 		if (!msg) {
 			LOG_ERR("Unable to get a lwm2m message!");

subsys/net/lib/lwm2m/lwm2m_message_handling.h

@@ -75,5 +75,6 @@ enum coap_block_size lwm2m_default_block_size(void);
 
 int lwm2m_parse_peerinfo(char *url, struct lwm2m_ctx *client_ctx, bool is_firmware_uri);
 void lwm2m_clear_block_contexts(void);
+bool lwm2m_outgoing_is_part_of_blockwise(struct lwm2m_message *msg);
 
 #endif /* LWM2M_MESSAGE_HANDLING_H */

subsys/net/lib/lwm2m/lwm2m_object.h

@@ -521,8 +521,11 @@ struct lwm2m_message {
 	/** Incoming message action */
 	uint8_t operation;
 
-	/* Information whether the message was acknowledged. */
+	/** Information whether the message was acknowledged. */
 	bool acknowledged : 1;
+
+	/** Indicate that this is part of outgoing block transfer. */
+	bool block_send : 1;
 };
 
 /* LWM2M format writer for the various formats supported */