fp@39: /****************************************************************************** fp@0: * fp@2085: * $Id$ fp@0: * fp@2433: * Copyright (C) 2006-2012 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@1873: * vim: expandtab fp@1873: * 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 fp@0: #include fp@0: #include fp@0: #include fp@0: #include fp@1015: #include fp@1015: #include martin@1600: #include fp@2692: fp@2692: #if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0) fp@2692: #include // struct sched_param fp@2692: #endif fp@2692: 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@2530: /** Always output corrupted frames. fp@2530: */ fp@2530: #define FORCE_OUTPUT_CORRUPTED 0 fp@2530: fp@1279: #ifdef EC_HAVE_CYCLES fp@1279: fp@1279: /** Frame timeout in cycles. fp@1279: */ fp@1279: static cycles_t timeout_cycles; fp@1906: fp@1906: /** Timeout for external datagram injection [cycles]. fp@1906: */ fp@1906: static cycles_t ext_injection_timeout_cycles; fp@1906: fp@1279: #else fp@1279: fp@1279: /** Frame timeout in jiffies. fp@1279: */ fp@1279: static unsigned long timeout_jiffies; fp@1906: fp@1906: /** Timeout for external datagram injection [jiffies]. fp@1906: */ fp@1906: static unsigned long ext_injection_timeout_jiffies; martin@1585: fp@1279: #endif fp@1279: fp@2158: /** List of intervals for statistics [s]. fp@2158: */ fp@2158: const unsigned int rate_intervals[] = { fp@2158: 1, 10, 60 fp@2158: }; fp@2158: 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@2158: void ec_master_clear_device_stats(ec_master_t *); fp@2158: void ec_master_update_device_stats(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); fp@2267: ext_injection_timeout_cycles = fp@2267: (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); fp@2267: ext_injection_timeout_jiffies = fp@2267: 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@2498: unsigned int dev_idx, i; 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@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) { fp@2453: master->macs[dev_idx] = NULL; fp@2453: } fp@2453: fp@2267: master->macs[EC_DEVICE_MAIN] = main_mac; fp@2453: fp@2453: #if EC_MAX_NUM_DEVICES > 1 fp@2267: master->macs[EC_DEVICE_BACKUP] = backup_mac; fp@2453: master->num_devices = 1 + !ec_mac_is_zero(backup_mac); fp@2453: #else fp@2453: if (!ec_mac_is_zero(backup_mac)) { fp@2453: EC_MASTER_WARN(master, "Ignoring backup MAC address!"); fp@2453: } fp@2453: #endif fp@2453: fp@2158: ec_master_clear_device_stats(master); martin@1579: martin@1579: sema_init(&master->device_sem, 1); fp@446: fp@1029: master->phase = EC_ORPHANED; fp@1530: master->active = 0; fp@1925: master->config_changed = 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@2421: fp@792: INIT_LIST_HEAD(&master->configs); fp@2300: INIT_LIST_HEAD(&master->domains); fp@792: fp@1507: master->app_time = 0ULL; fp@1507: master->app_start_time = 0ULL; fp@1925: master->has_app_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; martin@1579: sema_init(&master->config_sem, 1); fp@656: init_waitqueue_head(&master->config_queue); fp@2421: 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: fp@2498: master->ext_ring_idx_rt = 0; fp@2498: master->ext_ring_idx_fsm = 0; fp@2498: fp@2498: // init external datagram ring fp@2498: for (i = 0; i < EC_EXT_RING_SIZE; i++) { fp@2498: ec_datagram_t *datagram = &master->ext_datagram_ring[i]; fp@2498: ec_datagram_init(datagram); fp@2498: snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i); fp@2498: } fp@2421: fp@1773: // send interval in IDLE phase fp@1773: ec_master_set_send_interval(master, 1000000 / HZ); martin@1583: fp@2498: master->fsm_slave = NULL; fp@2498: INIT_LIST_HEAD(&master->fsm_exec_list); fp@2498: master->fsm_exec_count = 0U; fp@2498: 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@2529: INIT_LIST_HEAD(&master->emerg_reg_requests); fp@2467: fp@2467: init_waitqueue_head(&master->request_queue); fp@601: fp@579: // init devices fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2453: ret = ec_device_init(&master->devices[dev_idx], master); fp@2453: if (ret < 0) { fp@2453: goto out_clear_devices; fp@2453: } fp@2453: } 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@1921: EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n"); fp@2453: goto out_clear_devices; 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@2498: // alloc external datagram ring fp@2498: for (i = 0; i < EC_EXT_RING_SIZE; i++) { fp@2498: ec_datagram_t *datagram = &master->ext_datagram_ring[i]; fp@2498: ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE); fp@2498: if (ret) { fp@2498: EC_MASTER_ERR(master, "Failed to allocate external" fp@2498: " datagram %u.\n", i); fp@2498: goto out_clear_ext_datagrams; fp@2498: } fp@2498: } fp@2498: fp@1396: // init reference sync datagram fp@1396: ec_datagram_init(&master->ref_sync_datagram); fp@2267: snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, fp@2267: "refsync"); fp@2447: ret = ec_datagram_prealloc(&master->ref_sync_datagram, 4); fp@1396: if (ret < 0) { fp@1396: ec_datagram_clear(&master->ref_sync_datagram); fp@1921: EC_MASTER_ERR(master, "Failed to allocate reference" fp@1921: " synchronisation datagram.\n"); fp@2498: goto out_clear_ext_datagrams; 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@1921: EC_MASTER_ERR(master, "Failed to allocate" fp@1921: " 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@2267: snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, fp@2267: "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@1921: EC_MASTER_ERR(master, "Failed to allocate sync" fp@1921: " monitoring datagram.\n"); fp@1535: goto out_clear_sync; fp@1535: } fp@1535: fp@2447: master->dc_ref_config = NULL; fp@2447: master->dc_ref_clock = NULL; 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@2421: 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@1921: EC_MASTER_ERR(master, "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@2433: #ifdef EC_RTDM fp@2433: // init RTDM device fp@2433: ret = ec_rtdm_dev_init(&master->rtdm_dev, master); fp@2433: if (ret) { fp@2433: goto out_unregister_class_device; fp@2433: } fp@2433: #endif fp@2433: fp@178: return 0; fp@226: fp@2433: #ifdef EC_RTDM fp@2433: out_unregister_class_device: fp@2433: #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26) fp@2433: device_unregister(master->class_device); fp@2433: #else fp@2433: class_device_unregister(master->class_device); fp@2433: #endif fp@2433: #endif 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@2498: out_clear_ext_datagrams: fp@2498: for (i = 0; i < EC_EXT_RING_SIZE; i++) { fp@2498: ec_datagram_clear(&master->ext_datagram_ring[i]); fp@2498: } fp@659: ec_fsm_master_clear(&master->fsm); fp@997: ec_datagram_clear(&master->fsm_datagram); fp@2453: out_clear_devices: fp@2453: for (; dev_idx > 0; dev_idx--) { fp@2453: ec_device_clear(&master->devices[dev_idx - 1]); fp@2453: } 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@2498: unsigned int dev_idx, i; fp@2453: fp@2433: #ifdef EC_RTDM fp@2433: ec_rtdm_dev_clear(&master->rtdm_dev); fp@2433: #endif fp@2433: 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@2421: 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@2498: fp@2498: for (i = 0; i < EC_EXT_RING_SIZE; i++) { fp@2498: ec_datagram_clear(&master->ext_datagram_ring[i]); fp@2498: } fp@2498: fp@528: ec_fsm_master_clear(&master->fsm); fp@528: ec_datagram_clear(&master->fsm_datagram); fp@2453: fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2453: ec_device_clear(&master->devices[dev_idx]); fp@2453: } 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@2447: master->dc_ref_config = NULL; fp@2457: master->fsm.sdo_request = NULL; // mark sdo_request as invalid fp@2447: 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@2467: // External requests are obsolete, so we wake pending waiters and remove fp@2467: // them from the list. fp@1921: fp@1921: while (!list_empty(&master->sii_requests)) { fp@1921: ec_sii_write_request_t *request = fp@1921: list_entry(master->sii_requests.next, fp@1921: ec_sii_write_request_t, list); fp@1773: list_del_init(&request->list); // dequeue fp@1921: EC_MASTER_WARN(master, "Discarding SII request, slave %u about" fp@1921: " to be deleted.\n", request->slave->ring_position); fp@1773: request->state = EC_INT_REQUEST_FAILURE; fp@2498: wake_up_all(&master->request_queue); fp@2498: } fp@2498: fp@2498: master->fsm_slave = NULL; fp@2498: INIT_LIST_HEAD(&master->fsm_exec_list); fp@2498: master->fsm_exec_count = 0; fp@1773: fp@1000: for (slave = master->slaves; fp@1000: slave < master->slaves + master->slave_count; fp@1000: slave++) { 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@1939: /** Clear the configuration applied by the application. fp@1939: */ fp@1939: void ec_master_clear_config( fp@1939: ec_master_t *master /**< EtherCAT master. */ fp@1939: ) fp@1939: { fp@1939: down(&master->master_sem); fp@1939: ec_master_clear_domains(master); fp@1939: ec_master_clear_slave_configs(master); fp@1939: up(&master->master_sem); fp@1939: } fp@1939: fp@1939: /*****************************************************************************/ fp@1939: 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@1921: EC_MASTER_INFO(master, "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@1921: EC_MASTER_ERR(master, "Failed to start master thread (error %i)!\n", fp@1921: err); fp@1241: master->thread = NULL; fp@1313: return err; fp@1241: } fp@2421: 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@2421: fp@1241: if (!master->thread) { fp@1921: EC_MASTER_WARN(master, "%s(): Already finished!\n", __func__); fp@656: return; fp@656: } fp@656: fp@1921: EC_MASTER_DBG(master, 1, "Stopping master thread.\n"); fp@1006: fp@1241: kthread_stop(master->thread); fp@1241: master->thread = NULL; fp@1921: EC_MASTER_INFO(master, "Master thread exited.\n"); fp@656: fp@2455: if (master->fsm_datagram.state != EC_DATAGRAM_SENT) { fp@1198: return; fp@2455: } fp@2421: 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@2522: * fp@2522: * \return Zero on success, otherwise a negative error code. 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@2374: ec_device_index_t dev_idx; fp@1313: fp@1921: EC_MASTER_DBG(master, 1, "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@2301: fp@2301: // reset number of responding slaves to trigger scanning fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2374: master->fsm.slaves_responding[dev_idx] = 0; fp@2374: } fp@2301: 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@1921: EC_MASTER_DBG(master, 1, "IDLE -> ORPHANED.\n"); fp@1006: fp@1029: master->phase = EC_ORPHANED; fp@2421: 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@2456: fp@2456: ec_fsm_master_reset(&master->fsm); fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@1029: /** Transition function from IDLE to OPERATION phase. fp@2522: * fp@2522: * \return Zero on success, otherwise a negative error code. fp@1029: */ fp@2267: int ec_master_enter_operation_phase( fp@2267: ec_master_t *master /**< EtherCAT master */ fp@2267: ) 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@1921: EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n"); fp@1006: fp@656: down(&master->config_sem); 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@1921: EC_MASTER_INFO(master, "Finishing slave configuration" fp@1921: " interrupted by signal.\n"); fp@2349: goto out_return; fp@900: } fp@900: fp@1921: EC_MASTER_DBG(master, 1, "Waiting for pending slave" fp@1921: " 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@2267: ret = wait_event_interruptible(master->scan_queue, fp@2267: !master->scan_busy); fp@1313: if (ret) { fp@1921: EC_MASTER_INFO(master, "Waiting for slave scan" fp@1921: " interrupted by signal.\n"); fp@794: goto out_allow; fp@794: } fp@2421: fp@1921: EC_MASTER_DBG(master, 1, "Waiting for pending" fp@1921: " 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@2421: fp@656: out_allow: fp@656: master->allow_scan = 1; fp@2349: out_return: 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@1939: if (master->active) { fp@1939: ecrt_master_deactivate(master); // also clears config fp@1939: } else { fp@1939: ec_master_clear_config(master); fp@1939: } fp@656: fp@2349: /* Re-allow scanning for IDLE phase. */ fp@2349: master->allow_scan = 1; fp@2349: fp@1921: EC_MASTER_DBG(master, 1, "OPERATION -> IDLE.\n"); fp@1006: fp@1029: master->phase = EC_IDLE; fp@446: } fp@446: 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@2498: ec_datagram_t *datagram; fp@2498: size_t queue_size = 0, new_queue_size = 0; fp@2498: #if DEBUG_INJECT fp@2498: unsigned int datagram_count = 0; fp@2498: #endif fp@2498: fp@2498: if (master->ext_ring_idx_rt == master->ext_ring_idx_fsm) { fp@2498: // nothing to inject fp@2498: return; fp@2498: } fp@1774: fp@1773: list_for_each_entry(datagram, &master->datagram_queue, queue) { fp@2498: if (datagram->state == EC_DATAGRAM_QUEUED) { fp@2498: queue_size += datagram->data_size; fp@2498: } fp@2498: } fp@2498: martin@1597: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "Injecting datagrams, queue_size=%zu\n", fp@2498: queue_size); fp@2498: #endif fp@2498: fp@2498: while (master->ext_ring_idx_rt != master->ext_ring_idx_fsm) { fp@2498: datagram = &master->ext_datagram_ring[master->ext_ring_idx_rt]; fp@2498: fp@2498: if (datagram->state != EC_DATAGRAM_INIT) { fp@2498: // skip datagram fp@2498: master->ext_ring_idx_rt = fp@2498: (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE; fp@2498: continue; fp@2498: } fp@2498: fp@2498: new_queue_size = queue_size + datagram->data_size; fp@2498: if (new_queue_size <= master->max_queue_size) { fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "Injecting datagram %s" fp@2498: " size=%zu, queue_size=%zu\n", datagram->name, fp@2498: datagram->data_size, new_queue_size); fp@2498: datagram_count++; 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@2374: ec_master_queue_datagram(master, datagram); fp@2498: queue_size = new_queue_size; fp@2498: } fp@2498: else if (datagram->data_size > master->max_queue_size) { fp@2498: datagram->state = EC_DATAGRAM_ERROR; fp@2498: EC_MASTER_ERR(master, "External datagram %s is too large," fp@2498: " size=%zu, max_queue_size=%zu\n", fp@2498: datagram->name, datagram->data_size, fp@2498: master->max_queue_size); fp@2498: } fp@2498: else { // datagram does not fit in the current cycle fp@2498: #ifdef EC_HAVE_CYCLES fp@2498: cycles_t cycles_now = get_cycles(); fp@2498: fp@2498: if (cycles_now - datagram->cycles_sent fp@2498: > ext_injection_timeout_cycles) fp@2498: #else fp@2498: if (jiffies - datagram->jiffies_sent fp@2498: > ext_injection_timeout_jiffies) fp@2498: #endif fp@2498: { fp@2532: #if defined EC_RT_SYSLOG || DEBUG_INJECT fp@2498: unsigned int time_us; fp@2532: #endif fp@2498: fp@1773: datagram->state = EC_DATAGRAM_ERROR; fp@2532: fp@2532: #if defined EC_RT_SYSLOG || DEBUG_INJECT martin@1585: #ifdef EC_HAVE_CYCLES fp@2498: time_us = (unsigned int) fp@2498: ((cycles_now - datagram->cycles_sent) * 1000LL) fp@2498: / cpu_khz; martin@1585: #else fp@2498: time_us = (unsigned int) fp@2498: ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); fp@2498: #endif fp@2498: EC_MASTER_ERR(master, "Timeout %u us: Injecting" fp@2498: " external datagram %s size=%zu," fp@2498: " max_queue_size=%zu\n", time_us, datagram->name, fp@2498: datagram->data_size, master->max_queue_size); fp@2532: #endif fp@2498: } fp@2498: else { martin@1597: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "Deferred injecting" fp@2498: " external datagram %s size=%u, queue_size=%u\n", fp@2498: datagram->name, datagram->data_size, queue_size); fp@2498: #endif fp@2498: break; fp@1773: } fp@1773: } fp@2498: fp@2498: master->ext_ring_idx_rt = fp@2498: (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE; fp@2498: } fp@2498: fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "Injected %u datagrams.\n", datagram_count); fp@2498: #endif 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@1930: unsigned int 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: fp@2498: /** Searches for a free datagram in the external datagram ring. fp@2522: * fp@2522: * \return Next free datagram, or NULL. fp@2498: */ fp@2498: ec_datagram_t *ec_master_get_external_datagram( fp@2498: ec_master_t *master /**< EtherCAT master */ fp@2498: ) fp@2498: { fp@2498: if ((master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE != fp@2498: master->ext_ring_idx_rt) { fp@2498: ec_datagram_t *datagram = fp@2498: &master->ext_datagram_ring[master->ext_ring_idx_fsm]; fp@2498: return datagram; fp@2498: } fp@2498: else { fp@2498: return NULL; fp@2498: } 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@2374: ec_datagram_t *datagram /**< datagram */ fp@1500: ) fp@293: { fp@2085: ec_datagram_t *queued_datagram; fp@2085: fp@2085: /* It is possible, that a datagram in the queue is re-initialized with the fp@2085: * ec_datagram_() methods and then shall be queued with this method. fp@2085: * In that case, the state is already reset to EC_DATAGRAM_INIT. Check if fp@2085: * the datagram is queued to avoid duplicate queuing (which results in an fp@2085: * infinite loop!). Set the state to EC_DATAGRAM_QUEUED again, probably fp@2085: * causing an unmatched datagram. */ fp@2085: list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { fp@2085: if (queued_datagram == datagram) { fp@2085: datagram->skip_count++; fp@2532: #ifdef EC_RT_SYSLOG fp@2443: EC_MASTER_DBG(master, 1, fp@2443: "Datagram %p already queued (skipping).\n", datagram); fp@2532: #endif fp@325: datagram->state = EC_DATAGRAM_QUEUED; fp@2085: return; fp@2085: } fp@2085: } fp@2085: fp@2085: list_add_tail(&datagram->queue, &master->datagram_queue); fp@2085: 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@2268: /** Sends the datagrams in the queue for a certain device. fp@997: * fp@997: */ fp@2268: void ec_master_send_datagrams( fp@2268: ec_master_t *master, /**< EtherCAT master */ fp@2268: ec_device_index_t device_index /**< Device index. */ fp@2268: ) fp@293: { fp@398: ec_datagram_t *datagram, *next; fp@1804: size_t datagram_size; fp@2268: uint8_t *frame_data, *cur_data = NULL; 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@2268: EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n", fp@2268: __func__, device_index); fp@98: fp@176: do { fp@2268: frame_data = NULL; 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@2268: if (datagram->state != EC_DATAGRAM_QUEUED || fp@2268: datagram->device_index != device_index) { fp@2268: continue; fp@2268: } fp@2268: fp@2268: if (!frame_data) { fp@2268: // fetch pointer to transmit socket buffer fp@2268: frame_data = fp@2268: ec_device_tx_data(&master->devices[device_index]); fp@2268: cur_data = frame_data + EC_FRAME_HEADER_SIZE; fp@2268: } 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@2268: EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n", fp@1921: datagram->index); fp@293: fp@2268: // set "datagram following" flag in previous datagram fp@2268: if (follows_word) { fp@2267: EC_WRITE_U16(follows_word, fp@2267: EC_READ_U16(follows_word) | 0x8000); fp@2268: } fp@176: fp@293: // EtherCAT datagram header fp@2268: 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@1921: EC_MASTER_DBG(master, 2, "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@2267: - 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@1921: EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data); fp@176: fp@195: // send frame fp@2268: ec_device_send(&master->devices[device_index], fp@2267: 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@2268: EC_MASTER_DBG(master, 0, "%s()" fp@2268: " sent %u frames in %uus.\n", __func__, 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@2421: * fp@997: * \return 0 in case of success, else < 0 fp@997: */ fp@2530: void ec_master_receive_datagrams( fp@2530: ec_master_t *master, /**< EtherCAT master */ fp@2530: ec_device_t *device, /**< EtherCAT device */ fp@2530: const uint8_t *frame_data, /**< frame data */ fp@2530: size_t size /**< size of the received data */ fp@2530: ) 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@2530: if (master->debug_level || FORCE_OUTPUT_CORRUPTED) { fp@1921: EC_MASTER_DBG(master, 0, "Corrupted frame received" fp@2530: " on %s (size %zu < %u byte):\n", fp@2530: device->dev->name, size, EC_FRAME_HEADER_SIZE); fp@1303: ec_print_data(frame_data, size); fp@1303: } fp@98: master->stats.corrupted++; fp@2532: #ifdef EC_RT_SYSLOG fp@98: ec_master_output_stats(master); fp@2532: #endif 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@2530: if (master->debug_level || FORCE_OUTPUT_CORRUPTED) { fp@1921: EC_MASTER_DBG(master, 0, "Corrupted frame received" fp@2530: " on %s (invalid frame size %zu for " fp@2530: "received size %zu):\n", device->dev->name, fp@2530: frame_size, size); fp@1303: ec_print_data(frame_data, size); fp@1303: } fp@98: master->stats.corrupted++; fp@2532: #ifdef EC_RT_SYSLOG fp@98: ec_master_output_stats(master); fp@2532: #endif 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@2530: if (master->debug_level || FORCE_OUTPUT_CORRUPTED) { fp@1921: EC_MASTER_DBG(master, 0, "Corrupted frame received" fp@2530: " on %s (invalid data size %zu):\n", fp@2530: device->dev->name, data_size); fp@1303: ec_print_data(frame_data, size); fp@1303: } fp@98: master->stats.corrupted++; fp@2532: #ifdef EC_RT_SYSLOG fp@98: ec_master_output_stats(master); fp@2532: #endif 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@2532: #ifdef EC_RT_SYSLOG fp@98: ec_master_output_stats(master); fp@2532: #endif fp@684: fp@684: if (unlikely(master->debug_level > 0)) { fp@1921: EC_MASTER_DBG(master, 0, "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@2267: ec_device_debug_ring_print(&master->devices[EC_DEVICE_MAIN]); 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@2114: if (datagram->type != EC_DATAGRAM_APWR && fp@2114: datagram->type != EC_DATAGRAM_FPWR && fp@2114: datagram->type != EC_DATAGRAM_BWR && fp@2114: datagram->type != EC_DATAGRAM_LWR) { fp@2114: // copy received data into the datagram memory, fp@2114: // if something has been read fp@2114: memcpy(datagram->data, cur_data, data_size); fp@2114: } 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@2267: datagram->cycles_received = fp@2267: master->devices[EC_DEVICE_MAIN].cycles_poll; fp@2267: #endif fp@2267: datagram->jiffies_received = fp@2267: master->devices[EC_DEVICE_MAIN].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@1921: EC_MASTER_WARN(master, "%u datagram%s TIMED OUT!\n", fp@1921: 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@1921: EC_MASTER_WARN(master, "%u frame%s CORRUPTED!\n", fp@1921: 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@1921: EC_MASTER_WARN(master, "%u datagram%s UNMATCHED!\n", fp@1921: master->stats.unmatched, fp@396: master->stats.unmatched == 1 ? "" : "s"); fp@98: master->stats.unmatched = 0; fp@98: } fp@73: } fp@54: } fp@54: fp@2158: /*****************************************************************************/ fp@2158: fp@2158: /** Clears the common device statistics. fp@2158: */ fp@2158: void ec_master_clear_device_stats( fp@2158: ec_master_t *master /**< EtherCAT master */ fp@2158: ) fp@2158: { fp@2158: unsigned int i; fp@2158: fp@2158: // zero frame statistics fp@2158: master->device_stats.tx_count = 0; fp@2158: master->device_stats.last_tx_count = 0; fp@2158: master->device_stats.rx_count = 0; fp@2158: master->device_stats.last_rx_count = 0; fp@2158: master->device_stats.tx_bytes = 0; fp@2158: master->device_stats.last_tx_bytes = 0; fp@2158: master->device_stats.rx_bytes = 0; fp@2158: master->device_stats.last_rx_bytes = 0; fp@2158: master->device_stats.last_loss = 0; fp@2158: fp@2158: for (i = 0; i < EC_RATE_COUNT; i++) { fp@2158: master->device_stats.tx_frame_rates[i] = 0; fp@2688: master->device_stats.rx_frame_rates[i] = 0; fp@2158: master->device_stats.tx_byte_rates[i] = 0; fp@2688: master->device_stats.rx_byte_rates[i] = 0; fp@2158: master->device_stats.loss_rates[i] = 0; fp@2158: } fp@2688: fp@2688: master->device_stats.jiffies = 0; fp@2158: } fp@2158: fp@2158: /*****************************************************************************/ fp@2158: fp@2158: /** Updates the common device statistics. fp@2158: */ fp@2158: void ec_master_update_device_stats( fp@2158: ec_master_t *master /**< EtherCAT master */ fp@2158: ) fp@2158: { fp@2158: ec_device_stats_t *s = &master->device_stats; fp@2372: s32 tx_frame_rate, rx_frame_rate, tx_byte_rate, rx_byte_rate, loss_rate; fp@2158: u64 loss; fp@2453: unsigned int i, dev_idx; fp@2158: fp@2158: // frame statistics fp@2158: if (likely(jiffies - s->jiffies < HZ)) { fp@2158: return; fp@2158: } fp@2158: fp@2372: tx_frame_rate = (s->tx_count - s->last_tx_count) * 1000; fp@2372: rx_frame_rate = (s->rx_count - s->last_rx_count) * 1000; fp@2372: tx_byte_rate = s->tx_bytes - s->last_tx_bytes; fp@2372: rx_byte_rate = s->rx_bytes - s->last_rx_bytes; fp@2158: loss = s->tx_count - s->rx_count; fp@2372: loss_rate = (loss - s->last_loss) * 1000; fp@2372: fp@2372: /* Low-pass filter: fp@2372: * Y_n = y_(n - 1) + T / tau * (x - y_(n - 1)) | T = 1 fp@2372: * -> Y_n += (x - y_(n - 1)) / tau fp@2372: */ fp@2158: for (i = 0; i < EC_RATE_COUNT; i++) { fp@2372: s32 n = rate_intervals[i]; fp@2372: s->tx_frame_rates[i] += (tx_frame_rate - s->tx_frame_rates[i]) / n; fp@2372: s->rx_frame_rates[i] += (rx_frame_rate - s->rx_frame_rates[i]) / n; fp@2372: s->tx_byte_rates[i] += (tx_byte_rate - s->tx_byte_rates[i]) / n; fp@2372: s->rx_byte_rates[i] += (rx_byte_rate - s->rx_byte_rates[i]) / n; fp@2372: s->loss_rates[i] += (loss_rate - s->loss_rates[i]) / n; fp@2372: } fp@2372: fp@2158: s->last_tx_count = s->tx_count; fp@2158: s->last_rx_count = s->rx_count; fp@2158: s->last_tx_bytes = s->tx_bytes; fp@2158: s->last_rx_bytes = s->rx_bytes; fp@2371: s->last_loss = loss; fp@2158: fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2453: ec_device_update_stats(&master->devices[dev_idx]); fp@2453: } fp@2158: fp@2158: s->jiffies = jiffies; fp@2158: } 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@2498: /** Execute slave FSMs. fp@2498: */ fp@2498: void ec_master_exec_slave_fsms( fp@2498: ec_master_t *master /**< EtherCAT master. */ fp@2498: ) fp@2498: { fp@2498: ec_datagram_t *datagram; fp@2498: ec_fsm_slave_t *fsm, *next; fp@2498: unsigned int count = 0; fp@2498: fp@2498: list_for_each_entry_safe(fsm, next, &master->fsm_exec_list, list) { fp@2498: if (!fsm->datagram) { fp@2498: EC_MASTER_WARN(master, "Slave %u FSM has zero datagram." fp@2498: "This is a bug!\n", fsm->slave->ring_position); fp@2498: list_del_init(&fsm->list); fp@2498: master->fsm_exec_count--; fp@2498: return; fp@2498: } fp@2498: fp@2498: if (fsm->datagram->state == EC_DATAGRAM_INIT || fp@2498: fsm->datagram->state == EC_DATAGRAM_QUEUED || fp@2498: fsm->datagram->state == EC_DATAGRAM_SENT) { fp@2498: // previous datagram was not sent or received yet. fp@2498: // wait until next thread execution fp@2498: return; fp@2498: } fp@2498: fp@2498: datagram = ec_master_get_external_datagram(master); fp@2498: if (!datagram) { fp@2498: // no free datagrams at the moment fp@2498: EC_MASTER_WARN(master, "No free datagram during" fp@2498: " slave FSM execution. This is a bug!\n"); fp@2498: continue; fp@2498: } fp@2498: fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "Executing slave %u FSM.\n", fp@2498: fsm->slave->ring_position); fp@2498: #endif fp@2498: if (ec_fsm_slave_exec(fsm, datagram)) { fp@2498: // FSM consumed datagram fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "FSM consumed datagram %s\n", fp@2498: datagram->name); fp@2498: #endif fp@2498: master->ext_ring_idx_fsm = fp@2498: (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE; fp@2498: } fp@2498: else { fp@2498: // FSM finished fp@2498: list_del_init(&fsm->list); fp@2498: master->fsm_exec_count--; fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "FSM finished. %u remaining.\n", fp@2498: master->fsm_exec_count); fp@2498: #endif fp@2498: } fp@2498: } fp@2498: fp@2498: while (master->fsm_exec_count < EC_EXT_RING_SIZE / 2 fp@2498: && count < master->slave_count) { fp@2498: fp@2498: if (ec_fsm_slave_is_ready(&master->fsm_slave->fsm)) { fp@2498: datagram = ec_master_get_external_datagram(master); fp@2498: fp@2498: if (ec_fsm_slave_exec(&master->fsm_slave->fsm, datagram)) { fp@2498: master->ext_ring_idx_fsm = fp@2498: (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE; fp@2498: list_add_tail(&master->fsm_slave->fsm.list, fp@2498: &master->fsm_exec_list); fp@2498: master->fsm_exec_count++; fp@2498: #if DEBUG_INJECT fp@2498: EC_MASTER_DBG(master, 1, "New slave %u FSM" fp@2498: " consumed datagram %s, now %u FSMs in list.\n", fp@2498: master->fsm_slave->ring_position, datagram->name, fp@2498: master->fsm_exec_count); fp@2498: #endif fp@2498: } fp@2498: } fp@2498: fp@2498: master->fsm_slave++; fp@2498: if (master->fsm_slave >= master->slaves + master->slave_count) { fp@2498: master->fsm_slave = master->slaves; fp@2498: } fp@2498: count++; fp@2498: } fp@2498: } fp@2498: fp@2498: /*****************************************************************************/ fp@2498: 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: int fsm_exec; fp@2336: #ifdef EC_USE_HRTIMER fp@1804: size_t sent_bytes; fp@2336: #endif fp@1774: fp@1774: // send interval in IDLE phase fp@2421: ec_master_set_send_interval(master, 1000000 / HZ); fp@1804: fp@1930: EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us," fp@1930: " max data size=%zu\n", master->send_interval, fp@1921: 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: // execute master & slave state machines fp@2455: if (down_interruptible(&master->master_sem)) { fp@1241: break; fp@2455: } fp@2455: martin@1583: fsm_exec = ec_fsm_master_exec(&master->fsm); fp@2455: fp@2498: ec_master_exec_slave_fsms(master); fp@2455: fp@1075: up(&master->master_sem); fp@1031: fp@1031: // queue and send fp@1489: down(&master->io_sem); martin@1583: if (fsm_exec) { fp@2374: ec_master_queue_datagram(master, &master->fsm_datagram); martin@1583: } fp@1031: ecrt_master_send(master); fp@2336: #ifdef EC_USE_HRTIMER fp@2267: sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ fp@2267: master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; fp@2336: #endif 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@2421: fp@1921: EC_MASTER_DBG(master, 1, "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; fp@1773: fp@1921: EC_MASTER_DBG(master, 1, "Operation thread running" fp@1930: " with fsm interval = %u us, max data size=%zu\n", fp@1921: master->send_interval, 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: // execute master & slave state machines fp@2455: if (down_interruptible(&master->master_sem)) { martin@1583: break; fp@2455: } fp@2455: fp@2498: if (ec_fsm_master_exec(&master->fsm)) { fp@2498: // Inject datagrams (let the RT thread queue them, see fp@2498: // ecrt_master_send()) martin@1583: master->injection_seq_fsm++; fp@2455: } fp@2498: fp@2498: ec_master_exec_slave_fsms(master); fp@2498: fp@2498: up(&master->master_sem); 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@2421: fp@1921: EC_MASTER_DBG(master, 1, "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@1921: EC_MASTER_WARN(master, "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@1921: EC_MASTER_WARN(master, "No EoE processing" fp@1921: " because of missing callbacks!\n"); fp@251: return; fp@251: } fp@251: fp@1921: EC_MASTER_INFO(master, "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@1921: EC_MASTER_ERR(master, "Failed to start EoE thread (error %i)!\n", fp@1921: 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@1921: EC_MASTER_INFO(master, "Stopping EoE thread.\n"); fp@1489: fp@1489: kthread_stop(master->eoe_thread); fp@1489: master->eoe_thread = NULL; fp@1921: EC_MASTER_INFO(master, "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@1921: EC_MASTER_DBG(master, 1, "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@2421: fp@1921: EC_MASTER_DBG(master, 1, "EoE thread exiting...\n"); fp@1489: return 0; fp@197: } fp@715: #endif fp@197: fp@325: /*****************************************************************************/ fp@325: 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@1909: if (slave->effective_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@2522: * fp@2522: * \return Search result, or NULL. 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@2522: * fp@2522: * \return Search result, or NULL. 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@1921: EC_MASTER_ERR(master, "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@1921: EC_MASTER_INFO(master, "Master debug level set to %u.\n", fp@1921: 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@2447: if (master->dc_ref_config) { fp@2447: // Use application-selected reference clock fp@2447: slave = master->dc_ref_config->slave; fp@2447: fp@2447: if (slave) { fp@2447: if (slave->base_dc_supported && slave->has_dc_system_time) { fp@2447: ref = slave; fp@2447: } fp@2447: else { fp@2447: EC_MASTER_WARN(master, "Slave %u can not act as a" fp@2447: " DC reference clock!", slave->ring_position); fp@2447: } fp@2447: } fp@2447: else { fp@2447: EC_MASTER_WARN(master, "DC reference clock config (%u-%u)" fp@2447: " has no slave attached!\n", master->dc_ref_config->alias, fp@2447: master->dc_ref_config->position); fp@2447: } fp@2447: } fp@2447: else { fp@2447: // Use first slave with DC support as reference clock fp@2447: for (slave = master->slaves; fp@2447: slave < master->slaves + master->slave_count; fp@2447: slave++) { fp@2447: if (slave->base_dc_supported && slave->has_dc_system_time) { fp@2447: ref = slave; fp@2447: break; fp@2447: } fp@2447: } fp@2447: fp@1425: } fp@1425: fp@1425: master->dc_ref_clock = ref; fp@2421: fp@2447: if (ref) { fp@2447: EC_MASTER_INFO(master, "Using slave %u as DC reference clock.\n", fp@2447: ref->ring_position); fp@2447: } fp@2447: else { fp@2447: EC_MASTER_INFO(master, "No DC reference clock found.\n"); fp@2447: } fp@2447: fp@2447: // These calls always succeed, because the fp@2447: // datagrams have been pre-allocated. fp@2447: ec_datagram_fpwr(&master->ref_sync_datagram, fp@2447: ref ? ref->station_address : 0xffff, 0x0910, 4); 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@2522: * fp@2522: * \return Zero on success, otherwise a negative error code. 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@2360: unsigned int port_index; fp@1421: int ret; fp@1421: fp@2360: static const unsigned int next_table[EC_MAX_PORTS] = { fp@2360: 3, 2, 0, 1 fp@2360: }; fp@2360: fp@1425: slave->ports[0].next_slave = port0_slave; fp@1421: fp@2360: port_index = 3; fp@2360: while (port_index != 0) { fp@2360: if (!slave->ports[port_index].link.loop_closed) { fp@1421: *slave_position = *slave_position + 1; fp@1421: if (*slave_position < master->slave_count) { fp@2360: slave->ports[port_index].next_slave = fp@2360: master->slaves + *slave_position; fp@1421: ret = ec_master_calc_topology_rec(master, fp@1421: slave, slave_position); fp@2360: if (ret) { fp@1421: return ret; fp@2360: } fp@1421: } else { fp@1421: return -1; fp@1421: } fp@1421: } fp@2360: fp@2360: port_index = next_table[port_index]; 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@1921: EC_MASTER_ERR(master, "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@1925: /*****************************************************************************/ fp@1925: fp@1925: /** Request OP state for configured slaves. fp@1925: */ fp@1925: void ec_master_request_op( fp@1925: ec_master_t *master /**< EtherCAT master. */ fp@1925: ) fp@1925: { fp@1925: unsigned int i; fp@1925: ec_slave_t *slave; fp@1925: fp@1925: if (!master->active) fp@1925: return; fp@1925: fp@1925: EC_MASTER_DBG(master, 1, "Requesting OP...\n"); fp@1925: fp@1925: // request OP for all configured slaves fp@1925: for (i = 0; i < master->slave_count; i++) { fp@1925: slave = master->slaves + i; fp@1925: if (slave->config) { fp@1925: ec_slave_request_state(slave, EC_SLAVE_STATE_OP); fp@1925: } fp@1925: } fp@1925: fp@1925: // always set DC reference clock to OP fp@1925: if (master->dc_ref_clock) { fp@2476: ec_slave_request_state(master->dc_ref_clock, EC_SLAVE_STATE_OP); fp@1925: } fp@1925: } fp@1925: 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@2522: * fp@2522: * \return New domain, or 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@1921: EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n", fp@1921: master); fp@1181: fp@2267: if (!(domain = fp@2267: (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { fp@1921: EC_MASTER_ERR(master, "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@1921: EC_MASTER_DBG(master, 1, "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@1914: #ifdef EC_EOE fp@1914: int eoe_was_running; fp@1914: #endif fp@640: fp@1921: EC_MASTER_DBG(master, 1, "ecrt_master_activate(master = 0x%p)\n", master); fp@1181: fp@1530: if (master->active) { fp@1921: EC_MASTER_WARN(master, "%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@1921: EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain); fp@1313: return ret; fp@640: } fp@640: domain_offset += domain->data_size; fp@640: } fp@2421: fp@1075: up(&master->master_sem); fp@640: fp@656: // restart EoE process and master thread with new locking fp@1914: fp@1914: ec_master_thread_stop(master); fp@715: #ifdef EC_EOE fp@1914: eoe_was_running = master->eoe_thread != NULL; fp@656: ec_master_eoe_stop(master); fp@715: #endif fp@656: fp@1921: EC_MASTER_DBG(master, 1, "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@2421: fp@1914: #ifdef EC_EOE fp@1914: if (eoe_was_running) { fp@1914: ec_master_eoe_start(master); fp@1914: } fp@1914: #endif fp@1313: ret = ec_master_thread_start(master, ec_master_operation_thread, fp@1313: "EtherCAT-OP"); fp@1313: if (ret < 0) { fp@1921: EC_MASTER_ERR(master, "Failed to start master thread!\n"); fp@1313: return ret; fp@640: } fp@902: fp@2349: /* Allow scanning after a topology change. */ fp@2349: master->allow_scan = 1; fp@2349: fp@1530: master->active = 1; fp@1925: fp@1925: // notify state machine, that the configuration shall now be applied fp@1925: master->config_changed = 1; fp@1925: 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@1914: int eoe_was_running; fp@1530: #endif fp@1530: fp@1939: EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master); fp@1530: fp@1530: if (!master->active) { fp@1921: EC_MASTER_WARN(master, "%s: Master not active.\n", __func__); fp@1530: return; fp@1530: } fp@1530: fp@1914: ec_master_thread_stop(master); fp@1530: #ifdef EC_EOE fp@1914: eoe_was_running = master->eoe_thread != NULL; fp@1530: ec_master_eoe_stop(master); fp@1530: #endif fp@2421: 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@2421: fp@1939: ec_master_clear_config(master); 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@1925: master->has_app_time = 0; fp@1530: fp@1914: #ifdef EC_EOE fp@1914: if (eoe_was_running) { fp@1914: ec_master_eoe_start(master); fp@1914: } fp@1914: #endif fp@1530: if (ec_master_thread_start(master, ec_master_idle_thread, fp@2455: "EtherCAT-IDLE")) { fp@1921: EC_MASTER_WARN(master, "Failed to restart master thread!\n"); fp@2455: } fp@1530: fp@2349: /* Disallow scanning to get into the same state like after a master fp@2349: * request (after ec_master_enter_operation_phase() is called). */ fp@2349: master->allow_scan = 0; fp@2349: 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@2374: ec_device_index_t dev_idx; fp@640: fp@640: if (master->injection_seq_rt != master->injection_seq_fsm) { fp@2498: // inject datagram produced by master FSM fp@2374: ec_master_queue_datagram(master, &master->fsm_datagram); fp@640: master->injection_seq_rt = master->injection_seq_fsm; fp@640: } fp@2305: martin@1597: ec_master_inject_external_datagrams(master); fp@640: fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2374: if (unlikely(!master->devices[dev_idx].link_state)) { fp@2269: // link is down, no datagram can be sent fp@2269: list_for_each_entry_safe(datagram, n, fp@2269: &master->datagram_queue, queue) { fp@2374: if (datagram->device_index == dev_idx) { fp@2269: datagram->state = EC_DATAGRAM_ERROR; fp@2269: list_del_init(&datagram->queue); fp@2269: } fp@2269: } fp@2269: fp@2374: if (!master->devices[dev_idx].dev) { fp@2269: continue; fp@2269: } fp@2269: fp@2269: // query link state fp@2374: ec_device_poll(&master->devices[dev_idx]); fp@2269: fp@2269: // clear frame statistics fp@2374: ec_device_clear_stats(&master->devices[dev_idx]); fp@2370: continue; fp@2269: } fp@2269: fp@2269: // send frames fp@2374: ec_master_send_datagrams(master, dev_idx); fp@2268: } fp@640: } fp@640: fp@640: /*****************************************************************************/ fp@640: fp@792: void ecrt_master_receive(ec_master_t *master) fp@640: { fp@2453: unsigned int dev_idx; fp@640: ec_datagram_t *datagram, *next; fp@640: fp@640: // receive datagrams fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2453: ec_device_poll(&master->devices[dev_idx]); fp@2157: } fp@2158: ec_master_update_device_stats(master); 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@2267: if (master->devices[EC_DEVICE_MAIN].cycles_poll - fp@2267: datagram->cycles_sent > timeout_cycles) { fp@1040: #else fp@2267: if (master->devices[EC_DEVICE_MAIN].jiffies_poll - fp@2267: datagram->jiffies_sent > 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@2532: fp@2532: #ifdef EC_RT_SYSLOG 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@2267: time_us = (unsigned int) fp@2267: (master->devices[EC_DEVICE_MAIN].cycles_poll - fp@1237: datagram->cycles_sent) * 1000 / cpu_khz; fp@1237: #else fp@2267: time_us = (unsigned int) fp@2267: ((master->devices[EC_DEVICE_MAIN].jiffies_poll - fp@1279: datagram->jiffies_sent) * 1000000 / HZ); fp@1237: #endif fp@1921: EC_MASTER_DBG(master, 0, "TIMED OUT datagram %p," fp@1921: " index %02X waited %u us.\n", fp@1543: datagram, datagram->index, time_us); fp@684: } fp@2532: #endif /* RT_SYSLOG */ 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@2374: 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@1921: EC_MASTER_DBG(master, 1, "ecrt_master_slave_config(master = 0x%p," fp@1921: " alias = %u, position = %u, vendor_id = 0x%08x," fp@1921: " product_code = 0x%08x)\n", fp@1921: 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@1921: EC_MASTER_ERR(master, "Slave type mismatch. Slave was" fp@1921: " configured as 0x%08X/0x%08X before. Now configuring" fp@1921: " with 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@1921: EC_MASTER_DBG(master, 1, "Creating slave configuration for %u:%u," fp@1921: " 0x%08X/0x%08X.\n", fp@1921: 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@1921: EC_MASTER_ERR(master, "Failed to allocate memory" fp@1921: " 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@2447: int ecrt_master_select_reference_clock(ec_master_t *master, fp@2447: ec_slave_config_t *sc) fp@2447: { fp@2447: if (sc) { fp@2447: ec_slave_t *slave = sc->slave; fp@2447: fp@2447: // output an early warning fp@2447: if (slave && fp@2447: (!slave->base_dc_supported || !slave->has_dc_system_time)) { fp@2447: EC_MASTER_WARN(master, "Slave %u can not act as" fp@2447: " a reference clock!", slave->ring_position); fp@2447: } fp@2447: } fp@2447: fp@2447: master->dc_ref_config = sc; fp@2447: return 0; fp@2447: } fp@2447: fp@2447: /*****************************************************************************/ fp@2447: fp@1594: int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) fp@1594: { fp@1921: EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p," fp@1921: " master_info = 0x%p)\n", master, master_info); fp@1594: fp@1594: master_info->slave_count = master->slave_count; fp@2267: master_info->link_up = master->devices[EC_DEVICE_MAIN].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@2340: unsigned int i; fp@2541: int ret = 0; 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@2541: if (slave == NULL) { fp@2541: ret = -ENOENT; fp@2541: goto out_get_slave; fp@2541: } fp@2541: 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@1909: slave_info->alias = slave->effective_alias; fp@1594: slave_info->current_on_ebus = slave->sii.current_on_ebus; fp@2340: fp@2340: for (i = 0; i < EC_MAX_PORTS; i++) { fp@2340: slave_info->ports[i].desc = slave->ports[i].desc; fp@2340: slave_info->ports[i].link.link_up = slave->ports[i].link.link_up; fp@2340: slave_info->ports[i].link.loop_closed = fp@2340: slave->ports[i].link.loop_closed; fp@2340: slave_info->ports[i].link.signal_detected = fp@2340: slave->ports[i].link.signal_detected; fp@2340: slave_info->ports[i].receive_time = slave->ports[i].receive_time; fp@2340: if (slave->ports[i].next_slave) { fp@2340: slave_info->ports[i].next_slave = fp@2340: slave->ports[i].next_slave->ring_position; fp@2340: } else { fp@2340: slave_info->ports[i].next_slave = 0xffff; fp@2340: } fp@2340: slave_info->ports[i].delay_to_next_dc = fp@2340: slave->ports[i].delay_to_next_dc; fp@2340: } fp@2340: 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@2541: out_get_slave: fp@1594: up(&master->master_sem); fp@1594: fp@2541: return ret; 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@1921: EC_MASTER_DBG(master, 1, "ecrt_master_callbacks(master = 0x%p," fp@1921: " send_cb = 0x%p, receive_cb = 0x%p, cb_data = 0x%p)\n", fp@1921: master, 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@2374: ec_device_index_t dev_idx; fp@2374: fp@2374: state->slaves_responding = 0U; fp@2374: state->al_states = 0; fp@2374: state->link_up = 0U; fp@2374: fp@2453: for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); fp@2453: dev_idx++) { fp@2374: /* Announce sum of responding slaves on all links. */ fp@2374: state->slaves_responding += master->fsm.slaves_responding[dev_idx]; fp@2374: fp@2374: /* Binary-or slave states of all links. */ fp@2374: state->al_states |= master->fsm.slave_states[dev_idx]; fp@2374: fp@2374: /* Signal link up if at least one device has link. */ fp@2374: state->link_up |= master->devices[dev_idx].link_state; fp@2374: } fp@612: } fp@612: fp@612: /*****************************************************************************/ fp@612: fp@2380: int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx, fp@2380: ec_master_link_state_t *state) fp@2380: { fp@2453: if (dev_idx >= ec_master_num_devices(master)) { fp@2380: return -EINVAL; fp@2380: } fp@2380: fp@2380: state->slaves_responding = master->fsm.slaves_responding[dev_idx]; fp@2380: state->al_states = master->fsm.slave_states[dev_idx]; fp@2380: state->link_up = master->devices[dev_idx].link_state; fp@2380: fp@2380: return 0; fp@2380: } fp@2380: fp@2380: /*****************************************************************************/ fp@2380: 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@1925: if (unlikely(!master->has_app_time)) { fp@1436: master->app_start_time = app_time; fp@1925: master->has_app_time = 1; fp@1436: } fp@1434: } fp@1434: fp@1434: /*****************************************************************************/ fp@1434: fp@2447: int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) fp@2447: { fp@2447: if (!master->dc_ref_clock) { fp@2447: return -ENXIO; fp@2447: } fp@2447: fp@2447: if (master->sync_datagram.state != EC_DATAGRAM_RECEIVED) { fp@2447: return -EIO; fp@2447: } fp@2447: fp@2447: // Get returned datagram time, transmission delay removed. fp@2447: *time = EC_READ_U32(master->sync_datagram.data) - fp@2447: master->dc_ref_clock->transmission_delay; fp@2447: fp@2447: return 0; fp@2447: } fp@2447: fp@2447: /*****************************************************************************/ fp@2447: fp@1434: void ecrt_master_sync_reference_clock(ec_master_t *master) fp@1434: { fp@2476: if (master->dc_ref_clock) { fp@2476: EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); fp@2476: ec_master_queue_datagram(master, &master->ref_sync_datagram); fp@2476: } fp@1410: } fp@1410: fp@1410: /*****************************************************************************/ fp@1410: fp@1410: void ecrt_master_sync_slave_clocks(ec_master_t *master) fp@1410: { fp@2476: if (master->dc_ref_clock) { fp@2476: ec_datagram_zero(&master->sync_datagram); fp@2476: ec_master_queue_datagram(master, &master->sync_datagram); fp@2476: } 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@2374: 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@2109: int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, fp@2109: uint16_t index, uint8_t subindex, uint8_t *data, fp@2109: size_t data_size, uint32_t *abort_code) fp@2109: { fp@2461: ec_sdo_request_t request; fp@2461: ec_slave_t *slave; fp@2467: int ret; fp@2109: fp@2109: EC_MASTER_DBG(master, 1, "%s(master = 0x%p," fp@2109: " slave_position = %u, index = 0x%04X, subindex = 0x%02X," fp@2109: " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", fp@2109: __func__, master, slave_position, index, subindex, fp@2109: data, data_size, abort_code); fp@2109: fp@2109: if (!data_size) { fp@2109: EC_MASTER_ERR(master, "Zero data size!\n"); fp@2109: return -EINVAL; fp@2109: } fp@2109: fp@2461: ec_sdo_request_init(&request); fp@2461: ecrt_sdo_request_index(&request, index, subindex); fp@2467: ret = ec_sdo_request_alloc(&request, data_size); fp@2467: if (ret) { fp@2461: ec_sdo_request_clear(&request); fp@2467: return ret; fp@2109: } fp@2109: fp@2461: memcpy(request.data, data, data_size); fp@2461: request.data_size = data_size; fp@2461: ecrt_sdo_request_write(&request); fp@2461: fp@2461: if (down_interruptible(&master->master_sem)) { fp@2461: ec_sdo_request_clear(&request); fp@2109: return -EINTR; fp@2461: } fp@2461: fp@2461: if (!(slave = ec_master_find_slave(master, 0, slave_position))) { fp@2109: up(&master->master_sem); fp@2109: EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); fp@2461: ec_sdo_request_clear(&request); fp@2109: return -EINVAL; fp@2109: } fp@2109: fp@2467: EC_SLAVE_DBG(slave, 1, "Scheduling SDO download request.\n"); fp@2109: fp@2109: // schedule request. fp@2461: list_add_tail(&request.list, &slave->sdo_requests); fp@2109: fp@2109: up(&master->master_sem); fp@2109: fp@2109: // wait for processing through FSM fp@2467: if (wait_event_interruptible(master->request_queue, fp@2461: request.state != EC_INT_REQUEST_QUEUED)) { fp@2109: // interrupted by signal fp@2109: down(&master->master_sem); fp@2461: if (request.state == EC_INT_REQUEST_QUEUED) { fp@2109: list_del(&request.list); fp@2109: up(&master->master_sem); fp@2461: ec_sdo_request_clear(&request); fp@2109: return -EINTR; fp@2109: } fp@2109: // request already processing: interrupt not possible. fp@2109: up(&master->master_sem); fp@2109: } fp@2109: fp@2109: // wait until master FSM has finished processing fp@2467: wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY); fp@2461: fp@2461: *abort_code = request.abort_code; fp@2461: fp@2461: if (request.state == EC_INT_REQUEST_SUCCESS) { fp@2467: ret = 0; fp@2461: } else if (request.errno) { fp@2467: ret = -request.errno; fp@2109: } else { fp@2467: ret = -EIO; fp@2467: } fp@2467: fp@2467: ec_sdo_request_clear(&request); fp@2467: return ret; fp@2109: } fp@2109: fp@2109: /*****************************************************************************/ fp@2109: fp@2124: int ecrt_master_sdo_download_complete(ec_master_t *master, fp@2124: uint16_t slave_position, uint16_t index, uint8_t *data, fp@2124: size_t data_size, uint32_t *abort_code) fp@2124: { fp@2461: ec_sdo_request_t request; fp@2461: ec_slave_t *slave; fp@2467: int ret; fp@2124: fp@2124: EC_MASTER_DBG(master, 1, "%s(master = 0x%p," fp@2124: " slave_position = %u, index = 0x%04X," fp@2124: " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n", fp@2124: __func__, master, slave_position, index, data, data_size, fp@2124: abort_code); fp@2124: fp@2124: if (!data_size) { fp@2124: EC_MASTER_ERR(master, "Zero data size!\n"); fp@2124: return -EINVAL; fp@2124: } fp@2124: fp@2461: ec_sdo_request_init(&request); fp@2461: ecrt_sdo_request_index(&request, index, 0); fp@2467: ret = ec_sdo_request_alloc(&request, data_size); fp@2467: if (ret) { fp@2461: ec_sdo_request_clear(&request); fp@2467: return ret; fp@2124: } fp@2124: fp@2461: request.complete_access = 1; fp@2461: memcpy(request.data, data, data_size); fp@2461: request.data_size = data_size; fp@2461: ecrt_sdo_request_write(&request); fp@2461: fp@2461: if (down_interruptible(&master->master_sem)) { fp@2461: ec_sdo_request_clear(&request); fp@2124: return -EINTR; fp@2461: } fp@2461: fp@2461: if (!(slave = ec_master_find_slave(master, 0, slave_position))) { fp@2124: up(&master->master_sem); fp@2124: EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); fp@2461: ec_sdo_request_clear(&request); fp@2124: return -EINVAL; fp@2124: } fp@2124: fp@2467: EC_SLAVE_DBG(slave, 1, "Scheduling SDO download request" fp@2124: " (complete access).\n"); fp@2124: fp@2124: // schedule request. fp@2461: list_add_tail(&request.list, &slave->sdo_requests); fp@2124: fp@2124: up(&master->master_sem); fp@2124: fp@2124: // wait for processing through FSM fp@2467: if (wait_event_interruptible(master->request_queue, fp@2461: request.state != EC_INT_REQUEST_QUEUED)) { fp@2124: // interrupted by signal fp@2124: down(&master->master_sem); fp@2461: if (request.state == EC_INT_REQUEST_QUEUED) { fp@2124: list_del(&request.list); fp@2124: up(&master->master_sem); fp@2461: ec_sdo_request_clear(&request); fp@2124: return -EINTR; fp@2124: } fp@2124: // request already processing: interrupt not possible. fp@2124: up(&master->master_sem); fp@2124: } fp@2124: fp@2124: // wait until master FSM has finished processing fp@2467: wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY); fp@2124: fp@2461: *abort_code = request.abort_code; fp@2461: fp@2461: if (request.state == EC_INT_REQUEST_SUCCESS) { fp@2467: ret = 0; fp@2461: } else if (request.errno) { fp@2467: ret = -request.errno; fp@2124: } else { fp@2467: ret = -EIO; fp@2467: } fp@2467: fp@2467: ec_sdo_request_clear(&request); fp@2467: return ret; fp@2124: } fp@2124: fp@2124: /*****************************************************************************/ fp@2124: fp@2109: int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, fp@2109: uint16_t index, uint8_t subindex, uint8_t *target, fp@2109: size_t target_size, size_t *result_size, uint32_t *abort_code) fp@2109: { fp@2461: ec_sdo_request_t request; fp@2461: ec_slave_t *slave; fp@2467: int ret = 0; fp@2109: fp@2109: EC_MASTER_DBG(master, 1, "%s(master = 0x%p," fp@2109: " slave_position = %u, index = 0x%04X, subindex = 0x%02X," fp@2109: " target = 0x%p, target_size = %zu, result_size = 0x%p," fp@2109: " abort_code = 0x%p)\n", fp@2109: __func__, master, slave_position, index, subindex, fp@2109: target, target_size, result_size, abort_code); fp@2109: fp@2461: ec_sdo_request_init(&request); fp@2461: ecrt_sdo_request_index(&request, index, subindex); fp@2461: ecrt_sdo_request_read(&request); fp@2109: fp@2109: if (down_interruptible(&master->master_sem)) { fp@2461: ec_sdo_request_clear(&request); fp@2109: return -EINTR; fp@2109: } fp@2109: fp@2461: if (!(slave = ec_master_find_slave(master, 0, slave_position))) { fp@2109: up(&master->master_sem); fp@2461: ec_sdo_request_clear(&request); fp@2109: EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); fp@2109: return -EINVAL; fp@2109: } fp@2109: fp@2467: EC_SLAVE_DBG(slave, 1, "Scheduling SDO upload request.\n"); fp@2109: fp@2109: // schedule request. fp@2461: list_add_tail(&request.list, &slave->sdo_requests); fp@2109: fp@2109: up(&master->master_sem); fp@2109: fp@2109: // wait for processing through FSM fp@2467: if (wait_event_interruptible(master->request_queue, fp@2461: request.state != EC_INT_REQUEST_QUEUED)) { fp@2109: // interrupted by signal fp@2109: down(&master->master_sem); fp@2461: if (request.state == EC_INT_REQUEST_QUEUED) { fp@2109: list_del(&request.list); fp@2109: up(&master->master_sem); fp@2461: ec_sdo_request_clear(&request); fp@2109: return -EINTR; fp@2109: } fp@2109: // request already processing: interrupt not possible. fp@2109: up(&master->master_sem); fp@2109: } fp@2109: fp@2109: // wait until master FSM has finished processing fp@2467: wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY); fp@2461: fp@2461: *abort_code = request.abort_code; fp@2461: fp@2461: if (request.state != EC_INT_REQUEST_SUCCESS) { fp@2109: *result_size = 0; fp@2461: if (request.errno) { fp@2467: ret = -request.errno; fp@2109: } else { fp@2467: ret = -EIO; fp@2109: } fp@2109: } else { fp@2461: if (request.data_size > target_size) { fp@2693: EC_SLAVE_ERR(slave, "%s(): Buffer too small.\n", __func__); fp@2467: ret = -EOVERFLOW; fp@2467: } fp@2467: else { fp@2467: memcpy(target, request.data, request.data_size); fp@2467: *result_size = request.data_size; fp@2467: ret = 0; fp@2467: } fp@2461: } fp@2461: fp@2461: ec_sdo_request_clear(&request); fp@2467: return ret; fp@2109: } fp@2109: fp@2109: /*****************************************************************************/ fp@2109: fp@1947: int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, fp@1952: uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size, fp@1952: uint16_t *error_code) fp@1947: { fp@2465: ec_soe_request_t request; fp@2465: ec_slave_t *slave; fp@2465: int ret; fp@1947: fp@1952: if (drive_no > 7) { fp@1952: EC_MASTER_ERR(master, "Invalid drive number!\n"); fp@1952: return -EINVAL; fp@1952: } fp@1952: fp@2465: ec_soe_request_init(&request); fp@2465: ec_soe_request_set_drive_no(&request, drive_no); fp@2465: ec_soe_request_set_idn(&request, idn); fp@2465: fp@2465: ret = ec_soe_request_alloc(&request, data_size); fp@2465: if (ret) { fp@2465: ec_soe_request_clear(&request); fp@2465: return ret; fp@2465: } fp@2465: fp@2465: memcpy(request.data, data, data_size); fp@2465: request.data_size = data_size; fp@2465: ec_soe_request_write(&request); fp@2465: fp@2465: if (down_interruptible(&master->master_sem)) { fp@2465: ec_soe_request_clear(&request); fp@1947: return -EINTR; fp@2465: } fp@2465: fp@2465: if (!(slave = ec_master_find_slave(master, 0, slave_position))) { fp@1947: up(&master->master_sem); fp@1947: EC_MASTER_ERR(master, "Slave %u does not exist!\n", fp@1947: slave_position); fp@2465: ec_soe_request_clear(&request); fp@1947: return -EINVAL; fp@1947: } fp@1947: fp@2465: EC_SLAVE_DBG(slave, 1, "Scheduling SoE write request.\n"); fp@1947: fp@1947: // schedule SoE write request. fp@2465: list_add_tail(&request.list, &slave->soe_requests); fp@1947: fp@1947: up(&master->master_sem); fp@1947: fp@1947: // wait for processing through FSM fp@2467: if (wait_event_interruptible(master->request_queue, fp@2465: request.state != EC_INT_REQUEST_QUEUED)) { fp@1947: // interrupted by signal fp@1947: down(&master->master_sem); fp@2465: if (request.state == EC_INT_REQUEST_QUEUED) { fp@1947: // abort request fp@1947: list_del(&request.list); fp@1947: up(&master->master_sem); fp@2465: ec_soe_request_clear(&request); fp@1947: return -EINTR; fp@1947: } fp@1947: up(&master->master_sem); fp@1947: } fp@1947: fp@1947: // wait until master FSM has finished processing fp@2467: wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY); fp@1947: fp@1947: if (error_code) { fp@2465: *error_code = request.error_code; fp@2465: } fp@2465: ret = request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; fp@2465: ec_soe_request_clear(&request); fp@2465: fp@2465: return ret; fp@1947: } fp@1947: fp@1947: /*****************************************************************************/ fp@1947: fp@1947: int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, fp@1952: uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size, fp@1947: size_t *result_size, uint16_t *error_code) fp@1947: { fp@2465: ec_soe_request_t request; fp@2465: ec_slave_t *slave; fp@2465: int ret; fp@1947: fp@1952: if (drive_no > 7) { fp@1952: EC_MASTER_ERR(master, "Invalid drive number!\n"); fp@1952: return -EINVAL; fp@1952: } fp@1952: fp@2465: ec_soe_request_init(&request); fp@2465: ec_soe_request_set_drive_no(&request, drive_no); fp@2465: ec_soe_request_set_idn(&request, idn); fp@2465: ec_soe_request_read(&request); fp@2465: fp@2465: if (down_interruptible(&master->master_sem)) { fp@2465: ec_soe_request_clear(&request); fp@1947: return -EINTR; fp@2465: } fp@2465: fp@2465: if (!(slave = ec_master_find_slave(master, 0, slave_position))) { fp@1947: up(&master->master_sem); fp@2465: ec_soe_request_clear(&request); fp@1947: EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); fp@1947: return -EINVAL; fp@1947: } fp@1947: fp@2467: EC_SLAVE_DBG(slave, 1, "Scheduling SoE read request.\n"); fp@2467: fp@1947: // schedule request. fp@2465: list_add_tail(&request.list, &slave->soe_requests); fp@1947: fp@1947: up(&master->master_sem); fp@1947: fp@1947: // wait for processing through FSM fp@2467: if (wait_event_interruptible(master->request_queue, fp@2465: request.state != EC_INT_REQUEST_QUEUED)) { fp@1947: // interrupted by signal fp@1947: down(&master->master_sem); fp@2465: if (request.state == EC_INT_REQUEST_QUEUED) { fp@1947: list_del(&request.list); fp@1947: up(&master->master_sem); fp@2465: ec_soe_request_clear(&request); fp@1947: return -EINTR; fp@1947: } fp@1947: // request already processing: interrupt not possible. fp@1947: up(&master->master_sem); fp@1947: } fp@1947: fp@1947: // wait until master FSM has finished processing fp@2467: wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY); fp@1947: fp@1947: if (error_code) { fp@2465: *error_code = request.error_code; fp@2465: } fp@2465: fp@2465: if (request.state != EC_INT_REQUEST_SUCCESS) { fp@1947: if (result_size) { fp@1947: *result_size = 0; fp@1947: } fp@2465: ret = -EIO; fp@2465: } else { // success fp@2465: if (request.data_size > target_size) { fp@2693: EC_SLAVE_ERR(slave, "%s(): Buffer too small.\n", __func__); fp@2465: ret = -EOVERFLOW; fp@2465: } fp@2465: else { // data fits in buffer fp@2465: if (result_size) { fp@2465: *result_size = request.data_size; fp@2465: } fp@2465: memcpy(target, request.data, request.data_size); fp@2465: ret = 0; fp@2465: } fp@2465: } fp@2465: fp@2465: ec_soe_request_clear(&request); fp@2465: return ret; fp@1947: } fp@1947: fp@1947: /*****************************************************************************/ fp@1947: fp@2009: void ecrt_master_reset(ec_master_t *master) fp@2009: { fp@2009: ec_slave_config_t *sc; fp@2009: fp@2009: list_for_each_entry(sc, &master->configs, list) { fp@2009: if (sc->slave) { fp@2009: ec_slave_request_state(sc->slave, EC_SLAVE_STATE_OP); fp@2009: } fp@2009: } fp@2009: } fp@2009: fp@2009: /*****************************************************************************/ fp@2009: 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@2447: EXPORT_SYMBOL(ecrt_master_select_reference_clock); fp@792: EXPORT_SYMBOL(ecrt_master_state); fp@2380: EXPORT_SYMBOL(ecrt_master_link_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@2447: EXPORT_SYMBOL(ecrt_master_reference_clock_time); fp@1535: EXPORT_SYMBOL(ecrt_master_sync_monitor_queue); fp@1535: EXPORT_SYMBOL(ecrt_master_sync_monitor_process); fp@2109: EXPORT_SYMBOL(ecrt_master_sdo_download); fp@2124: EXPORT_SYMBOL(ecrt_master_sdo_download_complete); fp@2109: EXPORT_SYMBOL(ecrt_master_sdo_upload); fp@1947: EXPORT_SYMBOL(ecrt_master_write_idn); fp@1947: EXPORT_SYMBOL(ecrt_master_read_idn); fp@2009: EXPORT_SYMBOL(ecrt_master_reset); fp@42: fp@199: /** \endcond */ fp@199: fp@199: /*****************************************************************************/