/***
 *
 *  rtcap/rtcap.c
 *
 *  Real-Time Capturing Interface
 *
 *  Copyright (C) 2004, 2005 Jan Kiszka <jan.kiszka@web.de>
 *
 *  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/module.h>
#include <linux/moduleparam.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/sched.h>

#include <rtdev.h>
#include <rtnet_chrdev.h>
#include <rtnet_port.h> /* for netdev_priv() */

MODULE_LICENSE("GPL");

static unsigned int rtcap_rtskbs = 128;
module_param(rtcap_rtskbs, uint, 0444);
MODULE_PARM_DESC(rtcap_rtskbs, "Number of real-time socket buffers per "
			       "real-time device");

#define TAP_DEV 1
#define RTMAC_TAP_DEV 2
#define XMIT_HOOK 4

static rtdm_nrtsig_t cap_signal;
static struct rtskb_queue cap_queue;
static struct rtskb_pool cap_pool;

static struct tap_device_t {
	struct net_device *tap_dev;
	struct net_device *rtmac_tap_dev;
	struct net_device_stats tap_dev_stats;
	int present;
	int (*orig_xmit)(struct rtskb *skb, struct rtnet_device *dev);
} tap_device[MAX_RT_DEVICES];

void rtcap_rx_hook(struct rtskb *rtskb)
{
	bool			trigger = false;

	if ((rtskb->cap_comp_skb = rtskb_pool_dequeue(&cap_pool)) == 0) {
		tap_device[rtskb->rtdev->ifindex].tap_dev_stats.rx_dropped++;
		return;
	}

	if (cap_queue.first == NULL) {
		cap_queue.first = rtskb;
		trigger = true;
	} else
		cap_queue.last->cap_next = rtskb;
	cap_queue.last = rtskb;
	rtskb->cap_next = NULL;

	rtskb->cap_flags |= RTSKB_CAP_SHARED;

	if (trigger)
		rtdm_nrtsig_pend(&cap_signal);
}

int rtcap_xmit_hook(struct rtskb *rtskb, struct rtnet_device *rtdev)
{
	struct tap_device_t *tap_dev = &tap_device[rtskb->rtdev->ifindex];
	rtdm_lockctx_t context;
	bool trigger = false;

	if ((rtskb->cap_comp_skb = rtskb_pool_dequeue(&cap_pool)) == 0) {
		tap_dev->tap_dev_stats.rx_dropped++;
		return tap_dev->orig_xmit(rtskb, rtdev);
	}

	rtskb->cap_next = NULL;
	rtskb->cap_start = rtskb->data;
	rtskb->cap_len = rtskb->len;
	rtskb->cap_flags |= RTSKB_CAP_SHARED;

	rtskb->time_stamp = rtdm_clock_read();

	rtdm_lock_get_irqsave(&rtcap_lock, context);

	if (cap_queue.first == NULL) {
		cap_queue.first = rtskb;
		trigger = true;
	} else
		cap_queue.last->cap_next = rtskb;
	cap_queue.last = rtskb;

	rtdm_lock_put_irqrestore(&rtcap_lock, context);

	if (trigger)
		rtdm_nrtsig_pend(&cap_signal);

	return tap_dev->orig_xmit(rtskb, rtdev);
}

int rtcap_loopback_xmit_hook(struct rtskb *rtskb, struct rtnet_device *rtdev)
{
	struct tap_device_t *tap_dev = &tap_device[rtskb->rtdev->ifindex];

	rtskb->time_stamp = rtdm_clock_read();

	return tap_dev->orig_xmit(rtskb, rtdev);
}

void rtcap_kfree_rtskb(struct rtskb *rtskb)
{
	rtdm_lockctx_t context;
	struct rtskb *comp_skb;

	rtdm_lock_get_irqsave(&rtcap_lock, context);

	if (rtskb->cap_flags & RTSKB_CAP_SHARED) {
		rtskb->cap_flags &= ~RTSKB_CAP_SHARED;

		comp_skb = rtskb->cap_comp_skb;

		rtdm_lock_put_irqrestore(&rtcap_lock, context);

		rtskb_pool_queue_tail(comp_skb->pool, comp_skb);

		return;
	}

	rtdm_lock_put_irqrestore(&rtcap_lock, context);

	rtskb->chain_end = rtskb;
	rtskb_pool_queue_tail(rtskb->pool, rtskb);
}

static void convert_timestamp(nanosecs_abs_t timestamp, struct sk_buff *skb)
{
#ifdef CONFIG_KTIME_SCALAR
	skb->tstamp.tv64 = timestamp;
#else /* !CONFIG_KTIME_SCALAR */
	unsigned long rem;

	rem = do_div(timestamp, NSEC_PER_SEC);
	skb->tstamp = ktime_set((long)timestamp, rem);
#endif /* !CONFIG_KTIME_SCALAR */
}

static void rtcap_signal_handler(rtdm_nrtsig_t *nrtsig, void *arg)
{
	struct rtskb *rtskb;
	struct sk_buff *skb;
	struct sk_buff *rtmac_skb;
	struct net_device_stats *stats;
	int ifindex;
	int active;
	rtdm_lockctx_t context;

	while (1) {
		rtdm_lock_get_irqsave(&rtcap_lock, context);

		if ((rtskb = cap_queue.first) == NULL) {
			rtdm_lock_put_irqrestore(&rtcap_lock, context);
			break;
		}

		cap_queue.first = rtskb->cap_next;

		rtdm_lock_put_irqrestore(&rtcap_lock, context);

		ifindex = rtskb->rtdev->ifindex;
		active = tap_device[ifindex].present;

		if (active) {
			if ((tap_device[ifindex].tap_dev->flags & IFF_UP) == 0)
				active &= ~TAP_DEV;
			if (active & RTMAC_TAP_DEV &&
			    !(tap_device[ifindex].rtmac_tap_dev->flags &
			      IFF_UP))
				active &= ~RTMAC_TAP_DEV;
		}

		if (active == 0) {
			tap_device[ifindex].tap_dev_stats.rx_dropped++;
			rtcap_kfree_rtskb(rtskb);
			continue;
		}

		skb = dev_alloc_skb(rtskb->cap_len);
		if (skb) {
			memcpy(skb_put(skb, rtskb->cap_len), rtskb->cap_start,
			       rtskb->cap_len);

			if (active & TAP_DEV) {
				skb->dev = tap_device[ifindex].tap_dev;
				skb->protocol = eth_type_trans(skb, skb->dev);
				convert_timestamp(rtskb->time_stamp, skb);

				rtmac_skb = NULL;
				if ((rtskb->cap_flags &
				     RTSKB_CAP_RTMAC_STAMP) &&
				    (active & RTMAC_TAP_DEV)) {
					rtmac_skb = skb_clone(skb, GFP_ATOMIC);
					if (rtmac_skb != NULL)
						convert_timestamp(
							rtskb->cap_rtmac_stamp,
							rtmac_skb);
				}

				rtcap_kfree_rtskb(rtskb);

				stats = &tap_device[ifindex].tap_dev_stats;
				stats->rx_packets++;
				stats->rx_bytes += skb->len;

				if (rtmac_skb != NULL) {
					rtmac_skb->dev = tap_device[ifindex]
								 .rtmac_tap_dev;
					netif_rx(rtmac_skb);
				}
				netif_rx(skb);
			} else if (rtskb->cap_flags & RTSKB_CAP_RTMAC_STAMP) {
				skb->dev = tap_device[ifindex].rtmac_tap_dev;
				skb->protocol = eth_type_trans(skb, skb->dev);
				convert_timestamp(rtskb->cap_rtmac_stamp, skb);

				rtcap_kfree_rtskb(rtskb);

				stats = &tap_device[ifindex].tap_dev_stats;
				stats->rx_packets++;
				stats->rx_bytes += skb->len;

				netif_rx(skb);
			} else {
				dev_kfree_skb(skb);
				rtcap_kfree_rtskb(rtskb);
			}
		} else {
			printk("RTcap: unable to allocate linux skb\n");
			rtcap_kfree_rtskb(rtskb);
		}
	}
}

static int tap_dev_open(struct net_device *dev)
{
	int err;

	err = try_module_get(THIS_MODULE);
	if (err == 0)
		return -EIDRM;

	dev_addr_set(dev,
		     (*(struct rtnet_device **)netdev_priv(dev))->dev_addr);

	return 0;
}

static int tap_dev_stop(struct net_device *dev)
{
	module_put(THIS_MODULE);
	return 0;
}

static int tap_dev_xmit(struct sk_buff *skb, struct net_device *dev)
{
	netif_stop_queue(dev);
	return 1;
}

static struct net_device_stats *tap_dev_get_stats(struct net_device *dev)
{
	struct rtnet_device *rtdev = *(struct rtnet_device **)netdev_priv(dev);

	return &tap_device[rtdev->ifindex].tap_dev_stats;
}

static int tap_dev_change_mtu(struct net_device *dev, int new_mtu)
{
	return -EINVAL;
}

static const struct net_device_ops tap_netdev_ops = {
	.ndo_open = tap_dev_open,
	.ndo_stop = tap_dev_stop,
	.ndo_start_xmit = tap_dev_xmit,
	.ndo_get_stats = tap_dev_get_stats,
	.ndo_change_mtu = tap_dev_change_mtu,
};

static void tap_dev_setup(struct net_device *dev)
{
	ether_setup(dev);

	dev->netdev_ops = &tap_netdev_ops;
	dev->mtu = 1500;
	dev->flags &= ~IFF_MULTICAST;
}

void cleanup_tap_devices(void)
{
	int i;
	struct rtnet_device *rtdev;

	for (i = 0; i < MAX_RT_DEVICES; i++)
		if ((tap_device[i].present & TAP_DEV) != 0) {
			if ((tap_device[i].present & XMIT_HOOK) != 0) {
				rtdev = *(struct rtnet_device **)netdev_priv(
					tap_device[i].tap_dev);

				mutex_lock(&rtdev->nrt_lock);
				rtdev->hard_start_xmit =
					tap_device[i].orig_xmit;
				if (rtdev->features & NETIF_F_LLTX)
					rtdev->start_xmit =
						tap_device[i].orig_xmit;
				mutex_unlock(&rtdev->nrt_lock);

				rtdev_dereference(rtdev);
			}

			if ((tap_device[i].present & RTMAC_TAP_DEV) != 0) {
				unregister_netdev(tap_device[i].rtmac_tap_dev);
				free_netdev(tap_device[i].rtmac_tap_dev);
			}

			unregister_netdev(tap_device[i].tap_dev);
			free_netdev(tap_device[i].tap_dev);
		}
}

int __init rtcap_init(void)
{
	struct rtnet_device *rtdev;
	struct net_device *dev;
	int ret;
	int devices = 0;
	int i;

	printk("RTcap: real-time capturing interface\n");

	rtskb_queue_init(&cap_queue);

	rtdm_nrtsig_init(&cap_signal, rtcap_signal_handler, NULL);

	for (i = 0; i < MAX_RT_DEVICES; i++) {
		tap_device[i].present = 0;

		rtdev = rtdev_get_by_index(i);
		if (rtdev != NULL) {
			mutex_lock(&rtdev->nrt_lock);

			if (test_bit(PRIV_FLAG_UP, &rtdev->priv_flags)) {
				mutex_unlock(&rtdev->nrt_lock);
				printk("RTcap: %s busy, skipping device!\n",
				       rtdev->name);
				rtdev_dereference(rtdev);
				continue;
			}

			if (rtdev->mac_priv != NULL) {
				mutex_unlock(&rtdev->nrt_lock);

				printk("RTcap: RTmac discipline already active on device %s. "
				       "Load RTcap before RTmac!\n",
				       rtdev->name);

				rtdev_dereference(rtdev);
				continue;
			}

			memset(&tap_device[i].tap_dev_stats, 0,
			       sizeof(struct net_device_stats));

			dev = alloc_netdev(sizeof(struct rtnet_device *),
					   rtdev->name, NET_NAME_UNKNOWN,
					   tap_dev_setup);
			if (!dev) {
				ret = -ENOMEM;
				goto error3;
			}

			tap_device[i].tap_dev = dev;
			*(struct rtnet_device **)netdev_priv(dev) = rtdev;

			ret = register_netdev(dev);
			if (ret < 0)
				goto error3;

			tap_device[i].present = TAP_DEV;

			tap_device[i].orig_xmit = rtdev->hard_start_xmit;

			if ((rtdev->flags & IFF_LOOPBACK) == 0) {
				dev = alloc_netdev(
					sizeof(struct rtnet_device *),
					rtdev->name, NET_NAME_UNKNOWN,
					tap_dev_setup);
				if (!dev) {
					ret = -ENOMEM;
					goto error3;
				}

				tap_device[i].rtmac_tap_dev = dev;
				*(struct rtnet_device **)netdev_priv(dev) =
					rtdev;
				strncat(dev->name, "-mac",
					IFNAMSIZ - strlen(dev->name));

				ret = register_netdev(dev);
				if (ret < 0)
					goto error3;

				tap_device[i].present |= RTMAC_TAP_DEV;

				rtdev->hard_start_xmit = rtcap_xmit_hook;
			} else
				rtdev->hard_start_xmit =
					rtcap_loopback_xmit_hook;

			/* If the device requires no xmit_lock, start_xmit points equals
	     * hard_start_xmit => we have to update this as well
	     */
			if (rtdev->features & NETIF_F_LLTX)
				rtdev->start_xmit = rtdev->hard_start_xmit;

			tap_device[i].present |= XMIT_HOOK;

			mutex_unlock(&rtdev->nrt_lock);

			devices++;
		}
	}

	if (devices == 0) {
		printk("RTcap: no real-time devices found!\n");
		ret = -ENODEV;
		goto error2;
	}

	if (rtskb_module_pool_init(&cap_pool, rtcap_rtskbs * devices) <
	    rtcap_rtskbs * devices) {
		rtskb_pool_release(&cap_pool);
		ret = -ENOMEM;
		goto error2;
	}

	/* register capturing handlers with RTnet core
     * (adding the handler need no locking) */
	rtcap_handler = rtcap_rx_hook;

	return 0;

error3:
	mutex_unlock(&rtdev->nrt_lock);
	rtdev_dereference(rtdev);
	printk("RTcap: unable to register %s!\n", dev->name);

error2:
	cleanup_tap_devices();
	rtdm_nrtsig_destroy(&cap_signal);

	return ret;
}

void rtcap_cleanup(void)
{
	rtdm_lockctx_t context;

	rtdm_nrtsig_destroy(&cap_signal);

	/* unregister capturing handlers
     * (take lock to avoid any unloading code before handler was left) */
	rtdm_lock_get_irqsave(&rtcap_lock, context);
	rtcap_handler = NULL;
	rtdm_lock_put_irqrestore(&rtcap_lock, context);

	/* empty queue (should be already empty) */
	rtcap_signal_handler(0, NULL /* we ignore them anyway */);

	cleanup_tap_devices();

	rtskb_pool_release(&cap_pool);

	printk("RTcap: unloaded\n");
}

module_init(rtcap_init);
module_exit(rtcap_cleanup);