nanocoap: introduce coap_opt_remove()

sys/include/net/nanocoap.h

@@ -927,6 +927,7 @@ static inline unsigned coap_szx2size(unsigned szx)
 
 /**
  * @name    Functions -- Options Write Packet API
+ * @anchor  net_nanocoap_opt_add
  *
  * Use a coap_pkt_t struct to manage writing Options to the PDU.
  *
@@ -1266,6 +1267,25 @@ static inline ssize_t coap_opt_add_uri_path_buffer(coap_pkt_t *pkt,
  * @return        -ENOSPC if no buffer space for payload marker
  */
 ssize_t coap_opt_finish(coap_pkt_t *pkt, uint16_t flags);
+
+/**
+ * @brief   Removes an option previously added with function in
+ *          [the coap_opt_add_...() group](@ref net_nanocoap_opt_add)
+ *
+ * @param[in] pkt       Packet to remove option from.
+ * @param[in] optnum    Option number of the option to remove. If the option appears more than once,
+ *                      the first occurrence will be removed
+ *
+ * @pre     pkt != NULL
+ * @pre     The packet was finished using @ref coap_opt_finish() and any payload is in its final
+ *          configuration.
+ *
+ * @return  The new size of the packet with option of number @p optnum removed. In turn this means
+ *          if there is no such option in the packet, the original size of the packet will be
+ *          removed.
+ * @return  -EINVAL if there was a parsing error for the existing options
+ */
+ssize_t coap_opt_remove(coap_pkt_t *pkt, uint16_t optnum);
 /**@}*/
 
 /**

sys/net/application_layer/nanocoap/nanocoap.c

@@ -982,6 +982,81 @@ ssize_t coap_opt_finish(coap_pkt_t *pkt, uint16_t flags)
     return pkt->payload - (uint8_t *)pkt->hdr;
 }
 
+ssize_t coap_opt_remove(coap_pkt_t *pkt, uint16_t opt_num)
+{
+    assert(pkt);
+    coap_optpos_t *optpos = pkt->options;
+    coap_optpos_t *prev_opt = NULL;
+    unsigned opt_count = pkt->options_len;
+    uint8_t *start_old = NULL; /* First byte that is memmoved: content of next
+                                * option, payload marker or end of message */
+    uint8_t *start_new = NULL; /* memmove destination: content after rewritten
+                                * next option header, or offset of removed option */
+
+    while (opt_count--) {
+        if ((start_old == NULL) && (optpos->opt_num == opt_num)) {
+            unsigned new_hdr_len, old_hdr_len;
+            int option_len;
+            uint8_t *opt_start;
+            uint16_t old_delta, new_delta;
+
+            if (opt_count == 0) {
+                /* this is the last option => use payload / end pointer as old start */
+                start_old = (pkt->payload_len) ? pkt->payload - 1 : pkt->payload;
+                start_new = (uint8_t *)pkt->hdr + optpos->offset;
+                break;
+            }
+
+            if (prev_opt == NULL) {
+                /* this is the first option => new_delta is just opt_num of next option */
+                new_delta = optpos[1].opt_num;
+            } else {
+                new_delta = optpos[1].opt_num - prev_opt->opt_num;
+            }
+            prev_opt = optpos;
+            optpos++;
+            opt_count--;
+            /* select start of next option */
+            opt_start = (uint8_t *)pkt->hdr + optpos->offset;
+            start_old = _parse_option(pkt, opt_start, &old_delta, &option_len);
+            old_hdr_len = start_old - opt_start;
+
+            /* select start of to be deleted option and set delta/length of next option */
+            start_new = (uint8_t *)pkt->hdr + prev_opt->offset;
+            *start_new = 0;
+            /* write new_delta value to option header: 4 upper bits of header (shift 4) +
+             * 1 or 2 optional bytes depending on delta value) */
+            new_hdr_len = _put_delta_optlen(start_new, 1, 4, new_delta);
+            /* write option length to option header: 4 lower bits of header (shift 0) +
+             * 1 or 2 optional bytes depending of the length of the option */
+            new_hdr_len = _put_delta_optlen(start_new, new_hdr_len, 0, option_len);
+            start_new += new_hdr_len;
+
+            /* account for header length change in next option */
+            optpos->offset -= (new_hdr_len - old_hdr_len);
+        }
+        /* start_old implies start_new */
+        if (start_old != NULL) {
+            assert(start_new);
+            /* adapt options array for removed option */
+            memcpy(prev_opt, optpos, sizeof(*prev_opt));
+            prev_opt->offset -= (start_old - start_new);
+        }
+        prev_opt = optpos;
+        optpos++;
+    }
+    if (start_old != NULL) {
+        size_t move_size = (pkt->payload - start_old) + pkt->payload_len;
+
+        pkt->options_len--;
+        if (move_size > 0) {
+            memmove(start_new, start_old, move_size);
+        }
+        pkt->payload -= (start_old - start_new);
+    }
+    return (pkt->payload - ((uint8_t *)pkt->hdr)) + pkt->payload_len;
+}
+
 ssize_t coap_payload_put_bytes(coap_pkt_t *pkt, const void *data, size_t len)
 {
     if (pkt->payload_len < len) {