From 42aa54ad6a7879a82bc9b29b2519fa35a8293da7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E5=91=A8=E7=9D=BF?= <zrufo747@outlook.com>
Date: Tue, 3 Dec 2024 03:23:12 +0000
Subject: [PATCH] fix cache

---
 modules/axhal/src/arch/aarch64/cache.rs | 70 ++++++++++++-------------
 modules/axhal/src/dma.rs                |  4 +-
 2 files changed, 37 insertions(+), 37 deletions(-)

diff --git a/modules/axhal/src/arch/aarch64/cache.rs b/modules/axhal/src/arch/aarch64/cache.rs
index bffef3561d..ee76f62ac0 100644
--- a/modules/axhal/src/arch/aarch64/cache.rs
+++ b/modules/axhal/src/arch/aarch64/cache.rs
@@ -2,47 +2,47 @@
 
 use core::{arch::asm, ptr::NonNull};
 
-fn dcache_line_size() -> usize {
-    unsafe {
-        let result;
-        asm!(
-            "mrs    x8, CTR_EL0",
-            "ubfm x8, x8, #16, #19",	// cache line size encoding
-            "mov		{0}, #4",		// bytes per word
-            "lsl		{0}, {0}, x8",	// actual cache line size""",
-            out(reg) result);
-        result
-    }
-}
-
 /// Invalidate data cache
 pub fn dcache_invalidate_range(addr: NonNull<u8>, size: usize) {
-    let addr = addr.as_ptr() as usize;
-    unsafe {
-        let line_size = dcache_line_size();
-        let start = addr & !(line_size - 1);
-        let end = (addr + size + line_size - 1) & !(line_size - 1);
+    let start = addr.as_ptr() as usize;
+    let end = start + size;
 
-        for addr in (start..end).step_by(line_size) {
-            asm!("dc ivac, {0}", in(reg) addr);
-        }
-
-        asm!("dsb sy; isb");
+    unsafe {
+        asm!(
+            "mrs	x3, ctr_el0",
+            "ubfx	x3, x3, #16, #4",
+            "mov	x2, #4",
+            "lsl	x2, x2, x3", /* cache line size */
+            /* x2 <- minimal cache line size in cache system */
+            "sub	x3, x2, #1",
+            "bic	x0, x0, x3",
+            "1:	dc	ivac, x0", /* invalidate data or unified cache */
+            "add	x0, x0, x2",
+            "cmp	x0, x1",
+            "b.lo	1b",
+            "dsb	sy"
+        );
     }
 }
 
-/// Clean data cache
-pub fn dcache_clean_range(addr: NonNull<u8>, size: usize) {
-    let addr = addr.as_ptr() as usize;
+/// Flush data cache
+pub fn dcache_flush_range(addr: NonNull<u8>, size: usize) {
+    let start = addr.as_ptr() as usize;
+    let end = start + size;
     unsafe {
-        let line_size = dcache_line_size();
-        let start = addr & !(line_size - 1);
-        let end = (addr + size + line_size - 1) & !(line_size - 1);
-
-        for addr in (start..end).step_by(line_size) {
-            asm!("dc cvac, {0}", in(reg) addr);
-        }
-
-        asm!("dsb sy; isb");
+        asm!(
+            "mrs	x3, ctr_el0",
+            "ubfx	x3, x3, #16, #4",
+            "mov	x2, #4",
+            "lsl	x2, x2, x3", /* cache line size */
+            /* x2 <- minimal cache line size in cache system */
+            "sub	x3, x2, #1",
+            "bic	x0, x0, x3",
+            "1:	dc	civac, x0", /* clean & invalidate data or unified cache */
+            "add	x0, x0, x2",
+            "cmp	x0, x1",
+            "b.lo	1b",
+            "dsb	sy"
+        );
     }
 }
diff --git a/modules/axhal/src/dma.rs b/modules/axhal/src/dma.rs
index a3aae04aa7..73caf29179 100644
--- a/modules/axhal/src/dma.rs
+++ b/modules/axhal/src/dma.rs
@@ -1,7 +1,7 @@
 use dma_api::Impl;
 
 use crate::{
-    arch::cache::{dcache_clean_range, dcache_invalidate_range},
+    arch::cache::{dcache_flush_range, dcache_invalidate_range},
     mem::virt_to_phys,
 };
 
@@ -16,7 +16,7 @@ impl Impl for DmaImpl {
     fn unmap(_addr: core::ptr::NonNull<u8>, _size: usize) {}
 
     fn flush(addr: core::ptr::NonNull<u8>, size: usize) {
-        dcache_clean_range(addr, size);
+        dcache_flush_range(addr, size);
     }
 
     fn invalidate(addr: core::ptr::NonNull<u8>, size: usize) {