287 lines
8.3 KiB
C
287 lines
8.3 KiB
C
|
/*
|
||
|
|
* 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 "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;
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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 int can_host_socketcan_can_receive(CanBusClientState *client)
|
||
|
|
{
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
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);
|