From 39aa2ad719a87df8b426a925faded60ca34d017c Mon Sep 17 00:00:00 2001
From: Michal Morsisko <morsisko@gmail.com>
Date: Wed, 9 Aug 2023 21:14:16 +0200
Subject: [PATCH] lib: crc: Add CRC4

Add two new functions: crc4 for generic calculations
of CRC4, and crc4_ti which use look-up table for
faster calculations of CRC4 algortihms that base
on 0x03 polynomial.

Signed-off-by: Michal Morsisko <morsisko@gmail.com>
---
 include/zephyr/sys/crc.h | 41 +++++++++++++++++++++++++++++++
 lib/crc/CMakeLists.txt   |  1 +
 lib/crc/crc4_sw.c        | 53 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 95 insertions(+)
 create mode 100644 lib/crc/crc4_sw.c

diff --git a/include/zephyr/sys/crc.h b/include/zephyr/sys/crc.h
index 7c131f4cbfd..e29fce45d9e 100644
--- a/include/zephyr/sys/crc.h
+++ b/include/zephyr/sys/crc.h
@@ -47,6 +47,8 @@ extern "C" {
  * These values should be used with the @ref crc dispatch function.
  */
 enum crc_type {
+	CRC4,        /**< Use @ref crc4 */
+	CRC4_TI,     /**< Use @ref crc4_ti */
 	CRC7_BE,     /**< Use @ref crc7_be */
 	CRC8,	     /**< Use @ref crc8 */
 	CRC8_CCITT,  /**< Use @ref crc8_ccitt */
@@ -278,6 +280,41 @@ uint8_t crc8_ccitt(uint8_t initial_value, const void *buf, size_t len);
  */
 uint8_t crc7_be(uint8_t seed, const uint8_t *src, size_t len);
 
+/**
+ * @brief Compute the CRC-4 checksum of a buffer.
+ *
+ * Used by the TMAG5170 sensor. Uses 0x03 as the
+ * polynomial with no reflection. 4 most significant
+ * bits of the CRC result will be set to zero.
+ *
+ * @param seed Value to seed the CRC with
+ * @param src Input bytes for the computation
+ * @param len Length of the input in bytes
+ *
+ * @return The computed CRC4 value
+ */
+uint8_t crc4_ti(uint8_t seed, const uint8_t *src, size_t len);
+
+/**
+ * @brief Generic function for computing CRC 4
+ *
+ * Compute CRC 4 by passing in the address of the input, the input length
+ * and polynomial used in addition to the initial value. The input buffer
+ * must be aligned to a whole byte. It is guaranteed that 4 most significant
+ * bits of the result will be set to zero.
+ *
+ * @param src Input bytes for the computation
+ * @param len Length of the input in bytes
+ * @param polynomial The polynomial to use omitting the leading x^4
+ *        coefficient
+ * @param initial_value Initial value for the CRC computation
+ * @param reversed Should we use reflected/reversed values or not
+ *
+ * @return The computed CRC4 value
+ */
+uint8_t crc4(const uint8_t *src, size_t len, uint8_t polynomial, uint8_t initial_value,
+	  bool reversed);
+
 /**
  * @brief Compute a CRC checksum, in a generic way.
  *
@@ -305,6 +342,10 @@ static inline uint32_t crc_by_type(enum crc_type type, const uint8_t *src, size_
 				   bool last)
 {
 	switch (type) {
+	case CRC4:
+		return crc4(src, len, poly, seed, reflect);
+	case CRC4_TI:
+		return crc4_ti(seed, src, len);
 	case CRC7_BE:
 		return crc7_be(seed, src, len);
 	case CRC8:
diff --git a/lib/crc/CMakeLists.txt b/lib/crc/CMakeLists.txt
index bf652ba4fcf..a85cc3b05cc 100644
--- a/lib/crc/CMakeLists.txt
+++ b/lib/crc/CMakeLists.txt
@@ -6,5 +6,6 @@ zephyr_sources_ifdef(CONFIG_CRC
   crc16_sw.c
   crc8_sw.c
   crc7_sw.c
+  crc4_sw.c
   )
 zephyr_sources_ifdef(CONFIG_CRC_SHELL crc_shell.c)
diff --git a/lib/crc/crc4_sw.c b/lib/crc/crc4_sw.c
new file mode 100644
index 00000000000..e636a2c8fae
--- /dev/null
+++ b/lib/crc/crc4_sw.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2023 Michal Morsisko
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/sys/crc.h>
+
+uint8_t crc4(const uint8_t *src, size_t len, uint8_t polynomial, uint8_t initial_value,
+	  bool reversed)
+{
+	uint8_t crc = initial_value;
+	size_t i, j, k;
+
+	for (i = 0; i < len; i++) {
+		for (j = 0; j < 2; j++) {
+			crc ^= ((src[i] >> (4 * (1 - j))) & 0xf);
+
+			for (k = 0; k < 4; k++) {
+				if (reversed) {
+					if (crc & 0x01) {
+						crc = (crc >> 1) ^ polynomial;
+					} else {
+						crc >>= 1;
+					}
+				} else {
+					if (crc & 0x8) {
+						crc = (crc << 1) ^ polynomial;
+					} else {
+						crc <<= 1;
+					}
+				}
+			}
+		}
+	}
+
+	return crc & 0xF;
+}
+
+uint8_t crc4_ti(uint8_t seed, const uint8_t *src, size_t len)
+{
+	static const uint8_t lookup[8] = { 0x03, 0x65, 0xcf, 0xa9, 0xb8, 0xde, 0x74, 0x12 };
+	uint8_t index;
+
+	for (size_t i = 0; i < len; i++) {
+		for (size_t j = 0U; j < 2U; j++) {
+			index = seed ^ ((src[i] >> (4*(1-j))) & 0xf);
+			seed = (lookup[index >> 1] >> (1 - (index & 1)) * 4) & 0xf;
+		}
+	}
+
+	return seed;
+}