Line data Source code
1 : // Copyright 2024 Accenture.
2 :
3 : #include "can/SocketCanTransceiver.h"
4 :
5 : #include <can/CanLogger.h>
6 : #include <can/canframes/ICANFrameSentListener.h>
7 : #include <linux/can.h>
8 : #include <linux/can/raw.h>
9 : #include <net/if.h>
10 : #include <sys/ioctl.h>
11 : #include <sys/socket.h>
12 :
13 : #include <fcntl.h>
14 : #include <signal.h>
15 : #include <type_traits>
16 : #include <unistd.h>
17 :
18 : #include <etl/error_handler.h>
19 : #include <etl/span.h>
20 :
21 : static_assert(
22 : std::is_standard_layout<::can::CANFrame>::value
23 : && std::is_trivially_destructible<::can::CANFrame>::value,
24 : "check for UB while passing through TxQueue");
25 :
26 : namespace can
27 : {
28 :
29 : using ::util::logger::CAN;
30 : using ::util::logger::Logger;
31 :
32 : namespace
33 : {
34 :
35 : template<typename F>
36 0 : void signalGuarded(F&& function)
37 : {
38 : sigset_t set, oldSet;
39 0 : sigfillset(&set);
40 0 : pthread_sigmask(SIG_SETMASK, &set, &oldSet);
41 0 : ::std::forward<F>(function)();
42 0 : pthread_sigmask(SIG_SETMASK, &oldSet, nullptr);
43 0 : }
44 :
45 : } // namespace
46 :
47 : // needed if ODR-used
48 : size_t const SocketCanTransceiver::TX_QUEUE_SIZE_BYTES;
49 :
50 1 : SocketCanTransceiver::SocketCanTransceiver(DeviceConfig const& config)
51 1 : : AbstractCANTransceiver(config.busId)
52 1 : , _txQueue()
53 1 : , _txReader(_txQueue)
54 1 : , _txWriter(_txQueue)
55 1 : , _config(config)
56 1 : , _fileDescriptor(-1)
57 2 : , _writable(false)
58 1 : {}
59 :
60 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::init()
61 : {
62 0 : if (!isInState(State::CLOSED))
63 : {
64 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
65 : }
66 0 : setState(State::INITIALIZED);
67 0 : return ErrorCode::CAN_ERR_OK;
68 : }
69 :
70 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::open()
71 : {
72 0 : if (!isInState(State::INITIALIZED))
73 : {
74 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
75 : }
76 0 : signalGuarded([this] { guardedOpen(); });
77 0 : setState(State::OPEN);
78 0 : _writable.store(true);
79 0 : return ErrorCode::CAN_ERR_OK;
80 : }
81 :
82 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::open(CANFrame const& /* frame */)
83 : {
84 0 : ETL_ASSERT_FAIL(ETL_ERROR_GENERIC("not implemented"));
85 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
86 : }
87 :
88 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::close()
89 : {
90 0 : if (!isInState(State::OPEN) && !isInState(State::MUTED))
91 : {
92 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
93 : }
94 0 : signalGuarded([this] { guardedClose(); });
95 0 : _writable.store(false);
96 0 : setState(State::CLOSED);
97 0 : return ErrorCode::CAN_ERR_OK;
98 : }
99 :
100 0 : void SocketCanTransceiver::shutdown() {}
101 :
102 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::write(CANFrame const& frame)
103 : {
104 0 : return writeImpl(frame, nullptr);
105 : }
106 :
107 : ICanTransceiver::ErrorCode
108 0 : SocketCanTransceiver::write(CANFrame const& frame, ICANFrameSentListener& listener)
109 : {
110 0 : return writeImpl(frame, &listener);
111 : }
112 :
113 : ICanTransceiver::ErrorCode
114 0 : SocketCanTransceiver::writeImpl(CANFrame const& frame, ICANFrameSentListener* listener)
115 : {
116 0 : if (!_writable.load(std::memory_order_relaxed))
117 : {
118 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
119 : }
120 0 : auto memory = _txWriter.allocate(TX_ELEMENT_SIZE_BYTES);
121 0 : if (memory.size() < TX_ELEMENT_SIZE_BYTES)
122 : {
123 0 : return ErrorCode::CAN_ERR_TX_HW_QUEUE_FULL;
124 : }
125 0 : ::std::memcpy(memory.data(), &frame, sizeof(frame));
126 0 : ::std::memcpy(memory.data() + sizeof(frame), static_cast<void*>(&listener), sizeof(void*));
127 0 : _txWriter.commit();
128 0 : return ErrorCode::CAN_ERR_OK;
129 : }
130 :
131 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::mute()
132 : {
133 0 : if (!isInState(State::OPEN))
134 : {
135 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
136 : }
137 0 : _writable.store(false);
138 0 : setState(State::MUTED);
139 0 : return ErrorCode::CAN_ERR_OK;
140 : }
141 :
142 0 : ICanTransceiver::ErrorCode SocketCanTransceiver::unmute()
143 : {
144 0 : if (!isInState(State::MUTED))
145 : {
146 0 : return ErrorCode::CAN_ERR_ILLEGAL_STATE;
147 : }
148 0 : setState(State::OPEN);
149 0 : _writable.store(true);
150 0 : return ErrorCode::CAN_ERR_OK;
151 : }
152 :
153 0 : uint32_t SocketCanTransceiver::getBaudrate() const { return 500000U; }
154 :
155 0 : uint16_t SocketCanTransceiver::getHwQueueTimeout() const { return 1U; }
156 :
157 0 : void SocketCanTransceiver::run(int maxSentPerRun, int maxReceivedPerRun)
158 : {
159 0 : signalGuarded([this, maxSentPerRun, maxReceivedPerRun]
160 0 : { guardedRun(maxSentPerRun, maxReceivedPerRun); });
161 0 : }
162 :
163 0 : void SocketCanTransceiver::guardedOpen()
164 : {
165 0 : char const* const name = _config.name;
166 0 : int error = 0;
167 0 : int const fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
168 0 : if (fd < 0)
169 : {
170 0 : Logger::error(
171 : CAN, "[SocketCanTransceiver] Failed to create socket (node=%s, error=%d)", name, fd);
172 0 : return;
173 : }
174 :
175 : struct ifreq ifr;
176 0 : ::std::strcpy(ifr.ifr_name, name);
177 0 : error = ioctl(fd, SIOCGIFINDEX, &ifr);
178 0 : if (error < 0)
179 : {
180 0 : Logger::error(
181 : CAN, "[SocketCanTransceiver] Failed to ioctl socket (node=%s, error=%d)", name, error);
182 0 : return;
183 : }
184 :
185 0 : int const enable_canfd = 1;
186 0 : error = setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd));
187 0 : if (error < 0)
188 : {
189 0 : Logger::error(
190 : CAN,
191 : "[SocketCanTransceiver] Failed to setsockopt socket (node=%s, error=%d)",
192 : name,
193 : error);
194 0 : return;
195 : }
196 :
197 0 : error = fcntl(fd, F_SETFL, O_NONBLOCK);
198 0 : if (error < 0)
199 : {
200 0 : Logger::error(
201 : CAN,
202 : "[SocketCanTransceiver] Failed to switch to non-blocking mode (node=%s, error=%d)",
203 : name,
204 : error);
205 0 : return;
206 : }
207 :
208 : struct sockaddr_can addr;
209 0 : ::std::memset(&addr, 0, sizeof(addr));
210 0 : addr.can_family = AF_CAN;
211 0 : addr.can_ifindex = ifr.ifr_ifindex;
212 0 : error = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
213 0 : if (error < 0)
214 : {
215 0 : Logger::error(
216 : CAN, "[SocketCanTransceiver] Failed to bind socket (node=%s, error=%d)", name, error);
217 0 : return;
218 : }
219 :
220 0 : _fileDescriptor = fd;
221 : }
222 :
223 0 : void SocketCanTransceiver::guardedClose()
224 : {
225 0 : ::close(_fileDescriptor);
226 0 : _fileDescriptor = -1;
227 0 : }
228 :
229 0 : void SocketCanTransceiver::guardedRun(int maxSentPerRun, int maxReceivedPerRun)
230 : {
231 : // MUTED condition does not affect the messages already in the write queue;
232 : // the idea is that once we confirmed that we had accepted the message for delivery,
233 : // we shall try to deliver it.
234 0 : for (int count = 0; count < maxSentPerRun; ++count)
235 : {
236 0 : auto memory = _txReader.peek();
237 0 : if (memory.size() < TX_ELEMENT_SIZE_BYTES)
238 : {
239 0 : break;
240 : }
241 0 : CANFrame canFrame;
242 0 : ::std::memcpy(static_cast<void*>(&canFrame), memory.data(), sizeof(canFrame));
243 0 : ICANFrameSentListener* listener = nullptr;
244 0 : ::std::memcpy(
245 0 : static_cast<void*>(&listener), memory.data() + sizeof(canFrame), sizeof(void*));
246 0 : _txReader.release();
247 : can_frame socketCanFrame;
248 0 : ::std::memset(&socketCanFrame, 0, sizeof(socketCanFrame));
249 0 : socketCanFrame.can_id = canFrame.getId();
250 0 : int length = canFrame.getPayloadLength();
251 0 : socketCanFrame.can_dlc = length;
252 0 : ::std::memcpy(socketCanFrame.data, canFrame.getPayload(), length);
253 0 : ::std::memset(socketCanFrame.data + length, 0, sizeof(socketCanFrame.data) - length);
254 0 : length = ::write(_fileDescriptor, reinterpret_cast<char*>(&socketCanFrame), CAN_MTU);
255 0 : if (length != CAN_MTU)
256 : {
257 0 : break;
258 : }
259 0 : if (listener != nullptr)
260 : {
261 0 : listener->canFrameSent(canFrame);
262 : }
263 0 : notifySentListeners(canFrame);
264 : }
265 :
266 0 : for (int count = 0; count < maxReceivedPerRun; ++count)
267 : {
268 : alignas(can_frame) uint8_t buffer[CANFD_MTU];
269 0 : int const length = read(_fileDescriptor, reinterpret_cast<char*>(buffer), CANFD_MTU);
270 0 : if (length < 0)
271 : {
272 0 : break;
273 : }
274 0 : if (length == CAN_MTU)
275 : {
276 0 : can_frame const& socketCanFrame = *reinterpret_cast<can_frame const*>(buffer);
277 0 : Logger::debug(
278 : CAN,
279 : "[SocketCanTransceiver] received CAN frame, id=0x%X, length=%d",
280 0 : (int)socketCanFrame.can_id,
281 0 : (int)socketCanFrame.can_dlc);
282 0 : CANFrame canFrame;
283 0 : canFrame.setId(socketCanFrame.can_id);
284 0 : canFrame.setPayload(socketCanFrame.data, socketCanFrame.can_dlc);
285 0 : canFrame.setPayloadLength(socketCanFrame.can_dlc);
286 0 : canFrame.setTimestamp(0);
287 :
288 0 : notifyListeners(canFrame);
289 : }
290 : }
291 0 : }
292 :
293 : } // namespace can
|
