sched.c

pebble

			put_log((int)s, s->count);
		}
		enqueue(&readyq[pri], p, asid, pri, 0);
	}

	/* the following condition will be eliminated by the compiler */
	/* if INTR_INSIDE is zero. */
	if (INTR_INSIDE && is_intr_stack() && caller_asid >= 0) {
		/* we are called directly or indirectly from the interrupts */
		/* dispatcher, which is included in the nucleus. */
		/* we may be called by an interrupt handler, which was */
		/* called directly by the interrupts dispatcher */
		/* bypass sys_sched and return directly to the caller */
		/* must not allow sys_sched() to block thread */
		run(t, 0);
	}

	/* return to caller only if it has the higher priority of all ready */
	/* threads */
	if (caller_asid < 0)
		caller_asid = -caller_asid;
	sys_sched(t, caller_asid, 0);
}


int
sys_sem_create(Thread *t, int val, int caller_asid)
{
	Semaphore *s;
	int i;

	DIAG(SCHED_DIAG, ("sys_sem_create called for task %p val=%d\n", t, val));

	if ((s = (Semaphore *)malloc(sizeof(Semaphore))) == NULL)
		error(SEM_OVERFLOW);

	memset(s, 0, sizeof(Semaphore));
	s->next = sem_list;
	sem_list = s;
	s->asid = caller_asid;
	s->count = val;

	/* create semaphore portals in caller's table */
	if ((i = portal_alloc(2, 1)) < 0)
		error(PORTAL_CREATE);
#ifdef	Cobalt
	if (portal_create(i, "sfaiict", (int)s, (Func) sys_sem_wait, 1) < 0)
#else
	/* double-word parameters are passed in a register pair */
	if (portal_create(i, "sfaiiict", (int)s, (Func) sys_sem_wait, 1) < 0)
#endif
		error(PORTAL_CREATE);
	if (portal_create(i+1, "sptaci", (int)s, (Func) sys_sem_post, 1) < 0) {
		/* must delete portal from caller, not self */
		portal_delete(i, 1, 1);
		error(PORTAL_CREATE);
	}
	s->portal_no = i;

	DIAG(SCHED_DIAG, ("sys_sem_create before return %d sem=%p\n", i, s));

	return PORT2SEM(i);
}


/*
 * This routine is called only from sem_nwait() via portal traversal.
 * This portal should not be called by anyone outside the scheduler domain.
 */
static int
sys_sem_nwait(Thread *t, int asid, Semaphore *s, int n)
{
	/* the following condition will be eliminated by the compiler */
	/* if INTR_INSIDE is zero. */
	if (INTR_INSIDE && is_intr_stack())
		panic("interrupt handler thread %p blocked on sem_nwait on %p",
			t, s);

	if (SCHED_LOG)
		put_log(SCHED_LOG_SEM_NWAIT, (int)t);

        enqueue(&s->q, t, asid, asid2pri[asid], n);
        sys_sched(0, 0, 0);

        /* not reached */
        return (0);
}

/*
 * Assumption: this code is executed with interrupts disabled
 * on a single CPU.
 */
static int
sem_nwait(Semaphore *s, int n)
{
	int m;

	if (s->count <= 0) {
		/* bad luck -- must wait */
		m = call_portal(SYS_SEM_NWAIT, 0, s, n);
		/* we wakeup here after somebody did a sem_npost and  */
		/* decreased the s->count for us. */
	} else	{
		m = min(n, s->count);
		s->count -= m;
	}

	return m;
}

/*
 * Post value n to semaphore "s".
 * Do not do a scheduling decision here, even if waking up
 * a high priority process.
 */
static void
sem_npost(Semaphore *s, int n)
{
	Thread *p;
	int val, asid, m, pri;

	s->count += n;
	while (s->count > 0 && (p = dequeue(&s->q, &asid, &pri, &val)) != NULL){
		m = min(s->count, val);
		s->count -= m;
		enqueue(&readyq[pri], p, asid, pri, m);
	}
}

/*
 * must do a scheduling decision before returning to caller,
 * since sem_npost does not do it.
 */
int
sys_pipe_read(Thread *t, void *buf, int len, int caller_asid, Pipe *p)
{
	int n, m;

	if (len < 0)
		error(BUF_LEN);
	if (len == 0)
		return 0;
        param_check(buf, len, 0);

	sem_nwait(&p->read_lock, 1);
	n = sem_nwait(&p->full, len);

	/* copy n bytes from FIFO buffer to user buffer */
	m = min(n, PIPE_SIZE - p->tail);
	memcpy(buf, p->buf + p->tail, m);
	if (m < n)
		memcpy(buf + m, p->buf, n - m);
	p->tail += n;
	if (p->tail >= PIPE_SIZE)
		p->tail -= PIPE_SIZE;

	sem_npost(&p->empty, n);
	sem_npost(&p->read_lock, 1);

#if	0
	/* run higher priority threads, if any */
	/* if there are no higher priority tasks, return to caller. */
	sys_sched(t, caller_asid, n);
#endif

	/* never returns from here */
	return n;
}


int
sys_pipe_write(Thread *t, void *buf, int len, int caller_asid, Pipe *p)
{
	int n, m, written;

	if (len < 0)
		error(BUF_LEN);
	if (len == 0)
		return 0;
        param_check(buf, len, 0);

	sem_nwait(&p->write_lock, 1);
	for (written = 0; written < len; written += n) {
		n = sem_nwait(&p->empty, len - written);

		/* copy n bytes from user buffer to FIFO buffer */
		m = min(n, PIPE_SIZE - p->head);
		memcpy(p->buf + p->head, buf + written, m);
		if (m < n)
			memcpy(p->buf, buf + written + m, n - m);

		p->head += n;
		if (p->head >= PIPE_SIZE)
			p->head -= PIPE_SIZE;

		sem_npost(&p->full, n);
	}

	sem_npost(&p->write_lock, 1);

#if	0
	/* run higher priority threads, if any */
	/* if there are no higher priority tasks, return to caller. */
	sys_sched(t, caller_asid, len);
#endif

	/* never returns from here */
	return len;
}

int
sys_pipe_create(Thread *t)
{
	Pipe *p;
	int i;
	static int pipe_offset = 0;

	DIAG(SCHED_DIAG, ("sys_pipe_create called for task %p\n", t));

	if ((p = (Pipe *)malloc(sizeof(Pipe))) == NULL)
		error(MEM_ALLOC);

	memset(p, 0, sizeof(Pipe));
	if ((p->buf = (char *)malloc(PIPE_SIZE)) == NULL) {
		free(p);
		error(MEM_ALLOC);
	}

	p->read_lock.count = 1;
	p->write_lock.count = 1;
	p->empty.count = PIPE_SIZE;
	p->full.count = 0;

	/* stagger pipe head and tail pointers to reduce cache contention */
	p->head = p->tail = pipe_offset;
	pipe_offset += PIPE_OFFSET;
	if (pipe_offset >= PIPE_SIZE)
		pipe_offset = 0;

	/* create pipe portals in caller's table */
	if ((i = portal_alloc(2, 1)) < 0)
		error(PORTAL_CREATE);
	if (portal_create(i, "sptiiac", (int)p, (Func) sys_pipe_read, 1) < 0)
		error(PORTAL_CREATE);
	if (portal_create(i+1, "sptiiac", (int)p, (Func) sys_pipe_write, 1) < 0) {
		/* must delete portal from caller, not self */
		portal_delete(i, 1, 1);
		error(PORTAL_CREATE);
	}

	DIAG(SCHED_DIAG, ("sys_pipe_create before return %d id=%d pipe=%p\n", PORT2FD(i), i, p));

	return PORT2FD(i);
}


int
sys_vq_wait(Thread *t, int asid)
{
	ValQueue *vq;
	Thread *new_p;
	int new_asid, new_pri, new_val;

	/* the following condition will be eliminated by the compiler */
	/* if INTR_INSIDE is zero. */
	if (INTR_INSIDE && is_intr_stack())
		panic("interrupt handler thread %p called vq_wait", t);

	DIAG(SCHED_DIAG, ("sys_vq_wait: thread %p asid=%d\n", t, asid));
	vq = &val_q[asid];
	if ((new_p = dequeue(&vq->free_q, &new_asid, &new_pri, &new_val))!=NULL)
		return new_val;

	/* Bad luck: no value is waiting for us. Must wait here. */
	enqueue(&vq->wait_q, t, asid, asid2pri[asid], 0);
	sys_sched(0, 0, 0);

	/* not reached */
	return (0);
}


int
sys_vq_post(Thread *t, int caller_asid, int val)
{
	ValQueue *vq;
	Thread *new_p;
	int new_asid, new_val, new_pri;

	DIAG(SCHED_DIAG, ("sys_vq_post: thread %p asid=%d val=%d\n", t, caller_asid, val));
	vq = &val_q[caller_asid];
	if ((new_p = dequeue(&vq->wait_q, &new_asid, &new_pri, &new_val))
            != NULL) {
		enqueue(&readyq[new_pri], new_p, new_asid, new_pri, val);
		/* always return to caller, since the priority of the awakened */
		/* thread will be the same as the caller thread, since the first */
		/* is entering the same domain that the latter is exiting. */
	} else	{
		enqueue(&vq->free_q, t, 0, 0, val);	/* t value is unused */
	}
	
	return 0;
}

/*
 * Called by interrupt handler.
 * This routine MUST NOT call other domains before disabling
 * timer interrupt!
 *
 * "orig_caller_asid" is non-zero if this is the last call from the interrupt
 * dispatcher.
 */
void
sys_timer_handler(Thread *t, int orig_caller_asid)
{
	Time now;
	Elem *ep, *nep;
	Queue *q;

	if (SCHED_LOG)
		put_log(SCHED_LOG_TIMER, orig_caller_asid);

	now = hrtime();
	for (ep = wakeup_q; ep != NULL && ep->wakeup < (now + fuzz); ep = nep) {
		if ((q = (Queue *)ep->sem_q) != NULL) {
			/* dequeue from semaphore queue */
			if (ep->sem_last == NULL) {
				assert(q->head == ep);
				q->head = ep->sem_next;
			} else
				ep->sem_last->sem_next = ep->sem_next;
			if (ep->sem_next == NULL) {
				assert(q->tail == ep);
				q->tail = ep->sem_last;
			} else
				ep->sem_next->sem_last = ep->sem_last;
		}

		if (SCHED_LOG) {
			put_log(SCHED_LOG_READY, (int)ep->thread);
			put_log(SCHED_LOG_RUN_PRIORITY, ep->pri);
		}

		nep = ep->time_next;
		enqueue(&readyq[ep->pri], ep->thread, ep->asid, ep->pri,
			(ep->sem_q == NULL) ? 0 : -1);
		free_elem(ep);
		/* must get current time, since enqueue() might have taken */
		/* a long time */
		now = hrtime();
	}

	if ((wakeup_q = ep) != NULL) {
		next_wakeup = wakeup_q->wakeup;
		ep->time_last = NULL;
	} else
		/* we're using a 64-bit h/r timer */
		next_wakeup = hrtime() + MAX_TIME;

	/* reset timer interrupt in any case */
	/* we are sure that next_wakeup is at later than current time */
	/* by at least fuzz (actually slightly less, but it is still safe) */
	set_timer(next_wakeup);

	/* if we are called from the interrupt dispatcher */
	if (orig_caller_asid != 0) {
		pop_portal_call(t);
		sys_sched(t, orig_caller_asid, 0);
	}
}


/*
 * sleep until the specified high-resolution time in the future.
 *
 * Assumption:
 * This routine is executed with interrupts disabled.
 * The fuzz constant is large enough to ensure that we never
 * set the timer interrupt to a value which is earlier than the
 * time of calling set_timer(). If we set the timer to a value less
 * than the current time (unsigned comparison),
 * we will have to wait until the high-resolution timer
 * wraps around, which takes almost forever.
 */
int
sys_hrsleep(Thread *t, int asid, Time wakeup_time)
{
	Elem *ep;
	
	/* the following condition will be eliminated by the compiler */
	/* if INTR_INSIDE is zero. */
	if (INTR_INSIDE && is_intr_stack())
		panic("interrupt handler thread %p called hrsleep", t);

	if (asid == ASID_NUCLEUS)
		panic("sched: nucleus domain called hrsleep");

	if (SCHED_LOG)
		put_log(SCHED_LOG_HRSLEEP, (int)t);

	/* do seperate checks here to accomodate unsigned time values */
	if (wakeup_time < hrtime())
		error(HRSLEEP_AMOUNT);

	ep = new_elem();
	ep->thread = t;
	ep->asid = asid;
	ep->sem_q = ep->sem_next = ep->sem_last = NULL;

	enqueue_timer(ep, wakeup_time);

	sys_sched(0, 0, 0);
	/* not reached */
	return (0);
}

int
sys_set_priority(Thread *t, int asid, int pri, int caller_asid)
{
	int old_pri;
	Elem *ep, *nep;
	Queue q;

	if (asid < 0 || asid >= NASID || pri < 0 || pri >= NPRIORITY)
		error(PRIORITY_ERROR);
	DIAG(SCHED_DIAG, ("set_priority(%d, %d)\n", asid, pri));

	if (SCHED_LOG)
		put_log(SCHED_LOG_SET_PRIORITY, (int)t);

	if (asid == ASID_NUCLEUS)
		panic("set_priority: attempting to change nucleus priority");

	old_pri = asid2pri[asid];
	asid2pri[asid] = pri;

	if (old_pri == pri)
		return old_pri;

	/* re-assign waiting threads from the old priority queue */
	/* we are certain that ready threads do not wait on the timer queue */
	q = readyq[old_pri];
	readyq[old_pri].head = readyq[old_pri].tail = NULL;
	for (ep = q.head; ep != NULL; ep = nep) {
		nep = ep->sem_next;
		/* change priority of threads belonging to specified domain */
		if (ep->asid == asid) 
			ep->pri = pri;
		if (ep->asid == ASID_NUCLEUS)
			panic("set_priority thread=%p asid=%d pri=%d\n",
				ep->thread, ep->asid, ep->pri);
		enqueue(&readyq[ep->pri], ep->thread, ep->asid, ep->pri, ep->val);
                free_elem(ep);
	}

	if (SCHED_DIAG)
		sys_dump_sched(t);

	/* the following condition will be eliminated by the compiler */
	/* if INTR_INSIDE is zero. */
	if (INTR_INSIDE && is_intr_stack())
		return old_pri;

	if (caller_asid == asid) {
		/* yield to higher priority threads */
		if (SCHED_LOG) {
			put_log(SCHED_LOG_READY, (int)t);
			put_log(SCHED_LOG_RUN_PRIORITY, pri);
		}

		enqueue(&readyq[pri], t, asid, pri, old_pri);
		sys_sched(0, 0, old_pri);
	}

	return old_pri;
}


int
sys_get_priority(int caller_asid)
{
	return asid2pri[caller_asid];
}


/* main scheduler thread */
int main()
{
	Time now;

	printf("scheduler is active\n");

	/* verify that we are running in user mode with interrupts disabled */
	if (!check_psw(1,0))
		panic("sched: invalid processor status: %08lx\n", get_psw());

	elem_init();
	asid2pri[ASID_NUCLEUS] = PRI_MAX;

	/* set next timer interrupts and measure "fuzz" */
	now = hrtime();
	next_wakeup = MAX_TIME; 
	set_timer(next_wakeup);
	fuzz = 4*(hrtime() - now);

	if (portal_create(SYS_READY, "sptiii", 0, (Func) sys_ready, 0) < 0)
		panic("portal_create for ready():");
	if (portal_create(SYS_YIELD, "sptaii", 0, (Func) sys_yield, 0) < 0)
		panic("portal_create for yield():");
	if (portal_create0(SYS_SCHED, (Func) sys_sched) < 0)
		panic("portal_create for sched():");
	/* must pass ASID of caller in 3rd argument to sem_create */
	if (portal_create(SYS_SEM_CREATE, "sptiai", 0, (Func)sys_sem_create, 0) < 0)
		panic("portal_create for sem_create():");
	if (portal_create0(SYS_SEM_POST_SCHED, (Func) sys_sem_post) < 0)
		panic("portal_create for sem_post_sched():");
	if (portal_create(SYS_PIPE_CREATE, "sptiii", 0, (Func) sys_pipe_create, 0) < 0)
		panic("portal_create for pipe_create():");
	if (portal_create(SYS_TIMER_HANDLER, "sptiii", 0,
	    (Func) sys_timer_handler, 0) < 0)
		panic("portal_create for timer_handler():");
	if (portal_create(SYS_HRSLEEP, "sptaii", 0, (Func) sys_hrsleep, 0) < 0)
		panic("portal_create for usleep():");
	if (portal_create(SYS_SET_PRIORITY, "sptiia", 0, sys_set_priority, 0) < 0)
		panic("portal_create for set_priority():");
	if (portal_create(SYS_GET_PRIORITY, "spaiii", 0, sys_get_priority, 0) < 0)
		panic("portal_create for get_priority():");
	if (portal_create(SYS_DUMP_SCHED, "sptiii", 0, sys_dump_sched, 0) < 0)
		panic("portal_create for dump_sched():");
	if (portal_create(SYS_SEM_NWAIT, "sptaii", 0, sys_sem_nwait, 0) < 0)
		panic("portal_create for sem_nwait():");
	if (portal_create(SYS_VQ_WAIT, "sptaii", 0, sys_vq_wait, 0) < 0)
		panic("portal_create for vq_wait():");
	/* VQ_POST should be a null portal, so that returning from sys_vq_post */
	/* would return to the caller of the current server. */
	if (portal_create(SYS_VQ_POST, "sptaii", 0, sys_vq_post, 0) < 0)
		panic("portal_create for vq_post():");


	DIAG(SCHED_DIAG, ("before returning to initialization code\n"));

	/*
	 * return to initialization code.
	 * cannot just "return", since the startup code (crt0.S) calls
	 * exit when main routine terminates.
	 */
	call_portal(SYS_RTN_RPC);
	return(1);
}