46 #include <linux/can/raw.h>
47 #include <linux/sockios.h>
48 #include <sys/ioctl.h>
50 #include "../posix/fd.h"
55 #ifndef LELY_IO_CAN_RXLEN
56 #define LELY_IO_CAN_RXLEN 1024
62 struct can_frame frame;
64 struct canfd_frame frame;
70 static int io_can_fd_set_default(
int fd);
72 static int io_can_fd_read(
int fd,
struct can_frame *frame,
size_t *pnbytes,
73 int *pflags,
struct timespec *tp,
int timeout);
75 static int io_can_fd_read(
int fd,
struct canfd_frame *frame,
size_t *pnbytes,
76 int *pflags,
struct timespec *tp,
int timeout);
79 static int io_can_fd_write(
int fd,
const struct can_frame *frame,
size_t nbytes,
82 static int io_can_fd_write(
int fd,
const struct canfd_frame *frame,
83 size_t nbytes,
int timeout);
85 static int io_can_fd_write_msg(
int fd,
const struct can_msg *msg,
int timeout);
89 static size_t io_can_chan_impl_dev_cancel(
io_dev_t *dev,
struct ev_task *task);
90 static size_t io_can_chan_impl_dev_abort(
io_dev_t *dev,
struct ev_task *task);
93 static const struct io_dev_vtbl io_can_chan_impl_dev_vtbl = {
94 &io_can_chan_impl_dev_get_ctx,
95 &io_can_chan_impl_dev_get_exec,
96 &io_can_chan_impl_dev_cancel,
97 &io_can_chan_impl_dev_abort
102 static int io_can_chan_impl_get_flags(
const io_can_chan_t *chan);
104 struct can_err *err,
struct timespec *tp,
int timeout);
105 static void io_can_chan_impl_submit_read(
107 static int io_can_chan_impl_write(
109 static void io_can_chan_impl_submit_write(
114 &io_can_chan_impl_get_dev,
115 &io_can_chan_impl_get_flags,
116 &io_can_chan_impl_read,
117 &io_can_chan_impl_submit_read,
118 &io_can_chan_impl_write,
119 &io_can_chan_impl_submit_write
123 static void io_can_chan_impl_svc_shutdown(
struct io_svc *svc);
126 static const struct io_svc_vtbl io_can_chan_impl_svc_vtbl = {
128 &io_can_chan_impl_svc_shutdown
159 pthread_mutex_t
c_mtx;
197 static void io_can_chan_impl_watch_func(
199 static void io_can_chan_impl_rxbuf_task_func(
struct ev_task *task);
200 static void io_can_chan_impl_read_task_func(
struct ev_task *task);
201 static void io_can_chan_impl_write_task_func(
struct ev_task *task);
210 static void io_can_chan_impl_c_signal(
struct spscring *ring,
void *arg);
217 struct sllist *queue,
int *pwouldblock);
221 static size_t io_can_chan_impl_do_abort_tasks(
struct io_can_chan_impl *impl);
223 static int io_can_chan_impl_set_fd(
227 io_can_chan_alloc(
void)
235 io_can_chan_free(
void *ptr)
238 free(io_can_chan_impl_from_chan(ptr));
253 impl->
dev_vptr = &io_can_chan_impl_dev_vtbl;
254 impl->
chan_vptr = &io_can_chan_impl_vtbl;
264 &io_can_chan_impl_watch_func);
267 impl->
exec, &io_can_chan_impl_rxbuf_task_func);
269 impl->
exec, &io_can_chan_impl_read_task_func);
271 impl->
exec, &io_can_chan_impl_write_task_func);
274 if ((errsv = pthread_mutex_init(&impl->
c_mtx, NULL)))
275 goto error_init_c_mtx;
282 goto error_alloc_rxbuf;
286 if ((errsv = pthread_mutex_init(&impl->
mtx, NULL)))
316 pthread_mutex_destroy(&impl->
c_mtx);
331 io_can_chan_impl_svc_shutdown(&impl->
svc);
335 pthread_mutex_lock(&impl->
c_mtx);
337 pthread_mutex_unlock(&impl->
c_mtx);
340 pthread_mutex_lock(&impl->
mtx);
345 if (io_can_chan_impl_do_abort_tasks(impl))
347 pthread_mutex_unlock(&impl->
mtx);
351 "io_can_chan_fini() invoked with pending operations");
354 pthread_mutex_lock(&impl->
mtx);
356 pthread_mutex_unlock(&impl->
mtx);
360 if (impl->
fd != -1) {
367 pthread_mutex_destroy(&impl->
mtx);
373 pthread_mutex_destroy(&impl->
c_mtx);
398 io_can_chan_free((
void *)chan);
408 io_can_chan_fini(chan);
409 io_can_chan_free((
void *)chan);
419 pthread_mutex_lock((pthread_mutex_t *)&impl->
mtx);
423 pthread_mutex_unlock((pthread_mutex_t *)&impl->
mtx);
440 int fd = socket(AF_CAN, SOCK_RAW | SOCK_CLOEXEC, CAN_RAW);
446 struct sockaddr_can addr = { .can_family = AF_CAN,
449 if (bind(fd, (
struct sockaddr *)&addr,
sizeof(addr)) == -1) {
455 can_err_mask_t optval = CAN_ERR_MASK;
457 if (setsockopt(fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &optval,
458 sizeof(optval)) == -1) {
461 goto error_setsockopt;
469 if (setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &optval,
470 sizeof(optval)) == -1) {
473 goto error_setsockopt;
478 if (io_can_fd_set_default(fd) == -1) {
480 goto error_set_default;
483 fd = io_can_chan_impl_set_fd(impl, fd, flags);
503 struct sockaddr_can addr = { .can_family = AF_UNSPEC };
504 socklen_t addrlen =
sizeof(addr);
505 if (getsockname(fd, (
struct sockaddr *)&addr, &addrlen) == -1)
507 if (addrlen <
sizeof(addr) || addr.can_family != AF_CAN) {
511 unsigned int ifindex = addr.can_ifindex;
514 if (io_can_attr_get(&attr, ifindex) == -1)
516 int flags = attr.flags;
519 can_err_mask_t optval = 0;
520 socklen_t optlen =
sizeof(optval);
522 if (getsockopt(fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &optval,
526 if (optval & CAN_ERR_MASK)
536 socklen_t optlen =
sizeof(optval);
538 if (!getsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &optval,
546 if (io_can_fd_set_default(fd) == -1)
549 fd = io_can_chan_impl_set_fd(impl, fd, flags);
561 return io_can_chan_impl_set_fd(impl, -1, 0);
574 return fd != -1 ? close(
fd) : 0;
578 io_can_fd_set_default(
int fd)
585 if (setsockopt(
fd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &optval,
586 sizeof(optval)) == -1)
594 if (setsockopt(
fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &optval,
595 sizeof(optval)) == -1)
604 if (setsockopt(
fd, SOL_SOCKET, SO_SNDBUF, &optval,
sizeof(optval))
614 io_can_fd_read(
int fd,
struct can_frame *frame,
size_t *pnbytes,
int *pflags,
615 struct timespec *tp,
int timeout)
617 io_can_fd_read(
int fd,
struct canfd_frame *frame,
size_t *pnbytes,
int *pflags,
618 struct timespec *tp,
int timeout)
621 struct iovec iov = { .iov_base = (
void *)frame,
622 .iov_len =
sizeof(*frame) };
623 struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 };
631 if (result == CAN_MTU)
633 if (result == CAN_MTU || result == CANFD_MTU)
644 *pflags = msg.msg_flags;
647 if (msg.msg_flags & MSG_CONFIRM) {
649 *tp = (
struct timespec){ 0, 0 };
651 struct timeval tv = { 0, 0 };
652 if (ioctl(fd, SIOCGSTAMP, &tv) == -1)
654 tp->tv_sec = tv.tv_sec;
655 tp->tv_nsec = tv.tv_usec * 1000;
664 io_can_fd_write(
int fd,
const struct can_frame *frame,
size_t nbytes,
667 io_can_fd_write(
int fd,
const struct canfd_frame *frame,
size_t nbytes,
671 struct iovec iov = { .iov_base = (
void *)frame, .iov_len = nbytes };
672 struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 };
678 io_can_fd_write_msg(
int fd,
const struct can_msg *msg,
int timeout)
690 frame.nbytes = CANFD_MTU;
698 frame.nbytes = CAN_MTU;
703 return io_can_fd_write(fd, &frame.frame, frame.nbytes, timeout);
707 io_can_chan_impl_dev_get_ctx(
const io_dev_t *dev)
715 io_can_chan_impl_dev_get_exec(
const io_dev_t *dev)
729 struct sllist read_queue, write_queue, confirm_queue;
735 pthread_mutex_lock(&impl->
mtx);
737 io_can_chan_impl_do_pop(
738 impl, &read_queue, &write_queue, &confirm_queue, task);
745 pthread_mutex_unlock(&impl->
mtx);
748 size_t nread = io_can_chan_read_queue_post(&read_queue, -1, ECANCELED);
749 n = n < SIZE_MAX - nread ? n + nread : SIZE_MAX;
750 size_t nwrite = io_can_chan_write_queue_post(&write_queue, ECANCELED);
751 n = n < SIZE_MAX - nwrite ? n + nwrite : SIZE_MAX;
753 io_can_chan_write_queue_post(&confirm_queue, ECANCELED);
754 n = n < SIZE_MAX - nconfirm ? n + nconfirm : SIZE_MAX;
768 pthread_mutex_lock(&impl->
mtx);
770 io_can_chan_impl_do_pop(impl, &queue, &queue, &queue, task);
772 pthread_mutex_unlock(&impl->
mtx);
791 pthread_mutex_lock((pthread_mutex_t *)&impl->
mtx);
795 pthread_mutex_unlock((pthread_mutex_t *)&impl->
mtx);
802 struct can_err *err,
struct timespec *tp,
int timeout)
809 pthread_mutex_lock(&impl->
c_mtx);
817 frame = &impl->
rxbuf[i];
821 pthread_mutex_unlock(&impl->
c_mtx);
822 pthread_mutex_lock(&impl->
mtx);
827 pthread_mutex_unlock(&impl->
mtx);
831 if (io_can_fd_read(fd, &frame->frame, &frame->nbytes, &flags,
832 &frame->ts, timeout) < 0)
836 if (!(flags & MSG_CONFIRM))
839 void *src = &frame->frame;
842 if (frame->nbytes == CANFD_MTU)
851 pthread_mutex_lock(&impl->
mtx);
853 io_can_chan_impl_do_confirm(impl, &queue, &msg);
855 pthread_mutex_unlock(&impl->
mtx);
863 pthread_mutex_lock(&impl->
c_mtx);
867 void *data = &frame->frame;
868 int is_err = can_frame2can_err(data, err);
869 if (!is_err && msg) {
871 if (frame->nbytes == CANFD_MTU)
883 pthread_mutex_unlock(&impl->
c_mtx);
887 return is_err == -1 ? -1 : !is_err;
903 pthread_mutex_lock(&impl->
mtx);
907 pthread_mutex_unlock(&impl->
mtx);
909 io_can_chan_read_post(read, -1, ECANCELED);
917 pthread_mutex_unlock(&impl->
mtx);
926 io_can_chan_impl_write(
940 pthread_mutex_lock(&impl->
mtx);
945 pthread_mutex_unlock(&impl->
mtx);
953 pthread_mutex_unlock(&impl->
mtx);
956 return io_can_fd_write_msg(
fd, msg, timeout);
960 io_can_chan_impl_submit_write(
982 pthread_mutex_lock(&impl->
mtx);
986 pthread_mutex_unlock(&impl->
mtx);
988 io_can_chan_write_post(write, ECANCELED);
990 }
else if ((flags & impl->
flags) != flags) {
992 pthread_mutex_unlock(&impl->
mtx);
994 io_can_chan_write_post(write, EINVAL);
1002 #if !LELY_NO_THREADS
1003 pthread_mutex_unlock(&impl->
mtx);
1012 io_can_chan_impl_svc_shutdown(
struct io_svc *svc)
1017 #if !LELY_NO_THREADS
1018 pthread_mutex_lock(&impl->
mtx);
1032 io_can_chan_impl_do_abort_tasks(impl);
1034 #if !LELY_NO_THREADS
1035 pthread_mutex_unlock(&impl->
mtx);
1040 io_can_chan_impl_dev_cancel(dev, NULL);
1050 #if !LELY_NO_THREADS
1051 pthread_mutex_lock(&impl->
mtx);
1060 &impl->
watch) == -1) {
1083 #if !LELY_NO_THREADS
1084 pthread_mutex_unlock(&impl->
mtx);
1094 io_can_chan_impl_rxbuf_task_func(
struct ev_task *task)
1109 #if !LELY_NO_THREADS
1110 pthread_mutex_lock(&impl->
mtx);
1121 #if !LELY_NO_THREADS
1122 pthread_mutex_unlock(&impl->
mtx);
1131 frame = &impl->
rxbuf[i];
1135 result = io_can_fd_read(fd, &frame->frame, &frame->nbytes,
1138 errc = !result ? 0 : errno;
1139 wouldblock = errc == EAGAIN || errc == EWOULDBLOCK;
1144 if (!result && (
flags & MSG_CONFIRM)) {
1145 void *src = &frame->frame;
1147 if (frame->nbytes == CANFD_MTU)
1155 if (!result && !(
flags & MSG_CONFIRM))
1158 #if !LELY_NO_THREADS
1159 pthread_mutex_lock(&impl->
mtx);
1162 if (!result &&
flags & MSG_CONFIRM)
1163 io_can_chan_impl_do_confirm(impl, &queue, &msg);
1167 if (!impl->
poll || result < 0)
1171 if (result < 0 && !wouldblock) {
1172 io_can_chan_impl_do_read(impl, &queue, NULL);
1178 read->
r.
errc = errc;
1189 if (post_rxbuf && impl->
poll && wouldblock) {
1202 #if !LELY_NO_THREADS
1203 pthread_mutex_unlock(&impl->
mtx);
1215 io_can_chan_impl_read_task_func(
struct ev_task *
task)
1224 #if !LELY_NO_THREADS
1225 pthread_mutex_lock(&impl->
mtx);
1229 io_can_chan_impl_do_read(impl, &queue, &wouldblock);
1234 if (post_read && wouldblock) {
1235 #if !LELY_NO_THREADS
1236 pthread_mutex_lock(&impl->
c_mtx);
1241 io_can_chan_impl_c_signal, impl);
1242 #if !LELY_NO_THREADS
1243 pthread_mutex_unlock(&impl->
c_mtx);
1255 #if !LELY_NO_THREADS
1256 pthread_mutex_unlock(&impl->
mtx);
1269 io_can_chan_impl_write_task_func(
struct ev_task *task)
1279 #if !LELY_NO_THREADS
1280 pthread_mutex_lock(&impl->
mtx);
1287 #if !LELY_NO_THREADS
1288 pthread_mutex_unlock(&impl->
mtx);
1292 int result = io_can_fd_write_msg(fd, write->
msg,
1294 int errc = !result ? 0 : errno;
1295 wouldblock =
errc == EAGAIN ||
errc == EWOULDBLOCK;
1296 if (!wouldblock &&
errc)
1298 io_can_chan_write_post(write,
errc);
1299 #if !LELY_NO_THREADS
1300 pthread_mutex_lock(&impl->
mtx);
1317 if (!impl->
poll || wouldblock)
1333 if (post_write && impl->
poll && wouldblock) {
1346 #if !LELY_NO_THREADS
1347 pthread_mutex_unlock(&impl->
mtx);
1350 if (
task && wouldblock)
1353 io_can_chan_write_post(
1366 io_can_chan_impl_from_dev(
const io_dev_t *dev)
1382 io_can_chan_impl_from_svc(
const struct io_svc *
svc)
1390 io_can_chan_impl_c_signal(
struct spscring *ring,
void *arg)
1396 #if !LELY_NO_THREADS
1397 pthread_mutex_lock(&impl->
mtx);
1403 #if !LELY_NO_THREADS
1404 pthread_mutex_unlock(&impl->
mtx);
1450 #if !LELY_NO_THREADS
1451 pthread_mutex_lock(&impl->
c_mtx);
1458 #if !LELY_NO_THREADS
1459 pthread_mutex_unlock(&impl->
c_mtx);
1467 void *data = &frame->frame;
1468 int is_err = can_frame2can_err(data, read->
err);
1469 if (!is_err && read->
msg) {
1471 if (frame->nbytes == CANFD_MTU)
1478 *read->
tp = frame->ts;
1481 #if !LELY_NO_THREADS
1482 pthread_mutex_unlock(&impl->
c_mtx);
1493 *pwouldblock = wouldblock;
1526 if (&
task->_node == node) {
1576 assert(!(flags & ~IO_CAN_BUS_FLAG_MASK));
1578 struct sllist read_queue, write_queue, confirm_queue;
1583 #if !LELY_NO_THREADS
1584 pthread_mutex_lock(&impl->
mtx);
1593 #if !LELY_NO_THREADS
1594 pthread_mutex_lock(&impl->
c_mtx);
1598 size_t n = SIZE_MAX;
1602 #if !LELY_NO_THREADS
1603 pthread_mutex_unlock(&impl->
c_mtx);
1610 impl->
flags = flags;
1620 #if !LELY_NO_THREADS
1621 pthread_mutex_unlock(&impl->
mtx);
1624 io_can_chan_read_queue_post(&read_queue, -1, ECANCELED);
1625 io_can_chan_write_queue_post(&write_queue, ECANCELED);
1626 io_can_chan_write_queue_post(&confirm_queue, ECANCELED);