trap.c

早期freebsd实现

		switch ((int)inst.RType.func) {
		case OP_JR:
		case OP_JALR:
			retAddr = regsPtr[inst.RType.rs];
			break;

		default:
			if (!allowNonBranch)
				panic("MachEmulateBranch: Non-branch");
			retAddr = instPC + 4;
			break;
		}
		break;

	case OP_BCOND:
		switch ((int)inst.IType.rt) {
		case OP_BLTZ:
		case OP_BLTZAL:
			if ((int)(regsPtr[inst.RType.rs]) < 0)
				retAddr = GetBranchDest((InstFmt *)instPC);
			else
				retAddr = instPC + 8;
			break;

		case OP_BGEZAL:
		case OP_BGEZ:
			if ((int)(regsPtr[inst.RType.rs]) >= 0)
				retAddr = GetBranchDest((InstFmt *)instPC);
			else
				retAddr = instPC + 8;
			break;

		default:
			panic("MachEmulateBranch: Bad branch cond");
		}
		break;

	case OP_J:
	case OP_JAL:
		retAddr = (inst.JType.target << 2) | 
			((unsigned)instPC & 0xF0000000);
		break;

	case OP_BEQ:
		if (regsPtr[inst.RType.rs] == regsPtr[inst.RType.rt])
			retAddr = GetBranchDest((InstFmt *)instPC);
		else
			retAddr = instPC + 8;
		break;

	case OP_BNE:
		if (regsPtr[inst.RType.rs] != regsPtr[inst.RType.rt])
			retAddr = GetBranchDest((InstFmt *)instPC);
		else
			retAddr = instPC + 8;
		break;

	case OP_BLEZ:
		if ((int)(regsPtr[inst.RType.rs]) <= 0)
			retAddr = GetBranchDest((InstFmt *)instPC);
		else
			retAddr = instPC + 8;
		break;

	case OP_BGTZ:
		if ((int)(regsPtr[inst.RType.rs]) > 0)
			retAddr = GetBranchDest((InstFmt *)instPC);
		else
			retAddr = instPC + 8;
		break;

	case OP_COP1:
		switch (inst.RType.rs) {
		case OP_BCx:
		case OP_BCy:
			if ((inst.RType.rt & COPz_BC_TF_MASK) == COPz_BC_TRUE)
				condition = fpcCSR & MACH_FPC_COND_BIT;
			else
				condition = !(fpcCSR & MACH_FPC_COND_BIT);
			if (condition)
				retAddr = GetBranchDest((InstFmt *)instPC);
			else
				retAddr = instPC + 8;
			break;

		default:
			if (!allowNonBranch)
				panic("MachEmulateBranch: Bad coproc branch instruction");
			retAddr = instPC + 4;
		}
		break;

	default:
		if (!allowNonBranch)
			panic("MachEmulateBranch: Non-branch instruction");
		retAddr = instPC + 4;
	}
#if 0
	printf("Target addr=%x\n", retAddr);
#endif
	return (retAddr);
}

unsigned
GetBranchDest(InstPtr)
	InstFmt *InstPtr;
{
	return ((unsigned)InstPtr + 4 + ((short)InstPtr->IType.imm << 2));
}

/*
 * This routine is called by procxmt() to single step one instruction.
 * We do this by storing a break instruction after the current instruction,
 * resuming execution, and then restoring the old instruction.
 */
cpu_singlestep(p)
	register struct proc *p;
{
	register unsigned va;
	register int *locr0 = p->p_md.md_regs;
	int i;

	/* compute next address after current location */
	va = MachEmulateBranch(locr0, locr0[PC], 0, 1);
	if (p->p_md.md_ss_addr || p->p_md.md_ss_addr == va ||
	    !useracc((caddr_t)va, 4, B_READ)) {
		printf("SS %s (%d): breakpoint already set at %x (va %x)\n",
			p->p_comm, p->p_pid, p->p_md.md_ss_addr, va); /* XXX */
		return (EFAULT);
	}
	p->p_md.md_ss_addr = va;
	p->p_md.md_ss_instr = fuiword((caddr_t)va);
	i = suiword((caddr_t)va, MACH_BREAK_SSTEP);
	if (i < 0) {
		vm_offset_t sa, ea;
		int rv;

		sa = trunc_page((vm_offset_t)va);
		ea = round_page((vm_offset_t)va+sizeof(int)-1);
		rv = vm_map_protect(&p->p_vmspace->vm_map, sa, ea,
			VM_PROT_DEFAULT, FALSE);
		if (rv == KERN_SUCCESS) {
			i = suiword((caddr_t)va, MACH_BREAK_SSTEP);
			(void) vm_map_protect(&p->p_vmspace->vm_map,
				sa, ea, VM_PROT_READ|VM_PROT_EXECUTE, FALSE);
		}
	}
	if (i < 0)
		return (EFAULT);
	printf("SS %s (%d): breakpoint set at %x: %x (pc %x)\n",
		p->p_comm, p->p_pid, p->p_md.md_ss_addr,
		p->p_md.md_ss_instr, locr0[PC]); /* XXX */
	return (0);
}

/*
 * news3400 - INT0 service routine.
 *
 * INTST0 bit	4:	dma
 *		3:	slot #1
 *		2:	slot #3
 *		1:	external #1
 *		0:	external #3
 */

#define	LEVEL0_MASK	\
	(INTST1_DMA|INTST1_SLOT1|INTST1_SLOT3|INTST1_EXT1|INTST1_EXT3)

level0_intr()
{
	register int stat;

	stat = *(volatile u_char *)INTST1 & LEVEL0_MASK;
	*(u_char *)INTCLR1 = stat;

	if (stat & INTST1_DMA)
		dma_intr();
	if (stat & INTST1_SLOT1)
		exec_hb_intr2();
#if NEN > 0
	if (stat & INTST1_SLOT3) {
		int s, t;

		s = splimp();
		t = lance_intr();
		(void) splx(s);
		if (t == 0)
			exec_hb_intr4();
	}
#endif
#if NLE > 0
	if (stat & INTST1_SLOT3) {
		int s;

		s = splimp();
		leintr(0);
		(void) splx(s);
	}
#endif
	if (stat & INTST1_EXT1)
		print_int_stat("EXT #1");
	if (stat & INTST1_EXT3)
		print_int_stat("EXT #3");
}

/*
 * news3400 - INT1 service routine.
 *
 * INTST0 bit	1:	centro fault
 *		0:	centro busy
 * INTST1 bit	7:	beep
 *		6:	scc
 *		5:	lance
 */

#define LEVEL1_MASK2	(INTST0_CFLT|INTST0_CBSY)
#define LEVEL1_MASK1	(INTST1_BEEP|INTST1_SCC|INTST1_LANCE)

level1_intr(pc)
	unsigned pc;
{
	register int stat;
	register u_int saved_inten1 = *(u_char *)INTEN1;

	*(u_char *)INTEN1 = 0;		/* disable intr: beep, lance, scc */

	stat = *(volatile u_char *)INTST1 & LEVEL1_MASK1;
	*(u_char *)INTCLR1 = stat;

	stat &= saved_inten1;

	if (stat & INTST1_BEEP) {
		*(volatile u_char *)INTCLR1 = INTCLR1_BEEP;
		print_int_stat("BEEP");
	}
	if (stat & INTST1_SCC) {
		scc_intr();
		if (saved_inten1 & *(u_char *)INTST1 & INTST1_SCC)
			scc_intr();
	}
#if NEN > 0
	if (stat & INTST1_LANCE)
		lance_intr();
#endif
#if NLE > 0
	if (stat & INTST1_LANCE)
		leintr(0);
#endif

	*(u_char *)INTEN1 = saved_inten1;

#if NLP > 0
	/*
	 * The PARK2 cannot find centro interrupt correctly.
	 * We must check it by reading the cause register of cpu
	 * while other interrupts are disabled.
	 */
	{
		register int causereg;
		int s = splhigh();

		causereg = get_causereg();
		(void) splx(s);

		if ((causereg & CAUSE_IP4) == 0)
			return;
	}
#endif

	stat = (int)(*(u_char *)INTST0) & LEVEL1_MASK2;
	*(u_char *)INTCLR0 = stat;

	if (stat & INTST0_CBSY)		/* centro busy */
#if NLP > 0
		lpxint(0);
#else
		printf("stray intr: CBSY\n");
#endif
}

/*
 * DMA interrupt service routine.
 */
dma_intr()
{
        register volatile u_char *gsp = (u_char *)DMAC_GSTAT;
        register u_int gstat = *gsp;
        register int mrqb, i;

	/*
	 * when DMA intrrupt occurs there remain some untransferred data.
	 * wait data transfer completion.
	 */
	mrqb = (gstat & (CH0_INT|CH1_INT|CH2_INT|CH3_INT)) << 1;
	if (gstat & mrqb) {
		/*
		 * SHOULD USE DELAY()
		 */
		for (i = 0; i < 50; i++)
			;
		if (*gsp & mrqb)
			printf("dma_intr: MRQ\n");
	}

	/* SCSI Dispatch */
	if (gstat & CH_INT(CH_SCSI))
		scintr();

#include "fd.h"
#if NFD > 0
        /* FDC Interrupt Dispatch */
	if (gstat & CH_INT(CH_FDC))
		fdc_intr(0);
#endif /* NFD > 0 */

#include "sb.h"
#if NSB > 0
        /* Audio Interface Dispatch */
	sbintr(0);
#endif /* NSB > 0 */

        /* Video I/F Dispatch */
	if (gstat & CH_INT(CH_VIDEO))
		;
}

/*
 * SCC vector interrupt service routine.
 */
scc_intr()
{
	int vec;
	extern int scc_xint(), scc_sint(), scc_rint(), scc_cint();
	static int (*func[])() = {
		scc_xint,
		scc_sint,
		scc_rint,
		scc_cint
	};

	vec = *(volatile u_char *)SCCVECT;
	(*func[(vec & SCC_INT_MASK) >> 1])(vec);
}

print_int_stat(msg)
	char *msg;
{
	int s0 = *(volatile u_char *)INTST0;
	int s1 = *(volatile u_char *)INTST1;

	if (msg)
		printf("%s: ", msg);
	else
		printf("intr: ");
	printf("INTST0=0x%x, INTST1=0x%x.\n", s0, s1);
}

traceback()
{
	u_int pc, sp;

	getpcsp(&pc, &sp);
	backtr(pc, sp);
}

#define EF_RA   	        92              /* r31: return address */
#define KERN_REG_SIZE		(18 * 4)
#define STAND_FRAME_SIZE	24
#define EF_SIZE			STAND_FRAME_SIZE + KERN_REG_SIZE + 12

extern u_int MachKernGenExceptionEnd[];
extern u_int end[];
#define	ENDOFTXT	(end + 1)

#define VALID_TEXT(pc)	\
	((u_int *)MACH_CODE_START <= (u_int *)MACH_UNCACHED_TO_CACHED(pc) && \
	 (u_int *)MACH_UNCACHED_TO_CACHED(pc) <= (u_int *)ENDOFTXT)

#define ExceptionHandler(x) \
	((u_int*)MachKernGenException < (u_int*)MACH_UNCACHED_TO_CACHED(x) && \
	 (u_int*)MACH_UNCACHED_TO_CACHED(x) < (u_int*)MachKernGenExceptionEnd)

backtr(pc, sp)
	register u_int *pc;
	register caddr_t sp;
{
	int fsize;
	u_int *getra();
	extern int _gp[];

	printf("start trace back pc=%x, sp=%x, pid=%d[%s]\n",
		pc, sp, curproc->p_pid, curproc->p_comm);

	while (VALID_TEXT(pc)) {
		if (sp >= (caddr_t)KERNELSTACK || sp < (caddr_t)UADDR) {
			printf("stack exhausted (sp=0x%x)\n", sp);
			break;
		}
		if (ExceptionHandler(pc)) {
			pc = (u_int *)(*((u_int *)&sp[EF_RA]));
			sp += EF_SIZE;
			printf("trapped from pc=%x, sp=%x\n", pc, sp);
		} else {
			pc = getra(pc, sp, &fsize);
			sp += fsize;
			printf("called from pc=%x, sp=%x\n", pc, sp);
		}
	}
	printf("trace back END. pid=%d[%s]\n", curproc->p_pid, curproc->p_comm);
}

#define	NPCSTOCK	128

u_int *
getra(pc, sp, fsize)
	register int *pc;
	register caddr_t sp;
	int *fsize;
{
	u_int regs[32];
	int *opcs[NPCSTOCK];
	register int i, nbpc = 0;
	int printed = 0;
	InstFmt I;

	*fsize = 0;
	for (i = 0; i < 32; i++) regs[i] = 0;
	for (; (u_int*)MACH_UNCACHED_TO_CACHED(pc) < (u_int*)ENDOFTXT; pc++) {
		I.word = *pc;
		switch (I.IType.op) {

		case OP_ADDIU:
			/* sp += fsize */
			if (I.IType.rs == SP && I.IType.rt == SP)
				*fsize = (u_short)I.IType.imm;
			break;

		case OP_LW:
			if (I.IType.rs != SP)
				break;
			regs[I.IType.rt] = *(u_int *)&sp[(short)I.IType.imm];
			break;

		case OP_BEQ:
			if (I.IType.rs != ZERO || I.IType.rt != ZERO)
				break;
			for (i = 0; i < nbpc; i++)
				if (pc == opcs[i]) {
					/*
					 * Brach constructs infinite loop.
					 */
					if (!printed) {
						printf("branch loop\n");
						printed = 1;
					}
					break;
				}
			if (i == nbpc) {
				opcs[nbpc] = pc;
				nbpc = imin(nbpc + 1, NPCSTOCK);
				pc = pc + (short)I.IType.imm;
			}
			break;

		default:
			break;
		}

		I.word = *(pc - 1);
		if (I.RType.op == OP_SPECIAL && I.RType.func == OP_JR)
			return ((int *)regs[I.RType.rs]);
	}
	printf("pc run out of TEXT\n");
	return (0);
}