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