# HG changeset patch # User Florian Pose # Date 1267464822 -3600 # Node ID 1875b9fea0ba5cc1dced5e90272e146a9dc7e4df # Parent ef09f0ea0c4cf1e21dce951412729d86ee944c2e Implemented reading Sercos-over-EtherCAT services. To be continued... diff -r ef09f0ea0c4c -r 1875b9fea0ba master/Kbuild.in --- a/master/Kbuild.in Fri Feb 26 18:22:02 2010 +0100 +++ b/master/Kbuild.in Mon Mar 01 18:33:42 2010 +0100 @@ -44,12 +44,13 @@ fsm_coe.o \ fsm_foe.o \ fsm_master.o \ - fsm_slave.o \ fsm_pdo.o \ fsm_pdo_entry.o \ fsm_sii.o \ + fsm_slave.o \ fsm_slave_config.o \ fsm_slave_scan.o \ + fsm_soe.o \ mailbox.o \ master.o \ module.o \ @@ -61,6 +62,7 @@ sdo_request.o \ slave.o \ slave_config.o \ + soe_request.o \ sync.o \ sync_config.o \ voe_handler.o diff -r ef09f0ea0c4c -r 1875b9fea0ba master/Makefile.am --- a/master/Makefile.am Fri Feb 26 18:22:02 2010 +0100 +++ b/master/Makefile.am Mon Mar 01 18:33:42 2010 +0100 @@ -43,12 +43,13 @@ fsm_coe.c fsm_coe.h \ fsm_foe.c fsm_foe.h \ fsm_master.c fsm_master.h \ - fsm_slave.c fsm_slave.h \ fsm_pdo.c fsm_pdo.h \ fsm_pdo_entry.c fsm_pdo_entry.h \ fsm_sii.c fsm_sii.h \ + fsm_slave.c fsm_slave.h \ fsm_slave_config.c fsm_slave_config.h \ fsm_slave_scan.c fsm_slave_scan.h \ + fsm_soe.c fsm_soe.h \ globals.h \ ioctl.h \ mailbox.c mailbox.h \ @@ -62,6 +63,7 @@ sdo_request.c sdo_request.h \ slave.c slave.h \ slave_config.c slave_config.h \ + soe_request.c soe_request.h \ sync.c sync.h \ sync_config.c sync_config.h \ voe_handler.c voe_handler.h diff -r ef09f0ea0c4c -r 1875b9fea0ba master/cdev.c --- a/master/cdev.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/cdev.c Mon Mar 01 18:33:42 2010 +0100 @@ -284,10 +284,12 @@ data.ports[i].desc = slave->ports[i].desc; data.ports[i].link.link_up = slave->ports[i].link.link_up; data.ports[i].link.loop_closed = slave->ports[i].link.loop_closed; - data.ports[i].link.signal_detected = slave->ports[i].link.signal_detected; + data.ports[i].link.signal_detected = + slave->ports[i].link.signal_detected; data.ports[i].receive_time = slave->ports[i].receive_time; if (slave->ports[i].next_slave) { - data.ports[i].next_slave = slave->ports[i].next_slave->ring_position; + data.ports[i].next_slave = + slave->ports[i].next_slave->ring_position; } else { data.ports[i].next_slave = 0xffff; } @@ -825,7 +827,8 @@ } if (master->debug_level) - EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position); + EC_DBG("Schedule SDO upload request for slave %u\n", + request.slave->ring_position); // schedule request. list_add_tail(&request.list, &request.slave->slave_sdo_requests); @@ -847,10 +850,12 @@ } // wait until master FSM has finished processing - wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); + wait_event(request.slave->sdo_queue, + request.req.state != EC_INT_REQUEST_BUSY); if (master->debug_level) - EC_DBG("Scheduled SDO upload request for slave %u done\n",request.slave->ring_position); + EC_DBG("Scheduled SDO upload request for slave %u done\n", + request.slave->ring_position); data.abort_code = request.req.abort_code; @@ -931,7 +936,8 @@ } if (master->debug_level) - EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position); + EC_DBG("Schedule SDO download request for slave %u\n", + request.slave->ring_position); // schedule request. list_add_tail(&request.list, &request.slave->slave_sdo_requests); @@ -953,10 +959,12 @@ } // wait until master FSM has finished processing - wait_event(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY); + wait_event(request.slave->sdo_queue, + request.req.state != EC_INT_REQUEST_BUSY); if (master->debug_level) - EC_DBG("Scheduled SDO download request for slave %u done\n",request.slave->ring_position); + EC_DBG("Scheduled SDO download request for slave %u done\n", + request.slave->ring_position); data.abort_code = request.req.abort_code; @@ -1120,7 +1128,8 @@ return 0; if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for register data.\n", data.length); + EC_ERR("Failed to allocate %u bytes for register data.\n", + data.length); return -ENOMEM; } @@ -1197,7 +1206,8 @@ return 0; if (!(contents = kmalloc(data.length, GFP_KERNEL))) { - EC_ERR("Failed to allocate %u bytes for register data.\n", data.length); + EC_ERR("Failed to allocate %u bytes for register data.\n", + data.length); return -ENOMEM; } @@ -2240,7 +2250,8 @@ up(&master->master_sem); // FIXME if (data.complete_access) { - ret = ecrt_slave_config_complete_sdo(sc, data.index, sdo_data, data.size); + ret = ecrt_slave_config_complete_sdo(sc, + data.index, sdo_data, data.size); } else { ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data, data.size); @@ -3102,7 +3113,8 @@ } // wait until master FSM has finished processing - wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY); + wait_event(request.slave->foe_queue, + request.req.state != EC_INT_REQUEST_BUSY); data.result = request.req.result; data.error_code = request.req.error_code; @@ -3212,7 +3224,8 @@ } // wait until master FSM has finished processing - wait_event(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_BUSY); + wait_event(request.slave->foe_queue, + request.req.state != EC_INT_REQUEST_BUSY); data.result = request.req.result; data.error_code = request.req.error_code; @@ -3226,7 +3239,195 @@ ec_foe_request_clear(&request.req); if (master->debug_level) { - printk("Finished FoE writing.\n"); + EC_DBG("Finished FoE writing.\n"); + } + + return retval; +} + +/*****************************************************************************/ + +/** Read an SoE IDN. + */ +int ec_cdev_ioctl_slave_soe_read( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_soe_t data; + ec_master_soe_request_t request; + int retval; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + ec_soe_request_init(&request.req); + ec_soe_request_set_idn(&request.req, data.idn); + ec_soe_request_read(&request.req); + + if (down_interruptible(&master->master_sem)) + return -EINTR; + + if (!(request.slave = ec_master_find_slave( + master, 0, data.slave_position))) { + up(&master->master_sem); + ec_soe_request_clear(&request.req); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + return -EINVAL; + } + + // schedule request. + list_add_tail(&request.list, &request.slave->soe_requests); + + up(&master->master_sem); + + if (master->debug_level) { + EC_DBG("Scheduled SoE read request on slave %u.\n", + request.slave->ring_position); + } + + // wait for processing through FSM + if (wait_event_interruptible(request.slave->soe_queue, + request.req.state != EC_INT_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->master_sem); + if (request.req.state == EC_INT_REQUEST_QUEUED) { + list_del(&request.list); + up(&master->master_sem); + ec_soe_request_clear(&request.req); + return -EINTR; + } + // request already processing: interrupt not possible. + up(&master->master_sem); + } + + // wait until master FSM has finished processing + wait_event(request.slave->soe_queue, + request.req.state != EC_INT_REQUEST_BUSY); + + data.error_code = request.req.error_code; + + if (master->debug_level) { + EC_DBG("Read %zd bytes via SoE.\n", request.req.data_size); + } + + if (request.req.state != EC_INT_REQUEST_SUCCESS) { + data.data_size = 0; + retval = -EIO; + } else { + if (request.req.data_size > data.mem_size) { + EC_ERR("Buffer too small.\n"); + ec_soe_request_clear(&request.req); + return -EOVERFLOW; + } + data.data_size = request.req.data_size; + if (copy_to_user((void __user *) data.data, + request.req.data, data.data_size)) { + ec_soe_request_clear(&request.req); + return -EFAULT; + } + retval = 0; + } + + if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { + retval = -EFAULT; + } + + if (master->debug_level) + EC_DBG("SoE read request finished on slave %u.\n", + request.slave->ring_position); + + ec_soe_request_clear(&request.req); + + return retval; +} + +/*****************************************************************************/ + +/** Write an IDN to a slave via SoE. + */ +int ec_cdev_ioctl_slave_soe_write( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg /**< ioctl() argument. */ + ) +{ + ec_ioctl_slave_soe_t data; + ec_master_soe_request_t request; + int retval; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + return -EFAULT; + } + + INIT_LIST_HEAD(&request.list); + + ec_soe_request_init(&request.req); + ec_soe_request_set_idn(&request.req, data.idn); + + if (ec_soe_request_alloc(&request.req, data.mem_size)) { + ec_soe_request_clear(&request.req); + return -ENOMEM; + } + if (copy_from_user(request.req.data, + (void __user *) data.data, data.mem_size)) { + ec_soe_request_clear(&request.req); + return -EFAULT; + } + request.req.data_size = data.mem_size; + ec_soe_request_write(&request.req); + + if (down_interruptible(&master->master_sem)) + return -EINTR; + + if (!(request.slave = ec_master_find_slave( + master, 0, data.slave_position))) { + up(&master->master_sem); + EC_ERR("Slave %u does not exist!\n", data.slave_position); + ec_soe_request_clear(&request.req); + return -EINVAL; + } + + if (master->debug_level) { + EC_DBG("Scheduling SoE write request.\n"); + } + + // schedule SoE write request. + list_add_tail(&request.list, &request.slave->soe_requests); + + up(&master->master_sem); + + // wait for processing through FSM + if (wait_event_interruptible(request.slave->soe_queue, + request.req.state != EC_INT_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->master_sem); + if (request.req.state == EC_INT_REQUEST_QUEUED) { + // abort request + list_del(&request.list); + up(&master->master_sem); + ec_soe_request_clear(&request.req); + return -EINTR; + } + up(&master->master_sem); + } + + // wait until master FSM has finished processing + wait_event(request.slave->soe_queue, + request.req.state != EC_INT_REQUEST_BUSY); + + //data.result = request.req.result; + + retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; + + if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { + retval = -EFAULT; + } + + ec_soe_request_clear(&request.req); + + if (master->debug_level) { + EC_DBG("Finished SoE writing.\n"); } return retval; @@ -3354,6 +3555,12 @@ if (!(filp->f_mode & FMODE_WRITE)) return -EPERM; return ec_cdev_ioctl_slave_foe_write(master, arg); + case EC_IOCTL_SLAVE_SOE_READ: + return ec_cdev_ioctl_slave_soe_read(master, arg); + case EC_IOCTL_SLAVE_SOE_WRITE: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_slave_soe_write(master, arg); case EC_IOCTL_CONFIG: return ec_cdev_ioctl_config(master, arg); case EC_IOCTL_CONFIG_PDO: diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_coe.c --- a/master/fsm_coe.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/fsm_coe.c Mon Mar 01 18:33:42 2010 +0100 @@ -1964,7 +1964,7 @@ if (rec_size < 6 + fsm->complete_size) { fsm->state = ec_fsm_coe_error; - EC_ERR("Received currupted SDO expedited upload" + EC_ERR("Received corrupted SDO expedited upload" " response (only %zu bytes)!\n", rec_size); ec_print_data(data, rec_size); return; diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_master.c --- a/master/fsm_master.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/fsm_master.c Mon Mar 01 18:33:42 2010 +0100 @@ -426,8 +426,8 @@ if (ec_sdo_request_timed_out(req)) { req->state = EC_INT_REQUEST_FAILURE; if (master->debug_level) - EC_DBG("Internal SDO request for slave %u timed out...\n", - slave->ring_position); + EC_DBG("Internal SDO request for slave %u" + " timed out...\n", slave->ring_position); continue; } @@ -472,7 +472,7 @@ if (ec_fsm_master_action_process_sdo(fsm)) return; - // enable processing of SDO/FOE requests + // enable processing of requests for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) { diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_master.h --- a/master/fsm_master.h Fri Feb 26 18:22:02 2010 +0100 +++ b/master/fsm_master.h Mon Mar 01 18:33:42 2010 +0100 @@ -41,6 +41,7 @@ #include "datagram.h" #include "foe_request.h" #include "sdo_request.h" +#include "soe_request.h" #include "fsm_slave_config.h" #include "fsm_slave_scan.h" #include "fsm_pdo.h" @@ -85,7 +86,7 @@ /*****************************************************************************/ -/** FoE write request. +/** FoE request. */ typedef struct { struct list_head list; /**< List head. */ @@ -95,6 +96,16 @@ /*****************************************************************************/ +/** SoE request. + */ +typedef struct { + struct list_head list; /**< List head. */ + ec_slave_t *slave; /**< EtherCAT slave. */ + ec_soe_request_t req; /**< SoE request. */ +} ec_master_soe_request_t; + +/*****************************************************************************/ + typedef struct ec_fsm_master ec_fsm_master_t; /**< \see ec_fsm_master */ /** Finite state machine of an EtherCAT master. diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_slave.c --- a/master/fsm_slave.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/fsm_slave.c Mon Mar 01 18:33:42 2010 +0100 @@ -44,10 +44,11 @@ void ec_fsm_slave_state_idle(ec_fsm_slave_t *); void ec_fsm_slave_state_ready(ec_fsm_slave_t *); int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); +void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); -void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); - +int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *); +void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *); /*****************************************************************************/ @@ -71,6 +72,7 @@ // init sub-state-machines ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); + ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram); } /*****************************************************************************/ @@ -84,6 +86,7 @@ // clear sub-state machines ec_fsm_coe_clear(&fsm->fsm_coe); ec_fsm_foe_clear(&fsm->fsm_foe); + ec_fsm_soe_clear(&fsm->fsm_soe); } /*****************************************************************************/ @@ -107,11 +110,9 @@ return; } - /*****************************************************************************/ /** Sets the current state of the state machine to READY - * */ void ec_fsm_slave_ready( ec_fsm_slave_t *fsm /**< Slave state machine. */ @@ -119,23 +120,18 @@ { if (fsm->state == ec_fsm_slave_state_idle) { if (fsm->slave->master->debug_level) { - EC_DBG("Slave %u ready for SDO/FOE.\n", + EC_DBG("Slave %u ready for requests.\n", fsm->slave->ring_position); } fsm->state = ec_fsm_slave_state_ready; } - return; } /****************************************************************************** * Slave state machine *****************************************************************************/ -/*****************************************************************************/ - /** Slave state: IDLE. - * - * */ void ec_fsm_slave_state_idle( ec_fsm_slave_t *fsm /**< Slave state machine. */ @@ -148,8 +144,6 @@ /*****************************************************************************/ /** Slave state: READY. - * - * */ void ec_fsm_slave_state_ready( ec_fsm_slave_t *fsm /**< Slave state machine. */ @@ -159,9 +153,13 @@ if (ec_fsm_slave_action_process_sdo(fsm)) return; - // Check for pending FOE requests + // Check for pending FoE requests if (ec_fsm_slave_action_process_foe(fsm)) return; + + // Check for pending SoE requests + if (ec_fsm_slave_action_process_soe(fsm)) + return; } /*****************************************************************************/ @@ -220,51 +218,6 @@ return 0; } - -/*****************************************************************************/ - -/** Check for pending FOE requests and process one. - * - * \return non-zero, if an FOE request is processed. - */ -int ec_fsm_slave_action_process_foe( - ec_fsm_slave_t *fsm /**< Slave state machine. */ - ) -{ - ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - ec_master_foe_request_t *request, *next; - - // search the first request to be processed - list_for_each_entry_safe(request, next, &slave->foe_requests, list) { - if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { - EC_WARN("Aborting FOE request, slave %u has ERROR.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&slave->sdo_queue); - fsm->sdo_request = NULL; - fsm->state = ec_fsm_slave_state_idle; - return 0; - } - list_del_init(&request->list); // dequeue - request->req.state = EC_INT_REQUEST_BUSY; - - if (master->debug_level) - EC_DBG("Processing FOE request for slave %u.\n", - slave->ring_position); - - fsm->foe_request = &request->req; - fsm->state = ec_fsm_slave_state_foe_request; - ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); - ec_fsm_foe_exec(&fsm->fsm_foe); - ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); - return 1; - } - return 0; -} - - - /*****************************************************************************/ /** Slave state: SDO_REQUEST. @@ -306,6 +259,48 @@ /*****************************************************************************/ +/** Check for pending FOE requests and process one. + * + * \return non-zero, if an FOE request is processed. + */ +int ec_fsm_slave_action_process_foe( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_master_foe_request_t *request, *next; + + // search the first request to be processed + list_for_each_entry_safe(request, next, &slave->foe_requests, list) { + if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { + EC_WARN("Aborting FOE request, slave %u has ERROR.\n", + slave->ring_position); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->sdo_queue); + fsm->sdo_request = NULL; + fsm->state = ec_fsm_slave_state_idle; + return 0; + } + list_del_init(&request->list); // dequeue + request->req.state = EC_INT_REQUEST_BUSY; + + if (master->debug_level) + EC_DBG("Processing FOE request for slave %u.\n", + slave->ring_position); + + fsm->foe_request = &request->req; + fsm->state = ec_fsm_slave_state_foe_request; + ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); + ec_fsm_foe_exec(&fsm->fsm_foe); + ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); + return 1; + } + return 0; +} + +/*****************************************************************************/ + /** Slave state: FOE REQUEST. */ void ec_fsm_slave_state_foe_request( @@ -345,3 +340,96 @@ } /*****************************************************************************/ + +/** Check for pending SoE requests and process one. + * + * \return non-zero, if a request is processed. + */ +int ec_fsm_slave_action_process_soe( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_master_soe_request_t *req, *next; + + // search the first request to be processed + list_for_each_entry_safe(req, next, &slave->soe_requests, list) { + + list_del_init(&req->list); // dequeue + if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { + EC_WARN("Aborting SoE request, slave %u has ERROR.\n", + slave->ring_position); + req->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->soe_queue); + fsm->state = ec_fsm_slave_state_idle; + return 0; + } + + if (slave->current_state == EC_SLAVE_STATE_INIT) { + EC_WARN("Aborting SoE request, slave %u is in INIT.\n", + slave->ring_position); + req->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->soe_queue); + fsm->state = ec_fsm_slave_state_idle; + return 0; + } + + req->req.state = EC_INT_REQUEST_BUSY; + + // Found pending request. Execute it! + if (master->debug_level) + EC_DBG("Processing SoE request for slave %u...\n", + slave->ring_position); + + // Start SoE transfer + fsm->soe_request = &req->req; + fsm->state = ec_fsm_slave_state_soe_request; + ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req); + ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately + ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); + return 1; + } + return 0; +} + +/*****************************************************************************/ + +/** Slave state: SOE_REQUEST. + */ +void ec_fsm_slave_state_soe_request( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_soe_request_t *request = fsm->soe_request; + + if (ec_fsm_soe_exec(&fsm->fsm_soe)) { + ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); + return; + } + + if (!ec_fsm_soe_success(&fsm->fsm_soe)) { + EC_DBG("Failed to process SoE request for slave %u.\n", + fsm->slave->ring_position); + request->state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->soe_queue); + fsm->soe_request = NULL; + fsm->state = ec_fsm_slave_state_idle; + return; + } + + if (master->debug_level) + EC_DBG("Finished SoE request for slave %u.\n", + fsm->slave->ring_position); + + // SoE request finished + request->state = EC_INT_REQUEST_SUCCESS; + wake_up(&slave->soe_queue); + + fsm->soe_request = NULL; + fsm->state = ec_fsm_slave_state_ready; +} + +/*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_slave.h --- a/master/fsm_slave.h Fri Feb 26 18:22:02 2010 +0100 +++ b/master/fsm_slave.h Mon Mar 01 18:33:42 2010 +0100 @@ -41,6 +41,7 @@ #include "sdo_request.h" #include "fsm_coe.h" #include "fsm_foe.h" +#include "fsm_soe.h" typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */ @@ -54,9 +55,11 @@ ec_sdo_request_t *sdo_request; /**< SDO request to process. */ ec_foe_request_t *foe_request; /**< FoE request to process. */ off_t foe_index; /**< index to FoE write request data */ + ec_soe_request_t *soe_request; /**< SoE request to process. */ ec_fsm_coe_t fsm_coe; /**< CoE state machine */ ec_fsm_foe_t fsm_foe; /**< FoE state machine */ + ec_fsm_soe_t fsm_soe; /**< SoE state machine */ }; /*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_soe.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_soe.c Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,389 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** + \file + EtherCAT SoE state machines. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" +#include "fsm_soe.h" + +/*****************************************************************************/ + +/** Mailbox type for SoE. + */ +#define EC_MBOX_TYPE_SOE 0x05 + +#define EC_SOE_OPCODE_READ_REQUEST 0x01 +#define EC_SOE_OPCODE_READ_RESPONSE 0x02 + +#define EC_SOE_READ_REQUEST_SIZE 0x04 +#define EC_SOE_READ_RESPONSE_SIZE 0x04 + +#define EC_SOE_RESPONSE_TIMEOUT 1000 + +/*****************************************************************************/ + +void ec_fsm_soe_read_start(ec_fsm_soe_t *); +void ec_fsm_soe_read_request(ec_fsm_soe_t *); +void ec_fsm_soe_read_check(ec_fsm_soe_t *); +void ec_fsm_soe_read_response(ec_fsm_soe_t *); + +void ec_fsm_soe_end(ec_fsm_soe_t *); +void ec_fsm_soe_error(ec_fsm_soe_t *); + +/*****************************************************************************/ + +/** Constructor. + */ +void ec_fsm_soe_init( + ec_fsm_soe_t *fsm, /**< finite state machine */ + ec_datagram_t *datagram /**< datagram */ + ) +{ + fsm->state = NULL; + fsm->datagram = datagram; +} + +/*****************************************************************************/ + +/** Destructor. + */ +void ec_fsm_soe_clear( + ec_fsm_soe_t *fsm /**< finite state machine */ + ) +{ +} + +/*****************************************************************************/ + +/** Starts to transfer an IDN to/from a slave. + */ +void ec_fsm_soe_transfer( + ec_fsm_soe_t *fsm, /**< State machine. */ + ec_slave_t *slave, /**< EtherCAT slave. */ + ec_soe_request_t *request /**< SoE request. */ + ) +{ + fsm->slave = slave; + fsm->request = request; + if (request->dir == EC_DIR_OUTPUT) { + //fsm->state = ec_fsm_soe_write_start; + } else { + fsm->state = ec_fsm_soe_read_start; + } +} + +/*****************************************************************************/ + +/** + Executes the current state of the state machine. + \return false, if state machine has terminated +*/ + +int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */) +{ + fsm->state(fsm); + + return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; +} + +/*****************************************************************************/ + +/** + Returns, if the state machine terminated with success. + \return non-zero if successful. +*/ + +int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */) +{ + return fsm->state == ec_fsm_soe_end; +} + +/****************************************************************************** + * SoE read state machine + *****************************************************************************/ + +/** SoE state: READ START. + */ +void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_soe_request_t *request = fsm->request; + uint8_t *data; + + if (master->debug_level) + EC_DBG("Reading IDN 0x%04X from slave %u.\n", + request->idn, slave->ring_position); + + if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { + EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); + fsm->state = ec_fsm_soe_error; + return; + } + + data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, + EC_SOE_READ_REQUEST_SIZE); + if (IS_ERR(data)) { + fsm->state = ec_fsm_soe_error; + return; + } + + EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST); + EC_WRITE_U8(data + 1, 1 << 6); // request value + EC_WRITE_U16(data + 2, request->idn); + + if (master->debug_level) { + EC_DBG("SCC read request:\n"); + ec_print_data(data, EC_SOE_READ_REQUEST_SIZE); + } + + fsm->request->jiffies_sent = jiffies; + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_soe_read_request; +} + +/*****************************************************************************/ + +/** SoE state: READ REQUEST. + */ +void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + unsigned long diff_ms; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; // FIXME: check for response first? + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Failed to receive SoE read request for slave %u: ", + slave->ring_position); + ec_datagram_print_state(datagram); + return; + } + + diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; + + if (datagram->working_counter != 1) { + if (!datagram->working_counter) { + if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { + // no response; send request datagram again + return; + } + } + fsm->state = ec_fsm_soe_error; + EC_ERR("Reception of SoE read request for IDN 0x%04x failed" + " after %u ms on slave %u: ", + fsm->request->idn, (u32) diff_ms, + fsm->slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } + + fsm->jiffies_start = datagram->jiffies_sent; + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_soe_read_check; +} + +/*****************************************************************************/ + +/** CoE state: READ CHECK. + */ +void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ", + slave->ring_position); + ec_datagram_print_state(datagram); + return; + } + + if (datagram->working_counter != 1) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ", + slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } + + if (!ec_slave_mbox_check(datagram)) { + unsigned long diff_ms = + (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; + if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" + " read response on slave %u.\n", (u32) diff_ms, + fsm->request->idn, slave->ring_position); + return; + } + + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + return; + } + + // Fetch response + ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_soe_read_response; +} + +/*****************************************************************************/ + +/** SoE state: READ RESPONSE. + */ +void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot, opcode, error_flag, value_included; + size_t rec_size, data_size; + ec_soe_request_t *req = fsm->request; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; // FIXME: request again? + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Failed to receive SoE read response datagram for" + " slave %u: ", slave->ring_position); + ec_datagram_print_state(datagram); + return; + } + + if (datagram->working_counter != 1) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Reception of SoE read response failed on slave %u: ", + slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } + + data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); + if (IS_ERR(data)) { + fsm->state = ec_fsm_soe_error; + return; + } + + if (master->debug_level) { + EC_DBG("SCC read response:\n"); + ec_print_data(data, rec_size); + } + + if (mbox_prot != EC_MBOX_TYPE_SOE) { + fsm->state = ec_fsm_soe_error; + EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); + return; + } + + if (rec_size < EC_SOE_READ_RESPONSE_SIZE) { + fsm->state = ec_fsm_soe_error; + EC_ERR("Received currupted SoE read response (%zu bytes)!\n", + rec_size); + ec_print_data(data, rec_size); + return; + } + + opcode = EC_READ_U8(data) & 0x3; + if (opcode != EC_SOE_OPCODE_READ_RESPONSE) { + EC_ERR("Received no read response (opcode %x).\n", opcode); + ec_print_data(data, rec_size); + fsm->state = ec_fsm_soe_error; + return; + } + + error_flag = (EC_READ_U8(data) >> 4) & 1; + if (error_flag) { + req->error_code = EC_READ_U16(data + rec_size - 2); + EC_ERR("Received error response: 0x%04x.\n", + req->error_code); + fsm->state = ec_fsm_soe_error; + return; + } else { + req->error_code = 0x0000; + } + + value_included = (EC_READ_U8(data + 1) >> 6) & 1; + if (!value_included) { + EC_ERR("No value included!\n"); + fsm->state = ec_fsm_soe_error; + return; + } + + data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE; + if (ec_soe_request_copy_data(req, + data + EC_SOE_READ_RESPONSE_SIZE, data_size)) { + fsm->state = ec_fsm_soe_error; + return; + } + + if (master->debug_level) { + EC_DBG("IDN data:\n"); + ec_print_data(req->data, req->data_size); + } + + fsm->state = ec_fsm_soe_end; // success +} + +/*****************************************************************************/ + +/** State: ERROR. + */ +void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ + +/** State: END. + */ +void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */) +{ +} + +/*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba master/fsm_soe.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_soe.h Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,73 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** + \file + EtherCAT CoE state machines. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_SOE_H__ +#define __EC_FSM_SOE_H__ + +#include "globals.h" +#include "datagram.h" +#include "slave.h" +#include "soe_request.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_soe ec_fsm_soe_t; /**< \see ec_fsm_soe */ + +/** Finite state machines for the Sercos over EtherCAT protocol. + */ +struct ec_fsm_soe { + ec_slave_t *slave; /**< slave the FSM runs on */ + ec_datagram_t *datagram; /**< datagram used in the state machine */ + unsigned int retries; /**< retries upon datagram timeout */ + + void (*state)(ec_fsm_soe_t *); /**< CoE state function */ + unsigned long jiffies_start; /**< CoE timestamp. */ + ec_soe_request_t *request; /**< SoE request */ +}; + +/*****************************************************************************/ + +void ec_fsm_soe_init(ec_fsm_soe_t *, ec_datagram_t *); +void ec_fsm_soe_clear(ec_fsm_soe_t *); + +void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *); + +int ec_fsm_soe_exec(ec_fsm_soe_t *); +int ec_fsm_soe_success(ec_fsm_soe_t *); + +/*****************************************************************************/ + +#endif diff -r ef09f0ea0c4c -r 1875b9fea0ba master/ioctl.h --- a/master/ioctl.h Fri Feb 26 18:22:02 2010 +0100 +++ b/master/ioctl.h Mon Mar 01 18:33:42 2010 +0100 @@ -56,7 +56,7 @@ * * Increment this when changing the ioctl interface! */ -#define EC_IOCTL_VERSION_MAGIC 1 +#define EC_IOCTL_VERSION_MAGIC 2 // Command-line tool #define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t) @@ -80,57 +80,59 @@ #define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x12, ec_ioctl_slave_reg_t) #define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x13, ec_ioctl_slave_foe_t) #define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x14, ec_ioctl_slave_foe_t) -#define EC_IOCTL_CONFIG EC_IOWR(0x15, ec_ioctl_config_t) -#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x16, ec_ioctl_config_pdo_t) -#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x17, ec_ioctl_config_pdo_entry_t) -#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x18, ec_ioctl_config_sdo_t) +#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x15, ec_ioctl_slave_soe_t) +#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x16, ec_ioctl_slave_soe_t) +#define EC_IOCTL_CONFIG EC_IOWR(0x17, ec_ioctl_config_t) +#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x18, ec_ioctl_config_pdo_t) +#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x19, ec_ioctl_config_pdo_entry_t) +#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x1a, ec_ioctl_config_sdo_t) #ifdef EC_EOE -#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x19, ec_ioctl_eoe_handler_t) +#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x1b, ec_ioctl_eoe_handler_t) #endif // Application interface -#define EC_IOCTL_REQUEST EC_IO(0x1a) -#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x1b) -#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x1c, ec_ioctl_config_t) -#define EC_IOCTL_ACTIVATE EC_IOR(0x1d, size_t) -#define EC_IOCTL_DEACTIVATE EC_IO(0x1e) -#define EC_IOCTL_SEND EC_IO(0x1f) -#define EC_IOCTL_RECEIVE EC_IO(0x20) -#define EC_IOCTL_MASTER_STATE EC_IOR(0x21, ec_master_state_t) -#define EC_IOCTL_APP_TIME EC_IOW(0x22, ec_ioctl_app_time_t) -#define EC_IOCTL_SYNC_REF EC_IO(0x23) -#define EC_IOCTL_SYNC_SLAVES EC_IO(0x24) -#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x25) -#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x26, uint32_t) -#define EC_IOCTL_SC_SYNC EC_IOW(0x27, ec_ioctl_config_t) -#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x28, ec_ioctl_config_t) -#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x29, ec_ioctl_config_pdo_t) -#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x2a, ec_ioctl_config_pdo_t) -#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x2b, ec_ioctl_add_pdo_entry_t) -#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x2c, ec_ioctl_config_pdo_t) -#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x2d, ec_ioctl_reg_pdo_entry_t) -#define EC_IOCTL_SC_DC EC_IOW(0x2e, ec_ioctl_config_t) -#define EC_IOCTL_SC_SDO EC_IOW(0x2f, ec_ioctl_sc_sdo_t) -#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x20, ec_ioctl_sdo_request_t) -#define EC_IOCTL_SC_VOE EC_IOWR(0x31, ec_ioctl_voe_t) -#define EC_IOCTL_SC_STATE EC_IOWR(0x32, ec_ioctl_sc_state_t) -#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x33) -#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x34) -#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x35) -#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x36, ec_ioctl_domain_state_t) -#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x37, ec_ioctl_sdo_request_t) -#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x38, ec_ioctl_sdo_request_t) -#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x39, ec_ioctl_sdo_request_t) -#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x3a, ec_ioctl_sdo_request_t) -#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x3b, ec_ioctl_sdo_request_t) -#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x3c, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x3d, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_READ EC_IOW(0x3e, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x3f, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_WRITE EC_IOWR(0x40, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_EXEC EC_IOWR(0x41, ec_ioctl_voe_t) -#define EC_IOCTL_VOE_DATA EC_IOWR(0x42, ec_ioctl_voe_t) -#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x43, size_t) +#define EC_IOCTL_REQUEST EC_IO(0x1c) +#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x1d) +#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x1e, ec_ioctl_config_t) +#define EC_IOCTL_ACTIVATE EC_IOR(0x1f, size_t) +#define EC_IOCTL_DEACTIVATE EC_IO(0x20) +#define EC_IOCTL_SEND EC_IO(0x21) +#define EC_IOCTL_RECEIVE EC_IO(0x22) +#define EC_IOCTL_MASTER_STATE EC_IOR(0x23, ec_master_state_t) +#define EC_IOCTL_APP_TIME EC_IOW(0x24, ec_ioctl_app_time_t) +#define EC_IOCTL_SYNC_REF EC_IO(0x25) +#define EC_IOCTL_SYNC_SLAVES EC_IO(0x26) +#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x27) +#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x28, uint32_t) +#define EC_IOCTL_SC_SYNC EC_IOW(0x29, ec_ioctl_config_t) +#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x2a, ec_ioctl_config_t) +#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x2b, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x2c, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x2d, ec_ioctl_add_pdo_entry_t) +#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x2e, ec_ioctl_config_pdo_t) +#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x2f, ec_ioctl_reg_pdo_entry_t) +#define EC_IOCTL_SC_DC EC_IOW(0x20, ec_ioctl_config_t) +#define EC_IOCTL_SC_SDO EC_IOW(0x31, ec_ioctl_sc_sdo_t) +#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x32, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SC_VOE EC_IOWR(0x33, ec_ioctl_voe_t) +#define EC_IOCTL_SC_STATE EC_IOWR(0x34, ec_ioctl_sc_state_t) +#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x35) +#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x36) +#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x37) +#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x38, ec_ioctl_domain_state_t) +#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x39, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x3a, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x3b, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x3c, ec_ioctl_sdo_request_t) +#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x3d, ec_ioctl_sdo_request_t) +#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x3e, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x3f, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_READ EC_IOW(0x40, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x41, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_WRITE EC_IOWR(0x42, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_EXEC EC_IOWR(0x43, ec_ioctl_voe_t) +#define EC_IOCTL_VOE_DATA EC_IOWR(0x44, ec_ioctl_voe_t) +#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x45, size_t) /*****************************************************************************/ @@ -404,6 +406,21 @@ typedef struct { // inputs + uint16_t slave_position; + uint16_t idn; + uint32_t mem_size; + uint8_t *data; + + // outputs + uint32_t data_size; + uint32_t result; + uint16_t error_code; +} ec_ioctl_slave_soe_t; + +/*****************************************************************************/ + +typedef struct { + // inputs uint32_t config_index; // outputs diff -r ef09f0ea0c4c -r 1875b9fea0ba master/master.c --- a/master/master.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/master.c Mon Mar 01 18:33:42 2010 +0100 @@ -417,7 +417,8 @@ request = list_entry(slave->slave_sdo_requests.next, ec_master_sdo_request_t, list); list_del_init(&request->list); // dequeue - EC_INFO("Discarding SDO request, slave %u does not exist anymore.\n", + EC_INFO("Discarding SDO request," + " slave %u does not exist anymore.\n", slave->ring_position); request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->sdo_queue); @@ -431,11 +432,27 @@ request = list_entry(slave->foe_requests.next, ec_master_foe_request_t, list); list_del_init(&request->list); // dequeue - EC_INFO("Discarding FOE request, slave %u does not exist anymore.\n", + EC_INFO("Discarding FoE request," + " slave %u does not exist anymore.\n", slave->ring_position); request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&slave->foe_queue); } + // SoE requests + while (1) { + ec_master_soe_request_t *request; + if (list_empty(&slave->soe_requests)) + break; + // get first request + request = list_entry(slave->soe_requests.next, + ec_master_soe_request_t, list); + list_del_init(&request->list); // dequeue + EC_INFO("Discarding SoE request," + " slave %u does not exist anymore.\n", + slave->ring_position); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->soe_queue); + } ec_slave_clear(slave); } diff -r ef09f0ea0c4c -r 1875b9fea0ba master/sdo_request.c --- a/master/sdo_request.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/sdo_request.c Mon Mar 01 18:33:42 2010 +0100 @@ -191,7 +191,7 @@ } /***************************************************************************** - * Realtime interface. + * Application interface. ****************************************************************************/ void ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout) diff -r ef09f0ea0c4c -r 1875b9fea0ba master/slave.c --- a/master/slave.c Fri Feb 26 18:22:02 2010 +0100 +++ b/master/slave.c Mon Mar 01 18:33:42 2010 +0100 @@ -155,6 +155,9 @@ INIT_LIST_HEAD(&slave->foe_requests); init_waitqueue_head(&slave->foe_queue); + INIT_LIST_HEAD(&slave->soe_requests); + init_waitqueue_head(&slave->soe_queue); + // init state machine datagram ec_datagram_init(&slave->fsm_datagram); snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "slave%u-fsm",slave->ring_position); diff -r ef09f0ea0c4c -r 1875b9fea0ba master/slave.h --- a/master/slave.h Fri Feb 26 18:22:02 2010 +0100 +++ b/master/slave.h Mon Mar 01 18:33:42 2010 +0100 @@ -163,8 +163,11 @@ wait_queue_head_t sdo_queue; /**< Wait queue for SDO access requests from user space. */ struct list_head foe_requests; /**< FoE write requests. */ - wait_queue_head_t foe_queue; /**< Wait queue for FoE - write requests from user space. */ + wait_queue_head_t foe_queue; /**< Wait queue for FoE requests from user + space. */ + struct list_head soe_requests; /**< FoE write requests. */ + wait_queue_head_t soe_queue; /**< Wait queue for SoE requests from user + space. */ ec_fsm_slave_t fsm; /**< Slave state machine. */ ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */ }; diff -r ef09f0ea0c4c -r 1875b9fea0ba master/soe_request.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/soe_request.c Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,198 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** \file + * Sercos-over-EtherCAT request functions. + */ + +/*****************************************************************************/ + +#include +#include + +#include "soe_request.h" + +/*****************************************************************************/ + +/** Default timeout in ms to wait for SoE responses. + */ +#define EC_SOE_REQUEST_RESPONSE_TIMEOUT 1000 + +/*****************************************************************************/ + +void ec_soe_request_clear_data(ec_soe_request_t *); + +/*****************************************************************************/ + +/** SoE request constructor. + */ +void ec_soe_request_init( + ec_soe_request_t *req /**< SoE request. */ + ) +{ + req->data = NULL; + req->mem_size = 0; + req->data_size = 0; + req->dir = EC_DIR_INVALID; + req->state = EC_INT_REQUEST_INIT; + //req->jiffies_start = 0U; + req->jiffies_sent = 0U; + req->error_code = 0x0000; +} + +/*****************************************************************************/ + +/** SoE request destructor. + */ +void ec_soe_request_clear( + ec_soe_request_t *req /**< SoE request. */ + ) +{ + ec_soe_request_clear_data(req); +} + +/*****************************************************************************/ + +/** Set IDN. + */ +void ec_soe_request_set_idn( + ec_soe_request_t *req, /**< SoE request. */ + uint16_t idn /** IDN. */ + ) +{ + req->idn = idn; +} + +#if 0 +/*****************************************************************************/ + +/** Copy another SoE request. + * + * \attention Only the index subindex and data are copied. + */ +int ec_soe_request_copy( + ec_soe_request_t *req, /**< SoE request. */ + const ec_soe_request_t *other /**< Other SoE request to copy from. */ + ) +{ + req->complete_access = other->complete_access; + req->index = other->index; + req->subindex = other->subindex; + return ec_soe_request_copy_data(req, other->data, other->data_size); +} +#endif + +/*****************************************************************************/ + +/** Free allocated memory. + */ +void ec_soe_request_clear_data( + ec_soe_request_t *req /**< SoE request. */ + ) +{ + if (req->data) { + kfree(req->data); + req->data = NULL; + } + + req->mem_size = 0; + req->data_size = 0; +} + +/*****************************************************************************/ + +/** Pre-allocates the data memory. + * + * If the \a mem_size is already bigger than \a size, nothing is done. + * + * \return 0 on success, otherwise -ENOMEM. + */ +int ec_soe_request_alloc( + ec_soe_request_t *req, /**< SoE request. */ + size_t size /**< Data size to allocate. */ + ) +{ + if (size <= req->mem_size) + return 0; + + ec_soe_request_clear_data(req); + + if (!(req->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %zu bytes of SoE memory.\n", size); + return -ENOMEM; + } + + req->mem_size = size; + req->data_size = 0; + return 0; +} + +/*****************************************************************************/ + +/** Copies SoE data from an external source. + * + * If the \a mem_size is to small, new memory is allocated. + * + * \retval 0 Success. + * \retval <0 Error code. + */ +int ec_soe_request_copy_data( + ec_soe_request_t *req, /**< SoE request. */ + const uint8_t *source, /**< Source data. */ + size_t size /**< Number of bytes in \a source. */ + ) +{ + int ret = ec_soe_request_alloc(req, size); + if (ret < 0) + return ret; + + memcpy(req->data, source, size); + req->data_size = size; + return 0; +} + +/*****************************************************************************/ + +void ec_soe_request_read(ec_soe_request_t *req) +{ + req->dir = EC_DIR_INPUT; + req->state = EC_INT_REQUEST_QUEUED; + req->error_code = 0x0000; +} + +/*****************************************************************************/ + +void ec_soe_request_write(ec_soe_request_t *req) +{ + req->dir = EC_DIR_OUTPUT; + req->state = EC_INT_REQUEST_QUEUED; + req->error_code = 0x0000; +} + +/*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba master/soe_request.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/soe_request.h Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,80 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** + \file + EtherCAT SoE request structure. +*/ + +/*****************************************************************************/ + +#ifndef __EC_SOE_REQUEST_H__ +#define __EC_SOE_REQUEST_H__ + +#include + +#include "globals.h" + +/*****************************************************************************/ + +/** Sercos-over-EtherCAT request. + */ +typedef struct { + struct list_head list; /**< List item. */ + uint16_t idn; /**< Sercos ID-Number. */ + uint8_t *data; /**< Pointer to SDO data. */ + size_t mem_size; /**< Size of SDO data memory. */ + size_t data_size; /**< Size of SDO data. */ + ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to the + slave, EC_DIR_INPUT means reading from the slave. */ + ec_internal_request_state_t state; /**< Request state. */ + unsigned long jiffies_sent; /**< Jiffies, when the upload/download + request was sent. */ + uint16_t error_code; /**< SoE error code. */ +} ec_soe_request_t; + +/*****************************************************************************/ + +void ec_soe_request_init(ec_soe_request_t *); +void ec_soe_request_clear(ec_soe_request_t *); + +void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t); +int ec_soe_request_alloc(ec_soe_request_t *, size_t); +int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t); +void ec_soe_request_read(ec_soe_request_t *); +void ec_soe_request_write(ec_soe_request_t *); + +#if 0 +int ec_soe_request_copy(ec_soe_request_t *, const ec_soe_request_t *); +int ec_soe_request_timed_out(const ec_soe_request_t *); +#endif + +/*****************************************************************************/ + +#endif diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/CommandSoeRead.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tool/CommandSoeRead.cpp Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,142 @@ +/***************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + ****************************************************************************/ + +#include +#include +using namespace std; + +#include "CommandSoeRead.h" +#include "MasterDevice.h" + +/*****************************************************************************/ + +CommandSoeRead::CommandSoeRead(): + Command("soe_read", "Read an SoE IDN from a slave.") +{ +} + +/*****************************************************************************/ + +string CommandSoeRead::helpString() const +{ + stringstream str; + + str << getName() << " [OPTIONS] " << endl + << endl + << getBriefDescription() << endl + << endl + << "This command requires a single slave to be selected." << endl + << endl + << "Arguments:" << endl + << " IDN is the IDN and must be an unsigned" << endl + << " 16 bit number." << endl + << endl + << "Command-specific options:" << endl + << " --alias -a " << endl + << " --position -p Slave selection. See the help of" << endl + << " the 'slaves' command." << endl + << endl + << numericInfo(); + + return str.str(); +} + +/****************************************************************************/ + +void CommandSoeRead::execute(const StringVector &args) +{ + SlaveList slaves; + stringstream err, strIdn; + ec_ioctl_slave_soe_t data; + + if (args.size() != 1) { + err << "'" << getName() << "' takes one argument!"; + throwInvalidUsageException(err); + } + + strIdn << args[0]; + strIdn + >> resetiosflags(ios::basefield) // guess base from prefix + >> data.idn; + if (strIdn.fail()) { + err << "Invalid IDN '" << args[0] << "'!"; + throwInvalidUsageException(err); + } + + if (getMasterIndices().size() != 1) { + err << getName() << " requires to select a single master!"; + throwInvalidUsageException(err); + } + MasterDevice m(getMasterIndices().front()); + m.open(MasterDevice::Read); + slaves = selectedSlaves(m); + if (slaves.size() != 1) { + throwSingleSlaveRequired(slaves.size()); + } + data.slave_position = slaves.front().position; + + data.mem_size = 1024; + data.data = new uint8_t[data.mem_size + 1]; + + try { + m.readSoe(&data); + } catch (MasterDeviceSoeException &e) { + delete [] data.data; + err << "CoE read command aborted with code 0x" + << setfill('0') << hex << setw(4) << e.errorCode; + throwCommandException(err); + } catch (MasterDeviceException &e) { + delete [] data.data; + throw e; + } + + m.close(); + + printRawData(data.data, data.data_size); + + delete [] data.data; +} + +/****************************************************************************/ + +void CommandSoeRead::printRawData( + const uint8_t *data, + unsigned int size + ) +{ + cout << hex << setfill('0'); + while (size--) { + cout << "0x" << setw(2) << (unsigned int) *data++; + if (size) + cout << " "; + } + cout << endl; +} + +/*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/CommandSoeRead.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tool/CommandSoeRead.h Mon Mar 01 18:33:42 2010 +0100 @@ -0,0 +1,52 @@ +/***************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + ****************************************************************************/ + +#ifndef __COMMANDSOEREAD_H__ +#define __COMMANDSOEREAD_H__ + +#include "Command.h" + +/****************************************************************************/ + +class CommandSoeRead: + public Command +{ + public: + CommandSoeRead(); + + string helpString() const; + void execute(const StringVector &); + + protected: + static void printRawData(const uint8_t *, unsigned int); +}; + +/****************************************************************************/ + +#endif diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/Makefile.am --- a/tool/Makefile.am Fri Feb 26 18:22:02 2010 +0100 +++ b/tool/Makefile.am Mon Mar 01 18:33:42 2010 +0100 @@ -56,6 +56,7 @@ CommandSiiRead.cpp \ CommandSiiWrite.cpp \ CommandSlaves.cpp \ + CommandSoeRead.cpp \ CommandStates.cpp \ CommandUpload.cpp \ CommandVersion.cpp \ @@ -94,6 +95,7 @@ CommandSiiRead.h \ CommandSiiWrite.h \ CommandSlaves.h \ + CommandSoeRead.h \ CommandStates.h \ CommandUpload.h \ CommandVersion.h \ diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/MasterDevice.cpp --- a/tool/MasterDevice.cpp Fri Feb 26 18:22:02 2010 +0100 +++ b/tool/MasterDevice.cpp Mon Mar 01 18:33:42 2010 +0100 @@ -526,4 +526,19 @@ #endif +/****************************************************************************/ + +void MasterDevice::readSoe(ec_ioctl_slave_soe_t *data) +{ + if (ioctl(fd, EC_IOCTL_SLAVE_SOE_READ, data) < 0) { + if (errno == EIO && data->error_code) { + throw MasterDeviceSoeException(data->error_code); + } else { + stringstream err; + err << "Failed to read IDN: " << strerror(errno); + throw MasterDeviceException(err); + } + } +} + /*****************************************************************************/ diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/MasterDevice.h --- a/tool/MasterDevice.h Fri Feb 26 18:22:02 2010 +0100 +++ b/tool/MasterDevice.h Mon Mar 01 18:33:42 2010 +0100 @@ -67,7 +67,7 @@ uint32_t abortCode; protected: - /** Constructor with stringstream parameter. */ + /** Constructor with abort code parameter. */ MasterDeviceSdoAbortException(uint32_t code): MasterDeviceException("SDO transfer aborted.") { abortCode = code; @@ -76,6 +76,24 @@ /****************************************************************************/ +class MasterDeviceSoeException: + public MasterDeviceException +{ + friend class MasterDevice; + + public: + uint16_t errorCode; + + protected: + /** Constructor with error code parameter. */ + MasterDeviceSoeException(uint16_t code): + MasterDeviceException("SoE transfer aborted.") { + errorCode = code; + }; +}; + +/****************************************************************************/ + class MasterDevice { public: @@ -122,6 +140,7 @@ #ifdef EC_EOE void getEoeHandler(ec_ioctl_eoe_handler_t *, uint16_t); #endif + void readSoe(ec_ioctl_slave_soe_t *); unsigned int getMasterCount() const {return masterCount;} diff -r ef09f0ea0c4c -r 1875b9fea0ba tool/main.cpp --- a/tool/main.cpp Fri Feb 26 18:22:02 2010 +0100 +++ b/tool/main.cpp Mon Mar 01 18:33:42 2010 +0100 @@ -56,6 +56,7 @@ #include "CommandSiiRead.h" #include "CommandSiiWrite.h" #include "CommandSlaves.h" +#include "CommandSoeRead.h" #include "CommandStates.h" #include "CommandUpload.h" #include "CommandVersion.h" @@ -335,6 +336,7 @@ commandList.push_back(new CommandSiiRead()); commandList.push_back(new CommandSiiWrite()); commandList.push_back(new CommandSlaves()); + commandList.push_back(new CommandSoeRead()); commandList.push_back(new CommandStates()); commandList.push_back(new CommandUpload()); commandList.push_back(new CommandVersion());