code.c

pebble

		*p++ = SD(T2, S_T2*8, base);
		*p++ = SD(T3, S_T3*8, base);
		*p++ = SD(T4, S_T4*8, base);
		*p++ = SD(T5, S_T5*8, base);
		*p++ = SD(T6, S_T6*8, base);
		*p++ = SD(T7, S_T7*8, base);
		*p++ = SD(T8, S_T8*8, base);
		*p++ = SD(T9, S_T9*8, base);
		*p++ = SD(V0, S_LO*8, base);
		*p++ = SD(V1, S_HI*8, base);
	}

	if (PORTAL_LOG)
		p = gen_write_log_portal(p, A0);

	p = gen_li(p, T0,
		(int)lookup_portal_rtn(stack_type, stack_save,
			window_reg, window_type));
	*p++ = SD(T0, S_PORTAL_RTN*8, base);
	while ((p - last_mtc) <= MTC0_TO_TLB_OP)
		*p++ = NOP;

	*p++ = TLBR;
	*p++ = LW(T1, THR_STACK_DESC*4, treg);
	*p++ = LW(T8, THR_STACK_Q*4, treg);
	*p++ = LW(T6, THR_PORTAL_TBL*4, treg);
	*p++ = LD(T7, THR_PTE*4, treg);
	*p++ = DMFC0(T4, C0_EPC);
	*p++ = MFC0(T5, C0_STATUS);
	*p++ = DMFC0(T2, C0_TLBHI);
	*p++ = DMFC0(T3, C0_TLBLO0);
	if (stack_type != STACK_EXCEPTION)
		*p++ = DADDIU(T4, T4, 4);
	*p++ = SD(T4, S_EPC*8, base);
	*p++ = SW(T5, S_STATUS*8, base);
	if (asid_reg != 0)
		*p++ = ANDI(asid_reg, T2, TLBHI_PIDMASK);
	*p++ = SD(T2, S_TLBHI*8, base);
	*p++ = SD(T3, S_TLBLO0*8, base);
	*p++ = SW(T1, S_STACK_DESC*8, base);
	*p++ = SW(T8, S_STACK_Q*8, base);
	*p++ = SW(T6, S_PORTAL_TBL*8, base);
	*p++ = SD(T7, S_PTE*8, base);

	/* a memory window; could be either 'w', '>' or '=' type */
	if (window_reg != 0) {
		/* check against heap address bounds */
		/* leave the high order byte only */
		*p++ = SRL(T3, window_reg, 24);
		*p++ = SLTIU(T4, T3, (HEAP_START >> 24) & 0xff);
		*p++ = BEQ(T4, ZERO, 3);
		/* always executed in the delay slot */
		*p++ = SLTIU(T5, T3, (HEAP_END >> 24) & 0xff);
		label = p;
		p = gen_j(p, bad_win_call);
		*p++ = NOP;
		/* continue checking of heap address */
		*p++ = BEQ(T5, ZERO, label- (p+1));
		/* compute level 1 page table index in the branch delay slot */
		*p++ = SRL(T3, window_reg, (PAGESHIFT + PTABLE1_SHIFT));
		*p++ = SLL(T3, T3, 3);		/* zero three low-order bits */
		p = gen_lw(p, T4,
		     ((ulong)ptable_level1)+(PTABLE1_L2_ENTRIES*WORD_SIZE), T3);
		p = gen_lw(p, T5,
		     ((ulong)ptable_level1)+(PTABLE1_L2_TABLE*WORD_SIZE), T3);
		/* access level1 page table entry */
		*p++ = SRL(T3, window_reg, (PAGESHIFT-PTESHIFT));
		/* mask only level2 index bits */
		*p++ = ANDI(T3, T3, PAGEMASK - (PTESIZE-1));
		*p++ = SLTU(T4, T3, T4);
		*p++ = BEQ(T4, ZERO, label - (p + 1));
		/* access level2 page table */
		*p++ = ADD(T3, T3, T5);
		*p++ = LW(T4, PTE_ACCESS*4, T3);
		*p++ = ANDI(T2, T2, TLBHI_PIDMASK);
		p = gen_li(p, T7, TLBHI_VPN2MASK);
		*p++ = BEQ(T4, ZERO, label - (p + 1));
		*p++ = AND(T7, T7, window_reg);

		switch (window_type) {
		case 'w':
		case '=':
			/* a regular window or a shared window */
			*p++ = ORI(T7, T7, new_asid & TLBHI_PIDMASK);
			*p++ = SD(T7, S_WIN_VADDR*8, base);
			/* check if caller had access rights to page */
			*p++ = ADD(T5, T4, T2);
			*p++ = LB(T6, 0, T5);
			*p++ = ADDI(T7, T4, new_asid & TLBHI_PIDMASK);
			*p++ = BEQ(T6, ZERO, label - (p + 1));
			/* increase the callee's access count */
			*p++ = LB(T8, 0, T7);
			*p++ = ADDIU(T8, T8, 1);
			/* did the callee access count overflow? */
			*p++ = ANDI(T8, T8, 0xff);
			*p++ = BEQ(T8, ZERO, label - (p + 1));
			*p++ = SD(T7, S_WIN_COUNT*8, base);
			*p++ = SB(T8, 0, T7);
			break;

		case '>':
			/* give window to called domain */
			/* check if caller had access rights to page */
			*p++ = ADD(T5, T4, T2);
			*p++ = LB(T6, 0, T5);
			*p++ = LB(T8, new_asid & TLBHI_PIDMASK, T4);
			*p++ = BEQ(T6, ZERO, label - (p + 1));
			/* branch delay slot here */
			*p++ = ADDIU(T8, T8, 1);
			*p++ = ANDI(T8, T8, 0xff);
			/* did the callee access count overflow? */
			*p++ = BEQ(T8, ZERO, label - (p + 1));
			/* branch delay slot here */
			*p++ = ADDIU(T6, T6, -1);

			/* set the caller and callee counters */
			*p++ = SB(T6, 0, T5);
			*p++ = SB(T8, new_asid & TLBHI_PIDMASK, T4);

			start = p;
			label9 = label10 = p;
			/* must generate code twice to get correct branch */
			/* offsets */
			for (i = 0; i < 2; i++) {
				p = start;

				/* change page ownership if calling domain */
				/* can not access the page any longer */
				*p++ = BNE(T6, ZERO, label10 - (p + 1));
				/* branch delay slot */
				/* T7 contains the window's starting address */
				*p++ = ORI(T8, T7, new_asid & TLBHI_PIDMASK);
				*p++ = LB(T6, NASID, T5);

				/* change page owner domain to "new_asid" */
				*p++ = SW(T8, PTE_HI*4, T3);

				/* was this page loaded to the TLB? */
				*p++ = BNE(T6, ZERO, label10 - (p + 1));
				/* reset caller's TLB load flag */
				*p++ = SB(T1, NASID, T5);

				/* yes, it was loaded to the TLB */
				/* remove TLB entry from caller */
				/* T2 contains the caller's ASID */
				*p++ = OR(T7, T7, T2);
				p = gen_tlb_probe_erase(p, T7, label9);
				label9 = p;
				/* ASID will be loaded by gen_restrpc */
				/* fix C0_INDEX that was changed by tlb_probe */
				*p++ = MTC0(ZERO, C0_INDEX);
				label10 = p;
			}
			break;

		default:
			panic(
			"portal manager: gen_savereg: invalid window type");
		}
	}

	if (stack_save != SAVE_MINIMAL) {
		*p++ = SD(S0, S_S0*8, base);
		*p++ = SD(S1, S_S1*8, base);
		*p++ = SD(S2, S_S2*8, base);
		*p++ = SD(S3, S_S3*8, base);
		*p++ = SD(S4, S_S4*8, base);
		*p++ = SD(S5, S_S5*8, base);
		*p++ = SD(S6, S_S6*8, base);
		*p++ = SD(S7, S_S7*8, base);
		*p++ = SD(S8, S_S8*8, base);
	}
	*p++ = SD(GP, S_GP*8, base);
	*p++ = SD(SP, S_SP*8, base);
	*p++ = SD(RA, S_RA*8, base);


	return p;
}


/*
 * Note: gen_savereg must not change T9!
 * gen_pushreg can also leave the most recent bad virtual address in
 * the register specified by bad_vaddr_reg.
 */
static int *
gen_pushreg(int *p, int treg, PortalStack stack_type, PortalSave stack_save,
	uint asid_reg, uint window_reg, uchar window_type,
	uint bad_vaddr_reg, uint new_asid, int *last_mtc)
{
	*p++ = LW(K1, THR_CALL_SP*4, treg);
	if (stack_type != STACK_EXCEPTION)
		*p++ = LW(T9, THR_CALL_LO*4, treg);
	*p++ = DADDIU(K1, K1, -8*S_REGS);
	*p++ = SW(K1, THR_CALL_SP*4, treg);
	p = gen_savereg(p, treg, K1, stack_type, stack_save, asid_reg,
		window_reg, window_type, new_asid, last_mtc);
	if (bad_vaddr_reg != 0)
		*p++ = MFC0(bad_vaddr_reg, C0_BADVADDR);
	if (stack_type != STACK_EXCEPTION) {
		*p++ = SLTU(T2, K1, T9);
		*p++ = BEQ(T2, ZERO, 3);
		*p++ = NOP;
		p = gen_j(p, thr_stack_overflow);
		*p++ = MOVE(A0, treg);
	}

	return p;
}


/*
 *	assumption: C0_INDEX is zero on entry.
 *
 *	If the stack type is exception, we set register A2 to
 * 	!= 0 if we indeed switched to exception stack,
 *	Otherwise, it is zero, which indicates a nested exception
 *	while serving a previous exception.
 */
static int *
gen_restrpc(int *p, int treg, int asid, int status, int gp, int portal_tbl,
	int (*entry)(),
	int *epilog, vlong domain_ptable, PortalStack stack_type,
	StackQ *stack_q, int *last_mtc)
{
	int *old_p;
	int *last_tlbr = NULL;
	Ptable1Union ptable_convert;

	DIAG(PORTAL_DIAG, ("gen_restrpc asid=%d status=%08x gp=%08x portal_tbl=%08x entry=%08x epilog=%08x\n",
		asid, status, gp, portal_tbl, (uint)entry, (uint)epilog));
	DIAG(PORTAL_DIAG, ("stack_type=%d stack_q=%p\n", stack_type, stack_q));

	ptable_convert.x = domain_ptable;
	DIAG(PORTAL_DIAG, ("private_ptable.pte=%p entries=%d\n",
		(PTE *)ptable_convert.s.v[PTABLE1_L2_TABLE],
		(int)ptable_convert.s.v[PTABLE1_L2_ENTRIES]));

	if (stack_type == STACK_SHARED) {
		while ((p - last_mtc) <= MTC0_TO_TLB_OP)
			*p++ = NOP;
		last_tlbr = p;
		*p++ = TLBR;
	}

	switch (stack_type) {
	case STACK_EXCEPTION:
		p = gen_li(p, T2, (excpt_stack_tlbhi & ~TLBHI_PIDMASK) | asid);
		p = gen_li(p, T7, excpt_stack_tlblo0);
		*p++ = SW(ZERO, THR_STACK_Q*4, treg);

		/* switch stack descriptors */
		/* interrupt stack descriptor is zero (until we know better) */
		*p++ = SW(ZERO, THR_STACK_DESC*4, treg);

		/* switch stacks only if not using exception stack already */
		p = gen_li(p, T3, excpt_stack_sp);
		*p++ = XOR(A2, T3, SP);
		*p++ = SRL(A2, A2, PAGESHIFT);
		/* A2 is != 0 if we switched to exception stack */
		/* A2 is zero if this is a nested exception, */
		/* which means that we are using the same stack */
		/* note: we pass this condition in the 3rd parameter to the */
		/* interrupt handler */
		*p++ = MOVN(SP, T3, A2);
		*p++ = DMFC0(A3, C0_BADVADDR);
		break;

	case STACK_QUEUE:
		p = gen_li(p, T1, (int)stack_q);
		*p++ = LW(T4, STACK_Q_HEAD*4, T1);
		*p++ = SW(T1, THR_STACK_Q*4, treg);
		*p++ = TEQI(T4, 0);	/* trap if queue is empty */
		*p++ = LW(T3, STACK_DESC_NEXT*4, T4);
		*p++ = LW(T2, STACK_DESC_TLBHI*4, T4);
		*p++ = LW(T7, STACK_DESC_TLBLO0*4, T4);
		*p++ = SW(T3, STACK_Q_HEAD*4, T1);
		*p++ = LW(SP, STACK_DESC_SP*4, T4);
		*p++ = SW(T4, THR_STACK_DESC*4, treg);
		p = gen_li(p, T6, ~TLBHI_PIDMASK);
		break;

	case STACK_SHARED:
		*p++ = SW(ZERO, THR_STACK_Q*4, treg);
		p = gen_li(p, T6, ~TLBHI_PIDMASK);
		/* must wait some cycles between TLBR and DMFC0 (on RM7000) */
		while ((p - last_tlbr) <= TLBR_TO_MFC0)
			*p++ = NOP;
		*p++ = DMFC0(T2, C0_TLBHI);
		break;

	default:
		panic("gen_restrpc: invalid stack_type value");
	}

	/* generate new ASID and keep the same stack mapping */
	if (stack_type == STACK_QUEUE || stack_type == STACK_SHARED) {
		*p++ = AND(T2, T2, T6);
		*p++ = ORI(T2, T2, asid);
	}
	*p++ = DMTC0(T2, C0_TLBHI);
	p = gen_li(p, T3, status);
	*p++ = MTC0(T3, C0_STATUS);
	p = gen_li(p, T4, (int)entry);
	*p++ = DMTC0(T4, C0_EPC);
	if (stack_type == STACK_EXCEPTION || stack_type == STACK_QUEUE) {
		*p++ = DMTC0(T7, C0_TLBLO0);
		*p++ = DADDIU(T7, T7, TLBLO0toLO1);
		*p++ = DMTC0(T7, C0_TLBLO1);
		*p++ = MTC0(ZERO, C0_PAGEMASK);
	}

	p = gen_li(p, T5, portal_tbl);
	p = gen_li(p, T7, (int)ptable_convert.s.v[PTABLE1_L2_TABLE]);
	p = gen_li(p, T8, (int)ptable_convert.s.v[PTABLE1_L2_ENTRIES]);
	*p++ = SW(T5, THR_PORTAL_TBL*4, treg);
	*p++ = SW(T7, THR_PTE*4, treg);
	*p++ = SW(T8, THR_L2_ENTRIES*4, treg);
	*p++ = TLBWI;

	old_p = p;
	p = gen_li(p, GP, gp);
	/* Note: we do not clear FP (S8) here */
	/* If we do, we must save/restore it also in SAVE_MINIMAL */
	/* p = gen_li(p, S8, 0); */
	p = gen_li(p, RA, (int)epilog);

	/* TLBWI must be followed by at least TLBW_TO_BRANCH integer  */
	/* instructions */
	while ((p - old_p) < TLBW_TO_BRANCH)
		*p++ = NOP;

	return p;
}


/*
 * The null portal transfers control to the destination domain without
 * saving the context of the calling domain.
 *
 * The null portal should be traversed only in two cases:
 * a.	When the initialization domain relinquishes control to the scheduler.
 *	The scheduler NEVER returns to the initialization domain!
 * b.	When the interrupt dispatcher finishes its work.
 *	In this case we want the scheduler to pick the next thread to run.
 *	If the scheduler picks the calling thread, then we should return
 *	to the domain that was interrupted, and not to the interrupt handler
 *	domain.
 *
 * Return the virtual address of the portal code.
 */
Portal
gen_null_portal(vlong *save_p, int (*entry)(), int *portal_len)
{
	int *p, *start, *phys_start;
	int len;

	DIAG(PORTAL_DIAG, ("gen_null_portal: save_p=%08x entry=%08x\n", (uint)save_p, (uint)entry));

	if (portal_code == NULL)
		portal_code = (int *)malloc(MAX_PORTAL_LEN);
	p = portal_code;

	if (PORTAL_LOG)
		p = gen_write_log_portal(p, A0);

	*p++ = MTC0(ZERO, C0_INDEX);
	p = gen_restrpc(p, K0, (int)save_p[S_TLBHI] & TLBHI_PIDMASK,
		(int)save_p[S_STATUS],
		(int)save_p[S_GP], (int)save_p[S_PORTAL_TBL], entry, 0,
		save_p[S_PTE], STACK_SHARED, NULL, p-1);
	*p++ = MOVE(A0, A1);
	*p++ = MOVE(A1, A2);
	*p++ = MOVE(A2, A3);

	if (PORTAL_LOG) {
		*p++ = LW(K1, THR_CALL_SP*4, K0);
		p = gen_li(p, T5, (int)save_p[S_TLBHI] & TLBHI_PIDMASK);
		p = gen_write_log(p, K0, K1, T5, A0, A1);
	}
	*p++ = ERET;

	/* pad resulting code to 8 byte boundary */
	while ((uint)p & 07)
		*p++ = NOP;

	len = p - portal_code;
	if (len*sizeof(int) > MAX_PORTAL_LEN)
		panic("portal code overflow");

	start = (int *)aligned_malloc(len*sizeof(int));
	memcpy((char *)start, (char *)portal_code, len*sizeof(int));

	phys_start = (int *)PA_TO_KVA0((ulong)virt2phys(start));
	clean_cache(phys_start, len*sizeof(int));
	clean_cache(start, len*sizeof(int));
	*portal_len = len;

	return (Portal)start;
}

/*
 * Generate a portal and return the virtual address of the portal code.
 */
Portal
gen_portal(vlong *save_p, int (*entry)(), int *epilog,
	PortalStack stack_type, PortalSave stack_save, StackQ *stack_q,
	uint thread_reg, uint const_reg, int c, uint asid_reg, uint window_reg,
	uchar window_type, uint bad_vaddr_reg,
	int *portal_len, int *const_offset, int *c_reg)
{
	int *p, *start, *phys_start;
	int len;
	int *last_mtc;

	/* the portal code is created in a single large buffer, */
	/* "portal_code", which is allocated by the first call to gen_portal */
	if (portal_code == NULL)
		portal_code = (int *)malloc(MAX_PORTAL_LEN);
	p = portal_code;

	last_mtc = p;

	*p++ = MTC0(ZERO, C0_INDEX);
	p = gen_pushreg(p, K0, stack_type, stack_save, asid_reg, window_reg,
		window_type,
		bad_vaddr_reg, (int)save_p[S_TLBHI] & TLBHI_PIDMASK, last_mtc);
	p = gen_restrpc(p, K0, (int)save_p[S_TLBHI] & TLBHI_PIDMASK,
		(int)save_p[S_STATUS], (int)save_p[S_GP],
		(int)save_p[S_PORTAL_TBL], entry, epilog, save_p[S_PTE],
		stack_type, stack_q, last_mtc);

	/* increase stack pointer for parameters #4 and #5 */
	if (thread_reg > A3 || const_reg > A3) {
		if ((thread_reg == (A3+2) || const_reg == (A3+2)) &&
		     PARAM_SIZE == 8)
			/* new calling sequence: two synthetic parameters */
			/* each parameter is passed in 8 bytes */
			*p++ = ADDIU(SP, SP, -16);
		else
			/* old calling sequence: every parameter is passed */
			/* in 4 bytes and the stack pointer is double-word */
			/* aligned. */
			/* Thus both one and two synthetic parameters */
			/* increase the stack by 8 bytes */
			*p++ = ADDIU(SP, SP, -8);
	}

	if (thread_reg != 0) {
		if (thread_reg <= A3)
			*p++ = MOVE(thread_reg, K0);
		else
			*p++ = SW(K0, SAVEREGS + PARAM_SIZE*(thread_reg-(A3+1)),
				SP);
	}

	if (const_reg != 0) {
		*const_offset = p - portal_code;
		if (const_reg <= A3) {
			p = gen_li_2inst(p, const_reg, c);
			*c_reg = const_reg;
		} else	{
			p = gen_li_2inst(p, T0, c);
			*p++ = SW(T0, SAVEREGS + PARAM_SIZE*(const_reg-(A3+1)),
				SP);
			*c_reg = T0;
		}
	} else {
		*const_offset = 0;
		*c_reg = ZERO;
	}

	if (PORTAL_LOG) {
		p = gen_li(p, T5, (int)save_p[S_TLBHI] & TLBHI_PIDMASK);
		*p++ = LW(K1, THR_CALL_SP*4, K0);
		p = gen_write_log(p, K0, K1, T5, A0, A1);
	}

	*p++ = ERET;

	/* pad resulting code to 8 byte boundary */
	while ((uint)p & 07)
		*p++ = NOP;

	len = p - portal_code;
	if (len*sizeof(int) > MAX_PORTAL_LEN)
		panic("portal code overflow");

	start = (int *)aligned_malloc(len*sizeof(int));
	memcpy((char *)start, (char *)portal_code, len*sizeof(int));

	phys_start = (int *)PA_TO_KVA0((ulong)virt2phys(start));
	clean_cache(phys_start, len*sizeof(int));
	clean_cache(start, len*sizeof(int));
	*portal_len = len;

	return (Portal)start;
}