nanocoap: added function for finding options

nanocoap/nanocoap.c

@@ -50,6 +50,7 @@ int coap_parse(coap_pkt_t *pkt, uint8_t *buf, size_t len)
     }
 
     /* parse options */
+    pkt->options = (pkt_pos != pkt_end) ? pkt_pos : NULL;
     int option_nr = 0;
     while (pkt_pos != pkt_end) {
         uint8_t option_byte = *pkt_pos++;
@@ -109,6 +110,13 @@ int coap_parse(coap_pkt_t *pkt, uint8_t *buf, size_t len)
         }
     }
 
+    /* set payload pointer to first byte after the options in case no payload
+     * is present, so we can use it as reference to find the end of options
+     * at a later point in time */
+    if (pkt_pos == pkt_end) {
+        pkt->payload = pkt_end;
+    }
+
     DEBUG("coap pkt parsed. code=%u detail=%u payload_len=%u, 0x%02x\n",
             coap_get_code_class(pkt),
             coap_get_code_detail(pkt),
@@ -369,3 +377,68 @@ ssize_t coap_well_known_core_default_handler(coap_pkt_t* pkt, uint8_t *buf, \
 
     return coap_build_reply(pkt, COAP_CODE_205, buf, len, payload_len);
 }
+
+static size_t _decode_optlen(uint8_t *buf, uint16_t *val)
+{
+    size_t len = 0;
+
+    if (*val == 13) {
+        *val += *buf;
+        len = 1;
+    }
+    else if (*val == 14) {
+        memcpy(val, buf, 2);
+        *val = htons(*val) + 269;
+        len = 2;
+    }
+
+    return len;
+}
+
+static int _parse_opt(uint8_t *optpos, coap_opt_t *opt)
+{
+    opt->val = optpos + 1;
+    opt->delta = ((*optpos & 0xf0) >> 4);
+    opt->len =  (*optpos & 0x0f);
+
+    /* make sure delta and len raw values are valid */
+    if ((opt->delta == 15) || (opt->len == 15)) {
+        return -1;
+    }
+
+    opt->val += _decode_optlen(opt->val, &opt->delta);
+    opt->val += _decode_optlen(opt->val, &opt->len);
+
+    return (opt->val - optpos) + opt->len;
+}
+
+uint8_t *coap_find_option(uint8_t *payload_pos, uint8_t *bufpos,
+                          coap_opt_t *opt, uint16_t optnum)
+{
+    assert(opt);
+
+    /* check if we reached the end of options */
+    if (!bufpos || (*bufpos == 0xff)) {
+        return NULL;
+    }
+
+    uint16_t delta = 0;
+
+    do {
+        if (bufpos >= payload_pos) {
+            return NULL;
+        }
+        int res = _parse_opt(bufpos, opt);
+        if (res < 0) {
+            return NULL;
+        }
+        bufpos += res;
+        delta += opt->delta;
+    } while (delta < optnum);
+
+    if (delta != optnum) {
+        bufpos = NULL;
+    }
+
+    return bufpos;
+}

nanocoap/nanocoap.h

@@ -13,6 +13,7 @@
 
 #define COAP_OPT_URI_HOST       (3)
 #define COAP_OPT_OBSERVE        (6)
+#define COAP_OPT_LOCATION_PATH  (8)
 #define COAP_OPT_URI_PATH       (11)
 #define COAP_OPT_CONTENT_FORMAT (12)
 #define COAP_OPT_URI_QUERY      (15)
@@ -141,6 +142,7 @@ typedef struct {
     uint8_t url[NANOCOAP_URL_MAX];
     uint8_t qs[NANOCOAP_QS_MAX];
     uint8_t *token;
+    uint8_t *options;
     uint8_t *payload;
     unsigned payload_len;
     uint16_t content_type;
@@ -155,6 +157,12 @@ typedef struct {
     coap_handler_t handler;
 } coap_resource_t;
 
+typedef struct {
+    uint16_t delta;
+    uint16_t len;
+    uint8_t *val;
+} coap_opt_t;
+
 extern const coap_resource_t coap_resources[];
 extern const unsigned coap_resources_numof;
 
@@ -175,6 +183,8 @@ size_t coap_put_option(uint8_t *buf, uint16_t lastonum, uint16_t onum, uint8_t *
 size_t coap_put_option_ct(uint8_t *buf, uint16_t lastonum, uint16_t content_type);
 size_t coap_put_option_uri(uint8_t *buf, uint16_t lastonum, const char *uri, uint16_t optnum);
 
+uint8_t *coap_find_option(uint8_t *payload_pos, uint8_t *bufpos, coap_opt_t *opt, uint16_t optnum);
+
 static inline unsigned coap_get_ver(coap_pkt_t *pkt)
 {
     return (pkt->hdr->ver_t_tkl & 0x60) >> 6;