cpu_asm.s

T-kernel 的extension源代码

/*
 *----------------------------------------------------------------------
 *    T-Kernel / Standard Extension
 *
 *    Copyright (C) 2006 by Ken Sakamura. All rights reserved.
 *    T-Kernel / Standard Extension is distributed 
 *      under the T-License for T-Kernel / Standard Extension.
 *----------------------------------------------------------------------
 *
 *    Version:   1.00.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2006/8/11.
 *
 *----------------------------------------------------------------------
 */

/*
 *	cpu_asm.S (memory)
 *
 *	VR5500-dependent functions
 */

#define _in_asm_source_
#include <machine.h>
#include <tk/asm.h>
#include <sys/sysinfo.h>
#include <sys/memdef.h>
#include "cpu_asm.h"

/* Model that use FPU */
#define	USE_FPU		(1)

/*
 * Restore interrupt-disabled state
 *	How to handle CU1 when FPU exists
 *	CU1 is used in a context switch of FPU. CU1 may be changed at any time
 *	if task dispatch is allowed.
 *	CU1 may be changed into 0 after PSR is read.
 *	When set at "CU1=1" in such a case,
 *	wrong FPU context may be used.
 *	PSR cannot be updated (read->modify->write) exclusively. To cope with
 *	this problem, always set CU1 at 0 when updating PSR.
 */
.macro mtc0_PSR reg
#if USE_FPU
	and	\reg, \reg, ~SR_CU1
#endif
	mtc0	\reg, $CP0_PSR
.endm

/* ------------------------------------------------------------------------ */

/*
 * Call system program
 * ER CallSysProgInit( INT ac, UB *av[], FP entry )
 */
	.text
	.balign	4
	.globl	Csym(CallSysProgInit)
	.type	Csym(CallSysProgInit), %function
Csym(CallSysProgInit):
#if USE_FPU
	subu	$sp, $sp, 23*8		// Save FPU register
	sdc1	$f10,  0*8($sp)
	sdc1	$f11,  1*8($sp)
	sdc1	$f12,  2*8($sp)
	sdc1	$f13,  3*8($sp)
	sdc1	$f14,  4*8($sp)
	sdc1	$f15,  5*8($sp)
	sdc1	$f16,  6*8($sp)
	sdc1	$f17,  7*8($sp)
	sdc1	$f18,  8*8($sp)
	sdc1	$f19,  9*8($sp)
	sdc1	$f20, 10*8($sp)
	sdc1	$f21, 11*8($sp)
	sdc1	$f22, 12*8($sp)
	sdc1	$f23, 13*8($sp)
	sdc1	$f24, 14*8($sp)
	sdc1	$f25, 15*8($sp)
	sdc1	$f26, 16*8($sp)
	sdc1	$f27, 17*8($sp)
	sdc1	$f28, 18*8($sp)
	sdc1	$f29, 19*8($sp)
	sdc1	$f30, 20*8($sp)
	sdc1	$f31, 21*8($sp)
	cfc1	$t0,  $31
	sd	$t0,  22*8($sp)
#endif
	subu	$sp, $sp, 15*8		// Save register
	sd	$s0,  4*8($sp)
	sd	$s1,  5*8($sp)
	sd	$s2,  6*8($sp)
	sd	$s3,  7*8($sp)
	sd	$s4,  8*8($sp)
	sd	$s5,  9*8($sp)
	sd	$s6, 10*8($sp)
	sd	$s7, 11*8($sp)
	sd	$gp, 12*8($sp)
	sd	$fp, 13*8($sp)
	sd	$ra, 14*8($sp)

	jalr	$a2			// call entry(ac, av)

	ld	$s0,  4*8($sp)		// Restore register
	ld	$s1,  5*8($sp)
	ld	$s2,  6*8($sp)
	ld	$s3,  7*8($sp)
	ld	$s4,  8*8($sp)
	ld	$s5,  9*8($sp)
	ld	$s6, 10*8($sp)
	ld	$s7, 11*8($sp)
	ld	$gp, 12*8($sp)
	ld	$fp, 13*8($sp)
	ld	$ra, 14*8($sp)
	addu	$sp, $sp, 15*8
#if USE_FPU
	ldc1	$f10,  0*8($sp)		// Restore FPU register
	ldc1	$f11,  1*8($sp)
	ldc1	$f12,  2*8($sp)
	ldc1	$f13,  3*8($sp)
	ldc1	$f14,  4*8($sp)
	ldc1	$f15,  5*8($sp)
	ldc1	$f16,  6*8($sp)
	ldc1	$f17,  7*8($sp)
	ldc1	$f18,  8*8($sp)
	ldc1	$f19,  9*8($sp)
	ldc1	$f20, 10*8($sp)
	ldc1	$f21, 11*8($sp)
	ldc1	$f22, 12*8($sp)
	ldc1	$f23, 13*8($sp)
	ldc1	$f24, 14*8($sp)
	ldc1	$f25, 15*8($sp)
	ldc1	$f26, 16*8($sp)
	ldc1	$f27, 17*8($sp)
	ldc1	$f28, 18*8($sp)
	ldc1	$f29, 19*8($sp)
	ldc1	$f30, 20*8($sp)
	ldc1	$f31, 21*8($sp)
	ld	$t0,  22*8($sp)
	ctc1	$t0,  $31
	addu	$sp, $sp, 23*8
#endif
	jr	$ra

/* ------------------------------------------------------------------------ */
/*
 *	Cache-related
 */

	/* Cache line size */
	.extern	Csym(ICacheLineSize), 4		/* Instruction cache */
	.extern	Csym(DCacheLineSize), 4		/* Data cache */

/*
 * Cancel memory cache content (both instruction/data caches)
 *	void InvalidateCache( VP laddr, UW lsid )
 *      Cancel (do not write back) a page (4KB) that contains laddr.
 *	Do not specify an invalid page (PTE.p=0).
 */
	.text
	.balign	4
	.globl	Csym(InvalidateCache)
	.type	Csym(InvalidateCache), %function
Csym(InvalidateCache):
	mfc0	$t7, $CP0_PSR		// t7 = PSR Save
	and	$t0, $t7, ~SR_IE
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled IE=0
	NOP(HzD_MTC0 - HzS_INT - 1)
	nop
	dmfc0	$t8, $CP0_EHI		// t8 = Save entry Hi (ASID)

	and	$a0, $a0, ~(PAGESIZE-1)	// laddr at start of page
	addu	$t1, $a0, PAGESIZE	// t1 = End address

	dmtc0	$a1, $CP0_EHI		// Switch logical spaces
	NOP(HzD_MTC0 - HzS_FETCH - 1)

	move	$t0, $a0		// Flush instruction cache
	lw	$t2, Csym(ICacheLineSize)
  lv_1:
	cache	(4 << 2)|0, ($t0)	// Hit Invalidate (instruction)
	addu	$t0, $t2
	bltu	$t0, $t1, lv_1

	move	$t0, $a0		// Flush data cache
	lw	$t2, Csym(DCacheLineSize)
  lv_2:
	cache	(4 << 2)|1, ($t0)	// Hit Invalidate (data)
	addu	$t0, $t2
	bltu	$t0, $t1, lv_2

	dmtc0	$t8, $CP0_EHI		// Restore entry Hi (ASID)
	NOP(HzD_MTC0 - HzS_FETCH)
	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/*
 * Flush memory cache (both instruction/data caches)
 *	void ExtFlushCache( VP laddr, UW lsid )
 *      Flush a page (4KB) that contains laddr and cancel it.
 *	Do not specify an invalid page (PTE.p=0).
 */
	.text
	.balign	4
	.globl	Csym(ExtFlushCache)
	.type	Csym(ExtFlushCache), %function
Csym(ExtFlushCache):
	mfc0	$t7, $CP0_PSR		// t7 = PSR Save
	and	$t0, $t7, ~SR_IE
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled IE=0
	NOP(HzD_MTC0 - HzS_INT - 1)
	nop
	dmfc0	$t8, $CP0_EHI		// t8 = Save entry Hi (ASID)

	and	$a0, $a0, ~(PAGESIZE-1)	// laddr at start of page
	addu	$t1, $a0, PAGESIZE	// t1 = End address

	dmtc0	$a1, $CP0_EHI		// Switch logical spaces
	NOP(HzD_MTC0 - HzS_FETCH - 1)

	move	$t0, $a0		// Flush instruction cache
	lw	$t2, Csym(ICacheLineSize)
  lc_1:
	cache	(4 << 2)|0, ($t0)	// Hit Invalidate (instruction)
	addu	$t0, $t2
	bltu	$t0, $t1, lc_1

	move	$t0, $a0		// Flush data cache
	lw	$t2, Csym(DCacheLineSize)
  lc_2:
	cache	(5 << 2)|1, ($t0)	// Hit Write Back Invalidate (data)
	addu	$t0, $t2
	bltu	$t0, $t1, lc_2

	dmtc0	$t8, $CP0_EHI		// Restore entry Hi (ASID)
	NOP(HzD_MTC0 - HzS_FETCH)
	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/*
 * Write back memory cache (data cache only).
 *	void FlushDCache( VP laddr, UW lsid );
 *	Write back (do not cancel) a page (4KB) that contains laddr.
 *	Do not specify an invalid page (PTE.p=0).
 */
	.text
	.balign	4
	.globl	Csym(FlushDCache)
	.type	Csym(FlushDCache), %function
Csym(FlushDCache):
	mfc0	$t7, $CP0_PSR		// t7 = PSR Save
	and	$t0, $t7, ~SR_IE
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled IE=0
	NOP(HzD_MTC0 - HzS_INT - 1)
	nop
	dmfc0	$t8, $CP0_EHI		// t8 = Save entry Hi (ASID)

	and	$a0, $a0, ~(PAGESIZE-1)	// laddr at start of page
	addu	$t1, $a0, PAGESIZE	// t1 = End address

	dmtc0	$a1, $CP0_EHI		// Switch logical spaces
	NOP(HzD_MTC0 - HzS_FETCH - 1)

	move	$t0, $a0		// Write back data cache
	lw	$t2, Csym(DCacheLineSize)
  lw_1:
	cache	(6 << 2)|1, ($t0)	// Hit Write Back (data)
	addu	$t0, $t2
	bltu	$t0, $t1, lw_1

	dmtc0	$t8, $CP0_EHI		// Restore entry Hi (ASID)
	NOP(HzD_MTC0 - HzS_FETCH)
	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/* ------------------------------------------------------------------------ */
/*
 *	TLB-related
 */

/*
 * Page directory
 *	PDE	*SATB		Shared space page directory
 *	PDE	*UATB		Unique space page directory
 *
 *	It is necessary to access page directories when handling TLB exceptions at re-startup.
 *	Therefore, SATB and UATB must not be located in data areas. At re-startup, data areas
 *	(data,bss) are initialized.
 */
#define	SATB	_SATB
#define	UATB	_UATB

	.text
	.balign	4
	.globl	Csym(SATB)
	.type	Csym(SATB), %object
	.globl	Csym(UATB)
	.type	Csym(UATB), %object
Csym(SATB):	.int	0	/* Shared space page directory */
Csym(UATB):	.int	0	/* Unique space page directory */

/*
 * Initialize TLB
 *	void InitializeTLB( void )
 */
	.text
	.balign	4
	.globl	Csym(InitializeTLB)
	.type	Csym(InitializeTLB), %function
Csym(InitializeTLB):
	mfc0	$t7, $CP0_PSR
	and	$t0, $t7, ~SR_IE
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled
	NOP(HzD_MTC0 - HzS_INT)

	mtc0	$zero, $CP0_WIR		// Initialize wired register

	li	$t0, TLB_PMSK_4KB	// Set page size (4KB)
	mtc0	$t0, $CP0_PMSK

	li	$t1, ELO_INIT		// Entry Lo initial value
	dmtc0	$t1, $CP0_ELO0
	dmtc0	$t1, $CP0_ELO1

	li	$t2, 0xffffffff80000000	// Address of TLB invalid area

	li	$t1, N_TLBENTRY

  li_1:
	subu	$t1, $t1, 1
	mtc0	$t1, $CP0_IDX

	dmtc0	$t2, $CP0_EHI	//	moved

	NOP(HzD_MTC0 - HzS_TLBW)
	tlbwi
	NOP(HzD_TLBW - HzS_TLBW - 1)

	addu $t2, $t2, 0x2000	//	The case that the page size is 4kB

	bnez	$t1, li_1

	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/*
 * Purge the whole TLB
 *	void PurgeAllTLB( void )
 */
	.text
	.balign	4
	.globl	Csym(PurgeAllTLB)
	.type	Csym(PurgeAllTLB), %function
Csym(PurgeAllTLB):
	mfc0	$t7, $CP0_PSR
	and	$t0, $t7, ~SR_IE	// t7 = PSR Save
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled
	NOP(HzD_MTC0 - HzS_INT)
	dmfc0	$t8, $CP0_EHI		// t8 = Save entry Hi

	li	$t0, TLB_PMSK_4KB	// Set page size (4KB)
	mtc0	$t0, $CP0_PMSK

	li	$t1, ELO_INIT		// Entry Lo initial value
	dmtc0	$t1, $CP0_ELO0
	dmtc0	$t1, $CP0_ELO1

	li	$t3, 0xffffffff80000000	// Address of TLB invalid area

	li	$t1, N_TLBENTRY
	mfc0	$t2, $CP0_WIR

  la_1:
	subu	$t1, $t1, 1
	mtc0	$t1, $CP0_IDX

	dmtc0	$t3, $CP0_EHI	//	moved

	NOP(HzD_MTC0 - HzS_TLBW)
	tlbwi
	NOP(HzD_TLBW - HzS_TLBW - 1)

	addu $t3, $t3, 0x2000	//	The case that the page size is 4kB

	bne	$t1, $t2, la_1

	dmtc0	$t8, $CP0_EHI		// Restore entry Hi
	NOP(HzD_MTC0 - HzS_FETCH)
	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/*
 * Purge TLB that contains specified logical address.
 *	void PurgePageTLB( VP laddr, UW lsid )
 */
	.text
	.balign	4
	.globl	Csym(PurgePageTLB)
	.type	Csym(PurgePageTLB), %function
Csym(PurgePageTLB):
	mfc0	$t7, $CP0_PSR
	and	$t0, $t7, ~SR_IE	// t7 = PSR Save
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled
	NOP(HzD_MTC0 - HzS_INT)
	dmfc0	$t8, $CP0_EHI		// t8 = Save entry Hi

	and	$t0, $a0, EHI_VPNMSK
	or	$t0, $t0, $a1
	dmtc0	$t0, $CP0_EHI
	NOP(HzD_MTC0 - HzS_TLBP)
	tlbp				// Search for intended entry
	NOP(HzD_TLBP - HzS_MFC0)
	mfc0	$t0, $CP0_IDX
	bltz	$t0, lp_1		// If no entries are registered in TLB, processing is unnecessary.

	tlbr				// Read indented entry
	NOP(HzD_TLBR - HzS_MFC0 - 2)
	and	$t0, $a0, EHI_LOSEL	// Select Lo register
	bnez	$t0, lp_2

	dmfc0	$t1, $CP0_ELO0		// Lo0
	and	$t1, $t1, ~ELO_V	// Clear V bit
	dmtc0	$t1, $CP0_ELO0
	j	lp_3

  lp_2:
	dmfc0	$t1, $CP0_ELO1		// Lo1
	and	$t1, $t1, ~ELO_V	// Clear V bit
	dmtc0	$t1, $CP0_ELO1

  lp_3:
	NOP(HzD_MTC0 - HzS_TLBW)
	tlbwi				// Update TLB
	NOP(HzD_TLBW - HzS_FETCH)

  lp_1:
	dmtc0	$t8, $CP0_EHI		// Restore entry Hi
	NOP(HzD_MTC0 - HzS_FETCH)
	mtc0_PSR $t7			// Turn back interrupt-disabled state
	jr	$ra

/*
 * Update TLB
 *	void UpdateTLB( VP laddr, UW lsid, UW pte )
 *
 *	If there is an entry whose logical address matches the current TLB, update TLB;
 *	if not, register a new TLB.
 */
	.text
	.balign	4
	.globl	Csym(UpdateTLB)
	.type	Csym(UpdateTLB), %function
Csym(UpdateTLB):
	mfc0	$t7, $CP0_PSR		// t7 = PSR Save
	and	$t0, $t7, ~SR_IE
	mtc0	$t0, $CP0_PSR		// Interrupt-disabled
	NOP(HzD_MTC0 - HzS_INT)