[Xenomai-core] [PATCH] add virtual CAN bus driver

[Jan Kiszka <jan.kiszka@domain.hid>]

[-- Attachment #1.1: Type: text/plain, Size: 412 bytes --]

For review:

Here comes a tiny but fairly useful new CAN driver: a virtual CAN bus
consisting of 2..n devices (see module parameter). Transmission takes
places from the sender to all other devices on the bus, just like in
real life.

This driver was already helpful for me while adapting CanFestival, a
LGPL CANopen library, to our CAN API -- inside qemu, i.e. without any
real hardware around.

Jan

[-- Warning: decoded text below may be mangled, UTF-8 assumed --]
[-- Attachment #1.2: rtcan_virt.patch --]
[-- Type: text/x-patch; name="rtcan_virt.patch", Size: 8585 bytes --]

Index: scripts/Modules.frag
===================================================================
--- scripts/Modules.frag	(Revision 1527)
+++ scripts/Modules.frag	(Arbeitskopie)
@@ -12,6 +12,7 @@ DRIVERS-$(CONFIG_XENO_DRIVERS_TIMERBENCH
 DRIVERS-$(CONFIG_XENO_DRIVERS_IRQBENCH) += drivers/xenomai/testing/xeno_irqbench.o
 DRIVERS-$(CONFIG_XENO_DRIVERS_SWITCHTEST) += drivers/xenomai/testing/xeno_switchtest.o
 DRIVERS-$(CONFIG_XENO_DRIVERS_RTCAN) += drivers/xenomai/can/xeno_rtcan.o
+DRIVERS-$(CONFIG_XENO_DRIVERS_RTCAN_VIRT) += drivers/xenomai/can/xeno_rtcan_virt.o
 DRIVERS-$(CONFIG_XENO_DRIVERS_RTCAN_MSCAN) += drivers/xenomai/can/mscan/xeno_rtcan_mscan.o
 DRIVERS-$(CONFIG_XENO_DRIVERS_RTCAN_SJA1000) += drivers/xenomai/can/sja1000/xeno_rtcan_sja1000.o
 DRIVERS-$(CONFIG_XENO_DRIVERS_RTCAN_SJA1000_PEAK_PCI) += drivers/xenomai/can/sja1000/xeno_rtcan_peak_pci.o
Index: ksrc/drivers/can/Kconfig
===================================================================
--- ksrc/drivers/can/Kconfig	(Revision 1527)
+++ ksrc/drivers/can/Kconfig	(Arbeitskopie)
@@ -26,6 +26,10 @@ config XENO_DRIVERS_RTCAN_MAX_RECEIVERS
        int "Maximum number of receive filters per device"
        default 16
 
+config XENO_DRIVERS_RTCAN_VIRT
+	depends on XENO_DRIVERS_RTCAN
+	tristate "Virtual tunnelling driver"
+
 source drivers/xenomai/can/mscan/Kconfig
 source drivers/xenomai/can/sja1000/Kconfig
 
Index: ksrc/drivers/can/rtcan_virt.c
===================================================================
--- ksrc/drivers/can/rtcan_virt.c	(Revision 0)
+++ ksrc/drivers/can/rtcan_virt.c	(Revision 0)
@@ -0,0 +1,195 @@
+/*
+ * Copyright (C) 2006 Jan Kiszka <jan.kiszka@domain.hid>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+
+#include <linux/module.h>
+#include <rtdm/rtdm_driver.h>
+#include <rtdm/rtcan.h>
+#include "rtcan_dev.h"
+#include "rtcan_raw.h"
+
+#define RTCAN_DEV_NAME          "rtcan%d"
+#define RTCAN_DRV_NAME          "VIRT"
+#define RTCAN_MAX_VIRT_DEVS     8
+
+#define VIRT_TX_BUFS            1
+
+static char *virt_ctlr_name  = "<virtual>";
+static char *virt_board_name = "<virtual>";
+
+MODULE_AUTHOR("Jan Kiszka <jan.kiszka@domain.hid>");
+MODULE_DESCRIPTION("Virtual RT-Socket-CAN driver");
+MODULE_LICENSE("GPL");
+
+static unsigned int devices = 2;
+
+module_param(devices, uint, 0400);
+MODULE_PARM_DESC(devices, "Number of devices on the virtual bus");
+
+static struct rtcan_device *rtcan_virt_devs[RTCAN_MAX_VIRT_DEVS];
+
+
+static int rtcan_virt_start_xmit(struct rtcan_device *tx_dev,
+                                 can_frame_t *tx_frame)
+{
+	int i;
+	struct rtcan_device *rx_dev;
+	struct rtcan_skb skb;
+	struct rtcan_rb_frame *rx_frame = &skb.rb_frame;
+	nanosecs_abs_t timestamp = rtdm_clock_read();
+	rtdm_lockctx_t lock_ctx;
+
+	/* we can transmit immediately again */
+	rtdm_sem_up(&tx_dev->tx_sem);
+
+	skb.rb_frame_size = EMPTY_RB_FRAME_SIZE;
+
+	rx_frame->can_dlc = tx_frame->can_dlc;
+	rx_frame->can_id  = tx_frame->can_id;
+
+	if (!(tx_frame->can_id & CAN_RTR_FLAG)) {
+		memcpy(rx_frame->data, tx_frame->data, tx_frame->can_dlc);
+		skb.rb_frame_size += tx_frame->can_dlc;
+	}
+
+	/* Copy timestamp to skb */
+	memcpy((void *)&skb.rb_frame + skb.rb_frame_size,
+	       &timestamp, TIMESTAMP_SIZE);
+
+	rtdm_lock_get_irqsave(&rtcan_recv_list_lock, lock_ctx);
+	rtdm_lock_get(&rtcan_socket_lock);
+
+	/* Deliver to all other devices on the virtual bus */
+	for (i = 0; i < devices; i++) {
+		rx_dev = rtcan_virt_devs[i];
+		if (tx_dev != rx_dev && rx_dev->state == CAN_STATE_ACTIVE) {
+			rx_frame->can_ifindex = rx_dev->ifindex;
+			rtcan_rcv(rx_dev, &skb);
+		}
+	}
+
+	rtdm_lock_put(&rtcan_socket_lock);
+	rtdm_lock_put_irqrestore(&rtcan_recv_list_lock, lock_ctx);
+
+	return 0;
+}
+
+
+static int rtcan_virt_set_mode(struct rtcan_device *dev, can_mode_t mode,
+                               rtdm_lockctx_t *lock_ctx)
+{
+	int err = 0;
+
+	switch (mode) {
+	case CAN_MODE_STOP:
+		dev->state = CAN_STATE_STOPPED;
+		/* Wake up waiting senders */
+		rtdm_sem_destroy(&dev->tx_sem);
+		break;
+
+	case CAN_MODE_START:
+		if (dev->state == CAN_STATE_STOPPED)
+			rtdm_sem_init(&dev->tx_sem, VIRT_TX_BUFS);
+		dev->state = CAN_STATE_ACTIVE;
+		break;
+
+	default:
+		err = -EOPNOTSUPP;
+	}
+
+	return err;
+}
+
+
+static int __init rtcan_virt_init_one(int idx)
+{
+	struct rtcan_device *dev;
+	int err;
+
+	if ((dev = rtcan_dev_alloc(0, 0)) == NULL)
+		return -ENOMEM;
+
+	dev->ctrl_name = virt_ctlr_name;
+	dev->board_name = virt_board_name;
+
+	dev->state = CAN_STATE_STOPPED;
+
+	strncpy(dev->name, RTCAN_DEV_NAME, IFNAMSIZ);
+
+	dev->hard_start_xmit = rtcan_virt_start_xmit;
+	dev->do_set_mode = rtcan_virt_set_mode;
+
+	/* Register RTDM device */
+	err = rtcan_dev_register(dev);
+	if (err) {
+		printk(KERN_ERR "ERROR while trying to register RTCAN device!\n");
+		goto error_out;
+	}
+
+	/* Remember initialized devices */
+	rtcan_virt_devs[idx] = dev;
+
+	printk("%s: %s driver loaded\n", dev->name, RTCAN_DRV_NAME);
+
+	return 0;
+
+ error_out:
+	rtcan_dev_free(dev);
+	return err;
+}
+
+
+/** Init module */
+static int __init rtcan_virt_init(void)
+{
+	int i, err = 0;
+
+	for (i = 0; i < devices; i++) {
+		err = rtcan_virt_init_one(i);
+		if (err)
+			while (--i >= 0) {
+				struct rtcan_device *dev = rtcan_virt_devs[i];
+
+				rtcan_dev_unregister(dev);
+				rtcan_dev_free(dev);
+			}
+	}
+
+	return err;
+}
+
+
+/** Cleanup module */
+static void __exit rtcan_virt_exit(void)
+{
+	int i;
+	struct rtcan_device *dev;
+
+	for (i = 0; i < devices; i++) {
+		dev = rtcan_virt_devs[i];
+
+		printk("Unloading %s device %s\n", RTCAN_DRV_NAME, dev->name);
+
+		rtcan_dev_unregister(dev);
+		rtcan_dev_free(dev);
+	}
+}
+
+module_init(rtcan_virt_init);
+module_exit(rtcan_virt_exit);
Index: ksrc/drivers/can/Config.in
===================================================================
--- ksrc/drivers/can/Config.in	(Revision 1527)
+++ ksrc/drivers/can/Config.in	(Arbeitskopie)
@@ -13,6 +13,8 @@ if [ "$CONFIG_XENO_DRIVERS_RTCAN" != "n"
    int 'Maximum number of devices' CONFIG_XENO_DRIVERS_RTCAN_MAX_DEVICES 4
    int 'Maximum number of receive filters per device' CONFIG_XENO_DRIVERS_RTCAN_MAX_RECEIVERS 16
 
+   dep_tristate 'Virtual tunnelling driver' CONFIG_XENO_DRIVERS_RTCAN_VIRT $CONFIG_XENO_DRIVERS_RTCAN
+
    source drivers/xenomai/can/mscan/Config.in
    source drivers/xenomai/can/sja1000/Config.in
 
Index: ksrc/drivers/can/Makefile
===================================================================
--- ksrc/drivers/can/Makefile	(Revision 1527)
+++ ksrc/drivers/can/Makefile	(Arbeitskopie)
@@ -5,8 +5,10 @@ ifeq ($(PATCHLEVEL),6)
 EXTRA_CFLAGS += -Iinclude/xenomai -Idrivers/xenomai/can
 
 obj-$(CONFIG_XENO_DRIVERS_RTCAN) += xeno_rtcan.o mscan/ sja1000/
+obj-$(CONFIG_XENO_DRIVERS_RTCAN_VIRT) += xeno_rtcan_virt.o
 
 xeno_rtcan-y := rtcan_dev.o rtcan_socket.o rtcan_module.o rtcan_raw.o rtcan_raw_dev.o rtcan_raw_filter.o
+xeno_rtcan_virt-y := rtcan_virt.o
 
 else
 
@@ -19,11 +21,13 @@ subdir-$(CONFIG_XENO_DRIVERS_RTCAN_SJA10
 
 O_TARGET := built-in.o
 
-obj-$(CONFIG_XENO_DRIVERS_RTCAN) := xeno_rtcan.o
+obj-$(CONFIG_XENO_DRIVERS_RTCAN) += xeno_rtcan.o
+obj-$(CONFIG_XENO_DRIVERS_RTCAN_VIRT) += xeno_rtcan_virt.o
 
 list-multi := xeno_rtcan.o
 
 xeno_rtcan-objs := rtcan_dev.o rtcan_socket.o rtcan_module.o rtcan_raw.o rtcan_raw_dev.o rtcan_raw_filter.o
+xeno_rtcan_virt-objs := rtcan_virt.o
 
 export-objs := $(xeno_rtcan-objs)
 
@@ -34,4 +38,7 @@ include $(TOPDIR)/Rules.make
 xeno_rtcan.o: $(xeno_rtcan-objs)
 	$(LD) -r -o $@ $(xeno_rtcan-objs)
 
+xeno_rtcan_virt.o: $(xeno_rtcan_virt-objs)
+	$(LD) -r -o $@ $(xeno_rtcan_virt-objs)
+
 endif

[-- Attachment #2: OpenPGP digital signature --]
[-- Type: application/pgp-signature, Size: 249 bytes --]