28 * |
28 * |
29 *****************************************************************************/ |
29 *****************************************************************************/ |
30 |
30 |
31 #include <linux/module.h> |
31 #include <linux/module.h> |
32 #include <linux/mman.h> |
32 #include <linux/mman.h> |
33 #include <rtdm/rtdm_driver.h> |
33 |
|
34 |
|
35 #ifdef ENABLE_XENOMAI |
34 #include <native/task.h> |
36 #include <native/task.h> |
35 #include <native/sem.h> |
37 #include <native/sem.h> |
36 #include <native/mutex.h> |
38 #include <native/mutex.h> |
37 #include <native/timer.h> |
39 #include <native/timer.h> |
|
40 #endif |
|
41 |
|
42 #ifdef ENABLE_RTAI |
|
43 #include <rtai_sched.h> |
|
44 #include <rtai_sem.h> |
|
45 #endif |
|
46 |
|
47 |
|
48 #include <rtdm/rtdm_driver.h> |
38 |
49 |
39 #include "../include/ecrt.h" |
50 #include "../include/ecrt.h" |
40 #include "../include/ec_rtdm.h" |
51 #include "../include/ec_rtdm.h" |
|
52 |
|
53 #ifdef ENABLE_XENOMAI |
|
54 #define my_mutex_create(X,Y) rt_mutex_create(X, Y) |
|
55 #define my_mutex_acquire(X,Y) rt_mutex_acquire(X,Y) |
|
56 #define my_mutex_release(X) rt_mutex_release(X) |
|
57 #define my_mutex_delete(X) rt_mutex_delete(X) |
|
58 #endif |
|
59 |
|
60 #ifdef ENABLE_RTAI |
|
61 #define my_mutex_create(X,Y) rt_sem_init(X, 1) |
|
62 #define my_mutex_acquire(X,Y) rt_sem_wait(X) |
|
63 #define my_mutex_release(X) rt_sem_signal(X) |
|
64 #define my_mutex_delete(X) rt_sem_delete(X) |
|
65 #define TM_INFINITE |
|
66 #endif |
|
67 |
|
68 |
|
69 |
41 |
70 |
42 #define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */ |
71 #define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */ |
43 |
72 |
44 #define EC_RTDM_GINFO(fmt, args...) \ |
73 #define EC_RTDM_GINFO(fmt, args...) \ |
45 rtdm_printk(KERN_INFO "EtherCATrtdm: " fmt, ##args) |
74 rtdm_printk(KERN_INFO "EtherCATrtdm: " fmt, ##args) |
137 EC_RTDM_DRV_STRUCT * pdrvstruc; |
171 EC_RTDM_DRV_STRUCT * pdrvstruc; |
138 |
172 |
139 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
173 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
140 if (pdrvstruc->master) |
174 if (pdrvstruc->master) |
141 { |
175 { |
142 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
176 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
143 ecrt_master_send_ext(pdrvstruc->master); |
177 ecrt_master_send_ext(pdrvstruc->master); |
144 rt_mutex_release(&pdrvstruc->masterlock); |
178 my_mutex_release(&pdrvstruc->masterlock); |
145 } |
179 } |
146 } |
180 } |
147 |
181 |
148 /*****************************************************************************/ |
182 /*****************************************************************************/ |
149 |
183 |
152 EC_RTDM_DRV_STRUCT * pdrvstruc; |
186 EC_RTDM_DRV_STRUCT * pdrvstruc; |
153 |
187 |
154 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
188 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
155 if (pdrvstruc->master) |
189 if (pdrvstruc->master) |
156 { |
190 { |
157 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
191 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
158 ecrt_master_receive(pdrvstruc->master); |
192 ecrt_master_receive(pdrvstruc->master); |
159 rt_mutex_release(&pdrvstruc->masterlock); |
193 my_mutex_release(&pdrvstruc->masterlock); |
160 } |
194 } |
161 } |
195 } |
162 |
196 |
163 |
197 |
164 |
198 |
169 if (pdrvstruc->isattached) |
203 if (pdrvstruc->isattached) |
170 { |
204 { |
171 EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n"); |
205 EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n"); |
172 ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL); |
206 ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL); |
173 EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n"); |
207 EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n"); |
174 rt_mutex_delete(&pdrvstruc->masterlock); |
208 my_mutex_delete(&pdrvstruc->masterlock); |
175 pdrvstruc->master = NULL; |
209 pdrvstruc->master = NULL; |
176 pdrvstruc->isattached=0; |
210 pdrvstruc->isattached=0; |
177 EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n"); |
211 EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n"); |
178 } |
212 } |
179 } |
213 } |
276 { |
310 { |
277 return -EFAULT; |
311 return -EFAULT; |
278 } |
312 } |
279 if (pdrvstruc->master) |
313 if (pdrvstruc->master) |
280 { |
314 { |
281 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
315 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
282 |
316 |
283 ecrt_master_state(pdrvstruc->master, &ms); |
317 ecrt_master_state(pdrvstruc->master, &ms); |
284 |
318 |
285 rt_mutex_release(&pdrvstruc->masterlock); |
319 my_mutex_release(&pdrvstruc->masterlock); |
286 |
320 |
287 } |
321 } |
288 if (rtdm_rw_user_ok(user_info, arg, sizeof(ms))) |
322 if (rtdm_rw_user_ok(user_info, arg, sizeof(ms))) |
289 { |
323 { |
290 // copy data to user |
324 // copy data to user |
302 { |
336 { |
303 return -EFAULT; |
337 return -EFAULT; |
304 } |
338 } |
305 if (pdrvstruc->domain) |
339 if (pdrvstruc->domain) |
306 { |
340 { |
307 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
341 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
308 |
342 |
309 ecrt_domain_state(pdrvstruc->domain, &ds); |
343 ecrt_domain_state(pdrvstruc->domain, &ds); |
310 |
344 |
311 rt_mutex_release(&pdrvstruc->masterlock); |
345 my_mutex_release(&pdrvstruc->masterlock); |
312 } |
346 } |
313 if (rtdm_rw_user_ok(user_info, arg, sizeof(ds))) |
347 if (rtdm_rw_user_ok(user_info, arg, sizeof(ds))) |
314 { |
348 { |
315 // copy data to user |
349 // copy data to user |
316 if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds))) |
350 if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds))) |
324 { |
358 { |
325 if (pdrvstruc->isattached) |
359 if (pdrvstruc->isattached) |
326 { |
360 { |
327 if (pdrvstruc->master) |
361 if (pdrvstruc->master) |
328 { |
362 { |
329 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
363 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
330 ecrt_master_receive(pdrvstruc->master); |
364 ecrt_master_receive(pdrvstruc->master); |
331 pdrvstruc->reccnt++; |
365 pdrvstruc->reccnt++; |
332 rt_mutex_release(&pdrvstruc->masterlock); |
366 my_mutex_release(&pdrvstruc->masterlock); |
333 } |
367 } |
334 } |
368 } |
335 } |
369 } |
336 break; |
370 break; |
337 case EC_RTDM_DOMAIN_PROCESS: |
371 case EC_RTDM_DOMAIN_PROCESS: |
338 { |
372 { |
339 if (pdrvstruc->isattached) |
373 if (pdrvstruc->isattached) |
340 { |
374 { |
341 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
375 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
342 ecrt_domain_process(pdrvstruc->domain); |
376 ecrt_domain_process(pdrvstruc->domain); |
343 rt_mutex_release(&pdrvstruc->masterlock); |
377 my_mutex_release(&pdrvstruc->masterlock); |
344 } |
378 } |
345 } |
379 } |
346 break; |
380 break; |
347 case EC_RTDM_MASTER_SEND: |
381 case EC_RTDM_MASTER_SEND: |
348 { |
382 { |
349 |
383 |
350 if (pdrvstruc->isattached) |
384 if (pdrvstruc->isattached) |
351 { |
385 { |
352 if (pdrvstruc->master) |
386 if (pdrvstruc->master) |
353 { |
387 { |
354 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
388 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
355 ecrt_master_send(pdrvstruc->master); |
389 ecrt_master_send(pdrvstruc->master); |
356 pdrvstruc->sendcnt++; |
390 pdrvstruc->sendcnt++; |
357 rt_mutex_release(&pdrvstruc->masterlock); |
391 my_mutex_release(&pdrvstruc->masterlock); |
358 } |
392 } |
359 } |
393 } |
360 } |
394 } |
361 break; |
395 break; |
362 case EC_RTDM_DOMAIN_QUEQUE: |
396 case EC_RTDM_DOMAIN_QUEQUE: |
363 { |
397 { |
364 if (pdrvstruc->isattached) |
398 if (pdrvstruc->isattached) |
365 { |
399 { |
366 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
400 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
367 ecrt_domain_queue(pdrvstruc->domain); |
401 ecrt_domain_queue(pdrvstruc->domain); |
368 rt_mutex_release(&pdrvstruc->masterlock); |
402 my_mutex_release(&pdrvstruc->masterlock); |
369 } |
403 } |
370 } |
404 } |
371 break; |
405 break; |
372 |
406 |
373 case EC_RTDM_MASTER_APP_TIME: |
407 case EC_RTDM_MASTER_APP_TIME: |
383 return -EFAULT; |
417 return -EFAULT; |
384 } |
418 } |
385 |
419 |
386 if (pdrvstruc->master) |
420 if (pdrvstruc->master) |
387 { |
421 { |
388 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
422 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
389 |
423 |
390 ecrt_master_application_time(pdrvstruc->master, app_time); |
424 ecrt_master_application_time(pdrvstruc->master, app_time); |
391 rt_mutex_release(&pdrvstruc->masterlock); |
425 my_mutex_release(&pdrvstruc->masterlock); |
392 |
426 |
393 } |
427 } |
394 } |
428 } |
395 break; |
429 break; |
396 case EC_RTDM_SYNC_REF_CLOCK: |
430 case EC_RTDM_SYNC_REF_CLOCK: |
399 { |
433 { |
400 return -EFAULT; |
434 return -EFAULT; |
401 } |
435 } |
402 if (pdrvstruc->master) |
436 if (pdrvstruc->master) |
403 { |
437 { |
404 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
438 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
405 |
439 |
406 ecrt_master_sync_reference_clock(pdrvstruc->master); |
440 ecrt_master_sync_reference_clock(pdrvstruc->master); |
407 |
441 |
408 rt_mutex_release(&pdrvstruc->masterlock); |
442 my_mutex_release(&pdrvstruc->masterlock); |
409 |
443 |
410 } |
444 } |
411 } |
445 } |
412 break; |
446 break; |
413 case EC_RTDM_SYNC_SLAVE_CLOCK: |
447 case EC_RTDM_SYNC_SLAVE_CLOCK: |
416 { |
450 { |
417 return -EFAULT; |
451 return -EFAULT; |
418 } |
452 } |
419 if (pdrvstruc->master) |
453 if (pdrvstruc->master) |
420 { |
454 { |
421 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
455 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
422 |
456 |
423 ecrt_master_sync_slave_clocks(pdrvstruc->master); |
457 ecrt_master_sync_slave_clocks(pdrvstruc->master); |
424 |
458 |
425 rt_mutex_release(&pdrvstruc->masterlock); |
459 my_mutex_release(&pdrvstruc->masterlock); |
426 |
460 |
427 } |
461 } |
428 } |
462 } |
429 break; |
463 break; |
430 case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE: |
464 case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE: |
433 { |
467 { |
434 return -EFAULT; |
468 return -EFAULT; |
435 } |
469 } |
436 if (pdrvstruc->master) |
470 if (pdrvstruc->master) |
437 { |
471 { |
438 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
472 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
439 ecrt_master_sync_monitor_queue(pdrvstruc->master); |
473 ecrt_master_sync_monitor_queue(pdrvstruc->master); |
440 rt_mutex_release(&pdrvstruc->masterlock); |
474 my_mutex_release(&pdrvstruc->masterlock); |
441 } |
475 } |
442 } |
476 } |
443 break; |
477 break; |
444 case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS: |
478 case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS: |
445 { |
479 { |
448 { |
482 { |
449 return -EFAULT; |
483 return -EFAULT; |
450 } |
484 } |
451 if (pdrvstruc->master) |
485 if (pdrvstruc->master) |
452 { |
486 { |
453 rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
487 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
454 ret = ecrt_master_sync_monitor_process(pdrvstruc->master); |
488 ret = ecrt_master_sync_monitor_process(pdrvstruc->master); |
455 rt_mutex_release(&pdrvstruc->masterlock); |
489 my_mutex_release(&pdrvstruc->masterlock); |
456 if (rtdm_safe_copy_to_user(user_info, arg, &ret, sizeof(ret))) |
490 if (rtdm_safe_copy_to_user(user_info, arg, &ret, sizeof(ret))) |
457 { |
491 { |
458 EC_RTDM_ERR(pdrvstruc->masterno,"copy to user param failed!\n"); |
492 EC_RTDM_ERR(pdrvstruc->masterno,"copy to user param failed!\n"); |
459 ret=-EFAULT; |
493 ret=-EFAULT; |
460 } |
494 } |
523 { |
557 { |
524 |
558 |
525 // set device name |
559 // set device name |
526 snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno); |
560 snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno); |
527 EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]); |
561 EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]); |
528 rt_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]); |
562 my_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]); |
529 //ecrt_release_master(mstr); |
563 //ecrt_release_master(mstr); |
530 ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc); |
564 ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc); |
531 EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain); |
565 EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain); |
532 pdrvstruc->isattached=1; |
566 pdrvstruc->isattached=1; |
533 ret = 0; |
567 ret = 0; |