
/*
**
********************************************************************************
**
**   Copyright 1995 David F. Carlson  (carlson@dot4.com)
**   Copyright 1995 Dot4, Inc.
**   All rights reserved
**
**   POSIX 1003.1b Process Scheduler
**   Provided to the Linux Community by Dot4, Inc.
**
**   Dot4 is a provider of professional services for POSIX-based 
**   real-time systems from embedded to distributed.
**
**   For more information call:
**
**   Dot4, Inc.  (800) 834-1951 **  email: dot4@dot4.com
**   The Real-Time Specialists
**
********************************************************************************
**
** Permission is granted to any individual or institution to use, copy,
** or redistribute this software so long as it is not sold for profit,
** and provided redistributions must retain the above copyright
** notice, this list of conditions and the following disclaimer.
**
** THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND THE COMPANY ``AS IS'' AND
** ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
** ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR THE COMPANY BE LIABLE
** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
** OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
** HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
** LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
** OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
** SUCH DAMAGE.
**
********************************************************************************
**
*/




#ifdef __KERNEL__

#include <linux/sched.h>
#include <linux/errno.h>  /* errno */
#include <linux/mm.h>  /* VERIFY_* */
#include <linux/kernel.h>  /* suser */

#include <asm/segment.h>  /* copy routines */

#endif /* __KERNEL__ */

#include <linux/psched.h>

extern struct task_struct *current;  /* what is running now */
extern struct task_struct init_task;  /* head of LINUX task loop */
extern int need_resched;  /* run schedule in return_from_sys_call */

volatile unsigned long int psched_runq_bits = 0;  /* global POSIX queue tickle mask */

struct psched_runq		{
	struct task_struct *headp;  /* linked lists of processes */
	struct task_struct *tailp;  /* tails of the lists  */
};

static struct psched_runq runq[PSCHED_N_PRI] = { { 0, 0 } };  /* run queue for POSIX scheduler */

static inline int remove_task_from_queue(
	struct task_struct *task,   /* task to search for */
	struct psched_runq *qp)     /* queue pointer */
	{

	if (!task)	return -ESRCH;

	if (qp->headp == (struct task_struct *) NULL) return -ESRCH;

	if (task->pprev)	{	
		task->pprev->pnext = task->pnext;  /* skip over p */

		if (task->pnext)	{
			task->pnext->pprev = task->pprev;  /* skip over p */
		}

		if (!task->pprev->pnext) 	{  /* if new tail then tp must be updated */
			qp->tailp = task->pprev;
 		}
	}
	else	{  /* prev == NULL case */
		if (task->pnext)	{ /* there is a new head */
			qp->headp = task->pnext;
			task->pnext->pprev = (struct task_struct *) NULL;
 		}
 		else	{  /* task is the only task => now empty */
			qp->headp = qp->tailp = (struct task_struct *) NULL;  /* removed: now empty */
 		}
	}
	task->pnext = task->pprev = (struct task_struct *) NULL;  
		/* removed: now empty */

	return 0;
}

static inline void prepend_task_to_queue(
	struct task_struct *task, 
	struct psched_runq *qp)
	{
	if (task == qp->headp)	return;  /* already there */

	if (qp->headp)	{
		task->pnext = qp->headp;  /* non-NULL */
		task->pnext->pprev = task;
	}

	qp->headp = task;  /* prepend must be head */


		/* ASSERT: if there is a tail, then it is invariant */
	if (!qp->tailp)	{  /* if there was no tail... */
		qp->tailp = task;
	}
}


static inline void append_task_to_queue(
	struct task_struct *task, 
	struct psched_runq *qp)
	{
	if (task == qp->tailp)	return;  /* already there */

		/* ASSERT: if there is a head, then it is invariant */
	if (qp->tailp)	{
		qp->tailp->pnext = task;
		task->pprev = qp->tailp;
	}
	qp->tailp = task;  /* new tail */

	if (!qp->headp)	{
		qp->headp = task;  /* first element on queue case */
	}
}

inline struct task_struct *find_task_by_pid(pid_t pid)
	{
	struct task_struct *p;  /* iterator over tasks */

	for_each_task(p)	{
		if (pid == p->pid)	return p;  /* found it! */
 	}

 	return (struct task_struct *) NULL;  /* couldn't find it */
}


/*
**
**  This is the main POSIX scheduler called from schedule().
**
*/

struct task_struct *psched(void)
	{
	unsigned long bit;  /* bit mask being scanned */
	unsigned long bits;  /* local copy of volatile global */
	struct task_struct *p;  /* process pointer */
	struct task_struct *next ;  /* schedule this one */
	struct psched_runq *qp;  /* pointer to active run queue */
	unsigned long snap_bits;  /* local copy of volatile global */
	unsigned long flags;

#define QUEUE_BIT(x)		(1 << (x))
#define FIRST_QUARTER	(0xff000000)
#define SECOND_QUARTER	(0x00ff0000)
#define THIRD_QUARTER	(0x0000ff00)
#define FOURTH_QUARTER	(0x000000ff)

 	snap_bits = bits = psched_runq_bits;  /* get local copy of bits */

 	if (bits & FIRST_QUARTER)   { 
 		bit = QUEUE_BIT(SCHED_MAX);
 		qp = &runq[SCHED_MAX];
	}
 	else 
 	if (bits & SECOND_QUARTER)   {
 		bit = QUEUE_BIT(23);
 		qp = &runq[23];
	}
 	else
 	if (bits & THIRD_QUARTER)   {
 		bit = QUEUE_BIT(15);
 		qp = &runq[15];
	}
   else
   if (bits & FOURTH_QUARTER)   {
   	bit = QUEUE_BIT(7);
   	qp = &runq[7];	
	}
   else 
	   return (struct task_struct *) NULL;  /* no bits set anywhere */

#ifdef DEBUG
 	printk("psched: bits = 0x%x.\n", psched_runq_bits);
#endif /* DEBUG */

		/* over all priorities highest -> lowest */
	for (next = (struct task_struct *) NULL ; 
		bit && !next ;  qp--, bit >>= 1)	{
		/* while we have queues and not picked one */ 

		if (!(bit & bits)) continue;  /* empty runq */

			/* Assume queue is empty... */
 		bits ^= bit;  /* reset runnable until we find one */

		for (p = qp->headp;  
 			p && !next ;	/* while we have more and haven't picked one */
 				p = p->pnext)	{

			if (p->state != TASK_RUNNING)	{
				continue;  /* skip this */
			}
			/* now we have a runnable task */

			bits ^= bit;  /* fix local */

	 		next = p;  /* we have selected a next: get out of loop */

			if (current != next)	{  /* if current, then don't mess with queues! */

		 		save_flags(flags);
		 		cli();

					/* remove from this queue */
				(void) remove_task_from_queue(next, qp);

					/* move runnable to front of queue... */
					/* pass in the pointer to the queue tail too. */
				prepend_task_to_queue(next, qp);

				restore_flags(flags);
#ifdef DEBUG
				printk("psched: next = %d.\n", next->pid);
#endif /* DEBUG */
			}
#ifdef DEBUG
			else
				printk("psched: current = %d.\n", current->pid);
#endif /* DEBUG */
	 	}  /* end over this queue */

	}  /* end over all queues */

 	save_flags(flags);
 	cli();  /* interrupts can't touch when we twiddle runq bits! */

 	snap_bits ^= psched_runq_bits; /* what has changed since we started */
 	psched_runq_bits = bits | snap_bits;  /* export the global copy */

	restore_flags(flags);  /* unprotect */

	/* end of POSIX scheduler proper */
	return next;  /* NULL if no task was selected */
}


/*
**	psched_enqueue() can be called from sys_fork() in fork.c.
**
*/

void psched_enqueue(struct task_struct *p)
	{
	unsigned long flags;

	/* ASSERT this must be POSIX scheduled */

	p->pprev = p->pnext = (struct task_struct *) NULL;

	save_flags(flags);
	cli();  /* critical region */

	append_task_to_queue(p, &runq[p->psched_priority]);  /* no change to run state */

	restore_flags(flags);  /* critical region */

		/* for being so good, SCHED_RR gets more time */
	if (p->psched_policy == SCHED_RR)	{  
		struct timespec ts = POSIX_RR_INTERVAL;  /* the interval */
		p->counter = TIMESPEC_to_JIFFIES(&ts);
	}
}


/*
**	psched_yield() can be called from do_timer() in sched.c.
**
**	scheduling is *EXTERNAL* to psched_yield() the caller!
*/

void psched_yield(struct task_struct *p)
	{
	unsigned long flags;

		/* this case not called from interrupt */
	if (p->psched_policy == SCHED_TS)	{  
		p->counter = 0;
	}
	else	{

		/* ASSERT this must be POSIX scheduled */

		save_flags(flags);
		cli();  /* critical region */

		(void) remove_task_from_queue(p, &runq[p->psched_priority]);

		append_task_to_queue(p, &runq[p->psched_priority]);  /* no change to run state */

		restore_flags(flags);  /* critical region */

			/* for being so good, SCHED_RR gets more time */
		if (p->psched_policy == SCHED_RR)	{  
			struct timespec ts = POSIX_RR_INTERVAL;  /* the interval */
			p->counter = TIMESPEC_to_JIFFIES(&ts);
		}
	}
}


/*
**  Used by sys_exit() to leave the POSIX scheduling class.
*/

void psched_setsched_default(struct task_struct *p)
	{
	unsigned long flags;

 		/* rather than touch exit.c, ZOMBIE removal for POSIX scheduled is here */
	if (IS_POSIX_SCHEDULED(p))	{
		save_flags(flags);
		cli();

		(void) remove_task_from_queue(p, &runq[p->psched_priority]); 

		p->counter = SCHED_COUNTER;
		p->priority = SCHED_PRIORITY;
		p->psched_policy = SCHED_DEFAULT;

		restore_flags(flags);
	}
#ifdef DEBUG
	printk("psched: zombie reap (%d).\n", p->pid);
#endif /* DEBUG */
}


/*
** POSIX.1b Functions
*/

#ifdef __KERNEL__


asmlinkage int sys_sched_setparam(pid_t pid, struct sched_param *param)
	{
	struct task_struct *p = (struct task_struct *) NULL;
	struct sched_param local_param;
	struct sched_param *pp = &local_param;
	unsigned long flags;
	int error;

	if (param) {
		error = verify_area(VERIFY_READ, param, sizeof(struct sched_param));
		if (error)
			return -EINVAL;  /* POSIX says so */
	}
	else
		return -EINVAL;  /* no arg makes this call useless but POSIX defines no EINVAL */

		/* t, f, s */
	memcpy_fromfs(pp, param, sizeof (struct sched_param));

	if ((pp->sched_priority < SCHED_MIN) || (pp->sched_priority > SCHED_MAX))	{
		return -EINVAL;
 	}


	if (!pid)	{  /* current task */
		p = current;
	}
	else	{

		if (!(p = find_task_by_pid(pid)))	{
			return -ESRCH;  /* failed to find */
 		}

			/* if not root or the current user */
		if (!suser() && (p->uid != current->uid)) 	{
			return -EPERM;
 		}
	}

		/* must be POSIX scheduled to do this */
	if (!IS_POSIX_SCHEDULED(p))
		return -EINVAL;

	/* ASSERT: got a good p */


	if (pp->sched_priority != p->psched_priority)	{  /* doing the priority */
			/* get p out of old scheduler queue */

 		save_flags(flags);
 		cli();

 		(void) remove_task_from_queue(p, &runq[p->psched_priority]);

 		p->psched_priority = pp->sched_priority;

			/* POSIX says make me the tail and reschedule */

 		append_task_to_queue(p, &runq[p->psched_priority]);

 		restore_flags(flags);

 		POSIX_RUNQ(p);  /* if state == TASK_RUNNING, note it */

	}
	return 0;
}

asmlinkage int sys_sched_getparam(pid_t pid, struct sched_param *param)
	{
	struct task_struct *p = (struct task_struct *) NULL;
	struct sched_param my_param;
	int error;

	if (param) {
		error = verify_area(VERIFY_WRITE, param, sizeof(struct sched_param));
		if (error)
			return error;
	}
	else
		return 0;  /* no arg makes this call useless but POSIX defines no EINVAL */


	if (!pid)	{  /* current task */
		p = current;
	}
	else	{
		if (!(p = find_task_by_pid(pid)))	{
			return -ESRCH;  /* failed to find */
 		}

			/* if not root or the current user */
		if (!suser() && (p->uid != current->uid)) 	{
			return -EPERM;
 		}
	}

	/* ASSERT: got a good p */

		/* copy in fields */
	my_param.sched_priority = p->psched_priority;

		/* t, f, s */
	memcpy_tofs(param, &my_param, sizeof (struct sched_param));

	return 0;
}

asmlinkage int sys_sched_setscheduler(pid_t pid, int policy, struct sched_param *param)
	{
	struct task_struct *p = (struct task_struct *) NULL;
	struct sched_param local_param;
	struct sched_param *pp = &local_param;
	int sched_needed = 0;  /* private scheduling flag */
	int old_priority, old_policy;  /* local copy */
	unsigned long flags;
	int error;

	if (param) {
		error = verify_area(VERIFY_READ, param, sizeof(struct sched_param));
		if (error)
			return -EINVAL;  /* POSIX says so */
	}
	else
		return -EINVAL;  /* no arg makes this call useless but POSIX defines no EINVAL */

	if ((policy != SCHED_TS) && (!(policy & POSIX_SCHED_CLASS)))	{
		return -EINVAL;
	}

		/* t, f, s */
	memcpy_fromfs(pp, param, sizeof (struct sched_param));

		/* strictly, when policy->SCHED_TS timeshare priorities should be OK */

	if ((pp->sched_priority < SCHED_MIN) || (pp->sched_priority > SCHED_MAX))	{
		return -EINVAL;
 	}

	if (!pid)	{  /* current task */
		p = current;
	}
	else	{

		if (!(p = find_task_by_pid(pid)))	{
			return -ESRCH;  /* failed to find */
 		}

			/* if not root or the current user */
		if (!suser() && (p->uid != current->uid)) 	{
			return -EPERM;
 		}
	}

	/* ASSERT: got a good p */

#ifdef DEBUG
	printk("setsched: good p: p = 0x%x pid = %d pri = %d policy = %d.\n", 
		p, p->pid, pp->sched_priority, policy);  
#endif /* DEBUG */

	old_priority = p->psched_priority;  /* save these */
	old_policy = p->psched_policy;

	if (p->psched_priority != pp->sched_priority)	{  /* resched needed */
		p->psched_priority = pp->sched_priority;
		sched_needed = 1;
	}

	if (policy != p->psched_policy)	{  /* resched needed */
		p->psched_policy = policy;
		sched_needed = 1;
	}

	if (sched_needed)	{

		if (old_policy != SCHED_TS)	{ /* POSIX scheduled needs to be removed */

 			(void) remove_task_from_queue(p, &runq[old_priority]);
		}

			/* POSIX says make me the tail and reschedule */

 		if (policy == SCHED_TS)	{  /* TS is not inserted into psched queues */
 			p->priority = SCHED_PRIORITY;  /* base priority in sched.h */
	 		p->counter = SCHED_COUNTER;  /* base priority in sched.h */
 		}
 		else	{

	 		if (p->psched_policy == SCHED_RR)	{
				struct timespec rr = POSIX_RR_INTERVAL;

	 			p->counter = TIMESPEC_to_JIFFIES(&rr);
	 		}
	 		else	{
	 			p->counter = -1;  /* spoof TS in entry.S */
	 		}

 			save_flags(flags);
 			cli();

 			append_task_to_queue(p, &runq[p->psched_priority]);

 			restore_flags(flags);

			POSIX_RUNQ(p);
		}

#ifdef DEBUG
		printk("setsched: scheduler!\n");
#endif /* DEBUG */

		need_resched = 1;
	}

	return 0;
}

asmlinkage int sys_sched_getscheduler(pid_t pid)
	{
	struct task_struct *p = (struct task_struct *) NULL;

	if (!pid)	{  /* current task */
		p = current;
	}
	else	{

		if (!(p = find_task_by_pid(pid)))	{
			return -ESRCH;  /* failed to find */
 		}

			/* if not root or the current user */
		if (!suser() && (p->uid != current->uid)) 	{
			return -EPERM;
 		}
	}

	/* ASSERT: got a good p */

	return p->psched_policy;
}

asmlinkage int sys_sched_yield(void)
	{
	psched_yield(current);  /* common code to do_timer routine */
	need_resched = 1;
	return (0);
}

asmlinkage int sys_sched_get_priority_max(int policy)
	{
	if (!(policy & POSIX_SCHED_CLASS)){
		return -EINVAL;
	}

	return SCHED_MAX;
}

asmlinkage int sys_sched_get_priority_min(int policy)
	{
	if (!(policy & POSIX_SCHED_CLASS)){
		return -EINVAL;
	}

	return SCHED_MIN;
}

asmlinkage int sys_sched_rr_get_interval(pid_t pid, struct timespec *interval)
	{
	int error;
	struct timespec my_ts = POSIX_RR_INTERVAL;

	/* Note: POSIX spec says ESRCH shall be returned.  It is not */

	/* For our implementation, all timevslices are the same */

	if (interval) {
		error = verify_area(VERIFY_WRITE, interval, sizeof(struct timespec));
		if (error)
			return -EINVAL;  /* POSIX says so */
	}
	else
		return -EINVAL;  /* no arg makes this call useless but POSIX defines no EINVAL */

	memcpy_tofs(interval, &my_ts, sizeof (struct timespec));

	return 0;
}

#endif /* __KERNEL__ */

