Simple interface for Mbed Cloud Client
Embed:
(wiki syntax)
Show/hide line numbers
cn-encoder.c
00001 #ifndef CN_ENCODER_C 00002 #define CN_ENCODER_C 00003 00004 #ifdef __cplusplus 00005 extern "C" { 00006 #endif 00007 #ifdef EMACS_INDENTATION_HELPER 00008 } /* Duh. */ 00009 #endif 00010 00011 #include <string.h> 00012 #include <stdbool.h> 00013 #include <assert.h> 00014 #include <limits.h> 00015 #include <math.h> 00016 #include "cn-cbor.h" 00017 #include "cbor.h" 00018 00019 // Provide our own implementation of htons() and htonl() to avoid inclusion of inet.h or arpa/inet,h, 00020 // which can create unintended depedencies. 00021 00022 static uint16_t htons(const uint16_t host16) { 00023 uint16_t be16; 00024 uint8_t *be16_ptr = (uint8_t*)&be16; 00025 00026 be16_ptr[0] = (host16 >> 8) & 0xff; 00027 be16_ptr[1] = (host16 >> 0) & 0xff; 00028 00029 return be16; 00030 } 00031 00032 static uint32_t htonl(const uint32_t host32) { 00033 uint32_t be32; 00034 uint8_t *be32_ptr = (uint8_t*)&be32; 00035 00036 be32_ptr[0] = (host32 >> 24) & 0xff; 00037 be32_ptr[1] = (host32 >> 16) & 0xff; 00038 be32_ptr[2] = (host32 >> 8) & 0xff; 00039 be32_ptr[3] = (host32 >> 0) & 0xff; 00040 00041 return be32; 00042 } 00043 00044 #define hton8p(p) (*(uint8_t*)(p)) 00045 #define hton16p(p) (htons(*(uint16_t*)(p))) 00046 #define hton32p(p) (htonl(*(uint32_t*)(p))) 00047 static uint64_t hton64p(const uint8_t *p) { 00048 /* TODO: does this work on both BE and LE systems? */ 00049 uint64_t ret = hton32p(p); 00050 ret <<= 32; 00051 ret |= hton32p(p+4); 00052 return ret; 00053 } 00054 00055 typedef struct _write_state 00056 { 00057 uint8_t *buf; 00058 int offset; 00059 int size; 00060 } cn_write_state; 00061 00062 #define ensure_writable(sz) if ((ws->offset<0) || (ws->offset + (sz) > ws->size)) { \ 00063 ws->offset = -1; \ 00064 return; \ 00065 } 00066 00067 #define write_byte_and_data(b, data, sz) \ 00068 if (ws->buf == NULL) { ws->offset += sz + 1; } \ 00069 else { \ 00070 ws->buf[ws->offset++] = (b); \ 00071 memcpy(ws->buf + ws->offset, (data), (sz)); \ 00072 ws->offset += sz; \ 00073 } 00074 00075 #define write_byte(b) \ 00076 if (ws->buf == NULL) { ws->offset++; } \ 00077 else { ws->buf[ws->offset++] = (b); } \ 00078 00079 #define write_byte_ensured(b) \ 00080 ensure_writable(1); \ 00081 write_byte(b); \ 00082 00083 static uint8_t _xlate[] = { 00084 IB_FALSE, /* CN_CBOR_FALSE */ 00085 IB_TRUE, /* CN_CBOR_TRUE */ 00086 IB_NIL, /* CN_CBOR_NULL */ 00087 IB_UNDEF, /* CN_CBOR_UNDEF */ 00088 IB_UNSIGNED, /* CN_CBOR_UINT */ 00089 IB_NEGATIVE, /* CN_CBOR_INT */ 00090 IB_BYTES, /* CN_CBOR_BYTES */ 00091 IB_TEXT, /* CN_CBOR_TEXT */ 00092 IB_BYTES, /* CN_CBOR_BYTES_CHUNKED */ 00093 IB_TEXT, /* CN_CBOR_TEXT_CHUNKED */ 00094 IB_ARRAY, /* CN_CBOR_ARRAY */ 00095 IB_MAP, /* CN_CBOR_MAP */ 00096 IB_TAG, /* CN_CBOR_TAG */ 00097 IB_PRIM, /* CN_CBOR_SIMPLE */ 00098 0xFF, /* CN_CBOR_DOUBLE */ 00099 0xFF /* CN_CBOR_INVALID */ 00100 }; 00101 00102 static inline bool is_indefinite(const cn_cbor *cb) 00103 { 00104 return (cb->flags & CN_CBOR_FL_INDEF) != 0; 00105 } 00106 00107 static void _write_positive(cn_write_state *ws, cn_cbor_type typ, uint64_t val) { 00108 uint8_t ib; 00109 assert((size_t)typ < sizeof(_xlate)); 00110 00111 ib = _xlate[typ]; 00112 if (ib == 0xFF) { 00113 ws->offset = -1; 00114 return; 00115 } 00116 00117 if (val < 24) { 00118 ensure_writable(1); 00119 write_byte(ib | val); 00120 } else if (val < 256) { 00121 ensure_writable(2); 00122 write_byte(ib | 24); 00123 write_byte((uint8_t)val); 00124 } else if (val < 65536) { 00125 uint16_t be16 = (uint16_t)val; 00126 ensure_writable(3); 00127 be16 = hton16p(&be16); 00128 write_byte_and_data(ib | 25, (const void*)&be16, 2); 00129 } else if (val < 0x100000000L) { 00130 uint32_t be32 = (uint32_t)val; 00131 ensure_writable(5); 00132 be32 = hton32p(&be32); 00133 write_byte_and_data(ib | 26, (const void*)&be32, 4); 00134 } else { 00135 uint64_t be64; 00136 ensure_writable(9); 00137 be64 = hton64p((const uint8_t*)&val); 00138 write_byte_and_data(ib | 27, (const void*)&be64, 8); 00139 } 00140 } 00141 00142 #ifndef CBOR_NO_FLOAT 00143 static void _write_double(cn_write_state *ws, double val) 00144 { 00145 float float_val = val; 00146 00147 /* NOTE - the following code block assures portability across 00148 different OSs, it is even necessary when running 00149 different OSs under same toolchain. */ 00150 if (isnan(val)) { 00151 float_val = NAN; 00152 } else if (isinf(val) && isgreater(val, 0)) { 00153 float_val = INFINITY; 00154 } else if (isinf(val) && isless(val, 0)) { 00155 float_val = -INFINITY; 00156 } 00157 00158 if (((float_val == val) && !isnan(val)) || (isinf(val) && isinf(float_val))) { /* 32 bits is enough and we aren't NaN or INFINITY */ 00159 uint32_t be32; 00160 uint16_t be16, u16; 00161 union { 00162 float f; 00163 uint32_t u; 00164 } u32; 00165 u32.f = float_val; 00166 if ((u32.u & 0x1FFF) == 0) { /* worth trying half */ 00167 int s16 = (u32.u >> 16) & 0x8000; 00168 int exp = (u32.u >> 23) & 0xff; 00169 int mant = u32.u & 0x7fffff; 00170 if (exp == 0 && mant == 0) 00171 ; /* 0.0, -0.0 */ 00172 else if (exp >= 113 && exp <= 142) /* normalized */ 00173 s16 += ((exp - 112) << 10) + (mant >> 13); 00174 else if (exp >= 103 && exp < 113) { /* denorm, exp16 = 0 */ 00175 if (mant & ((1 << (126 - exp)) - 1)) 00176 goto float32; /* loss of precision */ 00177 s16 += ((mant + 0x800000) >> (126 - exp)); 00178 } else if (exp == 255 && mant == 0) { /* Inf */ 00179 s16 += 0x7c00; 00180 } else 00181 goto float32; /* loss of range */ 00182 00183 ensure_writable(3); 00184 u16 = s16; 00185 be16 = hton16p((const uint8_t*)&u16); 00186 00187 write_byte_and_data(IB_PRIM | 25, (const void*)&be16, 2); 00188 return; 00189 } 00190 float32: 00191 ensure_writable(5); 00192 be32 = hton32p((const uint8_t*)&u32.u); 00193 00194 write_byte_and_data(IB_PRIM | 26, (const void*)&be32, 4); 00195 00196 } else if (isnan(val)) { /* NaN -- we always write a half NaN*/ 00197 ensure_writable(3); 00198 write_byte_and_data(IB_PRIM | 25, (const void*)"\x7e\x00", 2); 00199 } else { 00200 uint64_t be64; 00201 /* Copy the same problematic implementation from the decoder. */ 00202 union { 00203 double d; 00204 uint64_t u; 00205 } u64; 00206 00207 u64.d = val; 00208 00209 ensure_writable(9); 00210 be64 = hton64p((const uint8_t*)&u64.u); 00211 00212 write_byte_and_data(IB_PRIM | 27, (const void*)&be64, 8); 00213 00214 } 00215 } 00216 #endif /* CBOR_NO_FLOAT */ 00217 00218 // TODO: make public? 00219 typedef void (*cn_visit_func)(const cn_cbor *cb, int depth, void *context); 00220 static void _visit(const cn_cbor *cb, 00221 cn_visit_func visitor, 00222 cn_visit_func breaker, 00223 void *context) 00224 { 00225 const cn_cbor *p = cb; 00226 int depth = 0; 00227 while (p) 00228 { 00229 visit: 00230 00231 visitor(p, depth, context); 00232 00233 if (p->first_child) { 00234 00235 p = p->first_child; 00236 depth++; 00237 } else{ 00238 00239 // Empty indefinite 00240 if (is_indefinite(p)) { 00241 00242 breaker(p->parent, depth, context); 00243 } 00244 if (p->next) { 00245 p = p->next; 00246 } else { 00247 while (p->parent) { 00248 depth--; 00249 if (is_indefinite(p->parent)) { 00250 breaker(p->parent, depth, context); 00251 } 00252 if (p->parent->next) { 00253 p = p->parent->next; 00254 goto visit; 00255 } 00256 p = p->parent; 00257 } 00258 return; 00259 } 00260 } 00261 } 00262 } 00263 00264 #define CHECK(st) (st); \ 00265 if (ws->offset < 0) {return;} 00266 00267 void _encoder_visitor(const cn_cbor *cb, int depth, void *context) 00268 { 00269 cn_write_state *ws = context; 00270 UNUSED_PARAM(depth); 00271 00272 switch (cb->type) { 00273 case CN_CBOR_ARRAY: 00274 if (is_indefinite(cb)) { 00275 write_byte_ensured(IB_ARRAY | AI_INDEF); 00276 } else { 00277 CHECK(_write_positive(ws, CN_CBOR_ARRAY, cb->length)); 00278 } 00279 break; 00280 case CN_CBOR_MAP: 00281 if (is_indefinite(cb)) { 00282 write_byte_ensured(IB_MAP | AI_INDEF); 00283 } else { 00284 CHECK(_write_positive(ws, CN_CBOR_MAP, cb->length/2)); 00285 } 00286 break; 00287 case CN_CBOR_BYTES_CHUNKED: 00288 case CN_CBOR_TEXT_CHUNKED: 00289 write_byte_ensured(_xlate[cb->type] | AI_INDEF); 00290 break; 00291 00292 case CN_CBOR_TEXT: 00293 case CN_CBOR_BYTES: 00294 CHECK(_write_positive(ws, cb->type, cb->length)); 00295 ensure_writable(cb->length); 00296 if (ws->buf != NULL) 00297 { 00298 memcpy(ws->buf + ws->offset, cb->v.str, cb->length); 00299 } 00300 00301 ws->offset += cb->length; 00302 00303 break; 00304 00305 case CN_CBOR_FALSE: 00306 case CN_CBOR_TRUE: 00307 case CN_CBOR_NULL: 00308 case CN_CBOR_UNDEF: 00309 write_byte_ensured(_xlate[cb->type]); 00310 break; 00311 00312 case CN_CBOR_TAG: 00313 case CN_CBOR_UINT: 00314 case CN_CBOR_SIMPLE: 00315 CHECK(_write_positive(ws, cb->type, cb->v.uint)); 00316 break; 00317 00318 case CN_CBOR_INT: 00319 assert(cb->v.sint < 0); 00320 CHECK(_write_positive(ws, CN_CBOR_INT, ~(cb->v.sint))); 00321 break; 00322 00323 case CN_CBOR_DOUBLE: 00324 #ifndef CBOR_NO_FLOAT 00325 CHECK(_write_double(ws, cb->v.dbl)); 00326 #endif /* CBOR_NO_FLOAT */ 00327 break; 00328 00329 case CN_CBOR_INVALID: 00330 ws->offset = -1; 00331 break; 00332 } 00333 } 00334 00335 void _encoder_breaker(const cn_cbor *cb, int depth, void *context) 00336 { 00337 cn_write_state *ws = context; 00338 UNUSED_PARAM(cb); 00339 UNUSED_PARAM(depth); 00340 write_byte_ensured(IB_BREAK); 00341 } 00342 00343 /* 00344 If called with a NULL buf, will run as usual but will not copy any data. 00345 This is done in order to acquire the size of the buffer we will allocate for the decoded buffer. 00346 */ 00347 static int cn_cbor_encoder(uint8_t *buf, 00348 size_t buf_offset, 00349 size_t buf_size, 00350 const cn_cbor *cb) 00351 { 00352 int ret; 00353 cn_write_state ws = { buf, buf_offset, buf_size }; 00354 _visit(cb, _encoder_visitor, _encoder_breaker, &ws); 00355 00356 if (ws.offset < 0) { return -1; } 00357 ret = ws.offset - buf_offset; 00358 return ret; 00359 } 00360 00361 int cn_cbor_get_encoded_size(const cn_cbor *cb, cn_cbor_errback *err) 00362 { 00363 int ret; 00364 00365 if (cb == NULL) { 00366 err->err = CN_CBOR_ERR_INVALID_PARAMETER; 00367 return -1; 00368 } 00369 00370 ret = cn_cbor_encoder(NULL, 0, INT_MAX, cb); 00371 if (ret <= 0) { 00372 err->err = CN_CBOR_ERR_ENCODER; 00373 return -1; 00374 } 00375 00376 err->err = CN_CBOR_NO_ERROR; 00377 return ret; 00378 } 00379 00380 int cn_cbor_encoder_write(const cn_cbor *cb, uint8_t *bufOut, int bufSize, cn_cbor_errback *err) 00381 { 00382 int bytesWritten; 00383 00384 if (bufOut == NULL) { 00385 err->err = CN_CBOR_ERR_INVALID_PARAMETER; 00386 return -1; 00387 } 00388 00389 bytesWritten = cn_cbor_encoder(bufOut, 0, bufSize, cb); 00390 if (bytesWritten <= 0) 00391 { 00392 err->err = CN_CBOR_ERR_ENCODER; 00393 } 00394 else 00395 { 00396 err->err = CN_CBOR_NO_ERROR; 00397 00398 } 00399 00400 return bytesWritten; 00401 } 00402 00403 #ifdef __cplusplus 00404 } 00405 #endif 00406 00407 #endif /* CN_CBOR_C */
Generated on Tue Jul 12 2022 19:01:33 by 1.7.2