fp@39: /******************************************************************************
fp@0: *
fp@39: * $Id$
fp@0: *
fp@1326: * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
fp@197: *
fp@197: * This file is part of the IgH EtherCAT Master.
fp@197: *
fp@1326: * The IgH EtherCAT Master is free software; you can redistribute it and/or
fp@1326: * modify it under the terms of the GNU General Public License version 2, as
fp@1326: * published by the Free Software Foundation.
fp@1326: *
fp@1326: * The IgH EtherCAT Master is distributed in the hope that it will be useful,
fp@1326: * but WITHOUT ANY WARRANTY; without even the implied warranty of
fp@1326: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
fp@1326: * Public License for more details.
fp@1326: *
fp@1326: * You should have received a copy of the GNU General Public License along
fp@1326: * with the IgH EtherCAT Master; if not, write to the Free Software
fp@197: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
fp@197: *
fp@1363: * ---
fp@1363: *
fp@1363: * The license mentioned above concerns the source code only. Using the
fp@1363: * EtherCAT technology and brand is only permitted in compliance with the
fp@1363: * industrial property and similar rights of Beckhoff Automation GmbH.
fp@246: *
fp@39: *****************************************************************************/
fp@0:
fp@199: /**
fp@199: \file
fp@199: EtherCAT master methods.
fp@199: */
fp@199:
fp@199: /*****************************************************************************/
fp@199:
fp@24: #include <linux/module.h>
fp@0: #include <linux/kernel.h>
fp@0: #include <linux/string.h>
fp@0: #include <linux/slab.h>
fp@0: #include <linux/delay.h>
fp@1015: #include <linux/device.h>
fp@1015: #include <linux/version.h>
martin@1600: #include <linux/hrtimer.h>
fp@54: #include "globals.h"
fp@55: #include "slave.h"
fp@792: #include "slave_config.h"
fp@54: #include "device.h"
fp@293: #include "datagram.h"
fp@715: #ifdef EC_EOE
fp@145: #include "ethernet.h"
fp@715: #endif
fp@792: #include "master.h"
fp@792:
fp@792: /*****************************************************************************/
fp@792:
martin@1597: /** Set to 1 to enable external datagram injection debugging.
martin@1597: */
martin@1597: #define DEBUG_INJECT 0
martin@1597:
fp@1279: #ifdef EC_HAVE_CYCLES
fp@1279:
fp@1279: /** Frame timeout in cycles.
fp@1279: */
fp@1279: static cycles_t timeout_cycles;
martin@1585: static cycles_t sdo_injection_timeout_cycles;
fp@1279: #else
fp@1279:
fp@1279: /** Frame timeout in jiffies.
fp@1279: */
fp@1279: static unsigned long timeout_jiffies;
martin@1585: static unsigned long sdo_injection_timeout_jiffies;
martin@1585:
fp@1279: #endif
fp@1279:
fp@1279: /*****************************************************************************/
fp@1279:
fp@995: void ec_master_clear_slave_configs(ec_master_t *);
fp@993: void ec_master_clear_domains(ec_master_t *);
fp@1241: static int ec_master_idle_thread(void *);
fp@1241: static int ec_master_operation_thread(void *);
fp@715: #ifdef EC_EOE
fp@1489: static int ec_master_eoe_thread(void *);
fp@715: #endif
fp@1425: void ec_master_find_dc_ref_clock(ec_master_t *);
fp@179:
fp@179: /*****************************************************************************/
fp@179:
fp@1279: /** Static variables initializer.
fp@1279: */
fp@1279: void ec_master_init_static(void)
fp@1279: {
fp@1279: #ifdef EC_HAVE_CYCLES
fp@1279: timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
martin@1585: sdo_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
fp@1279: #else
fp@1279: // one jiffy may always elapse between time measurement
fp@1279: timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
martin@1585: sdo_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
fp@1279: #endif
fp@1279: }
fp@1279:
fp@1279: /*****************************************************************************/
fp@1279:
fp@0: /**
fp@195: Master constructor.
fp@195: \return 0 in case of success, else < 0
fp@195: */
fp@195:
fp@195: int ec_master_init(ec_master_t *master, /**< EtherCAT master */
fp@576: unsigned int index, /**< master index */
fp@639: const uint8_t *main_mac, /**< MAC address of main device */
fp@922: const uint8_t *backup_mac, /**< MAC address of backup device */
fp@1013: dev_t device_number, /**< Character device number. */
fp@1399: struct class *class, /**< Device class. */
fp@1399: unsigned int debug_level /**< Debug level (module parameter). */
fp@576: )
fp@178: {
fp@1313: int ret;
fp@1313:
fp@178: master->index = index;
fp@647: master->reserved = 0;
fp@446:
martin@1579: sema_init(&master->master_sem, 1);
fp@1075:
fp@639: master->main_mac = main_mac;
fp@639: master->backup_mac = backup_mac;
martin@1579:
martin@1579: sema_init(&master->device_sem, 1);
fp@446:
fp@1029: master->phase = EC_ORPHANED;
fp@1530: master->active = 0;
fp@637: master->injection_seq_fsm = 0;
fp@637: master->injection_seq_rt = 0;
fp@446:
fp@1000: master->slaves = NULL;
fp@446: master->slave_count = 0;
fp@656:
fp@792: INIT_LIST_HEAD(&master->configs);
fp@792:
fp@1507: master->app_time = 0ULL;
fp@1507: master->app_start_time = 0ULL;
fp@1507: master->has_start_time = 0;
fp@1408:
fp@900: master->scan_busy = 0;
fp@656: master->allow_scan = 1;
martin@1579: sema_init(&master->scan_sem, 1);
fp@656: init_waitqueue_head(&master->scan_queue);
fp@656:
fp@900: master->config_busy = 0;
fp@656: master->allow_config = 1;
martin@1579: sema_init(&master->config_sem, 1);
fp@656: init_waitqueue_head(&master->config_queue);
fp@656:
fp@293: INIT_LIST_HEAD(&master->datagram_queue);
fp@446: master->datagram_index = 0;
fp@446:
fp@1500: INIT_LIST_HEAD(&master->ext_datagram_queue);
martin@1579: sema_init(&master->ext_queue_sem, 1);
fp@1500:
martin@1597: INIT_LIST_HEAD(&master->external_datagram_queue);
fp@1773:
fp@1773: // send interval in IDLE phase
fp@1773: ec_master_set_send_interval(master, 1000000 / HZ);
martin@1583:
fp@95: INIT_LIST_HEAD(&master->domains);
fp@792:
fp@1399: master->debug_level = debug_level;
fp@446: master->stats.timeouts = 0;
fp@446: master->stats.corrupted = 0;
fp@446: master->stats.unmatched = 0;
fp@446: master->stats.output_jiffies = 0;
fp@446:
fp@1241: master->thread = NULL;
fp@1241:
fp@1041: #ifdef EC_EOE
fp@1489: master->eoe_thread = NULL;
fp@446: INIT_LIST_HEAD(&master->eoe_handlers);
fp@715: #endif
fp@446:
martin@1579: sema_init(&master->io_sem, 1);
fp@1500: master->send_cb = NULL;
fp@1500: master->receive_cb = NULL;
fp@1513: master->cb_data = NULL;
fp@1500: master->app_send_cb = NULL;
fp@1500: master->app_receive_cb = NULL;
fp@1513: master->app_cb_data = NULL;
fp@446:
fp@872: INIT_LIST_HEAD(&master->sii_requests);
fp@872: init_waitqueue_head(&master->sii_queue);
fp@601:
fp@1388: INIT_LIST_HEAD(&master->reg_requests);
fp@1388: init_waitqueue_head(&master->reg_queue);
fp@1200:
fp@579: // init devices
fp@1313: ret = ec_device_init(&master->main_device, master);
fp@1313: if (ret < 0)
fp@997: goto out_return;
fp@579:
fp@1313: ret = ec_device_init(&master->backup_device, master);
fp@1313: if (ret < 0)
fp@579: goto out_clear_main;
fp@579:
fp@528: // init state machine datagram
fp@528: ec_datagram_init(&master->fsm_datagram);
fp@719: snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm");
fp@1313: ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE);
fp@1313: if (ret < 0) {
fp@1394: ec_datagram_clear(&master->fsm_datagram);
fp@528: EC_ERR("Failed to allocate FSM datagram.\n");
fp@659: goto out_clear_backup;
fp@528: }
fp@528:
fp@251: // create state machine object
fp@528: ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
fp@226:
fp@1396: // init reference sync datagram
fp@1396: ec_datagram_init(&master->ref_sync_datagram);
fp@1396: snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync");
fp@1396: ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8);
fp@1396: if (ret < 0) {
fp@1396: ec_datagram_clear(&master->ref_sync_datagram);
fp@1396: EC_ERR("Failed to allocate reference synchronisation datagram.\n");
fp@1396: goto out_clear_fsm;
fp@1396: }
fp@1396:
fp@1394: // init sync datagram
fp@1394: ec_datagram_init(&master->sync_datagram);
fp@1394: snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync");
fp@1408: ret = ec_datagram_prealloc(&master->sync_datagram, 4);
fp@1394: if (ret < 0) {
fp@1394: ec_datagram_clear(&master->sync_datagram);
fp@1394: EC_ERR("Failed to allocate synchronisation datagram.\n");
fp@1396: goto out_clear_ref_sync;
fp@1394: }
fp@1535:
fp@1535: // init sync monitor datagram
fp@1535: ec_datagram_init(&master->sync_mon_datagram);
fp@1535: snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon");
fp@1535: ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4);
fp@1535: if (ret < 0) {
fp@1535: ec_datagram_clear(&master->sync_mon_datagram);
fp@1535: EC_ERR("Failed to allocate sync monitoring datagram.\n");
fp@1535: goto out_clear_sync;
fp@1535: }
fp@1535:
fp@1507: ec_master_find_dc_ref_clock(master);
fp@1394:
fp@997: // init character device
fp@1313: ret = ec_cdev_init(&master->cdev, master, device_number);
fp@1313: if (ret)
fp@1535: goto out_clear_sync_mon;
fp@1013:
fp@1369: #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
fp@1369: master->class_device = device_create(class, NULL,
fp@1369: MKDEV(MAJOR(device_number), master->index), NULL,
fp@1369: "EtherCAT%u", master->index);
fp@1369: #elif LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26)
fp@1250: master->class_device = device_create(class, NULL,
fp@1250: MKDEV(MAJOR(device_number), master->index),
fp@1250: "EtherCAT%u", master->index);
fp@1250: #elif LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 15)
fp@1250: master->class_device = class_device_create(class, NULL,
fp@1369: MKDEV(MAJOR(device_number), master->index), NULL,
fp@1369: "EtherCAT%u", master->index);
fp@1015: #else
fp@1250: master->class_device = class_device_create(class,
fp@1369: MKDEV(MAJOR(device_number), master->index), NULL,
fp@1369: "EtherCAT%u", master->index);
fp@1015: #endif
fp@1013: if (IS_ERR(master->class_device)) {
fp@1013: EC_ERR("Failed to create class device!\n");
fp@1313: ret = PTR_ERR(master->class_device);
fp@1013: goto out_clear_cdev;
fp@1013: }
fp@144:
fp@178: return 0;
fp@226:
fp@1013: out_clear_cdev:
fp@1013: ec_cdev_clear(&master->cdev);
fp@1535: out_clear_sync_mon:
fp@1535: ec_datagram_clear(&master->sync_mon_datagram);
fp@1394: out_clear_sync:
fp@1394: ec_datagram_clear(&master->sync_datagram);
fp@1396: out_clear_ref_sync:
fp@1396: ec_datagram_clear(&master->ref_sync_datagram);
fp@659: out_clear_fsm:
fp@659: ec_fsm_master_clear(&master->fsm);
fp@997: ec_datagram_clear(&master->fsm_datagram);
fp@659: out_clear_backup:
fp@579: ec_device_clear(&master->backup_device);
fp@579: out_clear_main:
fp@579: ec_device_clear(&master->main_device);
fp@579: out_return:
fp@1313: return ret;
fp@0: }
fp@0:
fp@39: /*****************************************************************************/
fp@0:
fp@997: /** Destructor.
fp@195: */
fp@639: void ec_master_clear(
fp@639: ec_master_t *master /**< EtherCAT master */
fp@639: )
fp@639: {
fp@1250: #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26)
fp@1250: device_unregister(master->class_device);
fp@1250: #else
fp@1013: class_device_unregister(master->class_device);
fp@1250: #endif
fp@1394:
fp@995: ec_cdev_clear(&master->cdev);
fp@1394:
fp@715: #ifdef EC_EOE
fp@661: ec_master_clear_eoe_handlers(master);
fp@715: #endif
fp@994: ec_master_clear_domains(master);
fp@995: ec_master_clear_slave_configs(master);
fp@992: ec_master_clear_slaves(master);
fp@1394:
fp@1535: ec_datagram_clear(&master->sync_mon_datagram);
fp@1394: ec_datagram_clear(&master->sync_datagram);
fp@1396: ec_datagram_clear(&master->ref_sync_datagram);
fp@528: ec_fsm_master_clear(&master->fsm);
fp@528: ec_datagram_clear(&master->fsm_datagram);
fp@661: ec_device_clear(&master->backup_device);
fp@661: ec_device_clear(&master->main_device);
fp@661: }
fp@661:
fp@661: /*****************************************************************************/
fp@661:
fp@715: #ifdef EC_EOE
fp@792: /** Clear and free all EoE handlers.
fp@661: */
fp@661: void ec_master_clear_eoe_handlers(
fp@661: ec_master_t *master /**< EtherCAT master */
fp@661: )
fp@661: {
fp@661: ec_eoe_t *eoe, *next;
fp@661:
fp@661: list_for_each_entry_safe(eoe, next, &master->eoe_handlers, list) {
fp@251: list_del(&eoe->list);
fp@251: ec_eoe_clear(eoe);
fp@251: kfree(eoe);
fp@251: }
fp@73: }
fp@715: #endif
fp@73:
fp@73: /*****************************************************************************/
fp@73:
fp@995: /** Clear all slave configurations.
fp@792: */
fp@995: void ec_master_clear_slave_configs(ec_master_t *master)
fp@792: {
fp@792: ec_slave_config_t *sc, *next;
fp@792:
fp@792: list_for_each_entry_safe(sc, next, &master->configs, list) {
fp@792: list_del(&sc->list);
fp@995: ec_slave_config_clear(sc);
fp@995: kfree(sc);
fp@792: }
fp@792: }
fp@792:
fp@792: /*****************************************************************************/
fp@792:
fp@997: /** Clear all slaves.
fp@792: */
fp@992: void ec_master_clear_slaves(ec_master_t *master)
fp@238: {
fp@1000: ec_slave_t *slave;
fp@1000:
fp@1425: master->dc_ref_clock = NULL;
fp@1425:
fp@1538: // external requests are obsolete, so we wake pending waiters and remove
fp@1538: // them from the list
fp@1538: //
fp@1773: // SII requests
fp@1773: while (1) {
fp@1773: ec_sii_write_request_t *request;
fp@1773: if (list_empty(&master->sii_requests))
fp@1773: break;
fp@1773: // get first request
fp@1538: request = list_entry(master->sii_requests.next,
fp@1538: ec_sii_write_request_t, list);
fp@1773: list_del_init(&request->list); // dequeue
fp@1773: EC_INFO("Discarding SII request, slave %u does not exist anymore.\n",
fp@1773: request->slave->ring_position);
fp@1773: request->state = EC_INT_REQUEST_FAILURE;
fp@1773: wake_up(&master->sii_queue);
fp@1773: }
fp@1773:
fp@1773: // Register requests
fp@1773: while (1) {
fp@1773: ec_reg_request_t *request;
fp@1773: if (list_empty(&master->reg_requests))
fp@1773: break;
fp@1773: // get first request
fp@1773: request = list_entry(master->reg_requests.next,
fp@1773: ec_reg_request_t, list);
fp@1773: list_del_init(&request->list); // dequeue
fp@1773: EC_INFO("Discarding Reg request, slave %u does not exist anymore.\n",
fp@1773: request->slave->ring_position);
fp@1773: request->state = EC_INT_REQUEST_FAILURE;
fp@1773: wake_up(&master->reg_queue);
fp@1773: }
fp@1538:
fp@1000: for (slave = master->slaves;
fp@1000: slave < master->slaves + master->slave_count;
fp@1000: slave++) {
martin@1596: // SDO requests
martin@1596: while (1) {
martin@1596: ec_master_sdo_request_t *request;
martin@1596: if (list_empty(&slave->slave_sdo_requests))
martin@1596: break;
martin@1596: // get first request
martin@1596: request = list_entry(slave->slave_sdo_requests.next,
martin@1596: ec_master_sdo_request_t, list);
martin@1596: list_del_init(&request->list); // dequeue
fp@1831: EC_INFO("Discarding SDO request,"
fp@1831: " slave %u does not exist anymore.\n",
martin@1597: slave->ring_position);
martin@1596: request->req.state = EC_INT_REQUEST_FAILURE;
martin@1596: wake_up(&slave->sdo_queue);
martin@1596: }
martin@1597: // FoE requests
martin@1597: while (1) {
martin@1597: ec_master_foe_request_t *request;
martin@1597: if (list_empty(&slave->foe_requests))
martin@1597: break;
martin@1597: // get first request
martin@1597: request = list_entry(slave->foe_requests.next,
martin@1597: ec_master_foe_request_t, list);
martin@1597: list_del_init(&request->list); // dequeue
fp@1831: EC_INFO("Discarding FoE request,"
fp@1831: " slave %u does not exist anymore.\n",
martin@1597: slave->ring_position);
martin@1597: request->req.state = EC_INT_REQUEST_FAILURE;
martin@1597: wake_up(&slave->foe_queue);
martin@1597: }
fp@1831: // SoE requests
fp@1831: while (1) {
fp@1831: ec_master_soe_request_t *request;
fp@1831: if (list_empty(&slave->soe_requests))
fp@1831: break;
fp@1831: // get first request
fp@1831: request = list_entry(slave->soe_requests.next,
fp@1831: ec_master_soe_request_t, list);
fp@1831: list_del_init(&request->list); // dequeue
fp@1831: EC_INFO("Discarding SoE request,"
fp@1831: " slave %u does not exist anymore.\n",
fp@1831: slave->ring_position);
fp@1831: request->req.state = EC_INT_REQUEST_FAILURE;
fp@1831: wake_up(&slave->soe_queue);
fp@1831: }
fp@992: ec_slave_clear(slave);
fp@1004: }
fp@1004:
fp@1004: if (master->slaves) {
fp@1000: kfree(master->slaves);
fp@1000: master->slaves = NULL;
fp@1004: }
fp@1004:
fp@1004: master->slave_count = 0;
fp@74: }
fp@74:
fp@74: /*****************************************************************************/
fp@74:
fp@997: /** Clear all domains.
fp@997: */
fp@993: void ec_master_clear_domains(ec_master_t *master)
fp@448: {
fp@792: ec_domain_t *domain, *next;
fp@792:
fp@792: list_for_each_entry_safe(domain, next, &master->domains, list) {
fp@448: list_del(&domain->list);
fp@993: ec_domain_clear(domain);
fp@993: kfree(domain);
fp@448: }
fp@448: }
fp@448:
fp@448: /*****************************************************************************/
fp@448:
fp@1500: /** Internal sending callback.
fp@1500: */
fp@1500: void ec_master_internal_send_cb(
fp@1513: void *cb_data /**< Callback data. */
fp@1513: )
fp@1513: {
fp@1513: ec_master_t *master = (ec_master_t *) cb_data;
fp@1489: down(&master->io_sem);
fp@1500: ecrt_master_send_ext(master);
fp@1500: up(&master->io_sem);
fp@1500: }
fp@1500:
fp@1500: /*****************************************************************************/
fp@1500:
fp@1500: /** Internal receiving callback.
fp@1500: */
fp@1500: void ec_master_internal_receive_cb(
fp@1513: void *cb_data /**< Callback data. */
fp@1513: )
fp@1513: {
fp@1513: ec_master_t *master = (ec_master_t *) cb_data;
fp@1500: down(&master->io_sem);
fp@1500: ecrt_master_receive(master);
fp@1489: up(&master->io_sem);
fp@515: }
fp@515:
fp@515: /*****************************************************************************/
fp@515:
fp@997: /** Starts the master thread.
fp@1313: *
fp@1313: * \retval 0 Success.
fp@1313: * \retval <0 Error code.
fp@997: */
fp@656: int ec_master_thread_start(
fp@656: ec_master_t *master, /**< EtherCAT master */
fp@1241: int (*thread_func)(void *), /**< thread function to start */
fp@1241: const char *name /**< Thread name. */
fp@656: )
fp@656: {
fp@1241: EC_INFO("Starting %s thread.\n", name);
fp@1241: master->thread = kthread_run(thread_func, master, name);
fp@1241: if (IS_ERR(master->thread)) {
fp@1313: int err = (int) PTR_ERR(master->thread);
fp@1313: EC_ERR("Failed to start master thread (error %i)!\n", err);
fp@1241: master->thread = NULL;
fp@1313: return err;
fp@1241: }
fp@525:
fp@525: return 0;
fp@525: }
fp@525:
fp@525: /*****************************************************************************/
fp@525:
fp@997: /** Stops the master thread.
fp@997: */
fp@1029: void ec_master_thread_stop(
fp@1029: ec_master_t *master /**< EtherCAT master */
fp@1029: )
fp@525: {
fp@1040: unsigned long sleep_jiffies;
fp@1040:
fp@1241: if (!master->thread) {
fp@1241: EC_WARN("ec_master_thread_stop(): Already finished!\n");
fp@656: return;
fp@656: }
fp@656:
fp@1006: if (master->debug_level)
fp@1006: EC_DBG("Stopping master thread.\n");
fp@1006:
fp@1241: kthread_stop(master->thread);
fp@1241: master->thread = NULL;
fp@656: EC_INFO("Master thread exited.\n");
fp@656:
fp@1198: if (master->fsm_datagram.state != EC_DATAGRAM_SENT)
fp@1198: return;
fp@656:
fp@656: // wait for FSM datagram
fp@1040: sleep_jiffies = max(HZ / 100, 1); // 10 ms, at least 1 jiffy
fp@1040: schedule_timeout(sleep_jiffies);
fp@525: }
fp@525:
fp@525: /*****************************************************************************/
fp@525:
fp@1029: /** Transition function from ORPHANED to IDLE phase.
fp@1029: */
fp@1029: int ec_master_enter_idle_phase(
fp@1029: ec_master_t *master /**< EtherCAT master */
fp@1029: )
fp@446: {
fp@1313: int ret;
fp@1313:
fp@1075: if (master->debug_level)
fp@1075: EC_DBG("ORPHANED -> IDLE.\n");
fp@1075:
fp@1500: master->send_cb = ec_master_internal_send_cb;
fp@1500: master->receive_cb = ec_master_internal_receive_cb;
fp@1513: master->cb_data = master;
fp@637:
fp@1029: master->phase = EC_IDLE;
fp@1313: ret = ec_master_thread_start(master, ec_master_idle_thread,
fp@1313: "EtherCAT-IDLE");
fp@1313: if (ret)
fp@1029: master->phase = EC_ORPHANED;
fp@1313:
fp@1313: return ret;
fp@446: }
fp@446:
fp@446: /*****************************************************************************/
fp@446:
fp@1029: /** Transition function from IDLE to ORPHANED phase.
fp@1029: */
fp@1029: void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */)
fp@446: {
fp@1006: if (master->debug_level)
fp@1006: EC_DBG("IDLE -> ORPHANED.\n");
fp@1006:
fp@1029: master->phase = EC_ORPHANED;
fp@525:
fp@715: #ifdef EC_EOE
fp@446: ec_master_eoe_stop(master);
fp@715: #endif
fp@525: ec_master_thread_stop(master);
fp@1075:
fp@1075: down(&master->master_sem);
fp@992: ec_master_clear_slaves(master);
fp@1075: up(&master->master_sem);
fp@446: }
fp@446:
fp@446: /*****************************************************************************/
fp@446:
fp@1029: /** Transition function from IDLE to OPERATION phase.
fp@1029: */
fp@1029: int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */)
fp@446: {
fp@1313: int ret = 0;
fp@446: ec_slave_t *slave;
fp@715: #ifdef EC_EOE
fp@661: ec_eoe_t *eoe;
fp@715: #endif
fp@446:
fp@1006: if (master->debug_level)
fp@1006: EC_DBG("IDLE -> OPERATION.\n");
fp@1006:
fp@656: down(&master->config_sem);
fp@656: master->allow_config = 0; // temporarily disable slave configuration
fp@900: if (master->config_busy) {
fp@900: up(&master->config_sem);
fp@900:
fp@900: // wait for slave configuration to complete
fp@1313: ret = wait_event_interruptible(master->config_queue,
fp@1313: !master->config_busy);
fp@1313: if (ret) {
fp@900: EC_INFO("Finishing slave configuration interrupted by signal.\n");
fp@900: goto out_allow;
fp@900: }
fp@900:
fp@900: if (master->debug_level)
fp@900: EC_DBG("Waiting for pending slave configuration returned.\n");
fp@900: } else {
fp@900: up(&master->config_sem);
fp@900: }
fp@900:
fp@656: down(&master->scan_sem);
fp@656: master->allow_scan = 0; // 'lock' the slave list
fp@900: if (!master->scan_busy) {
fp@900: up(&master->scan_sem);
fp@900: } else {
fp@900: up(&master->scan_sem);
fp@900:
fp@794: // wait for slave scan to complete
fp@1313: ret = wait_event_interruptible(master->scan_queue, !master->scan_busy);
fp@1313: if (ret) {
fp@794: EC_INFO("Waiting for slave scan interrupted by signal.\n");
fp@794: goto out_allow;
fp@794: }
fp@900:
fp@900: if (master->debug_level)
fp@900: EC_DBG("Waiting for pending slave scan returned.\n");
fp@900: }
fp@656:
fp@656: // set states for all slaves
fp@1000: for (slave = master->slaves;
fp@1000: slave < master->slaves + master->slave_count;
fp@1000: slave++) {
fp@643: ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
fp@446: }
fp@1451:
fp@715: #ifdef EC_EOE
fp@661: // ... but set EoE slaves to OP
fp@661: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@661: if (ec_eoe_is_open(eoe))
fp@661: ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
fp@661: }
fp@715: #endif
fp@446:
fp@1029: master->phase = EC_OPERATION;
fp@1500: master->app_send_cb = NULL;
fp@1500: master->app_receive_cb = NULL;
fp@1513: master->app_cb_data = NULL;
fp@1313: return ret;
fp@656:
fp@656: out_allow:
fp@656: master->allow_scan = 1;
fp@656: master->allow_config = 1;
fp@1313: return ret;
fp@446: }
fp@446:
fp@446: /*****************************************************************************/
fp@446:
fp@1029: /** Transition function from OPERATION to IDLE phase.
fp@1029: */
fp@1530: void ec_master_leave_operation_phase(
fp@1530: ec_master_t *master /**< EtherCAT master */
fp@1530: )
fp@1530: {
fp@1530: if (master->active)
fp@1530: ecrt_master_deactivate(master);
fp@656:
fp@1006: if (master->debug_level)
fp@1006: EC_DBG("OPERATION -> IDLE.\n");
fp@1006:
fp@1029: master->phase = EC_IDLE;
fp@446: }
fp@446:
martin@1583:
martin@1583: /*****************************************************************************/
martin@1583:
fp@1774: /** Injects external datagrams that fit into the datagram queue.
martin@1597: */
martin@1597: void ec_master_inject_external_datagrams(
fp@1773: ec_master_t *master /**< EtherCAT master */
fp@1773: )
fp@1773: {
fp@1773: ec_datagram_t *datagram, *n;
fp@1773: size_t queue_size = 0;
fp@1774:
fp@1773: list_for_each_entry(datagram, &master->datagram_queue, queue) {
fp@1773: queue_size += datagram->data_size;
fp@1773: }
fp@1774:
fp@1774: list_for_each_entry_safe(datagram, n, &master->external_datagram_queue,
fp@1774: queue) {
fp@1773: queue_size += datagram->data_size;
fp@1773: if (queue_size <= master->max_queue_size) {
fp@1773: list_del_init(&datagram->queue);
martin@1597: #if DEBUG_INJECT
fp@1773: if (master->debug_level) {
fp@1773: EC_DBG("Injecting external datagram %08x size=%u,"
fp@1773: " queue_size=%u\n", (unsigned int) datagram,
fp@1773: datagram->data_size, queue_size);
fp@1773: }
martin@1597: #endif
martin@1585: #ifdef EC_HAVE_CYCLES
fp@1773: datagram->cycles_sent = 0;
fp@1773: #endif
fp@1773: datagram->jiffies_sent = 0;
fp@1773: ec_master_queue_datagram(master, datagram);
fp@1773: }
fp@1773: else {
fp@1773: if (datagram->data_size > master->max_queue_size) {
fp@1773: list_del_init(&datagram->queue);
fp@1773: datagram->state = EC_DATAGRAM_ERROR;
fp@1774: EC_ERR("External datagram %p is too large,"
fp@1774: " size=%u, max_queue_size=%u\n",
fp@1774: datagram, datagram->data_size,
fp@1774: master->max_queue_size);
fp@1774: } else {
martin@1585: #ifdef EC_HAVE_CYCLES
fp@1773: cycles_t cycles_now = get_cycles();
fp@1774:
fp@1773: if (cycles_now - datagram->cycles_sent
fp@1774: > sdo_injection_timeout_cycles)
martin@1585: #else
fp@1774: if (jiffies - datagram->jiffies_sent
fp@1774: > sdo_injection_timeout_jiffies)
fp@1774: #endif
fp@1774: {
fp@1774: unsigned int time_us;
fp@1774:
fp@1774: list_del_init(&datagram->queue);
fp@1774: datagram->state = EC_DATAGRAM_ERROR;
martin@1585: #ifdef EC_HAVE_CYCLES
fp@1774: time_us = (unsigned int)
fp@1774: ((cycles_now - datagram->cycles_sent) * 1000LL)
fp@1774: / cpu_khz;
martin@1585: #else
fp@1774: time_us = (unsigned int)
fp@1774: ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
fp@1774: #endif
fp@1774: EC_ERR("Timeout %u us: injecting external datagram %p"
fp@1774: " size=%u, max_queue_size=%u\n",
fp@1774: time_us, datagram,
fp@1774: datagram->data_size, master->max_queue_size);
fp@1774: }
martin@1597: #if DEBUG_INJECT
fp@1774: else if (master->debug_level) {
fp@1774: EC_DBG("Deferred injecting of external datagram %p"
fp@1774: " size=%u, queue_size=%u\n",
fp@1774: datagram, datagram->data_size, queue_size);
fp@1773: }
fp@1774: #endif
fp@1773: }
fp@1773: }
fp@1773: }
fp@1774: }
fp@1774:
fp@1774: /*****************************************************************************/
fp@1774:
fp@1774: /** Sets the expected interval between calls to ecrt_master_send
fp@1774: * and calculates the maximum amount of data to queue.
fp@1774: */
fp@1774: void ec_master_set_send_interval(
fp@1774: ec_master_t *master, /**< EtherCAT master */
fp@1774: size_t send_interval /**< Send interval */
fp@1774: )
fp@1774: {
fp@1774: master->send_interval = send_interval;
fp@1774: master->max_queue_size =
fp@1774: (send_interval * 1000) / EC_BYTE_TRANSMISSION_TIME_NS;
fp@1774: master->max_queue_size -= master->max_queue_size / 10;
fp@1774: }
martin@1583:
martin@1583: /*****************************************************************************/
martin@1583:
martin@1597: /** Places an external datagram in the sdo datagram queue.
martin@1597: */
martin@1597: void ec_master_queue_external_datagram(
martin@1583: ec_master_t *master, /**< EtherCAT master */
martin@1583: ec_datagram_t *datagram /**< datagram */
martin@1583: )
martin@1583: {
fp@1804: ec_datagram_t *queued_datagram;
martin@1600:
martin@1600: down(&master->io_sem);
fp@1774:
fp@1804: // check, if the datagram is already queued
fp@1804: list_for_each_entry(queued_datagram, &master->external_datagram_queue,
fp@1774: queue) {
fp@1804: if (queued_datagram == datagram) {
fp@1804: datagram->state = EC_DATAGRAM_QUEUED;
fp@1804: return;
fp@1804: }
fp@1804: }
fp@1774:
martin@1597: #if DEBUG_INJECT
fp@1804: if (master->debug_level) {
fp@1804: EC_DBG("Requesting external datagram %p size=%u\n",
fp@1774: datagram, datagram->data_size);
fp@1804: }
fp@1804: #endif
fp@1804:
fp@1804: list_add_tail(&datagram->queue, &master->external_datagram_queue);
fp@1804: datagram->state = EC_DATAGRAM_QUEUED;
martin@1585: #ifdef EC_HAVE_CYCLES
fp@1804: datagram->cycles_sent = get_cycles();
fp@1804: #endif
fp@1804: datagram->jiffies_sent = jiffies;
fp@1804:
fp@1804: master->fsm.idle = 0;
fp@1804: up(&master->io_sem);
martin@1583: }
martin@1583:
fp@446: /*****************************************************************************/
fp@446:
fp@997: /** Places a datagram in the datagram queue.
fp@997: */
fp@1500: void ec_master_queue_datagram(
fp@1500: ec_master_t *master, /**< EtherCAT master */
fp@1500: ec_datagram_t *datagram /**< datagram */
fp@1500: )
fp@293: {
fp@293: ec_datagram_t *queued_datagram;
fp@293:
martin@1583: if (datagram->state == EC_DATAGRAM_SENT)
martin@1583: return;
fp@293: // check, if the datagram is already queued
fp@293: list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
fp@293: if (queued_datagram == datagram) {
fp@719: datagram->skip_count++;
fp@637: if (master->debug_level)
fp@1543: EC_DBG("skipping datagram %p.\n", datagram);
fp@325: datagram->state = EC_DATAGRAM_QUEUED;
fp@98: return;
fp@98: }
fp@98: }
fp@98:
fp@293: list_add_tail(&datagram->queue, &master->datagram_queue);
fp@325: datagram->state = EC_DATAGRAM_QUEUED;
fp@293: }
fp@293:
fp@293: /*****************************************************************************/
fp@293:
fp@1500: /** Places a datagram in the non-application datagram queue.
fp@1500: */
fp@1500: void ec_master_queue_datagram_ext(
fp@1500: ec_master_t *master, /**< EtherCAT master */
fp@1500: ec_datagram_t *datagram /**< datagram */
fp@1500: )
fp@1500: {
fp@1500: down(&master->ext_queue_sem);
fp@1500: list_add_tail(&datagram->queue, &master->ext_datagram_queue);
fp@1500: up(&master->ext_queue_sem);
fp@1500: }
fp@1500:
fp@1500: /*****************************************************************************/
fp@1500:
fp@997: /** Sends the datagrams in the queue.
fp@997: *
fp@997: */
fp@293: void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
fp@293: {
fp@398: ec_datagram_t *datagram, *next;
fp@1804: size_t datagram_size;
fp@98: uint8_t *frame_data, *cur_data;
fp@98: void *follows_word;
fp@1040: #ifdef EC_HAVE_CYCLES
fp@398: cycles_t cycles_start, cycles_sent, cycles_end;
fp@1040: #endif
fp@398: unsigned long jiffies_sent;
fp@293: unsigned int frame_count, more_datagrams_waiting;
fp@398: struct list_head sent_datagrams;
fp@398:
fp@1040: #ifdef EC_HAVE_CYCLES
fp@398: cycles_start = get_cycles();
fp@1040: #endif
fp@176: frame_count = 0;
fp@398: INIT_LIST_HEAD(&sent_datagrams);
fp@208:
fp@303: if (unlikely(master->debug_level > 1))
fp@293: EC_DBG("ec_master_send_datagrams\n");
fp@98:
fp@176: do {
fp@195: // fetch pointer to transmit socket buffer
fp@579: frame_data = ec_device_tx_data(&master->main_device);
fp@176: cur_data = frame_data + EC_FRAME_HEADER_SIZE;
fp@176: follows_word = NULL;
fp@293: more_datagrams_waiting = 0;
fp@293:
fp@293: // fill current frame with datagrams
fp@293: list_for_each_entry(datagram, &master->datagram_queue, queue) {
fp@325: if (datagram->state != EC_DATAGRAM_QUEUED) continue;
fp@293:
fp@293: // does the current datagram fit in the frame?
fp@293: datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
fp@293: + EC_DATAGRAM_FOOTER_SIZE;
fp@293: if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) {
fp@293: more_datagrams_waiting = 1;
fp@176: break;
fp@176: }
fp@176:
fp@398: list_add_tail(&datagram->sent, &sent_datagrams);
fp@293: datagram->index = master->datagram_index++;
fp@176:
fp@303: if (unlikely(master->debug_level > 1))
fp@293: EC_DBG("adding datagram 0x%02X\n", datagram->index);
fp@293:
fp@293: // set "datagram following" flag in previous frame
fp@176: if (follows_word)
fp@176: EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
fp@176:
fp@293: // EtherCAT datagram header
fp@293: EC_WRITE_U8 (cur_data, datagram->type);
fp@293: EC_WRITE_U8 (cur_data + 1, datagram->index);
fp@708: memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
fp@293: EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
fp@176: EC_WRITE_U16(cur_data + 8, 0x0000);
fp@176: follows_word = cur_data + 6;
fp@293: cur_data += EC_DATAGRAM_HEADER_SIZE;
fp@293:
fp@293: // EtherCAT datagram data
fp@293: memcpy(cur_data, datagram->data, datagram->data_size);
fp@293: cur_data += datagram->data_size;
fp@293:
fp@293: // EtherCAT datagram footer
fp@195: EC_WRITE_U16(cur_data, 0x0000); // reset working counter
fp@293: cur_data += EC_DATAGRAM_FOOTER_SIZE;
fp@176: }
fp@176:
fp@398: if (list_empty(&sent_datagrams)) {
fp@303: if (unlikely(master->debug_level > 1))
fp@176: EC_DBG("nothing to send.\n");
fp@176: break;
fp@176: }
fp@176:
fp@176: // EtherCAT frame header
fp@176: EC_WRITE_U16(frame_data, ((cur_data - frame_data
fp@176: - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);
fp@176:
fp@195: // pad frame
fp@211: while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN)
fp@176: EC_WRITE_U8(cur_data++, 0x00);
fp@98:
fp@303: if (unlikely(master->debug_level > 1))
fp@1543: EC_DBG("frame size: %zu\n", cur_data - frame_data);
fp@176:
fp@195: // send frame
fp@579: ec_device_send(&master->main_device, cur_data - frame_data);
fp@1040: #ifdef EC_HAVE_CYCLES
fp@398: cycles_sent = get_cycles();
fp@1040: #endif
fp@398: jiffies_sent = jiffies;
fp@398:
fp@398: // set datagram states and sending timestamps
fp@398: list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) {
fp@398: datagram->state = EC_DATAGRAM_SENT;
fp@1040: #ifdef EC_HAVE_CYCLES
fp@398: datagram->cycles_sent = cycles_sent;
fp@1040: #endif
fp@398: datagram->jiffies_sent = jiffies_sent;
fp@398: list_del_init(&datagram->sent); // empty list of sent datagrams
fp@398: }
fp@398:
fp@176: frame_count++;
fp@176: }
fp@293: while (more_datagrams_waiting);
fp@98:
fp@1040: #ifdef EC_HAVE_CYCLES
fp@303: if (unlikely(master->debug_level > 1)) {
fp@344: cycles_end = get_cycles();
fp@986: EC_DBG("ec_master_send_datagrams sent %u frames in %uus.\n",
fp@344: frame_count,
fp@344: (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
fp@176: }
fp@1040: #endif
fp@176: }
fp@176:
fp@176: /*****************************************************************************/
fp@176:
fp@997: /** Processes a received frame.
fp@997: *
fp@997: * This function is called by the network driver for every received frame.
fp@997: *
fp@997: * \return 0 in case of success, else < 0
fp@997: */
fp@331: void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */
fp@331: const uint8_t *frame_data, /**< frame data */
fp@331: size_t size /**< size of the received data */
fp@331: )
fp@98: {
fp@98: size_t frame_size, data_size;
fp@293: uint8_t datagram_type, datagram_index;
fp@98: unsigned int cmd_follows, matched;
fp@98: const uint8_t *cur_data;
fp@293: ec_datagram_t *datagram;
fp@98:
fp@98: if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
fp@1303: if (master->debug_level) {
fp@1543: EC_DBG("Corrupted frame received (size %zu < %u byte):\n",
fp@1303: size, EC_FRAME_HEADER_SIZE);
fp@1303: ec_print_data(frame_data, size);
fp@1303: }
fp@98: master->stats.corrupted++;
fp@98: ec_master_output_stats(master);
fp@98: return;
fp@98: }
fp@98:
fp@98: cur_data = frame_data;
fp@98:
fp@195: // check length of entire frame
fp@98: frame_size = EC_READ_U16(cur_data) & 0x07FF;
fp@98: cur_data += EC_FRAME_HEADER_SIZE;
fp@98:
fp@98: if (unlikely(frame_size > size)) {
fp@1303: if (master->debug_level) {
fp@1543: EC_DBG("Corrupted frame received (invalid frame size %zu for "
fp@1543: "received size %zu):\n", frame_size, size);
fp@1303: ec_print_data(frame_data, size);
fp@1303: }
fp@98: master->stats.corrupted++;
fp@98: ec_master_output_stats(master);
fp@98: return;
fp@98: }
fp@98:
fp@98: cmd_follows = 1;
fp@98: while (cmd_follows) {
fp@293: // process datagram header
fp@293: datagram_type = EC_READ_U8 (cur_data);
fp@293: datagram_index = EC_READ_U8 (cur_data + 1);
fp@293: data_size = EC_READ_U16(cur_data + 6) & 0x07FF;
fp@293: cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000;
fp@293: cur_data += EC_DATAGRAM_HEADER_SIZE;
fp@98:
fp@98: if (unlikely(cur_data - frame_data
fp@293: + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) {
fp@1303: if (master->debug_level) {
fp@1543: EC_DBG("Corrupted frame received (invalid data size %zu):\n",
fp@1303: data_size);
fp@1303: ec_print_data(frame_data, size);
fp@1303: }
fp@98: master->stats.corrupted++;
fp@98: ec_master_output_stats(master);
fp@98: return;
fp@98: }
fp@98:
fp@293: // search for matching datagram in the queue
fp@98: matched = 0;
fp@293: list_for_each_entry(datagram, &master->datagram_queue, queue) {
fp@690: if (datagram->index == datagram_index
fp@690: && datagram->state == EC_DATAGRAM_SENT
fp@293: && datagram->type == datagram_type
fp@293: && datagram->data_size == data_size) {
fp@98: matched = 1;
fp@98: break;
fp@98: }
fp@98: }
fp@98:
fp@293: // no matching datagram was found
fp@98: if (!matched) {
fp@98: master->stats.unmatched++;
fp@98: ec_master_output_stats(master);
fp@684:
fp@684: if (unlikely(master->debug_level > 0)) {
fp@684: EC_DBG("UNMATCHED datagram:\n");
fp@684: ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE,
fp@684: EC_DATAGRAM_HEADER_SIZE + data_size
fp@684: + EC_DATAGRAM_FOOTER_SIZE);
fp@692: #ifdef EC_DEBUG_RING
fp@692: ec_device_debug_ring_print(&master->main_device);
fp@692: #endif
fp@684: }
fp@684:
fp@293: cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
fp@98: continue;
fp@98: }
fp@98:
fp@345: // copy received data into the datagram memory
fp@293: memcpy(datagram->data, cur_data, data_size);
fp@98: cur_data += data_size;
fp@98:
fp@293: // set the datagram's working counter
fp@293: datagram->working_counter = EC_READ_U16(cur_data);
fp@293: cur_data += EC_DATAGRAM_FOOTER_SIZE;
fp@293:
fp@293: // dequeue the received datagram
fp@325: datagram->state = EC_DATAGRAM_RECEIVED;
fp@1040: #ifdef EC_HAVE_CYCLES
fp@579: datagram->cycles_received = master->main_device.cycles_poll;
fp@1040: #endif
fp@579: datagram->jiffies_received = master->main_device.jiffies_poll;
fp@293: list_del_init(&datagram->queue);
fp@293: }
fp@293: }
fp@293:
fp@293: /*****************************************************************************/
fp@293:
fp@1029: /** Output master statistics.
fp@997: *
fp@997: * This function outputs statistical data on demand, but not more often than
fp@997: * necessary. The output happens at most once a second.
fp@997: */
fp@195: void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */)
fp@98: {
fp@344: if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) {
fp@344: master->stats.output_jiffies = jiffies;
fp@344:
fp@98: if (master->stats.timeouts) {
fp@986: EC_WARN("%u datagram%s TIMED OUT!\n", master->stats.timeouts,
fp@396: master->stats.timeouts == 1 ? "" : "s");
fp@98: master->stats.timeouts = 0;
fp@98: }
fp@98: if (master->stats.corrupted) {
fp@986: EC_WARN("%u frame%s CORRUPTED!\n", master->stats.corrupted,
fp@396: master->stats.corrupted == 1 ? "" : "s");
fp@98: master->stats.corrupted = 0;
fp@98: }
fp@98: if (master->stats.unmatched) {
fp@986: EC_WARN("%u datagram%s UNMATCHED!\n", master->stats.unmatched,
fp@396: master->stats.unmatched == 1 ? "" : "s");
fp@98: master->stats.unmatched = 0;
fp@98: }
fp@73: }
fp@54: }
fp@54:
martin@1600:
martin@1600: /*****************************************************************************/
fp@1773:
fp@1773: #ifdef EC_USE_HRTIMER
fp@1773:
martin@1600: /*
martin@1600: * Sleep related functions:
martin@1600: */
martin@1600: static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer)
martin@1600: {
fp@1773: struct hrtimer_sleeper *t =
fp@1773: container_of(timer, struct hrtimer_sleeper, timer);
fp@1773: struct task_struct *task = t->task;
fp@1773:
fp@1773: t->task = NULL;
fp@1773: if (task)
fp@1773: wake_up_process(task);
fp@1773:
fp@1773: return HRTIMER_NORESTART;
fp@1773: }
fp@1773:
fp@1773: /*****************************************************************************/
martin@1600:
ch1010472@1603: #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
fp@1773:
ch1010472@1603: /* compatibility with new hrtimer interface */
ch1010472@1603: static inline ktime_t hrtimer_get_expires(const struct hrtimer *timer)
ch1010472@1603: {
fp@1773: return timer->expires;
fp@1773: }
fp@1773:
fp@1773: /*****************************************************************************/
ch1010472@1603:
ch1010472@1603: static inline void hrtimer_set_expires(struct hrtimer *timer, ktime_t time)
ch1010472@1603: {
fp@1773: timer->expires = time;
fp@1773: }
fp@1773:
fp@1773: #endif
fp@1773:
fp@1773: /*****************************************************************************/
ch1010472@1603:
martin@1600: void ec_master_nanosleep(const unsigned long nsecs)
martin@1600: {
fp@1773: struct hrtimer_sleeper t;
fp@1773: enum hrtimer_mode mode = HRTIMER_MODE_REL;
fp@1773:
fp@1773: hrtimer_init(&t.timer, CLOCK_MONOTONIC, mode);
fp@1773: t.timer.function = ec_master_nanosleep_wakeup;
fp@1773: t.task = current;
martin@1600: #ifdef CONFIG_HIGH_RES_TIMERS
ch1010472@1603: #if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 24)
fp@1773: t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART;
ch1010472@1603: #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 26)
fp@1773: t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_SOFTIRQ;
ch1010472@1603: #elif LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 28)
fp@1773: t.timer.cb_mode = HRTIMER_CB_IRQSAFE_UNLOCKED;
fp@1773: #endif
fp@1773: #endif
fp@1773: hrtimer_set_expires(&t.timer, ktime_set(0, nsecs));
fp@1773:
fp@1773: do {
fp@1773: set_current_state(TASK_INTERRUPTIBLE);
fp@1773: hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode);
fp@1773:
fp@1773: if (likely(t.task))
fp@1773: schedule();
fp@1773:
fp@1773: hrtimer_cancel(&t.timer);
fp@1773: mode = HRTIMER_MODE_ABS;
fp@1773:
fp@1773: } while (t.task && !signal_pending(current));
fp@1773: }
fp@1773:
fp@1773: #endif // EC_USE_HRTIMER
martin@1600:
fp@68: /*****************************************************************************/
fp@68:
fp@1029: /** Master kernel thread function for IDLE phase.
fp@997: */
fp@1241: static int ec_master_idle_thread(void *priv_data)
fp@1241: {
fp@1241: ec_master_t *master = (ec_master_t *) priv_data;
martin@1583: ec_slave_t *slave = NULL;
martin@1583: int fsm_exec;
fp@1804: size_t sent_bytes;
fp@1774:
fp@1774: // send interval in IDLE phase
fp@1804: ec_master_set_send_interval(master, 1000000 / HZ);
fp@1804:
fp@1804: if (master->debug_level)
fp@1804: EC_DBG("Idle thread running with send interval = %d us,"
fp@1774: " max data size=%d\n", master->send_interval,
fp@1774: master->max_queue_size);
fp@1241:
fp@1241: while (!kthread_should_stop()) {
fp@719: ec_datagram_output_stats(&master->fsm_datagram);
fp@525:
fp@1031: // receive
fp@1489: down(&master->io_sem);
fp@1031: ecrt_master_receive(master);
fp@1489: up(&master->io_sem);
fp@1031:
martin@1583: fsm_exec = 0;
martin@1583: // execute master & slave state machines
fp@1241: if (down_interruptible(&master->master_sem))
fp@1241: break;
martin@1583: fsm_exec = ec_fsm_master_exec(&master->fsm);
martin@1583: for (slave = master->slaves;
martin@1583: slave < master->slaves + master->slave_count;
martin@1583: slave++) {
martin@1583: ec_fsm_slave_exec(&slave->fsm);
martin@1583: }
fp@1075: up(&master->master_sem);
fp@1031:
fp@1031: // queue and send
fp@1489: down(&master->io_sem);
martin@1583: if (fsm_exec) {
martin@1583: ec_master_queue_datagram(master, &master->fsm_datagram);
martin@1583: }
martin@1597: ec_master_inject_external_datagrams(master);
fp@1031: ecrt_master_send(master);
fp@1804: sent_bytes = master->main_device.tx_skb[
fp@1774: master->main_device.tx_ring_index]->len;
fp@1489: up(&master->io_sem);
martin@1583:
fp@1804: if (ec_fsm_master_idle(&master->fsm)) {
fp@1773: #ifdef EC_USE_HRTIMER
fp@1804: ec_master_nanosleep(master->send_interval * 1000);
fp@1773: #else
fp@1773: set_current_state(TASK_INTERRUPTIBLE);
fp@1773: schedule_timeout(1);
fp@1773: #endif
fp@1773: } else {
fp@1773: #ifdef EC_USE_HRTIMER
fp@1804: ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS);
fp@1773: #else
fp@1773: schedule();
fp@1773: #endif
fp@1773: }
fp@525: }
fp@518:
fp@656: if (master->debug_level)
fp@656: EC_DBG("Master IDLE thread exiting...\n");
fp@1774:
fp@1241: return 0;
fp@191: }
fp@191:
fp@637: /*****************************************************************************/
fp@637:
martin@1583: /** Master kernel thread function for OPERATION phase.
fp@997: */
fp@1241: static int ec_master_operation_thread(void *priv_data)
fp@1241: {
fp@1241: ec_master_t *master = (ec_master_t *) priv_data;
martin@1583: ec_slave_t *slave = NULL;
martin@1583: int fsm_exec;
fp@1773:
fp@1241: if (master->debug_level)
fp@1804: EC_DBG("Operation thread running with fsm interval = %d us,"
fp@1773: " max data size=%d\n",
fp@1773: master->send_interval,
fp@1773: master->max_queue_size);
fp@1241:
fp@1241: while (!kthread_should_stop()) {
fp@719: ec_datagram_output_stats(&master->fsm_datagram);
fp@1773:
martin@1583: if (master->injection_seq_rt == master->injection_seq_fsm) {
martin@1583: // output statistics
martin@1583: ec_master_output_stats(master);
martin@1583:
martin@1583: fsm_exec = 0;
martin@1583: // execute master & slave state machines
martin@1583: if (down_interruptible(&master->master_sem))
martin@1583: break;
martin@1583: fsm_exec += ec_fsm_master_exec(&master->fsm);
martin@1583: for (slave = master->slaves;
martin@1583: slave < master->slaves + master->slave_count;
martin@1583: slave++) {
martin@1583: ec_fsm_slave_exec(&slave->fsm);
martin@1583: }
martin@1583: up(&master->master_sem);
martin@1583:
martin@1583: // inject datagrams (let the rt thread queue them, see ecrt_master_send)
martin@1583: if (fsm_exec)
martin@1583: master->injection_seq_fsm++;
martin@1583: }
fp@1773:
fp@1773: #ifdef EC_USE_HRTIMER
fp@1804: // the op thread should not work faster than the sending RT thread
fp@1804: ec_master_nanosleep(master->send_interval * 1000);
fp@1773: #else
fp@1773: if (ec_fsm_master_idle(&master->fsm)) {
fp@1773: set_current_state(TASK_INTERRUPTIBLE);
fp@1773: schedule_timeout(1);
fp@1773: }
fp@1773: else {
fp@1773: schedule();
fp@1773: }
fp@1773: #endif
fp@1804: }
fp@637:
fp@656: if (master->debug_level)
fp@656: EC_DBG("Master OP thread exiting...\n");
fp@1241: return 0;
fp@637: }
fp@578:
fp@578: /*****************************************************************************/
fp@578:
fp@715: #ifdef EC_EOE
fp@1327: /** Starts Ethernet over EtherCAT processing on demand.
fp@997: */
fp@251: void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */)
fp@251: {
fp@1489: struct sched_param param = { .sched_priority = 0 };
fp@1489:
fp@1489: if (master->eoe_thread) {
fp@661: EC_WARN("EoE already running!\n");
fp@661: return;
fp@661: }
fp@661:
fp@661: if (list_empty(&master->eoe_handlers))
fp@661: return;
fp@661:
fp@1500: if (!master->send_cb || !master->receive_cb) {
fp@1500: EC_WARN("No EoE processing because of missing callbacks!\n");
fp@251: return;
fp@251: }
fp@251:
fp@1489: EC_INFO("Starting EoE thread.\n");
fp@1489: master->eoe_thread = kthread_run(ec_master_eoe_thread, master,
fp@1489: "EtherCAT-EoE");
fp@1489: if (IS_ERR(master->eoe_thread)) {
fp@1489: int err = (int) PTR_ERR(master->eoe_thread);
fp@1489: EC_ERR("Failed to start EoE thread (error %i)!\n", err);
fp@1489: master->eoe_thread = NULL;
fp@1489: return;
fp@1489: }
fp@1489:
fp@1489: sched_setscheduler(master->eoe_thread, SCHED_NORMAL, ¶m);
fp@1489: set_user_nice(master->eoe_thread, 0);
fp@251: }
fp@251:
fp@251: /*****************************************************************************/
fp@251:
fp@1327: /** Stops the Ethernet over EtherCAT processing.
fp@997: */
fp@251: void ec_master_eoe_stop(ec_master_t *master /**< EtherCAT master */)
fp@251: {
fp@1489: if (master->eoe_thread) {
fp@1489: EC_INFO("Stopping EoE thread.\n");
fp@1489:
fp@1489: kthread_stop(master->eoe_thread);
fp@1489: master->eoe_thread = NULL;
fp@1489: EC_INFO("EoE thread exited.\n");
fp@1489: }
fp@251: }
fp@251:
fp@251: /*****************************************************************************/
fp@441:
fp@1327: /** Does the Ethernet over EtherCAT processing.
fp@997: */
fp@1489: static int ec_master_eoe_thread(void *priv_data)
fp@1489: {
fp@1489: ec_master_t *master = (ec_master_t *) priv_data;
fp@197: ec_eoe_t *eoe;
fp@1489: unsigned int none_open, sth_to_send, all_idle;
fp@1489:
fp@1489: if (master->debug_level)
fp@1489: EC_DBG("EoE thread running.\n");
fp@1489:
fp@1489: while (!kthread_should_stop()) {
fp@1489: none_open = 1;
fp@1489: all_idle = 1;
fp@1489:
fp@1489: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1489: if (ec_eoe_is_open(eoe)) {
fp@1489: none_open = 0;
fp@1489: break;
fp@1489: }
fp@1489: }
fp@1489: if (none_open)
fp@1489: goto schedule;
fp@1489:
fp@1489: // receive datagrams
fp@1513: master->receive_cb(master->cb_data);
fp@1489:
fp@1489: // actual EoE processing
fp@1489: sth_to_send = 0;
fp@1489: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1489: ec_eoe_run(eoe);
fp@1489: if (eoe->queue_datagram) {
fp@1489: sth_to_send = 1;
fp@1489: }
fp@1489: if (!ec_eoe_is_idle(eoe)) {
fp@1489: all_idle = 0;
fp@1489: }
fp@1489: }
fp@1489:
fp@1489: if (sth_to_send) {
fp@1489: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1489: ec_eoe_queue(eoe);
fp@1489: }
fp@1500: // (try to) send datagrams
fp@1500: down(&master->ext_queue_sem);
fp@1513: master->send_cb(master->cb_data);
fp@1500: up(&master->ext_queue_sem);
fp@1489: }
fp@1489:
fp@1489: schedule:
fp@1489: if (all_idle) {
fp@1489: set_current_state(TASK_INTERRUPTIBLE);
fp@1489: schedule_timeout(1);
fp@1489: } else {
fp@1489: schedule();
fp@1489: }
fp@1489: }
fp@1041:
fp@1489: if (master->debug_level)
fp@1489: EC_DBG("EoE thread exiting...\n");
fp@1489: return 0;
fp@197: }
fp@715: #endif
fp@197:
fp@325: /*****************************************************************************/
fp@325:
fp@792: /** Detaches the slave configurations from the slaves.
fp@640: */
fp@792: void ec_master_detach_slave_configs(
fp@792: ec_master_t *master /**< EtherCAT master. */
fp@640: )
fp@197: {
fp@792: ec_slave_config_t *sc;
fp@792:
fp@792: list_for_each_entry(sc, &master->configs, list) {
fp@792: ec_slave_config_detach(sc);
fp@792: }
fp@792: }
fp@792:
fp@792: /*****************************************************************************/
fp@792:
fp@792: /** Attaches the slave configurations to the slaves.
fp@792: */
fp@1028: void ec_master_attach_slave_configs(
fp@792: ec_master_t *master /**< EtherCAT master. */
fp@792: )
fp@792: {
fp@792: ec_slave_config_t *sc;
fp@792:
fp@792: list_for_each_entry(sc, &master->configs, list) {
fp@1028: ec_slave_config_attach(sc);
fp@1028: }
fp@197: }
fp@197:
fp@927: /*****************************************************************************/
fp@927:
fp@1077: /** Common implementation for ec_master_find_slave()
fp@1077: * and ec_master_find_slave_const().
fp@1077: */
fp@1077: #define EC_FIND_SLAVE \
fp@1077: do { \
fp@1077: if (alias) { \
fp@1077: for (; slave < master->slaves + master->slave_count; \
fp@1077: slave++) { \
fp@1077: if (slave->sii.alias == alias) \
fp@1077: break; \
fp@1077: } \
fp@1077: if (slave == master->slaves + master->slave_count) \
fp@1077: return NULL; \
fp@1077: } \
fp@1077: \
fp@1077: slave += position; \
fp@1077: if (slave < master->slaves + master->slave_count) { \
fp@1077: return slave; \
fp@1077: } else { \
fp@1077: return NULL; \
fp@1077: } \
fp@1077: } while (0)
fp@1077:
fp@927: /** Finds a slave in the bus, given the alias and position.
fp@927: */
fp@927: ec_slave_t *ec_master_find_slave(
fp@927: ec_master_t *master, /**< EtherCAT master. */
fp@927: uint16_t alias, /**< Slave alias. */
fp@927: uint16_t position /**< Slave position. */
fp@927: )
fp@927: {
fp@1000: ec_slave_t *slave = master->slaves;
fp@1077: EC_FIND_SLAVE;
fp@1077: }
fp@1077:
fp@1077: /** Finds a slave in the bus, given the alias and position.
fp@1077: *
fp@1077: * Const version.
fp@1077: */
fp@1077: const ec_slave_t *ec_master_find_slave_const(
fp@1077: const ec_master_t *master, /**< EtherCAT master. */
fp@1077: uint16_t alias, /**< Slave alias. */
fp@1077: uint16_t position /**< Slave position. */
fp@1077: )
fp@1077: {
fp@1077: const ec_slave_t *slave = master->slaves;
fp@1077: EC_FIND_SLAVE;
fp@927: }
fp@927:
fp@946: /*****************************************************************************/
fp@946:
fp@1092: /** Get the number of slave configurations provided by the application.
fp@1092: *
fp@1092: * \return Number of configurations.
fp@1092: */
fp@990: unsigned int ec_master_config_count(
fp@1507: const ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: const ec_slave_config_t *sc;
fp@1507: unsigned int count = 0;
fp@1507:
fp@1507: list_for_each_entry(sc, &master->configs, list) {
fp@1507: count++;
fp@1507: }
fp@1507:
fp@1507: return count;
fp@990: }
fp@990:
fp@990: /*****************************************************************************/
fp@990:
fp@1252: /** Common implementation for ec_master_get_config()
fp@1252: * and ec_master_get_config_const().
fp@1252: */
fp@1252: #define EC_FIND_CONFIG \
fp@1252: do { \
fp@1507: list_for_each_entry(sc, &master->configs, list) { \
fp@1507: if (pos--) \
fp@1507: continue; \
fp@1507: return sc; \
fp@1507: } \
fp@1507: return NULL; \
fp@1252: } while (0)
fp@1252:
fp@1252: /** Get a slave configuration via its position in the list.
fp@1252: *
fp@1252: * \return Slave configuration or \a NULL.
fp@1252: */
fp@1252: ec_slave_config_t *ec_master_get_config(
fp@1507: const ec_master_t *master, /**< EtherCAT master. */
fp@1507: unsigned int pos /**< List position. */
fp@1507: )
fp@1507: {
fp@1507: ec_slave_config_t *sc;
fp@1507: EC_FIND_CONFIG;
fp@1252: }
fp@1252:
fp@1092: /** Get a slave configuration via its position in the list.
fp@1092: *
fp@1092: * Const version.
fp@1092: *
fp@1092: * \return Slave configuration or \a NULL.
fp@1092: */
fp@990: const ec_slave_config_t *ec_master_get_config_const(
fp@1507: const ec_master_t *master, /**< EtherCAT master. */
fp@1507: unsigned int pos /**< List position. */
fp@1507: )
fp@1507: {
fp@1507: const ec_slave_config_t *sc;
fp@1507: EC_FIND_CONFIG;
fp@990: }
fp@990:
fp@990: /*****************************************************************************/
fp@990:
fp@1092: /** Get the number of domains.
fp@1092: *
fp@1092: * \return Number of domains.
fp@1092: */
fp@946: unsigned int ec_master_domain_count(
fp@1507: const ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: const ec_domain_t *domain;
fp@1507: unsigned int count = 0;
fp@1507:
fp@1507: list_for_each_entry(domain, &master->domains, list) {
fp@1507: count++;
fp@1507: }
fp@1507:
fp@1507: return count;
fp@946: }
fp@946:
fp@946: /*****************************************************************************/
fp@946:
fp@1082: /** Common implementation for ec_master_find_domain() and
fp@1082: * ec_master_find_domain_const().
fp@1082: */
fp@1078: #define EC_FIND_DOMAIN \
fp@1078: do { \
fp@1078: list_for_each_entry(domain, &master->domains, list) { \
fp@1078: if (index--) \
fp@1078: continue; \
fp@1078: return domain; \
fp@1078: } \
fp@1078: \
fp@1078: return NULL; \
fp@1078: } while (0)
fp@1078:
fp@1092: /** Get a domain via its position in the list.
fp@1092: *
fp@1092: * \return Domain pointer, or \a NULL if not found.
fp@1092: */
fp@946: ec_domain_t *ec_master_find_domain(
fp@1507: ec_master_t *master, /**< EtherCAT master. */
fp@1507: unsigned int index /**< Domain index. */
fp@1507: )
fp@1507: {
fp@1507: ec_domain_t *domain;
fp@1078: EC_FIND_DOMAIN;
fp@1078: }
fp@1078:
fp@1092: /** Get a domain via its position in the list.
fp@1092: *
fp@1092: * Const version.
fp@1092: *
fp@1092: * \return Domain pointer, or \a NULL if not found.
fp@1092: */
fp@1078: const ec_domain_t *ec_master_find_domain_const(
fp@1507: const ec_master_t *master, /**< EtherCAT master. */
fp@1507: unsigned int index /**< Domain index. */
fp@1507: )
fp@1507: {
fp@1507: const ec_domain_t *domain;
fp@1078: EC_FIND_DOMAIN;
fp@946: }
fp@946:
fp@956: /*****************************************************************************/
fp@956:
fp@1516: #ifdef EC_EOE
fp@1516:
fp@1485: /** Get the number of EoE handlers.
fp@1485: *
fp@1485: * \return Number of EoE handlers.
fp@1485: */
fp@1485: uint16_t ec_master_eoe_handler_count(
fp@1507: const ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: const ec_eoe_t *eoe;
fp@1507: unsigned int count = 0;
fp@1507:
fp@1507: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1507: count++;
fp@1507: }
fp@1507:
fp@1507: return count;
fp@1485: }
fp@1485:
fp@1485: /*****************************************************************************/
fp@1485:
fp@1485: /** Get an EoE handler via its position in the list.
fp@1485: *
fp@1485: * Const version.
fp@1485: *
fp@1485: * \return EoE handler pointer, or \a NULL if not found.
fp@1485: */
fp@1485: const ec_eoe_t *ec_master_get_eoe_handler_const(
fp@1507: const ec_master_t *master, /**< EtherCAT master. */
fp@1507: uint16_t index /**< EoE handler index. */
fp@1507: )
fp@1507: {
fp@1507: const ec_eoe_t *eoe;
fp@1507:
fp@1507: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1507: if (index--)
fp@1507: continue;
fp@1507: return eoe;
fp@1507: }
fp@1507:
fp@1507: return NULL;
fp@1485: }
fp@1485:
fp@1516: #endif
fp@1516:
fp@1485: /*****************************************************************************/
fp@1485:
fp@1092: /** Set the debug level.
fp@1092: *
fp@1313: * \retval 0 Success.
fp@1313: * \retval -EINVAL Invalid debug level.
fp@1092: */
fp@956: int ec_master_debug_level(
fp@1092: ec_master_t *master, /**< EtherCAT master. */
fp@1399: unsigned int level /**< Debug level. May be 0, 1 or 2. */
fp@956: )
fp@956: {
fp@1399: if (level > 2) {
fp@1399: EC_ERR("Invalid debug level %u!\n", level);
fp@1313: return -EINVAL;
fp@956: }
fp@956:
fp@956: if (level != master->debug_level) {
fp@956: master->debug_level = level;
fp@1399: EC_INFO("Master debug level set to %u.\n", master->debug_level);
fp@956: }
fp@956:
fp@956: return 0;
fp@956: }
fp@956:
fp@1408: /*****************************************************************************/
fp@1408:
fp@1408: /** Finds the DC reference clock.
fp@1408: */
fp@1408: void ec_master_find_dc_ref_clock(
fp@1408: ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: ec_slave_t *slave, *ref = NULL;
fp@1408:
fp@1408: for (slave = master->slaves;
fp@1408: slave < master->slaves + master->slave_count;
fp@1408: slave++) {
fp@1425: if (slave->base_dc_supported && slave->has_dc_system_time) {
fp@1425: ref = slave;
fp@1425: break;
fp@1425: }
fp@1425: }
fp@1425:
fp@1425: master->dc_ref_clock = ref;
fp@1425:
fp@1425: // This call always succeeds, because the datagram has been pre-allocated.
fp@1425: ec_datagram_frmw(&master->sync_datagram,
fp@1425: ref ? ref->station_address : 0xffff, 0x0910, 4);
fp@1408: }
fp@1408:
fp@1421: /*****************************************************************************/
fp@1421:
fp@1421: /** Calculates the bus topology; recursion function.
fp@1421: */
fp@1421: int ec_master_calc_topology_rec(
fp@1421: ec_master_t *master, /**< EtherCAT master. */
fp@1421: ec_slave_t *port0_slave, /**< Slave at port 0. */
fp@1421: unsigned int *slave_position /**< Slave position. */
fp@1507: )
fp@1421: {
fp@1421: ec_slave_t *slave = master->slaves + *slave_position;
fp@1421: unsigned int i;
fp@1421: int ret;
fp@1421:
fp@1425: slave->ports[0].next_slave = port0_slave;
fp@1421:
fp@1421: for (i = 1; i < EC_MAX_PORTS; i++) {
fp@1425: if (!slave->ports[i].link.loop_closed) {
fp@1421: *slave_position = *slave_position + 1;
fp@1421: if (*slave_position < master->slave_count) {
fp@1425: slave->ports[i].next_slave = master->slaves + *slave_position;
fp@1421: ret = ec_master_calc_topology_rec(master,
fp@1421: slave, slave_position);
fp@1421: if (ret)
fp@1421: return ret;
fp@1421: } else {
fp@1421: return -1;
fp@1421: }
fp@1421: }
fp@1421: }
fp@1421:
fp@1421: return 0;
fp@1421: }
fp@1421:
fp@1421: /*****************************************************************************/
fp@1421:
fp@1421: /** Calculates the bus topology.
fp@1421: */
fp@1421: void ec_master_calc_topology(
fp@1421: ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1421: {
fp@1421: unsigned int slave_position = 0;
fp@1421:
fp@1421: if (master->slave_count == 0)
fp@1421: return;
fp@1421:
fp@1421: if (ec_master_calc_topology_rec(master, NULL, &slave_position))
fp@1421: EC_ERR("Failed to calculate bus topology.\n");
fp@1421: }
fp@1421:
fp@1425: /*****************************************************************************/
fp@1425:
fp@1426: /** Calculates the bus transmission delays.
fp@1426: */
fp@1426: void ec_master_calc_transmission_delays(
fp@1425: ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: ec_slave_t *slave;
fp@1425:
fp@1425: for (slave = master->slaves;
fp@1425: slave < master->slaves + master->slave_count;
fp@1425: slave++) {
fp@1425: ec_slave_calc_port_delays(slave);
fp@1425: }
fp@1425:
fp@1425: if (master->dc_ref_clock) {
fp@1425: uint32_t delay = 0;
fp@1426: ec_slave_calc_transmission_delays_rec(master->dc_ref_clock, &delay);
fp@1425: }
fp@1425: }
fp@1425:
fp@1425: /*****************************************************************************/
fp@1425:
fp@1425: /** Distributed-clocks calculations.
fp@1425: */
fp@1425: void ec_master_calc_dc(
fp@1425: ec_master_t *master /**< EtherCAT master. */
fp@1507: )
fp@1507: {
fp@1507: // find DC reference clock
fp@1507: ec_master_find_dc_ref_clock(master);
fp@1425:
fp@1425: // calculate bus topology
fp@1425: ec_master_calc_topology(master);
fp@1425:
fp@1426: ec_master_calc_transmission_delays(master);
fp@1425: }
fp@1425:
fp@640: /******************************************************************************
fp@1394: * Application interface
fp@640: *****************************************************************************/
fp@640:
fp@1332: /** Same as ecrt_master_create_domain(), but with ERR_PTR() return value.
fp@1332: */
fp@1312: ec_domain_t *ecrt_master_create_domain_err(
fp@1312: ec_master_t *master /**< master */
fp@1312: )
fp@640: {
fp@640: ec_domain_t *domain, *last_domain;
fp@640: unsigned int index;
fp@640:
fp@1181: if (master->debug_level)
fp@1543: EC_DBG("ecrt_master_create_domain(master = 0x%p)\n", master);
fp@1181:
fp@640: if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
fp@640: EC_ERR("Error allocating domain memory!\n");
fp@1312: return ERR_PTR(-ENOMEM);
fp@640: }
fp@640:
fp@1075: down(&master->master_sem);
fp@1075:
fp@993: if (list_empty(&master->domains)) {
fp@993: index = 0;
fp@993: } else {
fp@640: last_domain = list_entry(master->domains.prev, ec_domain_t, list);
fp@640: index = last_domain->index + 1;
fp@640: }
fp@640:
fp@993: ec_domain_init(domain, master, index);
fp@640: list_add_tail(&domain->list, &master->domains);
fp@640:
fp@1075: up(&master->master_sem);
fp@1075:
fp@1181: if (master->debug_level)
fp@1181: EC_DBG("Created domain %u.\n", domain->index);
fp@1181:
fp@640: return domain;
fp@640: }
fp@640:
fp@640: /*****************************************************************************/
fp@640:
fp@1312: ec_domain_t *ecrt_master_create_domain(
fp@1312: ec_master_t *master /**< master */
fp@1312: )
fp@1312: {
fp@1312: ec_domain_t *d = ecrt_master_create_domain_err(master);
fp@1312: return IS_ERR(d) ? NULL : d;
fp@1312: }
fp@1312:
fp@1312: /*****************************************************************************/
fp@1312:
fp@792: int ecrt_master_activate(ec_master_t *master)
fp@640: {
fp@640: uint32_t domain_offset;
fp@640: ec_domain_t *domain;
fp@1313: int ret;
fp@640:
fp@1181: if (master->debug_level)
fp@1543: EC_DBG("ecrt_master_activate(master = 0x%p)\n", master);
fp@1181:
fp@1530: if (master->active) {
fp@1530: EC_WARN("%s: Master already active!\n", __func__);
fp@1530: return 0;
fp@1530: }
fp@1530:
fp@1075: down(&master->master_sem);
fp@1075:
fp@792: // finish all domains
fp@640: domain_offset = 0;
fp@640: list_for_each_entry(domain, &master->domains, list) {
fp@1313: ret = ec_domain_finish(domain, domain_offset);
fp@1313: if (ret < 0) {
fp@1197: up(&master->master_sem);
fp@1543: EC_ERR("Failed to finish domain 0x%p!\n", domain);
fp@1313: return ret;
fp@640: }
fp@640: domain_offset += domain->data_size;
fp@640: }
fp@1075:
fp@1451: // always set DC reference clock to OP
fp@1451: if (master->dc_ref_clock) {
fp@1451: ec_slave_request_state(master->dc_ref_clock, EC_SLAVE_STATE_OP);
fp@1451: }
fp@1451:
fp@1075: up(&master->master_sem);
fp@640:
fp@656: // restart EoE process and master thread with new locking
fp@715: #ifdef EC_EOE
fp@656: ec_master_eoe_stop(master);
fp@715: #endif
fp@656: ec_master_thread_stop(master);
fp@656:
fp@640: if (master->debug_level)
fp@1543: EC_DBG("FSM datagram is %p.\n", &master->fsm_datagram);
fp@640:
fp@656: master->injection_seq_fsm = 0;
fp@656: master->injection_seq_rt = 0;
fp@1500:
fp@1500: master->send_cb = master->app_send_cb;
fp@1500: master->receive_cb = master->app_receive_cb;
fp@1513: master->cb_data = master->app_cb_data;
fp@656:
fp@1313: ret = ec_master_thread_start(master, ec_master_operation_thread,
fp@1313: "EtherCAT-OP");
fp@1313: if (ret < 0) {
fp@640: EC_ERR("Failed to start master thread!\n");
fp@1313: return ret;
fp@640: }
fp@715: #ifdef EC_EOE
fp@661: ec_master_eoe_start(master);
fp@715: #endif
fp@902:
fp@902: master->allow_config = 1; // request the current configuration
fp@902: master->allow_scan = 1; // allow re-scanning on topology change
fp@1530: master->active = 1;
fp@640: return 0;
fp@640: }
fp@640:
fp@640: /*****************************************************************************/
fp@640:
fp@1530: void ecrt_master_deactivate(ec_master_t *master)
fp@1530: {
fp@1530: ec_slave_t *slave;
fp@1530: #ifdef EC_EOE
fp@1530: ec_eoe_t *eoe;
fp@1530: #endif
fp@1530:
fp@1530: if (master->debug_level)
fp@1530: EC_DBG("ecrt_master_deactivate(master = 0x%x)\n", (u32) master);
fp@1530:
fp@1530: if (!master->active) {
fp@1530: EC_WARN("%s: Master not active.\n", __func__);
fp@1530: return;
fp@1530: }
fp@1530:
fp@1530: #ifdef EC_EOE
fp@1530: ec_master_eoe_stop(master);
fp@1530: #endif
fp@1530: ec_master_thread_stop(master);
fp@1530:
fp@1530: master->send_cb = ec_master_internal_send_cb;
fp@1530: master->receive_cb = ec_master_internal_receive_cb;
fp@1530: master->cb_data = master;
fp@1530:
fp@1530: down(&master->master_sem);
fp@1530: ec_master_clear_domains(master);
fp@1530: ec_master_clear_slave_configs(master);
fp@1530: up(&master->master_sem);
fp@1530:
fp@1530: for (slave = master->slaves;
fp@1530: slave < master->slaves + master->slave_count;
fp@1530: slave++) {
fp@1530:
fp@1530: // set states for all slaves
fp@1530: ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
fp@1530:
fp@1530: // mark for reconfiguration, because the master could have no
fp@1530: // possibility for a reconfiguration between two sequential operation
fp@1530: // phases.
fp@1530: slave->force_config = 1;
fp@1530: }
fp@1530:
fp@1530: #ifdef EC_EOE
fp@1530: // ... but leave EoE slaves in OP
fp@1530: list_for_each_entry(eoe, &master->eoe_handlers, list) {
fp@1530: if (ec_eoe_is_open(eoe))
fp@1530: ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
fp@1530: }
fp@1530: #endif
fp@1530:
fp@1530: master->app_time = 0ULL;
fp@1530: master->app_start_time = 0ULL;
fp@1530: master->has_start_time = 0;
fp@1530:
fp@1530: if (ec_master_thread_start(master, ec_master_idle_thread,
fp@1530: "EtherCAT-IDLE"))
fp@1530: EC_WARN("Failed to restart master thread!\n");
fp@1530: #ifdef EC_EOE
fp@1530: ec_master_eoe_start(master);
fp@1530: #endif
fp@1530:
fp@1530: master->allow_scan = 1;
fp@1530: master->allow_config = 1;
fp@1530: master->active = 0;
fp@1530: }
fp@1530:
fp@1530: /*****************************************************************************/
fp@1530:
fp@792: void ecrt_master_send(ec_master_t *master)
fp@640: {
fp@640: ec_datagram_t *datagram, *n;
fp@640:
fp@640: if (master->injection_seq_rt != master->injection_seq_fsm) {
martin@1583: // inject datagrams produced by master & slave FSMs
fp@640: ec_master_queue_datagram(master, &master->fsm_datagram);
fp@640: master->injection_seq_rt = master->injection_seq_fsm;
fp@640: }
martin@1597: ec_master_inject_external_datagrams(master);
fp@640:
fp@640: if (unlikely(!master->main_device.link_state)) {
fp@640: // link is down, no datagram can be sent
fp@640: list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
fp@640: datagram->state = EC_DATAGRAM_ERROR;
fp@640: list_del_init(&datagram->queue);
fp@640: }
fp@640:
fp@640: // query link state
fp@640: ec_device_poll(&master->main_device);
fp@640: return;
fp@640: }
fp@640:
fp@640: // send frames
fp@1804: ec_master_send_datagrams(master);
fp@640: }
fp@640:
fp@640: /*****************************************************************************/
fp@640:
fp@792: void ecrt_master_receive(ec_master_t *master)
fp@640: {
fp@640: ec_datagram_t *datagram, *next;
fp@640:
fp@640: // receive datagrams
fp@640: ec_device_poll(&master->main_device);
fp@640:
fp@640: // dequeue all datagrams that timed out
fp@640: list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
fp@640: if (datagram->state != EC_DATAGRAM_SENT) continue;
fp@640:
fp@1040: #ifdef EC_HAVE_CYCLES
fp@640: if (master->main_device.cycles_poll - datagram->cycles_sent
fp@1279: > timeout_cycles) {
fp@1040: #else
fp@1279: if (master->main_device.jiffies_poll - datagram->jiffies_sent
fp@1279: > timeout_jiffies) {
fp@1040: #endif
fp@640: list_del_init(&datagram->queue);
fp@640: datagram->state = EC_DATAGRAM_TIMED_OUT;
fp@640: master->stats.timeouts++;
fp@640: ec_master_output_stats(master);
fp@684:
fp@684: if (unlikely(master->debug_level > 0)) {
fp@1237: unsigned int time_us;
fp@1237: #ifdef EC_HAVE_CYCLES
fp@1237: time_us = (unsigned int) (master->main_device.cycles_poll -
fp@1237: datagram->cycles_sent) * 1000 / cpu_khz;
fp@1237: #else
fp@1279: time_us = (unsigned int) ((master->main_device.jiffies_poll -
fp@1279: datagram->jiffies_sent) * 1000000 / HZ);
fp@1237: #endif
fp@1543: EC_DBG("TIMED OUT datagram %p, index %02X waited %u us.\n",
fp@1543: datagram, datagram->index, time_us);
fp@684: }
fp@640: }
fp@640: }
fp@640: }
fp@640:
fp@640: /*****************************************************************************/
fp@640:
fp@1500: void ecrt_master_send_ext(ec_master_t *master)
fp@1500: {
fp@1500: ec_datagram_t *datagram, *next;
fp@1500:
fp@1500: list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
fp@1500: queue) {
fp@1500: list_del(&datagram->queue);
fp@1500: ec_master_queue_datagram(master, datagram);
fp@1500: }
fp@1500:
fp@1500: ecrt_master_send(master);
fp@1500: }
fp@1500:
fp@1500: /*****************************************************************************/
fp@1500:
fp@1332: /** Same as ecrt_master_slave_config(), but with ERR_PTR() return value.
fp@1332: */
fp@1312: ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master,
fp@792: uint16_t alias, uint16_t position, uint32_t vendor_id,
fp@1010: uint32_t product_code)
fp@792: {
fp@792: ec_slave_config_t *sc;
fp@697: unsigned int found = 0;
fp@697:
fp@1181:
fp@1181: if (master->debug_level)
fp@1543: EC_DBG("ecrt_master_slave_config(master = 0x%p, alias = %u, "
fp@1483: "position = %u, vendor_id = 0x%08x, product_code = 0x%08x)\n",
fp@1543: master, alias, position, vendor_id, product_code);
fp@1181:
fp@792: list_for_each_entry(sc, &master->configs, list) {
fp@792: if (sc->alias == alias && sc->position == position) {
fp@697: found = 1;
fp@697: break;
fp@697: }
fp@697: }
fp@697:
fp@1024: if (found) { // config with same alias/position already existing
fp@1010: if (sc->vendor_id != vendor_id || sc->product_code != product_code) {
fp@792: EC_ERR("Slave type mismatch. Slave was configured as"
fp@1010: " 0x%08X/0x%08X before. Now configuring with"
fp@1010: " 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code,
fp@1010: vendor_id, product_code);
fp@1312: return ERR_PTR(-ENOENT);
fp@792: }
fp@792: } else {
fp@1055: if (master->debug_level)
fp@1055: EC_DBG("Creating slave configuration for %u:%u, 0x%08X/0x%08X.\n",
fp@1055: alias, position, vendor_id, product_code);
fp@792:
fp@792: if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t),
fp@792: GFP_KERNEL))) {
fp@792: EC_ERR("Failed to allocate memory for slave configuration.\n");
fp@1312: return ERR_PTR(-ENOMEM);
fp@792: }
fp@792:
fp@995: ec_slave_config_init(sc, master,
fp@1010: alias, position, vendor_id, product_code);
fp@792:
fp@1075: down(&master->master_sem);
fp@1075:
fp@792: // try to find the addressed slave
fp@1055: ec_slave_config_attach(sc);
fp@1055: ec_slave_config_load_default_sync_config(sc);
fp@792: list_add_tail(&sc->list, &master->configs);
fp@1075:
fp@1075: up(&master->master_sem);
fp@792: }
fp@792:
fp@792: return sc;
fp@792: }
fp@792:
fp@792: /*****************************************************************************/
fp@792:
fp@1312: ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
fp@1312: uint16_t alias, uint16_t position, uint32_t vendor_id,
fp@1312: uint32_t product_code)
fp@1312: {
fp@1312: ec_slave_config_t *sc = ecrt_master_slave_config_err(master, alias,
fp@1312: position, vendor_id, product_code);
fp@1312: return IS_ERR(sc) ? NULL : sc;
fp@1312: }
fp@1312:
fp@1312: /*****************************************************************************/
fp@1312:
fp@1594: int ecrt_master(ec_master_t *master, ec_master_info_t *master_info)
fp@1594: {
fp@1594: if (master->debug_level)
fp@1594: EC_DBG("ecrt_master(master = 0x%p, master_info = 0x%p)\n",
fp@1594: master, master_info);
fp@1594:
fp@1594: master_info->slave_count = master->slave_count;
fp@1594: master_info->link_up = master->main_device.link_state;
fp@1594: master_info->scan_busy = master->scan_busy;
fp@1594: master_info->app_time = master->app_time;
fp@1594: return 0;
fp@1594: }
fp@1594:
fp@1594: /*****************************************************************************/
fp@1594:
fp@1594: int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
fp@1594: ec_slave_info_t *slave_info)
fp@1594: {
fp@1594: const ec_slave_t *slave;
fp@1594:
fp@1594: if (down_interruptible(&master->master_sem)) {
fp@1594: return -EINTR;
fp@1594: }
fp@1594:
fp@1594: slave = ec_master_find_slave_const(master, 0, slave_position);
fp@1594:
fp@1594: slave_info->position = slave->ring_position;
fp@1594: slave_info->vendor_id = slave->sii.vendor_id;
fp@1594: slave_info->product_code = slave->sii.product_code;
fp@1594: slave_info->revision_number = slave->sii.revision_number;
fp@1594: slave_info->serial_number = slave->sii.serial_number;
fp@1594: slave_info->alias = slave->sii.alias;
fp@1594: slave_info->current_on_ebus = slave->sii.current_on_ebus;
fp@1594: slave_info->al_state = slave->current_state;
fp@1594: slave_info->error_flag = slave->error_flag;
fp@1594: slave_info->sync_count = slave->sii.sync_count;
fp@1594: slave_info->sdo_count = ec_slave_sdo_count(slave);
fp@1594: if (slave->sii.name) {
fp@1594: strncpy(slave_info->name, slave->sii.name, EC_MAX_STRING_LENGTH);
fp@1594: } else {
fp@1594: slave_info->name[0] = 0;
fp@1594: }
fp@1594:
fp@1594: up(&master->master_sem);
fp@1594:
fp@1594: return 0;
fp@1594: }
fp@1594:
fp@1594: /*****************************************************************************/
fp@1594:
fp@1500: void ecrt_master_callbacks(ec_master_t *master,
fp@1513: void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data)
fp@204: {
fp@1181: if (master->debug_level)
fp@1543: EC_DBG("ecrt_master_callbacks(master = 0x%p, send_cb = 0x%p, "
fp@1543: " receive_cb = 0x%p, cb_data = 0x%p)\n", master,
fp@1543: send_cb, receive_cb, cb_data);
fp@1500:
fp@1500: master->app_send_cb = send_cb;
fp@1500: master->app_receive_cb = receive_cb;
fp@1513: master->app_cb_data = cb_data;
fp@204: }
fp@204:
fp@204: /*****************************************************************************/
fp@204:
fp@792: void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
fp@792: {
fp@792: state->slaves_responding = master->fsm.slaves_responding;
fp@1022: state->al_states = master->fsm.slave_states;
ha@1018: state->link_up = master->main_device.link_state;
fp@612: }
fp@612:
fp@612: /*****************************************************************************/
fp@612:
fp@1434: void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
fp@1417: {
fp@1417: master->app_time = app_time;
fp@1436:
fp@1436: if (unlikely(!master->has_start_time)) {
fp@1436: master->app_start_time = app_time;
fp@1436: master->has_start_time = 1;
fp@1436: }
fp@1434: }
fp@1434:
fp@1434: /*****************************************************************************/
fp@1434:
fp@1434: void ecrt_master_sync_reference_clock(ec_master_t *master)
fp@1434: {
fp@1396: EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
fp@1396: ec_master_queue_datagram(master, &master->ref_sync_datagram);
fp@1410: }
fp@1410:
fp@1410: /*****************************************************************************/
fp@1410:
fp@1410: void ecrt_master_sync_slave_clocks(ec_master_t *master)
fp@1410: {
fp@1394: ec_datagram_zero(&master->sync_datagram);
fp@1394: ec_master_queue_datagram(master, &master->sync_datagram);
fp@1394: }
fp@1394:
fp@1394: /*****************************************************************************/
fp@1394:
fp@1535: void ecrt_master_sync_monitor_queue(ec_master_t *master)
fp@1535: {
fp@1535: ec_datagram_zero(&master->sync_mon_datagram);
fp@1535: ec_master_queue_datagram(master, &master->sync_mon_datagram);
fp@1535: }
fp@1535:
fp@1535: /*****************************************************************************/
fp@1535:
fp@1535: uint32_t ecrt_master_sync_monitor_process(ec_master_t *master)
fp@1535: {
fp@1535: if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) {
fp@1535: return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff;
fp@1535: } else {
fp@1535: return 0xffffffff;
fp@1535: }
fp@1535: }
fp@1535:
fp@1535: /*****************************************************************************/
fp@1535:
fp@199: /** \cond */
fp@199:
fp@104: EXPORT_SYMBOL(ecrt_master_create_domain);
fp@104: EXPORT_SYMBOL(ecrt_master_activate);
fp@1530: EXPORT_SYMBOL(ecrt_master_deactivate);
fp@325: EXPORT_SYMBOL(ecrt_master_send);
fp@1500: EXPORT_SYMBOL(ecrt_master_send_ext);
fp@325: EXPORT_SYMBOL(ecrt_master_receive);
fp@206: EXPORT_SYMBOL(ecrt_master_callbacks);
fp@1594: EXPORT_SYMBOL(ecrt_master);
fp@1594: EXPORT_SYMBOL(ecrt_master_get_slave);
fp@792: EXPORT_SYMBOL(ecrt_master_slave_config);
fp@792: EXPORT_SYMBOL(ecrt_master_state);
fp@1434: EXPORT_SYMBOL(ecrt_master_application_time);
fp@1410: EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
fp@1410: EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
fp@1535: EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
fp@1535: EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
fp@42:
fp@199: /** \endcond */
fp@199:
fp@199: /*****************************************************************************/