arch: arm: aarch64: add arch_dcache_range

Add arch_dcache_range to support flush and invalidate

Signed-off-by: Peng Fan <peng.fan@nxp.com>

arch/arm/core/aarch64/CMakeLists.txt

@@ -27,3 +27,5 @@ zephyr_library_sources_ifdef(CONFIG_HAS_ARM_SMCCC smccc-call.S)
 zephyr_library_sources_ifdef(CONFIG_AARCH64_IMAGE_HEADER header.S)
 
 add_subdirectory_ifdef(CONFIG_ARM_MMU mmu)
+zephyr_library_sources_ifdef(CONFIG_CACHE_MANAGEMENT cache.S)
+zephyr_library_sources_ifdef(CONFIG_CACHE_MANAGEMENT cache.c)

arch/arm/core/aarch64/cache.S

@@ -0,0 +1,49 @@
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/*
+ * ARM64 Cortex-A Cache management
+ */
+
+#include <toolchain.h>
+#include <linker/sections.h>
+#include <arch/cpu.h>
+
+_ASM_FILE_PROLOGUE
+
+.macro	dcache_range_op, dcache op
+	/* Calculate dcache line size */
+	mrs	x3, ctr_el0
+	mov	x2, #4
+	ubfm	x3, x3, #16, #19
+	lsl	x2, x2, x3
+
+	/*
+	 * x2 = cacheline_size
+	 * x1 = start + end
+	 * x3 = cacheline_size - 1
+	 * x0 = x0 & ~(x3)
+	 */
+	add	x1, x0, x1
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+1:
+	\dcache \op, x0
+	add	x0, x0, x2
+	cmp	x0, x1
+	blo	1b
+	dsb	sy
+.endm
+
+GTEXT(arch_dcache_flush)
+SECTION_FUNC(TEXT, arch_dcache_flush)
+	dcache_range_op dc civac
+	ret
+
+GTEXT(arch_dcache_invd)
+SECTION_FUNC(TEXT, arch_dcache_invd)
+	dcache_range_op dc ivac
+	ret

arch/arm/core/aarch64/cache.c

@@ -0,0 +1,59 @@
+/* cache.c - d-cache support for AARCH64 CPUs */
+
+/*
+ * Copyright 2020 NXP
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/**
+ * @file
+ * @brief d-cache manipulation
+ *
+ * This module contains functions for manipulation of the d-cache.
+ */
+
+#include <cache.h>
+
+#define	CTR_EL0_DMINLINE_SHIFT	16
+#define	CTR_EL0_DMINLINE_MASK	GENMASK(19, 16)
+#define	CTR_EL0_CWG_SHIFT	24
+#define	CTR_EL0_CWG_MASK	GENMASK(27, 24)
+
+int arch_dcache_flush(void *addr, size_t size);
+int arch_dcache_invd(void *addr, size_t size);
+
+static size_t dcache_line_size;
+
+int arch_dcache_range(void *addr, size_t size, int op)
+{
+	if (op == K_CACHE_INVD) {
+		arch_dcache_invd(addr, size);
+	} else if (op == K_CACHE_WB_INVD) {
+		arch_dcache_flush(addr, size);
+	} else {
+		return -ENOTSUP;
+	}
+
+	return 0;
+}
+
+size_t arch_dcache_line_size_get(void)
+{
+	uint64_t ctr_el0;
+	uint32_t cwg;
+	uint32_t dminline;
+
+	if (dcache_line_size)
+		return dcache_line_size;
+
+	ctr_el0 = read_sysreg(CTR_EL0);
+
+	cwg = (ctr_el0 & CTR_EL0_CWG_MASK) >> CTR_EL0_CWG_SHIFT;
+	dminline = (ctr_el0 & CTR_EL0_DMINLINE_MASK) >>
+		CTR_EL0_DMINLINE_SHIFT;
+
+	dcache_line_size = cwg ? 4 << cwg : 4 << dminline;
+
+	return dcache_line_size;
+}