4 files changed, 565 insertions, 0 deletions
diff --git a/libs/bitfield-c/tests/8byte_tests.c b/libs/bitfield-c/tests/8byte_tests.c
new file mode 100644
index 00000000..64554acc
--- /dev/null
+++ b/libs/bitfield-c/tests/8byte_tests.c
@@ -0,0 +1,261 @@
+#include <check.h>
+#include <stdint.h>
+#include <bitfield/bitfield.h>
+#include <bitfield/8byte.h>
+
+START_TEST (test_large_bitmask)
+{
+    uint64_t result = bitmask(32);
+    fail_if(result != 0xffffffff);
+}
+END_TEST
+
+START_TEST (test_one_bit_not_swapped)
+{
+    uint64_t data = 0x80;
+    uint64_t result = eightbyte_get_bitfield(data, 0, 1, false);
+    fail_if(result == 1);
+}
+END_TEST
+
+START_TEST (test_one_bit)
+{
+    uint64_t data = 0x8000000000000000;
+    uint64_t result = eightbyte_get_bitfield(data, 0, 1, false);
+    fail_unless(result == 0x1,
+            "First bit in 0x%llx was 0x%llx instead of 0x1", data, result);
+}
+END_TEST
+
+START_TEST (test_32_bit_parse)
+{
+    uint64_t data = 0x0402574d555a0401;
+    uint64_t result = eightbyte_get_bitfield(data, 16, 32, false);
+    uint64_t expectedValue = 0x574d555a;
+    fail_unless(result == expectedValue,
+            "Field retrieved in 0x%llx was 0x%llx instead of 0x%llx", data,
+            result, expectedValue);
+}
+END_TEST
+
+START_TEST (test_16_bit_parse)
+{
+    uint64_t data = 0xF34DFCFF00000000;
+    uint64_t result = eightbyte_get_bitfield(data, 16, 16, false);
+    uint64_t expectedValue = 0xFCFF;
+    fail_unless(result == expectedValue,
+            "Field retrieved in 0x%llx was 0x%llx instead of 0x%llx", data,
+            result, expectedValue);
+}
+END_TEST
+
+START_TEST (test_one_byte)
+{
+    uint64_t data = 0xFA00000000000000;
+    uint64_t result = eightbyte_get_bitfield(data, 0, 4, false);
+    fail_unless(result == 0xF,
+            "First nibble in 0x%llx was 0x%llx instead of 0xF", data, result);
+    result = eightbyte_get_bitfield(data, 4, 4, false);
+    fail_unless(result == 0xA,
+            "Second nibble in 0x%llx was 0x%llx instead of 0xA", data, result);
+    result = eightbyte_get_bitfield(data, 0, 8, false);
+    fail_unless(result == 0xFA,
+            "All bits in 0x%llx were 0x%llx instead of 0x%llx", data, result, data);
+}
+END_TEST
+
+START_TEST (test_multi_byte)
+{
+    uint64_t data = 0x12FA000000000000;
+    uint64_t result = eightbyte_get_bitfield(data, 0, 4, false);
+    fail_unless(result == 0x1,
+            "First 4 bits in 0x%llx was 0x%llx instead of 0xF", (data >> 60) & 0xF,
+            result);
+    result = eightbyte_get_bitfield(data, 4, 4, false);
+    fail_unless(result == 0x2,
+            "Second 4 bits in 0x%llx was 0x%llx instead of 0xA", (data >> 56) & 0xF,
+            result);
+    result = eightbyte_get_bitfield(data, 8, 4, false);
+    fail_unless(result == 0xF,
+            "First 4 bits in 0x%llx was 0x%llx instead of 0x1", (data >> 52) & 0xF,
+            result);
+    result = eightbyte_get_bitfield(data, 12, 4, false);
+    fail_unless(result == 0xA,
+            "Second 4 bits in 0x%llx was 0x%llx instead of 0x2", (data >> 48) % 0xF,
+            result);
+}
+END_TEST
+
+START_TEST (test_get_multi_byte)
+{
+    uint64_t data = 0x12FA000000000000;
+    uint64_t result = eightbyte_get_bitfield(data, 0, 9, false);
+    ck_assert_int_eq(result, 0x25);
+}
+END_TEST
+
+START_TEST (test_get_off_byte_boundary)
+{
+    uint64_t data = 0x000012FA00000000;
+    uint64_t result = eightbyte_get_bitfield(data, 12, 8, false);
+    ck_assert_int_eq(result, 0x01);
+} END_TEST
+
+START_TEST (test_set_wont_fit)
+{
+    uint64_t data = 0;
+    fail_if(eightbyte_set_bitfield(100, 0, 1, &data));
+}
+END_TEST
+
+START_TEST (test_set_field)
+{
+    uint64_t data = 0;
+    fail_unless(eightbyte_set_bitfield(1, 0, 1, &data));
+    uint64_t result = eightbyte_get_bitfield(data, 0, 1, false);
+    ck_assert_int_eq(result, 0x1);
+    data = 0;
+    fail_unless(eightbyte_set_bitfield(1, 1, 1, &data));
+    result = eightbyte_get_bitfield(data, 1, 1, false);
+    ck_assert_int_eq(result, 0x1);
+
+    data = 0;
+    fail_unless(eightbyte_set_bitfield(0xf, 3, 4, &data));
+    result = eightbyte_get_bitfield(data, 3, 4, false);
+    ck_assert_int_eq(result, 0xf);
+}
+END_TEST
+
+START_TEST (test_set_doesnt_clobber_existing_data)
+{
+    uint64_t data = 0xFFFC4DF300000000;
+    fail_unless(eightbyte_set_bitfield(0x4fc8, 16, 16, &data));
+    uint64_t result = eightbyte_get_bitfield(data, 16, 16, false);
+    fail_unless(result == 0x4fc8,
+            "Field retrieved in 0x%llx was 0x%llx instead of 0x%x", data, result,
+            0xc84f);
+
+    data = 0x8000000000000000;
+    fail_unless(eightbyte_set_bitfield(1, 21, 1, &data));
+    fail_unless(data == 0x8000040000000000LLU,
+            "Expected combined value 0x8000040000000000 but got 0x%llx%llx",
+            data >> 32, data);
+}
+END_TEST
+
+START_TEST (test_set_off_byte_boundary)
+{
+    uint64_t data = 0xFFFC4DF300000000;
+    fail_unless(eightbyte_set_bitfield(0x12, 12, 8, &data));
+    uint64_t result = eightbyte_get_bitfield(data, 12, 12, false);
+    ck_assert_int_eq(result,0x12d);
+}
+END_TEST
+
+START_TEST (test_set_odd_number_of_bits)
+{
+    uint64_t data = 0xFFFC4DF300000000LLU;
+    fail_unless(eightbyte_set_bitfield(0x12, 11, 5, &data));
+    uint64_t result = eightbyte_get_bitfield(data, 11, 5, false);
+    fail_unless(result == 0x12,
+            "Field set in 0x%llx%llx%llx%llx was 0x%llx instead of 0x%llx", data, result,
+            0x12);
+
+    data = 0xFFFC4DF300000000LLU;
+    fail_unless(eightbyte_set_bitfield(0x2, 11, 5, &data));
+    result = eightbyte_get_bitfield(data, 11, 5, false);
+    fail_unless(result == 0x2,
+            "Field set in 0x%llx%llx%llx%llx was 0x%llx instead of 0x%llx", data, result,
+            0x2);
+}
+END_TEST
+
+START_TEST(test_eightbyte_get_byte)
+{
+    uint64_t data = 0x00000000F34DFCFF;
+    uint8_t result = eightbyte_get_byte(data, 0, false);
+    uint8_t expected = 0x0;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_byte(data, 4, false);
+    expected = 0xF3;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_byte(data, 5, false);
+    expected = 0x4D;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_byte(data, 6, false);
+    expected = 0xFC;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_byte(data, 7, false);
+    expected = 0xFF;
+    ck_assert_int_eq(result, expected);
+}
+END_TEST
+
+START_TEST(test_eightbyte_get_nibble)
+{
+    uint64_t data = 0x00000000F34DFCFF;
+    uint8_t result = eightbyte_get_nibble(data, 0, false);
+    uint8_t expected = 0x0;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_nibble(data, 2, false);
+    expected = 0x0;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_nibble(data, 8, false);
+    expected = 0xF;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_nibble(data, 9, false);
+    expected = 0x3;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_nibble(data, 10, false);
+    expected = 0x4;
+    ck_assert_int_eq(result, expected);
+
+    result = eightbyte_get_nibble(data, 13, false);
+    expected = 0xC;
+    ck_assert_int_eq(result, expected);
+}
+END_TEST
+
+Suite* bitfieldSuite(void) {
+    Suite* s = suite_create("bitfield");
+    TCase *tc_core = tcase_create("core");
+    tcase_add_test(tc_core, test_large_bitmask);
+    tcase_add_test(tc_core, test_one_bit);
+    tcase_add_test(tc_core, test_one_bit_not_swapped);
+    tcase_add_test(tc_core, test_one_byte);
+    tcase_add_test(tc_core, test_16_bit_parse);
+    tcase_add_test(tc_core, test_32_bit_parse);
+    tcase_add_test(tc_core, test_multi_byte);
+    tcase_add_test(tc_core, test_get_multi_byte);
+    tcase_add_test(tc_core, test_get_off_byte_boundary);
+    tcase_add_test(tc_core, test_set_wont_fit);
+    tcase_add_test(tc_core, test_set_field);
+    tcase_add_test(tc_core, test_set_doesnt_clobber_existing_data);
+    tcase_add_test(tc_core, test_set_off_byte_boundary);
+    tcase_add_test(tc_core, test_set_odd_number_of_bits);
+    tcase_add_test(tc_core, test_eightbyte_get_nibble);
+    tcase_add_test(tc_core, test_eightbyte_get_byte);
+    suite_add_tcase(s, tc_core);
+
+    return s;
+}
+
+int main(void) {
+    int numberFailed;
+    Suite* s = bitfieldSuite();
+    SRunner *sr = srunner_create(s);
+    // Don't fork so we can actually use gdb
+    srunner_set_fork_status(sr, CK_NOFORK);
+    srunner_run_all(sr, CK_NORMAL);
+    numberFailed = srunner_ntests_failed(sr);
+    srunner_free(sr);
+    return (numberFailed == 0) ? 0 : 1;
+}
diff --git a/libs/bitfield-c/tests/bitfield_tests.c b/libs/bitfield-c/tests/bitfield_tests.c
new file mode 100644
index 00000000..b8c83b59
--- /dev/null
+++ b/libs/bitfield-c/tests/bitfield_tests.c
@@ -0,0 +1,132 @@
+#include <check.h>
+#include <stdint.h>
+#include <bitfield/bitfield.h>
+
+START_TEST (test_get_byte)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result = get_byte(data, sizeof(data), 0);
+    ck_assert_int_eq(result, 0x12);
+    result = get_byte(data, sizeof(data), 3);
+    ck_assert_int_eq(result, 0x78);
+}
+END_TEST
+
+START_TEST (test_set_nibble)
+{
+    uint8_t data[4] = {0};
+    fail_unless(set_nibble(0, 0x1, data, sizeof(data)));
+    fail_unless(set_nibble(1, 0x2, data, sizeof(data)));
+    fail_unless(set_nibble(2, 0x3, data, sizeof(data)));
+    fail_unless(set_nibble(3, 0x4, data, sizeof(data)));
+    fail_unless(set_nibble(4, 0x5, data, sizeof(data)));
+    ck_assert_int_eq(data[0], 0x12);
+    ck_assert_int_eq(data[1], 0x34);
+    ck_assert_int_eq(data[2], 0x50);
+}
+END_TEST
+
+START_TEST (test_set_bitfield)
+{
+    uint8_t data[4] = {0};
+    fail_unless(set_bitfield(0x12, 0, 8, data, sizeof(data)));
+    fail_unless(set_bitfield(bitmask(3), 10, 3, data, sizeof(data)));
+    ck_assert_int_eq(data[0], 0x12);
+    ck_assert_int_eq(data[1], 0x38);
+}
+END_TEST
+
+START_TEST (test_set_bitfield_doesnt_fit)
+{
+    uint8_t data[4] = {0};
+    fail_if(set_bitfield(0xffff, 0, 8, data, sizeof(data)));
+    ck_assert_int_eq(data[0], 0);
+    ck_assert_int_eq(data[1], 0);
+    ck_assert_int_eq(data[2], 0);
+    ck_assert_int_eq(data[3], 0);
+}
+END_TEST
+
+START_TEST (test_get_nibble)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result = get_nibble(data, sizeof(data), 0);
+    ck_assert_int_eq(result, 0x1);
+    result = get_nibble(data, sizeof(data), 1);
+    ck_assert_int_eq(result, 0x2);
+    result = get_nibble(data, sizeof(data), 2);
+    ck_assert_int_eq(result, 0x3);
+}
+END_TEST
+
+START_TEST (test_get_bits_out_of_range)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result[4];
+    fail_if(copy_bits_right_aligned(data, sizeof(data), 25, 16, result,
+                sizeof(result)));
+}
+END_TEST
+
+START_TEST (test_get_bits)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result[4] = {0};
+    fail_unless(copy_bits_right_aligned(data, sizeof(data), 0, 16, result,
+                sizeof(result)));
+    ck_assert_int_eq(result[2], 0x12);
+    ck_assert_int_eq(result[3], 0x34);
+}
+END_TEST
+
+START_TEST (test_copy_bytes)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result[4] = {0};
+    fail_unless(copy_bytes_right_aligned(data, sizeof(data), 1, 3, result,
+                sizeof(result)));
+    ck_assert_int_eq(result[1], 0x34);
+    ck_assert_int_eq(result[2], 0x56);
+    ck_assert_int_eq(result[3], 0x78);
+}
+END_TEST
+
+START_TEST (test_get_uneven_bits)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result[4] = {0};
+    fail_unless(copy_bits_right_aligned(data, sizeof(data), 4, 12, result,
+                sizeof(result)));
+    ck_assert_int_eq(result[2], 0x2);
+    ck_assert_int_eq(result[3], 0x34);
+}
+END_TEST
+
+Suite* bitfieldSuite(void) {
+    Suite* s = suite_create("bitfield");
+    TCase *tc_core = tcase_create("core");
+    tcase_add_test(tc_core, test_get_byte);
+    tcase_add_test(tc_core, test_get_nibble);
+    tcase_add_test(tc_core, test_set_nibble);
+    tcase_add_test(tc_core, test_set_bitfield);
+    tcase_add_test(tc_core, test_set_bitfield_doesnt_fit);
+    tcase_add_test(tc_core, test_get_bits);
+    tcase_add_test(tc_core, test_copy_bytes);
+    tcase_add_test(tc_core, test_get_bits_out_of_range);
+    tcase_add_test(tc_core, test_get_uneven_bits);
+    suite_add_tcase(s, tc_core);
+
+    return s;
+}
+
+int main(void) {
+    int numberFailed;
+    Suite* s = bitfieldSuite();
+    SRunner *sr = srunner_create(s);
+    // Don't fork so we can actually use gdb
+    srunner_set_fork_status(sr, CK_NOFORK);
+    srunner_run_all(sr, CK_NORMAL);
+    numberFailed = srunner_ntests_failed(sr);
+    srunner_free(sr);
+    return (numberFailed == 0) ? 0 : 1;
+}
diff --git a/libs/bitfield-c/tests/read_tests.c b/libs/bitfield-c/tests/read_tests.c
new file mode 100644
index 00000000..5008cc52
--- /dev/null
+++ b/libs/bitfield-c/tests/read_tests.c
@@ -0,0 +1,67 @@
+#include <check.h>
+#include <stdint.h>
+#include <canutil/read.h>
+
+const uint64_t BIG_ENDIAN_TEST_DATA = __builtin_bswap64(0xEB00000000000000);
+const uint8_t ARRAY_TEST_DATA[] = {0xEB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+START_TEST (test_eightbyte_parse_float)
+{
+    float result = eightbyte_parse_float(BIG_ENDIAN_TEST_DATA, 2, 4, 1001.0,
+            -30000.0);
+    float correctResult = 0xA * 1001.0 - 30000.0;
+    fail_unless(result == correctResult,
+            "parse is incorrect: %f but should be %f", result, correctResult);
+}
+END_TEST
+
+START_TEST (test_eightbyte_parse_bool)
+{
+    bool result = eightbyte_parse_bool(BIG_ENDIAN_TEST_DATA, 0, 1, 1.0, 0);
+    bool correctResult = true;
+    fail_unless(result == correctResult,
+            "parse is incorrect: %d but should be %d", result, correctResult);
+}
+END_TEST
+
+START_TEST (test_bitfield_parse_float)
+{
+    float result = bitfield_parse_float(ARRAY_TEST_DATA,
+            sizeof(ARRAY_TEST_DATA), 2, 4, 1001.0, -30000.0);
+    float correctResult = 0xA * 1001.0 - 30000.0;
+    fail_unless(result == correctResult,
+            "parse is incorrect: %f but should be %f", result, correctResult);
+}
+END_TEST
+
+START_TEST (test_bitfield_parse_bool)
+{
+    fail_unless(bitfield_parse_bool(ARRAY_TEST_DATA, sizeof(ARRAY_TEST_DATA),
+            0, 1, 1.0, 0));
+}
+END_TEST
+
+Suite* canreadSuite(void) {
+    Suite* s = suite_create("read");
+    TCase *tc_core = tcase_create("core");
+    tcase_add_checked_fixture(tc_core, NULL, NULL);
+    tcase_add_test(tc_core, test_eightbyte_parse_float);
+    tcase_add_test(tc_core, test_eightbyte_parse_bool);
+    tcase_add_test(tc_core, test_bitfield_parse_float);
+    tcase_add_test(tc_core, test_bitfield_parse_bool);
+    suite_add_tcase(s, tc_core);
+
+    return s;
+}
+
+int main(void) {
+    int numberFailed;
+    Suite* s = canreadSuite();
+    SRunner *sr = srunner_create(s);
+    // Don't fork so we can actually use gdb
+    srunner_set_fork_status(sr, CK_NOFORK);
+    srunner_run_all(sr, CK_NORMAL);
+    numberFailed = srunner_ntests_failed(sr);
+    srunner_free(sr);
+    return (numberFailed == 0) ? 0 : 1;
+}
diff --git a/libs/bitfield-c/tests/write_tests.c b/libs/bitfield-c/tests/write_tests.c
new file mode 100644
index 00000000..4d5d8fc3
--- /dev/null
+++ b/libs/bitfield-c/tests/write_tests.c
@@ -0,0 +1,105 @@
+#include <canutil/write.h>
+#include <check.h>
+#include <stdint.h>
+
+START_TEST (test_eightbyte_encode_float_precision)
+{
+    uint64_t value = eightbyte_encode_float(50, 2, 19, 0.001, 0);
+    ck_assert_int_eq(value, 0x061a800000000000LLU);
+}
+END_TEST
+
+START_TEST (test_eightbyte_encode_float)
+{
+    uint64_t value = eightbyte_encode_float(0, 1, 3, 1, 0);
+    ck_assert_int_eq(value, 0);
+
+    value = eightbyte_encode_float(1, 1, 3, 1, 0);
+    ck_assert_int_eq(value, 0x1000000000000000LLU);
+}
+END_TEST
+
+START_TEST (test_eightbyte_encode_bool)
+{
+    uint64_t value = eightbyte_encode_bool(true, 1, 3);
+    ck_assert_int_eq(value, 0x1000000000000000LLU);
+    value = eightbyte_encode_bool(false, 1, 3);
+    ck_assert_int_eq(value, 0x0000000000000000LLU);
+}
+END_TEST
+
+START_TEST (test_bitfield_encode_float)
+{
+    uint8_t data[8] = {0};
+    bitfield_encode_float(0, 1, 3, 1, 0, data, sizeof(data));
+    ck_assert_int_eq(data[0], 0);
+    ck_assert_int_eq(data[1], 0);
+    ck_assert_int_eq(data[2], 0);
+    ck_assert_int_eq(data[3], 0);
+    ck_assert_int_eq(data[4], 0);
+    ck_assert_int_eq(data[5], 0);
+    ck_assert_int_eq(data[6], 0);
+    ck_assert_int_eq(data[7], 0);
+
+    bitfield_encode_float(1, 1, 3, 1, 0, data, sizeof(data));
+    ck_assert_int_eq(data[0], 0x10);
+    ck_assert_int_eq(data[1], 0);
+    ck_assert_int_eq(data[2], 0);
+    ck_assert_int_eq(data[3], 0);
+    ck_assert_int_eq(data[4], 0);
+    ck_assert_int_eq(data[5], 0);
+    ck_assert_int_eq(data[6], 0);
+    ck_assert_int_eq(data[7], 0);
+}
+END_TEST
+
+START_TEST (test_bitfield_encode_bool)
+{
+    uint8_t data[8] = {0};
+    bitfield_encode_bool(true, 1, 3, data, sizeof(data));
+    ck_assert_int_eq(data[0], 0x10);
+    ck_assert_int_eq(data[1], 0);
+    ck_assert_int_eq(data[2], 0);
+    ck_assert_int_eq(data[3], 0);
+    ck_assert_int_eq(data[4], 0);
+    ck_assert_int_eq(data[5], 0);
+    ck_assert_int_eq(data[6], 0);
+    ck_assert_int_eq(data[7], 0);
+
+    bitfield_encode_bool(false, 1, 3, data, sizeof(data));
+    ck_assert_int_eq(data[0], 0);
+    ck_assert_int_eq(data[1], 0);
+    ck_assert_int_eq(data[2], 0);
+    ck_assert_int_eq(data[3], 0);
+    ck_assert_int_eq(data[4], 0);
+    ck_assert_int_eq(data[5], 0);
+    ck_assert_int_eq(data[6], 0);
+    ck_assert_int_eq(data[7], 0);
+}
+END_TEST
+
+Suite* canwriteSuite(void) {
+    Suite* s = suite_create("write");
+    TCase *tc_core = tcase_create("core");
+    tcase_add_checked_fixture(tc_core, NULL, NULL);
+    tcase_add_test(tc_core, test_eightbyte_encode_float);
+    tcase_add_test(tc_core, test_eightbyte_encode_bool);
+    tcase_add_test(tc_core, test_eightbyte_encode_float_precision);
+    tcase_add_test(tc_core, test_bitfield_encode_float);
+    tcase_add_test(tc_core, test_bitfield_encode_bool);
+    suite_add_tcase(s, tc_core);
+
+    return s;
+}
+
+int main(void) {
+    int numberFailed;
+    Suite* s = canwriteSuite();
+    SRunner *sr = srunner_create(s);
+    // Don't fork so we can actually use gdb
+    srunner_set_fork_status(sr, CK_NOFORK);
+    srunner_run_all(sr, CK_NORMAL);
+    numberFailed = srunner_ntests_failed(sr);
+    srunner_free(sr);
+    return (numberFailed == 0) ? 0 : 1;
+}