fp@39: /****************************************************************************** fp@0: * fp@39: * $Id$ fp@0: * fp@197: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@197: * fp@197: * This file is part of the IgH EtherCAT Master. fp@197: * fp@197: * The IgH EtherCAT Master is free software; you can redistribute it fp@197: * and/or modify it under the terms of the GNU General Public License fp@246: * as published by the Free Software Foundation; either version 2 of the fp@246: * License, or (at your option) any later version. fp@197: * fp@197: * The IgH EtherCAT Master is distributed in the hope that it will be fp@197: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@197: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@197: * GNU General Public License for more details. fp@197: * fp@197: * You should have received a copy of the GNU General Public License fp@197: * along 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@246: * The right to use EtherCAT Technology is granted and comes free of fp@246: * charge under condition of compatibility of product made by fp@246: * Licensee. People intending to distribute/sell products based on the fp@246: * code, have to sign an agreement to guarantee that products using fp@246: * software based on IgH EtherCAT master stay compatible with the actual fp@246: * EtherCAT specification (which are released themselves as an open fp@246: * standard) as the (only) precondition to have the right to use EtherCAT fp@246: * Technology, IP and trade marks. fp@246: * fp@39: *****************************************************************************/ fp@0: fp@199: /** fp@199: \file fp@199: EtherCAT master methods. fp@199: */ fp@199: fp@199: /*****************************************************************************/ fp@199: fp@24: #include fp@0: #include fp@0: #include fp@0: #include fp@0: #include fp@1015: #include fp@1015: #include fp@0: 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: 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@251: void ec_master_eoe_run(unsigned long); fp@715: #endif fp@179: fp@179: /*****************************************************************************/ fp@179: 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@1013: struct class *class /**< Device class. */ fp@576: ) fp@178: { fp@178: master->index = index; fp@647: master->reserved = 0; fp@446: fp@1075: init_MUTEX(&master->master_sem); fp@1075: fp@639: master->main_mac = main_mac; fp@639: master->backup_mac = backup_mac; fp@381: init_MUTEX(&master->device_sem); fp@446: fp@1029: master->phase = EC_ORPHANED; fp@637: master->injection_seq_fsm = 0; fp@637: master->injection_seq_rt = 0; fp@446: fp@1000: master->slaves = NULL; fp@446: master->slave_count = 0; fp@656: fp@792: INIT_LIST_HEAD(&master->configs); fp@792: fp@900: master->scan_busy = 0; fp@656: master->allow_scan = 1; fp@656: init_MUTEX(&master->scan_sem); fp@656: init_waitqueue_head(&master->scan_queue); fp@656: fp@900: master->config_busy = 0; fp@656: master->allow_config = 1; fp@656: init_MUTEX(&master->config_sem); fp@656: init_waitqueue_head(&master->config_queue); fp@656: fp@293: INIT_LIST_HEAD(&master->datagram_queue); fp@446: master->datagram_index = 0; fp@446: fp@95: INIT_LIST_HEAD(&master->domains); fp@792: fp@446: master->debug_level = 0; 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@792: master->frames_timed_out = 0; fp@446: fp@1241: master->thread = NULL; fp@1241: fp@1041: #ifdef EC_EOE fp@226: init_timer(&master->eoe_timer); fp@251: master->eoe_timer.function = ec_master_eoe_run; fp@226: master->eoe_timer.data = (unsigned long) master; fp@251: master->eoe_running = 0; fp@446: INIT_LIST_HEAD(&master->eoe_handlers); fp@715: #endif fp@446: fp@446: master->internal_lock = SPIN_LOCK_UNLOCKED; fp@446: master->request_cb = NULL; fp@446: master->release_cb = NULL; fp@446: master->cb_data = NULL; fp@446: fp@872: INIT_LIST_HEAD(&master->sii_requests); fp@872: init_waitqueue_head(&master->sii_queue); fp@601: fp@849: INIT_LIST_HEAD(&master->slave_sdo_requests); fp@646: init_waitqueue_head(&master->sdo_queue); fp@251: fp@1200: INIT_LIST_HEAD(&master->phy_requests); fp@1200: init_waitqueue_head(&master->phy_queue); fp@1200: fp@579: // init devices fp@579: if (ec_device_init(&master->main_device, master)) fp@997: goto out_return; fp@579: fp@579: if (ec_device_init(&master->backup_device, master)) fp@579: goto out_clear_main; fp@579: fp@528: // init state machine datagram fp@528: ec_datagram_init(&master->fsm_datagram); fp@719: snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm"); fp@528: if (ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE)) { fp@528: EC_ERR("Failed to allocate FSM datagram.\n"); fp@659: goto out_clear_backup; fp@528: } fp@528: fp@251: // create state machine object fp@528: ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); fp@226: fp@997: // init character device fp@997: if (ec_cdev_init(&master->cdev, master, device_number)) fp@659: goto out_clear_fsm; fp@1013: fp@1015: #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 15) fp@1013: master->class_device = class_device_create(class, fp@1013: MKDEV(MAJOR(device_number), master->index), fp@1013: NULL, "EtherCAT%u", master->index); fp@1015: #else fp@1015: master->class_device = class_device_create(class, NULL, fp@1015: MKDEV(MAJOR(device_number), master->index), fp@1015: NULL, "EtherCAT%u", master->index); fp@1015: #endif fp@1013: if (IS_ERR(master->class_device)) { fp@1013: EC_ERR("Failed to create class device!\n"); fp@1013: goto out_clear_cdev; fp@1013: } fp@144: fp@178: return 0; fp@226: fp@1013: out_clear_cdev: fp@1013: ec_cdev_clear(&master->cdev); fp@659: out_clear_fsm: fp@659: ec_fsm_master_clear(&master->fsm); fp@997: ec_datagram_clear(&master->fsm_datagram); fp@659: out_clear_backup: fp@579: ec_device_clear(&master->backup_device); fp@579: out_clear_main: fp@579: ec_device_clear(&master->main_device); fp@579: out_return: fp@226: return -1; 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@1013: class_device_unregister(master->class_device); fp@995: ec_cdev_clear(&master->cdev); 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@528: ec_fsm_master_clear(&master->fsm); fp@528: ec_datagram_clear(&master->fsm_datagram); fp@661: ec_device_clear(&master->backup_device); fp@661: ec_device_clear(&master->main_device); fp@661: } fp@661: fp@661: /*****************************************************************************/ fp@661: fp@715: #ifdef EC_EOE fp@792: /** Clear and free all EoE handlers. fp@661: */ fp@661: void ec_master_clear_eoe_handlers( fp@661: ec_master_t *master /**< EtherCAT master */ fp@661: ) fp@661: { fp@661: ec_eoe_t *eoe, *next; fp@661: fp@661: list_for_each_entry_safe(eoe, next, &master->eoe_handlers, list) { fp@251: list_del(&eoe->list); fp@251: ec_eoe_clear(eoe); fp@251: kfree(eoe); fp@251: } fp@73: } fp@715: #endif fp@73: fp@73: /*****************************************************************************/ fp@73: fp@995: /** Clear all slave configurations. fp@792: */ fp@995: void ec_master_clear_slave_configs(ec_master_t *master) fp@792: { fp@792: ec_slave_config_t *sc, *next; fp@792: fp@792: list_for_each_entry_safe(sc, next, &master->configs, list) { fp@792: list_del(&sc->list); fp@995: ec_slave_config_clear(sc); fp@995: kfree(sc); fp@792: } fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@997: /** Clear all slaves. fp@792: */ fp@992: void ec_master_clear_slaves(ec_master_t *master) fp@238: { fp@1000: ec_slave_t *slave; fp@1000: fp@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@997: /** Internal locking callback. fp@997: */ fp@515: int ec_master_request_cb(void *master /**< callback data */) fp@515: { fp@515: spin_lock(&((ec_master_t *) master)->internal_lock); fp@515: return 0; fp@515: } fp@515: fp@515: /*****************************************************************************/ fp@515: fp@997: /** Internal unlocking callback. fp@997: */ fp@515: void ec_master_release_cb(void *master /**< callback data */) fp@515: { fp@515: spin_unlock(&((ec_master_t *) master)->internal_lock); fp@515: } fp@515: fp@515: /*****************************************************************************/ fp@515: fp@997: /** Starts the master thread. fp@997: */ fp@656: int ec_master_thread_start( fp@656: ec_master_t *master, /**< EtherCAT master */ fp@1241: int (*thread_func)(void *), /**< thread function to start */ fp@1241: const char *name /**< Thread name. */ fp@656: ) fp@656: { fp@1241: EC_INFO("Starting %s thread.\n", name); fp@1241: master->thread = kthread_run(thread_func, master, name); fp@1241: if (IS_ERR(master->thread)) { fp@1241: EC_ERR("Failed to start master thread (error %i)!\n", fp@1241: (int) PTR_ERR(master->thread)); fp@1241: master->thread = NULL; fp@525: return -1; fp@1241: } fp@525: fp@525: return 0; fp@525: } fp@525: fp@525: /*****************************************************************************/ fp@525: fp@997: /** Stops the master thread. fp@997: */ fp@1029: void ec_master_thread_stop( fp@1029: ec_master_t *master /**< EtherCAT master */ fp@1029: ) fp@525: { fp@1040: unsigned long sleep_jiffies; fp@1040: fp@1241: if (!master->thread) { fp@1241: EC_WARN("ec_master_thread_stop(): Already finished!\n"); fp@656: return; fp@656: } fp@656: fp@1006: if (master->debug_level) fp@1006: EC_DBG("Stopping master thread.\n"); fp@1006: fp@1241: kthread_stop(master->thread); fp@1241: master->thread = NULL; fp@656: EC_INFO("Master thread exited.\n"); fp@656: fp@1198: if (master->fsm_datagram.state != EC_DATAGRAM_SENT) fp@1198: return; fp@656: fp@656: // wait for FSM datagram fp@1040: sleep_jiffies = max(HZ / 100, 1); // 10 ms, at least 1 jiffy fp@1040: schedule_timeout(sleep_jiffies); fp@525: } fp@525: fp@525: /*****************************************************************************/ fp@525: fp@1029: /** Transition function from ORPHANED to IDLE phase. fp@1029: */ fp@1029: int ec_master_enter_idle_phase( fp@1029: ec_master_t *master /**< EtherCAT master */ fp@1029: ) fp@446: { fp@1075: if (master->debug_level) fp@1075: EC_DBG("ORPHANED -> IDLE.\n"); fp@1075: fp@515: master->request_cb = ec_master_request_cb; fp@515: master->release_cb = ec_master_release_cb; fp@515: master->cb_data = master; fp@637: fp@1029: master->phase = EC_IDLE; fp@1241: if (ec_master_thread_start(master, ec_master_idle_thread, fp@1241: "EtherCAT-IDLE")) { fp@1029: master->phase = EC_ORPHANED; fp@525: return -1; fp@525: } fp@525: fp@446: return 0; fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@1029: /** Transition function from IDLE to ORPHANED phase. fp@1029: */ fp@1029: void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */) fp@446: { fp@1006: if (master->debug_level) fp@1006: EC_DBG("IDLE -> ORPHANED.\n"); fp@1006: fp@1029: master->phase = EC_ORPHANED; fp@525: fp@715: #ifdef EC_EOE fp@446: ec_master_eoe_stop(master); fp@715: #endif fp@525: ec_master_thread_stop(master); fp@1075: fp@1075: down(&master->master_sem); fp@992: ec_master_clear_slaves(master); fp@1075: up(&master->master_sem); fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@1029: /** Transition function from IDLE to OPERATION phase. fp@1029: */ fp@1029: int ec_master_enter_operation_phase(ec_master_t *master /**< EtherCAT master */) fp@446: { fp@446: ec_slave_t *slave; fp@715: #ifdef EC_EOE fp@661: ec_eoe_t *eoe; fp@715: #endif fp@446: fp@1006: if (master->debug_level) fp@1006: EC_DBG("IDLE -> OPERATION.\n"); fp@1006: fp@656: down(&master->config_sem); fp@656: master->allow_config = 0; // temporarily disable slave configuration fp@900: if (master->config_busy) { fp@900: up(&master->config_sem); fp@900: fp@900: // wait for slave configuration to complete fp@900: if (wait_event_interruptible(master->config_queue, fp@900: !master->config_busy)) { fp@900: EC_INFO("Finishing slave configuration interrupted by signal.\n"); fp@900: goto out_allow; fp@900: } fp@900: fp@900: if (master->debug_level) fp@900: EC_DBG("Waiting for pending slave configuration returned.\n"); fp@900: } else { fp@900: up(&master->config_sem); fp@900: } fp@900: fp@656: down(&master->scan_sem); fp@656: master->allow_scan = 0; // 'lock' the slave list fp@900: if (!master->scan_busy) { fp@900: up(&master->scan_sem); fp@900: } else { fp@900: up(&master->scan_sem); fp@900: fp@794: // wait for slave scan to complete fp@900: if (wait_event_interruptible(master->scan_queue, !master->scan_busy)) { fp@794: EC_INFO("Waiting for slave scan interrupted by signal.\n"); fp@794: goto out_allow; fp@794: } fp@900: fp@900: if (master->debug_level) fp@900: EC_DBG("Waiting for pending slave scan returned.\n"); fp@900: } fp@656: fp@656: // set states for all slaves fp@1000: for (slave = master->slaves; fp@1000: slave < master->slaves + master->slave_count; fp@1000: slave++) { fp@643: ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); fp@446: } fp@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@661: master->ext_request_cb = NULL; fp@661: master->ext_release_cb = NULL; fp@661: master->ext_cb_data = NULL; fp@446: return 0; fp@656: fp@656: out_allow: fp@656: master->allow_scan = 1; fp@656: master->allow_config = 1; fp@446: return -1; fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@1029: /** Transition function from OPERATION to IDLE phase. fp@1029: */ fp@1029: void ec_master_leave_operation_phase(ec_master_t *master fp@446: /**< EtherCAT master */) fp@446: { fp@446: ec_slave_t *slave; fp@715: #ifdef EC_EOE fp@661: ec_eoe_t *eoe; fp@715: #endif fp@656: fp@1006: if (master->debug_level) fp@1006: EC_DBG("OPERATION -> IDLE.\n"); fp@1006: fp@1029: master->phase = EC_IDLE; fp@656: fp@715: #ifdef EC_EOE fp@656: ec_master_eoe_stop(master); fp@715: #endif fp@656: ec_master_thread_stop(master); fp@656: fp@656: master->request_cb = ec_master_request_cb; fp@656: master->release_cb = ec_master_release_cb; fp@656: master->cb_data = master; fp@528: fp@1075: down(&master->master_sem); fp@993: ec_master_clear_domains(master); fp@995: ec_master_clear_slave_configs(master); fp@1075: up(&master->master_sem); fp@792: fp@1000: for (slave = master->slaves; fp@1000: slave < master->slaves + master->slave_count; fp@1000: slave++) { fp@1178: fp@1178: // set states for all slaves fp@449: ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); fp@1178: fp@1178: // mark for reconfiguration, because the master could have no fp@1178: // possibility for a reconfiguration between two sequential operation fp@1178: // phases. fp@1178: slave->force_config = 1; fp@1178: } fp@1178: fp@715: #ifdef EC_EOE fp@661: // ... but leave EoE slaves in 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@448: fp@1241: if (ec_master_thread_start(master, ec_master_idle_thread, fp@1241: "EtherCAT-IDLE")) fp@661: EC_WARN("Failed to restart master thread!\n"); fp@715: #ifdef EC_EOE fp@661: ec_master_eoe_start(master); fp@715: #endif fp@661: fp@656: master->allow_scan = 1; fp@656: master->allow_config = 1; fp@446: } fp@446: fp@446: /*****************************************************************************/ fp@446: fp@997: /** Places a datagram in the datagram queue. fp@997: */ fp@293: void ec_master_queue_datagram(ec_master_t *master, /**< EtherCAT master */ fp@293: ec_datagram_t *datagram /**< datagram */ fp@293: ) fp@293: { fp@293: ec_datagram_t *queued_datagram; fp@293: fp@293: // check, if the datagram is already queued fp@293: list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { fp@293: if (queued_datagram == datagram) { fp@719: datagram->skip_count++; fp@637: if (master->debug_level) fp@637: EC_DBG("skipping datagram %x.\n", (unsigned int) datagram); fp@325: datagram->state = EC_DATAGRAM_QUEUED; fp@98: return; fp@98: } fp@98: } fp@98: fp@293: list_add_tail(&datagram->queue, &master->datagram_queue); fp@325: datagram->state = EC_DATAGRAM_QUEUED; fp@293: } fp@293: fp@293: /*****************************************************************************/ fp@293: fp@997: /** Sends the datagrams in the queue. fp@997: * fp@997: * \return 0 in case of success, else < 0 fp@997: */ fp@293: void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) fp@293: { fp@398: ec_datagram_t *datagram, *next; fp@293: size_t datagram_size; fp@98: uint8_t *frame_data, *cur_data; fp@98: void *follows_word; fp@1040: #ifdef EC_HAVE_CYCLES fp@398: cycles_t cycles_start, cycles_sent, cycles_end; fp@1040: #endif fp@398: unsigned long jiffies_sent; fp@293: unsigned int frame_count, more_datagrams_waiting; fp@398: struct list_head sent_datagrams; fp@398: fp@1040: #ifdef EC_HAVE_CYCLES fp@398: cycles_start = get_cycles(); fp@1040: #endif fp@176: frame_count = 0; fp@398: INIT_LIST_HEAD(&sent_datagrams); fp@208: fp@303: if (unlikely(master->debug_level > 1)) fp@293: EC_DBG("ec_master_send_datagrams\n"); fp@98: fp@176: do { fp@195: // fetch pointer to transmit socket buffer fp@579: frame_data = ec_device_tx_data(&master->main_device); fp@176: cur_data = frame_data + EC_FRAME_HEADER_SIZE; fp@176: follows_word = NULL; fp@293: more_datagrams_waiting = 0; fp@293: fp@293: // fill current frame with datagrams fp@293: list_for_each_entry(datagram, &master->datagram_queue, queue) { fp@325: if (datagram->state != EC_DATAGRAM_QUEUED) continue; fp@293: fp@293: // does the current datagram fit in the frame? fp@293: datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size fp@293: + EC_DATAGRAM_FOOTER_SIZE; fp@293: if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) { fp@293: more_datagrams_waiting = 1; fp@176: break; fp@176: } fp@176: fp@398: list_add_tail(&datagram->sent, &sent_datagrams); fp@293: datagram->index = master->datagram_index++; fp@176: fp@303: if (unlikely(master->debug_level > 1)) fp@293: EC_DBG("adding datagram 0x%02X\n", datagram->index); fp@293: fp@293: // set "datagram following" flag in previous frame fp@176: if (follows_word) fp@176: EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000); fp@176: fp@293: // EtherCAT datagram header fp@293: EC_WRITE_U8 (cur_data, datagram->type); fp@293: EC_WRITE_U8 (cur_data + 1, datagram->index); fp@708: memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN); fp@293: EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF); fp@176: EC_WRITE_U16(cur_data + 8, 0x0000); fp@176: follows_word = cur_data + 6; fp@293: cur_data += EC_DATAGRAM_HEADER_SIZE; fp@293: fp@293: // EtherCAT datagram data fp@293: memcpy(cur_data, datagram->data, datagram->data_size); fp@293: cur_data += datagram->data_size; fp@293: fp@293: // EtherCAT datagram footer fp@195: EC_WRITE_U16(cur_data, 0x0000); // reset working counter fp@293: cur_data += EC_DATAGRAM_FOOTER_SIZE; fp@176: } fp@176: fp@398: if (list_empty(&sent_datagrams)) { fp@303: if (unlikely(master->debug_level > 1)) fp@176: EC_DBG("nothing to send.\n"); fp@176: break; fp@176: } fp@176: fp@176: // EtherCAT frame header fp@176: EC_WRITE_U16(frame_data, ((cur_data - frame_data fp@176: - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000); fp@176: fp@195: // pad frame fp@211: while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN) fp@176: EC_WRITE_U8(cur_data++, 0x00); fp@98: fp@303: if (unlikely(master->debug_level > 1)) fp@986: EC_DBG("frame size: %u\n", cur_data - frame_data); fp@176: fp@195: // send frame fp@579: ec_device_send(&master->main_device, cur_data - frame_data); fp@1040: #ifdef EC_HAVE_CYCLES fp@398: cycles_sent = get_cycles(); fp@1040: #endif fp@398: jiffies_sent = jiffies; fp@398: fp@398: // set datagram states and sending timestamps fp@398: list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) { fp@398: datagram->state = EC_DATAGRAM_SENT; fp@1040: #ifdef EC_HAVE_CYCLES fp@398: datagram->cycles_sent = cycles_sent; fp@1040: #endif fp@398: datagram->jiffies_sent = jiffies_sent; fp@398: list_del_init(&datagram->sent); // empty list of sent datagrams fp@398: } fp@398: fp@176: frame_count++; fp@176: } fp@293: while (more_datagrams_waiting); fp@98: fp@1040: #ifdef EC_HAVE_CYCLES fp@303: if (unlikely(master->debug_level > 1)) { fp@344: cycles_end = get_cycles(); fp@986: EC_DBG("ec_master_send_datagrams sent %u frames in %uus.\n", fp@344: frame_count, fp@344: (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz); fp@176: } fp@1040: #endif fp@176: } fp@176: fp@176: /*****************************************************************************/ fp@176: fp@997: /** Processes a received frame. fp@997: * fp@997: * This function is called by the network driver for every received frame. fp@997: * fp@997: * \return 0 in case of success, else < 0 fp@997: */ fp@331: void ec_master_receive_datagrams(ec_master_t *master, /**< EtherCAT master */ fp@331: const uint8_t *frame_data, /**< frame data */ fp@331: size_t size /**< size of the received data */ fp@331: ) fp@98: { fp@98: size_t frame_size, data_size; fp@293: uint8_t datagram_type, datagram_index; fp@98: unsigned int cmd_follows, matched; fp@98: const uint8_t *cur_data; fp@293: ec_datagram_t *datagram; fp@98: fp@98: if (unlikely(size < EC_FRAME_HEADER_SIZE)) { fp@98: master->stats.corrupted++; fp@98: ec_master_output_stats(master); fp@98: return; fp@98: } fp@98: fp@98: cur_data = frame_data; fp@98: fp@195: // check length of entire frame fp@98: frame_size = EC_READ_U16(cur_data) & 0x07FF; fp@98: cur_data += EC_FRAME_HEADER_SIZE; fp@98: fp@98: if (unlikely(frame_size > size)) { fp@98: master->stats.corrupted++; fp@98: ec_master_output_stats(master); fp@98: return; fp@98: } fp@98: fp@98: cmd_follows = 1; fp@98: while (cmd_follows) { fp@293: // process datagram header fp@293: datagram_type = EC_READ_U8 (cur_data); fp@293: datagram_index = EC_READ_U8 (cur_data + 1); fp@293: data_size = EC_READ_U16(cur_data + 6) & 0x07FF; fp@293: cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; fp@293: cur_data += EC_DATAGRAM_HEADER_SIZE; fp@98: fp@98: if (unlikely(cur_data - frame_data fp@293: + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) { fp@98: master->stats.corrupted++; fp@98: ec_master_output_stats(master); fp@98: return; fp@98: } fp@98: fp@293: // search for matching datagram in the queue fp@98: matched = 0; fp@293: list_for_each_entry(datagram, &master->datagram_queue, queue) { fp@690: if (datagram->index == datagram_index fp@690: && datagram->state == EC_DATAGRAM_SENT fp@293: && datagram->type == datagram_type fp@293: && datagram->data_size == data_size) { fp@98: matched = 1; fp@98: break; fp@98: } fp@98: } fp@98: fp@293: // no matching datagram was found fp@98: if (!matched) { fp@98: master->stats.unmatched++; fp@98: ec_master_output_stats(master); fp@684: fp@684: if (unlikely(master->debug_level > 0)) { fp@684: EC_DBG("UNMATCHED datagram:\n"); fp@684: ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE, fp@684: EC_DATAGRAM_HEADER_SIZE + data_size fp@684: + EC_DATAGRAM_FOOTER_SIZE); fp@692: #ifdef EC_DEBUG_RING fp@692: ec_device_debug_ring_print(&master->main_device); fp@692: #endif fp@684: } fp@684: fp@293: cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE; fp@98: continue; fp@98: } fp@98: fp@345: // copy received data into the datagram memory fp@293: memcpy(datagram->data, cur_data, data_size); fp@98: cur_data += data_size; fp@98: fp@293: // set the datagram's working counter fp@293: datagram->working_counter = EC_READ_U16(cur_data); fp@293: cur_data += EC_DATAGRAM_FOOTER_SIZE; fp@293: fp@293: // dequeue the received datagram fp@325: datagram->state = EC_DATAGRAM_RECEIVED; fp@1040: #ifdef EC_HAVE_CYCLES fp@579: datagram->cycles_received = master->main_device.cycles_poll; fp@1040: #endif fp@579: datagram->jiffies_received = master->main_device.jiffies_poll; fp@293: list_del_init(&datagram->queue); fp@293: } fp@293: } fp@293: fp@293: /*****************************************************************************/ fp@293: fp@1029: /** Output master statistics. fp@997: * fp@997: * This function outputs statistical data on demand, but not more often than fp@997: * necessary. The output happens at most once a second. fp@997: */ fp@195: void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */) fp@98: { fp@344: if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) { fp@344: master->stats.output_jiffies = jiffies; fp@344: fp@98: if (master->stats.timeouts) { fp@986: EC_WARN("%u datagram%s TIMED OUT!\n", master->stats.timeouts, fp@396: master->stats.timeouts == 1 ? "" : "s"); fp@98: master->stats.timeouts = 0; fp@98: } fp@98: if (master->stats.corrupted) { fp@986: EC_WARN("%u frame%s CORRUPTED!\n", master->stats.corrupted, fp@396: master->stats.corrupted == 1 ? "" : "s"); fp@98: master->stats.corrupted = 0; fp@98: } fp@98: if (master->stats.unmatched) { fp@986: EC_WARN("%u datagram%s UNMATCHED!\n", master->stats.unmatched, fp@396: master->stats.unmatched == 1 ? "" : "s"); fp@98: master->stats.unmatched = 0; fp@98: } fp@73: } fp@54: } fp@54: fp@68: /*****************************************************************************/ fp@68: fp@1029: /** Master kernel thread function for IDLE phase. fp@997: */ fp@1241: static int ec_master_idle_thread(void *priv_data) fp@1241: { fp@1241: ec_master_t *master = (ec_master_t *) priv_data; fp@1241: fp@1241: if (master->debug_level) fp@1241: EC_DBG("Idle thread running.\n"); fp@1241: fp@1241: while (!kthread_should_stop()) { fp@719: ec_datagram_output_stats(&master->fsm_datagram); fp@525: fp@1031: // receive fp@1031: spin_lock_bh(&master->internal_lock); fp@1031: ecrt_master_receive(master); fp@1031: spin_unlock_bh(&master->internal_lock); fp@1031: fp@1031: if (master->fsm_datagram.state == EC_DATAGRAM_SENT) fp@1031: goto schedule; fp@651: fp@525: // execute master state machine fp@1241: if (down_interruptible(&master->master_sem)) fp@1241: break; fp@1031: ec_fsm_master_exec(&master->fsm); fp@1075: up(&master->master_sem); fp@1031: fp@1031: // queue and send fp@1031: spin_lock_bh(&master->internal_lock); fp@1031: ec_master_queue_datagram(master, &master->fsm_datagram); fp@1031: ecrt_master_send(master); fp@1031: spin_unlock_bh(&master->internal_lock); fp@525: fp@651: schedule: fp@650: if (ec_fsm_master_idle(&master->fsm)) { fp@650: set_current_state(TASK_INTERRUPTIBLE); fp@650: schedule_timeout(1); fp@650: } fp@650: else { fp@650: schedule(); fp@650: } fp@525: } fp@518: fp@656: if (master->debug_level) fp@656: EC_DBG("Master IDLE thread exiting...\n"); fp@1241: return 0; fp@191: } fp@191: fp@637: /*****************************************************************************/ fp@637: fp@1029: /** Master kernel thread function for IDLE 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@1241: fp@1241: if (master->debug_level) fp@1241: EC_DBG("Operation thread running.\n"); fp@1241: fp@1241: while (!kthread_should_stop()) { fp@719: ec_datagram_output_stats(&master->fsm_datagram); fp@637: if (master->injection_seq_rt != master->injection_seq_fsm || fp@637: master->fsm_datagram.state == EC_DATAGRAM_SENT || fp@637: master->fsm_datagram.state == EC_DATAGRAM_QUEUED) fp@637: goto schedule; fp@637: fp@637: // output statistics fp@637: ec_master_output_stats(master); fp@637: fp@637: // execute master state machine fp@1241: if (down_interruptible(&master->master_sem)) fp@1241: break; fp@1031: ec_fsm_master_exec(&master->fsm); fp@1075: up(&master->master_sem); fp@1031: fp@1031: // inject datagram fp@1031: master->injection_seq_fsm++; fp@637: fp@637: schedule: fp@650: if (ec_fsm_master_idle(&master->fsm)) { fp@650: set_current_state(TASK_INTERRUPTIBLE); fp@650: schedule_timeout(1); fp@650: } fp@650: else { fp@650: schedule(); fp@650: } fp@637: } fp@637: fp@656: if (master->debug_level) fp@656: EC_DBG("Master OP thread exiting...\n"); fp@1241: return 0; fp@637: } fp@578: fp@578: /*****************************************************************************/ fp@578: fp@715: #ifdef EC_EOE fp@997: /** 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@661: if (master->eoe_running) { fp@661: EC_WARN("EoE already running!\n"); fp@661: return; fp@661: } fp@661: fp@661: if (list_empty(&master->eoe_handlers)) fp@661: return; fp@661: fp@661: if (!master->request_cb || !master->release_cb) { fp@661: EC_WARN("No EoE processing because of missing locking callbacks!\n"); fp@251: return; fp@251: } fp@251: fp@251: EC_INFO("Starting EoE processing.\n"); fp@251: master->eoe_running = 1; fp@251: fp@251: // start EoE processing fp@251: master->eoe_timer.expires = jiffies + 10; fp@251: add_timer(&master->eoe_timer); fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@251: fp@997: /** Stops the Ethernet-over-EtherCAT processing. fp@997: */ fp@251: void ec_master_eoe_stop(ec_master_t *master /**< EtherCAT master */) fp@251: { fp@251: if (!master->eoe_running) return; fp@251: fp@251: EC_INFO("Stopping EoE processing.\n"); fp@251: fp@251: del_timer_sync(&master->eoe_timer); fp@251: master->eoe_running = 0; fp@251: } fp@251: fp@251: /*****************************************************************************/ fp@441: fp@997: /** Does the Ethernet-over-EtherCAT processing. fp@997: */ fp@251: void ec_master_eoe_run(unsigned long data /**< master pointer */) fp@206: { fp@206: ec_master_t *master = (ec_master_t *) data; fp@197: ec_eoe_t *eoe; fp@661: unsigned int none_open = 1; fp@344: unsigned long restart_jiffies; fp@235: fp@251: list_for_each_entry(eoe, &master->eoe_handlers, list) { fp@661: if (ec_eoe_is_open(eoe)) { fp@661: none_open = 0; fp@661: break; fp@661: } fp@661: } fp@661: if (none_open) fp@661: goto queue_timer; fp@251: fp@721: // receive datagrams fp@1041: if (master->request_cb(master->cb_data)) fp@1041: goto queue_timer; fp@1041: fp@325: ecrt_master_receive(master); fp@721: master->release_cb(master->cb_data); fp@515: fp@515: // actual EoE processing fp@251: list_for_each_entry(eoe, &master->eoe_handlers, list) { fp@251: ec_eoe_run(eoe); fp@251: } fp@334: fp@515: // send datagrams fp@721: if (master->request_cb(master->cb_data)) { fp@721: goto queue_timer; fp@721: } fp@721: list_for_each_entry(eoe, &master->eoe_handlers, list) { fp@721: ec_eoe_queue(eoe); fp@721: } fp@325: ecrt_master_send(master); fp@721: master->release_cb(master->cb_data); fp@334: fp@251: queue_timer: fp@344: restart_jiffies = HZ / EC_EOE_FREQUENCY; fp@344: if (!restart_jiffies) restart_jiffies = 1; fp@721: master->eoe_timer.expires = jiffies + restart_jiffies; fp@208: add_timer(&master->eoe_timer); fp@197: } fp@715: #endif fp@197: fp@325: /*****************************************************************************/ fp@325: fp@792: /** Detaches the slave configurations from the slaves. fp@640: */ fp@792: void ec_master_detach_slave_configs( fp@792: ec_master_t *master /**< EtherCAT master. */ fp@640: ) fp@197: { fp@792: ec_slave_config_t *sc; fp@792: fp@792: list_for_each_entry(sc, &master->configs, list) { fp@792: ec_slave_config_detach(sc); fp@792: } fp@792: } fp@792: fp@792: /*****************************************************************************/ fp@792: fp@792: /** Attaches the slave configurations to the slaves. fp@792: */ fp@1028: void ec_master_attach_slave_configs( fp@792: ec_master_t *master /**< EtherCAT master. */ fp@792: ) fp@792: { fp@792: ec_slave_config_t *sc; fp@792: fp@792: list_for_each_entry(sc, &master->configs, list) { fp@1028: ec_slave_config_attach(sc); fp@1028: } fp@197: } fp@197: fp@927: /*****************************************************************************/ fp@927: fp@1077: /** Common implementation for ec_master_find_slave() fp@1077: * and ec_master_find_slave_const(). fp@1077: */ fp@1077: #define EC_FIND_SLAVE \ fp@1077: do { \ fp@1077: if (alias) { \ fp@1077: for (; slave < master->slaves + master->slave_count; \ fp@1077: slave++) { \ fp@1077: if (slave->sii.alias == alias) \ fp@1077: break; \ fp@1077: } \ fp@1077: if (slave == master->slaves + master->slave_count) \ fp@1077: return NULL; \ fp@1077: } \ fp@1077: \ fp@1077: slave += position; \ fp@1077: if (slave < master->slaves + master->slave_count) { \ fp@1077: return slave; \ fp@1077: } else { \ fp@1077: return NULL; \ fp@1077: } \ fp@1077: } while (0) fp@1077: fp@927: /** Finds a slave in the bus, given the alias and position. fp@927: */ fp@927: ec_slave_t *ec_master_find_slave( fp@927: ec_master_t *master, /**< EtherCAT master. */ fp@927: uint16_t alias, /**< Slave alias. */ fp@927: uint16_t position /**< Slave position. */ fp@927: ) fp@927: { fp@1000: ec_slave_t *slave = master->slaves; fp@1077: EC_FIND_SLAVE; fp@1077: } fp@1077: fp@1077: /** Finds a slave in the bus, given the alias and position. fp@1077: * fp@1077: * Const version. fp@1077: */ fp@1077: const ec_slave_t *ec_master_find_slave_const( fp@1077: const ec_master_t *master, /**< EtherCAT master. */ fp@1077: uint16_t alias, /**< Slave alias. */ fp@1077: uint16_t position /**< Slave position. */ fp@1077: ) fp@1077: { fp@1077: const ec_slave_t *slave = master->slaves; fp@1077: EC_FIND_SLAVE; fp@927: } fp@927: fp@946: /*****************************************************************************/ fp@946: fp@1092: /** Get the number of slave configurations provided by the application. fp@1092: * fp@1092: * \return Number of configurations. fp@1092: */ fp@990: unsigned int ec_master_config_count( fp@990: const ec_master_t *master /**< EtherCAT master. */ fp@990: ) fp@990: { fp@990: const ec_slave_config_t *sc; fp@990: unsigned int count = 0; fp@990: fp@990: list_for_each_entry(sc, &master->configs, list) { fp@990: count++; fp@990: } fp@990: fp@990: return count; fp@990: } fp@990: fp@990: /*****************************************************************************/ fp@990: 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@990: const ec_master_t *master, /**< EtherCAT master. */ fp@1092: unsigned int pos /**< List position. */ fp@990: ) fp@990: { fp@990: const ec_slave_config_t *sc; fp@990: fp@990: list_for_each_entry(sc, &master->configs, list) { fp@1092: if (pos--) fp@990: continue; fp@990: return sc; fp@990: } fp@990: fp@990: return NULL; 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@946: const ec_master_t *master /**< EtherCAT master. */ fp@946: ) fp@946: { fp@946: const ec_domain_t *domain; fp@946: unsigned int count = 0; fp@946: fp@946: list_for_each_entry(domain, &master->domains, list) { fp@946: count++; fp@946: } fp@946: fp@946: 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@946: ec_master_t *master, /**< EtherCAT master. */ fp@946: unsigned int index /**< Domain index. */ fp@946: ) fp@946: { fp@946: 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@1078: const ec_master_t *master, /**< EtherCAT master. */ fp@1078: unsigned int index /**< Domain index. */ fp@1078: ) fp@1078: { fp@1078: const ec_domain_t *domain; fp@1078: EC_FIND_DOMAIN; fp@946: } fp@946: fp@956: /*****************************************************************************/ fp@956: fp@1092: /** Set the debug level. fp@1092: * fp@1092: * \retval 0 Success. fp@1092: * \retval -1 Invalid debug level. fp@1092: */ fp@956: int ec_master_debug_level( fp@1092: ec_master_t *master, /**< EtherCAT master. */ fp@1092: int level /**< Debug level. May be 0, 1 or 2. */ fp@956: ) fp@956: { fp@956: if (level < 0 || level > 2) { fp@956: EC_ERR("Invalid debug level %i!\n", level); fp@956: return -1; fp@956: } fp@956: fp@956: if (level != master->debug_level) { fp@956: master->debug_level = level; fp@956: EC_INFO("Master debug level set to %i.\n", master->debug_level); fp@956: } fp@956: fp@956: return 0; fp@956: } fp@956: fp@640: /****************************************************************************** fp@640: * Realtime interface fp@640: *****************************************************************************/ fp@640: fp@640: ec_domain_t *ecrt_master_create_domain(ec_master_t *master /**< master */) fp@640: { fp@640: ec_domain_t *domain, *last_domain; fp@640: unsigned int index; fp@640: fp@1181: if (master->debug_level) fp@1181: EC_DBG("ecrt_master_create_domain(master = 0x%x)\n", (u32) master); fp@1181: fp@640: if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) { fp@640: EC_ERR("Error allocating domain memory!\n"); fp@640: return NULL; fp@640: } fp@640: fp@1075: down(&master->master_sem); fp@1075: fp@993: if (list_empty(&master->domains)) { fp@993: index = 0; fp@993: } else { fp@640: last_domain = list_entry(master->domains.prev, ec_domain_t, list); fp@640: index = last_domain->index + 1; fp@640: } fp@640: fp@993: ec_domain_init(domain, master, index); fp@640: list_add_tail(&domain->list, &master->domains); fp@640: fp@1075: up(&master->master_sem); fp@1075: fp@1181: if (master->debug_level) fp@1181: EC_DBG("Created domain %u.\n", domain->index); fp@1181: fp@640: return domain; fp@640: } fp@640: fp@640: /*****************************************************************************/ fp@640: fp@792: int ecrt_master_activate(ec_master_t *master) fp@640: { fp@640: uint32_t domain_offset; fp@640: ec_domain_t *domain; fp@640: fp@1181: if (master->debug_level) fp@1181: EC_DBG("ecrt_master_activate(master = 0x%x)\n", (u32) master); fp@1181: 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@792: if (ec_domain_finish(domain, domain_offset)) { fp@1197: up(&master->master_sem); fp@926: EC_ERR("Failed to finish domain 0x%08X!\n", (u32) domain); fp@640: return -1; fp@640: } fp@640: domain_offset += domain->data_size; fp@640: } fp@1075: fp@1075: up(&master->master_sem); fp@640: fp@656: // restart EoE process and master thread with new locking fp@715: #ifdef EC_EOE fp@656: ec_master_eoe_stop(master); fp@715: #endif fp@656: ec_master_thread_stop(master); fp@656: fp@640: if (master->debug_level) fp@640: EC_DBG("FSM datagram is %x.\n", (unsigned int) &master->fsm_datagram); fp@640: fp@656: master->injection_seq_fsm = 0; fp@656: master->injection_seq_rt = 0; fp@661: master->request_cb = master->ext_request_cb; fp@661: master->release_cb = master->ext_release_cb; fp@661: master->cb_data = master->ext_cb_data; fp@656: fp@1241: if (ec_master_thread_start(master, ec_master_operation_thread, fp@1241: "EtherCAT-OP")) { fp@640: EC_ERR("Failed to start master thread!\n"); fp@640: return -1; fp@640: } fp@715: #ifdef EC_EOE fp@661: ec_master_eoe_start(master); fp@715: #endif fp@902: fp@902: master->allow_config = 1; // request the current configuration fp@902: master->allow_scan = 1; // allow re-scanning on topology change fp@640: return 0; fp@640: } fp@640: fp@640: /*****************************************************************************/ fp@640: fp@792: void ecrt_master_send(ec_master_t *master) fp@640: { fp@640: ec_datagram_t *datagram, *n; fp@640: fp@640: if (master->injection_seq_rt != master->injection_seq_fsm) { fp@640: // inject datagram produced by master FSM fp@640: ec_master_queue_datagram(master, &master->fsm_datagram); fp@640: master->injection_seq_rt = master->injection_seq_fsm; fp@640: } fp@640: fp@640: if (unlikely(!master->main_device.link_state)) { fp@640: // link is down, no datagram can be sent fp@640: list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { fp@640: datagram->state = EC_DATAGRAM_ERROR; fp@640: list_del_init(&datagram->queue); fp@640: } fp@640: fp@640: // query link state fp@640: ec_device_poll(&master->main_device); fp@640: return; fp@640: } fp@640: fp@640: // send frames fp@640: ec_master_send_datagrams(master); fp@640: } fp@640: fp@640: /*****************************************************************************/ fp@640: fp@792: void ecrt_master_receive(ec_master_t *master) fp@640: { fp@640: ec_datagram_t *datagram, *next; fp@1040: #ifdef EC_HAVE_CYCLES fp@640: cycles_t cycles_timeout; fp@1040: #else fp@1040: unsigned long diff_ms, timeout_ms; fp@1040: #endif fp@664: unsigned int frames_timed_out = 0; fp@640: fp@640: // receive datagrams fp@640: ec_device_poll(&master->main_device); fp@640: fp@1040: #ifdef EC_HAVE_CYCLES fp@640: cycles_timeout = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000); fp@1040: #else fp@1040: timeout_ms = max(EC_IO_TIMEOUT /* us */ / 1000, 2); fp@1040: #endif fp@640: fp@640: // dequeue all datagrams that timed out fp@640: list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { fp@640: if (datagram->state != EC_DATAGRAM_SENT) continue; fp@640: fp@1040: #ifdef EC_HAVE_CYCLES fp@640: if (master->main_device.cycles_poll - datagram->cycles_sent fp@640: > cycles_timeout) { fp@1040: #else fp@1040: diff_ms = (master->main_device.jiffies_poll fp@1040: - datagram->jiffies_sent) * 1000 / HZ; fp@1040: if (diff_ms > timeout_ms) { fp@1040: #endif fp@664: frames_timed_out = 1; fp@640: list_del_init(&datagram->queue); fp@640: datagram->state = EC_DATAGRAM_TIMED_OUT; fp@640: master->stats.timeouts++; fp@640: ec_master_output_stats(master); fp@684: fp@684: if (unlikely(master->debug_level > 0)) { fp@1237: unsigned int time_us; fp@1237: #ifdef EC_HAVE_CYCLES fp@1237: time_us = (unsigned int) (master->main_device.cycles_poll - fp@1237: datagram->cycles_sent) * 1000 / cpu_khz; fp@1237: #else fp@1237: time_us = (unsigned int) (diff_ms * 1000); fp@1237: #endif fp@691: EC_DBG("TIMED OUT datagram %08x, index %02X waited %u us.\n", fp@1237: (unsigned int) datagram, datagram->index, time_us); fp@684: } fp@640: } fp@640: } fp@664: fp@664: master->frames_timed_out = frames_timed_out; fp@640: } fp@640: fp@640: /*****************************************************************************/ fp@640: fp@792: ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, fp@792: uint16_t alias, uint16_t position, uint32_t vendor_id, fp@1010: uint32_t product_code) fp@792: { fp@792: ec_slave_config_t *sc; fp@697: unsigned int found = 0; fp@697: fp@1181: fp@1181: if (master->debug_level) fp@1181: EC_DBG("ecrt_master_slave_config(master = 0x%x, alias = %u, " fp@1181: "position = %u, vendor_id = %u, product_code = %u)\n", fp@1181: (u32) master, alias, position, vendor_id, product_code); fp@1181: fp@792: list_for_each_entry(sc, &master->configs, list) { fp@792: if (sc->alias == alias && sc->position == position) { fp@697: found = 1; fp@697: break; fp@697: } fp@697: } fp@697: fp@1024: if (found) { // config with same alias/position already existing fp@1010: if (sc->vendor_id != vendor_id || sc->product_code != product_code) { fp@792: EC_ERR("Slave type mismatch. Slave was configured as" fp@1010: " 0x%08X/0x%08X before. Now configuring with" fp@1010: " 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code, fp@1010: vendor_id, product_code); fp@792: return NULL; fp@792: } fp@792: } else { fp@1055: if (master->debug_level) fp@1055: EC_DBG("Creating slave configuration for %u:%u, 0x%08X/0x%08X.\n", fp@1055: alias, position, vendor_id, product_code); fp@792: fp@792: if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t), fp@792: GFP_KERNEL))) { fp@792: EC_ERR("Failed to allocate memory for slave configuration.\n"); fp@792: return NULL; 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@792: void ecrt_master_callbacks(ec_master_t *master, int (*request_cb)(void *), fp@792: void (*release_cb)(void *), void *cb_data) fp@204: { fp@1181: if (master->debug_level) fp@1181: EC_DBG("ecrt_master_callbacks(master = 0x%x, request_cb = 0x%x, " fp@1181: " release_cb = 0x%x, cb_data = 0x%x)\n", (u32) master, fp@1181: (u32) request_cb, (u32) release_cb, (u32) cb_data); fp@1181: fp@661: master->ext_request_cb = request_cb; fp@661: master->ext_release_cb = release_cb; fp@661: master->ext_cb_data = cb_data; fp@204: } fp@204: fp@204: /*****************************************************************************/ fp@204: fp@792: void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) fp@792: { fp@792: state->slaves_responding = master->fsm.slaves_responding; fp@1022: state->al_states = master->fsm.slave_states; ha@1018: state->link_up = master->main_device.link_state; fp@612: } fp@612: fp@612: /*****************************************************************************/ fp@612: fp@199: /** \cond */ fp@199: fp@104: EXPORT_SYMBOL(ecrt_master_create_domain); fp@104: EXPORT_SYMBOL(ecrt_master_activate); fp@325: EXPORT_SYMBOL(ecrt_master_send); fp@325: EXPORT_SYMBOL(ecrt_master_receive); fp@206: EXPORT_SYMBOL(ecrt_master_callbacks); fp@792: EXPORT_SYMBOL(ecrt_master_slave_config); fp@792: EXPORT_SYMBOL(ecrt_master_state); fp@42: fp@199: /** \endcond */ fp@199: fp@199: /*****************************************************************************/