1/*
2 * Copyright (c) 2006-2018, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date           Author       Notes
8 * 2013-07-05     Bernard      the first version
9 */
10
11.globl rt_cpu_get_smp_id
12rt_cpu_get_smp_id:
13    mrc     p15, #0, r0, c0, c0, #5
14    bx      lr
15
16.globl rt_cpu_vector_set_base
17rt_cpu_vector_set_base:
18    /* clear SCTRL.V to customize the vector address */
19    mrc     p15, #0, r1, c1, c0, #0
20    bic     r1, #(1 << 13)
21    mcr     p15, #0, r1, c1, c0, #0
22    /* set up the vector address */
23    mcr     p15, #0, r0, c12, c0, #0
24    dsb
25    bx      lr
26
27.globl rt_hw_cpu_dcache_enable
28rt_hw_cpu_dcache_enable:
29    mrc     p15, #0, r0, c1, c0, #0
30    orr     r0,  r0, #0x00000004
31    mcr     p15, #0, r0, c1, c0, #0
32    bx      lr
33
34.globl rt_hw_cpu_icache_enable
35rt_hw_cpu_icache_enable:
36    mrc     p15, #0, r0, c1, c0, #0
37    orr     r0,  r0, #0x00001000
38    mcr     p15, #0, r0, c1, c0, #0
39    bx      lr
40
41_FLD_MAX_WAY:
42   .word  0x3ff
43_FLD_MAX_IDX:
44   .word  0x7fff
45
46.globl rt_cpu_dcache_clean_flush
47rt_cpu_dcache_clean_flush:
48    push    {r4-r11}
49    dmb
50    mrc     p15, #1, r0, c0, c0, #1  @ read clid register
51    ands    r3, r0, #0x7000000       @ get level of coherency
52    mov     r3, r3, lsr #23
53    beq     finished
54    mov     r10, #0
55loop1:
56    add     r2, r10, r10, lsr #1
57    mov     r1, r0, lsr r2
58    and     r1, r1, #7
59    cmp     r1, #2
60    blt     skip
61    mcr     p15, #2, r10, c0, c0, #0
62    isb
63    mrc     p15, #1, r1, c0, c0, #0
64    and     r2, r1, #7
65    add     r2, r2, #4
66    ldr     r4, _FLD_MAX_WAY
67    ands    r4, r4, r1, lsr #3
68    clz     r5, r4
69    ldr     r7, _FLD_MAX_IDX
70    ands    r7, r7, r1, lsr #13
71loop2:
72    mov     r9, r4
73loop3:
74    orr     r11, r10, r9, lsl r5
75    orr     r11, r11, r7, lsl r2
76    mcr     p15, #0, r11, c7, c14, #2
77    subs    r9, r9, #1
78    bge     loop3
79    subs    r7, r7, #1
80    bge     loop2
81skip:
82    add     r10, r10, #2
83    cmp     r3, r10
84    bgt     loop1
85
86finished:
87    dsb
88    isb
89    pop     {r4-r11}
90    bx      lr
91
92.globl rt_cpu_icache_flush
93rt_cpu_icache_flush:
94    mov r0, #0
95    mcr p15, 0, r0, c7, c5, 0       @ I+BTB cache invalidate
96    dsb
97    isb
98    bx      lr
99
100.globl rt_hw_cpu_dcache_disable
101rt_hw_cpu_dcache_disable:
102    push    {r4-r11, lr}
103    bl      rt_cpu_dcache_clean_flush
104    mrc     p15, #0, r0, c1, c0, #0
105    bic     r0,  r0, #0x00000004
106    mcr     p15, #0, r0, c1, c0, #0
107    pop     {r4-r11, lr}
108    bx      lr
109
110.globl rt_hw_cpu_icache_disable
111rt_hw_cpu_icache_disable:
112    mrc     p15, #0, r0, c1, c0, #0
113    bic     r0,  r0, #0x00001000
114    mcr     p15, #0, r0, c1, c0, #0
115    bx      lr
116
117.globl rt_cpu_mmu_disable
118rt_cpu_mmu_disable:
119    mcr     p15, #0, r0, c8, c7, #0    @ invalidate tlb
120    mrc     p15, #0, r0, c1, c0, #0
121    bic     r0, r0, #1
122    mcr     p15, #0, r0, c1, c0, #0    @ clear mmu bit
123    dsb
124    bx      lr
125
126.globl rt_cpu_mmu_enable
127rt_cpu_mmu_enable:
128    mrc     p15, #0, r0, c1, c0, #0
129    orr     r0, r0, #0x001
130    mcr     p15, #0, r0, c1, c0, #0    @ set mmu enable bit
131    dsb
132    bx      lr
133
134.globl rt_cpu_tlb_set
135rt_cpu_tlb_set:
136    mcr     p15, #0, r0, c2, c0, #0
137    dmb
138    bx      lr
139