1/*- 2 * Copyright (c) 2014 Andrew Turner 3 * Copyright (c) 2014 The FreeBSD Foundation 4 * All rights reserved. 5 * 6 * This software was developed by Andrew Turner under sponsorship from 7 * the FreeBSD Foundation. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 1. Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * 2. Redistributions in binary form must reproduce the above copyright 15 * notice, this list of conditions and the following disclaimer in the 16 * documentation and/or other materials provided with the distribution. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28 * SUCH DAMAGE. 29 * 30 */ 31 32#include "assym.inc" 33#include "opt_kstack_pages.h" 34#include "opt_sched.h" 35 36#include <machine/asm.h> 37 38__FBSDID("$FreeBSD$"); 39 40.macro clear_step_flag pcbflags, tmp 41 tbz \pcbflags, #PCB_SINGLE_STEP_SHIFT, 999f 42 mrs \tmp, mdscr_el1 43 bic \tmp, \tmp, #1 44 msr mdscr_el1, \tmp 45 isb 46999: 47.endm 48 49.macro set_step_flag pcbflags, tmp 50 tbz \pcbflags, #PCB_SINGLE_STEP_SHIFT, 999f 51 mrs \tmp, mdscr_el1 52 orr \tmp, \tmp, #1 53 msr mdscr_el1, \tmp 54 isb 55999: 56.endm 57 58/* 59 * void cpu_throw(struct thread *old, struct thread *new) 60 */ 61ENTRY(cpu_throw) 62 /* Of old == NULL skip disabling stepping */ 63 cbz x0, 1f 64 65 /* If we were single stepping, disable it */ 66 ldr x4, [x0, #TD_PCB] 67 ldr w5, [x4, #PCB_FLAGS] 68 clear_step_flag w5, x6 691: 70 71#ifdef VFP 72 /* Backup the new thread pointer around a call to C code */ 73 mov x19, x0 74 mov x20, x1 75 bl vfp_discard 76 mov x1, x20 77 mov x0, x19 78#endif 79 80 bl pmap_switch 81 mov x4, x0 82 83 /* If we are single stepping, enable it */ 84 ldr w5, [x4, #PCB_FLAGS] 85 set_step_flag w5, x6 86 87 /* Restore the registers */ 88 ldp x5, x6, [x4, #PCB_SP] 89 mov sp, x5 90 msr tpidr_el0, x6 91 ldr x6, [x4, #PCB_TPIDRRO] 92 msr tpidrro_el0, x6 93 ldp x8, x9, [x4, #PCB_REGS + 8 * 8] 94 ldp x10, x11, [x4, #PCB_REGS + 10 * 8] 95 ldp x12, x13, [x4, #PCB_REGS + 12 * 8] 96 ldp x14, x15, [x4, #PCB_REGS + 14 * 8] 97 ldp x16, x17, [x4, #PCB_REGS + 16 * 8] 98 ldr x19, [x4, #PCB_REGS + 19 * 8] 99 ldp x20, x21, [x4, #PCB_REGS + 20 * 8] 100 ldp x22, x23, [x4, #PCB_REGS + 22 * 8] 101 ldp x24, x25, [x4, #PCB_REGS + 24 * 8] 102 ldp x26, x27, [x4, #PCB_REGS + 26 * 8] 103 ldp x28, x29, [x4, #PCB_REGS + 28 * 8] 104 ldr x30, [x4, #PCB_REGS + 30 * 8] 105 106 ret 107END(cpu_throw) 108 109/* 110 * void cpu_switch(struct thread *old, struct thread *new, struct mtx *mtx) 111 * 112 * x0 = old 113 * x1 = new 114 * x2 = mtx 115 * x3 to x7, x16 and x17 are caller saved 116 */ 117ENTRY(cpu_switch) 118 /* 119 * Save the old context. 120 */ 121 ldr x4, [x0, #TD_PCB] 122 123 /* Store the callee-saved registers */ 124 stp x8, x9, [x4, #PCB_REGS + 8 * 8] 125 stp x10, x11, [x4, #PCB_REGS + 10 * 8] 126 stp x12, x13, [x4, #PCB_REGS + 12 * 8] 127 stp x14, x15, [x4, #PCB_REGS + 14 * 8] 128 stp x16, x17, [x4, #PCB_REGS + 16 * 8] 129 stp x18, x19, [x4, #PCB_REGS + 18 * 8] 130 stp x20, x21, [x4, #PCB_REGS + 20 * 8] 131 stp x22, x23, [x4, #PCB_REGS + 22 * 8] 132 stp x24, x25, [x4, #PCB_REGS + 24 * 8] 133 stp x26, x27, [x4, #PCB_REGS + 26 * 8] 134 stp x28, x29, [x4, #PCB_REGS + 28 * 8] 135 str x30, [x4, #PCB_REGS + 30 * 8] 136 /* And the old stack pointer */ 137 mov x5, sp 138 mrs x6, tpidrro_el0 139 str x6, [x4, #PCB_TPIDRRO] 140 mrs x6, tpidr_el0 141 stp x5, x6, [x4, #PCB_SP] 142 143 /* If we were single stepping, disable it */ 144 ldr w5, [x4, #PCB_FLAGS] 145 clear_step_flag w5, x6 146 147 mov x19, x0 148 mov x20, x1 149 mov x21, x2 150 151#ifdef VFP 152 /* Load the pcb address */ 153 mov x1, x4 154 bl vfp_save_state 155 mov x1, x20 156 mov x0, x19 157#endif 158 159 bl pmap_switch 160 /* Move the new pcb out of the way */ 161 mov x4, x0 162 163 mov x2, x21 164 mov x1, x20 165 mov x0, x19 166 167 /* 168 * Release the old thread. 169 */ 170 stlr x2, [x0, #TD_LOCK] 171#if defined(SCHED_ULE) && defined(SMP) 172 /* Spin if TD_LOCK points to a blocked_lock */ 173 ldr x2, =_C_LABEL(blocked_lock) 1741: 175 ldar x3, [x1, #TD_LOCK] 176 cmp x3, x2 177 b.eq 1b 178#endif 179 180 /* If we are single stepping, enable it */ 181 ldr w5, [x4, #PCB_FLAGS] 182 set_step_flag w5, x6 183 184 /* Restore the registers */ 185 ldp x5, x6, [x4, #PCB_SP] 186 mov sp, x5 187 msr tpidr_el0, x6 188 ldr x6, [x4, #PCB_TPIDRRO] 189 msr tpidrro_el0, x6 190 ldp x8, x9, [x4, #PCB_REGS + 8 * 8] 191 ldp x10, x11, [x4, #PCB_REGS + 10 * 8] 192 ldp x12, x13, [x4, #PCB_REGS + 12 * 8] 193 ldp x14, x15, [x4, #PCB_REGS + 14 * 8] 194 ldp x16, x17, [x4, #PCB_REGS + 16 * 8] 195 ldr x19, [x4, #PCB_REGS + 19 * 8] 196 ldp x20, x21, [x4, #PCB_REGS + 20 * 8] 197 ldp x22, x23, [x4, #PCB_REGS + 22 * 8] 198 ldp x24, x25, [x4, #PCB_REGS + 24 * 8] 199 ldp x26, x27, [x4, #PCB_REGS + 26 * 8] 200 ldp x28, x29, [x4, #PCB_REGS + 28 * 8] 201 ldr x30, [x4, #PCB_REGS + 30 * 8] 202 203 str xzr, [x4, #PCB_REGS + 18 * 8] 204 ret 205.Lcpu_switch_panic_str: 206 .asciz "cpu_switch: %p\0" 207END(cpu_switch) 208 209ENTRY(fork_trampoline) 210 mov x0, x8 211 mov x1, x9 212 mov x2, sp 213 mov fp, #0 /* Stack traceback stops here. */ 214 bl _C_LABEL(fork_exit) 215 216 /* Restore the registers other than x0 and x1 */ 217 ldp x2, x3, [sp, #TF_X + 2 * 8] 218 ldp x4, x5, [sp, #TF_X + 4 * 8] 219 ldp x6, x7, [sp, #TF_X + 6 * 8] 220 ldp x8, x9, [sp, #TF_X + 8 * 8] 221 ldp x10, x11, [sp, #TF_X + 10 * 8] 222 ldp x12, x13, [sp, #TF_X + 12 * 8] 223 ldp x14, x15, [sp, #TF_X + 14 * 8] 224 ldp x16, x17, [sp, #TF_X + 16 * 8] 225 ldr x19, [sp, #TF_X + 19 * 8] 226 ldp x20, x21, [sp, #TF_X + 20 * 8] 227 ldp x22, x23, [sp, #TF_X + 22 * 8] 228 ldp x24, x25, [sp, #TF_X + 24 * 8] 229 ldp x26, x27, [sp, #TF_X + 26 * 8] 230 ldp x28, x29, [sp, #TF_X + 28 * 8] 231 232 /* 233 * Disable interrupts to avoid 234 * overwriting spsr_el1 and sp_el0 by an IRQ exception. 235 */ 236 msr daifset, #2 237 238 /* Restore sp and lr */ 239 ldp x0, x1, [sp, #TF_SP] 240 msr sp_el0, x0 241 mov lr, x1 242 243 /* Restore elr and spsr */ 244 ldp x0, x1, [sp, #TF_ELR] 245 msr elr_el1, x0 246 msr spsr_el1, x1 247 248 /* Finally x0 and x1 */ 249 ldp x0, x1, [sp, #TF_X + 0 * 8] 250 ldr x18, [sp, #TF_X + 18 * 8] 251 252 /* 253 * No need for interrupts reenabling since PSR 254 * will be set to the desired value anyway. 255 */ 256 ERET 257 258END(fork_trampoline) 259 260ENTRY(savectx) 261 /* Store the callee-saved registers */ 262 stp x8, x9, [x0, #PCB_REGS + 8 * 8] 263 stp x10, x11, [x0, #PCB_REGS + 10 * 8] 264 stp x12, x13, [x0, #PCB_REGS + 12 * 8] 265 stp x14, x15, [x0, #PCB_REGS + 14 * 8] 266 stp x16, x17, [x0, #PCB_REGS + 16 * 8] 267 stp x18, x19, [x0, #PCB_REGS + 18 * 8] 268 stp x20, x21, [x0, #PCB_REGS + 20 * 8] 269 stp x22, x23, [x0, #PCB_REGS + 22 * 8] 270 stp x24, x25, [x0, #PCB_REGS + 24 * 8] 271 stp x26, x27, [x0, #PCB_REGS + 26 * 8] 272 stp x28, x29, [x0, #PCB_REGS + 28 * 8] 273 str x30, [x0, #PCB_REGS + 30 * 8] 274 /* And the old stack pointer */ 275 mov x5, sp 276 mrs x6, tpidrro_el0 277 str x6, [x0, #PCB_TPIDRRO] 278 mrs x6, tpidr_el0 279 stp x5, x6, [x0, #PCB_SP] 280 281 /* Store the VFP registers */ 282#ifdef VFP 283 mov x28, lr 284 mov x1, x0 /* move pcb to the correct register */ 285 mov x0, xzr /* td = NULL */ 286 bl vfp_save_state 287 mov lr, x28 288#endif 289 290 ret 291END(savectx) 292 293