/*
 * Copyright (C) 2009 Philippe Gerum <rpm@xenomai.org>.
 *
 * Xenomai is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published
 * by the Free Software Foundation; either version 2 of the License,
 * or (at your option) any later version.
 *
 * Xenomai is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Xenomai; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 * 02111-1307, USA.
 */
#include <cobalt/kernel/sched.h>
#include <cobalt/kernel/heap.h>
#include <cobalt/uapi/sched.h>

#define MAX_REPLENISH CONFIG_XENO_OPT_SCHED_SPORADIC_MAXREPL

static void sporadic_post_recharge(struct xnthread *thread, xnticks_t budget);

#ifdef CONFIG_XENO_OPT_DEBUG_COBALT

static inline void sporadic_note_late_drop(struct xnsched *sched)
{
	/*
	 * This code should pull the break when a misconfigured
	 * sporadic thread is late on its drop date for more than a
	 * hundred times in a row. This normally reveals a time budget
	 * which is too tight.
	 */
	XENO_BUG_ON(COBALT, ++sched->pss.drop_retries > 100);
}

static inline void sporadic_note_valid_drop(struct xnsched *sched)
{
	sched->pss.drop_retries = 0;
}

#else /* !CONFIG_XENO_OPT_DEBUG_COBALT */

static inline void sporadic_note_late_drop(struct xnsched *sched)
{
}

static inline void sporadic_note_valid_drop(struct xnsched *sched)
{
}

#endif /* !CONFIG_XENO_OPT_DEBUG_COBALT */

static inline xnticks_t sporadic_diff_time(xnticks_t start, xnticks_t end)
{
	xnsticks_t d = (xnsticks_t)(end - start);
	return unlikely(d < 0) ? -d : d;
}

static void sporadic_drop_handler(struct xntimer *timer)
{
	struct xnsched_sporadic_data *pss;
	union xnsched_policy_param p;
	struct xnthread *thread;

	/*
	 * XXX: this code will work properly regardless of
	 * primary/secondary mode issues.
	 */
	pss = container_of(timer, struct xnsched_sporadic_data, drop_timer);
	thread = pss->thread;

	sporadic_post_recharge(thread, pss->budget);

	if (pss->budget == 0 && thread->cprio > pss->param.low_prio) {
		if (pss->param.low_prio < 0)
			/*
			 * Special case: low_prio == -1, we want the
			 * thread to suspend until a replenishment
			 * happens.
			 */
			xnthread_suspend(thread, XNHELD,
					 XN_INFINITE, XN_RELATIVE, NULL);
		else {
			p.pss.init_budget = 0;
			p.pss.current_prio = pss->param.low_prio;
			/* Move sporadic thread to the background. */
			__xnthread_set_schedparam(thread, &xnsched_class_sporadic, &p);
		}
	}
}

static void sporadic_schedule_drop(struct xnthread *thread)
{
	xnticks_t now = xnclock_read_monotonic(&nkclock);
	struct xnsched_sporadic_data *pss = thread->pss;
	int ret;

	pss->resume_date = now;
	/*
	 * Assuming this timer should not fire that often unless the
	 * monitored thread behaves badly, we don't pin it on the CPU
	 * the thread is running, trading cycles at firing time
	 * against cycles when arming the timer.
	 */
	xntimer_set_affinity(&pss->drop_timer, thread->sched);
	ret = xntimer_start(&pss->drop_timer, now + pss->budget,
			    XN_INFINITE, XN_ABSOLUTE);
	if (ret == -ETIMEDOUT) {
		sporadic_note_late_drop(thread->sched);
		sporadic_drop_handler(&pss->drop_timer);
	} else
		sporadic_note_valid_drop(thread->sched);
}

static void sporadic_replenish_handler(struct xntimer *timer)
{
	struct xnsched_sporadic_data *pss;
	union xnsched_policy_param p;
	struct xnthread *thread;
	xnticks_t now;
	int r, ret;

	pss = container_of(timer, struct xnsched_sporadic_data, repl_timer);
	thread = pss->thread;
	XENO_BUG_ON(COBALT, pss->repl_pending <= 0);

retry:
	now = xnclock_read_monotonic(&nkclock);

	do {
		r = pss->repl_out;
		if ((xnsticks_t)(now - pss->repl_data[r].date) <= 0)
			break;
		pss->budget += pss->repl_data[r].amount;
		if (pss->budget > pss->param.init_budget)
			pss->budget = pss->param.init_budget;
		pss->repl_out = (r + 1) % MAX_REPLENISH;
	} while(--pss->repl_pending > 0);

	if (pss->repl_pending > 0) {
		xntimer_set_affinity(&pss->repl_timer, thread->sched);
		ret = xntimer_start(&pss->repl_timer, pss->repl_data[r].date,
				    XN_INFINITE, XN_ABSOLUTE);
		if (ret == -ETIMEDOUT)
			goto retry; /* This plugs a tiny race. */
	}

	if (pss->budget == 0)
		return;

	if (xnthread_test_state(thread, XNHELD))
		xnthread_resume(thread, XNHELD);
	else if (thread->cprio < pss->param.normal_prio) {
		p.pss.init_budget = 0;
		p.pss.current_prio = pss->param.normal_prio;
		/* Move sporadic thread to the foreground. */
		__xnthread_set_schedparam(thread, &xnsched_class_sporadic, &p);
	}

	/*
	 * XXX: we have to reset the drop timer in case we preempted
	 * the thread which just got a budget increase.
	 */
	if (thread->sched->curr == thread)
		sporadic_schedule_drop(thread);
}

static void sporadic_post_recharge(struct xnthread *thread, xnticks_t budget)
{
	struct xnsched_sporadic_data *pss = thread->pss;
	int r, ret;

	if (pss->repl_pending >= pss->param.max_repl)
		return;

	if (budget > pss->budget) {
		budget = pss->budget;
		pss->budget = 0;
	} else
		pss->budget -= budget;

	r = pss->repl_in;
	pss->repl_data[r].date = pss->resume_date + pss->param.repl_period;
	pss->repl_data[r].amount = budget;
	pss->repl_in = (r + 1) % MAX_REPLENISH;

	if (pss->repl_pending++ == 0) {
		xntimer_set_affinity(&pss->repl_timer, thread->sched);
		ret = xntimer_start(&pss->repl_timer, pss->repl_data[r].date,
				    XN_INFINITE, XN_ABSOLUTE);
		/*
		 * The following case should not happen unless the
		 * initial budget value is inappropriate, but let's
		 * handle it anyway.
		 */
		if (ret == -ETIMEDOUT)
			sporadic_replenish_handler(&pss->repl_timer);
	}
}

static void sporadic_suspend_activity(struct xnthread *thread)
{
	struct xnsched_sporadic_data *pss = thread->pss;
	xnticks_t budget, now;

	if (pss->budget > 0) {
		xntimer_stop(&pss->drop_timer);
		now = xnclock_read_monotonic(&nkclock);
		budget = sporadic_diff_time(now, pss->resume_date);
		sporadic_post_recharge(thread, budget);
	}
}

static inline void sporadic_resume_activity(struct xnthread *thread)
{
	if (thread->pss->budget > 0)
		sporadic_schedule_drop(thread);
}

static void xnsched_sporadic_init(struct xnsched *sched)
{
	/*
	 * We litterally stack the sporadic scheduler on top of the RT
	 * one, reusing its run queue directly. This way, RT and
	 * sporadic threads are merged into the same runqueue and thus
	 * share the same priority scale, with the addition of budget
	 * management for the sporadic ones.
	 */
#ifdef CONFIG_XENO_OPT_DEBUG_COBALT
	sched->pss.drop_retries = 0;
#endif
}

static bool xnsched_sporadic_setparam(struct xnthread *thread,
				      const union xnsched_policy_param *p)
{
	struct xnsched_sporadic_data *pss = thread->pss;
	bool effective;

	xnthread_clear_state(thread, XNWEAK);
	effective = xnsched_set_effective_priority(thread, p->pss.current_prio);

	/*
	 * We use the budget information to determine whether we got
	 * here from one of our internal calls to
	 * xnthread_set_schedparam(), in which case we don't want to
	 * update the scheduling parameters, but only set the
	 * effective priority.
	 */
	if (p->pss.init_budget > 0) {
		pss->param = p->pss;
		pss->budget = p->pss.init_budget;
		pss->repl_in = 0;
		pss->repl_out = 0;
		pss->repl_pending = 0;
		if (effective && thread == thread->sched->curr) {
			xntimer_stop(&pss->drop_timer);
			sporadic_schedule_drop(thread);
		}
	}

	return effective;
}

static void xnsched_sporadic_getparam(struct xnthread *thread,
				      union xnsched_policy_param *p)
{
	p->pss = thread->pss->param;
	p->pss.current_prio = thread->cprio;
}

static void xnsched_sporadic_trackprio(struct xnthread *thread,
				       const union xnsched_policy_param *p)
{
	if (p)
		thread->cprio = p->pss.current_prio;
	else
		thread->cprio = thread->bprio;
}

static void xnsched_sporadic_protectprio(struct xnthread *thread, int prio)
{
	if (prio > XNSCHED_SPORADIC_MAX_PRIO)
		prio = XNSCHED_SPORADIC_MAX_PRIO;

	thread->cprio = prio;
}

static int xnsched_sporadic_chkparam(struct xnthread *thread,
				     const union xnsched_policy_param *p)
{
	if (p->pss.low_prio != -1 &&
	    (p->pss.low_prio < XNSCHED_SPORADIC_MIN_PRIO ||
	     p->pss.low_prio > XNSCHED_SPORADIC_MAX_PRIO))
		return -EINVAL;

	if (p->pss.normal_prio < XNSCHED_SPORADIC_MIN_PRIO ||
	    p->pss.normal_prio > XNSCHED_SPORADIC_MAX_PRIO)
		return -EINVAL;

	if (p->pss.init_budget == 0)
		return -EINVAL;

	if (p->pss.current_prio != p->pss.normal_prio)
		return -EINVAL;

	if (p->pss.repl_period < p->pss.init_budget)
		return -EINVAL;

	if (p->pss.normal_prio <= p->pss.low_prio)
		return -EINVAL;

	if (p->pss.max_repl < 1 || p->pss.max_repl > MAX_REPLENISH)
		return -EINVAL;

	return 0;
}

static int xnsched_sporadic_declare(struct xnthread *thread,
				    const union xnsched_policy_param *p)
{
	struct xnsched_sporadic_data *pss;

	pss = xnmalloc(sizeof(*pss));
	if (pss == NULL)
		return -ENOMEM;

	xntimer_init(&pss->repl_timer, &nkclock, sporadic_replenish_handler,
		     thread->sched, XNTIMER_IGRAVITY);
	xntimer_set_name(&pss->repl_timer, "pss-replenish");
	xntimer_init(&pss->drop_timer, &nkclock, sporadic_drop_handler,
		     thread->sched, XNTIMER_IGRAVITY);
	xntimer_set_name(&pss->drop_timer, "pss-drop");

	thread->pss = pss;
	pss->thread = thread;

	return 0;
}

static void xnsched_sporadic_forget(struct xnthread *thread)
{
	struct xnsched_sporadic_data *pss = thread->pss;

	xntimer_destroy(&pss->repl_timer);
	xntimer_destroy(&pss->drop_timer);
	xnfree(pss);
	thread->pss = NULL;
}

static void xnsched_sporadic_enqueue(struct xnthread *thread)
{
	__xnsched_rt_enqueue(thread);
}

static void xnsched_sporadic_dequeue(struct xnthread *thread)
{
	__xnsched_rt_dequeue(thread);
}

static void xnsched_sporadic_requeue(struct xnthread *thread)
{
	__xnsched_rt_requeue(thread);
}

static struct xnthread *xnsched_sporadic_pick(struct xnsched *sched)
{
	struct xnthread *curr = sched->curr, *next;

	next = xnsched_getq(&sched->rt.runnable);
	if (next == NULL)
		goto swap;

	if (curr == next)
		return next;

	/* Arm the drop timer for an incoming sporadic thread. */
	if (next->pss)
		sporadic_resume_activity(next);
swap:
	/*
	 * A non-sporadic outgoing thread is having a priority
	 * inheritance boost, so apply an infinite time budget as we
	 * want it to release the claimed resource asap. Otherwise,
	 * clear the drop timer, then schedule a replenishment
	 * operation.
	 */
	if (curr->pss)
		sporadic_suspend_activity(curr);

	return next;
}

#ifdef CONFIG_XENO_OPT_VFILE

struct xnvfile_directory sched_sporadic_vfroot;

struct vfile_sched_sporadic_priv {
	int nrthreads;
	struct xnthread *curr;
};

struct vfile_sched_sporadic_data {
	int cpu;
	pid_t pid;
	char name[XNOBJECT_NAME_LEN];
	int current_prio;
	int low_prio;
	int normal_prio;
	xnticks_t period;
	xnticks_t timeout;
	xnticks_t budget;
};

static struct xnvfile_snapshot_ops vfile_sched_sporadic_ops;

static struct xnvfile_snapshot vfile_sched_sporadic = {
	.privsz = sizeof(struct vfile_sched_sporadic_priv),
	.datasz = sizeof(struct vfile_sched_sporadic_data),
	.tag = &nkthreadlist_tag,
	.ops = &vfile_sched_sporadic_ops,
};

static int vfile_sched_sporadic_rewind(struct xnvfile_snapshot_iterator *it)
{
	struct vfile_sched_sporadic_priv *priv = xnvfile_iterator_priv(it);
	int nrthreads = xnsched_class_sporadic.nthreads;

	if (nrthreads == 0)
		return -ESRCH;

	priv->curr = list_first_entry(&nkthreadq, struct xnthread, glink);

	return nrthreads;
}

static int vfile_sched_sporadic_next(struct xnvfile_snapshot_iterator *it,
				     void *data)
{
	struct vfile_sched_sporadic_priv *priv = xnvfile_iterator_priv(it);
	struct vfile_sched_sporadic_data *p = data;
	struct xnthread *thread;

	if (priv->curr == NULL)
		return 0;	/* All done. */

	thread = priv->curr;
	if (list_is_last(&thread->glink, &nkthreadq))
		priv->curr = NULL;
	else
		priv->curr = list_next_entry(thread, glink);

	if (thread->base_class != &xnsched_class_sporadic)
		return VFILE_SEQ_SKIP;

	p->cpu = xnsched_cpu(thread->sched);
	p->pid = xnthread_host_pid(thread);
	memcpy(p->name, thread->name, sizeof(p->name));
	p->current_prio = thread->cprio;
	p->low_prio = thread->pss->param.low_prio;
	p->normal_prio = thread->pss->param.normal_prio;
	p->period = xnthread_get_period(thread);
	p->budget = thread->pss->param.init_budget;

	return 1;
}

static int vfile_sched_sporadic_show(struct xnvfile_snapshot_iterator *it,
				     void *data)
{
	char lpbuf[16], npbuf[16], ptbuf[16], btbuf[16];
	struct vfile_sched_sporadic_data *p = data;

	if (p == NULL)
		xnvfile_printf(it,
			       "%-3s  %-6s %-4s %-4s  %-10s %-10s %s\n",
			       "CPU", "PID", "LPRI", "NPRI", "BUDGET",
			       "PERIOD", "NAME");
	else {
		ksformat(lpbuf, sizeof(lpbuf), "%3d%c",
			 p->low_prio, p->current_prio == p->low_prio ? '*' : ' ');

		ksformat(npbuf, sizeof(npbuf), "%3d%c",
			 p->normal_prio, p->current_prio == p->normal_prio ? '*' : ' ');

		xntimer_format_time(p->period, ptbuf, sizeof(ptbuf));
		xntimer_format_time(p->budget, btbuf, sizeof(btbuf));

		xnvfile_printf(it,
			       "%3u  %-6d %-4s %-4s  %-10s %-10s %s\n",
			       p->cpu,
			       p->pid,
			       lpbuf,
			       npbuf,
			       btbuf,
			       ptbuf,
			       p->name);
	}

	return 0;
}

static struct xnvfile_snapshot_ops vfile_sched_sporadic_ops = {
	.rewind = vfile_sched_sporadic_rewind,
	.next = vfile_sched_sporadic_next,
	.show = vfile_sched_sporadic_show,
};

static int xnsched_sporadic_init_vfile(struct xnsched_class *schedclass,
				       struct xnvfile_directory *vfroot)
{
	int ret;

	ret = xnvfile_init_dir(schedclass->name,
			       &sched_sporadic_vfroot, vfroot);
	if (ret)
		return ret;

	return xnvfile_init_snapshot("threads", &vfile_sched_sporadic,
				     &sched_sporadic_vfroot);
}

static void xnsched_sporadic_cleanup_vfile(struct xnsched_class *schedclass)
{
	xnvfile_destroy_snapshot(&vfile_sched_sporadic);
	xnvfile_destroy_dir(&sched_sporadic_vfroot);
}

#endif /* CONFIG_XENO_OPT_VFILE */

struct xnsched_class xnsched_class_sporadic = {
	.sched_init		=	xnsched_sporadic_init,
	.sched_enqueue		=	xnsched_sporadic_enqueue,
	.sched_dequeue		=	xnsched_sporadic_dequeue,
	.sched_requeue		=	xnsched_sporadic_requeue,
	.sched_pick		=	xnsched_sporadic_pick,
	.sched_tick		=	NULL,
	.sched_rotate		=	NULL,
	.sched_migrate		=	NULL,
	.sched_chkparam		=	xnsched_sporadic_chkparam,
	.sched_setparam		=	xnsched_sporadic_setparam,
	.sched_getparam		=	xnsched_sporadic_getparam,
	.sched_trackprio	=	xnsched_sporadic_trackprio,
	.sched_protectprio	=	xnsched_sporadic_protectprio,
	.sched_declare		=	xnsched_sporadic_declare,
	.sched_forget		=	xnsched_sporadic_forget,
	.sched_kick		=	NULL,
#ifdef CONFIG_XENO_OPT_VFILE
	.sched_init_vfile	=	xnsched_sporadic_init_vfile,
	.sched_cleanup_vfile	=	xnsched_sporadic_cleanup_vfile,
#endif
	.weight			=	XNSCHED_CLASS_WEIGHT(3),
	.policy			=	SCHED_SPORADIC,
	.name			=	"pss"
};
EXPORT_SYMBOL_GPL(xnsched_class_sporadic);