Lely core libraries  2.3.4
can_ctrl.c
Go to the documentation of this file.
1 
24 #include "io.h"
25 
26 #if !LELY_NO_STDIO && defined(__linux__)
27 
28 #include <lely/io2/linux/can.h>
29 #include <lely/util/util.h>
30 
31 #include <assert.h>
32 #include <errno.h>
33 #include <stdlib.h>
34 
35 #include <net/if.h>
36 
37 #include "can_attr.h"
38 #define LELY_IO_IFREQ_DOMAIN AF_CAN
39 #define LELY_IO_IFREQ_TYPE SOCK_RAW
40 #define LELY_IO_IFREQ_PROTOCOL CAN_RAW
41 #include "if.h"
42 #include "ifreq.h"
43 
44 #ifndef LELY_IO_CAN_TXLEN
45 #define LELY_IO_CAN_TXLEN 128
47 #endif
48 
49 static int io_can_ctrl_impl_stop(io_can_ctrl_t *ctrl);
50 static int io_can_ctrl_impl_stopped(const io_can_ctrl_t *ctrl);
51 static int io_can_ctrl_impl_restart(io_can_ctrl_t *ctrl);
52 static int io_can_ctrl_impl_get_bitrate(
53  const io_can_ctrl_t *ctrl, int *pnominal, int *pdata);
54 static int io_can_ctrl_impl_set_bitrate(
55  io_can_ctrl_t *ctrl, int nominal, int data);
56 static int io_can_ctrl_impl_get_state(const io_can_ctrl_t *ctrl);
57 
58 // clang-format off
59 static const struct io_can_ctrl_vtbl io_can_ctrl_impl_vtbl = {
60  &io_can_ctrl_impl_stop,
61  &io_can_ctrl_impl_stopped,
62  &io_can_ctrl_impl_restart,
63  &io_can_ctrl_impl_get_bitrate,
64  &io_can_ctrl_impl_set_bitrate,
65  &io_can_ctrl_impl_get_state
66 };
67 // clang-format on
68 
70  const struct io_can_ctrl_vtbl *ctrl_vptr;
71  unsigned int index;
72  char name[IF_NAMESIZE];
73  int flags;
74 };
75 
76 static inline struct io_can_ctrl_impl *io_can_ctrl_impl_from_ctrl(
77  const io_can_ctrl_t *ctrl);
78 
79 void *
80 io_can_ctrl_alloc(void)
81 {
82  struct io_can_ctrl_impl *impl = malloc(sizeof(*impl));
83  if (!impl)
84  return NULL;
85  // Suppress a GCC maybe-uninitialized warning.
86  impl->ctrl_vptr = NULL;
87  // cppcheck-suppress memleak symbolName=impl
88  return &impl->ctrl_vptr;
89 }
90 
91 void
92 io_can_ctrl_free(void *ptr)
93 {
94  if (ptr)
95  free(io_can_ctrl_impl_from_ctrl(ptr));
96 }
97 
99 io_can_ctrl_init(io_can_ctrl_t *ctrl, unsigned int index, size_t txlen)
100 {
101  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
102 
103  if (!txlen)
104  txlen = LELY_IO_CAN_TXLEN;
105 
106  impl->ctrl_vptr = &io_can_ctrl_impl_vtbl;
107 
108  impl->index = index;
109 
110  memset(impl->name, 0, IF_NAMESIZE);
111  if (!if_indextoname(impl->index, impl->name))
112  return NULL;
113 
114  struct io_can_attr attr = IO_CAN_ATTR_INIT;
115  // Some CAN network interfaces, such as the serial line CAN interface
116  // provided by the SLCAN driver, do not provide the CAN bus attributes.
117  // This is not an error.
118  int errsv = errno;
119  if (io_can_attr_get(&attr, impl->index) == -1 && errno != ENOTSUP)
120  return NULL;
121  errno = errsv;
122 
123  impl->flags = attr.flags;
124 
125  if (io_if_set_txqlen(ARPHRD_CAN, impl->index, txlen) == -1)
126  return NULL;
127 
128  return ctrl;
129 }
130 
131 void
132 io_can_ctrl_fini(io_can_ctrl_t *ctrl)
133 {
134  (void)ctrl;
135 }
136 
138 io_can_ctrl_create_from_name(const char *name, size_t txlen)
139 {
140  unsigned int index = if_nametoindex(name);
141  if (!index)
142  return 0;
143  return io_can_ctrl_create_from_index(index, txlen);
144 }
145 
147 io_can_ctrl_create_from_index(unsigned int index, size_t txlen)
148 {
149  int errsv = 0;
150 
151  io_can_ctrl_t *ctrl = io_can_ctrl_alloc();
152  if (!ctrl) {
153  errsv = errno;
154  goto error_alloc;
155  }
156 
157  io_can_ctrl_t *tmp = io_can_ctrl_init(ctrl, index, txlen);
158  if (!tmp) {
159  errsv = errno;
160  goto error_init;
161  }
162  ctrl = tmp;
163 
164  return ctrl;
165 
166 error_init:
167  io_can_ctrl_free((void *)ctrl);
168 error_alloc:
169  errno = errsv;
170  return NULL;
171 }
172 
173 void
175 {
176  if (ctrl) {
177  io_can_ctrl_fini(ctrl);
178  io_can_ctrl_free((void *)ctrl);
179  }
180 }
181 
182 const char *
184 {
185  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
186 
187  return impl->name;
188 }
189 
190 unsigned int
192 {
193  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
194 
195  return impl->index;
196 }
197 
198 int
200 {
201  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
202 
203  return impl->flags;
204 }
205 
206 static int
207 io_can_ctrl_impl_stop(io_can_ctrl_t *ctrl)
208 {
209  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
210 
211  int flags = 0;
212  return ifr_set_flags(impl->name, &flags, IFF_UP);
213 }
214 
215 static int
216 io_can_ctrl_impl_stopped(const io_can_ctrl_t *ctrl)
217 {
218  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
219 
220  int flags = ifr_get_flags(impl->name);
221  return flags == -1 ? -1 : !(flags & IFF_UP);
222 }
223 
224 static int
225 io_can_ctrl_impl_restart(io_can_ctrl_t *ctrl)
226 {
227  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
228 
229  int flags = IFF_UP;
230  return ifr_set_flags(impl->name, &flags, IFF_UP);
231 }
232 
233 static int
234 io_can_ctrl_impl_get_bitrate(
235  const io_can_ctrl_t *ctrl, int *pnominal, int *pdata)
236 {
237  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
238 
239  struct io_can_attr attr = IO_CAN_ATTR_INIT;
240  if (io_can_attr_get(&attr, impl->index) == -1)
241  return -1;
242 
243  if (pnominal)
244  *pnominal = attr.nominal;
245 
246  if (pdata)
247 #if LELY_NO_CANFD
248  *pdata = 0;
249 #else
250  *pdata = attr.data;
251 #endif
252 
253  return 0;
254 }
255 
256 static int
257 io_can_ctrl_impl_set_bitrate(io_can_ctrl_t *ctrl, int nominal, int data)
258 {
259  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
260 
261  char buf[128];
262  struct rtattr *linkinfo = (struct rtattr *)buf;
263  *linkinfo = (struct rtattr){ .rta_len = RTA_LENGTH(0),
264  .rta_type = IFLA_LINKINFO };
265 
266  struct rtattr *info_kind = RTA_TAIL(linkinfo);
267  *info_kind = (struct rtattr){ .rta_len = RTA_LENGTH(strlen("can")),
268  .rta_type = IFLA_INFO_KIND };
269  strcpy(RTA_DATA(info_kind), "can");
270 
271  linkinfo->rta_len += RTA_ALIGN(info_kind->rta_len);
272 
273  struct rtattr *info_data = RTA_TAIL(info_kind);
274  *info_data = (struct rtattr){ .rta_len = RTA_LENGTH(0),
275  .rta_type = IFLA_INFO_DATA };
276  struct rtattr *rta = RTA_DATA(info_data);
277 
278  *rta = (struct rtattr){ .rta_len = RTA_LENGTH(
279  sizeof(struct can_bittiming)),
280  .rta_type = IFLA_CAN_BITTIMING };
281  *(struct can_bittiming *)RTA_DATA(rta) =
282  (struct can_bittiming){ .bitrate = nominal };
283  info_data->rta_len += RTA_ALIGN(rta->rta_len);
284  rta = RTA_TAIL(rta);
285 
286 #if LELY_NO_CANFD
287  (void)data;
288 #else
289  if (impl->flags & IO_CAN_BUS_FLAG_BRS) {
290  *rta = (struct rtattr){ .rta_len = RTA_LENGTH(sizeof(
291  struct can_bittiming)),
292  .rta_type = IFLA_CAN_DATA_BITTIMING };
293  *(struct can_bittiming *)RTA_DATA(rta) =
294  (struct can_bittiming){ .bitrate = data };
295  info_data->rta_len += RTA_ALIGN(rta->rta_len);
296  // rta = RTA_TAIL(rta); // Not needed if rta is not used below.
297  }
298 #endif
299 
300  linkinfo->rta_len += RTA_ALIGN(info_data->rta_len);
301 
302  int errsv = 0;
303 
304  int flags = 0;
305  if (ifr_set_flags(impl->name, &flags, IFF_UP) == -1) {
306  errsv = errno;
307  goto error_ifr_set_flags;
308  }
309 
310  struct rtnl_handle rth = { -1, 0, 0 };
311  if (rtnl_open(&rth) == -1) {
312  errsv = errno;
313  goto error_rtnl_open;
314  }
315 
316  // clang-format off
317  if (rtnl_send_newlink_request(&rth, AF_UNSPEC, ARPHRD_CAN, impl->index,
318  flags, linkinfo, RTA_ALIGN(linkinfo->rta_len)) == -1) {
319  // clang-format on
320  errsv = errno;
321  goto error_rtnl_send_newlink_request;
322  }
323 
324  if (rtnl_recv_ack(&rth) == -1) {
325  errsv = errno;
326  goto error_rtnl_recv_ack;
327  }
328 
329  rtnl_close(&rth);
330  return 0;
331 
332 error_rtnl_recv_ack:
333 error_rtnl_send_newlink_request:
334  rtnl_close(&rth);
335 error_rtnl_open:
336 error_ifr_set_flags:
337  errno = errsv;
338  return -1;
339 }
340 
341 static int
342 io_can_ctrl_impl_get_state(const io_can_ctrl_t *ctrl)
343 {
344  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
345 
346  int flags = ifr_get_flags(impl->name);
347  if (flags == -1)
348  return -1;
349  if (!(flags & IFF_UP))
350  return CAN_STATE_STOPPED;
351 
352  struct io_can_attr attr = IO_CAN_ATTR_INIT;
353  if (io_can_attr_get(&attr, impl->index) == -1)
354  return -1;
355 
356  return attr.state;
357 }
358 
359 static inline struct io_can_ctrl_impl *
360 io_can_ctrl_impl_from_ctrl(const io_can_ctrl_t *ctrl)
361 {
362  assert(ctrl);
363 
364  return structof(ctrl, struct io_can_ctrl_impl, ctrl_vptr);
365 }
366 
367 #endif // !LELY_NO_STDIO && __linux__
ifreq.h
io_can_ctrl_get_flags
int io_can_ctrl_get_flags(const io_can_ctrl_t *ctrl)
Returns the flags specifying which CAN bus features are enabled.
Definition: can_ctrl.c:199
io_can_ctrl_vtbl
Definition: can.h:139
io_can_ctrl_get_name
const char * io_can_ctrl_get_name(const io_can_ctrl_t *ctrl)
Returns the interface name of a CAN controller.
Definition: can_ctrl.c:183
util.h
LELY_IO_CAN_TXLEN
#define LELY_IO_CAN_TXLEN
The default SocketCAN transmit queue length (in number of CAN frames).
Definition: can_ctrl.c:46
io_can_ctrl_destroy
void io_can_ctrl_destroy(io_can_ctrl_t *ctrl)
Destroys a CAN controller.
Definition: can_ctrl.c:174
rtnl_handle
Definition: rtnl.h:47
io.h
io_can_attr
Definition: can_attr.h:50
CAN_STATE_STOPPED
@ CAN_STATE_STOPPED
The device is stopped.
Definition: err.h:38
RTA_TAIL
#define RTA_TAIL(rta)
Returns the address of the next attribute.
Definition: rtnl.h:35
io_can_ctrl_create_from_name
io_can_ctrl_t * io_can_ctrl_create_from_name(const char *name, size_t txlen)
Creates a new CAN controller from an interface name.
Definition: can_ctrl.c:138
can_attr.h
io_can_ctrl_get_index
unsigned int io_can_ctrl_get_index(const io_can_ctrl_t *ctrl)
Returns the interface index of a CAN controller.
Definition: can_ctrl.c:191
IO_CAN_BUS_FLAG_BRS
@ IO_CAN_BUS_FLAG_BRS
Bit Rate Switch support is enabled.
Definition: can.h:44
structof
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
if.h
io_can_ctrl_impl
Definition: can_ctrl.c:69
io_can_ctrl_create_from_index
io_can_ctrl_t * io_can_ctrl_create_from_index(unsigned int index, size_t txlen)
Creates a new CAN controller from an interface index.
Definition: can_ctrl.c:147
can.h
stdlib.h
io_can_ctrl_t
const struct io_can_ctrl_vtbl *const io_can_ctrl_t
An abstract CAN controller.
Definition: can.h:56