Patrick Barrett / libexositecoap

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

picocoap/src/coap.c@7:f9df43829cea, 2014-12-19 (annotated)

Committer:
Patrick Barrett
Date:
Fri Dec 19 15:43:03 2014 -0600
Revision:
7:f9df43829cea
Child:
22:6a6810897eb9
make picocoap a subtree instead of submodule

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Patrick Barrett 7:f9df43829cea 1
Patrick Barrett 7:f9df43829cea 2 #include <stddef.h>
Patrick Barrett 7:f9df43829cea 3 #include <string.h>
Patrick Barrett 7:f9df43829cea 4 #include "coap.h"
Patrick Barrett 7:f9df43829cea 5
Patrick Barrett 7:f9df43829cea 6
Patrick Barrett 7:f9df43829cea 7 //
Patrick Barrett 7:f9df43829cea 8 // Getters
Patrick Barrett 7:f9df43829cea 9 //
Patrick Barrett 7:f9df43829cea 10
Patrick Barrett 7:f9df43829cea 11 coap_error coap_validate_pkt(coap_pdu *pdu) //uint8_t *pkt, size_t pkt_len)
Patrick Barrett 7:f9df43829cea 12 {
Patrick Barrett 7:f9df43829cea 13 coap_error err;
Patrick Barrett 7:f9df43829cea 14 size_t ol;
Patrick Barrett 7:f9df43829cea 15 uint8_t *ov;
Patrick Barrett 7:f9df43829cea 16
Patrick Barrett 7:f9df43829cea 17 pdu->opt_ptr = NULL;
Patrick Barrett 7:f9df43829cea 18
Patrick Barrett 7:f9df43829cea 19 if (pdu->len > pdu->max)
Patrick Barrett 7:f9df43829cea 20 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 21
Patrick Barrett 7:f9df43829cea 22 if (pdu->len < 4)
Patrick Barrett 7:f9df43829cea 23 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 24
Patrick Barrett 7:f9df43829cea 25 // Check Version
Patrick Barrett 7:f9df43829cea 26 if (coap_get_version(pdu) != 1)
Patrick Barrett 7:f9df43829cea 27 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 28
Patrick Barrett 7:f9df43829cea 29 // Check TKL
Patrick Barrett 7:f9df43829cea 30 if (coap_get_tkl(pdu) > 8)
Patrick Barrett 7:f9df43829cea 31 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 32
Patrick Barrett 7:f9df43829cea 33 // Check Options
Patrick Barrett 7:f9df43829cea 34 ov = pdu->buf + 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 35 ol = 0;
Patrick Barrett 7:f9df43829cea 36 while((err = coap_decode_option(ov + ol, pdu->len-(ov-pdu->buf), NULL, &ol, &ov)) != 0){
Patrick Barrett 7:f9df43829cea 37 if (err == CE_NONE){
Patrick Barrett 7:f9df43829cea 38 continue;
Patrick Barrett 7:f9df43829cea 39 } else if (err == CE_END_OF_PACKET){
Patrick Barrett 7:f9df43829cea 40 break;
Patrick Barrett 7:f9df43829cea 41 } else if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 42 // Payload Marker, but No Payload
Patrick Barrett 7:f9df43829cea 43 if (pdu->len == (ov + ol - pdu->buf)){
Patrick Barrett 7:f9df43829cea 44 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 45 } else {
Patrick Barrett 7:f9df43829cea 46 break;
Patrick Barrett 7:f9df43829cea 47 }
Patrick Barrett 7:f9df43829cea 48 } else {
Patrick Barrett 7:f9df43829cea 49 return err;
Patrick Barrett 7:f9df43829cea 50 }
Patrick Barrett 7:f9df43829cea 51 }
Patrick Barrett 7:f9df43829cea 52
Patrick Barrett 7:f9df43829cea 53 return CE_NONE;
Patrick Barrett 7:f9df43829cea 54 }
Patrick Barrett 7:f9df43829cea 55
Patrick Barrett 7:f9df43829cea 56 uint64_t coap_get_token(coap_pdu *pdu)
Patrick Barrett 7:f9df43829cea 57 {
Patrick Barrett 7:f9df43829cea 58 uint8_t tkl;
Patrick Barrett 7:f9df43829cea 59 uint64_t token = 0;
Patrick Barrett 7:f9df43829cea 60
Patrick Barrett 7:f9df43829cea 61 // Extract TKL.
Patrick Barrett 7:f9df43829cea 62 tkl = pdu->buf[0] & 0x0F;
Patrick Barrett 7:f9df43829cea 63
Patrick Barrett 7:f9df43829cea 64 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 65 if (pdu->len < 4 + tkl)
Patrick Barrett 7:f9df43829cea 66 return 0;
Patrick Barrett 7:f9df43829cea 67
Patrick Barrett 7:f9df43829cea 68 // Set token.
Patrick Barrett 7:f9df43829cea 69 memcpy(&token, &pdu->buf[4], tkl);
Patrick Barrett 7:f9df43829cea 70
Patrick Barrett 7:f9df43829cea 71 return token;
Patrick Barrett 7:f9df43829cea 72 }
Patrick Barrett 7:f9df43829cea 73
Patrick Barrett 7:f9df43829cea 74 coap_option coap_get_option(coap_pdu *pdu, coap_option *last)
Patrick Barrett 7:f9df43829cea 75 {
Patrick Barrett 7:f9df43829cea 76 uint8_t *opt_ptr;
Patrick Barrett 7:f9df43829cea 77 coap_option option;
Patrick Barrett 7:f9df43829cea 78 coap_error err;
Patrick Barrett 7:f9df43829cea 79
Patrick Barrett 7:f9df43829cea 80 if (last != NULL && last->num != 0){
Patrick Barrett 7:f9df43829cea 81 option.num = last->num;
Patrick Barrett 7:f9df43829cea 82 option.len = 0;
Patrick Barrett 7:f9df43829cea 83 option.val = NULL;
Patrick Barrett 7:f9df43829cea 84
Patrick Barrett 7:f9df43829cea 85 opt_ptr = last->val + last->len;
Patrick Barrett 7:f9df43829cea 86 } else {
Patrick Barrett 7:f9df43829cea 87 option.num = 0;
Patrick Barrett 7:f9df43829cea 88 option.len = 0;
Patrick Barrett 7:f9df43829cea 89 option.val = NULL;
Patrick Barrett 7:f9df43829cea 90
Patrick Barrett 7:f9df43829cea 91 opt_ptr = pdu->buf + 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 92 }
Patrick Barrett 7:f9df43829cea 93
Patrick Barrett 7:f9df43829cea 94 // If opt_ptr is outside pkt range, put it at first opt.
Patrick Barrett 7:f9df43829cea 95 if (opt_ptr > (pdu->buf + pdu->len) || opt_ptr <= pdu->buf){
Patrick Barrett 7:f9df43829cea 96 opt_ptr = pdu->buf + 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 97 }
Patrick Barrett 7:f9df43829cea 98
Patrick Barrett 7:f9df43829cea 99 err = coap_decode_option(opt_ptr, pdu->len-(opt_ptr-pdu->buf), &option.num, &option.len, &option.val);
Patrick Barrett 7:f9df43829cea 100
Patrick Barrett 7:f9df43829cea 101 if (err != CE_NONE){
Patrick Barrett 7:f9df43829cea 102 if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 103 if (option.num == 0){
Patrick Barrett 7:f9df43829cea 104 option.val = opt_ptr + 1;
Patrick Barrett 7:f9df43829cea 105 option.len = pdu->len-(opt_ptr-pdu->buf) - 1;
Patrick Barrett 7:f9df43829cea 106 } else {
Patrick Barrett 7:f9df43829cea 107 option.val = option.val + option.len;
Patrick Barrett 7:f9df43829cea 108 option.len = pdu->len - (option.val - pdu->buf);
Patrick Barrett 7:f9df43829cea 109 }
Patrick Barrett 7:f9df43829cea 110 } else {
Patrick Barrett 7:f9df43829cea 111 option.val = NULL;
Patrick Barrett 7:f9df43829cea 112 option.len = 0;
Patrick Barrett 7:f9df43829cea 113 }
Patrick Barrett 7:f9df43829cea 114 option.num = 0;
Patrick Barrett 7:f9df43829cea 115 }
Patrick Barrett 7:f9df43829cea 116
Patrick Barrett 7:f9df43829cea 117 opt_ptr = option.val + option.len;
Patrick Barrett 7:f9df43829cea 118
Patrick Barrett 7:f9df43829cea 119 return option;
Patrick Barrett 7:f9df43829cea 120 }
Patrick Barrett 7:f9df43829cea 121
Patrick Barrett 7:f9df43829cea 122
Patrick Barrett 7:f9df43829cea 123 coap_option coap_get_option_by_num(coap_pdu *pdu, coap_option_number num, uint8_t occ)
Patrick Barrett 7:f9df43829cea 124 {
Patrick Barrett 7:f9df43829cea 125 coap_option option;
Patrick Barrett 7:f9df43829cea 126 uint8_t i = 0;
Patrick Barrett 7:f9df43829cea 127
Patrick Barrett 7:f9df43829cea 128 option.num = 0;
Patrick Barrett 7:f9df43829cea 129
Patrick Barrett 7:f9df43829cea 130 do {
Patrick Barrett 7:f9df43829cea 131 option = coap_get_option(pdu, &option);
Patrick Barrett 7:f9df43829cea 132
Patrick Barrett 7:f9df43829cea 133 if (option.num == num) {
Patrick Barrett 7:f9df43829cea 134 i++;
Patrick Barrett 7:f9df43829cea 135 }
Patrick Barrett 7:f9df43829cea 136 } while (i < occ);
Patrick Barrett 7:f9df43829cea 137
Patrick Barrett 7:f9df43829cea 138 return option;
Patrick Barrett 7:f9df43829cea 139 }
Patrick Barrett 7:f9df43829cea 140
Patrick Barrett 7:f9df43829cea 141
Patrick Barrett 7:f9df43829cea 142 //
Patrick Barrett 7:f9df43829cea 143 // Decoding Functions (Intended for Internal Use)
Patrick Barrett 7:f9df43829cea 144 //
Patrick Barrett 7:f9df43829cea 145
Patrick Barrett 7:f9df43829cea 146 coap_error coap_decode_option(uint8_t *pkt_ptr, size_t pkt_len,
Patrick Barrett 7:f9df43829cea 147 uint16_t *option_number, size_t *option_length, uint8_t **value)
Patrick Barrett 7:f9df43829cea 148 {
Patrick Barrett 7:f9df43829cea 149 uint8_t *ptr = pkt_ptr;
Patrick Barrett 7:f9df43829cea 150 uint16_t delta, length;
Patrick Barrett 7:f9df43829cea 151
Patrick Barrett 7:f9df43829cea 152 // Check for end of Packet
Patrick Barrett 7:f9df43829cea 153 if (pkt_len == 0){
Patrick Barrett 7:f9df43829cea 154 return CE_END_OF_PACKET;
Patrick Barrett 7:f9df43829cea 155 }
Patrick Barrett 7:f9df43829cea 156
Patrick Barrett 7:f9df43829cea 157 // Check for Payload Marker
Patrick Barrett 7:f9df43829cea 158 if (*ptr == 0xFF){
Patrick Barrett 7:f9df43829cea 159 return CE_FOUND_PAYLOAD_MARKER;
Patrick Barrett 7:f9df43829cea 160 }
Patrick Barrett 7:f9df43829cea 161
Patrick Barrett 7:f9df43829cea 162 // Get Base Delta and Length
Patrick Barrett 7:f9df43829cea 163 delta = *ptr >> 4;
Patrick Barrett 7:f9df43829cea 164 length = *ptr & 0x0F;
Patrick Barrett 7:f9df43829cea 165 ptr++;
Patrick Barrett 7:f9df43829cea 166
Patrick Barrett 7:f9df43829cea 167 // Check for and Get Extended Delta
Patrick Barrett 7:f9df43829cea 168 if (delta < 13) {
Patrick Barrett 7:f9df43829cea 169 //delta = delta;
Patrick Barrett 7:f9df43829cea 170 }else if (delta == 13) {
Patrick Barrett 7:f9df43829cea 171 delta = *ptr + 13;
Patrick Barrett 7:f9df43829cea 172 ptr += 1;
Patrick Barrett 7:f9df43829cea 173 }else if (delta == 14) {
Patrick Barrett 7:f9df43829cea 174 delta = (*ptr << 8) + *(ptr+1) + 269;
Patrick Barrett 7:f9df43829cea 175 ptr += 2;
Patrick Barrett 7:f9df43829cea 176 }else{
Patrick Barrett 7:f9df43829cea 177 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 178 }
Patrick Barrett 7:f9df43829cea 179
Patrick Barrett 7:f9df43829cea 180 // Check for and Get Extended Length
Patrick Barrett 7:f9df43829cea 181 if (length < 13) {
Patrick Barrett 7:f9df43829cea 182 //length = length;
Patrick Barrett 7:f9df43829cea 183 }else if (length == 13) {
Patrick Barrett 7:f9df43829cea 184 length = *ptr + 13;
Patrick Barrett 7:f9df43829cea 185 ptr += 1;
Patrick Barrett 7:f9df43829cea 186 }else if (length == 14) {
Patrick Barrett 7:f9df43829cea 187 length = (*ptr << 8) + *(ptr+1) + 269;
Patrick Barrett 7:f9df43829cea 188 ptr += 2;
Patrick Barrett 7:f9df43829cea 189 }else{
Patrick Barrett 7:f9df43829cea 190 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 191 }
Patrick Barrett 7:f9df43829cea 192
Patrick Barrett 7:f9df43829cea 193 if (option_number != NULL)
Patrick Barrett 7:f9df43829cea 194 *option_number += delta;
Patrick Barrett 7:f9df43829cea 195
Patrick Barrett 7:f9df43829cea 196 if (option_length != NULL)
Patrick Barrett 7:f9df43829cea 197 *option_length = length;
Patrick Barrett 7:f9df43829cea 198
Patrick Barrett 7:f9df43829cea 199 if (value != NULL)
Patrick Barrett 7:f9df43829cea 200 *value = ptr;
Patrick Barrett 7:f9df43829cea 201
Patrick Barrett 7:f9df43829cea 202 return CE_NONE;
Patrick Barrett 7:f9df43829cea 203 }
Patrick Barrett 7:f9df43829cea 204
Patrick Barrett 7:f9df43829cea 205
Patrick Barrett 7:f9df43829cea 206 coap_payload coap_get_payload(coap_pdu *pdu)
Patrick Barrett 7:f9df43829cea 207 {
Patrick Barrett 7:f9df43829cea 208
Patrick Barrett 7:f9df43829cea 209 size_t offset = 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 210 coap_option option;
Patrick Barrett 7:f9df43829cea 211 coap_payload payload;
Patrick Barrett 7:f9df43829cea 212 coap_error err;
Patrick Barrett 7:f9df43829cea 213
Patrick Barrett 7:f9df43829cea 214 // Defaults
Patrick Barrett 7:f9df43829cea 215 payload.len = 0;
Patrick Barrett 7:f9df43829cea 216 payload.val = NULL;
Patrick Barrett 7:f9df43829cea 217
Patrick Barrett 7:f9df43829cea 218 // Find Last Option
Patrick Barrett 7:f9df43829cea 219 do {
Patrick Barrett 7:f9df43829cea 220 err = coap_decode_option(pdu->buf+offset, pdu->len-offset, NULL, &option.len, &option.val);
Patrick Barrett 7:f9df43829cea 221 if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
Patrick Barrett 7:f9df43829cea 222 break;
Patrick Barrett 7:f9df43829cea 223
Patrick Barrett 7:f9df43829cea 224 if (err != CE_NONE)
Patrick Barrett 7:f9df43829cea 225 return payload;
Patrick Barrett 7:f9df43829cea 226
Patrick Barrett 7:f9df43829cea 227 // Add this option header and value length to offset.
Patrick Barrett 7:f9df43829cea 228 offset += (option.val - (pdu->buf+offset)) + option.len;
Patrick Barrett 7:f9df43829cea 229 } while (1);
Patrick Barrett 7:f9df43829cea 230
Patrick Barrett 7:f9df43829cea 231 if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 232 payload.len = pdu->len - offset - 1;
Patrick Barrett 7:f9df43829cea 233 payload.val = pdu->buf + offset + 1;
Patrick Barrett 7:f9df43829cea 234 }
Patrick Barrett 7:f9df43829cea 235
Patrick Barrett 7:f9df43829cea 236 return payload;
Patrick Barrett 7:f9df43829cea 237 }
Patrick Barrett 7:f9df43829cea 238
Patrick Barrett 7:f9df43829cea 239
Patrick Barrett 7:f9df43829cea 240 //
Patrick Barrett 7:f9df43829cea 241 // Setters
Patrick Barrett 7:f9df43829cea 242 //
Patrick Barrett 7:f9df43829cea 243
Patrick Barrett 7:f9df43829cea 244 coap_error coap_init_pdu(coap_pdu *pdu)
Patrick Barrett 7:f9df43829cea 245 {
Patrick Barrett 7:f9df43829cea 246 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 247 if (pdu->max < 4)
Patrick Barrett 7:f9df43829cea 248 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 249
Patrick Barrett 7:f9df43829cea 250 coap_set_version(pdu, COAP_V1);
Patrick Barrett 7:f9df43829cea 251 coap_set_type(pdu, CT_RST);
Patrick Barrett 7:f9df43829cea 252 coap_set_token(pdu, 0, 0);
Patrick Barrett 7:f9df43829cea 253 coap_set_code(pdu, CC_EMPTY);
Patrick Barrett 7:f9df43829cea 254 coap_set_mid(pdu, 0);
Patrick Barrett 7:f9df43829cea 255
Patrick Barrett 7:f9df43829cea 256 pdu->len = 4;
Patrick Barrett 7:f9df43829cea 257 pdu->opt_ptr = NULL;
Patrick Barrett 7:f9df43829cea 258
Patrick Barrett 7:f9df43829cea 259 return CE_NONE;
Patrick Barrett 7:f9df43829cea 260 }
Patrick Barrett 7:f9df43829cea 261
Patrick Barrett 7:f9df43829cea 262 coap_error coap_set_version(coap_pdu *pdu, coap_version ver)
Patrick Barrett 7:f9df43829cea 263 {
Patrick Barrett 7:f9df43829cea 264 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 265 if (pdu->max < 1)
Patrick Barrett 7:f9df43829cea 266 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 267
Patrick Barrett 7:f9df43829cea 268 pdu->buf[0] = (ver << 6) | (pdu->buf[0] & 0x3F);
Patrick Barrett 7:f9df43829cea 269
Patrick Barrett 7:f9df43829cea 270 if (pdu->len < 1)
Patrick Barrett 7:f9df43829cea 271 pdu->len = 1;
Patrick Barrett 7:f9df43829cea 272
Patrick Barrett 7:f9df43829cea 273 return CE_NONE;
Patrick Barrett 7:f9df43829cea 274 }
Patrick Barrett 7:f9df43829cea 275
Patrick Barrett 7:f9df43829cea 276 coap_error coap_set_type(coap_pdu *pdu, coap_type mtype)
Patrick Barrett 7:f9df43829cea 277 {
Patrick Barrett 7:f9df43829cea 278 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 279 if (pdu->max < 1)
Patrick Barrett 7:f9df43829cea 280 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 281
Patrick Barrett 7:f9df43829cea 282 pdu->buf[0] = (mtype << 4) | (pdu->buf[0] & 0xCF);
Patrick Barrett 7:f9df43829cea 283
Patrick Barrett 7:f9df43829cea 284 if (pdu->len < 1)
Patrick Barrett 7:f9df43829cea 285 pdu->len = 1;
Patrick Barrett 7:f9df43829cea 286
Patrick Barrett 7:f9df43829cea 287 return CE_NONE;
Patrick Barrett 7:f9df43829cea 288 }
Patrick Barrett 7:f9df43829cea 289
Patrick Barrett 7:f9df43829cea 290 coap_error coap_set_code(coap_pdu *pdu, coap_code code)
Patrick Barrett 7:f9df43829cea 291 {
Patrick Barrett 7:f9df43829cea 292 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 293 if (pdu->max < 2)
Patrick Barrett 7:f9df43829cea 294 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 295
Patrick Barrett 7:f9df43829cea 296 pdu->buf[1] = code;
Patrick Barrett 7:f9df43829cea 297
Patrick Barrett 7:f9df43829cea 298 if (pdu->len < 2)
Patrick Barrett 7:f9df43829cea 299 pdu->len = 2;
Patrick Barrett 7:f9df43829cea 300
Patrick Barrett 7:f9df43829cea 301 return CE_NONE;
Patrick Barrett 7:f9df43829cea 302 }
Patrick Barrett 7:f9df43829cea 303
Patrick Barrett 7:f9df43829cea 304 coap_error coap_set_mid(coap_pdu *pdu, uint16_t mid)
Patrick Barrett 7:f9df43829cea 305 {
Patrick Barrett 7:f9df43829cea 306 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 307 if (pdu->max < 4)
Patrick Barrett 7:f9df43829cea 308 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 309
Patrick Barrett 7:f9df43829cea 310 pdu->buf[2] = mid >> 8;
Patrick Barrett 7:f9df43829cea 311 pdu->buf[3] = mid & 0xFF;
Patrick Barrett 7:f9df43829cea 312
Patrick Barrett 7:f9df43829cea 313 if (pdu->len < 4)
Patrick Barrett 7:f9df43829cea 314 pdu->len = 4;
Patrick Barrett 7:f9df43829cea 315
Patrick Barrett 7:f9df43829cea 316 return CE_NONE;
Patrick Barrett 7:f9df43829cea 317 }
Patrick Barrett 7:f9df43829cea 318
Patrick Barrett 7:f9df43829cea 319 coap_error coap_set_token(coap_pdu *pdu, uint64_t token, uint8_t tkl)
Patrick Barrett 7:f9df43829cea 320 {
Patrick Barrett 7:f9df43829cea 321 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 322 if (pdu->max < 4 + tkl)
Patrick Barrett 7:f9df43829cea 323 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 324
Patrick Barrett 7:f9df43829cea 325 // Check token length for spec.
Patrick Barrett 7:f9df43829cea 326 if (tkl > 8)
Patrick Barrett 7:f9df43829cea 327 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 328
Patrick Barrett 7:f9df43829cea 329 // Check if we may need to make or take room.
Patrick Barrett 7:f9df43829cea 330 if (pdu->len > 4){
Patrick Barrett 7:f9df43829cea 331 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 332 if (pdu->max < pdu->len + (tkl - coap_get_tkl(pdu)))
Patrick Barrett 7:f9df43829cea 333 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 334
Patrick Barrett 7:f9df43829cea 335 // Move rest of packet to make room or take empty space.
Patrick Barrett 7:f9df43829cea 336 memmove(pdu->buf + 4 + tkl, pdu->buf + 4 + coap_get_tkl(pdu), pdu->len - 4 - coap_get_tkl(pdu));
Patrick Barrett 7:f9df43829cea 337 }
Patrick Barrett 7:f9df43829cea 338
Patrick Barrett 7:f9df43829cea 339 // Set token.
Patrick Barrett 7:f9df43829cea 340 memcpy(pdu->buf+4, &token, tkl);
Patrick Barrett 7:f9df43829cea 341
Patrick Barrett 7:f9df43829cea 342 pdu->len += tkl - coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 343
Patrick Barrett 7:f9df43829cea 344 pdu->buf[0] = (tkl & 0x0F) | (pdu->buf[0] & 0xF0);
Patrick Barrett 7:f9df43829cea 345
Patrick Barrett 7:f9df43829cea 346 return CE_NONE;
Patrick Barrett 7:f9df43829cea 347 }
Patrick Barrett 7:f9df43829cea 348
Patrick Barrett 7:f9df43829cea 349 coap_error coap_add_option(coap_pdu *pdu, int32_t opt_num, uint8_t* value, uint16_t opt_len)
Patrick Barrett 7:f9df43829cea 350 {
Patrick Barrett 7:f9df43829cea 351 uint8_t *pkt_ptr, *fopt_val, nopt_hdr_len;
Patrick Barrett 7:f9df43829cea 352 uint16_t fopt_num, lopt_num;
Patrick Barrett 7:f9df43829cea 353 size_t fopt_len, opts_len;
Patrick Barrett 7:f9df43829cea 354 coap_error err;
Patrick Barrett 7:f9df43829cea 355
Patrick Barrett 7:f9df43829cea 356 // Set pointer to "zeroth option's value" which is really first option header.
Patrick Barrett 7:f9df43829cea 357 fopt_val = pdu->buf + 4 + coap_get_tkl(pdu); // ptr to start of options
Patrick Barrett 7:f9df43829cea 358 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 359
Patrick Barrett 7:f9df43829cea 360 // Option number delta starts at zero.
Patrick Barrett 7:f9df43829cea 361 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 362
Patrick Barrett 7:f9df43829cea 363 // Find insertion point
Patrick Barrett 7:f9df43829cea 364 do{
Patrick Barrett 7:f9df43829cea 365 pkt_ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 366 lopt_num = fopt_num;
Patrick Barrett 7:f9df43829cea 367 err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 368 }while (err == CE_NONE && fopt_num <= opt_num && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
Patrick Barrett 7:f9df43829cea 369
Patrick Barrett 7:f9df43829cea 370 if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
Patrick Barrett 7:f9df43829cea 371 return err;
Patrick Barrett 7:f9df43829cea 372
Patrick Barrett 7:f9df43829cea 373 // Build New Header
Patrick Barrett 7:f9df43829cea 374 nopt_hdr_len = coap_compute_option_header_len(opt_num - lopt_num, opt_len);
Patrick Barrett 7:f9df43829cea 375
Patrick Barrett 7:f9df43829cea 376 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 377 if (pdu->max < pdu->len + nopt_hdr_len + opt_len)
Patrick Barrett 7:f9df43829cea 378 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 379
Patrick Barrett 7:f9df43829cea 380 // Check if we're adding an option in the middle of a packet.
Patrick Barrett 7:f9df43829cea 381 // But seriously, don't do this.
Patrick Barrett 7:f9df43829cea 382 if (pdu->len != pkt_ptr- pdu->buf){
Patrick Barrett 7:f9df43829cea 383 // Slide packet tail to make room.
Patrick Barrett 7:f9df43829cea 384 memmove(pkt_ptr + nopt_hdr_len + opt_len, pkt_ptr, pdu->len - (pkt_ptr - pdu->buf));
Patrick Barrett 7:f9df43829cea 385 pdu->len += nopt_hdr_len + opt_len;
Patrick Barrett 7:f9df43829cea 386
Patrick Barrett 7:f9df43829cea 387 // Find Current Length of Remaining Options
Patrick Barrett 7:f9df43829cea 388 opts_len = pdu->len - (pkt_ptr-pdu->buf);
Patrick Barrett 7:f9df43829cea 389
Patrick Barrett 7:f9df43829cea 390 // Adjust the option deltas for the rest of the options.
Patrick Barrett 7:f9df43829cea 391 coap_adjust_option_deltas(pkt_ptr + nopt_hdr_len + opt_len,
Patrick Barrett 7:f9df43829cea 392 &opts_len, pdu->max - (pkt_ptr - pdu->buf),
Patrick Barrett 7:f9df43829cea 393 lopt_num - opt_num);
Patrick Barrett 7:f9df43829cea 394
Patrick Barrett 7:f9df43829cea 395 // Update Total Packet Length
Patrick Barrett 7:f9df43829cea 396 pdu->len += opts_len - (pdu->len - (pkt_ptr-pdu->buf));
Patrick Barrett 7:f9df43829cea 397 }else{
Patrick Barrett 7:f9df43829cea 398 // Update Packet Length
Patrick Barrett 7:f9df43829cea 399 pdu->len = pdu->len + nopt_hdr_len + opt_len;
Patrick Barrett 7:f9df43829cea 400 }
Patrick Barrett 7:f9df43829cea 401
Patrick Barrett 7:f9df43829cea 402 // Insert the Header
Patrick Barrett 7:f9df43829cea 403 coap_build_option_header(pkt_ptr, nopt_hdr_len, opt_num - lopt_num, opt_len);
Patrick Barrett 7:f9df43829cea 404
Patrick Barrett 7:f9df43829cea 405 // Insert the Value
Patrick Barrett 7:f9df43829cea 406 memcpy(pkt_ptr + nopt_hdr_len, value, opt_len);
Patrick Barrett 7:f9df43829cea 407
Patrick Barrett 7:f9df43829cea 408 return CE_NONE;
Patrick Barrett 7:f9df43829cea 409 }
Patrick Barrett 7:f9df43829cea 410
Patrick Barrett 7:f9df43829cea 411 coap_error coap_set_payload(coap_pdu *pdu, uint8_t *payload, size_t payload_len){
Patrick Barrett 7:f9df43829cea 412 uint8_t *pkt_ptr, *fopt_val;
Patrick Barrett 7:f9df43829cea 413 uint16_t fopt_num;
Patrick Barrett 7:f9df43829cea 414 size_t fopt_len;
Patrick Barrett 7:f9df43829cea 415 coap_error err;
Patrick Barrett 7:f9df43829cea 416
Patrick Barrett 7:f9df43829cea 417 // Set pointer to "zeroth option's value" which is really first option header.
Patrick Barrett 7:f9df43829cea 418 fopt_val = pdu->buf + 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 419 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 420
Patrick Barrett 7:f9df43829cea 421 // Option number delta starts at zero.
Patrick Barrett 7:f9df43829cea 422 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 423
Patrick Barrett 7:f9df43829cea 424 // Find insertion point
Patrick Barrett 7:f9df43829cea 425 do{
Patrick Barrett 7:f9df43829cea 426 pkt_ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 427 err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 428 }while (err == CE_NONE && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
Patrick Barrett 7:f9df43829cea 429
Patrick Barrett 7:f9df43829cea 430 if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
Patrick Barrett 7:f9df43829cea 431 return err;
Patrick Barrett 7:f9df43829cea 432
Patrick Barrett 7:f9df43829cea 433 if (err == CE_END_OF_PACKET){
Patrick Barrett 7:f9df43829cea 434 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 435 if (pdu->max < pdu->len + payload_len + 1)
Patrick Barrett 7:f9df43829cea 436 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 437
Patrick Barrett 7:f9df43829cea 438 *(pkt_ptr++) = 0xFF;
Patrick Barrett 7:f9df43829cea 439 }else if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 440 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 441 if (pdu->max < pdu->len + payload_len)
Patrick Barrett 7:f9df43829cea 442 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 443 }
Patrick Barrett 7:f9df43829cea 444
Patrick Barrett 7:f9df43829cea 445 pdu->len = (pkt_ptr - pdu->buf) + payload_len;
Patrick Barrett 7:f9df43829cea 446 memcpy(pkt_ptr, payload, payload_len);
Patrick Barrett 7:f9df43829cea 447
Patrick Barrett 7:f9df43829cea 448 return CE_NONE;
Patrick Barrett 7:f9df43829cea 449 }
Patrick Barrett 7:f9df43829cea 450
Patrick Barrett 7:f9df43829cea 451 coap_error coap_adjust_option_deltas(uint8_t *opts_start, size_t *opts_len, size_t max_len, int32_t offset)
Patrick Barrett 7:f9df43829cea 452 {
Patrick Barrett 7:f9df43829cea 453 uint8_t *ptr, *fopt_val;
Patrick Barrett 7:f9df43829cea 454 uint16_t fopt_num, nopt_num;
Patrick Barrett 7:f9df43829cea 455 size_t fopt_len;
Patrick Barrett 7:f9df43829cea 456 int8_t nhdr_len, fhdr_len;
Patrick Barrett 7:f9df43829cea 457 coap_error err;
Patrick Barrett 7:f9df43829cea 458
Patrick Barrett 7:f9df43829cea 459 fopt_val = opts_start;
Patrick Barrett 7:f9df43829cea 460 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 461 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 462
Patrick Barrett 7:f9df43829cea 463 do{
Patrick Barrett 7:f9df43829cea 464 ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 465 if (ptr - opts_start > *opts_len)
Patrick Barrett 7:f9df43829cea 466 break;
Patrick Barrett 7:f9df43829cea 467
Patrick Barrett 7:f9df43829cea 468 err = coap_decode_option(ptr, *opts_len-(ptr-opts_start), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 469
Patrick Barrett 7:f9df43829cea 470 if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
Patrick Barrett 7:f9df43829cea 471 break;
Patrick Barrett 7:f9df43829cea 472 else if (err != CE_NONE)
Patrick Barrett 7:f9df43829cea 473 return err;
Patrick Barrett 7:f9df43829cea 474
Patrick Barrett 7:f9df43829cea 475 // New Option Number
Patrick Barrett 7:f9df43829cea 476 nopt_num = fopt_num + offset;
Patrick Barrett 7:f9df43829cea 477
Patrick Barrett 7:f9df43829cea 478 // Find the length of the found header.
Patrick Barrett 7:f9df43829cea 479 fhdr_len = fopt_val - ptr;
Patrick Barrett 7:f9df43829cea 480
Patrick Barrett 7:f9df43829cea 481 // Compute the length of the new header.
Patrick Barrett 7:f9df43829cea 482 nhdr_len = coap_compute_option_header_len(nopt_num, fopt_len);
Patrick Barrett 7:f9df43829cea 483
Patrick Barrett 7:f9df43829cea 484 // Make/Take room for new header size
Patrick Barrett 7:f9df43829cea 485 if (fhdr_len != nhdr_len){
Patrick Barrett 7:f9df43829cea 486 if (max_len < *opts_len + (nhdr_len - fhdr_len))
Patrick Barrett 7:f9df43829cea 487 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 488
Patrick Barrett 7:f9df43829cea 489 memmove(fopt_val + (nhdr_len - fhdr_len), fopt_val, fopt_len);
Patrick Barrett 7:f9df43829cea 490
Patrick Barrett 7:f9df43829cea 491 // Adjust Options Length
Patrick Barrett 7:f9df43829cea 492 *opts_len += (nhdr_len - fhdr_len);
Patrick Barrett 7:f9df43829cea 493 }
Patrick Barrett 7:f9df43829cea 494
Patrick Barrett 7:f9df43829cea 495 // Write New Header
Patrick Barrett 7:f9df43829cea 496 nhdr_len = coap_build_option_header(ptr, nhdr_len, nopt_num, fopt_len);
Patrick Barrett 7:f9df43829cea 497
Patrick Barrett 7:f9df43829cea 498 }while (1);
Patrick Barrett 7:f9df43829cea 499
Patrick Barrett 7:f9df43829cea 500 return CE_NONE;
Patrick Barrett 7:f9df43829cea 501
Patrick Barrett 7:f9df43829cea 502 }
Patrick Barrett 7:f9df43829cea 503
Patrick Barrett 7:f9df43829cea 504 int8_t coap_build_option_header(uint8_t *buf, size_t max_len, int32_t opt_delta, int32_t opt_len)
Patrick Barrett 7:f9df43829cea 505 {
Patrick Barrett 7:f9df43829cea 506 uint8_t *ptr, base_num, base_len;
Patrick Barrett 7:f9df43829cea 507
Patrick Barrett 7:f9df43829cea 508 if (max_len < 1)
Patrick Barrett 7:f9df43829cea 509 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 510
Patrick Barrett 7:f9df43829cea 511 ptr = buf+1;
Patrick Barrett 7:f9df43829cea 512
Patrick Barrett 7:f9df43829cea 513 if (opt_delta < 13) {
Patrick Barrett 7:f9df43829cea 514 base_num = opt_delta;
Patrick Barrett 7:f9df43829cea 515 }else if (opt_delta < 269) {
Patrick Barrett 7:f9df43829cea 516 if (max_len < ptr-buf + 1)
Patrick Barrett 7:f9df43829cea 517 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 518
Patrick Barrett 7:f9df43829cea 519 base_num = 13;
Patrick Barrett 7:f9df43829cea 520 *(ptr++) = opt_delta - 13;
Patrick Barrett 7:f9df43829cea 521 }else {
Patrick Barrett 7:f9df43829cea 522 if (max_len < ptr-buf + 2)
Patrick Barrett 7:f9df43829cea 523 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 524
Patrick Barrett 7:f9df43829cea 525 base_num = 14;
Patrick Barrett 7:f9df43829cea 526 *(ptr++) = (opt_delta - 269) >> 8;
Patrick Barrett 7:f9df43829cea 527 *(ptr++) = (opt_delta - 269) & 0xFF;
Patrick Barrett 7:f9df43829cea 528 }
Patrick Barrett 7:f9df43829cea 529
Patrick Barrett 7:f9df43829cea 530 if (opt_len < 13) {
Patrick Barrett 7:f9df43829cea 531 base_len = opt_len;
Patrick Barrett 7:f9df43829cea 532 }else if (opt_len < 269) {
Patrick Barrett 7:f9df43829cea 533 if (max_len < ptr-buf + 1)
Patrick Barrett 7:f9df43829cea 534 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 535
Patrick Barrett 7:f9df43829cea 536 base_len = 13;
Patrick Barrett 7:f9df43829cea 537 *(ptr++) = opt_len - 13;
Patrick Barrett 7:f9df43829cea 538 }else {
Patrick Barrett 7:f9df43829cea 539 if (max_len < ptr-buf + 2)
Patrick Barrett 7:f9df43829cea 540 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 541
Patrick Barrett 7:f9df43829cea 542 base_len = 14;
Patrick Barrett 7:f9df43829cea 543 *(ptr++) = (opt_len - 269) >> 8;
Patrick Barrett 7:f9df43829cea 544 *(ptr++) = (opt_len - 269) & 0xFF;
Patrick Barrett 7:f9df43829cea 545 }
Patrick Barrett 7:f9df43829cea 546
Patrick Barrett 7:f9df43829cea 547 buf[0] = (base_num << 4) | base_len;
Patrick Barrett 7:f9df43829cea 548
Patrick Barrett 7:f9df43829cea 549
Patrick Barrett 7:f9df43829cea 550 // Return the length of the new header.
Patrick Barrett 7:f9df43829cea 551 return ptr-buf;
Patrick Barrett 7:f9df43829cea 552
Patrick Barrett 7:f9df43829cea 553 }
Patrick Barrett 7:f9df43829cea 554
Patrick Barrett 7:f9df43829cea 555 int8_t coap_compute_option_header_len(int32_t opt_delta, int32_t opt_len)
Patrick Barrett 7:f9df43829cea 556 {
Patrick Barrett 7:f9df43829cea 557 int8_t len = 1;
Patrick Barrett 7:f9df43829cea 558
Patrick Barrett 7:f9df43829cea 559 if (opt_delta < 13) {
Patrick Barrett 7:f9df43829cea 560 }else if (opt_delta < 269) {
Patrick Barrett 7:f9df43829cea 561 len += 1;
Patrick Barrett 7:f9df43829cea 562 }else {
Patrick Barrett 7:f9df43829cea 563 len += 2;
Patrick Barrett 7:f9df43829cea 564 }
Patrick Barrett 7:f9df43829cea 565
Patrick Barrett 7:f9df43829cea 566 if (opt_len < 13) {
Patrick Barrett 7:f9df43829cea 567 }else if (opt_len < 269) {
Patrick Barrett 7:f9df43829cea 568 len += 1;
Patrick Barrett 7:f9df43829cea 569 }else {
Patrick Barrett 7:f9df43829cea 570 len += 2;
Patrick Barrett 7:f9df43829cea 571 }
Patrick Barrett 7:f9df43829cea 572
Patrick Barrett 7:f9df43829cea 573 return len;
Patrick Barrett 7:f9df43829cea 574
Patrick Barrett 7:f9df43829cea 575 }
Patrick Barrett 7:f9df43829cea 576