drivers/staging/comedi/rt_pend_tq.c

   1 #define __NO_VERSION__
   2 /* rt_pend_tq.c */
   3 #include <linux/kernel.h>
   4 #include <linux/errno.h>
   5 #include <linux/sched.h>
   6 #include "comedidev.h"  // for rt spinlocks
   7 #include "rt_pend_tq.h"
   8 #ifdef CONFIG_COMEDI_RTAI
   9 #include <rtai.h>
  10 #endif
  11 #ifdef CONFIG_COMEDI_FUSION
  12 #include <nucleus/asm/hal.h>
  13 #endif
  14 #ifdef CONFIG_COMEDI_RTL
  15 #include <rtl_core.h>
  16 #endif
  17
  18 #ifdef standalone
  19 #include <linux/module.h>
  20 #define rt_pend_tq_init init_module
  21 #define rt_pend_tq_cleanup cleanup_module
  22 #endif
  23
  24 volatile static struct rt_pend_tq rt_pend_tq[RT_PEND_TQ_SIZE];
  25 volatile static struct rt_pend_tq *volatile rt_pend_head = rt_pend_tq,
  26         *volatile rt_pend_tail = rt_pend_tq;
  27 int rt_pend_tq_irq = 0;
  28 spinlock_t rt_pend_tq_lock = SPIN_LOCK_UNLOCKED;
  29
  30 // WARNING: following code not checked against race conditions yet.
  31 #define INC_CIRCULAR_PTR(ptr,begin,size) do {if(++(ptr)>=(begin)+(size)) (ptr)=(begin); } while(0)
  32 #define DEC_CIRCULAR_PTR(ptr,begin,size) do {if(--(ptr)<(begin)) (ptr)=(begin)+(size)-1; } while(0)
  33
  34 int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1, void *arg2)
  35 {
  36         unsigned long flags;
  37
  38         if (func == NULL)
  39                 return -EINVAL;
  40         if (rt_pend_tq_irq <= 0)
  41                 return -ENODEV;
  42         comedi_spin_lock_irqsave(&rt_pend_tq_lock, flags);
  43         INC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
  44         if (rt_pend_head == rt_pend_tail) {
  45                 // overflow, we just refuse to take this request
  46                 DEC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
  47                 comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
  48                 return -EAGAIN;
  49         }
  50         rt_pend_head->func = func;
  51         rt_pend_head->arg1 = arg1;
  52         rt_pend_head->arg2 = arg2;
  53         comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
  54 #ifdef CONFIG_COMEDI_RTAI
  55         rt_pend_linux_srq(rt_pend_tq_irq);
  56 #endif
  57 #ifdef CONFIG_COMEDI_FUSION
  58         rthal_apc_schedule(rt_pend_tq_irq);
  59 #endif
  60 #ifdef CONFIG_COMEDI_RTL
  61         rtl_global_pend_irq(rt_pend_tq_irq);
  62
  63 #endif
  64         return 0;
  65 }
  66
  67 #ifdef CONFIG_COMEDI_RTAI
  68 void rt_pend_irq_handler(void)
  69 #elif defined(CONFIG_COMEDI_FUSION)
  70 void rt_pend_irq_handler(void *cookie)
  71 #elif defined(CONFIG_COMEDI_RTL)
  72 void rt_pend_irq_handler(int irq, void *dev PT_REGS_ARG)
  73 #endif
  74 {
  75         while (rt_pend_head != rt_pend_tail) {
  76                 INC_CIRCULAR_PTR(rt_pend_tail, rt_pend_tq, RT_PEND_TQ_SIZE);
  77                 rt_pend_tail->func(rt_pend_tail->arg1, rt_pend_tail->arg2);
  78         }
  79 }
  80
  81 int rt_pend_tq_init(void)
  82 {
  83         rt_pend_head = rt_pend_tail = rt_pend_tq;
  84 #ifdef CONFIG_COMEDI_RTAI
  85         rt_pend_tq_irq = rt_request_srq(0, rt_pend_irq_handler, NULL);
  86 #endif
  87 #ifdef CONFIG_COMEDI_FUSION
  88         rt_pend_tq_irq =
  89                 rthal_apc_alloc("comedi APC", rt_pend_irq_handler, NULL);
  90 #endif
  91 #ifdef CONFIG_COMEDI_RTL
  92         rt_pend_tq_irq = rtl_get_soft_irq(rt_pend_irq_handler, "rt_pend_irq");
  93 #endif
  94         if (rt_pend_tq_irq > 0)
  95                 printk("rt_pend_tq: RT bottom half scheduler initialized OK\n");
  96         else
  97                 printk("rt_pend_tq: rtl_get_soft_irq failed\n");
  98         return 0;
  99 }
 100
 101 void rt_pend_tq_cleanup(void)
 102 {
 103         printk("rt_pend_tq: unloading\n");
 104 #ifdef CONFIG_COMEDI_RTAI
 105         rt_free_srq(rt_pend_tq_irq);
 106 #endif
 107 #ifdef CONFIG_COMEDI_FUSION
 108         rthal_apc_free(rt_pend_tq_irq);
 109 #endif
 110 #ifdef CONFIG_COMEDI_RTL
 111         free_irq(rt_pend_tq_irq, NULL);
 112 #endif
 113 }