diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index fc8840ae59..e6fc8ba111 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -5,6 +5,7 @@ #pragma once #include +#include #include namespace uavcan_linux @@ -16,9 +17,14 @@ class Exception : public std::runtime_error { const int errno_; + static std::string makeErrorString(const std::string& descr) + { + return descr + " [errno " + std::to_string(errno) + " \"" + std::strerror(errno) + "\"]"; + } + public: explicit Exception(const std::string& descr) - : std::runtime_error(descr) + : std::runtime_error(makeErrorString(descr)) , errno_(errno) { } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 1ebb0df045..cc08b6ae8c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -381,6 +381,7 @@ public: */ virtual ~SocketCanIface() { + UAVCAN_TRACE("SocketCAN", "SocketCanIface: Closing fd %d", fd_); (void)::close(fd_); } @@ -515,68 +516,91 @@ public: */ static int openSocket(const std::string& iface_name) { + errno = 0; + const int s = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); if (s < 0) { return s; } + + class RaiiCloser + { + int fd_; + public: + RaiiCloser(int filedesc) : fd_(filedesc) + { + assert(fd_ >= 0); + } + ~RaiiCloser() + { + if (fd_ >= 0) + { + UAVCAN_TRACE("SocketCAN", "RaiiCloser: Closing fd %d", fd_); + (void)::close(fd_); + } + } + void disarm() { fd_ = -1; } + } raii_closer(s); + // Detect the iface index auto ifr = ::ifreq(); if (iface_name.length() >= IFNAMSIZ) { - goto fail; + errno = ENAMETOOLONG; + return -1; } (void)std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); if (::ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) { - goto fail; + return -1; } - // Bind to a CAN iface + + // Bind to the specified CAN iface { auto addr = ::sockaddr_can(); addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; if (::bind(s, reinterpret_cast(&addr), sizeof(addr)) < 0) { - goto fail; + return -1; } } + // Configure { const int on = 1; // Timestamping if (::setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { - goto fail; + return -1; } // Socket loopback if (::setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { - goto fail; + return -1; } // Non-blocking if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0) { - goto fail; + return -1; } } + // Validate the resulting socket { - int error = 0; - ::socklen_t errlen = sizeof(error); - (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); - if (error != 0) + int socket_error = 0; + ::socklen_t errlen = sizeof(socket_error); + (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&socket_error), &errlen); + if (socket_error != 0) { - goto fail; - UAVCAN_TRACE("SocketCAN", "Socket error: iface='%s' errno=%d error=%d", - iface_name.c_str(), errno, error); + errno = socket_error; + return -1; } } - return s; - fail: - (void)::close(s); - return -1; + raii_closer.disarm(); + return s; } };