Merge pull request #21428 from benpicco/sys/bcd_buf_from_str

sys/bcd: add `bcd_buf_to_u32()` and `bcd_buf_from_str()`

Author: maribu

sys/bcd/bcd.c

@@ -56,3 +56,61 @@ int bcd_buf_from_u32(uint32_t val, void *dst, size_t len)
 
     return (uintptr_t)tgt - (uintptr_t)dst;
 }
+
+/* Use the same code for 32 bit and 64 bit sum */
+#define _BCD_CONVERT(sum, len)                  \
+    for (int i = len * 2 - 1; i >= 0; --i) {    \
+        uint8_t digit = i & 1                   \
+                      ? bcd[i >> 1] >> 4        \
+                      : bcd[i >> 1] & 0xF;      \
+        sum = sum * 10 + digit;                 \
+    }
+
+uint32_t bcd_buf_to_u32(const void *src, size_t len)
+{
+    const uint8_t *bcd = src;
+    uint32_t sum = 0;
+
+    _BCD_CONVERT(sum, len);
+
+    return sum;
+}
+
+uint64_t bcd_buf_to_u64(const void *src, size_t len)
+{
+    const uint8_t *bcd = src;
+    uint64_t sum = 0;
+
+    _BCD_CONVERT(sum, len);
+
+    return sum;
+}
+
+static bool _is_digit(char c)
+{
+    return c >= '0' && c <= '9';
+}
+
+int bcd_buf_from_str(const char *str, size_t len, void *dst, size_t dst_len)
+{
+    uint8_t *bcd = dst;
+    memset(dst, 0, dst_len);
+    dst_len *= 2;
+
+    uint8_t i = 0;
+    for (int j = len; j >= 0; --j) {
+        if (i == dst_len) {
+            return -ENOBUFS;
+        }
+        if (!_is_digit(str[j])) {
+            continue;
+        }
+        uint8_t d = str[j] & 0xF;
+        bcd[i >> 1] |= i & 1
+                    ? d << 4
+                    : d;
+        ++i;
+    }
+
+    return (i & 1) + (i >> 1);
+}

sys/include/bcd.h

@@ -16,10 +16,12 @@
  * @brief       BCD definitions
  *
  * @author  Martine Lenders <[email protected]>
+ * @author  Benjamin Valentin <[email protected]>
  */
 #ifndef BCD_H
 #define BCD_H
 
+#include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
 
@@ -64,11 +66,48 @@ static inline uint8_t bcd_to_byte(uint8_t bcd)
  * @param[in]  len  Size of the destination buffer
  *
  * @return number of bytes written
- * @return -ENOBUFS if @p dst is not large enough
+ * @retval -ENOBUFS if @p dst is not large enough
  *         In that case the state of @p dst is undefined.
  */
 int bcd_buf_from_u32(uint32_t val, void *dst, size_t len);
 
+/**
+ * @brief   Convert a BCD buffer into it's binary representation
+ *          (This will reverse @ref bcd_buf_from_u32)
+ *
+ * @param[in] src   The BCD buffer to convert
+ * @param[in] len   Bytes in @p src
+ *
+ * @returns decimal representation of @p src
+ */
+uint32_t bcd_buf_to_u32(const void *src, size_t len);
+
+/**
+ * @brief   Convert a BCD buffer into it's binary representation
+ *
+ * @param[in] src   The BCD buffer to convert
+ * @param[in] len   Bytes in @p src
+ *
+ * @returns decimal representation of @p src
+ */
+uint64_t bcd_buf_to_u64(const void *src, size_t len);
+
+/**
+ * @brief   Convert a string into a BCD buffer
+ *          Digits may be separated by any character.
+ *
+ * @param[in]  str      Input string
+ * @param[in]  str_len  Length of the input string
+ * @param[out] dst      Destination buffer
+ * @param[in]  dst_len  Size of the destination buffer
+ *
+ * @return number of bytes written
+ * @retval -ENOBUFS if @p dst is not large enough
+ *         In that case the state of @p dst is undefined.
+ */
+int bcd_buf_from_str(const char *str, size_t str_len,
+                     void *dst, size_t dst_len);
+
 #ifdef __cplusplus
 }
 #endif

tests/unittests/tests-bcd/tests-bcd.c

@@ -85,6 +85,42 @@ static void test_bcd_buf_from_u32(void)
     TEST_ASSERT_EQUAL_INT(-ENOBUFS, bcd_buf_from_u32(0, NULL, 0));
 }
 
+static void test_bcd_buf_to_u32(void)
+{
+    char buf[4];
+
+    bcd_buf_from_u32(1, buf, sizeof(buf));
+    TEST_ASSERT_EQUAL_INT(1, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    bcd_buf_from_u32(12, buf, sizeof(buf));
+    TEST_ASSERT_EQUAL_INT(12, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    bcd_buf_from_u32(123, buf, sizeof(buf));
+    TEST_ASSERT_EQUAL_INT(123, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    bcd_buf_from_u32(1234, buf, sizeof(buf));
+    TEST_ASSERT_EQUAL_INT(1234, bcd_buf_to_u32(buf, sizeof(buf)));
+}
+
+static void test_bcd_buf_from_str(void)
+{
+    uint8_t buf[4];
+
+    TEST_ASSERT_EQUAL_INT(1, bcd_buf_from_str("1", 1, buf, sizeof(buf)));
+    TEST_ASSERT_EQUAL_INT(1, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    TEST_ASSERT_EQUAL_INT(1, bcd_buf_from_str("12", 2, buf, sizeof(buf)));
+    TEST_ASSERT_EQUAL_INT(12, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    TEST_ASSERT_EQUAL_INT(2, bcd_buf_from_str("123", 3, buf, sizeof(buf)));
+    TEST_ASSERT_EQUAL_INT(123, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    TEST_ASSERT_EQUAL_INT(2, bcd_buf_from_str("1234", 4, buf, sizeof(buf)));
+    TEST_ASSERT_EQUAL_INT(1234, bcd_buf_to_u32(buf, sizeof(buf)));
+
+    TEST_ASSERT_EQUAL_INT(-ENOBUFS, bcd_buf_from_str("1234567890", 10, buf, sizeof(buf)));
+}
+
 Test *tests_bcd_tests(void)
 {
     EMB_UNIT_TESTFIXTURES(fixtures) {
@@ -95,6 +131,8 @@ Test *tests_bcd_tests(void)
         new_TestFixture(test_bcd_to_byte__greater_0x99),
         new_TestFixture(test_bcd_to_byte),
         new_TestFixture(test_bcd_buf_from_u32),
+        new_TestFixture(test_bcd_buf_to_u32),
+        new_TestFixture(test_bcd_buf_from_str),
     };
 
     EMB_UNIT_TESTCALLER(bcd_tests, NULL, NULL, fixtures);