path: root/litmus/litmus_softirq.c

/* much copied shamelessly from Linux's softirq.c 
   as well as PREEMPT_RT's softirq.c */

#include <linux/interrupt.h>
#include <linux/percpu.h>
#include <linux/cpu.h>
#include <linux/kthread.h>
#include <linux/ftrace.h>
#include <linux/smp.h>
#include <linux/slab.h>

#include <litmus/litmus.h>
#include <litmus/jobs.h>
#include <litmus/sched_plugin.h>
#include <litmus/litmus_softirq.h>


/* counts number of daemons ready to handle litmus irqs. */
static atomic_t num_ready_klitirqds = ATOMIC_INIT(0);

enum pending_flags
{
    LIT_TASKLET_LOW = 0x1,
    LIT_TASKLET_HI  = LIT_TASKLET_LOW<<1
};

/* only support tasklet processing for now. */
struct tasklet_head
{
	struct tasklet_struct *head;
	struct tasklet_struct **tail;
};

struct klitirqd_info
{
	struct task_struct*		klitirqd;
	raw_spinlock_t			lock;
	
	u32						pending;
	struct tasklet_head		pending_tasklets;
	struct tasklet_head		pending_tasklets_hi;
};

/* one list for each klitirqd */
static struct klitirqd_info klitirqds[NR_LITMUS_SOFTIRQD];



inline unsigned int klitirqd_id(struct task_struct* tsk)
{
    int i;
    for(i = 0; i < NR_LITMUS_SOFTIRQD; ++i)
    {
        if(klitirqds[i].klitirqd == tsk)
        {
            return i;
        }
    }
    
    BUG();
    
    return 0;
}


inline static u32 litirq_pending_hi_irqoff(struct klitirqd_info* which)
{
    return (which->pending & LIT_TASKLET_HI);
};

inline static u32 litirq_pending_low_irqoff(struct klitirqd_info* which)
{
    return (which->pending & LIT_TASKLET_LOW);
};


inline static u32 litirq_pending_irqoff(struct klitirqd_info* which)
{
    return(which->pending);
};


inline static u32 litirq_pending(struct klitirqd_info* which)
{
    unsigned long flags;
    u32 pending;
    
    raw_spin_lock_irqsave(&which->lock, flags);
    pending = litirq_pending_irqoff(which);
    raw_spin_unlock_irqrestore(&which->lock, flags);
    
    return pending;
};


static int needs_prio_change(struct klitirqd_info* which,
                             struct tasklet_head* pending_tasklets)
{
    unsigned long flags;
    int ret = 0;

    raw_spin_lock_irqsave(&which->lock, flags);
    
    if((pending_tasklets == &which->pending_tasklets_hi) && litirq_pending_hi_irqoff(which))
    {
        if(which->pending_tasklets_hi.head->owner != tsk_rt(which->klitirqd)->inh_task)
        {
            ret = 1;
        }
    }
    else if((pending_tasklets == &which->pending_tasklets) && litirq_pending_irqoff(which))
    {
        if(which->pending_tasklets.head->owner != tsk_rt(which->klitirqd)->inh_task)
        {
            ret = 1;
        }        
    }
    
    raw_spin_unlock_irqrestore(&which->lock, flags);
    
    TRACE_TASK(which->klitirqd, "priority change needed: %d\n", ret);
    
    return ret;
}


static void __reeval_prio(struct klitirqd_info* which)
{
    u32 pending = 0;
	struct task_struct* tsk = which->klitirqd;
    struct task_struct* new_prio = tsk_rt(tsk)->inh_task;
    
    if(litirq_pending_irqoff(which))
    {
        pending = 1;
        if(which->pending_tasklets.head->owner != tsk_rt(tsk)->inh_task)
        {
            new_prio = which->pending_tasklets.head->owner;
        }        
    }    
    
    if(litirq_pending_hi_irqoff(which))
    {
        pending = 1;
        if(which->pending_tasklets_hi.head->owner != tsk_rt(tsk)->inh_task)
        {
            new_prio = which->pending_tasklets_hi.head->owner;
        }
    }
    
    if(!pending)
    {
        new_prio = NULL;
    }
    
    if(new_prio != tsk_rt(tsk)->inh_task)
    {
        if(new_prio != NULL)
        {
			if(!in_interrupt())
			{
				TRACE_CUR("%s: Priority change: %s/%d to %s/%d\n", __FUNCTION__,
						((tsk_rt(tsk)->inh_task) ? tsk_rt(tsk)->inh_task : tsk)->comm,
						((tsk_rt(tsk)->inh_task) ? tsk_rt(tsk)->inh_task : tsk)->pid,
						new_prio->comm, new_prio->pid);
			}
			else
			{
				TRACE("%s: Priority change: %s/%d to %s/%d\n", __FUNCTION__,
					((tsk_rt(tsk)->inh_task) ? tsk_rt(tsk)->inh_task : tsk)->comm,
					((tsk_rt(tsk)->inh_task) ? tsk_rt(tsk)->inh_task : tsk)->pid,
					new_prio->comm, new_prio->pid);				
			}
            litmus->set_prio_inh(tsk, new_prio);
        }
        else
        {
			if(likely(!in_interrupt()))
			{
				TRACE_CUR("%s: Priority change: %s/%d to NULL (reverting)\n",
						__FUNCTION__, tsk->comm, tsk->pid);
			}
			else
			{
				// is this a bug?
				TRACE("%s: Priority change: %s/%d to NULL (reverting)\n",
					__FUNCTION__, tsk->comm, tsk->pid);			
			}			

            litmus->clear_prio_inh(tsk);
        }
    }
}

static void reeval_prio(struct klitirqd_info* which)
{
    unsigned long flags;
    
    raw_spin_lock_irqsave(&which->lock, flags);
    __reeval_prio(which);
    raw_spin_unlock_irqrestore(&which->lock, flags);
}


static void wakeup_litirqd_locked(struct klitirqd_info* which)
{
	/* Interrupts are disabled: no need to stop preemption */
	if (which && which->klitirqd && which->klitirqd->state != TASK_RUNNING)
    {
        __reeval_prio(which); /* configure the proper priority */
        TRACE("%s: Waking up klitirqd: %s/%d\n", __FUNCTION__,
			  which->klitirqd->comm, which->klitirqd->pid);
		wake_up_process(which->klitirqd);
    }
}


#if 0
static void wakeup_litirqd(struct klitirqd_info* which)
{
	/* Interrupts are disabled: no need to stop preemption */
	if (which && which->klitirqd && which->klitirqd->state != TASK_RUNNING)
    {
        reeval_prio(which); /* configure the proper priority */
        TRACE("%s: Waking up klitirqd: %s/%d\n", __FUNCTION__,
			  which->klitirqd->comm, which->klitirqd->pid);
		wake_up_process(which->klitirqd);
    }
}


void trigger_litirqs(struct task_struct* tsk)
{
	struct klitirqd_info* which = &klitirqds[klitirqd_id(tsk)];
	
    TRACE("%s: entering, triggering %s/%d\n", __FUNCTION__, tsk->comm, tsk->pid);	
	
	while (litirq_pending(which))
    {
        wakeup_litirqd(which);
	}
    TRACE("%s: exiting, done triggering %s/%d\n", __FUNCTION__, tsk->comm, tsk->pid);
}
#endif

/* forward declarations */
static void ___litmus_tasklet_schedule(struct tasklet_struct *t,
										  struct klitirqd_info *which,
										  int wakeup);
static void ___litmus_tasklet_hi_schedule(struct tasklet_struct *t,
										  struct klitirqd_info *which,
										  int wakeup);

static void do_lit_tasklet(struct klitirqd_info* which,
						   struct tasklet_head* pending_tasklets)
{
    unsigned long flags;
	struct tasklet_struct *list;

    raw_spin_lock_irqsave(&which->lock, flags);
    
	/* copy out the tasklets for our private use. */
	list = pending_tasklets->head;
	pending_tasklets->head = NULL;
	pending_tasklets->tail = &pending_tasklets->head;
	which->pending &= (pending_tasklets == &which->pending_tasklets) ?
		~LIT_TASKLET_LOW :
		~LIT_TASKLET_HI;

    raw_spin_unlock_irqrestore(&which->lock, flags);
    
    while(list)
    {
        struct tasklet_struct *t = list;
        
        /* advance, lest we forget */
		list = list->next;
        
        /* execute tasklet if it has my priority and is free */
		if ((t->owner == tsk_rt(current)->inh_task) && tasklet_trylock(t)) {
			if (!atomic_read(&t->count)) {
				if (!test_and_clear_bit(TASKLET_STATE_SCHED, &t->state))
                {
					BUG();
                }
                TRACE_CUR("%s: Invoking tasklet.\n", __FUNCTION__);
				t->func(t->data);
				tasklet_unlock(t);
				continue;  /* process more tasklets */
			}
			tasklet_unlock(t);
		}
        
        TRACE_CUR("%s: Could not invoke tasklet. Requeuing.\n", __FUNCTION__);
        
		/* couldn't process tasklet.  put it back at the end of the queue. */
		if(pending_tasklets == &which->pending_tasklets)
			___litmus_tasklet_schedule(t, which, 0);
		else
			___litmus_tasklet_hi_schedule(t, which, 0);
    }
}

static void do_litirq(struct klitirqd_info* which)
{
    u32 pending;
    int resched = 0;
    
    if(in_interrupt())
    {
        TRACE("%s: exiting early: in interrupt context!\n", __FUNCTION__);
        return;
    }
	
	if(which->klitirqd != current)
	{
        TRACE_CUR("%s: exiting early: thread/info mismatch! Running %s/%d but given %s/%d.\n",
				  __FUNCTION__, current->comm, current->pid,
				  which->klitirqd->comm, which->klitirqd->pid);
        return;
	}
	
    if(!is_realtime(current))
    {
        TRACE_CUR("%s: exiting early: klitirqd is not real-time. Sched Policy = %d\n",
				  __FUNCTION__, current->policy);
        return;
    }
    
    /* since we only handle tasklets, no need for RCU triggers? */
    
    pending = litirq_pending(which);
    if(pending)
    {
        /* extract the work to do and do it! */
        if(pending & LIT_TASKLET_HI)
        {
            TRACE_CUR("%s: Invoking HI tasklets.\n", __FUNCTION__);
            do_lit_tasklet(which, &which->pending_tasklets_hi);
            resched = needs_prio_change(which, &which->pending_tasklets_hi);
            
            if(resched)
            {
                TRACE_CUR("%s: HI tasklets of another priority remain. Skipping LOW tasklets.\n", __FUNCTION__);
            }
        }
        
        if(!resched && (pending & LIT_TASKLET_LOW))
        {
            TRACE_CUR("%s: Invoking LOW tasklets.\n", __FUNCTION__);
			do_lit_tasklet(which, &which->pending_tasklets);
			resched = needs_prio_change(which, &which->pending_tasklets);
        }
    }
}


int set_litmus_daemon_sched(struct klitirqd_info* which)
{
    /* set up a daemon job that will never complete.
       it should only ever run on behalf of another
       real-time task.

       TODO: Transition to a new job whenever a
       new tasklet is handled */
    
    int ret = 0;
	struct task_struct* tsk = which->klitirqd;
	
	struct rt_task tp = {
		.exec_cost = 0,
		.period = 1000000000, /* dummy 1 second period */
		.phase = 0,
		.cpu = 0,
		.budget_policy = NO_ENFORCEMENT,
		.cls = RT_CLASS_BEST_EFFORT
	};
	
	struct sched_param param = { .sched_priority = 0};
	
	
	/* test task params and mark as proxy thread. */
	tsk->rt_param.task_params = tp;
	tsk->rt_param.is_proxy_thread = 1;
	
    /* litmus_admit_task */
    ret = litmus_admit_task(tsk);
    
	/* inform the OS we're SCHED_LITMUS --
	   must happen after litmus_admit_task() */
	sched_setscheduler_nocheck(current, SCHED_LITMUS, &param);	
	
    return ret;
}


/* TODO: WHAT'S THE DEAL WITH ENABLE/DISABLE BOTTOM HALVES
   IN ORIGINAL softirq.c?  DO WE NEED IT HERE TOO? */

/* main loop for klitsoftirqd */
static int run_klitirqd(void* dummy)
{
	struct klitirqd_info* which = &klitirqds[klitirqd_id(current)];
	
    int rt_status = set_litmus_daemon_sched(which);
    
    if(rt_status != 0)
    {
        TRACE_CUR("%s: Failed to transition to rt-task.\n", __FUNCTION__);
        goto rt_failed;
    }
    
	atomic_inc(&num_ready_klitirqds);
	
	set_current_state(TASK_INTERRUPTIBLE);
    
	while (!kthread_should_stop()) {
		preempt_disable();
		if (!litirq_pending(which)) {
            /* sleep for work */
            TRACE_CUR("%s: No more tasklets.  Going to sleep.\n", __FUNCTION__);
			preempt_enable_no_resched();
            schedule();
			preempt_disable();
		}
        
		__set_current_state(TASK_RUNNING);
        
		while (litirq_pending(which)) {        
            TRACE_CUR("%s: Executing tasklets.\n", __FUNCTION__);
			do_litirq(which);
            
            TRACE_CUR("%s: Setting up next priority.\n", __FUNCTION__);
            reeval_prio(which); /* check if we need to change priority here */
            
			preempt_enable_no_resched();
			cond_resched();
            
			preempt_disable();
		}
		preempt_enable();
		set_current_state(TASK_INTERRUPTIBLE);
	}
	__set_current_state(TASK_RUNNING);
    
	atomic_dec(&num_ready_klitirqds);
	
rt_failed:
    litmus_exit_task(current);
    
	return rt_status;
}


/* executed by a kworker from workqueues */
static void launch_klitirqd(struct work_struct *unused)
{
    int i;
    
    TRACE("%s: Creating %d klitirqds\n", __FUNCTION__, NR_LITMUS_SOFTIRQD);
    
    /* create the daemon threads */
    for(i = 0; i < NR_LITMUS_SOFTIRQD; ++i)
    {
		klitirqds[i].klitirqd = 
			kthread_create(run_klitirqd, NULL, "klitirqd_th%d", i);
    }    
    
    TRACE("%s: Launching %d klitirqds\n", __FUNCTION__, NR_LITMUS_SOFTIRQD);
    
    /* unleash the daemons */
    for(i = 0; i < NR_LITMUS_SOFTIRQD; ++i)
    {
        wake_up_process(klitirqds[i].klitirqd);
    }
    
    kfree(unused);
}


void spawn_klitirqd(void)
{
    int i;
    struct work_struct* delayed_launch;
	
	if(atomic_read(&num_ready_klitirqds) != 0)
	{
		TRACE("%s: At least one klitirqd is already running! Need to call kill_klitirqd()?\n");
		return;
	}
    
    /* init the tasklet queues */
    for(i = 0; i < NR_LITMUS_SOFTIRQD; ++i)
    {
		klitirqds[i].pending = 0;
		
        klitirqds[i].pending_tasklets.head = NULL;
        klitirqds[i].pending_tasklets.tail = &klitirqds[i].pending_tasklets.head;

        klitirqds[i].pending_tasklets_hi.head = NULL;
        klitirqds[i].pending_tasklets_hi.tail = &klitirqds[i].pending_tasklets_hi.head; 

		raw_spin_lock_init(&klitirqds[i].lock);
    }
    
    /* wait to flush the initializations to memory since other threads
       will access it. */    
    mb();
    
    /* tell a work queue to launch the threads.  we can't make scheduling
       calls since we're in an atomic state. */
    TRACE("%s: Setting callback up to launch klitirqds\n", __FUNCTION__);
    delayed_launch = kmalloc(sizeof(struct work_struct), GFP_ATOMIC);
    INIT_WORK(delayed_launch, launch_klitirqd);
    schedule_work(delayed_launch);
}


void kill_klitirqd(void)
{
    int i;
    
    TRACE("%s: Killing %d klitirqds\n", __FUNCTION__, NR_LITMUS_SOFTIRQD);
    
    for(i = 0; i < NR_LITMUS_SOFTIRQD; ++i)
    {
        kthread_stop(klitirqds[i].klitirqd);
    }
	
    /* TODO: Put pending tasklets SOMEWHERE-- back to the OS? */
}


int klitirqd_is_ready(void)
{
	return(atomic_read(&num_ready_klitirqds) == NR_LITMUS_SOFTIRQD);
}

int klitirqd_is_dead(void)
{
	return(atomic_read(&num_ready_klitirqds) == 0);
}



static void ___litmus_tasklet_schedule(struct tasklet_struct *t,
									   struct klitirqd_info *which,
									   int wakeup)
{
	unsigned long flags;
	
	t->next = NULL;
	
    raw_spin_lock_irqsave(&which->lock, flags);
    
    *(which->pending_tasklets.tail) = t;
    which->pending_tasklets.tail = &t->next;
    
	which->pending |= LIT_TASKLET_LOW;
	
	if(wakeup)
	{
		wakeup_litirqd_locked(which); /* wake up the klitirqd */
	}
	
    raw_spin_unlock_irqrestore(&which->lock, flags);	
}

void __litmus_tasklet_schedule(struct tasklet_struct *t, unsigned int k_id)
{
    if(unlikely((t->owner == NULL) || !is_realtime(t->owner)))
    {
        TRACE("%s: No owner associated with this tasklet!\n", __FUNCTION__);
        BUG();
    }
    
    if(unlikely(k_id >= NR_LITMUS_SOFTIRQD))
    {
        TRACE("%s: No klitirqd_th%d!\n", __FUNCTION__, k_id);
        BUG();        
    }

	___litmus_tasklet_schedule(t, &klitirqds[k_id], 1);
}

EXPORT_SYMBOL(__litmus_tasklet_schedule);


static void ___litmus_tasklet_hi_schedule(struct tasklet_struct *t,
									   struct klitirqd_info *which,
									   int wakeup)
{
	unsigned long flags;
	
	t->next = NULL;
	
    raw_spin_lock_irqsave(&which->lock, flags);

    *(which->pending_tasklets_hi.tail) = t;
    which->pending_tasklets_hi.tail = &t->next;
    
	which->pending |= LIT_TASKLET_HI;
	
	if(wakeup)
	{
		wakeup_litirqd_locked(which); /* wake up the klitirqd */
	}
	
    raw_spin_unlock_irqrestore(&which->lock, flags);	
}

void __litmus_tasklet_hi_schedule(struct tasklet_struct *t, unsigned int k_id)
{
    if(unlikely((t->owner == NULL) || !is_realtime(t->owner)))
    {
        TRACE("%s: No owner associated with this tasklet!\n", __FUNCTION__);
        BUG();
    }
    
    if(unlikely(k_id >= NR_LITMUS_SOFTIRQD))
    {
        TRACE("%s: No klitirqd_th%d!\n", __FUNCTION__, k_id);
        BUG();        
    }
    
	___litmus_tasklet_hi_schedule(t, &klitirqds[k_id], 1);
}

EXPORT_SYMBOL(__litmus_tasklet_hi_schedule);


void __litmus_tasklet_hi_schedule_first(struct tasklet_struct *t, unsigned int k_id)
{
	BUG_ON(!irqs_disabled());
    
    if(unlikely((t->owner == NULL) || !is_realtime(t->owner)))
    {
        TRACE("%s: No owner associated with this tasklet!\n", __FUNCTION__);
        BUG();
    }
    
    if(unlikely(k_id >= NR_LITMUS_SOFTIRQD))
    {
        TRACE("%s: No klitirqd_th%d!\n", __FUNCTION__, k_id);
        BUG();        
    }  
    
    raw_spin_lock(&klitirqds[k_id].lock);
    
	t->next = klitirqds[k_id].pending_tasklets_hi.head;
    klitirqds[k_id].pending_tasklets_hi.head = t;
	
	klitirqds[k_id].pending |= LIT_TASKLET_HI;
    
    wakeup_litirqd_locked(&klitirqds[k_id]); /* wake up the klitirqd */
        
    raw_spin_unlock(&klitirqds[k_id].lock);
}

EXPORT_SYMBOL(__litmus_tasklet_hi_schedule_first);