├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── jenkins-pipeline ├── soem_interface_examples ├── CMakeLists.txt ├── include │ └── soem_interface_examples │ │ └── ExampleSlave.hpp ├── package.xml └── src │ ├── example_slave.cpp │ └── soem_interface_examples │ └── ExampleSlave.cpp ├── soem_interface_rsl ├── CMakeLists.txt ├── include │ └── soem_interface_rsl │ │ ├── EthercatBusBase.hpp │ │ ├── EthercatBusManagerBase.hpp │ │ ├── EthercatSlaveBase.hpp │ │ └── common │ │ ├── EthercatTypes.hpp │ │ ├── ExtendedRegisters.hpp │ │ ├── Macros.hpp │ │ ├── ObjectDictionaryUtilities.hpp │ │ ├── ThreadSleep.hpp │ │ └── soem_rsl_export.h ├── package.xml └── src │ └── soem_interface_rsl │ ├── EthercatBusBase.cpp │ ├── EthercatBusManagerBase.cpp │ ├── EthercatSlaveBase.cpp │ └── common │ ├── Macros.cpp │ └── ThreadSleep.cpp └── soem_rsl ├── CMakeLists.txt ├── package.xml └── soem_rsl ├── .gitignore ├── .travis.yml ├── ChangeLog ├── Doxyfile ├── LICENSE ├── README.md ├── appveyor.yml ├── cmake ├── Modules │ └── Platform │ │ ├── rt-kernel-C.cmake │ │ ├── rt-kernel-gcc-bfin.cmake │ │ ├── rt-kernel-gcc-kinetis.cmake │ │ ├── rt-kernel-gcc.cmake │ │ ├── rt-kernel.cmake │ │ └── rtems.cmake └── Toolchains │ ├── rt-kernel-bfin.cmake │ └── rt-kernel-kinetis.cmake ├── doc ├── images │ ├── legacy_iomap.png │ ├── memory_layout.png │ └── overlapping_iomap.png ├── soem_rsl.dox └── tutorial.txt ├── drvcomment.txt ├── osal ├── erika │ ├── osal.c │ └── osal_defs.h ├── intime │ ├── osal.c │ └── osal_defs.h ├── linux │ ├── osal.c │ └── osal_defs.h ├── macosx │ ├── osal.c │ └── osal_defs.h ├── osal.h ├── rtems │ ├── osal.c │ └── osal_defs.h ├── rtk │ ├── osal.c │ └── osal_defs.h ├── vxworks │ ├── osal.c │ └── osal_defs.h └── win32 │ ├── inttypes.h │ ├── osal.c │ ├── osal_defs.h │ ├── osal_win32.h │ └── stdint.h ├── oshw ├── erika │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── intime │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── linux │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── macosx │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── rtems │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── rtk │ ├── fec │ │ ├── fec_ecat.c │ │ └── fec_ecat.h │ ├── lw_mac │ │ ├── lw_emac.c │ │ └── lw_emac.h │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h ├── vxworks │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ └── oshw.h └── win32 │ ├── nicdrv.c │ ├── nicdrv.h │ ├── oshw.c │ ├── oshw.h │ └── wpcap │ ├── Include │ ├── Packet32.h │ ├── Win32-Extensions.h │ ├── bittypes.h │ ├── ip6_misc.h │ ├── pcap-bpf.h │ ├── pcap-namedb.h │ ├── pcap-stdinc.h │ ├── pcap.h │ ├── pcap │ │ ├── bluetooth.h │ │ ├── bpf.h │ │ ├── namedb.h │ │ ├── pcap.h │ │ ├── sll.h │ │ ├── usb.h │ │ └── vlan.h │ └── remote-ext.h │ └── Lib │ ├── Packet.lib │ ├── libpacket.a │ ├── libwpcap.a │ ├── wpcap.lib │ └── x64 │ ├── Packet.lib │ └── wpcap.lib ├── soem_rsl ├── ethercat.h ├── ethercatbase.c ├── ethercatbase.h ├── ethercatcoe.c ├── ethercatcoe.h ├── ethercatconfig.c ├── ethercatconfig.h ├── ethercatconfiglist.h ├── ethercatdc.c ├── ethercatdc.h ├── ethercateoe.c ├── ethercateoe.h ├── ethercatfoe.c ├── ethercatfoe.h ├── ethercatmain.c ├── ethercatmain.h ├── ethercatprint.c ├── ethercatprint.h ├── ethercatsoe.c ├── ethercatsoe.h └── ethercattype.h └── test ├── intime └── ec_master │ └── ec_master.c ├── linux ├── aliastool.c ├── ebox │ └── ebox.c ├── eepromtool │ ├── CMakeLists.txt │ └── eepromtool.c ├── eoe_test │ └── eoe_test.c ├── firm_update │ └── firm_update.c ├── red_test │ └── red_test.c ├── simple_test │ ├── CMakeLists.txt │ └── simple_test.c └── slaveinfo │ ├── CMakeLists.txt │ └── slaveinfo.c ├── rtk ├── main.c └── schedule.tt └── win32 ├── ebox └── ebox.c ├── eepromtool └── eepromtool.c ├── firm_update └── firm_update.c ├── red_test └── red_test.c ├── simple_test └── simple_test.c └── slaveinfo └── slaveinfo.c /.gitignore: -------------------------------------------------------------------------------- 1 | .ccls-cache 2 | compile_commands.json 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "soem/soem"] 2 | path = soem/soem 3 | url = https://github.com/OpenEtherCATsociety/SOEM.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SOEM Interface 2 | 3 | ## Overview 4 | This software package serves as a C++ interface for one or more EtherCAT devices running on the same bus. 5 | The lower level EtherCAT communication is handled by the [SOEM](https://github.com/OpenEtherCATsociety/soem) library. 6 | 7 | The soem_interface has been developed on Ubuntu 18.04 LTS with ROS Melodic. 8 | 9 | The source code is released under the GPLv3 license. 10 | A copy of the license is available in the *COPYING* file. 11 | 12 | **Author:** Markus Staeuble 13 | 14 | **Affiliation:** Robotic Systems Lab - ETH Zurich 15 | 16 | **Maintainer:** Johannes Pankert, johannes.pankert@mavt.ethz.ch 17 | 18 | **Contributors:** Johannes Pankert, Jonas Junger, Lennart Nachtigall 19 | 20 | 21 | ## Installation 22 | 23 | ### Dependencies 24 | #### Catkin Packages 25 | 26 | | Repo | url | license | content | 27 | |:--------------:|:----------------------------------------------------:|:------------:|:------------------:| 28 | | message_logger | https://github.com/leggedrobotics/message_logger.git | BSD 3-Clause | simple log streams | 29 | 30 | #### System Dependencies (Ubuntu 18.04 LTS) 31 | - [ROS Melodic](https://wiki.ros.org/melodic) (full installation) 32 | - [catkin](https://wiki.ros.org/catkin) 33 | 34 | ### Building from Source 35 | 36 | To build the library from source, clone the latest version from this repository into your catkin workspace and compile the package using 37 | 38 | cd catkin_workspace/src 39 | git clone https://github.com/leggedrobotics/message_logger.git 40 | git clone https://github.com/leggedrobotics/soem_interface.git 41 | cd ../ 42 | catkin build soem_interface 43 | 44 | To build the examples, execute the following command inside of your catkin workspace: 45 | 46 | catkin build soem_interface_examples 47 | 48 | ## Classes 49 | 50 | #### EthercatSlaveBase 51 | This is an abstract base class for an ethercat slave. The ethercat slave class holds a non owning reference to the EthercatBusBase it is on (bus_). 52 | EthercatSlaves should stage their tx messages before writing to the ethercat bus in the updateWrite() method using the bus_ writeRxPdo() method. 53 | Similarly, the slaves can retrieve the buffered read messages from the bus with the readTxPdo() method in updateRead(). 54 | 55 | #### EthercatBusBase 56 | This class represents a physical ethercat bus containing multiple ethercat slaves. 57 | It manages the slaves on the bus using the methods from soem. With updateRead() and updatewrite() the staged slave messages are written/read to/from all the slaves on the bus simultaneously. 58 | EthercatSlaves can be added with addSlave(). After all the slaves have been added the startup() method to actually start the communication of the bus. 59 | 60 | #### EthercatBusManagerBase 61 | If multiple buses are connected to the same master then the buses are managed by the EthercatBusManagerBase. 62 | 63 | ## Note 64 | Due to the current pandemic we could not test this version of the soem_interface. 65 | 66 | Tests will be conducted as soon as possible. 67 | 68 | -------------------------------------------------------------------------------- /jenkins-pipeline: -------------------------------------------------------------------------------- 1 | library 'continuous_integration_pipeline' 2 | ciPipeline("") -------------------------------------------------------------------------------- /soem_interface_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 2 | # Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 3 | # Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 4 | # Mario Mauerer, Remo Diethelm 5 | # 6 | # This file is part of the soem_interface_rsl. 7 | # 8 | # The soem_interface_rsl is free software: you can redistribute it and/or modify 9 | # it under the terms of the GNU General Public License as published by 10 | # the Free Software Foundation, either version 3 of the License, or 11 | # (at your option) any later version. 12 | # 13 | # The seom_interface is distributed in the hope that it will be useful, 14 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | # GNU General Public License for more details. 17 | # 18 | # You should have received a copy of the GNU General Public License 19 | # along with the soem_interface_rsl. If not, see . 20 | 21 | cmake_minimum_required(VERSION 2.8.3) 22 | project(soem_interface_examples) 23 | 24 | add_definitions(-std=c++14 -Wall -Werror) 25 | add_definitions(-DMELO_USE_COUT) 26 | 27 | set(PACKAGE_DEPENDENCIES 28 | message_logger 29 | soem_rsl 30 | soem_interface_rsl 31 | ) 32 | 33 | find_package(catkin REQUIRED 34 | COMPONENTS 35 | ${PACKAGE_DEPENDENCIES} 36 | ) 37 | 38 | catkin_package( 39 | INCLUDE_DIRS 40 | include 41 | LIBRARIES 42 | ${PROJECT_NAME} 43 | CATKIN_DEPENDS 44 | ${PACKAGE_DEPENDENCIES} 45 | ) 46 | 47 | include_directories( 48 | include 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library( 53 | ${PROJECT_NAME} 54 | src/${PROJECT_NAME}/ExampleSlave.cpp 55 | ) 56 | add_dependencies( 57 | ${PROJECT_NAME} 58 | ${catkin_EXPORTED_TARGETS} 59 | ) 60 | target_link_libraries( 61 | ${PROJECT_NAME} 62 | ${catkin_LIBRARIES} 63 | ) 64 | 65 | add_executable( 66 | ${PROJECT_NAME}_1 67 | src/example_slave.cpp 68 | ) 69 | add_dependencies( 70 | ${PROJECT_NAME}_1 71 | ${PROJECT_NAME} 72 | ${catkin_EXPORTED_TARGETS} 73 | ) 74 | target_link_libraries( 75 | ${PROJECT_NAME}_1 76 | ${PROJECT_NAME} 77 | ${catkin_LIBRARIES} 78 | ) 79 | 80 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_1 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 84 | ) 85 | install(DIRECTORY include/${PROJECT_NAME}/ 86 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 87 | ) 88 | -------------------------------------------------------------------------------- /soem_interface_examples/include/soem_interface_examples/ExampleSlave.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #define RX_PDO_ID 0x6000 29 | #define TX_PDO_ID 0x7000 30 | 31 | namespace soem_interface_examples { 32 | 33 | struct TxPdo { 34 | uint8_t state = 0; 35 | float data1 = 0.0; 36 | float data2 = 0.0; 37 | } __attribute__((packed)); 38 | 39 | struct RxPdo { 40 | float command1 = 0.0; 41 | float command2 = 0.0; 42 | } __attribute__((packed)); 43 | 44 | class ExampleSlave : public soem_interface_rsl::EthercatSlaveBase { 45 | public: 46 | ExampleSlave(const std::string& name, soem_interface_rsl::EthercatBusBase* bus, const uint32_t address); 47 | ~ExampleSlave() override = default; 48 | 49 | std::string getName() const override { return name_; } 50 | 51 | bool startup() override; 52 | void updateRead() override; 53 | void updateWrite() override; 54 | void shutdown() override; 55 | 56 | PdoInfo getCurrentPdoInfo() const override { return pdoInfo_; } 57 | 58 | private: 59 | const std::string name_; 60 | PdoInfo pdoInfo_; 61 | TxPdo reading_; 62 | RxPdo command_; 63 | }; 64 | 65 | } // namespace soem_interface_examples 66 | -------------------------------------------------------------------------------- /soem_interface_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 4 | Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 5 | Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 6 | Mario Mauerer, Remo Diethelm 7 | 8 | This file is part of the soem_interface_rsl. 9 | 10 | The soem_interface_rsl is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation, either version 3 of the License, or 13 | (at your option) any later version. 14 | 15 | The seom_interface is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU General Public License 21 | along with the soem_interface_rsl. If not, see 22 | . 23 | <--> 24 | 25 | 26 | soem_interface_examples 27 | 0.4.0 28 | Examples on how to use the soem_interface_rsl package. 29 | 30 | GPLv3 31 | 32 | Markus Staeuble 33 | Jonas Junger 34 | Johannes Pankert 35 | Lennart Nachtigall 36 | 37 | Johannes Pankert 38 | 39 | catkin 40 | message_logger 41 | soem_rsl 42 | soem_interface_rsl 43 | 44 | 45 | -------------------------------------------------------------------------------- /soem_interface_examples/src/example_slave.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | // This shows a minimal example on how to use the soem_interface_rsl library. 27 | // Keep in mind that this is non-working example code, with only minimal error handling 28 | 29 | int main(int argc, char** argv) { 30 | const std::string busName = "eth1"; 31 | const std::string slaveName = "ExampleSlave"; 32 | const uint32_t slaveAddress = 0; 33 | 34 | std::unique_ptr bus = std::make_unique(busName); 35 | 36 | std::shared_ptr slave = 37 | std::make_shared(slaveName, bus.get(), slaveAddress); 38 | 39 | bus->addSlave(slave); 40 | bus->startup(); 41 | bus->setState(EC_STATE_OPERATIONAL); 42 | 43 | if (!bus->waitForState(EC_STATE_OPERATIONAL, slaveAddress)) { 44 | // Something is wrong 45 | return 1; 46 | } 47 | 48 | while (true) { 49 | bus->updateRead(); 50 | bus->updateWrite(); 51 | } 52 | 53 | bus->shutdown(); 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /soem_interface_examples/src/soem_interface_examples/ExampleSlave.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #include 24 | 25 | namespace soem_interface_examples { 26 | 27 | ExampleSlave::ExampleSlave(const std::string& name, soem_interface_rsl::EthercatBusBase* bus, const uint32_t address) 28 | : soem_interface_rsl::EthercatSlaveBase(bus, address), name_(name) { 29 | pdoInfo_.rxPdoId_ = RX_PDO_ID; 30 | pdoInfo_.txPdoId_ = TX_PDO_ID; 31 | pdoInfo_.rxPdoSize_ = sizeof(command_); 32 | pdoInfo_.txPdoSize_ = sizeof(reading_); 33 | pdoInfo_.moduleId_ = 0x00123456; 34 | } 35 | 36 | bool ExampleSlave::startup() { 37 | // Do nothing else 38 | return true; 39 | } 40 | 41 | void ExampleSlave::updateRead() { 42 | bus_->readTxPdo(address_, reading_); 43 | } 44 | 45 | void ExampleSlave::updateWrite() { 46 | bus_->writeRxPdo(address_, command_); 47 | } 48 | 49 | void ExampleSlave::shutdown() { 50 | // Do nothing 51 | } 52 | 53 | } // namespace soem_interface_examples 54 | -------------------------------------------------------------------------------- /soem_interface_rsl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 2 | # Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 3 | # Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 4 | # Mario Mauerer, Remo Diethelm 5 | # 6 | # This file is part of the soem_interface_rsl. 7 | # 8 | # The soem_interface_rsl is free software: you can redistribute it and/or modify 9 | # it under the terms of the GNU General Public License as published by 10 | # the Free Software Foundation, either version 3 of the License, or 11 | # (at your option) any later version. 12 | # 13 | # The seom_interface is distributed in the hope that it will be useful, 14 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | # GNU General Public License for more details. 17 | # 18 | # You should have received a copy of the GNU General Public License 19 | # along with the soem_interface_rsl. If not, see . 20 | 21 | cmake_minimum_required(VERSION 3.15) 22 | project(soem_interface_rsl) 23 | 24 | add_definitions(-std=c++17 -Wall -Werror) 25 | add_definitions(-DMELO_USE_COUT) 26 | 27 | 28 | 29 | set(PACKAGE_DEPENDENCIES 30 | message_logger 31 | soem_rsl 32 | ) 33 | 34 | find_package(catkin REQUIRED 35 | COMPONENTS 36 | ${PACKAGE_DEPENDENCIES} 37 | ) 38 | 39 | catkin_package( 40 | INCLUDE_DIRS 41 | include 42 | LIBRARIES 43 | ${PROJECT_NAME} 44 | CATKIN_DEPENDS 45 | ${PACKAGE_DEPENDENCIES} 46 | ) 47 | 48 | include_directories( 49 | include 50 | ${catkin_INCLUDE_DIRS} 51 | ) 52 | 53 | #this has to be a shared lib because we have to avoid symbol clashes with other (older) versions of SOEM which might exists in the workspace. 54 | # e.g. ros-noetic-soem package (used by BOTA, ANYbotics.) 55 | add_library( 56 | ${PROJECT_NAME} SHARED 57 | src/${PROJECT_NAME}/common/ThreadSleep.cpp 58 | src/${PROJECT_NAME}/common/Macros.cpp 59 | src/${PROJECT_NAME}/EthercatSlaveBase.cpp 60 | src/${PROJECT_NAME}/EthercatBusManagerBase.cpp 61 | src/${PROJECT_NAME}/EthercatBusBase.cpp 62 | ) 63 | 64 | add_dependencies( 65 | ${PROJECT_NAME} 66 | ${catkin_EXPORTED_TARGETS} 67 | ) 68 | 69 | # https://stackoverflow.com/questions/2222162/how-to-apply-fvisibility-option-to-symbols-in-static-libraries 70 | # special link option. 71 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--exclude-libs,ALL") 72 | # set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wl,--exclude-libs,ALL") 73 | target_link_options(${PROJECT_NAME} PRIVATE "-Wl,--exclude-libs,ALL") 74 | 75 | target_link_libraries( 76 | ${PROJECT_NAME} 77 | ${catkin_LIBRARIES} 78 | ) 79 | 80 | #hide all symboles except the explicitly marked ones. 81 | set_target_properties(${PROJECT_NAME} PROPERTIES CXX_VISIBILITY_PRESET hidden) 82 | set_target_properties(${PROJECT_NAME} PROPERTIES C_VISIBILITY_PRESET hidden) 83 | 84 | install(TARGETS ${PROJECT_NAME} 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 88 | ) 89 | install(DIRECTORY include/${PROJECT_NAME}/ 90 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 91 | ) 92 | -------------------------------------------------------------------------------- /soem_interface_rsl/include/soem_interface_rsl/EthercatBusManagerBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #pragma once 24 | 25 | // std 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | namespace soem_interface_rsl { 33 | 34 | /** 35 | * @brief Class for managing multiple ethercat busses 36 | */ 37 | class EthercatBusManagerBase { 38 | public: 39 | using BusMap = std::unordered_map>; 40 | 41 | EthercatBusManagerBase() = default; 42 | virtual ~EthercatBusManagerBase() = default; 43 | 44 | /** 45 | * @brief Adds an ethercat bus to the manager. The manager takes 46 | * ownership of the bus pointer 47 | * 48 | * @param bus Raw bus ptr 49 | * 50 | * @return True if bus has been added successfully 51 | */ 52 | bool addEthercatBus(soem_interface_rsl::EthercatBusBase* bus); 53 | 54 | /** 55 | * @brief Adds an ethercat bus to the manager. The manager takes 56 | * ownership of the bus pointer 57 | * 58 | * @param bus Unique bus ptr 59 | * 60 | * @return True if bus has been added successfully 61 | */ 62 | bool addEthercatBus(std::unique_ptr bus); 63 | 64 | /** 65 | * @brief Starts up all busses and puts them in operational mode 66 | * 67 | * @return True if successful 68 | */ 69 | bool startupAllBuses(); 70 | 71 | /** 72 | * @brief Starts up all busses 73 | * 74 | * @return True if successful 75 | */ 76 | bool startupCommunication(); 77 | 78 | /** 79 | * @brief Sets all busses to safe operational state 80 | */ 81 | void setBussesSafeOperational(); 82 | 83 | /** 84 | * @brief Sets all busses to pre operational state 85 | */ 86 | void setBussesPreOperational(); 87 | 88 | /** 89 | * @brief Sets all busses to operational state 90 | */ 91 | void setBussesOperational(); 92 | 93 | /** 94 | * @brief Waits for the slave to reach a state 95 | * 96 | * @param[in] state Ethercat state 97 | * @param[in] slave Slave address, 0 = all slaves 98 | * @param[in] busName The name of the bus 99 | * @param[in] maxRetries The maximum retries 100 | * @param[in] retrySleep The retry sleep 101 | */ 102 | void waitForState(const uint16_t state, const uint16_t slave = 0, const std::string busName = "", const unsigned int maxRetries = 40); 103 | 104 | /** 105 | * @brief Calls update read on all busses 106 | */ 107 | void readAllBuses(); 108 | 109 | /** 110 | * @brief Calls update write on all busses 111 | */ 112 | void writeToAllBuses(); 113 | 114 | /** 115 | * @brief Calls shutdown on all busses 116 | */ 117 | void shutdownAllBuses(); 118 | 119 | /** 120 | * @brief Returns a non owning bus pointer. The manager still manages all 121 | * the buses 122 | * 123 | * @param[in] name The bus name 124 | * 125 | * @return A shared_ptr to the bus. 126 | */ 127 | EthercatBusBase* getBusByName(const std::string& name) const { return buses_.at(name).get(); } 128 | 129 | /** 130 | * @brief Returns an owning bus pointer. The manager is now not managing 131 | * this bus anymore 132 | * 133 | * @param[in] name The bus name 134 | * 135 | * @return A unique_ptr to the bus. 136 | */ 137 | std::unique_ptr extractBusByName(const std::string& name); 138 | 139 | /** 140 | * @brief Extracts all buses from the manager. The manager is now not 141 | * managing any buses anymore 142 | * 143 | * @return Bus map 144 | */ 145 | BusMap extractBuses(); 146 | 147 | /** 148 | * Check if all buses are ok. 149 | * @return True if all buses are ok. 150 | */ 151 | bool allBusesAreOk(); 152 | 153 | protected: 154 | // Mutex prohibiting simultaneous access to EtherCAT bus manager. 155 | std::mutex busMutex_; 156 | BusMap buses_; 157 | }; 158 | 159 | using EthercatBusManagerBasePtr = std::shared_ptr; 160 | 161 | } // namespace soem_interface_rsl 162 | -------------------------------------------------------------------------------- /soem_interface_rsl/include/soem_interface_rsl/common/EthercatTypes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include "soem_rsl_export.h" 27 | 28 | namespace soem_interface_rsl { 29 | 30 | // mainly reexports some of SOEM's Macros in a namespace fashion to not collide with any other soem installations on the system. 31 | 32 | // namespaced export of the EC_STATE from soem for ethercat slave sdks. 33 | enum class SOEM_RSL_EXPORT ETHERCAT_SM_STATE : uint16_t { 34 | /** No valid state. */ 35 | NONE = 0x00, 36 | /** Init state*/ 37 | INIT = 0x01, 38 | /** Pre-operational. */ 39 | PRE_OP = 0x02, 40 | /** Boot state*/ 41 | BOOT = 0x03, 42 | /** Safe-operational. */ 43 | SAFE_OP = 0x04, 44 | /** Operational */ 45 | OPERATIONAL = 0x08, 46 | /** Error or ACK error */ 47 | ACK = 0x10, 48 | ERROR = 0x10 49 | }; 50 | 51 | enum class SOEM_RSL_EXPORT ETHERCAT_TYPE : uint16_t { 52 | ECT_BOOLEAN = 0x0001, 53 | ECT_INTEGER8 = 0x0002, 54 | ECT_INTEGER16 = 0x0003, 55 | ECT_INTEGER32 = 0x0004, 56 | ECT_UNSIGNED8 = 0x0005, 57 | ECT_UNSIGNED16 = 0x0006, 58 | ECT_UNSIGNED32 = 0x0007, 59 | ECT_REAL32 = 0x0008, 60 | ECT_VISIBLE_STRING = 0x0009, 61 | ECT_OCTET_STRING = 0x000A, 62 | ECT_UNICODE_STRING = 0x000B, 63 | ECT_TIME_OF_DAY = 0x000C, 64 | ECT_TIME_DIFFERENCE = 0x000D, 65 | ECT_DOMAIN = 0x000F, 66 | ECT_INTEGER24 = 0x0010, 67 | ECT_REAL64 = 0x0011, 68 | ECT_INTEGER64 = 0x0015, 69 | ECT_UNSIGNED24 = 0x0016, 70 | ECT_UNSIGNED64 = 0x001B, 71 | ECT_BIT1 = 0x0030, 72 | ECT_BIT2 = 0x0031, 73 | ECT_BIT3 = 0x0032, 74 | ECT_BIT4 = 0x0033, 75 | ECT_BIT5 = 0x0034, 76 | ECT_BIT6 = 0x0035, 77 | ECT_BIT7 = 0x0036, 78 | ECT_BIT8 = 0x0037 79 | }; 80 | 81 | // note this is a simplification - the ethercat specs have a more fine grained access control. (Ecat specificiation Part 6, page 61.) 82 | namespace EcAccess { // make sure that no name conflicts, but allowing implicit conversion. 83 | enum EcAccess : uint16_t { 84 | NOT_IMPL = 0b0000'0000'0000'0000, 85 | WO = 0b0000'0000'0011'1000, 86 | RO = 0b0000'0000'0000'0111, 87 | RW = 0b0000'0000'0011'1111 88 | // more specialization. 89 | }; 90 | } 91 | 92 | } // namespace soem_interface_rsl 93 | -------------------------------------------------------------------------------- /soem_interface_rsl/include/soem_interface_rsl/common/Macros.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #pragma once 24 | 25 | // std 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // message logger 32 | #include "soem_rsl_export.h" 33 | #include 34 | 35 | namespace soem_interface_rsl::common { 36 | 37 | class MessageLog { 38 | public: 39 | using Log = std::deque>; 40 | 41 | protected: 42 | static constexpr size_t maxLogSize_ = 20; 43 | 44 | static std::mutex logMutex_; 45 | static Log log_; 46 | 47 | public: 48 | static void insertMessage(message_logger::log::levels::Level level, const std::string& message); 49 | static Log getLog(); 50 | static void clearLog(); 51 | static Log getAndClearLog(); 52 | }; 53 | 54 | } // namespace soem_interface_rsl 55 | -------------------------------------------------------------------------------- /soem_interface_rsl/include/soem_interface_rsl/common/ThreadSleep.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #pragma once 24 | 25 | namespace soem_interface_rsl { 26 | 27 | void threadSleep(const double duration); 28 | 29 | } // namespace soem_interface_rsl 30 | -------------------------------------------------------------------------------- /soem_interface_rsl/include/soem_interface_rsl/common/soem_rsl_export.h: -------------------------------------------------------------------------------- 1 | #ifndef SOEM_RSL_EXPORT_H 2 | #define SOEM_RSL_EXPORT_H 3 | 4 | #ifdef SOEM_RSL_STATIC_DEFINE 5 | # define SOEM_RSL_EXPORT 6 | # define SOEM_RSL_NO_EXPORT 7 | #else 8 | # ifndef SOEM_RSL_EXPORT 9 | # ifdef soem_rsl_EXPORTS 10 | /* We are building this library */ 11 | # define SOEM_RSL_EXPORT __attribute__((visibility("default"))) 12 | # else 13 | /* We are using this library */ 14 | # define SOEM_RSL_EXPORT __attribute__((visibility("default"))) 15 | # endif 16 | # endif 17 | 18 | # ifndef SOEM_RSL_NO_EXPORT 19 | # define SOEM_RSL_NO_EXPORT __attribute__((visibility("hidden"))) 20 | # endif 21 | #endif 22 | 23 | #ifndef SOEM_RSL_DEPRECATED 24 | # define SOEM_RSL_DEPRECATED __attribute__ ((__deprecated__)) 25 | #endif 26 | 27 | #ifndef SOEM_RSL_DEPRECATED_EXPORT 28 | # define SOEM_RSL_DEPRECATED_EXPORT SOEM_RSL_EXPORT SOEM_RSL_DEPRECATED 29 | #endif 30 | 31 | #ifndef SOEM_RSL_DEPRECATED_NO_EXPORT 32 | # define SOEM_RSL_DEPRECATED_NO_EXPORT SOEM_RSL_NO_EXPORT SOEM_RSL_DEPRECATED 33 | #endif 34 | 35 | #if 0 /* DEFINE_NO_DEPRECATED */ 36 | # ifndef SOEM_RSL_NO_DEPRECATED 37 | # define SOEM_RSL_NO_DEPRECATED 38 | # endif 39 | #endif 40 | 41 | #endif /* SOEM_RSL_EXPORT_H */ 42 | -------------------------------------------------------------------------------- /soem_interface_rsl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 4 | Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 5 | Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 6 | Mario Mauerer, Remo Diethelm 7 | 8 | This file is part of the soem_interface_rsl. 9 | 10 | The soem_interface_rsl is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation, either version 3 of the License, or 13 | (at your option) any later version. 14 | 15 | The seom_interface is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU General Public License 21 | along with the soem_interface_rsl. If not, see . 22 | <--> 23 | 24 | 25 | 26 | soem_interface_rsl 27 | 0.4.0 28 | C++ library to create multiple ethercat devices on one or multiple buses. Contains abstract base classes for the communication interface. 29 | 30 | GPLv3 31 | 32 | Markus Staeuble 33 | Jonas Junger 34 | Johannes Pankert 35 | Lennart Nachtigall 36 | 37 | 38 | Johannes Pankert 39 | 40 | catkin 41 | message_logger 42 | soem_rsl 43 | 44 | 45 | -------------------------------------------------------------------------------- /soem_interface_rsl/src/soem_interface_rsl/EthercatBusManagerBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | // anydrive 24 | #include "soem_interface_rsl/EthercatBusManagerBase.hpp" 25 | 26 | namespace soem_interface_rsl { 27 | 28 | bool EthercatBusManagerBase::addEthercatBus(soem_interface_rsl::EthercatBusBase* bus) { 29 | if (bus == nullptr) { 30 | MELO_ERROR_STREAM("[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") 31 | return false; 32 | } 33 | 34 | std::lock_guard lock(busMutex_); 35 | const auto& it = buses_.find(bus->getName()); 36 | if (it == buses_.end()) { 37 | buses_.insert(std::make_pair(bus->getName(), std::unique_ptr(bus))); 38 | return true; 39 | } else { 40 | return false; 41 | } 42 | } 43 | 44 | bool EthercatBusManagerBase::addEthercatBus(std::unique_ptr bus) { 45 | if (bus == nullptr) { 46 | MELO_ERROR_STREAM("[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") 47 | return false; 48 | } 49 | 50 | std::lock_guard lock(busMutex_); 51 | const auto& it = buses_.find(bus->getName()); 52 | if (it == buses_.end()) { 53 | buses_.insert(std::make_pair(bus->getName(), std::move(bus))); 54 | return true; 55 | } else { 56 | return false; 57 | } 58 | } 59 | 60 | bool EthercatBusManagerBase::startupAllBuses() { 61 | bool success = startupCommunication(); 62 | setBussesOperational(); 63 | return success; 64 | } 65 | 66 | void EthercatBusManagerBase::setBussesOperational() { 67 | std::lock_guard lock(busMutex_); 68 | // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. 69 | for (auto& bus : buses_) { 70 | bus.second->setState(ETHERCAT_SM_STATE::OPERATIONAL); 71 | } 72 | } 73 | 74 | void EthercatBusManagerBase::setBussesPreOperational() { 75 | std::lock_guard lock(busMutex_); 76 | // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. 77 | for (auto& bus : buses_) { 78 | bus.second->setState(ETHERCAT_SM_STATE::PRE_OP); 79 | } 80 | } 81 | 82 | void EthercatBusManagerBase::setBussesSafeOperational() { 83 | std::lock_guard lock(busMutex_); 84 | // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. 85 | for (auto& bus : buses_) { 86 | bus.second->setState(ETHERCAT_SM_STATE::SAFE_OP); 87 | } 88 | } 89 | 90 | void EthercatBusManagerBase::waitForState(const uint16_t state, const uint16_t slave, const std::string busName, 91 | const unsigned int maxRetries) { 92 | std::lock_guard lock(busMutex_); 93 | if (busName.empty()) { 94 | for (auto& bus : buses_) { 95 | bus.second->waitForState(state, slave, maxRetries); 96 | } 97 | } else { 98 | buses_.at(busName)->waitForState(state, slave, maxRetries); 99 | } 100 | } 101 | 102 | bool EthercatBusManagerBase::startupCommunication() { 103 | std::lock_guard lock(busMutex_); 104 | for (auto& bus : buses_) { 105 | if (!bus.second->startup(true)) { 106 | MELO_ERROR_STREAM("Failed to startup bus '" << bus.first << "'."); 107 | return false; 108 | } 109 | } 110 | return true; 111 | } 112 | 113 | void EthercatBusManagerBase::readAllBuses() { 114 | std::lock_guard lock(busMutex_); 115 | for (auto& bus : buses_) { 116 | bus.second->updateRead(); 117 | } 118 | } 119 | 120 | void EthercatBusManagerBase::writeToAllBuses() { 121 | std::lock_guard lock(busMutex_); 122 | for (auto& bus : buses_) { 123 | bus.second->updateWrite(); 124 | } 125 | } 126 | 127 | void EthercatBusManagerBase::shutdownAllBuses() { 128 | std::lock_guard lock(busMutex_); 129 | for (auto& bus : buses_) { 130 | bus.second->shutdown(); 131 | } 132 | } 133 | 134 | std::unique_ptr EthercatBusManagerBase::extractBusByName(const std::string& name) { 135 | std::unique_ptr busOut = std::move(buses_.at(name)); 136 | buses_.erase(name); 137 | return busOut; 138 | } 139 | 140 | EthercatBusManagerBase::BusMap EthercatBusManagerBase::extractBuses() { 141 | BusMap busMapOut; 142 | 143 | for (auto& bus : buses_) { 144 | busMapOut.insert(std::make_pair(bus.first, std::move(bus.second))); 145 | } 146 | 147 | buses_.clear(); 148 | return busMapOut; 149 | } 150 | 151 | bool EthercatBusManagerBase::allBusesAreOk() { 152 | for (const auto& bus : buses_) { 153 | if (!bus.second->busIsOk()) { 154 | return false; 155 | } 156 | } 157 | return true; 158 | } 159 | 160 | } // namespace soem_interface_rsl 161 | -------------------------------------------------------------------------------- /soem_interface_rsl/src/soem_interface_rsl/EthercatSlaveBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | // soem_interface_rsl 24 | #include 25 | 26 | namespace soem_interface_rsl { 27 | 28 | EthercatSlaveBase::EthercatSlaveBase(EthercatBusBase* bus, const uint32_t address) : bus_(bus), address_(address) {} 29 | EthercatSlaveBase::EthercatSlaveBase() : bus_(nullptr), address_(0) {} 30 | 31 | bool EthercatSlaveBase::sendSdoReadGeneric(const std::string& indexString, const std::string& subindexString, 32 | const std::string& valueTypeString, std::string& valueString) { 33 | (void)indexString; 34 | (void)subindexString; 35 | (void)valueTypeString; 36 | (void)valueString; 37 | printWarnNotImplemented(); 38 | return false; 39 | } 40 | 41 | bool EthercatSlaveBase::sendSdoWriteGeneric(const std::string& indexString, const std::string& subindexString, 42 | const std::string& valueTypeString, const std::string& valueString) { 43 | (void)indexString; 44 | (void)subindexString; 45 | (void)valueTypeString; 46 | (void)valueString; 47 | printWarnNotImplemented(); 48 | return false; 49 | } 50 | 51 | bool EthercatSlaveBase::sendSdoReadVisibleString(const uint16_t index, const uint8_t subindex, std::string& value) { 52 | std::lock_guard lock(mutex_); 53 | return bus_->sendSdoReadVisibleString(address_, index, subindex, value); 54 | } 55 | 56 | } // namespace soem_interface_rsl 57 | -------------------------------------------------------------------------------- /soem_interface_rsl/src/soem_interface_rsl/common/Macros.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | // soem_interface_rsl 24 | #include "soem_interface_rsl/common/Macros.hpp" 25 | 26 | namespace soem_interface_rsl { 27 | namespace common { 28 | 29 | std::mutex MessageLog::logMutex_; 30 | MessageLog::Log MessageLog::log_; 31 | 32 | void MessageLog::insertMessage(message_logger::log::levels::Level level, const std::string& message) { 33 | std::lock_guard lock(logMutex_); 34 | log_.push_back({level, message}); 35 | if (log_.size() > maxLogSize_) { 36 | log_.pop_front(); 37 | } 38 | } 39 | 40 | MessageLog::Log MessageLog::getLog() { 41 | std::lock_guard lock(logMutex_); 42 | return log_; 43 | } 44 | 45 | void MessageLog::clearLog() { 46 | std::lock_guard lock(logMutex_); 47 | log_.clear(); 48 | } 49 | 50 | MessageLog::Log MessageLog::getAndClearLog() { 51 | const Log log = getLog(); 52 | clearLog(); 53 | return log; 54 | } 55 | 56 | } // namespace common 57 | } // namespace soem_interface_rsl 58 | -------------------------------------------------------------------------------- /soem_interface_rsl/src/soem_interface_rsl/common/ThreadSleep.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** Copyright (2019-2020) Robotics Systems Lab - ETH Zurich: 3 | ** Markus Staeuble, Jonas Junger, Johannes Pankert, Philipp Leemann, 4 | ** Tom Lankhorst, Samuel Bachmann, Gabriel Hottiger, Lennert Nachtigall, 5 | ** Mario Mauerer, Remo Diethelm 6 | ** 7 | ** This file is part of the soem_interface_rsl. 8 | ** 9 | ** The soem_interface_rsl is free software: you can redistribute it and/or modify 10 | ** it under the terms of the GNU General Public License as published by 11 | ** the Free Software Foundation, either version 3 of the License, or 12 | ** (at your option) any later version. 13 | ** 14 | ** The seom_interface is distributed in the hope that it will be useful, 15 | ** but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | ** GNU General Public License for more details. 18 | ** 19 | ** You should have received a copy of the GNU General Public License 20 | ** along with the soem_interface_rsl. If not, see . 21 | */ 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace soem_interface_rsl { 28 | 29 | void threadSleep(const double duration) { 30 | std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(1e9 * duration))); 31 | } 32 | 33 | } // namespace soem_interface_rsl 34 | -------------------------------------------------------------------------------- /soem_rsl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake/Modules") 3 | project(soem_rsl C) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | if(WIN32) 8 | set(OS "win32") 9 | include_directories(oshw/win32/wpcap/Include) 10 | if(CMAKE_SIZEOF_VOID_P EQUAL 8) 11 | link_directories(${CMAKE_CURRENT_LIST_DIR}/oshw/win32/wpcap/Lib/x64) 12 | elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) 13 | link_directories(${CMAKE_CURRENT_LIST_DIR}/oshw/win32/wpcap/Lib) 14 | endif() 15 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_SECURE_NO_WARNINGS") 16 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /WX") 17 | set(OS_LIBS wpcap.lib Packet.lib Ws2_32.lib Winmm.lib) 18 | elseif(UNIX AND NOT APPLE) 19 | set(OS "linux") 20 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror") 21 | set(OS_LIBS pthread rt) 22 | elseif(APPLE) 23 | # This must come *before* linux or MacOSX will identify as Unix. 24 | set(OS "macosx") 25 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror") 26 | set(OS_LIBS pthread pcap) 27 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "rt-kernel") 28 | set(OS "rtk") 29 | message("ARCH is ${ARCH}") 30 | message("BSP is ${BSP}") 31 | include_directories(oshw/${OS}/${ARCH}) 32 | file(GLOB OSHW_EXTRA_SOURCES oshw/${OS}/${ARCH}/*.c) 33 | set(OSHW_SOURCES "${OS_HW_SOURCES} ${OSHW_ARCHSOURCES}") 34 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror") 35 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-but-set-variable") 36 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-function") 37 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-format") 38 | set(OS_LIBS "-Wl,--start-group -l${BSP} -l${ARCH} -lkern -ldev -lsio -lblock -lfs -lusb -llwip -leth -li2c -lrtc -lcan -lnand -lspi -lnor -lpwm -ladc -ltrace -lc -lm -Wl,--end-group") 39 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "rtems") 40 | message("Building for RTEMS") 41 | set(OS "rtems") 42 | set(soem_rsl_LIB_INSTALL_DIR ${LIB_DIR}) 43 | set(BUILD_TESTS FALSE) 44 | endif() 45 | 46 | message("OS is ${OS}") 47 | 48 | 49 | catkin_package( 50 | INCLUDE_DIRS 51 | ./soem_rsl 52 | ./soem_rsl/soem_rsl 53 | ./soem_rsl/osal 54 | ./soem_rsl/osal/${OS} 55 | ./soem_rsl/oshw/${OS} 56 | LIBRARIES soem_rsl) 57 | 58 | set(soem_rsl_INCLUDE_INSTALL_DIR include/soem_rsl) 59 | set(soem_rsl_LIB_INSTALL_DIR lib) 60 | set(BUILD_TESTS TRUE) 61 | 62 | 63 | 64 | file(GLOB soem_rsl_SOURCES soem_rsl/soem_rsl/*.c) 65 | file(GLOB OSAL_SOURCES soem_rsl/osal/${OS}/*.c) 66 | file(GLOB OSHW_SOURCES soem_rsl/oshw/${OS}/*.c) 67 | 68 | file(GLOB soem_rsl_HEADERS soem_rsl/soem_rsl/*.h) 69 | file(GLOB OSAL_HEADERS soem_rsl/osal/osal.h soem_rsl/osal/${OS}/*.h) 70 | file(GLOB OSHW_HEADERS soem_rsl/oshw/${OS}/*.h) 71 | 72 | message(${OSAL_HEADERS}) 73 | 74 | add_library(soem_rsl STATIC 75 | ${soem_rsl_SOURCES} 76 | ${OSAL_SOURCES} 77 | ${OSHW_SOURCES} 78 | ${OSHW_EXTRA_SOURCES}) 79 | target_link_libraries(soem_rsl ${OS_LIBS}) 80 | set_target_properties (soem_rsl PROPERTIES POSITION_INDEPENDENT_CODE ON) 81 | 82 | target_include_directories(soem_rsl PUBLIC 83 | $ 84 | $) 85 | 86 | target_include_directories(soem_rsl PUBLIC 87 | $ 88 | $) 89 | 90 | target_include_directories(soem_rsl PUBLIC 91 | $ 92 | $) 93 | 94 | target_include_directories(soem_rsl 95 | PUBLIC $ 96 | $) 97 | 98 | message("LIB_DIR: ${soem_rsl_LIB_INSTALL_DIR}") 99 | 100 | install(TARGETS soem_rsl EXPORT soem_rslConfig DESTINATION ${soem_rsl_LIB_INSTALL_DIR}) 101 | 102 | install(EXPORT soem_rslConfig DESTINATION share/soem_rsl/cmake) 103 | 104 | install(FILES 105 | ${soem_rsl_HEADERS} 106 | ${OSAL_HEADERS} 107 | ${OSHW_HEADERS} 108 | DESTINATION ${soem_rsl_INCLUDE_INSTALL_DIR} 109 | ) 110 | 111 | install(FILES 112 | ./soem_rsl/soem_rsl/ethercattype.h 113 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include 114 | ) 115 | 116 | #message(${CMAKE_INSTALL_PREFIX}) 117 | #add_custom_target(link_target ALL 118 | # COMMAND ${CMAKE_COMMAND} -E create_symlink ${CMAKE_INSTALL_PREFIX}/include/soem_rsl/ethercattype.h ${CMAKE_INSTALL_PREFIX}/include/ethercattype.h 119 | # VERBATIM) 120 | 121 | if(BUILD_TESTS) 122 | add_subdirectory(soem_rsl/test/linux/slaveinfo) 123 | add_subdirectory(soem_rsl/test/linux/eepromtool) 124 | add_subdirectory(soem_rsl/test/linux/simple_test) 125 | endif() -------------------------------------------------------------------------------- /soem_rsl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | soem_rsl 4 | 1.6.0 5 | The soem_rsl package 6 | Johannes Pankert 7 | GPLv3 8 | Remo Diethelm 9 | catkin 10 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | install 3 | *~ 4 | /doc/latex 5 | /doc/html 6 | tags 7 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/.travis.yml: -------------------------------------------------------------------------------- 1 | jobs: 2 | include: 3 | - dist: xenial 4 | - dist: bionic 5 | - os: osx 6 | 7 | language: c 8 | 9 | script: 10 | - mkdir build 11 | - pushd build 12 | - cmake .. -DCMAKE_BUILD_TYPE=Release 13 | - make install 14 | - popd 15 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/ChangeLog: -------------------------------------------------------------------------------- 1 | Version 1.1.2 : 2008-12-23 2 | - First public release 3 | Version 1.1.3 : 2009-01-18 4 | - Added CoE, RxPDO and TxPDO (still Beta) 5 | - Added FoE, Read and Write (still Beta) 6 | - Fixed BigEndian conversion missing in ethercatconfig.c (credit:Serge Bloch) 7 | - Fixed segmented transfer bug in ethercatcoe.c 8 | Version 1.1.4 : 2009-04-22 9 | - Changed FMMU configuration algorithm 10 | - Changed ec_slave structure around SM and FMMU storage 11 | - Added SYNC1 configuration 12 | - Fixed bug in FoE write 13 | Version 1.2.0 : 2009-09-12 14 | - Changed the license to GPLv2 only 15 | - Added note to usage terms (please read carefully) 16 | - Eeprom acces is released to slave after preop 17 | - Fixed linux-64 define bug (uint32 and int32) 18 | - Fixed maximum frame size 19 | Version 1.2.2 : 2010-02-22 20 | - Fixed bugs in ec_adddatagram. 21 | - Fixed several bugs in CoE object dictionary read functions. 22 | - Fixed bug in PDO mapping read function. 23 | - Changed ec_slave structure around topology and delay variables. 24 | - Added several constants in ethercattype.c 25 | Version 1.2.3 : 2010-03-07 26 | - Clear SM enable if size is 0, even if enable is set in SII. 27 | - Fixed bug in DC propagation delay calculation. Branches with only non DC slaves now correctly close root port. 28 | - Fixed bug in ec_receive_processdata(), wkc now checks for EC_NOFRAME instead of 0. 29 | - Fixed bug in makefile 30 | Version 1.2.4 : 2010-04-10 31 | - Added SoE, servo over EtherCAT support. 32 | - Added SoE read request and write request. 33 | - Added SoE segmented transfers. 34 | - Added SoE error response. 35 | - Added SoE errors to print module. 36 | - Added Auto config of SoE process data. 37 | Version 1.2.5 : 2011-06-13 38 | - Added eepromtool, it can read and write the ESC eeprom of a designated slave. 39 | - Rewrite of eeprom read/write functions. 40 | - Added infrastructure change to allow slave groups. 41 | - Added recovery and reconfiguration of slaves after connection loss. 42 | - Improved CoE PDO assignment read from slaves, no longer assume assign indexes as functionally fixed. 43 | - Fixed bugs in ec_config_map(). 44 | - Added EC_STATE_BOOT constant. 45 | - Fixed mailbox size bug, In and Out mailbox can now be of different size. 46 | - Fixed SM type bug. 47 | - Fixed FoE bugs. 48 | - Fixed siigetbyte() unaligned copy. 49 | - Fixed bug in nicdrv.c, socket handles are 0 included. 50 | - Fixed bug in ethercatconfig.c causing memory corruption. 51 | Version 1.2.8 : 2012-06-14 52 | - Changed directory structure. 53 | - Changed make file. 54 | - Moved hardware / OS dependend part in separate directories. 55 | - Added firm_update tool to upload firmware to slaves in Boot state, use with care. 56 | - Added DC for LRD/LWR case. 57 | - Separated expectedWKC to inputsWKC and outputsWKC. 58 | - Added PreOP->SafeOP hooks in configuration functions. 59 | - With CoE use expedited download if mailbox size is very small and object <= 4 bytes. 60 | - Fixed NetX mailbox configuration behaviour. 61 | - Fixed FoE write bug. 62 | - Added mailbox error handling. 63 | - Fixed SII string read bug. 64 | - Fixed bug in table lookup for printing 65 | - Rewrite of ec_recover_slave() and ec_reconfigure_slave() 66 | - Added -map option in slaveinfo, shows soem_rsl IO mapping of all slaves found. 67 | Version 1.3.0 : 2013-02-24 68 | - Added win32 target. 69 | - Added rtk target. 70 | - Compiles under gcc / visual-c / borland-c. 71 | - Multiple port support. One master can run concurrent stacks on multiple network ports. 72 | - All global vars are encapsulated in context struct. 73 | - All timing abstracted in osal.c. 74 | - Linux timing converted to get_clock(CLOCK_MONOTONIC). 75 | - Error messages updated to latest ETG1020 document. 76 | - FoE transfers now support busy response. 77 | - Fixed NetX100 configuration behaviour. 78 | - Fixed linux gettimeofday() to get_clock(). 79 | - Fixed eeprom cache flush on reinit. 80 | - Fixed make for new gcc linker version. 81 | Version 1.3.1 : 2015-03-11 82 | - Added intime target. 83 | - Added rtk\fec target. 84 | - Compiles under gcc / visual-c / borland-c / intime. 85 | - Added multi-threaded configuration for parallel configurations of slaves 86 | Version 1.3.2 : 2018-02-02 87 | - Made a mistake. DON'T USE! 88 | Version 1.3.3 : 2018-02-02 89 | - Added rtems target. 90 | - Added support for overlapping IOmap. 91 | 92 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/LICENSE: -------------------------------------------------------------------------------- 1 | Simple Open EtherCAT Master Library 2 | 3 | Copyright (C) 2005-2017 Speciaal Machinefabriek Ketels v.o.f. 4 | Copyright (C) 2005-2017 Arthur Ketels 5 | Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 6 | Copyright (C) 2009-2017 rt-labs AB, Sweden 7 | 8 | soem_rsl is free software; you can redistribute it and/or modify it under the terms 9 | of the GNU General Public License version 2 as published by the Free Software 10 | Foundation. 11 | 12 | soem_rsl is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | As a special exception, if other files instantiate templates or use macros or 17 | inline functions from this file, or you compile this file and link it with other 18 | works to produce a work based on this file, this file does not by itself cause 19 | the resulting work to be covered by the GNU General Public License. However the 20 | source code for this file must still be made available in accordance with 21 | section (3) of the GNU General Public License. 22 | 23 | This exception does not invalidate any other reasons why a work based on this 24 | file might be covered by the GNU General Public License. 25 | 26 | The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual 27 | property of, and protected by Beckhoff Automation GmbH. You can use soem_rsl for the 28 | sole purpose of creating, using and/or selling or otherwise distributing an 29 | EtherCAT network master provided that an EtherCAT Master License is obtained 30 | from Beckhoff Automation GmbH. 31 | 32 | In case you did not receive a copy of the EtherCAT Master License along with 33 | soem_rsl write to Beckhoff Automation GmbH, Eiserstrasse 5, D-33415 Verl, Germany 34 | (www.beckhoff.com). 35 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/README.md: -------------------------------------------------------------------------------- 1 | # Simple Open EtherCAT Master Library 2 | [![Build Status](https://travis-ci.org/OpenEtherCATsociety/soem_rsl.svg?branch=master)](https://travis-ci.org/OpenEtherCATsociety/soem_rsl) 3 | [![Build status](https://ci.appveyor.com/api/projects/status/bqgirjsxog9k1odf?svg=true)](https://ci.appveyor.com/project/hefloryd/soem_rsl-5kq8b) 4 | 5 | BUILDING 6 | ======== 7 | 8 | 9 | Prerequisites for all platforms 10 | ------------------------------- 11 | 12 | * CMake 2.8.0 or later 13 | 14 | 15 | Windows (Visual Studio) 16 | ----------------------- 17 | 18 | * Start a Visual Studio command prompt then: 19 | * `mkdir build` 20 | * `cd build` 21 | * `cmake .. -G "NMake Makefiles"` 22 | * `nmake` 23 | 24 | Linux & macOS 25 | -------------- 26 | 27 | * `mkdir build` 28 | * `cd build` 29 | * `cmake ..` 30 | * `make` 31 | 32 | ERIKA Enterprise RTOS 33 | --------------------- 34 | 35 | * Refer to http://www.erika-enterprise.com/wiki/index.php?title=EtherCAT_Master 36 | 37 | Documentation 38 | ------------- 39 | 40 | See https://openethercatsociety.github.io/doc/soem_rsl/ 41 | 42 | 43 | Want to contribute to soem_rsl or SOES? 44 | ----------------------------------- 45 | 46 | If you want to contribute to soem_rsl or SOES you will need to sign a Contributor 47 | License Agreement and send it to us either by e-mail or by physical mail. More 48 | information is available in the [PDF](http://openethercatsociety.github.io/cla/cla_soem_rsl_soes.pdf). 49 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/appveyor.yml: -------------------------------------------------------------------------------- 1 | version: "{build}" 2 | 3 | install: 4 | - cmd: '"C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x86' 5 | 6 | build_script: 7 | - cmd: mkdir build 8 | - cmd: cd build 9 | - cmd: cmake .. -G "NMake Makefiles" -DCMAKE_BUILD_TYPE=Release 10 | - cmd: nmake install 11 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rt-kernel-C.cmake: -------------------------------------------------------------------------------- 1 | 2 | message("rt-kernel-C.cmake") 3 | 4 | # Determine toolchain 5 | include(CMakeForceCompiler) 6 | 7 | if(${ARCH} MATCHES "kinetis") 8 | cmake_force_c_compiler(arm-eabi-gcc GNU) 9 | cmake_force_cxx_compiler(arm-eabi-g++ GNU) 10 | elseif(${ARCH} MATCHES "bfin") 11 | cmake_force_c_compiler(bfin-elf-gcc GNU) 12 | cmake_force_cxx_compiler(bfin-elf-g++ GNU) 13 | endif() 14 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rt-kernel-gcc-bfin.cmake: -------------------------------------------------------------------------------- 1 | 2 | set(MACHINE "-mcpu=bf537") 3 | set(LDFLAGS "-T${RT_KERNEL_PATH}/bsp/${BSP}/${BSP}.ld") 4 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rt-kernel-gcc-kinetis.cmake: -------------------------------------------------------------------------------- 1 | 2 | message("rt-kernel-gcc-kinetis.cmake") 3 | 4 | #SET_PROPERTY(GLOBAL PROPERTY ARCH kinetis) 5 | #SET_PROPERTY(GLOBAL PROPERTY BSP twrk60) 6 | 7 | set(MACHINE "-mfpu=vfp -mcpu=cortex-m3 -mthumb") 8 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rt-kernel-gcc.cmake: -------------------------------------------------------------------------------- 1 | 2 | message("rt-kernel-gcc.cmake") 3 | 4 | set(CMAKE_C_OUTPUT_EXTENSION .o) 5 | set_property(GLOBAL PROPERTY TARGET_SUPPORTS_SHARED_LIBS FALSE) 6 | 7 | set(CFLAGS "${CFLAGS} -Wall -Wextra -Wno-unused-parameter -Werror") 8 | set(CFLAGS "${CFLAGS} -fomit-frame-pointer -fno-strict-aliasing -fshort-wchar") 9 | #set(CFLAGS" ${CFLAGS} -B$(GCC_PATH)/libexec/gcc") 10 | 11 | set(CXXFLAGS "${CXXFLAGS} -fno-rtti -fno-exceptions") 12 | 13 | set(LDFLAGS "${LDFLAGS} -nostartfiles") 14 | 15 | set(CMAKE_C_FLAGS "${CFLAGS} ${MACHINE}" CACHE STRING "") 16 | set(CMAKE_EXE_LINKER_FLAGS "${MACHINE} ${LDFLAGS}" CACHE STRING "") 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rt-kernel.cmake: -------------------------------------------------------------------------------- 1 | if(__RTK_CMAKE_INCLUDED) 2 | return() 3 | endif() 4 | set(__RTK_CMAKE_INCLUDED TRUE) 5 | message("rt-kernel.cmake") 6 | 7 | include_directories( 8 | ${RT_KERNEL_PATH}/include 9 | ${RT_KERNEL_PATH}/include/kern 10 | ${RT_KERNEL_PATH}/kern 11 | ${RT_KERNEL_PATH}/include/drivers 12 | ${RT_KERNEL_PATH}/include/arch/${ARCH} 13 | ${RT_KERNEL_PATH}/bsp/${BSP}/include 14 | ${RT_KERNEL_PATH}/lwip/src/include 15 | ${RT_KERNEL_PATH}/lwip/src/include/ipv4 16 | ) 17 | 18 | link_directories( 19 | ${RT_KERNEL_PATH}/lib/${ARCH} 20 | ) 21 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Modules/Platform/rtems.cmake: -------------------------------------------------------------------------------- 1 | message("rtems.cmake") 2 | 3 | set(ARCH ${HOST}) 4 | set(BSP ${RTEMS_BSP}) 5 | 6 | set(CMAKE_C_COMPILER_FORCED true) 7 | set(CMAKE_CXX_COMPILER_FORCED true) 8 | set(CMAKE_C_COMPILER ${RTEMS_TOOLS_PATH}/bin/${ARCH}-gcc) 9 | set(CMAKE_CXX_COMPILER ${RTEMS_TOOLS_PATH}/bin/${ARCH}-g++) 10 | 11 | set(soem_rsl_INCLUDE_INSTALL_DIR ${INCLUDE_DIR}/soem_rsl) 12 | set(soem_rsl_LIB_INSTALL_DIR ${LIB_DIR}) 13 | 14 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${HOST_C_FLAGS}") 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${HOST_CXX_FLAGS}") 16 | 17 | if(NOT ${HOST_LIBS} STREQUAL "") 18 | set(OS_LIBS "rtemscpu bsd ${HOST_LIBS}") 19 | else() 20 | set(OS_LIBS "-lrtemscpu -lbsd") 21 | endif() 22 | 23 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Toolchains/rt-kernel-bfin.cmake: -------------------------------------------------------------------------------- 1 | 2 | message("rt-kernel-kinetis.cmake") 3 | 4 | set(CMAKE_SYSTEM_NAME rt-kernel) 5 | set(CMAKE_SYSTEM_VERSION 1) 6 | set(CMAKE_SYSTEM_PROCESSOR bfin) 7 | 8 | set(ARCH bfin CACHE STRING "Architecture") 9 | set(BSP stamp537 CACHE STRING "Board") 10 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/cmake/Toolchains/rt-kernel-kinetis.cmake: -------------------------------------------------------------------------------- 1 | 2 | message("rt-kernel-kinetis.cmake") 3 | 4 | set(CMAKE_SYSTEM_NAME rt-kernel) 5 | set(CMAKE_SYSTEM_VERSION 1) 6 | set(CMAKE_SYSTEM_PROCESSOR kinetis) 7 | 8 | set(ARCH kinetis CACHE STRING "Architecture") 9 | set(BSP twrk60 CACHE STRING "Board") 10 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/doc/images/legacy_iomap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/doc/images/legacy_iomap.png -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/doc/images/memory_layout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/doc/images/memory_layout.png -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/doc/images/overlapping_iomap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/doc/images/overlapping_iomap.png -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/drvcomment.txt: -------------------------------------------------------------------------------- 1 | For faster irq response through the NIC/NAPI/Socket layer set for TG3 (at eth0) 2 | ethtool -C eth0 rx-usecs 0 rx-frames 1 tx-usecs 0 tx-frames 1 3 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/erika/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "ee_x86_64_tsc.h" 14 | 15 | #define USECS_PER_SEC 1000000 16 | #define NSECS_PER_SEC 1000000000 17 | 18 | uint64_t osEE_x86_64_tsc_read(void); 19 | 20 | void ee_usleep(uint32 usec); 21 | 22 | inline int osal_usleep (uint32 usec) 23 | { 24 | ee_usleep(usec); 25 | return 0; 26 | } 27 | 28 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 29 | { 30 | uint64_t time = osEE_x86_64_tsc_read(); 31 | tv->tv_sec = time/NSECS_PER_SEC; 32 | tv->tv_sec += 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start */ 33 | tv->tv_usec = (time%NSECS_PER_SEC)/1000; 34 | return 0; 35 | } 36 | 37 | ec_timet osal_current_time(void) 38 | { 39 | struct timeval current_time; 40 | ec_timet ret; 41 | 42 | osal_gettimeofday(¤t_time, 0); 43 | ret.sec = current_time.tv_sec; 44 | ret.usec = current_time.tv_usec; 45 | return ret; 46 | } 47 | 48 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) 49 | { 50 | if (end->usec < start->usec) { 51 | diff->sec = end->sec - start->sec - 1; 52 | diff->usec = end->usec + USECS_PER_SEC - start->usec; 53 | } else { 54 | diff->sec = end->sec - start->sec; 55 | diff->usec = end->usec - start->usec; 56 | } 57 | } 58 | 59 | void osal_timer_start(osal_timert *self, uint32 timeout_usec) 60 | { 61 | struct timeval start_time; 62 | struct timeval timeout; 63 | struct timeval stop_time; 64 | 65 | osal_gettimeofday(&start_time, 0); 66 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 67 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 68 | timeradd(&start_time, &timeout, &stop_time); 69 | 70 | self->stop_time.sec = stop_time.tv_sec; 71 | self->stop_time.usec = stop_time.tv_usec; 72 | } 73 | 74 | boolean osal_timer_is_expired (osal_timert *self) 75 | { 76 | struct timeval current_time; 77 | struct timeval stop_time; 78 | int is_not_yet_expired; 79 | 80 | osal_gettimeofday (¤t_time, 0); 81 | stop_time.tv_sec = self->stop_time.sec; 82 | stop_time.tv_usec = self->stop_time.usec; 83 | is_not_yet_expired = timercmp (¤t_time, &stop_time, <); 84 | /* OSEE_PRINT("current: %d:%d -- expire: %d:%d -- result: %d\n", */ 85 | /* current_time.tv_sec, */ 86 | /* current_time.tv_usec, */ 87 | /* stop_time.tv_sec, */ 88 | /* stop_time.tv_usec, */ 89 | /* is_not_yet_expired); */ 90 | 91 | return is_not_yet_expired == FALSE; 92 | } 93 | 94 | void *osal_malloc(size_t size) 95 | { 96 | return malloc(size); 97 | } 98 | 99 | void osal_free(void *ptr) 100 | { 101 | free(ptr); 102 | } 103 | 104 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/erika/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | // define if debug print is needed 19 | #define EC_DEBUG 20 | 21 | #ifdef EC_DEBUG 22 | #define EC_PRINT OSEE_PRINT 23 | #else 24 | #define EC_PRINT(...) do {} while (0) 25 | #endif 26 | 27 | #ifndef PACKED 28 | #define PACKED_BEGIN 29 | #define PACKED __attribute__((__packed__)) 30 | #define PACKED_END 31 | #endif 32 | 33 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz); 34 | void *osal_malloc(size_t size); 35 | void osal_free(void *ptr); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/intime/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | static int64_t sysfrequency; 11 | static double qpc2usec; 12 | 13 | #define USECS_PER_SEC 1000000 14 | 15 | int osal_gettimeofday (struct timeval *tv, struct timezone *tz) 16 | { 17 | return gettimeofday (tv, tz); 18 | } 19 | 20 | ec_timet osal_current_time (void) 21 | { 22 | struct timeval current_time; 23 | ec_timet return_value; 24 | 25 | osal_gettimeofday (¤t_time, 0); 26 | return_value.sec = current_time.tv_sec; 27 | return_value.usec = current_time.tv_usec; 28 | return return_value; 29 | } 30 | 31 | void osal_timer_start (osal_timert * self, uint32 timeout_usec) 32 | { 33 | struct timeval start_time; 34 | struct timeval timeout; 35 | struct timeval stop_time; 36 | 37 | osal_gettimeofday (&start_time, 0); 38 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 39 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 40 | timeradd (&start_time, &timeout, &stop_time); 41 | 42 | self->stop_time.sec = stop_time.tv_sec; 43 | self->stop_time.usec = stop_time.tv_usec; 44 | } 45 | 46 | boolean osal_timer_is_expired (osal_timert * self) 47 | { 48 | struct timeval current_time; 49 | struct timeval stop_time; 50 | int is_not_yet_expired; 51 | 52 | osal_gettimeofday (¤t_time, 0); 53 | stop_time.tv_sec = self->stop_time.sec; 54 | stop_time.tv_usec = self->stop_time.usec; 55 | is_not_yet_expired = timercmp (¤t_time, &stop_time, <); 56 | 57 | return is_not_yet_expired == FALSE; 58 | } 59 | 60 | int osal_usleep(uint32 usec) 61 | { 62 | RtSleepEx (usec / 1000); 63 | return 1; 64 | } 65 | 66 | /* Mutex is not needed when running single threaded */ 67 | 68 | void osal_mtx_lock(osal_mutex_t * mtx) 69 | { 70 | /* RtWaitForSingleObject((HANDLE)mtx, INFINITE); */ 71 | } 72 | 73 | void osal_mtx_unlock(osal_mutex_t * mtx) 74 | { 75 | /* RtReleaseMutex((HANDLE)mtx); */ 76 | } 77 | 78 | int osal_mtx_lock_timeout(osal_mutex_t * mtx, uint32_t time_ms) 79 | { 80 | /* return RtWaitForSingleObject((HANDLE)mtx, time_ms); */ 81 | return 0; 82 | } 83 | 84 | osal_mutex_t * osal_mtx_create(void) 85 | { 86 | /* return (void*)RtCreateMutex(NULL, FALSE, NULL); */ 87 | return (void *)0; 88 | } 89 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/intime/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #ifdef _MSC_VER 25 | #define PACKED_BEGIN __pragma(pack(push, 1)) 26 | #define PACKED 27 | #define PACKED_END __pragma(pack(pop)) 28 | #elif defined(__GNUC__) 29 | #define PACKED_BEGIN 30 | #define PACKED __attribute__((__packed__)) 31 | #define PACKED_END 32 | #endif 33 | #endif 34 | 35 | #define OSAL_THREAD_HANDLE RTHANDLE 36 | #define OSAL_THREAD_FUNC void 37 | #define OSAL_THREAD_FUNC_RT void 38 | 39 | #ifdef __cplusplus 40 | } 41 | #endif 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/linux/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define USECS_PER_SEC 1000000 14 | 15 | int osal_usleep (uint32 usec) 16 | { 17 | struct timespec ts; 18 | ts.tv_sec = usec / USECS_PER_SEC; 19 | ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; 20 | /* usleep is deprecated, use nanosleep instead */ 21 | return nanosleep(&ts, NULL); 22 | } 23 | 24 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 25 | { 26 | struct timespec ts; 27 | int return_value; 28 | (void)tz; /* Not used */ 29 | 30 | /* Use clock_gettime to prevent possible live-lock. 31 | * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. 32 | * If this function preempts timeadjust and it uses vpage it live-locks. 33 | * Also when using XENOMAI, only clock_gettime is RT safe */ 34 | return_value = clock_gettime(CLOCK_MONOTONIC, &ts); 35 | tv->tv_sec = ts.tv_sec; 36 | tv->tv_usec = ts.tv_nsec / 1000; 37 | return return_value; 38 | } 39 | 40 | ec_timet osal_current_time(void) 41 | { 42 | struct timeval current_time; 43 | ec_timet return_value; 44 | 45 | osal_gettimeofday(¤t_time, 0); 46 | return_value.sec = current_time.tv_sec; 47 | return_value.usec = current_time.tv_usec; 48 | return return_value; 49 | } 50 | 51 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) 52 | { 53 | if (end->usec < start->usec) { 54 | diff->sec = end->sec - start->sec - 1; 55 | diff->usec = end->usec + 1000000 - start->usec; 56 | } 57 | else { 58 | diff->sec = end->sec - start->sec; 59 | diff->usec = end->usec - start->usec; 60 | } 61 | } 62 | 63 | void osal_timer_start(osal_timert * self, uint32 timeout_usec) 64 | { 65 | struct timeval start_time; 66 | struct timeval timeout; 67 | struct timeval stop_time; 68 | 69 | osal_gettimeofday(&start_time, 0); 70 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 71 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 72 | timeradd(&start_time, &timeout, &stop_time); 73 | 74 | self->stop_time.sec = stop_time.tv_sec; 75 | self->stop_time.usec = stop_time.tv_usec; 76 | } 77 | 78 | boolean osal_timer_is_expired (osal_timert * self) 79 | { 80 | struct timeval current_time; 81 | struct timeval stop_time; 82 | int is_not_yet_expired; 83 | 84 | osal_gettimeofday(¤t_time, 0); 85 | stop_time.tv_sec = self->stop_time.sec; 86 | stop_time.tv_usec = self->stop_time.usec; 87 | is_not_yet_expired = timercmp(¤t_time, &stop_time, <); 88 | 89 | return is_not_yet_expired == FALSE; 90 | } 91 | 92 | void *osal_malloc(size_t size) 93 | { 94 | return malloc(size); 95 | } 96 | 97 | void osal_free(void *ptr) 98 | { 99 | free(ptr); 100 | } 101 | 102 | int osal_thread_create(void *thandle, int stacksize, void *func, void *param) 103 | { 104 | int ret; 105 | pthread_attr_t attr; 106 | pthread_t *threadp; 107 | 108 | threadp = thandle; 109 | pthread_attr_init(&attr); 110 | pthread_attr_setstacksize(&attr, stacksize); 111 | ret = pthread_create(threadp, &attr, func, param); 112 | if(ret < 0) 113 | { 114 | return 0; 115 | } 116 | return 1; 117 | } 118 | 119 | int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) 120 | { 121 | int ret; 122 | pthread_attr_t attr; 123 | struct sched_param schparam; 124 | pthread_t *threadp; 125 | 126 | threadp = thandle; 127 | pthread_attr_init(&attr); 128 | pthread_attr_setstacksize(&attr, stacksize); 129 | ret = pthread_create(threadp, &attr, func, param); 130 | pthread_attr_destroy(&attr); 131 | if(ret < 0) 132 | { 133 | return 0; 134 | } 135 | memset(&schparam, 0, sizeof(schparam)); 136 | schparam.sched_priority = 40; 137 | ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); 138 | if(ret < 0) 139 | { 140 | return 0; 141 | } 142 | 143 | return 1; 144 | } 145 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/linux/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #define PACKED_BEGIN 25 | #define PACKED __attribute__((__packed__)) 26 | #define PACKED_END 27 | #endif 28 | 29 | #include 30 | #define OSAL_THREAD_HANDLE pthread_t * 31 | #define OSAL_THREAD_FUNC void 32 | #define OSAL_THREAD_FUNC_RT void 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/macosx/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define USECS_PER_SEC 1000000 14 | 15 | int osal_usleep (uint32 usec) 16 | { 17 | struct timespec ts; 18 | ts.tv_sec = usec / USECS_PER_SEC; 19 | ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; 20 | /* usleep is deprecated, use nanosleep instead */ 21 | return nanosleep(&ts, NULL); 22 | } 23 | 24 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 25 | { 26 | struct timespec ts; 27 | int return_value; 28 | (void)tz; /* Not used */ 29 | 30 | /* Use clock_gettime to prevent possible live-lock. 31 | * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. 32 | * If this function preempts timeadjust and it uses vpage it live-locks. 33 | * Also when using XENOMAI, only clock_gettime is RT safe */ 34 | return_value = clock_gettime(CLOCK_MONOTONIC, &ts); 35 | tv->tv_sec = ts.tv_sec; 36 | tv->tv_usec = ts.tv_nsec / 1000; 37 | return return_value; 38 | } 39 | 40 | ec_timet osal_current_time(void) 41 | { 42 | struct timeval current_time; 43 | ec_timet return_value; 44 | 45 | osal_gettimeofday(¤t_time, 0); 46 | return_value.sec = current_time.tv_sec; 47 | return_value.usec = current_time.tv_usec; 48 | return return_value; 49 | } 50 | 51 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) 52 | { 53 | if (end->usec < start->usec) { 54 | diff->sec = end->sec - start->sec - 1; 55 | diff->usec = end->usec + 1000000 - start->usec; 56 | } 57 | else { 58 | diff->sec = end->sec - start->sec; 59 | diff->usec = end->usec - start->usec; 60 | } 61 | } 62 | 63 | void osal_timer_start(osal_timert * self, uint32 timeout_usec) 64 | { 65 | struct timeval start_time; 66 | struct timeval timeout; 67 | struct timeval stop_time; 68 | 69 | osal_gettimeofday(&start_time, 0); 70 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 71 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 72 | timeradd(&start_time, &timeout, &stop_time); 73 | 74 | self->stop_time.sec = stop_time.tv_sec; 75 | self->stop_time.usec = stop_time.tv_usec; 76 | } 77 | 78 | boolean osal_timer_is_expired (osal_timert * self) 79 | { 80 | struct timeval current_time; 81 | struct timeval stop_time; 82 | int is_not_yet_expired; 83 | 84 | osal_gettimeofday(¤t_time, 0); 85 | stop_time.tv_sec = self->stop_time.sec; 86 | stop_time.tv_usec = self->stop_time.usec; 87 | is_not_yet_expired = timercmp(¤t_time, &stop_time, <); 88 | 89 | return is_not_yet_expired == FALSE; 90 | } 91 | 92 | void *osal_malloc(size_t size) 93 | { 94 | return malloc(size); 95 | } 96 | 97 | void osal_free(void *ptr) 98 | { 99 | free(ptr); 100 | } 101 | 102 | int osal_thread_create(void *thandle, int stacksize, void *func, void *param) 103 | { 104 | int ret; 105 | pthread_attr_t attr; 106 | pthread_t *threadp; 107 | 108 | threadp = thandle; 109 | pthread_attr_init(&attr); 110 | pthread_attr_setstacksize(&attr, stacksize); 111 | ret = pthread_create(threadp, &attr, func, param); 112 | if(ret < 0) 113 | { 114 | return 0; 115 | } 116 | return 1; 117 | } 118 | 119 | int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) 120 | { 121 | int ret; 122 | pthread_attr_t attr; 123 | struct sched_param schparam; 124 | pthread_t *threadp; 125 | 126 | threadp = thandle; 127 | pthread_attr_init(&attr); 128 | pthread_attr_setstacksize(&attr, stacksize); 129 | ret = pthread_create(threadp, &attr, func, param); 130 | pthread_attr_destroy(&attr); 131 | if(ret < 0) 132 | { 133 | return 0; 134 | } 135 | memset(&schparam, 0, sizeof(schparam)); 136 | schparam.sched_priority = 40; 137 | ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); 138 | if(ret < 0) 139 | { 140 | return 0; 141 | } 142 | 143 | return 1; 144 | } 145 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/macosx/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #define PACKED_BEGIN 25 | #define PACKED __attribute__((__packed__)) 26 | #define PACKED_END 27 | #endif 28 | 29 | #include 30 | #define OSAL_THREAD_HANDLE pthread_t * 31 | #define OSAL_THREAD_FUNC void 32 | #define OSAL_THREAD_FUNC_RT void 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/osal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_ 7 | #define _osal_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | #include "osal_defs.h" 15 | #include 16 | 17 | /* General types */ 18 | #ifndef TRUE 19 | #define TRUE 1 20 | #endif 21 | #ifndef FALSE 22 | #define FALSE 0 23 | #endif 24 | typedef uint8_t boolean; 25 | typedef int8_t int8; 26 | typedef int16_t int16; 27 | typedef int32_t int32; 28 | typedef uint8_t uint8; 29 | typedef uint16_t uint16; 30 | typedef uint32_t uint32; 31 | typedef int64_t int64; 32 | typedef uint64_t uint64; 33 | typedef float float32; 34 | typedef double float64; 35 | 36 | typedef struct 37 | { 38 | uint32 sec; /*< Seconds elapsed since the Epoch (Jan 1, 1970) */ 39 | uint32 usec; /*< Microseconds elapsed since last second boundary */ 40 | } ec_timet; 41 | 42 | typedef struct osal_timer 43 | { 44 | ec_timet stop_time; 45 | } osal_timert; 46 | 47 | void osal_timer_start(osal_timert * self, uint32 timeout_us); 48 | boolean osal_timer_is_expired(osal_timert * self); 49 | int osal_usleep(uint32 usec); 50 | ec_timet osal_current_time(void); 51 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff); 52 | int osal_thread_create(void *thandle, int stacksize, void *func, void *param); 53 | int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/rtems/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define USECS_PER_SEC 1000000 14 | 15 | int osal_usleep (uint32 usec) 16 | { 17 | struct timespec ts; 18 | ts.tv_sec = usec / USECS_PER_SEC; 19 | ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; 20 | /* usleep is deprecated, use nanosleep instead */ 21 | return nanosleep(&ts, NULL); 22 | } 23 | 24 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 25 | { 26 | struct timespec ts; 27 | int return_value; 28 | (void)tz; /* Not used */ 29 | 30 | /* Use clock_gettime to prevent possible live-lock. 31 | * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. 32 | * If this function preempts timeadjust and it uses vpage it live-locks. 33 | * Also when using XENOMAI, only clock_gettime is RT safe */ 34 | return_value = clock_gettime(CLOCK_MONOTONIC, &ts); 35 | tv->tv_sec = ts.tv_sec; 36 | tv->tv_usec = ts.tv_nsec / 1000; 37 | return return_value; 38 | } 39 | 40 | ec_timet osal_current_time(void) 41 | { 42 | struct timeval current_time; 43 | ec_timet return_value; 44 | 45 | osal_gettimeofday(¤t_time, 0); 46 | return_value.sec = current_time.tv_sec; 47 | return_value.usec = current_time.tv_usec; 48 | return return_value; 49 | } 50 | 51 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) 52 | { 53 | if (end->usec < start->usec) { 54 | diff->sec = end->sec - start->sec - 1; 55 | diff->usec = end->usec + 1000000 - start->usec; 56 | } 57 | else { 58 | diff->sec = end->sec - start->sec; 59 | diff->usec = end->usec - start->usec; 60 | } 61 | } 62 | 63 | void osal_timer_start(osal_timert * self, uint32 timeout_usec) 64 | { 65 | struct timeval start_time; 66 | struct timeval timeout; 67 | struct timeval stop_time; 68 | 69 | osal_gettimeofday(&start_time, 0); 70 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 71 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 72 | timeradd(&start_time, &timeout, &stop_time); 73 | 74 | self->stop_time.sec = stop_time.tv_sec; 75 | self->stop_time.usec = stop_time.tv_usec; 76 | } 77 | 78 | boolean osal_timer_is_expired (osal_timert * self) 79 | { 80 | struct timeval current_time; 81 | struct timeval stop_time; 82 | int is_not_yet_expired; 83 | 84 | osal_gettimeofday(¤t_time, 0); 85 | stop_time.tv_sec = self->stop_time.sec; 86 | stop_time.tv_usec = self->stop_time.usec; 87 | is_not_yet_expired = timercmp(¤t_time, &stop_time, <); 88 | 89 | return is_not_yet_expired == FALSE; 90 | } 91 | 92 | void *osal_malloc(size_t size) 93 | { 94 | return malloc(size); 95 | } 96 | 97 | void osal_free(void *ptr) 98 | { 99 | free(ptr); 100 | } 101 | 102 | int osal_thread_create(void *thandle, int stacksize, void *func, void *param) 103 | { 104 | int ret; 105 | pthread_attr_t attr; 106 | pthread_t *threadp; 107 | 108 | threadp = thandle; 109 | pthread_attr_init(&attr); 110 | pthread_attr_setstacksize(&attr, stacksize); 111 | ret = pthread_create(threadp, &attr, func, param); 112 | if(ret < 0) 113 | { 114 | return 0; 115 | } 116 | return 1; 117 | } 118 | 119 | int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) 120 | { 121 | int ret; 122 | pthread_attr_t attr; 123 | struct sched_param schparam; 124 | pthread_t *threadp; 125 | 126 | threadp = thandle; 127 | pthread_attr_init(&attr); 128 | pthread_attr_setstacksize(&attr, stacksize); 129 | ret = pthread_create(threadp, &attr, func, param); 130 | pthread_attr_destroy(&attr); 131 | if(ret < 0) 132 | { 133 | return 0; 134 | } 135 | memset(&schparam, 0, sizeof(schparam)); 136 | schparam.sched_priority = 40; 137 | ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); 138 | if(ret < 0) 139 | { 140 | return 0; 141 | } 142 | 143 | return 1; 144 | } 145 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/rtems/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #define PACKED_BEGIN 25 | #define PACKED __attribute__((__packed__)) 26 | #define PACKED_END 27 | #endif 28 | 29 | #include 30 | #define OSAL_THREAD_HANDLE pthread_t * 31 | #define OSAL_THREAD_FUNC void 32 | #define OSAL_THREAD_FUNC_RT void 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/rtk/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define timercmp(a, b, CMP) \ 13 | (((a)->tv_sec == (b)->tv_sec) ? \ 14 | ((a)->tv_usec CMP (b)->tv_usec) : \ 15 | ((a)->tv_sec CMP (b)->tv_sec)) 16 | #define timeradd(a, b, result) \ 17 | do { \ 18 | (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ 19 | (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ 20 | if ((result)->tv_usec >= 1000000) \ 21 | { \ 22 | ++(result)->tv_sec; \ 23 | (result)->tv_usec -= 1000000; \ 24 | } \ 25 | } while (0) 26 | #define timersub(a, b, result) \ 27 | do { \ 28 | (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ 29 | (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ 30 | if ((result)->tv_usec < 0) { \ 31 | --(result)->tv_sec; \ 32 | (result)->tv_usec += 1000000; \ 33 | } \ 34 | } while (0) 35 | 36 | #define USECS_PER_SEC 1000000 37 | #define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND) 38 | 39 | 40 | /* Workaround for rt-labs defect 776. 41 | * Default implementation of udelay() didn't work correctly when tick was 42 | * shorter than one millisecond. 43 | */ 44 | void udelay (uint32_t us) 45 | { 46 | tick_t ticks = (us / USECS_PER_TICK) + 1; 47 | task_delay (ticks); 48 | } 49 | 50 | int gettimeofday(struct timeval *tp, void *tzp) 51 | { 52 | tick_t tick = tick_get(); 53 | tick_t ticks_left; 54 | 55 | ASSERT (tp != NULL); 56 | 57 | tp->tv_sec = tick / CFG_TICKS_PER_SECOND; 58 | 59 | ticks_left = tick % CFG_TICKS_PER_SECOND; 60 | tp->tv_usec = ticks_left * USECS_PER_TICK; 61 | ASSERT (tp->tv_usec < USECS_PER_SEC); 62 | 63 | return 0; 64 | } 65 | 66 | int osal_usleep (uint32 usec) 67 | { 68 | udelay(usec); 69 | return 0; 70 | } 71 | 72 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 73 | { 74 | return gettimeofday(tv, tz); 75 | } 76 | 77 | ec_timet osal_current_time (void) 78 | { 79 | struct timeval current_time; 80 | ec_timet return_value; 81 | 82 | gettimeofday (¤t_time, 0); 83 | return_value.sec = current_time.tv_sec; 84 | return_value.usec = current_time.tv_usec; 85 | return return_value; 86 | } 87 | 88 | void osal_timer_start (osal_timert * self, uint32 timeout_usec) 89 | { 90 | struct timeval start_time; 91 | struct timeval timeout; 92 | struct timeval stop_time; 93 | 94 | gettimeofday (&start_time, 0); 95 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 96 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 97 | timeradd (&start_time, &timeout, &stop_time); 98 | 99 | self->stop_time.sec = stop_time.tv_sec; 100 | self->stop_time.usec = stop_time.tv_usec; 101 | } 102 | 103 | boolean osal_timer_is_expired (osal_timert * self) 104 | { 105 | struct timeval current_time; 106 | struct timeval stop_time; 107 | int is_not_yet_expired; 108 | 109 | gettimeofday (¤t_time, 0); 110 | stop_time.tv_sec = self->stop_time.sec; 111 | stop_time.tv_usec = self->stop_time.usec; 112 | is_not_yet_expired = timercmp (¤t_time, &stop_time, <); 113 | 114 | return is_not_yet_expired == false; 115 | } 116 | 117 | void *osal_malloc(size_t size) 118 | { 119 | return malloc(size); 120 | } 121 | 122 | void osal_free(void *ptr) 123 | { 124 | free(ptr); 125 | } 126 | 127 | int osal_thread_create(void *thandle, int stacksize, void *func, void *param) 128 | { 129 | thandle = task_spawn ("worker", func, 6,stacksize, param); 130 | if(!thandle) 131 | { 132 | return 0; 133 | } 134 | return 1; 135 | } 136 | 137 | int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) 138 | { 139 | thandle = task_spawn ("worker_rt", func, 15 ,stacksize, param); 140 | if(!thandle) 141 | { 142 | return 0; 143 | } 144 | return 1; 145 | } 146 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/rtk/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #define PACKED_BEGIN 25 | #define PACKED __attribute__((__packed__)) 26 | #define PACKED_END 27 | #endif 28 | 29 | #define OSAL_THREAD_HANDLE task_t * 30 | #define OSAL_THREAD_FUNC void 31 | #define OSAL_THREAD_FUNC_RT void 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/vxworks/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | // define if debug printf is needed 10 | //#define EC_DEBUG 11 | 12 | #ifdef EC_DEBUG 13 | #define EC_PRINT printf 14 | #else 15 | #define EC_PRINT(...) do {} while (0) 16 | #endif 17 | 18 | #ifndef PACKED 19 | #define PACKED_BEGIN 20 | #define PACKED __attribute__((__packed__)) 21 | #define PACKED_END 22 | #endif 23 | 24 | #define OSAL_THREAD_HANDLE TASK_ID 25 | #define OSAL_THREAD_FUNC void 26 | #define OSAL_THREAD_FUNC_RT void 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/win32/osal.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include "osal_win32.h" 9 | 10 | static int64_t sysfrequency; 11 | static double qpc2usec; 12 | 13 | #define USECS_PER_SEC 1000000 14 | 15 | int osal_getrelativetime(struct timeval *tv, struct timezone *tz) 16 | { 17 | int64_t wintime, usecs; 18 | if(!sysfrequency) 19 | { 20 | timeBeginPeriod(1); 21 | QueryPerformanceFrequency((LARGE_INTEGER *)&sysfrequency); 22 | qpc2usec = 1000000.0 / sysfrequency; 23 | } 24 | QueryPerformanceCounter((LARGE_INTEGER *)&wintime); 25 | usecs = (int64_t)((double)wintime * qpc2usec); 26 | tv->tv_sec = (long)(usecs / 1000000); 27 | tv->tv_usec = (long)(usecs - (tv->tv_sec * 1000000)); 28 | 29 | return 1; 30 | } 31 | 32 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 33 | { 34 | FILETIME system_time; 35 | int64 system_time64, usecs; 36 | 37 | /* The offset variable is required to switch from Windows epoch (January 1, 1601) to 38 | * Unix epoch (January 1, 1970). Number of days between both epochs: 134.774 39 | * 40 | * The time returned by GetSystemTimeAsFileTime() changes in 100 ns steps, so the 41 | * following factors are required for the conversion from days to 100 ns steps: 42 | * 43 | * 86.400 seconds per day; 1.000.000 microseconds per second; 10 * 100 ns per microsecond 44 | */ 45 | int64 offset = -134774LL * 86400LL * 1000000LL * 10LL; 46 | 47 | GetSystemTimeAsFileTime(&system_time); 48 | 49 | system_time64 = ((int64)(system_time.dwHighDateTime) << 32) + (int64)system_time.dwLowDateTime; 50 | system_time64 += offset; 51 | usecs = system_time64 / 10; 52 | 53 | tv->tv_sec = (long)(usecs / 1000000); 54 | tv->tv_usec = (long)(usecs - (tv->tv_sec * 1000000)); 55 | 56 | return 1; 57 | } 58 | 59 | ec_timet osal_current_time (void) 60 | { 61 | struct timeval current_time; 62 | ec_timet return_value; 63 | 64 | osal_gettimeofday (¤t_time, 0); 65 | return_value.sec = current_time.tv_sec; 66 | return_value.usec = current_time.tv_usec; 67 | return return_value; 68 | } 69 | 70 | void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) 71 | { 72 | if (end->usec < start->usec) { 73 | diff->sec = end->sec - start->sec - 1; 74 | diff->usec = end->usec + 1000000 - start->usec; 75 | } 76 | else { 77 | diff->sec = end->sec - start->sec; 78 | diff->usec = end->usec - start->usec; 79 | } 80 | } 81 | 82 | void osal_timer_start (osal_timert *self, uint32 timeout_usec) 83 | { 84 | struct timeval start_time; 85 | struct timeval timeout; 86 | struct timeval stop_time; 87 | 88 | osal_getrelativetime (&start_time, 0); 89 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 90 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 91 | timeradd (&start_time, &timeout, &stop_time); 92 | 93 | self->stop_time.sec = stop_time.tv_sec; 94 | self->stop_time.usec = stop_time.tv_usec; 95 | } 96 | 97 | boolean osal_timer_is_expired (osal_timert *self) 98 | { 99 | struct timeval current_time; 100 | struct timeval stop_time; 101 | int is_not_yet_expired; 102 | 103 | osal_getrelativetime (¤t_time, 0); 104 | stop_time.tv_sec = self->stop_time.sec; 105 | stop_time.tv_usec = self->stop_time.usec; 106 | is_not_yet_expired = timercmp (¤t_time, &stop_time, <); 107 | 108 | return is_not_yet_expired == FALSE; 109 | } 110 | 111 | int osal_usleep(uint32 usec) 112 | { 113 | osal_timert qtime; 114 | osal_timer_start(&qtime, usec); 115 | if(usec >= 1000) 116 | { 117 | SleepEx(usec / 1000, FALSE); 118 | } 119 | while(!osal_timer_is_expired(&qtime)); 120 | return 1; 121 | } 122 | 123 | void *osal_malloc(size_t size) 124 | { 125 | return malloc(size); 126 | } 127 | 128 | void osal_free(void *ptr) 129 | { 130 | free(ptr); 131 | } 132 | 133 | int osal_thread_create(void **thandle, int stacksize, void *func, void *param) 134 | { 135 | *thandle = CreateThread(NULL, stacksize, func, param, 0, NULL); 136 | if(!thandle) 137 | { 138 | return 0; 139 | } 140 | return 1; 141 | } 142 | 143 | int osal_thread_create_rt(void **thandle, int stacksize, void *func, void *param) 144 | { 145 | int ret; 146 | ret = osal_thread_create(thandle, stacksize, func, param); 147 | if (ret) 148 | { 149 | ret = SetThreadPriority(*thandle, THREAD_PRIORITY_TIME_CRITICAL); 150 | } 151 | return ret; 152 | } 153 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/win32/osal_defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_defs_ 7 | #define _osal_defs_ 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | // define if debug printf is needed 15 | //#define EC_DEBUG 16 | 17 | #ifdef EC_DEBUG 18 | #define EC_PRINT printf 19 | #else 20 | #define EC_PRINT(...) do {} while (0) 21 | #endif 22 | 23 | #ifndef PACKED 24 | #define PACKED_BEGIN __pragma(pack(push, 1)) 25 | #define PACKED 26 | #define PACKED_END __pragma(pack(pop)) 27 | #endif 28 | 29 | #define OSAL_THREAD_HANDLE HANDLE 30 | #define OSAL_THREAD_FUNC void 31 | #define OSAL_THREAD_FUNC_RT void 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/osal/win32/osal_win32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #ifndef _osal_win32_ 7 | #define _osal_win32_ 8 | 9 | /* Convenience macros for operations on timevals. 10 | NOTE: `timercmp' does not work for >= or <=. */ 11 | # define timerisset(tvp) ((tvp)->tv_sec || (tvp)->tv_usec) 12 | # define timeradd(a, b, result) \ 13 | do { \ 14 | (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ 15 | (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ 16 | if ((result)->tv_usec >= 1000000) \ 17 | { \ 18 | ++(result)->tv_sec; \ 19 | (result)->tv_usec -= 1000000; \ 20 | } \ 21 | } while (0) 22 | # define timersub(a, b, result) \ 23 | do { \ 24 | (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ 25 | (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ 26 | if ((result)->tv_usec < 0) { \ 27 | --(result)->tv_sec; \ 28 | (result)->tv_usec += 1000000; \ 29 | } \ 30 | } while (0) 31 | 32 | int osal_gettimeofday (struct timeval *tv, struct timezone *tz); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/erika/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | typedef struct 20 | { 21 | /** socket connection used */ 22 | int *sock; 23 | /** tx buffer */ 24 | ec_bufT (*txbuf)[EC_MAXBUF]; 25 | /** tx buffer lengths */ 26 | int (*txbuflength)[EC_MAXBUF]; 27 | /** temporary receive buffer */ 28 | ec_bufT *tempbuf; 29 | /** rx buffers */ 30 | ec_bufT (*rxbuf)[EC_MAXBUF]; 31 | /** rx buffer status fields */ 32 | int (*rxbufstat)[EC_MAXBUF]; 33 | /** received MAC source address (middle word) */ 34 | int (*rxsa)[EC_MAXBUF]; 35 | } ec_stackT; 36 | 37 | /** pointer structure to buffers for redundant port */ 38 | typedef struct 39 | { 40 | ec_stackT stack; 41 | int sockhandle; 42 | /** rx buffers */ 43 | ec_bufT rxbuf[EC_MAXBUF]; 44 | /** rx buffer status */ 45 | int rxbufstat[EC_MAXBUF]; 46 | /** rx MAC source address */ 47 | int rxsa[EC_MAXBUF]; 48 | /** temporary rx buffer */ 49 | ec_bufT tempinbuf; 50 | } ecx_redportt; 51 | 52 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 53 | typedef struct 54 | { 55 | ec_stackT stack; 56 | int sockhandle; 57 | /** rx buffers */ 58 | ec_bufT rxbuf[EC_MAXBUF]; 59 | /** rx buffer status */ 60 | int rxbufstat[EC_MAXBUF]; 61 | /** rx MAC source address */ 62 | int rxsa[EC_MAXBUF]; 63 | /** temporary rx buffer */ 64 | ec_bufT tempinbuf; 65 | /** temporary rx buffer status */ 66 | int tempinbufs; 67 | /** transmit buffers */ 68 | ec_bufT txbuf[EC_MAXBUF]; 69 | /** transmit buffer lengths */ 70 | int txbuflength[EC_MAXBUF]; 71 | /** temporary tx buffer */ 72 | ec_bufT txbuf2; 73 | /** temporary tx buffer length */ 74 | int txbuflength2; 75 | /** last used frame index */ 76 | int lastidx; 77 | /** current redundancy state */ 78 | int redstate; 79 | /** pointer to redundancy port and buffers */ 80 | ecx_redportt *redport; 81 | 82 | /** Device id in the device pool */ 83 | int dev_id; 84 | 85 | // TODO: add mutex support 86 | } ecx_portt; 87 | 88 | extern const uint16 priMAC[3]; 89 | extern const uint16 secMAC[3]; 90 | 91 | #ifdef EC_VER1 92 | extern ecx_portt ecx_port; 93 | extern ecx_redportt ecx_redport; 94 | 95 | int ec_setupnic(const char * ifname, int secondary); 96 | int ec_closenic(void); 97 | void ec_setbufstat(int idx, int bufstat); 98 | int ec_getindex(void); 99 | int ec_outframe(int idx, int sock); 100 | int ec_outframe_red(int idx); 101 | int ec_waitinframe(int idx, int timeout); 102 | int ec_srconfirm(int idx,int timeout); 103 | int ec_inframe(int idx, int stacknumber); 104 | #endif 105 | 106 | void ec_setupheader(void *p); 107 | int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary); 108 | int ecx_closenic(ecx_portt *port); 109 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 110 | int ecx_getindex(ecx_portt *port); 111 | int ecx_outframe(ecx_portt *port, int idx, int sock); 112 | int ecx_outframe_red(ecx_portt *port, int idx); 113 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 114 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 115 | 116 | int ecx_inframe(ecx_portt *port, int idx, int stacknumber); 117 | 118 | #ifdef __cplusplus 119 | } 120 | #endif 121 | 122 | #endif 123 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/erika/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "oshw.h" 12 | #include "intel_i210.h" 13 | #include "ethercat.h" 14 | 15 | #if !defined(__gnu_linux__) 16 | #include 17 | #else 18 | #include 19 | #define __htons(x) htobe16(x) 20 | #define __ntohs(x) be16toh(x) 21 | #endif 22 | 23 | ec_adaptert adapters [DEVS_MAX_NB]; 24 | 25 | /** 26 | * Host to Network byte order (i.e. to big endian). 27 | * 28 | * Note that Ethercat uses little endian byte order, except for the Ethernet 29 | * header which is big endian as usual. 30 | */ 31 | inline uint16 oshw_htons(uint16 host) 32 | { 33 | // __htons() is provided by the bare-metal x86 compiler 34 | return __htons(host); 35 | } 36 | 37 | /** 38 | * Network (i.e. big endian) to Host byte order. 39 | * 40 | * Note that Ethercat uses little endian byte order, except for the Ethernet 41 | * header which is big endian as usual. 42 | */ 43 | inline uint16 oshw_ntohs(uint16 network) 44 | { 45 | // __ntohs() is provided by the bare-metal x86 compiler 46 | return __ntohs(network); 47 | } 48 | 49 | /** Create list over available network adapters. 50 | * @return First element in linked list of adapters 51 | */ 52 | ec_adaptert* oshw_find_adapters(void) 53 | { 54 | ec_adaptert *ret = NULL; 55 | if (eth_discover_devices() >= 0) { 56 | for (int i = 0;; ++i) { 57 | struct eth_device *dev = eth_get_device(i); 58 | if (dev == NULL) { 59 | adapters[i-1].next = NULL; 60 | break; 61 | } 62 | strncpy(adapters[i].name, dev->name, MAX_DEVICE_NAME); 63 | adapters[i].next = &adapters[i+1]; 64 | } 65 | ret = &(adapters[0]); 66 | } 67 | return ret; 68 | } 69 | 70 | /** Free memory allocated memory used by adapter collection. 71 | * @param[in] adapter = First element in linked list of adapters 72 | * EC_NOFRAME. 73 | */ 74 | void oshw_free_adapters(ec_adaptert *adapter) 75 | { 76 | } 77 | 78 | extern int ec_slavecount; 79 | 80 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/erika/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "ethercattype.h" 19 | #include "nicdrv.h" 20 | #include "ethercatmain.h" 21 | 22 | uint16 oshw_htons(uint16 hostshort); 23 | uint16 oshw_ntohs(uint16 networkshort); 24 | ec_adaptert* oshw_find_adapters(void); 25 | void oshw_free_adapters(ec_adaptert * adapter); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/intime/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #define HAVE_REMOTE 15 | 16 | #include 17 | #include "hpeif2.h" 18 | 19 | /** pointer structure to Tx and Rx stacks */ 20 | typedef struct 21 | { 22 | /** socket connection used */ 23 | int *sock; 24 | /** tx buffer */ 25 | ec_bufT (*txbuf)[EC_MAXBUF]; 26 | /** tx buffer lengths */ 27 | int (*txbuflength)[EC_MAXBUF]; 28 | /** temporary receive buffer */ 29 | ec_bufT *tempbuf; 30 | /** rx buffers */ 31 | ec_bufT (*rxbuf)[EC_MAXBUF]; 32 | /** rx buffer status fields */ 33 | int (*rxbufstat)[EC_MAXBUF]; 34 | /** received MAC source address (middle word) */ 35 | int (*rxsa)[EC_MAXBUF]; 36 | } ec_stackT; 37 | 38 | typedef struct 39 | { 40 | ec_stackT stack; 41 | /** rx buffers */ 42 | ec_bufT rxbuf[EC_MAXBUF]; 43 | /** rx buffer status */ 44 | int rxbufstat[EC_MAXBUF]; 45 | /** rx MAC source address */ 46 | int rxsa[EC_MAXBUF]; 47 | /** temporary rx buffer */ 48 | ec_bufT tempinbuf; 49 | /* Intime */ 50 | HPEHANDLE handle; 51 | HPERXBUFFERSET *rx_buffers; 52 | HPETXBUFFERSET *tx_buffers[EC_MAXBUF]; 53 | } ecx_redportt; 54 | 55 | typedef struct 56 | { 57 | ec_stackT stack; 58 | /** rx buffers */ 59 | ec_bufT rxbuf[EC_MAXBUF]; 60 | /** rx buffer status */ 61 | int rxbufstat[EC_MAXBUF]; 62 | /** rx MAC source address */ 63 | int rxsa[EC_MAXBUF]; 64 | /** temporary rx buffer */ 65 | ec_bufT tempinbuf; 66 | /** temporary rx buffer status */ 67 | int tempinbufs; 68 | /** transmit buffers */ 69 | ec_bufT txbuf[EC_MAXBUF]; 70 | /** transmit buffer lengths */ 71 | int txbuflength[EC_MAXBUF]; 72 | /** temporary tx buffer */ 73 | ec_bufT txbuf2; 74 | /** temporary tx buffer length */ 75 | int txbuflength2; 76 | /** last used frame index */ 77 | int lastidx; 78 | /** current redundancy state */ 79 | int redstate; 80 | /** pointer to redundancy port and buffers */ 81 | ecx_redportt *redport; 82 | RTHANDLE getindex_region; 83 | RTHANDLE tx_region; 84 | RTHANDLE rx_region; 85 | /* Intime */ 86 | HPEHANDLE handle; 87 | HPERXBUFFERSET *rx_buffers; 88 | HPETXBUFFERSET *tx_buffers[EC_MAXBUF]; 89 | } ecx_portt; 90 | 91 | extern const uint16 priMAC[3]; 92 | extern const uint16 secMAC[3]; 93 | 94 | //extern ecx_portt ecx_port; 95 | //extern ecx_redportt ecx_redport; 96 | 97 | int ec_setupnic(const char * ifname, int secondary); 98 | int ec_closenic(void); 99 | void ec_setupheader(void *p); 100 | void ec_setbufstat(int idx, int bufstat); 101 | int ec_getindex(void); 102 | int ec_outframe(int idx, int sock); 103 | int ec_outframe_red(int idx); 104 | int ec_waitinframe(int idx, int timeout); 105 | int ec_srconfirm(int idx,int timeout); 106 | 107 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 108 | int ecx_closenic(ecx_portt *port); 109 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 110 | int ecx_getindex(ecx_portt *port); 111 | int ecx_outframe(ecx_portt *port, int idx, int sock); 112 | int ecx_outframe_red(ecx_portt *port, int idx); 113 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 114 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/intime/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include "oshw.h" 8 | 9 | /** 10 | * Host to Network byte order (i.e. to big endian). 11 | * 12 | * Note that Ethercat uses little endian byte order, except for the Ethernet 13 | * header which is big endian as usual. 14 | */ 15 | uint16 oshw_htons (uint16 host) 16 | { 17 | uint16 network = htons (host); 18 | return network; 19 | } 20 | 21 | /** 22 | * Network (i.e. big endian) to Host byte order. 23 | * 24 | * Note that Ethercat uses little endian byte order, except for the Ethernet 25 | * header which is big endian as usual. 26 | */ 27 | uint16 oshw_ntohs (uint16 network) 28 | { 29 | uint16 host = ntohs (network); 30 | return host; 31 | } 32 | 33 | /* Create list over available network adapters. 34 | * @return First element in linked list of adapters 35 | */ 36 | ec_adaptert * oshw_find_adapters (void) 37 | { 38 | return NULL; 39 | } 40 | 41 | /** Free memory allocated memory used by adapter collection. 42 | * @param[in] adapter = First element in linked list of adapters 43 | * EC_NOFRAME. 44 | */ 45 | void oshw_free_adapters (ec_adaptert * adapter) 46 | { 47 | 48 | } 49 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/intime/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for oshw.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #include "ethercattype.h" 15 | #include "ethercatmain.h" 16 | #include "nicdrv.h" 17 | 18 | #ifdef __cplusplus 19 | extern "C" { 20 | #endif 21 | 22 | uint16 oshw_htons (uint16 hostshort); 23 | uint16 oshw_ntohs (uint16 networkshort); 24 | ec_adaptert * oshw_find_adapters (void); 25 | void oshw_free_adapters (ec_adaptert * adapter); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/linux/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include 20 | 21 | /** pointer structure to Tx and Rx stacks */ 22 | typedef struct 23 | { 24 | /** socket connection used */ 25 | int *sock; 26 | /** tx buffer */ 27 | ec_bufT (*txbuf)[EC_MAXBUF]; 28 | /** tx buffer lengths */ 29 | int (*txbuflength)[EC_MAXBUF]; 30 | /** temporary receive buffer */ 31 | ec_bufT *tempbuf; 32 | /** rx buffers */ 33 | ec_bufT (*rxbuf)[EC_MAXBUF]; 34 | /** rx buffer status fields */ 35 | int (*rxbufstat)[EC_MAXBUF]; 36 | /** received MAC source address (middle word) */ 37 | int (*rxsa)[EC_MAXBUF]; 38 | } ec_stackT; 39 | 40 | /** pointer structure to buffers for redundant port */ 41 | typedef struct 42 | { 43 | ec_stackT stack; 44 | int sockhandle; 45 | /** rx buffers */ 46 | ec_bufT rxbuf[EC_MAXBUF]; 47 | /** rx buffer status */ 48 | int rxbufstat[EC_MAXBUF]; 49 | /** rx MAC source address */ 50 | int rxsa[EC_MAXBUF]; 51 | /** temporary rx buffer */ 52 | ec_bufT tempinbuf; 53 | } ecx_redportt; 54 | 55 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 56 | typedef struct 57 | { 58 | ec_stackT stack; 59 | int sockhandle; 60 | /** rx buffers */ 61 | ec_bufT rxbuf[EC_MAXBUF]; 62 | /** rx buffer status */ 63 | int rxbufstat[EC_MAXBUF]; 64 | /** rx MAC source address */ 65 | int rxsa[EC_MAXBUF]; 66 | /** temporary rx buffer */ 67 | ec_bufT tempinbuf; 68 | /** temporary rx buffer status */ 69 | int tempinbufs; 70 | /** transmit buffers */ 71 | ec_bufT txbuf[EC_MAXBUF]; 72 | /** transmit buffer lengths */ 73 | int txbuflength[EC_MAXBUF]; 74 | /** temporary tx buffer */ 75 | ec_bufT txbuf2; 76 | /** temporary tx buffer length */ 77 | int txbuflength2; 78 | /** last used frame index */ 79 | int lastidx; 80 | /** current redundancy state */ 81 | int redstate; 82 | /** pointer to redundancy port and buffers */ 83 | ecx_redportt *redport; 84 | pthread_mutex_t getindex_mutex; 85 | pthread_mutex_t tx_mutex; 86 | pthread_mutex_t rx_mutex; 87 | } ecx_portt; 88 | 89 | extern const uint16 priMAC[3]; 90 | extern const uint16 secMAC[3]; 91 | 92 | #ifdef EC_VER1 93 | extern ecx_portt ecx_port; 94 | extern ecx_redportt ecx_redport; 95 | 96 | int ec_setupnic(const char * ifname, int secondary); 97 | int ec_closenic(void); 98 | void ec_setbufstat(int idx, int bufstat); 99 | int ec_getindex(void); 100 | int ec_outframe(int idx, int sock); 101 | int ec_outframe_red(int idx); 102 | int ec_waitinframe(int idx, int timeout); 103 | int ec_srconfirm(int idx,int timeout); 104 | #endif 105 | 106 | void ec_setupheader(void *p); 107 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 108 | int ecx_closenic(ecx_portt *port); 109 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 110 | int ecx_getindex(ecx_portt *port); 111 | int ecx_outframe(ecx_portt *port, int idx, int sock); 112 | int ecx_outframe_red(ecx_portt *port, int idx); 113 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 114 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 115 | 116 | #ifdef __cplusplus 117 | } 118 | #endif 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/linux/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "oshw.h" 14 | 15 | /** 16 | * Host to Network byte order (i.e. to big endian). 17 | * 18 | * Note that Ethercat uses little endian byte order, except for the Ethernet 19 | * header which is big endian as usual. 20 | */ 21 | uint16 oshw_htons(uint16 host) 22 | { 23 | uint16 network = htons (host); 24 | return network; 25 | } 26 | 27 | /** 28 | * Network (i.e. big endian) to Host byte order. 29 | * 30 | * Note that Ethercat uses little endian byte order, except for the Ethernet 31 | * header which is big endian as usual. 32 | */ 33 | uint16 oshw_ntohs(uint16 network) 34 | { 35 | uint16 host = ntohs (network); 36 | return host; 37 | } 38 | 39 | /** Create list over available network adapters. 40 | * @return First element in linked list of adapters 41 | */ 42 | ec_adaptert * oshw_find_adapters(void) 43 | { 44 | int i; 45 | struct if_nameindex *ids; 46 | ec_adaptert * adapter; 47 | ec_adaptert * prev_adapter; 48 | ec_adaptert * ret_adapter = NULL; 49 | 50 | 51 | /* Iterate all devices and create a local copy holding the name and 52 | * description. 53 | */ 54 | 55 | ids = if_nameindex (); 56 | for(i = 0; ids[i].if_index != 0; i++) 57 | { 58 | adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); 59 | /* If we got more than one adapter save link list pointer to previous 60 | * adapter. 61 | * Else save as pointer to return. 62 | */ 63 | if (i) 64 | { 65 | prev_adapter->next = adapter; 66 | } 67 | else 68 | { 69 | ret_adapter = adapter; 70 | } 71 | 72 | /* fetch description and name, in Linux we use the same on both */ 73 | adapter->next = NULL; 74 | 75 | if (ids[i].if_name) 76 | { 77 | strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 78 | adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 79 | strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 80 | adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 81 | } 82 | else 83 | { 84 | adapter->name[0] = '\0'; 85 | adapter->desc[0] = '\0'; 86 | } 87 | 88 | prev_adapter = adapter; 89 | } 90 | 91 | if_freenameindex (ids); 92 | 93 | return ret_adapter; 94 | } 95 | 96 | /** Free memory allocated memory used by adapter collection. 97 | * @param[in] adapter = First element in linked list of adapters 98 | * EC_NOFRAME. 99 | */ 100 | void oshw_free_adapters(ec_adaptert * adapter) 101 | { 102 | ec_adaptert * next_adapter; 103 | /* Iterate the linked list and free all elements holding 104 | * adapter information 105 | */ 106 | if(adapter) 107 | { 108 | next_adapter = adapter->next; 109 | free (adapter); 110 | while (next_adapter) 111 | { 112 | adapter = next_adapter; 113 | next_adapter = adapter->next; 114 | free (adapter); 115 | } 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/linux/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "ethercattype.h" 19 | #include "nicdrv.h" 20 | #include "ethercatmain.h" 21 | 22 | uint16 oshw_htons(uint16 hostshort); 23 | uint16 oshw_ntohs(uint16 networkshort); 24 | ec_adaptert * oshw_find_adapters(void); 25 | void oshw_free_adapters(ec_adaptert * adapter); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/macosx/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include 20 | 21 | /** pointer structure to Tx and Rx stacks */ 22 | typedef struct 23 | { 24 | /** socket connection used */ 25 | pcap_t **sock; 26 | /** tx buffer */ 27 | ec_bufT (*txbuf)[EC_MAXBUF]; 28 | /** tx buffer lengths */ 29 | int (*txbuflength)[EC_MAXBUF]; 30 | /** temporary receive buffer */ 31 | ec_bufT *tempbuf; 32 | /** rx buffers */ 33 | ec_bufT (*rxbuf)[EC_MAXBUF]; 34 | /** rx buffer status fields */ 35 | int (*rxbufstat)[EC_MAXBUF]; 36 | /** received MAC source address (middle word) */ 37 | int (*rxsa)[EC_MAXBUF]; 38 | } ec_stackT; 39 | 40 | /** pointer structure to buffers for redundant port */ 41 | typedef struct 42 | { 43 | ec_stackT stack; 44 | pcap_t *sockhandle; 45 | /** rx buffers */ 46 | ec_bufT rxbuf[EC_MAXBUF]; 47 | /** rx buffer status */ 48 | int rxbufstat[EC_MAXBUF]; 49 | /** rx MAC source address */ 50 | int rxsa[EC_MAXBUF]; 51 | /** temporary rx buffer */ 52 | ec_bufT tempinbuf; 53 | } ecx_redportt; 54 | 55 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 56 | typedef struct 57 | { 58 | ec_stackT stack; 59 | pcap_t *sockhandle; 60 | /** rx buffers */ 61 | ec_bufT rxbuf[EC_MAXBUF]; 62 | /** rx buffer status */ 63 | int rxbufstat[EC_MAXBUF]; 64 | /** rx MAC source address */ 65 | int rxsa[EC_MAXBUF]; 66 | /** temporary rx buffer */ 67 | ec_bufT tempinbuf; 68 | /** temporary rx buffer status */ 69 | int tempinbufs; 70 | /** transmit buffers */ 71 | ec_bufT txbuf[EC_MAXBUF]; 72 | /** transmit buffer lenghts */ 73 | int txbuflength[EC_MAXBUF]; 74 | /** temporary tx buffer */ 75 | ec_bufT txbuf2; 76 | /** temporary tx buffer length */ 77 | int txbuflength2; 78 | /** last used frame index */ 79 | int lastidx; 80 | /** current redundancy state */ 81 | int redstate; 82 | /** pointer to redundancy port and buffers */ 83 | ecx_redportt *redport; 84 | pthread_mutex_t getindex_mutex; 85 | pthread_mutex_t tx_mutex; 86 | pthread_mutex_t rx_mutex; 87 | } ecx_portt; 88 | 89 | extern const uint16 priMAC[3]; 90 | extern const uint16 secMAC[3]; 91 | 92 | #ifdef EC_VER1 93 | extern ecx_portt ecx_port; 94 | extern ecx_redportt ecx_redport; 95 | 96 | int ec_setupnic(const char * ifname, int secondary); 97 | int ec_closenic(void); 98 | void ec_setbufstat(int idx, int bufstat); 99 | int ec_getindex(void); 100 | int ec_outframe(int idx, int sock); 101 | int ec_outframe_red(int idx); 102 | int ec_waitinframe(int idx, int timeout); 103 | int ec_srconfirm(int idx,int timeout); 104 | #endif 105 | 106 | void ec_setupheader(void *p); 107 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 108 | int ecx_closenic(ecx_portt *port); 109 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 110 | int ecx_getindex(ecx_portt *port); 111 | int ecx_outframe(ecx_portt *port, int idx, int sock); 112 | int ecx_outframe_red(ecx_portt *port, int idx); 113 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 114 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 115 | 116 | #ifdef __cplusplus 117 | } 118 | #endif 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/macosx/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "oshw.h" 14 | 15 | /** 16 | * Host to Network byte order (i.e. to big endian). 17 | * 18 | * Note that Ethercat uses little endian byte order, except for the Ethernet 19 | * header which is big endian as usual. 20 | */ 21 | uint16 oshw_htons(uint16 host) 22 | { 23 | uint16 network = htons (host); 24 | return network; 25 | } 26 | 27 | /** 28 | * Network (i.e. big endian) to Host byte order. 29 | * 30 | * Note that Ethercat uses little endian byte order, except for the Ethernet 31 | * header which is big endian as usual. 32 | */ 33 | uint16 oshw_ntohs(uint16 network) 34 | { 35 | uint16 host = ntohs (network); 36 | return host; 37 | } 38 | 39 | /** Create list over available network adapters. 40 | * @return First element in linked list of adapters 41 | */ 42 | ec_adaptert * oshw_find_adapters(void) 43 | { 44 | int i; 45 | struct if_nameindex *ids; 46 | ec_adaptert * adapter; 47 | ec_adaptert * prev_adapter; 48 | ec_adaptert * ret_adapter = NULL; 49 | 50 | 51 | /* Iterate all devices and create a local copy holding the name and 52 | * description. 53 | */ 54 | 55 | ids = if_nameindex (); 56 | for(i = 0; ids[i].if_index != 0; i++) 57 | { 58 | adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); 59 | /* If we got more than one adapter save link list pointer to previous 60 | * adapter. 61 | * Else save as pointer to return. 62 | */ 63 | if (i) 64 | { 65 | prev_adapter->next = adapter; 66 | } 67 | else 68 | { 69 | ret_adapter = adapter; 70 | } 71 | 72 | /* fetch description and name, in macosx we use the same on both */ 73 | adapter->next = NULL; 74 | 75 | if (ids[i].if_name) 76 | { 77 | strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 78 | adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 79 | strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 80 | adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 81 | } 82 | else 83 | { 84 | adapter->name[0] = '\0'; 85 | adapter->desc[0] = '\0'; 86 | } 87 | 88 | prev_adapter = adapter; 89 | } 90 | 91 | if_freenameindex (ids); 92 | 93 | return ret_adapter; 94 | } 95 | 96 | /** Free memory allocated memory used by adapter collection. 97 | * @param[in] adapter = First element in linked list of adapters 98 | * EC_NOFRAME. 99 | */ 100 | void oshw_free_adapters(ec_adaptert * adapter) 101 | { 102 | ec_adaptert * next_adapter; 103 | /* Iterate the linked list and free all elements holding 104 | * adapter information 105 | */ 106 | if(adapter) 107 | { 108 | next_adapter = adapter->next; 109 | free (adapter); 110 | while (next_adapter) 111 | { 112 | adapter = next_adapter; 113 | next_adapter = adapter->next; 114 | free (adapter); 115 | } 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/macosx/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "ethercattype.h" 19 | #include "nicdrv.h" 20 | #include "ethercatmain.h" 21 | 22 | uint16 oshw_htons(uint16 hostshort); 23 | uint16 oshw_ntohs(uint16 networkshort); 24 | ec_adaptert * oshw_find_adapters(void); 25 | void oshw_free_adapters(ec_adaptert * adapter); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtems/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include 20 | 21 | /** pointer structure to Tx and Rx stacks */ 22 | typedef struct 23 | { 24 | /** socket connection used */ 25 | int *sock; 26 | /** tx buffer */ 27 | ec_bufT (*txbuf)[EC_MAXBUF]; 28 | /** tx buffer lengths */ 29 | int (*txbuflength)[EC_MAXBUF]; 30 | /** temporary receive buffer */ 31 | ec_bufT *tempbuf; 32 | /** rx buffers */ 33 | ec_bufT (*rxbuf)[EC_MAXBUF]; 34 | /** rx buffer status fields */ 35 | int (*rxbufstat)[EC_MAXBUF]; 36 | /** received MAC source address (middle word) */ 37 | int (*rxsa)[EC_MAXBUF]; 38 | } ec_stackT; 39 | 40 | /** pointer structure to buffers for redundant port */ 41 | typedef struct 42 | { 43 | ec_stackT stack; 44 | int sockhandle; 45 | /** rx buffers */ 46 | ec_bufT rxbuf[EC_MAXBUF]; 47 | /** rx buffer status */ 48 | int rxbufstat[EC_MAXBUF]; 49 | /** rx MAC source address */ 50 | int rxsa[EC_MAXBUF]; 51 | /** temporary rx buffer */ 52 | ec_bufT tempinbuf; 53 | } ecx_redportt; 54 | 55 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 56 | typedef struct 57 | { 58 | ec_stackT stack; 59 | int sockhandle; 60 | /** rx buffers */ 61 | ec_bufT rxbuf[EC_MAXBUF]; 62 | /** rx buffer status */ 63 | int rxbufstat[EC_MAXBUF]; 64 | /** rx MAC source address */ 65 | int rxsa[EC_MAXBUF]; 66 | /** temporary rx buffer */ 67 | ec_bufT tempinbuf; 68 | /** temporary rx buffer status */ 69 | int tempinbufs; 70 | /** transmit buffers */ 71 | ec_bufT txbuf[EC_MAXBUF]; 72 | /** transmit buffer lengths */ 73 | int txbuflength[EC_MAXBUF]; 74 | /** temporary tx buffer */ 75 | ec_bufT txbuf2; 76 | /** temporary tx buffer length */ 77 | int txbuflength2; 78 | /** last used frame index */ 79 | int lastidx; 80 | /** current redundancy state */ 81 | int redstate; 82 | /** pointer to redundancy port and buffers */ 83 | ecx_redportt *redport; 84 | pthread_mutex_t getindex_mutex; 85 | pthread_mutex_t tx_mutex; 86 | pthread_mutex_t rx_mutex; 87 | } ecx_portt; 88 | 89 | extern const uint16 priMAC[3]; 90 | extern const uint16 secMAC[3]; 91 | 92 | #ifdef EC_VER1 93 | extern ecx_portt ecx_port; 94 | extern ecx_redportt ecx_redport; 95 | 96 | int ec_setupnic(const char * ifname, int secondary); 97 | int ec_closenic(void); 98 | void ec_setbufstat(int idx, int bufstat); 99 | int ec_getindex(void); 100 | int ec_outframe(int idx, int sock); 101 | int ec_outframe_red(int idx); 102 | int ec_waitinframe(int idx, int timeout); 103 | int ec_srconfirm(int idx,int timeout); 104 | #endif 105 | 106 | void ec_setupheader(void *p); 107 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 108 | int ecx_closenic(ecx_portt *port); 109 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 110 | int ecx_getindex(ecx_portt *port); 111 | int ecx_outframe(ecx_portt *port, int idx, int sock); 112 | int ecx_outframe_red(ecx_portt *port, int idx); 113 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 114 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 115 | 116 | #ifdef __cplusplus 117 | } 118 | #endif 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtems/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "oshw.h" 14 | 15 | /** 16 | * Host to Network byte order (i.e. to big endian). 17 | * 18 | * Note that Ethercat uses little endian byte order, except for the Ethernet 19 | * header which is big endian as usual. 20 | */ 21 | uint16 oshw_htons(uint16 host) 22 | { 23 | uint16 network = htons (host); 24 | return network; 25 | } 26 | 27 | /** 28 | * Network (i.e. big endian) to Host byte order. 29 | * 30 | * Note that Ethercat uses little endian byte order, except for the Ethernet 31 | * header which is big endian as usual. 32 | */ 33 | uint16 oshw_ntohs(uint16 network) 34 | { 35 | uint16 host = ntohs (network); 36 | return host; 37 | } 38 | 39 | /** Create list over available network adapters. 40 | * @return First element in linked list of adapters 41 | */ 42 | ec_adaptert * oshw_find_adapters(void) 43 | { 44 | int i; 45 | struct if_nameindex *ids; 46 | ec_adaptert * adapter; 47 | ec_adaptert * prev_adapter; 48 | ec_adaptert * ret_adapter = NULL; 49 | 50 | 51 | /* Iterate all devices and create a local copy holding the name and 52 | * description. 53 | */ 54 | 55 | ids = if_nameindex (); 56 | for(i = 0; ids[i].if_index != 0; i++) 57 | { 58 | adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); 59 | /* If we got more than one adapter save link list pointer to previous 60 | * adapter. 61 | * Else save as pointer to return. 62 | */ 63 | if (i) 64 | { 65 | prev_adapter->next = adapter; 66 | } 67 | else 68 | { 69 | ret_adapter = adapter; 70 | } 71 | 72 | /* fetch description and name, in rtems we use the same on both */ 73 | adapter->next = NULL; 74 | 75 | if (ids[i].if_name) 76 | { 77 | strncpy(adapter->name, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 78 | adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 79 | strncpy(adapter->desc, ids[i].if_name, EC_MAXLEN_ADAPTERNAME); 80 | adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 81 | } 82 | else 83 | { 84 | adapter->name[0] = '\0'; 85 | adapter->desc[0] = '\0'; 86 | } 87 | 88 | prev_adapter = adapter; 89 | } 90 | 91 | if_freenameindex (ids); 92 | 93 | return ret_adapter; 94 | } 95 | 96 | /** Free memory allocated memory used by adapter collection. 97 | * @param[in] adapter = First element in linked list of adapters 98 | * EC_NOFRAME. 99 | */ 100 | void oshw_free_adapters(ec_adaptert * adapter) 101 | { 102 | ec_adaptert * next_adapter; 103 | /* Iterate the linked list and free all elements holding 104 | * adapter information 105 | */ 106 | if(adapter) 107 | { 108 | next_adapter = adapter->next; 109 | free (adapter); 110 | while (next_adapter) 111 | { 112 | adapter = next_adapter; 113 | next_adapter = adapter->next; 114 | free (adapter); 115 | } 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtems/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #include "ethercattype.h" 19 | #include "nicdrv.h" 20 | #include "ethercatmain.h" 21 | 22 | uint16 oshw_htons(uint16 hostshort); 23 | uint16 oshw_ntohs(uint16 networkshort); 24 | ec_adaptert * oshw_find_adapters(void); 25 | void oshw_free_adapters(ec_adaptert * adapter); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtk/fec/fec_ecat.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * * *** *** 3 | * *** *** *** 4 | * *** **** ********** *** ***** *** **** ***** 5 | * ********* ********** *** ********* ************ ********* 6 | * **** *** *** *** *** **** *** 7 | * *** *** ****** *** *********** *** **** ***** 8 | * *** *** ****** *** ************* *** **** ***** 9 | * *** **** **** *** *** *** **** *** 10 | * *** ******* ***** ************** ************* ********* 11 | * *** ***** *** ******* ** ** ****** ***** 12 | * t h e r e a l t i m e t a r g e t e x p e r t s 13 | * 14 | * http://www.rt-labs.com 15 | * Copyright (C) 2007. rt-labs AB, Sweden. All rights reserved. 16 | *------------------------------------------------------------------------------ 17 | * $Id: fec_ecat.h 91 2014-04-02 13:32:29Z rtlfrm $ 18 | *------------------------------------------------------------------------------ 19 | */ 20 | 21 | /** 22 | * \defgroup fec EtherCat Ethernet MAC driver for Frescale K60 SoCs. 23 | * 24 | * \{ 25 | */ 26 | 27 | #ifndef FEC_H 28 | #define FEC_H 29 | 30 | #include 31 | 32 | #ifdef __cplusplus 33 | extern "C" 34 | { 35 | #endif 36 | 37 | typedef struct fec_mac_address 38 | { 39 | uint8_t octet[6]; 40 | } fec_mac_address_t; 41 | 42 | int fec_ecat_init (const fec_mac_address_t * mac_address, bool phy_loopback_mode); 43 | 44 | int fec_ecat_send (const void *payload, size_t tot_len); 45 | 46 | int fec_ecat_recv (void * buffer, size_t buffer_length); 47 | 48 | #ifdef __cplusplus 49 | } 50 | #endif 51 | 52 | #endif /* FEC_H */ 53 | 54 | /** 55 | * \} 56 | */ 57 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtk/lw_mac/lw_emac.h: -------------------------------------------------------------------------------- 1 | /* 2 | * author: Tomas Vestelind 3 | */ 4 | 5 | #ifndef LWIP_MAC_H 6 | #define LWIP_MAC_H 7 | 8 | int bfin_EMAC_init(uint8_t *enetaddr); 9 | int bfin_EMAC_send(void *packet, int length); 10 | int bfin_EMAC_recv(uint8_t * packet, size_t size); 11 | 12 | #endif /* LWIP_MAC_H */ 13 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtk/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | /** pointer structure to Tx and Rx stacks */ 15 | typedef struct 16 | { 17 | /** socket connection used */ 18 | int *sock; 19 | /** tx buffer */ 20 | ec_bufT (*txbuf)[EC_MAXBUF]; 21 | /** tx buffer lengths */ 22 | int (*txbuflength)[EC_MAXBUF]; 23 | /** temporary receive buffer */ 24 | ec_bufT *tempbuf; 25 | /** rx buffers */ 26 | ec_bufT (*rxbuf)[EC_MAXBUF]; 27 | /** rx buffer status fields */ 28 | int (*rxbufstat)[EC_MAXBUF]; 29 | /** received MAC source address (middle word) */ 30 | int (*rxsa)[EC_MAXBUF]; 31 | } ec_stackT; 32 | 33 | /** pointer structure to buffers for redundant port */ 34 | typedef struct 35 | { 36 | ec_stackT stack; 37 | int sockhandle; 38 | /** rx buffers */ 39 | ec_bufT rxbuf[EC_MAXBUF]; 40 | /** rx buffer status */ 41 | int rxbufstat[EC_MAXBUF]; 42 | /** rx MAC source address */ 43 | int rxsa[EC_MAXBUF]; 44 | /** temporary rx buffer */ 45 | ec_bufT tempinbuf; 46 | } ecx_redportt; 47 | 48 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 49 | typedef struct 50 | { 51 | ec_stackT stack; 52 | int sockhandle; 53 | /** rx buffers */ 54 | ec_bufT rxbuf[EC_MAXBUF]; 55 | /** rx buffer status */ 56 | int rxbufstat[EC_MAXBUF]; 57 | /** rx MAC source address */ 58 | int rxsa[EC_MAXBUF]; 59 | /** temporary rx buffer */ 60 | ec_bufT tempinbuf; 61 | /** temporary rx buffer status */ 62 | int tempinbufs; 63 | /** transmit buffers */ 64 | ec_bufT txbuf[EC_MAXBUF]; 65 | /** transmit buffer lengths */ 66 | int txbuflength[EC_MAXBUF]; 67 | /** temporary tx buffer */ 68 | ec_bufT txbuf2; 69 | /** temporary tx buffer length */ 70 | int txbuflength2; 71 | /** last used frame index */ 72 | int lastidx; 73 | /** current redundancy state */ 74 | int redstate; 75 | /** pointer to redundancy port and buffers */ 76 | ecx_redportt *redport; 77 | mtx_t * getindex_mutex; 78 | mtx_t * tx_mutex; 79 | mtx_t * rx_mutex; 80 | } ecx_portt; 81 | 82 | extern const uint16 priMAC[3]; 83 | extern const uint16 secMAC[3]; 84 | 85 | #ifdef EC_VER1 86 | extern ecx_portt ecx_port; 87 | extern ecx_redportt ecx_redport; 88 | 89 | int ec_setupnic(const char * ifname, int secondary); 90 | int ec_closenic(void); 91 | void ec_setbufstat(int idx, int bufstat); 92 | int ec_getindex(void); 93 | int ec_outframe(int idx, int stacknumber); 94 | int ec_outframe_red(int idx); 95 | int ec_waitinframe(int idx, int timeout); 96 | int ec_srconfirm(int idx,int timeout); 97 | #endif 98 | 99 | void ec_setupheader(void *p); 100 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 101 | int ecx_closenic(ecx_portt *port); 102 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 103 | int ecx_getindex(ecx_portt *port); 104 | int ecx_outframe(ecx_portt *port, int idx, int stacknumber); 105 | int ecx_outframe_red(ecx_portt *port, int idx); 106 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 107 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtk/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include "oshw.h" 7 | #include 8 | #include 9 | 10 | /** 11 | * Host to Network byte order (i.e. to big endian). 12 | * 13 | * Note that Ethercat uses little endian byte order, except for the Ethernet 14 | * header which is big endian as usual. 15 | */ 16 | uint16 oshw_htons(const uint16 host) 17 | { 18 | uint16 network = htons (host); 19 | return network; 20 | } 21 | 22 | /** 23 | * Network (i.e. big endian) to Host byte order. 24 | * 25 | * Note that Ethercat uses little endian byte order, except for the Ethernet 26 | * header which is big endian as usual. 27 | */ 28 | uint16 oshw_ntohs(const uint16 network) 29 | { 30 | uint16 host = ntohs (network); 31 | return host; 32 | } 33 | 34 | /* Create list over available network adapters. 35 | * @return First element in linked list of adapters 36 | */ 37 | ec_adaptert * oshw_find_adapters(void) 38 | { 39 | ec_adaptert * ret_adapter = NULL; 40 | 41 | /* TODO if needed */ 42 | 43 | return ret_adapter; 44 | } 45 | 46 | /** Free memory allocated memory used by adapter collection. 47 | * @param[in] adapter = First element in linked list of adapters 48 | * EC_NOFRAME. 49 | */ 50 | void oshw_free_adapters(ec_adaptert * adapter) 51 | { 52 | /* TODO if needed */ 53 | } 54 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/rtk/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for oshw.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include 20 | #include "ethercattype.h" 21 | #include "nicdrv.h" 22 | #include "ethercatmain.h" 23 | 24 | 25 | uint16 oshw_htons(uint16 host); 26 | uint16 oshw_ntohs(uint16 network); 27 | 28 | ec_adaptert * oshw_find_adapters(void); 29 | void oshw_free_adapters(ec_adaptert * adapter); 30 | 31 | #ifdef __cplusplus 32 | } 33 | #endif 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/vxworks/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include 20 | 21 | /** structure to connect EtherCAT stack and VxWorks device */ 22 | typedef struct ETHERCAT_PKT_DEV 23 | { 24 | struct ecx_port *port; 25 | void *pCookie; 26 | void *endObj; 27 | UINT32 redundant; 28 | UINT32 tx_count; 29 | UINT32 rx_count; 30 | UINT32 overrun_count; 31 | UINT32 abandoned_count; 32 | }ETHERCAT_PKT_DEV; 33 | 34 | /** pointer structure to Tx and Rx stacks */ 35 | typedef struct 36 | { 37 | /** tx buffer */ 38 | ec_bufT (*txbuf)[EC_MAXBUF]; 39 | /** tx buffer lengths */ 40 | int (*txbuflength)[EC_MAXBUF]; 41 | /** rx buffers */ 42 | ec_bufT (*rxbuf)[EC_MAXBUF]; 43 | /** rx buffer status fields */ 44 | int (*rxbufstat)[EC_MAXBUF]; 45 | /** received MAC source address (middle word) */ 46 | int (*rxsa)[EC_MAXBUF]; 47 | } ec_stackT; 48 | 49 | /** pointer structure to buffers for redundant port */ 50 | typedef struct ecx_redport 51 | { 52 | /** Stack reference */ 53 | ec_stackT stack; 54 | /** Packet device instance */ 55 | ETHERCAT_PKT_DEV pktDev; 56 | /** rx buffers */ 57 | ec_bufT rxbuf[EC_MAXBUF]; 58 | /** rx buffer status */ 59 | int rxbufstat[EC_MAXBUF]; 60 | /** rx MAC source address */ 61 | int rxsa[EC_MAXBUF]; 62 | /** MSG Q for receive callbacks to post into */ 63 | MSG_Q_ID msgQId[EC_MAXBUF]; 64 | } ecx_redportt; 65 | 66 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 67 | typedef struct ecx_port 68 | { 69 | /** Stack reference */ 70 | ec_stackT stack; 71 | /** Packet device instance */ 72 | ETHERCAT_PKT_DEV pktDev; 73 | /** rx buffers */ 74 | ec_bufT rxbuf[EC_MAXBUF]; 75 | /** rx buffer status */ 76 | int rxbufstat[EC_MAXBUF]; 77 | /** rx MAC source address */ 78 | int rxsa[EC_MAXBUF]; 79 | /** transmit buffers */ 80 | ec_bufT txbuf[EC_MAXBUF]; 81 | /** transmit buffer lengths */ 82 | int txbuflength[EC_MAXBUF]; 83 | /** temporary tx buffer */ 84 | ec_bufT txbuf2; 85 | /** temporary tx buffer length */ 86 | int txbuflength2; 87 | /** last used frame index */ 88 | int lastidx; 89 | /** current redundancy state */ 90 | int redstate; 91 | /** pointer to redundancy port and buffers */ 92 | ecx_redportt *redport; 93 | /** Semaphore to protect single resources */ 94 | SEM_ID sem_get_index; 95 | /** MSG Q for receive callbacks to post into */ 96 | MSG_Q_ID msgQId[EC_MAXBUF]; 97 | } ecx_portt; 98 | 99 | extern const uint16 priMAC[3]; 100 | extern const uint16 secMAC[3]; 101 | 102 | #ifdef EC_VER1 103 | extern ecx_portt ecx_port; 104 | extern ecx_redportt ecx_redport; 105 | 106 | int ec_setupnic(const char * ifname, int secondary); 107 | int ec_closenic(void); 108 | void ec_setbufstat(int idx, int bufstat); 109 | int ec_getindex(void); 110 | int ec_outframe(int idx, int sock); 111 | int ec_outframe_red(int idx); 112 | int ec_waitinframe(int idx, int timeout); 113 | int ec_srconfirm(int idx,int timeout); 114 | #endif 115 | 116 | void ec_setupheader(void *p); 117 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 118 | int ecx_closenic(ecx_portt *port); 119 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 120 | int ecx_getindex(ecx_portt *port); 121 | int ecx_outframe(ecx_portt *port, int idx, int sock); 122 | int ecx_outframe_red(ecx_portt *port, int idx); 123 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 124 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 125 | 126 | #ifdef __cplusplus 127 | } 128 | #endif 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/vxworks/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "oshw.h" 15 | 16 | /** 17 | * Host to Network byte order (i.e. to big endian). 18 | * 19 | * Note that Ethercat uses little endian byte order, except for the Ethernet 20 | * header which is big endian as usual. 21 | */ 22 | uint16 oshw_htons(uint16 host) 23 | { 24 | uint16 network = htons (host); 25 | return network; 26 | } 27 | 28 | /** 29 | * Network (i.e. big endian) to Host byte order. 30 | * 31 | * Note that Ethercat uses little endian byte order, except for the Ethernet 32 | * header which is big endian as usual. 33 | */ 34 | uint16 oshw_ntohs(uint16 network) 35 | { 36 | uint16 host = ntohs (network); 37 | return host; 38 | } 39 | 40 | /** Create list over available network adapters. 41 | * @return First element in linked list of adapters 42 | */ 43 | ec_adaptert * oshw_find_adapters(void) 44 | { 45 | ec_adaptert * ret_adapter = NULL; 46 | /* Not implemented */ 47 | assert(0); 48 | 49 | return ret_adapter; 50 | } 51 | 52 | /** Free memory allocated memory used by adapter collection. 53 | * @param[in] adapter = First element in linked list of adapters 54 | * EC_NOFRAME. 55 | */ 56 | void oshw_free_adapters(ec_adaptert * adapter) 57 | { 58 | /* Not implemented */ 59 | assert(0); 60 | } 61 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/vxworks/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for oshw.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #include "ethercattype.h" 15 | #include "nicdrv.h" 16 | #include "ethercatmain.h" 17 | 18 | uint16 oshw_htons(uint16 hostshort); 19 | uint16 oshw_ntohs(uint16 networkshort); 20 | ec_adaptert * oshw_find_adapters(void); 21 | void oshw_free_adapters(ec_adaptert * adapter); 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for nicdrv.c 9 | */ 10 | 11 | #ifndef _nicdrvh_ 12 | #define _nicdrvh_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #define HAVE_REMOTE 20 | 21 | #include 22 | #include 23 | 24 | /** pointer structure to Tx and Rx stacks */ 25 | typedef struct 26 | { 27 | /** socket connection used */ 28 | pcap_t **sock; 29 | /** tx buffer */ 30 | ec_bufT (*txbuf)[EC_MAXBUF]; 31 | /** tx buffer lengths */ 32 | int (*txbuflength)[EC_MAXBUF]; 33 | /** temporary receive buffer */ 34 | ec_bufT *tempbuf; 35 | /** rx buffers */ 36 | ec_bufT (*rxbuf)[EC_MAXBUF]; 37 | /** rx buffer status fields */ 38 | int (*rxbufstat)[EC_MAXBUF]; 39 | /** received MAC source address (middle word) */ 40 | int (*rxsa)[EC_MAXBUF]; 41 | } ec_stackT; 42 | 43 | /** pointer structure to buffers for redundant port */ 44 | typedef struct 45 | { 46 | ec_stackT stack; 47 | pcap_t *sockhandle; 48 | /** rx buffers */ 49 | ec_bufT rxbuf[EC_MAXBUF]; 50 | /** rx buffer status */ 51 | int rxbufstat[EC_MAXBUF]; 52 | /** rx MAC source address */ 53 | int rxsa[EC_MAXBUF]; 54 | /** temporary rx buffer */ 55 | ec_bufT tempinbuf; 56 | } ecx_redportt; 57 | 58 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 59 | typedef struct 60 | { 61 | ec_stackT stack; 62 | pcap_t *sockhandle; 63 | /** rx buffers */ 64 | ec_bufT rxbuf[EC_MAXBUF]; 65 | /** rx buffer status */ 66 | int rxbufstat[EC_MAXBUF]; 67 | /** rx MAC source address */ 68 | int rxsa[EC_MAXBUF]; 69 | /** temporary rx buffer */ 70 | ec_bufT tempinbuf; 71 | /** temporary rx buffer status */ 72 | int tempinbufs; 73 | /** transmit buffers */ 74 | ec_bufT txbuf[EC_MAXBUF]; 75 | /** transmit buffer lengths */ 76 | int txbuflength[EC_MAXBUF]; 77 | /** temporary tx buffer */ 78 | ec_bufT txbuf2; 79 | /** temporary tx buffer length */ 80 | int txbuflength2; 81 | /** last used frame index */ 82 | int lastidx; 83 | /** current redundancy state */ 84 | int redstate; 85 | /** pointer to redundancy port and buffers */ 86 | ecx_redportt *redport; 87 | CRITICAL_SECTION getindex_mutex; 88 | CRITICAL_SECTION tx_mutex; 89 | CRITICAL_SECTION rx_mutex; 90 | } ecx_portt; 91 | 92 | extern const uint16 priMAC[3]; 93 | extern const uint16 secMAC[3]; 94 | 95 | #ifdef EC_VER1 96 | extern ecx_portt ecx_port; 97 | extern ecx_redportt ecx_redport; 98 | 99 | int ec_setupnic(const char * ifname, int secondary); 100 | int ec_closenic(void); 101 | void ec_setbufstat(int idx, int bufstat); 102 | int ec_getindex(void); 103 | int ec_outframe(int idx, int sock); 104 | int ec_outframe_red(int idx); 105 | int ec_waitinframe(int idx, int timeout); 106 | int ec_srconfirm(int idx,int timeout); 107 | #endif 108 | 109 | void ec_setupheader(void *p); 110 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 111 | int ecx_closenic(ecx_portt *port); 112 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 113 | int ecx_getindex(ecx_portt *port); 114 | int ecx_outframe(ecx_portt *port, int idx, int sock); 115 | int ecx_outframe_red(ecx_portt *port, int idx); 116 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 117 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 118 | 119 | #ifdef __cplusplus 120 | } 121 | #endif 122 | 123 | #endif 124 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/oshw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | #include "oshw.h" 7 | 8 | /** 9 | * Host to Network byte order (i.e. to big endian). 10 | * 11 | * Note that Ethercat uses little endian byte order, except for the Ethernet 12 | * header which is big endian as usual. 13 | */ 14 | uint16 oshw_htons (uint16 host) 15 | { 16 | uint16 network = htons (host); 17 | return network; 18 | } 19 | 20 | /** 21 | * Network (i.e. big endian) to Host byte order. 22 | * 23 | * Note that Ethercat uses little endian byte order, except for the Ethernet 24 | * header which is big endian as usual. 25 | */ 26 | uint16 oshw_ntohs (uint16 network) 27 | { 28 | uint16 host = ntohs (network); 29 | return host; 30 | } 31 | 32 | /* Create list over available network adapters. 33 | * @return First element in linked list of adapters 34 | */ 35 | ec_adaptert * oshw_find_adapters (void) 36 | { 37 | int i = 0; 38 | int ret = 0; 39 | pcap_if_t *alldevs; 40 | pcap_if_t *d; 41 | ec_adaptert * adapter; 42 | ec_adaptert * prev_adapter; 43 | ec_adaptert * ret_adapter = NULL; 44 | char errbuf[PCAP_ERRBUF_SIZE]; 45 | 46 | /* find all devices */ 47 | if (pcap_findalldevs(&alldevs, errbuf) == -1) 48 | { 49 | fprintf(stderr,"Error in pcap_findalldevs_ex: %s\n", errbuf); 50 | return (NULL); 51 | } 52 | /* Iterate all devices and create a local copy holding the name and 53 | * description. 54 | */ 55 | for(d= alldevs; d != NULL; d= d->next) 56 | { 57 | adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); 58 | /* If we got more than one adapter save link list pointer to previous 59 | * adapter. 60 | * Else save as pointer to return. 61 | */ 62 | if (i) 63 | { 64 | prev_adapter->next = adapter; 65 | } 66 | else 67 | { 68 | ret_adapter = adapter; 69 | } 70 | 71 | /* fetch description and name of the device from libpcap */ 72 | adapter->next = NULL; 73 | if (d->name) 74 | { 75 | strncpy(adapter->name, d->name, EC_MAXLEN_ADAPTERNAME); 76 | adapter->name[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 77 | } 78 | else 79 | { 80 | adapter->name[0] = '\0'; 81 | } 82 | if (d->description) 83 | { 84 | strncpy(adapter->desc, d->description, EC_MAXLEN_ADAPTERNAME); 85 | adapter->desc[EC_MAXLEN_ADAPTERNAME-1] = '\0'; 86 | } 87 | else 88 | { 89 | adapter->desc[0] = '\0'; 90 | } 91 | prev_adapter = adapter; 92 | i++; 93 | } 94 | /* free all devices allocated */ 95 | pcap_freealldevs(alldevs); 96 | 97 | return ret_adapter; 98 | } 99 | 100 | /** Free memory allocated memory used by adapter collection. 101 | * @param[in] adapter = First element in linked list of adapters 102 | * EC_NOFRAME. 103 | */ 104 | void oshw_free_adapters (ec_adaptert * adapter) 105 | { 106 | ec_adaptert * next_adapter; 107 | /* Iterate the linked list and free all elements holding 108 | * adapter information 109 | */ 110 | if(adapter) 111 | { 112 | next_adapter = adapter->next; 113 | free (adapter); 114 | while (next_adapter) 115 | { 116 | adapter = next_adapter; 117 | next_adapter = adapter->next; 118 | free (adapter); 119 | } 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/oshw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _oshw_ 12 | #define _oshw_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #include "ethercattype.h" 20 | #include "nicdrv.h" 21 | #include "ethercatmain.h" 22 | 23 | uint16 oshw_htons (uint16 hostshort); 24 | uint16 oshw_ntohs (uint16 networkshort); 25 | ec_adaptert * oshw_find_adapters (void); 26 | void oshw_free_adapters (ec_adaptert * adapter); 27 | 28 | #ifdef __cplusplus 29 | } 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/Win32-Extensions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 1999 - 2005 NetGroup, Politecnico di Torino (Italy) 3 | * Copyright (c) 2005 - 2006 CACE Technologies, Davis (California) 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * 3. Neither the name of the Politecnico di Torino, CACE Technologies 16 | * nor the names of its contributors may be used to endorse or promote 17 | * products derived from this software without specific prior written 18 | * permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 23 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 24 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 25 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 26 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 27 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 28 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef __WIN32_EXTENSIONS_H__ 35 | #define __WIN32_EXTENSIONS_H__ 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | /* Definitions */ 42 | 43 | /*! 44 | \brief A queue of raw packets that will be sent to the network with pcap_sendqueue_transmit(). 45 | */ 46 | struct pcap_send_queue 47 | { 48 | u_int maxlen; ///< Maximum size of the the queue, in bytes. This variable contains the size of the buffer field. 49 | u_int len; ///< Current size of the queue, in bytes. 50 | char *buffer; ///< Buffer containing the packets to be sent. 51 | }; 52 | 53 | typedef struct pcap_send_queue pcap_send_queue; 54 | 55 | /*! 56 | \brief This typedef is a support for the pcap_get_airpcap_handle() function 57 | */ 58 | #if !defined(AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_) 59 | #define AIRPCAP_HANDLE__EAE405F5_0171_9592_B3C2_C19EC426AD34__DEFINED_ 60 | typedef struct _AirpcapHandle *PAirpcapHandle; 61 | #endif 62 | 63 | #define BPF_MEM_EX_IMM 0xc0 64 | #define BPF_MEM_EX_IND 0xe0 65 | 66 | /*used for ST*/ 67 | #define BPF_MEM_EX 0xc0 68 | #define BPF_TME 0x08 69 | 70 | #define BPF_LOOKUP 0x90 71 | #define BPF_EXECUTE 0xa0 72 | #define BPF_INIT 0xb0 73 | #define BPF_VALIDATE 0xc0 74 | #define BPF_SET_ACTIVE 0xd0 75 | #define BPF_RESET 0xe0 76 | #define BPF_SET_MEMORY 0x80 77 | #define BPF_GET_REGISTER_VALUE 0x70 78 | #define BPF_SET_REGISTER_VALUE 0x60 79 | #define BPF_SET_WORKING 0x50 80 | #define BPF_SET_ACTIVE_READ 0x40 81 | #define BPF_SET_AUTODELETION 0x30 82 | #define BPF_SEPARATION 0xff 83 | 84 | /* Prototypes */ 85 | pcap_send_queue* pcap_sendqueue_alloc(u_int memsize); 86 | 87 | void pcap_sendqueue_destroy(pcap_send_queue* queue); 88 | 89 | int pcap_sendqueue_queue(pcap_send_queue* queue, const struct pcap_pkthdr *pkt_header, const u_char *pkt_data); 90 | 91 | u_int pcap_sendqueue_transmit(pcap_t *p, pcap_send_queue* queue, int sync); 92 | 93 | HANDLE pcap_getevent(pcap_t *p); 94 | 95 | struct pcap_stat *pcap_stats_ex(pcap_t *p, int *pcap_stat_size); 96 | 97 | int pcap_setuserbuffer(pcap_t *p, int size); 98 | 99 | int pcap_live_dump(pcap_t *p, char *filename, int maxsize, int maxpacks); 100 | 101 | int pcap_live_dump_ended(pcap_t *p, int sync); 102 | 103 | int pcap_offline_filter(struct bpf_program *prog, const struct pcap_pkthdr *header, const u_char *pkt_data); 104 | 105 | int pcap_start_oem(char* err_str, int flags); 106 | 107 | PAirpcapHandle pcap_get_airpcap_handle(pcap_t *p); 108 | 109 | #ifdef __cplusplus 110 | } 111 | #endif 112 | 113 | #endif //__WIN32_EXTENSIONS_H__ 114 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/bittypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 1999 WIDE Project. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the project nor the names of its contributors 14 | * may be used to endorse or promote products derived from this software 15 | * without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND 18 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE 21 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 23 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 24 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 26 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 27 | * SUCH DAMAGE. 28 | */ 29 | #ifndef _BITTYPES_H 30 | #define _BITTYPES_H 31 | 32 | #ifndef HAVE_U_INT8_T 33 | 34 | #if SIZEOF_CHAR == 1 35 | typedef unsigned char u_int8_t; 36 | typedef signed char int8_t; 37 | #elif SIZEOF_INT == 1 38 | typedef unsigned int u_int8_t; 39 | typedef signed int int8_t; 40 | #else /* XXX */ 41 | #error "there's no appropriate type for u_int8_t" 42 | #endif 43 | #define HAVE_U_INT8_T 1 44 | #define HAVE_INT8_T 1 45 | 46 | #endif /* HAVE_U_INT8_T */ 47 | 48 | #ifndef HAVE_U_INT16_T 49 | 50 | #if SIZEOF_SHORT == 2 51 | typedef unsigned short u_int16_t; 52 | typedef signed short int16_t; 53 | #elif SIZEOF_INT == 2 54 | typedef unsigned int u_int16_t; 55 | typedef signed int int16_t; 56 | #elif SIZEOF_CHAR == 2 57 | typedef unsigned char u_int16_t; 58 | typedef signed char int16_t; 59 | #else /* XXX */ 60 | #error "there's no appropriate type for u_int16_t" 61 | #endif 62 | #define HAVE_U_INT16_T 1 63 | #define HAVE_INT16_T 1 64 | 65 | #endif /* HAVE_U_INT16_T */ 66 | 67 | #ifndef HAVE_U_INT32_T 68 | 69 | #if SIZEOF_INT == 4 70 | typedef unsigned int u_int32_t; 71 | typedef signed int int32_t; 72 | #elif SIZEOF_LONG == 4 73 | typedef unsigned long u_int32_t; 74 | typedef signed long int32_t; 75 | #elif SIZEOF_SHORT == 4 76 | typedef unsigned short u_int32_t; 77 | typedef signed short int32_t; 78 | #else /* XXX */ 79 | #error "there's no appropriate type for u_int32_t" 80 | #endif 81 | #define HAVE_U_INT32_T 1 82 | #define HAVE_INT32_T 1 83 | 84 | #endif /* HAVE_U_INT32_T */ 85 | 86 | #ifndef HAVE_U_INT64_T 87 | #if SIZEOF_LONG_LONG == 8 88 | typedef unsigned long long u_int64_t; 89 | typedef long long int64_t; 90 | #elif defined(_MSC_EXTENSIONS) 91 | typedef unsigned _int64 u_int64_t; 92 | typedef _int64 int64_t; 93 | #elif SIZEOF_INT == 8 94 | typedef unsigned int u_int64_t; 95 | #elif SIZEOF_LONG == 8 96 | typedef unsigned long u_int64_t; 97 | #elif SIZEOF_SHORT == 8 98 | typedef unsigned short u_int64_t; 99 | #else /* XXX */ 100 | #error "there's no appropriate type for u_int64_t" 101 | #endif 102 | 103 | #endif /* HAVE_U_INT64_T */ 104 | 105 | #ifndef PRId64 106 | #ifdef _MSC_EXTENSIONS 107 | #define PRId64 "I64d" 108 | #else /* _MSC_EXTENSIONS */ 109 | #define PRId64 "lld" 110 | #endif /* _MSC_EXTENSIONS */ 111 | #endif /* PRId64 */ 112 | 113 | #ifndef PRIo64 114 | #ifdef _MSC_EXTENSIONS 115 | #define PRIo64 "I64o" 116 | #else /* _MSC_EXTENSIONS */ 117 | #define PRIo64 "llo" 118 | #endif /* _MSC_EXTENSIONS */ 119 | #endif /* PRIo64 */ 120 | 121 | #ifndef PRIx64 122 | #ifdef _MSC_EXTENSIONS 123 | #define PRIx64 "I64x" 124 | #else /* _MSC_EXTENSIONS */ 125 | #define PRIx64 "llx" 126 | #endif /* _MSC_EXTENSIONS */ 127 | #endif /* PRIx64 */ 128 | 129 | #ifndef PRIu64 130 | #ifdef _MSC_EXTENSIONS 131 | #define PRIu64 "I64u" 132 | #else /* _MSC_EXTENSIONS */ 133 | #define PRIu64 "llu" 134 | #endif /* _MSC_EXTENSIONS */ 135 | #endif /* PRIu64 */ 136 | 137 | #endif /* _BITTYPES_H */ 138 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap-bpf.h: -------------------------------------------------------------------------------- 1 | /*- 2 | * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 3 | * The Regents of the University of California. All rights reserved. 4 | * 5 | * This code is derived from the Stanford/CMU enet packet filter, 6 | * (net/enet.c) distributed as part of 4.3BSD, and code contributed 7 | * to Berkeley by Steven McCanne and Van Jacobson both of Lawrence 8 | * Berkeley Laboratory. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 1. Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * 2. Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in the 17 | * documentation and/or other materials provided with the distribution. 18 | * 3. All advertising materials mentioning features or use of this software 19 | * must display the following acknowledgement: 20 | * This product includes software developed by the University of 21 | * California, Berkeley and its contributors. 22 | * 4. Neither the name of the University nor the names of its contributors 23 | * may be used to endorse or promote products derived from this software 24 | * without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 27 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 29 | * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 32 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 33 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 35 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 36 | * SUCH DAMAGE. 37 | * 38 | * @(#) $Header: /tcpdump/master/libpcap/pcap-bpf.h,v 1.50 2007/04/01 21:43:55 guy Exp $ (LBL) 39 | */ 40 | 41 | /* 42 | * For backwards compatibility. 43 | * 44 | * Note to OS vendors: do NOT get rid of this file! Some applications 45 | * might expect to be able to include . 46 | */ 47 | #include 48 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap-namedb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 1994, 1996 3 | * The Regents of the University of California. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. All advertising materials mentioning features or use of this software 14 | * must display the following acknowledgement: 15 | * This product includes software developed by the Computer Systems 16 | * Engineering Group at Lawrence Berkeley Laboratory. 17 | * 4. Neither the name of the University nor of the Laboratory may be used 18 | * to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 27 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 30 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 | * SUCH DAMAGE. 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap-namedb.h,v 1.13 2006/10/04 18:13:32 guy Exp $ (LBL) 34 | */ 35 | 36 | /* 37 | * For backwards compatibility. 38 | * 39 | * Note to OS vendors: do NOT get rid of this file! Some applications 40 | * might expect to be able to include . 41 | */ 42 | #include 43 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap-stdinc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2002 - 2005 NetGroup, Politecnico di Torino (Italy) 3 | * Copyright (c) 2005 - 2009 CACE Technologies, Inc. Davis (California) 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * 3. Neither the name of the Politecnico di Torino nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * @(#) $Header: /tcpdump/master/libpcap/pcap-stdinc.h,v 1.10.2.1 2008-10-06 15:38:39 gianluca Exp $ (LBL) 32 | */ 33 | 34 | #define SIZEOF_CHAR 1 35 | #define SIZEOF_SHORT 2 36 | #define SIZEOF_INT 4 37 | #ifndef _MSC_EXTENSIONS 38 | #define SIZEOF_LONG_LONG 8 39 | #endif 40 | 41 | /* 42 | * Avoids a compiler warning in case this was already defined 43 | * (someone defined _WINSOCKAPI_ when including 'windows.h', in order 44 | * to prevent it from including 'winsock.h') 45 | */ 46 | #ifdef _WINSOCKAPI_ 47 | #undef _WINSOCKAPI_ 48 | #endif 49 | #include 50 | 51 | #include 52 | 53 | #include "bittypes.h" 54 | #include 55 | #include 56 | 57 | #ifndef __MINGW32__ 58 | #include "IP6_misc.h" 59 | #endif 60 | 61 | #define caddr_t char* 62 | 63 | #if _MSC_VER < 1500 64 | #define snprintf _snprintf 65 | #define vsnprintf _vsnprintf 66 | #define strdup _strdup 67 | #endif 68 | 69 | #define inline __inline 70 | 71 | #ifdef __MINGW32__ 72 | #include 73 | #else /*__MINGW32__*/ 74 | /* MSVC compiler */ 75 | #ifndef _UINTPTR_T_DEFINED 76 | #ifdef _WIN64 77 | typedef unsigned __int64 uintptr_t; 78 | #else 79 | typedef _W64 unsigned int uintptr_t; 80 | #endif 81 | #define _UINTPTR_T_DEFINED 82 | #endif 83 | 84 | #ifndef _INTPTR_T_DEFINED 85 | #ifdef _WIN64 86 | typedef __int64 intptr_t; 87 | #else 88 | typedef _W64 int intptr_t; 89 | #endif 90 | #define _INTPTR_T_DEFINED 91 | #endif 92 | 93 | #endif /*__MINGW32__*/ 94 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 1993, 1994, 1995, 1996, 1997 3 | * The Regents of the University of California. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. All advertising materials mentioning features or use of this software 14 | * must display the following acknowledgement: 15 | * This product includes software developed by the Computer Systems 16 | * Engineering Group at Lawrence Berkeley Laboratory. 17 | * 4. Neither the name of the University nor of the Laboratory may be used 18 | * to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 27 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 30 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 | * SUCH DAMAGE. 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap.h,v 1.59 2006/10/04 18:09:22 guy Exp $ (LBL) 34 | */ 35 | 36 | /* 37 | * For backwards compatibility. 38 | * 39 | * Note to OS vendors: do NOT get rid of this file! Many applications 40 | * expect to be able to include , and at least some of them 41 | * go through contortions in their configure scripts to try to detect 42 | * OSes that have "helpfully" moved pcap.h to without 43 | * leaving behind a file. 44 | */ 45 | #include 46 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap/bluetooth.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2006 Paolo Abeni (Italy) 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. The name of the author may not be used to endorse or promote 15 | * products derived from this software without specific prior written 16 | * permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * bluetooth data struct 31 | * By Paolo Abeni 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap/bluetooth.h,v 1.1 2007/09/22 02:10:17 guy Exp $ 34 | */ 35 | 36 | #ifndef _PCAP_BLUETOOTH_STRUCTS_H__ 37 | #define _PCAP_BLUETOOTH_STRUCTS_H__ 38 | 39 | /* 40 | * Header prepended libpcap to each bluetooth h:4 frame. 41 | * fields are in network byte order 42 | */ 43 | typedef struct _pcap_bluetooth_h4_header { 44 | u_int32_t direction; /* if first bit is set direction is incoming */ 45 | } pcap_bluetooth_h4_header; 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap/namedb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 1994, 1996 3 | * The Regents of the University of California. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. All advertising materials mentioning features or use of this software 14 | * must display the following acknowledgement: 15 | * This product includes software developed by the Computer Systems 16 | * Engineering Group at Lawrence Berkeley Laboratory. 17 | * 4. Neither the name of the University nor of the Laboratory may be used 18 | * to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 27 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 30 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 | * SUCH DAMAGE. 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap/namedb.h,v 1.1 2006/10/04 18:09:22 guy Exp $ (LBL) 34 | */ 35 | 36 | #ifndef lib_pcap_namedb_h 37 | #define lib_pcap_namedb_h 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | /* 44 | * As returned by the pcap_next_etherent() 45 | * XXX this stuff doesn't belong in this interface, but this 46 | * library already must do name to address translation, so 47 | * on systems that don't have support for /etc/ethers, we 48 | * export these hooks since they'll 49 | */ 50 | struct pcap_etherent { 51 | u_char addr[6]; 52 | char name[122]; 53 | }; 54 | #ifndef PCAP_ETHERS_FILE 55 | #define PCAP_ETHERS_FILE "/etc/ethers" 56 | #endif 57 | struct pcap_etherent *pcap_next_etherent(FILE *); 58 | u_char *pcap_ether_hostton(const char*); 59 | u_char *pcap_ether_aton(const char *); 60 | 61 | bpf_u_int32 **pcap_nametoaddr(const char *); 62 | #ifdef INET6 63 | struct addrinfo *pcap_nametoaddrinfo(const char *); 64 | #endif 65 | bpf_u_int32 pcap_nametonetaddr(const char *); 66 | 67 | int pcap_nametoport(const char *, int *, int *); 68 | int pcap_nametoportrange(const char *, int *, int *, int *); 69 | int pcap_nametoproto(const char *); 70 | int pcap_nametoeproto(const char *); 71 | int pcap_nametollc(const char *); 72 | /* 73 | * If a protocol is unknown, PROTO_UNDEF is returned. 74 | * Also, pcap_nametoport() returns the protocol along with the port number. 75 | * If there are ambiguous entried in /etc/services (i.e. domain 76 | * can be either tcp or udp) PROTO_UNDEF is returned. 77 | */ 78 | #define PROTO_UNDEF -1 79 | 80 | /* XXX move these to pcap-int.h? */ 81 | int __pcap_atodn(const char *, bpf_u_int32 *); 82 | int __pcap_atoin(const char *, bpf_u_int32 *); 83 | u_short __pcap_nametodnaddr(const char *); 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap/usb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2006 Paolo Abeni (Italy) 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. The name of the author may not be used to endorse or promote 15 | * products derived from this software without specific prior written 16 | * permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * Basic USB data struct 31 | * By Paolo Abeni 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap/usb.h,v 1.6 2007/09/22 02:06:08 guy Exp $ 34 | */ 35 | 36 | #ifndef _PCAP_USB_STRUCTS_H__ 37 | #define _PCAP_USB_STRUCTS_H__ 38 | 39 | /* 40 | * possible transfer mode 41 | */ 42 | #define URB_TRANSFER_IN 0x80 43 | #define URB_ISOCHRONOUS 0x0 44 | #define URB_INTERRUPT 0x1 45 | #define URB_CONTROL 0x2 46 | #define URB_BULK 0x3 47 | 48 | /* 49 | * possible event type 50 | */ 51 | #define URB_SUBMIT 'S' 52 | #define URB_COMPLETE 'C' 53 | #define URB_ERROR 'E' 54 | 55 | /* 56 | * USB setup header as defined in USB specification. 57 | * Appears at the front of each packet in DLT_USB captures. 58 | */ 59 | typedef struct _usb_setup { 60 | u_int8_t bmRequestType; 61 | u_int8_t bRequest; 62 | u_int16_t wValue; 63 | u_int16_t wIndex; 64 | u_int16_t wLength; 65 | } pcap_usb_setup; 66 | 67 | 68 | /* 69 | * Header prepended by linux kernel to each event. 70 | * Appears at the front of each packet in DLT_USB_LINUX captures. 71 | */ 72 | typedef struct _usb_header { 73 | u_int64_t id; 74 | u_int8_t event_type; 75 | u_int8_t transfer_type; 76 | u_int8_t endpoint_number; 77 | u_int8_t device_address; 78 | u_int16_t bus_id; 79 | char setup_flag;/*if !=0 the urb setup header is not present*/ 80 | char data_flag; /*if !=0 no urb data is present*/ 81 | int64_t ts_sec; 82 | int32_t ts_usec; 83 | int32_t status; 84 | u_int32_t urb_len; 85 | u_int32_t data_len; /* amount of urb data really present in this event*/ 86 | pcap_usb_setup setup; 87 | } pcap_usb_header; 88 | 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Include/pcap/vlan.h: -------------------------------------------------------------------------------- 1 | /*- 2 | * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997 3 | * The Regents of the University of California. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. All advertising materials mentioning features or use of this software 14 | * must display the following acknowledgement: 15 | * This product includes software developed by the University of 16 | * California, Berkeley and its contributors. 17 | * 4. Neither the name of the University nor the names of its contributors 18 | * may be used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 27 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 30 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 | * SUCH DAMAGE. 32 | * 33 | * @(#) $Header: /tcpdump/master/libpcap/pcap/vlan.h,v 1.1.2.2 2008-08-06 07:45:59 guy Exp $ 34 | */ 35 | 36 | #ifndef lib_pcap_vlan_h 37 | #define lib_pcap_vlan_h 38 | 39 | struct vlan_tag { 40 | u_int16_t vlan_tpid; /* ETH_P_8021Q */ 41 | u_int16_t vlan_tci; /* VLAN TCI */ 42 | }; 43 | 44 | #define VLAN_TAG_LEN 4 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/Packet.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/Packet.lib -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/libpacket.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/libpacket.a -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/libwpcap.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/libwpcap.a -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/wpcap.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/wpcap.lib -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/x64/Packet.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/x64/Packet.lib -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/x64/wpcap.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/soem_interface/6e8ab4d62bc9204dcd25454e19abfa9318bfed6f/soem_rsl/soem_rsl/oshw/win32/wpcap/Lib/x64/wpcap.lib -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercat.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for all ethercat headers 9 | */ 10 | 11 | #ifndef _EC_ETHERCAT_H 12 | #define _EC_ETHERCAT_H 13 | 14 | #include "ethercattype.h" 15 | #include "nicdrv.h" 16 | #include "ethercatbase.h" 17 | #include "ethercatmain.h" 18 | #include "ethercatdc.h" 19 | #include "ethercatcoe.h" 20 | #include "ethercatfoe.h" 21 | #include "ethercatsoe.h" 22 | #include "ethercateoe.h" 23 | #include "ethercatconfig.h" 24 | #include "ethercatprint.h" 25 | 26 | #endif /* _EC_ETHERCAT_H */ 27 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatbase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatbase.c 9 | */ 10 | 11 | #ifndef _ethercatbase_ 12 | #define _ethercatbase_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); 20 | int ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); 21 | int ecx_BWR(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 22 | int ecx_BRD(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 23 | int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 24 | int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 25 | int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 26 | uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); 27 | int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 28 | uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); 29 | int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); 30 | int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 31 | int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); 32 | int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 33 | int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 34 | int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 35 | int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 36 | int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); 37 | 38 | #ifdef EC_VER1 39 | int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); 40 | int ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); 41 | int ec_BWR(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 42 | int ec_BRD(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 43 | int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 44 | int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 45 | int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 46 | uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout); 47 | int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 48 | uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout); 49 | int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); 50 | int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 51 | int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); 52 | int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 53 | int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout); 54 | int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout); 55 | int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout); 56 | int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); 57 | #endif 58 | 59 | #ifdef __cplusplus 60 | } 61 | #endif 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatcoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatcoe.c 9 | */ 10 | 11 | #ifndef _ethercatcoe_ 12 | #define _ethercatcoe_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | /** max entries in Object Description list */ 20 | #define EC_MAXODLIST 1024 21 | 22 | /** max entries in Object Entry list */ 23 | #define EC_MAXOELIST 256 24 | 25 | /* Storage for object description list */ 26 | typedef struct 27 | { 28 | /** slave number */ 29 | uint16 Slave; 30 | /** number of entries in list */ 31 | uint16 Entries; 32 | /** array of indexes */ 33 | uint16 Index[EC_MAXODLIST]; 34 | /** array of datatypes, see EtherCAT specification */ 35 | uint16 DataType[EC_MAXODLIST]; 36 | /** array of object codes, see EtherCAT specification */ 37 | uint8 ObjectCode[EC_MAXODLIST]; 38 | /** number of subindexes for each index */ 39 | uint8 MaxSub[EC_MAXODLIST]; 40 | /** textual description of each index */ 41 | char Name[EC_MAXODLIST][EC_MAXNAME+1]; 42 | } ec_ODlistt; 43 | 44 | /* storage for object list entry information */ 45 | typedef struct 46 | { 47 | /** number of entries in list */ 48 | uint16 Entries; 49 | /** array of value infos, see EtherCAT specification */ 50 | uint8 ValueInfo[EC_MAXOELIST]; 51 | /** array of value infos, see EtherCAT specification */ 52 | uint16 DataType[EC_MAXOELIST]; 53 | /** array of bit lengths, see EtherCAT specification */ 54 | uint16 BitLength[EC_MAXOELIST]; 55 | /** array of object access bits, see EtherCAT specification */ 56 | uint16 ObjAccess[EC_MAXOELIST]; 57 | /** textual description of each index */ 58 | char Name[EC_MAXOELIST][EC_MAXNAME+1]; 59 | } ec_OElistt; 60 | 61 | #ifdef EC_VER1 62 | void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); 63 | int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, 64 | boolean CA, int *psize, void *p, int timeout); 65 | int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, 66 | boolean CA, int psize, void *p, int Timeout); 67 | int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber , int psize, void *p); 68 | int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); 69 | int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize); 70 | int ec_readPDOmapCA(uint16 Slave, int Thread_n, int *Osize, int *Isize); 71 | int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist); 72 | int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist); 73 | int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); 74 | int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); 75 | #endif 76 | 77 | void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); 78 | int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, 79 | boolean CA, int *psize, void *p, int timeout); 80 | int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, 81 | boolean CA, int psize, void *p, int Timeout); 82 | int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber , int psize, void *p); 83 | int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); 84 | int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize); 85 | int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, int *Osize, int *Isize); 86 | int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist); 87 | int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist); 88 | int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); 89 | int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); 90 | 91 | #ifdef __cplusplus 92 | } 93 | #endif 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatconfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatconfig.c 9 | */ 10 | 11 | #ifndef _ethercatconfig_ 12 | #define _ethercatconfig_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #define EC_NODEOFFSET 0x1000 20 | #define EC_TEMPNODE 0xffff 21 | 22 | #ifdef EC_VER1 23 | int ec_config_init(uint8 usetable); 24 | int ec_config_map(void *pIOmap); 25 | int ec_config_overlap_map(void *pIOmap); 26 | int ec_config_map_group(void *pIOmap, uint8 group); 27 | int ec_config_overlap_map_group(void *pIOmap, uint8 group); 28 | int ec_config(uint8 usetable, void *pIOmap); 29 | int ec_config_overlap(uint8 usetable, void *pIOmap); 30 | int ec_recover_slave(uint16 slave, int timeout); 31 | int ec_reconfig_slave(uint16 slave, int timeout); 32 | #endif 33 | 34 | int ecx_detect_slaves(ecx_contextt* context); 35 | int ecx_config_init(ecx_contextt* context, uint8 usetable); 36 | int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group); 37 | int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group); 38 | int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout); 39 | int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout); 40 | 41 | #ifdef __cplusplus 42 | } 43 | #endif 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatdc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatdc.c 9 | */ 10 | 11 | #ifndef _EC_ECATDC_H 12 | #define _EC_ECATDC_H 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #ifdef EC_VER1 20 | boolean ec_configdc(); 21 | void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift); 22 | void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift); 23 | #endif 24 | 25 | boolean ecx_configdc(ecx_contextt *context); 26 | void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift); 27 | void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift); 28 | 29 | #ifdef __cplusplus 30 | } 31 | #endif 32 | 33 | #endif /* _EC_ECATDC_H */ 34 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatfoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatfoe.c 9 | */ 10 | 11 | #ifndef _ethercatfoe_ 12 | #define _ethercatfoe_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | #ifdef EC_VER1 20 | int ec_FOEdefinehook(void *hook); 21 | int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); 22 | int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); 23 | #endif 24 | 25 | int ecx_FOEdefinehook(ecx_contextt *context, void *hook); 26 | int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); 27 | int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); 28 | 29 | #ifdef __cplusplus 30 | } 31 | #endif 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatprint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatprint.c 9 | */ 10 | 11 | #ifndef _ethercatprint_ 12 | #define _ethercatprint_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" 16 | { 17 | #endif 18 | 19 | char* ec_sdoerror2string( uint32 sdoerrorcode); 20 | char* ec_ALstatuscode2string( uint16 ALstatuscode); 21 | char* ec_soeerror2string( uint16 errorcode); 22 | char* ec_mbxerror2string( uint16 errorcode); 23 | char* ecx_err2string(const ec_errort Ec); 24 | char* ecx_elist2string(ecx_contextt *context); 25 | 26 | #ifdef EC_VER1 27 | char* ec_elist2string(void); 28 | #endif 29 | 30 | #ifdef __cplusplus 31 | } 32 | #endif 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/soem_rsl/ethercatsoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Licensed under the GNU General Public License version 2 with exceptions. See 3 | * LICENSE file in the project root for full license information 4 | */ 5 | 6 | /** \file 7 | * \brief 8 | * Headerfile for ethercatsoe.c 9 | */ 10 | 11 | #ifndef _ethercatsoe_ 12 | #define _ethercatsoe_ 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | 18 | #define EC_SOE_DATASTATE_B 0x01 19 | #define EC_SOE_NAME_B 0x02 20 | #define EC_SOE_ATTRIBUTE_B 0x04 21 | #define EC_SOE_UNIT_B 0x08 22 | #define EC_SOE_MIN_B 0x10 23 | #define EC_SOE_MAX_B 0x20 24 | #define EC_SOE_VALUE_B 0x40 25 | #define EC_SOE_DEFAULT_B 0x80 26 | 27 | #define EC_SOE_MAXNAME 60 28 | #define EC_SOE_MAXMAPPING 64 29 | 30 | #define EC_IDN_MDTCONFIG 24 31 | #define EC_IDN_ATCONFIG 16 32 | 33 | /** SoE name structure */ 34 | PACKED_BEGIN 35 | typedef struct PACKED { 36 | /** current length in bytes of list */ 37 | uint16 currentlength; 38 | /** maximum length in bytes of list */ 39 | uint16 maxlength; 40 | char name[EC_SOE_MAXNAME]; 41 | } ec_SoEnamet; 42 | PACKED_END 43 | 44 | /** SoE list structure */ 45 | PACKED_BEGIN 46 | typedef struct PACKED { 47 | /** current length in bytes of list */ 48 | uint16 currentlength; 49 | /** maximum length in bytes of list */ 50 | uint16 maxlength; 51 | union { 52 | uint8 byte[8]; 53 | uint16 word[4]; 54 | uint32 dword[2]; 55 | uint64 lword[1]; 56 | }; 57 | } ec_SoElistt; 58 | PACKED_END 59 | 60 | /** SoE IDN mapping structure */ 61 | PACKED_BEGIN 62 | typedef struct PACKED { 63 | /** current length in bytes of list */ 64 | uint16 currentlength; 65 | /** maximum length in bytes of list */ 66 | uint16 maxlength; 67 | uint16 idn[EC_SOE_MAXMAPPING]; 68 | } ec_soem_rslappingt; 69 | PACKED_END 70 | 71 | #define EC_SOE_LENGTH_1 0x00 72 | #define EC_SOE_LENGTH_2 0x01 73 | #define EC_SOE_LENGTH_4 0x02 74 | #define EC_SOE_LENGTH_8 0x03 75 | #define EC_SOE_TYPE_BINARY 0x00 76 | #define EC_SOE_TYPE_UINT 0x01 77 | #define EC_SOE_TYPE_INT 0x02 78 | #define EC_SOE_TYPE_HEX 0x03 79 | #define EC_SOE_TYPE_STRING 0x04 80 | #define EC_SOE_TYPE_IDN 0x05 81 | #define EC_SOE_TYPE_FLOAT 0x06 82 | #define EC_SOE_TYPE_PARAMETER 0x07 83 | 84 | /** SoE attribute structure */ 85 | PACKED_BEGIN 86 | typedef struct PACKED { 87 | /** evaluation factor for display purposes */ 88 | uint32 evafactor : 16; 89 | /** length of IDN element(s) */ 90 | uint32 length : 2; 91 | /** IDN is list */ 92 | uint32 list : 1; 93 | /** IDN is command */ 94 | uint32 command : 1; 95 | /** datatype */ 96 | uint32 datatype : 3; 97 | uint32 reserved1 : 1; 98 | /** decimals to display if float datatype */ 99 | uint32 decimals : 4; 100 | /** write protected in pre-op */ 101 | uint32 wppreop : 1; 102 | /** write protected in safe-op */ 103 | uint32 wpsafeop : 1; 104 | /** write protected in op */ 105 | uint32 wpop : 1; 106 | uint32 reserved2 : 1; 107 | } ec_SoEattributet; 108 | PACKED_END 109 | 110 | #ifdef EC_VER1 111 | int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int* psize, void* p, int timeout); 112 | int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void* p, int timeout); 113 | int ec_readIDNmap(uint16 slave, int* Osize, int* Isize); 114 | #endif 115 | 116 | int ecx_SoEread(ecx_contextt* context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int* psize, void* p, int timeout); 117 | int ecx_SoEwrite(ecx_contextt* context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void* p, int timeout); 118 | int ecx_readIDNmap(ecx_contextt* context, uint16 slave, int* Osize, int* Isize); 119 | 120 | #ifdef __cplusplus 121 | } 122 | #endif 123 | 124 | #endif -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/linux/eepromtool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(SOURCES eepromtool.c) 3 | add_executable(eepromtool ${SOURCES}) 4 | target_link_libraries(eepromtool soem_rsl) 5 | install(TARGETS eepromtool DESTINATION bin) 6 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/linux/firm_update/firm_update.c: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage: firm_update ifname1 slave fname 5 | * ifname is NIC interface, f.e. eth0 6 | * slave = slave number in EtherCAT order 1..n 7 | * fname = binary file to store in slave 8 | * CAUTION! Using the wrong file can result in a bricked slave! 9 | * 10 | * This is a slave firmware update test. 11 | * 12 | * (c)Arthur Ketels 2011 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "ethercat.h" 22 | 23 | #define FWBUFSIZE (8 * 1024 * 1024) 24 | 25 | uint8 ob; 26 | uint16 ow; 27 | uint32 data; 28 | char filename[256]; 29 | char filebuffer[FWBUFSIZE]; // 8MB buffer 30 | int filesize; 31 | int j; 32 | uint16 argslave; 33 | 34 | int input_bin(char *fname, int *length) 35 | { 36 | FILE *fp; 37 | 38 | int cc = 0, c; 39 | 40 | fp = fopen(fname, "rb"); 41 | if(fp == NULL) 42 | return 0; 43 | while (((c = fgetc(fp)) != EOF) && (cc < FWBUFSIZE)) 44 | filebuffer[cc++] = (uint8)c; 45 | *length = cc; 46 | fclose(fp); 47 | return 1; 48 | } 49 | 50 | 51 | void boottest(char *ifname, uint16 slave, char *filename) 52 | { 53 | printf("Starting firmware update example\n"); 54 | 55 | /* initialise soem_rsl, bind socket to ifname */ 56 | if (ec_init(ifname)) 57 | { 58 | printf("ec_init on %s succeeded.\n",ifname); 59 | /* find and auto-config slaves */ 60 | 61 | 62 | if ( ec_config_init(FALSE) > 0 ) 63 | { 64 | printf("%d slaves found and configured.\n",ec_slavecount); 65 | 66 | printf("Request init state for slave %d\n", slave); 67 | ec_slave[slave].state = EC_STATE_INIT; 68 | ec_writestate(slave); 69 | 70 | /* wait for slave to reach INIT state */ 71 | ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4); 72 | printf("Slave %d state to INIT.\n", slave); 73 | 74 | /* read BOOT mailbox data, master -> slave */ 75 | data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP); 76 | ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data); 77 | ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data); 78 | /* store boot write mailbox address */ 79 | ec_slave[slave].mbx_wo = (uint16)LO_WORD(data); 80 | /* store boot write mailbox size */ 81 | ec_slave[slave].mbx_l = (uint16)HI_WORD(data); 82 | 83 | /* read BOOT mailbox data, slave -> master */ 84 | data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP); 85 | ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data); 86 | ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data); 87 | /* store boot read mailbox address */ 88 | ec_slave[slave].mbx_ro = (uint16)LO_WORD(data); 89 | /* store boot read mailbox size */ 90 | ec_slave[slave].mbx_rl = (uint16)HI_WORD(data); 91 | 92 | printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength, 93 | (int)ec_slave[slave].SM[0].SMflags); 94 | printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength, 95 | (int)ec_slave[slave].SM[1].SMflags); 96 | /* program SM0 mailbox in for slave */ 97 | ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET); 98 | /* program SM1 mailbox out for slave */ 99 | ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET); 100 | 101 | printf("Request BOOT state for slave %d\n", slave); 102 | ec_slave[slave].state = EC_STATE_BOOT; 103 | ec_writestate(slave); 104 | 105 | /* wait for slave to reach BOOT state */ 106 | if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT) 107 | { 108 | printf("Slave %d state to BOOT.\n", slave); 109 | 110 | if (input_bin(filename, &filesize)) 111 | { 112 | printf("File read OK, %d bytes.\n",filesize); 113 | printf("FoE write...."); 114 | j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE); 115 | printf("result %d.\n",j); 116 | printf("Request init state for slave %d\n", slave); 117 | ec_slave[slave].state = EC_STATE_INIT; 118 | ec_writestate(slave); 119 | } 120 | else 121 | printf("File not read OK.\n"); 122 | } 123 | 124 | } 125 | else 126 | { 127 | printf("No slaves found!\n"); 128 | } 129 | printf("End firmware update example, close socket\n"); 130 | /* stop soem_rsl, close socket */ 131 | ec_close(); 132 | } 133 | else 134 | { 135 | printf("No socket connection on %s\nExcecute as root\n",ifname); 136 | } 137 | } 138 | 139 | int main(int argc, char *argv[]) 140 | { 141 | printf("soem_rsl (Simple Open EtherCAT Master)\nFirmware update example\n"); 142 | 143 | if (argc > 3) 144 | { 145 | argslave = atoi(argv[2]); 146 | boottest(argv[1], argslave, argv[3]); 147 | } 148 | else 149 | { 150 | printf("Usage: firm_update ifname1 slave fname\n"); 151 | printf("ifname = eth0 for example\n"); 152 | printf("slave = slave number in EtherCAT order 1..n\n"); 153 | printf("fname = binary file to store in slave\n"); 154 | printf("CAUTION! Using the wrong file can result in a bricked slave!\n"); 155 | } 156 | 157 | printf("End program\n"); 158 | return (0); 159 | } 160 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/linux/simple_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(SOURCES simple_test.c) 3 | add_executable(simple_test ${SOURCES}) 4 | target_link_libraries(simple_test soem_rsl) 5 | install(TARGETS simple_test DESTINATION bin) 6 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/linux/slaveinfo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(SOURCES slaveinfo.c) 3 | add_executable(slaveinfo ${SOURCES}) 4 | target_link_libraries(slaveinfo soem_rsl) 5 | install(TARGETS slaveinfo DESTINATION bin) 6 | -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/rtk/schedule.tt: -------------------------------------------------------------------------------- 1 | stack_size: 2048 2 | 3 | tasks: 4 | - name : tReadIO 5 | entry : read_io 6 | arg : 'NULL' 7 | 8 | schedules: 9 | - name : sched1 10 | period : 1 11 | events : 12 | - task : tReadIO 13 | start : 0 14 | stop : 1 -------------------------------------------------------------------------------- /soem_rsl/soem_rsl/test/win32/firm_update/firm_update.c: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage: firm_update ifname1 slave fname 5 | * ifname is NIC interface, f.e. eth0 6 | * slave = slave number in EtherCAT order 1..n 7 | * fname = binary file to store in slave 8 | * CAUTION! Using the wrong file can result in a bricked slave! 9 | * 10 | * This is a slave firmware update test. 11 | * 12 | * (c)Arthur Ketels 2011 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "ethercat.h" 22 | 23 | #define FWBUFSIZE (8 * 1024 * 1024) 24 | 25 | uint8 ob; 26 | uint16 ow; 27 | uint32 data; 28 | char filename[256]; 29 | char filebuffer[FWBUFSIZE]; // 8MB buffer 30 | int filesize; 31 | int j; 32 | uint16 argslave; 33 | 34 | int input_bin(char *fname, int *length) 35 | { 36 | FILE *fp; 37 | 38 | int cc = 0, c; 39 | 40 | fp = fopen(fname, "rb"); 41 | if(fp == NULL) 42 | return 0; 43 | while (((c = fgetc(fp)) != EOF) && (cc < FWBUFSIZE)) 44 | filebuffer[cc++] = (uint8)c; 45 | *length = cc; 46 | fclose(fp); 47 | return 1; 48 | } 49 | 50 | 51 | void boottest(char *ifname, uint16 slave, char *filename) 52 | { 53 | printf("Starting firmware update example\n"); 54 | 55 | /* initialise soem_rsl, bind socket to ifname */ 56 | if (ec_init(ifname)) 57 | { 58 | printf("ec_init on %s succeeded.\n",ifname); 59 | /* find and auto-config slaves */ 60 | 61 | 62 | if ( ec_config_init(FALSE) > 0 ) 63 | { 64 | printf("%d slaves found and configured.\n",ec_slavecount); 65 | 66 | printf("Request init state for slave %d\n", slave); 67 | ec_slave[slave].state = EC_STATE_INIT; 68 | ec_writestate(slave); 69 | 70 | /* wait for slave to reach INIT state */ 71 | ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4); 72 | printf("Slave %d state to INIT.\n", slave); 73 | 74 | /* read BOOT mailbox data, master -> slave */ 75 | data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP); 76 | ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data); 77 | ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data); 78 | /* store boot write mailbox address */ 79 | ec_slave[slave].mbx_wo = (uint16)LO_WORD(data); 80 | /* store boot write mailbox size */ 81 | ec_slave[slave].mbx_l = (uint16)HI_WORD(data); 82 | 83 | /* read BOOT mailbox data, slave -> master */ 84 | data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP); 85 | ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data); 86 | ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data); 87 | /* store boot read mailbox address */ 88 | ec_slave[slave].mbx_ro = (uint16)LO_WORD(data); 89 | /* store boot read mailbox size */ 90 | ec_slave[slave].mbx_rl = (uint16)HI_WORD(data); 91 | 92 | printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength, 93 | (int)ec_slave[slave].SM[0].SMflags); 94 | printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength, 95 | (int)ec_slave[slave].SM[1].SMflags); 96 | /* program SM0 mailbox in for slave */ 97 | ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET); 98 | /* program SM1 mailbox out for slave */ 99 | ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET); 100 | 101 | printf("Request BOOT state for slave %d\n", slave); 102 | ec_slave[slave].state = EC_STATE_BOOT; 103 | ec_writestate(slave); 104 | 105 | /* wait for slave to reach BOOT state */ 106 | if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT) 107 | { 108 | printf("Slave %d state to BOOT.\n", slave); 109 | 110 | if (input_bin(filename, &filesize)) 111 | { 112 | printf("File read OK, %d bytes.\n",filesize); 113 | printf("FoE write...."); 114 | j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE); 115 | printf("result %d.\n",j); 116 | printf("Request init state for slave %d\n", slave); 117 | ec_slave[slave].state = EC_STATE_INIT; 118 | ec_writestate(slave); 119 | } 120 | else 121 | printf("File not read OK.\n"); 122 | } 123 | 124 | } 125 | else 126 | { 127 | printf("No slaves found!\n"); 128 | } 129 | printf("End firmware update example, close socket\n"); 130 | /* stop soem_rsl, close socket */ 131 | ec_close(); 132 | } 133 | else 134 | { 135 | printf("No socket connection on %s\nExcecute as root\n",ifname); 136 | } 137 | } 138 | 139 | int main(int argc, char *argv[]) 140 | { 141 | printf("soem_rsl (Simple Open EtherCAT Master)\nFirmware update example\n"); 142 | 143 | if (argc > 3) 144 | { 145 | argslave = atoi(argv[2]); 146 | boottest(argv[1], argslave, argv[3]); 147 | } 148 | else 149 | { 150 | printf("Usage: firm_update ifname1 slave fname\n"); 151 | printf("ifname = eth0 for example\n"); 152 | printf("slave = slave number in EtherCAT order 1..n\n"); 153 | printf("fname = binary file to store in slave\n"); 154 | printf("CAUTION! Using the wrong file can result in a bricked slave!\n"); 155 | } 156 | 157 | printf("End program\n"); 158 | return (0); 159 | } 160 | --------------------------------------------------------------------------------