Home | History | Annotate | Line # | Download | only in arm32
cpuswitch.S revision 1.28.2.7
      1 /*	$NetBSD: cpuswitch.S,v 1.28.2.7 2002/10/24 21:25:46 bjh21 Exp $	*/
      2 
      3 /*
      4  * Copyright (c) 1994-1998 Mark Brinicombe.
      5  * Copyright (c) 1994 Brini.
      6  * All rights reserved.
      7  *
      8  * This code is derived from software written for Brini by Mark Brinicombe
      9  *
     10  * Redistribution and use in source and binary forms, with or without
     11  * modification, are permitted provided that the following conditions
     12  * are met:
     13  * 1. Redistributions of source code must retain the above copyright
     14  *    notice, this list of conditions and the following disclaimer.
     15  * 2. Redistributions in binary form must reproduce the above copyright
     16  *    notice, this list of conditions and the following disclaimer in the
     17  *    documentation and/or other materials provided with the distribution.
     18  * 3. All advertising materials mentioning features or use of this software
     19  *    must display the following acknowledgement:
     20  *	This product includes software developed by Brini.
     21  * 4. The name of the company nor the name of the author may be used to
     22  *    endorse or promote products derived from this software without specific
     23  *    prior written permission.
     24  *
     25  * THIS SOFTWARE IS PROVIDED BY BRINI ``AS IS'' AND ANY EXPRESS OR IMPLIED
     26  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
     27  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
     28  * IN NO EVENT SHALL BRINI OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
     29  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
     30  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
     31  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
     32  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
     33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
     34  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
     35  * SUCH DAMAGE.
     36  *
     37  * RiscBSD kernel project
     38  *
     39  * cpuswitch.S
     40  *
     41  * cpu switching functions
     42  *
     43  * Created      : 15/10/94
     44  */
     45 
     46 #include "opt_armfpe.h"
     47 #include "opt_multiprocessor.h"
     48 
     49 #include "assym.h"
     50 #include <machine/param.h>
     51 #include <machine/cpu.h>
     52 #include <machine/frame.h>
     53 #include <machine/asm.h>
     54 
     55 #undef IRQdisable
     56 #undef IRQenable
     57 
     58 /*
     59  * New experimental definitions of IRQdisable and IRQenable
     60  * These keep FIQ's enabled since FIQ's are special.
     61  */
     62 
     63 #define IRQdisable \
     64 	mrs	r14, cpsr ; \
     65 	orr	r14, r14, #(I32_bit) ; \
     66 	msr	cpsr_c, r14 ; \
     67 
     68 #define IRQenable \
     69 	mrs	r14, cpsr ; \
     70 	bic	r14, r14, #(I32_bit) ; \
     71 	msr	cpsr_c, r14 ; \
     72 
     73 	.text
     74 
     75 .Lwhichqs:
     76 	.word	_C_LABEL(sched_whichqs)
     77 
     78 .Lqs:
     79 	.word	_C_LABEL(sched_qs)
     80 
     81 /*
     82  * cpuswitch()
     83  *
     84  * preforms a process context switch.
     85  * This function has several entry points
     86  */
     87 
     88 #ifdef MULTIPROCESSOR
     89 .Lcpu_info:
     90 	.word	_C_LABEL(cpu_info)
     91 #else
     92 .Lcurproc:
     93 	.word	_C_LABEL(curproc)
     94 
     95 .Lcurpcb:
     96 	.word	_C_LABEL(curpcb)
     97 #endif
     98 
     99 .Lwant_resched:
    100 	.word	_C_LABEL(want_resched)
    101 
    102 .Lcpufuncs:
    103 	.word	_C_LABEL(cpufuncs)
    104 
    105 #ifndef MULTIPROCESSOR
    106 	.data
    107 	.global	_C_LABEL(curpcb)
    108 _C_LABEL(curpcb):
    109 	.word	0x00000000
    110 	.text
    111 #endif
    112 
    113 .Lblock_userspace_access:
    114 	.word	_C_LABEL(block_userspace_access)
    115 
    116 .Lcpu_do_powersave:
    117 	.word	_C_LABEL(cpu_do_powersave)
    118 
    119 /*
    120  * Idle loop, exercised while waiting for a process to wake up.
    121  *
    122  * NOTE: When we jump back to .Lswitch_search, we must have a
    123  * pointer to whichqs in r7, which is what it is when we arrive
    124  * here.
    125  */
    126 /* LINTSTUB: Ignore */
    127 ASENTRY_NP(idle)
    128 #ifdef MULTIPROCESSOR
    129 	/* Switch to the idle PCB unless we're already running on it. */
    130 	cmp	r8, #0			/* old process was exiting? */
    131 	beq	1f
    132 
    133 	/* Get the user structure for the old process. */
    134 	ldr	r0, [r8, #(P_ADDR)]
    135 
    136 	/* Save the remaining registers in the old process's pcb */
    137 	add	r0, r0, #(PCB_R11)
    138 	stmia	r0, {r11-r13}
    139 1:
    140 	ldr	r0, [r9, #(CI_IDLEPCB)]
    141 	add	r0, r0, #(PCB_R11)
    142 	ldmia	r0, {r11-r13}
    143 	mov	r8, #0			/* old process is irrelevant now */
    144 #endif
    145 #if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
    146 	bl	_C_LABEL(sched_unlock_idle)
    147 #endif
    148 	ldr	r3, .Lcpu_do_powersave
    149 
    150 	/* Enable interrupts */
    151 	IRQenable
    152 
    153 	/* If we don't want to sleep, use a simpler loop. */
    154 	ldr	r3, [r3]		/* r3 = cpu_do_powersave */
    155 	teq	r3, #0
    156 	bne	2f
    157 
    158 	/* Non-powersave idle. */
    159 1:	/* should maybe do uvm pageidlezero stuff here */
    160 #ifdef MULTIPROCESSOR
    161 	/* XXX This will thrash the data bus.  We need a better way. */
    162 	ldr	r4, .Lcpufuncs
    163 	mov	lr, pc
    164 	ldr	pc, [r4, #(CF_IDCACHE_WBINV_ALL)]
    165 #endif
    166 	ldr	r3, [r7]		/* r3 = whichqs */
    167 	teq	r3, #0x00000000
    168 	bne	.Lswitch_search
    169 	b	1b
    170 
    171 2:	/* Powersave idle. */
    172 	ldr	r4, .Lcpufuncs
    173 3:	ldr	r3, [r7]		/* r3 = whichqs */
    174 	teq	r3, #0x00000000
    175 	bne	.Lswitch_search
    176 
    177 	/* if saving power, don't want to pageidlezero */
    178 	mov	r0, #0
    179 	adr	lr, 3b
    180 	ldr	pc, [r4, #(CF_SLEEP)]
    181 	/* loops back around */
    182 
    183 
    184 /*
    185  * Find a new process to run, save the current context and
    186  * load the new context
    187  */
    188 
    189 ENTRY(cpu_switch)
    190 /*
    191  * Local register usage. Some of these registers are out of date.
    192  * r0-r2 = scratch
    193  * r3 = whichqs
    194  * r4 = queue
    195  * r5 = &qs[queue]
    196  * r6 = newproc
    197  * r7 = &whichqs, new first proc in q, old pcb, new pcb
    198  * r8 = oldproc
    199  * r9 = curcpu()
    200  */
    201 	mov	ip, sp
    202 	stmfd	sp!, {r4-r10, fp, ip, lr, pc}
    203 	sub	fp, ip, #4
    204 
    205 #ifdef MULTIPROCESSOR
    206 	/* XXX Probably not appropriate for non-Hydra SMPs */
    207 	bl	_C_LABEL(cpu_number)
    208 	ldr	r9, .Lcpu_info
    209 	ldr	r9, [r9, r0, lsl #2]
    210 #endif
    211 
    212 	/*
    213 	 * Get the current process and indicate that there is no longer
    214 	 * a valid process (curproc = 0).  Zero the current PCB pointer
    215 	 * while we're at it.
    216 	 */
    217 #ifdef MULTIPROCESSOR
    218 	ldr	r8, [r9, #CI_CURPROC]	/* r8 = curproc */
    219 	mov	r0, #0
    220 	str	r0, [r9, #CI_CURPROC]	/* curproc = NULL */
    221 	str	r0, [r9, #CI_CURPCB]	/* curpcb = NULL */
    222 #else
    223 	ldr	r1, .Lcurproc
    224 	ldr	r2, .Lcurpcb
    225 	mov	r0, #0x00000000
    226 	ldr	r8, [r1]		/* r8 = curproc */
    227 	str	r0, [r1]		/* curproc = NULL */
    228 	str	r0, [r2]		/* curpcb = NULL */
    229 #endif
    230 
    231 #if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
    232 	/* release the sched_lock before handling interrupts */
    233 	bl	_C_LABEL(sched_unlock_idle)
    234 #endif
    235 
    236 	/* Lower the spl level to spl0 and get the current spl level. */
    237 #ifdef __NEWINTR
    238 	mov	r0, #(IPL_NONE)
    239 	bl	_C_LABEL(_spllower)
    240 #else /* ! __NEWINTR */
    241 #ifdef spl0
    242 	mov	r0, #(_SPL_0)
    243 	bl	_C_LABEL(splx)
    244 #else
    245 	bl	_C_LABEL(spl0)
    246 #endif /* spl0 */
    247 #endif /* __NEWINTR */
    248 
    249 	/* Push the old spl level onto the stack */
    250 	str	r0, [sp, #-0x0004]!
    251 
    252 	/* First phase : find a new process */
    253 
    254 	ldr	r7, .Lwhichqs
    255 
    256 	/* rem: r7 = &whichqs */
    257 	/* rem: r8 = old proc */
    258 
    259 .Lswitch_search:
    260 	IRQdisable
    261 #if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
    262 	bl	_C_LABEL(sched_lock_idle)
    263 #endif
    264 
    265 	/* Do we have any active queues  */
    266 	ldr	r3, [r7]
    267 
    268 	/* If not we must idle until we do. */
    269 	teq	r3, #0x00000000
    270 	beq	_ASM_LABEL(idle)
    271 
    272 	/* rem: r8 = old proc */
    273 	/* rem: r3 = whichqs */
    274 	/* rem: interrupts are disabled */
    275 
    276 	/*
    277 	 * We have found an active queue. Currently we do not know which queue
    278 	 * is active just that one of them is.
    279 	 */
    280 	/* this is the ffs algorithm devised by d.seal and posted to
    281 	 * comp.sys.arm on 16 Feb 1994.
    282 	 */
    283  	rsb	r5, r3, #0
    284  	ands	r0, r3, r5
    285 
    286 	adr	r5, .Lcpu_switch_ffs_table
    287 
    288 				    /* X = R0 */
    289 	orr	r4, r0, r0, lsl #4  /* r4 = X * 0x11 */
    290 	orr	r4, r4, r4, lsl #6  /* r4 = X * 0x451 */
    291 	rsb	r4, r4, r4, lsl #16 /* r4 = X * 0x0450fbaf */
    292 
    293 	/* used further down, saves SA stall */
    294 	ldr	r6, .Lqs
    295 
    296 	/* now lookup in table indexed on top 6 bits of a4 */
    297 	ldrb	r4, [ r5, r4, lsr #26 ]
    298 
    299 	/* rem: r0 = bit mask of chosen queue (1 << r4) */
    300 	/* rem: r3 = whichqs */
    301 	/* rem: r4 = queue number */
    302 	/* rem: r8 = old proc */
    303 	/* rem: interrupts are disabled */
    304 
    305 	/* Get the address of the queue (&qs[queue]) */
    306 	add	r5, r6, r4, lsl #3
    307 
    308 	/*
    309 	 * Get the process from the queue and place the next process in
    310 	 * the queue at the head. This basically unlinks the process at
    311 	 * the head of the queue.
    312 	 */
    313 	ldr	r6, [r5, #(P_FORW)]
    314 
    315 	/* rem: r6 = new process */
    316 	ldr	r7, [r6, #(P_FORW)]
    317 	str	r7, [r5, #(P_FORW)]
    318 
    319 	/* rem: r7 = new queue head */
    320 
    321 	/*
    322 	 * Test to see if the queue is now empty. If the head of the queue
    323 	 * points to the queue itself then there are no more processes in
    324 	 * the queue. We can therefore clear the queue not empty flag held
    325 	 * in r3.
    326 	 */
    327 
    328 	teq	r5, r7
    329 	biceq	r3, r3, r0
    330 
    331 	/* Fix the back pointer for the process now at the head of the queue. */
    332 	ldr	r0, [r6, #(P_BACK)]
    333 	str	r0, [r7, #(P_BACK)]
    334 
    335 	/* Update the RAM copy of the queue not empty flags word. */
    336 	ldr	r7, .Lwhichqs
    337 	str	r3, [r7]
    338 
    339 	/* rem: r8 = old proc */
    340 	/* rem: r3 = whichqs - NOT NEEDED ANY MORE */
    341 	/* rem: r4 = queue number - NOT NEEDED ANY MORE */
    342 	/* rem: r6 = new process */
    343 	/* rem: interrupts are disabled */
    344 
    345 	/* Clear the want_resched flag */
    346 	ldr	r1, .Lwant_resched
    347 	mov	r0, #0x00000000
    348 	str	r0, [r1]
    349 
    350 	/*
    351 	 * Clear the back pointer of the process we have removed from
    352 	 * the head of the queue. The new process is isolated now.
    353 	 */
    354 	str	r0, [r6, #(P_BACK)]
    355 
    356 #if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
    357 	/*
    358 	 * unlock the sched_lock, but leave interrupts off, for now.
    359 	 */
    360 	bl	_C_LABEL(sched_unlock_idle)
    361 #endif
    362 
    363 #ifdef MULTIPROCESSOR
    364 	str	r9, [r6, #(P_CPU)]
    365 #else
    366 	/* p->p_cpu initialized in fork1() for single-processor */
    367 #endif
    368 
    369 	/* Process is now on a processor. */
    370 	mov	r0, #SONPROC			/* p->p_stat = SONPROC */
    371 	strb	r0, [r6, #(P_STAT)]
    372 
    373 	/* We have a new curproc now so make a note it */
    374 #ifdef MULTIPROCESSOR
    375 	str	r6, [r9, #CI_CURPROC]
    376 #else
    377 	ldr	r7, .Lcurproc
    378 	str	r6, [r7]
    379 #endif
    380 
    381 	/* Hook in a new pcb */
    382 #ifdef MULTIPROCESSOR
    383 	ldr	r0, [r6, #(P_ADDR)]
    384 	str	r0, [r9, #CI_CURPCB]
    385 #else
    386 	ldr	r7, .Lcurpcb
    387 	ldr	r0, [r6, #(P_ADDR)]
    388 	str	r0, [r7]
    389 #endif
    390 
    391 	/* At this point we can allow IRQ's again. */
    392 	IRQenable
    393 
    394 	/* rem: r8 = old proc */
    395 	/* rem: r6 = new process */
    396 	/* rem: interrupts are enabled */
    397 
    398 	/*
    399 	 * If the new process is the same as the process that called
    400 	 * cpu_switch() then we do not need to save and restore any
    401 	 * contexts. This means we can make a quick exit.
    402 	 * The test is simple if curproc on entry (now in r8) is the
    403 	 * same as the proc removed from the queue we can jump to the exit.
    404 	 */
    405 	teq	r8, r6
    406 	beq	.Lswitch_return
    407 
    408 	/*
    409 	 * If the curproc on entry to cpu_switch was zero then the
    410 	 * process that called it was exiting. This means that we do
    411 	 * not need to save the current context. Instead we can jump
    412 	 * straight to restoring the context for the new process.
    413 	 *
    414 	 * We also take this path if we're switching from the idle PCB.
    415 	 */
    416 	teq	r8, #0x00000000
    417 	beq	.Lswitch_exited
    418 
    419 	/* rem: r8 = old proc */
    420 	/* rem: r6 = new process */
    421 	/* rem: interrupts are enabled */
    422 
    423 	/* Stage two : Save old context */
    424 
    425 	/* Get the user structure for the old process. */
    426 	ldr	r7, [r8, #(P_ADDR)]
    427 
    428 	/* Save the remaining registers in the old process's pcb */
    429 	add	r0, r7, #(PCB_R11)
    430 	stmia	r0, {r11-r13}
    431 
    432 	/* rem: r7 = old pcb */
    433 
    434 	/*
    435 	 * This can be optimised... We know we want to go from SVC32
    436 	 * mode to UND32 mode
    437 	 */
    438         mrs	r3, cpsr
    439 	bic	r2, r3, #(PSR_MODE)
    440 	orr	r2, r2, #(PSR_UND32_MODE | I32_bit)
    441         msr	cpsr_c, r2
    442 
    443 	str	sp, [r7, #(PCB_UND_SP)]
    444 
    445         msr	cpsr_c, r3		/* Restore the old mode */
    446 
    447 	/* rem: r6 = new process */
    448 	/* rem: r7 = old pcb */
    449 	/* rem: r8 = old proc */
    450 	/* rem: interrupts are enabled */
    451 
    452 	/* What else needs to be saved  Only FPA stuff when that is supported */
    453 
    454 	/* r7 now free! */
    455 
    456 	/* Third phase : restore saved context */
    457 
    458 	/* rem: r6 = new process */
    459 	/* rem: r8 = old proc */
    460 	/* rem: interrupts are enabled */
    461 
    462 	/*
    463 	 * Don't allow user space access between the purge and the switch.
    464 	 */
    465 	ldr	r3, .Lblock_userspace_access
    466 	mov	r1, #0x00000001
    467 	mov	r2, #0x00000000
    468 	str	r1, [r3]
    469 
    470 	stmfd	sp!, {r0-r3}
    471 	ldr	r1, .Lcpufuncs
    472 	mov	lr, pc
    473 	ldr	pc, [r1, #CF_IDCACHE_WBINV_ALL]
    474 	ldmfd	sp!, {r0-r3}
    475 
    476 .Lcs_cache_purge_skipped:
    477 	/* At this point we need to kill IRQ's again. */
    478 	IRQdisable
    479 
    480 	/*
    481 	 * Interrupts are disabled so we can allow user space accesses again
    482 	 * as none will occur until interrupts are re-enabled after the
    483 	 * switch.
    484 	 */
    485 	str	r2, [r3]
    486 
    487 	/* Get the user structure for the new process in r1 */
    488 	ldr	r7, [r6, #(P_ADDR)]
    489 
    490 	/* Get the pagedir physical address for the process. */
    491 	ldr	r0, [r7, #(PCB_PAGEDIR)]
    492 
    493 	/* Switch the memory to the new process */
    494 	ldr	r3, .Lcpufuncs
    495 	mov	lr, pc
    496 	ldr	pc, [r3, #CF_CONTEXT_SWITCH]
    497 
    498 	/*
    499 	 * This can be optimised... We know we want to go from SVC32
    500 	 * mode to UND32 mode
    501 	 */
    502         mrs	r3, cpsr
    503 	bic	r2, r3, #(PSR_MODE)
    504 	orr	r2, r2, #(PSR_UND32_MODE)
    505         msr	cpsr_c, r2
    506 
    507 	ldr	sp, [r7, #(PCB_UND_SP)]
    508 
    509         msr	cpsr_c, r3		/* Restore the old mode */
    510 
    511 	/* Restore the saved registers from the PCB */
    512 	add	r0, r7, #PCB_R11
    513 	ldmia	r0, {r11-r13}
    514 
    515 #ifdef ARMFPE
    516 	add	r0, r1, #(USER_SIZE) & 0x00ff
    517 	add	r0, r0, #(USER_SIZE) & 0xff00
    518 	bl	_C_LABEL(arm_fpe_core_changecontext)
    519 #endif
    520 
    521 	/* We can enable interrupts again */
    522 	IRQenable
    523 
    524 	/* rem: r6 = new proc */
    525 	/* rem: r7 = new PCB */
    526 
    527 	/*
    528 	 * Check for restartable atomic sequences (RAS).
    529 	 */
    530 
    531 	ldr	r2, [r6, #(P_NRAS)]
    532 	ldr	r4, [r7, #(PCB_TF)]	/* r4 = trapframe (used below) */
    533 	teq	r2, #0			/* p->p_nras == 0? */
    534 	bne	.Lswitch_do_ras		/* no, check for one */
    535 
    536 .Lswitch_return:
    537 
    538 	/* Get the spl level from the stack and update the current spl level */
    539 	ldr	r0, [sp], #0x0004
    540 	bl	_C_LABEL(splx)
    541 
    542 	/* cpu_switch returns the proc it switched to. */
    543 	mov	r0, r6
    544 
    545 	/*
    546 	 * Pull the registers that got pushed when either savectx() or
    547 	 * cpu_switch() was called and return.
    548 	 */
    549 	ldmdb	fp, {r4-r10, fp, sp, pc}
    550 
    551 .Lswitch_do_ras:
    552 	ldr	r1, [r4, #(TF_PC)]	/* second ras_lookup() arg */
    553 	mov	r0, r6			/* first ras_lookup() arg */
    554 	bl	_C_LABEL(ras_lookup)
    555 	cmn	r0, #1			/* -1 means "not in a RAS" */
    556 	strne	r0, [r4, #(TF_PC)]
    557 	b	.Lswitch_return
    558 
    559 .Lswitch_exited:
    560 	/*
    561 	 * We skip the cache purge because switch_exit() already did
    562 	 * it.  Load up registers the way Lcs_cache_purge_skipped
    563 	 * expects.  Userspace access already blocked in switch_exit().
    564 	 */
    565 	ldr	r3, .Lblock_userspace_access
    566 	mov	r2, #0x00000000
    567 	b	.Lcs_cache_purge_skipped
    568 
    569 /*
    570  * void switch_exit(struct proc *p, struct proc *p0);
    571  * Switch to proc0's saved context and deallocate the address space and kernel
    572  * stack for p.  Then jump into cpu_switch(), as if we were in proc0 all along.
    573  */
    574 
    575 /* LINTSTUB: Func: void switch_exit(struct proc *p, struct proc *p0) */
    576 ENTRY(switch_exit)
    577 	/* Could create a stack frame, but we'll never return anyway. */
    578 
    579 	mov	r4, r0			/* r4 = p */
    580 	mov	r5, r1			/* r5 = p0 */
    581 
    582 #ifdef MULTIPROCESSOR
    583 	/* XXX Probably not appropriate for non-Hydra SMPs */
    584 	bl	_C_LABEL(cpu_number)
    585 	ldr	r9, .Lcpu_info
    586 	ldr	r9, [r9, r0, lsl #2]
    587 #endif
    588 
    589 	/* In case we fault */
    590 #ifdef MULTIPROCESSOR
    591 	mov	r2, #0
    592 	str	r2, [r9, #CI_CURPROC]
    593 /*	str	r2, [r9, #CI_CURPCB] */
    594 #else
    595 	ldr	r0, .Lcurproc
    596 	mov	r2, #0x00000000
    597 	str	r2, [r0]
    598 
    599 /*	ldr	r0, .Lcurpcb
    600 	str	r2, [r0]*/
    601 #endif
    602 
    603 	/*
    604 	 * Don't allow user space access between the purge and the switch.
    605 	 */
    606 	ldr	r0, .Lblock_userspace_access
    607 	mov	r2, #0x00000001
    608 	str	r2, [r0]
    609 
    610 	/* Switch to proc0 context */
    611 
    612 	ldr	r0, .Lcpufuncs
    613 	mov	lr, pc
    614 	ldr	pc, [r0, #CF_IDCACHE_WBINV_ALL]
    615 
    616 	IRQdisable
    617 
    618 	ldr	r7, [r5, #(P_ADDR)]
    619 	ldr	r0, [r7, #(PCB_PAGEDIR)]
    620 
    621 	/* Switch the memory to the new process */
    622 	ldr	r1, .Lcpufuncs
    623 	mov	lr, pc
    624 	ldr	pc, [r1, #CF_CONTEXT_SWITCH]
    625 
    626 	/* Restore all the save registers */
    627 	add	r0, r7, #PCB_R11
    628 	ldmia	r0, {r11-r13}
    629 
    630 	/* This is not really needed ! */
    631 	/* Yes it is for the su and fu routines */
    632 #ifdef MULTIPROCESSOR
    633 	str	r7, [r9, #CI_CURPCB]
    634 #else
    635 	ldr	r0, .Lcurpcb
    636 	str	r7, [r0]
    637 #endif
    638 
    639 	IRQenable
    640 
    641 	/*
    642 	 * Schedule the vmspace and stack to be freed.
    643 	 */
    644 	mov	r0, r4			/* exit2(p) */
    645 	bl	_C_LABEL(exit2)
    646 
    647 	/* Paranoia */
    648 #ifdef MULTIPROCESSOR
    649 	mov	r0, #0
    650 	str	r0, [r9, #CI_CURPCB]
    651 #else
    652 	ldr	r1, .Lcurproc
    653 	mov	r0, #0x00000000
    654 	str	r0, [r1]
    655 #endif
    656 
    657 	ldr	r7, .Lwhichqs		/* r7 = &whichqs */
    658 	mov	r8, #0x00000000		/* r8 = old proc = NULL */
    659 	b	.Lswitch_search
    660 
    661 /* LINTSTUB: Func: void savectx(struct pcb *pcb) */
    662 ENTRY(savectx)
    663 	/*
    664 	 * r0 = pcb
    665 	 */
    666 
    667 	/* Push registers.*/
    668 	mov	ip, sp
    669 	stmfd	sp!, {r4-r10, fp, ip, lr, pc}
    670 	sub	fp, ip, #4
    671 
    672 	/* Store all the registers in the process's pcb */
    673 	add	r2, r0, #(PCB_R11)
    674 	stmia	r2, {r11-r13}
    675 
    676 	/* Pull the regs of the stack */
    677 	ldmdb	fp, {r4-r10, fp, sp, pc}
    678 
    679 ENTRY(proc_trampoline)
    680 #ifdef MULTIPROCESSOR
    681 	bl	_C_LABEL(proc_trampoline_mp)
    682 #endif
    683 	mov	r0, r5
    684 	mov	r1, sp
    685 	mov	lr, pc
    686 	mov	pc, r4
    687 
    688 	/* Kill irq's */
    689         mrs     r0, cpsr
    690         orr     r0, r0, #(I32_bit)
    691         msr     cpsr_c, r0
    692 
    693 	PULLFRAME
    694 
    695 	movs	pc, lr			/* Exit */
    696 
    697 	.type .Lcpu_switch_ffs_table, _ASM_TYPE_OBJECT;
    698 .Lcpu_switch_ffs_table:
    699 /* same as ffs table but all nums are -1 from that */
    700 /*               0   1   2   3   4   5   6   7           */
    701 	.byte	 0,  0,  1, 12,  2,  6,  0, 13  /*  0- 7 */
    702 	.byte	 3,  0,  7,  0,  0,  0,  0, 14  /*  8-15 */
    703 	.byte	10,  4,  0,  0,  8,  0,  0, 25  /* 16-23 */
    704 	.byte	 0,  0,  0,  0,  0, 21, 27, 15  /* 24-31 */
    705 	.byte	31, 11,  5,  0,  0,  0,  0,  0	/* 32-39 */
    706 	.byte	 9,  0,  0, 24,  0,  0, 20, 26  /* 40-47 */
    707 	.byte	30,  0,  0,  0,  0, 23,  0, 19  /* 48-55 */
    708 	.byte   29,  0, 22, 18, 28, 17, 16,  0  /* 56-63 */
    709 
    710 /* End of cpuswitch.S */
    711