226 |
226 |
227 data_reg->slave = sync->slave; |
227 data_reg->slave = sync->slave; |
228 data_reg->sync = sync; |
228 data_reg->sync = sync; |
229 data_reg->sync_offset = byte_offset; |
229 data_reg->sync_offset = byte_offset; |
230 data_reg->data_ptr = data_ptr; |
230 data_reg->data_ptr = data_ptr; |
231 list_add_tail(&data_reg->list, &domain->data_regs); |
|
232 |
|
233 return 0; |
|
234 } |
|
235 |
|
236 /*****************************************************************************/ |
|
237 |
|
238 /** |
|
239 Registeres a PDO range. |
|
240 \return 0 in case of success, else < 0 |
|
241 */ |
|
242 |
|
243 int ec_domain_reg_pdo_range(ec_domain_t *domain, /**< EtherCAT domain */ |
|
244 ec_slave_t *slave, /**< slave */ |
|
245 ec_direction_t dir, /**< data direction */ |
|
246 uint16_t offset, /**< offset */ |
|
247 uint16_t length, /**< length */ |
|
248 void **data_ptr /**< pointer to the process data |
|
249 pointer */ |
|
250 ) |
|
251 { |
|
252 ec_data_reg_t *data_reg; |
|
253 ec_sync_t *sync; |
|
254 uint16_t sync_length; |
|
255 |
|
256 if (!(sync = ec_slave_get_pdo_sync(slave, dir))) { |
|
257 EC_ERR("No sync manager found for PDO range.\n"); |
|
258 return -1; |
|
259 } |
|
260 |
|
261 // Allocate memory for data registration object |
|
262 if (!(data_reg = |
|
263 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
|
264 EC_ERR("Failed to allocate data registration.\n"); |
|
265 return -1; |
|
266 } |
|
267 |
|
268 if (ec_slave_prepare_fmmu(slave, domain, sync)) { |
|
269 EC_ERR("FMMU configuration failed.\n"); |
|
270 kfree(data_reg); |
|
271 return -1; |
|
272 } |
|
273 |
|
274 data_reg->slave = slave; |
|
275 data_reg->sync = sync; |
|
276 data_reg->sync_offset = offset; |
|
277 data_reg->data_ptr = data_ptr; |
|
278 |
|
279 // estimate sync manager length |
|
280 sync_length = offset + length; |
|
281 if (sync->est_length < sync_length) { |
|
282 sync->est_length = sync_length; |
|
283 if (domain->master->debug_level) { |
|
284 EC_DBG("Estimating length of sync manager %i of slave %i to %i.\n", |
|
285 sync->index, slave->ring_position, sync_length); |
|
286 } |
|
287 } |
|
288 |
|
289 list_add_tail(&data_reg->list, &domain->data_regs); |
231 list_add_tail(&data_reg->list, &domain->data_regs); |
290 |
232 |
291 return 0; |
233 return 0; |
292 } |
234 } |
293 |
235 |
458 * Realtime interface |
400 * Realtime interface |
459 *****************************************************************************/ |
401 *****************************************************************************/ |
460 |
402 |
461 /** |
403 /** |
462 * Registers a PDO for a domain. |
404 * Registers a PDO for a domain. |
463 * \return pointer to the slave on success, else NULL |
405 * \return 0 on success, else non-zero |
464 * \ingroup RealtimeInterface |
406 * \ingroup RealtimeInterface |
465 */ |
407 */ |
466 |
408 |
467 ec_slave_t *ecrt_domain_register_pdo( |
409 int ecrt_domain_register_pdo( |
468 ec_domain_t *domain, /**< EtherCAT domain */ |
410 ec_domain_t *domain, /**< EtherCAT domain */ |
469 const char *address, /**< ASCII address of the slave, |
411 ec_slave_t *slave, /**< EtherCAT slave */ |
470 see ecrt_master_get_slave() */ |
412 uint16_t pdo_entry_index, /**< PDO entry index */ |
471 uint32_t vendor_id, /**< vendor ID */ |
413 uint8_t pdo_entry_subindex, /**< PDO entry subindex */ |
472 uint32_t product_code, /**< product code */ |
|
473 uint16_t entry_index, /**< PDO entry index */ |
|
474 uint8_t entry_subindex, /**< PDO entry subindex */ |
|
475 void **data_ptr /**< address of the process data pointer */ |
414 void **data_ptr /**< address of the process data pointer */ |
476 ) |
415 ) |
477 { |
416 { |
478 ec_slave_t *slave; |
|
479 ec_master_t *master; |
|
480 ec_sync_t *sync; |
417 ec_sync_t *sync; |
481 const ec_pdo_t *pdo; |
418 const ec_pdo_t *pdo; |
482 const ec_pdo_entry_t *entry; |
419 const ec_pdo_entry_t *entry; |
483 unsigned int i; |
420 unsigned int i; |
484 |
421 |
485 master = domain->master; |
422 // search for PDO entry |
486 |
|
487 // translate address and validate slave |
|
488 if (!(slave = ecrt_master_get_slave(master, address))) return NULL; |
|
489 if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; |
|
490 |
|
491 for (i = 0; i < slave->sii_sync_count; i++) { |
423 for (i = 0; i < slave->sii_sync_count; i++) { |
492 sync = &slave->sii_syncs[i]; |
424 sync = &slave->sii_syncs[i]; |
493 list_for_each_entry(pdo, &sync->pdos, list) { |
425 list_for_each_entry(pdo, &sync->pdos, list) { |
494 list_for_each_entry(entry, &pdo->entries, list) { |
426 list_for_each_entry(entry, &pdo->entries, list) { |
495 if (entry->index != entry_index || |
427 if (entry->index != pdo_entry_index || |
496 entry->subindex != entry_subindex) continue; |
428 entry->subindex != pdo_entry_subindex) continue; |
497 |
429 |
|
430 // PDO entry found |
498 if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { |
431 if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { |
499 return NULL; |
432 return -1; |
500 } |
433 } |
501 |
434 |
502 return slave; |
435 return 0; |
503 } |
436 } |
504 } |
437 } |
505 } |
438 } |
506 |
439 |
507 EC_ERR("PDO entry 0x%04X:%u is not mapped in slave %u.\n", |
440 EC_ERR("PDO entry 0x%04X:%u is not mapped in slave %u.\n", |
508 entry_index, entry_subindex, slave->ring_position); |
441 pdo_entry_index, pdo_entry_subindex, slave->ring_position); |
509 return NULL; |
442 return -1; |
510 } |
443 } |
511 |
444 |
512 /*****************************************************************************/ |
445 /*****************************************************************************/ |
513 |
446 |
514 /** |
447 /** |
518 * \ingroup RealtimeInterface |
451 * \ingroup RealtimeInterface |
519 */ |
452 */ |
520 |
453 |
521 int ecrt_domain_register_pdo_list( |
454 int ecrt_domain_register_pdo_list( |
522 ec_domain_t *domain, /**< EtherCAT domain */ |
455 ec_domain_t *domain, /**< EtherCAT domain */ |
523 const ec_pdo_reg_t *pdos /**< array of PDO registrations */ |
456 const ec_pdo_reg_t *pdo_regs /**< array of PDO registrations */ |
524 ) |
457 ) |
525 { |
458 { |
526 const ec_pdo_reg_t *pdo; |
459 const ec_pdo_reg_t *reg; |
527 |
460 ec_slave_t *slave; |
528 for (pdo = pdos; pdo->slave_address; pdo++) |
461 |
529 if (!ecrt_domain_register_pdo(domain, pdo->slave_address, |
462 for (reg = pdo_regs; reg->slave_address; reg++) { |
530 pdo->vendor_id, pdo->product_code, |
463 if (!(slave = ecrt_master_get_slave(domain->master, |
531 pdo->pdo_entry_index, pdo->pdo_entry_subindex, |
464 reg->slave_address, reg->vendor_id, |
532 pdo->data_ptr)) |
465 reg->product_code))) |
533 return -1; |
466 return -1; |
534 |
467 |
|
468 if (ecrt_domain_register_pdo(domain, slave, reg->pdo_entry_index, |
|
469 reg->pdo_entry_subindex, reg->data_ptr)) |
|
470 return -1; |
|
471 } |
|
472 |
535 return 0; |
473 return 0; |
536 } |
474 } |
537 |
475 |
538 /*****************************************************************************/ |
476 /*****************************************************************************/ |
539 |
477 |
540 /** |
478 /** |
541 Registers a PDO range in a domain. |
479 * Registers a PDO range in a domain. |
542 \return pointer to the slave on success, else NULL |
480 * \return 0 on success, else non-zero |
543 \ingroup RealtimeInterface |
481 * \ingroup RealtimeInterface |
544 */ |
482 */ |
545 |
483 |
546 ec_slave_t *ecrt_domain_register_pdo_range(ec_domain_t *domain, |
484 int ecrt_domain_register_pdo_range( |
547 /**< EtherCAT domain */ |
485 ec_domain_t *domain, /**< EtherCAT domain */ |
548 const char *address, |
486 ec_slave_t *slave, /**< EtherCAT slave */ |
549 /**< ASCII address of the slave, |
487 ec_direction_t dir, /**< data direction */ |
550 see ecrt_master_get_slave() */ |
488 uint16_t offset, /**< offset in slave's PDO range */ |
551 uint32_t vendor_id, |
489 uint16_t length, /**< length of this range */ |
552 /**< vendor ID */ |
490 void **data_ptr /**< address of the process data pointer */ |
553 uint32_t product_code, |
491 ) |
554 /**< product code */ |
492 { |
555 ec_direction_t direction, |
493 ec_data_reg_t *data_reg; |
556 /**< data direction */ |
494 ec_sync_t *sync; |
557 uint16_t offset, |
495 uint16_t sync_length; |
558 /**< offset in slave's PDO range */ |
496 |
559 uint16_t length, |
497 if (!(sync = ec_slave_get_pdo_sync(slave, dir))) { |
560 /**< length of this range */ |
498 EC_ERR("No sync manager found for PDO range.\n"); |
561 void **data_ptr |
499 return -1; |
562 /**< address of the process data |
500 } |
563 pointer */ |
501 |
564 ) |
502 // Allocate memory for data registration object |
565 { |
503 if (!(data_reg = |
566 ec_slave_t *slave; |
504 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
567 ec_master_t *master; |
505 EC_ERR("Failed to allocate data registration.\n"); |
568 |
506 return -1; |
569 master = domain->master; |
507 } |
570 |
508 |
571 // translate address and validate slave |
509 if (ec_slave_prepare_fmmu(slave, domain, sync)) { |
572 if (!(slave = ecrt_master_get_slave(master, address))) return NULL; |
510 EC_ERR("FMMU configuration failed.\n"); |
573 if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; |
511 kfree(data_reg); |
574 |
512 return -1; |
575 if (ec_domain_reg_pdo_range(domain, slave, |
513 } |
576 direction, offset, length, data_ptr)) { |
514 |
577 return NULL; |
515 data_reg->slave = slave; |
578 } |
516 data_reg->sync = sync; |
579 |
517 data_reg->sync_offset = offset; |
580 return slave; |
518 data_reg->data_ptr = data_ptr; |
|
519 |
|
520 // estimate sync manager length |
|
521 sync_length = offset + length; |
|
522 if (sync->est_length < sync_length) { |
|
523 sync->est_length = sync_length; |
|
524 if (domain->master->debug_level) { |
|
525 EC_DBG("Estimating length of sync manager %i of slave %i to %i.\n", |
|
526 sync->index, slave->ring_position, sync_length); |
|
527 } |
|
528 } |
|
529 |
|
530 list_add_tail(&data_reg->list, &domain->data_regs); |
|
531 return 0; |
581 } |
532 } |
582 |
533 |
583 /*****************************************************************************/ |
534 /*****************************************************************************/ |
584 |
535 |
585 /** |
536 /** |