From 802a6ed2caff69a4f881f109462931de4f4f2d40 Mon Sep 17 00:00:00 2001
From: GuEe-GUI <2991707448@qq.com>
Date: Thu, 30 May 2024 17:30:03 +0800
Subject: [PATCH] [DM] Extended drivers MISC interface

1. RT_FIELD_PREP: prepare a bitfield element.
2. RT_FIELD_GET: extract a bitfield element.
3. rt_offsetof: member offset of a struct
4. rt_upper_32_bits: high 32 bits of value.
5. rt_lower_32_bits: lower 32 bits of value.
6. rt_upper_16_bits: high 16 bits of value.
7. rt_lower_16_bits: lower 16 bits of value.
8. rt_max_t: fix type of max(...).
9. rt_ilog2: integer logarithm base 2.

Signed-off-by: GuEe-GUI <2991707448@qq.com>
---
 components/drivers/include/drivers/misc.h | 32 +++++++++++++++++++++++
 include/rtthread.h                        |  1 +
 libcpu/aarch64/common/cpuport.c           | 18 ++++++++++++-
 3 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/components/drivers/include/drivers/misc.h b/components/drivers/include/drivers/misc.h
index aa2b715512..1e017d8be2 100644
--- a/components/drivers/include/drivers/misc.h
+++ b/components/drivers/include/drivers/misc.h
@@ -12,6 +12,7 @@
 #define __MISC_H__
 
 #include <rtdef.h>
+#include <cpuport.h>
 
 #ifdef ARCH_CPU_64BIT
 #define RT_BITS_PER_LONG 64
@@ -33,6 +34,9 @@
             (((__x) - ((__d) / 2)) / (__d));    \
 })
 
+#define RT_FIELD_PREP(mask, val)    (((rt_uint64_t)(val) << (__rt_ffsl((mask)) - 1)) & (mask))
+#define RT_FIELD_GET(mask, val)     (((val) & (mask)) >> (__rt_ffsl((mask)) - 1))
+
 #define RT_BIT(n)               (1UL << (n))
 #define RT_BIT_ULL(n)           (1ULL << (n))
 #define RT_BIT_MASK(nr)         (1UL << ((nr) % RT_BITS_PER_LONG))
@@ -48,6 +52,13 @@
 
 #define RT_ARRAY_SIZE(arr)      (sizeof(arr) / sizeof(arr[0]))
 
+#define rt_offsetof(s, field)   ((rt_size_t)&((s *)0)->field)
+
+#define rt_upper_32_bits(n)     ((rt_uint32_t)(((n) >> 16) >> 16))
+#define rt_lower_32_bits(n)     ((rt_uint32_t)((n) & 0xffffffff))
+#define rt_upper_16_bits(n)     ((rt_uint16_t)((n) >> 16))
+#define rt_lower_16_bits(n)     ((rt_uint16_t)((n) & 0xffff))
+
 #define rt_min(x, y)            \
 ({                              \
     typeof(x) _x = (x);         \
@@ -71,6 +82,13 @@
     _x < _y ? _x: _y;           \
 })
 
+#define rt_max_t(type, x, y)    \
+({                              \
+    type _x = (x);              \
+    type _y = (y);              \
+    _x > _y ? _x: _y;           \
+})
+
 #define rt_clamp(val, lo, hi)   rt_min((typeof(val))rt_max(val, lo), hi)
 
 #define rt_do_div(n, base)              \
@@ -83,4 +101,18 @@
     _rem;                               \
 })
 
+#ifndef rt_ilog2
+rt_inline int rt_ilog2(rt_ubase_t v)
+{
+    int l = 0;
+
+    while ((1UL << l) < v)
+    {
+        l++;
+    }
+
+    return l;
+}
+#endif /* !rt_ilog2 */
+
 #endif /* __MISC_H__ */
diff --git a/include/rtthread.h b/include/rtthread.h
index dc33a6c835..f28db560d3 100644
--- a/include/rtthread.h
+++ b/include/rtthread.h
@@ -787,6 +787,7 @@ rt_device_t rt_console_get_device(void);
 #endif /* defined(RT_USING_DEVICE) && defined(RT_USING_CONSOLE) */
 
 int __rt_ffs(int value);
+unsigned long __rt_ffsl(unsigned long value);
 
 void rt_show_version(void);
 
diff --git a/libcpu/aarch64/common/cpuport.c b/libcpu/aarch64/common/cpuport.c
index 4c1bf01cf3..b2592c854d 100644
--- a/libcpu/aarch64/common/cpuport.c
+++ b/libcpu/aarch64/common/cpuport.c
@@ -40,4 +40,20 @@ int __rt_ffs(int value)
 #endif
 }
 
-#endif /* RT_USING_CPU_FFS */
\ No newline at end of file
+unsigned long __rt_ffsl(unsigned long value)
+{
+#ifdef __GNUC__
+    return __builtin_ffsl(value);
+#else
+    if (!value)
+    {
+        return 0;
+    }
+
+    __asm__ volatile ("rbit %0, %0" : "+r" (value));
+
+    return __rt_clz(value);
+#endif
+}
+
+#endif /* RT_USING_CPU_FFS */