362 EC_DBG("Sdo request for slave %u timed out...\n", |
362 EC_DBG("Sdo request for slave %u timed out...\n", |
363 slave->ring_position); |
363 slave->ring_position); |
364 continue; |
364 continue; |
365 } |
365 } |
366 |
366 |
367 if (slave->current_state == EC_SLAVE_STATE_INIT || |
367 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
368 slave->error_flag) { |
|
369 req->state = EC_REQUEST_FAILURE; |
368 req->state = EC_REQUEST_FAILURE; |
370 continue; |
369 continue; |
371 } |
370 } |
372 |
371 |
373 req->state = EC_REQUEST_BUSY; |
372 req->state = EC_REQUEST_BUSY; |
448 list_for_each_entry(slave, &master->slaves, list) { |
447 list_for_each_entry(slave, &master->slaves, list) { |
449 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
448 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
450 || slave->sdo_dictionary_fetched |
449 || slave->sdo_dictionary_fetched |
451 || slave->current_state == EC_SLAVE_STATE_INIT |
450 || slave->current_state == EC_SLAVE_STATE_INIT |
452 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
451 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
453 || slave->error_flag) continue; |
452 ) continue; |
454 |
453 |
455 if (master->debug_level) { |
454 if (master->debug_level) { |
456 EC_DBG("Fetching Sdo dictionary from slave %u.\n", |
455 EC_DBG("Fetching Sdo dictionary from slave %u.\n", |
457 slave->ring_position); |
456 slave->ring_position); |
458 } |
457 } |
512 { |
511 { |
513 ec_master_t *master = fsm->master; |
512 ec_master_t *master = fsm->master; |
514 ec_slave_t *slave = fsm->slave; |
513 ec_slave_t *slave = fsm->slave; |
515 |
514 |
516 // Does the slave have to be configured? |
515 // Does the slave have to be configured? |
517 if (!slave->error_flag |
516 if ((slave->current_state != slave->requested_state |
518 && (slave->current_state != slave->requested_state |
517 || slave->force_config) && !slave->error_flag) { |
519 || slave->force_config)) { |
|
520 // Start slave configuration, if it is allowed. |
518 // Start slave configuration, if it is allowed. |
521 down(&master->config_sem); |
519 down(&master->config_sem); |
522 if (!master->allow_config) { |
520 if (!master->allow_config) { |
523 up(&master->config_sem); |
521 up(&master->config_sem); |
524 } else { |
522 } else { |
551 ec_fsm_master_action_next_slave_state(fsm); |
549 ec_fsm_master_action_next_slave_state(fsm); |
552 } |
550 } |
553 |
551 |
554 /*****************************************************************************/ |
552 /*****************************************************************************/ |
555 |
553 |
556 /** Master action: Acknowledge. |
554 /** Master state: READ STATE. |
557 */ |
555 * |
558 void ec_fsm_master_action_acknowledge( |
556 * Fetches the AL state of a slave. |
|
557 */ |
|
558 void ec_fsm_master_state_read_state( |
559 ec_fsm_master_t *fsm /**< Master state machine. */ |
559 ec_fsm_master_t *fsm /**< Master state machine. */ |
560 ) |
560 ) |
561 { |
561 { |
562 ec_slave_t *slave = fsm->slave; |
562 ec_slave_t *slave = fsm->slave; |
|
563 ec_datagram_t *datagram = fsm->datagram; |
|
564 |
|
565 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
566 return; |
|
567 |
|
568 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
569 EC_ERR("Failed to receive AL state datagram for slave %u" |
|
570 " (datagram state %u)\n", |
|
571 slave->ring_position, datagram->state); |
|
572 fsm->state = ec_fsm_master_state_error; |
|
573 return; |
|
574 } |
|
575 |
|
576 // did the slave not respond to its station address? |
|
577 if (datagram->working_counter != 1) { |
|
578 if (!slave->error_flag) { |
|
579 slave->error_flag = 1; |
|
580 if (fsm->master->debug_level) |
|
581 EC_DBG("Slave %u did not respond to state query.\n", |
|
582 fsm->slave->ring_position); |
|
583 } |
|
584 fsm->topology_change_pending = 1; |
|
585 fsm->state = ec_fsm_master_state_error; |
|
586 return; |
|
587 } |
|
588 |
|
589 // A single slave responded |
|
590 ec_slave_set_state(slave, EC_READ_U8(datagram->data)); |
563 |
591 |
564 if (!slave->error_flag) { |
592 if (!slave->error_flag) { |
565 // Check, if new slave state has to be acknowledged |
593 // Check, if new slave state has to be acknowledged |
566 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
594 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
567 fsm->idle = 0; |
595 fsm->idle = 0; |
576 return; |
604 return; |
577 } |
605 } |
578 |
606 |
579 // slave has error flag set; process next one |
607 // slave has error flag set; process next one |
580 ec_fsm_master_action_next_slave_state(fsm); |
608 ec_fsm_master_action_next_slave_state(fsm); |
581 } |
|
582 |
|
583 /*****************************************************************************/ |
|
584 |
|
585 /** Master state: READ STATE. |
|
586 * |
|
587 * Fetches the AL state of a slave. |
|
588 */ |
|
589 void ec_fsm_master_state_read_state( |
|
590 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
591 ) |
|
592 { |
|
593 ec_slave_t *slave = fsm->slave; |
|
594 ec_datagram_t *datagram = fsm->datagram; |
|
595 |
|
596 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
597 return; |
|
598 |
|
599 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
600 EC_ERR("Failed to receive AL state datagram for slave %u" |
|
601 " (datagram state %u)\n", |
|
602 slave->ring_position, datagram->state); |
|
603 fsm->state = ec_fsm_master_state_error; |
|
604 return; |
|
605 } |
|
606 |
|
607 // did the slave not respond to its station address? |
|
608 if (datagram->working_counter != 1) { |
|
609 if (!slave->error_flag) { |
|
610 slave->error_flag = 1; |
|
611 if (fsm->master->debug_level) |
|
612 EC_DBG("Slave %u did not respond to state query.\n", |
|
613 fsm->slave->ring_position); |
|
614 } |
|
615 fsm->topology_change_pending = 1; |
|
616 fsm->state = ec_fsm_master_state_error; |
|
617 return; |
|
618 } |
|
619 |
|
620 // A single slave responded |
|
621 ec_slave_set_state(slave, EC_READ_U8(datagram->data)); |
|
622 ec_fsm_master_action_acknowledge(fsm); |
|
623 } |
609 } |
624 |
610 |
625 /*****************************************************************************/ |
611 /*****************************************************************************/ |
626 |
612 |
627 /** Master state: ACKNOWLEDGE. |
613 /** Master state: ACKNOWLEDGE. |
779 ec_slave_t *slave = request->slave; |
765 ec_slave_t *slave = request->slave; |
780 |
766 |
781 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
767 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
782 |
768 |
783 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
769 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
784 slave->error_flag = 1; |
|
785 EC_ERR("Failed to write SII data to slave %u.\n", |
770 EC_ERR("Failed to write SII data to slave %u.\n", |
786 slave->ring_position); |
771 slave->ring_position); |
787 request->state = EC_REQUEST_FAILURE; |
772 request->state = EC_REQUEST_FAILURE; |
788 wake_up(&master->sii_queue); |
773 wake_up(&master->sii_queue); |
789 fsm->state = ec_fsm_master_state_error; |
774 fsm->state = ec_fsm_master_state_error; |