KPN IoT / senml

The KPN SenML library helps you create and parse senml documents in both json and cbor format. The library can be used for sending sensor data and receiving actuator commands.

Fork of kpn_senml by KPN IoT

cbor.cpp@0:a9259748d982, 2018-05-19 (annotated)

Committer:
kpniot
Date:
Sat May 19 17:35:20 2018 +0000
Revision:
0:a9259748d982
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kpniot 0:a9259748d982 1 /* _ __ ____ _ _
kpniot 0:a9259748d982 2 * | |/ / | _ \ | \ | |
kpniot 0:a9259748d982 3 * | ' / | |_) | | \| |
kpniot 0:a9259748d982 4 * | . \ | __/ | |\ |
kpniot 0:a9259748d982 5 * |_|\_\ |_| |_| \_|
kpniot 0:a9259748d982 6 *
kpniot 0:a9259748d982 7 * (c) 2018 KPN
kpniot 0:a9259748d982 8 * License: MIT License.
kpniot 0:a9259748d982 9 * Author: Jan Bogaerts
kpniot 0:a9259748d982 10 *
kpniot 0:a9259748d982 11 * cbor parsing and rendering
kpniot 0:a9259748d982 12 */
kpniot 0:a9259748d982 13
kpniot 0:a9259748d982 14
kpniot 0:a9259748d982 15 #include <cbor.h>
kpniot 0:a9259748d982 16 #include <senml_helpers.h>
kpniot 0:a9259748d982 17
kpniot 0:a9259748d982 18
kpniot 0:a9259748d982 19 #ifndef htons
kpniot 0:a9259748d982 20 #define htons(x) ( ((x)<<8 & 0xFF00) | ((x)>>8 & 0x00FF) )
kpniot 0:a9259748d982 21 #endif // !htons
kpniot 0:a9259748d982 22
kpniot 0:a9259748d982 23 #ifndef htonl
kpniot 0:a9259748d982 24 #define htonl(x) ( ((x)<<24 & 0xFF000000UL) | \
kpniot 0:a9259748d982 25 ((x)<< 8 & 0x00FF0000UL) | \
kpniot 0:a9259748d982 26 ((x)>> 8 & 0x0000FF00UL) | \
kpniot 0:a9259748d982 27 ((x)>>24 & 0x000000FFUL) )
kpniot 0:a9259748d982 28 #endif // ! htonl
kpniot 0:a9259748d982 29
kpniot 0:a9259748d982 30 #ifndef htonll
kpniot 0:a9259748d982 31 #define htonll(x) ((((uint64_t)htonl(x)) << 32) + htonl((x) >> 32))
kpniot 0:a9259748d982 32 #endif // ! htonll
kpniot 0:a9259748d982 33
kpniot 0:a9259748d982 34
kpniot 0:a9259748d982 35 #ifndef ntohl
kpniot 0:a9259748d982 36 #define ntohl(x) ( ((x) & 0xFF000000UL) >> 24 | \
kpniot 0:a9259748d982 37 ((x) & 0x00FF0000UL) >> 8 | \
kpniot 0:a9259748d982 38 ((x) & 0x0000FF00UL) << 8 | \
kpniot 0:a9259748d982 39 ((x) & 0x000000FFUL) << 24 )
kpniot 0:a9259748d982 40 #endif // ! ntohl
kpniot 0:a9259748d982 41
kpniot 0:a9259748d982 42
kpniot 0:a9259748d982 43
kpniot 0:a9259748d982 44 /**
kpniot 0:a9259748d982 45 * Convert float @p x to host format
kpniot 0:a9259748d982 46 */
kpniot 0:a9259748d982 47 inline float ntohf(uint32_t x)
kpniot 0:a9259748d982 48 {
kpniot 0:a9259748d982 49 union u {
kpniot 0:a9259748d982 50 float f;
kpniot 0:a9259748d982 51 uint32_t i;
kpniot 0:a9259748d982 52 } u = { .i = ntohl(x) };
kpniot 0:a9259748d982 53 return u.f;
kpniot 0:a9259748d982 54 }
kpniot 0:a9259748d982 55
kpniot 0:a9259748d982 56 /**
kpniot 0:a9259748d982 57 * Convert double @p x to network format
kpniot 0:a9259748d982 58 */
kpniot 0:a9259748d982 59 static uint64_t htond(double x)
kpniot 0:a9259748d982 60 {
kpniot 0:a9259748d982 61 union u {
kpniot 0:a9259748d982 62 double d;
kpniot 0:a9259748d982 63 uint64_t i;
kpniot 0:a9259748d982 64 } u = { .d = x };
kpniot 0:a9259748d982 65 return htonll(u.i);
kpniot 0:a9259748d982 66 }
kpniot 0:a9259748d982 67
kpniot 0:a9259748d982 68 /**
kpniot 0:a9259748d982 69 * Convert double @p x to host format
kpniot 0:a9259748d982 70 */
kpniot 0:a9259748d982 71 inline double ntohd(uint64_t x)
kpniot 0:a9259748d982 72 {
kpniot 0:a9259748d982 73 union u {
kpniot 0:a9259748d982 74 double d;
kpniot 0:a9259748d982 75 uint64_t i;
kpniot 0:a9259748d982 76 } u = { .i = htonll(x) };
kpniot 0:a9259748d982 77 return u.d;
kpniot 0:a9259748d982 78 }
kpniot 0:a9259748d982 79
kpniot 0:a9259748d982 80 inline float ntohd_avr(uint64_t x)
kpniot 0:a9259748d982 81 {
kpniot 0:a9259748d982 82 union u {
kpniot 0:a9259748d982 83 unsigned char dn[8];
kpniot 0:a9259748d982 84 uint64_t i;
kpniot 0:a9259748d982 85 } u = { .i = htonll(x) };
kpniot 0:a9259748d982 86
kpniot 0:a9259748d982 87
kpniot 0:a9259748d982 88 union {
kpniot 0:a9259748d982 89 float f;
kpniot 0:a9259748d982 90 unsigned char b[4];
kpniot 0:a9259748d982 91 uint32_t ui;
kpniot 0:a9259748d982 92 } fn;
kpniot 0:a9259748d982 93 int expd = ((u.dn[7] & 127) << 4) + ((u.dn[6] & 240) >> 4);
kpniot 0:a9259748d982 94 int expf = expd ? (expd - 1024) + 128 : 0;
kpniot 0:a9259748d982 95
kpniot 0:a9259748d982 96 fn.b[3] = (u.dn[7] & 128) + (expf >> 1);
kpniot 0:a9259748d982 97 fn.b[2] = ((expf & 1) << 7) + ((u.dn[6] & 15) << 3) + ((u.dn[5] & 0xe0) >> 5);
kpniot 0:a9259748d982 98 fn.b[1] = ((u.dn[5] & 0x1f) << 3) + ((u.dn[4] & 0xe0) >> 5);
kpniot 0:a9259748d982 99 fn.b[0] = ((u.dn[4] & 0x1f) << 3) + ((u.dn[3] & 0xe0) >> 5);
kpniot 0:a9259748d982 100 return fn.f;
kpniot 0:a9259748d982 101 }
kpniot 0:a9259748d982 102
kpniot 0:a9259748d982 103 /**
kpniot 0:a9259748d982 104 * Source: CBOR RFC reference implementation
kpniot 0:a9259748d982 105 */
kpniot 0:a9259748d982 106 double decode_float_half(unsigned char *halfp)
kpniot 0:a9259748d982 107 {
kpniot 0:a9259748d982 108 int half = (halfp[0] << 8) + halfp[1];
kpniot 0:a9259748d982 109 int exp = (half >> 10) & 0x1f;
kpniot 0:a9259748d982 110 int mant = half & 0x3ff;
kpniot 0:a9259748d982 111 double val;
kpniot 0:a9259748d982 112
kpniot 0:a9259748d982 113 #ifdef __MBED__
kpniot 0:a9259748d982 114 if (exp == 0) {
kpniot 0:a9259748d982 115 val = ldexp((double)mant, -24);
kpniot 0:a9259748d982 116 }
kpniot 0:a9259748d982 117 else if (exp != 31) {
kpniot 0:a9259748d982 118 val = ldexp((double)(mant + 1024), exp - 25);
kpniot 0:a9259748d982 119 }
kpniot 0:a9259748d982 120 #else
kpniot 0:a9259748d982 121 if (exp == 0) {
kpniot 0:a9259748d982 122 val = ldexp(mant, -24);
kpniot 0:a9259748d982 123 }
kpniot 0:a9259748d982 124 else if (exp != 31) {
kpniot 0:a9259748d982 125 val = ldexp(mant + 1024, exp - 25);
kpniot 0:a9259748d982 126 }
kpniot 0:a9259748d982 127 #endif
kpniot 0:a9259748d982 128 else {
kpniot 0:a9259748d982 129 val = mant == 0 ? INFINITY : NAN;
kpniot 0:a9259748d982 130 }
kpniot 0:a9259748d982 131
kpniot 0:a9259748d982 132 return (half & 0x8000) ? -val : val;
kpniot 0:a9259748d982 133 }
kpniot 0:a9259748d982 134
kpniot 0:a9259748d982 135 /**
kpniot 0:a9259748d982 136 * Return additional info field value for input value @p val
kpniot 0:a9259748d982 137 *
kpniot 0:a9259748d982 138 * @return Byte with the additional info bits set
kpniot 0:a9259748d982 139 */
kpniot 0:a9259748d982 140 static unsigned char uint_additional_info(uint64_t val)
kpniot 0:a9259748d982 141 {
kpniot 0:a9259748d982 142 if (val < CBOR_UINT8_FOLLOWS) {
kpniot 0:a9259748d982 143 return val;
kpniot 0:a9259748d982 144 }
kpniot 0:a9259748d982 145 else if (val <= 0xff) {
kpniot 0:a9259748d982 146 return CBOR_UINT8_FOLLOWS;
kpniot 0:a9259748d982 147 }
kpniot 0:a9259748d982 148 else if (val <= 0xffff) {
kpniot 0:a9259748d982 149 return CBOR_UINT16_FOLLOWS;
kpniot 0:a9259748d982 150 }
kpniot 0:a9259748d982 151 else if (val <= 0xffffffffL) {
kpniot 0:a9259748d982 152 return CBOR_UINT32_FOLLOWS;
kpniot 0:a9259748d982 153 }
kpniot 0:a9259748d982 154
kpniot 0:a9259748d982 155 return CBOR_UINT64_FOLLOWS;
kpniot 0:a9259748d982 156 }
kpniot 0:a9259748d982 157
kpniot 0:a9259748d982 158 /**
kpniot 0:a9259748d982 159 * Return the number of bytes that would follow the additional info field @p additional_info
kpniot 0:a9259748d982 160 *
kpniot 0:a9259748d982 161 * @param additional_info Must be in the range [CBOR_UINT8_FOLLOWS, CBOR_UINT64_FOLLOWS]
kpniot 0:a9259748d982 162 */
kpniot 0:a9259748d982 163 static unsigned char uint_bytes_follow(unsigned char additional_info)
kpniot 0:a9259748d982 164 {
kpniot 0:a9259748d982 165 if (additional_info < CBOR_UINT8_FOLLOWS || additional_info > CBOR_UINT64_FOLLOWS) {
kpniot 0:a9259748d982 166 return 0;
kpniot 0:a9259748d982 167 }
kpniot 0:a9259748d982 168
kpniot 0:a9259748d982 169 const unsigned char BYTES_FOLLOW[] = {1, 2, 4, 8};
kpniot 0:a9259748d982 170 return BYTES_FOLLOW[additional_info - CBOR_UINT8_FOLLOWS];
kpniot 0:a9259748d982 171 }
kpniot 0:a9259748d982 172
kpniot 0:a9259748d982 173 static size_t encode_int(unsigned char major_type, uint64_t val)
kpniot 0:a9259748d982 174 {
kpniot 0:a9259748d982 175 unsigned char additional_info = uint_additional_info(val);
kpniot 0:a9259748d982 176 unsigned char bytes_follow = uint_bytes_follow(additional_info);
kpniot 0:a9259748d982 177 unsigned char value = (major_type | additional_info);
kpniot 0:a9259748d982 178 printText( (const char*)&value, 1);
kpniot 0:a9259748d982 179
kpniot 0:a9259748d982 180 for (int i = bytes_follow - 1; i >= 0; --i) {
kpniot 0:a9259748d982 181 value = (val >> (8 * i)) & 0xff;
kpniot 0:a9259748d982 182 printText((const char*)&value, 1);
kpniot 0:a9259748d982 183 }
kpniot 0:a9259748d982 184
kpniot 0:a9259748d982 185 return bytes_follow + 1;
kpniot 0:a9259748d982 186 }
kpniot 0:a9259748d982 187
kpniot 0:a9259748d982 188 size_t decode_int(uint64_t *val)
kpniot 0:a9259748d982 189 {
kpniot 0:a9259748d982 190
kpniot 0:a9259748d982 191 *val = 0; /* clear val first */
kpniot 0:a9259748d982 192
kpniot 0:a9259748d982 193 unsigned char in = readChar();
kpniot 0:a9259748d982 194 unsigned char bytes_follow = uint_bytes_follow(in);
kpniot 0:a9259748d982 195
kpniot 0:a9259748d982 196 switch (bytes_follow) {
kpniot 0:a9259748d982 197 case 0:
kpniot 0:a9259748d982 198 *val = (in & CBOR_INFO_MASK);
kpniot 0:a9259748d982 199 break;
kpniot 0:a9259748d982 200
kpniot 0:a9259748d982 201 case 1:
kpniot 0:a9259748d982 202 *val = readChar();
kpniot 0:a9259748d982 203 break;
kpniot 0:a9259748d982 204
kpniot 0:a9259748d982 205 case 2:
kpniot 0:a9259748d982 206 uint16_t data;
kpniot 0:a9259748d982 207 readChars((unsigned char*)&data, 2);
kpniot 0:a9259748d982 208 *val = htons(data);
kpniot 0:a9259748d982 209 break;
kpniot 0:a9259748d982 210
kpniot 0:a9259748d982 211 case 4:
kpniot 0:a9259748d982 212 uint32_t data32;
kpniot 0:a9259748d982 213 readChars((unsigned char*)&data32, 4);
kpniot 0:a9259748d982 214 *val = htonl(data32);
kpniot 0:a9259748d982 215 break;
kpniot 0:a9259748d982 216
kpniot 0:a9259748d982 217 default:
kpniot 0:a9259748d982 218 uint64_t data64;
kpniot 0:a9259748d982 219 readChars((unsigned char*)&data64, 8);
kpniot 0:a9259748d982 220 *val = htonll(data64);
kpniot 0:a9259748d982 221 break;
kpniot 0:a9259748d982 222 }
kpniot 0:a9259748d982 223
kpniot 0:a9259748d982 224 return bytes_follow + 1;
kpniot 0:a9259748d982 225 }
kpniot 0:a9259748d982 226
kpniot 0:a9259748d982 227 static size_t encode_bytes(unsigned char major_type, const char *data, size_t length)
kpniot 0:a9259748d982 228 {
kpniot 0:a9259748d982 229 uint_bytes_follow(uint_additional_info(length)) + 1;
kpniot 0:a9259748d982 230 size_t bytes_start = encode_int(major_type, (uint64_t) length);
kpniot 0:a9259748d982 231
kpniot 0:a9259748d982 232 if (!bytes_start) {
kpniot 0:a9259748d982 233 return 0;
kpniot 0:a9259748d982 234 }
kpniot 0:a9259748d982 235
kpniot 0:a9259748d982 236 printText(data, length);
kpniot 0:a9259748d982 237 return (bytes_start + length);
kpniot 0:a9259748d982 238 }
kpniot 0:a9259748d982 239
kpniot 0:a9259748d982 240 size_t cbor_serialize_array(size_t array_length)
kpniot 0:a9259748d982 241 {
kpniot 0:a9259748d982 242 /* serialize number of array items */
kpniot 0:a9259748d982 243 return encode_int(CBOR_ARRAY, array_length);
kpniot 0:a9259748d982 244 }
kpniot 0:a9259748d982 245
kpniot 0:a9259748d982 246 size_t cbor_serialize_map(size_t map_length)
kpniot 0:a9259748d982 247 {
kpniot 0:a9259748d982 248 /* serialize number of item key-value pairs */
kpniot 0:a9259748d982 249 return encode_int(CBOR_MAP, map_length);
kpniot 0:a9259748d982 250 }
kpniot 0:a9259748d982 251
kpniot 0:a9259748d982 252 size_t cbor_serialize_int(int val)
kpniot 0:a9259748d982 253 {
kpniot 0:a9259748d982 254 if (val >= 0) {
kpniot 0:a9259748d982 255 /* Major type 0: an unsigned integer */
kpniot 0:a9259748d982 256 return encode_int(CBOR_UINT, val);
kpniot 0:a9259748d982 257 }
kpniot 0:a9259748d982 258 else {
kpniot 0:a9259748d982 259 /* Major type 1: an negative integer */
kpniot 0:a9259748d982 260 return encode_int(CBOR_NEGINT, -1 - val);
kpniot 0:a9259748d982 261 }
kpniot 0:a9259748d982 262 }
kpniot 0:a9259748d982 263
kpniot 0:a9259748d982 264
kpniot 0:a9259748d982 265 size_t cbor_serialize_unicode_string(const char *val)
kpniot 0:a9259748d982 266 {
kpniot 0:a9259748d982 267 return encode_bytes(CBOR_TEXT, val, strlen(val));
kpniot 0:a9259748d982 268 }
kpniot 0:a9259748d982 269
kpniot 0:a9259748d982 270 size_t cbor_serialize_double(double val)
kpniot 0:a9259748d982 271 {
kpniot 0:a9259748d982 272 unsigned char value = CBOR_FLOAT64;
kpniot 0:a9259748d982 273 printText((const char*)&value, 1);
kpniot 0:a9259748d982 274 uint64_t encoded_val = htond(val);
kpniot 0:a9259748d982 275 printText( (const char*)&encoded_val, 8);
kpniot 0:a9259748d982 276 return 9;
kpniot 0:a9259748d982 277 }
kpniot 0:a9259748d982 278
kpniot 0:a9259748d982 279 size_t cbor_deserialize_float_half(float *val)
kpniot 0:a9259748d982 280 {
kpniot 0:a9259748d982 281 if (CBOR_TYPE != CBOR_7 || !val) {
kpniot 0:a9259748d982 282 return 0;
kpniot 0:a9259748d982 283 }
kpniot 0:a9259748d982 284
kpniot 0:a9259748d982 285 unsigned char dataType = readChar();
kpniot 0:a9259748d982 286 if (dataType == CBOR_FLOAT16) {
kpniot 0:a9259748d982 287 uint16_t data;
kpniot 0:a9259748d982 288 readChars((unsigned char*)&data, 2);
kpniot 0:a9259748d982 289 *val = (float)decode_float_half((unsigned char*)&data);
kpniot 0:a9259748d982 290 return 3;
kpniot 0:a9259748d982 291 }
kpniot 0:a9259748d982 292
kpniot 0:a9259748d982 293 return 0;
kpniot 0:a9259748d982 294 }
kpniot 0:a9259748d982 295
kpniot 0:a9259748d982 296 size_t cbor_deserialize_float(float *val)
kpniot 0:a9259748d982 297 {
kpniot 0:a9259748d982 298 if (CBOR_TYPE != CBOR_7 || !val) {
kpniot 0:a9259748d982 299 return 0;
kpniot 0:a9259748d982 300 }
kpniot 0:a9259748d982 301
kpniot 0:a9259748d982 302 unsigned char dataType = readChar();
kpniot 0:a9259748d982 303
kpniot 0:a9259748d982 304 if (dataType == CBOR_FLOAT32) {
kpniot 0:a9259748d982 305 uint32_t data;
kpniot 0:a9259748d982 306 readChars((unsigned char*)&data, 4);
kpniot 0:a9259748d982 307 *val = ntohf(data);
kpniot 0:a9259748d982 308 return 5;
kpniot 0:a9259748d982 309 }
kpniot 0:a9259748d982 310 return 0;
kpniot 0:a9259748d982 311 }
kpniot 0:a9259748d982 312
kpniot 0:a9259748d982 313
kpniot 0:a9259748d982 314 size_t cbor_deserialize_double(double *val)
kpniot 0:a9259748d982 315 {
kpniot 0:a9259748d982 316 if (CBOR_TYPE != CBOR_7 || !val) {
kpniot 0:a9259748d982 317 return 0;
kpniot 0:a9259748d982 318 }
kpniot 0:a9259748d982 319
kpniot 0:a9259748d982 320 unsigned char dataType = readChar();
kpniot 0:a9259748d982 321
kpniot 0:a9259748d982 322 if (dataType == CBOR_FLOAT64) {
kpniot 0:a9259748d982 323 uint64_t data;
kpniot 0:a9259748d982 324 readChars((unsigned char*)&data, 8);
kpniot 0:a9259748d982 325 if(sizeof(double) == 8) //this is the default on most systems
kpniot 0:a9259748d982 326 {
kpniot 0:a9259748d982 327 *val = ntohd(data);
kpniot 0:a9259748d982 328 }
kpniot 0:a9259748d982 329 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
kpniot 0:a9259748d982 330 {
kpniot 0:a9259748d982 331 *val = ntohd_avr(data);
kpniot 0:a9259748d982 332 }
kpniot 0:a9259748d982 333 return 9;
kpniot 0:a9259748d982 334 }
kpniot 0:a9259748d982 335
kpniot 0:a9259748d982 336 return 0;
kpniot 0:a9259748d982 337 }
kpniot 0:a9259748d982 338
kpniot 0:a9259748d982 339 size_t cbor_deserialize_int64_t(int64_t *val)
kpniot 0:a9259748d982 340 {
kpniot 0:a9259748d982 341 unsigned char type = CBOR_TYPE;
kpniot 0:a9259748d982 342 if ((type != CBOR_UINT && type != CBOR_NEGINT) || !val) {
kpniot 0:a9259748d982 343 return 0;
kpniot 0:a9259748d982 344 }
kpniot 0:a9259748d982 345
kpniot 0:a9259748d982 346 uint64_t buf;
kpniot 0:a9259748d982 347 size_t read_bytes = decode_int(&buf);
kpniot 0:a9259748d982 348
kpniot 0:a9259748d982 349 if (type == CBOR_UINT)
kpniot 0:a9259748d982 350 *val = buf; /* resolve as CBOR_UINT */
kpniot 0:a9259748d982 351 else
kpniot 0:a9259748d982 352 *val = -1 - buf; /* resolve as CBOR_NEGINT */
kpniot 0:a9259748d982 353 return read_bytes;
kpniot 0:a9259748d982 354 }
kpniot 0:a9259748d982 355
kpniot 0:a9259748d982 356 size_t cbor_deserialize_uint64_t(uint64_t *val)
kpniot 0:a9259748d982 357 {
kpniot 0:a9259748d982 358 if (CBOR_TYPE != CBOR_UINT || !val) {
kpniot 0:a9259748d982 359 return 0;
kpniot 0:a9259748d982 360 }
kpniot 0:a9259748d982 361 return decode_int(val);
kpniot 0:a9259748d982 362 }
kpniot 0:a9259748d982 363
kpniot 0:a9259748d982 364 size_t cbor_serialize_bool(bool val)
kpniot 0:a9259748d982 365 {
kpniot 0:a9259748d982 366 unsigned char value = (val ? CBOR_TRUE : CBOR_FALSE);
kpniot 0:a9259748d982 367 printText((const char*)&value , 1);
kpniot 0:a9259748d982 368 return 1;
kpniot 0:a9259748d982 369 }
kpniot 0:a9259748d982 370
kpniot 0:a9259748d982 371 size_t cbor_serialize_byte_string(const char *val, int length)
kpniot 0:a9259748d982 372 {
kpniot 0:a9259748d982 373 return encode_bytes(CBOR_BYTES, val, length);
kpniot 0:a9259748d982 374 }
kpniot 0:a9259748d982 375
kpniot 0:a9259748d982 376
kpniot 0:a9259748d982 377
kpniot 0:a9259748d982 378
kpniot 0:a9259748d982 379
kpniot 0:a9259748d982 380
kpniot 0:a9259748d982 381
kpniot 0:a9259748d982 382
kpniot 0:a9259748d982 383