177 lines
4.0 KiB
C
177 lines
4.0 KiB
C
|
|
// stolen from
|
|
// https://github.com/Xilinx/embeddedsw/blob/master/lib/bsp/standalone/src/arm/cortexa9/xil_cache.c
|
|
|
|
#define IRQ_FIQ_MASK 0xC0
|
|
|
|
// CP15_CACHE_SIZE_SEL, CP15_CACHE_SIZE_ID, CP15_INVAL_IC_POU
|
|
// asm_cp15_inval_dc_line_sw
|
|
// asm_cp15_clean_inval_dc_line_sw
|
|
|
|
#define XREG_CP15_CACHE_SIZE_SEL "p15, 2, %0, c0, c0, 0"
|
|
#define XREG_CP15_CACHE_SIZE_ID "p15, 1, %0, c0, c0, 0"
|
|
#define XREG_CP15_INVAL_IC_POU "p15, 0, %0, c7, c5, 0"
|
|
#define XREG_CP15_INVAL_DC_LINE_SW "p15, 0, %0, c7, c6, 2"
|
|
#define XREG_CP15_CLEAN_INVAL_DC_LINE_SW "p15, 0, %0, c7, c14, 2"
|
|
|
|
|
|
#define asm_cp15_inval_dc_line_sw(param) __asm__ __volatile__("mcr " \
|
|
XREG_CP15_INVAL_DC_LINE_SW :: "r" (param)); \
|
|
|
|
#define asm_cp15_clean_inval_dc_line_sw(param) __asm__ __volatile__("mcr " \
|
|
XREG_CP15_CLEAN_INVAL_DC_LINE_SW :: "r" (param)); \
|
|
|
|
|
|
#define dsb() asm volatile("dsb":::"memory")
|
|
#define isb() asm volatile("isb":::"memory")
|
|
|
|
#define mfcpsr() ({uint32_t rval = 0U; \
|
|
__asm__ __volatile__(\
|
|
"mrs %0, cpsr\n"\
|
|
: "=r" (rval)\
|
|
);\
|
|
rval;\
|
|
}) \
|
|
|
|
#define mtcpsr(v) __asm__ __volatile__(\
|
|
"msr cpsr,%0\n"\
|
|
: : "r" (v) : "cc" \
|
|
) \
|
|
|
|
/* CP15 operations */
|
|
#define mtcp(rn, v) __asm__ __volatile__(\
|
|
"mcr " rn "\n"\
|
|
: : "r" (v)\
|
|
); \
|
|
|
|
#define mfcp(rn) ({uint32_t rval = 0U; \
|
|
__asm__ __volatile__(\
|
|
"mrc " rn "\n"\
|
|
: "=r" (rval)\
|
|
);\
|
|
rval;\
|
|
}) \
|
|
|
|
|
|
__attribute__((__unused__))
|
|
static void DC_InvalidateAll(void) {
|
|
register uint32_t CsidReg, C7Reg;
|
|
uint32_t CacheSize, LineSize, NumWays;
|
|
uint32_t Way, WayIndex, Set, SetIndex, NumSet;
|
|
uint32_t currmask;
|
|
|
|
currmask = mfcpsr();
|
|
mtcpsr(currmask | IRQ_FIQ_MASK);
|
|
|
|
/* Select cache level 0 and D cache in CSSR */
|
|
mtcp(XREG_CP15_CACHE_SIZE_SEL, 0U);
|
|
|
|
CsidReg = mfcp(XREG_CP15_CACHE_SIZE_ID);
|
|
/* Determine Cache Size */
|
|
CacheSize = (CsidReg >> 13U) & 0x1FFU;
|
|
CacheSize +=1U;
|
|
CacheSize *=128U; /* to get number of bytes */
|
|
|
|
/* Number of Ways */
|
|
NumWays = (CsidReg & 0x3ffU) >> 3U;
|
|
NumWays += 1U;
|
|
|
|
/* Get the cacheline size, way size, index size from csidr */
|
|
LineSize = (CsidReg & 0x07U) + 4U;
|
|
|
|
NumSet = CacheSize/NumWays;
|
|
NumSet /= (0x00000001U << LineSize);
|
|
|
|
Way = 0U;
|
|
Set = 0U;
|
|
|
|
/* Invalidate all the cachelines */
|
|
for (WayIndex =0U; WayIndex < NumWays; WayIndex++) {
|
|
for (SetIndex =0U; SetIndex < NumSet; SetIndex++) {
|
|
C7Reg = Way | Set;
|
|
|
|
/* Invalidate by Set/Way */
|
|
asm_cp15_inval_dc_line_sw(C7Reg);
|
|
Set += (0x00000001U << LineSize);
|
|
}
|
|
Set=0U;
|
|
Way += 0x40000000U;
|
|
}
|
|
|
|
/* Wait for L1 invalidate to complete */
|
|
dsb();
|
|
mtcpsr(currmask);
|
|
}
|
|
|
|
static void DC_FlushAll(void) {
|
|
register uint32_t CsidReg, C7Reg;
|
|
uint32_t CacheSize, LineSize, NumWays;
|
|
uint32_t Way;
|
|
uint32_t WayIndex, Set, SetIndex, NumSet;
|
|
uint32_t currmask;
|
|
|
|
currmask = mfcpsr();
|
|
mtcpsr(currmask | IRQ_FIQ_MASK);
|
|
|
|
/* Select cache level 0 and D cache in CSSR */
|
|
mtcp(XREG_CP15_CACHE_SIZE_SEL, 0);
|
|
|
|
CsidReg = mfcp(XREG_CP15_CACHE_SIZE_ID);
|
|
|
|
/* Determine Cache Size */
|
|
|
|
CacheSize = (CsidReg >> 13U) & 0x1FFU;
|
|
CacheSize +=1U;
|
|
CacheSize *=128U; /* to get number of bytes */
|
|
|
|
/* Number of Ways */
|
|
NumWays = (CsidReg & 0x3ffU) >> 3U;
|
|
NumWays += 1U;
|
|
|
|
/* Get the cacheline size, way size, index size from csidr */
|
|
LineSize = (CsidReg & 0x07U) + 4U;
|
|
|
|
NumSet = CacheSize/NumWays;
|
|
NumSet /= (0x00000001U << LineSize);
|
|
|
|
Way = 0U;
|
|
Set = 0U;
|
|
|
|
/* Invalidate all the cachelines */
|
|
for (WayIndex =0U; WayIndex < NumWays; WayIndex++) {
|
|
for (SetIndex =0U; SetIndex < NumSet; SetIndex++) {
|
|
C7Reg = Way | Set;
|
|
/* Flush by Set/Way */
|
|
|
|
asm_cp15_clean_inval_dc_line_sw(C7Reg);
|
|
Set += (0x00000001U << LineSize);
|
|
}
|
|
Set = 0U;
|
|
Way += 0x40000000U;
|
|
}
|
|
|
|
/* Wait for L1 flush to complete */
|
|
dsb();
|
|
mtcpsr(currmask);
|
|
}
|
|
|
|
static void IC_InvalidateAll(void) {
|
|
mtcp(XREG_CP15_CACHE_SIZE_SEL, 1U);
|
|
/* invalidate the instruction cache */
|
|
mtcp(XREG_CP15_INVAL_IC_POU, 0U);
|
|
|
|
/* Wait for L1 invalidate to complete */
|
|
dsb();
|
|
isb();
|
|
}
|
|
|
|
#undef dsb
|
|
#undef mfcpsr
|
|
#undef mtcpsr
|
|
#undef mtcp
|
|
#undef mfcp
|
|
#undef asm_cp15_inval_dc_line_sw
|
|
#undef asm_cp15_clean_inval_dc_line_sw
|
|
#undef IRQ_FIQ_MASK
|
|
|