nrf_security: drivers: cracen: make PK mem writes word-aligned on 54L20

Make the sx_*pkmem*() functions perform word-aligned
and word-sized writes on 54L20, which requires that.

Also, turn sx_rdpkmem() into an inline memcpy() as that's
what the implementation was basically doing.

Other than that, the original implementation is left untouched
on other board targets to avoid potential breakages.

ref: DLT-3873

Signed-off-by: Tomi Fontanilles <[email protected]>

Author: tomi-font

subsys/nrf_security/src/drivers/cracen/silexpk/include/silexpk/iomem.h

@@ -7,6 +7,8 @@
 #ifndef IOMEM_HEADER_FILE
 #define IOMEM_HEADER_FILE
 
+#include <string.h>
+
 #ifdef SX_INSTRUMENT_MMIO_WITH_PRINTFS
 #define SX_WARN_UNALIGNED_ADDR(addr) printk("%s: WARNING: unaligned address %p\r\n", __func__, addr)
 #else
@@ -44,11 +46,15 @@ void sx_wrpkmem(void *dst, const void *src, size_t sz);
  * Will be modified after this call
  * @param[in] input_byte The byte value to be written.
  */
+#ifndef CONFIG_SOC_NRF54L20
 static inline void sx_wrpkmem_byte(void *dst, char input_byte)
 {
 	volatile char *d = (volatile char *)dst;
 	*d = input_byte;
 }
+#else
+void sx_wrpkmem_byte(void *dst, char input_byte);
+#endif
 
 /** Read from device memory at src into normal memory at dst.
  *
@@ -60,7 +66,10 @@ static inline void sx_wrpkmem_byte(void *dst, char input_byte)
  * @param[in] src Source of read operation
  * @param[in] sz The number of bytes to read from src to dst
  */
-void sx_rdpkmem(void *dst, const void *src, size_t sz);
+static inline void sx_rdpkmem(void *dst, const void *src, size_t sz)
+{
+	memcpy(dst, src, sz);
+}
 
 /** Read a byte from device memory at src.
  *

subsys/nrf_security/src/drivers/cracen/silexpk/src/iomem.c

@@ -5,9 +5,10 @@
  */
 
 #include <stdint.h>
-#include <string.h>
 #include <silexpk/iomem.h>
 
+#ifndef CONFIG_SOC_NRF54L20
+
 #define PTR_ALIGNMENT(ptr, type)	(((uintptr_t)ptr) & (sizeof(type) - 1))
 #define IS_NATURAL_ALIGNED(ptr, type)	!PTR_ALIGNMENT(ptr, type)
 #define IS_SAME_ALIGNMENT(p1, p2, type) (PTR_ALIGNMENT(p1, type) == PTR_ALIGNMENT(p2, type))
@@ -67,19 +68,95 @@ void sx_wrpkmem(void *dst, const void *src, size_t sz)
 	memcpy((char *)d, (char *)s, sz);
 }
 
-void sx_rdpkmem(void *dst, const void *src, size_t sz)
+#else /* 54L20 requires word-aligned, word-sized memory accesses */
+
+static void write_incomplete_word(uint32_t *dst, const uint8_t *bytes,
+				  size_t first_byte_pos, size_t num_bytes)
+{
+	uint32_t word = *dst;
+
+	for (size_t i = 0; i != num_bytes; ++i) {
+		((uint8_t *)&word)[first_byte_pos + i] = bytes[i];
+	}
+
+#ifdef SX_INSTRUMENT_MMIO_WITH_PRINTFS
+	printk("write_incomplete_word(%p, 0x%x, %zu, %zu): 0x%x to 0x%x\r\n",
+		dst, *(const uint32_t *)bytes, first_byte_pos, num_bytes, *dst, word);
+#endif
+	if ((uintptr_t)dst % 4) {
+		SX_WARN_UNALIGNED_ADDR(dst);
+	}
+
+	*dst = word;
+}
+
+void sx_clrpkmem(void *dst, size_t sz)
+{
+	if (sz == 0) {
+		return;
+	}
+#ifdef SX_INSTRUMENT_MMIO_WITH_PRINTFS
+	printk("sx_clrpkmem(%p, %zu)\r\n", dst, sz);
+#endif
+
+	const uint8_t zero[4] = {};
+	const uintptr_t dst_addr = (uintptr_t)dst;
+
+	if (dst_addr % 4) {
+		const size_t first_byte_pos = dst_addr % 4;
+		const size_t byte_count = 4 - first_byte_pos;
+
+		write_incomplete_word((uint32_t *)(dst_addr & ~3), zero,
+				      first_byte_pos, byte_count);
+		dst = (void *)(dst_addr + byte_count);
+		sz -= byte_count;
+	}
+	uint32_t *word_dst = (uint32_t *)dst;
+
+	for (size_t i = 0; i != sz / 4; ++i) {
+		word_dst[i] = 0;
+	}
+	if (sz % 4) {
+		write_incomplete_word(&word_dst[sz / 4], zero, 0, sz % 4);
+	}
+}
+
+void sx_wrpkmem(void *dst, const void *src, size_t sz)
 {
-	if (IS_SAME_ALIGNMENT(dst, src, tfrblk) && IS_NATURAL_SIZE(sz, tfrblk)) {
-		memcpy(dst, src, sz);
-	} else {
-		volatile char *d = (volatile char *)dst;
-		volatile const char *s = (volatile const char *)src;
-
-		while (sz) {
-			*d = *s;
-			d++;
-			s++;
-			sz--;
-		}
+#ifdef SX_INSTRUMENT_MMIO_WITH_PRINTFS
+	printk("sx_wrpkmem(%p, %p, %zu)\r\n", dst, src, sz);
+#endif
+
+	if ((uintptr_t)dst % 4) {
+		const uintptr_t dst_addr = (uintptr_t)dst;
+		const size_t first_byte_pos = dst_addr % 4;
+		const size_t byte_count = 4 - first_byte_pos;
+
+		write_incomplete_word((uint32_t *)(dst_addr & ~3), src, first_byte_pos, byte_count);
+		*(uint8_t **)&dst += byte_count;
+		*(const uint8_t **)&src += byte_count;
+		sz -= byte_count;
 	}
+
+	for (size_t i = 0; i != sz / 4; ++i) {
+		((uint32_t *)dst)[i] = ((const uint32_t *)src)[i];
+	}
+
+	if (sz % 4) {
+		write_incomplete_word((uint32_t *)((uint8_t *)dst + (sz & ~3)),
+				      (const uint8_t *)src + (sz & ~3), 0, sz % 4);
+	}
+}
+
+void sx_wrpkmem_byte(void *dst, char input_byte)
+{
+	uintptr_t dst_addr = (uintptr_t)dst;
+	uint32_t *word_dst = (uint32_t *)(dst_addr & ~3);
+	uint32_t word = *word_dst;
+	size_t byte_index = dst_addr % 4;
+
+	((uint8_t *)&word)[byte_index] = input_byte;
+	*word_dst = word;
 }
+
+#endif