# HG changeset patch # User Florian Pose # Date 1262946367 -3600 # Node ID 7c49cd56f96f2c8fa424c96afd29da6e183d73e3 # Parent d46de2278ac617d2d396fab125c62ddaa5052586# Parent 8d8657654921cf46d175a93f812a7d8057ee4578 Merged Komax changes. diff -r 8d8657654921 -r 7c49cd56f96f devices/generic.c --- a/devices/generic.c Fri Jan 08 10:34:29 2010 +0100 +++ b/devices/generic.c Fri Jan 08 11:26:07 2010 +0100 @@ -70,14 +70,16 @@ typedef struct { struct list_head list; struct net_device *netdev; - struct socket *socket; + struct net_device *used_netdev; + struct socket *socket; ec_device_t *ecdev; uint8_t *rx_buf; } ec_gen_device_t; typedef struct { struct list_head list; - char name[IFNAMSIZ]; + struct net_device *netdev; + char name[IFNAMSIZ]; int ifindex; uint8_t dev_addr[ETH_ALEN]; } ec_gen_interface_desc_t; @@ -200,7 +202,7 @@ int ret; struct sockaddr_ll sa; - dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL); + dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL); if (!dev->rx_buf) { return -ENOMEM; } @@ -240,7 +242,8 @@ { int ret = 0; - memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN); + dev->used_netdev = desc->netdev; + memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN); dev->ecdev = ecdev_offer(dev->netdev, ec_gen_poll, THIS_MODULE); if (dev->ecdev) { @@ -251,7 +254,7 @@ ecdev_withdraw(dev->ecdev); dev->ecdev = NULL; } else { - ecdev_set_link(dev->ecdev, 1); // FIXME + ecdev_set_link(dev->ecdev, netif_carrier_ok(dev->used_netdev)); // FIXME ret = 1; } } @@ -293,6 +296,8 @@ size_t len = skb->len; int ret; + ecdev_set_link(dev->ecdev,netif_carrier_ok(dev->used_netdev)); + iov.iov_base = skb->data; iov.iov_len = len; memset(&msg, 0, sizeof(msg)); @@ -314,7 +319,8 @@ struct kvec iov; int ret, budget = 10; // FIXME - do { + ecdev_set_link(dev->ecdev,netif_carrier_ok(dev->used_netdev)); + do { iov.iov_base = dev->rx_buf; iov.iov_len = EC_GEN_RX_BUF_SIZE; memset(&msg, 0, sizeof(msg)); @@ -408,6 +414,7 @@ goto out_err; } strncpy(desc->name, netdev->name, IFNAMSIZ); + desc->netdev = netdev; desc->ifindex = netdev->ifindex; memcpy(desc->dev_addr, netdev->dev_addr, ETH_ALEN); list_add_tail(&desc->list, &descs); diff -r 8d8657654921 -r 7c49cd56f96f include/ecrt.h --- a/include/ecrt.h Fri Jan 08 10:34:29 2010 +0100 +++ b/include/ecrt.h Fri Jan 08 11:26:07 2010 +0100 @@ -695,12 +695,12 @@ ); -/** Set max. number of databytes in a cycle - * - */ -int ecrt_master_set_max_cycle_size( +/** Set interval between calls to ecrt_master_send + * + */ +int ecrt_master_set_send_interval( ec_master_t *master, /**< EtherCAT master. */ - size_t max_cycle_data_size /**< Max. number of databytes in a cycle */ + size_t send_interval /**< Send interval in us */ ); /** Sends all datagrams in the queue. diff -r 8d8657654921 -r 7c49cd56f96f lib/master.c --- a/lib/master.c Fri Jan 08 10:34:29 2010 +0100 +++ b/lib/master.c Fri Jan 08 11:26:07 2010 +0100 @@ -347,11 +347,11 @@ /*****************************************************************************/ -int ecrt_master_set_max_cycle_size(ec_master_t *master,size_t max_cycle_data_size) -{ - if (ioctl(master->fd, EC_IOCTL_SET_MAX_CYCLE_SIZE, - &max_cycle_data_size) == -1) { - fprintf(stderr, "Failed to activate master: %s\n", +int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us) +{ + if (ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, + &send_interval_us) == -1) { + fprintf(stderr, "Failed to set send interval: %s\n", strerror(errno)); return -1; // FIXME } diff -r 8d8657654921 -r 7c49cd56f96f master/cdev.c --- a/master/cdev.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/cdev.c Fri Jan 08 11:26:07 2010 +0100 @@ -808,12 +808,12 @@ if (master->debug_level) EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position); // schedule request. - list_add_tail(&request.list, &master->slave_sdo_requests); + list_add_tail(&request.list, &request.slave->slave_sdo_requests); up(&master->master_sem); // wait for processing through FSM - if (wait_event_interruptible(master->sdo_queue, + if (wait_event_interruptible(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -828,7 +828,7 @@ } // wait until master FSM has finished processing - wait_event(master->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); @@ -914,12 +914,12 @@ if (master->debug_level) EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position); // schedule request. - list_add_tail(&request.list, &master->slave_sdo_requests); + list_add_tail(&request.list, &request.slave->slave_sdo_requests); up(&master->master_sem); // wait for processing through FSM - if (wait_event_interruptible(master->sdo_queue, + if (wait_event_interruptible(request.slave->sdo_queue, request.req.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -934,7 +934,7 @@ } // wait until master FSM has finished processing - wait_event(master->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); @@ -1678,21 +1678,21 @@ /** Set max. number of databytes in a cycle */ -int ec_cdev_ioctl_set_max_cycle_size( - ec_master_t *master, /**< EtherCAT master. */ - unsigned long arg, /**< ioctl() argument. */ - ec_cdev_priv_t *priv /**< Private data structure of file handle. */ - ) -{ - size_t max_cycle_size; - - if (copy_from_user(&max_cycle_size, (void __user *) arg, sizeof(max_cycle_size))) { - return -EFAULT; - } - - if (down_interruptible(&master->master_sem)) - return -EINTR; - master->max_queue_size = max_cycle_size; +int ec_cdev_ioctl_set_send_interval( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg, /**< ioctl() argument. */ + ec_cdev_priv_t *priv /**< Private data structure of file handle. */ + ) +{ + size_t send_interval; + + if (copy_from_user(&send_interval, (void __user *) arg, sizeof(send_interval))) { + return -EFAULT; + } + + if (down_interruptible(&master->master_sem)) + return -EINTR; + ec_master_set_send_interval(master,send_interval); up(&master->master_sem); return 0; @@ -3057,7 +3057,7 @@ } // schedule request. - list_add_tail(&request.list, &master->foe_requests); + list_add_tail(&request.list, &request.slave->foe_requests); up(&master->master_sem); @@ -3067,7 +3067,7 @@ } // wait for processing through FSM - if (wait_event_interruptible(master->foe_queue, + if (wait_event_interruptible(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -3082,7 +3082,7 @@ } // wait until master FSM has finished processing - wait_event(master->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; @@ -3172,12 +3172,12 @@ } // schedule FoE write request. - list_add_tail(&request.list, &master->foe_requests); + list_add_tail(&request.list, &request.slave->foe_requests); up(&master->master_sem); // wait for processing through FSM - if (wait_event_interruptible(master->foe_queue, + if (wait_event_interruptible(request.slave->foe_queue, request.req.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal down(&master->master_sem); @@ -3192,7 +3192,7 @@ } // wait until master FSM has finished processing - wait_event(master->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; @@ -3492,10 +3492,10 @@ return ec_cdev_ioctl_voe_exec(master, arg, priv); case EC_IOCTL_VOE_DATA: return ec_cdev_ioctl_voe_data(master, arg, priv); - case EC_IOCTL_SET_MAX_CYCLE_SIZE: - if (!(filp->f_mode & FMODE_WRITE)) - return -EPERM; - return ec_cdev_ioctl_set_max_cycle_size(master,arg,priv); + case EC_IOCTL_SET_SEND_INTERVAL: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_set_send_interval(master,arg,priv); default: return -ENOTTY; } diff -r 8d8657654921 -r 7c49cd56f96f master/fsm_master.c --- a/master/fsm_master.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/fsm_master.c Fri Jan 08 11:26:07 2010 +0100 @@ -58,7 +58,6 @@ void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); void ec_fsm_master_state_reg_request(ec_fsm_master_t *); -void ec_fsm_master_state_foe_request(ec_fsm_master_t *); /*****************************************************************************/ @@ -82,7 +81,6 @@ // init sub-state-machines ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); - ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, @@ -102,7 +100,6 @@ { // clear sub-state machines ec_fsm_coe_clear(&fsm->fsm_coe); - ec_fsm_foe_clear(&fsm->fsm_foe); ec_fsm_pdo_clear(&fsm->fsm_pdo); ec_fsm_change_clear(&fsm->fsm_change); ec_fsm_slave_config_clear(&fsm->fsm_slave_config); @@ -456,70 +453,32 @@ return 0; } -/*****************************************************************************/ - -/** Check for pending FoE requests and process one. - * - * \return non-zero, if an FoE request is processed. - */ -int ec_fsm_master_action_process_foe( + +/*****************************************************************************/ + +/** Master action: IDLE. + * + * Does secondary work. + */ +void ec_fsm_master_action_idle( ec_fsm_master_t *fsm /**< Master state machine. */ ) { ec_master_t *master = fsm->master; ec_slave_t *slave; - ec_master_foe_request_t *request; - - // search the first request to be processed - while (1) { - if (list_empty(&master->foe_requests)) - break; - - // get first request - request = list_entry(master->foe_requests.next, - ec_master_foe_request_t, list); - list_del_init(&request->list); // dequeue - request->req.state = EC_INT_REQUEST_BUSY; - slave = request->slave; - - if (master->debug_level) - EC_DBG("Processing FoE request for slave %u.\n", - slave->ring_position); - - fsm->foe_request = &request->req; - fsm->slave = slave; - fsm->state = ec_fsm_master_state_foe_request; - fsm->idle = 0; - ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); - ec_fsm_foe_exec(&fsm->fsm_foe); - return 1; - } - - return 0; -} - -/*****************************************************************************/ - -/** Master action: IDLE. - * - * Does secondary work. - */ -void ec_fsm_master_action_idle( - ec_fsm_master_t *fsm /**< Master state machine. */ - ) -{ - ec_master_t *master = fsm->master; - ec_slave_t *slave; - - // Check for pending SDO requests + + // Check for pending internal SDO requests if (ec_fsm_master_action_process_sdo(fsm)) return; - // Check for pending FoE requests - if (ec_fsm_master_action_process_foe(fsm)) - return; - - // check, if slaves have an SDO dictionary to read out. + // enable processing of SDO/FOE requests + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + ec_fsm_slave_ready(&slave->fsm); + } + + // check, if slaves have an SDO dictionary to read out. for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) { @@ -932,43 +891,6 @@ /*****************************************************************************/ -/** Master state: WRITE FOE. - */ -void ec_fsm_master_state_foe_request( - ec_fsm_master_t *fsm /**< Master state machine. */ - ) -{ - ec_master_t *master = fsm->master; - ec_foe_request_t *request = fsm->foe_request; - ec_slave_t *slave = fsm->slave; - - if (ec_fsm_foe_exec(&fsm->fsm_foe)) - return; - - fsm->idle = 1; - - if (!ec_fsm_foe_success(&fsm->fsm_foe)) { - EC_ERR("Failed to handle FoE request to slave %u.\n", - slave->ring_position); - request->state = EC_INT_REQUEST_FAILURE; - wake_up(&master->foe_queue); - ec_fsm_master_restart(fsm); - return; - } - - // finished transferring FoE - if (master->debug_level) - EC_DBG("Successfully transferred %zu bytes of FoE data from/to" - " slave %u.\n", request->data_size, slave->ring_position); - - request->state = EC_INT_REQUEST_SUCCESS; - wake_up(&master->foe_queue); - - ec_fsm_master_restart(fsm); -} - -/*****************************************************************************/ - /** Master state: SDO DICTIONARY. */ void ec_fsm_master_state_sdo_dictionary( @@ -1017,14 +939,14 @@ EC_DBG("Failed to process internal SDO request for slave %u.\n", fsm->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; - wake_up(&master->sdo_queue); + wake_up(&fsm->slave->sdo_queue); ec_fsm_master_restart(fsm); return; } // SDO request finished request->state = EC_INT_REQUEST_SUCCESS; - wake_up(&master->sdo_queue); + wake_up(&fsm->slave->sdo_queue); if (master->debug_level) EC_DBG("Finished internal SDO request for slave %u.\n", diff -r 8d8657654921 -r 7c49cd56f96f master/fsm_master.h --- a/master/fsm_master.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/fsm_master.h Fri Jan 08 11:26:07 2010 +0100 @@ -116,8 +116,6 @@ off_t sii_index; /**< index to SII write request data */ ec_sdo_request_t *sdo_request; /**< SDO request to process. */ ec_reg_request_t *reg_request; /**< Register request to process. */ - ec_foe_request_t *foe_request; /**< FoE request to process. */ - off_t foe_index; /**< index to FoE write request data */ ec_fsm_coe_t fsm_coe; /**< CoE state machine */ ec_fsm_pdo_t fsm_pdo; /**< PDO configuration state machine. */ @@ -125,7 +123,6 @@ ec_fsm_slave_config_t fsm_slave_config; /**< slave state machine */ ec_fsm_slave_scan_t fsm_slave_scan; /**< slave state machine */ ec_fsm_sii_t fsm_sii; /**< SII state machine */ - ec_fsm_foe_t fsm_foe; /**< FoE state machine */ }; /*****************************************************************************/ diff -r 8d8657654921 -r 7c49cd56f96f master/fsm_sii.c --- a/master/fsm_sii.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/fsm_sii.c Fri Jan 08 11:26:07 2010 +0100 @@ -301,7 +301,6 @@ } // issue check/fetch datagram again - fsm->datagram->state = EC_DATAGRAM_INIT; fsm->retries = EC_FSM_RETRIES; return; } @@ -435,7 +434,6 @@ #endif // issue check datagram again fsm->retries = EC_FSM_RETRIES; - fsm->datagram->state = EC_DATAGRAM_INIT; return; } @@ -454,7 +452,6 @@ // issue check datagram again fsm->retries = EC_FSM_RETRIES; - fsm->datagram->state = EC_DATAGRAM_INIT; return; } diff -r 8d8657654921 -r 7c49cd56f96f master/fsm_slave.c --- a/master/fsm_slave.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/fsm_slave.c Fri Jan 08 11:26:07 2010 +0100 @@ -42,7 +42,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 *); +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 *); /*****************************************************************************/ @@ -58,10 +62,13 @@ fsm->slave = slave; fsm->datagram = datagram; fsm->datagram->data_size = 0; - fsm->state = ec_fsm_slave_state_idle; + if (slave->master->debug_level) + EC_DBG("init fsm for slave %u...\n",slave->ring_position); + fsm->state = ec_fsm_slave_state_idle; // init sub-state-machines ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); + ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); } /*****************************************************************************/ @@ -74,6 +81,7 @@ { // clear sub-state machines ec_fsm_coe_clear(&fsm->fsm_coe); + ec_fsm_foe_clear(&fsm->fsm_foe); } /*****************************************************************************/ @@ -97,16 +105,70 @@ return; } + +/*****************************************************************************/ + +/** Sets the current state of the state machine to READY + * + */ +void ec_fsm_slave_ready( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + if (fsm->state == ec_fsm_slave_state_idle) { + if (fsm->slave->master->debug_level) { + EC_DBG("Slave %u ready for SDO/FOE.\n",fsm->slave->ring_position); + } + fsm->state = ec_fsm_slave_state_ready; + } + return; +} + /****************************************************************************** * Slave state machine *****************************************************************************/ +/*****************************************************************************/ + /** Slave state: IDLE. * - * Check for pending SDO requests and process one. * */ void ec_fsm_slave_state_idle( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + // do nothing +} + + +/*****************************************************************************/ + +/** Slave state: READY. + * + * + */ +void ec_fsm_slave_state_ready( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + // Check for pending external SDO requests + if (ec_fsm_slave_action_process_sdo(fsm)) + return; + // Check for pending FOE requests + if (ec_fsm_slave_action_process_foe(fsm)) + return; + +} + + +/*****************************************************************************/ + +/** Check for pending SDO requests and process one. + * + * \return non-zero, if an SDO request is processed. + */ +int ec_fsm_slave_action_process_sdo( ec_fsm_slave_t *fsm /**< Slave state machine. */ ) { @@ -114,20 +176,29 @@ ec_master_t *master = slave->master; ec_master_sdo_request_t *request, *next; - // search the first matching external request to be processed - list_for_each_entry_safe(request, next, &master->slave_sdo_requests, list) { - if (request->slave != slave) - continue; + // search the first external request to be processed + list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { + list_del_init(&request->list); // dequeue - request->req.state = EC_INT_REQUEST_BUSY; - - if (slave->current_state == EC_SLAVE_STATE_INIT) { - EC_ERR("Discarding SDO request, slave %u is in INIT.\n", - slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&master->sdo_queue); - continue; - } + if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { + EC_WARN("Aborting SDO 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; + } + if (slave->current_state == EC_SLAVE_STATE_INIT) { + EC_WARN("Aborting SDO request, slave %u is in INIT.\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; + } + request->req.state = EC_INT_REQUEST_BUSY; // Found pending SDO request. Execute it! if (master->debug_level) @@ -139,10 +210,56 @@ fsm->state = ec_fsm_slave_state_sdo_request; ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately - ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); - return; - } -} + ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); + return 1; + } + 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; +} + + /*****************************************************************************/ @@ -158,28 +275,69 @@ if (ec_fsm_coe_exec(&fsm->fsm_coe)) { - ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); + ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); return; } if (!ec_fsm_coe_success(&fsm->fsm_coe)) { EC_DBG("Failed to process SDO request for slave %u.\n", fsm->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; - wake_up(&master->sdo_queue); - fsm->sdo_request = NULL; - fsm->state = ec_fsm_slave_state_idle; - return; - } - - // SDO request finished - request->state = EC_INT_REQUEST_SUCCESS; - wake_up(&master->sdo_queue); + wake_up(&slave->sdo_queue); + fsm->sdo_request = NULL; + fsm->state = ec_fsm_slave_state_idle; + return; + } if (master->debug_level) EC_DBG("Finished SDO request for slave %u.\n", fsm->slave->ring_position); + // SDO request finished + request->state = EC_INT_REQUEST_SUCCESS; + wake_up(&slave->sdo_queue); + fsm->sdo_request = NULL; - fsm->datagram->data_size = 0; - fsm->state = ec_fsm_slave_state_idle; -} + fsm->state = ec_fsm_slave_state_ready; +} + + +/*****************************************************************************/ + +/** Slave state: FOE REQUEST. + */ +void ec_fsm_slave_state_foe_request( + ec_fsm_slave_t *fsm /**< Slave state machine. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_foe_request_t *request = fsm->foe_request; + + if (ec_fsm_foe_exec(&fsm->fsm_foe)) + { + ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); + return; + } + + if (!ec_fsm_foe_success(&fsm->fsm_foe)) { + EC_ERR("Failed to handle FoE request to slave %u.\n", + slave->ring_position); + request->state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->foe_queue); + fsm->foe_request = NULL; + fsm->state = ec_fsm_slave_state_idle; + return; + } + + // finished transferring FoE + if (master->debug_level) + EC_DBG("Successfully transferred %u bytes of FoE data from/to" + " slave %u.\n", request->data_size, slave->ring_position); + + request->state = EC_INT_REQUEST_SUCCESS; + wake_up(&slave->foe_queue); + + fsm->foe_request = NULL; + fsm->state = ec_fsm_slave_state_ready; +} + diff -r 8d8657654921 -r 7c49cd56f96f master/fsm_slave.h --- a/master/fsm_slave.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/fsm_slave.h Fri Jan 08 11:26:07 2010 +0100 @@ -40,7 +40,7 @@ #include "datagram.h" #include "sdo_request.h" #include "fsm_coe.h" - +#include "fsm_foe.h" typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */ @@ -52,8 +52,11 @@ void (*state)(ec_fsm_slave_t *); /**< master state function */ 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_fsm_coe_t fsm_coe; /**< CoE state machine */ + ec_fsm_foe_t fsm_foe; /**< FoE state machine */ }; /*****************************************************************************/ @@ -62,7 +65,7 @@ void ec_fsm_slave_clear(ec_fsm_slave_t *); void ec_fsm_slave_exec(ec_fsm_slave_t *); -int ec_fsm_slave_idle(const ec_fsm_slave_t *); +void ec_fsm_slave_ready(ec_fsm_slave_t *); /*****************************************************************************/ diff -r 8d8657654921 -r 7c49cd56f96f master/globals.h --- a/master/globals.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/globals.h Fri Jan 08 11:26:07 2010 +0100 @@ -52,7 +52,7 @@ #define EC_SDO_INJECTION_TIMEOUT 10000 /** time to send a byte in nanoseconds. */ -#define EC_BYTE_TRANSMITION_TIME 80LL +#define EC_BYTE_TRANSMITION_TIME 80 /** Number of state machine retries on datagram timeout. */ #define EC_FSM_RETRIES 3 diff -r 8d8657654921 -r 7c49cd56f96f master/ioctl.h --- a/master/ioctl.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/ioctl.h Fri Jan 08 11:26:07 2010 +0100 @@ -123,7 +123,7 @@ #define EC_IOCTL_VOE_WRITE EC_IOWR(0x3f, ec_ioctl_voe_t) #define EC_IOCTL_VOE_EXEC EC_IOWR(0x40, ec_ioctl_voe_t) #define EC_IOCTL_VOE_DATA EC_IOWR(0x41, ec_ioctl_voe_t) -#define EC_IOCTL_SET_MAX_CYCLE_SIZE EC_IOW(0x42, size_t) +#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x42, size_t) /*****************************************************************************/ diff -r 8d8657654921 -r 7c49cd56f96f master/master.c --- a/master/master.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/master.c Fri Jan 08 11:26:07 2010 +0100 @@ -41,7 +41,7 @@ #include #include #include - +#include #include "globals.h" #include "slave.h" #include "slave_config.h" @@ -54,6 +54,10 @@ /*****************************************************************************/ +/** Set to 1 to enable external datagram injection debugging. + */ +#define DEBUG_INJECT 0 + #ifdef EC_HAVE_CYCLES /** Frame timeout in cycles. @@ -154,8 +158,8 @@ INIT_LIST_HEAD(&master->ext_datagram_queue); sema_init(&master->ext_queue_sem, 1); - INIT_LIST_HEAD(&master->sdo_datagram_queue); - master->max_queue_size = EC_MAX_DATA_SIZE; + INIT_LIST_HEAD(&master->external_datagram_queue); + ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase INIT_LIST_HEAD(&master->domains); @@ -184,15 +188,9 @@ INIT_LIST_HEAD(&master->sii_requests); init_waitqueue_head(&master->sii_queue); - INIT_LIST_HEAD(&master->slave_sdo_requests); - init_waitqueue_head(&master->sdo_queue); - INIT_LIST_HEAD(&master->reg_requests); init_waitqueue_head(&master->reg_queue); - INIT_LIST_HEAD(&master->foe_requests); - init_waitqueue_head(&master->foe_queue); - // init devices ret = ec_device_init(&master->main_device, master); if (ret < 0) @@ -405,39 +403,37 @@ wake_up(&master->reg_queue); } - // SDO requests - while (1) { - ec_master_sdo_request_t *request; - if (list_empty(&master->slave_sdo_requests)) - break; - // get first request - request = list_entry(master->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", - request->slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&master->sdo_queue); - } - - // FoE requests - while (1) { - ec_master_foe_request_t *request; - if (list_empty(&master->foe_requests)) - break; - // get first request - request = list_entry(master->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", - request->slave->ring_position); - request->req.state = EC_INT_REQUEST_FAILURE; - wake_up(&master->foe_queue); - } - for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) { + // SDO requests + while (1) { + ec_master_sdo_request_t *request; + if (list_empty(&slave->slave_sdo_requests)) + break; + // get first request + 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", + slave->ring_position); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->sdo_queue); + } + // FoE requests + while (1) { + ec_master_foe_request_t *request; + if (list_empty(&slave->foe_requests)) + break; + // get first request + 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", + slave->ring_position); + request->req.state = EC_INT_REQUEST_FAILURE; + wake_up(&slave->foe_queue); + } ec_slave_clear(slave); } @@ -693,86 +689,118 @@ /*****************************************************************************/ -/** Injects sdo datagrams that fit into the datagram queue - */ -void ec_master_inject_sdo_datagrams( - ec_master_t *master /**< EtherCAT master */ - ) -{ - ec_datagram_t *datagram, *n; - size_t queue_size = 0; - list_for_each_entry(datagram, &master->datagram_queue, queue) { - queue_size += datagram->data_size; - } - list_for_each_entry_safe(datagram, n, &master->sdo_datagram_queue, queue) { - queue_size += datagram->data_size; - if (queue_size <= master->max_queue_size) { - list_del_init(&datagram->queue); - if (master->debug_level) { - EC_DBG("Injecting SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); - } +/** Injects external datagrams that fit into the datagram queue + */ +void ec_master_inject_external_datagrams( + ec_master_t *master /**< EtherCAT master */ + ) +{ + ec_datagram_t *datagram, *n; + size_t queue_size = 0; + list_for_each_entry(datagram, &master->datagram_queue, queue) { + queue_size += datagram->data_size; + } + list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { + queue_size += datagram->data_size; + if (queue_size <= master->max_queue_size) { + list_del_init(&datagram->queue); +#if DEBUG_INJECT + if (master->debug_level) { + EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } +#endif #ifdef EC_HAVE_CYCLES - datagram->cycles_sent = 0; -#endif - datagram->jiffies_sent = 0; - ec_master_queue_datagram(master, datagram); - } - else { - if (datagram->data_size > master->max_queue_size) { - list_del_init(&datagram->queue); - datagram->state = EC_DATAGRAM_ERROR; - EC_ERR("SDO datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); - } - else { + datagram->cycles_sent = 0; +#endif + datagram->jiffies_sent = 0; + ec_master_queue_datagram(master, datagram); + } + else { + if (datagram->data_size > master->max_queue_size) { + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; + EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { #ifdef EC_HAVE_CYCLES - cycles_t cycles_now = get_cycles(); - if (cycles_now - datagram->cycles_sent - > sdo_injection_timeout_cycles) { + cycles_t cycles_now = get_cycles(); + if (cycles_now - datagram->cycles_sent + > sdo_injection_timeout_cycles) { #else - if (jiffies - datagram->jiffies_sent - > sdo_injection_timeout_jiffies) { -#endif - unsigned int time_us; - list_del_init(&datagram->queue); - datagram->state = EC_DATAGRAM_ERROR; + if (jiffies - datagram->jiffies_sent + > sdo_injection_timeout_jiffies) { +#endif + unsigned int time_us; + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; #ifdef EC_HAVE_CYCLES - time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; + time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; #else - time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); -#endif - EC_ERR("Timeout %u us: injecting SDO datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); - } - else { - if (master->debug_level) { - EC_DBG("Deferred injecting of SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); - } - } - } - } - } -} - -/*****************************************************************************/ - -/** Places a sdo datagram in the sdo datagram queue. - */ -void ec_master_queue_sdo_datagram( + time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); +#endif + EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { +#if DEBUG_INJECT + if (master->debug_level) { + EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } +#endif + } + } + } + } +} + +/*****************************************************************************/ + +/** sets the expected interval between calls to ecrt_master_send + and calculates the maximum amount of data to queue + */ +void ec_master_set_send_interval( + ec_master_t *master, /**< EtherCAT master */ + size_t send_interval /**< send interval */ + ) +{ + master->send_interval = send_interval; + master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; + master->max_queue_size -= master->max_queue_size / 10; +} + + +/*****************************************************************************/ + +/** Places an external datagram in the sdo datagram queue. + */ +void ec_master_queue_external_datagram( ec_master_t *master, /**< EtherCAT master */ ec_datagram_t *datagram /**< datagram */ ) { - if (master->debug_level) { - EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); - } - datagram->state = EC_DATAGRAM_QUEUED; + ec_datagram_t *queued_datagram; + + down(&master->io_sem); + // check, if the datagram is already queued + list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) { + if (queued_datagram == datagram) { + datagram->state = EC_DATAGRAM_QUEUED; + return; + } + } +#if DEBUG_INJECT + if (master->debug_level) { + EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); + } +#endif + list_add_tail(&datagram->queue, &master->external_datagram_queue); + datagram->state = EC_DATAGRAM_QUEUED; #ifdef EC_HAVE_CYCLES - datagram->cycles_sent = get_cycles(); -#endif - datagram->jiffies_sent = jiffies; - - down(&master->io_sem); - list_add_tail(&datagram->queue, &master->sdo_datagram_queue); - up(&master->io_sem); + datagram->cycles_sent = get_cycles(); +#endif + datagram->jiffies_sent = jiffies; + + master->fsm.idle = 0; + up(&master->io_sem); } /*****************************************************************************/ @@ -821,12 +849,11 @@ /** Sends the datagrams in the queue. * - * \return 0 in case of success, else < 0 */ void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) { ec_datagram_t *datagram, *next; - size_t datagram_size; + size_t datagram_size; uint8_t *frame_data, *cur_data; void *follows_word; #ifdef EC_HAVE_CYCLES @@ -1087,6 +1114,69 @@ } } + +/*****************************************************************************/ +/* + * Sleep related functions: + */ +static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) +{ + struct hrtimer_sleeper *t = + container_of(timer, struct hrtimer_sleeper, timer); + struct task_struct *task = t->task; + + t->task = NULL; + if (task) + wake_up_process(task); + + return HRTIMER_NORESTART; +} + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) +/* compatibility with new hrtimer interface */ +static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer) +{ + return timer->expires; +} + +static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time) +{ + timer->expires = time; +} +#endif + + +void ec_master_nanosleep(const unsigned long nsecs) +{ + struct hrtimer_sleeper t; + enum hrtimer_mode mode = HRTIMER_MODE_REL; + hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode); + t.timer.function = ec_master_nanosleep_wakeup; + t.task = current; +#ifdef CONFIG_HIGH_RES_TIMERS +#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24) + t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; +#elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26) + t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ; +#elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28) + t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED; +#endif +#endif + hrtimer_set_expires(&t.timer, ktime_set(0,nsecs)); + do { + set_current_state(TASK_INTERRUPTIBLE); + hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode); + + if (likely(t.task)) + schedule(); + + hrtimer_cancel(&t.timer); + mode = HRTIMER_MODE_ABS; + + } while (t.task && !signal_pending(current)); +} + + /*****************************************************************************/ /** Master kernel thread function for IDLE phase. @@ -1096,8 +1186,10 @@ ec_master_t *master = (ec_master_t *) priv_data; ec_slave_t *slave = NULL; int fsm_exec; - if (master->debug_level) - EC_DBG("Idle thread running.\n"); + size_t sent_bytes; + ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase + if (master->debug_level) + EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1124,17 +1216,15 @@ if (fsm_exec) { ec_master_queue_datagram(master, &master->fsm_datagram); } - ec_master_inject_sdo_datagrams(master); + ec_master_inject_external_datagrams(master); ecrt_master_send(master); + sent_bytes = master->main_device.tx_skb[master->main_device.tx_ring_index]->len; up(&master->io_sem); - if (ec_fsm_master_idle(&master->fsm)) { - set_current_state(TASK_INTERRUPTIBLE); - schedule_timeout(1); - } - else { - schedule(); - } + if (ec_fsm_master_idle(&master->fsm)) + ec_master_nanosleep(master->send_interval*1000); + else + ec_master_nanosleep(sent_bytes*EC_BYTE_TRANSMITION_TIME); } if (master->debug_level) @@ -1152,7 +1242,7 @@ ec_slave_t *slave = NULL; int fsm_exec; if (master->debug_level) - EC_DBG("Operation thread running.\n"); + EC_DBG("Operation thread running with fsm interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1176,14 +1266,9 @@ if (fsm_exec) master->injection_seq_fsm++; } - if (ec_fsm_master_idle(&master->fsm)) { - set_current_state(TASK_INTERRUPTIBLE); - schedule_timeout(1); - } - else { - schedule(); - } - } + // the op thread should not work faster than the sending RT thread + ec_master_nanosleep(master->send_interval*1000); + } if (master->debug_level) EC_DBG("Master OP thread exiting...\n"); @@ -1903,7 +1988,7 @@ ec_master_queue_datagram(master, &master->fsm_datagram); master->injection_seq_rt = master->injection_seq_fsm; } - ec_master_inject_sdo_datagrams(master); + ec_master_inject_external_datagrams(master); if (unlikely(!master->main_device.link_state)) { // link is down, no datagram can be sent @@ -1918,7 +2003,7 @@ } // send frames - ec_master_send_datagrams(master); + ec_master_send_datagrams(master); } /*****************************************************************************/ diff -r 8d8657654921 -r 7c49cd56f96f master/master.h --- a/master/master.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/master.h Fri Jan 08 11:26:07 2010 +0100 @@ -154,7 +154,8 @@ struct semaphore ext_queue_sem; /**< Semaphore protecting the \a ext_datagram_queue. */ - struct list_head sdo_datagram_queue; /**< SDO Datagram queue. */ + struct list_head external_datagram_queue; /**< External Datagram queue. */ + size_t send_interval; /* interval between calls to ecrt_master_send */ size_t max_queue_size; /** max. size of datagram queue */ struct list_head domains; /**< List of domains. */ @@ -185,16 +186,9 @@ wait_queue_head_t sii_queue; /**< Wait queue for SII write requests from user space. */ - struct list_head slave_sdo_requests; /**< SDO access requests. */ - wait_queue_head_t sdo_queue; /**< Wait queue for SDO access requests - from user space. */ - struct list_head reg_requests; /**< Register requests. */ wait_queue_head_t reg_queue; /**< Wait queue for register requests. */ - struct list_head foe_requests; /**< FoE write requests. */ - wait_queue_head_t foe_queue; /**< Wait queue for FoE - write requests from user space. */ }; /*****************************************************************************/ @@ -223,10 +217,11 @@ void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t); void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *); void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *); -void ec_master_queue_sdo_datagram(ec_master_t *, ec_datagram_t *); -void ec_master_inject_sdo_datagrams(ec_master_t *); +void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *); +void ec_master_inject_external_datagrams(ec_master_t *); // misc. +void ec_master_set_send_interval(ec_master_t *,size_t); void ec_master_attach_slave_configs(ec_master_t *); ec_slave_t *ec_master_find_slave(ec_master_t *, uint16_t, uint16_t); const ec_slave_t *ec_master_find_slave_const(const ec_master_t *, uint16_t, diff -r 8d8657654921 -r 7c49cd56f96f master/slave.c --- a/master/slave.c Fri Jan 08 10:34:29 2010 +0100 +++ b/master/slave.c Fri Jan 08 11:26:07 2010 +0100 @@ -149,6 +149,12 @@ slave->sdo_dictionary_fetched = 0; slave->jiffies_preop = 0; + INIT_LIST_HEAD(&slave->slave_sdo_requests); + init_waitqueue_head(&slave->sdo_queue); + + INIT_LIST_HEAD(&slave->foe_requests); + init_waitqueue_head(&slave->foe_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 8d8657654921 -r 7c49cd56f96f master/slave.h --- a/master/slave.h Fri Jan 08 10:34:29 2010 +0100 +++ b/master/slave.h Fri Jan 08 11:26:07 2010 +0100 @@ -159,6 +159,12 @@ uint8_t sdo_dictionary_fetched; /**< Dictionary has been fetched. */ unsigned long jiffies_preop; /**< Time, the slave went to PREOP. */ + struct list_head slave_sdo_requests; /**< SDO access requests. */ + 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. */ ec_fsm_slave_t fsm; /**< Slave state machine. */ ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */ };