2 files changed, 118 insertions, 0 deletions
diff --git a/src/bitfield/bitfield.c b/src/bitfield/bitfield.c
new file mode 100644
index 00000000..d383db04
--- /dev/null
+++ b/src/bitfield/bitfield.c
@@ -0,0 +1,60 @@
+#include <bitfield/bitfield.h>
+
+/**
+ * Find the ending bit of a bitfield within the final byte.
+ *
+ * Returns: a bit position from 0 to 7.
+ */
+int findEndBit(int startBit, int numBits) {
+    int endBit = (startBit + numBits) % 8;
+    return endBit == 0 ? 8 : endBit;
+}
+
+uint64_t bitmask(int numBits) {
+    return (((uint64_t)0x1) << numBits) - 1;
+}
+
+int startingByte(int startBit) {
+    return startBit / 8;
+}
+
+int endingByte(int startBit, int numBits) {
+    return (startBit + numBits - 1) / 8;
+}
+
+uint64_t getBitField(uint64_t data, int startBit, int numBits, bool bigEndian) {
+    int startByte = startingByte(startBit);
+    int endByte = endingByte(startBit, numBits);
+
+    if(!bigEndian) {
+        data = __builtin_bswap64(data);
+    }
+    uint8_t* bytes = (uint8_t*)&data;
+    uint64_t ret = bytes[startByte];
+    if(startByte != endByte) {
+        // The lowest byte address contains the most significant bit.
+        int i;
+        for(i = startByte + 1; i <= endByte; i++) {
+            ret = ret << 8;
+            ret = ret | bytes[i];
+        }
+    }
+
+    ret >>= 8 - findEndBit(startBit, numBits);
+    return ret & bitmask(numBits);
+}
+
+/**
+ * TODO it would be nice to have a warning if you call with this a value that
+ * won't fit in the number of bits you've specified it should use.
+ */
+void setBitField(uint64_t* data, uint64_t value, int startBit, int numBits) {
+    int shiftDistance = 64 - startBit - numBits;
+    value <<= shiftDistance;
+    *data &= ~(bitmask(numBits) << shiftDistance);
+    *data |= value;
+}
+
+uint8_t nthByte(uint64_t source, int byteNum) {
+    return (source >> (64 - ((byteNum + 1) * 8))) & 0xFF;
+}
diff --git a/src/bitfield/bitfield.h b/src/bitfield/bitfield.h
new file mode 100644
index 00000000..27766733
--- /dev/null
+++ b/src/bitfield/bitfield.h
@@ -0,0 +1,58 @@
+#ifndef __BITFIELD_H__
+#define __BITFIELD_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Public: Reads a subset of bits from a byte array.
+ *
+ * data - the bytes in question.
+ * startPos - the starting index of the bit field (beginning from 0).
+ * numBits - the width of the bit field to extract.
+ * bigEndian - if the data passed in is little endian, set this to false and it
+ *      will be flipped before grabbing the bit field.
+ *
+ * Bit fields are positioned according to big-endian bit layout, but inside the
+ * bit field, values are represented as little-endian. Therefore, to get the bit
+ * field, we swap the overall byte order if bigEndian == false and
+ * use the value we find in the field (assuming the embedded platform is little
+ * endian).
+ *
+ * For example, the bit layout of the value "42" (i.e. 00101010 set at position
+ * 14 with length 6 is:
+ *
+ *     000000000000001010100000000000000000000000000000000000000000000
+ *
+ * and the same value and position but with length 8 is:
+ *
+ *     000000000000000010101000000000000000000000000000000000000000000
+ *
+ * If the architecture where is code is running is little-endian, the input data
+ * will be swapped before grabbing the bit field.
+ *
+ * Examples
+ *
+ *  uint64_t value = getBitField(data, 2, 4);
+ *
+ * Returns the value of the requested bit field.
+ */
+uint64_t getBitField(uint64_t data, int startPos, int numBits, bool bigEndian);
+
+/* Public: Set the bit field in the given data array to the new value.
+ *
+ * data - a byte array with size at least startPos + numBits.
+ * value - the value to set in the bit field.
+ * startPos - the starting index of the bit field (beginning from 0).
+ */
+void setBitField(uint64_t* data, uint64_t value, int startPos, int numBits);
+
+/* Public: Retreive the nth byte out of 8 bytes in a uint64_t.
+ *
+ * source - the source data to retreive the byte from.
+ * byteNum - the index of the byte, starting at 0 and assuming big-endian order.
+ *
+ * Returns the requested byte from the source bytes.
+ */
+uint8_t nthByte(uint64_t source, int byteNum);
+
+#endif // __BITFIELD_H__