etherlabmaster: comparison master/fsm.c

equal deleted inserted replaced

-:0b1f877cf3f1
+:2cf6ae0a2419
 void ec_fsm_master_address(ec_fsm_t *);
 void ec_fsm_master_conf(ec_fsm_t *);
 void ec_fsm_master_eeprom(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_state(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_eeprom_size(ec_fsm_t *);
 void ec_fsm_slave_fetch_eeprom(ec_fsm_t *);
 void ec_fsm_slave_fetch_eeprom2(ec_fsm_t *);
 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;
 ec_command_init(&fsm->command);
 if (ec_command_prealloc(&fsm->command, EC_MAX_DATA_SIZE)) {
-EC_ERR("FSM failed to allocate FSM command.\n");
+EC_ERR("Failed to allocate FSM command.\n");
 return -1;
 }
 return 0;
 }
 {
 ec_master_t *master = fsm->master;
 ec_slave_t *slave = fsm->slave;
 // have all states been read?
-if (slave->list.next == &master->slaves) {
+if (slave->list.next != &master->slaves) {
+// process next slave
-// check, if a bus validation has to be done
+fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
-if (fsm->master_validation) {
+ec_command_nprd(&fsm->command, fsm->slave->station_address, 0x0130, 2);
-fsm->master_validation = 0;
+ec_master_queue_command(master, &fsm->command);
-list_for_each_entry(slave, &master->slaves, list) {
+fsm->master_state = ec_fsm_master_states;
-if (slave->online) continue;
+return;
+}
-// At least one slave is offline. validate!
-EC_INFO("Validating bus.\n");
+// all slave stated read; check, if a bus validation has to be done
-fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
+if (fsm->master_validation) {
-fsm->master_state = ec_fsm_master_validate_vendor;
+fsm->master_validation = 0;
-fsm->sii_offset = 0x0008; // vendor ID
+list_for_each_entry(slave, &master->slaves, list) {
-fsm->sii_mode = 0;
+if (slave->online) continue;
-fsm->sii_state = ec_fsm_sii_start_reading;
-fsm->sii_state(fsm); // execute immediately
+// At least one slave is offline. validate!
-return;
+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;
 }
+}
-fsm->master_state = ec_fsm_master_proc_states;
-fsm->master_state(fsm); // execute immediately
+fsm->master_state = ec_fsm_master_proc_states;
-return;
+fsm->master_state(fsm); // execute immediately
-}
-// 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;
 }
 /*****************************************************************************/
 /**
 if (master->mode == EC_MASTER_MODE_FREERUN) {
 // nothing to configure. check for pending EEPROM write operations.
 list_for_each_entry(slave, &master->slaves, list) {
 if (!slave->new_eeprom_data) continue;
+if (!slave->online || slave->error_flag) {
+kfree(slave->new_eeprom_data);
+slave->new_eeprom_data = NULL;
+EC_ERR("Discarding EEPROM data, slave %i not ready.\n",
+slave->ring_position);
+continue;
+}
 // found pending EEPROM write operation. execute it!
 EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position);
 fsm->sii_offset = 0x0000;
 memcpy(fsm->sii_value, slave->new_eeprom_data, 2);
 ec_slave_t *slave = fsm->slave;
 fsm->sii_state(fsm); // execute SII state machine
 if (fsm->sii_state == ec_fsm_sii_error) {
-	fsm->slave->error_flag = 1;
+fsm->slave->error_flag = 1;
 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_slave_t *slave = fsm->slave;
 fsm->sii_state(fsm); // execute SII state machine
 if (fsm->sii_state == ec_fsm_sii_error) {
-	fsm->slave->error_flag = 1;
+fsm->slave->error_flag = 1;
 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;
 }
 ident++;
 }
 if (!slave->type) {
-EC_WARN("FSM: Unknown slave device (vendor 0x%08X,"
+EC_WARN("Unknown slave device (vendor 0x%08X,"
 " code 0x%08X) at position %i.\n",
 slave->sii_vendor_id, slave->sii_product_code,
 slave->ring_position);
 }
 else {
 }
 }
 // determine initial state.
 if ((slave->type &&
-		 (slave->type->special == EC_TYPE_BUS_COUPLER ||
+(slave->type->special == EC_TYPE_BUS_COUPLER ||
-		  slave->type->special == EC_TYPE_INFRA))) {
+slave->type->special == EC_TYPE_INFRA))) {
 slave->requested_state = EC_SLAVE_STATE_OP;
 }
 else {
 if (master->mode == EC_MASTER_MODE_RUNNING)
 slave->requested_state = EC_SLAVE_STATE_PREOP;
 ec_slave_t *slave = fsm->slave;
 fsm->sii_state(fsm); // execute SII state machine
 if (fsm->sii_state == ec_fsm_sii_error) {
-	fsm->slave->error_flag = 1;
+fsm->slave->error_flag = 1;
 EC_ERR("Failed to write EEPROM contents to slave %i.\n",
 slave->ring_position);
 kfree(slave->new_eeprom_data);
 slave->new_eeprom_data = NULL;
 fsm->master_state = ec_fsm_master_start;
 // write station address
 ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2);
 EC_WRITE_U16(command->data, fsm->slave->station_address);
 ec_master_queue_command(fsm->master, command);
-fsm->slave_state = ec_fsm_slave_read_status;
+fsm->slave_state = ec_fsm_slave_read_state;
 }
 /*****************************************************************************/
 /**
 Slave state: READ_STATUS.
 */
-void ec_fsm_slave_read_status(ec_fsm_t *fsm /**< finite state machine */)
+void ec_fsm_slave_read_state(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",
+fsm->slave->error_flag = 1;
+fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Failed to write station address of slave %i.\n",
 fsm->slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
+return;
-return;
+}
-}
+// Read AL state
-// read AL status
 ec_command_nprd(command, fsm->slave->station_address, 0x0130, 2);
 ec_master_queue_command(fsm->master, command);
 fsm->slave_state = ec_fsm_slave_read_base;
 }
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 fsm->slave->error_flag = 1;
-EC_ERR("FSM failed to read AL status of slave %i.\n",
+fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Failed to read AL state of slave %i.\n",
 fsm->slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 slave->current_state = EC_READ_U8(command->data);
 if (slave->current_state & EC_ACK) {
-EC_WARN("Slave %i has status error bit set (0x%02X)!\n",
+EC_WARN("Slave %i has state error bit set (0x%02X)!\n",
 slave->ring_position, slave->current_state);
 slave->current_state &= 0x0F;
 }
 // read base data
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 fsm->slave->error_flag = 1;
-EC_ERR("FSM failed to read base data of slave %i.\n",
+fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Failed to read base data of slave %i.\n",
 slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 slave->base_type       = EC_READ_U8 (command->data);
 slave->base_revision   = EC_READ_U8 (command->data + 1);
 uint16_t dl_status;
 unsigned int i;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
 fsm->slave->error_flag = 1;
-EC_ERR("FSM failed to read DL status of slave %i.\n",
+fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Failed to read DL status of slave %i.\n",
 slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 dl_status = EC_READ_U16(command->data);
 for (i = 0; i < 4; i++) {
 // execute SII state machine
 fsm->sii_state(fsm);
 if (fsm->sii_state == ec_fsm_sii_error) {
-	fsm->slave->error_flag = 1;
+fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 EC_ERR("Failed to read EEPROM size of slave %i.\n",
 slave->ring_position);
 return;
 }
 }
 if (!(slave->eeprom_data =
 (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {
 fsm->slave->error_flag = 1;
+fsm->slave_state = ec_fsm_slave_end;
 EC_ERR("Failed to allocate EEPROM data on slave %i.\n",
 slave->ring_position);
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 // Start fetching EEPROM contents
 // execute SII state machine
 fsm->sii_state(fsm);
 if (fsm->sii_state == ec_fsm_sii_error) {
-	fsm->slave->error_flag = 1;
+fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",
 slave->ring_position);
 return;
 }
 }
 cat_word += cat_size + 2;
 }
 end:
 fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 }
 /*****************************************************************************/
 ec_command_t *command = &fsm->command;
 fsm->change_state(fsm); // execute state change state machine
 if (fsm->change_state == ec_fsm_change_error) {
-	slave->error_flag = 1;
+slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (fsm->change_state != ec_fsm_change_end) return;
 // slave is now in INIT
 if (slave->current_state == slave->requested_state) {
-fsm->slave_state = ec_fsm_slave_end;
+fsm->slave_state = ec_fsm_slave_end; // successful
 return;
 }
 // check for slave registration
 if (!slave->type) {
 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) {
+slave->error_flag = 1;
+fsm->slave_state = ec_fsm_slave_end;
 EC_ERR("Failed to reset FMMUs of slave %i.\n",
 slave->ring_position);
-slave->error_flag = 1;
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (!slave->base_sync_count) { // no sync managers
 fsm->slave_state = ec_fsm_slave_preop;
 {
 // does it supply sync manager configurations in its EEPROM?
 if (!list_empty(&slave->eeprom_syncs)) {
 list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) {
 if (eeprom_sync->index >= slave->base_sync_count) {
-EC_ERR("Invalid sync manager configuration found!");
 fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Invalid sync manager configuration found!");
 return;
 }
 ec_eeprom_sync_config(eeprom_sync,
 command->data + EC_SYNC_SIZE
 * eeprom_sync->index);
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+slave->error_flag = 1;
+fsm->slave_state = ec_fsm_slave_end;
 EC_ERR("Failed to set sync managers on slave %i.\n",
 slave->ring_position);
-slave->error_flag = 1;
-fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 fsm->change_new = EC_SLAVE_STATE_PREOP;
 fsm->change_state = ec_fsm_change_start;
 unsigned int j;
 fsm->change_state(fsm); // execute state change state machine
 if (fsm->change_state == ec_fsm_change_error) {
-	slave->error_flag = 1;
+slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (fsm->change_state != ec_fsm_change_end) return;
 // slave is now in PREOP
 if (slave->current_state == slave->requested_state) {
-fsm->slave_state = ec_fsm_slave_end;
+fsm->slave_state = ec_fsm_slave_end; // successful
 return;
 }
 // stop activation here for slaves without type
 if (!slave->type) {
-fsm->slave_state = ec_fsm_slave_end;
+fsm->slave_state = ec_fsm_slave_end; // successful
 return;
 }
 if (!slave->base_fmmu_count) {
 fsm->slave_state = ec_fsm_slave_saveop;
 {
 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->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
+EC_ERR("Failed to set FMMUs on slave %i.\n",
+fsm->slave->ring_position);
 return;
 }
 // set state to SAVEOP
 fsm->slave_state = ec_fsm_slave_op;
 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->error_flag = 1;
+fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (fsm->change_state != ec_fsm_change_end) return;
 // slave is now in SAVEOP
 if (fsm->slave->current_state == fsm->slave->requested_state) {
-fsm->slave_state = ec_fsm_slave_end;
+fsm->slave_state = ec_fsm_slave_end; // successful
 return;
 }
 // set state to OP
 fsm->slave_state = ec_fsm_slave_op2;
 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->error_flag = 1;
+fsm->slave->error_flag = 1;
 fsm->slave_state = ec_fsm_slave_end;
 return;
 }
 if (fsm->change_state != ec_fsm_change_end) return;
 // slave is now in OP
-fsm->slave_state = ec_fsm_slave_end;
+fsm->slave_state = ec_fsm_slave_end; // successful
 }
 /*****************************************************************************/
 /**
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED) {
+fsm->change_state = ec_fsm_change_error;
 EC_ERR("Failed to send state command to slave %i!\n",
 fsm->slave->ring_position);
+return;
+}
+if (command->working_counter != 1) {
 fsm->change_state = ec_fsm_change_error;
-return;
-}
-if (command->working_counter != 1) {
 EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not"
 " respond.\n", fsm->change_new, fsm->slave->ring_position);
-fsm->change_state = ec_fsm_change_error;
 return;
 }
 fsm->change_start = get_cycles();
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+fsm->change_state = ec_fsm_change_error;
 EC_ERR("Failed to check state 0x%02X on slave %i.\n",
 fsm->change_new, slave->ring_position);
-fsm->change_state = ec_fsm_change_error;
 return;
 }
 slave->current_state = EC_READ_U8(command->data);
 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) {
+fsm->change_state = ec_fsm_change_error;
 EC_ERR("Reception of AL status code command failed.\n");
-fsm->change_state = ec_fsm_change_error;
 return;
 }
 if ((code = EC_READ_U16(command->data))) {
 for (al_msg = al_status_messages; al_msg->code; al_msg++) {
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+fsm->change_state = ec_fsm_change_error;
 EC_ERR("Reception of state ack command failed.\n");
-fsm->change_state = ec_fsm_change_error;
 return;
 }
 // read new AL status
 ec_command_nprd(command, slave->station_address, 0x0130, 2);
 {
 ec_command_t *command = &fsm->command;
 ec_slave_t *slave = fsm->slave;
 if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {
+fsm->change_state = ec_fsm_change_error;
 EC_ERR("Reception of state ack check command failed.\n");
+return;
+}
+slave->current_state = EC_READ_U8(command->data);
+if (slave->current_state == fsm->change_new) {
 fsm->change_state = ec_fsm_change_error;
-return;
-}
-slave->current_state = EC_READ_U8(command->data);
-if (slave->current_state == fsm->change_new) {
 EC_INFO("Acknowleged state 0x%02X on slave %i.\n",
 slave->current_state, slave->ring_position);
-fsm->change_state = ec_fsm_change_error;
+return;
-return;
+}
-}
+fsm->change_state = ec_fsm_change_error;
 EC_WARN("Failed to acknowledge state 0x%02X on slave %i"
 " - Timeout!\n", fsm->change_new, slave->ring_position);
-fsm->change_state = ec_fsm_change_error;
 }
 /*****************************************************************************/
 /**

changeset 292	2cf6ae0a2419
parent 291	0b1f877cf3f1
child 293	14aeb79aa992