diff --git a/common/lib/elf.c b/common/lib/elf.c index 73c19979..702c95a1 100644 --- a/common/lib/elf.c +++ b/common/lib/elf.c @@ -577,7 +577,7 @@ again: } #if defined (__aarch64__) - clean_inval_dcache_poc(mem_base, mem_base + mem_size); + clean_dcache_poc(mem_base, mem_base + mem_size); inval_icache_pou(mem_base, mem_base + mem_size); #endif } diff --git a/common/sys/cpu.c b/common/sys/cpu.c index 7aabbbdc..90182abe 100644 --- a/common/sys/cpu.c +++ b/common/sys/cpu.c @@ -24,5 +24,6 @@ extern void delay(uint64_t cycles); extern size_t icache_line_size(void); extern size_t dcache_line_size(void); extern void clean_inval_dcache_poc(uintptr_t start, uintptr_t end); +extern void clean_dcache_poc(uintptr_t start, uintptr_t end); extern void inval_icache_pou(uintptr_t start, uintptr_t end); extern int current_el(void); diff --git a/common/sys/cpu.h b/common/sys/cpu.h index 61ce5e5d..42352f56 100644 --- a/common/sys/cpu.h +++ b/common/sys/cpu.h @@ -265,6 +265,19 @@ inline void clean_inval_dcache_poc(uintptr_t start, uintptr_t end) { asm volatile ("dsb sy\n\tisb"); } +// Clean D-Cache to Point of Coherency +inline void clean_dcache_poc(uintptr_t start, uintptr_t end) { + size_t dsz = dcache_line_size(); + + uintptr_t addr = start & ~(dsz - 1); + while (addr < end) { + asm volatile ("dc cvac, %0" :: "r"(addr) : "memory"); + addr += dsz; + } + + asm volatile ("dsb sy\n\tisb"); +} + // Invalidate I-Cache to Point of Unification inline void inval_icache_pou(uintptr_t start, uintptr_t end) { size_t isz = icache_line_size();