pack.c (2141B)
1 #include <ctype.h> 2 #include <stdarg.h> 3 #include <stdio.h> 4 #include <stdlib.h> 5 6 #include <scc/mach.h> 7 #include "libmach.h" 8 9 static int 10 lpack(unsigned char *dst, char *fmt, va_list va) 11 { 12 unsigned char *bp, *cp; 13 unsigned s; 14 unsigned long l; 15 unsigned long long q; 16 int n; 17 18 bp = dst; 19 while (*fmt) { 20 switch (*fmt++) { 21 case '\'': 22 n = atoi(fmt); 23 while (isdigit(*fmt)) 24 fmt++; 25 cp = va_arg(va, unsigned char *); 26 while (n--) 27 *bp++ = *cp++; 28 break; 29 case 'c': 30 *bp++ = va_arg(va, unsigned); 31 break; 32 case 's': 33 s = va_arg(va, unsigned); 34 *bp++ = s; 35 *bp++ = s >> 8; 36 break; 37 case 'l': 38 l = va_arg(va, unsigned long); 39 *bp++ = l; 40 *bp++ = l >> 8; 41 *bp++ = l >> 16; 42 *bp++ = l >> 24; 43 break; 44 case 'q': 45 q = va_arg(va, unsigned long long); 46 *bp++ = q; 47 *bp++ = q >> 8; 48 *bp++ = q >> 16; 49 *bp++ = q >> 24; 50 *bp++ = q >> 32; 51 *bp++ = q >> 40; 52 *bp++ = q >> 48; 53 *bp++ = q >> 56; 54 break; 55 default: 56 return -1; 57 } 58 } 59 60 return bp - dst; 61 } 62 63 static int 64 bpack(unsigned char *dst, char *fmt, va_list va) 65 { 66 unsigned char *bp, *cp; 67 unsigned s; 68 unsigned long l; 69 unsigned long long q; 70 int n; 71 72 bp = dst; 73 while (*fmt) { 74 switch (*fmt++) { 75 case '\'': 76 n = atoi(fmt); 77 while (isdigit(*fmt)) 78 fmt++; 79 cp = va_arg(va, unsigned char *); 80 while (n--) 81 *bp++ = *cp++; 82 break; 83 case 'c': 84 *bp++ = va_arg(va, unsigned); 85 break; 86 case 's': 87 s = va_arg(va, unsigned); 88 *bp++ = s >> 8; 89 *bp++ = s; 90 break; 91 case 'l': 92 l = va_arg(va, unsigned long); 93 *bp++ = l >> 24; 94 *bp++ = l >> 16; 95 *bp++ = l >> 8; 96 *bp++ = l; 97 break; 98 case 'q': 99 q = va_arg(va, unsigned long long); 100 *bp++ = q >> 56; 101 *bp++ = q >> 48; 102 *bp++ = q >> 40; 103 *bp++ = q >> 32; 104 *bp++ = q >> 24; 105 *bp++ = q >> 16; 106 *bp++ = q >> 8; 107 *bp++ = q; 108 break; 109 default: 110 return -1; 111 } 112 } 113 114 return bp - dst; 115 } 116 117 int 118 pack(int order, unsigned char *dst, char *fmt, ...) 119 { 120 int r; 121 int (*fn)(unsigned char *dst, char *fmt, va_list va); 122 va_list va; 123 124 va_start(va, fmt); 125 fn = (order == LITTLE_ENDIAN) ? lpack : bpack; 126 r = (*fn)(dst, fmt, va); 127 va_end(va); 128 129 return r; 130 }