From 2d02bc039429ba43765c0d55b1ee6bf4f25e5d9a Mon Sep 17 00:00:00 2001
From: Alexandre Abadie <alexandre.abadie@inria.fr>
Date: Wed, 10 Jan 2018 10:16:34 +0100
Subject: [PATCH] sys/fmt: add new byte to hex converters:

- fmt_bytes_hex: converts an array of bytes to an array of hex bytes
- fmt_hex_bytes: convert an array of hex bytes to an array of bytes
---
 sys/fmt/fmt.c     | 45 +++++++++++++++++++++++++++++++++++++++++++++
 sys/include/fmt.h | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 77 insertions(+)

diff --git a/sys/fmt/fmt.c b/sys/fmt/fmt.c
index a921f287c4..b2aadef8b9 100644
--- a/sys/fmt/fmt.c
+++ b/sys/fmt/fmt.c
@@ -62,6 +62,18 @@ size_t fmt_byte_hex(char *out, uint8_t byte)
     return 2;
 }
 
+size_t fmt_bytes_hex(char *out, const uint8_t *ptr, size_t n)
+{
+    size_t len = n * 2;
+    if (out) {
+        while (n--) {
+            out += fmt_byte_hex(out, *ptr++);
+        }
+    }
+
+    return len;
+}
+
 size_t fmt_strlen(const char *str)
 {
     const char *tmp = str;
@@ -95,6 +107,39 @@ size_t fmt_bytes_hex_reverse(char *out, const uint8_t *ptr, size_t n)
     return (n<<1);
 }
 
+static uint8_t _byte_mod25(uint8_t x)
+{
+    for (unsigned divisor = 200; divisor >= 25; divisor >>= 1) {
+        if (x >= divisor) {
+            x -= divisor;
+        }
+    }
+
+    return x;
+}
+
+static uint8_t _hex_nib(uint8_t nib)
+{
+    return _byte_mod25((nib & 0x1f) + 9);
+}
+
+size_t fmt_hex_bytes(uint8_t *out, const char *hex)
+{
+    size_t len = fmt_strlen(hex);
+
+    if (len & 1) {
+        out = NULL;
+        return 0;
+    }
+
+    size_t final_len = len >> 1;
+    for (size_t i = 0, j = 0; j < final_len; i += 2, j++) {
+        out[j] = (_hex_nib(hex[i]) << 4) | _hex_nib(hex[i+1]);
+    }
+
+    return final_len;
+}
+
 size_t fmt_u32_hex(char *out, uint32_t val)
 {
     return fmt_bytes_hex_reverse(out, (uint8_t*) &val, 4);
diff --git a/sys/include/fmt.h b/sys/include/fmt.h
index e1d0dfb38c..ceb98a5cf2 100644
--- a/sys/include/fmt.h
+++ b/sys/include/fmt.h
@@ -61,6 +61,21 @@ extern "C" {
  */
 size_t fmt_byte_hex(char *out, uint8_t byte);
 
+/**
+ * @brief Formats a sequence of bytes as hex bytes
+ *
+ * Will write 2*n bytes to @p out.
+ * If @p out is NULL, will only return the number of bytes that would have
+ * been written.
+ *
+ * @param[out] out  Pointer to output buffer, or NULL
+ * @param[in]  ptr  Pointer to bytes to convert
+ * @param[in]  n    Number of bytes to convert
+ *
+ * @return     2*n
+ */
+size_t fmt_bytes_hex(char *out, const uint8_t *ptr, size_t n);
+
 /**
  * @brief Formats a sequence of bytes as hex bytes, starting with the last byte
  *
@@ -76,6 +91,23 @@ size_t fmt_byte_hex(char *out, uint8_t byte);
  */
 size_t fmt_bytes_hex_reverse(char *out, const uint8_t *ptr, size_t n);
 
+/**
+ * @brief Converts a sequence of hex bytes to an array of bytes
+ *
+ * The sequence of hex characters must have an even length:
+ * 2 hex character => 1 byte. If the sequence of hex has an odd length, this
+ * function returns 0 and an empty @p out.
+ *
+ * The hex characters sequence must contain valid hexadecimal characters
+ * otherwise the result in @p out is undefined.
+ *
+ * @param[out] out  Pointer to converted bytes, or NULL
+ * @param[in]  hex  Pointer to input buffer
+ * @returns    strlen(hex) / 2 when length of @p hex was even
+ * @returns    0 otherwise
+ */
+size_t fmt_hex_bytes(uint8_t *out, const char *hex);
+
 /**
  * @brief Convert a uint32 value to hex string.
  *
-- 
GitLab