cpufunc_asm_armv7.S revision 278518
1239268Sgonzo/*- 2262420Sian * Copyright (c) 2010 Per Odlund <per.odlund@armagedon.se> 3239268Sgonzo * Copyright (C) 2011 MARVELL INTERNATIONAL LTD. 4239268Sgonzo * All rights reserved. 5239268Sgonzo * 6239268Sgonzo * Developed by Semihalf. 7239268Sgonzo * 8239268Sgonzo * Redistribution and use in source and binary forms, with or without 9239268Sgonzo * modification, are permitted provided that the following conditions 10239268Sgonzo * are met: 11239268Sgonzo * 1. Redistributions of source code must retain the above copyright 12239268Sgonzo * notice, this list of conditions and the following disclaimer. 13239268Sgonzo * 2. Redistributions in binary form must reproduce the above copyright 14239268Sgonzo * notice, this list of conditions and the following disclaimer in the 15239268Sgonzo * documentation and/or other materials provided with the distribution. 16239268Sgonzo * 3. Neither the name of MARVELL nor the names of contributors 17239268Sgonzo * may be used to endorse or promote products derived from this software 18239268Sgonzo * without specific prior written permission. 19239268Sgonzo * 20239268Sgonzo * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND 21239268Sgonzo * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22239268Sgonzo * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23239268Sgonzo * ARE DISCLAIMED. IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE 24239268Sgonzo * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25239268Sgonzo * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26239268Sgonzo * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27239268Sgonzo * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28239268Sgonzo * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29239268Sgonzo * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30239268Sgonzo * SUCH DAMAGE. 31239268Sgonzo */ 32239268Sgonzo 33239268Sgonzo#include <machine/asm.h> 34239268Sgonzo__FBSDID("$FreeBSD: head/sys/arm/arm/cpufunc_asm_armv7.S 278518 2015-02-10 14:11:23Z zbb $"); 35239268Sgonzo 36272209Sandrew#include <machine/sysreg.h> 37272209Sandrew 38239268Sgonzo .cpu cortex-a8 39239268Sgonzo 40239268Sgonzo.Lcoherency_level: 41239268Sgonzo .word _C_LABEL(arm_cache_loc) 42239268Sgonzo.Lcache_type: 43239268Sgonzo .word _C_LABEL(arm_cache_type) 44278518Szbb.Larmv7_dcache_line_size: 45278518Szbb .word _C_LABEL(arm_dcache_min_line_size) 46278518Szbb.Larmv7_icache_line_size: 47278518Szbb .word _C_LABEL(arm_icache_min_line_size) 48278518Szbb.Larmv7_idcache_line_size: 49278518Szbb .word _C_LABEL(arm_idcache_min_line_size) 50239268Sgonzo.Lway_mask: 51239268Sgonzo .word 0x3ff 52239268Sgonzo.Lmax_index: 53239268Sgonzo .word 0x7fff 54239268Sgonzo.Lpage_mask: 55239268Sgonzo .word 0xfff 56239268Sgonzo 57239268Sgonzo#define PT_NOS (1 << 5) 58239268Sgonzo#define PT_S (1 << 1) 59239268Sgonzo#define PT_INNER_NC 0 60239268Sgonzo#define PT_INNER_WT (1 << 0) 61239268Sgonzo#define PT_INNER_WB ((1 << 0) | (1 << 6)) 62239268Sgonzo#define PT_INNER_WBWA (1 << 6) 63239268Sgonzo#define PT_OUTER_NC 0 64239268Sgonzo#define PT_OUTER_WT (2 << 3) 65239268Sgonzo#define PT_OUTER_WB (3 << 3) 66239268Sgonzo#define PT_OUTER_WBWA (1 << 3) 67239268Sgonzo 68239268Sgonzo#ifdef SMP 69256707Scognet#define PT_ATTR (PT_S|PT_INNER_WBWA|PT_OUTER_WBWA|PT_NOS) 70239268Sgonzo#else 71256707Scognet#define PT_ATTR (PT_INNER_WBWA|PT_OUTER_WBWA) 72239268Sgonzo#endif 73239268Sgonzo 74239268SgonzoENTRY(armv7_setttb) 75239268Sgonzo stmdb sp!, {r0, lr} 76239268Sgonzo bl _C_LABEL(armv7_idcache_wbinv_all) /* clean the D cache */ 77239268Sgonzo ldmia sp!, {r0, lr} 78239268Sgonzo dsb 79239268Sgonzo 80239268Sgonzo orr r0, r0, #PT_ATTR 81272209Sandrew mcr CP15_TTBR0(r0) 82264128Sian isb 83243107Scognet#ifdef SMP 84272209Sandrew mcr CP15_TLBIALLIS 85243107Scognet#else 86272209Sandrew mcr CP15_TLBIALL 87243107Scognet#endif 88239268Sgonzo dsb 89239268Sgonzo isb 90239268Sgonzo RET 91248361SandrewEND(armv7_setttb) 92239268Sgonzo 93239268SgonzoENTRY(armv7_tlb_flushID) 94239268Sgonzo dsb 95239268Sgonzo#ifdef SMP 96272209Sandrew mcr CP15_TLBIALLIS 97272209Sandrew mcr CP15_BPIALLIS 98239268Sgonzo#else 99272209Sandrew mcr CP15_TLBIALL 100272209Sandrew mcr CP15_BPIALL 101239268Sgonzo#endif 102239268Sgonzo dsb 103239268Sgonzo isb 104239268Sgonzo mov pc, lr 105248361SandrewEND(armv7_tlb_flushID) 106239268Sgonzo 107239268SgonzoENTRY(armv7_tlb_flushID_SE) 108239268Sgonzo ldr r1, .Lpage_mask 109239268Sgonzo bic r0, r0, r1 110239268Sgonzo#ifdef SMP 111272209Sandrew mcr CP15_TLBIMVAAIS(r0) 112272209Sandrew mcr CP15_BPIALLIS 113239268Sgonzo#else 114272209Sandrew mcr CP15_TLBIMVA(r0) 115272209Sandrew mcr CP15_BPIALL 116239268Sgonzo#endif 117239268Sgonzo dsb 118239268Sgonzo isb 119239268Sgonzo mov pc, lr 120248361SandrewEND(armv7_tlb_flushID_SE) 121239268Sgonzo 122239268Sgonzo/* Based on algorithm from ARM Architecture Reference Manual */ 123239268SgonzoENTRY(armv7_dcache_wbinv_all) 124239268Sgonzo stmdb sp!, {r4, r5, r6, r7, r8, r9} 125239268Sgonzo 126239268Sgonzo /* Get cache level */ 127239268Sgonzo ldr r0, .Lcoherency_level 128239268Sgonzo ldr r3, [r0] 129239268Sgonzo cmp r3, #0 130239268Sgonzo beq Finished 131239268Sgonzo /* For each cache level */ 132239268Sgonzo mov r8, #0 133239268SgonzoLoop1: 134239268Sgonzo /* Get cache type for given level */ 135239268Sgonzo mov r2, r8, lsl #2 136239268Sgonzo add r2, r2, r2 137239268Sgonzo ldr r0, .Lcache_type 138239268Sgonzo ldr r1, [r0, r2] 139239268Sgonzo 140239268Sgonzo /* Get line size */ 141239268Sgonzo and r2, r1, #7 142239268Sgonzo add r2, r2, #4 143239268Sgonzo 144239268Sgonzo /* Get number of ways */ 145239268Sgonzo ldr r4, .Lway_mask 146239268Sgonzo ands r4, r4, r1, lsr #3 147239268Sgonzo clz r5, r4 148239268Sgonzo 149239268Sgonzo /* Get max index */ 150239268Sgonzo ldr r7, .Lmax_index 151239268Sgonzo ands r7, r7, r1, lsr #13 152239268SgonzoLoop2: 153239268Sgonzo mov r9, r4 154239268SgonzoLoop3: 155239268Sgonzo mov r6, r8, lsl #1 156239268Sgonzo orr r6, r6, r9, lsl r5 157239268Sgonzo orr r6, r6, r7, lsl r2 158239268Sgonzo 159239268Sgonzo /* Clean and invalidate data cache by way/index */ 160272209Sandrew mcr CP15_DCCISW(r6) 161239268Sgonzo subs r9, r9, #1 162239268Sgonzo bge Loop3 163239268Sgonzo subs r7, r7, #1 164239268Sgonzo bge Loop2 165239268SgonzoSkip: 166239268Sgonzo add r8, r8, #1 167239268Sgonzo cmp r3, r8 168239268Sgonzo bne Loop1 169239268SgonzoFinished: 170239268Sgonzo dsb 171239268Sgonzo ldmia sp!, {r4, r5, r6, r7, r8, r9} 172239268Sgonzo RET 173248361SandrewEND(armv7_dcache_wbinv_all) 174239268Sgonzo 175239268SgonzoENTRY(armv7_idcache_wbinv_all) 176239268Sgonzo stmdb sp!, {lr} 177239268Sgonzo bl armv7_dcache_wbinv_all 178243107Scognet#ifdef SMP 179272209Sandrew mcr CP15_ICIALLUIS 180243107Scognet#else 181272209Sandrew mcr CP15_ICIALLU 182243107Scognet#endif 183239268Sgonzo dsb 184239268Sgonzo isb 185239268Sgonzo ldmia sp!, {lr} 186239268Sgonzo RET 187248361SandrewEND(armv7_idcache_wbinv_all) 188239268Sgonzo 189239268SgonzoENTRY(armv7_dcache_wb_range) 190278518Szbb ldr ip, .Larmv7_dcache_line_size 191278518Szbb ldr ip, [ip] 192239268Sgonzo sub r3, ip, #1 193239268Sgonzo and r2, r0, r3 194239268Sgonzo add r1, r1, r2 195239268Sgonzo bic r0, r0, r3 196239268Sgonzo.Larmv7_wb_next: 197272209Sandrew mcr CP15_DCCMVAC(r0) 198239268Sgonzo add r0, r0, ip 199239268Sgonzo subs r1, r1, ip 200239268Sgonzo bhi .Larmv7_wb_next 201239268Sgonzo dsb /* data synchronization barrier */ 202239268Sgonzo RET 203248361SandrewEND(armv7_dcache_wb_range) 204239268Sgonzo 205239268SgonzoENTRY(armv7_dcache_wbinv_range) 206278518Szbb ldr ip, .Larmv7_dcache_line_size 207278518Szbb ldr ip, [ip] 208239268Sgonzo sub r3, ip, #1 209239268Sgonzo and r2, r0, r3 210239268Sgonzo add r1, r1, r2 211239268Sgonzo bic r0, r0, r3 212239268Sgonzo.Larmv7_wbinv_next: 213272209Sandrew mcr CP15_DCCIMVAC(r0) 214239268Sgonzo add r0, r0, ip 215239268Sgonzo subs r1, r1, ip 216239268Sgonzo bhi .Larmv7_wbinv_next 217239268Sgonzo dsb /* data synchronization barrier */ 218239268Sgonzo RET 219248361SandrewEND(armv7_dcache_wbinv_range) 220239268Sgonzo 221239268Sgonzo/* 222239268Sgonzo * Note, we must not invalidate everything. If the range is too big we 223239268Sgonzo * must use wb-inv of the entire cache. 224239268Sgonzo */ 225239268SgonzoENTRY(armv7_dcache_inv_range) 226278518Szbb ldr ip, .Larmv7_dcache_line_size 227278518Szbb ldr ip, [ip] 228239268Sgonzo sub r3, ip, #1 229239268Sgonzo and r2, r0, r3 230239268Sgonzo add r1, r1, r2 231239268Sgonzo bic r0, r0, r3 232239268Sgonzo.Larmv7_inv_next: 233272209Sandrew mcr CP15_DCIMVAC(r0) 234239268Sgonzo add r0, r0, ip 235239268Sgonzo subs r1, r1, ip 236239268Sgonzo bhi .Larmv7_inv_next 237239268Sgonzo dsb /* data synchronization barrier */ 238239268Sgonzo RET 239248361SandrewEND(armv7_dcache_inv_range) 240239268Sgonzo 241239268SgonzoENTRY(armv7_idcache_wbinv_range) 242278518Szbb ldr ip, .Larmv7_idcache_line_size 243278518Szbb ldr ip, [ip] 244239268Sgonzo sub r3, ip, #1 245239268Sgonzo and r2, r0, r3 246239268Sgonzo add r1, r1, r2 247239268Sgonzo bic r0, r0, r3 248239268Sgonzo.Larmv7_id_wbinv_next: 249272209Sandrew mcr CP15_ICIMVAU(r0) 250272209Sandrew mcr CP15_DCCIMVAC(r0) 251239268Sgonzo add r0, r0, ip 252239268Sgonzo subs r1, r1, ip 253239268Sgonzo bhi .Larmv7_id_wbinv_next 254239268Sgonzo isb /* instruction synchronization barrier */ 255239268Sgonzo dsb /* data synchronization barrier */ 256239268Sgonzo RET 257248361SandrewEND(armv7_idcache_wbinv_range) 258239268Sgonzo 259264994SianENTRY_NP(armv7_icache_sync_all) 260265036Sian#ifdef SMP 261272209Sandrew mcr CP15_ICIALLUIS 262265036Sian#else 263272209Sandrew mcr CP15_ICIALLU 264265036Sian#endif 265264994Sian isb /* instruction synchronization barrier */ 266264994Sian dsb /* data synchronization barrier */ 267264994Sian RET 268264994SianEND(armv7_icache_sync_all) 269264994Sian 270239268SgonzoENTRY_NP(armv7_icache_sync_range) 271278518Szbb ldr ip, .Larmv7_icache_line_size 272278518Szbb ldr ip, [ip] 273239268Sgonzo.Larmv7_sync_next: 274272209Sandrew mcr CP15_ICIMVAU(r0) 275272209Sandrew mcr CP15_DCCMVAC(r0) 276239268Sgonzo add r0, r0, ip 277239268Sgonzo subs r1, r1, ip 278239268Sgonzo bhi .Larmv7_sync_next 279239268Sgonzo isb /* instruction synchronization barrier */ 280239268Sgonzo dsb /* data synchronization barrier */ 281239268Sgonzo RET 282248361SandrewEND(armv7_icache_sync_range) 283239268Sgonzo 284239268SgonzoENTRY(armv7_cpu_sleep) 285239268Sgonzo dsb /* data synchronization barrier */ 286239268Sgonzo wfi /* wait for interrupt */ 287239268Sgonzo RET 288248361SandrewEND(armv7_cpu_sleep) 289239268Sgonzo 290239268SgonzoENTRY(armv7_context_switch) 291239268Sgonzo dsb 292239268Sgonzo orr r0, r0, #PT_ATTR 293272209Sandrew 294272209Sandrew mcr CP15_TTBR0(r0) 295264128Sian isb 296243107Scognet#ifdef SMP 297272209Sandrew mcr CP15_TLBIALLIS 298243107Scognet#else 299272209Sandrew mcr CP15_TLBIALL 300243107Scognet#endif 301239268Sgonzo dsb 302239268Sgonzo isb 303239268Sgonzo RET 304248361SandrewEND(armv7_context_switch) 305239268Sgonzo 306239268SgonzoENTRY(armv7_drain_writebuf) 307239268Sgonzo dsb 308239268Sgonzo RET 309248361SandrewEND(armv7_drain_writebuf) 310239268Sgonzo 311239268SgonzoENTRY(armv7_sev) 312239268Sgonzo dsb 313239268Sgonzo sev 314239268Sgonzo nop 315239268Sgonzo RET 316248361SandrewEND(armv7_sev) 317239268Sgonzo 318239268SgonzoENTRY(armv7_auxctrl) 319272209Sandrew mrc CP15_ACTLR(r2) 320239268Sgonzo bic r3, r2, r0 /* Clear bits */ 321239268Sgonzo eor r3, r3, r1 /* XOR bits */ 322239268Sgonzo 323239268Sgonzo teq r2, r3 324272209Sandrew mcrne CP15_ACTLR(r3) 325239268Sgonzo mov r0, r2 326239268Sgonzo RET 327248361SandrewEND(armv7_auxctrl) 328248361Sandrew 329265784Sian/* 330265784Sian * Invalidate all I+D+branch cache. Used by startup code, which counts 331265784Sian * on the fact that only r0-r3,ip are modified and no stack space is used. 332265784Sian */ 333262420SianENTRY(armv7_idcache_inv_all) 334262420Sian mov r0, #0 335272209Sandrew mcr CP15_CSSELR(r0) @ set cache level to L1 336272209Sandrew mrc CP15_CCSIDR(r0) 337262420Sian 338262420Sian ubfx r2, r0, #13, #15 @ get num sets - 1 from CCSIDR 339262420Sian ubfx r3, r0, #3, #10 @ get numways - 1 from CCSIDR 340262420Sian clz r1, r3 @ number of bits to MSB of way 341262420Sian lsl r3, r3, r1 @ shift into position 342262420Sian mov ip, #1 @ 343262420Sian lsl ip, ip, r1 @ ip now contains the way decr 344262420Sian 345262420Sian ubfx r0, r0, #0, #3 @ get linesize from CCSIDR 346262420Sian add r0, r0, #4 @ apply bias 347262420Sian lsl r2, r2, r0 @ shift sets by log2(linesize) 348262420Sian add r3, r3, r2 @ merge numsets - 1 with numways - 1 349262420Sian sub ip, ip, r2 @ subtract numsets - 1 from way decr 350262420Sian mov r1, #1 351262420Sian lsl r1, r1, r0 @ r1 now contains the set decr 352262420Sian mov r2, ip @ r2 now contains set way decr 353262420Sian 354262420Sian /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */ 355272209Sandrew1: mcr CP15_DCISW(r3) @ invalidate line 356262420Sian movs r0, r3 @ get current way/set 357262420Sian beq 2f @ at 0 means we are done. 358262420Sian movs r0, r0, lsl #10 @ clear way bits leaving only set bits 359262420Sian subne r3, r3, r1 @ non-zero?, decrement set # 360262420Sian subeq r3, r3, r2 @ zero?, decrement way # and restore set count 361262420Sian b 1b 362262420Sian 363262420Sian2: dsb @ wait for stores to finish 364262420Sian mov r0, #0 @ and ... 365272209Sandrew mcr CP15_ICIALLU @ invalidate instruction+branch cache 366262420Sian isb @ instruction sync barrier 367262420Sian bx lr @ return 368269390SianEND(armv7_idcache_inv_all) 369262420Sian 370262587SianENTRY_NP(armv7_sleep) 371262587Sian dsb 372262587Sian wfi 373262587Sian bx lr 374262587SianEND(armv7_sleep) 375262587Sian 376