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@26:3d46024a8c30, 2015-01-07 (annotated)

Committer:
Patrick Barrett
Date:
Wed Jan 07 12:37:30 2015 -0600
Revision:
26:3d46024a8c30
Parent:
22:6a6810897eb9 
picocoap updates (coap_get_option_by_num)



Merge commit '5156fb6753f54f34f0b3bc70825b7c87add66a9b' into cleanup

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 26:3d46024a8c30 135 } else if (option.num > num) {
Patrick Barrett 26:3d46024a8c30 136 option.num = 0;
Patrick Barrett 26:3d46024a8c30 137 option.len = 0;
Patrick Barrett 26:3d46024a8c30 138 option.val = NULL;
Patrick Barrett 26:3d46024a8c30 139 break;
Patrick Barrett 26:3d46024a8c30 140 } else if (option.num == 0) {
Patrick Barrett 26:3d46024a8c30 141 break;
Patrick Barrett 7:f9df43829cea 142 }
Patrick Barrett 26:3d46024a8c30 143 } while (i <= occ);
Patrick Barrett 7:f9df43829cea 144
Patrick Barrett 7:f9df43829cea 145 return option;
Patrick Barrett 7:f9df43829cea 146 }
Patrick Barrett 7:f9df43829cea 147
Patrick Barrett 7:f9df43829cea 148
Patrick Barrett 7:f9df43829cea 149 //
Patrick Barrett 7:f9df43829cea 150 // Decoding Functions (Intended for Internal Use)
Patrick Barrett 7:f9df43829cea 151 //
Patrick Barrett 7:f9df43829cea 152
Patrick Barrett 7:f9df43829cea 153 coap_error coap_decode_option(uint8_t *pkt_ptr, size_t pkt_len,
Patrick Barrett 7:f9df43829cea 154 uint16_t *option_number, size_t *option_length, uint8_t **value)
Patrick Barrett 7:f9df43829cea 155 {
Patrick Barrett 7:f9df43829cea 156 uint8_t *ptr = pkt_ptr;
Patrick Barrett 7:f9df43829cea 157 uint16_t delta, length;
Patrick Barrett 7:f9df43829cea 158
Patrick Barrett 7:f9df43829cea 159 // Check for end of Packet
Patrick Barrett 7:f9df43829cea 160 if (pkt_len == 0){
Patrick Barrett 7:f9df43829cea 161 return CE_END_OF_PACKET;
Patrick Barrett 7:f9df43829cea 162 }
Patrick Barrett 7:f9df43829cea 163
Patrick Barrett 7:f9df43829cea 164 // Check for Payload Marker
Patrick Barrett 7:f9df43829cea 165 if (*ptr == 0xFF){
Patrick Barrett 7:f9df43829cea 166 return CE_FOUND_PAYLOAD_MARKER;
Patrick Barrett 7:f9df43829cea 167 }
Patrick Barrett 7:f9df43829cea 168
Patrick Barrett 7:f9df43829cea 169 // Get Base Delta and Length
Patrick Barrett 7:f9df43829cea 170 delta = *ptr >> 4;
Patrick Barrett 7:f9df43829cea 171 length = *ptr & 0x0F;
Patrick Barrett 7:f9df43829cea 172 ptr++;
Patrick Barrett 7:f9df43829cea 173
Patrick Barrett 7:f9df43829cea 174 // Check for and Get Extended Delta
Patrick Barrett 7:f9df43829cea 175 if (delta < 13) {
Patrick Barrett 7:f9df43829cea 176 //delta = delta;
Patrick Barrett 7:f9df43829cea 177 }else if (delta == 13) {
Patrick Barrett 7:f9df43829cea 178 delta = *ptr + 13;
Patrick Barrett 7:f9df43829cea 179 ptr += 1;
Patrick Barrett 7:f9df43829cea 180 }else if (delta == 14) {
Patrick Barrett 7:f9df43829cea 181 delta = (*ptr << 8) + *(ptr+1) + 269;
Patrick Barrett 7:f9df43829cea 182 ptr += 2;
Patrick Barrett 7:f9df43829cea 183 }else{
Patrick Barrett 7:f9df43829cea 184 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 185 }
Patrick Barrett 7:f9df43829cea 186
Patrick Barrett 7:f9df43829cea 187 // Check for and Get Extended Length
Patrick Barrett 7:f9df43829cea 188 if (length < 13) {
Patrick Barrett 7:f9df43829cea 189 //length = length;
Patrick Barrett 7:f9df43829cea 190 }else if (length == 13) {
Patrick Barrett 7:f9df43829cea 191 length = *ptr + 13;
Patrick Barrett 7:f9df43829cea 192 ptr += 1;
Patrick Barrett 7:f9df43829cea 193 }else if (length == 14) {
Patrick Barrett 7:f9df43829cea 194 length = (*ptr << 8) + *(ptr+1) + 269;
Patrick Barrett 7:f9df43829cea 195 ptr += 2;
Patrick Barrett 7:f9df43829cea 196 }else{
Patrick Barrett 7:f9df43829cea 197 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 198 }
Patrick Barrett 7:f9df43829cea 199
Patrick Barrett 7:f9df43829cea 200 if (option_number != NULL)
Patrick Barrett 7:f9df43829cea 201 *option_number += delta;
Patrick Barrett 7:f9df43829cea 202
Patrick Barrett 7:f9df43829cea 203 if (option_length != NULL)
Patrick Barrett 7:f9df43829cea 204 *option_length = length;
Patrick Barrett 7:f9df43829cea 205
Patrick Barrett 7:f9df43829cea 206 if (value != NULL)
Patrick Barrett 7:f9df43829cea 207 *value = ptr;
Patrick Barrett 7:f9df43829cea 208
Patrick Barrett 7:f9df43829cea 209 return CE_NONE;
Patrick Barrett 7:f9df43829cea 210 }
Patrick Barrett 7:f9df43829cea 211
Patrick Barrett 7:f9df43829cea 212
Patrick Barrett 7:f9df43829cea 213 coap_payload coap_get_payload(coap_pdu *pdu)
Patrick Barrett 7:f9df43829cea 214 {
Patrick Barrett 7:f9df43829cea 215
Patrick Barrett 7:f9df43829cea 216 size_t offset = 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 217 coap_option option;
Patrick Barrett 7:f9df43829cea 218 coap_payload payload;
Patrick Barrett 7:f9df43829cea 219 coap_error err;
Patrick Barrett 7:f9df43829cea 220
Patrick Barrett 7:f9df43829cea 221 // Defaults
Patrick Barrett 7:f9df43829cea 222 payload.len = 0;
Patrick Barrett 7:f9df43829cea 223 payload.val = NULL;
Patrick Barrett 7:f9df43829cea 224
Patrick Barrett 7:f9df43829cea 225 // Find Last Option
Patrick Barrett 7:f9df43829cea 226 do {
Patrick Barrett 7:f9df43829cea 227 err = coap_decode_option(pdu->buf+offset, pdu->len-offset, NULL, &option.len, &option.val);
Patrick Barrett 7:f9df43829cea 228 if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
Patrick Barrett 7:f9df43829cea 229 break;
Patrick Barrett 7:f9df43829cea 230
Patrick Barrett 7:f9df43829cea 231 if (err != CE_NONE)
Patrick Barrett 7:f9df43829cea 232 return payload;
Patrick Barrett 7:f9df43829cea 233
Patrick Barrett 7:f9df43829cea 234 // Add this option header and value length to offset.
Patrick Barrett 7:f9df43829cea 235 offset += (option.val - (pdu->buf+offset)) + option.len;
Patrick Barrett 7:f9df43829cea 236 } while (1);
Patrick Barrett 7:f9df43829cea 237
Patrick Barrett 7:f9df43829cea 238 if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 239 payload.len = pdu->len - offset - 1;
Patrick Barrett 7:f9df43829cea 240 payload.val = pdu->buf + offset + 1;
Patrick Barrett 7:f9df43829cea 241 }
Patrick Barrett 7:f9df43829cea 242
Patrick Barrett 7:f9df43829cea 243 return payload;
Patrick Barrett 7:f9df43829cea 244 }
Patrick Barrett 7:f9df43829cea 245
Patrick Barrett 7:f9df43829cea 246
Patrick Barrett 7:f9df43829cea 247 //
Patrick Barrett 7:f9df43829cea 248 // Setters
Patrick Barrett 7:f9df43829cea 249 //
Patrick Barrett 7:f9df43829cea 250
Patrick Barrett 7:f9df43829cea 251 coap_error coap_init_pdu(coap_pdu *pdu)
Patrick Barrett 7:f9df43829cea 252 {
Patrick Barrett 7:f9df43829cea 253 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 254 if (pdu->max < 4)
Patrick Barrett 7:f9df43829cea 255 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 256
Patrick Barrett 22:6a6810897eb9 257 pdu->len = 0;
Patrick Barrett 22:6a6810897eb9 258 memset(pdu->buf, 0, 4);
Patrick Barrett 22:6a6810897eb9 259
Patrick Barrett 7:f9df43829cea 260 coap_set_version(pdu, COAP_V1);
Patrick Barrett 7:f9df43829cea 261 coap_set_type(pdu, CT_RST);
Patrick Barrett 7:f9df43829cea 262 coap_set_token(pdu, 0, 0);
Patrick Barrett 7:f9df43829cea 263 coap_set_code(pdu, CC_EMPTY);
Patrick Barrett 7:f9df43829cea 264 coap_set_mid(pdu, 0);
Patrick Barrett 7:f9df43829cea 265
Patrick Barrett 7:f9df43829cea 266 pdu->opt_ptr = NULL;
Patrick Barrett 7:f9df43829cea 267
Patrick Barrett 7:f9df43829cea 268 return CE_NONE;
Patrick Barrett 7:f9df43829cea 269 }
Patrick Barrett 7:f9df43829cea 270
Patrick Barrett 7:f9df43829cea 271 coap_error coap_set_version(coap_pdu *pdu, coap_version ver)
Patrick Barrett 7:f9df43829cea 272 {
Patrick Barrett 7:f9df43829cea 273 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 274 if (pdu->max < 1)
Patrick Barrett 7:f9df43829cea 275 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 276
Patrick Barrett 7:f9df43829cea 277 pdu->buf[0] = (ver << 6) | (pdu->buf[0] & 0x3F);
Patrick Barrett 7:f9df43829cea 278
Patrick Barrett 7:f9df43829cea 279 if (pdu->len < 1)
Patrick Barrett 7:f9df43829cea 280 pdu->len = 1;
Patrick Barrett 7:f9df43829cea 281
Patrick Barrett 7:f9df43829cea 282 return CE_NONE;
Patrick Barrett 7:f9df43829cea 283 }
Patrick Barrett 7:f9df43829cea 284
Patrick Barrett 7:f9df43829cea 285 coap_error coap_set_type(coap_pdu *pdu, coap_type mtype)
Patrick Barrett 7:f9df43829cea 286 {
Patrick Barrett 7:f9df43829cea 287 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 288 if (pdu->max < 1)
Patrick Barrett 7:f9df43829cea 289 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 290
Patrick Barrett 7:f9df43829cea 291 pdu->buf[0] = (mtype << 4) | (pdu->buf[0] & 0xCF);
Patrick Barrett 7:f9df43829cea 292
Patrick Barrett 7:f9df43829cea 293 if (pdu->len < 1)
Patrick Barrett 7:f9df43829cea 294 pdu->len = 1;
Patrick Barrett 7:f9df43829cea 295
Patrick Barrett 7:f9df43829cea 296 return CE_NONE;
Patrick Barrett 7:f9df43829cea 297 }
Patrick Barrett 7:f9df43829cea 298
Patrick Barrett 7:f9df43829cea 299 coap_error coap_set_code(coap_pdu *pdu, coap_code code)
Patrick Barrett 7:f9df43829cea 300 {
Patrick Barrett 7:f9df43829cea 301 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 302 if (pdu->max < 2)
Patrick Barrett 7:f9df43829cea 303 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 304
Patrick Barrett 7:f9df43829cea 305 pdu->buf[1] = code;
Patrick Barrett 7:f9df43829cea 306
Patrick Barrett 7:f9df43829cea 307 if (pdu->len < 2)
Patrick Barrett 7:f9df43829cea 308 pdu->len = 2;
Patrick Barrett 7:f9df43829cea 309
Patrick Barrett 7:f9df43829cea 310 return CE_NONE;
Patrick Barrett 7:f9df43829cea 311 }
Patrick Barrett 7:f9df43829cea 312
Patrick Barrett 7:f9df43829cea 313 coap_error coap_set_mid(coap_pdu *pdu, uint16_t mid)
Patrick Barrett 7:f9df43829cea 314 {
Patrick Barrett 7:f9df43829cea 315 // Check that we were given enough packet.
Patrick Barrett 7:f9df43829cea 316 if (pdu->max < 4)
Patrick Barrett 7:f9df43829cea 317 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 318
Patrick Barrett 7:f9df43829cea 319 pdu->buf[2] = mid >> 8;
Patrick Barrett 7:f9df43829cea 320 pdu->buf[3] = mid & 0xFF;
Patrick Barrett 7:f9df43829cea 321
Patrick Barrett 7:f9df43829cea 322 if (pdu->len < 4)
Patrick Barrett 7:f9df43829cea 323 pdu->len = 4;
Patrick Barrett 7:f9df43829cea 324
Patrick Barrett 7:f9df43829cea 325 return CE_NONE;
Patrick Barrett 7:f9df43829cea 326 }
Patrick Barrett 7:f9df43829cea 327
Patrick Barrett 7:f9df43829cea 328 coap_error coap_set_token(coap_pdu *pdu, uint64_t token, uint8_t tkl)
Patrick Barrett 7:f9df43829cea 329 {
Patrick Barrett 7:f9df43829cea 330 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 331 if (pdu->max < 4 + tkl)
Patrick Barrett 7:f9df43829cea 332 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 333
Patrick Barrett 7:f9df43829cea 334 // Check token length for spec.
Patrick Barrett 7:f9df43829cea 335 if (tkl > 8)
Patrick Barrett 7:f9df43829cea 336 return CE_INVALID_PACKET;
Patrick Barrett 7:f9df43829cea 337
Patrick Barrett 7:f9df43829cea 338 // Check if we may need to make or take room.
Patrick Barrett 7:f9df43829cea 339 if (pdu->len > 4){
Patrick Barrett 7:f9df43829cea 340 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 341 if (pdu->max < pdu->len + (tkl - coap_get_tkl(pdu)))
Patrick Barrett 7:f9df43829cea 342 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 343
Patrick Barrett 7:f9df43829cea 344 // Move rest of packet to make room or take empty space.
Patrick Barrett 7:f9df43829cea 345 memmove(pdu->buf + 4 + tkl, pdu->buf + 4 + coap_get_tkl(pdu), pdu->len - 4 - coap_get_tkl(pdu));
Patrick Barrett 7:f9df43829cea 346 }
Patrick Barrett 7:f9df43829cea 347
Patrick Barrett 7:f9df43829cea 348 // Set token.
Patrick Barrett 7:f9df43829cea 349 memcpy(pdu->buf+4, &token, tkl);
Patrick Barrett 7:f9df43829cea 350
Patrick Barrett 7:f9df43829cea 351 pdu->len += tkl - coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 352
Patrick Barrett 7:f9df43829cea 353 pdu->buf[0] = (tkl & 0x0F) | (pdu->buf[0] & 0xF0);
Patrick Barrett 7:f9df43829cea 354
Patrick Barrett 7:f9df43829cea 355 return CE_NONE;
Patrick Barrett 7:f9df43829cea 356 }
Patrick Barrett 7:f9df43829cea 357
Patrick Barrett 7:f9df43829cea 358 coap_error coap_add_option(coap_pdu *pdu, int32_t opt_num, uint8_t* value, uint16_t opt_len)
Patrick Barrett 7:f9df43829cea 359 {
Patrick Barrett 7:f9df43829cea 360 uint8_t *pkt_ptr, *fopt_val, nopt_hdr_len;
Patrick Barrett 7:f9df43829cea 361 uint16_t fopt_num, lopt_num;
Patrick Barrett 7:f9df43829cea 362 size_t fopt_len, opts_len;
Patrick Barrett 7:f9df43829cea 363 coap_error err;
Patrick Barrett 7:f9df43829cea 364
Patrick Barrett 7:f9df43829cea 365 // Set pointer to "zeroth option's value" which is really first option header.
Patrick Barrett 7:f9df43829cea 366 fopt_val = pdu->buf + 4 + coap_get_tkl(pdu); // ptr to start of options
Patrick Barrett 7:f9df43829cea 367 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 368
Patrick Barrett 7:f9df43829cea 369 // Option number delta starts at zero.
Patrick Barrett 7:f9df43829cea 370 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 371
Patrick Barrett 7:f9df43829cea 372 // Find insertion point
Patrick Barrett 7:f9df43829cea 373 do{
Patrick Barrett 7:f9df43829cea 374 pkt_ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 375 lopt_num = fopt_num;
Patrick Barrett 7:f9df43829cea 376 err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 377 }while (err == CE_NONE && fopt_num <= opt_num && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
Patrick Barrett 7:f9df43829cea 378
Patrick Barrett 7:f9df43829cea 379 if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
Patrick Barrett 7:f9df43829cea 380 return err;
Patrick Barrett 7:f9df43829cea 381
Patrick Barrett 7:f9df43829cea 382 // Build New Header
Patrick Barrett 7:f9df43829cea 383 nopt_hdr_len = coap_compute_option_header_len(opt_num - lopt_num, opt_len);
Patrick Barrett 7:f9df43829cea 384
Patrick Barrett 7:f9df43829cea 385 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 386 if (pdu->max < pdu->len + nopt_hdr_len + opt_len)
Patrick Barrett 7:f9df43829cea 387 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 388
Patrick Barrett 7:f9df43829cea 389 // Check if we're adding an option in the middle of a packet.
Patrick Barrett 7:f9df43829cea 390 // But seriously, don't do this.
Patrick Barrett 7:f9df43829cea 391 if (pdu->len != pkt_ptr- pdu->buf){
Patrick Barrett 7:f9df43829cea 392 // Slide packet tail to make room.
Patrick Barrett 7:f9df43829cea 393 memmove(pkt_ptr + nopt_hdr_len + opt_len, pkt_ptr, pdu->len - (pkt_ptr - pdu->buf));
Patrick Barrett 7:f9df43829cea 394 pdu->len += nopt_hdr_len + opt_len;
Patrick Barrett 7:f9df43829cea 395
Patrick Barrett 7:f9df43829cea 396 // Find Current Length of Remaining Options
Patrick Barrett 7:f9df43829cea 397 opts_len = pdu->len - (pkt_ptr-pdu->buf);
Patrick Barrett 7:f9df43829cea 398
Patrick Barrett 7:f9df43829cea 399 // Adjust the option deltas for the rest of the options.
Patrick Barrett 7:f9df43829cea 400 coap_adjust_option_deltas(pkt_ptr + nopt_hdr_len + opt_len,
Patrick Barrett 7:f9df43829cea 401 &opts_len, pdu->max - (pkt_ptr - pdu->buf),
Patrick Barrett 7:f9df43829cea 402 lopt_num - opt_num);
Patrick Barrett 7:f9df43829cea 403
Patrick Barrett 7:f9df43829cea 404 // Update Total Packet Length
Patrick Barrett 7:f9df43829cea 405 pdu->len += opts_len - (pdu->len - (pkt_ptr-pdu->buf));
Patrick Barrett 7:f9df43829cea 406 }else{
Patrick Barrett 7:f9df43829cea 407 // Update Packet Length
Patrick Barrett 7:f9df43829cea 408 pdu->len = pdu->len + nopt_hdr_len + opt_len;
Patrick Barrett 7:f9df43829cea 409 }
Patrick Barrett 7:f9df43829cea 410
Patrick Barrett 7:f9df43829cea 411 // Insert the Header
Patrick Barrett 7:f9df43829cea 412 coap_build_option_header(pkt_ptr, nopt_hdr_len, opt_num - lopt_num, opt_len);
Patrick Barrett 7:f9df43829cea 413
Patrick Barrett 7:f9df43829cea 414 // Insert the Value
Patrick Barrett 7:f9df43829cea 415 memcpy(pkt_ptr + nopt_hdr_len, value, opt_len);
Patrick Barrett 7:f9df43829cea 416
Patrick Barrett 7:f9df43829cea 417 return CE_NONE;
Patrick Barrett 7:f9df43829cea 418 }
Patrick Barrett 7:f9df43829cea 419
Patrick Barrett 7:f9df43829cea 420 coap_error coap_set_payload(coap_pdu *pdu, uint8_t *payload, size_t payload_len){
Patrick Barrett 7:f9df43829cea 421 uint8_t *pkt_ptr, *fopt_val;
Patrick Barrett 7:f9df43829cea 422 uint16_t fopt_num;
Patrick Barrett 7:f9df43829cea 423 size_t fopt_len;
Patrick Barrett 7:f9df43829cea 424 coap_error err;
Patrick Barrett 7:f9df43829cea 425
Patrick Barrett 7:f9df43829cea 426 // Set pointer to "zeroth option's value" which is really first option header.
Patrick Barrett 7:f9df43829cea 427 fopt_val = pdu->buf + 4 + coap_get_tkl(pdu);
Patrick Barrett 7:f9df43829cea 428 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 429
Patrick Barrett 7:f9df43829cea 430 // Option number delta starts at zero.
Patrick Barrett 7:f9df43829cea 431 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 432
Patrick Barrett 7:f9df43829cea 433 // Find insertion point
Patrick Barrett 7:f9df43829cea 434 do{
Patrick Barrett 7:f9df43829cea 435 pkt_ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 436 err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 437 }while (err == CE_NONE && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
Patrick Barrett 7:f9df43829cea 438
Patrick Barrett 7:f9df43829cea 439 if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
Patrick Barrett 7:f9df43829cea 440 return err;
Patrick Barrett 7:f9df43829cea 441
Patrick Barrett 7:f9df43829cea 442 if (err == CE_END_OF_PACKET){
Patrick Barrett 7:f9df43829cea 443 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 444 if (pdu->max < pdu->len + payload_len + 1)
Patrick Barrett 7:f9df43829cea 445 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 446
Patrick Barrett 7:f9df43829cea 447 *(pkt_ptr++) = 0xFF;
Patrick Barrett 7:f9df43829cea 448 }else if (err == CE_FOUND_PAYLOAD_MARKER){
Patrick Barrett 7:f9df43829cea 449 // Check that we were given enough buffer.
Patrick Barrett 7:f9df43829cea 450 if (pdu->max < pdu->len + payload_len)
Patrick Barrett 7:f9df43829cea 451 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 452 }
Patrick Barrett 7:f9df43829cea 453
Patrick Barrett 7:f9df43829cea 454 pdu->len = (pkt_ptr - pdu->buf) + payload_len;
Patrick Barrett 7:f9df43829cea 455 memcpy(pkt_ptr, payload, payload_len);
Patrick Barrett 7:f9df43829cea 456
Patrick Barrett 7:f9df43829cea 457 return CE_NONE;
Patrick Barrett 7:f9df43829cea 458 }
Patrick Barrett 7:f9df43829cea 459
Patrick Barrett 7:f9df43829cea 460 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 461 {
Patrick Barrett 7:f9df43829cea 462 uint8_t *ptr, *fopt_val;
Patrick Barrett 7:f9df43829cea 463 uint16_t fopt_num, nopt_num;
Patrick Barrett 7:f9df43829cea 464 size_t fopt_len;
Patrick Barrett 7:f9df43829cea 465 int8_t nhdr_len, fhdr_len;
Patrick Barrett 7:f9df43829cea 466 coap_error err;
Patrick Barrett 7:f9df43829cea 467
Patrick Barrett 7:f9df43829cea 468 fopt_val = opts_start;
Patrick Barrett 7:f9df43829cea 469 fopt_len = 0;
Patrick Barrett 7:f9df43829cea 470 fopt_num = 0;
Patrick Barrett 7:f9df43829cea 471
Patrick Barrett 7:f9df43829cea 472 do{
Patrick Barrett 7:f9df43829cea 473 ptr = fopt_val + fopt_len;
Patrick Barrett 7:f9df43829cea 474 if (ptr - opts_start > *opts_len)
Patrick Barrett 7:f9df43829cea 475 break;
Patrick Barrett 7:f9df43829cea 476
Patrick Barrett 7:f9df43829cea 477 err = coap_decode_option(ptr, *opts_len-(ptr-opts_start), &fopt_num, &fopt_len, &fopt_val);
Patrick Barrett 7:f9df43829cea 478
Patrick Barrett 7:f9df43829cea 479 if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
Patrick Barrett 7:f9df43829cea 480 break;
Patrick Barrett 7:f9df43829cea 481 else if (err != CE_NONE)
Patrick Barrett 7:f9df43829cea 482 return err;
Patrick Barrett 7:f9df43829cea 483
Patrick Barrett 7:f9df43829cea 484 // New Option Number
Patrick Barrett 7:f9df43829cea 485 nopt_num = fopt_num + offset;
Patrick Barrett 7:f9df43829cea 486
Patrick Barrett 7:f9df43829cea 487 // Find the length of the found header.
Patrick Barrett 7:f9df43829cea 488 fhdr_len = fopt_val - ptr;
Patrick Barrett 7:f9df43829cea 489
Patrick Barrett 7:f9df43829cea 490 // Compute the length of the new header.
Patrick Barrett 7:f9df43829cea 491 nhdr_len = coap_compute_option_header_len(nopt_num, fopt_len);
Patrick Barrett 7:f9df43829cea 492
Patrick Barrett 7:f9df43829cea 493 // Make/Take room for new header size
Patrick Barrett 7:f9df43829cea 494 if (fhdr_len != nhdr_len){
Patrick Barrett 7:f9df43829cea 495 if (max_len < *opts_len + (nhdr_len - fhdr_len))
Patrick Barrett 7:f9df43829cea 496 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 497
Patrick Barrett 7:f9df43829cea 498 memmove(fopt_val + (nhdr_len - fhdr_len), fopt_val, fopt_len);
Patrick Barrett 7:f9df43829cea 499
Patrick Barrett 7:f9df43829cea 500 // Adjust Options Length
Patrick Barrett 7:f9df43829cea 501 *opts_len += (nhdr_len - fhdr_len);
Patrick Barrett 7:f9df43829cea 502 }
Patrick Barrett 7:f9df43829cea 503
Patrick Barrett 7:f9df43829cea 504 // Write New Header
Patrick Barrett 7:f9df43829cea 505 nhdr_len = coap_build_option_header(ptr, nhdr_len, nopt_num, fopt_len);
Patrick Barrett 7:f9df43829cea 506
Patrick Barrett 7:f9df43829cea 507 }while (1);
Patrick Barrett 7:f9df43829cea 508
Patrick Barrett 7:f9df43829cea 509 return CE_NONE;
Patrick Barrett 7:f9df43829cea 510
Patrick Barrett 7:f9df43829cea 511 }
Patrick Barrett 7:f9df43829cea 512
Patrick Barrett 7:f9df43829cea 513 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 514 {
Patrick Barrett 7:f9df43829cea 515 uint8_t *ptr, base_num, base_len;
Patrick Barrett 7:f9df43829cea 516
Patrick Barrett 7:f9df43829cea 517 if (max_len < 1)
Patrick Barrett 7:f9df43829cea 518 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 519
Patrick Barrett 7:f9df43829cea 520 ptr = buf+1;
Patrick Barrett 7:f9df43829cea 521
Patrick Barrett 7:f9df43829cea 522 if (opt_delta < 13) {
Patrick Barrett 7:f9df43829cea 523 base_num = opt_delta;
Patrick Barrett 7:f9df43829cea 524 }else if (opt_delta < 269) {
Patrick Barrett 7:f9df43829cea 525 if (max_len < ptr-buf + 1)
Patrick Barrett 7:f9df43829cea 526 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 527
Patrick Barrett 7:f9df43829cea 528 base_num = 13;
Patrick Barrett 7:f9df43829cea 529 *(ptr++) = opt_delta - 13;
Patrick Barrett 7:f9df43829cea 530 }else {
Patrick Barrett 7:f9df43829cea 531 if (max_len < ptr-buf + 2)
Patrick Barrett 7:f9df43829cea 532 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 533
Patrick Barrett 7:f9df43829cea 534 base_num = 14;
Patrick Barrett 7:f9df43829cea 535 *(ptr++) = (opt_delta - 269) >> 8;
Patrick Barrett 7:f9df43829cea 536 *(ptr++) = (opt_delta - 269) & 0xFF;
Patrick Barrett 7:f9df43829cea 537 }
Patrick Barrett 7:f9df43829cea 538
Patrick Barrett 7:f9df43829cea 539 if (opt_len < 13) {
Patrick Barrett 7:f9df43829cea 540 base_len = opt_len;
Patrick Barrett 7:f9df43829cea 541 }else if (opt_len < 269) {
Patrick Barrett 7:f9df43829cea 542 if (max_len < ptr-buf + 1)
Patrick Barrett 7:f9df43829cea 543 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 544
Patrick Barrett 7:f9df43829cea 545 base_len = 13;
Patrick Barrett 7:f9df43829cea 546 *(ptr++) = opt_len - 13;
Patrick Barrett 7:f9df43829cea 547 }else {
Patrick Barrett 7:f9df43829cea 548 if (max_len < ptr-buf + 2)
Patrick Barrett 7:f9df43829cea 549 return CE_INSUFFICIENT_BUFFER;
Patrick Barrett 7:f9df43829cea 550
Patrick Barrett 7:f9df43829cea 551 base_len = 14;
Patrick Barrett 7:f9df43829cea 552 *(ptr++) = (opt_len - 269) >> 8;
Patrick Barrett 7:f9df43829cea 553 *(ptr++) = (opt_len - 269) & 0xFF;
Patrick Barrett 7:f9df43829cea 554 }
Patrick Barrett 7:f9df43829cea 555
Patrick Barrett 7:f9df43829cea 556 buf[0] = (base_num << 4) | base_len;
Patrick Barrett 7:f9df43829cea 557
Patrick Barrett 7:f9df43829cea 558
Patrick Barrett 7:f9df43829cea 559 // Return the length of the new header.
Patrick Barrett 7:f9df43829cea 560 return ptr-buf;
Patrick Barrett 7:f9df43829cea 561
Patrick Barrett 7:f9df43829cea 562 }
Patrick Barrett 7:f9df43829cea 563
Patrick Barrett 7:f9df43829cea 564 int8_t coap_compute_option_header_len(int32_t opt_delta, int32_t opt_len)
Patrick Barrett 7:f9df43829cea 565 {
Patrick Barrett 7:f9df43829cea 566 int8_t len = 1;
Patrick Barrett 7:f9df43829cea 567
Patrick Barrett 7:f9df43829cea 568 if (opt_delta < 13) {
Patrick Barrett 7:f9df43829cea 569 }else if (opt_delta < 269) {
Patrick Barrett 7:f9df43829cea 570 len += 1;
Patrick Barrett 7:f9df43829cea 571 }else {
Patrick Barrett 7:f9df43829cea 572 len += 2;
Patrick Barrett 7:f9df43829cea 573 }
Patrick Barrett 7:f9df43829cea 574
Patrick Barrett 7:f9df43829cea 575 if (opt_len < 13) {
Patrick Barrett 7:f9df43829cea 576 }else if (opt_len < 269) {
Patrick Barrett 7:f9df43829cea 577 len += 1;
Patrick Barrett 7:f9df43829cea 578 }else {
Patrick Barrett 7:f9df43829cea 579 len += 2;
Patrick Barrett 7:f9df43829cea 580 }
Patrick Barrett 7:f9df43829cea 581
Patrick Barrett 7:f9df43829cea 582 return len;
Patrick Barrett 7:f9df43829cea 583
Patrick Barrett 7:f9df43829cea 584 }
Patrick Barrett 7:f9df43829cea 585