etherlabmaster: comparison master/fsm.c

equal deleted inserted replaced

-:87b20532872b
+:5fe7df7f2433
 const ec_code_msg_t al_status_messages[];
 /*****************************************************************************/
 void ec_fsm_master_start(ec_fsm_t *);
-void ec_fsm_master_wait(ec_fsm_t *);
+void ec_fsm_master_broadcast(ec_fsm_t *);
+void ec_fsm_master_proc_states(ec_fsm_t *);
 void ec_fsm_master_scan(ec_fsm_t *);
+void ec_fsm_master_states(ec_fsm_t *);
+void ec_fsm_master_validate_vendor(ec_fsm_t *);
+void ec_fsm_master_validate_product(ec_fsm_t *);
+void ec_fsm_master_reconfigure(ec_fsm_t *);
+void ec_fsm_master_address(ec_fsm_t *);
 void ec_fsm_master_conf(ec_fsm_t *);
 void ec_fsm_slave_start_reading(ec_fsm_t *);
 void ec_fsm_slave_read_status(ec_fsm_t *);
 void ec_fsm_slave_read_base(ec_fsm_t *);
 void ec_fsm_slave_read_dl(ec_fsm_t *);
 void ec_fsm_slave_prepare_sii(ec_fsm_t *);
 void ec_fsm_slave_read_sii(ec_fsm_t *);
 void ec_fsm_slave_category_header(ec_fsm_t *);
 void ec_fsm_slave_category_data(ec_fsm_t *);
-void ec_fsm_slave_conf(ec_fsm_t *);
 void ec_fsm_slave_end(ec_fsm_t *);
 void ec_fsm_slave_conf(ec_fsm_t *);
 void ec_fsm_slave_sync(ec_fsm_t *);
 void ec_fsm_slave_preop(ec_fsm_t *);
 void ec_fsm_slave_op2(ec_fsm_t *);
 void ec_fsm_sii_start_reading(ec_fsm_t *);
 void ec_fsm_sii_check(ec_fsm_t *);
 void ec_fsm_sii_fetch(ec_fsm_t *);
-void ec_fsm_sii_finished(ec_fsm_t *);
+void ec_fsm_sii_end(ec_fsm_t *);
 void ec_fsm_sii_error(ec_fsm_t *);
 void ec_fsm_change_start(ec_fsm_t *);
 void ec_fsm_change_check(ec_fsm_t *);
 void ec_fsm_change_status(ec_fsm_t *);
 void ec_fsm_change_end(ec_fsm_t *);
 void ec_fsm_change_error(ec_fsm_t *);
 /*****************************************************************************/
-int ec_fsm_init(ec_fsm_t *fsm, ec_master_t *master)
+/**
+Constructor.
+*/
+int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */
+ec_master_t *master /**< EtherCAT master */
+)
 {
 fsm->master = master;
 fsm->master_state = ec_fsm_master_start;
 fsm->master_slaves_responding = 0;
 fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;
+fsm->master_validation = 0;
 fsm->slave_cat_data = NULL;
 ec_command_init(&fsm->command);
 if (ec_command_prealloc(&fsm->command, EC_MAX_DATA_SIZE)) {
 EC_ERR("FSM failed to allocate FSM command.\n");
 return 0;
 }
 /*****************************************************************************/
-void ec_fsm_clear(ec_fsm_t *fsm)
+/**
+Destructor.
+*/
+void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)
 {
 if (fsm->slave_cat_data) kfree(fsm->slave_cat_data);
 ec_command_clear(&fsm->command);
 }
 /*****************************************************************************/
-void ec_fsm_reset(ec_fsm_t *fsm)
+/**
+Resets the state machine.
+*/
+void ec_fsm_reset(ec_fsm_t *fsm /**< finite state machine */)
 {
 fsm->master_state = ec_fsm_master_start;
 fsm->master_slaves_responding = 0;
 fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;
 }
 }
 /*****************************************************************************/
-void ec_fsm_execute(ec_fsm_t *fsm)
+/**
+Executes the current state of the state machine.
+*/
+void ec_fsm_execute(ec_fsm_t *fsm /**< finite state machine */)
 {
 fsm->master_state(fsm);
 }
 /******************************************************************************
 *  master state machine
 *****************************************************************************/
 /**
-State: Start.
+Master state: START.
 Starts with getting slave count and slave states.
 */
 void ec_fsm_master_start(ec_fsm_t *fsm)
 {
 ec_command_brd(&fsm->command, 0x0130, 2);
 ec_master_queue_command(fsm->master, &fsm->command);
-fsm->master_state = ec_fsm_master_wait;
+fsm->master_state = ec_fsm_master_broadcast;
 }
 /*****************************************************************************/
-void ec_fsm_master_wait(ec_fsm_t *fsm)
+/**
-{
+Master state: BROADCAST.
-ec_command_t *command = &fsm->command;
+Processes the broadcast read slave count and slaves states.
-unsigned int topology_change, i, eoe_slaves_active;
+*/
+void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_command_t *command = &fsm->command;
+unsigned int topology_change, states_change, i;
 ec_slave_t *slave;
+ec_master_t *master = fsm->master;
 if (command->state != EC_CMD_RECEIVED) {
-if (!fsm->master->device->link_state)
+if (!master->device->link_state) {
-// treat link down as topology change
 fsm->master_slaves_responding = 0;
+list_for_each_entry(slave, &master->slaves, list) {
+slave->online = 0;
+}
+}
 fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
-if (command->working_counter == fsm->master_slaves_responding &&
+topology_change = (command->working_counter !=
-command->data[0] == fsm->master_slave_states)
+fsm->master_slaves_responding);
-{
+states_change = (EC_READ_U8(command->data) != fsm->master_slave_states);
-// check if any slaves are not in the state, they're supposed to be
-list_for_each_entry(slave, &fsm->master->slaves, list) {
+fsm->master_slave_states = EC_READ_U8(command->data);
-if (slave->state_error ||
+fsm->master_slaves_responding = command->working_counter;
-slave->requested_state == EC_SLAVE_STATE_UNKNOWN ||
-slave->current_state == slave->requested_state) continue;
+if (topology_change) {
+if (fsm->master_slaves_responding == master->slave_count) {
-EC_INFO("Changing state of slave %i from ", slave->ring_position);
+EC_INFO("%i slave%s responding (VALID).\n",
-ec_print_states(slave->current_state);
+fsm->master_slaves_responding,
-printk(" to ");
+fsm->master_slaves_responding == 1 ? "" : "s");
-ec_print_states(slave->requested_state);
+fsm->master_validation = 1; // start validation later
-printk(".\n");
+}
+else {
-fsm->slave = slave;
+EC_WARN("%i slave%s responding. Invalid slave count!\n",
-fsm->slave_state = ec_fsm_slave_conf;
+fsm->master_slaves_responding,
+fsm->master_slaves_responding == 1 ? "" : "s");
-fsm->change_new = EC_SLAVE_STATE_INIT;
+}
-fsm->change_state = ec_fsm_change_start;
+}
-fsm->master_state = ec_fsm_master_conf;
+if (states_change) {
+EC_INFO("Slave states: ");
+ec_print_states(fsm->master_slave_states);
+printk(".\n");
+}
+// topology change in free-run mode: clear all slaves and scan the bus
+if (topology_change && master->mode == EC_MASTER_MODE_FREERUN) {
+EC_INFO("Scanning bus.\n");
+ec_master_eoe_stop(master);
+ec_master_clear_slaves(master);
+if (!fsm->master_slaves_responding) {
+// no slaves present -> finish state machine.
+fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
-// nothing to configure...
+// init slaves
-eoe_slaves_active = 0;
+for (i = 0; i < fsm->master_slaves_responding; i++) {
-list_for_each_entry(slave, &fsm->master->slaves, list) {
+if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),
-if (slave->sii_mailbox_protocols & EC_MBOX_EOE) {
+GFP_ATOMIC))) {
-eoe_slaves_active++;
+EC_ERR("Failed to allocate slave %i!\n", i);
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+if (ec_slave_init(slave, master, i, i + 1)) {
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+if (kobject_add(&slave->kobj)) {
+EC_ERR("Failed to add kobject.\n");
+kobject_put(&slave->kobj); // free
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+list_add_tail(&slave->list, &master->slaves);
+}
+// begin scanning of slaves
+fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
+fsm->slave_state = ec_fsm_slave_start_reading;
+fsm->master_state = ec_fsm_master_scan;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+// fetch state from each slave
+fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
+ec_command_nprd(&fsm->command, fsm->slave->station_address, 0x0130, 2);
+ec_master_queue_command(master, &fsm->command);
+fsm->master_state = ec_fsm_master_states;
+}
+/*****************************************************************************/
+/**
+Master action: Get state of next slave.
+*/
+void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm
+/**< finite state machine */)
+{
+ec_master_t *master = fsm->master;
+ec_slave_t *slave = fsm->slave;
+// have all states been read?
+if (slave->list.next == &master->slaves) {
+// check, if a bus validation has to be done
+if (fsm->master_validation) {
+fsm->master_validation = 0;
+list_for_each_entry(slave, &master->slaves, list) {
+if (slave->online) continue;
+// At least one slave is offline. validate!
+EC_INFO("Validating bus.\n");
+fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
+fsm->master_state = ec_fsm_master_validate_vendor;
+fsm->sii_offset = 0x0008; // vendor ID
+fsm->sii_mode = 0;
+fsm->sii_state = ec_fsm_sii_start_reading;
+fsm->sii_state(fsm); // execute immediately
+return;
 }
 }
-if (eoe_slaves_active && !list_empty(&fsm->master->eoe_handlers))
+fsm->master_state = ec_fsm_master_proc_states;
-ec_master_eoe_start(fsm->master);
+fsm->master_state(fsm); // execute immediately
+return;
+}
+// process next slave
+fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
+ec_command_nprd(&fsm->command, fsm->slave->station_address, 0x0130, 2);
+ec_master_queue_command(master, &fsm->command);
+fsm->master_state = ec_fsm_master_states;
+}
+/*****************************************************************************/
+/**
+Master state: STATES.
+Fetches the AL- and online state of a slave.
+*/
+void ec_fsm_master_states(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_slave_t *slave = fsm->slave;
+ec_command_t *command = &fsm->command;
+uint8_t new_state;
+if (command->state != EC_CMD_RECEIVED) {
 fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
-topology_change = command->working_counter !=
+// did the slave not respond to its station address?
-fsm->master_slaves_responding;
+if (command->working_counter != 1) {
+if (slave->online) {
-fsm->master_slaves_responding = command->working_counter;
+slave->online = 0;
-fsm->master_slave_states = command->data[0];
+EC_INFO("Slave %i: offline.\n", slave->ring_position);
+}
-EC_INFO("FSM: %i slave%s responding (", fsm->master_slaves_responding,
+ec_fsm_master_action_next_slave_state(fsm);
-fsm->master_slaves_responding == 1 ? "" : "s");
+return;
-ec_print_states(fsm->master_slave_states);
+}
-printk(")\n");
+// slave responded
-if (!topology_change || fsm->master->mode == EC_MASTER_MODE_RUNNING) {
+new_state = EC_READ_U8(command->data);
+if (!slave->online) { // slave was offline before
+slave->online = 1;
+slave->state_error = 0;
+slave->current_state = new_state;
+EC_INFO("Slave %i: online (", slave->ring_position);
+ec_print_states(new_state);
+printk(").\n");
+}
+else if (new_state != slave->current_state) {
+EC_INFO("Slave %i: ", slave->ring_position);
+ec_print_states(slave->current_state);
+printk(" -> ");
+ec_print_states(new_state);
+printk(".\n");
+slave->current_state = new_state;
+}
+ec_fsm_master_action_next_slave_state(fsm);
+}
+/*****************************************************************************/
+/**
+Master state: PROC_STATES.
+Processes the slave states.
+*/
+void ec_fsm_master_proc_states(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_master_t *master = fsm->master;
+ec_slave_t *slave;
+// check if any slaves are not in the state, they're supposed to be
+list_for_each_entry(slave, &master->slaves, list) {
+if (slave->state_error || !slave->online ||
+slave->requested_state == EC_SLAVE_STATE_UNKNOWN ||
+slave->current_state == slave->requested_state) continue;
+EC_INFO("Changing state of slave %i from ", slave->ring_position);
+ec_print_states(slave->current_state);
+printk(" to ");
+ec_print_states(slave->requested_state);
+printk(".\n");
+fsm->slave = slave;
+fsm->slave_state = ec_fsm_slave_conf;
+fsm->change_new = EC_SLAVE_STATE_INIT;
+fsm->change_state = ec_fsm_change_start;
+fsm->master_state = ec_fsm_master_conf;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+// nothing to configure. restart master state machine.
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+}
+/*****************************************************************************/
+/**
+Master state: VALIDATE_VENDOR.
+Validates the vendor ID of a slave.
+*/
+void ec_fsm_master_validate_vendor(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_slave_t *slave = fsm->slave;
+fsm->sii_state(fsm); // execute SII state machine
+if (fsm->sii_state == ec_fsm_sii_error) {
+EC_ERR("Failed to validate vendor ID of slave %i.\n",
+slave->ring_position);
 fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
-EC_INFO("Topology change detected - Scanning bus.\n");
+if (fsm->sii_state != ec_fsm_sii_end) return;
-ec_master_eoe_stop(fsm->master);
+if (fsm->sii_result != slave->sii_vendor_id) {
-ec_master_clear_slaves(fsm->master);
+EC_ERR("Slave %i: invalid vendor ID!\n", slave->ring_position);
-if (!fsm->master_slaves_responding) {
-// no slaves present -> finish state machine.
 fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
-// init slaves
+// vendor ID is ok. check product code.
-for (i = 0; i < fsm->master_slaves_responding; i++) {
+fsm->master_state = ec_fsm_master_validate_product;
-if (!(slave =
+fsm->sii_offset = 0x000A; // product code
-(ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_ATOMIC))) {
+fsm->sii_mode = 0;
-EC_ERR("FSM failed to allocate slave %i!\n", i);
+fsm->sii_state = ec_fsm_sii_start_reading;
+fsm->sii_state(fsm); // execute immediately
+}
+/*****************************************************************************/
+/**
+Master state: VALIDATE_PRODUCT.
+Validates the product ID of a slave.
+*/
+void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_slave_t *slave = fsm->slave;
+fsm->sii_state(fsm); // execute SII state machine
+if (fsm->sii_state == ec_fsm_sii_error) {
+EC_ERR("Failed to validate product code of slave %i.\n",
+slave->ring_position);
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+if (fsm->sii_state != ec_fsm_sii_end) return;
+if (fsm->sii_result != slave->sii_product_code) {
+EC_ERR("Slave %i: invalid product code!\n", slave->ring_position);
+EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code,
+fsm->sii_result);
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+// have all states been validated?
+if (slave->list.next == &fsm->master->slaves) {
+fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list);
+fsm->master_state = ec_fsm_master_reconfigure;
+return;
+}
+// validate next slave
+fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
+fsm->master_state = ec_fsm_master_validate_vendor;
+fsm->sii_offset = 0x0008; // vendor ID
+fsm->sii_mode = 0;
+fsm->sii_state = ec_fsm_sii_start_reading;
+fsm->sii_state(fsm); // execute immediately
+}
+/*****************************************************************************/
+/**
+Master state: RECONFIGURE.
+Looks for slave, that have lost their configuration and writes
+their station address, so that they can be reconfigured later.
+*/
+void ec_fsm_master_reconfigure(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_command_t *command = &fsm->command;
+while (fsm->slave->online) {
+if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
 fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
 return;
 }
+// check next slave
-if (ec_slave_init(slave, fsm->master, i, i + 1)) {
+fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
-fsm->master_state = ec_fsm_master_start;
+}
-return;
-}
+EC_INFO("Reinitializing slave %i.\n", fsm->slave->ring_position);
-if (kobject_add(&slave->kobj)) {
+// write station address
-EC_ERR("FSM failed to add kobject.\n");
+ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2);
-kobject_put(&slave->kobj); // free
+EC_WRITE_U16(command->data, fsm->slave->station_address);
-fsm->master_state = ec_fsm_master_start;
+ec_master_queue_command(fsm->master, command);
-return;
+fsm->master_state = ec_fsm_master_address;
 }
-list_add_tail(&slave->list, &fsm->master->slaves);
+/*****************************************************************************/
-}
+/**
-// begin scanning of slaves
+Master state: ADDRESS.
-fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list);
+Checks, if the new station address has been written to the slave.
-fsm->slave_state = ec_fsm_slave_start_reading;
+*/
-fsm->master_state = ec_fsm_master_scan;
+void ec_fsm_master_address(ec_fsm_t *fsm /**< finite state machine */)
+{
+ec_slave_t *slave = fsm->slave;
+ec_command_t *command = &fsm->command;
+if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+EC_ERR("Failed to write station address on slave %i.\n",
+slave->ring_position);
+}
+if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
+fsm->master_state = ec_fsm_master_start;
+fsm->master_state(fsm); // execute immediately
+return;
+}
+// check next slave
+fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
+fsm->master_state = ec_fsm_master_reconfigure;
 fsm->master_state(fsm); // execute immediately
 }
 /*****************************************************************************/
 /**
-State: Get Slave.
+Master state: SCAN.
-Executes the sub-statemachine of a slave.
+Executes the sub-statemachine for the scanning of a slave.
 */
-void ec_fsm_master_scan(ec_fsm_t *fsm)
+void ec_fsm_master_scan(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_master_t *master = fsm->master;
 ec_slave_t *slave = fsm->slave;
+uint16_t coupler_index, coupler_subindex;
+uint16_t reverse_coupler_index, current_coupler_index;
+ec_slave_ident_t *ident;
 fsm->slave_state(fsm); // execute slave state machine
 if (fsm->slave_state != ec_fsm_slave_end) return;
 // have all slaves been fetched?
-if (slave->list.next == &master->slaves)
+if (slave->list.next == &master->slaves) {
-{
-uint16_t coupler_index, coupler_subindex;
-uint16_t reverse_coupler_index, current_coupler_index;
-ec_slave_t *slave;
-ec_slave_ident_t *ident;
 EC_INFO("Bus scanning completed.\n");
 // identify all slaves and calculate coupler addressing
 coupler_index = 0;
 if (master->mode == EC_MASTER_MODE_RUNNING)
 slave->requested_state = EC_SLAVE_STATE_PREOP;
 else
 slave->requested_state = EC_SLAVE_STATE_INIT;
 }
+slave->state_error = 0;
 // calculate coupler-based slave address
 slave->coupler_index = current_coupler_index;
 slave->coupler_subindex = coupler_subindex;
 coupler_subindex++;
 }
+if (master->mode == EC_MASTER_MODE_FREERUN) {
+// start EoE processing
+ec_master_eoe_start(master);
+}
 fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 return;
 }
 }
 /*****************************************************************************/
 /**
-Free-Run state: Configure slaves.
+Master state: CONF.
-*/
+Starts configuring a slave.
+*/
-void ec_fsm_master_conf(ec_fsm_t *fsm)
-{
+void ec_fsm_master_conf(ec_fsm_t *fsm /**< finite state machine */)
-ec_master_t *master = fsm->master;
+{
-ec_slave_t *slave;
 fsm->slave_state(fsm); // execute slave's state machine
 if (fsm->slave_state != ec_fsm_slave_end) return;
+fsm->master_state = ec_fsm_master_proc_states;
-// check if any slaves are not in the state, they're supposed to be
-list_for_each_entry(slave, &master->slaves, list) {
-if (slave->state_error ||
-slave->requested_state == EC_SLAVE_STATE_UNKNOWN ||
-slave->current_state == slave->requested_state) continue;
-EC_INFO("Changing state of slave %i from ", slave->ring_position);
-ec_print_states(slave->current_state);
-printk(" to ");
-ec_print_states(slave->requested_state);
-printk(".\n");
-fsm->slave = slave;
-fsm->slave_state = ec_fsm_slave_conf;
-fsm->change_new = EC_SLAVE_STATE_INIT;
-fsm->change_state = ec_fsm_change_start;
-fsm->master_state = ec_fsm_master_conf;
-fsm->master_state(fsm); // execute immediately
-return;
-}
-fsm->master_state = ec_fsm_master_start;
 fsm->master_state(fsm); // execute immediately
 }
 /******************************************************************************
 *  slave state machine
 *****************************************************************************/
 /**
-Slave state: Start.
+Slave state: START_READING.
 First state of the slave state machine. Writes the station address to the
 slave, according to its ring position.
 */
-void ec_fsm_slave_start_reading(ec_fsm_t *fsm)
+void ec_fsm_slave_start_reading(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 // write station address
 ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2);
 }
 /*****************************************************************************/
 /**
-Slave state: Read status.
+Slave state: READ_STATUS.
 */
-void ec_fsm_slave_read_status(ec_fsm_t *fsm)
+void ec_fsm_slave_read_status(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 EC_ERR("FSM failed to write station address of slave %i.\n",
 }
 /*****************************************************************************/
 /**
-Slave state: Read base.
+Slave state: READ_BASE.
 */
-void ec_fsm_slave_read_base(ec_fsm_t *fsm)
+void ec_fsm_slave_read_base(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 }
 /*****************************************************************************/
 /**
-Slave state: Read DL.
+Slave state: READ_DL.
 */
-void ec_fsm_slave_read_dl(ec_fsm_t *fsm)
+void ec_fsm_slave_read_dl(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 }
 /*****************************************************************************/
 /**
-Slave state: Prepare SII.
+Slave state: PREPARE_SII.
 */
-void ec_fsm_slave_prepare_sii(ec_fsm_t *fsm)
+void ec_fsm_slave_prepare_sii(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 uint16_t dl_status;
 unsigned int i;
 slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;
 slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;
 }
 fsm->sii_offset = 0x0004;
+fsm->sii_mode = 1;
 fsm->sii_state = ec_fsm_sii_start_reading;
 fsm->slave_sii_num = 0;
 fsm->slave_state = ec_fsm_slave_read_sii;
 fsm->slave_state(fsm); // execute state immediately
 }
 /*****************************************************************************/
 /**
-Slave state: Read SII.
+Slave state: READ_SII.
 */
-void ec_fsm_slave_read_sii(ec_fsm_t *fsm)
+void ec_fsm_slave_read_sii(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_slave_t *slave = fsm->slave;
 // execute SII state machine
 fsm->sii_state(fsm);
 EC_ERR("FSM failed to read SII data at 0x%04X on slave %i.\n",
 fsm->sii_offset, slave->ring_position);
 return;
 }
-if (fsm->sii_state != ec_fsm_sii_finished) return;
+if (fsm->sii_state != ec_fsm_sii_end) return;
 switch (fsm->slave_sii_num) {
 case 0:
 slave->sii_alias = fsm->sii_result & 0xFFFF;
 fsm->sii_offset = 0x0008;
 }
 /*****************************************************************************/
 /**
-Slave state: Read categories.
+Slave state: CATEGORY_HEADER.
 Start reading categories.
 */
-void ec_fsm_slave_category_header(ec_fsm_t *fsm)
+void ec_fsm_slave_category_header(ec_fsm_t *fsm /**< finite state machine */)
 {
 // execute SII state machine
 fsm->sii_state(fsm);
 if (fsm->sii_state == ec_fsm_sii_error) {
 EC_ERR("FSM failed to read category header at 0x%04X on slave %i.\n",
 fsm->slave_cat_offset, fsm->slave->ring_position);
 return;
 }
-if (fsm->sii_state != ec_fsm_sii_finished) return;
+if (fsm->sii_state != ec_fsm_sii_end) return;
 // last category?
 if ((fsm->sii_result & 0xFFFF) == 0xFFFF) {
 kfree(fsm->slave_cat_data);
 fsm->slave_cat_data = NULL;
 // start reading category data
 fsm->slave_cat_data_offset = 0;
 fsm->sii_offset = (fsm->slave_cat_offset + 2 +
 fsm->slave_cat_data_offset);
+fsm->sii_mode = 1;
 fsm->sii_state = ec_fsm_sii_start_reading;
 fsm->slave_state = ec_fsm_slave_category_data;
 fsm->slave_state(fsm); // execute state immediately
 }
 /*****************************************************************************/
 /**
-Slave state: Category data.
+Slave state: CATEGORY_DATA.
 Reads category data.
 */
-void ec_fsm_slave_category_data(ec_fsm_t *fsm)
+void ec_fsm_slave_category_data(ec_fsm_t *fsm /**< finite state machine */)
 {
 // execute SII state machine
 fsm->sii_state(fsm);
 if (fsm->sii_state == ec_fsm_sii_error) {
 " on slave %i.\n", fsm->slave_cat_type, fsm->sii_offset,
 fsm->slave->ring_position);
 return;
 }
-if (fsm->sii_state != ec_fsm_sii_finished) return;
+if (fsm->sii_state != ec_fsm_sii_end) return;
 fsm->slave_cat_data[fsm->slave_cat_data_offset * 2] =
 fsm->sii_result & 0xFF;
 fsm->slave_cat_data[fsm->slave_cat_data_offset * 2 + 1] =
 (fsm->sii_result >> 8) & 0xFF;
 fsm->slave_cat_data_offset++;
 if (fsm->slave_cat_data_offset < fsm->slave_cat_words) {
 fsm->sii_offset = (fsm->slave_cat_offset + 2 +
 fsm->slave_cat_data_offset);
+fsm->sii_mode = 1;
 fsm->sii_state = ec_fsm_sii_start_reading;
 fsm->slave_state = ec_fsm_slave_category_data;
 fsm->slave_state(fsm); // execute state immediately
 return;
 }
 }
 // start reading next category header
 fsm->slave_cat_offset += 2 + fsm->slave_cat_words;
 fsm->sii_offset = fsm->slave_cat_offset;
+fsm->sii_mode = 1;
 fsm->sii_state = ec_fsm_sii_start_reading;
 fsm->slave_state = ec_fsm_slave_category_header;
 fsm->slave_state(fsm); // execute state immediately
 return;
 }
 /*****************************************************************************/
 /**
-Slave state: Start configuring.
+Slave state: CONF.
 */
-void ec_fsm_slave_conf(ec_fsm_t *fsm)
+void ec_fsm_slave_conf(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_slave_t *slave = fsm->slave;
 ec_master_t *master = fsm->master;
 ec_command_t *command = &fsm->command;
 }
 /*****************************************************************************/
 /**
-Slave state: Configure sync managers.
+Slave state: SYNC.
-*/
+Configure sync managers.
+*/
-void ec_fsm_slave_sync(ec_fsm_t *fsm)
+void ec_fsm_slave_sync(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 unsigned int j;
 const ec_sync_t *sync;
 ec_eeprom_sync_t *eeprom_sync, mbox_sync;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM failed to reset FMMUs of slave %i.\n",
+EC_ERR("Failed to reset FMMUs of slave %i.\n",
 slave->ring_position);
+slave->state_error = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (!slave->base_sync_count) { // no sync managers
 }
 /*****************************************************************************/
 /**
-Slave state: Change slave state to PREOP.
+Slave state: PREOP.
-*/
+Change slave state to PREOP.
+*/
-void ec_fsm_slave_preop(ec_fsm_t *fsm)
+void ec_fsm_slave_preop(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM failed to set sync managers on slave %i.\n",
+EC_ERR("Failed to set sync managers on slave %i.\n",
 slave->ring_position);
+slave->state_error = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 fsm->change_new = EC_SLAVE_STATE_PREOP;
 fsm->change_state = ec_fsm_change_start;
 fsm->slave_state = ec_fsm_slave_fmmu;
 fsm->change_state(fsm); // execute immediately
 }
 /*****************************************************************************/
 /**
-Slave state: Configure FMMUs.
+Slave state: FMMU.
-*/
+Configure FMMUs.
+*/
-void ec_fsm_slave_fmmu(ec_fsm_t *fsm)
+void ec_fsm_slave_fmmu(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_slave_t *slave = fsm->slave;
 ec_master_t *master = fsm->master;
 ec_command_t *command = &fsm->command;
 unsigned int j;
 // stop activation here for slaves without type
 if (!slave->type) {
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
-#if 0
-// slaves that are not registered are only brought into PREOP
-// state -> nice blinking and mailbox communication possible
-if (!slave->registered && !slave->type->special) {
-EC_WARN("Slave %i was not registered!\n", slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
-return;
-}
-#endif
 if (!slave->base_fmmu_count) {
 fsm->slave_state = ec_fsm_slave_saveop;
 fsm->slave_state(fsm); // execute immediately
 return;
 }
 /*****************************************************************************/
 /**
-Slave state: Set slave state to SAVEOP.
+Slave state: SAVEOP.
-*/
+Set slave state to SAVEOP.
+*/
-void ec_fsm_slave_saveop(ec_fsm_t *fsm)
+void ec_fsm_slave_saveop(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 if (fsm->slave->base_fmmu_count && (command->state != EC_CMD_RECEIVED ||
 command->working_counter != 1)) {
 EC_ERR("FSM failed to set FMMUs on slave %i.\n",
 fsm->slave->ring_position);
+fsm->slave->state_error = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 // set state to SAVEOP
 }
 /*****************************************************************************/
 /**
-Slave state: Set slave state to OP.
+Slave state: OP.
-*/
+Set slave state to OP.
+*/
-void ec_fsm_slave_op(ec_fsm_t *fsm)
+void ec_fsm_slave_op(ec_fsm_t *fsm /**< finite state machine */)
 {
 fsm->change_state(fsm); // execute state change state machine
 if (fsm->change_state == ec_fsm_change_error) {
 fsm->slave_state = ec_fsm_slave_end;
 }
 /*****************************************************************************/
 /**
-Slave state: Set slave state to OP.
+Slave state: OP2
-*/
+Executes the change state machine, until the OP state is set.
+*/
-void ec_fsm_slave_op2(ec_fsm_t *fsm)
+void ec_fsm_slave_op2(ec_fsm_t *fsm /**< finite state machine */)
 {
 fsm->change_state(fsm); // execute state change state machine
 if (fsm->change_state == ec_fsm_change_error) {
 fsm->slave_state = ec_fsm_slave_end;
 }
 /*****************************************************************************/
 /**
-Slave state: End.
+Slave state: END.
 End state of the slave state machine.
 */
-void ec_fsm_slave_end(ec_fsm_t *fsm)
+void ec_fsm_slave_end(ec_fsm_t *fsm /**< finite state machine */)
 {
 }
 /******************************************************************************
 *  SII state machine
 *****************************************************************************/
 /**
-Slave SII state: Start reading.
+SII state: START_READING.
 Starts reading the slave information interface.
 */
-void ec_fsm_sii_start_reading(ec_fsm_t *fsm)
+void ec_fsm_sii_start_reading(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 // initiate read operation
-ec_command_npwr(command, fsm->slave->station_address, 0x502, 6);
+if (fsm->sii_mode) {
+ec_command_npwr(command, fsm->slave->station_address, 0x502, 4);
+}
+else {
+ec_command_apwr(command, fsm->slave->ring_position, 0x502, 4);
+}
 EC_WRITE_U8 (command->data,     0x00); // read-only access
 EC_WRITE_U8 (command->data + 1, 0x01); // request read operation
-EC_WRITE_U32(command->data + 2, fsm->sii_offset);
+EC_WRITE_U16(command->data + 2, fsm->sii_offset);
 ec_master_queue_command(fsm->master, command);
 fsm->sii_state = ec_fsm_sii_check;
 }
 /*****************************************************************************/
 /**
-Slave SII state: Check.
+SII state: CHECK.
 Checks, if the SII-read-command has been sent and issues a fetch command.
 */
-void ec_fsm_sii_check(ec_fsm_t *fsm)
+void ec_fsm_sii_check(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM SII: Reception of check command failed.\n");
+EC_ERR("SII: Reception of read command failed.\n");
 fsm->sii_state = ec_fsm_sii_error;
 return;
 }
-ec_command_nprd(command, fsm->slave->station_address, 0x502, 10);
+fsm->sii_start = get_cycles();
+// issue check/fetch command
+if (fsm->sii_mode) {
+ec_command_nprd(command, fsm->slave->station_address, 0x502, 10);
+}
+else {
+ec_command_aprd(command, fsm->slave->ring_position, 0x502, 10);
+}
 ec_master_queue_command(fsm->master, command);
 fsm->sii_state = ec_fsm_sii_fetch;
 }
 /*****************************************************************************/
 /**
-Slave SII state: Fetch.
+SII state: FETCH.
 Fetches the result of an SII-read command.
 */
-void ec_fsm_sii_fetch(ec_fsm_t *fsm)
+void ec_fsm_sii_fetch(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM SII: Reception of fetch command failed.\n");
+EC_ERR("SII: Reception of check/fetch command failed.\n");
 fsm->sii_state = ec_fsm_sii_error;
 return;
 }
 // check "busy bit"
-if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) {
+if (EC_READ_U8(command->data + 1) & 0x81) {
-fsm->sii_result = EC_READ_U32(command->data + 6);
+// still busy... timeout?
-fsm->sii_state = ec_fsm_sii_finished;
+if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) {
-}
+EC_ERR("SII: Timeout.\n");
-}
+fsm->sii_state = ec_fsm_sii_error;
+#if 0
-/*****************************************************************************/
+EC_DBG("SII busy: %02X %02X %02X %02X\n",
+EC_READ_U8(command->data + 0),
-/**
+EC_READ_U8(command->data + 1),
-Slave SII state: Finished.
+EC_READ_U8(command->data + 2),
+EC_READ_U8(command->data + 3));
+#endif
+}
+// issue check/fetch command again
+if (fsm->sii_mode) {
+ec_command_nprd(command, fsm->slave->station_address, 0x502, 10);
+}
+else {
+ec_command_aprd(command, fsm->slave->ring_position, 0x502, 10);
+}
+ec_master_queue_command(fsm->master, command);
+return;
+}
+#if 0
+EC_DBG("SII rec: %02X %02X %02X %02X - %02X %02X %02X %02X\n",
+EC_READ_U8(command->data + 0), EC_READ_U8(command->data + 1),
+EC_READ_U8(command->data + 2), EC_READ_U8(command->data + 3),
+EC_READ_U8(command->data + 6), EC_READ_U8(command->data + 7),
+EC_READ_U8(command->data + 8), EC_READ_U8(command->data + 9));
+#endif
+// SII value received.
+fsm->sii_result = EC_READ_U32(command->data + 6);
+fsm->sii_state = ec_fsm_sii_end;
+}
+/*****************************************************************************/
+/**
+SII state: END.
 End state of the slave SII state machine.
 */
-void ec_fsm_sii_finished(ec_fsm_t *fsm)
+void ec_fsm_sii_end(ec_fsm_t *fsm /**< finite state machine */)
 {
 }
 /*****************************************************************************/
 /**
-Slave SII state: Error.
+SII state: ERROR.
 End state of the slave SII state machine.
 */
-void ec_fsm_sii_error(ec_fsm_t *fsm)
+void ec_fsm_sii_error(ec_fsm_t *fsm /**< finite state machine */)
 {
 }
 /******************************************************************************
 *  state change state machine
 *****************************************************************************/
 /**
-State change state: Start.
+Change state: START.
 */
-void ec_fsm_change_start(ec_fsm_t *fsm)
+void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 // write new state to slave
 }
 /*****************************************************************************/
 /**
-State change state: Check.
+Change state: CHECK.
 */
-void ec_fsm_change_check(ec_fsm_t *fsm)
+void ec_fsm_change_check(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
-if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+if (command->state != EC_CMD_RECEIVED) {
-EC_ERR("FSM: Reception of state command failed.\n");
+EC_ERR("Failed to send state command to slave %i!\n",
+fsm->slave->ring_position);
 slave->state_error = 1;
 fsm->change_state = ec_fsm_change_error;
 return;
 }
-//start = get_cycles();
+if (command->working_counter != 1) {
-//timeout = (cycles_t) 10 * cpu_khz; // 10ms
+EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not"
+" respond.\n", fsm->change_new, fsm->slave->ring_position);
+slave->state_error = 1;
+fsm->change_state = ec_fsm_change_error;
+return;
+}
+fsm->change_start = get_cycles();
 // read AL status from slave
 ec_command_nprd(command, slave->station_address, 0x0130, 2);
 ec_master_queue_command(fsm->master, command);
 fsm->change_state = ec_fsm_change_status;
 }
 /*****************************************************************************/
 /**
-State change state: Status.
+Change state: STATUS.
 */
-void ec_fsm_change_status(ec_fsm_t *fsm)
+void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM: Reception of state check command failed.\n");
+EC_ERR("Failed to check state 0x%02X on slave %i.\n",
+fsm->change_new, slave->ring_position);
 slave->state_error = 1;
 fsm->change_state = ec_fsm_change_error;
 return;
 }
 slave->current_state = EC_READ_U8(command->data);
-if (slave->current_state & 0x10) { // state change error
+if (slave->current_state == fsm->change_new) {
+// state has been set successfully
+fsm->change_state = ec_fsm_change_end;
+return;
+}
+if (slave->current_state & 0x10) {
+// state change error
+fsm->change_new = slave->current_state & 0x0F;
 EC_ERR("Failed to set state 0x%02X - Slave %i refused state change"
 " (code 0x%02X)!\n", fsm->change_new, slave->ring_position,
 slave->current_state);
-fsm->change_new = slave->current_state & 0x0F;
 // fetch AL status error code
 ec_command_nprd(command, slave->station_address, 0x0134, 2);
 ec_master_queue_command(fsm->master, command);
 fsm->change_state = ec_fsm_change_code;
 return;
 }
-if (slave->current_state == fsm->change_new) {
+if (get_cycles() - fsm->change_start >= (cycles_t) 10 * cpu_khz) {
-fsm->change_state = ec_fsm_change_end;
+// timeout while checking
-return;
+slave->state_error = 1;
-}
+fsm->change_state = ec_fsm_change_error;
+EC_ERR("Timeout while setting state 0x%02X on slave %i.\n",
-EC_ERR("Failed to check state 0x%02X of slave %i - Timeout!\n",
+fsm->change_new, slave->ring_position);
-fsm->change_new, slave->ring_position);
+return;
-slave->state_error = 1;
+}
-fsm->change_state = ec_fsm_change_error;
-}
+// still old state: check again
+ec_command_nprd(command, slave->station_address, 0x0130, 2);
-/*****************************************************************************/
+ec_master_queue_command(fsm->master, command);
+}
-/**
-State change state: Code.
+/*****************************************************************************/
-*/
+/**
-void ec_fsm_change_code(ec_fsm_t *fsm)
+Change state: CODE.
+*/
+void ec_fsm_change_code(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 uint32_t code;
 const ec_code_msg_t *al_msg;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM: Reception of AL status code command failed.\n");
+EC_ERR("Reception of AL status code command failed.\n");
 slave->state_error = 1;
 fsm->change_state = ec_fsm_change_error;
 return;
 }
 }
 /*****************************************************************************/
 /**
-State change state: Acknowledge.
+Change state: ACK.
 */
-void ec_fsm_change_ack(ec_fsm_t *fsm)
+void ec_fsm_change_ack(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM: Reception of state ack command failed.\n");
+EC_ERR("Reception of state ack command failed.\n");
 slave->state_error = 1;
 fsm->change_state = ec_fsm_change_error;
 return;
 }
 }
 /*****************************************************************************/
 /**
-State change state: Acknowledge 2.
+Change state: ACK.
-*/
+Acknowledge 2.
+*/
-void ec_fsm_change_ack2(ec_fsm_t *fsm)
+void ec_fsm_change_ack2(ec_fsm_t *fsm /**< finite state machine */)
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
-EC_ERR("FSM: Reception of state ack check command failed.\n");
+EC_ERR("Reception of state ack check command failed.\n");
 slave->state_error = 1;
 fsm->change_state = ec_fsm_change_error;
 return;
 }
 }
 /*****************************************************************************/
 /**
-State change state: End.
+Change state: END.
 */
-void ec_fsm_change_end(ec_fsm_t *fsm)
+void ec_fsm_change_end(ec_fsm_t *fsm /**< finite state machine */)
 {
 }
 /*****************************************************************************/
 /**
-State change state: Error.
+Change state: ERROR.
 */
-void ec_fsm_change_error(ec_fsm_t *fsm)
+void ec_fsm_change_error(ec_fsm_t *fsm /**< finite state machine */)
 {
 }
 /*****************************************************************************/

changeset 260	5fe7df7f2433
parent 259	87b20532872b
child 262	636d43737371