|
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 EtherCAT slave configuration methods. |
|
37 */ |
|
38 |
|
39 /*****************************************************************************/ |
|
40 |
|
41 #include <linux/module.h> |
|
42 |
|
43 #include "globals.h" |
|
44 #include "master.h" |
|
45 |
|
46 #include "slave_config.h" |
|
47 |
|
48 /*****************************************************************************/ |
|
49 |
|
50 void ec_slave_config_clear(struct kobject *); |
|
51 ssize_t ec_show_slave_config_attribute(struct kobject *, struct attribute *, |
|
52 char *); |
|
53 |
|
54 /*****************************************************************************/ |
|
55 |
|
56 /** \cond */ |
|
57 |
|
58 EC_SYSFS_READ_ATTR(info); |
|
59 |
|
60 static struct attribute *def_attrs[] = { |
|
61 &attr_info, |
|
62 NULL, |
|
63 }; |
|
64 |
|
65 static struct sysfs_ops sysfs_ops = { |
|
66 .show = ec_show_slave_config_attribute |
|
67 }; |
|
68 |
|
69 static struct kobj_type ktype_ec_slave_config = { |
|
70 .release = ec_slave_config_clear, |
|
71 .sysfs_ops = &sysfs_ops, |
|
72 .default_attrs = def_attrs |
|
73 }; |
|
74 |
|
75 /** \endcond */ |
|
76 |
|
77 /*****************************************************************************/ |
|
78 |
|
79 /** Slave configuration constructor. |
|
80 * |
|
81 * See ecrt_master_slave_config() for the usage of the \a alias and \a |
|
82 * position parameters. |
|
83 * |
|
84 * \retval 0 Success. |
|
85 * \retval <0 Failure. |
|
86 */ |
|
87 int ec_slave_config_init(ec_slave_config_t *sc, /**< Slave configuration. */ |
|
88 ec_master_t *master, /**< EtherCAT master. */ |
|
89 uint16_t alias, /**< Slave alias. */ |
|
90 uint16_t position, /**< Slave position. */ |
|
91 uint32_t vendor_id, /**< Expected vendor ID. */ |
|
92 uint32_t product_code /**< Expected product code. */ |
|
93 ) |
|
94 { |
|
95 ec_direction_t dir; |
|
96 |
|
97 sc->master = master; |
|
98 sc->alias = alias; |
|
99 sc->position = position; |
|
100 sc->vendor_id = vendor_id; |
|
101 sc->product_code = product_code; |
|
102 sc->slave = NULL; |
|
103 |
|
104 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) |
|
105 ec_pdo_mapping_init(&sc->mapping[dir]); |
|
106 |
|
107 INIT_LIST_HEAD(&sc->sdo_configs); |
|
108 |
|
109 sc->used_fmmus = 0; |
|
110 |
|
111 // init kobject and add it to the hierarchy |
|
112 memset(&sc->kobj, 0x00, sizeof(struct kobject)); |
|
113 kobject_init(&sc->kobj); |
|
114 sc->kobj.ktype = &ktype_ec_slave_config; |
|
115 sc->kobj.parent = &master->kobj; |
|
116 if (kobject_set_name(&sc->kobj, "config-%u-%u", sc->alias, sc->position)) { |
|
117 EC_ERR("Failed to set kobject name for slave config %u:%u.\n", |
|
118 sc->alias, sc->position); |
|
119 goto out_put; |
|
120 } |
|
121 if (kobject_add(&sc->kobj)) { |
|
122 EC_ERR("Failed to add kobject for slave config %u:%u.\n", |
|
123 sc->alias, sc->position); |
|
124 goto out_put; |
|
125 } |
|
126 |
|
127 return 0; |
|
128 |
|
129 out_put: |
|
130 kobject_put(&sc->kobj); |
|
131 return -1; |
|
132 } |
|
133 |
|
134 /*****************************************************************************/ |
|
135 |
|
136 /** Slave configuration destructor. |
|
137 * |
|
138 * Clears and frees a slave configuration object. |
|
139 */ |
|
140 void ec_slave_config_destroy( |
|
141 ec_slave_config_t *sc /**< Slave configuration. */ |
|
142 ) |
|
143 { |
|
144 ec_slave_config_detach(sc); |
|
145 |
|
146 // destroy self |
|
147 kobject_del(&sc->kobj); |
|
148 kobject_put(&sc->kobj); |
|
149 } |
|
150 |
|
151 /*****************************************************************************/ |
|
152 |
|
153 /** Clear and free the slave configuration. |
|
154 * |
|
155 * This method is called by the kobject, once there are no more references to |
|
156 * it. |
|
157 */ |
|
158 void ec_slave_config_clear(struct kobject *kobj /**< kobject of the config. */) |
|
159 { |
|
160 ec_slave_config_t *sc; |
|
161 ec_direction_t dir; |
|
162 ec_sdo_data_t *sdodata, *next_sdodata; |
|
163 |
|
164 sc = container_of(kobj, ec_slave_config_t, kobj); |
|
165 |
|
166 // Free Pdo mappings |
|
167 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) |
|
168 ec_pdo_mapping_clear(&sc->mapping[dir]); |
|
169 |
|
170 // free all SDO configurations |
|
171 list_for_each_entry_safe(sdodata, next_sdodata, &sc->sdo_configs, list) { |
|
172 list_del(&sdodata->list); |
|
173 kfree(sdodata->data); |
|
174 kfree(sdodata); |
|
175 } |
|
176 |
|
177 /** \todo */ |
|
178 |
|
179 kfree(sc); |
|
180 } |
|
181 |
|
182 /*****************************************************************************/ |
|
183 |
|
184 /** Prepares an FMMU configuration. |
|
185 * |
|
186 * Configuration data for the FMMU is saved in the slave config structure and |
|
187 * is written to the slave during the configuration. The FMMU configuration |
|
188 * is done in a way, that the complete data range of the corresponding sync |
|
189 * manager is covered. Seperate FMMUs are configured for each domain. If the |
|
190 * FMMU configuration is already prepared, the function returns with success. |
|
191 * |
|
192 * \retval >=0 Logical offset address. |
|
193 * \retval -1 FMMU limit reached. |
|
194 */ |
|
195 int ec_slave_config_prepare_fmmu( |
|
196 ec_slave_config_t *sc, /**< Slave configuration. */ |
|
197 ec_domain_t *domain, /**< Domain. */ |
|
198 ec_direction_t dir /**< PDO direction. */ |
|
199 ) |
|
200 { |
|
201 unsigned int i; |
|
202 ec_fmmu_config_t *fmmu; |
|
203 |
|
204 // FMMU configuration already prepared? |
|
205 for (i = 0; i < sc->used_fmmus; i++) { |
|
206 fmmu = &sc->fmmu_configs[i]; |
|
207 if (fmmu->domain == domain && fmmu->dir == dir) |
|
208 return fmmu->logical_start_address; |
|
209 } |
|
210 |
|
211 if (sc->used_fmmus == EC_MAX_FMMUS) { |
|
212 EC_ERR("FMMU limit reached for slave configuration %u:%u!\n", |
|
213 sc->alias, sc->position); |
|
214 return -1; |
|
215 } |
|
216 |
|
217 fmmu = &sc->fmmu_configs[sc->used_fmmus++]; |
|
218 ec_fmmu_config_init(fmmu, sc, domain, dir); |
|
219 return fmmu->logical_start_address; |
|
220 } |
|
221 |
|
222 /*****************************************************************************/ |
|
223 |
|
224 /** Registers a Pdo entry. |
|
225 * |
|
226 * Searches the mapping and the Pdo configurations for the given Pdo entry. If |
|
227 * found, the curresponding sync manager/FMMU is added to the domain and the |
|
228 * offset of the Pdo entry's data in the domain process data is returned. |
|
229 * |
|
230 * \retval >=0 Offset of the Pdo entry's process data. |
|
231 * \retval -1 Pdo entry not found. |
|
232 * \retval -2 Failed to register Pdo entry. |
|
233 */ |
|
234 int ec_slave_config_reg_pdo_entry( |
|
235 ec_slave_config_t *sc, /**< Slave configuration. */ |
|
236 ec_domain_t *domain, /**< Domain. */ |
|
237 uint16_t index, /**< Index of Pdo entry to register. */ |
|
238 uint8_t subindex /**< Subindex of Pdo entry to register. */ |
|
239 ) |
|
240 { |
|
241 ec_direction_t dir; |
|
242 ec_pdo_mapping_t *map; |
|
243 unsigned int bit_offset, byte_offset; |
|
244 ec_pdo_t *pdo; |
|
245 ec_pdo_entry_t *entry; |
|
246 int ret; |
|
247 |
|
248 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { |
|
249 map = &sc->mapping[dir]; |
|
250 bit_offset = 0; |
|
251 list_for_each_entry(pdo, &map->pdos, list) { |
|
252 list_for_each_entry(entry, &pdo->entries, list) { |
|
253 if (entry->index != index || entry->subindex != subindex) { |
|
254 bit_offset += entry->bit_length; |
|
255 } else { |
|
256 goto found; |
|
257 } |
|
258 } |
|
259 } |
|
260 } |
|
261 |
|
262 EC_ERR("PDO entry 0x%04X:%u is not mapped in slave config %u:%u.\n", |
|
263 index, subindex, sc->alias, sc->position); |
|
264 return -1; |
|
265 |
|
266 found: |
|
267 byte_offset = bit_offset / 8; |
|
268 if ((ret = ec_slave_config_prepare_fmmu(sc, domain, dir)) < 0) |
|
269 return -2; |
|
270 return ret + byte_offset; |
|
271 } |
|
272 |
|
273 /*****************************************************************************/ |
|
274 |
|
275 /** Outputs all information about a certain slave configuration. |
|
276 */ |
|
277 ssize_t ec_slave_config_info( |
|
278 const ec_slave_config_t *sc, /**< Slave configuration. */ |
|
279 char *buffer /**< Output buffer */ |
|
280 ) |
|
281 { |
|
282 char *buf = buffer; |
|
283 ec_direction_t dir; |
|
284 const ec_pdo_mapping_t *map; |
|
285 const ec_pdo_t *pdo; |
|
286 const ec_pdo_entry_t *entry; |
|
287 char str[20]; |
|
288 ec_sdo_data_t *sdodata; |
|
289 |
|
290 buf += sprintf(buf, "Alias: 0x%04X (%u)\n", sc->alias, sc->alias); |
|
291 buf += sprintf(buf, "Position: %u\n", sc->position); |
|
292 |
|
293 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { |
|
294 map = &sc->mapping[dir]; |
|
295 |
|
296 if (!list_empty(&map->pdos)) { |
|
297 buf += sprintf(buf, "%s mapping:\n", |
|
298 dir == EC_DIR_OUTPUT ? "Output" : "Input"); |
|
299 |
|
300 list_for_each_entry(pdo, &map->pdos, list) { |
|
301 buf += sprintf(buf, " %s 0x%04X \"%s\"\n", |
|
302 pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo", |
|
303 pdo->index, pdo->name ? pdo->name : "???"); |
|
304 |
|
305 list_for_each_entry(entry, &pdo->entries, list) { |
|
306 buf += sprintf(buf, " 0x%04X:%X \"%s\", %u bit\n", |
|
307 entry->index, entry->subindex, |
|
308 entry->name ? entry->name : "???", |
|
309 entry->bit_length); |
|
310 } |
|
311 } |
|
312 } |
|
313 } |
|
314 |
|
315 // type-cast to avoid warnings on some compilers |
|
316 if (!list_empty((struct list_head *) &sc->sdo_configs)) { |
|
317 buf += sprintf(buf, "\nSDO configurations:\n"); |
|
318 |
|
319 list_for_each_entry(sdodata, &sc->sdo_configs, list) { |
|
320 switch (sdodata->size) { |
|
321 case 1: sprintf(str, "%u", EC_READ_U8(sdodata->data)); break; |
|
322 case 2: sprintf(str, "%u", EC_READ_U16(sdodata->data)); break; |
|
323 case 4: sprintf(str, "%u", EC_READ_U32(sdodata->data)); break; |
|
324 default: sprintf(str, "(invalid size)"); break; |
|
325 } |
|
326 buf += sprintf(buf, " 0x%04X:%-3i -> %s\n", |
|
327 sdodata->index, sdodata->subindex, str); |
|
328 } |
|
329 buf += sprintf(buf, "\n"); |
|
330 } |
|
331 |
|
332 return buf - buffer; |
|
333 } |
|
334 |
|
335 /*****************************************************************************/ |
|
336 |
|
337 /** Formats attribute data for SysFS read access. |
|
338 * |
|
339 * \return number of bytes to read |
|
340 */ |
|
341 ssize_t ec_show_slave_config_attribute( |
|
342 struct kobject *kobj, /**< Slave configuration's kobject */ |
|
343 struct attribute *attr, /**< Requested attribute. */ |
|
344 char *buffer /**< Memory to store data. */ |
|
345 ) |
|
346 { |
|
347 ec_slave_config_t *sc = container_of(kobj, ec_slave_config_t, kobj); |
|
348 |
|
349 if (attr == &attr_info) { |
|
350 return ec_slave_config_info(sc, buffer); |
|
351 } |
|
352 |
|
353 return 0; |
|
354 } |
|
355 |
|
356 /*****************************************************************************/ |
|
357 |
|
358 /** Adds an SDO configuration. |
|
359 */ |
|
360 int ec_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
|
361 uint8_t subindex, const uint8_t *data, size_t size) |
|
362 { |
|
363 ec_slave_t *slave = sc->slave; |
|
364 ec_sdo_data_t *sdodata; |
|
365 |
|
366 if (slave && !(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
|
367 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
|
368 return -1; |
|
369 } |
|
370 |
|
371 if (!(sdodata = (ec_sdo_data_t *) |
|
372 kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) { |
|
373 EC_ERR("Failed to allocate memory for SDO configuration object!\n"); |
|
374 return -1; |
|
375 } |
|
376 |
|
377 if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { |
|
378 EC_ERR("Failed to allocate memory for SDO configuration data!\n"); |
|
379 kfree(sdodata); |
|
380 return -1; |
|
381 } |
|
382 |
|
383 sdodata->index = index; |
|
384 sdodata->subindex = subindex; |
|
385 memcpy(sdodata->data, data, size); |
|
386 sdodata->size = size; |
|
387 |
|
388 list_add_tail(&sdodata->list, &sc->sdo_configs); |
|
389 return 0; |
|
390 } |
|
391 |
|
392 /*****************************************************************************/ |
|
393 |
|
394 /** Attaches the configuration to the addressed slave object. |
|
395 * |
|
396 * \retval 0 Success. |
|
397 * \retval -1 Slave not found. |
|
398 * \retval -2 Slave already configured. |
|
399 * \retval -3 Invalid slave type found at the given position. |
|
400 */ |
|
401 int ec_slave_config_attach( |
|
402 ec_slave_config_t *sc /**< Slave configuration. */ |
|
403 ) |
|
404 { |
|
405 ec_slave_t *slave; |
|
406 unsigned int alias_found = 0, relative_position = 0; |
|
407 |
|
408 if (sc->slave) |
|
409 return 0; // already attached |
|
410 |
|
411 list_for_each_entry(slave, &sc->master->slaves, list) { |
|
412 if (!alias_found) { |
|
413 if (sc->alias && slave->sii_alias != sc->alias) |
|
414 continue; |
|
415 alias_found = 1; |
|
416 relative_position = 0; |
|
417 } |
|
418 if (relative_position == sc->position) |
|
419 goto found; |
|
420 relative_position++; |
|
421 } |
|
422 |
|
423 EC_ERR("Failed to find slave for configuration %u:%u.\n", |
|
424 sc->alias, sc->position); |
|
425 return -1; |
|
426 |
|
427 found: |
|
428 if (slave->config) { |
|
429 EC_ERR("Failed to attach slave configuration %u:%u. Slave %u" |
|
430 " already has a configuration!\n", sc->alias, |
|
431 sc->position, slave->ring_position); |
|
432 return -2; |
|
433 } |
|
434 if (slave->sii_vendor_id != sc->vendor_id |
|
435 || slave->sii_product_code != sc->product_code) { |
|
436 EC_ERR("Slave %u has an invalid type (0x%08X/0x%08X) for" |
|
437 " configuration %u:%u (0x%08X/0x%08X).\n", |
|
438 slave->ring_position, slave->sii_vendor_id, |
|
439 slave->sii_product_code, sc->alias, sc->position, |
|
440 sc->vendor_id, sc->product_code); |
|
441 return -3; |
|
442 } |
|
443 |
|
444 // attach slave |
|
445 slave->config = sc; |
|
446 sc->slave = slave; |
|
447 |
|
448 ec_slave_request_state(slave, EC_SLAVE_STATE_OP); |
|
449 |
|
450 return 0; |
|
451 } |
|
452 |
|
453 /*****************************************************************************/ |
|
454 |
|
455 /** Detaches the configuration from a slave object. |
|
456 */ |
|
457 void ec_slave_config_detach( |
|
458 ec_slave_config_t *sc /**< Slave configuration. */ |
|
459 ) |
|
460 { |
|
461 if (sc->slave) { |
|
462 sc->slave->config = NULL; |
|
463 sc->slave = NULL; |
|
464 } |
|
465 } |
|
466 |
|
467 /*****************************************************************************/ |
|
468 |
|
469 /** Loads the default mapping from the slave object. |
|
470 */ |
|
471 void ec_slave_config_load_default_mapping(ec_slave_config_t *sc) |
|
472 { |
|
473 ec_direction_t dir; |
|
474 ec_pdo_mapping_t *map; |
|
475 ec_sync_t *sync; |
|
476 |
|
477 if (!sc->slave) |
|
478 return; |
|
479 |
|
480 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { |
|
481 map = &sc->mapping[dir]; |
|
482 if (!(sync = ec_slave_get_pdo_sync(sc->slave, dir))) |
|
483 continue; |
|
484 ec_pdo_mapping_copy(map, &sync->mapping); |
|
485 } |
|
486 } |
|
487 |
|
488 /****************************************************************************** |
|
489 * Realtime interface |
|
490 *****************************************************************************/ |
|
491 |
|
492 int ecrt_slave_config_mapping(ec_slave_config_t *sc, unsigned int n_entries, |
|
493 const ec_pdo_info_t pdo_infos[]) |
|
494 { |
|
495 unsigned int i; |
|
496 |
|
497 for (i = 0; i < n_entries; i++) |
|
498 if (ec_pdo_mapping_add_pdo_info(&sc->mapping[pdo_infos[i].dir], |
|
499 &pdo_infos[i], sc)) |
|
500 return -1; |
|
501 |
|
502 return 0; |
|
503 } |
|
504 |
|
505 /*****************************************************************************/ |
|
506 |
|
507 int ecrt_slave_config_sdo8(ec_slave_config_t *slave, uint16_t index, |
|
508 uint8_t subindex, uint8_t value) |
|
509 { |
|
510 uint8_t data[1]; |
|
511 EC_WRITE_U8(data, value); |
|
512 return ec_slave_config_sdo(slave, index, subindex, data, 1); |
|
513 } |
|
514 |
|
515 /*****************************************************************************/ |
|
516 |
|
517 int ecrt_slave_config_sdo16(ec_slave_config_t *slave, uint16_t index, |
|
518 uint8_t subindex, uint16_t value) |
|
519 { |
|
520 uint8_t data[2]; |
|
521 EC_WRITE_U16(data, value); |
|
522 return ec_slave_config_sdo(slave, index, subindex, data, 2); |
|
523 } |
|
524 |
|
525 /*****************************************************************************/ |
|
526 |
|
527 int ecrt_slave_config_sdo32(ec_slave_config_t *slave, uint16_t index, |
|
528 uint8_t subindex, uint32_t value) |
|
529 { |
|
530 uint8_t data[4]; |
|
531 EC_WRITE_U32(data, value); |
|
532 return ec_slave_config_sdo(slave, index, subindex, data, 4); |
|
533 } |
|
534 |
|
535 /*****************************************************************************/ |
|
536 |
|
537 /** \cond */ |
|
538 |
|
539 EXPORT_SYMBOL(ecrt_slave_config_mapping); |
|
540 EXPORT_SYMBOL(ecrt_slave_config_sdo8); |
|
541 EXPORT_SYMBOL(ecrt_slave_config_sdo16); |
|
542 EXPORT_SYMBOL(ecrt_slave_config_sdo32); |
|
543 |
|
544 /** \endcond */ |
|
545 |
|
546 /*****************************************************************************/ |