cbor.cpp

00001 /*  _  __  ____    _   _ 
00002  * | |/ / |  _ \  | \ | |
00003  * | ' /  | |_) | |  \| |
00004  * | . \  |  __/  | |\  |
00005  * |_|\_\ |_|     |_| \_|
00006  * 
00007  * (c) 2018 KPN
00008  * License: MIT License.
00009  * Author: Jan Bogaerts
00010  * 
00011  * cbor parsing and rendering
00012  */
00013 
00014 
00015 #include <cbor.h>
00016 #include <senml_helpers.h>
00017 
00018 
00019 #ifndef htons
00020 #define htons(x) ( ((x)<<8 & 0xFF00) | ((x)>>8 & 0x00FF) )
00021 #endif // !htons
00022 
00023 #ifndef  htonl
00024 #define htonl(x) ( ((x)<<24 & 0xFF000000UL) | \
00025                    ((x)<< 8 & 0x00FF0000UL) | \
00026                    ((x)>> 8 & 0x0000FF00UL) | \
00027                    ((x)>>24 & 0x000000FFUL) )
00028 #endif // ! htonl
00029 
00030 #ifndef  htonll
00031 #define htonll(x) ((((uint64_t)htonl(x)) << 32) + htonl((x) >> 32))
00032 #endif // ! htonll
00033 
00034 
00035 #ifndef  ntohl
00036 #define ntohl(x) ( ((x) & 0xFF000000UL) >> 24 | \
00037                    ((x) & 0x00FF0000UL) >> 8 | \
00038                    ((x) & 0x0000FF00UL) << 8 | \
00039                    ((x) & 0x000000FFUL) << 24 )
00040 #endif // ! ntohl
00041 
00042 
00043 
00044 /**
00045  * Convert float @p x to host format
00046  */
00047 inline float ntohf(uint32_t x)
00048 {
00049     union u {
00050         float f;
00051         uint32_t i;
00052     } u = { .i = ntohl(x) };
00053     return u.f;
00054 }
00055 
00056 /**
00057  * Convert double @p x to network format
00058  */
00059 static uint64_t htond(double x)
00060 {
00061     union u {
00062         double d;
00063         uint64_t i;
00064     } u = { .d = x };
00065     return htonll(u.i);
00066 }
00067 
00068 /**
00069  * Convert double @p x to host format
00070  */
00071 inline double ntohd(uint64_t x)
00072 {
00073     union u {
00074         double d;
00075         uint64_t i;
00076     } u = { .i = htonll(x) };
00077     return u.d;
00078 }
00079 
00080 inline float ntohd_avr(uint64_t x)
00081 {
00082     union u {
00083         unsigned char dn[8];
00084         uint64_t i;
00085     } u = { .i = htonll(x) };
00086 
00087 
00088    union {
00089        float f;
00090        unsigned char b[4];
00091        uint32_t ui;
00092    } fn;    
00093    int expd = ((u.dn[7] & 127) << 4) + ((u.dn[6] & 240) >> 4);
00094    int expf = expd ? (expd - 1024) + 128 : 0;
00095    
00096    fn.b[3] = (u.dn[7] & 128) + (expf >> 1);
00097    fn.b[2] = ((expf & 1) << 7) + ((u.dn[6] & 15) << 3) + ((u.dn[5] & 0xe0) >> 5);
00098    fn.b[1] = ((u.dn[5] & 0x1f) << 3) + ((u.dn[4] & 0xe0) >> 5);
00099    fn.b[0] = ((u.dn[4] & 0x1f) << 3) + ((u.dn[3] & 0xe0) >> 5);
00100   return fn.f;
00101 }
00102 
00103 /**
00104  * Source: CBOR RFC reference implementation
00105  */
00106 double decode_float_half(unsigned char *halfp)
00107 {
00108     int half = (halfp[0] << 8) + halfp[1];
00109     int exp = (half >> 10) & 0x1f;
00110     int mant = half & 0x3ff;
00111     double val;
00112 
00113     #ifdef __MBED__
00114     if (exp == 0) {
00115         val = ldexp((double)mant, -24);
00116     }
00117     else if (exp != 31) {
00118         val = ldexp((double)(mant + 1024), exp - 25);
00119     }
00120     #else
00121     if (exp == 0) {
00122         val = ldexp(mant, -24);
00123     }
00124     else if (exp != 31) {
00125         val = ldexp(mant + 1024, exp - 25);
00126     }
00127     #endif
00128     else {
00129         val = mant == 0 ? INFINITY : NAN;
00130     }
00131 
00132     return (half & 0x8000) ? -val : val;
00133 }
00134 
00135 /**
00136  * Return additional info field value for input value @p val
00137  *
00138  * @return Byte with the additional info bits set
00139  */
00140 static unsigned char uint_additional_info(uint64_t val)
00141 {
00142     if (val < CBOR_UINT8_FOLLOWS) {
00143         return val;
00144     }
00145     else if (val <= 0xff) {
00146         return CBOR_UINT8_FOLLOWS;
00147     }
00148     else if (val <= 0xffff) {
00149         return CBOR_UINT16_FOLLOWS;
00150     }
00151     else if (val <= 0xffffffffL) {
00152         return CBOR_UINT32_FOLLOWS;
00153     }
00154 
00155     return CBOR_UINT64_FOLLOWS;
00156 }
00157 
00158 /**
00159  * Return the number of bytes that would follow the additional info field @p additional_info
00160  *
00161  * @param additional_info Must be in the range [CBOR_UINT8_FOLLOWS, CBOR_UINT64_FOLLOWS]
00162  */
00163 static unsigned char uint_bytes_follow(unsigned char additional_info)
00164 {
00165     if (additional_info < CBOR_UINT8_FOLLOWS || additional_info > CBOR_UINT64_FOLLOWS) {
00166         return 0;
00167     }
00168 
00169     const unsigned char BYTES_FOLLOW[] = {1, 2, 4, 8};
00170     return BYTES_FOLLOW[additional_info - CBOR_UINT8_FOLLOWS];
00171 }
00172 
00173 static size_t encode_int(unsigned char major_type, uint64_t val)
00174 {
00175     unsigned char additional_info = uint_additional_info(val);
00176     unsigned char bytes_follow = uint_bytes_follow(additional_info);
00177     unsigned char value = (major_type | additional_info);
00178     printText( (const char*)&value, 1);
00179 
00180     for (int i = bytes_follow - 1; i >= 0; --i) {
00181         value = (val >> (8 * i)) & 0xff;
00182         printText((const char*)&value, 1);
00183     }
00184 
00185     return bytes_follow + 1;
00186 }
00187 
00188 size_t decode_int(uint64_t *val)
00189 {
00190 
00191     *val = 0; /* clear val first */
00192 
00193     unsigned char in = readChar();
00194     unsigned char bytes_follow = uint_bytes_follow(in);
00195 
00196     switch (bytes_follow) {
00197         case 0:
00198             *val = (in & CBOR_INFO_MASK);
00199             break;
00200 
00201         case 1:
00202             *val = readChar();
00203             break;
00204 
00205         case 2:
00206             uint16_t data;
00207             readChars((unsigned char*)&data, 2);
00208             *val = htons(data);
00209             break;
00210 
00211         case 4:
00212             uint32_t data32;
00213             readChars((unsigned char*)&data32, 4);
00214             *val = htonl(data32);
00215             break;
00216 
00217         default:
00218             uint64_t data64;
00219             readChars((unsigned char*)&data64, 8);
00220             *val = htonll(data64);
00221             break;
00222     }
00223 
00224     return bytes_follow + 1;
00225 }
00226 
00227 static size_t encode_bytes(unsigned char major_type, const char *data, size_t length)
00228 {
00229     uint_bytes_follow(uint_additional_info(length)) + 1;
00230     size_t bytes_start = encode_int(major_type, (uint64_t) length);
00231 
00232     if (!bytes_start) {
00233         return 0;
00234     }
00235 
00236     printText(data, length);
00237     return (bytes_start + length);
00238 }
00239 
00240 size_t cbor_serialize_array(size_t array_length)
00241 {
00242     /* serialize number of array items */
00243     return encode_int(CBOR_ARRAY, array_length);
00244 }
00245 
00246 size_t cbor_serialize_map(size_t map_length)
00247 {
00248     /* serialize number of item key-value pairs */
00249     return encode_int(CBOR_MAP, map_length);
00250 }
00251 
00252 size_t cbor_serialize_int(int val)
00253 {
00254     if (val >= 0) {
00255         /* Major type 0: an unsigned integer */
00256         return encode_int(CBOR_UINT, val);
00257     }
00258     else {
00259         /* Major type 1: an negative integer */
00260         return encode_int(CBOR_NEGINT, -1 - val);
00261     }
00262 }
00263 
00264 
00265 size_t cbor_serialize_unicode_string(const char *val)
00266 {
00267     return encode_bytes(CBOR_TEXT, val, strlen(val));
00268 }
00269 
00270 size_t cbor_serialize_double(double val)
00271 {
00272     unsigned char value = CBOR_FLOAT64;
00273     printText((const char*)&value, 1);
00274     uint64_t encoded_val = htond(val);
00275     printText( (const char*)&encoded_val, 8);
00276     return 9;
00277 }
00278 
00279 size_t cbor_deserialize_float_half(float *val)
00280 {
00281     if (CBOR_TYPE != CBOR_7 || !val) {
00282         return 0;
00283     }
00284 
00285     unsigned char dataType = readChar();
00286     if (dataType == CBOR_FLOAT16) {
00287         uint16_t data;
00288         readChars((unsigned char*)&data, 2);
00289         *val = (float)decode_float_half((unsigned char*)&data);
00290         return 3;
00291     }
00292 
00293     return 0;
00294 }
00295 
00296 size_t cbor_deserialize_float(float *val)
00297 {
00298     if (CBOR_TYPE != CBOR_7 || !val) {
00299         return 0;
00300     }
00301 
00302     unsigned char dataType = readChar();
00303 
00304     if (dataType == CBOR_FLOAT32) {
00305         uint32_t data;
00306         readChars((unsigned char*)&data, 4);
00307         *val = ntohf(data);
00308         return 5;
00309     }
00310     return 0;
00311 }
00312 
00313 
00314 size_t cbor_deserialize_double(double *val)
00315 {
00316     if (CBOR_TYPE != CBOR_7 || !val) {
00317         return 0;
00318     }
00319 
00320     unsigned char dataType = readChar();
00321 
00322     if (dataType == CBOR_FLOAT64) {
00323         uint64_t data;
00324         readChars((unsigned char*)&data, 8);
00325         if(sizeof(double) == 8)                                 //this is the default on most systems
00326         {
00327             *val = ntohd(data);
00328         }
00329         else if(sizeof(double) == 4)                            //8 bit processors such as avr don't support 8 byte floating point values, only 4 bytes, so need special conversion
00330         {
00331             *val = ntohd_avr(data);
00332         }
00333         return 9;
00334     }
00335 
00336     return 0;
00337 }
00338 
00339 size_t cbor_deserialize_int64_t(int64_t *val)
00340 {
00341     unsigned char type = CBOR_TYPE;
00342     if ((type != CBOR_UINT && type != CBOR_NEGINT) || !val) {
00343         return 0;
00344     }
00345 
00346     uint64_t buf;
00347     size_t read_bytes = decode_int(&buf);
00348 
00349     if (type == CBOR_UINT) 
00350         *val = buf;                         /* resolve as CBOR_UINT */
00351     else 
00352         *val = -1 - buf;                    /* resolve as CBOR_NEGINT */
00353     return read_bytes;
00354 }
00355 
00356 size_t cbor_deserialize_uint64_t(uint64_t *val)
00357 {
00358     if (CBOR_TYPE != CBOR_UINT || !val) {
00359         return 0;
00360     }
00361     return decode_int(val);
00362 }
00363 
00364 size_t cbor_serialize_bool(bool val)
00365 {
00366     unsigned char value = (val ? CBOR_TRUE : CBOR_FALSE);
00367     printText((const char*)&value , 1);
00368     return 1;
00369 }
00370 
00371 size_t cbor_serialize_byte_string(const char *val, int length)
00372 {
00373     return encode_bytes(CBOR_BYTES, val, length);
00374 }
00375 
00376 
00377 
00378 
00379 
00380 
00381 
00382 
00383