util: Add io32_memcpy, a memcpy that copies one word at a time

Implement memcpy32, a version of memcpy that only performs 32 bit
word-wise copying.

Based on Pete Dietl's and the mcan implementation.

Signed-off-by: Sebastian Bøe <sebastian.boe@nordicsemi.no>

Author: SebastianBoe

include/zephyr/sys/util.h

@@ -498,6 +498,26 @@ char *utf8_trunc(char *utf8_str);
  */
 char *utf8_lcpy(char *dst, const char *src, size_t n);
 
+/**
+ * @brief Copy src to dest word-wise
+ *
+ * This function copies src to dest 32 bits at a time.
+ * If one calls this function without src and dst being aligned
+ * on a 4-byte boundary or if n is not a multiple of 4, it will
+ * do nothing and return NULL.
+ *
+ * NOTE: This function uses __ASSERT to enforce the requirements
+ * of each parameter given below.
+ *
+ * @param dest Destination address (must be aligned to a 4-byte boundary)
+ * @param src  Source address (must be aligned to a 4-byte boundary)
+ * @param n    number of bytes to copy (must be a multiple of 4)
+ *
+ * @return destination pointer or NULL on error
+ */
+volatile void *
+io32_memcpy(volatile void * dest, const volatile void * src, size_t n);
+
 #ifdef __cplusplus
 }
 #endif

lib/os/CMakeLists.txt

@@ -64,6 +64,8 @@ zephyr_sources_ifdef(CONFIG_SYS_MEM_BLOCKS mem_blocks.c)
 
 zephyr_sources_ifdef(CONFIG_WINSTREAM winstream.c)
 
+zephyr_sources_ifdef(CONFIG_IO32_MEM io32_mem.c)
+
 zephyr_library_include_directories(
   ${ZEPHYR_BASE}/kernel/include
   ${ZEPHYR_BASE}/arch/${ARCH}/include

lib/os/Kconfig

@@ -172,6 +172,14 @@ config UTF8
 	  Enable the utf8 API. The API implements functions to specifically
 	  handle UTF-8 encoded strings.
 
+config IO32_MEM
+	bool "io32_memcpy()"
+	help
+	  Enable io32_memcpy(). This function is useful when word-wise
+	  copying is required, for example when copying from or to a
+	  memory-mapped peripheral. May be expanded with memset and
+	  similar in the future.
+
 rsource "Kconfig.cbprintf"
 
 rsource "Kconfig.heap"

lib/os/io32_mem.c

@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2021 Pete Dietl
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdint.h>
+#include <zephyr/sys/util.h>
+#include <zephyr/sys/__assert.h>
+
+#define IS_ALIGNED(X) (!((uintptr_t)X & (sizeof(uint32_t) - 1)))
+
+volatile void *
+io32_memcpy(volatile void * dst0, const volatile void * src0, size_t len)
+{
+	__ASSERT(len % 4 == 0, "len must be a multiple of 4!");
+
+	volatile uint32_t *dst = dst0;
+	const volatile uint32_t *src = src0;
+
+	__ASSERT(IS_ALIGNED(dst), "dst0 must be aligned to a multiple of 4!");
+	__ASSERT(IS_ALIGNED(src), "src0 must be aligned to a multiple of 4!");
+
+	if (!((len % 4 == 0) && IS_ALIGNED(src) && IS_ALIGNED(dst))) {
+		return NULL;
+	}
+
+	len /= sizeof(uint32_t);
+
+	while (len--) {
+		*dst = *src;
+		++dst;
+		++src;
+	}
+
+	return dst0;
+}