1 /***************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 ****************************************************************************/ |
|
6 |
|
7 #include <sys/types.h> |
|
8 #include <sys/stat.h> |
|
9 #include <fcntl.h> |
|
10 #include <errno.h> |
|
11 #include <sys/ioctl.h> |
|
12 |
|
13 #include <iostream> |
|
14 #include <iomanip> |
|
15 #include <sstream> |
|
16 #include <fstream> |
|
17 #include <cctype> // toupper() |
|
18 #include <list> |
|
19 using namespace std; |
|
20 |
|
21 #include "Master.h" |
|
22 |
|
23 #define swap16(x) \ |
|
24 ((uint16_t)( \ |
|
25 (((uint16_t)(x) & 0x00ffU) << 8) | \ |
|
26 (((uint16_t)(x) & 0xff00U) >> 8) )) |
|
27 #define swap32(x) \ |
|
28 ((uint32_t)( \ |
|
29 (((uint32_t)(x) & 0x000000ffUL) << 24) | \ |
|
30 (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ |
|
31 (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ |
|
32 (((uint32_t)(x) & 0xff000000UL) >> 24) )) |
|
33 |
|
34 #if __BYTE_ORDER == __LITTLE_ENDIAN |
|
35 |
|
36 #define le16tocpu(x) x |
|
37 #define le32tocpu(x) x |
|
38 |
|
39 #define cputole16(x) x |
|
40 #define cputole32(x) x |
|
41 |
|
42 #elif __BYTE_ORDER == __BIG_ENDIAN |
|
43 |
|
44 #define le16tocpu(x) swap16(x) |
|
45 #define le32tocpu(x) swap32(x) |
|
46 |
|
47 #define cputole16(x) swap16(x) |
|
48 #define cputole32(x) swap32(x) |
|
49 |
|
50 #endif |
|
51 |
|
52 /****************************************************************************/ |
|
53 |
|
54 struct CoEDataType { |
|
55 const char *name; |
|
56 uint16_t coeCode; |
|
57 unsigned int byteSize; |
|
58 }; |
|
59 |
|
60 static const CoEDataType dataTypes[] = { |
|
61 {"int8", 0x0002, 1}, |
|
62 {"int16", 0x0003, 2}, |
|
63 {"int32", 0x0004, 4}, |
|
64 {"uint8", 0x0005, 1}, |
|
65 {"uint16", 0x0006, 2}, |
|
66 {"uint32", 0x0007, 4}, |
|
67 {"string", 0x0009, 0}, |
|
68 {"raw", 0xffff, 0}, |
|
69 {} |
|
70 }; |
|
71 |
|
72 /****************************************************************************/ |
|
73 |
|
74 const CoEDataType *findDataType(const string &str) |
|
75 { |
|
76 const CoEDataType *d; |
|
77 |
|
78 for (d = dataTypes; d->name; d++) |
|
79 if (str == d->name) |
|
80 return d; |
|
81 |
|
82 return NULL; |
|
83 } |
|
84 |
|
85 /****************************************************************************/ |
|
86 |
|
87 const CoEDataType *findDataType(uint16_t code) |
|
88 { |
|
89 const CoEDataType *d; |
|
90 |
|
91 for (d = dataTypes; d->name; d++) |
|
92 if (code == d->coeCode) |
|
93 return d; |
|
94 |
|
95 return NULL; |
|
96 } |
|
97 |
|
98 /****************************************************************************/ |
|
99 |
|
100 Master::Master() |
|
101 { |
|
102 index = 0; |
|
103 verbosity = Normal; |
|
104 fd = -1; |
|
105 } |
|
106 |
|
107 /****************************************************************************/ |
|
108 |
|
109 Master::~Master() |
|
110 { |
|
111 close(); |
|
112 } |
|
113 |
|
114 /****************************************************************************/ |
|
115 |
|
116 void Master::setIndex(unsigned int i) |
|
117 { |
|
118 index = i; |
|
119 } |
|
120 |
|
121 /****************************************************************************/ |
|
122 |
|
123 void Master::setVerbosity(Verbosity v) |
|
124 { |
|
125 verbosity = v; |
|
126 } |
|
127 |
|
128 /*****************************************************************************/ |
|
129 |
|
130 /** |
|
131 * Writes the Secondary slave address (alias) to the slave's SII. |
|
132 */ |
|
133 void Master::writeAlias( |
|
134 int slavePosition, |
|
135 bool force, |
|
136 const vector<string> &commandArgs |
|
137 ) |
|
138 { |
|
139 uint16_t alias; |
|
140 stringstream err, strAlias; |
|
141 int number; |
|
142 |
|
143 if (commandArgs.size() != 1) { |
|
144 stringstream err; |
|
145 err << "'alias' takes exactly one argument!"; |
|
146 throw MasterException(err.str()); |
|
147 } |
|
148 |
|
149 strAlias << commandArgs[0]; |
|
150 strAlias |
|
151 >> resetiosflags(ios::basefield) // guess base from prefix |
|
152 >> number; |
|
153 if (strAlias.fail() || number < 0x0000 || number > 0xffff) { |
|
154 err << "Invalid alias '" << commandArgs[0] << "'!"; |
|
155 throw MasterException(err.str()); |
|
156 } |
|
157 alias = number; |
|
158 |
|
159 if (slavePosition == -1) { |
|
160 unsigned int numSlaves, i; |
|
161 |
|
162 if (!force) { |
|
163 err << "This will write the alias addresses of all slaves to " |
|
164 << alias << "! Please specify --force to proceed."; |
|
165 throw MasterException(err.str()); |
|
166 } |
|
167 |
|
168 open(ReadWrite); |
|
169 numSlaves = slaveCount(); |
|
170 |
|
171 for (i = 0; i < numSlaves; i++) { |
|
172 writeSlaveAlias(i, alias); |
|
173 } |
|
174 } else { |
|
175 open(ReadWrite); |
|
176 writeSlaveAlias(slavePosition, alias); |
|
177 } |
|
178 } |
|
179 |
|
180 /*****************************************************************************/ |
|
181 |
|
182 bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b) |
|
183 { |
|
184 return a.alias < b.alias |
|
185 || (a.alias == b.alias && a.position < b.position); |
|
186 } |
|
187 |
|
188 /** Lists the bus configuration. |
|
189 */ |
|
190 void Master::showConfigs() |
|
191 { |
|
192 ec_ioctl_master_t master; |
|
193 unsigned int i; |
|
194 ec_ioctl_config_t config; |
|
195 ConfigList configList; |
|
196 |
|
197 open(Read); |
|
198 getMaster(&master); |
|
199 |
|
200 for (i = 0; i < master.config_count; i++) { |
|
201 getConfig(&config, i); |
|
202 configList.push_back(config); |
|
203 } |
|
204 |
|
205 configList.sort(); |
|
206 |
|
207 if (verbosity == Verbose) { |
|
208 showDetailedConfigs(configList); |
|
209 } else { |
|
210 listConfigs(configList); |
|
211 } |
|
212 } |
|
213 |
|
214 /****************************************************************************/ |
|
215 |
|
216 void Master::outputData(int domainIndex) |
|
217 { |
|
218 open(Read); |
|
219 |
|
220 if (domainIndex == -1) { |
|
221 unsigned int i; |
|
222 ec_ioctl_master_t master; |
|
223 |
|
224 getMaster(&master); |
|
225 |
|
226 for (i = 0; i < master.domain_count; i++) { |
|
227 outputDomainData(i); |
|
228 } |
|
229 } else { |
|
230 outputDomainData(domainIndex); |
|
231 } |
|
232 } |
|
233 |
|
234 /****************************************************************************/ |
|
235 |
|
236 void Master::setDebug(const vector<string> &commandArgs) |
|
237 { |
|
238 stringstream str; |
|
239 int debugLevel; |
|
240 |
|
241 if (commandArgs.size() != 1) { |
|
242 stringstream err; |
|
243 err << "'debug' takes exactly one argument!"; |
|
244 throw MasterException(err.str()); |
|
245 } |
|
246 |
|
247 str << commandArgs[0]; |
|
248 str >> resetiosflags(ios::basefield) // guess base from prefix |
|
249 >> debugLevel; |
|
250 |
|
251 if (str.fail()) { |
|
252 stringstream err; |
|
253 err << "Invalid debug level '" << commandArgs[0] << "'!"; |
|
254 throw MasterException(err.str()); |
|
255 } |
|
256 |
|
257 open(ReadWrite); |
|
258 |
|
259 if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) { |
|
260 stringstream err; |
|
261 err << "Failed to set debug level: " << strerror(errno); |
|
262 throw MasterException(err.str()); |
|
263 } |
|
264 } |
|
265 |
|
266 /****************************************************************************/ |
|
267 |
|
268 void Master::showDomains(int domainIndex) |
|
269 { |
|
270 open(Read); |
|
271 |
|
272 if (domainIndex == -1) { |
|
273 unsigned int i; |
|
274 ec_ioctl_master_t master; |
|
275 |
|
276 getMaster(&master); |
|
277 |
|
278 for (i = 0; i < master.domain_count; i++) { |
|
279 showDomain(i); |
|
280 } |
|
281 } else { |
|
282 showDomain(domainIndex); |
|
283 } |
|
284 } |
|
285 |
|
286 /****************************************************************************/ |
|
287 |
|
288 void Master::showMaster() |
|
289 { |
|
290 ec_ioctl_master_t data; |
|
291 stringstream err; |
|
292 unsigned int i; |
|
293 |
|
294 open(Read); |
|
295 getMaster(&data); |
|
296 |
|
297 cout |
|
298 << "Master" << index << endl |
|
299 << " Phase: "; |
|
300 |
|
301 switch (data.phase) { |
|
302 case 0: cout << "Waiting for device..."; break; |
|
303 case 1: cout << "Idle"; break; |
|
304 case 2: cout << "Operation"; break; |
|
305 default: |
|
306 err << "Invalid master phase " << data.phase; |
|
307 throw MasterException(err.str()); |
|
308 } |
|
309 |
|
310 cout << endl |
|
311 << " Slaves: " << data.slave_count << endl; |
|
312 |
|
313 for (i = 0; i < 2; i++) { |
|
314 cout << " Device" << i << ": "; |
|
315 if (data.devices[i].address[0] == 0x00 |
|
316 && data.devices[i].address[1] == 0x00 |
|
317 && data.devices[i].address[2] == 0x00 |
|
318 && data.devices[i].address[3] == 0x00 |
|
319 && data.devices[i].address[4] == 0x00 |
|
320 && data.devices[i].address[5] == 0x00) { |
|
321 cout << "None."; |
|
322 } else { |
|
323 cout << hex << setfill('0') |
|
324 << setw(2) << (unsigned int) data.devices[i].address[0] << ":" |
|
325 << setw(2) << (unsigned int) data.devices[i].address[1] << ":" |
|
326 << setw(2) << (unsigned int) data.devices[i].address[2] << ":" |
|
327 << setw(2) << (unsigned int) data.devices[i].address[3] << ":" |
|
328 << setw(2) << (unsigned int) data.devices[i].address[4] << ":" |
|
329 << setw(2) << (unsigned int) data.devices[i].address[5] << " (" |
|
330 << (data.devices[i].attached ? "attached" : "waiting...") |
|
331 << ")" << endl << dec |
|
332 << " Tx count: " << data.devices[i].tx_count << endl |
|
333 << " Rx count: " << data.devices[i].rx_count; |
|
334 } |
|
335 cout << endl; |
|
336 } |
|
337 } |
|
338 |
|
339 /****************************************************************************/ |
|
340 |
|
341 void Master::listPdos(int slavePosition) |
|
342 { |
|
343 open(Read); |
|
344 |
|
345 if (slavePosition == -1) { |
|
346 unsigned int numSlaves = slaveCount(), i; |
|
347 |
|
348 for (i = 0; i < numSlaves; i++) { |
|
349 listSlavePdos(i, true); |
|
350 } |
|
351 } else { |
|
352 listSlavePdos(slavePosition, false); |
|
353 } |
|
354 } |
|
355 |
|
356 /****************************************************************************/ |
|
357 |
|
358 void Master::listSdos(int slavePosition) |
|
359 { |
|
360 open(Read); |
|
361 |
|
362 if (slavePosition == -1) { |
|
363 unsigned int numSlaves = slaveCount(), i; |
|
364 |
|
365 for (i = 0; i < numSlaves; i++) { |
|
366 listSlaveSdos(i, true); |
|
367 } |
|
368 } else { |
|
369 listSlaveSdos(slavePosition, false); |
|
370 } |
|
371 } |
|
372 |
|
373 /****************************************************************************/ |
|
374 |
|
375 void Master::sdoDownload( |
|
376 int slavePosition, |
|
377 const string &dataTypeStr, |
|
378 const vector<string> &commandArgs |
|
379 ) |
|
380 { |
|
381 stringstream strIndex, strSubIndex, strValue, err; |
|
382 ec_ioctl_slave_sdo_download_t data; |
|
383 unsigned int number; |
|
384 const CoEDataType *dataType = NULL; |
|
385 |
|
386 if (slavePosition < 0) { |
|
387 err << "'sdo_download' requires a slave! Please specify --slave."; |
|
388 throw MasterException(err.str()); |
|
389 } |
|
390 data.slave_position = slavePosition; |
|
391 |
|
392 if (commandArgs.size() != 3) { |
|
393 err << "'sdo_download' takes 3 arguments!"; |
|
394 throw MasterException(err.str()); |
|
395 } |
|
396 |
|
397 strIndex << commandArgs[0]; |
|
398 strIndex |
|
399 >> resetiosflags(ios::basefield) // guess base from prefix |
|
400 >> data.sdo_index; |
|
401 if (strIndex.fail()) { |
|
402 err << "Invalid Sdo index '" << commandArgs[0] << "'!"; |
|
403 throw MasterException(err.str()); |
|
404 } |
|
405 |
|
406 strSubIndex << commandArgs[1]; |
|
407 strSubIndex |
|
408 >> resetiosflags(ios::basefield) // guess base from prefix |
|
409 >> number; |
|
410 if (strSubIndex.fail() || number > 0xff) { |
|
411 err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; |
|
412 throw MasterException(err.str()); |
|
413 } |
|
414 data.sdo_entry_subindex = number; |
|
415 |
|
416 if (dataTypeStr != "") { // data type specified |
|
417 if (!(dataType = findDataType(dataTypeStr))) { |
|
418 err << "Invalid data type '" << dataTypeStr << "'!"; |
|
419 throw MasterException(err.str()); |
|
420 } |
|
421 } else { // no data type specified: fetch from dictionary |
|
422 ec_ioctl_slave_sdo_entry_t entry; |
|
423 |
|
424 open(ReadWrite); |
|
425 |
|
426 try { |
|
427 getSdoEntry(&entry, slavePosition, |
|
428 data.sdo_index, data.sdo_entry_subindex); |
|
429 } catch (MasterException &e) { |
|
430 err << "Failed to determine Sdo entry data type. " |
|
431 << "Please specify --type."; |
|
432 throw MasterException(err.str()); |
|
433 } |
|
434 if (!(dataType = findDataType(entry.data_type))) { |
|
435 err << "Pdo entry has unknown data type 0x" |
|
436 << hex << setfill('0') << setw(4) << entry.data_type << "!" |
|
437 << " Please specify --type."; |
|
438 throw MasterException(err.str()); |
|
439 } |
|
440 } |
|
441 |
|
442 if (dataType->byteSize) { |
|
443 data.data_size = dataType->byteSize; |
|
444 } else { |
|
445 data.data_size = DefaultBufferSize; |
|
446 } |
|
447 |
|
448 data.data = new uint8_t[data.data_size + 1]; |
|
449 |
|
450 strValue << commandArgs[2]; |
|
451 strValue >> resetiosflags(ios::basefield); // guess base from prefix |
|
452 strValue.exceptions(ios::failbit); |
|
453 |
|
454 try { |
|
455 switch (dataType->coeCode) { |
|
456 case 0x0002: // int8 |
|
457 { |
|
458 int16_t val; // uint8_t is interpreted as char |
|
459 strValue >> val; |
|
460 if (val > 127 || val < -128) |
|
461 throw ios::failure("Value out of range"); |
|
462 *data.data = val; |
|
463 break; |
|
464 } |
|
465 case 0x0003: // int16 |
|
466 { |
|
467 int16_t val; |
|
468 strValue >> val; |
|
469 *(int16_t *) data.data = cputole16(val); |
|
470 break; |
|
471 } |
|
472 case 0x0004: // int32 |
|
473 { |
|
474 int32_t val; |
|
475 strValue >> val; |
|
476 *(int32_t *) data.data = cputole32(val); |
|
477 break; |
|
478 } |
|
479 case 0x0005: // uint8 |
|
480 { |
|
481 uint16_t val; // uint8_t is interpreted as char |
|
482 strValue >> val; |
|
483 if (val > 0xff) |
|
484 throw ios::failure("Value out of range"); |
|
485 *data.data = val; |
|
486 break; |
|
487 } |
|
488 case 0x0006: // uint16 |
|
489 { |
|
490 uint16_t val; |
|
491 strValue >> val; |
|
492 *(uint16_t *) data.data = cputole16(val); |
|
493 break; |
|
494 } |
|
495 case 0x0007: // uint32 |
|
496 { |
|
497 uint32_t val; |
|
498 strValue >> val; |
|
499 *(uint32_t *) data.data = cputole32(val); |
|
500 break; |
|
501 } |
|
502 case 0x0009: // string |
|
503 if (strValue.str().size() >= data.data_size) { |
|
504 err << "String too large"; |
|
505 throw MasterException(err.str()); |
|
506 } |
|
507 data.data_size = strValue.str().size(); |
|
508 strValue >> (char *) data.data; |
|
509 break; |
|
510 |
|
511 default: |
|
512 delete [] data.data; |
|
513 err << "Unknown data type 0x" << hex << dataType->coeCode; |
|
514 throw MasterException(err.str()); |
|
515 } |
|
516 } catch (ios::failure &e) { |
|
517 delete [] data.data; |
|
518 err << "Invalid value argument '" << commandArgs[2] |
|
519 << "' for type '" << dataType->name << "'!"; |
|
520 throw MasterException(err.str()); |
|
521 } |
|
522 |
|
523 open(ReadWrite); |
|
524 |
|
525 if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &data) < 0) { |
|
526 stringstream err; |
|
527 err << "Failed to download Sdo: "; |
|
528 if (errno == EIO && data.abort_code) { |
|
529 err << "Abort code 0x" << hex << setfill('0') |
|
530 << setw(8) << data.abort_code; |
|
531 } else { |
|
532 err << strerror(errno); |
|
533 } |
|
534 delete [] data.data; |
|
535 throw MasterException(err.str()); |
|
536 } |
|
537 |
|
538 delete [] data.data; |
|
539 } |
|
540 |
|
541 /****************************************************************************/ |
|
542 |
|
543 void Master::sdoUpload( |
|
544 int slavePosition, |
|
545 const string &dataTypeStr, |
|
546 const vector<string> &commandArgs |
|
547 ) |
|
548 { |
|
549 stringstream strIndex, strSubIndex; |
|
550 int sval; |
|
551 ec_ioctl_slave_sdo_upload_t data; |
|
552 unsigned int uval; |
|
553 const CoEDataType *dataType = NULL; |
|
554 |
|
555 if (slavePosition < 0) { |
|
556 stringstream err; |
|
557 err << "'sdo_upload' requires a slave! Please specify --slave."; |
|
558 throw MasterException(err.str()); |
|
559 } |
|
560 data.slave_position = slavePosition; |
|
561 |
|
562 if (commandArgs.size() != 2) { |
|
563 stringstream err; |
|
564 err << "'sdo_upload' takes two arguments!"; |
|
565 throw MasterException(err.str()); |
|
566 } |
|
567 |
|
568 strIndex << commandArgs[0]; |
|
569 strIndex |
|
570 >> resetiosflags(ios::basefield) // guess base from prefix |
|
571 >> data.sdo_index; |
|
572 if (strIndex.fail()) { |
|
573 stringstream err; |
|
574 err << "Invalid Sdo index '" << commandArgs[0] << "'!"; |
|
575 throw MasterException(err.str()); |
|
576 } |
|
577 |
|
578 strSubIndex << commandArgs[1]; |
|
579 strSubIndex |
|
580 >> resetiosflags(ios::basefield) // guess base from prefix |
|
581 >> uval; |
|
582 if (strSubIndex.fail() || uval > 0xff) { |
|
583 stringstream err; |
|
584 err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; |
|
585 throw MasterException(err.str()); |
|
586 } |
|
587 data.sdo_entry_subindex = uval; |
|
588 |
|
589 if (dataTypeStr != "") { // data type specified |
|
590 if (!(dataType = findDataType(dataTypeStr))) { |
|
591 stringstream err; |
|
592 err << "Invalid data type '" << dataTypeStr << "'!"; |
|
593 throw MasterException(err.str()); |
|
594 } |
|
595 } else { // no data type specified: fetch from dictionary |
|
596 ec_ioctl_slave_sdo_entry_t entry; |
|
597 |
|
598 open(Read); |
|
599 |
|
600 try { |
|
601 getSdoEntry(&entry, slavePosition, |
|
602 data.sdo_index, data.sdo_entry_subindex); |
|
603 } catch (MasterException &e) { |
|
604 stringstream err; |
|
605 err << "Failed to determine Sdo entry data type. " |
|
606 << "Please specify --type."; |
|
607 throw MasterException(err.str()); |
|
608 } |
|
609 if (!(dataType = findDataType(entry.data_type))) { |
|
610 stringstream err; |
|
611 err << "Pdo entry has unknown data type 0x" |
|
612 << hex << setfill('0') << setw(4) << entry.data_type << "!" |
|
613 << " Please specify --type."; |
|
614 throw MasterException(err.str()); |
|
615 } |
|
616 } |
|
617 |
|
618 if (dataType->byteSize) { |
|
619 data.target_size = dataType->byteSize; |
|
620 } else { |
|
621 data.target_size = DefaultBufferSize; |
|
622 } |
|
623 |
|
624 data.target = new uint8_t[data.target_size + 1]; |
|
625 |
|
626 open(Read); |
|
627 |
|
628 if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &data) < 0) { |
|
629 stringstream err; |
|
630 err << "Failed to upload Sdo: "; |
|
631 if (errno == EIO && data.abort_code) { |
|
632 err << "Abort code 0x" << hex << setfill('0') |
|
633 << setw(8) << data.abort_code; |
|
634 } else { |
|
635 err << strerror(errno); |
|
636 } |
|
637 delete [] data.target; |
|
638 close(); |
|
639 throw MasterException(err.str()); |
|
640 } |
|
641 |
|
642 close(); |
|
643 |
|
644 if (dataType->byteSize && data.data_size != dataType->byteSize) { |
|
645 stringstream err; |
|
646 err << "Data type mismatch. Expected " << dataType->name |
|
647 << " with " << dataType->byteSize << " byte, but got " |
|
648 << data.data_size << " byte."; |
|
649 throw MasterException(err.str()); |
|
650 } |
|
651 |
|
652 cout << setfill('0'); |
|
653 switch (dataType->coeCode) { |
|
654 case 0x0002: // int8 |
|
655 sval = *(int8_t *) data.target; |
|
656 cout << sval << " 0x" << hex << setw(2) << sval << endl; |
|
657 break; |
|
658 case 0x0003: // int16 |
|
659 sval = le16tocpu(*(int16_t *) data.target); |
|
660 cout << sval << " 0x" << hex << setw(4) << sval << endl; |
|
661 break; |
|
662 case 0x0004: // int32 |
|
663 sval = le32tocpu(*(int32_t *) data.target); |
|
664 cout << sval << " 0x" << hex << setw(8) << sval << endl; |
|
665 break; |
|
666 case 0x0005: // uint8 |
|
667 uval = (unsigned int) *(uint8_t *) data.target; |
|
668 cout << uval << " 0x" << hex << setw(2) << uval << endl; |
|
669 break; |
|
670 case 0x0006: // uint16 |
|
671 uval = le16tocpu(*(uint16_t *) data.target); |
|
672 cout << uval << " 0x" << hex << setw(4) << uval << endl; |
|
673 break; |
|
674 case 0x0007: // uint32 |
|
675 uval = le32tocpu(*(uint32_t *) data.target); |
|
676 cout << uval << " 0x" << hex << setw(8) << uval << endl; |
|
677 break; |
|
678 case 0x0009: // string |
|
679 cout << string((const char *) data.target, data.data_size) |
|
680 << endl; |
|
681 break; |
|
682 default: |
|
683 printRawData(data.target, data.data_size); |
|
684 break; |
|
685 } |
|
686 |
|
687 delete [] data.target; |
|
688 } |
|
689 |
|
690 /****************************************************************************/ |
|
691 |
|
692 void Master::showSlaves(int slavePosition) |
|
693 { |
|
694 open(Read); |
|
695 |
|
696 if (verbosity == Verbose) { |
|
697 if (slavePosition == -1) { |
|
698 unsigned int numSlaves = slaveCount(), i; |
|
699 |
|
700 for (i = 0; i < numSlaves; i++) { |
|
701 showSlave(i); |
|
702 } |
|
703 } else { |
|
704 showSlave(slavePosition); |
|
705 } |
|
706 } else { |
|
707 listSlaves(slavePosition); |
|
708 } |
|
709 } |
|
710 |
|
711 /****************************************************************************/ |
|
712 |
|
713 struct CategoryName { |
|
714 uint16_t type; |
|
715 const char *name; |
|
716 }; |
|
717 |
|
718 static const CategoryName categoryNames[] = { |
|
719 {0x000a, "STRINGS"}, |
|
720 {0x0014, "DataTypes"}, |
|
721 {0x001e, "General"}, |
|
722 {0x0028, "FMMU"}, |
|
723 {0x0029, "SyncM"}, |
|
724 {0x0032, "TXPDO"}, |
|
725 {0x0033, "RXPDO"}, |
|
726 {0x003c, "DC"}, |
|
727 {} |
|
728 }; |
|
729 |
|
730 const char *getCategoryName(uint16_t type) |
|
731 { |
|
732 const CategoryName *cn = categoryNames; |
|
733 |
|
734 while (cn->type) { |
|
735 if (cn->type == type) { |
|
736 return cn->name; |
|
737 } |
|
738 cn++; |
|
739 } |
|
740 |
|
741 return "unknown"; |
|
742 } |
|
743 |
|
744 void Master::siiRead(int slavePosition) |
|
745 { |
|
746 ec_ioctl_slave_sii_t data; |
|
747 ec_ioctl_slave_t slave; |
|
748 unsigned int i; |
|
749 const uint16_t *categoryHeader; |
|
750 uint16_t categoryType, categorySize; |
|
751 stringstream err; |
|
752 |
|
753 if (slavePosition < 0) { |
|
754 stringstream err; |
|
755 err << "'sii_read' requires a slave! Please specify --slave."; |
|
756 throw MasterException(err.str()); |
|
757 } |
|
758 data.slave_position = slavePosition; |
|
759 |
|
760 open(Read); |
|
761 |
|
762 getSlave(&slave, slavePosition); |
|
763 |
|
764 if (!slave.sii_nwords) |
|
765 return; |
|
766 |
|
767 data.offset = 0; |
|
768 data.nwords = slave.sii_nwords; |
|
769 data.words = new uint16_t[data.nwords]; |
|
770 |
|
771 if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) { |
|
772 stringstream err; |
|
773 delete [] data.words; |
|
774 err << "Failed to read SII: " << strerror(errno); |
|
775 throw MasterException(err.str()); |
|
776 } |
|
777 |
|
778 if (verbosity == Verbose) { |
|
779 cout << "SII Area:" << hex << setfill('0'); |
|
780 for (i = 0; i < min(data.nwords, 0x0040U) * 2; i++) { |
|
781 if (i % BreakAfterBytes) { |
|
782 cout << " "; |
|
783 } else { |
|
784 cout << endl << " "; |
|
785 } |
|
786 cout << setw(2) << (unsigned int) *((uint8_t *) data.words + i); |
|
787 } |
|
788 cout << endl; |
|
789 |
|
790 if (data.nwords > 0x0040U) { |
|
791 // cycle through categories |
|
792 categoryHeader = data.words + 0x0040U; |
|
793 categoryType = le16tocpu(*categoryHeader); |
|
794 while (categoryType != 0xffff) { |
|
795 cout << "SII Category 0x" << hex |
|
796 << setw(4) << categoryType |
|
797 << " (" << getCategoryName(categoryType) << ")" << flush; |
|
798 |
|
799 if (categoryHeader + 1 > data.words + data.nwords) { |
|
800 err << "SII data seem to be corrupted!"; |
|
801 throw MasterException(err.str()); |
|
802 } |
|
803 categorySize = le16tocpu(*(categoryHeader + 1)); |
|
804 cout << ", " << dec << categorySize << " words" << flush; |
|
805 |
|
806 if (categoryHeader + 2 + categorySize |
|
807 > data.words + data.nwords) { |
|
808 err << "SII data seem to be corrupted!"; |
|
809 throw MasterException(err.str()); |
|
810 } |
|
811 |
|
812 cout << hex; |
|
813 for (i = 0; i < categorySize * 2U; i++) { |
|
814 if (i % BreakAfterBytes) { |
|
815 cout << " "; |
|
816 } else { |
|
817 cout << endl << " "; |
|
818 } |
|
819 cout << setw(2) << (unsigned int) |
|
820 *((uint8_t *) (categoryHeader + 2) + i); |
|
821 } |
|
822 cout << endl; |
|
823 |
|
824 if (categoryHeader + 2 + categorySize + 1 |
|
825 > data.words + data.nwords) { |
|
826 err << "SII data seem to be corrupted!"; |
|
827 throw MasterException(err.str()); |
|
828 } |
|
829 categoryHeader += 2 + categorySize; |
|
830 categoryType = le16tocpu(*categoryHeader); |
|
831 } |
|
832 } |
|
833 } else { |
|
834 for (i = 0; i < data.nwords; i++) { |
|
835 uint16_t *w = data.words + i; |
|
836 cout << *(uint8_t *) w << *((uint8_t *) w + 1); |
|
837 } |
|
838 } |
|
839 |
|
840 delete [] data.words; |
|
841 } |
|
842 |
|
843 /****************************************************************************/ |
|
844 |
|
845 void Master::siiWrite( |
|
846 int slavePosition, |
|
847 bool force, |
|
848 const vector<string> &commandArgs |
|
849 ) |
|
850 { |
|
851 stringstream err; |
|
852 ec_ioctl_slave_sii_t data; |
|
853 ifstream file; |
|
854 unsigned int byte_size; |
|
855 const uint16_t *categoryHeader; |
|
856 uint16_t categoryType, categorySize; |
|
857 uint8_t crc; |
|
858 |
|
859 if (slavePosition < 0) { |
|
860 err << "'sii_write' requires a slave! Please specify --slave."; |
|
861 throw MasterException(err.str()); |
|
862 } |
|
863 data.slave_position = slavePosition; |
|
864 |
|
865 if (commandArgs.size() != 1) { |
|
866 err << "'ssi_write' takes exactly one argument!"; |
|
867 throw MasterException(err.str()); |
|
868 } |
|
869 |
|
870 file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary); |
|
871 if (file.fail()) { |
|
872 err << "Failed to open '" << commandArgs[0] << "'!"; |
|
873 throw MasterException(err.str()); |
|
874 } |
|
875 |
|
876 // get length of file |
|
877 file.seekg(0, ios::end); |
|
878 byte_size = file.tellg(); |
|
879 file.seekg(0, ios::beg); |
|
880 |
|
881 if (!byte_size || byte_size % 2) { |
|
882 stringstream err; |
|
883 err << "Invalid file size! Must be non-zero and even."; |
|
884 throw MasterException(err.str()); |
|
885 } |
|
886 |
|
887 data.nwords = byte_size / 2; |
|
888 if (data.nwords < 0x0041 && !force) { |
|
889 err << "SII data too short (" << data.nwords << " words)! Mimimum is" |
|
890 " 40 fixed words + 1 delimiter. Use --force to write anyway."; |
|
891 throw MasterException(err.str()); |
|
892 } |
|
893 |
|
894 // allocate buffer and read file into buffer |
|
895 data.words = new uint16_t[data.nwords]; |
|
896 file.read((char *) data.words, byte_size); |
|
897 file.close(); |
|
898 |
|
899 if (!force) { |
|
900 // calculate checksum over words 0 to 6 |
|
901 crc = calcSiiCrc((const uint8_t *) data.words, 14); |
|
902 if (crc != ((const uint8_t *) data.words)[14]) { |
|
903 err << "CRC incorrect. Must be 0x" |
|
904 << hex << setfill('0') << setw(2) << (unsigned int) crc |
|
905 << ". Use --force to write anyway."; |
|
906 throw MasterException(err.str()); |
|
907 } |
|
908 |
|
909 // cycle through categories to detect corruption |
|
910 categoryHeader = data.words + 0x0040U; |
|
911 categoryType = le16tocpu(*categoryHeader); |
|
912 while (categoryType != 0xffff) { |
|
913 if (categoryHeader + 1 > data.words + data.nwords) { |
|
914 err << "SII data seem to be corrupted! " |
|
915 << "Use --force to write anyway."; |
|
916 throw MasterException(err.str()); |
|
917 } |
|
918 categorySize = le16tocpu(*(categoryHeader + 1)); |
|
919 if (categoryHeader + 2 + categorySize + 1 |
|
920 > data.words + data.nwords) { |
|
921 err << "SII data seem to be corrupted! " |
|
922 "Use --force to write anyway."; |
|
923 throw MasterException(err.str()); |
|
924 } |
|
925 categoryHeader += 2 + categorySize; |
|
926 categoryType = le16tocpu(*categoryHeader); |
|
927 } |
|
928 } |
|
929 |
|
930 // send data to master |
|
931 open(ReadWrite); |
|
932 data.offset = 0; |
|
933 if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) { |
|
934 stringstream err; |
|
935 err << "Failed to write SII: " << strerror(errno); |
|
936 throw MasterException(err.str()); |
|
937 } |
|
938 } |
|
939 |
|
940 /****************************************************************************/ |
|
941 |
|
942 void Master::requestStates( |
|
943 int slavePosition, |
|
944 const vector<string> &commandArgs |
|
945 ) |
|
946 { |
|
947 string stateStr; |
|
948 uint8_t state; |
|
949 |
|
950 if (commandArgs.size() != 1) { |
|
951 stringstream err; |
|
952 err << "'state' takes exactly one argument!"; |
|
953 throw MasterException(err.str()); |
|
954 } |
|
955 |
|
956 stateStr = commandArgs[0]; |
|
957 transform(stateStr.begin(), stateStr.end(), |
|
958 stateStr.begin(), (int (*) (int)) std::toupper); |
|
959 |
|
960 if (stateStr == "INIT") { |
|
961 state = 0x01; |
|
962 } else if (stateStr == "PREOP") { |
|
963 state = 0x02; |
|
964 } else if (stateStr == "SAFEOP") { |
|
965 state = 0x04; |
|
966 } else if (stateStr == "OP") { |
|
967 state = 0x08; |
|
968 } else { |
|
969 stringstream err; |
|
970 err << "Invalid state '" << commandArgs[0] << "'!"; |
|
971 throw MasterException(err.str()); |
|
972 } |
|
973 |
|
974 open(ReadWrite); |
|
975 |
|
976 if (slavePosition == -1) { |
|
977 unsigned int i, numSlaves = slaveCount(); |
|
978 for (i = 0; i < numSlaves; i++) |
|
979 requestState(i, state); |
|
980 } else { |
|
981 requestState(slavePosition, state); |
|
982 } |
|
983 } |
|
984 |
|
985 /****************************************************************************/ |
|
986 |
|
987 void Master::generateXml(int slavePosition) |
|
988 { |
|
989 open(Read); |
|
990 |
|
991 if (slavePosition == -1) { |
|
992 unsigned int numSlaves = slaveCount(), i; |
|
993 |
|
994 for (i = 0; i < numSlaves; i++) { |
|
995 generateSlaveXml(i); |
|
996 } |
|
997 } else { |
|
998 generateSlaveXml(slavePosition); |
|
999 } |
|
1000 } |
|
1001 |
|
1002 /****************************************************************************/ |
|
1003 |
|
1004 void Master::open(Permissions perm) |
|
1005 { |
|
1006 stringstream deviceName; |
|
1007 |
|
1008 if (fd != -1) { // already open |
|
1009 return; |
|
1010 } |
|
1011 |
|
1012 deviceName << "/dev/EtherCAT" << index; |
|
1013 |
|
1014 if ((fd = ::open(deviceName.str().c_str(), |
|
1015 perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) { |
|
1016 stringstream err; |
|
1017 err << "Failed to open master device " << deviceName.str() << ": " |
|
1018 << strerror(errno); |
|
1019 throw MasterException(err.str()); |
|
1020 } |
|
1021 } |
|
1022 |
|
1023 /****************************************************************************/ |
|
1024 |
|
1025 void Master::close() |
|
1026 { |
|
1027 if (fd == -1) |
|
1028 return; |
|
1029 |
|
1030 ::close(fd); |
|
1031 fd = -1; |
|
1032 } |
|
1033 |
|
1034 /*****************************************************************************/ |
|
1035 |
|
1036 /** |
|
1037 * Writes the Secondary slave address (alias) to the slave's SII. |
|
1038 */ |
|
1039 void Master::writeSlaveAlias( |
|
1040 uint16_t slavePosition, |
|
1041 uint16_t alias |
|
1042 ) |
|
1043 { |
|
1044 ec_ioctl_slave_sii_t data; |
|
1045 ec_ioctl_slave_t slave; |
|
1046 stringstream err; |
|
1047 uint8_t crc; |
|
1048 |
|
1049 open(ReadWrite); |
|
1050 |
|
1051 getSlave(&slave, slavePosition); |
|
1052 |
|
1053 if (slave.sii_nwords < 8) { |
|
1054 err << "Current SII contents are too small to set an alias " |
|
1055 << "(" << slave.sii_nwords << " words)!"; |
|
1056 throw MasterException(err.str()); |
|
1057 } |
|
1058 |
|
1059 data.slave_position = slavePosition; |
|
1060 data.offset = 0; |
|
1061 data.nwords = 8; |
|
1062 data.words = new uint16_t[data.nwords]; |
|
1063 |
|
1064 // read first 8 SII words |
|
1065 if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) { |
|
1066 delete [] data.words; |
|
1067 err << "Failed to read SII: " << strerror(errno); |
|
1068 throw MasterException(err.str()); |
|
1069 } |
|
1070 |
|
1071 // write new alias address in word 4 |
|
1072 data.words[4] = cputole16(alias); |
|
1073 |
|
1074 // calculate checksum over words 0 to 6 |
|
1075 crc = calcSiiCrc((const uint8_t *) data.words, 14); |
|
1076 |
|
1077 // write new checksum into first byte of word 7 |
|
1078 *(uint8_t *) (data.words + 7) = crc; |
|
1079 |
|
1080 // write first 8 words with new alias and checksum |
|
1081 if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) { |
|
1082 delete [] data.words; |
|
1083 err << "Failed to write SII: " << strerror(errno); |
|
1084 throw MasterException(err.str()); |
|
1085 } |
|
1086 |
|
1087 delete [] data.words; |
|
1088 } |
|
1089 |
|
1090 /*****************************************************************************/ |
|
1091 |
|
1092 /** Lists the complete bus configuration. |
|
1093 */ |
|
1094 void Master::showDetailedConfigs(const ConfigList &configList) |
|
1095 { |
|
1096 ConfigList::const_iterator configIter; |
|
1097 unsigned int j, k, l; |
|
1098 ec_ioctl_config_pdo_t pdo; |
|
1099 ec_ioctl_config_pdo_entry_t entry; |
|
1100 ec_ioctl_config_sdo_t sdo; |
|
1101 |
|
1102 for (configIter = configList.begin(); |
|
1103 configIter != configList.end(); |
|
1104 configIter++) { |
|
1105 |
|
1106 cout << "Alias: " |
|
1107 << dec << configIter->alias << endl |
|
1108 << "Position: " << configIter->position << endl |
|
1109 << "Vendor Id: 0x" |
|
1110 << hex << setfill('0') |
|
1111 << setw(8) << configIter->vendor_id << endl |
|
1112 << "Product code: 0x" |
|
1113 << setw(8) << configIter->product_code << endl |
|
1114 << "Attached: " << (configIter->attached ? "yes" : "no") << endl |
|
1115 << "Operational: " << (configIter->operational ? "yes" : "no") << endl; |
|
1116 |
|
1117 for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) { |
|
1118 if (configIter->syncs[j].pdo_count) { |
|
1119 cout << "SM" << dec << j << " (" |
|
1120 << (configIter->syncs[j].dir == EC_DIR_INPUT |
|
1121 ? "Input" : "Output") << ")" << endl; |
|
1122 for (k = 0; k < configIter->syncs[j].pdo_count; k++) { |
|
1123 getConfigPdo(&pdo, configIter->config_index, j, k); |
|
1124 |
|
1125 cout << " Pdo 0x" << hex |
|
1126 << setw(4) << pdo.index |
|
1127 << " \"" << pdo.name << "\"" << endl; |
|
1128 |
|
1129 for (l = 0; l < pdo.entry_count; l++) { |
|
1130 getConfigPdoEntry(&entry, configIter->config_index, j, k, l); |
|
1131 |
|
1132 cout << " Pdo entry 0x" << hex |
|
1133 << setw(4) << entry.index << ":" |
|
1134 << setw(2) << (unsigned int) entry.subindex |
|
1135 << ", " << dec << (unsigned int) entry.bit_length |
|
1136 << " bit, \"" << entry.name << "\"" << endl; |
|
1137 } |
|
1138 } |
|
1139 } |
|
1140 } |
|
1141 |
|
1142 cout << "Sdo configuration:" << endl; |
|
1143 if (configIter->sdo_count) { |
|
1144 for (j = 0; j < configIter->sdo_count; j++) { |
|
1145 getConfigSdo(&sdo, configIter->config_index, j); |
|
1146 |
|
1147 cout << " 0x" |
|
1148 << hex << setfill('0') |
|
1149 << setw(4) << sdo.index << ":" |
|
1150 << setw(2) << (unsigned int) sdo.subindex |
|
1151 << ", " << dec << sdo.size << " byte: " << hex; |
|
1152 |
|
1153 switch (sdo.size) { |
|
1154 case 1: |
|
1155 cout << "0x" << setw(2) |
|
1156 << (unsigned int) *(uint8_t *) &sdo.data; |
|
1157 break; |
|
1158 case 2: |
|
1159 cout << "0x" << setw(4) |
|
1160 << le16tocpu(*(uint16_t *) &sdo.data); |
|
1161 break; |
|
1162 case 4: |
|
1163 cout << "0x" << setw(8) |
|
1164 << le32tocpu(*(uint32_t *) &sdo.data); |
|
1165 break; |
|
1166 default: |
|
1167 cout << "???"; |
|
1168 } |
|
1169 |
|
1170 cout << endl; |
|
1171 } |
|
1172 } else { |
|
1173 cout << " None." << endl; |
|
1174 } |
|
1175 |
|
1176 cout << endl; |
|
1177 } |
|
1178 } |
|
1179 |
|
1180 /*****************************************************************************/ |
|
1181 |
|
1182 struct ConfigInfo { |
|
1183 string alias; |
|
1184 string pos; |
|
1185 string ident; |
|
1186 string att; |
|
1187 string op; |
|
1188 }; |
|
1189 |
|
1190 /** Lists the bus configuration. |
|
1191 */ |
|
1192 void Master::listConfigs(const ConfigList &configList) |
|
1193 { |
|
1194 ConfigList::const_iterator configIter; |
|
1195 stringstream str; |
|
1196 ConfigInfo info; |
|
1197 typedef list<ConfigInfo> ConfigInfoList; |
|
1198 ConfigInfoList list; |
|
1199 ConfigInfoList::const_iterator iter; |
|
1200 unsigned int maxAliasWidth = 0, maxPosWidth = 0, |
|
1201 maxAttWidth = 0, maxOpWidth = 0; |
|
1202 |
|
1203 for (configIter = configList.begin(); |
|
1204 configIter != configList.end(); |
|
1205 configIter++) { |
|
1206 |
|
1207 str << dec << configIter->alias; |
|
1208 info.alias = str.str(); |
|
1209 str.clear(); |
|
1210 str.str(""); |
|
1211 |
|
1212 str << configIter->position; |
|
1213 info.pos = str.str(); |
|
1214 str.clear(); |
|
1215 str.str(""); |
|
1216 |
|
1217 str << hex << setfill('0') |
|
1218 << "0x" << setw(8) << configIter->vendor_id |
|
1219 << "/0x" << setw(8) << configIter->product_code; |
|
1220 info.ident = str.str(); |
|
1221 str.clear(); |
|
1222 str.str(""); |
|
1223 |
|
1224 str << (configIter->attached ? "attached" : "-"); |
|
1225 info.att = str.str(); |
|
1226 str.clear(); |
|
1227 str.str(""); |
|
1228 |
|
1229 str << (configIter->operational ? "operational" : "-"); |
|
1230 info.op = str.str(); |
|
1231 str.clear(); |
|
1232 str.str(""); |
|
1233 |
|
1234 list.push_back(info); |
|
1235 |
|
1236 if (info.alias.length() > maxAliasWidth) |
|
1237 maxAliasWidth = info.alias.length(); |
|
1238 if (info.pos.length() > maxPosWidth) |
|
1239 maxPosWidth = info.pos.length(); |
|
1240 if (info.att.length() > maxAttWidth) |
|
1241 maxAttWidth = info.att.length(); |
|
1242 if (info.op.length() > maxOpWidth) |
|
1243 maxOpWidth = info.op.length(); |
|
1244 } |
|
1245 |
|
1246 for (iter = list.begin(); iter != list.end(); iter++) { |
|
1247 cout << setfill(' ') << right |
|
1248 << setw(maxAliasWidth) << iter->alias |
|
1249 << ":" << left |
|
1250 << setw(maxPosWidth) << iter->pos |
|
1251 << " " |
|
1252 << iter->ident |
|
1253 << " " |
|
1254 << setw(maxAttWidth) << iter->att << " " |
|
1255 << setw(maxOpWidth) << iter->op << " " |
|
1256 << endl; |
|
1257 } |
|
1258 } |
|
1259 |
|
1260 /****************************************************************************/ |
|
1261 |
|
1262 void Master::outputDomainData(unsigned int domainIndex) |
|
1263 { |
|
1264 ec_ioctl_domain_t domain; |
|
1265 ec_ioctl_domain_data_t data; |
|
1266 unsigned char *processData; |
|
1267 unsigned int i; |
|
1268 |
|
1269 getDomain(&domain, domainIndex); |
|
1270 |
|
1271 if (!domain.data_size) |
|
1272 return; |
|
1273 |
|
1274 processData = new unsigned char[domain.data_size]; |
|
1275 |
|
1276 try { |
|
1277 getData(&data, domainIndex, domain.data_size, processData); |
|
1278 } catch (MasterException &e) { |
|
1279 delete [] processData; |
|
1280 throw e; |
|
1281 } |
|
1282 |
|
1283 for (i = 0; i < data.data_size; i++) |
|
1284 cout << processData[i]; |
|
1285 cout.flush(); |
|
1286 |
|
1287 delete [] processData; |
|
1288 } |
|
1289 |
|
1290 /****************************************************************************/ |
|
1291 |
|
1292 void Master::showDomain(unsigned int domainIndex) |
|
1293 { |
|
1294 ec_ioctl_domain_t domain; |
|
1295 unsigned char *processData; |
|
1296 ec_ioctl_domain_data_t data; |
|
1297 unsigned int i, j; |
|
1298 ec_ioctl_domain_fmmu_t fmmu; |
|
1299 unsigned int dataOffset; |
|
1300 |
|
1301 getDomain(&domain, domainIndex); |
|
1302 |
|
1303 cout << "Domain" << dec << domainIndex << ":" |
|
1304 << " LogBaseAddr 0x" |
|
1305 << hex << setfill('0') |
|
1306 << setw(8) << domain.logical_base_address |
|
1307 << ", Size " << dec << setfill(' ') |
|
1308 << setw(3) << domain.data_size |
|
1309 << ", WorkingCounter " |
|
1310 << domain.working_counter << "/" |
|
1311 << domain.expected_working_counter << endl; |
|
1312 |
|
1313 if (!domain.data_size || verbosity != Verbose) |
|
1314 return; |
|
1315 |
|
1316 processData = new unsigned char[domain.data_size]; |
|
1317 |
|
1318 try { |
|
1319 getData(&data, domainIndex, domain.data_size, processData); |
|
1320 } catch (MasterException &e) { |
|
1321 delete [] processData; |
|
1322 throw e; |
|
1323 } |
|
1324 |
|
1325 for (i = 0; i < domain.fmmu_count; i++) { |
|
1326 getFmmu(&fmmu, domainIndex, i); |
|
1327 |
|
1328 cout << " SlaveConfig " |
|
1329 << dec << fmmu.slave_config_alias |
|
1330 << ":" << fmmu.slave_config_position |
|
1331 << ", SM" << (unsigned int) fmmu.sync_index << " (" |
|
1332 << setfill(' ') << setw(6) |
|
1333 << (fmmu.dir == EC_DIR_INPUT ? "Input" : "Output") |
|
1334 << "), LogAddr 0x" |
|
1335 << hex << setfill('0') |
|
1336 << setw(8) << fmmu.logical_address |
|
1337 << ", Size " << dec << fmmu.data_size << endl; |
|
1338 |
|
1339 dataOffset = fmmu.logical_address - domain.logical_base_address; |
|
1340 if (dataOffset + fmmu.data_size > domain.data_size) { |
|
1341 stringstream err; |
|
1342 err << "Fmmu information corrupted!"; |
|
1343 delete [] processData; |
|
1344 throw MasterException(err.str()); |
|
1345 } |
|
1346 |
|
1347 cout << " " << hex << setfill('0'); |
|
1348 for (j = 0; j < fmmu.data_size; j++) { |
|
1349 if (j && !(j % BreakAfterBytes)) |
|
1350 cout << endl << " "; |
|
1351 cout << setw(2) |
|
1352 << (unsigned int) *(processData + dataOffset + j) << " "; |
|
1353 } |
|
1354 cout << endl; |
|
1355 } |
|
1356 |
|
1357 delete [] processData; |
|
1358 } |
|
1359 |
|
1360 /****************************************************************************/ |
|
1361 |
|
1362 void Master::listSlavePdos( |
|
1363 uint16_t slavePosition, |
|
1364 bool withHeader |
|
1365 ) |
|
1366 { |
|
1367 ec_ioctl_slave_t slave; |
|
1368 ec_ioctl_slave_sync_t sync; |
|
1369 ec_ioctl_slave_sync_pdo_t pdo; |
|
1370 ec_ioctl_slave_sync_pdo_entry_t entry; |
|
1371 unsigned int i, j, k; |
|
1372 |
|
1373 getSlave(&slave, slavePosition); |
|
1374 |
|
1375 if (withHeader) |
|
1376 cout << "=== Slave " << slavePosition << " ===" << endl; |
|
1377 |
|
1378 for (i = 0; i < slave.sync_count; i++) { |
|
1379 getSync(&sync, slavePosition, i); |
|
1380 |
|
1381 cout << "SM" << i << ":" |
|
1382 << " PhysAddr 0x" |
|
1383 << hex << setfill('0') |
|
1384 << setw(4) << sync.physical_start_address |
|
1385 << ", DefaultSize " |
|
1386 << dec << setfill(' ') << setw(4) << sync.default_size |
|
1387 << ", ControlRegister 0x" |
|
1388 << hex << setfill('0') << setw(2) |
|
1389 << (unsigned int) sync.control_register |
|
1390 << ", Enable " << dec << (unsigned int) sync.enable |
|
1391 << endl; |
|
1392 |
|
1393 for (j = 0; j < sync.pdo_count; j++) { |
|
1394 getPdo(&pdo, slavePosition, i, j); |
|
1395 |
|
1396 cout << " " << (sync.control_register & 0x04 ? "R" : "T") |
|
1397 << "xPdo 0x" |
|
1398 << hex << setfill('0') |
|
1399 << setw(4) << pdo.index |
|
1400 << " \"" << pdo.name << "\"" << endl; |
|
1401 |
|
1402 if (verbosity == Quiet) |
|
1403 continue; |
|
1404 |
|
1405 for (k = 0; k < pdo.entry_count; k++) { |
|
1406 getPdoEntry(&entry, slavePosition, i, j, k); |
|
1407 |
|
1408 cout << " Pdo entry 0x" |
|
1409 << hex << setfill('0') |
|
1410 << setw(4) << entry.index |
|
1411 << ":" << setw(2) << (unsigned int) entry.subindex |
|
1412 << ", " << dec << (unsigned int) entry.bit_length |
|
1413 << " bit, \"" << entry.name << "\"" << endl; |
|
1414 } |
|
1415 } |
|
1416 } |
|
1417 } |
|
1418 |
|
1419 /****************************************************************************/ |
|
1420 |
|
1421 void Master::listSlaveSdos( |
|
1422 uint16_t slavePosition, |
|
1423 bool withHeader |
|
1424 ) |
|
1425 { |
|
1426 ec_ioctl_slave_t slave; |
|
1427 ec_ioctl_slave_sdo_t sdo; |
|
1428 ec_ioctl_slave_sdo_entry_t entry; |
|
1429 unsigned int i, j; |
|
1430 const CoEDataType *d; |
|
1431 |
|
1432 getSlave(&slave, slavePosition); |
|
1433 |
|
1434 if (withHeader) |
|
1435 cout << "=== Slave " << slavePosition << " ===" << endl; |
|
1436 |
|
1437 for (i = 0; i < slave.sdo_count; i++) { |
|
1438 getSdo(&sdo, slavePosition, i); |
|
1439 |
|
1440 cout << "Sdo 0x" |
|
1441 << hex << setfill('0') |
|
1442 << setw(4) << sdo.sdo_index |
|
1443 << ", \"" << sdo.name << "\"" << endl; |
|
1444 |
|
1445 if (verbosity == Quiet) |
|
1446 continue; |
|
1447 |
|
1448 for (j = 0; j <= sdo.max_subindex; j++) { |
|
1449 getSdoEntry(&entry, slavePosition, -i, j); |
|
1450 |
|
1451 cout << " 0x" << hex << setfill('0') |
|
1452 << setw(4) << sdo.sdo_index << ":" |
|
1453 << setw(2) << (unsigned int) entry.sdo_entry_subindex |
|
1454 << ", "; |
|
1455 |
|
1456 if ((d = findDataType(entry.data_type))) { |
|
1457 cout << d->name; |
|
1458 } else { |
|
1459 cout << "type " << setw(4) << entry.data_type; |
|
1460 } |
|
1461 |
|
1462 cout << ", " << dec << entry.bit_length << " bit, \"" |
|
1463 << entry.description << "\"" << endl; |
|
1464 } |
|
1465 } |
|
1466 } |
|
1467 |
|
1468 /****************************************************************************/ |
|
1469 |
|
1470 struct SlaveInfo { |
|
1471 string pos; |
|
1472 string alias; |
|
1473 string relPos; |
|
1474 string state; |
|
1475 string flag; |
|
1476 string name; |
|
1477 }; |
|
1478 |
|
1479 void Master::listSlaves(int slavePosition) |
|
1480 { |
|
1481 unsigned int numSlaves, i; |
|
1482 ec_ioctl_slave_t slave; |
|
1483 uint16_t lastAlias, aliasIndex; |
|
1484 SlaveInfo slaveInfo; |
|
1485 typedef list<SlaveInfo> SlaveInfoList; |
|
1486 SlaveInfoList slaveInfoList; |
|
1487 SlaveInfoList::const_iterator iter; |
|
1488 stringstream str; |
|
1489 unsigned int maxPosWidth = 0, maxAliasWidth = 0, |
|
1490 maxRelPosWidth = 0, maxStateWidth = 0; |
|
1491 |
|
1492 open(Read); |
|
1493 |
|
1494 numSlaves = slaveCount(); |
|
1495 |
|
1496 lastAlias = 0; |
|
1497 aliasIndex = 0; |
|
1498 for (i = 0; i < numSlaves; i++) { |
|
1499 getSlave(&slave, i); |
|
1500 |
|
1501 if (slave.alias) { |
|
1502 lastAlias = slave.alias; |
|
1503 aliasIndex = 0; |
|
1504 } |
|
1505 |
|
1506 if (slavePosition == -1 || i == (unsigned int) slavePosition) { |
|
1507 str << dec << i; |
|
1508 slaveInfo.pos = str.str(); |
|
1509 str.clear(); |
|
1510 str.str(""); |
|
1511 |
|
1512 str << lastAlias; |
|
1513 slaveInfo.alias = str.str(); |
|
1514 str.str(""); |
|
1515 |
|
1516 str << aliasIndex; |
|
1517 slaveInfo.relPos = str.str(); |
|
1518 str.str(""); |
|
1519 |
|
1520 slaveInfo.state = slaveState(slave.state); |
|
1521 slaveInfo.flag = (slave.error_flag ? 'E' : '+'); |
|
1522 |
|
1523 if (strlen(slave.name)) { |
|
1524 slaveInfo.name = slave.name; |
|
1525 } else { |
|
1526 str << "0x" << hex << setfill('0') |
|
1527 << setw(8) << slave.vendor_id << ":0x" |
|
1528 << setw(8) << slave.product_code; |
|
1529 slaveInfo.name = str.str(); |
|
1530 str.str(""); |
|
1531 } |
|
1532 |
|
1533 |
|
1534 slaveInfoList.push_back(slaveInfo); |
|
1535 |
|
1536 if (slaveInfo.pos.length() > maxPosWidth) |
|
1537 maxPosWidth = slaveInfo.pos.length(); |
|
1538 if (slaveInfo.alias.length() > maxAliasWidth) |
|
1539 maxAliasWidth = slaveInfo.alias.length(); |
|
1540 if (slaveInfo.relPos.length() > maxRelPosWidth) |
|
1541 maxRelPosWidth = slaveInfo.relPos.length(); |
|
1542 if (slaveInfo.state.length() > maxStateWidth) |
|
1543 maxStateWidth = slaveInfo.state.length(); |
|
1544 } |
|
1545 |
|
1546 aliasIndex++; |
|
1547 } |
|
1548 |
|
1549 for (iter = slaveInfoList.begin(); iter != slaveInfoList.end(); iter++) { |
|
1550 cout << setfill(' ') << right |
|
1551 << setw(maxPosWidth) << iter->pos << " " |
|
1552 << setw(maxAliasWidth) << iter->alias |
|
1553 << ":" << left |
|
1554 << setw(maxRelPosWidth) << iter->relPos << " " |
|
1555 << setw(maxStateWidth) << iter->state << " " |
|
1556 << iter->flag << " " |
|
1557 << iter->name << endl; |
|
1558 } |
|
1559 } |
|
1560 |
|
1561 /****************************************************************************/ |
|
1562 |
|
1563 void Master::showSlave(uint16_t slavePosition) |
|
1564 { |
|
1565 ec_ioctl_slave_t slave; |
|
1566 list<string> protoList; |
|
1567 list<string>::const_iterator protoIter; |
|
1568 |
|
1569 getSlave(&slave, slavePosition); |
|
1570 |
|
1571 cout << "=== Slave " << dec << slavePosition << " ===" << endl; |
|
1572 |
|
1573 if (slave.alias) |
|
1574 cout << "Alias: " << slave.alias << endl; |
|
1575 |
|
1576 cout |
|
1577 << "State: " << slaveState(slave.state) << endl |
|
1578 << "Flag: " << (slave.error_flag ? 'E' : '+') << endl |
|
1579 << "Identity:" << endl |
|
1580 << " Vendor Id: 0x" |
|
1581 << hex << setfill('0') |
|
1582 << setw(8) << slave.vendor_id << endl |
|
1583 << " Product code: 0x" |
|
1584 << setw(8) << slave.product_code << endl |
|
1585 << " Revision number: 0x" |
|
1586 << setw(8) << slave.revision_number << endl |
|
1587 << " Serial number: 0x" |
|
1588 << setw(8) << slave.serial_number << endl; |
|
1589 |
|
1590 if (slave.mailbox_protocols) { |
|
1591 cout << "Mailboxes:" << endl |
|
1592 << " RX: 0x" |
|
1593 << hex << setw(4) << slave.rx_mailbox_offset << "/" |
|
1594 << dec << slave.rx_mailbox_size |
|
1595 << ", TX: 0x" |
|
1596 << hex << setw(4) << slave.tx_mailbox_offset << "/" |
|
1597 << dec << slave.tx_mailbox_size << endl |
|
1598 << " Supported protocols: "; |
|
1599 |
|
1600 if (slave.mailbox_protocols & EC_MBOX_AOE) { |
|
1601 protoList.push_back("AoE"); |
|
1602 } |
|
1603 if (slave.mailbox_protocols & EC_MBOX_EOE) { |
|
1604 protoList.push_back("EoE"); |
|
1605 } |
|
1606 if (slave.mailbox_protocols & EC_MBOX_COE) { |
|
1607 protoList.push_back("CoE"); |
|
1608 } |
|
1609 if (slave.mailbox_protocols & EC_MBOX_FOE) { |
|
1610 protoList.push_back("FoE"); |
|
1611 } |
|
1612 if (slave.mailbox_protocols & EC_MBOX_SOE) { |
|
1613 protoList.push_back("SoE"); |
|
1614 } |
|
1615 if (slave.mailbox_protocols & EC_MBOX_VOE) { |
|
1616 protoList.push_back("VoE"); |
|
1617 } |
|
1618 |
|
1619 for (protoIter = protoList.begin(); protoIter != protoList.end(); |
|
1620 protoIter++) { |
|
1621 if (protoIter != protoList.begin()) |
|
1622 cout << ", "; |
|
1623 cout << *protoIter; |
|
1624 } |
|
1625 cout << endl; |
|
1626 } |
|
1627 |
|
1628 if (slave.has_general_category) { |
|
1629 cout << "General:" << endl |
|
1630 << " Group: " << slave.group << endl |
|
1631 << " Image name: " << slave.image << endl |
|
1632 << " Order number: " << slave.order << endl |
|
1633 << " Device name: " << slave.name << endl; |
|
1634 |
|
1635 if (slave.mailbox_protocols & EC_MBOX_COE) { |
|
1636 cout << " CoE details:" << endl |
|
1637 << " Enable Sdo: " |
|
1638 << (slave.coe_details.enable_sdo ? "yes" : "no") << endl |
|
1639 << " Enable Sdo Info: " |
|
1640 << (slave.coe_details.enable_sdo_info ? "yes" : "no") << endl |
|
1641 << " Enable Pdo Assign: " |
|
1642 << (slave.coe_details.enable_pdo_assign |
|
1643 ? "yes" : "no") << endl |
|
1644 << " Enable Pdo Configuration: " |
|
1645 << (slave.coe_details.enable_pdo_configuration |
|
1646 ? "yes" : "no") << endl |
|
1647 << " Enable Upload at startup: " |
|
1648 << (slave.coe_details.enable_upload_at_startup |
|
1649 ? "yes" : "no") << endl |
|
1650 << " Enable Sdo complete access: " |
|
1651 << (slave.coe_details.enable_sdo_complete_access |
|
1652 ? "yes" : "no") << endl; |
|
1653 } |
|
1654 |
|
1655 cout << " Flags:" << endl |
|
1656 << " Enable SafeOp: " |
|
1657 << (slave.general_flags.enable_safeop ? "yes" : "no") << endl |
|
1658 << " Enable notLRW: " |
|
1659 << (slave.general_flags.enable_not_lrw ? "yes" : "no") << endl |
|
1660 << " Current consumption: " |
|
1661 << dec << slave.current_on_ebus << " mA" << endl; |
|
1662 } |
|
1663 } |
|
1664 |
|
1665 /****************************************************************************/ |
|
1666 |
|
1667 void Master::generateSlaveXml(uint16_t slavePosition) |
|
1668 { |
|
1669 ec_ioctl_slave_t slave; |
|
1670 ec_ioctl_slave_sync_t sync; |
|
1671 ec_ioctl_slave_sync_pdo_t pdo; |
|
1672 string pdoType; |
|
1673 ec_ioctl_slave_sync_pdo_entry_t entry; |
|
1674 unsigned int i, j, k; |
|
1675 |
|
1676 getSlave(&slave, slavePosition); |
|
1677 |
|
1678 cout |
|
1679 << "<?xml version=\"1.0\" ?>" << endl |
|
1680 << " <EtherCATInfo>" << endl |
|
1681 << " <!-- Slave " << slave.position << " -->" << endl |
|
1682 << " <Vendor>" << endl |
|
1683 << " <Id>" << slave.vendor_id << "</Id>" << endl |
|
1684 << " </Vendor>" << endl |
|
1685 << " <Descriptions>" << endl |
|
1686 << " <Devices>" << endl |
|
1687 << " <Device>" << endl |
|
1688 << " <Type ProductCode=\"#x" |
|
1689 << hex << setfill('0') << setw(8) << slave.product_code |
|
1690 << "\" RevisionNo=\"#x" |
|
1691 << hex << setfill('0') << setw(8) << slave.revision_number |
|
1692 << "\">" << slave.order << "</Type>" << endl; |
|
1693 |
|
1694 if (strlen(slave.name)) { |
|
1695 cout |
|
1696 << " <Name><![CDATA[" |
|
1697 << slave.name |
|
1698 << "]]></Name>" << endl; |
|
1699 } |
|
1700 |
|
1701 for (i = 0; i < slave.sync_count; i++) { |
|
1702 getSync(&sync, slavePosition, i); |
|
1703 |
|
1704 cout |
|
1705 << " <Sm Enable=\"" << dec << (unsigned int) sync.enable |
|
1706 << "\" StartAddress=\"" << sync.physical_start_address |
|
1707 << "\" ControlByte=\"" << (unsigned int) sync.control_register |
|
1708 << "\" DefaultSize=\"" << sync.default_size |
|
1709 << "\" />" << endl; |
|
1710 } |
|
1711 |
|
1712 for (i = 0; i < slave.sync_count; i++) { |
|
1713 getSync(&sync, slavePosition, i); |
|
1714 |
|
1715 for (j = 0; j < sync.pdo_count; j++) { |
|
1716 getPdo(&pdo, slavePosition, i, j); |
|
1717 pdoType = (sync.control_register & 0x04 ? "R" : "T"); |
|
1718 pdoType += "xPdo"; |
|
1719 |
|
1720 cout |
|
1721 << " <" << pdoType |
|
1722 << " Sm=\"" << i << "\" Fixed=\"1\" Mandatory=\"1\">" << endl |
|
1723 << " <Index>#x" |
|
1724 << hex << setfill('0') << setw(4) << pdo.index |
|
1725 << "</Index>" << endl |
|
1726 << " <Name>" << pdo.name << "</Name>" << endl; |
|
1727 |
|
1728 for (k = 0; k < pdo.entry_count; k++) { |
|
1729 getPdoEntry(&entry, slavePosition, i, j, k); |
|
1730 |
|
1731 cout |
|
1732 << " <Entry>" << endl |
|
1733 << " <Index>#x" |
|
1734 << hex << setfill('0') << setw(4) << entry.index |
|
1735 << "</Index>" << endl; |
|
1736 if (entry.index) |
|
1737 cout |
|
1738 << " <SubIndex>" |
|
1739 << dec << (unsigned int) entry.subindex |
|
1740 << "</SubIndex>" << endl; |
|
1741 |
|
1742 cout |
|
1743 << " <BitLen>" |
|
1744 << dec << (unsigned int) entry.bit_length |
|
1745 << "</BitLen>" << endl; |
|
1746 |
|
1747 if (entry.index) { |
|
1748 cout |
|
1749 << " <Name>" << entry.name |
|
1750 << "</Name>" << endl |
|
1751 << " <DataType>"; |
|
1752 |
|
1753 if (entry.bit_length == 1) { |
|
1754 cout << "BOOL"; |
|
1755 } else if (!(entry.bit_length % 8)) { |
|
1756 if (entry.bit_length <= 64) |
|
1757 cout << "UINT" << (unsigned int) entry.bit_length; |
|
1758 else |
|
1759 cout << "STRING(" |
|
1760 << (unsigned int) (entry.bit_length / 8) |
|
1761 << ")"; |
|
1762 } else { |
|
1763 cerr << "Invalid bit length " |
|
1764 << (unsigned int) entry.bit_length << endl; |
|
1765 } |
|
1766 |
|
1767 cout << "</DataType>" << endl; |
|
1768 } |
|
1769 |
|
1770 cout << " </Entry>" << endl; |
|
1771 } |
|
1772 |
|
1773 cout |
|
1774 << " </" << pdoType << ">" << endl; |
|
1775 } |
|
1776 } |
|
1777 |
|
1778 cout |
|
1779 << " </Device>" << endl |
|
1780 << " </Devices>" << endl |
|
1781 << " </Descriptions>" << endl |
|
1782 << "</EtherCATInfo>" << endl; |
|
1783 } |
|
1784 |
|
1785 /****************************************************************************/ |
|
1786 |
|
1787 unsigned int Master::slaveCount() |
|
1788 { |
|
1789 ec_ioctl_master_t data; |
|
1790 |
|
1791 getMaster(&data); |
|
1792 return data.slave_count; |
|
1793 } |
|
1794 |
|
1795 /****************************************************************************/ |
|
1796 |
|
1797 void Master::getMaster(ec_ioctl_master_t *data) |
|
1798 { |
|
1799 if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) { |
|
1800 stringstream err; |
|
1801 err << "Failed to get master information: " << strerror(errno); |
|
1802 throw MasterException(err.str()); |
|
1803 } |
|
1804 } |
|
1805 |
|
1806 /****************************************************************************/ |
|
1807 |
|
1808 void Master::getConfig(ec_ioctl_config_t *data, unsigned int index) |
|
1809 { |
|
1810 data->config_index = index; |
|
1811 |
|
1812 if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) { |
|
1813 stringstream err; |
|
1814 err << "Failed to get slave configuration: " << strerror(errno); |
|
1815 throw MasterException(err.str()); |
|
1816 } |
|
1817 } |
|
1818 |
|
1819 /****************************************************************************/ |
|
1820 |
|
1821 void Master::getConfigPdo( |
|
1822 ec_ioctl_config_pdo_t *data, |
|
1823 unsigned int index, |
|
1824 uint8_t sync_index, |
|
1825 uint16_t pdo_pos |
|
1826 ) |
|
1827 { |
|
1828 data->config_index = index; |
|
1829 data->sync_index = sync_index; |
|
1830 data->pdo_pos = pdo_pos; |
|
1831 |
|
1832 if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) { |
|
1833 stringstream err; |
|
1834 err << "Failed to get slave config Pdo: " << strerror(errno); |
|
1835 throw MasterException(err.str()); |
|
1836 } |
|
1837 } |
|
1838 |
|
1839 /****************************************************************************/ |
|
1840 |
|
1841 void Master::getConfigPdoEntry( |
|
1842 ec_ioctl_config_pdo_entry_t *data, |
|
1843 unsigned int index, |
|
1844 uint8_t sync_index, |
|
1845 uint16_t pdo_pos, |
|
1846 uint8_t entry_pos |
|
1847 ) |
|
1848 { |
|
1849 data->config_index = index; |
|
1850 data->sync_index = sync_index; |
|
1851 data->pdo_pos = pdo_pos; |
|
1852 data->entry_pos = entry_pos; |
|
1853 |
|
1854 if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) { |
|
1855 stringstream err; |
|
1856 err << "Failed to get slave config Pdo entry: " << strerror(errno); |
|
1857 throw MasterException(err.str()); |
|
1858 } |
|
1859 } |
|
1860 |
|
1861 /****************************************************************************/ |
|
1862 |
|
1863 void Master::getConfigSdo( |
|
1864 ec_ioctl_config_sdo_t *data, |
|
1865 unsigned int index, |
|
1866 unsigned int sdo_pos |
|
1867 ) |
|
1868 { |
|
1869 data->config_index = index; |
|
1870 data->sdo_pos = sdo_pos; |
|
1871 |
|
1872 if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) { |
|
1873 stringstream err; |
|
1874 err << "Failed to get slave config Sdo: " << strerror(errno); |
|
1875 throw MasterException(err.str()); |
|
1876 } |
|
1877 } |
|
1878 |
|
1879 /****************************************************************************/ |
|
1880 |
|
1881 void Master::getDomain(ec_ioctl_domain_t *data, unsigned int index) |
|
1882 { |
|
1883 data->index = index; |
|
1884 |
|
1885 if (ioctl(fd, EC_IOCTL_DOMAIN, data)) { |
|
1886 stringstream err; |
|
1887 err << "Failed to get domain: "; |
|
1888 if (errno == EINVAL) |
|
1889 err << "Domain " << index << " does not exist!"; |
|
1890 else |
|
1891 err << strerror(errno); |
|
1892 throw MasterException(err.str()); |
|
1893 } |
|
1894 } |
|
1895 |
|
1896 /****************************************************************************/ |
|
1897 |
|
1898 void Master::getData(ec_ioctl_domain_data_t *data, unsigned int domainIndex, |
|
1899 unsigned int dataSize, unsigned char *mem) |
|
1900 { |
|
1901 data->domain_index = domainIndex; |
|
1902 data->data_size = dataSize; |
|
1903 data->target = mem; |
|
1904 |
|
1905 if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) { |
|
1906 stringstream err; |
|
1907 err << "Failed to get domain data: " << strerror(errno); |
|
1908 throw MasterException(err.str()); |
|
1909 } |
|
1910 } |
|
1911 |
|
1912 /****************************************************************************/ |
|
1913 |
|
1914 void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex) |
|
1915 { |
|
1916 slave->position = slaveIndex; |
|
1917 |
|
1918 if (ioctl(fd, EC_IOCTL_SLAVE, slave)) { |
|
1919 stringstream err; |
|
1920 err << "Failed to get slave: "; |
|
1921 if (errno == EINVAL) |
|
1922 err << "Slave " << slaveIndex << " does not exist!"; |
|
1923 else |
|
1924 err << strerror(errno); |
|
1925 throw MasterException(err.str()); |
|
1926 } |
|
1927 } |
|
1928 |
|
1929 /****************************************************************************/ |
|
1930 |
|
1931 void Master::getFmmu( |
|
1932 ec_ioctl_domain_fmmu_t *fmmu, |
|
1933 unsigned int domainIndex, |
|
1934 unsigned int fmmuIndex |
|
1935 ) |
|
1936 { |
|
1937 fmmu->domain_index = domainIndex; |
|
1938 fmmu->fmmu_index = fmmuIndex; |
|
1939 |
|
1940 if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) { |
|
1941 stringstream err; |
|
1942 err << "Failed to get domain FMMU: "; |
|
1943 if (errno == EINVAL) |
|
1944 err << "Either domain " << domainIndex << " does not exist, " |
|
1945 << "or it contains less than " << (unsigned int) fmmuIndex + 1 |
|
1946 << " FMMus!"; |
|
1947 else |
|
1948 err << strerror(errno); |
|
1949 throw MasterException(err.str()); |
|
1950 } |
|
1951 } |
|
1952 |
|
1953 /****************************************************************************/ |
|
1954 |
|
1955 void Master::getSync( |
|
1956 ec_ioctl_slave_sync_t *sync, |
|
1957 uint16_t slaveIndex, |
|
1958 uint8_t syncIndex |
|
1959 ) |
|
1960 { |
|
1961 sync->slave_position = slaveIndex; |
|
1962 sync->sync_index = syncIndex; |
|
1963 |
|
1964 if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) { |
|
1965 stringstream err; |
|
1966 err << "Failed to get sync manager: "; |
|
1967 if (errno == EINVAL) |
|
1968 err << "Either slave " << slaveIndex << " does not exist, " |
|
1969 << "or it contains less than " << (unsigned int) syncIndex + 1 |
|
1970 << " sync managers!"; |
|
1971 else |
|
1972 err << strerror(errno); |
|
1973 throw MasterException(err.str()); |
|
1974 } |
|
1975 } |
|
1976 |
|
1977 /****************************************************************************/ |
|
1978 |
|
1979 void Master::getPdo( |
|
1980 ec_ioctl_slave_sync_pdo_t *pdo, |
|
1981 uint16_t slaveIndex, |
|
1982 uint8_t syncIndex, |
|
1983 uint8_t pdoPos |
|
1984 ) |
|
1985 { |
|
1986 pdo->slave_position = slaveIndex; |
|
1987 pdo->sync_index = syncIndex; |
|
1988 pdo->pdo_pos = pdoPos; |
|
1989 |
|
1990 if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) { |
|
1991 stringstream err; |
|
1992 err << "Failed to get Pdo: "; |
|
1993 if (errno == EINVAL) |
|
1994 err << "Either slave " << slaveIndex << " does not exist, " |
|
1995 << "or it contains less than " << (unsigned int) syncIndex + 1 |
|
1996 << " sync managers, or sync manager " |
|
1997 << (unsigned int) syncIndex << " contains less than " |
|
1998 << pdoPos + 1 << " Pdos!" << endl; |
|
1999 else |
|
2000 err << strerror(errno); |
|
2001 throw MasterException(err.str()); |
|
2002 } |
|
2003 } |
|
2004 |
|
2005 /****************************************************************************/ |
|
2006 |
|
2007 void Master::getPdoEntry( |
|
2008 ec_ioctl_slave_sync_pdo_entry_t *entry, |
|
2009 uint16_t slaveIndex, |
|
2010 uint8_t syncIndex, |
|
2011 uint8_t pdoPos, |
|
2012 uint8_t entryPos |
|
2013 ) |
|
2014 { |
|
2015 entry->slave_position = slaveIndex; |
|
2016 entry->sync_index = syncIndex; |
|
2017 entry->pdo_pos = pdoPos; |
|
2018 entry->entry_pos = entryPos; |
|
2019 |
|
2020 if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) { |
|
2021 stringstream err; |
|
2022 err << "Failed to get Pdo entry: "; |
|
2023 if (errno == EINVAL) |
|
2024 err << "Either slave " << slaveIndex << " does not exist, " |
|
2025 << "or it contains less than " << (unsigned int) syncIndex + 1 |
|
2026 << " sync managers, or sync manager " |
|
2027 << (unsigned int) syncIndex << " contains less than " |
|
2028 << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos |
|
2029 << " contains less than " << (unsigned int) entryPos + 1 |
|
2030 << " entries!" << endl; |
|
2031 else |
|
2032 err << strerror(errno); |
|
2033 throw MasterException(err.str()); |
|
2034 } |
|
2035 } |
|
2036 |
|
2037 /****************************************************************************/ |
|
2038 |
|
2039 void Master::getSdo( |
|
2040 ec_ioctl_slave_sdo_t *sdo, |
|
2041 uint16_t slaveIndex, |
|
2042 uint16_t sdoPosition |
|
2043 ) |
|
2044 { |
|
2045 sdo->slave_position = slaveIndex; |
|
2046 sdo->sdo_position = sdoPosition; |
|
2047 |
|
2048 if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) { |
|
2049 stringstream err; |
|
2050 err << "Failed to get Sdo: "; |
|
2051 if (errno == EINVAL) |
|
2052 err << "Either slave " << slaveIndex << " does not exist, " |
|
2053 << "or it contains less than " << sdoPosition + 1 << " Sdos!" |
|
2054 << endl; |
|
2055 else |
|
2056 err << strerror(errno); |
|
2057 throw MasterException(err.str()); |
|
2058 } |
|
2059 } |
|
2060 |
|
2061 /****************************************************************************/ |
|
2062 |
|
2063 void Master::getSdoEntry( |
|
2064 ec_ioctl_slave_sdo_entry_t *entry, |
|
2065 uint16_t slaveIndex, |
|
2066 int sdoSpec, |
|
2067 uint8_t entrySubindex |
|
2068 ) |
|
2069 { |
|
2070 entry->slave_position = slaveIndex; |
|
2071 entry->sdo_spec = sdoSpec; |
|
2072 entry->sdo_entry_subindex = entrySubindex; |
|
2073 |
|
2074 if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) { |
|
2075 stringstream err; |
|
2076 err << "Failed to get Sdo entry: "; |
|
2077 err << strerror(errno); |
|
2078 throw MasterException(err.str()); |
|
2079 } |
|
2080 } |
|
2081 |
|
2082 /****************************************************************************/ |
|
2083 |
|
2084 void Master::requestState( |
|
2085 uint16_t slavePosition, |
|
2086 uint8_t state |
|
2087 ) |
|
2088 { |
|
2089 ec_ioctl_slave_state_t data; |
|
2090 |
|
2091 data.slave_position = slavePosition; |
|
2092 data.requested_state = state; |
|
2093 |
|
2094 if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) { |
|
2095 stringstream err; |
|
2096 err << "Failed to request slave state: "; |
|
2097 if (errno == EINVAL) |
|
2098 err << "Slave " << slavePosition << " does not exist!"; |
|
2099 else |
|
2100 err << strerror(errno); |
|
2101 throw MasterException(err.str()); |
|
2102 } |
|
2103 } |
|
2104 |
|
2105 /****************************************************************************/ |
|
2106 |
|
2107 string Master::slaveState(uint8_t state) |
|
2108 { |
|
2109 switch (state) { |
|
2110 case 1: return "INIT"; |
|
2111 case 2: return "PREOP"; |
|
2112 case 4: return "SAFEOP"; |
|
2113 case 8: return "OP"; |
|
2114 default: return "???"; |
|
2115 } |
|
2116 } |
|
2117 |
|
2118 /****************************************************************************/ |
|
2119 |
|
2120 void Master::printRawData( |
|
2121 const uint8_t *data, |
|
2122 unsigned int size) |
|
2123 { |
|
2124 cout << hex << setfill('0'); |
|
2125 while (size--) { |
|
2126 cout << "0x" << setw(2) << (unsigned int) *data++; |
|
2127 if (size) |
|
2128 cout << " "; |
|
2129 } |
|
2130 cout << endl; |
|
2131 } |
|
2132 |
|
2133 /*****************************************************************************/ |
|
2134 |
|
2135 /** |
|
2136 * Calculates the SII checksum field. |
|
2137 * |
|
2138 * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an |
|
2139 * initial value of 0xff (see IEC 61158-6-12 ch. 5.4). |
|
2140 * |
|
2141 * The below code was originally generated with PYCRC |
|
2142 * http://www.tty1.net/pycrc |
|
2143 * |
|
2144 * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff |
|
2145 * --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit |
|
2146 * |
|
2147 * \return CRC8 |
|
2148 */ |
|
2149 uint8_t Master::calcSiiCrc( |
|
2150 const uint8_t *data, /**< pointer to data */ |
|
2151 size_t length /**< number of bytes in \a data */ |
|
2152 ) |
|
2153 { |
|
2154 unsigned int i; |
|
2155 uint8_t bit, byte, crc = 0x48; |
|
2156 |
|
2157 while (length--) { |
|
2158 byte = *data++; |
|
2159 for (i = 0; i < 8; i++) { |
|
2160 bit = crc & 0x80; |
|
2161 crc = (crc << 1) | ((byte >> (7 - i)) & 0x01); |
|
2162 if (bit) crc ^= 0x07; |
|
2163 } |
|
2164 } |
|
2165 |
|
2166 for (i = 0; i < 8; i++) { |
|
2167 bit = crc & 0x80; |
|
2168 crc <<= 1; |
|
2169 if (bit) crc ^= 0x07; |
|
2170 } |
|
2171 |
|
2172 return crc; |
|
2173 } |
|
2174 |
|
2175 /*****************************************************************************/ |
|