From 2425a2a12eb0dc8c890fdb4c3bd34e480c49feae Mon Sep 17 00:00:00 2001
From: xj <kameblue@outlook.com>
Date: Thu, 24 Oct 2024 23:13:37 -0700
Subject: [PATCH] codes for MAIR supporting

---
 .../cortex-a55/preboot_for_3568/boot.S        | 48 +++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
index bf9535a90..ed4eedd2a 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
@@ -15,10 +15,21 @@
 #define SPSR_EL2_VALUE (7 << 6) | (5 << 0)
 #define SCTLR_EL1_VALUE (0x30D00800)
 
+/* Macros for MAIR setting. */
+#define MAIR(attr, mt)	((attr) << ((mt) * 8))
+#define MT_DEVICE_nGnRnE	0
+#define MT_DEVICE_nGnRE		1
+#define MT_DEVICE_GRE		2
+#define MT_NORMAL_NC		3
+#define MT_NORMAL	    	4
+#define MT_NORMAL_WT		5
+
 .section ".text", "ax"
 .global _boot_start
 .global primary_cpu_init
 .global el2_setup
+/* The function for setting memory types and MAIR registers. */
+.global __cpu_mair_setup
 
 _boot_start:
     bl el2_setup
@@ -53,6 +64,7 @@ _boot_start:
     mov     x5, x2
     cmp     x5, #0
     beq     primary_cpu_init
+    bl      __cpu_mair_setup
     bl      bootmain     // for secondary cpus, jump to argument function pointer passed in by ROM
 
     bl      .
@@ -68,6 +80,7 @@ primary_cpu_init:
     stp x3, x3, [x1], #16
     b.lt    1b  
 
+    bl __cpu_mair_setup
 	bl bootmain
 
     .func el2_setup
@@ -117,4 +130,39 @@ el2_setup:
     eret
     .endfunc
 
+    .func __cpu_mair_setup
+__cpu_mair_setup:
+	tlbi	vmalle1				// Invalidate local TLB
+	dsb	nsh
+
+	mov	x0, #3 << 20
+	msr	cpacr_el1, x0			// Enable FP/ASIMD
+	mov	x0, #1 << 12			// Reset mdscr_el1 and disable
+	msr	mdscr_el1, x0			// access to the DCC from EL0
+	isb					// Unmask debug exceptions now,
+//	enable_dbg				// since this is per-cpu
+	/*
+	 * Memory region attributes for LPAE:
+	 *
+	 *   n = AttrIndx[2:0]
+	 *			n	MAIR
+	 *   DEVICE_nGnRnE	000	00000000
+	 *   DEVICE_nGnRE	001	00000100
+	 *   DEVICE_GRE		010	00001100
+	 *   NORMAL_NC		011	01000100
+	 *   NORMAL		100	11111111
+	 *   NORMAL_WT		101	10111011
+	 */
+	ldr	x5, =MAIR(0x00, MT_DEVICE_nGnRnE) | \
+		     MAIR(0x04, MT_DEVICE_nGnRE) | \
+		     MAIR(0x0c, MT_DEVICE_GRE) | \
+		     MAIR(0x44, MT_NORMAL_NC) | \
+		     MAIR(0xff, MT_NORMAL) | \
+		     MAIR(0xbb, MT_NORMAL_WT)
+	msr	mair_el1, x5
+
+    eret
+    .endfunc
+
+
     .end  
\ No newline at end of file