etherlabmaster: comparison master/master.c

equal deleted inserted replaced

-:7b3126cf6dab
+:b4960499098f
 void ec_master_run_eoe(void *);
 #endif
 ssize_t ec_show_master_attribute(struct kobject *, struct attribute *, char *);
 void ec_master_process_watch_command(ec_master_t *);
-// state functions
-void ec_master_state_start(ec_master_t *);
-void ec_master_state_slave(ec_master_t *);
-void ec_master_state_finished(ec_master_t *);
 /*****************************************************************************/
 /** \cond */
 EC_SYSFS_READ_ATTR(slave_count);
 unsigned int index /**< master index */
 )
 {
 EC_INFO("Initializing master %i.\n", index);
+if (!(master->workqueue = create_singlethread_workqueue("EoE"))) {
+EC_ERR("Failed to create master workqueue.\n");
+goto out_return;
+}
+if (ec_fsm_init(&master->fsm, master)) goto out_free_wq;
 master->index = index;
 master->device = NULL;
 master->reserved = 0;
 INIT_LIST_HEAD(&master->slaves);
 INIT_LIST_HEAD(&master->command_queue);
 INIT_LIST_HEAD(&master->domains);
 INIT_LIST_HEAD(&master->eoe_slaves);
 ec_command_init(&master->simple_command);
-ec_command_init(&master->watch_command);
-if (!(master->workqueue = create_singlethread_workqueue("EoE"))) {
-EC_ERR("Failed to create master workqueue.\n");
-goto out_return;
-}
-// init freerun work
 INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master);
 // init eoe timer
 #ifdef EOE_TIMER
 init_timer(&master->eoe_timer);
 memset(&master->kobj, 0x00, sizeof(struct kobject));
 kobject_init(&master->kobj);
 master->kobj.ktype = &ktype_ec_master;
 if (kobject_set_name(&master->kobj, "ethercat%i", index)) {
 EC_ERR("Failed to set kobj name.\n");
-goto out_kobj_put;
+kobject_put(&master->kobj);
+return -1;
 }
 ec_master_reset(master);
 return 0;
-out_kobj_put:
+out_free_wq:
-kobject_put(&master->kobj);
+destroy_workqueue(master->workqueue);
 out_return:
 return -1;
 }
 /*****************************************************************************/
 ec_master_t *master = container_of(kobj, ec_master_t, kobj);
 EC_INFO("Clearing master %i...\n", master->index);
 ec_master_reset(master);
+ec_fsm_clear(&master->fsm);
+ec_command_clear(&master->simple_command);
+destroy_workqueue(master->workqueue);
 if (master->device) {
 ec_device_clear(master->device);
 kfree(master->device);
 }
-ec_command_clear(&master->simple_command);
-ec_command_clear(&master->watch_command);
-destroy_workqueue(master->workqueue);
 EC_INFO("Master %i cleared.\n", master->index);
 }
 /*****************************************************************************/
 called, to free the slave list, domains etc.
 */
 void ec_master_reset(ec_master_t *master /**< EtherCAT master */)
 {
-ec_slave_t *slave, *next_s;
 ec_command_t *command, *next_c;
 ec_domain_t *domain, *next_d;
 ec_eoe_t *eoe, *next_eoe;
 // stop EoE processing
 #endif
 // stop free-run mode
 ec_master_freerun_stop(master);
-// remove all slaves
+ec_master_clear_slaves(master);
-list_for_each_entry_safe(slave, next_s, &master->slaves, list) {
-list_del(&slave->list);
-kobject_del(&slave->kobj);
-kobject_put(&slave->kobj);
-}
-master->slave_count = 0;
 // empty command queue
 list_for_each_entry_safe(command, next_c, &master->command_queue, queue) {
 list_del_init(&command->queue);
 command->state = EC_CMD_ERROR;
 master->command_index = 0;
 master->debug_level = 0;
 master->timeout = 500; // 500us
-master->slaves_responding = 0;
-master->slave_states = EC_SLAVE_STATE_UNKNOWN;
 master->stats.timeouts = 0;
 master->stats.delayed = 0;
 master->stats.corrupted = 0;
 master->stats.unmatched = 0;
 master->stats.t_last = 0;
 master->mode = EC_MASTER_MODE_IDLE;
 master->request_cb = NULL;
 master->release_cb = NULL;
 master->cb_data = NULL;
+ec_fsm_reset(&master->fsm);
+}
+/*****************************************************************************/
+/**
+Clears all slaves.
+*/
+void ec_master_clear_slaves(ec_master_t *master)
+{
+ec_slave_t *slave, *next_slave;
+list_for_each_entry_safe(slave, next_slave, &master->slaves, list) {
+list_del(&slave->list);
+kobject_del(&slave->kobj);
+kobject_put(&slave->kobj);
+}
+master->slave_count = 0;
 }
 /*****************************************************************************/
 /**
 \return 0 in case of success, else < 0
 */
 int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */)
 {
-ec_slave_t *slave, *next;
+ec_slave_t *slave;
 ec_slave_ident_t *ident;
+ec_command_t *command;
 unsigned int i;
-ec_command_t *command;
 uint16_t coupler_index, coupler_subindex;
 uint16_t reverse_coupler_index, current_coupler_index;
 if (!list_empty(&master->slaves)) {
 EC_ERR("Slave scan already done!\n");
 if (!master->slave_count) return 0;
 // init slaves
 for (i = 0; i < master->slave_count; i++) {
-if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_KERNEL))) {
+if (!(slave =
+(ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_KERNEL))) {
 EC_ERR("Failed to allocate slave %i!\n", i);
 goto out_free;
 }
 if (ec_slave_init(slave, master, i, i + 1)) goto out_free;
 }
 if (!slave->type) {
 EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"
 " position %i.\n", slave->sii_vendor_id,
-slave->sii_product_code, i);
+slave->sii_product_code, slave->ring_position);
 }
 else if (slave->type->special == EC_TYPE_BUS_COUPLER) {
 if (slave->sii_alias)
 current_coupler_index = reverse_coupler_index--;
 else
 }
 return 0;
 out_free:
-list_for_each_entry_safe(slave, next, &master->slaves, list) {
+ec_master_clear_slaves(master);
-list_del(&slave->list);
-kobject_del(&slave->kobj);
-kobject_put(&slave->kobj);
-}
 return -1;
 }
 /*****************************************************************************/
 }
 EC_INFO("Starting Free-Run mode.\n");
 master->mode = EC_MASTER_MODE_FREERUN;
+ec_fsm_reset(&master->fsm);
-if (master->watch_command.state == EC_CMD_INIT) {
+queue_delayed_work(master->workqueue, &master->freerun_work, HZ / 100);
-if (ec_command_brd(&master->watch_command, 0x130, 2)) {
-EC_ERR("Failed to allocate watchdog command!\n");
-return;
-}
-}
-master->freerun_state = ec_master_state_start;
-ecrt_master_prepare_async_io(master); /** \todo necessary? */
-queue_delayed_work(master->workqueue, &master->freerun_work, HZ);
 }
 /*****************************************************************************/
 /**
 Stops the Free-Run mode.
 */
 void ec_master_freerun_stop(ec_master_t *master /**< EtherCAT master */)
 {
-ec_slave_t *slave, *next_slave;
 if (master->mode != EC_MASTER_MODE_FREERUN) return;
 EC_INFO("Stopping Free-Run mode.\n");
 if (!cancel_delayed_work(&master->freerun_work)) {
 flush_workqueue(master->workqueue);
 }
-// remove slaves
+ec_master_clear_slaves(master);
-list_for_each_entry_safe(slave, next_slave, &master->slaves, list) {
-list_del(&slave->list);
-kobject_del(&slave->kobj);
-kobject_put(&slave->kobj);
-}
 master->mode = EC_MASTER_MODE_IDLE;
 }
 /*****************************************************************************/
 */
 void ec_master_freerun(void *data /**< master pointer */)
 {
 ec_master_t *master = (ec_master_t *) data;
+unsigned long delay;
 ecrt_master_async_receive(master);
-// watchdog command
-ec_master_process_watch_command(master);
-if (master->watch_command.working_counter != master->slave_count) {
-master->slave_count = master->watch_command.working_counter;
-EC_INFO("Freerun: Topology change detected.\n");
-// reset freerun state machine
-master->freerun_state = ec_master_state_start;
-}
-ec_master_queue_command(master, &master->watch_command);
 // execute freerun state machine
-master->freerun_state(master);
+ec_fsm_execute(&master->fsm);
 ecrt_master_async_send(master);
-queue_delayed_work(master->workqueue, &master->freerun_work, HZ);
+delay = HZ / 100;
-}
+if (ec_fsm_idle(&master->fsm)) delay = HZ;
+queue_delayed_work(master->workqueue, &master->freerun_work, delay);
-/*****************************************************************************/
-/**
-Free-Run state: Start.
-Processes new slave count.
-*/
-void ec_master_state_start(ec_master_t *master /**< EtherCAT master*/)
-{
-ec_slave_t *slave, *next_slave;
-unsigned int i;
-// remove slaves
-list_for_each_entry_safe(slave, next_slave, &master->slaves, list) {
-list_del(&slave->list);
-kobject_del(&slave->kobj);
-kobject_put(&slave->kobj);
-}
-if (!master->slave_count) {
-// no slaves present -> finish state machine.
-master->freerun_state = ec_master_state_finished;
-EC_INFO("Freerun: No slaves found.\n");
-return;
-}
-// init slaves
-for (i = 0; i < master->slave_count; i++) {
-if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),
-GFP_KERNEL))) {
-EC_ERR("Failed to allocate slave %i!\n", i);
-master->freerun_state = ec_master_state_finished;
-return;
-}
-if (ec_slave_init(slave, master, i, i + 1)) {
-master->freerun_state = ec_master_state_finished;
-return;
-}
-if (kobject_add(&slave->kobj)) {
-EC_ERR("Failed to add kobject.\n");
-kobject_put(&slave->kobj); // free
-master->freerun_state = ec_master_state_finished;
-return;
-}
-list_add_tail(&slave->list, &master->slaves);
-}
-// begin scanning of slaves
-master->freerun_slave = list_entry(master->slaves.next, ec_slave_t, list);
-master->freerun_state = ec_master_state_slave;
-}
-/*****************************************************************************/
-/**
-Free-Run state: Get Slave.
-Executes the sub-statemachine of a slave.
-*/
-void ec_master_state_slave(ec_master_t *master /**< EtherCAT master*/)
-{
-master->freerun_state = ec_master_state_finished;
-}
-/*****************************************************************************/
-/**
-Free-Run state: Finished.
-End state of the state machine. Does nothing.
-*/
-void ec_master_state_finished(ec_master_t *master /**< EtherCAT master*/)
-{
-return;
 }
 /*****************************************************************************/
 /**
 }
 /*****************************************************************************/
 /**
-Processes the watchdog command.
-*/
-void ec_master_process_watch_command(ec_master_t *master
-/**< EtherCAT master */
-)
-{
-unsigned int first;
-if (unlikely(master->watch_command.state == EC_CMD_INIT)) return;
-first = 1;
-if (master->watch_command.working_counter != master->slaves_responding ||
-master->watch_command.data[0] != master->slave_states)
-{
-master->slaves_responding = master->watch_command.working_counter;
-master->slave_states = master->watch_command.data[0];
-EC_INFO("%i slave%s responding (", master->slaves_responding,
-master->slaves_responding == 1 ? "" : "s");
-if (master->slave_states & EC_SLAVE_STATE_INIT) {
-printk("INIT");
-first = 0;
-}
-if (master->slave_states & EC_SLAVE_STATE_PREOP) {
-if (!first) printk(", ");
-printk("PREOP");
-first = 0;
-}
-if (master->slave_states & EC_SLAVE_STATE_SAVEOP) {
-if (!first) printk(", ");
-printk("SAVEOP");
-first = 0;
-}
-if (master->slave_states & EC_SLAVE_STATE_OP) {
-if (!first) printk(", ");
-printk("OP");
-}
-printk(")\n");
-}
-}
-/*****************************************************************************/
-/**
 Does the Ethernet-over-EtherCAT processing.
 */
 #ifdef EOE_TIMER
 void ec_master_run_eoe(unsigned long data /**< master pointer */)
 uint32_t domain_offset;
 ec_domain_t *domain;
 ec_eeprom_sync_t *eeprom_sync, mbox_sync;
 command = &master->simple_command;
-if (master->watch_command.state == EC_CMD_INIT) {
-if (ec_command_brd(&master->watch_command, 0x130, 2)) {
-EC_ERR("Failed to allocate watchdog command!\n");
-return -1;
-}
-}
 // allocate all domains
 domain_offset = 0;
 list_for_each_entry(domain, &master->domains, list) {
 if (ec_domain_alloc(domain, domain_offset)) {
 // change state to OP
 if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_OP)))
 return -1;
 }
-master->slaves_responding = 0;
-master->slave_states = EC_SLAVE_STATE_INIT;
 return 0;
 }
 /*****************************************************************************/
 void ecrt_master_run(ec_master_t *master /**< EtherCAT master */)
 {
 // output statistics
 ec_master_output_stats(master);
-// watchdog command
+// execute master state machine
-ec_master_process_watch_command(master);
+ec_fsm_execute(&master->fsm);
-ec_master_queue_command(master, &master->watch_command);
 }
 /*****************************************************************************/
 /**

changeset 238	b4960499098f
parent 236	34d7ce41e9db
child 239	07bca2b018ed