intalib.s

VxWorks BSP框架源代码包含头文件和驱动

        stw     p6, FRAMEBASESZ (sp)            /* save lr */

        lis     p6, HIADJ(_func_evtLogT0)
        lwz     p1, LO(_func_evtLogT0)(p6)
        mtlr    p1
        blrl

        lwz     p6,  FRAMEBASESZ (sp)
        mtspr   LR, p6                          /* restore lr */
        addi    sp, sp, FRAMEBASESZ + _STACK_ALIGN_SIZE /* release stack */
trgCheckIntCrtExit:

        lis     p6, HIADJ(trgEvtClass)          /* are there any triggers? */
        lwz     p4, LO(trgEvtClass)(p6)
        lis     p1, HI(TRG_CLASS_1_ON)
        ori     p1, p1, LO(TRG_CLASS_1_ON)
        and     p4, p4, p1
        cmpw    p1, p4
        bne     noActionIntCrtExit                      /* if none, exit */

        lis     p3, HIADJ(workQIsEmpty)         /* is work queue empty? */
        lwz     p2, LO(workQIsEmpty)(p3)
        cmpwi   p2, 0
        beq     trgIntCrtExitNoK
        li      p0, EVENT_INT_EXIT_K
        b       trgIntCrtExitCont
trgIntCrtExitNoK:
        li      p0, EVENT_INT_EXIT              /* get event id */
trgIntCrtExitCont:

        lis     p6, HIADJ(_func_trgCheck)       /* check if trgCheck func */
        lwz     p1, LO(_func_trgCheck)(p6)      /* exists */
        cmpwi   p1, 0
        beq     noActionIntCrtExit                  /* if none, exit */

        stwu    sp, -(FRAMEBASESZ+ _STACK_ALIGN_SIZE)(sp) /* carve stack frame */
        mfspr   p6, LR                          /* read lr to p6 */
        stw     p6, FRAMEBASESZ (sp)            /* save lr */

        mtlr    p1

        li      p1, 0           /* CLASS1_INDEX */
        li      p2, 0
        li      p3, 0
        li      p4, 0
        li      p5, 0
        li      p6, 0
        li      p7, 0

        blrl                                    /* check for triggers */

        lwz     p6,  FRAMEBASESZ (sp)
        mtspr   LR, p6                          /* restore lr */
        addi    sp, sp, FRAMEBASESZ + _STACK_ALIGN_SIZE /* release stack */

noActionIntCrtExit:
        /* windview instrumentation - END */
#endif /* WV_INSTRUMENTATION */

	lwz	r0, _PPC_ESF_R0(sp)		/* restore general register 0 */
	lwz	r2, _PPC_ESF_R2(sp)		/* restore general register 2 */

	lwz	p4, _PPC_ESF_P4(sp)		/* restore general reg 7 */
	lwz	p5, _PPC_ESF_P5(sp)		/* restore general reg 8 */
	lwz	p6, _PPC_ESF_P6(sp)		/* restore general reg 9 */
	lwz	p7, _PPC_ESF_P7(sp)		/* restore general reg 10 */

	lwz	r11, _PPC_ESF_R11(sp)		/* restore general reg 11 */
	lwz	r12, _PPC_ESF_R12(sp)		/* restore general reg 12 */
	lwz	r13, _PPC_ESF_R13(sp)		/* restore general reg 13 */

	lwz	p0, _PPC_ESF_XER(sp)		/* load the saved XER to P0 */
	mtspr	XER, p0				/* restore XER */

	lwz	p0, _PPC_ESF_CTR(sp)		/* load the saved CTR to P0 */
	mtspr	CTR, p0				/* restore CTR */

	lis	p0, HIADJ(errno)
	lwz	p3, _PPC_ESF_ERRNO(sp)		/* load errno value */
	stw	p3, LO(errno)(p0)		/* restore errno value */

	lwz	p0, _PPC_ESF_LR(sp)		/* load the saved LR to P0 */
	mtspr	LR, p0				/* restore LR */

        lis     p1, HIADJ(intCnt)
        lwz     p2, LO(intCnt)(p1)		/* load intCnt value to P2 */
        addi    p2, p2, -1			/* decrement intCnt value */
        stw     p2, LO(intCnt)(p1)		/* save new intCnt value */

	mfspr	p0, SPRG1			/* load nesting count to P0 */
	addi	p0, p0, -1			/* decrement nesting count */
	mtspr	SPRG1, p0			/* update nesting count */

	/*
	 * we don't rely on the global variable <intCnt> because
	 * windTickAnnounce() modifies it to fake an ISR context.
	 */

	cmpwi	p0, 0				/* SPRG1 says nested int? */
	bne	intCrtRte			/* yes, just return */

	/*
	 * we need to determine if the CE interrupt occured while
	 * the CPU was in the excConnectCode stub before that Critical
	 * Exception was turned off. That is if an external interrupt is
	 * nested with this critical interrupt. If that's the case the
	 * nested int. counter was not incremented for the external
	 * interrupt: so even if SPRG1 is equal to 0 now , this is a nested
	 * interrupt situation and therefore we should return immediately
	 * via intCrtRte.
	 */

	lwz	p0, _PPC_ESF_PC(sp)		/* get PC value before CE int.*/
	rlwinm  p0, p0, 0, 0, 26		/* clear bits 27 to 31 of PC */

	cmpwi	p0, _EXC_OFF_INTR		/* was it an external int. */
	beq	intCrtRte

#ifdef	_EXC_OFF_PIT
	cmpwi	p0, _EXC_OFF_PIT		/* was it a PIT interrupt */
	beq	intCrtRte
#endif	/* _EXC_OFF_PIT */

#ifdef	_EXC_OFF_FIT
	cmpwi	p0, _EXC_OFF_FIT		/* was it a FIT interrupt */
	beq	intCrtRte
#endif	/* _EXC_OFF_FIT */

#ifdef	_EXC_NEW_OFF_PIT
	cmpwi	p0, _EXC_NEW_OFF_PIT		/* was it a PIT interrupt */
	beq	intCrtRte
#endif	/* _EXC_NEW_OFF_PIT */

#ifdef	_EXC_NEW_OFF_FIT
	cmpwi	p0, _EXC_NEW_OFF_FIT		/* was it a FIT interrupt */
	beq	intCrtRte
#endif	/* _EXC_NEW_OFF_FIT */

        lis     p2, HIADJ(kernelState)
        lwz     p0, LO(kernelState)(p2)		/* load kernelState to P0 */

	cmpwi	p0, 0				/* if kernelState == TRUE */
	bne	intCrtRte			/* just clean up and return */

        lis     p0, HIADJ(taskIdCurrent)
        lwz     p0, LO(taskIdCurrent)(p0)	/* load taskIdCurrent to P0 */

        lis     p1, HIADJ(readyQHead)
        lwz     p1, LO(readyQHead)(p1)		/* load readyQHead to P1 */

	cmpw	p0, p1				/* comp to highest ready task */
	beq	intCrtRte			/* return from interrput */

	lwz	p1, WIND_TCB_LOCK_CNT(p0)	/* load p1 with task lock cnt */
	cmpwi	p1, 0 				/* if task preemption allowed */
	beq	saveIntCrtContext		/* then save task context */

	lwz	p1, WIND_TCB_STATUS(p0)		/* set p2 to task's status */
	cmpwi	p1, 0				/* if task ready to run */
	bne	saveIntCrtContext		/* if no, save context */

intCrtRte:
	lwz	p1, _PPC_ESF_P1(sp)		/* restore p1 */
	lwz	p2, _PPC_ESF_P2(sp)		/* restore p2 */
	lwz	p3, _PPC_ESF_P3(sp)		/* restore general reg 6 */

	lwz	p0, _PPC_ESF_CR(sp)
	mtcrf	255,p0				/* restore CR */

	lwz	p0, _PPC_ESF_PC(sp)		/* load PC to P0 and */
	mtspr	CRIT_SAVE_PC, p0		/* restore CRIT_SAVE_PC */

	lwz	p0, _PPC_ESF_MSR(sp)		/* load MSR to P0 and */
	mtspr	CRIT_SAVE_MSR, p0		/* restore CRIT_SAVE_MSR */

	lwz	p0, _PPC_ESF_P0(sp)		/* restore p0 */

	lwz	sp, 0(sp)			/* pop up stack */

	rfci					/* return to previous context */

/* rescheduling is necessary.  p0 contains taskIdCurrent.
 * interrupts are still locked out */

saveIntCrtContext:
	/*
	 * when we arrive to this point
	 * p0 = taskIdCurrent
	 * p2 = kernelState MSB address
	 * p3 = errno
	 */

	li	p1, 1
        stw     p1, LO(kernelState)(p2)		/* kernelState = TRUE */

	stw	p3, WIND_TCB_ERRNO(p0)		/* save errno */

	lwz	p1, _PPC_ESF_PC(sp)		/* load PC to P1 */
	stw	p1, WIND_TCB_PC(p0)		/* store PC in tcb */

	lwz	p1, _PPC_ESF_MSR(sp)		/* read MSR to P1 */
	stw	p1, WIND_TCB_MSR(p0)		/* store msr in tcb */

	lwz	p1, _PPC_ESF_LR(sp)		/* load LR to P1 */
	stw	p1, WIND_TCB_LR(p0)		/* store LR to tcb */

	stw	r0, WIND_TCB_R0(p0) 		/* store R0 to tcb */

	stw	r2, WIND_TCB_R2(p0) 		/* store R2 to tcb */

	lwz	p1, _PPC_ESF_P0(sp)		/* load saved P0 */
	stw	p1, WIND_TCB_P0(p0) 		/* store P0 in tcb */

	lwz	p1, _PPC_ESF_P1(sp)		/* load saved P1 */
	stw	p1, WIND_TCB_P1(p0) 		/* store P1 to tcb */

	lwz	p1, _PPC_ESF_P2(sp)		/* load saved P2 */
	stw	p1, WIND_TCB_P2(p0) 		/* store P2 to tcb */

	lwz	p1, _PPC_ESF_P3(sp)		/* load saved P3 */
	stw	p1, WIND_TCB_P3(p0) 		/* store P3 to tcb */

	lwz	p1, _PPC_ESF_CR(sp)		/* load saved CR */
	stw	p1, WIND_TCB_CR(p0)		/* store CR to tcb */

	lwz	sp, 0(sp)			/* recover stack */
	stw	sp, WIND_TCB_SP(p0)		/* store SP to tcb */

	mfspr	p1, CTR				/* load CTR to p1 */
	stw	p1, WIND_TCB_CTR(p0)		/* store CTR to tcb */

	mfspr	p1, XER				/* load XER to p6 */
	stw	p1, WIND_TCB_XER(p0)		/* store XER to tcb */

# if 	TRUE
	stw	p4, WIND_TCB_P4(p0) 		/* store P4 to tcb */
	stw	p5, WIND_TCB_P5(p0) 		/* store P5 to tcb */
	stw	p6, WIND_TCB_P6(p0) 		/* store P6 to tcb */
	stw	p7, WIND_TCB_P7(p0) 		/* store P7 to tcb */

	stw	r11, WIND_TCB_R11(p0) 		/* store r11 to tcb */
	stw	r12, WIND_TCB_R12(p0) 		/* store r12 to tcb */
	stw	r13, WIND_TCB_R13(p0) 		/* store r13 to tcb */

	stw	t0, WIND_TCB_T0(p0) 		/* store t0 to tcb */
	stw	t1, WIND_TCB_T1(p0) 		/* store t1 to tcb */
	stw	t2, WIND_TCB_T2(p0) 		/* store t2 to tcb */
	stw	t3, WIND_TCB_T3(p0) 		/* store t3 to tcb */
	stw	t4, WIND_TCB_T4(p0) 		/* store t4 to tcb */
	stw	t5, WIND_TCB_T5(p0) 		/* store t5 to tcb */
	stw	t6, WIND_TCB_T6(p0) 		/* store t6 to tcb */
	stw	t7, WIND_TCB_T7(p0) 		/* store t7 to tcb */
	stw	t8, WIND_TCB_T8(p0) 		/* store t8 to tcb */
	stw	t9, WIND_TCB_T9(p0) 		/* store t9 to tcb */
	stw	t10, WIND_TCB_T10(p0) 		/* store t10 to tcb */
	stw	t11, WIND_TCB_T11(p0) 		/* store t11 to tcb */
	stw	t12, WIND_TCB_T12(p0) 		/* store t12 to tcb */
	stw	t13, WIND_TCB_T13(p0) 		/* store t13 to tcb */
	stw	t14, WIND_TCB_T14(p0) 		/* store t14 to tcb */
	stw	t15, WIND_TCB_T15(p0) 		/* store t15 to tcb */
	stw	t16, WIND_TCB_T16(p0) 		/* store t16 to tcb */
	stw	t17, WIND_TCB_T17(p0) 		/* store t17 to tcb */

# else	/* TRUE */
	stmw    p4, _PPC_ESF_P2(p0)             /* save general register 7 */
						/* through 31 */
# endif	/* TRUE */

	mfmsr	p2				/* load msr */
	ori	p2, p2, _PPC_MSR_EE		/* set EE bit */
	oris	p2, p2, _PPC_MSR_CE_U		/* set CE bit (critical intr) */
	mtmsr	p2				/* UNLOCK INTERRUPT */
	isync

	stwu    sp, -FRAMEBASESZ(sp)		/* carve stack frame */
	b	FUNC(reschedule)		/* goto rescheduler */

FUNC_END(intCrtExit)

#endif	/* _PPC_MSR_CE_U */

#if 	(CPU == PPC405F)
/* fpfix.s -- Workaround example for 405GF errata #18

   Assumptions:

   1) The routine was invoked using a brach-and-link instruction

   2) SRR0 has not been modified since entering the interrupt handler.

   3) No attempt was made to read or write any FPRs since entering the interrupt handler.
   Note: This code patch can be executed whether or not the instruction pointed to by SRR0 is a lfd,
   lfdx, lfdu, or lfdux.                                             */

/* PVR versions of 405 which need workaround for lfd instructions */
#define PVR_405GF       0x40310484      /* 405GF */
#define PVR_HIAWATHA    0x40310486      /* Hiawatha */

FUNC_BEGIN(fpfix)
	/* we need to check for two PVR versions of the 405 chip:
         * 0x40310484 and 0x40310486. Only these two versions need this
         * workaround.
         */
          mfpvr   r3                      /* get pvr */
          lis     r4, HI(PVR_405GF)
          ori     r4, r4, LO(PVR_405GF)

          cmpw    r3, r4
          beq     fpfixload	          /* 405GF */

          lis     r4, HI(PVR_HIAWATHA)
          ori     r4, r4, LO(PVR_HIAWATHA)

          cmpw    r3, r4
          beq     fpfixload	          /* HIAWATHA */

          blr			/* nothing to fix */

fpfixload:
	  mfmsr r3          /* get MSR */
          ori r3,r3,0x2000  /* set the FP bit in the MSR */
          mtmsr r3          /* update the MSR */
          isync

          /* initialize base register - method varies by assembler */
          addis r4,r0, HI(fprdata)
          ori   r4,r4, LO(fprdata)

          /* retrieve interrupting instruction */
          mfsrr0 r3         /* get SRR0 */
          lwz r3,0x0(r3)    /* retrieve the instruction */
          extrwi. r3,r3,5,6 /* extract target FPR field from instr */
          beq usefpr1       /* target FPR = 0? */

          /* execute hardware fix */
          stfd f0, 0(r4)    /* save fp reg */
          lfs f0, 0(r4)     /* dummy load to clear hardware */
          lfd f0, 0(r4)     /* restore fp reg */

          b	restoreMsr

usefpr1:  stfd f1, 0(r4)    /* save fp reg */
          lfs f1, 0(r4)     /* dummy load to clear hardware */
          lfd f1, 0(r4)     /* restore fp reg */
          b	restoreMsr

restoreMsr:
	  mfmsr	r3	/* get MSR */
	  addi  r4, r0, 0xdfff	/* r4 = 0xffffdfff */
	  and	r3, r3, r4	/* clear FP bit */
	  mtmsr r3	/* restore old MSR */
	  isync

	  blr		/* return */
FUNC_END(fpfix)

/* same work around as above, but called from a critical interrupt or
 * exception.
 */

FUNC_BEGIN(fpCrtfix)
	/* we need to check for two PVR versions of the 405 chip:
         * 0x40310484 and 0x40310486. Only these two versions need this
         * workaround.
         */
          mfpvr   r3                      /* get pvr */
          lis     r4, HI(PVR_405GF)
          ori     r4, r4, LO(PVR_405GF)

          cmpw    r3, r4
          beq     fpCrtfixload	          /* 405GF */

          lis     r4, HI(PVR_HIAWATHA)
          ori     r4, r4, LO(PVR_HIAWATHA)

          cmpw    r3, r4
          beq     fpCrtfixload	          /* HIAWATHA */

          blr			/* nothing to fix */

fpCrtfixload:
	  mfmsr r3          /* get MSR */
          ori r3,r3,0x2000  /* set the FP bit in the MSR */
          mtmsr r3          /* update the MSR */
          isync

          /* initialize base register - method varies by assembler */
          addis r4,r0, HI(fprdata)
          ori   r4,r4, LO(fprdata)

          /* retrieve interrupting instruction */
          mfspr r3, CRIT_SAVE_PC	/* PC where interrupt occurred */
          lwz r3,0x0(r3)    /* retrieve the instruction */
          extrwi. r3,r3,5,6 /* extract target FPR field from instr */
          beq useCrtfpr1       /* target FPR = 0? */

          /* execute hardware fix */
          stfd f0, 0(r4)    /* save fp reg */
          lfs f0, 0(r4)     /* dummy load to clear hardware */
          lfd f0, 0(r4)     /* restore fp reg */

          b	restoreCrtMsr

useCrtfpr1:  stfd f1, 0(r4)    /* save fp reg */
          lfs f1, 0(r4)     /* dummy load to clear hardware */
          lfd f1, 0(r4)     /* restore fp reg */
          b	restoreCrtMsr

restoreCrtMsr:
	  mfmsr	r3	/* get MSR */
	  addi  r4, r0, 0xdfff	/* r4 = 0xffffdfff */
	  and	r3, r3, r4	/* clear FP bit */
	  mtmsr r3	/* restore old MSR */
	  isync

	  blr		/* return */
FUNC_END(fpCrtfix)

.data
.align 3
fprdata: .double 0            /* save/restore location */

#endif 	/* CPU == PPC405F */