Lely core libraries  2.2.5
can_ctrl.c
Go to the documentation of this file.
1 
24 #include "io.h"
25 
26 #ifdef __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  // cppcheck-suppress memleak symbolName=impl
84  return impl ? &impl->ctrl_vptr : NULL;
85 }
86 
87 void
88 io_can_ctrl_free(void *ptr)
89 {
90  if (ptr)
91  free(io_can_ctrl_impl_from_ctrl(ptr));
92 }
93 
95 io_can_ctrl_init(io_can_ctrl_t *ctrl, unsigned int index, size_t txlen)
96 {
97  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
98 
99  if (!txlen)
100  txlen = LELY_IO_CAN_TXLEN;
101 
102  impl->ctrl_vptr = &io_can_ctrl_impl_vtbl;
103 
104  impl->index = index;
105 
106  memset(impl->name, 0, IF_NAMESIZE);
107  if (!if_indextoname(impl->index, impl->name))
108  return NULL;
109 
110  struct io_can_attr attr = IO_CAN_ATTR_INIT;
111  if (io_can_attr_get(&attr, impl->index) == -1)
112  return NULL;
113 
114  impl->flags = attr.flags;
115 
116  if (io_if_set_txqlen(ARPHRD_CAN, impl->index, txlen) == -1)
117  return NULL;
118 
119  return ctrl;
120 }
121 
122 void
123 io_can_ctrl_fini(io_can_ctrl_t *ctrl)
124 {
125  (void)ctrl;
126 }
127 
129 io_can_ctrl_create_from_name(const char *name, size_t txlen)
130 {
131  unsigned int index = if_nametoindex(name);
132  if (!index)
133  return 0;
134  return io_can_ctrl_create_from_index(index, txlen);
135 }
136 
138 io_can_ctrl_create_from_index(unsigned int index, size_t txlen)
139 {
140  int errsv = 0;
141 
142  io_can_ctrl_t *ctrl = io_can_ctrl_alloc();
143  if (!ctrl) {
144  errsv = errno;
145  goto error_alloc;
146  }
147 
148  io_can_ctrl_t *tmp = io_can_ctrl_init(ctrl, index, txlen);
149  if (!tmp) {
150  errsv = errno;
151  goto error_init;
152  }
153  ctrl = tmp;
154 
155  return ctrl;
156 
157 error_init:
158  io_can_ctrl_free((void *)ctrl);
159 error_alloc:
160  errno = errsv;
161  return NULL;
162 }
163 
164 void
166 {
167  if (ctrl) {
168  io_can_ctrl_fini(ctrl);
169  io_can_ctrl_free((void *)ctrl);
170  }
171 }
172 
173 const char *
175 {
176  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
177 
178  return impl->name;
179 }
180 
181 unsigned int
183 {
184  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
185 
186  return impl->index;
187 }
188 
189 int
191 {
192  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
193 
194  return impl->flags;
195 }
196 
197 static int
198 io_can_ctrl_impl_stop(io_can_ctrl_t *ctrl)
199 {
200  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
201 
202  int flags = 0;
203  return ifr_set_flags(impl->name, &flags, IFF_UP);
204 }
205 
206 static int
207 io_can_ctrl_impl_stopped(const io_can_ctrl_t *ctrl)
208 {
209  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
210 
211  int flags = ifr_get_flags(impl->name);
212  return flags == -1 ? -1 : !(flags & IFF_UP);
213 }
214 
215 static int
216 io_can_ctrl_impl_restart(io_can_ctrl_t *ctrl)
217 {
218  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
219 
220  int flags = IFF_UP;
221  return ifr_set_flags(impl->name, &flags, IFF_UP);
222 }
223 
224 static int
225 io_can_ctrl_impl_get_bitrate(
226  const io_can_ctrl_t *ctrl, int *pnominal, int *pdata)
227 {
228  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
229 
230  struct io_can_attr attr = IO_CAN_ATTR_INIT;
231  if (io_can_attr_get(&attr, impl->index) == -1)
232  return -1;
233 
234  if (pnominal)
235  *pnominal = attr.nominal;
236 
237  if (pdata)
238 #if LELY_NO_CANFD
239  *pdata = 0;
240 #else
241  *pdata = attr.data;
242 #endif
243 
244  return 0;
245 }
246 
247 static int
248 io_can_ctrl_impl_set_bitrate(io_can_ctrl_t *ctrl, int nominal, int data)
249 {
250  struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
251 
252  char buf[128];
253  struct rtattr *linkinfo = (struct rtattr *)buf;
254  *linkinfo = (struct rtattr){ .rta_len = RTA_LENGTH(0),
255  .rta_type = IFLA_LINKINFO };
256 
257  struct rtattr *info_kind = RTA_TAIL(linkinfo);
258  *info_kind = (struct rtattr){ .rta_len = RTA_LENGTH(strlen("can")),
259  .rta_type = IFLA_INFO_KIND };
260  strcpy(RTA_DATA(info_kind), "can");
261 
262  linkinfo->rta_len += RTA_ALIGN(info_kind->rta_len);
263 
264  struct rtattr *info_data = RTA_TAIL(info_kind);
265  *info_data = (struct rtattr){ .rta_len = RTA_LENGTH(0),
266  .rta_type = IFLA_INFO_DATA };
267  struct rtattr *rta = RTA_DATA(info_data);
268 
269  *rta = (struct rtattr){ .rta_len = RTA_LENGTH(
270  sizeof(struct can_bittiming)),
271  .rta_type = IFLA_CAN_BITTIMING };
272  *(struct can_bittiming *)RTA_DATA(rta) =
273  (struct can_bittiming){ .bitrate = nominal };
274  info_data->rta_len += RTA_ALIGN(rta->rta_len);
275  rta = RTA_TAIL(rta);
276 
277 #if LELY_NO_CANFD
278  (void)data;
279 #else
280  if (impl->flags & IO_CAN_BUS_FLAG_BRS) {
281  *rta = (struct rtattr){ .rta_len = RTA_LENGTH(sizeof(
282  struct can_bittiming)),
283  .rta_type = IFLA_CAN_DATA_BITTIMING };
284  *(struct can_bittiming *)RTA_DATA(rta) =
285  (struct can_bittiming){ .bitrate = data };
286  info_data->rta_len += RTA_ALIGN(rta->rta_len);
287  // rta = RTA_TAIL(rta); // Not needed if rta is not used below.
288  }
289 #endif
290 
291  linkinfo->rta_len += RTA_ALIGN(info_data->rta_len);
292 
293  int errsv = 0;
294 
295  int flags = 0;
296  if (ifr_set_flags(impl->name, &flags, IFF_UP) == -1) {
297  errsv = errno;
298  goto error_ifr_set_flags;
299  }
300 
301  struct rtnl_handle rth = { -1, 0, 0 };
302  if (rtnl_open(&rth) == -1) {
303  errsv = errno;
304  goto error_rtnl_open;
305  }
306 
307  // clang-format off
308  if (rtnl_send_newlink_request(&rth, AF_UNSPEC, ARPHRD_CAN, impl->index,
309  flags, linkinfo, RTA_ALIGN(linkinfo->rta_len)) == -1) {
310  // clang-format on
311  errsv = errno;
312  goto error_rtnl_send_newlink_request;
313  }
314 
315  if (rtnl_recv_ack(&rth) == -1) {
316  errsv = errno;
317  goto error_rtnl_recv_ack;
318  }
319 
320  rtnl_close(&rth);
321  return 0;
322 
323 error_rtnl_recv_ack:
324 error_rtnl_send_newlink_request:
325  rtnl_close(&rth);
326 error_rtnl_open:
327 error_ifr_set_flags:
328  errno = errsv;
329  return -1;
330 }
331 
332 static int
333 io_can_ctrl_impl_get_state(const io_can_ctrl_t *ctrl)
334 {
335  const struct io_can_ctrl_impl *impl = io_can_ctrl_impl_from_ctrl(ctrl);
336 
337  int flags = ifr_get_flags(impl->name);
338  if (flags == -1)
339  return -1;
340  if (!(flags & IFF_UP))
341  return CAN_STATE_STOPPED;
342 
343  struct io_can_attr attr = IO_CAN_ATTR_INIT;
344  if (io_can_attr_get(&attr, impl->index) == -1)
345  return -1;
346 
347  return attr.state;
348 }
349 
350 static inline struct io_can_ctrl_impl *
351 io_can_ctrl_impl_from_ctrl(const io_can_ctrl_t *ctrl)
352 {
353  assert(ctrl);
354 
355  return structof(ctrl, struct io_can_ctrl_impl, ctrl_vptr);
356 }
357 
358 #endif // __linux__
This is the internal header file of the ioctl network device configuration functions.
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:174
#define LELY_IO_CAN_TXLEN
The default SocketCAN transmit queue length (in number of CAN frames).
Definition: can_ctrl.c:46
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:182
void io_can_ctrl_destroy(io_can_ctrl_t *ctrl)
Destroys a CAN controller.
Definition: can_ctrl.c:165
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:190
This is the internal header file of the Linux-specific I/O declarations.
This is the internal header file of the SocketCAN rtnetlink attributes functions. ...
Bit Rate Switch support is enabled.
Definition: can.h:44
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:129
This header file is part of the I/O library; it contains the CAN bus declarations for Linux...
The device is stopped.
Definition: err.h:38
This is the internal header file of the rtnetlink network interface functions.
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
#define RTA_TAIL(rta)
Returns the address of the next attribute.
Definition: rtnl.h:35
const struct io_can_ctrl_vtbl *const io_can_ctrl_t
An abstract CAN controller.
Definition: can.h:56
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib.h> and defines any missing functionality.
This is the public header file of the utilities library.
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:138