This patch adds a class driver for sending and receiving raw Ethernet packets over a character device interface.
Signed-off-by: Richard Cochran <richard.coch...@omicron.at> --- include/rtdm/rtpacket.h | 99 ++++++++++++ ksrc/drivers/testing/Kconfig | 8 + ksrc/drivers/testing/Makefile | 3 + ksrc/drivers/testing/packet.c | 352 +++++++++++++++++++++++++++++++++++++++++ 4 files changed, 462 insertions(+), 0 deletions(-) create mode 100644 include/rtdm/rtpacket.h create mode 100644 ksrc/drivers/testing/packet.c diff --git a/include/rtdm/rtpacket.h b/include/rtdm/rtpacket.h new file mode 100644 index 0000000..87930de --- /dev/null +++ b/include/rtdm/rtpacket.h @@ -0,0 +1,99 @@ +/* + * Xenomai Packet Interface - class driver for raw Ethernet packets + * + * Copyright (C) 2011 OMICRON electronics GmbH + * + * 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., 675 Mass Ave, Cambridge, MA 02139, USA. + */ +#ifndef _RTPACKET_H +#define _RTPACKET_H + +#include <rtdm/rtdm.h> + +#define RTPACKET_MAX_FILTER 16 + +struct rtpacket_filter { + __u16 ethertype[RTPACKET_MAX_FILTER]; +}; + +#define RTPACKET_FILTER _IOW(RTDM_CLASS_TESTING, 0x80, struct rtpacket_filter) + +#ifdef __KERNEL__ + +#include <linux/list.h> +#include <rtdm/rtdm_driver.h> + +#define RTPACKET_TX_POOL_SIZE 128 +#define RTPACKET_BUFFER_SIZE 1536 + +struct rtp_addr { + void *va; + dma_addr_t pa; +}; + +struct rtpacket { + struct list_head list; + struct rtp_addr base; + struct rtp_addr data; + int length; +}; + +struct rtpacket_pool { + struct list_head list; + rtdm_lock_t lock; + struct rtpacket *pkt; + unsigned int npkt; + unsigned int pktsize; + struct device *dev; +}; + +struct rtpacket_driver_ops { + struct module *owner; + char name[16]; + int (*filter)(struct rtpacket_driver_ops *p, struct rtpacket_filter *f); + int (*transmit)(struct rtpacket_driver_ops *ops, struct rtpacket *rtp); + void (*recycle)(struct rtpacket_driver_ops *ops, struct rtpacket *rtp); +}; + +struct rtpacket_interface; + +/* packet pool methods */ + +void rtpacket_pool_drain(struct rtpacket_pool *p); + +int rtpacket_pool_init(struct rtpacket_pool *p, struct device *dev, + unsigned int reserved, + unsigned int npkt, unsigned int bufsize); + +struct rtpacket *rtpacket_pool_pop(struct rtpacket_pool *p); + +void rtpacket_pool_push(struct rtpacket_pool *p, struct rtpacket *rtp); + +/* registration methods */ + +void rtpacket_deregister(struct rtpacket_interface *iface); + +struct rtpacket_interface *rtpacket_register(struct rtpacket_driver_ops *ops, + struct device *dev, + unsigned int tx_reserved); + +/* receive and transmit path methods */ + +int rtpacket_receive(struct rtpacket_interface *iface, struct rtpacket *rtp); + +void rtpacket_recycle(struct rtpacket_interface *iface, struct rtpacket *rtp); + +#endif /* __KERNEL__ */ +#endif diff --git a/ksrc/drivers/testing/Kconfig b/ksrc/drivers/testing/Kconfig index 28504dc..6f67e7c 100644 --- a/ksrc/drivers/testing/Kconfig +++ b/ksrc/drivers/testing/Kconfig @@ -9,6 +9,14 @@ config XENO_DRIVERS_TESTING_LEGACY_NAMES Only enable this if you plan to use old userspace tools with the drivers. +config XENO_DRIVERS_PACKET + depends on XENO_SKIN_RTDM + tristate "Raw Ethernet packet driver" + default y + help + Provides a class driver for sending and receiving raw Ethernet + packets over a simple character device interface. + config XENO_DRIVERS_TIMERBENCH depends on XENO_SKIN_RTDM tristate "Timer benchmark driver" diff --git a/ksrc/drivers/testing/Makefile b/ksrc/drivers/testing/Makefile index 17a6cf1..78679c8 100644 --- a/ksrc/drivers/testing/Makefile +++ b/ksrc/drivers/testing/Makefile @@ -4,6 +4,7 @@ ifeq ($(PATCHLEVEL),6) EXTRA_CFLAGS += -D__IN_XENOMAI__ -Iinclude/xenomai +obj-$(CONFIG_XENO_DRIVERS_PACKET) += xeno_packet.o obj-$(CONFIG_XENO_DRIVERS_TIMERBENCH) += xeno_timerbench.o obj-$(CONFIG_XENO_DRIVERS_IRQBENCH) += xeno_irqbench.o obj-$(CONFIG_XENO_DRIVERS_SWITCHTEST) += xeno_switchtest.o @@ -11,6 +12,8 @@ obj-$(CONFIG_XENO_DRIVERS_KLATENCY) += xeno_klat.o obj-$(CONFIG_XENO_DRIVERS_SIGTEST) += xeno_sigtest.o obj-$(CONFIG_XENO_DRIVERS_RTDMTEST) += xeno_rtdmtest.o +xeno_packet-y := packet.o + xeno_timerbench-y := timerbench.o xeno_irqbench-y := irqbench.o diff --git a/ksrc/drivers/testing/packet.c b/ksrc/drivers/testing/packet.c new file mode 100644 index 0000000..b933888 --- /dev/null +++ b/ksrc/drivers/testing/packet.c @@ -0,0 +1,352 @@ +/* + * Xenomai Packet Interface - class driver for raw Ethernet packets + * + * Copyright (C) 2011 OMICRON electronics GmbH + * + * 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., 675 Mass Ave, Cambridge, MA 02139, USA. + */ +#include <linux/dma-mapping.h> +#include <linux/module.h> +#include <rtdm/rtpacket.h> + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("richard.coch...@omicron.at"); + +struct rtpacket_interface { + struct device *dev; + struct rtdm_device rtdev; + struct rtpacket_driver_ops *ops; + struct rtpacket_pool tx_pool; + struct rtpacket_pool rx_fifo; + rtdm_event_t rx_event; + nanosecs_rel_t rx_tmo; +}; + +/* Receive fifo */ + +static struct rtpacket *fifo_dequeue(struct rtpacket_pool *p) +{ + return rtpacket_pool_pop(p); +} + +static int fifo_empty(struct rtpacket_pool *p) +{ + int empty; + rtdm_lockctx_t ctx; + rtdm_lock_get_irqsave(&p->lock, ctx); + empty = list_empty(&p->list); + rtdm_lock_put_irqrestore(&p->lock, ctx); + return empty; +} + +static void fifo_enqueue(struct rtpacket_pool *p, struct rtpacket *rtp) +{ + rtdm_lockctx_t ctx; + rtdm_lock_get_irqsave(&p->lock, ctx); + list_add_tail(&rtp->list, &p->list); + rtdm_lock_put_irqrestore(&p->lock, ctx); +} + +/* Device operations */ + +static int rtpacket_open(struct rtdm_dev_context *ctx, + rtdm_user_info_t *info, int oflags) +{ + return 0; +} + +static int rtpacket_close(struct rtdm_dev_context *ctx, rtdm_user_info_t *info) +{ + return 0; +} + +static int rtpacket_ioctl_rt(struct rtdm_dev_context *ctx, + rtdm_user_info_t *info, + unsigned int request, void __user *arg) +{ + struct rtpacket_interface *pif = ctx->device->device_data; + struct rtpacket_filter __user *user = arg; + struct rtpacket_filter kf; + int err = 0; + + switch (request) { + case RTPACKET_FILTER: + err = rtdm_safe_copy_from_user(info, &kf, user, sizeof(kf)); + if (err) + return err; + + err = pif->ops->filter(pif->ops, &kf); + break; + default: + err = -ENOTTY; + } + + return err; +} + +static ssize_t rtpacket_read_rt(struct rtdm_dev_context *ctx, + rtdm_user_info_t *info, void *buf, size_t nbyte) +{ + struct rtpacket_interface *pif = ctx->device->device_data; + struct rtpacket *rtp; + size_t len; + int err; + + if (nbyte > RTPACKET_BUFFER_SIZE) + return -EINVAL; + + if (fifo_empty(&pif->rx_fifo)) { + err = rtdm_event_timedwait(&pif->rx_event, pif->rx_tmo, NULL); + if (err == -ETIMEDOUT) + return 0; + else if (err) + return err; + } + rtp = fifo_dequeue(&pif->rx_fifo); + if (!rtp) + return 0; + + len = rtp->length; + if (len > nbyte) + len = nbyte; + + err = rtdm_safe_copy_to_user(info, buf, rtp->data.va, len); + + pif->ops->recycle(pif->ops, rtp); + + return err ? err : len; +} + +static ssize_t rtpacket_write_rt(struct rtdm_dev_context *ctx, + rtdm_user_info_t *info, + const void *buf, size_t nbyte) +{ + struct rtpacket_interface *pif = ctx->device->device_data; + struct rtpacket *rtp; + int err; + + if (nbyte > RTPACKET_BUFFER_SIZE) + return -EINVAL; + + rtp = rtpacket_pool_pop(&pif->tx_pool); + if (!rtp) + return -EBUSY; + + err = rtdm_safe_copy_from_user(info, rtp->data.va, buf, nbyte); + if (err) + goto bail; + + rtp->length = nbyte; + + err = pif->ops->transmit(pif->ops, rtp); + if (err) + goto bail; + + return nbyte; +bail: + rtpacket_pool_push(&pif->tx_pool, rtp); + return err; +} + +/* Public methods */ + +void rtpacket_pool_drain(struct rtpacket_pool *p) +{ + struct rtpacket *rtp; + int count; + + if (!p->npkt) + return; + + for (count = 0; ; count++) { + rtp = rtpacket_pool_pop(p); + if (!rtp) + break; + } + + if (count != p->npkt) + pr_err("packet pool mismatch, %d of %d", count, p->npkt); + + dma_free_coherent(p->dev, p->npkt * p->pktsize, + p->pkt[0].base.va, p->pkt[0].base.pa); + + kfree(p->pkt); +} +EXPORT_SYMBOL_GPL(rtpacket_pool_drain); + +int rtpacket_pool_init(struct rtpacket_pool *p, struct device *dev, + unsigned int reserved, + unsigned int npkt, unsigned int bufsize) +{ + void *va; + dma_addr_t pa; + int i; + + INIT_LIST_HEAD(&p->list); + rtdm_lock_init(&p->lock); + p->pkt = NULL; + p->npkt = npkt; + p->pktsize = bufsize; + p->dev = dev; + + if (!npkt) + return 0; + + p->pkt = kmalloc(npkt * sizeof(*p->pkt), GFP_KERNEL); + if (!p->pkt) + goto no_pkt; + + va = dma_alloc_coherent(dev, npkt * bufsize, &pa, GFP_KERNEL); + if (!va) + goto no_dma; + + for (i = 0; i < npkt; i++) { + p->pkt[i].base.va = va; + p->pkt[i].base.pa = pa; + p->pkt[i].data.va = va + reserved; + p->pkt[i].data.pa = pa + reserved; + rtpacket_pool_push(p, &p->pkt[i]); + va += bufsize; + pa += bufsize; + } + return 0; +no_dma: + kfree(p->pkt); +no_pkt: + return -ENOMEM; +} +EXPORT_SYMBOL_GPL(rtpacket_pool_init); + +struct rtpacket *rtpacket_pool_pop(struct rtpacket_pool *p) +{ + struct rtpacket *rtp = NULL; + rtdm_lockctx_t ctx; + rtdm_lock_get_irqsave(&p->lock, ctx); + if (!list_empty(&p->list)) { + rtp = list_first_entry(&p->list, struct rtpacket, list); + list_del_init(&rtp->list); + } + rtdm_lock_put_irqrestore(&p->lock, ctx); + return rtp; +} +EXPORT_SYMBOL_GPL(rtpacket_pool_pop); + +void rtpacket_pool_push(struct rtpacket_pool *p, struct rtpacket *rtp) +{ + rtdm_lockctx_t ctx; + rtdm_lock_get_irqsave(&p->lock, ctx); + list_add(&rtp->list, &p->list); + rtdm_lock_put_irqrestore(&p->lock, ctx); +} +EXPORT_SYMBOL_GPL(rtpacket_pool_push); + +void rtpacket_deregister(struct rtpacket_interface *pif) +{ + rtdm_dev_unregister(&pif->rtdev, 10); + rtpacket_pool_drain(&pif->rx_fifo); + rtpacket_pool_drain(&pif->tx_pool); + rtdm_event_destroy(&pif->rx_event); + kfree(pif); +} +EXPORT_SYMBOL_GPL(rtpacket_deregister); + +struct rtpacket_interface *rtpacket_register(struct rtpacket_driver_ops *ops, + struct device *dev, + unsigned int tx_reserved) +{ + struct rtpacket_interface *pif; + + pif = kzalloc(sizeof(struct rtpacket_interface), GFP_KERNEL); + if (pif == NULL) + goto no_memory; + + pif->ops = ops; + snprintf(pif->rtdev.device_name, RTDM_MAX_DEVNAME_LEN, + "rt-%s", ops->name); + pif->rtdev.struct_version = RTDM_DEVICE_STRUCT_VER; + pif->rtdev.device_flags = RTDM_NAMED_DEVICE; + pif->rtdev.context_size = 0; + pif->rtdev.open_nrt = rtpacket_open; + pif->rtdev.ops.close_nrt = rtpacket_close; + pif->rtdev.ops.ioctl_rt = rtpacket_ioctl_rt, + pif->rtdev.ops.read_rt = rtpacket_read_rt; + pif->rtdev.ops.write_rt = rtpacket_write_rt; + pif->rtdev.device_class = RTDM_CLASS_EXPERIMENTAL; + pif->rtdev.device_sub_class = 0; + pif->rtdev.profile_version = 1; + pif->rtdev.driver_name = pif->rtdev.device_name; + pif->rtdev.driver_version = RTDM_DRIVER_VER(1,0,0); + pif->rtdev.peripheral_name = pif->rtdev.device_name; + pif->rtdev.provider_name = pif->rtdev.device_name; + pif->rtdev.proc_name = pif->rtdev.device_name; + pif->rtdev.device_id = 0; + pif->rtdev.device_data = pif; + + if (rtpacket_pool_init(&pif->tx_pool, dev, tx_reserved, + RTPACKET_TX_POOL_SIZE, + RTPACKET_BUFFER_SIZE)) + goto no_txpool; + + if (rtpacket_pool_init(&pif->rx_fifo, dev, 0, 0, 0)) + goto no_rxfifo; + + rtdm_event_init(&pif->rx_event, 0); + pif->rx_tmo = 1000000000; + + if (rtdm_dev_register(&pif->rtdev)) { + pr_err("register rtdm %s failed\n", pif->rtdev.device_name); + goto no_register; + } + + return pif; + +no_register: + rtpacket_pool_drain(&pif->rx_fifo); +no_rxfifo: + rtpacket_pool_drain(&pif->tx_pool); +no_txpool: + kfree(pif); +no_memory: + return NULL; +} +EXPORT_SYMBOL_GPL(rtpacket_register); + +int rtpacket_receive(struct rtpacket_interface *pif, struct rtpacket *rtp) +{ + fifo_enqueue(&pif->rx_fifo, rtp); + rtdm_event_signal(&pif->rx_event); + return 0; +} +EXPORT_SYMBOL_GPL(rtpacket_receive); + +void rtpacket_recycle(struct rtpacket_interface *pif, struct rtpacket *rtp) +{ + rtpacket_pool_push(&pif->tx_pool, rtp); +} +EXPORT_SYMBOL_GPL(rtpacket_recycle); + +/* Module code */ + +static int __init rtpacket_init(void) +{ + return 0; +} + +static void rtpacket_exit(void) +{ +} + +subsys_initcall(rtpacket_init); +module_exit(rtpacket_exit); -- 1.7.2.5 _______________________________________________ Xenomai-core mailing list Xenomai-core@gna.org https://mail.gna.org/listinfo/xenomai-core