From 4ca672e113bd7ab6c106c41d53b10a4828228b4a Mon Sep 17 00:00:00 2001 From: "Eric S. Raymond" Date: Wed, 16 Jul 2008 22:42:11 +0000 Subject: Factor out the bitfield functions, fix a bug in one, add tests. --- Makefile.am | 1 + bits.c | 61 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ bits.h | 11 ++++++++--- bits_test.c | 32 ++++++++++++++++++++++++++++++-- rtcm3.c | 38 ++------------------------------------ 5 files changed, 102 insertions(+), 41 deletions(-) create mode 100644 bits.c diff --git a/Makefile.am b/Makefile.am index e9e6af79..0599d1ec 100644 --- a/Makefile.am +++ b/Makefile.am @@ -143,6 +143,7 @@ libgpsd_c_sources = \ evermore.c \ italk.c \ navcom.c \ + bits.c \ crc24q.c libgpsd_h_sources = \ diff --git a/bits.c b/bits.c new file mode 100644 index 00000000..9f97ac5a --- /dev/null +++ b/bits.c @@ -0,0 +1,61 @@ +#define DEBUG +/* + * Bitfield extraction functions. In each, start is a bit index (not a byte + * index) and width is a bit width (bounded above by the bit width of long + * long). + * + * The sbits() function assumes twos-complement arithmetic. + */ +#include + +#include "bits.h" +#ifdef DEBUG +#include +#include "gpsd_config.h" +#include "gpsd.h" +#endif /* DEBUG */ + + +#define BITS_PER_BYTE 8 + +unsigned long long ubits(char buf[], unsigned int start, unsigned int width) +/* extract a bitfield from the buffer as an unsigned big-endian long */ +{ + unsigned long long fld = 0; + unsigned int i;; + + assert(width <= sizeof(long long) * BITS_PER_BYTE); + for (i = 0; i < (width + BITS_PER_BYTE - 1) / BITS_PER_BYTE; i++) { + fld <<= BITS_PER_BYTE; + fld |= (unsigned char)buf[start / BITS_PER_BYTE + i]; + } +#ifdef DEBUG + printf("Extracting %d:%d from %s: segment 0x%llx = %lld\n", start, width, gpsd_hexdump(buf, 12), fld, fld); +#endif /* DEBUG */ + + fld &= (0xffffffff >> (start % BITS_PER_BYTE)); +#ifdef DEBUG + printf("After masking: 0x%llx = %lld\n", fld, fld); +#endif /* DEBUG */ + fld >>= (BITS_PER_BYTE - 1) - ((start + width) % BITS_PER_BYTE); +#ifdef DEBUG + printf("After downshifting: 0x%llx = %lld\n", fld, fld); +#endif /* DEBUG */ + + return fld; +} + +signed long long sbits(char buf[], unsigned int start, unsigned int width) +/* extract a bitfield from the buffer as a signed big-endian long */ +{ + unsigned long long un = ubits(buf, start, width); + signed long long fld; + + if (un & (1 << width)) + fld = -(un & ~(1 << width)); + else + fld = (signed long long)un; + + return fld; +} + diff --git a/bits.h b/bits.h index c73ac8b4..07e2d0d3 100644 --- a/bits.h +++ b/bits.h @@ -2,9 +2,9 @@ /* * bits.h - extract binary data from message buffer * - * These macros extract bytes, words, longwords, floats or doubles from - * a message that contains these items in either MSB-first or LSB-first - * byte order. + * These macros extract bytes, words, longwords, floats, doubles, or + * bitfields of arbitrary length and size from a message that contains + * these items in either MSB-first or LSB-first byte order. * * By defining the GET_ORIGIN and PUT_ORIGIN macros before including * this header, it's possible to change the origin of the indexing. @@ -16,6 +16,7 @@ * The use of fixed-length types in the casts enforces these. * Both 32- and 64-bit systems with gcc are OK with this set. */ +#include #ifndef BITS_H #define BITS_H @@ -80,3 +81,7 @@ union long_double { | (buf[2*(n)+1] << 24)) #define getstringz(to, from, s, e) \ (void)memcpy(to, from+2*(s)-2, 2*((e)-(s)+1)) + +/* bitfield extraction */ +extern unsigned long long ubits(char buf[], unsigned int, unsigned int); +extern signed long long sbits(char buf[], unsigned int, unsigned int); diff --git a/bits_test.c b/bits_test.c index b6beb55f..3273c1f1 100644 --- a/bits_test.c +++ b/bits_test.c @@ -83,16 +83,33 @@ static void ledumpall(void) (void)printf("getled: %.16f %.16f\n", d1, getled(buf, 16)); } +struct unsigned_test { + unsigned int start, width; + unsigned long long expected; +}; + /*@ -duplicatequals +ignorequals @*/ int main(void) { + struct unsigned_test *up, unsigned_tests[] = { + {0, 1, 0}, /* first bit of first byte */ + {0, 7, 1}, /* third bit of first byte */ + }; + + unsigned char *sp; + memcpy(buf,"\x01\x02\x03\x04\x05\x06\x07\x08",8); memcpy(buf+8,"\xff\xfe\xfd\xfc\xfb\xfa\xf9\xf8",8); memcpy(buf+16,"\x40\x09\x21\xfb\x54\x44\x2d\x18",8); memcpy(buf+24,"\x40\x49\x0f\xdb",4); + (void)fputs("Test data:", stdout); + for (sp = buf; sp < buf + 28; sp++) + printf(" %02x", *sp); + putc('\n', stdout); + /* big-endian test */ - printf("Big-endian\n"); + printf("Big-endian:\n"); sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); @@ -114,7 +131,7 @@ int main(void) bedumpall(); /* little-endian test */ - printf("Little-endian\n"); + printf("Little-endian:\n"); sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); @@ -135,5 +152,16 @@ int main(void) d1 = getled(buf, 16); ledumpall(); + + (void)printf("Testing bitfield extraction:\n"); + for (up = unsigned_tests; + up < unsigned_tests+sizeof(unsigned_tests)/sizeof(unsigned_tests[0]); + up++) { + unsigned long long res = ubits((char *)buf, up->start, up->width); + (void)printf("ubits(..., %d, %d) should be %llu, is %llu: %s\n", + up->start, up->width, up->expected, res, + res == up->expected ? "succeeded" : "FAILED"); + } + exit(0); } diff --git a/rtcm3.c b/rtcm3.c index a1a4376c..f271cc5f 100644 --- a/rtcm3.c +++ b/rtcm3.c @@ -51,40 +51,6 @@ firmware. /* Other magic values */ #define INVALID_PSEUDORANGE 0x80000 /* DF012 */ -static unsigned long long ufld(char buf[], unsigned int start, unsigned int width) -/* extract a bitfield from the buffer as an unsigned big-endian long */ -{ - unsigned long long fld = 0; - unsigned int i;; - - assert(width <= 64); - for (i = 0; i < (width + 7) / 8; i++) { - fld <<= 8; - fld |= (unsigned char)buf[start / 8 + i]; - } - //printf("Extracting %d:%d from %s: segment 0x%llx = %lld\n", start, width, gpsd_hexdump(buf, 12), fld, fld); - - fld &= (0xffffffff >> (start % 8)); - //printf("After masking: 0x%llx = %lld\n", fld, fld); - fld >>= (start + width) % 8; - - return fld; -} - -static signed long long sfld(char buf[], unsigned int start, unsigned int width) -/* extract a bitfield from the buffer as a signed big-endian long */ -{ - unsigned long long un = ufld(buf, start, width); - signed long long fld; - - if (un & (1 << width)) - fld = -(un & ~(1 << width)); - else - fld = (signed long long)un; - - return fld; -} - void rtcm3_unpack(/*@out@*/struct rtcm3_t *rtcm, char *buf) /* break out the raw bits into the scaled report-structure fields */ { @@ -93,8 +59,8 @@ void rtcm3_unpack(/*@out@*/struct rtcm3_t *rtcm, char *buf) signed long temp; /*@ -evalorder -sefparams @*/ -#define ugrab(width) (bitcount += width, ufld(buf, bitcount-width, width)) -#define sgrab(width) (bitcount += width, sfld(buf, bitcount-width, width)) +#define ugrab(width) (bitcount += width, ubits(buf, bitcount-width, width)) +#define sgrab(width) (bitcount += width, sbits(buf, bitcount-width, width)) assert(ugrab(8) == 0xD3); assert(ugrab(6) == 0x00); -- cgit v1.2.1