1 #include "struct.h"
2 #include "struct_endian.h"
3
4 #include <ctype.h>
5 #include <errno.h>
6 #include <stdarg.h>
7 #include <stdint.h>
8 #include <stdlib.h>
9 #include <string.h>
10
11 // refer to
12 // http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html#serialization
13 // - Beej's Guide to Network Programming
14 //
15 // macros for packing floats and doubles:
16 #define PACK_IEEE754_32(f) (pack_ieee754((f), 32, 8))
17 #define PACK_IEEE754_64(f) (pack_ieee754((f), 64, 11))
18 #define UNPACK_IEEE754_32(i) (unpack_ieee754((i), 32, 8))
19 #define UNPACK_IEEE754_64(i) (unpack_ieee754((i), 64, 11))
20
21 #define INIT_REPETITION(_x) int _struct_rep = 0
22
23 #define BEGIN_REPETITION(_x) \
24 do { \
25 _struct_rep--
26
27 #define END_REPETITION(_x) \
28 } \
29 while (_struct_rep > 0)
30
31 #define INC_REPETITION(_x) _struct_rep = _struct_rep * 10 + (*p - '0')
32
33 #define CLEAR_REPETITION(_x) _struct_rep = 0
34
35 static int myendian = STRUCT_ENDIAN_NOT_SET;
36
struct_init(void)37 static void struct_init(void)
38 {
39 myendian = struct_get_endian();
40 }
41
pack_ieee754(long double f,unsigned int bits,unsigned int expbits)42 static unsigned long long pack_ieee754(long double f, unsigned int bits,
43 unsigned int expbits)
44 {
45 long double fnorm;
46 int shift;
47 long long sign;
48 long long exp;
49 long long significand;
50 unsigned int significandbits = bits - expbits - 1; // -1 for sign bit
51
52 if (f == 0.0) {
53 return 0; // get this special case out of the way
54 }
55
56 // check sign and begin normalization
57 if (f < 0) {
58 sign = 1;
59 fnorm = -f;
60 } else {
61 sign = 0;
62 fnorm = f;
63 }
64
65 // get the normalized form of f and track the exponent
66 shift = 0;
67 while (fnorm >= 2.0) {
68 fnorm /= 2.0;
69 shift++;
70 }
71 while (fnorm < 1.0) {
72 fnorm *= 2.0;
73 shift--;
74 }
75 fnorm = fnorm - 1.0;
76
77 // calculate the binary form (non-float) of the significand data
78 significand = (long long)(fnorm * ((1LL << significandbits) + 0.5F));
79
80 // get the biased exponent
81 // shift + bias
82 exp = shift + ((1LL << (expbits - 1)) - 1);
83
84 // return the final answer
85 return (sign << (bits - 1U)) | (exp << (bits - expbits - 1U)) | significand;
86 }
87
unpack_ieee754(unsigned long long int i,unsigned int bits,unsigned int expbits)88 static long double unpack_ieee754(unsigned long long int i, unsigned int bits,
89 unsigned int expbits)
90 {
91 long double result;
92 long long shift;
93 unsigned int bias;
94 unsigned int significandbits = bits - expbits - 1; // -1 for sign bit
95
96 if (i == 0) {
97 return 0.0;
98 }
99
100 // pull the significand
101 result = (i & ((1LL << significandbits) - 1)); // mask
102 result /= (1LL << significandbits); // convert back to float
103 result += 1.0F; // add the one back on
104
105 // deal with the exponent
106 bias = (1 << (expbits - 1)) - 1;
107 shift = ((i >> significandbits) & ((1LL << expbits) - 1)) - bias;
108 while (shift > 0) {
109 result *= 2.0;
110 shift--;
111 }
112 while (shift < 0) {
113 result /= 2.0;
114 shift++;
115 }
116
117 // sign it
118 result *= ((i >> (bits - 1)) & 1) ? -1.0 : 1.0;
119
120 return result;
121 }
122
pack_int16_t(unsigned char ** bp,uint16_t val,int endian)123 static void pack_int16_t(unsigned char** bp, uint16_t val, int endian)
124 {
125 if (endian == myendian) {
126 *((*bp)++) = val;
127 *((*bp)++) = val >> 8;
128 } else {
129 *((*bp)++) = val >> 8;
130 *((*bp)++) = val;
131 }
132 }
133
pack_int32_t(unsigned char ** bp,uint32_t val,int endian)134 static void pack_int32_t(unsigned char** bp, uint32_t val, int endian)
135 {
136 if (endian == myendian) {
137 *((*bp)++) = val;
138 *((*bp)++) = val >> 8;
139 *((*bp)++) = val >> 16;
140 *((*bp)++) = val >> 24;
141 } else {
142 *((*bp)++) = val >> 24;
143 *((*bp)++) = val >> 16;
144 *((*bp)++) = val >> 8;
145 *((*bp)++) = val;
146 }
147 }
148
pack_int64_t(unsigned char ** bp,uint64_t val,int endian)149 static void pack_int64_t(unsigned char** bp, uint64_t val, int endian)
150 {
151 if (endian == myendian) {
152 *((*bp)++) = val;
153 *((*bp)++) = val >> 8;
154 *((*bp)++) = val >> 16;
155 *((*bp)++) = val >> 24;
156 *((*bp)++) = val >> 32;
157 *((*bp)++) = val >> 40;
158 *((*bp)++) = val >> 48;
159 *((*bp)++) = val >> 56;
160 } else {
161 *((*bp)++) = val >> 56;
162 *((*bp)++) = val >> 48;
163 *((*bp)++) = val >> 40;
164 *((*bp)++) = val >> 32;
165 *((*bp)++) = val >> 24;
166 *((*bp)++) = val >> 16;
167 *((*bp)++) = val >> 8;
168 *((*bp)++) = val;
169 }
170 }
171
pack_float(unsigned char ** bp,float val,int endian)172 static void pack_float(unsigned char** bp, float val, int endian)
173 {
174 uint64_t ieee754_encoded_val = PACK_IEEE754_32(val);
175 pack_int32_t(bp, ieee754_encoded_val, endian);
176 }
177
pack_double(unsigned char ** bp,double val,int endian)178 static void pack_double(unsigned char** bp, double val, int endian)
179 {
180 uint64_t ieee754_encoded_val = PACK_IEEE754_64(val);
181 pack_int64_t(bp, ieee754_encoded_val, endian);
182 }
183
unpack_int16_t(const unsigned char ** bp,int16_t * dst,int endian)184 static void unpack_int16_t(const unsigned char** bp, int16_t* dst, int endian)
185 {
186 uint16_t val;
187 if (endian == myendian) {
188 val = *((*bp)++);
189 val |= (uint16_t)(*((*bp)++)) << 8;
190 } else {
191 val = (uint16_t)(*((*bp)++)) << 8;
192 val |= *((*bp)++);
193 }
194 if (val <= 0x7fffU) {
195 *dst = val;
196 } else {
197 *dst = -1 - (int16_t)(0xffffU - val);
198 }
199 }
200
unpack_uint16_t(const unsigned char ** bp,uint16_t * dst,int endian)201 static void unpack_uint16_t(const unsigned char** bp, uint16_t* dst, int endian)
202 {
203 if (endian == myendian) {
204 *dst = *((*bp)++);
205 *dst |= (uint16_t)(*((*bp)++)) << 8;
206 } else {
207 *dst = (uint16_t)(*((*bp)++)) << 8;
208 *dst |= *((*bp)++);
209 }
210 }
211
unpack_int32_t(const unsigned char ** bp,int32_t * dst,int endian)212 static void unpack_int32_t(const unsigned char** bp, int32_t* dst, int endian)
213 {
214 uint32_t val;
215 if (endian == myendian) {
216 val = *((*bp)++);
217 val |= (uint32_t)(*((*bp)++)) << 8;
218 val |= (uint32_t)(*((*bp)++)) << 16;
219 val |= (uint32_t)(*((*bp)++)) << 24;
220 } else {
221 val = *((*bp)++) << 24;
222 val |= (uint32_t)(*((*bp)++)) << 16;
223 val |= (uint32_t)(*((*bp)++)) << 8;
224 val |= (uint32_t)(*((*bp)++));
225 }
226 if (val <= 0x7fffffffU) {
227 *dst = val;
228 } else {
229 *dst = -1 - (int32_t)(0xffffffffU - val);
230 }
231 }
232
unpack_uint32_t(const unsigned char ** bp,uint32_t * dst,int endian)233 static void unpack_uint32_t(const unsigned char** bp, uint32_t* dst, int endian)
234 {
235 if (endian == myendian) {
236 *dst = *((*bp)++);
237 *dst |= (uint32_t)(*((*bp)++)) << 8;
238 *dst |= (uint32_t)(*((*bp)++)) << 16;
239 *dst |= (uint32_t)(*((*bp)++)) << 24;
240 } else {
241 *dst = *((*bp)++) << 24;
242 *dst |= (uint32_t)(*((*bp)++)) << 16;
243 *dst |= (uint32_t)(*((*bp)++)) << 8;
244 *dst |= (uint32_t)(*((*bp)++));
245 }
246 }
247
unpack_int64_t(const unsigned char ** bp,int64_t * dst,int endian)248 static void unpack_int64_t(const unsigned char** bp, int64_t* dst, int endian)
249 {
250 uint64_t val;
251 if (endian == myendian) {
252 val = *((*bp)++);
253 val |= (uint64_t)(*((*bp)++)) << 8;
254 val |= (uint64_t)(*((*bp)++)) << 16;
255 val |= (uint64_t)(*((*bp)++)) << 24;
256 val |= (uint64_t)(*((*bp)++)) << 32;
257 val |= (uint64_t)(*((*bp)++)) << 40;
258 val |= (uint64_t)(*((*bp)++)) << 48;
259 val |= (uint64_t)(*((*bp)++)) << 56;
260 } else {
261 val = (uint64_t)(*((*bp)++)) << 56;
262 val |= (uint64_t)(*((*bp)++)) << 48;
263 val |= (uint64_t)(*((*bp)++)) << 40;
264 val |= (uint64_t)(*((*bp)++)) << 32;
265 val |= (uint64_t)(*((*bp)++)) << 24;
266 val |= (uint64_t)(*((*bp)++)) << 16;
267 val |= (uint64_t)(*((*bp)++)) << 8;
268 val |= *((*bp)++);
269 }
270 if (val <= 0x7fffffffffffffffULL) {
271 *dst = val;
272 } else {
273 *dst = -1 - (int64_t)(0xffffffffffffffffULL - val);
274 }
275 }
276
unpack_uint64_t(const unsigned char ** bp,uint64_t * dst,int endian)277 static void unpack_uint64_t(const unsigned char** bp, uint64_t* dst, int endian)
278 {
279 if (endian == myendian) {
280 *dst = *((*bp)++);
281 *dst |= (uint64_t)(*((*bp)++)) << 8;
282 *dst |= (uint64_t)(*((*bp)++)) << 16;
283 *dst |= (uint64_t)(*((*bp)++)) << 24;
284 *dst |= (uint64_t)(*((*bp)++)) << 32;
285 *dst |= (uint64_t)(*((*bp)++)) << 40;
286 *dst |= (uint64_t)(*((*bp)++)) << 48;
287 *dst |= (uint64_t)(*((*bp)++)) << 56;
288 } else {
289 *dst = (uint64_t)(*((*bp)++)) << 56;
290 *dst |= (uint64_t)(*((*bp)++)) << 48;
291 *dst |= (uint64_t)(*((*bp)++)) << 40;
292 *dst |= (uint64_t)(*((*bp)++)) << 32;
293 *dst |= (uint64_t)(*((*bp)++)) << 24;
294 *dst |= (uint64_t)(*((*bp)++)) << 16;
295 *dst |= (uint64_t)(*((*bp)++)) << 8;
296 *dst |= *((*bp)++);
297 }
298 }
299
unpack_float(const unsigned char ** bp,float * dst,int endian)300 static void unpack_float(const unsigned char** bp, float* dst, int endian)
301 {
302 uint32_t ieee754_encoded_val = 0;
303 unpack_uint32_t(bp, &ieee754_encoded_val, endian);
304 *dst = UNPACK_IEEE754_32(ieee754_encoded_val);
305 }
306
unpack_double(const unsigned char ** bp,double * dst,int endian)307 static void unpack_double(const unsigned char** bp, double* dst, int endian)
308 {
309 uint64_t ieee754_encoded_val = 0;
310 unpack_uint64_t(bp, &ieee754_encoded_val, endian);
311 *dst = UNPACK_IEEE754_64(ieee754_encoded_val);
312 }
313
pack_va_list(unsigned char * buf,int offset,const char * fmt,va_list args)314 static int pack_va_list(unsigned char* buf, int offset, const char* fmt,
315 va_list args)
316 {
317 INIT_REPETITION();
318 const char* p;
319 unsigned char* bp;
320 int* ep = &myendian;
321 int endian;
322
323 char b;
324 unsigned char B;
325 int16_t h;
326 uint16_t H;
327 int32_t l;
328 uint32_t L;
329 int64_t q;
330 uint64_t Q;
331 float f;
332 double d;
333 char* s;
334
335 if (STRUCT_ENDIAN_NOT_SET == myendian) {
336 struct_init();
337 }
338
339 /*
340 * 'char' and 'short' values, they must be extracted as 'int's,
341 * because C promotes 'char' and 'short' arguments to 'int' when they are
342 * represented by an ellipsis ... parameter.
343 */
344
345 bp = buf + offset;
346 for (p = fmt; *p != '\0'; p++) {
347 switch (*p) {
348 case '=': /* native */
349 ep = &myendian;
350 break;
351 case '<': /* little-endian */
352 endian = STRUCT_ENDIAN_LITTLE;
353 ep = &endian;
354 break;
355 case '>': /* big-endian */
356 endian = STRUCT_ENDIAN_BIG;
357 ep = &endian;
358 break;
359 case '!': /* network (= big-endian) */
360 endian = STRUCT_ENDIAN_BIG;
361 ep = &endian;
362 break;
363 case 'b':
364 BEGIN_REPETITION();
365 b = va_arg(args, int);
366 *bp++ = b;
367 END_REPETITION();
368 break;
369 case 'B':
370 BEGIN_REPETITION();
371 B = va_arg(args, unsigned int);
372 *bp++ = B;
373 END_REPETITION();
374 break;
375 case 'h':
376 BEGIN_REPETITION();
377 h = va_arg(args, int);
378 pack_int16_t(&bp, h, *ep);
379 END_REPETITION();
380 break;
381 case 'H':
382 BEGIN_REPETITION();
383 H = va_arg(args, int);
384 pack_int16_t(&bp, H, *ep);
385 END_REPETITION();
386 break;
387 case 'i': /* fall through */
388 case 'l':
389 BEGIN_REPETITION();
390 l = va_arg(args, int32_t);
391 pack_int32_t(&bp, l, *ep);
392 END_REPETITION();
393 break;
394 case 'I': /* fall through */
395 case 'L':
396 BEGIN_REPETITION();
397 L = va_arg(args, uint32_t);
398 pack_int32_t(&bp, L, *ep);
399 END_REPETITION();
400 break;
401 case 'q':
402 BEGIN_REPETITION();
403 q = va_arg(args, int64_t);
404 pack_int64_t(&bp, q, *ep);
405 END_REPETITION();
406 break;
407 case 'Q':
408 BEGIN_REPETITION();
409 Q = va_arg(args, uint64_t);
410 pack_int64_t(&bp, Q, *ep);
411 END_REPETITION();
412 break;
413 case 'f':
414 BEGIN_REPETITION();
415 f = va_arg(args, double);
416 pack_float(&bp, f, *ep);
417 END_REPETITION();
418 break;
419 case 'd':
420 BEGIN_REPETITION();
421 d = va_arg(args, double);
422 pack_double(&bp, d, *ep);
423 END_REPETITION();
424 break;
425 case 's': /* fall through */
426 case 'p': {
427 int i = 0;
428 s = va_arg(args, char*);
429 BEGIN_REPETITION();
430 *bp++ = s[i++];
431 END_REPETITION();
432 } break;
433 case 'x':
434 BEGIN_REPETITION();
435 *bp++ = 0;
436 END_REPETITION();
437 break;
438 default:
439 if (isdigit((int)*p)) {
440 INC_REPETITION();
441 } else {
442 return -1;
443 }
444 }
445
446 if (!isdigit((int)*p)) {
447 CLEAR_REPETITION();
448 }
449 }
450 return (bp - buf);
451 }
452
unpack_va_list(const unsigned char * buf,int offset,const char * fmt,va_list args)453 static int unpack_va_list(const unsigned char* buf, int offset, const char* fmt,
454 va_list args)
455 {
456 INIT_REPETITION();
457 const char* p;
458 const unsigned char* bp;
459 int* ep = &myendian;
460 int endian;
461
462 char* b;
463 unsigned char* B;
464 int16_t* h;
465 uint16_t* H;
466 int32_t* l;
467 uint32_t* L;
468 int64_t* q;
469 uint64_t* Q;
470 float* f;
471 double* d;
472 char* s;
473
474 if (STRUCT_ENDIAN_NOT_SET == myendian) {
475 struct_init();
476 }
477
478 bp = buf + offset;
479 for (p = fmt; *p != '\0'; p++) {
480 switch (*p) {
481 case '=': /* native */
482 ep = &myendian;
483 break;
484 case '<': /* little-endian */
485 endian = STRUCT_ENDIAN_LITTLE;
486 ep = &endian;
487 break;
488 case '>': /* big-endian */
489 endian = STRUCT_ENDIAN_BIG;
490 ep = &endian;
491 break;
492 case '!': /* network (= big-endian) */
493 endian = STRUCT_ENDIAN_BIG;
494 ep = &endian;
495 break;
496 case 'b':
497 BEGIN_REPETITION();
498 b = va_arg(args, char*);
499 *b = *bp++;
500 END_REPETITION();
501 break;
502 case 'B':
503 BEGIN_REPETITION();
504 B = va_arg(args, unsigned char*);
505 *B = *bp++;
506 END_REPETITION();
507 break;
508 case 'h':
509 BEGIN_REPETITION();
510 h = va_arg(args, int16_t*);
511 unpack_int16_t(&bp, h, *ep);
512 END_REPETITION();
513 break;
514 case 'H':
515 BEGIN_REPETITION();
516 H = va_arg(args, uint16_t*);
517 unpack_uint16_t(&bp, H, *ep);
518 END_REPETITION();
519 break;
520 case 'i': /* fall through */
521 case 'l':
522 BEGIN_REPETITION();
523 l = va_arg(args, int32_t*);
524 unpack_int32_t(&bp, l, *ep);
525 END_REPETITION();
526 break;
527 case 'I': /* fall through */
528 case 'L':
529 BEGIN_REPETITION();
530 L = va_arg(args, uint32_t*);
531 unpack_uint32_t(&bp, L, *ep);
532 END_REPETITION();
533 break;
534 case 'q':
535 BEGIN_REPETITION();
536 q = va_arg(args, int64_t*);
537 unpack_int64_t(&bp, q, *ep);
538 END_REPETITION();
539 break;
540 case 'Q':
541 BEGIN_REPETITION();
542 Q = va_arg(args, uint64_t*);
543 unpack_uint64_t(&bp, Q, *ep);
544 END_REPETITION();
545 break;
546 case 'f':
547 BEGIN_REPETITION();
548 f = va_arg(args, float*);
549 unpack_float(&bp, f, *ep);
550 END_REPETITION();
551 break;
552 case 'd':
553 BEGIN_REPETITION();
554 d = va_arg(args, double*);
555 unpack_double(&bp, d, *ep);
556 END_REPETITION();
557 break;
558 case 's': /* fall through */
559 case 'p': {
560 int i = 0;
561 s = va_arg(args, char*);
562 BEGIN_REPETITION();
563 s[i++] = *bp++;
564 END_REPETITION();
565 } break;
566 case 'x':
567 BEGIN_REPETITION();
568 bp++;
569 END_REPETITION();
570 break;
571 default:
572 if (isdigit((int)*p)) {
573 INC_REPETITION();
574 } else {
575 return -1;
576 }
577 }
578
579 if (!isdigit((int)*p)) {
580 CLEAR_REPETITION();
581 }
582 }
583 return (bp - buf);
584 }
585
586 /*
587 * EXPORT
588 *
589 * preifx: struct_
590 *
591 */
struct_pack(void * buf,const char * fmt,...)592 int struct_pack(void* buf, const char* fmt, ...)
593 {
594 va_list args;
595 int packed_len = 0;
596
597 va_start(args, fmt);
598 packed_len = pack_va_list((unsigned char*)buf, 0, fmt, args);
599 va_end(args);
600
601 return packed_len;
602 }
603
struct_pack_into(int offset,void * buf,const char * fmt,...)604 int struct_pack_into(int offset, void* buf, const char* fmt, ...)
605 {
606 va_list args;
607 int packed_len = 0;
608
609 va_start(args, fmt);
610 packed_len = pack_va_list((unsigned char*)buf, offset, fmt, args);
611 va_end(args);
612
613 return packed_len;
614 }
615
struct_unpack(const void * buf,const char * fmt,...)616 int struct_unpack(const void* buf, const char* fmt, ...)
617 {
618 va_list args;
619 int unpacked_len = 0;
620
621 va_start(args, fmt);
622 unpacked_len = unpack_va_list((const unsigned char*)buf, 0, fmt, args);
623 va_end(args);
624
625 return unpacked_len;
626 }
627
struct_unpack_from(int offset,const void * buf,const char * fmt,...)628 int struct_unpack_from(int offset, const void* buf, const char* fmt, ...)
629 {
630 va_list args;
631 int unpacked_len = 0;
632
633 va_start(args, fmt);
634 unpacked_len = unpack_va_list((const unsigned char*)buf, offset, fmt, args);
635 va_end(args);
636
637 return unpacked_len;
638 }
639
struct_calcsize(const char * fmt)640 int struct_calcsize(const char* fmt)
641 {
642 INIT_REPETITION();
643 int ret = 0;
644 const char* p;
645
646 if (STRUCT_ENDIAN_NOT_SET == myendian) {
647 struct_init();
648 }
649
650 for (p = fmt; *p != '\0'; p++) {
651 switch (*p) {
652 case '=': /* fall through */
653 case '<': /* fall through */
654 case '>': /* fall through */
655 case '!': /* ignore endian characters */
656 break;
657 case 'b':
658 BEGIN_REPETITION();
659 ret += sizeof(char);
660 END_REPETITION();
661 break;
662 case 'B':
663 BEGIN_REPETITION();
664 ret += sizeof(unsigned char);
665 END_REPETITION();
666 break;
667 case 'h':
668 BEGIN_REPETITION();
669 ret += sizeof(int16_t);
670 END_REPETITION();
671 break;
672 case 'H':
673 BEGIN_REPETITION();
674 ret += sizeof(uint16_t);
675 END_REPETITION();
676 break;
677 case 'i': /* fall through */
678 case 'l':
679 BEGIN_REPETITION();
680 ret += sizeof(int32_t);
681 END_REPETITION();
682 break;
683 case 'I': /* fall through */
684 case 'L':
685 BEGIN_REPETITION();
686 ret += sizeof(uint32_t);
687 END_REPETITION();
688 break;
689 case 'q':
690 BEGIN_REPETITION();
691 ret += sizeof(int64_t);
692 END_REPETITION();
693 break;
694 case 'Q':
695 BEGIN_REPETITION();
696 ret += sizeof(uint64_t);
697 END_REPETITION();
698 break;
699 case 'f':
700 BEGIN_REPETITION();
701 ret += sizeof(float);
702 END_REPETITION();
703 break;
704 case 'd':
705 BEGIN_REPETITION();
706 ret += sizeof(double);
707 END_REPETITION();
708 break;
709 case 's': /* fall through */
710 case 'p':
711 BEGIN_REPETITION();
712 ret += sizeof(char);
713 END_REPETITION();
714 break;
715 case 'x':
716 BEGIN_REPETITION();
717 ret += sizeof(char);
718 END_REPETITION();
719 break;
720 default:
721 if (isdigit((int)*p)) {
722 INC_REPETITION();
723 } else {
724 return -1;
725 }
726 }
727
728 if (!isdigit((int)*p)) {
729 CLEAR_REPETITION();
730 }
731 }
732 return ret;
733 }
734