180 } |
180 } |
181 |
181 |
182 /*****************************************************************************/ |
182 /*****************************************************************************/ |
183 |
183 |
184 /** |
184 /** |
185 Registeres a PDO entry. |
185 * Registers a PDO entry. |
186 \return 0 in case of success, else < 0 |
186 * \return 0 in case of success, else < 0 |
187 */ |
187 */ |
188 |
188 |
189 int ec_domain_reg_pdo_entry(ec_domain_t *domain, /**< EtherCAT domain */ |
189 int ec_domain_reg_pdo_entry( |
190 ec_slave_t *slave, /**< slave */ |
190 ec_domain_t *domain, /**< EtherCAT domain */ |
191 const ec_pdo_t *pdo, /**< PDO */ |
191 ec_sync_t *sync, /**< sync manager */ |
192 const ec_pdo_entry_t *entry, |
192 const ec_pdo_entry_t *entry, /**< PDO entry to register */ |
193 /**< PDO registration entry */ |
193 void **data_ptr /**< pointer to the process data pointer */ |
194 void **data_ptr /**< pointer to the process data |
194 ) |
195 pointer */ |
|
196 ) |
|
197 { |
195 { |
198 ec_data_reg_t *data_reg; |
196 ec_data_reg_t *data_reg; |
199 const ec_sync_t *sync; |
|
200 const ec_pdo_t *other_pdo; |
197 const ec_pdo_t *other_pdo; |
201 const ec_pdo_entry_t *other_entry; |
198 const ec_pdo_entry_t *other_entry; |
202 unsigned int bit_offset, byte_offset; |
199 unsigned int bit_offset, byte_offset; |
203 |
200 |
204 // Find sync manager for PDO |
|
205 if (pdo->sync_index >= slave->sii_sync_count) { |
|
206 EC_ERR("No sync manager for PDO 0x%04X:%i.", |
|
207 pdo->index, entry->subindex); |
|
208 return -1; |
|
209 } |
|
210 sync = &slave->sii_syncs[pdo->sync_index]; |
|
211 |
|
212 // Calculate offset (in sync manager) for process data pointer |
201 // Calculate offset (in sync manager) for process data pointer |
213 bit_offset = 0; |
202 bit_offset = 0; |
214 byte_offset = 0; |
203 byte_offset = 0; |
215 list_for_each_entry(other_pdo, &slave->sii_pdos, list) { |
204 list_for_each_entry(other_pdo, &sync->pdos, list) { |
216 if (other_pdo->sync_index != sync->index) continue; |
|
217 |
|
218 list_for_each_entry(other_entry, &other_pdo->entries, list) { |
205 list_for_each_entry(other_entry, &other_pdo->entries, list) { |
219 if (other_entry == entry) { |
206 if (other_entry == entry) { |
220 byte_offset = bit_offset / 8; |
207 byte_offset = bit_offset / 8; |
221 break; |
208 break; |
222 } |
209 } |
229 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
216 (ec_data_reg_t *) kmalloc(sizeof(ec_data_reg_t), GFP_KERNEL))) { |
230 EC_ERR("Failed to allocate data registration.\n"); |
217 EC_ERR("Failed to allocate data registration.\n"); |
231 return -1; |
218 return -1; |
232 } |
219 } |
233 |
220 |
234 if (ec_slave_prepare_fmmu(slave, domain, sync)) { |
221 if (ec_slave_prepare_fmmu(sync->slave, domain, sync)) { |
235 EC_ERR("FMMU configuration failed.\n"); |
222 EC_ERR("FMMU configuration failed.\n"); |
236 kfree(data_reg); |
223 kfree(data_reg); |
237 return -1; |
224 return -1; |
238 } |
225 } |
239 |
226 |
240 data_reg->slave = slave; |
227 data_reg->slave = sync->slave; |
241 data_reg->sync = sync; |
228 data_reg->sync = sync; |
242 data_reg->sync_offset = byte_offset; |
229 data_reg->sync_offset = byte_offset; |
243 data_reg->data_ptr = data_ptr; |
230 data_reg->data_ptr = data_ptr; |
244 |
|
245 list_add_tail(&data_reg->list, &domain->data_regs); |
231 list_add_tail(&data_reg->list, &domain->data_regs); |
246 |
232 |
247 return 0; |
233 return 0; |
248 } |
234 } |
249 |
235 |
384 list_for_each_entry(slave, &domain->master->slaves, list) { |
370 list_for_each_entry(slave, &domain->master->slaves, list) { |
385 for (j = 0; j < slave->fmmu_count; j++) { |
371 for (j = 0; j < slave->fmmu_count; j++) { |
386 fmmu = &slave->fmmus[j]; |
372 fmmu = &slave->fmmus[j]; |
387 if (fmmu->domain == domain) { |
373 if (fmmu->domain == domain) { |
388 fmmu->logical_start_address = base_address + domain->data_size; |
374 fmmu->logical_start_address = base_address + domain->data_size; |
389 sync_size = ec_slave_calc_sync_size(slave, fmmu->sync); |
375 sync_size = ec_sync_size(fmmu->sync); |
390 domain->data_size += sync_size; |
376 domain->data_size += sync_size; |
391 if (datagram_data_size + sync_size > EC_MAX_DATA_SIZE) { |
377 if (datagram_data_size + sync_size > EC_MAX_DATA_SIZE) { |
392 if (ec_domain_add_datagram(domain, datagram_offset, |
378 if (ec_domain_add_datagram(domain, datagram_offset, |
393 datagram_data_size)) return -1; |
379 datagram_data_size)) return -1; |
394 datagram_offset += datagram_data_size; |
380 datagram_offset += datagram_data_size; |
471 /****************************************************************************** |
457 /****************************************************************************** |
472 * Realtime interface |
458 * Realtime interface |
473 *****************************************************************************/ |
459 *****************************************************************************/ |
474 |
460 |
475 /** |
461 /** |
476 Registers a PDO in a domain. |
462 * Registers a PDO for a domain. |
477 \return pointer to the slave on success, else NULL |
463 * \return pointer to the slave on success, else NULL |
478 \ingroup RealtimeInterface |
464 * \ingroup RealtimeInterface |
479 */ |
465 */ |
480 |
466 |
481 ec_slave_t *ecrt_domain_register_pdo(ec_domain_t *domain, |
467 ec_slave_t *ecrt_domain_register_pdo( |
482 /**< EtherCAT domain */ |
468 ec_domain_t *domain, /**< EtherCAT domain */ |
483 const char *address, |
469 const char *address, /**< ASCII address of the slave, |
484 /**< ASCII address of the slave, |
470 see ecrt_master_get_slave() */ |
485 see ecrt_master_get_slave() */ |
471 uint32_t vendor_id, /**< vendor ID */ |
486 uint32_t vendor_id, |
472 uint32_t product_code, /**< product code */ |
487 /**< vendor ID */ |
473 uint16_t entry_index, /**< PDO entry index */ |
488 uint32_t product_code, |
474 uint8_t entry_subindex, /**< PDO entry subindex */ |
489 /**< product code */ |
475 void **data_ptr /**< address of the process data pointer */ |
490 uint16_t pdo_index, |
476 ) |
491 /**< PDO index */ |
|
492 uint8_t pdo_subindex, |
|
493 /**< PDO subindex */ |
|
494 void **data_ptr |
|
495 /**< address of the process data |
|
496 pointer */ |
|
497 ) |
|
498 { |
477 { |
499 ec_slave_t *slave; |
478 ec_slave_t *slave; |
500 ec_master_t *master; |
479 ec_master_t *master; |
|
480 ec_sync_t *sync; |
501 const ec_pdo_t *pdo; |
481 const ec_pdo_t *pdo; |
502 const ec_pdo_entry_t *entry; |
482 const ec_pdo_entry_t *entry; |
|
483 unsigned int i; |
503 |
484 |
504 master = domain->master; |
485 master = domain->master; |
505 |
486 |
506 // translate address and validate slave |
487 // translate address and validate slave |
507 if (!(slave = ecrt_master_get_slave(master, address))) return NULL; |
488 if (!(slave = ecrt_master_get_slave(master, address))) return NULL; |
508 if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; |
489 if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; |
509 |
490 |
510 list_for_each_entry(pdo, &slave->sii_pdos, list) { |
491 for (i = 0; i < slave->sii_sync_count; i++) { |
511 list_for_each_entry(entry, &pdo->entries, list) { |
492 sync = &slave->sii_syncs[i]; |
512 if (entry->index != pdo_index |
493 list_for_each_entry(pdo, &sync->pdos, list) { |
513 || entry->subindex != pdo_subindex) continue; |
494 list_for_each_entry(entry, &pdo->entries, list) { |
514 |
495 if (entry->index != entry_index || |
515 if (ec_domain_reg_pdo_entry(domain, slave, pdo, entry, data_ptr)) { |
496 entry->subindex != entry_subindex) continue; |
516 return NULL; |
497 |
|
498 if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { |
|
499 return NULL; |
|
500 } |
|
501 |
|
502 return slave; |
517 } |
503 } |
518 |
504 } |
519 return slave; |
505 } |
520 } |
506 |
521 } |
507 EC_ERR("PDO entry 0x%04X:%u is not mapped in slave %u.\n", |
522 |
508 entry_index, entry_subindex, slave->ring_position); |
523 EC_ERR("Slave %i does not provide PDO 0x%04X:%i.\n", |
|
524 slave->ring_position, pdo_index, pdo_subindex); |
|
525 return NULL; |
509 return NULL; |
526 } |
510 } |
527 |
511 |
528 /*****************************************************************************/ |
512 /*****************************************************************************/ |
529 |
513 |
530 /** |
514 /** |
531 Registeres a bunch of data fields. |
515 * Registers a bunch of data fields. |
532 \attention The list has to be terminated with a NULL structure ({})! |
516 * \attention The list has to be terminated with a NULL structure ({})! |
533 \return 0 in case of success, else < 0 |
517 * \return 0 in case of success, else < 0 |
534 \ingroup RealtimeInterface |
518 * \ingroup RealtimeInterface |
535 */ |
519 */ |
536 |
520 |
537 int ecrt_domain_register_pdo_list(ec_domain_t *domain, |
521 int ecrt_domain_register_pdo_list( |
538 /**< EtherCAT domain */ |
522 ec_domain_t *domain, /**< EtherCAT domain */ |
539 const ec_pdo_reg_t *pdos |
523 const ec_pdo_reg_t *pdos /**< array of PDO registrations */ |
540 /**< array of PDO registrations */ |
524 ) |
541 ) |
|
542 { |
525 { |
543 const ec_pdo_reg_t *pdo; |
526 const ec_pdo_reg_t *pdo; |
544 |
527 |
545 for (pdo = pdos; pdo->slave_address; pdo++) |
528 for (pdo = pdos; pdo->slave_address; pdo++) |
546 if (!ecrt_domain_register_pdo(domain, pdo->slave_address, |
529 if (!ecrt_domain_register_pdo(domain, pdo->slave_address, |
547 pdo->vendor_id, |
530 pdo->vendor_id, pdo->product_code, |
548 pdo->product_code, |
531 pdo->pdo_entry_index, pdo->pdo_entry_subindex, |
549 pdo->pdo_index, |
532 pdo->data_ptr)) |
550 pdo->pdo_subindex, |
|
551 pdo->data_ptr)) |
|
552 return -1; |
533 return -1; |
553 |
534 |
554 return 0; |
535 return 0; |
555 } |
536 } |
556 |
537 |