/*
 * net/sched/sch_log.c	Monitoring qdisc discipline routines.
 *
 *		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.
 *
 * Authors:	Catalin(ux aka Dino) BOIE, <catab at umbrella dot ro>
 */

#include <linux/config.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/moduleparam.h>
#include <linux/sched.h>
#include <linux/smp_lock.h>
#include <linux/capability.h>
#include <linux/in.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include <linux/tcp.h>
#include <linux/time.h>

#include <linux/proc_fs.h>

#include <linux/string.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/if_ether.h>
#include <linux/if_arp.h>
#include <net/pkt_sched.h>
#include <linux/rtnetlink.h>

#include <asm/uaccess.h>

#define SCH_LOG_VERSION	"0.10"
#define MODULE_NAME "sch_log-v" SCH_LOG_VERSION

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Catalin(ux) BOIE - <catab at umbrella dot ro>");
MODULE_DESCRIPTION("sch_log - Send packets that pass, to a custom net device");
#if SCH_LOG_TARGET > 24
MODULE_VERSION(SCH_LOG_VERSION);
#endif

/*
#undef pr_debug
#define pr_debug(fmt,arg...) printk(KERN_DEBUG fmt,##arg)
*/

#define SCH_LOG_DEBUG(level,fmt,arg...) if (debug >= level) \
	printk(KERN_DEBUG MODULE_NAME ": " fmt,##arg)

/* module params */
static int logno = 4;
module_param(logno, int, S_IRUGO);
static int debug;
module_param(debug, int, S_IRUGO);

/* Kernel dependencies */
#if SCH_LOG_TARGET == 24
#include <linux/random.h>

#define QDISC_ALIGNTO		32
#define QDISC_ALIGN(len)	(((len) + QDISC_ALIGNTO-1) & ~(QDISC_ALIGNTO-1))

static inline void random_ether_addr(u8 *addr)
{
	get_random_bytes(addr, ETH_ALEN);
	addr[0] &= 0xfe;	/* clear multicast bit */
	addr[0] |= 0x02;	/* set local assignment bit (IEEE802) */ 
}

static inline void *qdisc_priv(struct Qdisc *q)
{
	return (char *) q + QDISC_ALIGN(sizeof(struct Qdisc));
}

static inline struct ethhdr *eth_hdr(const struct sk_buff *skb)
{
	return (struct ethhdr *)skb->mac.raw;
}
#endif


/* global variables */

/* qdisc internal data */
struct sch_log_sched_data {
	u32	limit;
	u32	idx;
	u32	flags;
};

/* qdisc options */
struct tc_sch_log_qopt {
	u32	limit;			/* pfifo limit */
	u32	idx;
	u32	flags;
};

/* pool for netdevices */
struct net_pool_t {
	struct net_device	*nd;
	unsigned short		alloc:1;
	unsigned short		reg:1;
};
static struct net_pool_t *net_pool;

static int net_xmit(struct sk_buff *skb, struct net_device *dev)
{
	SCH_LOG_DEBUG(3, "_xmit skb=%p.\n", skb);
	dev_kfree_skb(skb);
	return 0;
}

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

	dev->get_stats = NULL;
	dev->hard_start_xmit = net_xmit;

	dev->tx_queue_len = 0;
	dev->change_mtu = NULL;
	dev->flags |= IFF_NOARP;
	dev->flags &= ~IFF_MULTICAST;
	dev->features |= NETIF_F_NO_CSUM;
	SET_MODULE_OWNER(dev);
	random_ether_addr(dev->dev_addr);
}

static void net_stop(void)
{
	int i;

	if (net_pool == NULL)
		return;

	for (i = 0; i < logno; i++) {
		if (net_pool[i].reg == 1)
			unregister_netdev(net_pool[i].nd);
		if (net_pool[i].alloc == 1)
			free_netdev(net_pool[i].nd);
	}

	kfree(net_pool);
	net_pool = NULL;		
}

static int net_init(void)
{
	char name[IFNAMSIZ];
	int i;

	if (logno <= 0) {
		/* TODO: ratelimit */
		printk(KERN_ERR MODULE_NAME ": Invalid number of log devices (%d).\n",
			logno);
		return -EINVAL;
	}

	net_pool = kmalloc(logno * sizeof(struct net_pool_t), GFP_KERNEL);
	if (net_pool == NULL) {
		printk(KERN_ERR MODULE_NAME ": Cannot alloc netdevice pool!\n");
		return -ENOMEM;
	}
	memset(net_pool, 0, logno * sizeof(struct net_pool_t));

	/* init pool */
	for (i = 0; i < logno; i++) {
		snprintf(name, IFNAMSIZ, "log%02d", i);

		net_pool[i].nd = alloc_netdev(0, name, net_setup);
		if (net_pool[i].nd == NULL) {
			printk(KERN_ERR MODULE_NAME ": _net_init: cannot alloc netdev!\n");
			return -ENOMEM;
		}
		net_pool[i].alloc = 1;

		if (register_netdev(net_pool[i].nd)) {
			printk(KERN_ERR MODULE_NAME ": _net_init: cannot register netdev!\n");
			return -ENOMEM;
		}
		net_pool[i].reg = 1;
	}

	return 0;
}


/* Qdisc stuff */

static int sch_log_init(struct Qdisc *sch, struct rtattr *opt)
{
	struct sch_log_sched_data *q = qdisc_priv(sch);

	SCH_LOG_DEBUG(2, "_init\n");

	memset (q, 0, sizeof(struct sch_log_sched_data));

	if (!opt) {
		q->limit = sch->dev->tx_queue_len;
		q->idx = 0;
		q->flags = 0;
	} else {
		struct tc_sch_log_qopt *ctl = RTA_DATA(opt);

		if (opt->rta_len < RTA_LENGTH(sizeof(*ctl)))
			return -EINVAL;

		if (ctl->idx > logno)
			return -EINVAL;

		q->limit = ctl->limit;
		q->idx = ctl->idx;
		q->flags = ctl->flags;
	}

	SCH_LOG_DEBUG(2, "_init: limit=%d idx=%d flags=0x%x\n",
		q->limit, q->idx, q->flags);

	return 0;
}

static int sch_log_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct sch_log_sched_data *q = qdisc_priv(sch);
	struct net_device *nd;
	int ret, push;
	struct ethhdr *eth;
	struct sk_buff	*clone;
	int modify;
	int i, headroom;
	unsigned long nfmark;

	nd = net_pool[q->idx].nd;

	SCH_LOG_DEBUG(3, "_enqueue: Q%X:%X len=%d idx=%d. Target nd=%s\n",
		sch->handle >> 16, sch->handle & 0xffff,
		skb->len, q->idx, nd->name);

	/* do we have room? */
	if (sch->q.qlen >= q->limit) {
		kfree_skb(skb);
		return NET_XMIT_DROP;
	}

	if (skb->dev == NULL)
		goto out;

	modify = 0;
	if (q->flags & (1 | 2)) /* we have mark2dest (1) or q2src (2) */
		modify = 1;

	if (modify == 0)
		clone = skb_clone(skb, GFP_ATOMIC);
	else
		clone = skb_copy(skb, GFP_ATOMIC);
	if (clone == NULL) {
		SCH_LOG_DEBUG(0, "Cannot clone!\n");
		goto out;
	}

	/* This is because of the srinking to 32 bits, around 2.6.15 */
	nfmark = clone->nfmark;

	headroom = skb_headroom(skb);

	SCH_LOG_DEBUG(1, "clone: handle=0x%08x len=%d data_len=%d"
		" pkt_type=0x%x protocol=0x%x frags=%d"
		" truesize=%d head=%p data=+%d tail=%p end=%p"
		" mark=0x%lx h.raw=%p nh.raw=%p mac.raw=%p"
		" hard_header_len(%s)=%d hard_header_len(%s)=%d type=%d"
		" \n",
		sch->handle, clone->len, clone->data_len,
		clone->pkt_type, clone->protocol, skb_shinfo(skb)->nr_frags,
		clone->truesize, clone->head, headroom, clone->tail,
		clone->end,
		nfmark, clone->h.raw, clone->nh.raw, clone->mac.raw,
		clone->dev->name, clone->dev->hard_header_len,
		nd->name, nd->hard_header_len,
		clone->dev->type);

	if (unlikely(debug >= 2)) {
		printk(KERN_DEBUG MODULE_NAME ": head: ");
		for (i = 0; i < headroom; i++)
			printk("%02x ", clone->head[i]);
		printk("\n");

		printk(KERN_DEBUG MODULE_NAME ": data: ");
		for (i = 0; i < clone->len - clone->data_len; i++)
			printk("%02x ", clone->data[i]);
		if (skb_shinfo(skb)->nr_frags > 0) {
			printk("REST NOT DUMPED YET BECAUSE OF FRAGMENTS!");
		}
		printk("\n");
	}

	switch (clone->dev->type) {
		/* Borrowed from Jamal. Thanks, Jamal! */
		case ARPHRD_PPP:
		case ARPHRD_TUNNEL:
		case ARPHRD_TUNNEL6:
		case ARPHRD_SIT:
		case ARPHRD_IPGRE:
		case ARPHRD_VOID:
		case ARPHRD_NONE:
			push = 1;
			break;
		default:
			push = 0;
			break;
	}

	clone->dev = nd;

	if (push == 1) {
		/* build a fake ethernet header */
		skb_push(clone, clone->dev->hard_header_len);
		memset(clone->data, 0, clone->dev->hard_header_len);
		clone->mac.raw = clone->data;
		eth = eth_hdr(clone);
		if (eth)
			eth->h_proto = clone->protocol;
		SCH_LOG_DEBUG(2, "clone: push=1 added=%d\n",
			clone->dev->hard_header_len);
		if (unlikely(debug >= 2)) {
			printk(KERN_DEBUG MODULE_NAME ": new data: ");
			for (i = 0; i < clone->len - clone->data_len; i++)
				printk("%02x ", clone->data[i]);
			if (skb_shinfo(clone)->nr_frags > 0) {
				printk("REST NOT DUMPED YET BECAUSE OF FRAGMENTS!");
			}
			printk("\n");
		}
	} else {
		clone->mac.raw = clone->data;
		eth = eth_hdr(clone);
		SCH_LOG_DEBUG(2, "clone: push=0 (don't touch ->data)\n");
	}

	if (eth) {
		if (q->flags & 2) { /* q2src */
			eth->h_source[0] = 0;
			eth->h_source[1] = 0;
			eth->h_source[2] = 0;
			eth->h_source[3] = 0;
			eth->h_source[4] = (sch->handle >> 24) & 0xff;
			eth->h_source[5] = (sch->handle >> 16) & 0xff;
		}

		if (q->flags & 1) { /* mark2dest */
			eth->h_dest[0] = 0;
			eth->h_dest[1] = 0;
			eth->h_dest[2] = (clone->nfmark >> 24) & 0xff;
			eth->h_dest[3] = (clone->nfmark >> 16) & 0xff;
			eth->h_dest[4] = (clone->nfmark >>  8) & 0xff;
			eth->h_dest[5] = (clone->nfmark >>  0) & 0xff;
		}
	}

	ret = dev_queue_xmit(clone);
	SCH_LOG_DEBUG(3, "Enqueue/dev_queue_xmit returned %d.\n",
		ret);

	out:
	__skb_queue_tail(&sch->q, skb); /* autoinc qlen */

	return NET_XMIT_SUCCESS;
}

static struct sk_buff *sch_log_dequeue(struct Qdisc *sch)
{
	struct sk_buff *skb;

	skb = __skb_dequeue(&sch->q);
	SCH_LOG_DEBUG(3, "_dequeue: [%p]\n", skb);

	return skb;
}

static int sch_log_requeue(struct sk_buff *skb, struct Qdisc *sch)
{

	__skb_queue_head(&sch->q, skb);

	return NET_XMIT_SUCCESS;
}

static unsigned int sch_log_drop(struct Qdisc *sch)
{
	struct sk_buff *skb;

	skb = __skb_dequeue_tail(&sch->q);
	if (skb) {
		unsigned int len = skb->len;

		kfree_skb(skb);
		sch->q.qlen --;
		return len;
	}

	return 0;
}

static void sch_log_reset(struct Qdisc *sch)
{
	skb_queue_purge(&sch->q);
	sch->flags &= ~TCQ_F_THROTTLED;
}

static void sch_log_destroy(struct Qdisc *sch)
{
	SCH_LOG_DEBUG(3, "_destroy %04x:\n",
		sch->handle >> 16);

	skb_queue_purge(&sch->q);
}

static int sch_log_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct sch_log_sched_data *q = qdisc_priv(sch);
	struct tc_sch_log_qopt opt;
	unsigned char *b = skb->tail;

	opt.limit = q->limit;
	opt.idx = q->idx;
	opt.flags = q->flags;
	RTA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);

	return skb->len;

	rtattr_failure:
	skb_trim(skb, b - skb->data);
	return -1;
}

static struct Qdisc_ops sch_log_qdisc_ops = {
	.next		= NULL,
	.cl_ops		= NULL,
	.id		= "log",
	.priv_size	= sizeof(struct sch_log_sched_data),
	.enqueue	= sch_log_enqueue,
	.dequeue	= sch_log_dequeue,
	.requeue	= sch_log_requeue,
	.drop		= sch_log_drop,
	.init		= sch_log_init,
	.reset		= sch_log_reset,
	.destroy	= sch_log_destroy,
	.change		= NULL,
	.dump		= sch_log_dump,
	#if SCH_LOG_TARGET > 24
	.owner		= THIS_MODULE,
	#endif
};

static int __init init_sch_log(void)
{
	int ret;

	printk(KERN_INFO MODULE_NAME ": (C)opyright Catalin(ux aka Dino) BOIE 2004-2006\n");

	if (net_init() == -1)
		goto err;

	ret = register_qdisc(&sch_log_qdisc_ops);
	if (ret != 0) {
		printk(KERN_ERR MODULE_NAME ": Cannot register qdisc. Sorry!\n");
		goto err;
	}

	return 0;

	err:
	net_stop();
	return -ENOMEM;
}

static void __exit exit_sch_log(void)
{
	int ret;

	ret = unregister_qdisc(&sch_log_qdisc_ops);
	if (ret != 0) {
		printk(KERN_ERR MODULE_NAME ": Cannot unregister qdisc. Probably it's in use.\n");
	}

	net_stop();
}

module_init(init_sch_log);
module_exit(exit_sch_log);