diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt
index 94c9b92..88c8e2f 100644
--- a/lib/CMakeLists.txt
+++ b/lib/CMakeLists.txt
@@ -22,10 +22,16 @@ endif()
 install(TARGETS vanilla)
 
 if (VANILLA_BUILD_TESTS)
-    add_executable(bittest
-        test/bittest.c
-    )
+    function(add_test TEST_NAME TEST_FILES)
+        add_executable(${TEST_NAME}
+            ${TEST_FILES}
+        )
 
-    target_link_libraries(bittest PRIVATE vanilla m)
-    target_include_directories(bittest PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+        target_link_libraries(${TEST_NAME} PRIVATE vanilla m)
+        target_include_directories(${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+    endfunction()
+    
+    add_test(bittest "test/bittest.c")
+    add_test(reversebittest "test/reversebit.c")
+    add_test(reversebitstresstest "test/reversebitstresstest.c")
 endif()
\ No newline at end of file
diff --git a/lib/test/reversebit.c b/lib/test/reversebit.c
new file mode 100644
index 0000000..611ae46
--- /dev/null
+++ b/lib/test/reversebit.c
@@ -0,0 +1,70 @@
+/**
+ * Small unit test for ensuring our SPS/PPS bit writing functions are accurate
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include "util.h"
+
+int test1()
+{
+    const unsigned int control = 0b0000001111001100;
+    const unsigned int expected = 0b0000000011001111;
+    
+    unsigned int test = reverse_bits(control, 10);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    printf("SUCCESS\n");
+    return 0;
+}
+
+int test2()
+{
+    const unsigned int control = 0b10110;
+    const unsigned int expected = 0b01101;
+    
+    unsigned int test = reverse_bits(control, 5);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    printf("SUCCESS\n");
+    return 0;
+}
+
+int test3()
+{
+    const unsigned int control = 0b110011101010011;
+    const unsigned int expected = 0b110010101110011;
+    
+    unsigned int test = reverse_bits(control, 15);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    printf("SUCCESS\n");
+    return 0;
+}
+
+int main()
+{
+    if (test1()) {
+        return 1;
+    }
+
+    if (test2()) {
+        return 1;
+    }
+
+    if (test3()) {
+        return 1;
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/lib/test/reversebitstresstest.c b/lib/test/reversebitstresstest.c
new file mode 100644
index 0000000..63b093e
--- /dev/null
+++ b/lib/test/reversebitstresstest.c
@@ -0,0 +1,80 @@
+/**
+ * Small unit test for ensuring our SPS/PPS bit writing functions are accurate
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <sys/time.h>
+
+#include "util.h"
+
+int test1()
+{
+    const unsigned int control = 0b0000001111001100;
+    const unsigned int expected = 0b0000000011001111;
+    
+    unsigned int test = reverse_bits(control, 10);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    return 0;
+}
+
+int test2()
+{
+    const unsigned int control = 0b10110;
+    const unsigned int expected = 0b01101;
+    
+    unsigned int test = reverse_bits(control, 5);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    return 0;
+}
+
+int test3()
+{
+    const unsigned int control = 0b110011101010011;
+    const unsigned int expected = 0b110010101110011;
+    
+    unsigned int test = reverse_bits(control, 15);
+    if (test != expected) {
+        printf("FAIL (got %x, expected %x)\n", test, expected);
+        return 1;
+    }
+
+    return 0;
+}
+
+int main()
+{
+    struct timeval tv_start, tv_end;
+
+    gettimeofday(&tv_start, NULL);
+
+    for (size_t i = 0; i < 100000000; i++) {
+        if (test1()) {
+            return 1;
+        }
+
+        if (test2()) {
+            return 1;
+        }
+
+        if (test3()) {
+            return 1;
+        }
+    }
+
+    gettimeofday(&tv_end, NULL);
+
+    time_t t = (tv_end.tv_sec * 1000000 + tv_end.tv_usec) - (tv_start.tv_sec * 1000000 + tv_start.tv_usec);
+
+    printf("Took %lu microseconds\n", t);
+
+    return 0;
+}
\ No newline at end of file
diff --git a/lib/util.c b/lib/util.c
index 53c92ed..24e25d6 100644
--- a/lib/util.c
+++ b/lib/util.c
@@ -83,13 +83,19 @@ size_t get_millis()
     return (s * 1000) + ms;
 }
 
-unsigned int reverse_bits(unsigned int b, int bit_count)
+uint32_t reverse_bits(uint32_t b, int bit_count)
 {
-    unsigned int result = 0;
+    uint32_t mask = 0b11111111111111110000000000000000;
+    b = (b & mask) >> 16 | (b & ~mask) << 16;
+    mask = 0b11111111000000001111111100000000;
+    b = (b & mask) >> 8 | (b & ~mask) << 8;
+    mask = 0b11110000111100001111000011110000;
+    b = (b & mask) >> 4 | (b & ~mask) << 4;
+    mask = 0b11001100110011001100110011001100;
+    b = (b & mask) >> 2 | (b & ~mask) << 2;
+    mask = 0b10101010101010101010101010101010;
+    b = (b & mask) >> 1 | (b & ~mask) << 1;
 
-    for (int i = 0; i < bit_count; i++) {
-        result |= ((b >> i) & 1) << (bit_count - 1 -i );
-    }
-
-    return result;
+    b >>= 32 - bit_count;
+    return b;
 }
\ No newline at end of file