swtch-v4.S revision 262941
1129198Scognet/* $NetBSD: cpuswitch.S,v 1.41 2003/11/15 08:44:18 scw Exp $ */ 2129198Scognet 3139735Simp/*- 4129198Scognet * Copyright 2003 Wasabi Systems, Inc. 5129198Scognet * All rights reserved. 6129198Scognet * 7129198Scognet * Written by Steve C. Woodford for Wasabi Systems, Inc. 8129198Scognet * 9129198Scognet * Redistribution and use in source and binary forms, with or without 10129198Scognet * modification, are permitted provided that the following conditions 11129198Scognet * are met: 12129198Scognet * 1. Redistributions of source code must retain the above copyright 13129198Scognet * notice, this list of conditions and the following disclaimer. 14129198Scognet * 2. Redistributions in binary form must reproduce the above copyright 15129198Scognet * notice, this list of conditions and the following disclaimer in the 16129198Scognet * documentation and/or other materials provided with the distribution. 17129198Scognet * 3. All advertising materials mentioning features or use of this software 18129198Scognet * must display the following acknowledgement: 19129198Scognet * This product includes software developed for the NetBSD Project by 20129198Scognet * Wasabi Systems, Inc. 21129198Scognet * 4. The name of Wasabi Systems, Inc. may not be used to endorse 22129198Scognet * or promote products derived from this software without specific prior 23129198Scognet * written permission. 24129198Scognet * 25129198Scognet * THIS SOFTWARE IS PROVIDED BY WASABI SYSTEMS, INC. ``AS IS'' AND 26129198Scognet * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 27129198Scognet * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 28129198Scognet * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL WASABI SYSTEMS, INC 29129198Scognet * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30129198Scognet * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31129198Scognet * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32129198Scognet * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33129198Scognet * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34129198Scognet * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35129198Scognet * POSSIBILITY OF SUCH DAMAGE. 36129198Scognet */ 37139735Simp/*- 38129198Scognet * Copyright (c) 1994-1998 Mark Brinicombe. 39129198Scognet * Copyright (c) 1994 Brini. 40129198Scognet * All rights reserved. 41129198Scognet * 42129198Scognet * This code is derived from software written for Brini by Mark Brinicombe 43129198Scognet * 44129198Scognet * Redistribution and use in source and binary forms, with or without 45129198Scognet * modification, are permitted provided that the following conditions 46129198Scognet * are met: 47129198Scognet * 1. Redistributions of source code must retain the above copyright 48129198Scognet * notice, this list of conditions and the following disclaimer. 49129198Scognet * 2. Redistributions in binary form must reproduce the above copyright 50129198Scognet * notice, this list of conditions and the following disclaimer in the 51129198Scognet * documentation and/or other materials provided with the distribution. 52129198Scognet * 3. All advertising materials mentioning features or use of this software 53129198Scognet * must display the following acknowledgement: 54129198Scognet * This product includes software developed by Brini. 55129198Scognet * 4. The name of the company nor the name of the author may be used to 56129198Scognet * endorse or promote products derived from this software without specific 57129198Scognet * prior written permission. 58129198Scognet * 59129198Scognet * THIS SOFTWARE IS PROVIDED BY BRINI ``AS IS'' AND ANY EXPRESS OR IMPLIED 60129198Scognet * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 61129198Scognet * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 62129198Scognet * IN NO EVENT SHALL BRINI OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 63129198Scognet * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 64129198Scognet * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 65129198Scognet * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 66129198Scognet * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 67129198Scognet * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 68129198Scognet * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 69129198Scognet * SUCH DAMAGE. 70129198Scognet * 71129198Scognet * RiscBSD kernel project 72129198Scognet * 73129198Scognet * cpuswitch.S 74129198Scognet * 75129198Scognet * cpu switching functions 76129198Scognet * 77129198Scognet * Created : 15/10/94 78129198Scognet * 79129198Scognet */ 80129198Scognet 81137274Scognet#include "assym.s" 82245477Scognet#include "opt_sched.h" 83137274Scognet 84129198Scognet#include <machine/asm.h> 85129198Scognet#include <machine/asmacros.h> 86129198Scognet#include <machine/armreg.h> 87262941Sian#include <machine/vfp.h> 88262941Sian 89129198Scognet__FBSDID("$FreeBSD: head/sys/arm/arm/swtch.S 262941 2014-03-09 03:00:03Z ian $"); 90129198Scognet 91129198Scognet#define DOMAIN_CLIENT 0x01 92129198Scognet 93261419Scognet#if defined(_ARM_ARCH_6) && defined(SMP) 94261415Scognet#define GET_PCPU(tmp, tmp2) \ 95261415Scognet mrc p15, 0, tmp, c0, c0, 5; \ 96261415Scognet and tmp, tmp, #0xf; \ 97261415Scognet ldr tmp2, .Lcurpcpu+4; \ 98261415Scognet mul tmp, tmp, tmp2; \ 99261415Scognet ldr tmp2, .Lcurpcpu; \ 100261415Scognet add tmp, tmp, tmp2; 101239268Sgonzo#else 102129198Scognet 103261415Scognet#define GET_PCPU(tmp, tmp2) \ 104239268Sgonzo ldr tmp, .Lcurpcpu 105239268Sgonzo#endif 106129198Scognet 107262941Sian#ifdef VFP 108262941Sian .fpu vfp /* allow VFP instructions */ 109262941Sian#endif 110262941Sian 111261415Scognet.Lcurpcpu: 112261415Scognet .word _C_LABEL(__pcpu) 113261415Scognet .word PCPU_SIZE 114129198Scognet.Lcpufuncs: 115129198Scognet .word _C_LABEL(cpufuncs) 116171780Scognet.Lblocked_lock: 117171780Scognet .word _C_LABEL(blocked_lock) 118239268Sgonzo 119135655ScognetENTRY(cpu_throw) 120135655Scognet mov r5, r1 121129198Scognet 122135655Scognet /* 123239268Sgonzo * r0 = oldtd 124137274Scognet * r5 = newtd 125135655Scognet */ 126129198Scognet 127261415Scognet GET_PCPU(r7, r9) 128239268Sgonzo 129254461Sandrew#ifdef VFP 130262941Sian fmrx r0, fpexc /* This thread is dying, if the VFP */ 131262941Sian tst r0, #(VFPEXC_EN) /* is enabled, go shut it down */ 132262941Sian blne _C_LABEL(vfp_discard) /* without preserving its state. */ 133262941Sian#endif 134239268Sgonzo 135137274Scognet ldr r7, [r5, #(TD_PCB)] /* r7 = new thread's PCB */ 136239268Sgonzo 137135655Scognet /* Switch to lwp0 context */ 138135655Scognet 139135655Scognet ldr r9, .Lcpufuncs 140259640Sganbold#if !defined(CPU_ARM11) && !defined(CPU_CORTEXA) && !defined(CPU_MV_PJ4B) && !defined(CPU_KRAIT) 141135655Scognet mov lr, pc 142135655Scognet ldr pc, [r9, #CF_IDCACHE_WBINV_ALL] 143239268Sgonzo#endif 144135655Scognet ldr r0, [r7, #(PCB_PL1VEC)] 145135655Scognet ldr r1, [r7, #(PCB_DACR)] 146135655Scognet /* 147135655Scognet * r0 = Pointer to L1 slot for vector_page (or NULL) 148135655Scognet * r1 = lwp0's DACR 149135655Scognet * r5 = lwp0 150135655Scognet * r6 = exit func 151135655Scognet * r7 = lwp0's PCB 152135655Scognet * r9 = cpufuncs 153135655Scognet */ 154135655Scognet 155135655Scognet /* 156135655Scognet * Ensure the vector table is accessible by fixing up lwp0's L1 157135655Scognet */ 158135655Scognet cmp r0, #0 /* No need to fixup vector table? */ 159135655Scognet ldrne r3, [r0] /* But if yes, fetch current value */ 160135655Scognet ldrne r2, [r7, #(PCB_L1VEC)] /* Fetch new vector_page value */ 161135655Scognet mcr p15, 0, r1, c3, c0, 0 /* Update DACR for lwp0's context */ 162135655Scognet cmpne r3, r2 /* Stuffing the same value? */ 163135655Scognet strne r2, [r0] /* Store if not. */ 164135655Scognet 165135655Scognet#ifdef PMAP_INCLUDE_PTE_SYNC 166135655Scognet /* 167135655Scognet * Need to sync the cache to make sure that last store is 168135655Scognet * visible to the MMU. 169135655Scognet */ 170135655Scognet movne r1, #4 171135655Scognet movne lr, pc 172135655Scognet ldrne pc, [r9, #CF_DCACHE_WB_RANGE] 173135655Scognet#endif /* PMAP_INCLUDE_PTE_SYNC */ 174135655Scognet 175135655Scognet /* 176135655Scognet * Note: We don't do the same optimisation as cpu_switch() with 177135655Scognet * respect to avoiding flushing the TLB if we're switching to 178135655Scognet * the same L1 since this process' VM space may be about to go 179135655Scognet * away, so we don't want *any* turds left in the TLB. 180135655Scognet */ 181135655Scognet 182135655Scognet /* Switch the memory to the new process */ 183135655Scognet ldr r0, [r7, #(PCB_PAGEDIR)] 184135655Scognet mov lr, pc 185135655Scognet ldr pc, [r9, #CF_CONTEXT_SWITCH] 186135655Scognet 187135655Scognet /* Restore all the save registers */ 188172614Scognet#ifndef _ARM_ARCH_5E 189135655Scognet add r1, r7, #PCB_R8 190135655Scognet ldmia r1, {r8-r13} 191135655Scognet#else 192135655Scognet ldr r8, [r7, #(PCB_R8)] 193135655Scognet ldr r9, [r7, #(PCB_R9)] 194135655Scognet ldr r10, [r7, #(PCB_R10)] 195135655Scognet ldr r11, [r7, #(PCB_R11)] 196135655Scognet ldr r12, [r7, #(PCB_R12)] 197135655Scognet ldr r13, [r7, #(PCB_SP)] 198135655Scognet#endif 199135655Scognet 200261415Scognet GET_PCPU(r6, r4) 201261415Scognet /* Hook in a new pcb */ 202261415Scognet str r7, [r6, #PC_CURPCB] 203138751Scognet /* We have a new curthread now so make a note it */ 204261415Scognet add r6, r6, #PC_CURTHREAD 205138751Scognet str r5, [r6] 206261415Scognet#ifndef ARM_TP_ADDRESS 207261415Scognet mcr p15, 0, r5, c13, c0, 4 208261415Scognet#endif 209142570Scognet /* Set the new tp */ 210142955Scognet ldr r6, [r5, #(TD_MD + MD_TP)] 211239268Sgonzo#ifdef ARM_TP_ADDRESS 212188540Scognet ldr r4, =ARM_TP_ADDRESS 213188540Scognet str r6, [r4] 214188540Scognet ldr r6, [r5, #(TD_MD + MD_RAS_START)] 215188540Scognet str r6, [r4, #4] /* ARM_RAS_START */ 216188540Scognet ldr r6, [r5, #(TD_MD + MD_RAS_END)] 217188581Scognet str r6, [r4, #8] /* ARM_RAS_END */ 218239268Sgonzo#else 219239268Sgonzo mcr p15, 0, r6, c13, c0, 3 220239268Sgonzo#endif 221138751Scognet 222247864Sandrew add sp, sp, #4; 223138856Scognet ldmfd sp!, {r4-r7, pc} 224248361SandrewEND(cpu_throw) 225138751Scognet 226129198ScognetENTRY(cpu_switch) 227129198Scognet stmfd sp!, {r4-r7, lr} 228247864Sandrew sub sp, sp, #4; 229254847Sandrew#ifdef __ARM_EABI__ 230254847Sandrew .save {r4-r7, lr} 231254847Sandrew .pad #4 232254847Sandrew#endif 233254847Sandrew 234171780Scognet mov r6, r2 /* Save the mutex */ 235129198Scognet 236135655Scognet.Lswitch_resume: 237137274Scognet /* rem: r0 = old lwp */ 238129198Scognet /* rem: interrupts are disabled */ 239129198Scognet 240129198Scognet /* Process is now on a processor. */ 241135655Scognet /* We have a new curthread now so make a note it */ 242261415Scognet GET_PCPU(r7, r2) 243261419Scognet str r1, [r7, #PC_CURTHREAD] 244261415Scognet#ifndef ARM_TP_ADDRESS 245261415Scognet mcr p15, 0, r1, c13, c0, 4 246261415Scognet#endif 247129198Scognet 248129198Scognet /* Hook in a new pcb */ 249137274Scognet ldr r2, [r1, #TD_PCB] 250239268Sgonzo str r2, [r7, #PC_CURPCB] 251129198Scognet 252137274Scognet /* rem: r1 = new process */ 253129198Scognet /* rem: interrupts are enabled */ 254129198Scognet 255129198Scognet /* Stage two : Save old context */ 256129198Scognet 257171780Scognet /* Get the user structure for the old thread. */ 258137274Scognet ldr r2, [r0, #(TD_PCB)] 259171780Scognet mov r4, r0 /* Save the old thread. */ 260129198Scognet 261171780Scognet /* Save all the registers in the old thread's pcb */ 262172614Scognet#ifndef _ARM_ARCH_5E 263137274Scognet add r7, r2, #(PCB_R8) 264129198Scognet stmia r7, {r8-r13} 265129198Scognet#else 266137274Scognet strd r8, [r2, #(PCB_R8)] 267137274Scognet strd r10, [r2, #(PCB_R10)] 268137274Scognet strd r12, [r2, #(PCB_R12)] 269129198Scognet#endif 270181144Scognet str pc, [r2, #(PCB_PC)] 271236991Simp 272129198Scognet /* 273129198Scognet * NOTE: We can now use r8-r13 until it is time to restore 274129198Scognet * them for the new process. 275129198Scognet */ 276239268Sgonzo#ifdef ARM_TP_ADDRESS 277142570Scognet /* Store the old tp */ 278175982Sraj ldr r3, =ARM_TP_ADDRESS 279188540Scognet ldr r9, [r3] 280142570Scognet str r9, [r0, #(TD_MD + MD_TP)] 281188540Scognet ldr r9, [r3, #4] 282188540Scognet str r9, [r0, #(TD_MD + MD_RAS_START)] 283188540Scognet ldr r9, [r3, #8] 284188540Scognet str r9, [r0, #(TD_MD + MD_RAS_END)] 285129198Scognet 286142570Scognet /* Set the new tp */ 287142570Scognet ldr r9, [r1, #(TD_MD + MD_TP)] 288188540Scognet str r9, [r3] 289188540Scognet ldr r9, [r1, #(TD_MD + MD_RAS_START)] 290188540Scognet str r9, [r3, #4] 291188540Scognet ldr r9, [r1, #(TD_MD + MD_RAS_END)] 292188540Scognet str r9, [r3, #8] 293239268Sgonzo#else 294239268Sgonzo /* Store the old tp */ 295239268Sgonzo mrc p15, 0, r9, c13, c0, 3 296239268Sgonzo str r9, [r0, #(TD_MD + MD_TP)] 297129198Scognet 298239268Sgonzo /* Set the new tp */ 299239268Sgonzo ldr r9, [r1, #(TD_MD + MD_TP)] 300239268Sgonzo mcr p15, 0, r9, c13, c0, 3 301239268Sgonzo#endif 302239268Sgonzo 303129198Scognet /* Get the user structure for the new process in r9 */ 304137274Scognet ldr r9, [r1, #(TD_PCB)] 305129198Scognet 306138751Scognet mrs r3, cpsr 307129198Scognet /* 308236991Simp * We can do that, since 309138751Scognet * PSR_SVC32_MODE|PSR_UND32_MODE == MSR_UND32_MODE 310129198Scognet */ 311138751Scognet orr r8, r3, #(PSR_UND32_MODE) 312138751Scognet msr cpsr_c, r8 313129198Scognet 314138751Scognet str sp, [r2, #(PCB_UND_SP)] 315129198Scognet 316129198Scognet msr cpsr_c, r3 /* Restore the old mode */ 317239268Sgonzo /* rem: r2 = old PCB */ 318129198Scognet /* rem: r9 = new PCB */ 319129198Scognet /* rem: interrupts are enabled */ 320129198Scognet 321254461Sandrew#ifdef VFP 322262941Sian fmrx r0, fpexc /* If the VFP is enabled */ 323262941Sian tst r0, #(VFPEXC_EN) /* the current thread has */ 324262941Sian movne r1, #1 /* used it, so go save */ 325262941Sian addne r0, r2, #(PCB_VFPSTATE) /* the state into the PCB */ 326262941Sian blne _C_LABEL(vfp_store) /* and disable the VFP. */ 327239268Sgonzo#endif 328129198Scognet 329262941Sian /* r0-r3 now free! */ 330239268Sgonzo 331129198Scognet /* Third phase : restore saved context */ 332129198Scognet 333239268Sgonzo /* rem: r2 = old PCB */ 334129198Scognet /* rem: r9 = new PCB */ 335129198Scognet /* rem: interrupts are enabled */ 336129198Scognet 337138414Scognet ldr r5, [r9, #(PCB_DACR)] /* r5 = new DACR */ 338138414Scognet mov r2, #DOMAIN_CLIENT 339138414Scognet cmp r5, r2, lsl #(PMAP_DOMAIN_KERNEL * 2) /* Sw to kernel thread? */ 340138414Scognet beq .Lcs_context_switched /* Yup. Don't flush cache */ 341138414Scognet mrc p15, 0, r0, c3, c0, 0 /* r0 = old DACR */ 342129198Scognet /* 343129198Scognet * Get the new L1 table pointer into r11. If we're switching to 344129198Scognet * an LWP with the same address space as the outgoing one, we can 345129198Scognet * skip the cache purge and the TTB load. 346129198Scognet * 347129198Scognet * To avoid data dep stalls that would happen anyway, we try 348129198Scognet * and get some useful work done in the mean time. 349129198Scognet */ 350138414Scognet mrc p15, 0, r10, c2, c0, 0 /* r10 = old L1 */ 351129198Scognet ldr r11, [r9, #(PCB_PAGEDIR)] /* r11 = new L1 */ 352129198Scognet 353129198Scognet 354129198Scognet teq r10, r11 /* Same L1? */ 355137274Scognet cmpeq r0, r5 /* Same DACR? */ 356129198Scognet beq .Lcs_context_switched /* yes! */ 357129198Scognet 358259640Sganbold#if !defined(CPU_ARM11) && !defined(CPU_CORTEXA) && !defined(CPU_MV_PJ4B) && !defined(CPU_KRAIT) 359129198Scognet /* 360129198Scognet * Definately need to flush the cache. 361129198Scognet */ 362129198Scognet 363129198Scognet ldr r1, .Lcpufuncs 364129198Scognet mov lr, pc 365129198Scognet ldr pc, [r1, #CF_IDCACHE_WBINV_ALL] 366239268Sgonzo#endif 367129198Scognet.Lcs_cache_purge_skipped: 368171780Scognet /* rem: r6 = lock */ 369129198Scognet /* rem: r9 = new PCB */ 370129198Scognet /* rem: r10 = old L1 */ 371129198Scognet /* rem: r11 = new L1 */ 372129198Scognet 373129198Scognet mov r2, #0x00000000 374129198Scognet ldr r7, [r9, #(PCB_PL1VEC)] 375129198Scognet 376129198Scognet /* 377129198Scognet * Ensure the vector table is accessible by fixing up the L1 378129198Scognet */ 379129198Scognet cmp r7, #0 /* No need to fixup vector table? */ 380129198Scognet ldrne r2, [r7] /* But if yes, fetch current value */ 381129198Scognet ldrne r0, [r9, #(PCB_L1VEC)] /* Fetch new vector_page value */ 382137274Scognet mcr p15, 0, r5, c3, c0, 0 /* Update DACR for new context */ 383129198Scognet cmpne r2, r0 /* Stuffing the same value? */ 384135655Scognet#ifndef PMAP_INCLUDE_PTE_SYNC 385129198Scognet strne r0, [r7] /* Nope, update it */ 386129198Scognet#else 387129198Scognet beq .Lcs_same_vector 388129198Scognet str r0, [r7] /* Otherwise, update it */ 389129198Scognet 390129198Scognet /* 391129198Scognet * Need to sync the cache to make sure that last store is 392129198Scognet * visible to the MMU. 393129198Scognet */ 394129198Scognet ldr r2, .Lcpufuncs 395129198Scognet mov r0, r7 396129198Scognet mov r1, #4 397129198Scognet mov lr, pc 398129198Scognet ldr pc, [r2, #CF_DCACHE_WB_RANGE] 399129198Scognet 400129198Scognet.Lcs_same_vector: 401129198Scognet#endif /* PMAP_INCLUDE_PTE_SYNC */ 402129198Scognet 403129198Scognet cmp r10, r11 /* Switching to the same L1? */ 404129198Scognet ldr r10, .Lcpufuncs 405129198Scognet beq .Lcs_same_l1 /* Yup. */ 406129198Scognet /* 407129198Scognet * Do a full context switch, including full TLB flush. 408129198Scognet */ 409129198Scognet mov r0, r11 410129198Scognet mov lr, pc 411129198Scognet ldr pc, [r10, #CF_CONTEXT_SWITCH] 412129198Scognet 413129198Scognet b .Lcs_context_switched 414129198Scognet 415129198Scognet /* 416129198Scognet * We're switching to a different process in the same L1. 417129198Scognet * In this situation, we only need to flush the TLB for the 418129198Scognet * vector_page mapping, and even then only if r7 is non-NULL. 419129198Scognet */ 420129198Scognet.Lcs_same_l1: 421129198Scognet cmp r7, #0 422129198Scognet movne r0, #0 /* We *know* vector_page's VA is 0x0 */ 423129198Scognet movne lr, pc 424129198Scognet ldrne pc, [r10, #CF_TLB_FLUSHID_SE] 425138751Scognet /* 426236991Simp * We can do that, since 427138751Scognet * PSR_SVC32_MODE|PSR_UND32_MODE == MSR_UND32_MODE 428138751Scognet */ 429129198Scognet 430129198Scognet.Lcs_context_switched: 431129198Scognet 432171780Scognet /* Release the old thread */ 433171780Scognet str r6, [r4, #TD_LOCK] 434245477Scognet#if defined(SCHED_ULE) && defined(SMP) 435171780Scognet ldr r6, .Lblocked_lock 436239268Sgonzo GET_CURTHREAD_PTR(r3) 437171780Scognet1: 438171780Scognet ldr r4, [r3, #TD_LOCK] 439171780Scognet cmp r4, r6 440171780Scognet beq 1b 441245477Scognet#endif 442171780Scognet 443129198Scognet /* XXXSCW: Safe to re-enable FIQs here */ 444129198Scognet 445129198Scognet /* rem: r9 = new PCB */ 446129198Scognet 447138751Scognet mrs r3, cpsr 448129198Scognet /* 449236991Simp * We can do that, since 450138751Scognet * PSR_SVC32_MODE|PSR_UND32_MODE == MSR_UND32_MODE 451129198Scognet */ 452138751Scognet orr r2, r3, #(PSR_UND32_MODE) 453138751Scognet msr cpsr_c, r2 454129198Scognet 455129198Scognet ldr sp, [r9, #(PCB_UND_SP)] 456129198Scognet 457129198Scognet msr cpsr_c, r3 /* Restore the old mode */ 458129198Scognet /* Restore all the save registers */ 459172614Scognet#ifndef _ARM_ARCH_5E 460129198Scognet add r7, r9, #PCB_R8 461129198Scognet ldmia r7, {r8-r13} 462129198Scognet sub r7, r7, #PCB_R8 /* restore PCB pointer */ 463129198Scognet#else 464129198Scognet mov r7, r9 465129198Scognet ldr r8, [r7, #(PCB_R8)] 466129198Scognet ldr r9, [r7, #(PCB_R9)] 467129198Scognet ldr r10, [r7, #(PCB_R10)] 468129198Scognet ldr r11, [r7, #(PCB_R11)] 469129198Scognet ldr r12, [r7, #(PCB_R12)] 470129198Scognet ldr r13, [r7, #(PCB_SP)] 471129198Scognet#endif 472129198Scognet 473129198Scognet /* rem: r5 = new lwp's proc */ 474171780Scognet /* rem: r6 = lock */ 475129198Scognet /* rem: r7 = new PCB */ 476129198Scognet 477129198Scognet.Lswitch_return: 478129198Scognet 479129198Scognet /* 480129198Scognet * Pull the registers that got pushed when either savectx() or 481129198Scognet * cpu_switch() was called and return. 482129198Scognet */ 483247864Sandrew add sp, sp, #4; 484129198Scognet ldmfd sp!, {r4-r7, pc} 485129198Scognet#ifdef DIAGNOSTIC 486129198Scognet.Lswitch_bogons: 487129198Scognet adr r0, .Lswitch_panic_str 488129198Scognet bl _C_LABEL(panic) 489129198Scognet1: nop 490129198Scognet b 1b 491129198Scognet 492129198Scognet.Lswitch_panic_str: 493129198Scognet .asciz "cpu_switch: sched_qs empty with non-zero sched_whichqs!\n" 494129198Scognet#endif 495248361SandrewEND(cpu_switch) 496248361Sandrew 497129198ScognetENTRY(savectx) 498150856Scognet stmfd sp!, {r4-r7, lr} 499247864Sandrew sub sp, sp, #4 500150856Scognet /* 501150856Scognet * r0 = pcb 502150856Scognet */ 503150856Scognet /* Store all the registers in the process's pcb */ 504150856Scognet add r2, r0, #(PCB_R8) 505150856Scognet stmia r2, {r8-r13} 506254461Sandrew#ifdef VFP 507262941Sian fmrx r2, fpexc /* If the VFP is enabled */ 508262941Sian tst r2, #(VFPEXC_EN) /* the current thread has */ 509262941Sian movne r1, #1 /* used it, so go save */ 510262941Sian addne r0, r0, #(PCB_VFPSTATE) /* the state into the PCB */ 511262941Sian blne _C_LABEL(vfp_store) /* and disable the VFP. */ 512239268Sgonzo#endif 513247864Sandrew add sp, sp, #4; 514150856Scognet ldmfd sp!, {r4-r7, pc} 515248361SandrewEND(savectx) 516150856Scognet 517129198ScognetENTRY(fork_trampoline) 518250253Sian STOP_UNWINDING /* Can't unwind beyond the thread enty point */ 519129198Scognet mov r1, r5 520129198Scognet mov r2, sp 521129198Scognet mov r0, r4 522137976Scognet mov fp, #0 523129198Scognet bl _C_LABEL(fork_exit) 524135655Scognet /* Kill irq"s */ 525135655Scognet mrs r0, cpsr 526157616Scognet orr r0, r0, #(I32_bit|F32_bit) 527135655Scognet msr cpsr_c, r0 528146596Scognet DO_AST 529129198Scognet PULLFRAME 530129198Scognet 531129198Scognet movs pc, lr /* Exit */ 532129198Scognet 533135655ScognetAST_LOCALS 534248361SandrewEND(fork_trampoline) 535248361Sandrew 536