| Directory: | ./ |
|---|---|
| File: | src/IoLinkDevice.cpp |
| Date: | 2026-10-12 15:11:37 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 57 | 74 | 77.0% |
| Branches: | 32 | 76 | 42.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /***************************************************************************** | ||
| 2 | * | ||
| 3 | * This file is part of the ecapp library (EtherCAT application devices). | ||
| 4 | * | ||
| 5 | * Copyright (C) 2026 Florian Pose <fp@igh.de> | ||
| 6 | * | ||
| 7 | * The ecapp library is free software: you can redistribute it and/or | ||
| 8 | * modify it under the terms of the GNU Lesser General Public License | ||
| 9 | * as published by the Free Software Foundation, version 3 of the | ||
| 10 | * License. | ||
| 11 | * | ||
| 12 | * The ecapp library is distributed in the hope that it will be useful, | ||
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| 15 | * Lesser General Public License for more details. | ||
| 16 | * | ||
| 17 | * You should have received a copy of the GNU Lesser General Public | ||
| 18 | * License along with the ecapp library. If not, see | ||
| 19 | * <https://www.gnu.org/licenses/>. | ||
| 20 | * | ||
| 21 | ****************************************************************************/ | ||
| 22 | |||
| 23 | #include "ecapp/IoLinkDevice.h" | ||
| 24 | |||
| 25 | #include "ecapp/Exceptions.h" | ||
| 26 | |||
| 27 | #include <cmath> | ||
| 28 | #include <cstdint> | ||
| 29 | #include <sstream> | ||
| 30 | #include <stdexcept> | ||
| 31 | #include <string> | ||
| 32 | #include <utility> | ||
| 33 | |||
| 34 | using std::runtime_error; | ||
| 35 | using std::string; | ||
| 36 | using std::stringstream; | ||
| 37 | |||
| 38 | /****************************************************************************/ | ||
| 39 | |||
| 40 | namespace EcApp { | ||
| 41 | |||
| 42 | namespace { | ||
| 43 | |||
| 44 | /** Encodes a minimum cycle time (in ms) into the IO-Link "Min cycle | ||
| 45 | * time" byte format used by CoE object 0x80n0:04 sub 0x22 (EL6224 | ||
| 46 | * manual, chapter 6.5.1, Table 1): bit 6-7 select a time base, bit | ||
| 47 | * 0-5 a multiplier of it, covering four contiguous ranges | ||
| 48 | * (0.0-6.3, 6.4-31.6, 32.0-132.8, 134.4-537.6 ms). Picks the widest | ||
| 49 | * (least precise) range the requested time still fits, same as every | ||
| 50 | * blob decoded from a real device did. */ | ||
| 51 | 9 | uint8_t encodeMinCycleTime(double ms) | |
| 52 | { | ||
| 53 | static constexpr struct | ||
| 54 | { | ||
| 55 | double rangeStart; | ||
| 56 | double step; | ||
| 57 | uint8_t timeBase; | ||
| 58 | } ranges[] = { | ||
| 59 | {134.4, 6.4, 0x3}, | ||
| 60 | {32.0, 1.6, 0x2}, | ||
| 61 | {6.4, 0.4, 0x1}, | ||
| 62 | {0.0, 0.1, 0x0}, | ||
| 63 | }; | ||
| 64 |
1/2✓ Branch 0 taken 33 times.
✗ Branch 1 not taken.
|
33 | for (const auto &range : ranges) { |
| 65 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 24 times.
|
33 | if (ms >= range.rangeStart) { |
| 66 | long multiplier = | ||
| 67 | 9 | std::lround((ms - range.rangeStart) / range.step); | |
| 68 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 9 times.
|
9 | if (multiplier < 0) { |
| 69 | ✗ | multiplier = 0; | |
| 70 | } | ||
| 71 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 9 times.
|
9 | else if (multiplier > 0x3F) { |
| 72 | ✗ | multiplier = 0x3F; | |
| 73 | } | ||
| 74 | return static_cast<uint8_t>( | ||
| 75 | 9 | (range.timeBase << 6) | static_cast<uint8_t>(multiplier)); | |
| 76 | } | ||
| 77 | } | ||
| 78 | ✗ | return 0x00; | |
| 79 | } | ||
| 80 | |||
| 81 | /** Encodes a process data length into the IO-Link "Process data | ||
| 82 | * in/out length" byte format (0x80n0:04 sub 0x24/0x25): bit 7 = | ||
| 83 | * "BYTE" (length counts bytes-minus-one rather than bits directly), | ||
| 84 | * bit 6 = SIO mode supported, bit 0-4 = length. A length of zero | ||
| 85 | * bits (no process data in that direction) always encodes as a plain | ||
| 86 | * zero byte, same as every no-output ifm device blob does -- the SIO | ||
| 87 | * bit only ever appears together with actual process data. Lengths | ||
| 88 | * up to 31 bit are stated directly (bit 7 clear); wider ones need the | ||
| 89 | * BYTE encoding, since the 5-bit length field alone cannot reach | ||
| 90 | * them. */ | ||
| 91 | 18 | uint8_t encodeProcessDataLength(unsigned int bits, bool sioSupported) | |
| 92 | { | ||
| 93 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 12 times.
|
18 | if (bits == 0) { |
| 94 | 6 | return 0x00; | |
| 95 | } | ||
| 96 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 2 times.
|
12 | uint8_t sioBit = sioSupported ? 0x40 : 0x00; |
| 97 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
|
12 | if (bits <= 31) { |
| 98 | 6 | return sioBit | static_cast<uint8_t>(bits); | |
| 99 | } | ||
| 100 | 6 | unsigned int bytes = (bits + 7) / 8; | |
| 101 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
|
6 | if (bytes > 32) { |
| 102 | throw std::out_of_range( | ||
| 103 | ✗ | "IO-Link process data too long for the length field."); | |
| 104 | } | ||
| 105 | 6 | return 0x80 | sioBit | static_cast<uint8_t>(bytes - 1); | |
| 106 | } | ||
| 107 | |||
| 108 | } // namespace | ||
| 109 | |||
| 110 | /****************************************************************************/ | ||
| 111 | |||
| 112 | IoLinkDevice::~IoLinkDevice() = default; | ||
| 113 | |||
| 114 | /****************************************************************************/ | ||
| 115 | |||
| 116 | std::array<uint8_t, 22> | ||
| 117 | 9 | IoLinkDevice::buildIoLinkPortConfigSdo(const IoLinkPortIdentity &identity) | |
| 118 | { | ||
| 119 | /* | ||
| 120 | * CoE object 0x80n0 "IO Settings" (EL6224 manual, chapter 6.5.1), | ||
| 121 | * "complete access": 22 non-empty leading bytes of its 0x28 (=40) | ||
| 122 | * concatenated subindices. | ||
| 123 | * | ||
| 124 | * byte(s) Sub Meaning | ||
| 125 | * 0 0x00 Max subindex, always 0x28 -- not device data. | ||
| 126 | * 1 -- Subindices 0x01-0x03 are empty for every | ||
| 127 | * device this was derived from (SU6020/SU8020/ | ||
| 128 | * PV8004/DV2131/SM4000/SM7000/TA2xxx). | ||
| 129 | * 2-5 0x04 Device ID, UINT32 LE. | ||
| 130 | * 6-9 0x05 Vendor ID, UINT32 LE. | ||
| 131 | * 10 0x20 IO-Link revision: low nibble = minor, high | ||
| 132 | * nibble = major. | ||
| 133 | * 11 0x21 FrameCapability: bit 0 = ISDU supported, | ||
| 134 | * bit 1 = TYPE_1 interface supported, | ||
| 135 | * bit 7 = PHY1. | ||
| 136 | * 12 0x22 Min cycle time, see encodeMinCycleTime(). | ||
| 137 | * 13 0x23 Offset time, reserved, always 0x00. | ||
| 138 | * 14 0x24 Process data IN length, see | ||
| 139 | * encodeProcessDataLength(). | ||
| 140 | * 15 0x25 Process data OUT length, same encoding. | ||
| 141 | * 16-19 0x26/ Compatible ID / reserved, always 0x0000 each. | ||
| 142 | * 0x27 | ||
| 143 | * 20-21 0x28 Master Control, UINT16 LE: bit 0-3 = port | ||
| 144 | * mode (3 = cyclic IO-Link communication), | ||
| 145 | * bit 4-15 = data storage mode (2 = active). | ||
| 146 | */ | ||
| 147 | 9 | std::array<uint8_t, 22> data {}; | |
| 148 | 9 | data[0] = 0x28; | |
| 149 | |||
| 150 | 9 | data[2] = static_cast<uint8_t>(identity.deviceId); | |
| 151 | 9 | data[3] = static_cast<uint8_t>(identity.deviceId >> 8); | |
| 152 | 9 | data[4] = static_cast<uint8_t>(identity.deviceId >> 16); | |
| 153 | |||
| 154 | 9 | data[6] = static_cast<uint8_t>(identity.vendorId); | |
| 155 | 9 | data[7] = static_cast<uint8_t>(identity.vendorId >> 8); | |
| 156 | 9 | data[8] = static_cast<uint8_t>(identity.vendorId >> 16); | |
| 157 | |||
| 158 | 9 | data[10] = static_cast<uint8_t>( | |
| 159 | 9 | (identity.revisionMajor << 4) | (identity.revisionMinor & 0x0F)); | |
| 160 | 9 | data[11] = static_cast<uint8_t>( | |
| 161 | 9 | (identity.isduSupported ? 0x01 : 0x00) | |
| 162 |
3/4✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 6 times.
|
9 | | (identity.type1Supported ? 0x02 : 0x00) |
| 163 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8 times.
|
9 | | (identity.phy1 ? 0x80 : 0x00)); |
| 164 | 9 | data[12] = encodeMinCycleTime(identity.minCycleTimeMs); | |
| 165 | 18 | data[14] = encodeProcessDataLength( | |
| 166 | 18 | identity.processDataInBits, identity.sioSupported); | |
| 167 | 18 | data[15] = encodeProcessDataLength( | |
| 168 | 18 | identity.processDataOutBits, identity.sioSupported); | |
| 169 | |||
| 170 |
1/2✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
|
9 | uint16_t masterControl = |
| 171 | 9 | 0x0003 | (identity.dataStorage ? 0x0020 : 0x0000); | |
| 172 | 9 | data[20] = static_cast<uint8_t>(masterControl); | |
| 173 | 9 | data[21] = static_cast<uint8_t>(masterControl >> 8); | |
| 174 | |||
| 175 | 9 | return data; | |
| 176 | } | ||
| 177 | |||
| 178 | /****************************************************************************/ | ||
| 179 | |||
| 180 | ✗ | void IoLinkDevice::configurePort( | |
| 181 | ec_slave_config_t *sc, | ||
| 182 | uint16_t sdoIndex, | ||
| 183 | const IoLinkPortIdentity &identity) | ||
| 184 | { | ||
| 185 | ✗ | std::array<uint8_t, 22> data = buildIoLinkPortConfigSdo(identity); | |
| 186 | ✗ | if (ecrt_slave_config_complete_sdo( | |
| 187 | ✗ | sc, sdoIndex, data.data(), data.size())) { | |
| 188 | ✗ | stringstream err; | |
| 189 | ✗ | err << "Failed to configure IO-Link port SDO " << sdoIndex << "."; | |
| 190 | ✗ | throw runtime_error(err.str()); | |
| 191 | } | ||
| 192 | } | ||
| 193 | |||
| 194 | /****************************************************************************/ | ||
| 195 | |||
| 196 | 194 | IoLinkDeviceRegistry &IoLinkDeviceRegistry::instance() | |
| 197 | { | ||
| 198 |
3/4✓ Branch 0 taken 12 times.
✓ Branch 1 taken 182 times.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
194 | static IoLinkDeviceRegistry registry; |
| 199 | 194 | return registry; | |
| 200 | } | ||
| 201 | |||
| 202 | /****************************************************************************/ | ||
| 203 | |||
| 204 | 192 | void IoLinkDeviceRegistry::add( | |
| 205 | string name, | ||
| 206 | string sourceFile, | ||
| 207 | IoLinkDeviceFactory factory) | ||
| 208 | { | ||
| 209 |
1/2✓ Branch 4 taken 192 times.
✗ Branch 5 not taken.
|
768 | factories.push_back( |
| 210 | 576 | {std::move(name), std::move(sourceFile), std::move(factory)}); | |
| 211 | 192 | } | |
| 212 | |||
| 213 | /****************************************************************************/ | ||
| 214 | |||
| 215 | const IoLinkDeviceFactory & | ||
| 216 | 2 | IoLinkDeviceRegistry::find(const string &name) const | |
| 217 | { | ||
| 218 |
2/2✓ Branch 3 taken 17 times.
✓ Branch 4 taken 1 times.
|
18 | for (const auto &entry : factories) { |
| 219 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 16 times.
|
17 | if (entry.name == name) { |
| 220 | 2 | return entry.factory; | |
| 221 | } | ||
| 222 | } | ||
| 223 | |||
| 224 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | stringstream err; |
| 225 | err << "No IO-Link device driver registered under the name \"" << name | ||
| 226 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | << "\"."; |
| 227 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | throw UnknownDevice(err.str()); |
| 228 | } | ||
| 229 | |||
| 230 | /****************************************************************************/ | ||
| 231 | |||
| 232 | ✗ | std::vector<IoLinkDriverDescriptor> IoLinkDeviceRegistry::all() const | |
| 233 | { | ||
| 234 | ✗ | std::vector<IoLinkDriverDescriptor> ret; | |
| 235 | ✗ | ret.reserve(factories.size()); | |
| 236 | ✗ | for (const auto &entry : factories) { | |
| 237 | ✗ | ret.push_back({entry.name, entry.sourceFile}); | |
| 238 | } | ||
| 239 | ✗ | return ret; | |
| 240 | } | ||
| 241 | |||
| 242 | /****************************************************************************/ | ||
| 243 | |||
| 244 | 2 | std::unique_ptr<IoLinkDevice> createIoLinkDevice( | |
| 245 | const string &name, | ||
| 246 | pdserv *pdServ, | ||
| 247 | pdtask *task, | ||
| 248 | const string &prefix) | ||
| 249 | { | ||
| 250 | 2 | return IoLinkDeviceRegistry::instance().find(name)(pdServ, task, prefix); | |
| 251 | } | ||
| 252 | |||
| 253 | } // namespace EcApp | ||
| 254 | |||
| 255 | /****************************************************************************/ | ||
| 256 |