Commit 6a526039 authored by Lukas Woodtli's avatar Lukas Woodtli Committed by Carles Cufi
Browse files

net: coap: Allow insertion of an option at arbitrary position 



The insertion moves the data in the message buffer to make space for
the new option.

Signed-off-by: default avatarLukas Woodtli <lukas.woodtli@husqvarnagroup.com>
parent 7e20b819

subsys/net/lib/coap/coap.c

+135 −25
Original line number Diff line number Diff line
@@ -49,49 +49,103 @@ LOG_MODULE_REGISTER(net_coap, CONFIG_COAP_LOG_LEVEL); 
/* The CoAP message ID that is incremented each time coap_next_id() is called. */

static uint16_t message_id;



static int insert_option(struct coap_packet *cpkt, uint16_t code, const uint8_t *value,

			 uint16_t len);



static inline void encode_u8(struct coap_packet *cpkt, uint16_t offset, uint8_t data)

{

	cpkt->data[offset] = data;

	++cpkt->offset;

}



static inline void encode_be16(struct coap_packet *cpkt, uint16_t offset, uint16_t data)

{

	cpkt->data[offset] = data >> 8;

	cpkt->data[offset + 1] = (uint8_t)data;

	cpkt->offset += 2;

}



static inline void encode_buffer(struct coap_packet *cpkt, uint16_t offset, const uint8_t *data,

				 uint16_t len)

{

	memcpy(cpkt->data + offset, data, len);

	cpkt->offset += len;

}



static bool enough_space(struct coap_packet *cpkt, const uint16_t bytes_to_add)

{

	return (cpkt != NULL) && (cpkt->max_len - cpkt->offset >= bytes_to_add);

}



static inline bool append_u8(struct coap_packet *cpkt, uint8_t data)

{

	if (!cpkt) {

	if (!enough_space(cpkt, 1)) {

		return false;

	}



	if (cpkt->max_len - cpkt->offset < 1) {

	encode_u8(cpkt, cpkt->offset, data);



	return true;

}



static inline bool insert_u8(struct coap_packet *cpkt, uint8_t data, uint16_t offset)

{

	if (!enough_space(cpkt, 1)) {

		return false;

	}



	cpkt->data[cpkt->offset++] = data;

	memmove(&cpkt->data[offset + 1], &cpkt->data[offset], cpkt->offset - offset);



	encode_u8(cpkt, offset, data);



	return true;

}



static inline bool append_be16(struct coap_packet *cpkt, uint16_t data)

{

	if (!cpkt) {

	if (!enough_space(cpkt, 2)) {

		return false;

	}



	if (cpkt->max_len - cpkt->offset < 2) {

	encode_be16(cpkt, cpkt->offset, data);



	return true;

}



static inline bool insert_be16(struct coap_packet *cpkt, uint16_t data, size_t offset)

{

	if (!enough_space(cpkt, 2)) {

		return false;

	}



	cpkt->data[cpkt->offset++] = data >> 8;

	cpkt->data[cpkt->offset++] = (uint8_t) data;

	memmove(&cpkt->data[offset + 2], &cpkt->data[offset], cpkt->offset - offset);



	encode_be16(cpkt, cpkt->offset, data);



	return true;

}



static inline bool append(struct coap_packet *cpkt, const uint8_t *data, uint16_t len)

{

	if (!cpkt || !data) {

	if (data == NULL || !enough_space(cpkt, len)) {

		return false;

	}



	if (cpkt->max_len - cpkt->offset < len) {

	encode_buffer(cpkt, cpkt->offset, data, len);



	return true;

}



static inline bool insert(struct coap_packet *cpkt, const uint8_t *data, uint16_t len,

			  size_t offset)

{

	if (data == NULL || !enough_space(cpkt, len)) {

		return false;

	}



	memcpy(cpkt->data + cpkt->offset, data, len);

	cpkt->offset += len;

	memmove(&cpkt->data[offset + len], &cpkt->data[offset], cpkt->offset - offset);



	encode_buffer(cpkt, offset, data, len);



	return true;

}
@@ -192,8 +246,11 @@ static uint8_t encode_extended_option(uint16_t num, uint8_t *opt, uint16_t *ext) 
	return 2;

}



static int encode_option(struct coap_packet *cpkt, uint16_t code,

			 const uint8_t *value, uint16_t len)

/* Insert an option at position `offset`. This is not adjusting the code delta of the

 * option that follows the inserted one!

 */

static int encode_option(struct coap_packet *cpkt, uint16_t code, const uint8_t *value,

			 uint16_t len, size_t offset)

{

	uint16_t delta_ext; /* Extended delta */

	uint16_t len_ext; /* Extended length */
@@ -210,37 +267,43 @@ static int encode_option(struct coap_packet *cpkt, uint16_t code, 
	option_header_set_delta(&opt, opt_delta);

	option_header_set_len(&opt, opt_len);



	res = append_u8(cpkt, opt);

	res = insert_u8(cpkt, opt, offset);

	++offset;

	if (!res) {

		return -EINVAL;

	}



	if (delta_size == 1U) {

		res = append_u8(cpkt, (uint8_t)delta_ext);

		res = insert_u8(cpkt, (uint8_t)delta_ext, offset);

		++offset;

		if (!res) {

			return -EINVAL;

		}

	} else if (delta_size == 2U) {

		res = append_be16(cpkt, delta_ext);

		res = insert_be16(cpkt, delta_ext, offset);

		offset += 2;

		if (!res) {

			return -EINVAL;

		}

	}



	if (len_size == 1U) {

		res = append_u8(cpkt, (uint8_t)len_ext);

		res = insert_u8(cpkt, (uint8_t)len_ext, offset);

		++offset;

		if (!res) {

			return -EINVAL;

		}

	} else if (len_size == 2U) {

		res = append_be16(cpkt, len_ext);

		res = insert_be16(cpkt, len_ext, offset);

		offset += 2;

		if (!res) {

			return -EINVAL;

		}

	}



	if (len && value) {

		res = append(cpkt, value, len);

		res = insert(cpkt, value, len, offset);

		/* no need to update local offset */

		if (!res) {

			return -EINVAL;

		}
@@ -249,9 +312,6 @@ static int encode_option(struct coap_packet *cpkt, uint16_t code, 
	return  (1 + delta_size + len_size + len);

}



/* TODO Add support for inserting options in proper place

 * and modify other option's delta accordingly.

 */

int coap_packet_append_option(struct coap_packet *cpkt, uint16_t code,

			      const uint8_t *value, uint16_t len)

{
@@ -266,8 +326,8 @@ int coap_packet_append_option(struct coap_packet *cpkt, uint16_t code, 
	}



	if (code < cpkt->delta) {

		NET_ERR("Options should be in ascending order");

		return -EINVAL;

		NET_DBG("Option is not added in ascending order");

		return insert_option(cpkt, code, value, len);

	}



	/* Calculate delta, if this option is not the first one */
@@ -275,7 +335,7 @@ int coap_packet_append_option(struct coap_packet *cpkt, uint16_t code, 
		code = (code == cpkt->delta) ? 0 : code - cpkt->delta;

	}



	r = encode_option(cpkt, code, value, len);

	r = encode_option(cpkt, code, value, len, cpkt->offset);

	if (r < 0) {

		return -EINVAL;

	}
@@ -981,6 +1041,56 @@ int coap_get_option_int(const struct coap_packet *cpkt, uint16_t code) 
	return val;

}



int insert_option(struct coap_packet *cpkt, uint16_t code, const uint8_t *value, uint16_t len)

{

	uint16_t offset = cpkt->hdr_len;

	uint16_t opt_delta = 0;

	uint16_t opt_len = 0;

	uint16_t last_opt = 0;

	uint16_t last_offset = cpkt->hdr_len;

	struct coap_option option;

	int r;



	while (offset < cpkt->hdr_len + cpkt->opt_len) {

		r = parse_option(cpkt->data, offset, &offset, cpkt->hdr_len + cpkt->opt_len,

				 &opt_delta, &opt_len, &option);

		if (r < 0) {

			return -EILSEQ;

		}



		if (opt_delta > code) {

			break;

		}



		last_opt = opt_delta;

		last_offset = offset;

	}



	const uint16_t option_size = offset - last_offset;

	/* clear option after new option (delta changed) */

	memmove(cpkt->data + last_offset, cpkt->data + offset, cpkt->offset - offset);

	cpkt->opt_len -= option_size;

	cpkt->offset -= option_size;



	/* add the new option */

	const uint16_t new_option_delta = code - last_opt;



	r = encode_option(cpkt, new_option_delta, value, len, last_offset);

	if (r < 0) {

		return -EINVAL;

	}

	cpkt->opt_len += r;



	/* reinsert option that comes after the new option (with adjusted delta) */

	r = encode_option(cpkt, option.delta - code, option.value, option.len, last_offset + r);

	if (r < 0) {

		return -EINVAL;

	}

	cpkt->opt_len += r;



	return 0;

}



static int update_descriptive_block(struct coap_block_context *ctx,

				    int block, int size)

{

CRA Git | Maintained and supported by SUSTech CRA and CCSE