/*
 * Copyright 2022 Haiku, Inc. All rights reserved.
 * Released under the terms of the MIT License.
 */

#include <asm_defs.h>


        // the following code is based on ARM ARM
        // section B2.2.4, Cache maintenance functionality

FUNCTION(clean_dcache_all):
        dmb
        stmfd   sp!, {r4-r11, lr}

        // Query the number of cache levels to be cleaned
        // this is encoded in LoC which is bits 26..24 of CLIDR
        mrc             p15, 1, r0, c0, c0, 1
        mov             r3, r0, lsr #23
        ands    r3, r3, #7 << 1
        beq             cleaning_done

        // Start cleaning at cache level 0
        mov             r10, #0
clean_levels_loop:
        // Get type of current cache level
        add             r2, r10, r10, lsr #1
        mov             r1, r0, lsr r2
        and             r1, r1, #7

        // Skip this level if cache type is less than 2 i.e. no cache or only i-cache
        cmp             r1, #2
        blt             clean_next_level

        // Select current level in Cache Size selection register
        mcr             p15, 2, r10, c0, c0, 0
        isb

        // Read Cache Size ID for the current level
        mrc             p15, 1, r1, c0, c0, 0

        // Decode LineSize from bits[2:0]
        and             r2, r1, #7
        add             r2, r2, #4

        // Decode Associativity from bits[12:3]
        movw    r4, #0x3ff
        ands    r4, r4, r1, lsr #3
        clz             r5, r4

        // Decode NumSets from bits[27:13]
        movw    r7, #0x7fff
        ands    r7, r7, r1, lsr #13

clean_way_loop:
        mov             r9, r7
clean_index_loop:
        orr             r11, r10, r4, lsl r5
        orr             r11, r11, r9, lsl r2

        // Clean and Invalidate by set/way
        mcr             p15, 0, r11, c7, c14, 2

        subs    r9, r9, #1
        bge             clean_index_loop

        subs    r4, r4, #1
        bge             clean_way_loop
clean_next_level:
        add             r10, r10, #2
        cmp             r3, r10
        bgt             clean_levels_loop
cleaning_done:
        dsb
        isb
        ldmfd   sp!, {r4-r11, lr}
        bx              lr
FUNCTION_END(clean_dcache_all)


FUNCTION(invalidate_icache_all):
        mov             r0, #0
        mcr             p15, 0, r0, c7, c5, 0
        dsb
        isb
        bx              lr
FUNCTION_END(invalidate_icache_all)