Factor out the bitfield functions, fix a bug in one, add tests.

5 files changed, 102 insertions, 41 deletions

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 <assert.h>
+
+#include "bits.h"
+#ifdef DEBUG
+#include <stdlib.h>
+#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 <stdint.h>
 
 #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);