1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH |
|
6 * |
|
7 * This file is part of the IgH EtherCAT Master. |
|
8 * |
|
9 * The IgH EtherCAT Master is free software; you can redistribute it |
|
10 * and/or modify it under the terms of the GNU General Public License |
|
11 * as published by the Free Software Foundation; either version 2 of the |
|
12 * License, or (at your option) any later version. |
|
13 * |
|
14 * The IgH EtherCAT Master is distributed in the hope that it will be |
|
15 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
17 * GNU General Public License for more details. |
|
18 * |
|
19 * You should have received a copy of the GNU General Public License |
|
20 * along with the IgH EtherCAT Master; if not, write to the Free Software |
|
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|
22 * |
|
23 * The right to use EtherCAT Technology is granted and comes free of |
|
24 * charge under condition of compatibility of product made by |
|
25 * Licensee. People intending to distribute/sell products based on the |
|
26 * code, have to sign an agreement to guarantee that products using |
|
27 * software based on IgH EtherCAT master stay compatible with the actual |
|
28 * EtherCAT specification (which are released themselves as an open |
|
29 * standard) as the (only) precondition to have the right to use EtherCAT |
|
30 * Technology, IP and trade marks. |
|
31 * |
|
32 *****************************************************************************/ |
|
33 |
|
34 /** |
|
35 \file |
|
36 Canopen-over-EtherCAT functions. |
|
37 */ |
|
38 |
|
39 /*****************************************************************************/ |
|
40 |
|
41 #include <linux/module.h> |
|
42 |
|
43 #include "canopen.h" |
|
44 #include "master.h" |
|
45 |
|
46 /*****************************************************************************/ |
|
47 |
|
48 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *); |
|
49 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *, |
|
50 char *); |
|
51 void ec_sdo_clear(struct kobject *); |
|
52 void ec_sdo_entry_clear(struct kobject *); |
|
53 |
|
54 /*****************************************************************************/ |
|
55 |
|
56 /** \cond */ |
|
57 |
|
58 EC_SYSFS_READ_ATTR(info); |
|
59 EC_SYSFS_READ_ATTR(value); |
|
60 |
|
61 static struct attribute *sdo_def_attrs[] = { |
|
62 &attr_info, |
|
63 NULL, |
|
64 }; |
|
65 |
|
66 static struct sysfs_ops sdo_sysfs_ops = { |
|
67 .show = &ec_show_sdo_attribute, |
|
68 .store = NULL |
|
69 }; |
|
70 |
|
71 static struct kobj_type ktype_ec_sdo = { |
|
72 .release = ec_sdo_clear, |
|
73 .sysfs_ops = &sdo_sysfs_ops, |
|
74 .default_attrs = sdo_def_attrs |
|
75 }; |
|
76 |
|
77 static struct attribute *sdo_entry_def_attrs[] = { |
|
78 &attr_info, |
|
79 &attr_value, |
|
80 NULL, |
|
81 }; |
|
82 |
|
83 static struct sysfs_ops sdo_entry_sysfs_ops = { |
|
84 .show = &ec_show_sdo_entry_attribute, |
|
85 .store = NULL |
|
86 }; |
|
87 |
|
88 static struct kobj_type ktype_ec_sdo_entry = { |
|
89 .release = ec_sdo_entry_clear, |
|
90 .sysfs_ops = &sdo_entry_sysfs_ops, |
|
91 .default_attrs = sdo_entry_def_attrs |
|
92 }; |
|
93 |
|
94 /** \endcond */ |
|
95 |
|
96 /*****************************************************************************/ |
|
97 |
|
98 /** |
|
99 Sdo constructor. |
|
100 */ |
|
101 |
|
102 int ec_sdo_init(ec_sdo_t *sdo, /**< Sdo */ |
|
103 uint16_t index, /**< Sdo index */ |
|
104 ec_slave_t *slave /**< parent slave */ |
|
105 ) |
|
106 { |
|
107 sdo->slave = slave; |
|
108 sdo->index = index; |
|
109 sdo->object_code = 0x00; |
|
110 sdo->name = NULL; |
|
111 sdo->subindices = 0; |
|
112 INIT_LIST_HEAD(&sdo->entries); |
|
113 |
|
114 // init kobject and add it to the hierarchy |
|
115 memset(&sdo->kobj, 0x00, sizeof(struct kobject)); |
|
116 kobject_init(&sdo->kobj); |
|
117 sdo->kobj.ktype = &ktype_ec_sdo; |
|
118 sdo->kobj.parent = &slave->sdo_kobj; |
|
119 if (kobject_set_name(&sdo->kobj, "%4X", sdo->index)) { |
|
120 EC_ERR("Failed to set kobj name.\n"); |
|
121 kobject_put(&sdo->kobj); |
|
122 return -1; |
|
123 } |
|
124 if (kobject_add(&sdo->kobj)) { |
|
125 EC_ERR("Failed to add Sdo kobject.\n"); |
|
126 kobject_put(&sdo->kobj); |
|
127 return -1; |
|
128 } |
|
129 |
|
130 return 0; |
|
131 } |
|
132 |
|
133 /*****************************************************************************/ |
|
134 |
|
135 /** |
|
136 Sdo destructor. |
|
137 Clears and frees an Sdo object. |
|
138 */ |
|
139 |
|
140 void ec_sdo_destroy(ec_sdo_t *sdo /**< Sdo */) |
|
141 { |
|
142 ec_sdo_entry_t *entry, *next; |
|
143 |
|
144 // free all entries |
|
145 list_for_each_entry_safe(entry, next, &sdo->entries, list) { |
|
146 list_del(&entry->list); |
|
147 ec_sdo_entry_destroy(entry); |
|
148 } |
|
149 |
|
150 // destroy self |
|
151 kobject_del(&sdo->kobj); |
|
152 kobject_put(&sdo->kobj); |
|
153 } |
|
154 |
|
155 /*****************************************************************************/ |
|
156 |
|
157 /** |
|
158 Clear and free Sdo. |
|
159 This method is called by the kobject, |
|
160 once there are no more references to it. |
|
161 */ |
|
162 |
|
163 void ec_sdo_clear(struct kobject *kobj /**< Sdo's kobject */) |
|
164 { |
|
165 ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj); |
|
166 |
|
167 if (sdo->name) kfree(sdo->name); |
|
168 |
|
169 kfree(sdo); |
|
170 } |
|
171 |
|
172 /*****************************************************************************/ |
|
173 |
|
174 /** |
|
175 * Get and Sdo entry from an Sdo via its subindex. |
|
176 * \return pointer to Sdo entry, or NULL. |
|
177 */ |
|
178 |
|
179 ec_sdo_entry_t *ec_sdo_get_entry( |
|
180 ec_sdo_t *sdo, /**< Sdo */ |
|
181 uint8_t subindex /**< entry subindex */ |
|
182 ) |
|
183 { |
|
184 ec_sdo_entry_t *entry; |
|
185 |
|
186 list_for_each_entry(entry, &sdo->entries, list) { |
|
187 if (entry->subindex != subindex) continue; |
|
188 return entry; |
|
189 } |
|
190 |
|
191 return NULL; |
|
192 } |
|
193 |
|
194 /*****************************************************************************/ |
|
195 |
|
196 /** |
|
197 * Print Sdo information to a buffer. |
|
198 * /return size of bytes written to buffer. |
|
199 */ |
|
200 |
|
201 ssize_t ec_sdo_info(ec_sdo_t *sdo, /**< Sdo */ |
|
202 char *buffer /**< target buffer */ |
|
203 ) |
|
204 { |
|
205 off_t off = 0; |
|
206 |
|
207 off += sprintf(buffer + off, "Index: 0x%04X\n", sdo->index); |
|
208 off += sprintf(buffer + off, "Name: %s\n", sdo->name ? sdo->name : ""); |
|
209 off += sprintf(buffer + off, "Subindices: %i\n", sdo->subindices); |
|
210 |
|
211 return off; |
|
212 } |
|
213 |
|
214 /*****************************************************************************/ |
|
215 |
|
216 /** |
|
217 * Show an Sdo as Sysfs attribute. |
|
218 * /return size of bytes written to buffer. |
|
219 */ |
|
220 |
|
221 ssize_t ec_show_sdo_attribute(struct kobject *kobj, /**< kobject */ |
|
222 struct attribute *attr, |
|
223 char *buffer |
|
224 ) |
|
225 { |
|
226 ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj); |
|
227 |
|
228 if (attr == &attr_info) { |
|
229 return ec_sdo_info(sdo, buffer); |
|
230 } |
|
231 |
|
232 return 0; |
|
233 } |
|
234 |
|
235 /*****************************************************************************/ |
|
236 |
|
237 /** |
|
238 Sdo entry constructor. |
|
239 */ |
|
240 |
|
241 int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< Sdo entry */ |
|
242 uint8_t subindex, /**< Sdo entry subindex */ |
|
243 ec_sdo_t *sdo /**< parent Sdo */ |
|
244 ) |
|
245 { |
|
246 entry->sdo = sdo; |
|
247 entry->subindex = subindex; |
|
248 entry->data_type = 0x0000; |
|
249 entry->bit_length = 0; |
|
250 entry->description = NULL; |
|
251 |
|
252 // init kobject and add it to the hierarchy |
|
253 memset(&entry->kobj, 0x00, sizeof(struct kobject)); |
|
254 kobject_init(&entry->kobj); |
|
255 entry->kobj.ktype = &ktype_ec_sdo_entry; |
|
256 entry->kobj.parent = &sdo->kobj; |
|
257 if (kobject_set_name(&entry->kobj, "%i", entry->subindex)) { |
|
258 EC_ERR("Failed to set kobj name.\n"); |
|
259 kobject_put(&entry->kobj); |
|
260 return -1; |
|
261 } |
|
262 if (kobject_add(&entry->kobj)) { |
|
263 EC_ERR("Failed to add entry kobject.\n"); |
|
264 kobject_put(&entry->kobj); |
|
265 return -1; |
|
266 } |
|
267 |
|
268 return 0; |
|
269 } |
|
270 |
|
271 /*****************************************************************************/ |
|
272 |
|
273 /** |
|
274 Sdo entry destructor. |
|
275 Clears and frees an Sdo entry object. |
|
276 */ |
|
277 |
|
278 void ec_sdo_entry_destroy(ec_sdo_entry_t *entry /**< Sdo entry */) |
|
279 { |
|
280 // destroy self |
|
281 kobject_del(&entry->kobj); |
|
282 kobject_put(&entry->kobj); |
|
283 } |
|
284 |
|
285 /*****************************************************************************/ |
|
286 |
|
287 /** |
|
288 Clear and free Sdo entry. |
|
289 This method is called by the kobject, |
|
290 once there are no more references to it. |
|
291 */ |
|
292 |
|
293 void ec_sdo_entry_clear(struct kobject *kobj /**< Sdo entry's kobject */) |
|
294 { |
|
295 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
|
296 |
|
297 if (entry->description) kfree(entry->description); |
|
298 |
|
299 kfree(entry); |
|
300 } |
|
301 |
|
302 /*****************************************************************************/ |
|
303 |
|
304 /** |
|
305 * Print Sdo entry information to a buffer. |
|
306 * \return number of bytes written. |
|
307 */ |
|
308 |
|
309 ssize_t ec_sdo_entry_info(ec_sdo_entry_t *entry, /**< Sdo entry */ |
|
310 char *buffer /**< target buffer */ |
|
311 ) |
|
312 { |
|
313 off_t off = 0; |
|
314 |
|
315 off += sprintf(buffer + off, "Subindex: 0x%02X\n", entry->subindex); |
|
316 off += sprintf(buffer + off, "Description: %s\n", |
|
317 entry->description ? entry->description : ""); |
|
318 off += sprintf(buffer + off, "Data type: 0x%04X\n", entry->data_type); |
|
319 off += sprintf(buffer + off, "Bit length: %i\n", entry->bit_length); |
|
320 |
|
321 return off; |
|
322 } |
|
323 |
|
324 /*****************************************************************************/ |
|
325 |
|
326 /** |
|
327 * Format entry data based on the CANopen data type and print it to a buffer. |
|
328 * \return number of bytes written. |
|
329 */ |
|
330 |
|
331 ssize_t ec_sdo_entry_format_data(ec_sdo_entry_t *entry, /**< Sdo entry */ |
|
332 ec_sdo_request_t *request, /**< Sdo request */ |
|
333 char *buffer /**< target buffer */ |
|
334 ) |
|
335 { |
|
336 off_t off = 0; |
|
337 unsigned int i; |
|
338 |
|
339 if (entry->data_type == 0x0002) { // int8 |
|
340 int8_t value; |
|
341 if (entry->bit_length != 8) |
|
342 goto not_fit; |
|
343 value = EC_READ_S8(request->data); |
|
344 off += sprintf(buffer + off, "%i (0x%02X)\n", value, value); |
|
345 } |
|
346 else if (entry->data_type == 0x0003) { // int16 |
|
347 int16_t value; |
|
348 if (entry->bit_length != 16) |
|
349 goto not_fit; |
|
350 value = EC_READ_S16(request->data); |
|
351 off += sprintf(buffer + off, "%i (0x%04X)\n", value, value); |
|
352 } |
|
353 else if (entry->data_type == 0x0004) { // int32 |
|
354 int32_t value; |
|
355 if (entry->bit_length != 32) |
|
356 goto not_fit; |
|
357 value = EC_READ_S16(request->data); |
|
358 off += sprintf(buffer + off, "%i (0x%08X)\n", value, value); |
|
359 } |
|
360 else if (entry->data_type == 0x0005) { // uint8 |
|
361 uint8_t value; |
|
362 if (entry->bit_length != 8) |
|
363 goto not_fit; |
|
364 value = EC_READ_U8(request->data); |
|
365 off += sprintf(buffer + off, "%u (0x%02X)\n", value, value); |
|
366 } |
|
367 else if (entry->data_type == 0x0006) { // uint16 |
|
368 uint16_t value; |
|
369 if (entry->bit_length != 16) |
|
370 goto not_fit; |
|
371 value = EC_READ_U16(request->data); |
|
372 off += sprintf(buffer + off, "%u (0x%04X)\n", value, value); |
|
373 } |
|
374 else if (entry->data_type == 0x0007) { // uint32 |
|
375 uint32_t value; |
|
376 if (entry->bit_length != 32) |
|
377 goto not_fit; |
|
378 value = EC_READ_U32(request->data); |
|
379 off += sprintf(buffer + off, "%i (0x%08X)\n", value, value); |
|
380 } |
|
381 else if (entry->data_type == 0x0009) { // string |
|
382 off += sprintf(buffer + off, "%s\n", request->data); |
|
383 } |
|
384 else { |
|
385 off += sprintf(buffer + off, "Unknown data type %04X. Data:\n", |
|
386 entry->data_type); |
|
387 goto raw_data; |
|
388 } |
|
389 return off; |
|
390 |
|
391 not_fit: |
|
392 off += sprintf(buffer + off, |
|
393 "Invalid bit length %u for data type 0x%04X. Data:\n", |
|
394 entry->bit_length, entry->data_type); |
|
395 raw_data: |
|
396 for (i = 0; i < request->size; i++) |
|
397 off += sprintf(buffer + off, "%02X (%c)\n", |
|
398 request->data[i], request->data[i]); |
|
399 return off; |
|
400 } |
|
401 |
|
402 /*****************************************************************************/ |
|
403 |
|
404 /** |
|
405 * Start Sdo entry reading. |
|
406 * This function blocks, until reading is finished, and is interruptible as |
|
407 * long as the master state machine has not begun with reading. |
|
408 * \return number of bytes written to buffer, or error code. |
|
409 */ |
|
410 |
|
411 ssize_t ec_sdo_entry_read_value(ec_sdo_entry_t *entry, /**< Sdo entry */ |
|
412 char *buffer /**< target buffer */ |
|
413 ) |
|
414 { |
|
415 ec_master_t *master = entry->sdo->slave->master; |
|
416 off_t off = 0; |
|
417 ec_sdo_request_t request; |
|
418 |
|
419 ec_sdo_request_init_read(&request, entry->sdo->slave, |
|
420 entry->sdo->index, entry->subindex); |
|
421 |
|
422 // schedule request. |
|
423 down(&master->sdo_sem); |
|
424 list_add_tail(&request.list, &master->sdo_requests); |
|
425 up(&master->sdo_sem); |
|
426 |
|
427 // wait for processing through FSM |
|
428 if (wait_event_interruptible(master->sdo_queue, |
|
429 request.state != EC_REQUEST_QUEUED)) { |
|
430 // interrupted by signal |
|
431 down(&master->sdo_sem); |
|
432 if (request.state == EC_REQUEST_QUEUED) { |
|
433 list_del(&request.list); |
|
434 up(&master->sdo_sem); |
|
435 return -EINTR; |
|
436 } |
|
437 // request already processing: interrupt not possible. |
|
438 up(&master->sdo_sem); |
|
439 } |
|
440 |
|
441 // wait until master FSM has finished processing |
|
442 wait_event(master->sdo_queue, request.state != EC_REQUEST_IN_PROGRESS); |
|
443 |
|
444 if (request.state != EC_REQUEST_COMPLETE) |
|
445 return -EIO; |
|
446 |
|
447 off += ec_sdo_entry_format_data(entry, &request, buffer); |
|
448 |
|
449 ec_sdo_request_clear(&request); |
|
450 return off; |
|
451 } |
|
452 |
|
453 /*****************************************************************************/ |
|
454 |
|
455 /** |
|
456 * Show an Sdo entry as Sysfs attribute. |
|
457 * /return size of bytes written to buffer. |
|
458 */ |
|
459 |
|
460 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */ |
|
461 struct attribute *attr, |
|
462 char *buffer |
|
463 ) |
|
464 { |
|
465 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
|
466 |
|
467 if (attr == &attr_info) { |
|
468 return ec_sdo_entry_info(entry, buffer); |
|
469 } |
|
470 else if (attr == &attr_value) { |
|
471 return ec_sdo_entry_read_value(entry, buffer); |
|
472 } |
|
473 |
|
474 return 0; |
|
475 } |
|
476 |
|
477 /*****************************************************************************/ |
|
478 |
|
479 /** |
|
480 Sdo request constructor. |
|
481 */ |
|
482 |
|
483 void ec_sdo_request_init_read( |
|
484 ec_sdo_request_t *req, /**< Sdo request */ |
|
485 ec_slave_t *slave, /**< Slave owning the Sdo. */ |
|
486 uint16_t index, /**< Sdo index. */ |
|
487 uint8_t subindex /**< Sdo subindex. */ |
|
488 ) |
|
489 { |
|
490 req->slave = slave; |
|
491 req->index = index; |
|
492 req->subindex = subindex; |
|
493 req->data = NULL; |
|
494 req->size = 0; |
|
495 req->state = EC_REQUEST_QUEUED; |
|
496 } |
|
497 |
|
498 /*****************************************************************************/ |
|
499 |
|
500 /** |
|
501 Sdo request destructor. |
|
502 */ |
|
503 |
|
504 void ec_sdo_request_clear(ec_sdo_request_t *req /**< Sdo request */) |
|
505 { |
|
506 if (req->data) kfree(req->data); |
|
507 } |
|
508 |
|
509 /*****************************************************************************/ |
|