/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * cache.S - set/way cache maintenance, walked off CLIDR_EL1 the same
 * way u-boot and the kernel's own __flush_dcache_all do it. needed
 * before jumping to the payload so it starts from memory, not cache.
 *
 * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
 */

#include <asm/linkage.h>

.pushsection .text.tb_dcache_level, "ax"
ENTRY(tb_dcache_level)
	lsl	x12, x0, #1
	msr	csselr_el1, x12		/* select cache level */
	isb				/* sync change of ccsidr_el1 */
	mrs	x6, ccsidr_el1		/* read the new ccsidr_el1 */
	ubfx	x2, x6, #0, #3		/* x2 <- log2(cache line size)-4 */
	ubfx	x3, x6, #3, #10		/* x3 <- number of cache ways - 1 */
	ubfx	x4, x6, #13, #15	/* x4 <- number of cache sets - 1 */
	add	x2, x2, #4		/* x2 <- log2(cache line size) */
	clz	w5, w3			/* x5 <- bit position of #ways */
	/* x12 <- cache level << 1 */
	/* x2  <- line length offset */
	/* x3  <- number of cache ways - 1 */
	/* x4  <- number of cache sets - 1 */
	/* x5  <- bit position of #ways */

loop_set:
	mov	x6, x3			/* x6 <- working copy of #ways */
loop_way:
	lsl	x7, x6, x5
	orr	x9, x12, x7		/* map way and level to cisw value */
	lsl	x7, x4, x2
	orr	x9, x9, x7		/* map set number to cisw value */
	dc	cisw, x9		/* clean & invalidate by set/way */
	subs	x6, x6, #1		/* decrement the way */
	b.ge	loop_way
	subs	x4, x4, #1		/* decrement the set */
	b.ge	loop_set

	ret
ENDPROC(tb_dcache_level)
.popsection

/*
 * void tb_flush_dcache_all(void)
 *
 * clean & invalidate the whole D cache by set/way.
 */
.pushsection .text.tb_flush_dcache_all, "ax"
ENTRY(tb_flush_dcache_all)
	mov	x1, x0
	dsb	sy
	mrs	x10, clidr_el1		/* read clidr_el1 */
	ubfx	x11, x10, #24, #3	/* x11 <- loc */
	cbz	x11, finished		/* if loc is 0, exit */
	mov	x15, lr
	mov	x0, #0			/* start flush at cache level 0 */
	/* x0  <- cache level */
	/* x10 <- clidr_el1 */
	/* x11 <- loc */
	/* x15 <- return address */

loop_level:
	add	x12, x0, x0, lsl #1	/* x12 <- tripled cache level */
	lsr	x12, x10, x12
	and	x12, x12, #7		/* x12 <- cache type */
	cmp	x12, #2
	b.lt	skip			/* skip if no cache or icache */
	bl	tb_dcache_level		/* flush this level */
skip:
	add	x0, x0, #1		/* increment cache level */
	cmp	x11, x0
	b.gt	loop_level

	mov	x0, #0
	msr	csselr_el1, x0		/* restore csselr_el1 */
	dsb	sy
	isb
	mov	lr, x15

finished:
	ret
ENDPROC(tb_flush_dcache_all)
.popsection

/*
 * void tb_invalidate_icache_all(void)
 *
 * I cache invalidation to PoU, one ic iallu covers the local core.
 */
.pushsection .text.tb_invalidate_icache_all, "ax"
ENTRY(tb_invalidate_icache_all)
	ic	iallu
	dsb	sy
	isb
	ret
ENDPROC(tb_invalidate_icache_all)
.popsection