207 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
207 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
208 return; |
208 return; |
209 } |
209 } |
210 |
210 |
211 if (slave->master->debug_level) |
211 if (slave->master->debug_level) |
212 EC_DBG("Reading of Pdo configuration finished.\n"); |
212 EC_DBG("Reading of PDO configuration finished.\n"); |
213 |
213 |
214 fsm->state = ec_fsm_pdo_state_end; |
214 fsm->state = ec_fsm_pdo_state_end; |
215 } |
215 } |
216 |
216 |
217 /*****************************************************************************/ |
217 /*****************************************************************************/ |
218 |
218 |
219 /** Count assigned Pdos. |
219 /** Count assigned PDOs. |
220 */ |
220 */ |
221 void ec_fsm_pdo_read_state_pdo_count( |
221 void ec_fsm_pdo_read_state_pdo_count( |
222 ec_fsm_pdo_t *fsm /**< finite state machine */ |
222 ec_fsm_pdo_t *fsm /**< finite state machine */ |
223 ) |
223 ) |
224 { |
224 { |
225 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
225 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
226 |
226 |
227 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
227 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
228 EC_ERR("Failed to read number of assigned Pdos for SM%u.\n", |
228 EC_ERR("Failed to read number of assigned PDOs for SM%u.\n", |
229 fsm->sync_index); |
229 fsm->sync_index); |
230 fsm->state = ec_fsm_pdo_state_error; |
230 fsm->state = ec_fsm_pdo_state_error; |
231 return; |
231 return; |
232 } |
232 } |
233 |
233 |
234 if (fsm->request.data_size != sizeof(uint8_t)) { |
234 if (fsm->request.data_size != sizeof(uint8_t)) { |
235 EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X " |
235 EC_ERR("Invalid data size %u returned when uploading SDO 0x%04X:%02X " |
236 "from slave %u.\n", fsm->request.data_size, |
236 "from slave %u.\n", fsm->request.data_size, |
237 fsm->request.index, fsm->request.subindex, |
237 fsm->request.index, fsm->request.subindex, |
238 fsm->slave->ring_position); |
238 fsm->slave->ring_position); |
239 fsm->state = ec_fsm_pdo_state_error; |
239 fsm->state = ec_fsm_pdo_state_error; |
240 return; |
240 return; |
241 } |
241 } |
242 fsm->pdo_count = EC_READ_U8(fsm->request.data); |
242 fsm->pdo_count = EC_READ_U8(fsm->request.data); |
243 |
243 |
244 if (fsm->slave->master->debug_level) |
244 if (fsm->slave->master->debug_level) |
245 EC_DBG("%u Pdos assigned.\n", fsm->pdo_count); |
245 EC_DBG("%u PDOs assigned.\n", fsm->pdo_count); |
246 |
246 |
247 // read first Pdo |
247 // read first PDO |
248 fsm->pdo_pos = 1; |
248 fsm->pdo_pos = 1; |
249 ec_fsm_pdo_read_action_next_pdo(fsm); |
249 ec_fsm_pdo_read_action_next_pdo(fsm); |
250 } |
250 } |
251 |
251 |
252 /*****************************************************************************/ |
252 /*****************************************************************************/ |
253 |
253 |
254 /** Read next Pdo. |
254 /** Read next PDO. |
255 */ |
255 */ |
256 void ec_fsm_pdo_read_action_next_pdo( |
256 void ec_fsm_pdo_read_action_next_pdo( |
257 ec_fsm_pdo_t *fsm /**< finite state machine */ |
257 ec_fsm_pdo_t *fsm /**< finite state machine */ |
258 ) |
258 ) |
259 { |
259 { |
280 ec_fsm_pdo_read_action_next_sync(fsm); |
280 ec_fsm_pdo_read_action_next_sync(fsm); |
281 } |
281 } |
282 |
282 |
283 /*****************************************************************************/ |
283 /*****************************************************************************/ |
284 |
284 |
285 /** Fetch Pdo information. |
285 /** Fetch PDO information. |
286 */ |
286 */ |
287 void ec_fsm_pdo_read_state_pdo( |
287 void ec_fsm_pdo_read_state_pdo( |
288 ec_fsm_pdo_t *fsm /**< finite state machine */ |
288 ec_fsm_pdo_t *fsm /**< finite state machine */ |
289 ) |
289 ) |
290 { |
290 { |
291 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
291 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
292 |
292 |
293 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
293 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
294 EC_ERR("Failed to read index of assigned Pdo %u from SM%u.\n", |
294 EC_ERR("Failed to read index of assigned PDO %u from SM%u.\n", |
295 fsm->pdo_pos, fsm->sync_index); |
295 fsm->pdo_pos, fsm->sync_index); |
296 fsm->state = ec_fsm_pdo_state_error; |
296 fsm->state = ec_fsm_pdo_state_error; |
297 return; |
297 return; |
298 } |
298 } |
299 |
299 |
300 if (fsm->request.data_size != sizeof(uint16_t)) { |
300 if (fsm->request.data_size != sizeof(uint16_t)) { |
301 EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X " |
301 EC_ERR("Invalid data size %u returned when uploading SDO 0x%04X:%02X " |
302 "from slave %u.\n", fsm->request.data_size, |
302 "from slave %u.\n", fsm->request.data_size, |
303 fsm->request.index, fsm->request.subindex, |
303 fsm->request.index, fsm->request.subindex, |
304 fsm->slave->ring_position); |
304 fsm->slave->ring_position); |
305 fsm->state = ec_fsm_pdo_state_error; |
305 fsm->state = ec_fsm_pdo_state_error; |
306 return; |
306 return; |
307 } |
307 } |
308 |
308 |
309 if (!(fsm->pdo = (ec_pdo_t *) |
309 if (!(fsm->pdo = (ec_pdo_t *) |
310 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
310 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
311 EC_ERR("Failed to allocate Pdo.\n"); |
311 EC_ERR("Failed to allocate PDO.\n"); |
312 fsm->state = ec_fsm_pdo_state_error; |
312 fsm->state = ec_fsm_pdo_state_error; |
313 return; |
313 return; |
314 } |
314 } |
315 |
315 |
316 ec_pdo_init(fsm->pdo); |
316 ec_pdo_init(fsm->pdo); |
317 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
317 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
318 fsm->pdo->sync_index = fsm->sync_index; |
318 fsm->pdo->sync_index = fsm->sync_index; |
319 |
319 |
320 if (fsm->slave->master->debug_level) |
320 if (fsm->slave->master->debug_level) |
321 EC_DBG("Pdo 0x%04X.\n", fsm->pdo->index); |
321 EC_DBG("PDO 0x%04X.\n", fsm->pdo->index); |
322 |
322 |
323 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
323 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
324 |
324 |
325 fsm->state = ec_fsm_pdo_read_state_pdo_entries; |
325 fsm->state = ec_fsm_pdo_read_state_pdo_entries; |
326 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo); |
326 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo); |
327 fsm->state(fsm); // execute immediately |
327 fsm->state(fsm); // execute immediately |
328 } |
328 } |
329 |
329 |
330 /*****************************************************************************/ |
330 /*****************************************************************************/ |
331 |
331 |
332 /** Fetch Pdo information. |
332 /** Fetch PDO information. |
333 */ |
333 */ |
334 void ec_fsm_pdo_read_state_pdo_entries( |
334 void ec_fsm_pdo_read_state_pdo_entries( |
335 ec_fsm_pdo_t *fsm /**< finite state machine */ |
335 ec_fsm_pdo_t *fsm /**< finite state machine */ |
336 ) |
336 ) |
337 { |
337 { |
338 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
338 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
339 return; |
339 return; |
340 |
340 |
341 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) { |
341 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) { |
342 EC_ERR("Failed to read mapped Pdo entries for Pdo 0x%04X.\n", |
342 EC_ERR("Failed to read mapped PDO entries for PDO 0x%04X.\n", |
343 fsm->pdo->index); |
343 fsm->pdo->index); |
344 fsm->state = ec_fsm_pdo_state_error; |
344 fsm->state = ec_fsm_pdo_state_error; |
345 return; |
345 return; |
346 } |
346 } |
347 |
347 |
348 // next Pdo |
348 // next PDO |
349 fsm->pdo_pos++; |
349 fsm->pdo_pos++; |
350 ec_fsm_pdo_read_action_next_pdo(fsm); |
350 ec_fsm_pdo_read_action_next_pdo(fsm); |
351 } |
351 } |
352 |
352 |
353 /****************************************************************************** |
353 /****************************************************************************** |
354 * Writing state functions. |
354 * Writing state functions. |
355 *****************************************************************************/ |
355 *****************************************************************************/ |
356 |
356 |
357 /** Start Pdo configuration. |
357 /** Start PDO configuration. |
358 */ |
358 */ |
359 void ec_fsm_pdo_conf_state_start( |
359 void ec_fsm_pdo_conf_state_start( |
360 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
360 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
361 ) |
361 ) |
362 { |
362 { |
363 if (!fsm->slave->config) { |
363 if (!fsm->slave->config) { |
364 fsm->state = ec_fsm_pdo_state_end; |
364 fsm->state = ec_fsm_pdo_state_end; |
365 return; |
365 return; |
488 /** Check if the mapping has to be re-configured. |
488 /** Check if the mapping has to be re-configured. |
489 * |
489 * |
490 * \todo Display mapping differences. |
490 * \todo Display mapping differences. |
491 */ |
491 */ |
492 void ec_fsm_pdo_conf_action_check_mapping( |
492 void ec_fsm_pdo_conf_action_check_mapping( |
493 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
493 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
494 ) |
494 ) |
495 { |
495 { |
496 if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) { |
496 if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) { |
497 if (fsm->slave->master->debug_level) |
497 if (fsm->slave->master->debug_level) |
498 EC_DBG("Mapping of Pdo 0x%04X is already configured correctly.\n", |
498 EC_DBG("Mapping of PDO 0x%04X is already configured correctly.\n", |
499 fsm->pdo->index); |
499 fsm->pdo->index); |
500 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
500 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
501 return; |
501 return; |
502 } |
502 } |
503 |
503 |
504 if (fsm->slave->master->debug_level) { |
504 if (fsm->slave->master->debug_level) { |
505 // TODO display diff |
505 // TODO display diff |
506 EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index); |
506 EC_DBG("Changing mapping of PDO 0x%04X.\n", fsm->pdo->index); |
507 } |
507 } |
508 |
508 |
509 ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave, |
509 ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave, |
510 fsm->pdo); |
510 fsm->pdo); |
511 fsm->state = ec_fsm_pdo_conf_state_mapping; |
511 fsm->state = ec_fsm_pdo_conf_state_mapping; |
512 fsm->state(fsm); // execure immediately |
512 fsm->state(fsm); // execure immediately |
513 } |
513 } |
514 |
514 |
515 /*****************************************************************************/ |
515 /*****************************************************************************/ |
516 |
516 |
517 /** Let the Pdo entry state machine configure the current Pdo's mapping. |
517 /** Let the PDO entry state machine configure the current PDO's mapping. |
518 */ |
518 */ |
519 void ec_fsm_pdo_conf_state_mapping( |
519 void ec_fsm_pdo_conf_state_mapping( |
520 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
520 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
521 ) |
521 ) |
522 { |
522 { |
523 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
523 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
524 return; |
524 return; |
525 |
525 |
526 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
526 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
527 EC_WARN("Failed to configure mapping of Pdo 0x%04X.\n", |
527 EC_WARN("Failed to configure mapping of PDO 0x%04X.\n", |
528 fsm->pdo->index); |
528 fsm->pdo->index); |
529 |
529 |
530 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
530 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
531 } |
531 } |
532 |
532 |
533 /*****************************************************************************/ |
533 /*****************************************************************************/ |
534 |
534 |
535 /** Check mapping of next Pdo, otherwise configure assignment. |
535 /** Check mapping of next PDO, otherwise configure assignment. |
536 */ |
536 */ |
537 void ec_fsm_pdo_conf_action_next_pdo_mapping( |
537 void ec_fsm_pdo_conf_action_next_pdo_mapping( |
538 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
538 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
539 ) |
539 ) |
540 { |
540 { |
541 // get next configured Pdo |
541 // get next configured PDO |
542 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
542 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
543 // no more configured pdos |
543 // no more configured pdos |
544 ec_fsm_pdo_conf_action_check_assignment(fsm); |
544 ec_fsm_pdo_conf_action_check_assignment(fsm); |
545 return; |
545 return; |
546 } |
546 } |
548 ec_fsm_pdo_conf_action_pdo_mapping(fsm); |
548 ec_fsm_pdo_conf_action_pdo_mapping(fsm); |
549 } |
549 } |
550 |
550 |
551 /*****************************************************************************/ |
551 /*****************************************************************************/ |
552 |
552 |
553 /** Check if the Pdo assignment of the current SM has to be re-configured. |
553 /** Check if the PDO assignment of the current SM has to be re-configured. |
554 */ |
554 */ |
555 void ec_fsm_pdo_conf_action_check_assignment( |
555 void ec_fsm_pdo_conf_action_check_assignment( |
556 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
556 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
557 ) |
557 ) |
558 { |
558 { |
559 // check if assignment has to be re-configured |
559 // check if assignment has to be re-configured |
560 if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) { |
560 if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) { |
561 |
561 |
562 if (fsm->slave->master->debug_level) |
562 if (fsm->slave->master->debug_level) |
563 EC_DBG("Pdo assignment for SM%u is already configured " |
563 EC_DBG("PDO assignment for SM%u is already configured " |
564 "correctly.\n", fsm->sync_index); |
564 "correctly.\n", fsm->sync_index); |
565 |
565 |
566 ec_fsm_pdo_conf_action_next_sync(fsm); |
566 ec_fsm_pdo_conf_action_next_sync(fsm); |
567 return; |
567 return; |
568 } |
568 } |
569 |
569 |
570 if (fsm->slave->master->debug_level) { |
570 if (fsm->slave->master->debug_level) { |
571 EC_DBG("Pdo assignment of SM%u differs:\n", fsm->sync_index); |
571 EC_DBG("PDO assignment of SM%u differs:\n", fsm->sync_index); |
572 EC_DBG("Currently assigned Pdos: "); |
572 EC_DBG("Currently assigned PDOs: "); |
573 ec_pdo_list_print(&fsm->sync->pdos); |
573 ec_pdo_list_print(&fsm->sync->pdos); |
574 printk("\n"); |
574 printk("\n"); |
575 EC_DBG("Pdos to assign: "); |
575 EC_DBG("PDOs to assign: "); |
576 ec_pdo_list_print(&fsm->pdos); |
576 ec_pdo_list_print(&fsm->pdos); |
577 printk("\n"); |
577 printk("\n"); |
578 } |
578 } |
579 |
579 |
580 // Pdo assignment has to be changed. Does the slave support this? |
580 // PDO assignment has to be changed. Does the slave support this? |
581 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
581 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
582 || (fsm->slave->sii.has_general |
582 || (fsm->slave->sii.has_general |
583 && !fsm->slave->sii.coe_details.enable_pdo_assign)) { |
583 && !fsm->slave->sii.coe_details.enable_pdo_assign)) { |
584 EC_WARN("Slave %u does not support assigning Pdos!\n", |
584 EC_WARN("Slave %u does not support assigning PDOs!\n", |
585 fsm->slave->ring_position); |
585 fsm->slave->ring_position); |
586 ec_fsm_pdo_conf_action_next_sync(fsm); |
586 ec_fsm_pdo_conf_action_next_sync(fsm); |
587 return; |
587 return; |
588 } |
588 } |
589 |
589 |
590 if (ec_sdo_request_alloc(&fsm->request, 2)) { |
590 if (ec_sdo_request_alloc(&fsm->request, 2)) { |
591 fsm->state = ec_fsm_pdo_state_error; |
591 fsm->state = ec_fsm_pdo_state_error; |
592 return; |
592 return; |
593 } |
593 } |
594 |
594 |
595 // set mapped Pdo count to zero |
595 // set mapped PDO count to zero |
596 EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped |
596 EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped |
597 fsm->request.data_size = 1; |
597 fsm->request.data_size = 1; |
598 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
598 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
599 ecrt_sdo_request_write(&fsm->request); |
599 ecrt_sdo_request_write(&fsm->request); |
600 |
600 |
601 if (fsm->slave->master->debug_level) |
601 if (fsm->slave->master->debug_level) |
602 EC_DBG("Setting number of assigned Pdos to zero.\n"); |
602 EC_DBG("Setting number of assigned PDOs to zero.\n"); |
603 |
603 |
604 fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count; |
604 fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count; |
605 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
605 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
606 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
606 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
607 } |
607 } |
608 |
608 |
609 /*****************************************************************************/ |
609 /*****************************************************************************/ |
610 |
610 |
611 /** Set the number of assigned Pdos to zero. |
611 /** Set the number of assigned PDOs to zero. |
612 */ |
612 */ |
613 void ec_fsm_pdo_conf_state_zero_pdo_count( |
613 void ec_fsm_pdo_conf_state_zero_pdo_count( |
614 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
614 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
615 ) |
615 ) |
616 { |
616 { |
617 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
617 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
618 return; |
618 return; |
619 |
619 |
620 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
620 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
621 EC_WARN("Failed to clear Pdo assignment of SM%u.\n", fsm->sync_index); |
621 EC_WARN("Failed to clear PDO assignment of SM%u.\n", fsm->sync_index); |
622 fsm->state = ec_fsm_pdo_state_error; |
622 fsm->state = ec_fsm_pdo_state_error; |
623 return; |
623 return; |
624 } |
624 } |
625 |
625 |
626 // the sync manager's assigned Pdos have been cleared |
626 // the sync manager's assigned PDOs have been cleared |
627 ec_pdo_list_clear_pdos(&fsm->sync->pdos); |
627 ec_pdo_list_clear_pdos(&fsm->sync->pdos); |
628 |
628 |
629 // assign all Pdos belonging to the current sync manager |
629 // assign all PDOs belonging to the current sync manager |
630 |
630 |
631 // find first Pdo |
631 // find first PDO |
632 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) { |
632 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) { |
633 |
633 |
634 if (fsm->slave->master->debug_level) |
634 if (fsm->slave->master->debug_level) |
635 EC_DBG("No Pdos to assign.\n"); |
635 EC_DBG("No PDOs to assign.\n"); |
636 |
636 |
637 // check for mapping to be altered |
637 // check for mapping to be altered |
638 ec_fsm_pdo_conf_action_next_sync(fsm); |
638 ec_fsm_pdo_conf_action_next_sync(fsm); |
639 return; |
639 return; |
640 } |
640 } |
641 |
641 |
642 // assign first Pdo |
642 // assign first PDO |
643 fsm->pdo_pos = 1; |
643 fsm->pdo_pos = 1; |
644 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
644 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
645 } |
645 } |
646 |
646 |
647 /*****************************************************************************/ |
647 /*****************************************************************************/ |
648 |
648 |
649 /** Assign a Pdo. |
649 /** Assign a PDO. |
650 */ |
650 */ |
651 void ec_fsm_pdo_conf_action_assign_pdo( |
651 void ec_fsm_pdo_conf_action_assign_pdo( |
652 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
652 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
653 ) |
653 ) |
654 { |
654 { |
655 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
655 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
656 fsm->request.data_size = 2; |
656 fsm->request.data_size = 2; |
657 ec_sdo_request_address(&fsm->request, |
657 ec_sdo_request_address(&fsm->request, |
658 0x1C10 + fsm->sync_index, fsm->pdo_pos); |
658 0x1C10 + fsm->sync_index, fsm->pdo_pos); |
659 ecrt_sdo_request_write(&fsm->request); |
659 ecrt_sdo_request_write(&fsm->request); |
660 |
660 |
661 if (fsm->slave->master->debug_level) |
661 if (fsm->slave->master->debug_level) |
662 EC_DBG("Assigning Pdo 0x%04X at position %u.\n", |
662 EC_DBG("Assigning PDO 0x%04X at position %u.\n", |
663 fsm->pdo->index, fsm->pdo_pos); |
663 fsm->pdo->index, fsm->pdo_pos); |
664 |
664 |
665 fsm->state = ec_fsm_pdo_conf_state_assign_pdo; |
665 fsm->state = ec_fsm_pdo_conf_state_assign_pdo; |
666 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
666 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
667 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
667 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
668 } |
668 } |
669 |
669 |
670 /*****************************************************************************/ |
670 /*****************************************************************************/ |
671 |
671 |
672 /** Add a Pdo to the sync managers Pdo assignment. |
672 /** Add a PDO to the sync managers PDO assignment. |
673 */ |
673 */ |
674 void ec_fsm_pdo_conf_state_assign_pdo( |
674 void ec_fsm_pdo_conf_state_assign_pdo( |
675 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
675 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
676 ) |
676 ) |
677 { |
677 { |
678 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
678 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
679 |
679 |
680 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
680 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
681 EC_WARN("Failed to assign Pdo 0x%04X at position %u of SM%u.\n", |
681 EC_WARN("Failed to assign PDO 0x%04X at position %u of SM%u.\n", |
682 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index); |
682 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index); |
683 fsm->state = ec_fsm_pdo_state_error; |
683 fsm->state = ec_fsm_pdo_state_error; |
684 return; |
684 return; |
685 } |
685 } |
686 |
686 |
687 // find next Pdo |
687 // find next PDO |
688 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
688 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
689 |
689 |
690 // no more Pdos to assign, set Pdo count |
690 // no more PDOs to assign, set PDO count |
691 EC_WRITE_U8(fsm->request.data, fsm->pdo_pos); |
691 EC_WRITE_U8(fsm->request.data, fsm->pdo_pos); |
692 fsm->request.data_size = 1; |
692 fsm->request.data_size = 1; |
693 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
693 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
694 ecrt_sdo_request_write(&fsm->request); |
694 ecrt_sdo_request_write(&fsm->request); |
695 |
695 |
696 if (fsm->slave->master->debug_level) |
696 if (fsm->slave->master->debug_level) |
697 EC_DBG("Setting number of assigned Pdos to %u.\n", fsm->pdo_pos); |
697 EC_DBG("Setting number of assigned PDOs to %u.\n", fsm->pdo_pos); |
698 |
698 |
699 fsm->state = ec_fsm_pdo_conf_state_set_pdo_count; |
699 fsm->state = ec_fsm_pdo_conf_state_set_pdo_count; |
700 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
700 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
701 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
701 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
702 return; |
702 return; |
703 } |
703 } |
704 |
704 |
705 // add next Pdo to assignment |
705 // add next PDO to assignment |
706 fsm->pdo_pos++; |
706 fsm->pdo_pos++; |
707 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
707 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
708 } |
708 } |
709 |
709 |
710 /*****************************************************************************/ |
710 /*****************************************************************************/ |
711 |
711 |
712 /** Set the number of assigned Pdos. |
712 /** Set the number of assigned PDOs. |
713 */ |
713 */ |
714 void ec_fsm_pdo_conf_state_set_pdo_count( |
714 void ec_fsm_pdo_conf_state_set_pdo_count( |
715 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
715 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
716 ) |
716 ) |
717 { |
717 { |
718 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
718 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
719 |
719 |
720 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
720 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
721 EC_WARN("Failed to set number of assigned Pdos of SM%u.\n", |
721 EC_WARN("Failed to set number of assigned PDOs of SM%u.\n", |
722 fsm->sync_index); |
722 fsm->sync_index); |
723 fsm->state = ec_fsm_pdo_state_error; |
723 fsm->state = ec_fsm_pdo_state_error; |
724 return; |
724 return; |
725 } |
725 } |
726 |
726 |
727 // Pdos have been configured |
727 // PDOs have been configured |
728 ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos); |
728 ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos); |
729 |
729 |
730 if (fsm->slave->master->debug_level) |
730 if (fsm->slave->master->debug_level) |
731 EC_DBG("Successfully configured Pdo assignment of SM%u.\n", |
731 EC_DBG("Successfully configured PDO assignment of SM%u.\n", |
732 fsm->sync_index); |
732 fsm->sync_index); |
733 |
733 |
734 // check if Pdo mapping has to be altered |
734 // check if PDO mapping has to be altered |
735 ec_fsm_pdo_conf_action_next_sync(fsm); |
735 ec_fsm_pdo_conf_action_next_sync(fsm); |
736 } |
736 } |
737 |
737 |
738 /****************************************************************************** |
738 /****************************************************************************** |
739 * Common state functions |
739 * Common state functions |
740 *****************************************************************************/ |
740 *****************************************************************************/ |
741 |
741 |
742 /** State: ERROR. |
742 /** State: ERROR. |
743 */ |
743 */ |
744 void ec_fsm_pdo_state_error( |
744 void ec_fsm_pdo_state_error( |
745 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
745 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
746 ) |
746 ) |
747 { |
747 { |
748 } |
748 } |
749 |
749 |
750 /*****************************************************************************/ |
750 /*****************************************************************************/ |
751 |
751 |
752 /** State: END. |
752 /** State: END. |
753 */ |
753 */ |
754 void ec_fsm_pdo_state_end( |
754 void ec_fsm_pdo_state_end( |
755 ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */ |
755 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
756 ) |
756 ) |
757 { |
757 { |
758 } |
758 } |
759 |
759 |
760 /*****************************************************************************/ |
760 /*****************************************************************************/ |