/* XXX Temporary set it to 32 for MV cores, however this value should be
 * get from Cache Type register
 */
.Larmv7_line_size:
        .word   32
.Larmv7_ways:
        .word   4
.Larmv7_set_mask:
        .word   0xff


ENTRY(armv7_dcache_wbinv_range_index)
        stmdb   sp!, {r4, r5, r6, r7, r8, r9}
        ldr     ip, .Larmv7_line_size
        sub     r3, ip, #1
        and     r2, r0, r3
        add     r1, r1, r2
        bic     r0, r0, r3
        ldr     r4, .Larmv7_ways
        sub     r4, r4, #1
        ldr     r5, .Larmv7_set_mask
.Larmv7_id_wbinv_next_index:
        mov     r8, r0, lsr #5
        and     r8, r8, r5
                                        /* recalculate set */
        mov     r9, r4                  /* reload way */
.Larmv7_id_wbinv_next_way:
        mov     r6, r8, lsl #5
        orr     r6, r6, r9, lsl #30
        mcr     p15, 0, r6, c7, c14, 2  /* Purge D cache SE with VA */
        subs    r9, r9, #1
        bge     .Larmv7_id_wbinv_next_way
        add     r0, r0, ip
        subs    r1, r1, ip
        bhi     .Larmv7_id_wbinv_next_index
        dsb                             /* data synchronization barrier */
        ldmia   sp!, {r4, r5, r6, r7, r8, r9}
        RET