[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/arch/arm64/cache-ops.S b/src/bsp/lk/arch/arm64/cache-ops.S
new file mode 100644
index 0000000..04ffef8
--- /dev/null
+++ b/src/bsp/lk/arch/arm64/cache-ops.S
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2014, Google Inc. All rights reserved
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <asm.h>
+#include <arch/ops.h>
+#include <arch/defines.h>
+
+#define LOC_SHIFT 24
+#define CLIDR_FIELD_WIDTH 3
+#define LEVEL_SHIFT 1
+#define DCISW 0x0
+#define DCCISW 0x1
+
+.text
+
+.macro cache_range_op, cache op
+ add x2, x0, x1 // calculate the end address
+ bic x3, x0, #(CACHE_LINE-1) // align the start with a cache line
+.Lcache_range_op_loop\@:
+ \cache \op, x3
+ add x3, x3, #CACHE_LINE
+ cmp x3, x2
+ blo .Lcache_range_op_loop\@
+ dsb sy
+.endm
+
+ /* void arch_flush_cache_range(addr_t start, size_t len); */
+FUNCTION(arch_clean_cache_range)
+ cache_range_op dc cvac // clean cache to PoC by MVA
+ ret
+
+ /* void arch_flush_invalidate_cache_range(addr_t start, size_t len); */
+FUNCTION(arch_clean_invalidate_cache_range)
+ cache_range_op dc civac // clean & invalidate dcache to PoC by MVA
+ ret
+
+ /* void arch_invalidate_cache_range(addr_t start, size_t len); */
+FUNCTION(arch_invalidate_cache_range)
+ cache_range_op dc ivac // invalidate dcache to PoC by MVA
+ ret
+
+ /* void arch_sync_cache_range(addr_t start, size_t len); */
+FUNCTION(arch_sync_cache_range)
+ cache_range_op dc cvau // clean dcache to PoU by MVA
+ cache_range_op ic ivau // invalidate icache to PoU by MVA
+ ret
+
+/* will trash x0-x2, x4-x9, x11, x14, x16-x17 */
+LOCAL_FUNCTION(do_dcsw_op)
+ cbz x3, exit
+ adr x14, dcsw_loop_table // compute inner loop address
+ add x14, x14, x0, lsl #5 // inner loop is 8x32-bit instructions
+ mov x0, x9
+ mov w8, #1
+loop1:
+ add x2, x10, x10, lsr #1 // work out 3x current cache level
+ lsr x1, x0, x2 // extract cache type bits from clidr
+ and x1, x1, #7 // mask the bits for current cache only
+ cmp x1, #2 // see what cache we have at this level
+ b.lt level_done // nothing to do if no cache or icache
+
+ msr csselr_el1, x10 // select current cache level in csselr
+ isb // isb to sych the new cssr&csidr
+ mrs x1, ccsidr_el1 // read the new ccsidr
+ and x2, x1, #7 // extract the length of the cache lines
+ add x2, x2, #4 // add 4 (line length offset)
+ ubfx x4, x1, #3, #10 // maximum way number
+ clz w5, w4 // bit position of way size increment
+ lsl w9, w4, w5 // w9 = aligned max way number
+ lsl w16, w8, w5 // w16 = way number loop decrement
+ orr w9, w10, w9 // w9 = combine way and cache number
+ ubfx w6, w1, #13, #15 // w6 = max set number
+ lsl w17, w8, w2 // w17 = set number loop decrement
+ dsb sy // barrier before we start this level
+ br x14 // jump to DC operation specific loop
+
+ .macro dcsw_loop _op
+loop2_\_op:
+ lsl w7, w6, w2 // w7 = aligned max set number
+
+loop3_\_op:
+ orr w11, w9, w7 // combine cache, way and set number
+ dc \_op, x11
+ subs w7, w7, w17 // decrement set number
+ b.ge loop3_\_op
+
+ subs x9, x9, x16 // decrement way number
+ b.ge loop2_\_op
+
+ b level_done
+.endm
+
+level_done:
+ add x10, x10, #2 // increment cache number
+ cmp x3, x10
+ b.gt loop1
+ msr csselr_el1, xzr // select cache level 0 in csselr
+ dsb sy // barrier to complete final cache operation
+ isb
+exit:
+ ret
+
+dcsw_loop_table:
+ dcsw_loop isw
+ dcsw_loop cisw
+ dcsw_loop csw
+
+/* will trash x3, x9, x10 */
+.macro dcsw_op shift, fw, ls
+ mrs x9, clidr_el1
+ ubfx x3, x9, \shift, \fw
+ lsl x3, x3, \ls
+ mov x10, xzr
+ bl do_dcsw_op
+.endm
+
+/* void arch_enable_cache(uint flags);
+ * For EL1 only.
+ */
+FUNCTION(arch_enable_cache)
+ stp x29, x30, [sp, #-32]!
+ stp x24, x25, [sp, #16]
+
+ mov x25, x0
+ /* check DCACHE flag */
+ tst x25, #DCACHE
+ b.eq .L__enable_icache
+ mrs x24, sctlr_el1
+ tst x24, #(1<<2)
+ b.ne .L__enable_icache
+
+ /* invalidate dcache */
+ mov x0, #DCISW
+ dcsw_op #LOC_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+
+ /* enable dcache enable bit */
+ orr x24, x24, #(1<<2)
+ msr sctlr_el1, x24
+
+.L__enable_icache:
+ /* check ICACHE flag */
+ tst x25, #ICACHE
+ b.eq .L__done_enable
+ mrs x24, sctlr_el1
+ tst x24, #(1<<12)
+ b.ne .L__done_enable
+
+ /* invalidate icache */
+ dsb sy
+ ic iallu
+ dsb sy
+ isb
+
+ /* enable icache enable bit */
+ mrs x24, sctlr_el1
+ orr x24, x24, #(1<<12)
+ msr sctlr_el1, x24
+
+.L__done_enable:
+ ldp x24, x25, [sp, #16]
+ ldp x29, x30, [sp], #32
+ ret
+
+/* void arch_disable_cache(uint flags) */
+/* only for el1 here */
+FUNCTION(arch_disable_cache)
+ stp x29, x30, [sp, #-32]!
+ str x25, [sp, #16]
+
+ mov x25, x0
+ /* check DCACHE flag */
+ tst x25, #DCACHE
+ b.eq .L__disable_icache
+ mrs x1, sctlr_el1
+ tst x1, #(1<<2)
+ b.eq .L__dcache_already_disabled
+
+ /* disable dcache enable bit */
+ bic x1, x1, #(1<<2)
+ msr sctlr_el1, x1
+
+ /* clean & invalidate dcache */
+ mov x0, #DCCISW
+ b .L__flush_dcache
+
+.L__dcache_already_disabled:
+ /* invalidate dcache */
+ mov x0, #DCISW
+.L__flush_dcache:
+ dcsw_op #LOC_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+
+.L__disable_icache:
+ /* check ICACHE flag */
+ tst x25, #ICACHE
+ b.eq .L__done_disable
+ /* disable icache enable bit */
+ mrs x1, sctlr_el1
+ bic x1, x1, #(1<<12)
+ msr sctlr_el1, x1
+
+ /* invalidate icache for PE to PoU */
+ dsb sy
+ ic iallu
+ dsb sy
+ isb
+
+.L__done_disable:
+ ldr x25, [sp, #16]
+ ldp x29, x30, [sp], #32
+ ret