|
1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * ec_rtdm.c Copyright (C) 2009-2010 Moehwald GmbH B.Benner |
|
6 * 2011 IgH Andreas Stewering-Bone |
|
7 * |
|
8 * |
|
9 * This file is part of the IgH EtherCAT master |
|
10 * |
|
11 * The IgH EtherCAT master is free software; you can |
|
12 * redistribute it and/or modify it under the terms of the GNU Lesser General |
|
13 * Public License as published by the Free Software Foundation; version 2.1 |
|
14 * of the License. |
|
15 * |
|
16 * The IgH EtherCAT master userspace library is distributed in the hope that |
|
17 * it will be useful, but WITHOUT ANY WARRANTY; without even the implied |
|
18 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
19 * GNU Lesser General Public License for more details. |
|
20 * |
|
21 * You should have received a copy of the GNU Lesser General Public License |
|
22 * along with the IgH EtherCAT master userspace library. If not, see |
|
23 * <http://www.gnu.org/licenses/>. |
|
24 * |
|
25 * The license mentioned above concerns the source code only. Using the |
|
26 * EtherCAT technology and brand is only permitted in compliance with the |
|
27 * industrial property and similar rights of Beckhoff Automation GmbH. |
|
28 * |
|
29 *****************************************************************************/ |
|
30 |
|
31 #include <linux/module.h> |
|
32 #include <linux/mman.h> |
|
33 |
|
34 |
|
35 #ifdef ENABLE_XENOMAI |
|
36 #include <native/task.h> |
|
37 #include <native/sem.h> |
|
38 #include <native/mutex.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> |
|
49 |
|
50 #include "../include/ecrt.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 |
|
70 |
|
71 #define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */ |
|
72 |
|
73 #define EC_RTDM_GINFO(fmt, args...) \ |
|
74 rtdm_printk(KERN_INFO "EtherCATrtdm: " fmt, ##args) |
|
75 |
|
76 #define EC_RTDM_GERR(fmt, args...) \ |
|
77 rtdm_printk(KERN_ERR "EtherCATrtdm ERROR: " fmt, ##args) |
|
78 |
|
79 #define EC_RTDM_GWARN(fmt, args...) \ |
|
80 rtdm_printk(KERN_WARNING "EtherCATrtdm WARNING: " fmt, ##args) |
|
81 |
|
82 |
|
83 #define EC_RTDM_INFO(devno, fmt, args...) \ |
|
84 rtdm_printk(KERN_INFO "EtherCATrtdm %u: " fmt, devno, ##args) |
|
85 |
|
86 #define EC_RTDM_ERR(devno, fmt, args...) \ |
|
87 rtdm_printk(KERN_ERR "EtherCATrtdm %u ERROR: " fmt, devno, ##args) |
|
88 |
|
89 #define EC_RTDM_WARN(devno, fmt, args...) \ |
|
90 rtdm_printk(KERN_WARNING "EtherCATrtdm %u WARNING: " fmt, devno, ##args) |
|
91 |
|
92 |
|
93 |
|
94 |
|
95 typedef struct _EC_RTDM_DRV_STRUCT { |
|
96 unsigned int isattached; |
|
97 ec_master_t * master; |
|
98 ec_domain_t * domain; |
|
99 #ifdef ENABLE_XENOMAI |
|
100 RT_MUTEX masterlock; |
|
101 #endif |
|
102 #ifdef ENABLE_RTAI |
|
103 SEM masterlock; |
|
104 #endif |
|
105 unsigned int sendcnt; |
|
106 unsigned int reccnt; |
|
107 unsigned int sendcntlv; |
|
108 unsigned int reccntlv; |
|
109 char mutexname[64]; |
|
110 unsigned int masterno; |
|
111 } EC_RTDM_DRV_STRUCT; |
|
112 |
|
113 |
|
114 static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS]; |
|
115 |
|
116 |
|
117 /* import from ethercat */ |
|
118 ec_master_t *ecrt_attach_master(unsigned int master_index /**< Index of the master to request. */ |
|
119 ); |
|
120 |
|
121 // driver context struct: used for storing various information |
|
122 typedef struct _EC_RTDM_DRV_CONTEXT { |
|
123 int dev_id; |
|
124 EC_RTDM_DRV_STRUCT* pdrvstruc; |
|
125 } EC_RTDM_DRV_CONTEXT; |
|
126 |
|
127 |
|
128 |
|
129 /**********************************************************/ |
|
130 /* Utilities */ |
|
131 /**********************************************************/ |
|
132 |
|
133 static int _atoi(const char* text) |
|
134 { |
|
135 char b; |
|
136 int wd=-1; |
|
137 int nfak=1; |
|
138 |
|
139 wd=0; |
|
140 |
|
141 while ((*text==' ') || (*text=='\t')) text++; |
|
142 if (*text=='-') |
|
143 { |
|
144 nfak=-1; |
|
145 text++; |
|
146 } |
|
147 if (*text=='+') |
|
148 { |
|
149 text++; |
|
150 } |
|
151 while (*text!=0) |
|
152 { |
|
153 b = *text; |
|
154 |
|
155 if ( (b>='0') && (b<='9') ) |
|
156 { |
|
157 b=b-'0'; |
|
158 wd=wd*10+b; |
|
159 } |
|
160 text++; |
|
161 } |
|
162 return (nfak*wd); |
|
163 } |
|
164 |
|
165 |
|
166 /**********************************************************/ |
|
167 /* DRIVER sendcallback */ |
|
168 /**********************************************************/ |
|
169 void send_callback(void *cb_data) |
|
170 { |
|
171 EC_RTDM_DRV_STRUCT * pdrvstruc; |
|
172 |
|
173 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
|
174 if (pdrvstruc->master) |
|
175 { |
|
176 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
177 ecrt_master_send_ext(pdrvstruc->master); |
|
178 my_mutex_release(&pdrvstruc->masterlock); |
|
179 } |
|
180 } |
|
181 |
|
182 /*****************************************************************************/ |
|
183 |
|
184 void receive_callback(void *cb_data) |
|
185 { |
|
186 EC_RTDM_DRV_STRUCT * pdrvstruc; |
|
187 |
|
188 pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data; |
|
189 if (pdrvstruc->master) |
|
190 { |
|
191 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
192 ecrt_master_receive(pdrvstruc->master); |
|
193 my_mutex_release(&pdrvstruc->masterlock); |
|
194 } |
|
195 } |
|
196 |
|
197 |
|
198 |
|
199 |
|
200 void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc) |
|
201 { |
|
202 |
|
203 if (pdrvstruc->isattached) |
|
204 { |
|
205 EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n"); |
|
206 ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL); |
|
207 EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n"); |
|
208 my_mutex_delete(&pdrvstruc->masterlock); |
|
209 pdrvstruc->master = NULL; |
|
210 pdrvstruc->isattached=0; |
|
211 EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n"); |
|
212 } |
|
213 } |
|
214 |
|
215 |
|
216 |
|
217 |
|
218 /**********************************************************/ |
|
219 /* DRIVER OPEN */ |
|
220 /**********************************************************/ |
|
221 int ec_rtdm_open_rt(struct rtdm_dev_context *context, |
|
222 rtdm_user_info_t *user_info, |
|
223 int oflags) |
|
224 { |
|
225 EC_RTDM_DRV_CONTEXT* my_context; |
|
226 EC_RTDM_DRV_STRUCT * pdrvstruc; |
|
227 const char * p; |
|
228 int dev_no; |
|
229 unsigned int namelen; |
|
230 |
|
231 //int ret; |
|
232 int dev_id; |
|
233 |
|
234 // get the context for our driver - used to store driver info |
|
235 my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private; |
|
236 |
|
237 dev_no = -1; |
|
238 namelen = strlen(context->device->driver_name); |
|
239 p = &context->device->driver_name[namelen-1]; |
|
240 if (p!=&context->device->driver_name[0]) |
|
241 { |
|
242 while ((*p>='0') && (*p<='9')) |
|
243 { |
|
244 p--; |
|
245 if (p==&context->device->driver_name[0]) break; |
|
246 } |
|
247 dev_no=_atoi(p); |
|
248 if ((dev_no!=-1) && (dev_no<EC_RTDM_MAX_MASTERS)) |
|
249 { |
|
250 dev_id = context->device->device_id; |
|
251 pdrvstruc = (EC_RTDM_DRV_STRUCT*)&ec_rtdm_masterintf[dev_no]; |
|
252 |
|
253 my_context->dev_id = dev_id; |
|
254 my_context->pdrvstruc = pdrvstruc; |
|
255 |
|
256 // enable interrupt in RTDM |
|
257 return 0; |
|
258 } |
|
259 } |
|
260 EC_RTDM_GERR("open - Cannot detect master device no\n"); |
|
261 return -EFAULT; |
|
262 } |
|
263 |
|
264 /**********************************************************/ |
|
265 /* DRIVER CLOSE */ |
|
266 /**********************************************************/ |
|
267 int ec_rtdm_close_rt(struct rtdm_dev_context *context, |
|
268 rtdm_user_info_t *user_info) |
|
269 { |
|
270 EC_RTDM_DRV_CONTEXT* my_context; |
|
271 EC_RTDM_DRV_STRUCT * pdrvstruc; |
|
272 |
|
273 // get the context |
|
274 my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private; |
|
275 |
|
276 pdrvstruc = my_context->pdrvstruc; |
|
277 EC_RTDM_INFO(pdrvstruc->masterno,"close called!\n"); |
|
278 detach_master(pdrvstruc); |
|
279 return 0; |
|
280 |
|
281 } |
|
282 |
|
283 /**********************************************************/ |
|
284 /* DRIVER IOCTL */ |
|
285 /**********************************************************/ |
|
286 int ec_rtdm_ioctl_rt(struct rtdm_dev_context *context, |
|
287 rtdm_user_info_t *user_info, |
|
288 int request, |
|
289 void *arg) |
|
290 { |
|
291 EC_RTDM_DRV_CONTEXT* my_context; |
|
292 EC_RTDM_DRV_STRUCT * pdrvstruc; |
|
293 int ret; |
|
294 unsigned int l_ioctlvalue[]={0,0,0,0,0,0,0,0}; |
|
295 ec_domain_state_t ds; |
|
296 ec_master_state_t ms; |
|
297 uint64_t app_time; |
|
298 |
|
299 |
|
300 ret = 0; |
|
301 |
|
302 // get the context |
|
303 my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private; |
|
304 pdrvstruc = my_context->pdrvstruc; |
|
305 |
|
306 switch (request) { |
|
307 case EC_RTDM_MASTERSTATE: |
|
308 { |
|
309 if (!pdrvstruc->isattached) |
|
310 { |
|
311 return -EFAULT; |
|
312 } |
|
313 if (pdrvstruc->master) |
|
314 { |
|
315 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
316 |
|
317 ecrt_master_state(pdrvstruc->master, &ms); |
|
318 |
|
319 my_mutex_release(&pdrvstruc->masterlock); |
|
320 |
|
321 } |
|
322 if (rtdm_rw_user_ok(user_info, arg, sizeof(ms))) |
|
323 { |
|
324 // copy data to user |
|
325 if (rtdm_copy_to_user(user_info, arg, &ms,sizeof(ms))) |
|
326 { |
|
327 return -EFAULT; |
|
328 } |
|
329 } |
|
330 |
|
331 } |
|
332 break; |
|
333 case EC_RTDM_DOMAINSTATE: |
|
334 { |
|
335 if (!pdrvstruc->isattached) |
|
336 { |
|
337 return -EFAULT; |
|
338 } |
|
339 if (pdrvstruc->domain) |
|
340 { |
|
341 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
342 |
|
343 ecrt_domain_state(pdrvstruc->domain, &ds); |
|
344 |
|
345 my_mutex_release(&pdrvstruc->masterlock); |
|
346 } |
|
347 if (rtdm_rw_user_ok(user_info, arg, sizeof(ds))) |
|
348 { |
|
349 // copy data to user |
|
350 if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds))) |
|
351 { |
|
352 return -EFAULT; |
|
353 } |
|
354 } |
|
355 } |
|
356 break; |
|
357 case EC_RTDM_MASTER_RECEIVE: |
|
358 { |
|
359 if (pdrvstruc->isattached) |
|
360 { |
|
361 if (pdrvstruc->master) |
|
362 { |
|
363 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
364 ecrt_master_receive(pdrvstruc->master); |
|
365 pdrvstruc->reccnt++; |
|
366 my_mutex_release(&pdrvstruc->masterlock); |
|
367 } |
|
368 } |
|
369 } |
|
370 break; |
|
371 case EC_RTDM_DOMAIN_PROCESS: |
|
372 { |
|
373 if (pdrvstruc->isattached) |
|
374 { |
|
375 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
376 ecrt_domain_process(pdrvstruc->domain); |
|
377 my_mutex_release(&pdrvstruc->masterlock); |
|
378 } |
|
379 } |
|
380 break; |
|
381 case EC_RTDM_MASTER_SEND: |
|
382 { |
|
383 |
|
384 if (pdrvstruc->isattached) |
|
385 { |
|
386 if (pdrvstruc->master) |
|
387 { |
|
388 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
389 ecrt_master_send(pdrvstruc->master); |
|
390 pdrvstruc->sendcnt++; |
|
391 my_mutex_release(&pdrvstruc->masterlock); |
|
392 } |
|
393 } |
|
394 } |
|
395 break; |
|
396 case EC_RTDM_DOMAIN_QUEQUE: |
|
397 { |
|
398 if (pdrvstruc->isattached) |
|
399 { |
|
400 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
401 ecrt_domain_queue(pdrvstruc->domain); |
|
402 my_mutex_release(&pdrvstruc->masterlock); |
|
403 } |
|
404 } |
|
405 break; |
|
406 |
|
407 case EC_RTDM_MASTER_APP_TIME: |
|
408 { |
|
409 if (!pdrvstruc->isattached) |
|
410 { |
|
411 rtdm_printk("ERROR : No Master attached\n"); |
|
412 return -EFAULT; |
|
413 } |
|
414 if (rtdm_safe_copy_from_user(user_info, &app_time, arg, sizeof(app_time))) |
|
415 { |
|
416 rtdm_printk("ERROR : can't copy data to driver\n"); |
|
417 return -EFAULT; |
|
418 } |
|
419 |
|
420 if (pdrvstruc->master) |
|
421 { |
|
422 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
423 |
|
424 ecrt_master_application_time(pdrvstruc->master, app_time); |
|
425 my_mutex_release(&pdrvstruc->masterlock); |
|
426 |
|
427 } |
|
428 } |
|
429 break; |
|
430 case EC_RTDM_SYNC_REF_CLOCK: |
|
431 { |
|
432 if (!pdrvstruc->isattached) |
|
433 { |
|
434 return -EFAULT; |
|
435 } |
|
436 if (pdrvstruc->master) |
|
437 { |
|
438 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
439 |
|
440 ecrt_master_sync_reference_clock(pdrvstruc->master); |
|
441 |
|
442 my_mutex_release(&pdrvstruc->masterlock); |
|
443 |
|
444 } |
|
445 } |
|
446 break; |
|
447 case EC_RTDM_SYNC_SLAVE_CLOCK: |
|
448 { |
|
449 if (!pdrvstruc->isattached) |
|
450 { |
|
451 return -EFAULT; |
|
452 } |
|
453 if (pdrvstruc->master) |
|
454 { |
|
455 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
456 |
|
457 ecrt_master_sync_slave_clocks(pdrvstruc->master); |
|
458 |
|
459 my_mutex_release(&pdrvstruc->masterlock); |
|
460 |
|
461 } |
|
462 } |
|
463 break; |
|
464 case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE: |
|
465 { |
|
466 if (!pdrvstruc->isattached) |
|
467 { |
|
468 return -EFAULT; |
|
469 } |
|
470 if (pdrvstruc->master) |
|
471 { |
|
472 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
473 ecrt_master_sync_monitor_queue(pdrvstruc->master); |
|
474 my_mutex_release(&pdrvstruc->masterlock); |
|
475 } |
|
476 } |
|
477 break; |
|
478 case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS: |
|
479 { |
|
480 uint32_t ret; |
|
481 if (!pdrvstruc->isattached) |
|
482 { |
|
483 return -EFAULT; |
|
484 } |
|
485 if (pdrvstruc->master) |
|
486 { |
|
487 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE); |
|
488 ret = ecrt_master_sync_monitor_process(pdrvstruc->master); |
|
489 my_mutex_release(&pdrvstruc->masterlock); |
|
490 if (rtdm_safe_copy_to_user(user_info, arg, &ret, sizeof(ret))) |
|
491 { |
|
492 EC_RTDM_ERR(pdrvstruc->masterno,"copy to user param failed!\n"); |
|
493 ret=-EFAULT; |
|
494 } |
|
495 } |
|
496 } |
|
497 break; |
|
498 case EC_RTDM_MSTRATTACH: |
|
499 { |
|
500 unsigned int mstridx; |
|
501 |
|
502 mstridx = 0; |
|
503 ret = 0; |
|
504 |
|
505 EC_RTDM_INFO(pdrvstruc->masterno,"Master attach start!\n"); |
|
506 if (user_info) |
|
507 { |
|
508 if (rtdm_read_user_ok(user_info, arg, sizeof(unsigned int))) |
|
509 { |
|
510 if (rtdm_copy_from_user(user_info, &l_ioctlvalue[0], arg,sizeof(unsigned int))==0) |
|
511 { |
|
512 pdrvstruc->domain = (ec_domain_t*)l_ioctlvalue[0]; |
|
513 } |
|
514 else |
|
515 { |
|
516 EC_RTDM_ERR(pdrvstruc->masterno,"copy user param failed!\n"); |
|
517 ret=-EFAULT; |
|
518 } |
|
519 } |
|
520 else |
|
521 { |
|
522 EC_RTDM_ERR(pdrvstruc->masterno,"user parameter domain missing!\n"); |
|
523 ret=-EFAULT; |
|
524 } |
|
525 } |
|
526 if (ret!=0) |
|
527 { |
|
528 return ret; |
|
529 } |
|
530 |
|
531 if ( (pdrvstruc->master) && (pdrvstruc->isattached)) |
|
532 // master is allready attached |
|
533 { |
|
534 // master is allready attached |
|
535 EC_RTDM_ERR(pdrvstruc->masterno,"Master is allready attached!\n"); |
|
536 ret = -EFAULT; |
|
537 } |
|
538 else |
|
539 { |
|
540 //mstr=ecrt_request_master(0); |
|
541 mstridx = pdrvstruc->masterno; |
|
542 |
|
543 pdrvstruc->master=ecrt_attach_master(mstridx); |
|
544 |
|
545 if (pdrvstruc->master) |
|
546 { |
|
547 // Ok |
|
548 EC_RTDM_INFO(pdrvstruc->masterno,"Master searching for domain!\n"); |
|
549 pdrvstruc->domain = ecrt_master_find_domain(pdrvstruc->master,l_ioctlvalue[0]); |
|
550 if (!pdrvstruc->domain) |
|
551 { |
|
552 // |
|
553 EC_RTDM_ERR(pdrvstruc->masterno,"Cannot find domain from index %u!\n",l_ioctlvalue[0]); |
|
554 ret = -EFAULT; |
|
555 } |
|
556 else |
|
557 { |
|
558 |
|
559 // set device name |
|
560 snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno); |
|
561 EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]); |
|
562 my_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]); |
|
563 //ecrt_release_master(mstr); |
|
564 ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc); |
|
565 EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain); |
|
566 pdrvstruc->isattached=1; |
|
567 ret = 0; |
|
568 } |
|
569 |
|
570 } |
|
571 else |
|
572 { |
|
573 EC_RTDM_ERR(pdrvstruc->masterno,"Master attach failed!\n"); |
|
574 pdrvstruc->master = NULL; |
|
575 ret = -EFAULT; |
|
576 } |
|
577 } |
|
578 } |
|
579 break; |
|
580 default: |
|
581 ret = -ENOTTY; |
|
582 } |
|
583 return ret; |
|
584 } |
|
585 |
|
586 |
|
587 /**********************************************************/ |
|
588 /* DRIVER READ */ |
|
589 /**********************************************************/ |
|
590 int ec_rtdm_read_rt(struct rtdm_dev_context *context, |
|
591 rtdm_user_info_t *user_info, void *buf, size_t nbyte) |
|
592 { |
|
593 int ret; |
|
594 #if defined(USE_THIS) |
|
595 EC_RTDM_DRV_CONTEXT* my_context; |
|
596 char *out_pos; |
|
597 int dev_id; |
|
598 rtdm_toseq_t timeout_seq; |
|
599 int ret; |
|
600 |
|
601 out_pos = (char *)buf; |
|
602 |
|
603 my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private; |
|
604 |
|
605 // zero bytes requested ? return! |
|
606 if (nbyte == 0) |
|
607 return 0; |
|
608 |
|
609 // check if R/W actions to user-space are allowed |
|
610 if (user_info && !rtdm_rw_user_ok(user_info, buf, nbyte)) |
|
611 return -EFAULT; |
|
612 |
|
613 dev_id = my_context->dev_id; |
|
614 |
|
615 // in case we need to check if reading is allowed (locking) |
|
616 /* if (test_and_set_bit(0, &ctx->in_lock)) |
|
617 return -EBUSY; |
|
618 */ |
|
619 /* // if we need to do some stuff with preemption disabled: |
|
620 rtdm_lock_get_irqsave(&ctx->lock, lock_ctx); |
|
621 // stuff here |
|
622 rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx); |
|
623 */ |
|
624 |
|
625 // wait: if ctx->timeout = 0, it will block infintely until |
|
626 // rtdm_event_signal(&ctx->irq_event); is called from our |
|
627 // interrupt routine |
|
628 //ret = rtdm_event_timedwait(&ctx->irq_event, ctx->timeout, &timeout_seq); |
|
629 |
|
630 // now write the requested stuff to user-space |
|
631 if (rtdm_copy_to_user(user_info, out_pos, |
|
632 dummy_buffer, BUFSIZE) != 0) { |
|
633 ret = -EFAULT; |
|
634 } else { |
|
635 ret = BUFSIZE; |
|
636 } |
|
637 #else |
|
638 ret = -EFAULT; |
|
639 #endif |
|
640 return ret; |
|
641 } |
|
642 |
|
643 /**********************************************************/ |
|
644 /* DRIVER WRITE */ |
|
645 /**********************************************************/ |
|
646 int ec_rtdm_write_rt(struct rtdm_dev_context *context, |
|
647 rtdm_user_info_t *user_info, |
|
648 const void *buf, size_t nbyte) |
|
649 { |
|
650 int ret; |
|
651 |
|
652 #if defined(USE_THIS) |
|
653 int dev_id; |
|
654 char *in_pos = (char *)buf; |
|
655 |
|
656 EC_RTDM_DRV_CONTEXT* my_context; |
|
657 |
|
658 |
|
659 my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private; |
|
660 |
|
661 |
|
662 if (nbyte == 0) |
|
663 return 0; |
|
664 if (user_info && !rtdm_read_user_ok(user_info, buf, nbyte)) |
|
665 return -EFAULT; |
|
666 |
|
667 dev_id = my_context->dev_id; |
|
668 |
|
669 if (rtdm_copy_from_user(user_info, dummy_buffer, |
|
670 in_pos, BUFSIZE) != 0) { |
|
671 ret = -EFAULT; |
|
672 } else { |
|
673 ret = BUFSIZE; |
|
674 } |
|
675 #else |
|
676 ret = -EFAULT; |
|
677 #endif |
|
678 // used when it is atomic |
|
679 // rtdm_mutex_unlock(&ctx->out_lock); |
|
680 return ret; |
|
681 } |
|
682 |
|
683 /**********************************************************/ |
|
684 /* DRIVER OPERATIONS */ |
|
685 /**********************************************************/ |
|
686 |
|
687 // Template |
|
688 |
|
689 static struct rtdm_device ec_rtdm_device_t = { |
|
690 struct_version: RTDM_DEVICE_STRUCT_VER, |
|
691 |
|
692 device_flags: RTDM_NAMED_DEVICE, |
|
693 context_size: sizeof(EC_RTDM_DRV_CONTEXT), |
|
694 device_name: EC_RTDM_DEV_FILE_NAME, |
|
695 |
|
696 /* open and close functions are not real-time safe due kmalloc |
|
697 and kfree. If you do not use kmalloc and kfree, and you made |
|
698 sure that there is no syscall in the open/close handler, you |
|
699 can declare the open_rt and close_rt handler. |
|
700 */ |
|
701 open_rt: NULL, |
|
702 open_nrt: ec_rtdm_open_rt, |
|
703 |
|
704 ops: { |
|
705 close_rt: NULL, |
|
706 close_nrt: ec_rtdm_close_rt, |
|
707 |
|
708 ioctl_rt: ec_rtdm_ioctl_rt, |
|
709 ioctl_nrt: ec_rtdm_ioctl_rt, // rtdm_mmap_to_user is not RT safe |
|
710 |
|
711 read_rt: ec_rtdm_read_rt, |
|
712 read_nrt: NULL, |
|
713 |
|
714 write_rt: ec_rtdm_write_rt, |
|
715 write_nrt: NULL, |
|
716 |
|
717 recvmsg_rt: NULL, |
|
718 recvmsg_nrt: NULL, |
|
719 |
|
720 sendmsg_rt: NULL, |
|
721 sendmsg_nrt: NULL, |
|
722 }, |
|
723 |
|
724 device_class: RTDM_CLASS_EXPERIMENTAL, |
|
725 device_sub_class: 222, |
|
726 driver_name: EC_RTDM_DEV_FILE_NAME, |
|
727 driver_version: RTDM_DRIVER_VER(1,0,1), |
|
728 peripheral_name: EC_RTDM_DEV_FILE_NAME, |
|
729 provider_name: "EtherLab Community", |
|
730 // proc_name: ethcatrtdm_device.device_name, |
|
731 }; |
|
732 |
|
733 |
|
734 static struct rtdm_device ec_rtdm_devices[EC_RTDM_MAX_MASTERS]; |
|
735 |
|
736 |
|
737 /**********************************************************/ |
|
738 /* INIT DRIVER */ |
|
739 /**********************************************************/ |
|
740 int init_module(void) |
|
741 { |
|
742 unsigned int i; |
|
743 int ret; |
|
744 |
|
745 ret = 0; |
|
746 |
|
747 EC_RTDM_GINFO("Initlializing EtherCAT RTDM Interface to Igh EtherCAT Master\n"); |
|
748 memset(&ec_rtdm_masterintf[0],0,sizeof(ec_rtdm_masterintf)); |
|
749 for (i=0;( (i<EC_RTDM_MAX_MASTERS) && (ret==0) ) ;i++) |
|
750 { |
|
751 // master no to struct |
|
752 ec_rtdm_masterintf[i].masterno = i; |
|
753 // copy from template |
|
754 memcpy(&ec_rtdm_devices[i],&ec_rtdm_device_t,sizeof(ec_rtdm_devices[0])); |
|
755 |
|
756 // set device name |
|
757 snprintf(&ec_rtdm_devices[i].device_name[0],RTDM_MAX_DEVNAME_LEN,"%s%d",EC_RTDM_DEV_FILE_NAME,i); |
|
758 // set proc_name |
|
759 ec_rtdm_devices[i].proc_name = &ec_rtdm_devices[i].device_name[0]; |
|
760 ec_rtdm_devices[i].driver_name = &ec_rtdm_devices[i].device_name[0]; |
|
761 ec_rtdm_devices[i].peripheral_name = &ec_rtdm_devices[i].device_name[0]; |
|
762 |
|
763 EC_RTDM_GINFO("Registering device %s!\n",ec_rtdm_devices[i].driver_name); |
|
764 ret = rtdm_dev_register(&ec_rtdm_devices[i]); |
|
765 |
|
766 } |
|
767 if (ret!=0) |
|
768 { |
|
769 // register m |
|
770 EC_RTDM_GERR("Initialization of EtherCAT RTDM Interface failed\n"); |
|
771 } |
|
772 return ret; |
|
773 } |
|
774 |
|
775 /**********************************************************/ |
|
776 /* CLEANUP DRIVER */ |
|
777 /**********************************************************/ |
|
778 void cleanup_module(void) |
|
779 { |
|
780 unsigned int i; |
|
781 |
|
782 EC_RTDM_GINFO("Cleanup EtherCAT RTDM Interface \n"); |
|
783 for (i=0;i<EC_RTDM_MAX_MASTERS;i++) |
|
784 { |
|
785 if (ec_rtdm_masterintf[i].isattached) |
|
786 { |
|
787 detach_master(&ec_rtdm_masterintf[i]); |
|
788 } |
|
789 EC_RTDM_GINFO("Unregistering device %s!\n",ec_rtdm_devices[i].driver_name); |
|
790 rtdm_dev_unregister(&ec_rtdm_devices[i],1000); |
|
791 } |
|
792 } |
|
793 |
|
794 MODULE_LICENSE("GPL"); |
|
795 MODULE_DESCRIPTION("EtherCAT RTDM Interface"); |