/*
 * 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 embedromix dot ro>
 *
 * Contributors:
 * Radu Negut <radu opalid ro> - ether device statistics
 */

#include <linux/version.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/moduleparam.h>
#include <linux/sched.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.11"
#define MODULE_NAME "sch_log-v" SCH_LOG_VERSION

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Catalin(ux) M. BOIE - <catab at embedromix dot ro>");
MODULE_DESCRIPTION("sch_log - Mirror packets that pass a class to a custom net device");
MODULE_VERSION(SCH_LOG_VERSION);

/*
#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_ERR MODULE_NAME ": " fmt,##arg)

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

/* 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;
};

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

	SCH_LOG_DEBUG(3, "_init\n");

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

	if (opt == NULL) {
		q->limit = qdisc_dev(sch)->tx_queue_len ? : 1;
		q->idx = 0;
		q->flags = 0;
	} else {
		struct tc_sch_log_qopt *ctl = nla_data(opt);

		if (nla_len(opt) < sizeof(*ctl))
			return -EINVAL;

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

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

	return 0;
}

/* TODO: Seems we need to call dev_put somewhere */
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 i, headroom;
	unsigned long mark;

	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) {
		SCH_LOG_DEBUG(3, "_enqueue: Q%X:%X qdisc full (%u >= %u).\n",
			sch->handle >> 16, sch->handle & 0xffff,
			sch->q.qlen, q->limit);
		return qdisc_reshape_fail(skb, sch);
	}

	nd = dev_get_by_index(&init_net, q->idx);
	if (nd == NULL) {
		SCH_LOG_DEBUG(1, "Cannot find interface with index %u.\n", q->idx);
		sch->qstats.drops++;
		kfree_skb(skb);
		return NET_XMIT_DROP;
	}

	if (!(nd->flags & IFF_UP)) {
		SCH_LOG_DEBUG(1, "Interface with index %u is down.\n", q->idx);
		sch->qstats.drops++;
		kfree_skb(skb);
		return NET_XMIT_DROP;
	}

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

	if (q->flags & (1 | 2)) { /* we have mark2dest (1) or q2src (2) */
		clone = skb_copy(skb, GFP_ATOMIC);
	} else {
		clone = skb_clone(skb, GFP_ATOMIC);
	}
	if (clone == NULL) {
		SCH_LOG_DEBUG(1, "Cannot clone!\n");
		goto out;
	}

	/* This is because of the shrinking to 32 bits, around 2.6.15 */
	#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
	mark = clone->nfmark;
	#else
	mark = clone->mark;
	#endif

	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 headroom=%d tail=%p end=%p"
		" mark=0x%lx"
		" 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,
		mark,
		clone->dev->name, clone->dev->hard_header_len,
		nd->name, nd->hard_header_len,
		clone->dev->type);

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

		SCH_LOG_DEBUG(2, "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");
	}

	/* tun is using skb->data as ether if skb_mac_hdr is incorrect */
	/*
	skb_mac_hdr()
	skb_reset_mac_header(skb)
	skb_pull(skb, dev->hard_header_len)
	eth = eth_hdr(skb)
	*/

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

	/* redirect packet to logXX interface */
	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);

		skb_reset_mac_header(clone);
		skb_pull(skb, clone->dev->hard_header_len);

		/*clone->mac.raw = clone->data;*/
		eth = eth_hdr(clone);
		if (eth) {
			SCH_LOG_DEBUG(4, "clone: setting eth->h_proto to 0x%x.\n", clone->protocol);
			eth->h_proto = clone->protocol;
		}
		SCH_LOG_DEBUG(2, "clone: push=1 added=%d\n",
			clone->dev->hard_header_len);
		if (unlikely(debug >= 2)) {
			SCH_LOG_DEBUG(0, "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 == NULL) {
		SCH_LOG_DEBUG(3, "clone: eth is NULL!\n");
	} else {
		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] = (mark >> 24) & 0xff;
			eth->h_dest[3] = (mark >> 16) & 0xff;
			eth->h_dest[4] = (mark >>  8) & 0xff;
			eth->h_dest[5] = (mark >>  0) & 0xff;
		}
	}

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

	out:
	return qdisc_enqueue_tail(skb, sch);
}

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_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct sch_log_sched_data *q = qdisc_priv(sch);
	struct tc_sch_log_qopt opt = {
		.limit = q->limit,
		.idx = q->idx,
		.flags = q->flags,
	};

	NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
	return skb->len;

nla_put_failure:
	return -1;
}

static struct Qdisc_ops sch_log_qdisc_ops __read_mostly = {
	.id		= "log",
	.priv_size	= sizeof(struct sch_log_sched_data),
	.enqueue	= sch_log_enqueue,
	.dequeue	= sch_log_dequeue,
	.peek		= qdisc_peek_head,
	.drop		= qdisc_queue_drop,
	.init		= sch_log_init,
	.reset		= qdisc_reset_queue,
	.change		= sch_log_init,
	.dump		= sch_log_dump,
	.owner		= THIS_MODULE,
};

static int __init init_sch_log(void)
{
	int ret;

	SCH_LOG_DEBUG(0, "(C)opyright Catalin(ux) M. BOIE 2004-2012\n");

	ret = register_qdisc(&sch_log_qdisc_ops);
	if (ret != 0) {
		SCH_LOG_DEBUG(1, "Cannot register qdisc[%d].\n", ret);
		return -ENOMEM;
	}

	return 0;
}

static void __exit exit_sch_log(void)
{
	int ret;

	SCH_LOG_DEBUG(0, "Exiting...\n");
	ret = unregister_qdisc(&sch_log_qdisc_ops);
	if (ret != 0) {
		SCH_LOG_DEBUG(1, "Cannot unregister qdisc."
			" Probably it's in use (%d).\n", ret);
		return;
	}
}

module_init(init_sch_log);
module_exit(exit_sch_log);