scc

  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
  9static int
 10lunpack(unsigned char *src, char *fmt, va_list va)
 11{
 12	unsigned char *bp, *cp;
 13	unsigned short *sp;
 14	unsigned s;
 15	unsigned long *lp, l;
 16	unsigned long long *qp, q;
 17	int n;
 18
 19	bp = src;
 20	while (*fmt) {
 21		switch (*fmt++) {
 22		case '\'':
 23			n = atoi(fmt);
 24			while (isdigit(*fmt))
 25				fmt++;
 26			cp = va_arg(va, unsigned char *);
 27			while (n--)
 28				*cp++ = *bp++;
 29			break;
 30		case 'c':
 31			cp = va_arg(va, unsigned char *);
 32			*cp = *bp++;
 33			break;
 34		case 's':
 35			sp = va_arg(va, unsigned short *);
 36			s =  (unsigned) *bp++;
 37			s |= (unsigned) *bp++ << 8;
 38			*sp = s;
 39			break;
 40		case 'l':
 41			lp = va_arg(va, unsigned long *);
 42			l = (unsigned long) *bp++;
 43			l |= (unsigned long) *bp++ << 8;
 44			l |= (unsigned long) *bp++ << 16;
 45			l |= (unsigned long) *bp++ << 24;
 46			*lp = l;
 47			break;
 48		case 'q':
 49			qp = va_arg(va, unsigned long long *);
 50			q = (unsigned long long) *bp++;
 51			q |= (unsigned long long) *bp++ << 8;
 52			q |= (unsigned long long) *bp++ << 16;
 53			q |= (unsigned long long) *bp++ << 24;
 54			q |= (unsigned long long) *bp++ << 32;
 55			q |= (unsigned long long) *bp++ << 40;
 56			q |= (unsigned long long) *bp++ << 48;
 57			q |= (unsigned long long) *bp++ << 56;
 58			*qp = q;
 59			break;
 60		default:
 61			return -1;
 62		}
 63	}
 64
 65	return bp - src;
 66}
 67
 68static int
 69bunpack(unsigned char *src, char *fmt, va_list va)
 70{
 71	unsigned char *bp, *cp;
 72	unsigned short *sp;
 73	unsigned s;
 74	unsigned long *lp, l;
 75	unsigned long long *qp, q;
 76	int n;
 77
 78	bp = src;
 79	while (*fmt) {
 80		switch (*fmt++) {
 81		case '\'':
 82			n = atoi(fmt);
 83			while (isdigit(*fmt))
 84				fmt++;
 85			cp = va_arg(va, unsigned char *);
 86			while (n--)
 87				*cp++ = *bp++;
 88			break;
 89		case 'c':
 90			cp = va_arg(va, unsigned char *);
 91			*cp = *bp++;
 92			break;
 93		case 's':
 94			sp = va_arg(va, unsigned short *);
 95			s =  (unsigned) *bp++ << 8;
 96			s |= (unsigned) *bp++;
 97			*sp = s;
 98			break;
 99		case 'l':
100			lp = va_arg(va, unsigned long *);
101			l =  (unsigned long) *bp++ << 24;
102			l |= (unsigned long) *bp++ << 16;
103			l |= (unsigned long) *bp++ << 8;
104			l |= (unsigned long) *bp++;
105			*lp = l;
106			break;
107		case 'q':
108			qp = va_arg(va, unsigned long long *);
109			q =  (unsigned long long) *bp++ << 56;
110			q |= (unsigned long long) *bp++ << 48;
111			q |= (unsigned long long) *bp++ << 40;
112			q |= (unsigned long long) *bp++ << 32;
113			q |= (unsigned long long) *bp++ << 24;
114			q |= (unsigned long long) *bp++ << 16;
115			q |= (unsigned long long) *bp++ << 8;
116			q |= (unsigned long long) *bp++;
117			*qp = q;
118			break;
119		default:
120			return -1;
121		}
122	}
123
124	return bp - src;
125}
126
127int
128unpack(int order, unsigned char *src, char *fmt, ...)
129{
130	int r;
131	int (*fn)(unsigned char *dst, char *fmt, va_list va);
132	va_list va;
133
134	va_start(va, fmt);
135	fn = (order == LITTLE_ENDIAN) ? lunpack : bunpack;
136	r = (*fn)(src, fmt, va);
137	va_end(va);
138
139	return r;
140}