diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..ba39449e --- /dev/null +++ b/.clang-format @@ -0,0 +1,93 @@ +--- +Language: Cpp +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignConsecutiveMacros: false +AlignEscapedNewlines: DontAlign +AlignOperands: false +AlignTrailingComments: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllArgumentsOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: InlineOnly +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: No +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: true + AfterControlStatement: Never + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterStruct: true + AfterUnion: true + AfterExternBlock: false + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeBraces: Custom # Same as 'BraceWrapping' +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: AfterColon +BreakInheritanceList: AfterColon +BreakStringLiterals: true +ColumnLimit: 78 +CompactNamespaces: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 8 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: false +FixNamespaceComments: true +IncludeBlocks: Preserve +IndentAccessModifiers: true +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 4 +InsertBraces: true +KeepEmptyLinesAtTheStartOfBlocks: false +MaxEmptyLinesToKeep: 2 +NamespaceIndentation: None +PackConstructorInitializers: Never +PointerAlignment: Right +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: true +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: true +SpaceBeforeCtorInitializerColon: false +SpaceBeforeInheritanceColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 2 +UseTab: Never +... diff --git a/fake_lib/fakeethercat.cpp b/fake_lib/fakeethercat.cpp index 77970582..9894b98c 100644 --- a/fake_lib/fakeethercat.cpp +++ b/fake_lib/fakeethercat.cpp @@ -25,12 +25,13 @@ #include #include +#include +#include #include #include -#include #include -#include -#include +#include + #include /****************************************************************************/ @@ -49,28 +50,23 @@ static std::ostream &operator<<(std::ostream &os, const ec_address &a) static void add_spaces(std::ostream &out, int const num) { - for (int i = 0; i < num; ++i) - { + for (int i = 0; i < num; ++i) { out << ' '; } } -template -static void map2Json(std::ostream &out, const Map &map, Func &&print_func, - int indent = 0) +template static void +map2Json(std::ostream &out, const Map &map, Func &&print_func, int indent = 0) { indent += 4; out << "{"; bool is_first = true; - for (const auto &kv : map) - { - if (is_first) - { + for (const auto &kv : map) { + if (is_first) { out << '\n'; is_first = false; } - else - { + else { out << ",\n"; } add_spaces(out, indent); @@ -84,11 +80,14 @@ static void map2Json(std::ostream &out, const Map &map, Func &&print_func, out << "}"; } +/***************************************************************************** + * Pdo + ****************************************************************************/ + size_t pdo::sizeInBytes() const { size_t ans = 0; - for (const auto &entry : entries) - { + for (const auto &entry : entries) { ans += entry.bit_length; } return (ans + 7) / 8; @@ -97,10 +96,8 @@ size_t pdo::sizeInBytes() const Offset pdo::findEntry(uint16_t idx, uint8_t subindex) const { size_t offset_bits = 0; - for (const auto &entry : entries) - { - if (entry.index == idx && entry.subindex == subindex) - { + for (const auto &entry : entries) { + if (entry.index == idx && entry.subindex == subindex) { return Offset(offset_bits / 8, offset_bits % 8); } offset_bits += entry.bit_length; @@ -108,68 +105,63 @@ Offset pdo::findEntry(uint16_t idx, uint8_t subindex) const return NotFound; } +Offset pdo::findEntryByPos(unsigned int entry_pos) const +{ + size_t offset_bits = 0; + unsigned int pos {0}; + for (const auto &entry : entries) { + if (pos == entry_pos) { + return Offset(offset_bits / 8, offset_bits % 8); + } + offset_bits += entry.bit_length; + pos++; + } + return NotFound; +} + +/***************************************************************************** + * Common + ****************************************************************************/ + +unsigned int ecrt_version_magic(void) +{ + return ECRT_VERSION_MAGIC; +} + +ec_master_t *ecrt_request_master( + unsigned int master_index /**< Index of the master to request. */ +) +{ + return new ec_master(master_index); +} + +ec_master_t *ecrt_open_master( + unsigned int master_index /**< Index of the master to request. */ +) +{ + return new ec_master(master_index); +} + +void ecrt_release_master(ec_master_t *master) +{ + delete master; +} + /***************************************************************************** * Master ****************************************************************************/ -int ec_master::activate() -{ - for (auto &domain : domains) - { - if (domain.activate()) - return -1; - } - - { - std::ofstream out(rt_ipc_dir + "/" + rt_ipc_name + "_slaves.json"); - if (!out.is_open()) - { - std::cerr << "could not dump json.\n"; - return -1; - } - out << "{\n \"slaves\": "; - map2Json(out, slaves, [](std::ostream &out, - const ec_slave_config &slave) - { slave.dumpJson(out, 8); }, 4); - out << "\n}\n"; - } - return rtipc_prepare(rt_ipc.get()); -} - -int ecrt_master_activate( - ec_master_t *master /**< EtherCAT master. */ -) -{ - try - { - return master->activate(); - } - catch (const std::exception &e) - { - std::cerr << "Could not activate: " << e.what() << '\n'; - return -1; - } -} - -int ecrt_master_application_time( - ec_master_t *master, /**< EtherCAT master. */ - uint64_t app_time /**< Application time. */ +int ecrt_master_reserve(ec_master_t *master /**< EtherCAT master */ ) { return 0; } -ec_domain_t *ecrt_master_create_domain( - ec_master_t *master /**< EtherCAT master. */ -) -{ - return master->createDomain(); -} - static const char *getPrefix() { - if (const auto ans = getenv("FAKE_EC_PREFIX")) + if (const auto ans = getenv("FAKE_EC_PREFIX")) { return ans; + } return "/FakeEtherCAT"; } @@ -179,95 +171,307 @@ ec_domain *ec_master::createDomain() return &domains.back(); } -int ecrt_master_link_state( - const ec_master_t *master, /**< EtherCAT master. */ - unsigned int dev_idx, /**< Index of the device (0 = main device, - 1 = first backup device, ...). */ - ec_master_link_state_t *state /**< Structure to store the information. - */ +ec_domain_t * +ecrt_master_create_domain(ec_master_t *master /**< EtherCAT master. */ ) { - state->slaves_responding = master->getNoSlaves(); - state->al_states = 4; - state->link_up = 1; - return 0; + return master->createDomain(); } -int ecrt_master_receive( - ec_master_t *master /**< EtherCAT master. */ +ec_slave_config_t *ec_master::slave_config( + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ +) +{ + const ec_address address {alias, position}; + const auto it = slaves.find(address); + if (it != slaves.end()) { + if (it->second.vendor_id == vendor_id + && it->second.product_code == product_code) { + return &it->second; + } + else { + std::cerr << "Attempted to reconfigure slave (" << alias << "," + << position << ")!\n"; + return nullptr; + } + } + else { + return &slaves.insert(std::make_pair( + ec_address {address}, + ec_slave_config { + address, + vendor_id, + product_code})) + .first->second; + } +} + +ec_slave_config_t *ecrt_master_slave_config( + ec_master_t *master, /**< EtherCAT master */ + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ +) +{ + return master->slave_config(alias, position, vendor_id, product_code); +} + +int ecrt_master_select_reference_clock( + ec_master_t *master, /**< EtherCAT master. */ + ec_slave_config_t *sc /**< Slave config of the slave to use as the + * reference slave (or NULL). */ ) { return 0; } -int ecrt_master_reset( - ec_master_t *master /**< EtherCAT master. */ +int ecrt_master( + ec_master_t *master, /**< EtherCAT master */ + ec_master_info_t *master_info /**< Structure that will output + the information */ ) { + master_info->slave_count = 0; + master_info->link_up = 1; + master_info->scan_busy = 0; + master_info->app_time = 0; return 0; } int ecrt_master_scan_progress( - ec_master_t *master, /**< EtherCAT master */ - ec_master_scan_progress_t *progress /**< Structure that will output - the progress information. */ + ec_master_t *master, /**< EtherCAT master */ + ec_master_scan_progress_t *progress /**< Structure that will output + the progress information. */ ) { progress->scan_index = progress->slave_count = master->getNoSlaves(); return 0; } -int ecrt_master_send( - ec_master_t *master /**< EtherCAT master. */ +int ecrt_master_get_slave( + ec_master_t *master, /**< EtherCAT master */ + uint16_t slave_position, /**< Slave position. */ + ec_slave_info_t *slave_info /**< Structure that will output the + information */ +) +{ + slave_info->position = slave_position; + slave_info->vendor_id = 0x00000000; + slave_info->product_code = 0x00000000; + slave_info->revision_number = 0x00000000; + slave_info->serial_number = 0x00000000; + slave_info->alias = 0x0000; + slave_info->current_on_ebus = 0; + for (unsigned int i = 0; i < EC_MAX_PORTS; i++) { + slave_info->ports[i].desc = EC_PORT_NOT_IMPLEMENTED; + slave_info->ports[i].link.link_up = 0; + slave_info->ports[i].link.loop_closed = 1; + slave_info->ports[i].link.signal_detected = 0; + slave_info->ports[i].next_slave = 0; + slave_info->ports[i].delay_to_next_dc = 0; + } + slave_info->al_state = EC_AL_STATE_OP; + slave_info->error_flag = 0; + slave_info->sync_count = 8; + slave_info->sdo_count = 0; + slave_info->name[0] = 0; + return 0; +} + +int ecrt_master_get_sync_manager( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_sync_info_t *sync /**< Pointer to output structure. */ +) +{ + sync->index = sync_index; + sync->dir = EC_DIR_INVALID; + sync->n_pdos = 0; + sync->pdos = nullptr; + sync->watchdog_mode = EC_WD_DEFAULT; + return 0; +} + +int ecrt_master_get_pdo( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pos, /**< Zero-based PDO position. */ + ec_pdo_info_t *pdo /**< Pointer to output structure. */ +) +{ + return -ENOENT; +} + +int ecrt_master_get_pdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pdo_pos, /**< Zero-based PDO position. */ + uint16_t entry_pos, /**< Zero-based PDO entry position. */ + ec_pdo_entry_info_t *entry /**< Pointer to output structure. */ +) +{ + return -ENOENT; +} + +int ecrt_master_sdo_download( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ ) { return 0; } -ec_slave_config_t *ecrt_master_slave_config( - ec_master_t *master, /**< EtherCAT master */ - uint16_t alias, /**< Slave alias. */ - uint16_t position, /**< Slave position. */ - uint32_t vendor_id, /**< Expected vendor ID. */ - uint32_t product_code /**< Expected product code. */ +int ecrt_master_sdo_download_complete( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ ) { - return master->slave_config(alias, position, vendor_id, product_code); + return 0; } -ec_slave_config_t *ec_master::slave_config( - uint16_t alias, /**< Slave alias. */ - uint16_t position, /**< Slave position. */ - uint32_t vendor_id, /**< Expected vendor ID. */ - uint32_t product_code /**< Expected product code. */ +int ecrt_master_sdo_upload( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + uint8_t *target, /**< Target buffer for the upload. */ + size_t target_size, /**< Size of the target buffer. */ + size_t *result_size, /**< Uploaded data size. */ + uint32_t *abort_code /**< Abort code of the SDO upload. */ ) { - const ec_address address{alias, position}; - const auto it = slaves.find(address); - if (it != slaves.end()) - { - if (it->second.vendor_id == vendor_id - && it->second.product_code == product_code) - return &it->second; - else - { - std::cerr << "Attempted to reconfigure slave (" << alias - << "," << position << ")!\n"; - return nullptr; + *result_size = 0; + return 0; +} + +int ecrt_master_write_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + const uint8_t *data, /**< Pointer to data to write. */ + size_t data_size, /**< Size of data to write. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ +) +{ + return 0; +} + +int ecrt_master_read_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + uint8_t *target, /**< Pointer to memory where the read data can be + stored. */ + size_t target_size, /**< Size of the memory \a target points to. */ + size_t *result_size, /**< Actual size of the received data. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ +) +{ + *result_size = 0; + return 0; +} + +int ec_master::activate() +{ + for (auto &domain : domains) { + if (domain.activate()) { + return -1; } } - else + { - return &slaves.insert(std::make_pair(ec_address{address}, - ec_slave_config{address, vendor_id, - product_code})).first->second; + std::ofstream out(rt_ipc_dir + "/" + rt_ipc_name + "_slaves.json"); + if (!out.is_open()) { + std::cerr << "could not dump json.\n"; + return -1; + } + out << "{\n \"slaves\": "; + map2Json( + out, + slaves, + [](std::ostream &out, const ec_slave_config &slave) { + slave.dumpJson(out, 8); + }, + 4); + out << "\n}\n"; } + return rtipc_prepare(rt_ipc.get()); +} + +int ecrt_master_activate(ec_master_t *master /**< EtherCAT master. */ +) +{ + try { + return master->activate(); + } + catch (const std::exception &e) { + std::cerr << "Could not activate: " << e.what() << '\n'; + return -1; + } +} + +int ec_master::deactivate() +{ + rt_ipc.reset(rtipc_create(rt_ipc_name.c_str(), rt_ipc_dir.c_str())); + + domains.clear(); + slaves.clear(); + + return 0; +} + +int ecrt_master_deactivate(ec_master_t *master /**< EtherCAT master. */ +) +{ + return master->deactivate(); +} + +int ecrt_master_set_send_interval( + ec_master_t *master, /**< EtherCAT master. */ + size_t send_interval /**< Send interval in us */ +) +{ + return 0; +} + +int ecrt_master_send(ec_master_t *master /**< EtherCAT master. */ +) +{ + return 0; +} + +int ecrt_master_receive(ec_master_t *master /**< EtherCAT master. */ +) +{ + return 0; } int ecrt_master_state( - const ec_master_t *master, /**< EtherCAT master. */ - ec_master_state_t *state /**< Structure to store the information. */ + const ec_master_t *master, /**< EtherCAT master. */ + ec_master_state_t *state /**< Structure to store the information. */ ) { state->slaves_responding = master->getNoSlaves(); @@ -276,86 +480,90 @@ int ecrt_master_state( return 0; } -int ecrt_master_sync_monitor_queue( - ec_master_t *master /**< EtherCAT master. */ +int ecrt_master_link_state( + const ec_master_t *master, /**< EtherCAT master. */ + unsigned int dev_idx, /**< Index of the device (0 = main device, + 1 = first backup device, ...). */ + ec_master_link_state_t *state /**< Structure to store the information. + */ +) +{ + state->slaves_responding = master->getNoSlaves(); + state->al_states = 4; + state->link_up = 1; + return 0; +} + +int ecrt_master_application_time( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t app_time /**< Application time. */ ) { return 0; } -uint32_t ecrt_master_sync_monitor_process( - const ec_master_t *master /**< EtherCAT master. */ -) -{ - return 32; -} - int ecrt_master_sync_reference_clock( - ec_master_t *master /**< EtherCAT master. */ + ec_master_t *master /**< EtherCAT master. */ ) { return 0; } int ecrt_master_sync_reference_clock_to( - ec_master_t *master, /**< EtherCAT master. */ - uint64_t sync_time /**< Sync reference clock to this time. */ + ec_master_t *master, /**< EtherCAT master. */ + uint64_t sync_time /**< Sync reference clock to this time. */ ) { return 0; } -int ecrt_master_sync_slave_clocks( - ec_master_t *master /**< EtherCAT master. */ +int ecrt_master_sync_slave_clocks(ec_master_t *master /**< EtherCAT master. */ ) { return 0; } -void ecrt_release_master(ec_master_t *master) -{ - delete master; -} - -ec_master_t *ecrt_request_master( - unsigned int master_index /**< Index of the master to request. */ +int ecrt_master_reference_clock_time( + const ec_master_t *master, /**< EtherCAT master. */ + uint32_t *time /**< Pointer to store the queried system time. */ ) { - return new ec_master(master_index); + *time = 0; + return 0; } -static const char *getName() +int ecrt_master_sync_monitor_queue( + ec_master_t *master /**< EtherCAT master. */ +) { - static const char *default_name = "FakeEtherCAT"; + return 0; +} - if (const auto ans = getenv("FAKE_EC_NAME")) - { - return ans; - } +uint32_t ecrt_master_sync_monitor_process( + const ec_master_t *master /**< EtherCAT master. */ +) +{ + return 32; +} - std::cerr - << "\nThe environment variable \"FAKE_EC_NAME\" is not set.\n" - << "Using the default value \"" << default_name << "\".\n" - << "Please consider to set unique names when using multiple" - << " instances.\n\n"; - - return default_name; +int ecrt_master_reset(ec_master_t *master /**< EtherCAT master. */ +) +{ + return 0; } static int mkpath(const std::string &file_path) { - if (file_path.empty()) + if (file_path.empty()) { return 0; + } std::size_t offset = 0; - do - { + do { offset = file_path.find('/', offset + 1); const auto subpath = file_path.substr(0, offset); - if (mkdir(subpath.c_str(), 0755) == -1) - { - if (errno != EEXIST) - { + if (mkdir(subpath.c_str(), 0755) == -1) { + if (errno != EEXIST) { return -1; } } @@ -366,8 +574,7 @@ static int mkpath(const std::string &file_path) static std::string getRtIpcDir(int idx) { std::string ans; - if (const auto e = getenv("FAKE_EC_HOMEDIR")) - { + if (const auto e = getenv("FAKE_EC_HOMEDIR")) { ans = e + std::string("/") + std::to_string(idx); } ans = "/tmp/FakeEtherCAT/" + std::to_string(idx); @@ -375,132 +582,66 @@ static std::string getRtIpcDir(int idx) return ans; } -ec_master::ec_master(int id): - rt_ipc_dir(getRtIpcDir(id)), rt_ipc_name(getName()), - rt_ipc(rtipc_create(rt_ipc_name.c_str(), rt_ipc_dir.c_str())), id_(id) +static const char *getName() { + static const char *default_name = "FakeEtherCAT"; + + if (const auto ans = getenv("FAKE_EC_NAME")) { + return ans; + } + + std::cerr << "\nThe environment variable \"FAKE_EC_NAME\" is not set.\n" + << "Using the default value \"" << default_name << "\".\n" + << "Please consider to set unique names when using multiple" + << " instances.\n\n"; + + return default_name; } +ec_master::ec_master(int id): + rt_ipc_dir(getRtIpcDir(id)), + rt_ipc_name(getName()), + rt_ipc(rtipc_create(rt_ipc_name.c_str(), rt_ipc_dir.c_str())), + id_(id) +{} + /***************************************************************************** * Slave Configuration ****************************************************************************/ -int ecrt_slave_config_complete_sdo( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t index, /**< Index of the SDO to configure. */ - const uint8_t *data, /**< Pointer to the data. */ - size_t size /**< Size of the \a data. */ -) -{ - return ecrt_slave_config_sdo(sc, index, 0, data, size); -} - -ec_sdo_request_t *ecrt_slave_config_create_sdo_request( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t index, /**< SDO index. */ - uint8_t subindex, /**< SDO subindex. */ - size_t size /**< Data size to reserve. */ -) -{ - return nullptr; -} - -int ecrt_slave_config_dc( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t assign_activate, /**< AssignActivate word. */ - uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */ - int32_t sync0_shift, /**< SYNC0 shift time [ns]. */ - uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */ - int32_t sync1_shift /**< SYNC1 shift time [ns]. */ -) -{ - return 0; -} - -int ecrt_slave_config_idn( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint8_t drive_no, /**< Drive number. */ - uint16_t idn, /**< SoE IDN. */ - ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or - SAFEOP). */ - const uint8_t *data, /**< Pointer to the data. */ - size_t size /**< Size of the \a data. */ -) -{ - return 0; -} - -int ecrt_slave_config_pdos( - ec_slave_config_t *sc, /**< Slave configuration. */ - unsigned int n_syncs, /**< Number of sync manager configurations in - \a syncs. */ - const ec_sync_info_t syncs[] /**< Array of sync manager - configurations. */ -) -{ - if (!syncs) - return 0; - for (unsigned int sync_idx = 0; sync_idx < n_syncs; ++sync_idx) - { - if (syncs[sync_idx].index == 0xff) - { - return 0; - } - auto &syncManager = sc->sync_managers[syncs[sync_idx].index]; - syncManager.dir = syncs[sync_idx].dir; - for (unsigned int i = 0; i < syncs[sync_idx].n_pdos; ++i) - { - const auto &in_pdo = syncs[sync_idx].pdos[i]; - if (in_pdo.n_entries == 0 || !in_pdo.entries) - { - std::cerr << "Default mapping not supported."; - return -1; - } - auto &out_pdo = syncManager.pdos[in_pdo.index]; - for (unsigned int pdo_entry_idx = 0; - pdo_entry_idx < in_pdo.n_entries; ++pdo_entry_idx) - { - out_pdo.entries.push_back(in_pdo.entries[pdo_entry_idx]); - } - } - } - - return 0; -} - int ecrt_slave_config_sync_manager( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint8_t sync_index, /**< Sync manager index. Must be less - than #EC_MAX_SYNC_MANAGERS. */ - ec_direction_t direction, /**< Input/Output. */ + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_direction_t direction, /**< Input/Output. */ ec_watchdog_mode_t watchdog_mode /** Watchdog mode. */ - ) +) { return 0; } int ecrt_slave_config_watchdog( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t watchdog_divider, /**< Number of 40 ns intervals (register - 0x0400). Used as a base unit for all - slave watchdogs^. If set to zero, the - value is not written, so the default is - used. */ + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t watchdog_divider, /**< Number of 40 ns intervals (register + 0x0400). Used as a base unit for all + slave watchdogs^. If set to zero, the + value is not written, so the default is + used. */ uint16_t watchdog_intervals /**< Number of base intervals for sync manager watchdog (register 0x0420). If set to zero, the value is not written, so the default is used. */ - ) +) { return 0; } int ecrt_slave_config_pdo_assign_add( ec_slave_config_t *sc, /**< Slave configuration. */ - uint8_t sync_index, /**< Sync manager index. Must be less - than #EC_MAX_SYNC_MANAGERS. */ - uint16_t index /**< Index of the PDO to assign. */ - ) + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t index /**< Index of the PDO to assign. */ +) { auto &syncManager = sc->sync_managers[sync_index]; syncManager.pdos[index]; @@ -509,9 +650,9 @@ int ecrt_slave_config_pdo_assign_add( int ecrt_slave_config_pdo_assign_clear( ec_slave_config_t *sc, /**< Slave configuration. */ - uint8_t sync_index /**< Sync manager index. Must be less - than #EC_MAX_SYNC_MANAGERS. */ - ) + uint8_t sync_index /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ +) { auto &syncManager = sc->sync_managers[sync_index]; syncManager.pdos.clear(); @@ -520,20 +661,19 @@ int ecrt_slave_config_pdo_assign_clear( int ecrt_slave_config_pdo_mapping_add( ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t pdo_index, /**< Index of the PDO. */ - uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's - mapping. */ - uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the - PDO's mapping. */ + uint16_t pdo_index, /**< Index of the PDO. */ + uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's + mapping. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the + PDO's mapping. */ uint8_t entry_bit_length /**< Size of the PDO entry in bit. */ - ) +) { for (auto smIt = sc->sync_managers.begin(); - smIt != sc->sync_managers.end(); - ++smIt) { - - auto pdoIt = smIt->second.pdos.find(pdo_index); - if (pdoIt == smIt->second.pdos.end()) { + smIt != sc->sync_managers.end(); + ++smIt) { + auto pdo_it = smIt->second.pdos.find(pdo_index); + if (pdo_it == smIt->second.pdos.end()) { continue; } @@ -541,124 +681,383 @@ int ecrt_slave_config_pdo_mapping_add( entry_info.index = entry_index; entry_info.subindex = entry_subindex; entry_info.bit_length = entry_bit_length; - pdoIt->second.entries.push_back(entry_info); + pdo_it->second.entries.push_back(entry_info); return 0; } std::cerr << __func__ << "(): PDO " << std::hex << pdo_index - << " not found." << std::endl; + << " not found." << std::endl; return -1; } int ecrt_slave_config_pdo_mapping_clear( ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t pdo_index /**< Index of the PDO. */ - ) + uint16_t pdo_index /**< Index of the PDO. */ +) { for (auto smIt = sc->sync_managers.begin(); - smIt != sc->sync_managers.end(); - ++smIt) { - auto pdoIt = smIt->second.pdos.find(pdo_index); - if (pdoIt == smIt->second.pdos.end()) { + smIt != sc->sync_managers.end(); + ++smIt) { + auto pdo_it = smIt->second.pdos.find(pdo_index); + if (pdo_it == smIt->second.pdos.end()) { continue; } - pdoIt->second.entries.clear(); + pdo_it->second.entries.clear(); return 0; } std::cerr << __func__ << "(): PDO " << std::hex << pdo_index - << " not found." << std::endl; + << " not found." << std::endl; return -1; } -int ecrt_slave_config_reg_pdo_entry( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t entry_index, /**< Index of the PDO entry to register. */ - uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */ - ec_domain_t *domain, /**< Domain. */ - unsigned int *bit_position /**< Optional address if bit addressing - is desired */ +int ecrt_slave_config_pdos( + ec_slave_config_t *sc, /**< Slave configuration. */ + unsigned int n_syncs, /**< Number of sync manager configurations in + \a syncs. */ + const ec_sync_info_t syncs[] /**< Array of sync manager + configurations. */ ) { - for (auto sync_it : sc->sync_managers) - { - for (auto pdo_it : sync_it.second.pdos) - { - const auto offset = pdo_it.second.findEntry(entry_index, - entry_subindex); - if (offset != NotFound) - { + if (!syncs) { + return 0; + } + for (unsigned int sync_idx = 0; sync_idx < n_syncs; ++sync_idx) { + if (syncs[sync_idx].index == 0xff) { + return 0; + } + auto &syncManager = sc->sync_managers[syncs[sync_idx].index]; + syncManager.dir = syncs[sync_idx].dir; + for (unsigned int i = 0; i < syncs[sync_idx].n_pdos; ++i) { + const auto &in_pdo = syncs[sync_idx].pdos[i]; + if (in_pdo.n_entries == 0 || !in_pdo.entries) { + std::cerr << "Default mapping not supported."; + return -1; + } + auto &out_pdo = syncManager.pdos[in_pdo.index]; + for (unsigned int pdo_entry_idx = 0; + pdo_entry_idx < in_pdo.n_entries; + ++pdo_entry_idx) { + out_pdo.entries.push_back(in_pdo.entries[pdo_entry_idx]); + } + } + } + + return 0; +} + +int ecrt_slave_config_reg_pdo_entry( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t entry_index, /**< Index of the PDO entry to register. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ +) +{ + for (auto sync_it : sc->sync_managers) { + for (auto pdo_it : sync_it.second.pdos) { + const auto offset = + pdo_it.second.findEntry(entry_index, entry_subindex); + if (offset != NotFound) { const auto domain_offset = - domain->map(*sc, sync_it.first, pdo_it.first); - if (domain_offset != -1) - { - if (bit_position) + domain->map(*sc, sync_it.first, pdo_it.first); + if (domain_offset != -1) { + if (bit_position) { *bit_position = offset.bits; - else if (offset.bits) - { + } + else if (offset.bits) { std::cerr << "Pdo Entry is not byte aligned" - << " but bit offset is ignored!\n"; + << " but bit offset is ignored!\n"; return -1; } return domain_offset + offset.bytes; } - else - { + else { return -1; } } } } - return -1; // offset + return -1; // offset +} + +int ecrt_slave_config_reg_pdo_entry_pos( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. */ + unsigned int pdo_pos, /**< Position of the PDO inside the SM. */ + unsigned int entry_pos, /**< Position of the entry inside the PDO. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ +) +{ + auto syncIt {sc->sync_managers.find(sync_index)}; + if (syncIt == sc->sync_managers.end()) { + return -1; + } + + auto pdo_it {syncIt->second.pdos.find(pdo_pos)}; + if (pdo_it == syncIt->second.pdos.end()) { + return -1; + } + + const auto offset = pdo_it->second.findEntryByPos(entry_pos); + if (offset != NotFound) { + return -1; + } + + const auto domain_offset = domain->map(*sc, sync_index, pdo_it->first); + if (domain_offset == -1) { + return -1; + } + + if (bit_position) { + *bit_position = offset.bits; + } + else if (offset.bits) { + std::cerr << "Pdo Entry is not byte aligned" + << " but bit offset is ignored!\n"; + return -1; + } + return domain_offset + offset.bytes; +} + +int ecrt_slave_config_dc( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t assign_activate, /**< AssignActivate word. */ + uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */ + int32_t sync0_shift, /**< SYNC0 shift time [ns]. */ + uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */ + int32_t sync1_shift /**< SYNC1 shift time [ns]. */ +) +{ + return 0; +} + +int ecrt_slave_config_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + uint8_t subindex, /**< Subindex of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ +) +{ + sc->sdos[sdo_address {index, subindex}] = + std::basic_string(data, data + size); + return 0; } int ecrt_slave_config_sdo8( - ec_slave_config_t *sc, /**< Slave configuration */ - uint16_t sdo_index, /**< Index of the SDO to configure. */ - uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ - uint8_t value /**< Value to set. */ + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint8_t value /**< Value to set. */ ) { return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, &value, 1); } int ecrt_slave_config_sdo16( - ec_slave_config_t *sc, /**< Slave configuration */ - uint16_t sdo_index, /**< Index of the SDO to configure. */ - uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ - uint16_t const value /**< Value to set. */ + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint16_t const value /**< Value to set. */ ) { uint8_t buf[sizeof(value)]; memcpy(&buf, &value, sizeof(value)); - return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, - buf, sizeof(buf)); + return ecrt_slave_config_sdo( + sc, + sdo_index, + sdo_subindex, + buf, + sizeof(buf)); } int ecrt_slave_config_sdo32( - ec_slave_config_t *sc, /**< Slave configuration */ - uint16_t sdo_index, /**< Index of the SDO to configure. */ - uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ - uint32_t const value /**< Value to set. */ + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint32_t const value /**< Value to set. */ ) { uint8_t buf[sizeof(value)]; memcpy(&buf, &value, sizeof(value)); - return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, - buf, sizeof(buf)); + return ecrt_slave_config_sdo( + sc, + sdo_index, + sdo_subindex, + buf, + sizeof(buf)); } -int ecrt_slave_config_sdo( - ec_slave_config_t *sc, /**< Slave configuration. */ - uint16_t index, /**< Index of the SDO to configure. */ - uint8_t subindex, /**< Subindex of the SDO to configure. */ - const uint8_t *data, /**< Pointer to the data. */ - size_t size /**< Size of the \a data. */ +int ecrt_slave_config_complete_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ +) +{ + return ecrt_slave_config_sdo(sc, index, 0, data, size); +} + +int ecrt_slave_config_emerg_size( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t elements /**< Number of records of the CoE emergency ring. */ +) +{ + return 0; +} + +int ecrt_slave_config_emerg_pop( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t *target /**< Pointer to target memory (at least + EC_COE_EMERGENCY_MSG_SIZE bytes). */ +) +{ + return -ENOENT; +} + +int ecrt_slave_config_emerg_clear( + ec_slave_config_t *sc /**< Slave configuration. */ +) +{ + return 0; +} + +int ecrt_slave_config_emerg_overruns( + const ec_slave_config_t *sc /**< Slave configuration. */ +) +{ + return 0; +} + +ec_sdo_request_t *ecrt_slave_config_create_sdo_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex, /**< SDO subindex. */ + size_t size /**< Data size to reserve. */ +) +{ + return nullptr; +} + +ec_soe_request_t *ecrt_slave_config_create_soe_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< Sercos ID-Number. */ + size_t size /**< Data size to reserve. */ +) +{ + return nullptr; +} + +ec_voe_handler_t *ecrt_slave_config_create_voe_handler( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ +) +{ + return nullptr; +} + +ec_reg_request_t *ecrt_slave_config_create_reg_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ +) +{ + return nullptr; +} + +int ecrt_slave_config_state( + const ec_slave_config_t *sc, /**< Slave configuration */ + ec_slave_config_state_t *state /**< State object to write to. */ +) +{ + state->online = 1; + state->operational = 1; + state->al_state = EC_AL_STATE_OP; + return 0; +} + +int ecrt_slave_config_idn( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN. */ + ec_al_state_t state, /**< AL state in which to write + the IDN (PREOP or SAFEOP). */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ +) +{ + return 0; +} + +int ecrt_slave_config_flag( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *key, /**< Key as null-terminated ASCII string. */ + int32_t value /**< Value to store. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_mac_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + const unsigned char *mac_address /**< MAC address. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_ip_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr ip_address /**< IPv4 address. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_subnet_mask( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr subnet_mask /**< IPv4 subnet mask. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_default_gateway( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr gateway_address /**< Gateway's IPv4 address. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_dns_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr dns_address /**< IPv4 address of the DNS server. */ +) +{ + return 0; +} + +int ecrt_slave_config_eoe_hostname( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *name /**< Zero-terminated host name. */ +) +{ + return 0; +} + +int ecrt_slave_config_state_timeout( + ec_slave_config_t *sc, /**< Slave configuration. */ + ec_al_state_t from_state, /**< Initial state. */ + ec_al_state_t to_state, /**< Target state. */ + unsigned int timeout_ms /**< Timeout in [ms]. */ ) { - sc->sdos[sdo_address{index, subindex}] = - std::basic_string(data, data + size); return 0; } @@ -667,26 +1066,34 @@ int ecrt_slave_config_sdo( ****************************************************************************/ int ecrt_domain_reg_pdo_entry_list( - ec_domain_t *domain, /**< Domain. */ - const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO - registrations. */ + ec_domain_t *domain, /**< Domain. */ + const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO + registrations. */ ) { const ec_pdo_entry_reg_t *reg; ec_slave_config_t *sc; int ret; - for (reg = pdo_entry_regs; reg->index; reg++) - { + for (reg = pdo_entry_regs; reg->index; reg++) { if (!(sc = ecrt_master_slave_config( - domain->getMaster(), reg->alias, - reg->position, reg->vendor_id, reg->product_code))) + domain->getMaster(), + reg->alias, + reg->position, + reg->vendor_id, + reg->product_code))) { return -ENOENT; + } if ((ret = ecrt_slave_config_reg_pdo_entry( - sc, reg->index, reg->subindex, domain, - reg->bit_position)) < 0) + sc, + reg->index, + reg->subindex, + domain, + reg->bit_position)) + < 0) { return ret; + } *reg->offset = ret; } @@ -694,28 +1101,31 @@ int ecrt_domain_reg_pdo_entry_list( return 0; } -uint8_t *ecrt_domain_data( - const ec_domain_t *domain) +size_t ecrt_domain_size(const ec_domain_t *domain /**< Domain. */ +) +{ + return domain->getSize(); +} + +uint8_t *ecrt_domain_data(const ec_domain_t *domain) { return domain->getData(); } -int ecrt_domain_process( - ec_domain_t *domain) +int ecrt_domain_process(ec_domain_t *domain) { return domain->process(); } -int ecrt_domain_queue( - ec_domain_t *domain) +int ecrt_domain_queue(ec_domain_t *domain) { return domain->queue(); } int ecrt_domain_state( - const ec_domain_t *domain, /**< Domain. */ - ec_domain_state_t *state /**< Pointer to a state object to store the - information. */ + const ec_domain_t *domain, /**< Domain. */ + ec_domain_state_t *state /**< Pointer to a state object to + store the information. */ ) { state->working_counter = domain->getNumSlaves(); @@ -725,9 +1135,10 @@ int ecrt_domain_state( } ec_domain::ec_domain(rtipc *rtipc, const char *prefix, ec_master_t *master): - rt_group(rtipc_create_group(rtipc, 1.0)), prefix(prefix), master(master) -{ -} + rt_group(rtipc_create_group(rtipc, 1.0)), + prefix(prefix), + master(master) +{} int ec_domain::activate() { @@ -735,38 +1146,47 @@ int ec_domain::activate() connected.resize(mapped_pdos.size()); size_t idx = 0; - for (const auto &pdo : mapped_pdos) - { + for (const auto &pdo : mapped_pdos) { slaves.insert(pdo.slave_address.getCombined()); void *rt_pdo = nullptr; char buf[512]; - const auto fmt = snprintf(buf, sizeof(buf), "%s/%d/%08X/%04X", - prefix, master->getId(), pdo.slave_address.getCombined(), + const auto fmt = snprintf( + buf, + sizeof(buf), + "%s/%d/%08X/%04X", + prefix, + master->getId(), + pdo.slave_address.getCombined(), pdo.pdo_index); - if (fmt < 0 || fmt >= (int)sizeof(buf)) - { + if (fmt < 0 || fmt >= (int) sizeof(buf)) { return -ENOBUFS; } - switch (pdo.dir) - { - case EC_DIR_OUTPUT: - rt_pdo = rtipc_txpdo(rt_group, buf, rtipc_uint8_T, - data.data() + pdo.offset, pdo.size_bytes); - std::cerr << "Registering " << buf << " as Output\n"; - break; - case EC_DIR_INPUT: - rt_pdo = rtipc_rxpdo(rt_group, buf, rtipc_uint8_T, - data.data() + pdo.offset, pdo.size_bytes, - connected.data() + idx); - std::cerr << "Registering " << buf << " as Input\n"; - break; - default: - std::cerr << "Unknown direction " << pdo.dir << '\n'; - return -1; + switch (pdo.dir) { + case EC_DIR_OUTPUT: + rt_pdo = rtipc_txpdo( + rt_group, + buf, + rtipc_uint8_T, + data.data() + pdo.offset, + pdo.size_bytes); + std::cerr << "Registering " << buf << " as Output\n"; + break; + case EC_DIR_INPUT: + rt_pdo = rtipc_rxpdo( + rt_group, + buf, + rtipc_uint8_T, + data.data() + pdo.offset, + pdo.size_bytes, + connected.data() + idx); + std::cerr << "Registering " << buf << " as Input\n"; + break; + default: + std::cerr << "Unknown direction " << pdo.dir << '\n'; + return -1; } - if (!rt_pdo) - { + if (!rt_pdo) { std::cerr << "Failed to register RtIPC PDO\n"; return -1; } @@ -789,32 +1209,249 @@ int ec_domain::queue() return 0; } -ssize_t ec_domain::map(ec_slave_config const &config, - unsigned int syncManager, uint16_t pdo_index) +ssize_t ec_domain::map( + ec_slave_config const &config, + unsigned int syncManager, + uint16_t pdo_index) { - if (activated_) + if (activated_) { return -1; - for (const auto &pdo : mapped_pdos) - { + } + for (const auto &pdo : mapped_pdos) { if (pdo.slave_address == config.address - && syncManager == pdo.syncManager - && pdo_index == pdo.pdo_index) - { + && syncManager == pdo.syncManager && pdo_index == pdo.pdo_index) { // already mapped; return pdo.offset; } } const auto ans = data.size(); - const auto size = - config.sync_managers.at(syncManager).pdos.at(pdo_index).sizeInBytes(); - mapped_pdos.emplace_back(ans, size, config.address, syncManager, - pdo_index, config.sync_managers.at(syncManager).dir); + const auto size = config.sync_managers.at(syncManager) + .pdos.at(pdo_index) + .sizeInBytes(); + mapped_pdos.emplace_back( + ans, + size, + config.address, + syncManager, + pdo_index, + config.sync_managers.at(syncManager).dir); data.resize(ans + size); return ans; } /***************************************************************************** - * Helpers + * SDO requests + ****************************************************************************/ + +int ecrt_sdo_request_index( + ec_sdo_request_t *req, /**< SDO request. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex /**< SDO subindex. */ +) +{ + return 0; +} + +int ecrt_sdo_request_timeout( + ec_sdo_request_t *req, /**< SDO request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero + means no timeout. */ +) +{ + return 0; +} + +uint8_t * +ecrt_sdo_request_data(const ec_sdo_request_t *req /**< SDO request. */ +) +{ + return nullptr; +} + +size_t +ecrt_sdo_request_data_size(const ec_sdo_request_t *req /**< SDO request. */ +) +{ + return 0; +} + +ec_request_state_t +ecrt_sdo_request_state(ec_sdo_request_t *req /**< SDO request. */ +) +{ + return EC_REQUEST_ERROR; +} + +int ecrt_sdo_request_write(ec_sdo_request_t *req /**< SDO request. */ +) +{ + return 0; +} + +int ecrt_sdo_request_read(ec_sdo_request_t *req /**< SDO request. */ +) +{ + return 0; +} + +/***************************************************************************** + * SoE request + ****************************************************************************/ + +int ecrt_soe_request_idn( + ec_soe_request_t *req, /**< IDN request. */ + uint8_t drive_no, /**< SDO index. */ + uint16_t idn /**< SoE IDN. */ +) +{ + return 0; +} + +int ecrt_soe_request_timeout( + ec_soe_request_t *req, /**< SoE request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero + means no timeout. */ +) +{ + return 0; +} + +uint8_t * +ecrt_soe_request_data(const ec_soe_request_t *req /**< SoE request. */ +) +{ + return nullptr; +} + +size_t +ecrt_soe_request_data_size(const ec_soe_request_t *req /**< SoE request. */ +) +{ + return 0; +} + +ec_request_state_t +ecrt_soe_request_state(ec_soe_request_t *req /**< SoE request. */ +) +{ + return EC_REQUEST_ERROR; +} + +int ecrt_soe_request_write(ec_soe_request_t *req /**< SoE request. */ +) +{ + return 0; +} + +int ecrt_soe_request_read(ec_soe_request_t *req /**< SoE request. */ +) +{ + return 0; +} + +/***************************************************************************** + * VoE handler + ****************************************************************************/ + +int ecrt_voe_handler_send_header( + ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t vendor_id, /**< Vendor ID. */ + uint16_t vendor_type /**< Vendor-specific type. */ +) +{ + return 0; +} + +int ecrt_voe_handler_received_header( + const ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t *vendor_id, /**< Vendor ID. */ + uint16_t *vendor_type /**< Vendor-specific type. */ +) +{ + *vendor_id = 0; + *vendor_type = 0; + return 0; +} + +uint8_t * +ecrt_voe_handler_data(const ec_voe_handler_t *voe /**< VoE handler. */ +) +{ + return nullptr; +} + +size_t +ecrt_voe_handler_data_size(const ec_voe_handler_t *voe /**< VoE handler. */ +) +{ + return 0; +} + +int ecrt_voe_handler_write( + ec_voe_handler_t *voe, /**< VoE handler. */ + size_t size /**< Number of bytes to write (without the VoE header). */ +) +{ + return 0; +} + +int ecrt_voe_handler_read(ec_voe_handler_t *voe /**< VoE handler. */ +) +{ + return 0; +} + +int ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe /**< VoE handler. */ +) +{ + return 0; +} + +ec_request_state_t +ecrt_voe_handler_execute(ec_voe_handler_t *voe /**< VoE handler. */ +) +{ + return EC_REQUEST_ERROR; +} + +/***************************************************************************** + * Register request + ****************************************************************************/ + +uint8_t * +ecrt_reg_request_data(const ec_reg_request_t *req /**< Register request. */ +) +{ + return nullptr; +} + +ec_request_state_t +ecrt_reg_request_state(const ec_reg_request_t *req /**< Register request. */ +) +{ + return EC_REQUEST_ERROR; +} + +int ecrt_reg_request_write( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ +) +{ + return 0; +} + +int ecrt_reg_request_read( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ +) +{ + return 0; +} + +/***************************************************************************** + * Floating-point read functions ****************************************************************************/ void ecrt_write_lreal(void *data, double const value) @@ -853,16 +1490,18 @@ void ec_slave_config::dumpJson(std::ostream &out, int indent) const out << "\"product_id\": " << product_code << ",\n"; add_spaces(out, indent); out << "\"sdos\": "; - map2Json(out, sdos, - [](std::ostream &out, const std::basic_string &value) - { + map2Json( + out, + sdos, + [](std::ostream &out, const std::basic_string &value) { out << "\"0x"; for (const auto s : value) { - out << std::hex << std::setfill('0') - << std::setw(2) << (unsigned)s; + out << std::hex << std::setfill('0') << std::setw(2) + << (unsigned) s; } out << '"'; - }, indent); + }, + indent); out << '\n'; add_spaces(out, indent - 4); out << "}"; diff --git a/fake_lib/fakeethercat.h b/fake_lib/fakeethercat.h index a7c98264..383ca279 100644 --- a/fake_lib/fakeethercat.h +++ b/fake_lib/fakeethercat.h @@ -35,194 +35,209 @@ struct Offset { - int bytes; - int bits; + int bytes; + int bits; - constexpr Offset(int bytes, - int bits) : bytes(bytes), bits(bits) {} + constexpr Offset(int bytes, int bits): + bytes(bytes), + bits(bits) + {} - constexpr bool operator!=(const Offset &other) const noexcept - { - return bytes != other.bytes || bits != other.bits; - } + constexpr bool operator!=(const Offset &other) const noexcept + { + return bytes != other.bytes || bits != other.bits; + } }; constexpr Offset NotFound(-1, -1); struct pdo { - std::vector entries; + std::vector entries; - size_t sizeInBytes() const; + size_t sizeInBytes() const; - Offset findEntry(uint16_t idx, uint8_t subindex) const; + Offset findEntry(uint16_t idx, uint8_t subindex) const; + Offset findEntryByPos(unsigned int) const; }; struct syncManager { - ec_direction_t dir; - std::map pdos; + ec_direction_t dir; + std::map pdos; }; class ec_address { - uint32_t value; + private: + uint32_t value; -public: - ec_address(uint16_t alias, /**< Slave alias. */ - uint16_t position /**< Slave position. */) - : value(static_cast(alias) << 16 | position) - { - } + public: + ec_address( + uint16_t alias, /**< Slave alias. */ + uint16_t position /**< Slave position. */): + value(static_cast(alias) << 16 | position) + {} - uint16_t getAlias() const { return value >> 16; } - uint16_t getPosition() const { return value & 0xFFFF; } - uint32_t getCombined() const { return value; } + uint16_t getAlias() const { return value >> 16; } + uint16_t getPosition() const { return value & 0xFFFF; } + uint32_t getCombined() const { return value; } - bool operator<(const ec_address &other) const noexcept - { - return value < other.value; - } + bool operator<(const ec_address &other) const noexcept + { + return value < other.value; + } - bool operator==(const ec_address &other) const noexcept - { - return value == other.value; - } + bool operator==(const ec_address &other) const noexcept + { + return value == other.value; + } }; class sdo_address { - uint32_t value; + private: + uint32_t value; -public: - sdo_address(uint16_t index, /**< Slave alias. */ - uint8_t subindex /**< Slave position. */) - : value(static_cast(index) << 8 | subindex) - { - } + public: + sdo_address( + uint16_t index, /**< Slave alias. */ + uint8_t subindex /**< Slave position. */): + value(static_cast(index) << 8 | subindex) + {} - uint16_t getIndex() const { return value >> 8; } - uint8_t getSubIndex() const { return value & 0xFF; } - uint32_t getCombined() const { return value; } + uint16_t getIndex() const { return value >> 8; } + uint8_t getSubIndex() const { return value & 0xFF; } + uint32_t getCombined() const { return value; } - bool operator<(const sdo_address &other) const noexcept - { - return value < other.value; - } + bool operator<(const sdo_address &other) const noexcept + { + return value < other.value; + } - bool operator==(const sdo_address &other) const noexcept - { - return value == other.value; - } + bool operator==(const sdo_address &other) const noexcept + { + return value == other.value; + } }; struct ec_slave_config { - ec_address address; - uint32_t vendor_id; /**< Expected vendor ID. */ - uint32_t product_code; /**< Expected product code. */ - std::map sync_managers; - std::map> sdos; + ec_address address; + uint32_t vendor_id; /**< Expected vendor ID. */ + uint32_t product_code; /**< Expected product code. */ + std::map sync_managers; + std::map> sdos; - ec_slave_config( - ec_address address, - uint32_t vendor_id, /**< Expected vendor ID. */ - uint32_t product_code /**< Expected product code. */) - : address(address), vendor_id(vendor_id), product_code(product_code) - { - } + ec_slave_config( + ec_address address, + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */): + address(address), + vendor_id(vendor_id), + product_code(product_code) + {} - void dumpJson(std::ostream &out, int indent) const; + void dumpJson(std::ostream &out, int indent) const; }; struct ec_domain { - -private: - struct PdoMap - { - size_t offset; - size_t size_bytes; - ec_address slave_address; - unsigned int syncManager; - uint16_t pdo_index; - ec_direction_t dir; - - PdoMap( - size_t offset, - size_t size_bytes, - ec_address slave_address, - unsigned int syncManager, - uint16_t pdo_index, - ec_direction_t dir) - : offset(offset), size_bytes(size_bytes), slave_address(slave_address), syncManager(syncManager), pdo_index(pdo_index), dir(dir) + private: + struct PdoMap { + size_t offset; + size_t size_bytes; + ec_address slave_address; + unsigned int syncManager; + uint16_t pdo_index; + ec_direction_t dir; + + PdoMap(size_t offset, + size_t size_bytes, + ec_address slave_address, + unsigned int syncManager, + uint16_t pdo_index, + ec_direction_t dir): + offset(offset), + size_bytes(size_bytes), + slave_address(slave_address), + syncManager(syncManager), + pdo_index(pdo_index), + dir(dir) + {} + }; + + std::vector data; + std::vector connected; + std::vector mapped_pdos; + rtipc_group *rt_group; + const char *prefix; + ec_master_t *master; + bool activated_ = false; + size_t numSlaves = 0; + + public: + explicit ec_domain( + struct rtipc *rtipc, + const char *prefix, + ec_master_t *master); + + uint8_t *getData() const + { + if (!activated_) { + return nullptr; + } + return const_cast(data.data()); } - }; - std::vector data; - std::vector connected; - std::vector mapped_pdos; - rtipc_group *rt_group; - const char *prefix; - ec_master_t *master; - bool activated_ = false; - size_t numSlaves = 0; + size_t getSize() const { return data.size(); } -public: - explicit ec_domain(struct rtipc *rtipc, const char *prefix, ec_master_t *master); + int activate(); + int process(); + int queue(); - uint8_t *getData() const - { - if (!activated_) - return nullptr; - return const_cast(data.data()); - } + size_t getNumSlaves() const { return numSlaves; } - int activate(); - int process(); - int queue(); + ssize_t + map(ec_slave_config const &config, + unsigned int syncManager, + uint16_t pdo_index); - size_t getNumSlaves() const { return numSlaves; } - - ssize_t map(ec_slave_config const &config, unsigned int syncManager, - uint16_t pdo_index); - - ec_master_t *getMaster() const { return master; } + ec_master_t *getMaster() const { return master; } }; struct ec_master { -private: - struct RtIpcDeleter - { - void operator()(struct rtipc *r) const + private: + struct RtIpcDeleter { - rtipc_exit(r); - } - }; + void operator()(struct rtipc *r) const { rtipc_exit(r); } + }; - std::string rt_ipc_dir; - std::string rt_ipc_name; - std::list domains; - std::map slaves; - std::unique_ptr rt_ipc; - int id_; + std::string rt_ipc_dir; + std::string rt_ipc_name; + std::list domains; + std::map slaves; + std::unique_ptr rt_ipc; + int id_; -public: - explicit ec_master(int id); + public: + explicit ec_master(int id); - int activate(); - ec_domain *createDomain(); + int activate(); + int deactivate(); - int getNoSlaves() const { return slaves.size(); } - int getId() const { return id_; } + ec_domain *createDomain(); -ec_slave_config_t *slave_config( - uint16_t alias, /**< Slave alias. */ - uint16_t position, /**< Slave position. */ - uint32_t vendor_id, /**< Expected vendor ID. */ - uint32_t product_code /**< Expected product code. */ -); + int getNoSlaves() const { return slaves.size(); } + int getId() const { return id_; } + + ec_slave_config_t *slave_config( + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ + ); };