1  /*
  2   * Copyright (C) 2008-2010 Paolo Mantegazza <mantegazza@aero.polimi.it>
  3   *
  4   * This program is free software; you can redistribute it and/or
  5   * modify it under the terms of the GNU General Public License as
  6   * published by the Free Software Foundation; either version 2 of the
  7   * License, or (at your option) any later version.
  8   *
  9   * This program is distributed in the hope that it will be useful,
 10   * but WITHOUT ANY WARRANTY; without even the implied warranty of
 11   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12   * GNU General Public License for more details.
 13   *
 14   * You should have received a copy of the GNU General Public License
 15   * along with this program; if not, write to the Free Software
 16   * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 17   */
 18  
 19  
 20  #include <linux/module.h>
 21  
 22  #include "rtai_taskq.h"
 23  
 24  #ifdef CONFIG_SMP
 25  volatile unsigned long tosched_mask;
 26  #endif
 27  
 28  void rt_schedule_readied(void)
 29  {
 30  	unsigned long flags;
 31  #ifdef CONFIG_SMP
 32  	unsigned long cpumask, rmask, lmask;
 33  	flags = rt_global_save_flags_and_cli();
 34  	lmask = tosched_mask;
 35  	tosched_mask = 0;
 36  	rt_global_restore_flags(flags);
 37  	rmask = lmask & ~(cpumask = (1 << rtai_cpuid()));
 38  	if (rmask) {
 39  		rtai_save_flags_and_cli(flags);
 40  		send_sched_ipi(rmask);
 41  		rtai_restore_flags(flags);
 42  	}
 43  	if (lmask | cpumask)
 44  #endif
 45  	{
 46  		flags = rt_global_save_flags_and_cli();
 47  		rt_schedule();
 48  		rt_global_restore_flags(flags);
 49  	}
 50  }
 51  EXPORT_SYMBOL(rt_schedule_readied);
 52   
 53  void rt_taskq_init(TASKQ *taskq, unsigned long type)
 54  {
 55  	taskq->qtype = (type & TASKQ_FIFO) ? 1 : 0;
 56  	taskq->queue = (QUEUE) { &taskq->queue, &taskq->queue, NULL };
 57  }
 58  EXPORT_SYMBOL(rt_taskq_init);
 59  
 60  RT_TASK *rt_taskq_ready_one(TASKQ *taskq)
 61  {
 62  	unsigned long flags;
 63  	RT_TASK *task = NULL;
 64  
 65  	flags = rt_global_save_flags_and_cli();
 66  	if ((task = (taskq->queue.next)->task)) {
 67  		dequeue_blocked(task);
 68  		rem_timed_task(task);
 69  		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_TASKQ | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
 70  			enq_ready_task(task);
 71  			TOSCHED_TASK(task);
 72  		}
 73  	}
 74  	rt_global_restore_flags(flags);
 75  	return task;
 76  }
 77  EXPORT_SYMBOL(rt_taskq_ready_one);
 78  
 79  int rt_taskq_ready_all(TASKQ *taskq, unsigned long why)
 80  {
 81  	unsigned long flags, tosched;
 82  	RT_TASK *task;
 83  	QUEUE *q;
 84  
 85  	tosched = 0;
 86  	q = &(taskq->queue);
 87  	flags = rt_global_save_flags_and_cli();
 88  	while ((q = q->next) != &(taskq->queue)) {
 89  		if ((task = q->task)) {
 90  			dequeue_blocked(task = q->task);
 91  			rem_timed_task(task);
 92  			task->retval = why;
 93  			if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_TASKQ | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
 94  				enq_ready_task(task);
 95  				TOSCHED_TASK(task);
 96  				tosched = 1;
 97  			}
 98  		}
 99  		rt_global_restore_flags(flags);
100  		flags = rt_global_save_flags_and_cli();
101  	}
102  	rt_global_restore_flags(flags);
103  	return tosched;
104  }
105  EXPORT_SYMBOL(rt_taskq_ready_all);
106  
107  void rt_taskq_wait(TASKQ *taskq)
108  {
109  	RT_TASK *rt_current;
110  	unsigned long flags;
111  	void *retp;
112  
113  	flags = rt_global_save_flags_and_cli();
114  	rt_current = RT_CURRENT;
115  	rt_current->retval = 0;
116  	rt_current->state |= RT_SCHED_TASKQ;
117  	rem_ready_current(rt_current);
118  	enqueue_blocked(rt_current, &taskq->queue, taskq->qtype);
119  	rt_schedule();
120  	if (unlikely((retp = rt_current->blocked_on) != NULL)) { 
121  		if (likely(retp != RTP_OBJREM)) { 
122  			dequeue_blocked(rt_current);
123  			rt_current->retval  = XNBREAK;
124  		} else {
125  			rt_current->prio_passed_to = NULL;
126  			rt_current->retval = XNRMID;
127  		}
128  	}
129  	rt_global_restore_flags(flags);
130  }
131  EXPORT_SYMBOL(rt_taskq_wait);
132  
133  
134  void rt_taskq_wait_until(TASKQ *taskq, RTIME time)
135  {
136  	DECLARE_RT_CURRENT;
137  	unsigned long flags;
138  	void *retp;
139  
140  	REALTIME2COUNT(time);
141  
142  	flags = rt_global_save_flags_and_cli();
143  	ASSIGN_RT_CURRENT;
144  	rt_current->retval = 0;
145  	rt_current->blocked_on = &taskq->queue;
146  	if ((rt_current->resume_time = time) > rt_time_h) {
147  		rt_current->state |= (RT_SCHED_TASKQ | RT_SCHED_DELAYED);
148  		rem_ready_current(rt_current);
149  		enqueue_blocked(rt_current, &taskq->queue, taskq->qtype);
150  		enq_timed_task(rt_current);
151  		rt_schedule();
152  	}
153  	if (unlikely((retp = rt_current->blocked_on) != NULL)) {
154  		if (likely(retp != RTP_OBJREM)) {
155  			dequeue_blocked(rt_current);
156  			rt_current->retval = retp > RTP_HIGERR ? XNTIMEO : XNBREAK;
157  		} else {
158  			rt_current->prio_passed_to = NULL;
159  			rt_current->retval = XNRMID;
160  		}
161  	}
162  	rt_global_restore_flags(flags);
163  }
164  EXPORT_SYMBOL(rt_taskq_wait_until);