PowerPC FP handling weakness.
Duncan Smith
dds at flavors.com
Wed Oct 11 08:40:42 UTC 2000
At 2:31 PM +0400 10/2/00, Sergei Organov wrote:
>Eric Valette <valette at crf.canon.fr> writes:
>
>> Sergei Organov wrote:
>[...]
> > > Yes. RTEMS doesn't have "on-demand" FP context switch. Only "deferred" context
>> > switch. I.e. switch context only when new task is marked to be "floating
>> > point" and doesn't already own the FP context.
>> >
>>
>> Yes but on-demand FP context switch is rather easy to implement since we
>> now can handle the FP unavail exception. We just need to know to wich
>> thread the current FP context belong and store/restore it.
>
>Your are right, but on-demand switch has its own drawbacks. One unfortunate
>thing is that FP bit and interrupt enable bit are in the same MSR on PowerPC,
>and this brings troubles with _ISR_Disable/Enable when on-demand FP context
>switch is used (see my previous mail for details).
>
>BR,
>Sergei Organov.
Here is "on-demand" FP context switch for OUR environment. You can adapt it as you see fit. It is Copyright Flavors so be sure to hack it up real good so it looks different.
N.B.:
1) I have ripped out some proprietary stuff of no general interest. The holes are marked:
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
Because of this there may be dangling pointers/ missing labels.
2) The comments and header are way out of date.
3) I think the only thing left (c) OAR is the copyright notice itself.
4) MPW hasn't been used for years to compile this. CodeWarrior builtin ASM is used.
5) I have removed a few bogus comments, but many probably remain...
6) Many old fragments of code remain, commented out... Some are really out of context, so it is best to ignore them.
------------------------------------------------------------------------------------------
Here is h.s:
# rtems/cpu_asm.i
#
# Equates etc.
#
# Copyright (c) 1995 Flavors Technology, Inc.
#
# 1995/08/22 dds Created. Names from linux.
#
# SPRs
DMISS equ 976
DCMP equ 977
HASH1 equ 978
HASH2 equ 979
IMISS equ 980
ICMP equ 981
RPA equ 982
#/* Bit encodings for Machine State Register (MSR) */
MSRS_POW equ (1<<2) #/* Enable Power Management */
MSRS_TGPR equ (1<<1) #/* TLB Update registers in use */
MSRS_ILE equ (1<<0) #/* Interrupt Little-Endian enable */
MSR_POW equ (1<<18) #/* Enable Power Management */
MSR_TGPR equ (1<<17) #/* TLB Update registers in use */
MSR_ILE equ (1<<16) #/* Interrupt Little-Endian enable */
MSR_EE equ (1<<15) #/* External Interrupt enable */
MSR_PR equ (1<<14) #/* Supervisor/User privelege */
MSR_FP equ (1<<13) #/* Floating Point enable */
MSR_ME equ (1<<12) #/* Machine Check enable */
MSR_FE0 equ (1<<11) #/* Floating Exception mode 0 */
MSR_SE equ (1<<10) #/* Single Step */
MSR_BE equ (1<<9) #/* Branch Trace */
MSR_FE1 equ (1<<8) #/* Floating Exception mode 1 */
MSR_IP equ (1<<6) #/* Exception prefix 0x000/0xFFF */
MSR_IR equ (1<<5) #/* Instruction MMU enable */
MSR_DR equ (1<<4) #/* Data MMU enable */
MSR_RI equ (1<<1) #/* Recoverable Exception */
MSR_LE equ (1<<0) #/* Little-Endian enable */
MSR_FPx equ MSR_FP|MSR_FE0|MSR_FE1
MSR_USER equ MSR_ME|MSR_PR|MSR_EE|MSR_IR|MSR_DR
# dds Equates
MSR_L1 equ MSR_ME|MSR_IR|MSR_DR|MSR_RI #Reenable translation and take Machine Checks
#/* Bit encodings for Hardware Implementation Register (HID0) */
HID0_EMCP equ (1<<31) #/* Enable Machine Check pin */
HID0_EBA equ (1<<29) #/* Enable Bus Address Parity */
HID0_EBD equ (1<<28) #/* Enable Bus Data Parity */
HID0_SBCLK equ (1<<27)
HID0_EICE equ (1<<26)
HID0_ECLK equ (1<<25)
HID0_PAR equ (1<<24)
HID0_DOZE equ (1<<23)
HID0_NAP equ (1<<22)
HID0_SLEEP equ (1<<21)
HID0_DPM equ (1<<20)
HID0_ICE equ (1<<15) #/* Instruction Cache Enable */
HID0_DCE equ (1<<14) #/* Data Cache Enable */
HID0_ILOCK equ (1<<13) #/* Instruction Cache Lock */
HID0_DLOCK equ (1<<12) #/* Data Cache Lock */
HID0_ICFI equ (1<<11) #/* Instruction Cache Flash Invalidate */
HID0_DCI equ (1<<10) #/* Data Cache Invalidate */
bootData record
clientHandler ds.l 1
iStack ds.l 1
iStart ds.l 1
iSize ds.l 1
dStart ds.l 1
dSize ds.l 1
fpSaveOffset ds.l 1
endr
timebaseData record
nextTickH ds.l 1
nextTickL ds.l 1
tickCount ds.l 1
endr
saveArea record
SavedSP ds.l 1
SavedCR ds.l 1
SavedLR ds.l 1
ds.l 2
SavedRTOC ds.l 1
endr
RschedControl record
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>> endr
RtileRegs record
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>> endr
# end
------------------------------------------------------------------------------------------
Here is rtems/cpu_asm.s
# rtems/cpu_asm.s
#
# This file contains all assembly code for the Flavors PPC 604 implementation
# of RTEMS.
#
# Copyright (c) 1995 Flavors Technology, Inc.
#
# based on: cpu_asm.s (for m68k) which is...
#
# COPYRIGHT (c) 1989, 1990, 1991, 1992, 1993, 1994.
# On-Line Applications Research Corporation (OAR).
# All rights assigned to U.S. Government, 1994.
#
# This material may be reproduced by or for the U.S. Government pursuant
# to the copyright license under the clause at DFARS 252.227-7013. This
# notice must appear in all copies of this file and its derivatives.
#
# rtems/cpu_asm.s,v 1.3 1995/08/07 e
#
include 'h.s'
# ###############################################################################
#
# Externs Data
#
import _ISR_Nest_level{RW}
import _Context_Switch_necessary{RW}
import _ISR_Signals_to_thread_executing{RW}
import _Thread_Dispatch_disable_level{RW}
import _ISR_Vector_table{RW}
import ._Thread_Dispatch{PR}
import _Thread_Executing{RW}
import _Thread_Allocated_fp{RW}
import _bootData{RW}
import .i8042g_isr{PR}
import .rtems_clock_tick{PR}
# ###############################################################################
#
# Data
#
csect _timebaseData{RW}
export _timebaseData{RW}
dc.l 0
dc.l 0
dc.l 0
# ###############################################################################
#
# TOC Data
#
csect __ISR_Root{TD}
export __ISR_Root{TD}
dc.l 0
csect __ISR_FPU_Owner{TD}
export __ISR_FPU_Owner{TD}
dc.l 0
# ###############################################################################
#
# TOC entries
#
toc
# tc __ISR_Root{TC},0
# tc __ISR_FPU_Owner{TC},0
tc _ISR_Nest_level{TC},_ISR_Nest_level{RW}
tc _Context_Switch_necessary{TC},_Context_Switch_necessary{RW}
tc _ISR_Signals_to_thread_executing{TC},_ISR_Signals_to_thread_executing{RW}
tc _Thread_Dispatch_disable_level{TC},_Thread_Dispatch_disable_level{RW}
tc _ISR_Vector_table{TC},_ISR_Vector_table{RW}
tc _Thread_Executing{TC},_Thread_Executing{RW}
tc _Thread_Allocated_fp{TC},_Thread_Allocated_fp{RW}
tc _bootData{TC},_bootData{RW}
tc _Context_Enter{TC},_Context_Enter{PR}
tc _timebaseData{TC},_timebaseData{RW}
# ###############################################################################
#
# Constants
#
# ###############################################################################
#
# Interrupt Handler Conventions
#
# This routine provides the RTEMS interrupt management.
# It assumes the interrupt handlers do their own FPU save/restore if needed. NOT! -dds 20001011
#
# SPRG0 -- Register save area
# SPRG1 -- <<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
# SPRG2 -- RTOC of this code
# SPRG3 -- Register save area
#
# Interrupt Request Frame
# 00: SP at entry
# 20: RTOC at entry
# 24: R0,R3-R12 at entry
# 68: CR at entry
# 72: LR at entry
# 76: CTR at entry
# 80: XER at entry
# 84: SRR0 at entry
# 88: SRR1 at entry
# 92: R27-R31
# 112: end of irq stack frame, start of red zone, 224 bytes
#
IRQ_FSIZE equ 336 #IRQ Frame Size
IRQ_RTOC equ 20
IRQ_R0 equ 24
IRQ_R3 equ (IRQ_R0+4)
IRQ_R4 equ (IRQ_R0+8)
IRQ_R5 equ (IRQ_R0+12)
IRQ_R6 equ (IRQ_R0+16)
IRQ_R7 equ (IRQ_R0+20)
IRQ_R8 equ (IRQ_R0+24)
IRQ_R9 equ (IRQ_R0+28)
IRQ_R10 equ (IRQ_R0+32)
IRQ_R11 equ (IRQ_R0+36)
IRQ_R12 equ (IRQ_R0+40)
IRQ_CR equ (IRQ_R0+44)
IRQ_LR equ (IRQ_R0+48)
IRQ_CTR equ (IRQ_R0+52)
IRQ_XER equ (IRQ_R0+56)
IRQ_SRR0 equ (IRQ_R0+60)
IRQ_SRR1 equ (IRQ_R0+64)
IRQ_R27 equ (IRQ_R0+68)
# Interrupt Service Frame
# 00: Saved SP
# 24: Parameter area for 2 args
# 32: Saved IRQ Root frame pointer
# 36: Frame valid flag
# 40: Saved FPSCR
# 48: FPR0-FPR13
#
# FPSCR Initial Value
FPSCR_INIT equ 0
#
ISR_FSIZE equ 160 #ISR Frame Size
ISR_RTOC equ 20
ISR_ROOT equ 32
ISR_FRVALID equ 36
ISR_FPSCR equ 40
ISR_FPSCRW equ (ISR_FPSCR+4)
ISR_FRBASE equ 48
ISR_FR0 equ 0
ISR_FR1 equ 8
ISR_FR2 equ 16
ISR_FR3 equ 24
ISR_FR4 equ 32
ISR_FR5 equ 40
ISR_FR6 equ 48
ISR_FR7 equ 56
ISR_FR8 equ 64
ISR_FR9 equ 72
ISR_FR10 equ 80
ISR_FR11 equ 88
ISR_FR12 equ 96
ISR_FR13 equ 104
# ###############################################################################
#
# External Interrupt Handler (0x500) Level 0
#
# We invoke the ISR with Interrupts Disabled since the EOI has NOT been issued
# to the Interrupt Controller(s). If the interrupt is secondary then the EOI
# should be issued to the primary controller ASAP, though not absolutely required.
# (It is OK to be interrupted by higher priority primaries before enabling
# necessarily lower priority secondaries.)
#
csect _EE_Handler{RW}
export _EE_Handler{RW}
# EE Handler
dc.l _L0_EE_Handler{PR}
dc.l (_L0_EE_Handler_End - _L0_EE_Handler{PR}) >> 2
dc.l 0x0500
dc.l 1
dc.l (_L0_EE_Handler_LISORI - _L0_EE_Handler{PR}) >> 1
dc.l _L1_EE_Handler{PR}
csect _L0_EE_Handler{PR}
export _L0_EE_Handler{PR}
# Save R2 and establish addressability
mtsprg 3,r2
mtsprg 0,r0
mfcr r0
mfsprg r2,1 #<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
cmplwi r2,0
bne- _L0_EE_Handler_Spurious
#mfmsr r2
#ori r2,r2,MSR_ME #Don't |MSR_DR #Go virtual on Data
#mtmsr r2
#isync
mfsprg r2,2 #Get RTOC
# Save Volatile Registers
stwu SP,-IRQ_FSIZE(SP) #Make a new IRQ frame
stw r0,IRQ_CR(SP)
stw r3,IRQ_R3(SP)
stw r4,IRQ_R4(SP)
stw r5,IRQ_R5(SP)
stw r6,IRQ_R6(SP)
stw r7,IRQ_R7(SP)
stw r8,IRQ_R8(SP)
mfsprg r3,3
stw r9,IRQ_R9(SP)
mfsprg r0,0
stw r10,IRQ_R10(SP)
mflr r4
stw r11,IRQ_R11(SP)
mfctr r5
stw r12,IRQ_R12(SP)
mfxer r6
stw r3,IRQ_RTOC(SP)
mfsrr0 r7
stw r0,IRQ_R0(SP)
mfsrr1 r8
stw r4,IRQ_LR(SP)
stw r5,IRQ_CTR(SP)
ori r8,r8,MSR_FP
stw r6,IRQ_XER(SP)
xori r8,r8,MSR_FP
stw r7,IRQ_SRR0(SP)
stw r8,IRQ_SRR1(SP)
stmw r27,IRQ_R27(SP)
# Make Int Frame
lwz r31,_bootData{TC}(RTOC)
with bootData
lwz r3,__ISR_Root{TD}(RTOC) #Get IRQ Root frame pointer
mr r4,SP
cmplwi r3,0
bne @0
lwz r4,iStack(r31) #Get IRQ stack pointer
@0: stwu SP,-ISR_FSIZE(r4) #Make a new ISR frame
mr SP,r4
li r0,0
li r4,FPSCR_INIT
stw r3,ISR_ROOT(SP) #Link ISR roots
stw SP,__ISR_Root{TD}(RTOC)
stw r0,ISR_FRVALID(SP) #Mark FPR save area invalid
stw r4,ISR_FPSCRW(SP) # and init FPSCR word
# Return up to L1
mfmsr r0 #Get Int MSR
ori r0,r0,MSR_L1 #Level 1 MSR
_L0_EE_Handler_LISORI:
lis r3,0
ori r3,r3,0
mtsrr1 r0
mtsrr0 r3
rfi #Transfer to Level 1
_L0_EE_Handler_Spurious:
mtcr r0
mfsprg r0,0
mfsprg r2,3 #Restore R2
rfi #Ignore
_L0_EE_Handler_End:
endwith
# ###############################################################################
#
# External Interrupt Handler (0x500) Level 1
#
# We invoke the ISR with Interrupts Disabled since the EOI has NOT been issued
# to the Interrupt Controller(s). If the interrupt is secondary then the EOI
# should be issued to the primary controller ASAP, though not absolutely required.
# (It is OK to be interrupted by higher priority primaries before enabling
# necessarily lower priority secondaries.)
#
csect _L1_EE_Handler{PR}
export _L1_EE_Handler{PR}
# Establish addressability
lis r3,0xbfff #Intack Space
ori r3,r3,0xfff0
lwz r31,_Thread_Dispatch_disable_level{TC}(RTOC)
lwz r30,_ISR_Nest_level{TC}(RTOC)
lwz r5,_ISR_Vector_table{TC}(RTOC)
# Get Interrupt Vector
lbz r3,0(r3)
# mfmsr r0
lwz r29,0(r31)
slwi r4,r3,2 #Scale interrupt vector
addi r7,r29,1
lwz r28,0(r30)
# ori r0,r0,MSR_EE
addi r6,r28,1
stw r7,0(r31) #++_Thread_Dispatch_disable_level
stw r6,0(r30) #++_ISR_Nest_level
# Enable Interrupts NOT
# mtmsr r0
# Index through vector and call
lwzx r12,r4,r5 # the fcn ptr
lwz r0,0(r12) # the code ptr
stw RTOC,ISR_RTOC(SP) # save TOC
mtctr r0 # code ptr to CTR
lwz RTOC,4(r12) # new TOC
bctrl # BRANCH and LINK to ISR
lwz RTOC,ISR_RTOC(SP) # restore TOC
# Returned from ISR, Disable Interrupts
mfmsr r0
ori r0,r0,MSR_EE|MSR_FP
xori r0,r0,MSR_EE|MSR_FP
mtmsr r0
# Unlink ISR_ROOT, and invalidate if FPU owner
DECend: lwz r3,ISR_ROOT(SP)
#mfsprg r4,0
lwz r4,__ISR_Root{TD}(RTOC) #Get ISR Root frame pointer
lwz r5,__ISR_FPU_Owner{TD}(RTOC)
#mfsprg r5,1
#mtsprg 0,r3
stw r3,__ISR_Root{TD}(RTOC) #Set ISR Root frame pointer
cmplw r4,r5
bne @0
li r5,0
stw r5,__ISR_FPU_Owner{TD}(RTOC)
#mtsprg 1,r5
# Check nest level, and Pop Int Stack
@0: cmplwi r28,0 #(_ISR_Nest_level == 0) ?
lwz SP,0(SP) #Pop Int stack
stw r29,0(r31) #--_Thread_Dispatch_disable_level
stw r28,0(r30) #--_ISR_Nest_level
lwz r27,IRQ_SRR1(SP)
bne Return_from_Nested
cmplwi r29,0 #(_Thread_Dispatch_disable_level == 0) ?
lwz r28,_Thread_Allocated_fp{TC}(RTOC)
lwz r29,_Thread_Executing{TC}(RTOC)
bne Return_from_First
# Now how much will you pay!?
lwz r30,_ISR_Signals_to_thread_executing{TC}(RTOC)
lwz r31,_Context_Switch_necessary{TC}(RTOC)
li r0,0
lwz r3,0(r30)
lwz r4,0(r31)
cmplw r3,r0
cmplw cr5,r4,r0
crand cr0_EQ,cr0_EQ,cr5_EQ
beq Return_from_First
# Need to call _Thread_Dispatch
stw r0,0(r30) #N.B. RTEMS never seems to clear this!
bl ._Thread_Dispatch{PR}
nop
Return_from_First:
lwz r3,0(r28)
lwz r5,0(r29)
cmplw r3,r5
bne Return_from_Nested
ori r27,r27,MSR_FP
Return_from_Nested:
mfmsr r28
ori r28,r28,MSR_RI|MSR_IR|MSR_DR
xori r28,r28,MSR_RI|MSR_IR|MSR_DR
lwz r29,IRQ_SRR0(SP)
lwz r6,IRQ_XER(SP)
lwz r5,IRQ_CTR(SP)
lwz r4,IRQ_LR(SP)
lwz r3,IRQ_CR(SP)
lwz r2,IRQ_RTOC(SP)
mtxer r6
lwz r12,IRQ_R12(SP)
mtctr r5
lwz r11,IRQ_R11(SP)
mtlr r4
lwz r10,IRQ_R10(SP)
mtcr r3
lwz r9,IRQ_R9(SP)
lwz r8,IRQ_R8(SP)
lwz r7,IRQ_R7(SP)
lwz r6,IRQ_R6(SP)
lwz r5,IRQ_R5(SP)
lwz r4,IRQ_R4(SP)
lwz r3,IRQ_R3(SP)
lwz r0,IRQ_R0(SP)
# Now go real and load
mtmsr r28
mtsrr1 r27
mtsrr0 r29
lmw r27,IRQ_R27(SP)
lwz SP,0(SP)
rfi
# ###############################################################################
#
# FP Unavailable Handler (0x800) Level 0
#
# N.B. ASR's MAY NOT safely use FP, due to limitations imposed by the
# EE Handler, and related elements of asynchronous calls to the ASR.
# One solution to this is to link FP_Contexts in EE frames.
#
# If ( <In_an_ISR> )
# if ( <ISR_doesn't_own_FP> )
# {
# if ( <Some_Other_ISR_Owns_FP> )
# Save_ISR_Context( <Other_ISR_Context> );
# else If (_Thread_Allocated_fp != NULL)
# {
# Save_Context( _Thread_Allocated_fp );
# _Thread_Allocated_fp = NULL;
# }
# Restore_ISR_Context( <This_ISR_Context> );
# <ISR_owns_FP>
# }
# else If (_Thread_Allocated_fp != _Thread_Executing)
# {
# If (_Thread_Allocated_fp != NULL)
# Save_Context( _Thread_Allocated_fp );
# _Thread_Allocated_fp = _Thread_Executing;
# Restore_Context( _Thread_Allocated_fp );
# }
# Handle <ISR_owns_FP> at Level 0 (Total Instruction budget 61)
# 21 instructions budgeted for the <ISR_owns_FP> case.
csect _FPU_Handler{RW}
export _FPU_Handler{RW}
dc.l _L0_FPU_Handler{PR}
dc.l (_L0_FPU_Handler_End - _L0_FPU_Handler{PR}) >> 2
dc.l 0x0800
dc.l 2
dc.l (_L0_FPU_Thread - _L0_FPU_Handler{PR}) >> 1
dc.l _L1_FPU_Thread
dc.l (_L0_FPU_ISR - _L0_FPU_Handler{PR}) >> 1
dc.l _L1_FPU_ISR
# FPU Handler
csect _L0_FPU_Handler{PR}
export _L0_FPU_Handler{PR}
# Save R2 and establish addressability
mtsprg 3,r2
mfsrr1 r2 #Get MSR
ori r2,r2,MSR_FP
mtsrr1 r2 # and mark FPAvailable
#li r2,0x8f4 #Our temp save area
mfsprg r2,2 #RTOC
# Save Volatile Registers
stw r0,0x8f4(0)
stw r3,0x8f8(0)
mfcr r0
stw r4,0x8fc(0)
# See how we got here
mfsprg RTOC,2
lwz r3,__ISR_Root{TD}(RTOC) #Get ISR Root frame pointer
cmplwi r3,0
beq _L0_FPU_Thread
lwz r4,__ISR_FPU_Owner{TD}(RTOC) #FPU Owner
cmplw r3,r4
bne _L0_FPU_ISR
# Done
_L0_FPU_Done:
mtcrf 255,r0 #Restore CR
lwz r0,0x8f4(0)
lwz r3,0x8f8(0)
lwz r4,0x8fc(0)
mfsprg r2,3
rfi
# Bail out to L1 (Instruction budget 40)
# Go virtual on Data (NOT), enable FP, and move saved regs to virtual
_L0_FPU_Thread:
lis r4,0
ori r4,r4,0
b _L0_FPU_Crawlout
# Bail out to L1
_L0_FPU_ISR:
lis r4,0
ori r4,r4,0
_L0_FPU_Crawlout:
mtcrf 255,r0 #Restore CR
lwz r0,0x8f4(0) # and regs
lwz r3,0x8f8(0)
mfmsr r2
#ori r2,r2,MSR_ME #|MSR_DR
#mtmsr r2 #Go virtual
#isync
stwu SP,-IRQ_FSIZE(SP) #Make a new IRQ frame
stw r0,IRQ_R0(SP) #Save Volatile Registers
lwz r0,0x8fc(0)
stw r3,IRQ_R3(SP)
stw r0,IRQ_R4(SP)
mfsrr0 r0
mfsrr1 r3
stw r0,IRQ_SRR0(SP)
stw r3,IRQ_SRR1(SP)
ori r0,r2,MSR_L1|MSR_FP
mtsrr0 r4
mfsprg r2,3
mtsrr1 r0
rfi #Transfer to Level 1
_L0_FPU_Handler_End:
# ###############################################################################
#
# FP Unavailable Handler (0x800) Level 1
#
# On entry:
# IRQ frame is allocated and linked in SP
# R0, R3, R4, SRR0, and SRR1 are saved
csect _L1_FPU_Handler{PR}
export _L1_FPU_Handler{PR}
_L1_FPU_ISR:
mfsprg r4,2 #TOC
stw r5,IRQ_R5(SP)
mfcr r0
mflr r3
lwz r5,__ISR_Root{TD}(r4) #Get ISR Root frame pointer
lwz r4,__ISR_FPU_Owner{TD}(r4)
#mfsprg r4,1 #FP Owner
#mfsprg r5,0 #IRQ Root
cmplwi r4,0
bne @3
bl _L1_FPU_UnAllocated_P
beq @2
# Save Thread FP Context
bl _L1__FPU_Save
li r8,0 #NULL
stw r8,0(r7) #=>_Thread_Allocated_fp
# Restore registers and go to load
@2: lwz r6,IRQ_R6(SP)
lwz r7,IRQ_R7(SP)
lwz r8,IRQ_R8(SP)
lwz RTOC,IRQ_RTOC(SP)
b @4
# Save ISR FP Context
@3: addi r4,r4,ISR_FRBASE
bl _L1__FPU_Save_Volatile
mffs fp0 #Get FPSCR
stfd fp0,ISR_FPSCR-ISR_FRBASE(r4) # and save
stw r4,ISR_FRVALID-ISR_FRBASE(r4) # and mark FPR's valid
# Restore ISR FP Context
@4: mfsprg r4,2 #TOC
#mtsprg 1,r5 #IRQ Root is now FPU Owner
stw r5,__ISR_FPU_Owner{TD}(r4) #IRQ Root is now FPU Owner
#
lwz r4,ISR_FRVALID(r5) #Get FPR valid flag
lfd fp13,ISR_FPSCR(r5)
cmplwi r4,0 #Test valid flag
mtfsf 255,fp13 #Restore FPSCR
beq _L1_FPU_Done #Done
addi r4,r5,ISR_FRBASE
bl _L1__FPU_Restore_Volatile
b _L1_FPU_Done
_L1_FPU_Thread:
stw r5,IRQ_R5(SP)
mfcr r0
mflr r3
bl _L1_FPU_UnAllocated_P
lwz r5,_Thread_Executing{TC}(RTOC)
lwz r5,0(r5)
beq @6
# Save Thread FP Context
bl _L1__FPU_Save
# Restore Thread FP Context
@6: lwzx r4,r5,r6
bl _L1__FPU_Restore
stw r5,0(r7) #=>_Thread_Allocated_fp
lwz r6,IRQ_R6(SP)
lwz r7,IRQ_R7(SP)
lwz r8,IRQ_R8(SP)
lwz RTOC,IRQ_RTOC(SP)
# Done
_L1_FPU_Done:
mtcrf 255,r0 #Restore CR
mtlr r3 # and LR
mfmsr r0
lwz r4,IRQ_SRR0(SP)
lwz r5,IRQ_SRR1(SP)
ori r0,r0,MSR_RI|MSR_IR|MSR_DR
xori r0,r0,MSR_RI|MSR_IR|MSR_DR
lwz r3,IRQ_R3(SP)
mtmsr r0
mtsrr0 r4
mtsrr1 r5
lwz r0,IRQ_R0(SP)
lwz r4,IRQ_R4(SP)
lwz r5,IRQ_R5(SP)
lwz SP,0(SP)
rfi
# Save RTOC, R6-R8
# Init RTOC
# Get fpSaveOffset -> R6
# Get &_Thread_Allocated_fp -> R7
# Get _Thread_Allocated_fp -> R8
# Get (_Thread_Allocated_fp == NULL) -> CR
_L1_FPU_UnAllocated_P:
stw RTOC,IRQ_RTOC(SP)
stw r6,IRQ_R6(SP)
stw r7,IRQ_R7(SP)
mfsprg RTOC,2
stw r8,IRQ_R8(SP)
lwz r6,_bootData{TC}(RTOC)
lwz r7,_Thread_Allocated_fp{TC}(RTOC)
with bootData
lwz r6,fpSaveOffset(r6)
endwith
lwz r8,0(r7)
cmplwi r8,0
blr
# Store full FP Context at r6(r8)
_L1__FPU_Save:
lwzx r4,r8,r6
_FPU_Save_Context:
stfd fp14,112(r4) #Save fp14-fp31
stfd fp15,120(r4)
stfd fp16,128(r4)
stfd fp17,136(r4)
stfd fp18,144(r4)
stfd fp19,152(r4)
stfd fp20,160(r4)
stfd fp21,168(r4)
stfd fp22,176(r4)
mffs fp14 #Get FPSCR FP14 Munged!
stfd fp23,184(r4)
stfd fp24,192(r4)
stfd fp25,200(r4)
stfd fp26,208(r4)
stfd fp27,216(r4)
stfd fp28,224(r4)
stfd fp29,232(r4)
stfd fp30,240(r4)
stfd fp31,248(r4)
stfd fp14,256(r4) #Save FPSCR
# Store volatile FP Context at r4
_L1__FPU_Save_Volatile:
stfd fp0,0(r4) #Save fp0-fp13
stfd fp1,8(r4)
stfd fp2,16(r4)
stfd fp3,24(r4)
stfd fp4,32(r4)
stfd fp5,40(r4)
stfd fp6,48(r4)
stfd fp7,56(r4)
stfd fp8,64(r4)
stfd fp9,72(r4)
stfd fp10,80(r4)
stfd fp11,88(r4)
stfd fp12,96(r4)
stfd fp13,104(r4)
blr
# Restore full FP Context from r4
_L1__FPU_Restore:
_FPU_Restore_Context:
lfd fp31,256(r4) #Get FPSCR
lfd fp14,112(r4) #Restore fp14-fp31
lfd fp15,120(r4)
lfd fp16,128(r4)
lfd fp17,136(r4)
lfd fp18,144(r4)
lfd fp19,152(r4)
lfd fp20,160(r4)
lfd fp21,168(r4)
lfd fp22,176(r4)
mtfsf 255,fp31 #Restore FPSCR
lfd fp23,184(r4)
lfd fp24,192(r4)
lfd fp25,200(r4)
lfd fp26,208(r4)
lfd fp27,216(r4)
lfd fp28,224(r4)
lfd fp29,232(r4)
lfd fp30,240(r4)
lfd fp31,248(r4)
# Restore volatile FP Context from r4
_L1__FPU_Restore_Volatile:
lfd fp0,0(r4) #Restore fp0-fp13
lfd fp1,8(r4)
lfd fp2,16(r4)
lfd fp3,24(r4)
lfd fp4,32(r4)
lfd fp5,40(r4)
lfd fp6,48(r4)
lfd fp7,56(r4)
lfd fp8,64(r4)
lfd fp9,72(r4)
lfd fp10,80(r4)
lfd fp11,88(r4)
lfd fp12,96(r4)
lfd fp13,104(r4)
blr
# ###############################################################################
#
# Decrementer Interrupt Handler (0x900) Level 0
#
# We invoke the ISR with Interrupts Enabled but the EOI has NOT been issued
# to the Interrupt Controller(s).
#
csect _DEC_Handler{RW}
export _DEC_Handler{RW}
# DEC Handler
dc.l _L0_DEC_Handler{PR}
dc.l (_L0_DEC_Handler_End - _L0_DEC_Handler{PR}) >> 2
dc.l 0x0900
dc.l 1
dc.l (_L0_DEC_Handler_LISORI - _L0_DEC_Handler{PR}) >> 1
dc.l _L1_DEC_Handler{PR}
csect _L0_DEC_Handler{PR}
export _L0_DEC_Handler{PR}
# Save R2 and establish addressability
mtsprg 3,r2
mtsprg 0,r0
mfcr r0
mfdec r2
cmpwi r2,0
bgt- _L0_DEC_Handler_Spurious
mfsprg r2,1 #Get Sched reg
cmplwi r2,0
bnea+ (_L0_SC_Cell_Timeout-_L0_SC_Handler{PR})+0xc00
#mfmsr r2
#ori r2,r2,MSR_ME #Don't |MSR_DR #Go virtual on Data
#mtmsr r2
#isync Used to Isync after going virtual
mfsprg r2,2 #Get RTOC
# Save Volatile Registers
stwu SP,-IRQ_FSIZE(SP) #Make a new IRQ frame
stw r0,IRQ_CR(SP)
stw r3,IRQ_R3(SP)
stw r4,IRQ_R4(SP)
stw r5,IRQ_R5(SP)
stw r6,IRQ_R6(SP)
stw r7,IRQ_R7(SP)
stw r8,IRQ_R8(SP)
mfsprg r3,3
stw r9,IRQ_R9(SP)
mfsprg r0,0
stw r10,IRQ_R10(SP)
mflr r4
stw r11,IRQ_R11(SP)
mfctr r5
stw r12,IRQ_R12(SP)
mfxer r6
stw r3,IRQ_RTOC(SP)
mfsrr0 r7
stw r0,IRQ_R0(SP)
mfsrr1 r8
stw r4,IRQ_LR(SP)
stw r5,IRQ_CTR(SP)
ori r8,r8,MSR_FP
stw r6,IRQ_XER(SP)
xori r8,r8,MSR_FP
stw r7,IRQ_SRR0(SP)
stw r8,IRQ_SRR1(SP)
stmw r27,IRQ_R27(SP)
# Make Int Frame
lwz r31,_bootData{TC}(RTOC)
with bootData
lwz r3,__ISR_Root{TD}(RTOC) #Get IRQ Root frame pointer
mr r4,SP
cmplwi r3,0
bne @1
lwz r4,iStack(r31) #Get IRQ stack pointer
@1: stwu SP,-ISR_FSIZE(r4) #Make a new ISR frame
mr SP,r4
li r0,0
li r4,FPSCR_INIT
stw r3,ISR_ROOT(SP) #Link ISR roots
stw SP,__ISR_Root{TD}(RTOC)
stw r0,ISR_FRVALID(SP) #Mark FPR save area invalid
stw r4,ISR_FPSCRW(SP) # and init FPSCR word
# Return up to L1
mfmsr r0 #Get Int MSR
ori r0,r0,MSR_L1 #Level 1 MSR
_L0_DEC_Handler_LISORI:
lis r3,0
ori r3,r3,0
mtsrr1 r0
mtsrr0 r3
rfi #Transfer to Level 1
_L0_DEC_Handler_Spurious:
mtcr r0
mfsprg r0,0
mfsprg r2,3 #Restore R2
rfi #Ignore
_L0_DEC_Handler_End:
endwith
# ###############################################################################
#
# Decrementer Interrupt Handler (0x900) Level 1
#
# We invoke the ISR with Interrupts Enabled but the EOI has NOT been issued
# to the Interrupt Controller(s).
#
csect _L1_DEC_Handler{PR}
export _L1_DEC_Handler{PR}
# Establish addressability
lwz r31,_Thread_Dispatch_disable_level{TC}(RTOC)
lwz r30,_ISR_Nest_level{TC}(RTOC)
lwz r27,_timebaseData{TC}(RTOC)
with timebaseData
mfmsr r0
lwz r29,0(r31)
lwz r28,0(r30)
addi r7,r29,1
addi r6,r28,1
ori r0,r0,MSR_EE
stw r7,0(r31) #++_Thread_Dispatch_disable_level
stw r6,0(r30) #++_ISR_Nest_level
# Enable Interrupts and call rtems_clock_tick()
@0: mtmsr r0
bl .i8042g_isr{PR}
bl .rtems_clock_tick{PR}
# Returned from ISR, Disable Interrupts
mfmsr r0
ori r0,r0,MSR_EE|MSR_FP
xori r0,r0,MSR_EE|MSR_FP
mtmsr r0
# Update _timebaseData
lwz r3,tickCount(r27) #Get data
lwz r8,nextTickL(r27)
lwz r7,nextTickH(r27)
ori r0,r0,MSR_EE
@1: mftbu r4 #Get current TB value
mftb r5
mftbu r6
cmpw r4,r6
bne- @1
addc r8,r8,r3 #Compute time of next tick
addze r7,r7
stw r8,nextTickL(r27) # and save it
cmplw r7,r4
stw r7,nextTickH(r27)
blt- @0 #Next tick has passed
bne- @2
cmplw r8,r5
ble- @0 #Next tick has passed
@2: subf r6,r5,r8 #Count to next tick
cmplwi r6,250 #1000-1500 instructions
blt @0 #Tick early
mtdec r6
b DECend
endwith
# ###############################################################################
#
# System Call (0xc00) Level 0
#
# Exactly 64 words (max.) -dds 96-02-15
#
# Because the FP bit can be modified at any time due to an enabled
# External Exception, it is necessary that MFMSR/MTMSR used to clear
# the EE bit be atomic. To satisfy this we use the System Call Exception
# The protocol is:
# Bit 31 of R3 is inserted into MSR( EE ), Bits 0-30 of R3 must be zero.
# Previous MSR( EE ) is returned in Bit 31 of R3, Bits 0-30 of R3 are cleared.
# R4 and R5 are destroyed.
#
# 0,1 - Bit to EE
#
csect _SC_Handler{RW}
export _SC_Handler{RW}
# SC Handler
dc.l _L0_SC_Handler{PR}
dc.l (_L0_SC_Handler_End - _L0_SC_Handler{PR}) >> 2
dc.l 0x0c00
dc.l 2
dc.l (_L0_SC_Cell_Done_LISORI - _L0_SC_Handler{PR}) >> 1
dc.l _L1_CS_Done
dc.l (_L0_SC_Cell_Timeout_LISORI - _L0_SC_Handler{PR}) >> 1
dc.l _L1_CS_Timeout
csect _L0_SC_Handler{PR}
export _L0_SC_Handler{PR}
cmplwi r3,1
bgt- _L0_SC_Cell
mfsrr1 r4 #Get MSR
extrwi r5,r4,1,16 #Extract EE bit
insrwi r4,r3,1,16 #Insert new EE bit
mtsrr1 r4 #Put MSR
mr r3,r5 #Copy result
rfi #Get done
# ###############################################################################
#
# Cell Scheduler (0xc00) Level 0
#
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
# ###############################################################################
#
# Cell Scheduler Level 1
#
csect _L1_CS_Handler{PR}
export _L1_CS_Handler{PR}
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
# ###############################################################################
#
# ITLB Miss Handler (0x1000) Level 0
#
#Not used
<<<<<<<<<<<<<<<<<<< STUFF REMOVED >>>>>>>>>>>>>>>>>>>>>>>
# ###############################################################################
#
# void _CPU_Context_save_fp( void **fp_context_ptr );
#
# save the FP context
#
csect ._CPU_Context_save_fp{PR}
export ._CPU_Context_save_fp{PR}
lwz r4,0(r3) # r3 now points to save area
b _FPU_Save_Context
# ###############################################################################
#
# void _CPU_Context_restore_fp( void **fp_context_ptr );
#
# restore the FP context
#
csect ._CPU_Context_restore_fp{PR}
export ._CPU_Context_restore_fp{PR}
lwz r4,0(r3) # r3 now points to save area
b _FPU_Restore_Context
# ###############################################################################
#
# void _CPU_Context_save_nvfp( void **fp_context_ptr );
#
# save the non-volatile FP context
#
csect ._CPU_Context_save_nvfp{PR}
export ._CPU_Context_save_nvfp{PR}
stfd fp14,112(r3) #Save fp14-fp31
stfd fp15,120(r3)
stfd fp16,128(r3)
stfd fp17,136(r3)
stfd fp18,144(r3)
stfd fp19,152(r3)
stfd fp20,160(r3)
stfd fp21,168(r3)
stfd fp22,176(r3)
mffs fp14 #Get FPSCR FP14 Munged!
stfd fp23,184(r3)
stfd fp24,192(r3)
stfd fp25,200(r3)
stfd fp26,208(r3)
stfd fp27,216(r3)
stfd fp28,224(r3)
stfd fp29,232(r3)
stfd fp30,240(r3)
stfd fp31,248(r3)
stfd fp14,256(r3) #Save FPSCR
blr
# ###############################################################################
#
# void _CPU_initTimebase( milliseconds, ftimebase, mu_s_per_tick );
#
# milliseconds -- ... since OF startup
# ftimebase -- timebase frequency in Hz
# mu_s_per_tick -- microseconds per tick
#
# It is probable that the TB has been initialized by OF, but we
# redo it here.
#
# Initialize the Decrementer and _timebaseData.
# First tick is usually pending.
#
# To compute counts_per_tick, with minimal roundoff error
# we use: floor((ftimebase * mu_s_per_tick) / 1000000)
# We use 48 bits for the intermediate results. -dds
#
csect ._CPU_initTimebase{PR}
export ._CPU_initTimebase{PR}
li r7,1000
li r6,2
divwu r9,r4,r7 #Counts per millisecond
li r8,0
mulhwu r10,r9,r3 #High time
mullw r11,r9,r3 #Low time
mttb r8 #Init TB
mtctr r6
lwz r3,_timebaseData{TC}(RTOC)
with timebaseData
mttbu r10
mttb r11
lis r9,0x7fff #Nearly most positive count
mulhwu r10,r4,r5 #f*p high
mullw r11,r4,r5 #f*p low
@0: slwi r6,r10,16 #Get the high 32 bits of 48
inslwi r6,r11,16,16 # into r6
divwu r10,r6,r7 #r10 = Q
mullw r8,r10,r7
subf r8,r8,r6 #r8 = R
insrwi r11,r8,16,0 #Insert remainder
divwu r11,r11,r7
insrwi r11,r10,16,0
srwi r10,r10,16
bdnz @0
stw r11,tickCount(r3)
@1: mftbu r4
mftb r5
mftbu r6
cmpw r4,r6
mtdec r11
bne- @1
addc r5,r5,r11
addze r4,r4
stw r5,nextTickL(r3)
stw r4,nextTickH(r3)
endwith
blr
# ###############################################################################
# void _CPU_Context_switch( Context_Control *run, Context_Control *heir );
#
# This routine performs a normal non-FP context switch.
#
# void _CPU_Context_restore( Context_Control *new_context );
#
# This routine is generally used only to restart self in an efficient manner.
# It is very similar to the second half of _CPU_Context_switch
# It IS the second half of _CPU_Context_switch -dds
#
#
csect ._CPU_Context_switch{PR}
export ._CPU_Context_switch{PR}
export ._CPU_Context_restore
mr r6,r3
mr r7,r4
li r3,0
sc
stw r3,92(r6)
mflr r8
mfcr r4
stw SP,0(r6) # save SP
stw r4,4(r6) # save CR
stw r8,8(r6) # save LR
stw RTOC,12(r6) # save RTOC
stmw r13,16(r6) # save r13-r31
b _rest
._CPU_Context_restore:
mr r7,r3
li r3,0
sc
_rest: lwarx SP,0,r7 # restore SP
stwcx. SP,0,r7
lwz r6,4(r7)
lwz r5,8(r7)
lwz r3,92(r7)
mfmsr r4
mtcrf 255,r6 # restore CR
mtlr r5 # restore LR
insrwi r4,r3,1,16 #Insert new EE bit
mtmsr r4 #Put MSR
lwz RTOC,12(r7) # restore RTOC
lmw r13,16(r7) # restore r13-r31
lwz r3,_Thread_Allocated_fp{TC}(RTOC)
lwz r4,_Thread_Executing{TC}(RTOC)
lwz r3,0(r3)
lwz r4,0(r4)
cmplw r3,r4
beq @1
blr
@1: mfmsr r0
ori r0,r0,MSR_FP
mtmsr r0
blr
# ###############################################################################
# void _CPU_Context_Initialize( _the_context, _stack_base, _size, _isr, _entry_point )
#
csect ._CPU_Context_Initialize{PR}
export ._CPU_Context_Initialize{PR}
lwz r8,_Context_Enter{TC}(RTOC)
add r4,r4,r5
li r5,0
stw r5,4(r3) #Init CR
subi r4,r4,32
stw r8,8(r3) #Init LR
stw RTOC,12(r3)
stw r4,0(r3)
subfic r5,r6,1 #_isr -> EE
stw r5,92(r3) #EE
stw r7,16(r3)
blr
csect _Context_Enter{PR}
li r4,0
lwz r0,0(r13) #_entry_point code ptr
mtlr r4 #Never come back
mtctr r0 # code ptr to CTR
lwz RTOC,4(r13) # new TOC
bctr # BRANCH to ISR
#
# ###############################################################################
# end of rtems/cpu_asm.c
More information about the users
mailing list