26 *****************************************************************************/ |
24 *****************************************************************************/ |
27 |
25 |
28 #include <linux/module.h> |
26 #include <linux/module.h> |
29 #include <linux/delay.h> |
27 #include <linux/delay.h> |
30 #include <linux/timer.h> |
28 #include <linux/timer.h> |
31 #include <linux/spinlock.h> |
|
32 #include <linux/interrupt.h> |
29 #include <linux/interrupt.h> |
33 |
30 |
|
31 #include "rtai.h" |
|
32 #include "rtai_sched.h" |
|
33 #include "rtai_sem.h" |
|
34 |
34 #include "../../include/ecrt.h" // EtherCAT realtime interface |
35 #include "../../include/ecrt.h" // EtherCAT realtime interface |
35 |
36 |
36 #define ASYNC |
37 #define ASYNC |
37 #define FREQUENCY 100 |
38 #define TIMERTICKS 1000000 |
38 |
39 |
39 /*****************************************************************************/ |
40 /*****************************************************************************/ |
40 |
41 |
41 struct timer_list timer; |
42 // RTAI |
|
43 RT_TASK task; |
|
44 SEM master_sem; |
42 |
45 |
43 // EtherCAT |
46 // EtherCAT |
44 ec_master_t *master = NULL; |
47 ec_master_t *master = NULL; |
45 ec_domain_t *domain1 = NULL; |
48 ec_domain_t *domain1 = NULL; |
46 spinlock_t master_lock = SPIN_LOCK_UNLOCKED; |
|
47 |
49 |
48 // data fields |
50 // data fields |
49 //void *r_ssi_input, *r_ssi_status, *r_4102[3]; |
51 //void *r_ssi_input, *r_ssi_status, *r_4102[3]; |
50 |
52 |
51 // channels |
53 // channels |
58 {} |
60 {} |
59 }; |
61 }; |
60 |
62 |
61 /*****************************************************************************/ |
63 /*****************************************************************************/ |
62 |
64 |
63 void run(unsigned long data) |
65 void run(long data) |
64 { |
66 { |
65 static unsigned int counter = 0; |
67 while (1) { |
66 |
68 rt_sem_wait(&master_sem); |
67 spin_lock(&master_lock); |
|
68 |
69 |
69 #ifdef ASYNC |
70 #ifdef ASYNC |
70 // receive |
71 // receive |
71 ecrt_master_async_receive(master); |
72 ecrt_master_async_receive(master); |
72 ecrt_domain_process(domain1); |
73 ecrt_domain_process(domain1); |
73 #else |
74 #else |
74 // send and receive |
75 // send and receive |
75 ecrt_domain_queue(domain1); |
76 ecrt_domain_queue(domain1); |
76 ecrt_master_run(master); |
77 ecrt_master_run(master); |
77 ecrt_master_sync_io(master); |
78 ecrt_master_sync_io(master); |
78 ecrt_domain_process(domain1); |
79 ecrt_domain_process(domain1); |
79 #endif |
80 #endif |
80 |
81 |
81 // process data |
82 // process data |
82 //k_pos = EC_READ_U32(r_ssi); |
83 //k_pos = EC_READ_U32(r_ssi); |
83 |
84 |
84 #ifdef ASYNC |
85 #ifdef ASYNC |
85 // send |
86 // send |
86 ecrt_domain_queue(domain1); |
87 ecrt_domain_queue(domain1); |
87 ecrt_master_run(master); |
88 ecrt_master_run(master); |
88 ecrt_master_async_send(master); |
89 ecrt_master_async_send(master); |
89 #endif |
90 #endif |
90 |
91 |
91 spin_unlock(&master_lock); |
92 rt_sem_signal(&master_sem); |
92 |
93 |
93 if (counter) { |
94 rt_task_wait_period(); |
94 counter--; |
95 } |
95 } |
|
96 else { |
|
97 counter = FREQUENCY; |
|
98 //printk(KERN_INFO "k_pos = %i\n", k_pos); |
|
99 //printk(KERN_INFO "k_stat = 0x%02X\n", k_stat); |
|
100 } |
|
101 |
|
102 // restart timer |
|
103 timer.expires += HZ / FREQUENCY; |
|
104 add_timer(&timer); |
|
105 } |
96 } |
106 |
97 |
107 /*****************************************************************************/ |
98 /*****************************************************************************/ |
108 |
99 |
109 int request_lock(void *data) |
100 int request_lock(void *data) |
110 { |
101 { |
111 unsigned int tries = 0; |
102 rt_sem_wait(&master_sem); |
112 while (1) { |
103 return 0; |
113 if (spin_trylock(&master_lock)) { |
|
114 if (tries) printk(KERN_INFO "lock: %i tries needed.\n", tries); |
|
115 return 1; |
|
116 } |
|
117 tries++; |
|
118 } |
|
119 } |
104 } |
120 |
105 |
121 /*****************************************************************************/ |
106 /*****************************************************************************/ |
122 |
107 |
123 void release_lock(void *data) |
108 void release_lock(void *data) |
124 { |
109 { |
125 spin_unlock(&master_lock); |
110 rt_sem_signal(&master_sem); |
126 } |
111 } |
127 |
112 |
128 /*****************************************************************************/ |
113 /*****************************************************************************/ |
129 |
114 |
130 int __init init_mini_module(void) |
115 int __init init_mod(void) |
131 { |
116 { |
132 printk(KERN_INFO "=== Starting Minimal EtherCAT environment... ===\n"); |
117 RTIME tick_period, requested_ticks, now; |
|
118 |
|
119 printk(KERN_INFO "=== Starting EtherCAT RTAI sample module... ===\n"); |
|
120 |
|
121 rt_sem_init(&master_sem, 1); |
133 |
122 |
134 if ((master = ecrt_request_master(0)) == NULL) { |
123 if ((master = ecrt_request_master(0)) == NULL) { |
135 printk(KERN_ERR "Requesting master 0 failed!\n"); |
124 printk(KERN_ERR "Requesting master 0 failed!\n"); |
136 goto out_return; |
125 goto out_return; |
137 } |
126 } |
198 #ifdef ASYNC |
187 #ifdef ASYNC |
199 // send once and wait... |
188 // send once and wait... |
200 ecrt_master_prepare_async_io(master); |
189 ecrt_master_prepare_async_io(master); |
201 #endif |
190 #endif |
202 |
191 |
203 #if 1 |
192 #if 0 |
204 if (ecrt_master_start_eoe(master)) { |
193 if (ecrt_master_start_eoe(master)) { |
205 printk(KERN_ERR "Failed to start EoE processing!\n"); |
194 printk(KERN_ERR "Failed to start EoE processing!\n"); |
206 goto out_deactivate; |
195 goto out_deactivate; |
207 } |
196 } |
208 #endif |
197 #endif |
209 |
198 |
210 printk("Starting cyclic sample thread.\n"); |
199 printk("Starting cyclic sample thread...\n"); |
211 init_timer(&timer); |
200 requested_ticks = nano2count(TIMERTICKS); |
212 timer.function = run; |
201 tick_period = start_rt_timer(requested_ticks); |
213 timer.expires = jiffies + 10; |
202 printk(KERN_INFO "RT timer started with %i/%i ticks.\n", |
214 add_timer(&timer); |
203 (int) tick_period, (int) requested_ticks); |
215 |
204 |
216 printk(KERN_INFO "=== Minimal EtherCAT environment started. ===\n"); |
205 if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { |
|
206 printk(KERN_ERR "Failed to init RTAI task!\n"); |
|
207 goto out_stop_timer; |
|
208 } |
|
209 |
|
210 now = rt_get_time(); |
|
211 if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { |
|
212 printk(KERN_ERR "Failed to run RTAI task!\n"); |
|
213 goto out_stop_task; |
|
214 } |
|
215 |
|
216 printk(KERN_INFO "=== EtherCAT RTAI sample module started. ===\n"); |
217 return 0; |
217 return 0; |
218 |
218 |
219 #if 1 |
219 out_stop_task: |
|
220 rt_task_delete(&task); |
|
221 out_stop_timer: |
|
222 stop_rt_timer(); |
220 out_deactivate: |
223 out_deactivate: |
221 ecrt_master_deactivate(master); |
224 ecrt_master_deactivate(master); |
222 #endif |
|
223 out_release_master: |
225 out_release_master: |
224 ecrt_release_master(master); |
226 ecrt_release_master(master); |
225 out_return: |
227 out_return: |
|
228 rt_sem_delete(&master_sem); |
226 return -1; |
229 return -1; |
227 } |
230 } |
228 |
231 |
229 /*****************************************************************************/ |
232 /*****************************************************************************/ |
230 |
233 |
231 void __exit cleanup_mini_module(void) |
234 void __exit cleanup_mod(void) |
232 { |
235 { |
233 printk(KERN_INFO "=== Stopping Minimal EtherCAT environment... ===\n"); |
236 printk(KERN_INFO "=== Stopping EtherCAT RTAI sample module... ===\n"); |
234 |
237 |
235 if (master) { |
238 printk(KERN_INFO "Stopping RT task...\n"); |
236 del_timer_sync(&timer); |
239 rt_task_delete(&task); |
237 printk(KERN_INFO "Deactivating master...\n"); |
240 stop_rt_timer(); |
238 ecrt_master_deactivate(master); |
241 printk(KERN_INFO "Deactivating EtherCAT master...\n"); |
239 ecrt_release_master(master); |
242 ecrt_master_deactivate(master); |
240 } |
243 ecrt_release_master(master); |
241 |
244 rt_sem_delete(&master_sem); |
242 printk(KERN_INFO "=== Minimal EtherCAT environment stopped. ===\n"); |
245 |
|
246 printk(KERN_INFO "=== EtherCAT RTAI sample module stopped. ===\n"); |
243 } |
247 } |
244 |
248 |
245 /*****************************************************************************/ |
249 /*****************************************************************************/ |
246 |
250 |
247 MODULE_LICENSE("GPL"); |
251 MODULE_LICENSE("GPL"); |
248 MODULE_AUTHOR ("Florian Pose <fp@igh-essen.com>"); |
252 MODULE_AUTHOR ("Florian Pose <fp@igh-essen.com>"); |
249 MODULE_DESCRIPTION ("EtherCAT minimal test environment"); |
253 MODULE_DESCRIPTION ("EtherCAT RTAI sample module"); |
250 |
254 |
251 module_init(init_mini_module); |
255 module_init(init_mod); |
252 module_exit(cleanup_mini_module); |
256 module_exit(cleanup_mod); |
253 |
257 |
254 /*****************************************************************************/ |
258 /*****************************************************************************/ |
255 |
259 |