218 if (tv.tv_usec >= 1000000) { |
218 if (tv.tv_usec >= 1000000) { |
219 tv.tv_usec -= 1000000; |
219 tv.tv_usec -= 1000000; |
220 tv.tv_sec++; |
220 tv.tv_sec++; |
221 } |
221 } |
222 ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); |
222 ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); |
223 |
223 |
224 if (sync_ref_counter) { |
224 if (sync_ref_counter) { |
225 sync_ref_counter--; |
225 sync_ref_counter--; |
226 } else { |
226 } else { |
227 sync_ref_counter = 9; |
227 sync_ref_counter = 9; |
228 ecrt_master_sync_reference_clock(master); |
228 ecrt_master_sync_reference_clock(master); |
229 } |
229 } |
230 ecrt_master_sync_slave_clocks(master); |
230 ecrt_master_sync_slave_clocks(master); |
231 ecrt_domain_queue(domain1); |
231 ecrt_domain_queue(domain1); |
|
232 ecrt_master_send(master); |
232 rt_sem_signal(&master_sem); |
233 rt_sem_signal(&master_sem); |
233 ecrt_master_send(master); |
|
234 |
234 |
235 rt_task_wait_period(); |
235 rt_task_wait_period(); |
236 } |
236 } |
237 } |
237 } |
238 |
238 |
239 /*****************************************************************************/ |
239 /*****************************************************************************/ |
240 |
240 |
241 void request_lock_callback(void *cb_data) |
241 void send_callback(void *cb_data) |
242 { |
242 { |
243 ec_master_t *m = (ec_master_t *) cb_data; |
243 ec_master_t *m = (ec_master_t *) cb_data; |
244 rt_sem_wait(&master_sem); |
244 |
245 } |
245 // too close to the next real time cycle: deny access... |
246 |
246 if (get_cycles() - t_last_cycle <= t_critical) { |
247 /*****************************************************************************/ |
247 rt_sem_wait(&master_sem); |
248 |
248 ecrt_master_send_ext(m); |
249 void release_lock_callback(void *cb_data) |
249 rt_sem_signal(&master_sem); |
|
250 } |
|
251 } |
|
252 |
|
253 /*****************************************************************************/ |
|
254 |
|
255 void receive_callback(void *cb_data) |
250 { |
256 { |
251 ec_master_t *m = (ec_master_t *) cb_data; |
257 ec_master_t *m = (ec_master_t *) cb_data; |
252 rt_sem_signal(&master_sem); |
258 |
|
259 // too close to the next real time cycle: deny access... |
|
260 if (get_cycles() - t_last_cycle <= t_critical) { |
|
261 rt_sem_wait(&master_sem); |
|
262 ecrt_master_receive(m); |
|
263 rt_sem_signal(&master_sem); |
|
264 } |
253 } |
265 } |
254 |
266 |
255 /*****************************************************************************/ |
267 /*****************************************************************************/ |
256 |
268 |
257 int __init init_mod(void) |
269 int __init init_mod(void) |
266 |
278 |
267 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
279 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
268 |
280 |
269 master = ecrt_request_master(0); |
281 master = ecrt_request_master(0); |
270 if (!master) { |
282 if (!master) { |
271 ret = -EBUSY; |
283 ret = -EBUSY; |
272 printk(KERN_ERR PFX "Requesting master 0 failed!\n"); |
284 printk(KERN_ERR PFX "Requesting master 0 failed!\n"); |
273 goto out_return; |
285 goto out_return; |
274 } |
286 } |
275 |
287 |
276 ecrt_master_callbacks(master, request_lock_callback, release_lock_callback, master); |
288 ecrt_master_callbacks(master, send_callback, receive_callback, master); |
277 |
289 |
278 printk(KERN_INFO PFX "Registering domain...\n"); |
290 printk(KERN_INFO PFX "Registering domain...\n"); |
279 if (!(domain1 = ecrt_master_create_domain(master))) { |
291 if (!(domain1 = ecrt_master_create_domain(master))) { |
280 printk(KERN_ERR PFX "Domain creation failed!\n"); |
292 printk(KERN_ERR PFX "Domain creation failed!\n"); |
281 goto out_release_master; |
293 goto out_release_master; |