| /* |
| * CAN c support to connect to the Linux host SocketCAN interfaces |
| * |
| * Copyright (c) 2013-2014 Jin Yang |
| * Copyright (c) 2014-2018 Pavel Pisa |
| * |
| * Initial development supported by Google GSoC 2013 from RTEMS project slot |
| * |
| * Permission is hereby granted, free of charge, to any person obtaining a copy |
| * of this software and associated documentation files (the "Software"), to deal |
| * in the Software without restriction, including without limitation the rights |
| * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| * copies of the Software, and to permit persons to whom the Software is |
| * furnished to do so, subject to the following conditions: |
| * |
| * The above copyright notice and this permission notice shall be included in |
| * all copies or substantial portions of the Software. |
| * |
| * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL |
| * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| * THE SOFTWARE. |
| */ |
| |
| #include "qemu/osdep.h" |
| #include "qemu/log.h" |
| #include "qemu/main-loop.h" |
| #include "qemu/module.h" |
| #include "qapi/error.h" |
| #include "chardev/char.h" |
| #include "qemu/sockets.h" |
| #include "qemu/error-report.h" |
| #include "net/can_emu.h" |
| #include "net/can_host.h" |
| |
| #include <sys/ioctl.h> |
| #include <net/if.h> |
| #include <linux/can.h> |
| #include <linux/can/raw.h> |
| |
| #ifndef DEBUG_CAN |
| #define DEBUG_CAN 0 |
| #endif /*DEBUG_CAN*/ |
| |
| #define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan" |
| #define CAN_HOST_SOCKETCAN(obj) \ |
| OBJECT_CHECK(CanHostSocketCAN, (obj), TYPE_CAN_HOST_SOCKETCAN) |
| |
| #define CAN_READ_BUF_LEN 5 |
| typedef struct CanHostSocketCAN { |
| CanHostState parent; |
| char *ifname; |
| |
| qemu_can_filter *rfilter; |
| int rfilter_num; |
| can_err_mask_t err_mask; |
| |
| qemu_can_frame buf[CAN_READ_BUF_LEN]; |
| int bufcnt; |
| int bufptr; |
| |
| int fd; |
| } CanHostSocketCAN; |
| |
| /* Check that QEMU and Linux kernel flags encoding and structure matches */ |
| QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG); |
| QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG); |
| QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG); |
| QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER); |
| QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data) |
| != offsetof(struct can_frame, data)); |
| |
| static void can_host_socketcan_display_msg(struct qemu_can_frame *msg) |
| { |
| int i; |
| FILE *logfile = qemu_log_lock(); |
| qemu_log("[cansocketcan]: %03X [%01d] %s %s", |
| msg->can_id & QEMU_CAN_EFF_MASK, |
| msg->can_dlc, |
| msg->can_id & QEMU_CAN_EFF_FLAG ? "EFF" : "SFF", |
| msg->can_id & QEMU_CAN_RTR_FLAG ? "RTR" : "DAT"); |
| |
| for (i = 0; i < msg->can_dlc; i++) { |
| qemu_log(" %02X", msg->data[i]); |
| } |
| qemu_log("\n"); |
| qemu_log_flush(); |
| qemu_log_unlock(logfile); |
| } |
| |
| static void can_host_socketcan_read(void *opaque) |
| { |
| CanHostSocketCAN *c = opaque; |
| CanHostState *ch = CAN_HOST(c); |
| |
| /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */ |
| c->bufcnt = read(c->fd, c->buf, sizeof(qemu_can_frame)); |
| if (c->bufcnt < 0) { |
| warn_report("CAN bus host read failed (%s)", strerror(errno)); |
| return; |
| } |
| |
| can_bus_client_send(&ch->bus_client, c->buf, 1); |
| |
| if (DEBUG_CAN) { |
| can_host_socketcan_display_msg(c->buf); |
| } |
| } |
| |
| static bool can_host_socketcan_can_receive(CanBusClientState *client) |
| { |
| return true; |
| } |
| |
| static ssize_t can_host_socketcan_receive(CanBusClientState *client, |
| const qemu_can_frame *frames, size_t frames_cnt) |
| { |
| CanHostState *ch = container_of(client, CanHostState, bus_client); |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| |
| size_t len = sizeof(qemu_can_frame); |
| int res; |
| |
| if (c->fd < 0) { |
| return -1; |
| } |
| |
| res = write(c->fd, frames, len); |
| |
| if (!res) { |
| warn_report("[cansocketcan]: write message to host returns zero"); |
| return -1; |
| } |
| |
| if (res != len) { |
| if (res < 0) { |
| warn_report("[cansocketcan]: write to host failed (%s)", |
| strerror(errno)); |
| } else { |
| warn_report("[cansocketcan]: write to host truncated"); |
| } |
| return -1; |
| } |
| |
| return 1; |
| } |
| |
| static void can_host_socketcan_disconnect(CanHostState *ch) |
| { |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| |
| if (c->fd >= 0) { |
| qemu_set_fd_handler(c->fd, NULL, NULL, c); |
| close(c->fd); |
| c->fd = -1; |
| } |
| |
| g_free(c->rfilter); |
| c->rfilter = NULL; |
| c->rfilter_num = 0; |
| } |
| |
| static CanBusClientInfo can_host_socketcan_bus_client_info = { |
| .can_receive = can_host_socketcan_can_receive, |
| .receive = can_host_socketcan_receive, |
| }; |
| |
| static void can_host_socketcan_connect(CanHostState *ch, Error **errp) |
| { |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| int s; /* can raw socket */ |
| struct sockaddr_can addr; |
| struct ifreq ifr; |
| |
| /* open socket */ |
| s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW); |
| if (s < 0) { |
| error_setg_errno(errp, errno, "failed to create CAN_RAW socket"); |
| return; |
| } |
| |
| addr.can_family = AF_CAN; |
| memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name)); |
| strcpy(ifr.ifr_name, c->ifname); |
| if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) { |
| error_setg_errno(errp, errno, |
| "SocketCAN host interface %s not available", c->ifname); |
| goto fail; |
| } |
| addr.can_ifindex = ifr.ifr_ifindex; |
| |
| c->err_mask = 0xffffffff; /* Receive error frame. */ |
| setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, |
| &c->err_mask, sizeof(c->err_mask)); |
| |
| c->rfilter_num = 1; |
| c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num); |
| |
| /* Receive all data frame. If |= CAN_INV_FILTER no data. */ |
| c->rfilter[0].can_id = 0; |
| c->rfilter[0].can_mask = 0; |
| c->rfilter[0].can_mask &= ~CAN_ERR_FLAG; |
| |
| setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter, |
| c->rfilter_num * sizeof(struct qemu_can_filter)); |
| |
| if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { |
| error_setg_errno(errp, errno, "failed to bind to host interface %s", |
| c->ifname); |
| goto fail; |
| } |
| |
| c->fd = s; |
| ch->bus_client.info = &can_host_socketcan_bus_client_info; |
| qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c); |
| return; |
| |
| fail: |
| close(s); |
| g_free(c->rfilter); |
| c->rfilter = NULL; |
| c->rfilter_num = 0; |
| } |
| |
| static char *can_host_socketcan_get_if(Object *obj, Error **errp) |
| { |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| |
| return g_strdup(c->ifname); |
| } |
| |
| static void can_host_socketcan_set_if(Object *obj, const char *value, Error **errp) |
| { |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| struct ifreq ifr; |
| |
| if (strlen(value) >= sizeof(ifr.ifr_name)) { |
| error_setg(errp, "CAN interface name longer than %zd characters", |
| sizeof(ifr.ifr_name) - 1); |
| return; |
| } |
| |
| if (c->fd != -1) { |
| error_setg(errp, "CAN interface already connected"); |
| return; |
| } |
| |
| g_free(c->ifname); |
| c->ifname = g_strdup(value); |
| } |
| |
| static void can_host_socketcan_instance_init(Object *obj) |
| { |
| CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| |
| c->fd = -1; |
| } |
| |
| static void can_host_socketcan_class_init(ObjectClass *klass, |
| void *class_data G_GNUC_UNUSED) |
| { |
| CanHostClass *chc = CAN_HOST_CLASS(klass); |
| |
| object_class_property_add_str(klass, "if", |
| can_host_socketcan_get_if, |
| can_host_socketcan_set_if, |
| &error_abort); |
| chc->connect = can_host_socketcan_connect; |
| chc->disconnect = can_host_socketcan_disconnect; |
| } |
| |
| static const TypeInfo can_host_socketcan_info = { |
| .parent = TYPE_CAN_HOST, |
| .name = TYPE_CAN_HOST_SOCKETCAN, |
| .instance_size = sizeof(CanHostSocketCAN), |
| .instance_init = can_host_socketcan_instance_init, |
| .class_init = can_host_socketcan_class_init, |
| }; |
| |
| static void can_host_register_types(void) |
| { |
| type_register_static(&can_host_socketcan_info); |
| } |
| |
| type_init(can_host_register_types); |