cpu_asm.s

RTEMS (Real-Time Executive for Multiprocessor Systems) is a free open source real-time operating sys

 */
	.import    _Context_Switch_necessary,data
	ldil	   L%_Context_Switch_necessary,%r8
	ldw	   R%_Context_Switch_necessary(%r8),%r8
	comibf,=,n 0,%r8,ISR_dispatch

/*
 * check whether or not a context switch is necessary because an ISR
 *    sent signals to the interrupted task
 */
	ldw	   R%_ISR_Signals_to_thread_executing(%r7),%r8
	comibt,=,n 0,%r8,isr_restore


/*
 * OK, something happened while in ISR and we need to switch to a task
 * other than the one which was interrupted or the
 *    ISR_Signals_to_thread_executing case
 * We also turn on interrupts, since the interrupted task had them
 *   on (obviously :-) and Thread_Dispatch is happy to leave ints on.
 */

ISR_dispatch:
	stw	   %r0, R%_ISR_Signals_to_thread_executing(%r7)

        ssm        HPPA_PSW_I, %r0

        .import    _Thread_Dispatch,code
        .call
        bl         _Thread_Dispatch,%r2
        ldo        128(sp),sp

        ldo        -128(sp),sp

isr_restore:

/*
 * enable interrupts during most of restore
 */
        ssm        HPPA_PSW_I, %r0

/*
 * Get a pointer to beginning of our stack frame
 */
        ldo        -CPU_INTERRUPT_FRAME_SIZE(sp), %arg1

/*
 * restore float
 */
        .call ARGW0=GR
        bl         _CPU_Restore_float_context,%r2
        ldo        FP_CONTEXT_OFFSET(%arg1), arg0

        copy       %arg1, %arg0

/*
 *   ********** FALL THRU **********
 */

/*
 * Jump here from bottom of Context_Switch
 * Also called directly by _CPU_Context_Restart_self via _Thread_Restart_self
 * restore interrupt state
 */

	.EXPORT _CPU_Context_restore
_CPU_Context_restore:

/*
 * restore integer state
 */
        ldw         R1_OFFSET(arg0),%r1
        ldw         R2_OFFSET(arg0),%r2
        ldw         R3_OFFSET(arg0),%r3
        ldw         R4_OFFSET(arg0),%r4
        ldw         R5_OFFSET(arg0),%r5
        ldw         R6_OFFSET(arg0),%r6
        ldw         R7_OFFSET(arg0),%r7
        ldw         R8_OFFSET(arg0),%r8
        ldw         R9_OFFSET(arg0),%r9
        ldw         R10_OFFSET(arg0),%r10
        ldw         R11_OFFSET(arg0),%r11
        ldw         R12_OFFSET(arg0),%r12
        ldw         R13_OFFSET(arg0),%r13
        ldw         R14_OFFSET(arg0),%r14
        ldw         R15_OFFSET(arg0),%r15
        ldw         R16_OFFSET(arg0),%r16
        ldw         R17_OFFSET(arg0),%r17
        ldw         R18_OFFSET(arg0),%r18
        ldw         R19_OFFSET(arg0),%r19
        ldw         R20_OFFSET(arg0),%r20
        ldw         R21_OFFSET(arg0),%r21
        ldw         R22_OFFSET(arg0),%r22
        ldw         R23_OFFSET(arg0),%r23
        ldw         R24_OFFSET(arg0),%r24
/*
 * skipping r25; used as scratch register below
 * skipping r26 (arg0) until we are done with it
 */
        ldw         R27_OFFSET(arg0),%r27
        ldw         R28_OFFSET(arg0),%r28
        ldw         R29_OFFSET(arg0),%r29
/*
 * skipping r30 (sp) until we turn off interrupts
 */
        ldw         R31_OFFSET(arg0),%r31

/*
 * Turn off Q & R & I so we can write r30 and interrupt control registers
 */
        rsm        HPPA_PSW_Q + HPPA_PSW_R + HPPA_PSW_I, %r0

/*
 * now safe to restore r30
 */
        ldw         R30_OFFSET(arg0),%r30

        ldw        IPSW_OFFSET(arg0), %r25
        mtctl      %r25, ipsw

        ldw        SAR_OFFSET(arg0), %r25
        mtctl      %r25, sar

        ldw        PCOQFRONT_OFFSET(arg0), %r25
        mtctl      %r25, pcoq

        ldw        PCOQBACK_OFFSET(arg0), %r25
        mtctl      %r25, pcoq

/*
 * Load r25 with interrupts off
 */
        ldw         R25_OFFSET(arg0),%r25
/*
 * Must load r26 (arg0) last
 */
        ldw         R26_OFFSET(arg0),%r26

isr_exit:
        rfi
        .EXIT
        .PROCEND

/*
 *  This section is used to context switch floating point registers.
 *  Ref:  6-35 of Architecture 1.1
 *
 *  NOTE:    since integer multiply uses the floating point unit,
 *           we have to save/restore fp on every trap.  We cannot
 *           just try to keep track of fp usage.
 */

	.align 32
	.EXPORT _CPU_Save_float_context,ENTRY,PRIV_LEV=0
_CPU_Save_float_context:
	.PROC
	.CALLINFO FRAME=0,NO_CALLS
	.ENTRY
        fstds,ma    %fr0,8(%arg0)
        fstds,ma    %fr1,8(%arg0)
        fstds,ma    %fr2,8(%arg0)
        fstds,ma    %fr3,8(%arg0)
        fstds,ma    %fr4,8(%arg0)
        fstds,ma    %fr5,8(%arg0)
        fstds,ma    %fr6,8(%arg0)
        fstds,ma    %fr7,8(%arg0)
        fstds,ma    %fr8,8(%arg0)
        fstds,ma    %fr9,8(%arg0)
        fstds,ma    %fr10,8(%arg0)
        fstds,ma    %fr11,8(%arg0)
        fstds,ma    %fr12,8(%arg0)
        fstds,ma    %fr13,8(%arg0)
        fstds,ma    %fr14,8(%arg0)
        fstds,ma    %fr15,8(%arg0)
        fstds,ma    %fr16,8(%arg0)
        fstds,ma    %fr17,8(%arg0)
        fstds,ma    %fr18,8(%arg0)
        fstds,ma    %fr19,8(%arg0)
        fstds,ma    %fr20,8(%arg0)
        fstds,ma    %fr21,8(%arg0)
        fstds,ma    %fr22,8(%arg0)
        fstds,ma    %fr23,8(%arg0)
        fstds,ma    %fr24,8(%arg0)
        fstds,ma    %fr25,8(%arg0)
        fstds,ma    %fr26,8(%arg0)
        fstds,ma    %fr27,8(%arg0)
        fstds,ma    %fr28,8(%arg0)
        fstds,ma    %fr29,8(%arg0)
        fstds,ma    %fr30,8(%arg0)
        fstds       %fr31,0(%arg0)
        bv          0(%r2)
        addi        -(31*8), %arg0, %arg0        ; restore arg0 just for fun
	.EXIT
	.PROCEND

	.align 32
	.EXPORT _CPU_Restore_float_context,ENTRY,PRIV_LEV=0
_CPU_Restore_float_context:
	.PROC
	.CALLINFO FRAME=0,NO_CALLS
	.ENTRY
        addi        (31*8), %arg0, %arg0         ; point at last double
        fldds       0(%arg0),%fr31
        fldds,mb    -8(%arg0),%fr30
        fldds,mb    -8(%arg0),%fr29
        fldds,mb    -8(%arg0),%fr28
        fldds,mb    -8(%arg0),%fr27
        fldds,mb    -8(%arg0),%fr26
        fldds,mb    -8(%arg0),%fr25
        fldds,mb    -8(%arg0),%fr24
        fldds,mb    -8(%arg0),%fr23
        fldds,mb    -8(%arg0),%fr22
        fldds,mb    -8(%arg0),%fr21
        fldds,mb    -8(%arg0),%fr20
        fldds,mb    -8(%arg0),%fr19
        fldds,mb    -8(%arg0),%fr18
        fldds,mb    -8(%arg0),%fr17
        fldds,mb    -8(%arg0),%fr16
        fldds,mb    -8(%arg0),%fr15
        fldds,mb    -8(%arg0),%fr14
        fldds,mb    -8(%arg0),%fr13
        fldds,mb    -8(%arg0),%fr12
        fldds,mb    -8(%arg0),%fr11
        fldds,mb    -8(%arg0),%fr10
        fldds,mb    -8(%arg0),%fr9
        fldds,mb    -8(%arg0),%fr8
        fldds,mb    -8(%arg0),%fr7
        fldds,mb    -8(%arg0),%fr6
        fldds,mb    -8(%arg0),%fr5
        fldds,mb    -8(%arg0),%fr4
        fldds,mb    -8(%arg0),%fr3
        fldds,mb    -8(%arg0),%fr2
        fldds,mb    -8(%arg0),%fr1
        bv          0(%r2)
        fldds,mb    -8(%arg0),%fr0
	.EXIT
	.PROCEND

/*
 * These 2 small routines are unused right now.
 * Normally we just go thru _CPU_Save_float_context (and Restore)
 *
 * Here we just deref the ptr and jump up, letting _CPU_Save_float_context
 *  do the return for us.
 */

	.EXPORT _CPU_Context_save_fp,ENTRY,PRIV_LEV=0
_CPU_Context_save_fp:
	.PROC
	.CALLINFO FRAME=0,NO_CALLS
	.ENTRY
        bl          _CPU_Save_float_context, %r0
        ldw         0(%arg0), %arg0
	.EXIT
	.PROCEND

	.EXPORT _CPU_Context_restore_fp,ENTRY,PRIV_LEV=0
_CPU_Context_restore_fp:
	.PROC
	.CALLINFO FRAME=0,NO_CALLS
	.ENTRY
        bl          _CPU_Restore_float_context, %r0
        ldw         0(%arg0), %arg0
	.EXIT
	.PROCEND


/*
 *  void _CPU_Context_switch( run_context, heir_context )
 *
 *  This routine performs a normal non-FP context switch.
 */

	.align 32
	.EXPORT _CPU_Context_switch,ENTRY,PRIV_LEV=0,ARGW0=GR,ARGW1=GR
_CPU_Context_switch:
    	.PROC
	.CALLINFO FRAME=64
	.ENTRY

/*
 * Save the integer context
 */
        stw         %r1,R1_OFFSET(arg0)
        stw         %r2,R2_OFFSET(arg0)
        stw         %r3,R3_OFFSET(arg0)
        stw         %r4,R4_OFFSET(arg0)
        stw         %r5,R5_OFFSET(arg0)
        stw         %r6,R6_OFFSET(arg0)
        stw         %r7,R7_OFFSET(arg0)
        stw         %r8,R8_OFFSET(arg0)
        stw         %r9,R9_OFFSET(arg0)
        stw         %r10,R10_OFFSET(arg0)
        stw         %r11,R11_OFFSET(arg0)
        stw         %r12,R12_OFFSET(arg0)
        stw         %r13,R13_OFFSET(arg0)
        stw         %r14,R14_OFFSET(arg0)
        stw         %r15,R15_OFFSET(arg0)
        stw         %r16,R16_OFFSET(arg0)
        stw         %r17,R17_OFFSET(arg0)
        stw         %r18,R18_OFFSET(arg0)
        stw         %r19,R19_OFFSET(arg0)
        stw         %r20,R20_OFFSET(arg0)
        stw         %r21,R21_OFFSET(arg0)
        stw         %r22,R22_OFFSET(arg0)
        stw         %r23,R23_OFFSET(arg0)
        stw         %r24,R24_OFFSET(arg0)
        stw         %r25,R25_OFFSET(arg0)
        stw         %r26,R26_OFFSET(arg0)
        stw         %r27,R27_OFFSET(arg0)
        stw         %r28,R28_OFFSET(arg0)
        stw         %r29,R29_OFFSET(arg0)
        stw         %r30,R30_OFFSET(arg0)
        stw         %r31,R31_OFFSET(arg0)

/*
 * fill in interrupt context section
 */
        stw         %r2, PCOQFRONT_OFFSET(%arg0)
        ldo         4(%r2), %r2
        stw         %r2, PCOQBACK_OFFSET(%arg0)

/*
 * Generate a suitable IPSW by using the system default psw
 *  with the current low bits added in.
 */

        ldil        L%CPU_PSW_DEFAULT, %r2
        ldo         R%CPU_PSW_DEFAULT(%r2), %r2
        ssm         0, %arg2
        dep         %arg2, 31, 8, %r2
        stw         %r2, IPSW_OFFSET(%arg0)

/*
 * at this point, the running task context is completely saved
 * Now jump to the bottom of the interrupt handler to load the
 * heirs context
 */

        b           _CPU_Context_restore
        copy        %arg1, %arg0

        .EXIT
        .PROCEND


/*
 * Find first bit
 * NOTE:
 *   This is used (and written) only for the ready chain code and
 *   priority bit maps.
 *   Any other use constitutes fraud.
 *   Returns first bit from the least significant side.
 *   Eg:  if input is 0x8001
 *        output will indicate the '1' bit and return 0.
 *   This is counter to HPPA bit numbering which calls this
 *   bit 31.  This way simplifies the macros _CPU_Priority_Mask
 *   and _CPU_Priority_Bits_index.
 *
 *   NOTE:
 *       We just use 16 bit version
 *       does not handle zero case
 *
 *  Based on the UTAH Mach libc version of ffs.
 */

        .align 32
	.EXPORT hppa_rtems_ffs,ENTRY,PRIV_LEV=0,ARGW0=GR
hppa_rtems_ffs:
    	.PROC
	.CALLINFO FRAME=0,NO_CALLS
	.ENTRY

#ifdef RETURN_ERROR_ON_ZERO
	comb,=	%arg0,%r0,ffsdone	; If arg0 is 0
	ldi	-1,%ret0		;   return -1
#endif

#if BITFIELD_SIZE == 32
	ldi	31,%ret0		; Set return to high bit
	extru,=	%arg0,31,16,%r0		; If low 16 bits are non-zero
	addi,tr	-16,%ret0,%ret0		;   subtract 16 from bitpos
	shd	%r0,%arg0,16,%arg0	; else shift right 16 bits
#else
	ldi	15,%ret0		; Set return to high bit
#endif
	extru,=	%arg0,31,8,%r0		; If low 8 bits are non-zero
	addi,tr	-8,%ret0,%ret0		;   subtract 8 from bitpos
	shd	%r0,%arg0,8,%arg0	; else shift right 8 bits
	extru,=	%arg0,31,4,%r0		; If low 4 bits are non-zero
	addi,tr	-4,%ret0,%ret0		;   subtract 4 from bitpos
	shd	%r0,%arg0,4,%arg0	; else shift right 4 bits
	extru,=	%arg0,31,2,%r0		; If low 2 bits are non-zero
	addi,tr	-2,%ret0,%ret0		;   subtract 2 from bitpos
	shd	%r0,%arg0,2,%arg0	; else shift right 2 bits
	extru,=	%arg0,31,1,%r0		; If low bit is non-zero
	addi	-1,%ret0,%ret0		;   subtract 1 from bitpos
ffsdone:
        bv,n    0(%r2)
        nop
        .EXIT
        .PROCEND