1/*
2 * Copyright (c) 2011-2022, Shanghai Real-Thread Electronic Technology Co.,Ltd
3 *
4 * Change Logs:
5 * Date           Author       Notes
6 * 2022-08-29     RT-Thread    first version
7 */
8
9
10.globl rt_cpu_get_smp_id
11rt_cpu_get_smp_id:
12    mrc     p15, #0, r0, c0, c0, #5
13    bx      lr
14
15.globl rt_cpu_vector_set_base
16rt_cpu_vector_set_base:
17    /* clear SCTRL.V to customize the vector address */
18    mrc     p15, #0, r1, c1, c0, #0
19    bic     r1, #(1 << 13)
20    mcr     p15, #0, r1, c1, c0, #0
21    /* set up the vector address */
22    mcr     p15, #0, r0, c12, c0, #0
23    dsb
24    bx      lr
25
26.globl rt_hw_cpu_dcache_enable
27rt_hw_cpu_dcache_enable:
28    mrc     p15, #0, r0, c1, c0, #0
29    orr     r0,  r0, #0x00000004
30    mcr     p15, #0, r0, c1, c0, #0
31    bx      lr
32
33.globl rt_hw_cpu_icache_enable
34rt_hw_cpu_icache_enable:
35    mrc     p15, #0, r0, c1, c0, #0
36    orr     r0,  r0, #0x00001000
37    mcr     p15, #0, r0, c1, c0, #0
38    bx      lr
39
40_FLD_MAX_WAY:
41   .word  0x3ff
42_FLD_MAX_IDX:
43   .word  0x7fff
44
45.globl rt_cpu_dcache_clean_flush
46rt_cpu_dcache_clean_flush:
47stmfd   sp!, {r0-r12, lr}
48    bl  v7_flush_dcache_all
49    mov r0, #0
50    mcr p15, 0, r0, c7, c5, 0       @ I+BTB cache invalidate
51    dsb
52    isb
53    ldmfd   sp!, {r0-r12, lr}
54    mov pc, lr
55
56v7_flush_dcache_all:
57    dmb                 @ ensure ordering with previous memory accesses
58    mrc p15, 1, r0, c0, c0, 1       @ read clidr
59    ands    r3, r0, #0x7000000      @ extract loc from clidr
60    mov r3, r3, lsr #23         @ left align loc bit field
61    beq finished            @ if loc is 0, then no need to clean
62    mov r10, #0             @ start clean at cache level 0
63loop1:
64    add r2, r10, r10, lsr #1        @ work out 3x current cache level
65    mov r1, r0, lsr r2          @ extract cache type bits from clidr
66    and r1, r1, #7          @ mask of the bits for current cache only
67    cmp r1, #2              @ see what cache we have at this level
68    blt skip                @ skip if no cache, or just i-cache
69    mcr p15, 2, r10, c0, c0, 0      @ select current cache level in cssr
70    isb                 @ isb to sych the new cssr&csidr
71    mrc p15, 1, r1, c0, c0, 0       @ read the new csidr
72    and r2, r1, #7          @ extract the length of the cache lines
73    add r2, r2, #4          @ add 4 (line length offset)
74    ldr r4, =0x3ff
75    ands    r4, r4, r1, lsr #3      @ find maximum number on the way size
76    clz r5, r4              @ find bit position of way size increment
77    ldr r7, =0x7fff
78    ands    r7, r7, r1, lsr #13     @ extract max number of the index size
79loop2:
80    mov r9, r4              @ create working copy of max way size
81loop3:
82    orr r11, r10, r9, lsl r5        @ factor way and cache number into r11
83    orr r11, r11, r7, lsl r2        @ factor index number into r11
84    mcr p15, 0, r11, c7, c14, 2     @ clean & invalidate by set/way
85    subs    r9, r9, #1          @ decrement the way
86    bge loop3
87    subs    r7, r7, #1          @ decrement the index
88    bge loop2
89skip:
90    add r10, r10, #2            @ increment cache number
91        cmp r3, r10
92        bgt loop1
93finished:
94    mov r10, #0             @ swith back to cache level 0
95    mcr p15, 2, r10, c0, c0, 0      @ select current cache level in cssr
96    dsb
97    isb
98    mov pc, lr
99#if 0
100    push    {r4-r11}
101    dmb
102    mrc     p15, #1, r0, c0, c0, #1  @ read clid register
103    ands    r3, r0, #0x7000000       @ get level of coherency
104    mov     r3, r3, lsr #23
105    beq     finished
106    mov     r10, #0
107loop1:
108    add     r2, r10, r10, lsr #1
109    mov     r1, r0, lsr r2
110    and     r1, r1, #7
111    cmp     r1, #2
112    blt     skip
113    mcr     p15, #2, r10, c0, c0, #0
114    isb
115    mrc     p15, #1, r1, c0, c0, #0
116    and     r2, r1, #7
117    add     r2, r2, #4
118    ldr     r4, _FLD_MAX_WAY
119    ands    r4, r4, r1, lsr #3
120    clz     r5, r4
121    ldr     r7, _FLD_MAX_IDX
122    ands    r7, r7, r1, lsr #13
123loop2:
124    mov     r9, r4
125loop3:
126    orr     r11, r10, r9, lsl r5
127    orr     r11, r11, r7, lsl r2
128    mcr     p15, #0, r11, c7, c14, #2
129    subs    r9, r9, #1
130    bge     loop3
131    subs    r7, r7, #1
132    bge     loop2
133skip:
134    add     r10, r10, #2
135    cmp     r3, r10
136    bgt     loop1
137
138finished:
139    dsb
140    isb
141    pop     {r4-r11}
142    bx      lr
143#endif
144.globl rt_cpu_icache_flush
145rt_cpu_icache_flush:
146    mov r0, #0
147    mcr p15, 0, r0, c7, c5, 0       @ I+BTB cache invalidate
148    dsb
149    isb
150    bx      lr
151
152.globl rt_hw_cpu_dcache_disable
153rt_hw_cpu_dcache_disable:
154    push    {r4-r11, lr}
155    bl      rt_cpu_dcache_clean_flush
156    mrc     p15, #0, r0, c1, c0, #0
157    bic     r0,  r0, #0x00000004
158    mcr     p15, #0, r0, c1, c0, #0
159    pop     {r4-r11, lr}
160    bx      lr
161
162.globl rt_hw_cpu_icache_disable
163rt_hw_cpu_icache_disable:
164    mrc     p15, #0, r0, c1, c0, #0
165    bic     r0,  r0, #0x00001000
166    mcr     p15, #0, r0, c1, c0, #0
167    bx      lr
168