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