ppctrap.s

可用于嵌入式编程学习

    cmpwi   r4, 0                       // Check if it's zero.
    beq     CheckRunList                // If no KCResched, go look at runlist

    li      r3, 0                       // load a constant 0
    stw     r3, dwKCRes(0)              // Write 0 to KCResched Flag
    bl      KCNextThread                // Call KC Sceduler
    lwz     r4, dwKCRes(0)              // Load value of KCResched after func
    cmpwi   r4, 0                       // Compare KCResched to 0
    bne     CheckResched                // If it's non-zero do resched again

CheckRunList:

    lis     r4, [hia]RunList            // Load address of run list
    addi    r4, r4, [lo]RunList         // ..
    lwz     r3, 4(r4)                   // Get pth from runlist
    cmpwi   r3, 0                       // if it's zero, we're idle
    bne     DoNewThread                 // Go get a new thread if there's one

Idle:
	mfmsr   r0							// Turn off interrupts
    MSR_CLR_EE(r0)
    mtmsr   r0
	
    bl      OEMIdle                     // Call OEM's idle routine

	mfmsr   r0							// Turn on interrupts
    ori     r0, r0, MSR_EE
    mtmsr   r0

    b       CheckResched                // Try reschedule path

DoNewThread:

    //
    // Scheduling new thread:
    //
    //  (r3) = RunList.pth
    //

    bl      SetCPUASID
    lis     r4, [hia]RunList            // Load address of run list
    addi    r4, r4, [lo]RunList         // ..
    lwz     r3, 4(r4)                   // Get pth from runlist
    lwz     r4, 0x3c(r3)                // get RunList.pth->hTh
    stw     r4, hCurThread(0)           // Store back current thread

    //
    // Ready to load our new thread.
    //
    // (r3) = RunList.pth
    //


    cmpwi   r14, 0
    beq-    LoadNewThread

    stw     r15, TcxR15(r14)    // save remaining general registers
    stw     r16, TcxR16(r14)    // ...
    stw     r17, TcxR17(r14)    // ...
    stw     r18, TcxR18(r14)    // ...
    stw     r19, TcxR19(r14)    // ...
    stw     r20, TcxR20(r14)    // ...
    stw     r21, TcxR21(r14)    // ...
    stw     r22, TcxR22(r14)    // ...
    stw     r23, TcxR23(r14)    // ...
    stw     r24, TcxR24(r14)    // ...
    stw     r25, TcxR25(r14)    // ...
    stw     r26, TcxR26(r14)    // ...
    stw     r27, TcxR27(r14)    // ...
    stw     r28, TcxR28(r14)    // ...
    stw     r29, TcxR29(r14)    // ...
    stw     r30, TcxR30(r14)    // ...
    stw     r31, TcxR31(r14)    // ...

LoadNewThread:

    mr      r14, r3             // (r14) = new Current Thread
    stw     r3, CurThdPtr(0)    // remember it for next interrupt

    lwz     r3, ThTlsPtr(r14)   // load the TLS pointer for the new thread
    stw     r3, lpvTls(0)       // set the KPage TLS Pointer

    lwz     r15, TcxR15(r14)    // reload non-volatile registers
    lwz     r16, TcxR16(r14)    // ...
    lwz     r17, TcxR17(r14)    // ...
    lwz     r18, TcxR18(r14)    // ...
    lwz     r19, TcxR19(r14)    // ...
    lwz     r20, TcxR20(r14)    // ...
    lwz     r21, TcxR21(r14)    // ...
    lwz     r22, TcxR22(r14)    // ...
    lwz     r23, TcxR23(r14)    // ...
    lwz     r24, TcxR24(r14)    // ...
    lwz     r25, TcxR25(r14)    // ...
    lwz     r26, TcxR26(r14)    // ...
    lwz     r27, TcxR27(r14)    // ...
    lwz     r28, TcxR28(r14)    // ...
    lwz     r29, TcxR29(r14)    // ...
    lwz     r30, TcxR30(r14)    // ...
    lwz     r31, TcxR31(r14)    // ...

ContinueCurThread:

    mfmsr   r0                  // Get current MSR
    MSR_CLR_EE(r0)              // Interrupts off
    MSR_CLR_RI(r0)              // Non-recoverable interrupts off
    mtmsr   r0                  // Update MSR
    isync
    lha     r3, ReschedFlag(0)
    li      r4, 0               // fake interrupt id for rescheduling
    cmpwi   r3, 1
    addi    r3, r3, 256
    beq-    Reschedule          // must reschedule
    sth     r3, ReschedFlag(0)

    //
    // Reload volatile register state.
    //

    lwz     r3, TcxLr(r14)
    lwz     r4, TcxCtr(r14)
    lwz     r5, TcxXer(r14)
    lwz     r6, TcxCr(r14)
    mtlr    r3                  // restore link register
    mtctr   r4                  // restore count register
    mtxer   r5                  // restore integer exception register
    mtcrf   0xff, r6            // restore condition register
    lwz     r3, TcxR3(r14)      // restore argument registers
    lwz     r4, TcxR4(r14)      // ...
    lwz     r5, TcxR5(r14)      // ...
    lwz     r6, TcxR6(r14)      // ...
    lwz     r7, TcxR7(r14)      // ...
    lwz     r8, TcxR8(r14)      // ...
    lwz     r9, TcxR9(r14)      // ...
    lwz     r10, TcxR10(r14)    // ...
    lwz     r0, TcxIar(r14)
    lwz     r11, TcxMsr(r14)
    lwz     r1, TcxR1(r14)
    lwz     r2, TcxR2(r14)
    mtspr   SRR0, r0            // set continuation address
    mtspr   SRR1, r11           // set continuation status register
    lwz     r0, TcxR0(r14)
    lwz     r11, TcxR11(r14)
    lwz     r12, TcxR12(r14)
    lwz     r13, TcxR13(r14)
    lwz     r14, TcxR14(r14)
    rfi

//
// END - KSEG Interrupt Handling Routines
//




    LEAF_ENTRY(INTERRUPTS_ON)
// INTERRUPTS_ON - enable external interrupts
//
//  Entry   none
//  Exit    none
//  Uses    r0
    mfmsr   r0
    ori     r0, r0, MSR_EE
    mtmsr   r0
    LEAF_EXIT(INTERRUPTS_ON)

    LEAF_ENTRY(INTERRUPTS_OFF)
// INTERRUPTS_OFF - disable external interrupts
//
//  Entry   none
//  Exit    none
//  Uses    r0
    mfmsr   r0
    MSR_CLR_EE(r0)
    mtmsr   r0
    LEAF_EXIT(INTERRUPTS_OFF)

PosTable:
    .byte 0,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 6,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 7,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 6,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 8,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 6,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 7,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1
    .byte 6,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1,5,1,2,1,3,1,2,1,4,1,2,1,3,1,2,1

    LEAF_ENTRY(GetHighPos)

    lis     r4, [hia]PosTable
    addi    r4, r4, [lo]PosTable                // r4 == Ptr to PosTable

    subi    r5, r0, 1                           // r5 = offset to bit 0 of byte
    rlwinm  r6, r3, 0, 0xff                     // r6 = current byte of DWORD
    lbzx    r7, r4, r6                          // r7 = bit offset within byte
    cmpwi   r7, 0
    bne     res

    addi    r5, r5, 8                           // r5 == 7
    rlwinm  r6, r3, 24, 0xff
    lbzx    r7, r4, r6
    cmpwi   r7, 0
    bne     res

    addi    r5, r5, 8                           // r5 == 15
    rlwinm  r6, r3, 16, 0xff
    lbzx    r7, r4, r6
    cmpwi   r7, 0
    bne     res

    addi    r5, r5, 8                           // r5 == 23
    rlwinm  r6, r3, 8, 0xff
    lbzx    r7, r4, r6
    cmpwi   r7, 0
    bne     res
    li      r7, 9                               // set r7 so r5+r7=32  (r3==0)
res:
    add     r3, r5, r7
    blr
    LEAF_EXIT(GetHighPos)

    LEAF_ENTRY(ZeroPage)
//  void ZeroPage(void *vpPage)
//
//  Entry   (r3) = (vpPage) = ptr to address of page to zero
//  Return  none
//  Uses    r3, r4
//
//
    li      r4, PAGE_SIZE/32          // Number of loop iterations
    mtctr   r4                        //  stored to count register
    li      r4, 0

zp10:
    stw     r4, 0(r3)
    stw     r4, 4(r3)
    stw     r4, 8(r3)
    stw     r4, 12(r3)
    stw     r4, 16(r3)
    stw     r4, 20(r3)
    stw     r4, 24(r3)
    stw     r4, 28(r3)

    addi    r3, r3, 32                // Move pointer ahead
    bdnz+   zp10                      // Continue if not end of page

    LEAF_EXIT(ZeroPage)


    LEAF_ENTRY(InterlockedPopList)
// InterlockedPopList - remove first item from list
//
//  Entry   (r3) = ptr to list head
//  Return  (r3) = item at head of list
//      If (list not emtpy) next item is now head of list
//  Uses    r4, r5
    mr  r5, r3
ipop:
    lwarx   r4, 0, r5
    mr      r3, r4          // (r3) = ptr to item
    cmpwi   r4, 0
    beq-    ipopx
    lwz     r4, 0(r4)       // (r4) = ptr to next item
ipopx:
    stwcx.  r4, 0, r5       // update linkage & clear reservation
    bne-    ipop            // do it again, if stwcx. failed
    LEAF_EXIT(InterlockedPopList)

    LEAF_ENTRY(InterlockedPushList)
// InterlockedPushList - add an item to head of list
//
//  Entry   (r3) = ptr to list head
//          (r4) = ptr to item
//  Return  (r3) = old head of list
//  Uses    r0
ipsh:
    lwarx   r0, 0, r3       // (r0) = old list head
    stw     r0, 0(r4)       // store linkage
    stwcx.  r4, 0, r3       // store new list head
    bne-    ipsh            // do it again, if stwcx. failed
    mr      r3, r0
    LEAF_EXIT(InterlockedPushList)


    DUMMY_ENTRY(xKCall)
//++
// The following code is never executed. Its purpose is to support unwinding
// through the call to the exception dispatcher.
//--
    stw     r1,0(r1)        // save stack pointer
    stw     r31,4(r1)       // save linkage register
    mflr    r31             // save return address
    PROLOGUE_END(xKCall)

    ALTERNATE_ENTRY(KCall)
// KCall - call kernel function
//
// KCall invokes a kernel function in a non-preemtable state by incrementing
// the kernel nest level and switching onto a kernel stack.
//
// While in a preemtible state, the thread's register save area is
// volatile. On the way in, nothing can be saved into the thread
// structure until KNest is set and on the way out anything needed from the
// thread structure must be loaded before restoring KNest.
//
// The sequence of stack switching must be handled carefully because
// whenever KNest != 1, the general exception handler assumes that the kernel
// stack is current and will not switch stacks. On the way in, we must switch
// to the kernel stack before setting KNest but not use it until after KNest
// is set.  On the way out, we must reset KNest before restoring the thread's
// stack pointer.
//
//  Entry   (r3) = ptr to function to call
//          (r4) = first function arg
//          (r5) = second fucntion arg
//          (r6) = third function arg
//  Exit    (r3) = function return value
//  Uses    r0, r3-r12, ctr

    lha     r12, ReschedFlag(0)
    mtctr   r3                  // (r3) = ptr to function to call
    subic.  r12, r12, 256
    mr      r3, r4              // ripple args down
    mr      r4, r5
    mr      r5, r6
    bltctr-                     // nested kernel call, jump directly
                                //  to the function

// Entering non-preemptible state. We must switch onto the kernel stack
// before setting KNest in case an interrupt occurs during the switch.

    mr      r11, r1             // (r11) = original stack pointer
    li      r1, KStack          // switch to kernel stack
    stw     r12, ReschedFlag(0) // switch to non-preemtible state
    isync
    stw     r11, 0(r1)          // save original stack pointer
    stw     r31, 4(r1)          // save register for linkage
    mflr    r31
    bctrl                       // invoke non-preemptible function

// Function complete. Return to preemtible state after checking if a
//   reschedule is needed.

    lha     r10, ReschedFlag(0)
    cmpwi   r10, 1
    beq-    kc20

    lwz     r11, dwKCRes(0)
	cmpwi	r11, 1
	beq-	kc20

    mtlr    r31
    addi    r10, r10, 256
    lwz     r31, 4(r1)          // restore caller's R31
    lwz     r11, 0(r1)          // (r11) = caller's stack pointer
    sth     r10, ReschedFlag(0) // exit non-preemptible state
    mr      r1, r11             // restore the caller's stack pointer
    blr                         //  & return to caller

// ReschedFlag set, so must run the scheduler to find which thread
// to dispatch next.
//
//  (r3) = KCall return value
//  (r31) = return address

kc20:
    lwz     r11, CurThdPtr(0)   // (r11) = ptr to current thread
    mfmsr   r0                  // Current MSR is the one we want
    stw     r31, TcxIar(r11)    // thread resumes at return address
    stw     r0, TcxMsr(r11)     //   & in kernel mode.
    lwz     r0, 0(r1)           // (r0) = caller's R1 (stack pointer)
    stw     r2, TcxR2(r11)      // save Global pointer
    stw     r3, TcxR3(r11)      // save return value
    stw     r0, TcxR1(r11)      // save caller's stack pointer
    stw     r14, TcxR14(r11)
    lwz     r31, 4(r1)          // (r31) = caller's R31 value
    mr      r14, r11
    mfcr    r0
    stw     r0, TcxCr(r11)

    li      r4, ID_RESCHEDULE       // fake interrupt id for rescheduling