// 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