coap.c

00001 
00002 #include <stddef.h>
00003 #include <string.h>
00004 #include "coap.h"
00005 
00006 
00007 //
00008 // Getters
00009 //
00010 
00011 coap_error coap_validate_pkt(coap_pdu *pdu) //uint8_t *pkt, size_t pkt_len)
00012 {
00013     coap_error err;
00014     size_t ol;
00015     uint8_t *ov;
00016 
00017     pdu->opt_ptr = NULL;
00018 
00019     if (pdu->len > pdu->max)
00020         return CE_INVALID_PACKET;
00021 
00022     if (pdu->len < 4)
00023         return CE_INVALID_PACKET;
00024 
00025     // Check Version
00026     if (coap_get_version(pdu) != 1)
00027         return CE_INVALID_PACKET;
00028 
00029     // Check TKL
00030     if (coap_get_tkl(pdu) > 8)
00031         return CE_INVALID_PACKET;
00032 
00033     // Check Options
00034     ov = pdu->buf + 4 + coap_get_tkl(pdu);
00035     ol = 0;
00036     while((err = coap_decode_option(ov + ol, pdu->len-(ov-pdu->buf), NULL, &ol, &ov)) != 0){
00037         if (err == CE_NONE){
00038             continue;
00039         } else if (err == CE_END_OF_PACKET){
00040             break;
00041         } else if (err == CE_FOUND_PAYLOAD_MARKER){
00042             // Payload Marker, but No Payload
00043             if (pdu->len == (ov + ol - pdu->buf)){
00044                 return CE_INVALID_PACKET;
00045             } else {
00046                 break;
00047             }
00048         } else {
00049             return err;
00050         }
00051     }
00052 
00053     return CE_NONE;
00054 }
00055 
00056 uint64_t coap_get_token(coap_pdu *pdu)
00057 {
00058     uint8_t tkl;
00059     uint64_t token = 0;
00060 
00061     // Extract TKL.
00062     tkl = pdu->buf[0] & 0x0F;
00063 
00064     // Check that we were given enough packet.
00065     if (pdu->len < 4 + tkl)
00066         return 0;
00067 
00068     // Set token.
00069     memcpy(&token, &pdu->buf[4], tkl);
00070 
00071     return token;
00072 }
00073 
00074 coap_option coap_get_option(coap_pdu *pdu, coap_option *last)
00075 {
00076     uint8_t *opt_ptr;
00077     coap_option option;
00078     coap_error err;
00079 
00080     if (last != NULL && last->num != 0){
00081         option.num = last->num;
00082         option.len = 0;
00083         option.val = NULL;
00084 
00085         opt_ptr = last->val + last->len;
00086     } else {
00087         option.num = 0;
00088         option.len = 0;
00089         option.val = NULL;
00090 
00091         opt_ptr = pdu->buf + 4 + coap_get_tkl(pdu);
00092     }
00093 
00094     // If opt_ptr is outside pkt range, put it at first opt.
00095     if (opt_ptr > (pdu->buf + pdu->len) || opt_ptr <= pdu->buf){
00096         opt_ptr = pdu->buf + 4 + coap_get_tkl(pdu);
00097     }
00098 
00099     err = coap_decode_option(opt_ptr, pdu->len-(opt_ptr-pdu->buf), &option.num, &option.len, &option.val);
00100 
00101     if (err != CE_NONE){
00102         if (err == CE_FOUND_PAYLOAD_MARKER){
00103             if (option.num == 0){
00104                 option.val = opt_ptr + 1;
00105                 option.len = pdu->len-(opt_ptr-pdu->buf) - 1;
00106             } else {
00107                 option.val = option.val + option.len;
00108                 option.len = pdu->len - (option.val - pdu->buf);
00109             }
00110         } else {
00111             option.val = NULL;
00112             option.len = 0;
00113         }
00114         option.num = 0;
00115     }
00116 
00117     opt_ptr = option.val + option.len;
00118 
00119     return option;
00120 }
00121 
00122 
00123 coap_option coap_get_option_by_num(coap_pdu *pdu, coap_option_number num, uint8_t occ)
00124 {
00125     coap_option option;
00126     uint8_t i = 0;
00127 
00128     option.num = 0;
00129 
00130     do {
00131         option = coap_get_option(pdu, &option);
00132 
00133         if (option.num == num) {
00134             i++;
00135         } else if (option.num > num) {
00136             option.num = 0;
00137             option.len = 0;
00138             option.val = NULL;
00139             break;
00140         } else if (option.num == 0) {
00141             break;
00142         }
00143     } while (i <= occ);
00144 
00145     return option;
00146 }
00147 
00148 
00149 //
00150 // Decoding Functions (Intended for Internal Use)
00151 //
00152 
00153 coap_error coap_decode_option(uint8_t *pkt_ptr, size_t pkt_len,
00154                                uint16_t *option_number, size_t *option_length, uint8_t **value)
00155 {
00156     uint8_t *ptr = pkt_ptr;
00157     uint16_t delta, length;
00158 
00159     // Check for end of Packet
00160     if (pkt_len == 0){
00161         return CE_END_OF_PACKET;
00162     }
00163 
00164     // Check for Payload Marker
00165     if (*ptr == 0xFF){
00166         return CE_FOUND_PAYLOAD_MARKER;
00167     }
00168 
00169     // Get Base Delta and Length
00170     delta = *ptr >> 4;
00171     length = *ptr & 0x0F;
00172     ptr++;
00173 
00174     // Check for and Get Extended Delta
00175     if (delta < 13) {
00176         //delta = delta;
00177     }else if (delta == 13) {
00178         delta = *ptr + 13;
00179         ptr += 1;
00180     }else if (delta == 14) {
00181         delta = (*ptr << 8) + *(ptr+1) + 269;
00182         ptr += 2;
00183     }else{
00184         return CE_INVALID_PACKET;
00185     }
00186 
00187     // Check for and Get Extended Length
00188     if (length < 13) {
00189         //length = length;
00190     }else if (length == 13) {
00191         length = *ptr + 13;
00192         ptr += 1;
00193     }else if (length == 14) {
00194         length = (*ptr << 8) + *(ptr+1) + 269;
00195         ptr += 2;
00196     }else{
00197         return CE_INVALID_PACKET;
00198     }
00199 
00200     if (option_number != NULL)
00201         *option_number += delta;
00202 
00203     if (option_length != NULL)
00204         *option_length = length;
00205 
00206     if (value != NULL)
00207         *value = ptr;
00208 
00209     return CE_NONE;
00210 }
00211 
00212 
00213 coap_payload coap_get_payload(coap_pdu *pdu)
00214 {
00215     
00216     size_t offset = 4 + coap_get_tkl(pdu);
00217     coap_option option;
00218     coap_payload payload;
00219     coap_error err;
00220 
00221     // Defaults
00222     payload.len = 0;
00223     payload.val = NULL;
00224 
00225     // Find Last Option
00226     do {
00227         err = coap_decode_option(pdu->buf+offset, pdu->len-offset, NULL, &option.len, &option.val);
00228         if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
00229             break;
00230 
00231         if (err != CE_NONE)
00232             return payload;
00233 
00234         // Add this option header and value length to offset.
00235         offset += (option.val - (pdu->buf+offset)) + option.len;
00236     } while (1);
00237 
00238     if (err == CE_FOUND_PAYLOAD_MARKER){
00239         payload.len = pdu->len - offset - 1;
00240         payload.val = pdu->buf + offset + 1;
00241     }
00242 
00243     return payload;
00244 }
00245 
00246 
00247 //
00248 // Setters
00249 //
00250 
00251 coap_error coap_init_pdu(coap_pdu *pdu)
00252 {
00253     // Check that we were given enough packet.
00254     if (pdu->max < 4)
00255         return CE_INSUFFICIENT_BUFFER;
00256 
00257     pdu->len = 0;
00258     memset(pdu->buf, 0, 4);
00259 
00260     coap_set_version(pdu, COAP_V1);
00261     coap_set_type(pdu, CT_RST);
00262     coap_set_token(pdu, 0, 0);
00263     coap_set_code(pdu, CC_EMPTY);
00264     coap_set_mid(pdu, 0);
00265 
00266     pdu->opt_ptr = NULL;
00267 
00268     return CE_NONE;
00269 }
00270 
00271 coap_error coap_set_version(coap_pdu *pdu, coap_version ver)
00272 {
00273     // Check that we were given enough packet.
00274     if (pdu->max < 1)
00275         return CE_INSUFFICIENT_BUFFER;
00276 
00277     pdu->buf[0] = (ver << 6) | (pdu->buf[0] & 0x3F);
00278 
00279     if (pdu->len < 1)
00280         pdu->len = 1;
00281 
00282     return CE_NONE;
00283 }
00284 
00285 coap_error coap_set_type(coap_pdu *pdu, coap_type mtype)
00286 {
00287     // Check that we were given enough packet.
00288     if (pdu->max < 1)
00289         return CE_INSUFFICIENT_BUFFER;
00290 
00291     pdu->buf[0] = (mtype << 4) | (pdu->buf[0] & 0xCF);
00292 
00293     if (pdu->len < 1)
00294         pdu->len = 1;
00295 
00296     return CE_NONE;
00297 }
00298 
00299 coap_error coap_set_code(coap_pdu *pdu, coap_code code)
00300 {
00301     // Check that we were given enough packet.
00302     if (pdu->max < 2)
00303         return CE_INSUFFICIENT_BUFFER;
00304 
00305     pdu->buf[1] = code;
00306 
00307     if (pdu->len < 2)
00308         pdu->len = 2;
00309 
00310     return CE_NONE;
00311 }
00312 
00313 coap_error coap_set_mid(coap_pdu *pdu, uint16_t mid)
00314 {
00315     // Check that we were given enough packet.
00316     if (pdu->max < 4)
00317         return CE_INSUFFICIENT_BUFFER;
00318 
00319     pdu->buf[2] = mid >> 8;
00320     pdu->buf[3] = mid & 0xFF;
00321 
00322     if (pdu->len < 4)
00323         pdu->len = 4;
00324 
00325     return CE_NONE;
00326 }
00327 
00328 coap_error coap_set_token(coap_pdu *pdu, uint64_t token, uint8_t tkl)
00329 {
00330     // Check that we were given enough buffer.
00331     if (pdu->max < 4 + tkl)
00332         return CE_INSUFFICIENT_BUFFER;
00333 
00334     // Check token length for spec.
00335     if (tkl > 8)
00336         return CE_INVALID_PACKET;
00337 
00338     // Check if we may need to make or take room.
00339     if (pdu->len > 4){
00340         // Check that we were given enough buffer.
00341         if (pdu->max < pdu->len + (tkl - coap_get_tkl(pdu)))
00342             return CE_INSUFFICIENT_BUFFER;
00343 
00344         // Move rest of packet to make room or take empty space.
00345         memmove(pdu->buf + 4 + tkl, pdu->buf + 4 + coap_get_tkl(pdu), pdu->len - 4 - coap_get_tkl(pdu));
00346     }
00347 
00348     // Set token.
00349     memcpy(pdu->buf+4, &token, tkl);
00350 
00351     pdu->len += tkl - coap_get_tkl(pdu);
00352 
00353     pdu->buf[0] = (tkl & 0x0F) | (pdu->buf[0] & 0xF0);
00354 
00355     return CE_NONE;
00356 }
00357 
00358 coap_error coap_add_option(coap_pdu *pdu, int32_t opt_num, uint8_t* value, uint16_t opt_len)
00359 {
00360     uint8_t *pkt_ptr, *fopt_val, nopt_hdr_len;
00361     uint16_t fopt_num, lopt_num;
00362     size_t fopt_len, opts_len;
00363     coap_error err;
00364 
00365     // Set pointer to "zeroth option's value" which is really first option header.
00366     fopt_val = pdu->buf + 4 + coap_get_tkl(pdu); // ptr to start of options
00367     fopt_len = 0;
00368 
00369     // Option number delta starts at zero.
00370     fopt_num = 0;
00371 
00372     // Find insertion point
00373     do{
00374         pkt_ptr = fopt_val + fopt_len;
00375         lopt_num = fopt_num;
00376         err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
00377     }while (err == CE_NONE && fopt_num <= opt_num && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
00378 
00379     if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
00380         return err;
00381 
00382     // Build New Header
00383     nopt_hdr_len = coap_compute_option_header_len(opt_num - lopt_num, opt_len);
00384 
00385     // Check that we were given enough buffer.
00386     if (pdu->max < pdu->len + nopt_hdr_len + opt_len)
00387         return CE_INSUFFICIENT_BUFFER;
00388 
00389     // Check if we're adding an option in the middle of a packet.
00390     // But seriously, don't do this.
00391     if (pdu->len != pkt_ptr- pdu->buf){
00392         // Slide packet tail to make room.
00393         memmove(pkt_ptr + nopt_hdr_len + opt_len, pkt_ptr, pdu->len - (pkt_ptr - pdu->buf));
00394         pdu->len += nopt_hdr_len + opt_len;
00395 
00396         // Find Current Length of Remaining Options
00397         opts_len = pdu->len - (pkt_ptr-pdu->buf);
00398 
00399         // Adjust the option deltas for the rest of the options.
00400         coap_adjust_option_deltas(pkt_ptr + nopt_hdr_len + opt_len, 
00401                                   &opts_len, pdu->max - (pkt_ptr - pdu->buf),
00402                                   lopt_num - opt_num);
00403 
00404         // Update Total Packet Length
00405         pdu->len += opts_len - (pdu->len - (pkt_ptr-pdu->buf));
00406     }else{
00407         // Update Packet Length
00408         pdu->len = pdu->len + nopt_hdr_len + opt_len;
00409     }
00410 
00411     // Insert the Header
00412     coap_build_option_header(pkt_ptr, nopt_hdr_len, opt_num - lopt_num, opt_len);
00413 
00414     // Insert the Value
00415     memcpy(pkt_ptr + nopt_hdr_len, value, opt_len);
00416 
00417     return CE_NONE;
00418 }
00419 
00420 coap_error coap_set_payload(coap_pdu *pdu, uint8_t *payload, size_t payload_len){
00421     uint8_t *pkt_ptr, *fopt_val;
00422     uint16_t fopt_num;
00423     size_t fopt_len;
00424     coap_error err;
00425 
00426     // Set pointer to "zeroth option's value" which is really first option header.
00427     fopt_val = pdu->buf + 4 + coap_get_tkl(pdu);
00428     fopt_len = 0;
00429 
00430     // Option number delta starts at zero.
00431     fopt_num = 0;
00432 
00433     // Find insertion point
00434     do{
00435         pkt_ptr = fopt_val + fopt_len;
00436         err = coap_decode_option(pkt_ptr, (pdu->len)-(pkt_ptr-pdu->buf), &fopt_num, &fopt_len, &fopt_val);
00437     }while (err == CE_NONE && (pkt_ptr-pdu->buf) + fopt_len < pdu->len);
00438 
00439     if (err != CE_FOUND_PAYLOAD_MARKER && err != CE_END_OF_PACKET && err != CE_NONE)
00440         return err;
00441 
00442     if (err == CE_END_OF_PACKET){
00443         // Check that we were given enough buffer.
00444         if (pdu->max < pdu->len + payload_len + 1)
00445             return CE_INSUFFICIENT_BUFFER;
00446 
00447         *(pkt_ptr++) = 0xFF;
00448     }else if (err == CE_FOUND_PAYLOAD_MARKER){
00449         // Check that we were given enough buffer.
00450         if (pdu->max < pdu->len + payload_len)
00451             return CE_INSUFFICIENT_BUFFER;  
00452     }
00453 
00454     pdu->len = (pkt_ptr - pdu->buf) + payload_len;
00455     memcpy(pkt_ptr, payload, payload_len);
00456 
00457     return CE_NONE;
00458 }
00459 
00460 coap_error coap_adjust_option_deltas(uint8_t *opts_start, size_t *opts_len, size_t max_len, int32_t offset)
00461 {
00462     uint8_t *ptr, *fopt_val;
00463     uint16_t fopt_num, nopt_num;
00464     size_t fopt_len;
00465     int8_t nhdr_len, fhdr_len;
00466     coap_error err;
00467 
00468     fopt_val = opts_start;
00469     fopt_len = 0;
00470     fopt_num = 0;
00471 
00472     do{
00473         ptr = fopt_val + fopt_len;
00474         if (ptr - opts_start  > *opts_len)
00475             break;
00476 
00477         err = coap_decode_option(ptr, *opts_len-(ptr-opts_start), &fopt_num, &fopt_len, &fopt_val);
00478 
00479         if (err == CE_FOUND_PAYLOAD_MARKER || err == CE_END_OF_PACKET)
00480             break;
00481         else if (err != CE_NONE)
00482             return err;
00483 
00484         // New Option Number
00485         nopt_num = fopt_num + offset;
00486 
00487         // Find the length of the found header.
00488         fhdr_len = fopt_val - ptr;
00489 
00490         // Compute the length of the new header.
00491         nhdr_len = coap_compute_option_header_len(nopt_num, fopt_len);
00492 
00493         // Make/Take room for new header size
00494         if (fhdr_len != nhdr_len){
00495             if (max_len < *opts_len + (nhdr_len - fhdr_len))
00496                 return CE_INSUFFICIENT_BUFFER;
00497 
00498             memmove(fopt_val + (nhdr_len - fhdr_len), fopt_val, fopt_len);
00499 
00500             // Adjust Options Length
00501             *opts_len += (nhdr_len - fhdr_len);
00502         }
00503 
00504         // Write New Header
00505         nhdr_len = coap_build_option_header(ptr, nhdr_len, nopt_num, fopt_len);
00506 
00507     }while (1);
00508 
00509     return CE_NONE;
00510 
00511 }
00512 
00513 int8_t coap_build_option_header(uint8_t *buf, size_t max_len, int32_t opt_delta, int32_t opt_len)
00514 {
00515     uint8_t *ptr, base_num, base_len;
00516 
00517     if (max_len < 1)
00518         return CE_INSUFFICIENT_BUFFER;
00519 
00520     ptr = buf+1;
00521 
00522     if (opt_delta < 13) {
00523         base_num = opt_delta;
00524     }else if (opt_delta < 269) {
00525         if (max_len < ptr-buf + 1)
00526             return CE_INSUFFICIENT_BUFFER;
00527 
00528         base_num = 13;
00529         *(ptr++) = opt_delta - 13;
00530     }else {
00531         if (max_len < ptr-buf + 2)
00532             return CE_INSUFFICIENT_BUFFER;
00533 
00534         base_num = 14;
00535         *(ptr++) = (opt_delta - 269) >> 8;
00536         *(ptr++) = (opt_delta - 269) & 0xFF;
00537     }
00538 
00539     if (opt_len < 13) {
00540         base_len = opt_len;
00541     }else if (opt_len < 269) {
00542         if (max_len < ptr-buf + 1)
00543             return CE_INSUFFICIENT_BUFFER;
00544 
00545         base_len = 13;
00546         *(ptr++) = opt_len - 13;
00547     }else {
00548         if (max_len < ptr-buf + 2)
00549             return CE_INSUFFICIENT_BUFFER;
00550 
00551         base_len = 14;
00552         *(ptr++) = (opt_len - 269) >> 8;
00553         *(ptr++) = (opt_len - 269) & 0xFF;
00554     }
00555 
00556     buf[0] = (base_num << 4) | base_len;
00557 
00558 
00559     // Return the length of the new header.
00560     return ptr-buf;
00561 
00562 }
00563 
00564 int8_t coap_compute_option_header_len(int32_t opt_delta, int32_t opt_len)
00565 {
00566     int8_t len = 1;
00567 
00568     if (opt_delta < 13) {
00569     }else if (opt_delta < 269) {
00570         len += 1;
00571     }else {
00572         len += 2;
00573     }
00574 
00575     if (opt_len < 13) {
00576     }else if (opt_len < 269) {
00577         len += 1;
00578     }else {
00579         len += 2;
00580     }
00581 
00582     return len;
00583 
00584 }
00585