#include <asm/uaccess.h>
#include <asm/system.h>
#include <linux/bitops.h>
#include <linux/config.h>
#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/string.h>
#include <linux/mm.h>
#include <linux/socket.h>
#include <linux/sockios.h>
#include <linux/in.h>
#include <linux/errno.h>
#include <linux/interrupt.h>
#include <linux/netdevice.h>
#include <linux/skbuff.h>
#include <linux/rtnetlink.h>
#include <linux/init.h>
#include <linux/rcupdate.h>
#include <linux/list.h>
#include <net/sock.h>
#include <net/pkt_sched.h>
#include <linux/proc_fs.h>
#include <linux/seq_file.h>
#include "cpmac_sched.h"
#include "cpmac_if.h"
#include "cpmac_reg.h"
#include "cpmac_debug.h"

struct cpmacqueues {
	__u32 maxqlen;
	int nextqueue;
	cpmac_priv_t *cpmacpriv;
	struct sk_buff_head wanqueues[CPMAC_WAN_QUEUE_BANDS];
	struct sk_buff_head lanqueues[CPMAC_LAN_QUEUE_BANDS];
};

static const u8 wanprio2band[TC_PRIO_MAX+1] =
	{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };

static const u8 lanprio2band[TC_PRIO_MAX+1] =
	{ 0, 1, 2, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 };

static inline struct sk_buff_head *
wanprio2list(struct sk_buff *skb, struct Qdisc *qdisc)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	return &priv->wanqueues[wanprio2band[skb->priority & TC_PRIO_MAX]];
}

static inline struct sk_buff_head *
lanprio2list(struct sk_buff *skb, struct Qdisc *qdisc)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	return &priv->lanqueues[lanprio2band[skb->priority & TC_PRIO_MAX]];
}

#ifdef CPMAC_SCHED_PRIOQUEUE_DEBUG
static char buf[60];
static int bufidx = 0;
static char cwmap[] = "0123456789abcdef";
static char clmap[] = "ABCD";

static void log_dequeue(int wan, int priority)
{
  if (bufidx == sizeof(buf)-1) {
	 buf[bufidx] = 0;
	 DEB_INFO("cpmac_sched: %s\n", buf);
	 bufidx = 0;
  }
  if (wan)
     buf[bufidx++] = cwmap[priority % (sizeof(cwmap)-1)]);
  else
     buf[bufidx++] = clmap[priority % (sizeof(clmap)-1)]);
}

#endif

static struct sk_buff *
cpmac_queue_dequeue_tail(struct Qdisc* qdisc, int wan)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	int prio;

	if (wan) {
	   struct sk_buff_head *list = priv->wanqueues;
	   for (prio = CPMAC_WAN_QUEUE_BANDS-1; prio >= 0; prio--) {
		   if (!skb_queue_empty(list + prio)) {
			   qdisc->q.qlen--;
#ifdef CPMAC_SCHED_PRIOQUEUE_DEBUG
               log_dequeue(1, prio);
#endif
			   return __qdisc_dequeue_tail(qdisc, list + prio);
		   }
	   }
	} else {
	   struct sk_buff_head *list = priv->lanqueues;
	   for (prio = CPMAC_LAN_QUEUE_BANDS-1; prio >= 0; prio--) {
		   if (!skb_queue_empty(list + prio)) {
			   qdisc->q.qlen--;
#ifdef CPMAC_SCHED_PRIOQUEUE_DEBUG
               log_dequeue(0, prio);
#endif
			   return __qdisc_dequeue_tail(qdisc, list + prio);
		   }
	   }
	}

	return NULL;
}

static int cpmac_queue_enqueue(struct sk_buff *skb, struct Qdisc* qdisc)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	struct sk_buff_head *list;
	int wan, ret;

    qdisc->q.qlen++;
	if (qdisc->q.qlen > priv->maxqlen)
	   priv->maxqlen = qdisc->q.qlen;
	wan = (skb->uniq_id >> 24) != 0 ? 1 : 0;
	if (wan) list = wanprio2list(skb, qdisc);
	else list = lanprio2list(skb, qdisc);
	skb->uniq_id = (wan<<24) | (skb->uniq_id & 0xffffff);
	ret = __qdisc_enqueue_tail(skb, qdisc, list);
	if (qdisc->q.qlen >= qdisc->dev->tx_queue_len) {
       if ((skb = cpmac_queue_dequeue_tail(qdisc, wan)) == 0)
          skb = cpmac_queue_dequeue_tail(qdisc, !wan);
	   if (skb)
	      return qdisc_drop(skb, qdisc);
	   DEB_ERR("cpmac_sched_enqueue: skb == 0\n");
	}
	return ret;
}

static struct sk_buff *queue_dequeue(struct Qdisc* qdisc, int wan)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	int prio;

	if (wan) {
	   struct sk_buff_head *list = priv->wanqueues;
	   for (prio = 0; prio < CPMAC_WAN_QUEUE_BANDS; prio++) {
		   if (!skb_queue_empty(list + prio)) {
			   qdisc->q.qlen--;
	           if (qdisc->dev->tx_queue_len & 1) {
				  DEB_INFO("cpmac_dequeue(wan): prio %d qlen %lu\n",
					  prio, qdisc->q.qlen);
			   }
			   return __qdisc_dequeue_head(qdisc, list + prio);
		   }
	   }
	} else {
	   struct sk_buff_head *list = priv->lanqueues;
	   for (prio = 0; prio < CPMAC_LAN_QUEUE_BANDS; prio++) {
		   if (!skb_queue_empty(list + prio)) {
			   qdisc->q.qlen--;
	           if (qdisc->dev->tx_queue_len & 1) {
				  DEB_INFO("cpmac_dequeue(lan): prio %d qlen %lu\n",
					  prio, qdisc->q.qlen);
			   }
			   return __qdisc_dequeue_head(qdisc, list + prio);
		   }
	   }
	}

	return NULL;
}

static struct sk_buff *cpmac_queue_dequeue(struct Qdisc* qdisc)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	cpmac_priv_t *cpmacpriv = priv->cpmacpriv;
	struct sk_buff *skb = 0;
	int notfull;

	/* CPMAC is stopped, try paket for LAN first */
    if (CPMAC_TXPAUSE(qdisc->dev->base_addr))
	   priv->nextqueue = 0;

	notfull = cpmacpriv->TxPrioQueues->q[priv->nextqueue].Free;
	if (notfull)
       skb = queue_dequeue(qdisc, priv->nextqueue);
	priv->nextqueue = priv->nextqueue ? 0 : 1;
	if (skb) return skb;
    return queue_dequeue(qdisc, priv->nextqueue);
}

static int cpmac_queue_requeue(struct sk_buff *skb, struct Qdisc* qdisc)
{
	struct sk_buff_head *list;
	int wan;

	qdisc->q.qlen++;
	wan = (skb->uniq_id >> 24) != 0;
	if (wan) list = wanprio2list(skb, qdisc);
	else list = lanprio2list(skb, qdisc);
	return __qdisc_requeue(skb, qdisc, list);
}

static void cpmac_queue_reset(struct Qdisc* qdisc)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	struct sk_buff_head *list;
	int prio;

	list = priv->wanqueues;
	for (prio = 0; prio < CPMAC_WAN_QUEUE_BANDS	; prio++)
		__qdisc_reset_queue(qdisc, list + prio);
	list = priv->lanqueues;
	for (prio = 0; prio < CPMAC_LAN_QUEUE_BANDS	; prio++)
		__qdisc_reset_queue(qdisc, list + prio);

	qdisc->qstats.backlog = 0;
	qdisc->q.qlen = 0;
}

static int cpmac_queue_dump(struct Qdisc *qdisc, struct sk_buff *skb)
{
	struct tc_prio_qopt opt = { .bands = CPMAC_WAN_QUEUE_BANDS };

	memcpy(&opt.priomap, wanprio2band, TC_PRIO_MAX+1);
	RTA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
	return skb->len;

rtattr_failure:
	return -1;
}

static int cpmac_queue_init(struct Qdisc *qdisc, struct rtattr *opt)
{
    struct cpmacqueues *priv = qdisc_priv(qdisc);
	struct sk_buff_head *list;
	int prio;

	priv->maxqlen = 0;
	priv->cpmacpriv = (cpmac_priv_t *)netdev_priv(qdisc->dev);
	list = priv->wanqueues;
	for (prio = 0; prio < CPMAC_WAN_QUEUE_BANDS	; prio++)
		skb_queue_head_init(list + prio);
	list = priv->lanqueues;
	for (prio = 0; prio < CPMAC_LAN_QUEUE_BANDS	; prio++)
		skb_queue_head_init(list + prio);
    DEB_INFO("cpmac_sched: init done.\n");
	return 0;
}

static struct Qdisc_ops cpmac_queue_ops = {
	.id		=	"cpmac_queue",
	.priv_size	=	sizeof(struct cpmacqueues),
	.enqueue	=	cpmac_queue_enqueue,
	.dequeue	=	cpmac_queue_dequeue,
	.requeue	=	cpmac_queue_requeue,
	.init		=	cpmac_queue_init,
	.reset		=	cpmac_queue_reset,
	.dump		=	cpmac_queue_dump,
	.owner		=	THIS_MODULE,
};

/* ---------------------------------------------------------------------- */

static struct net_device *qdev = 0;
struct Qdisc *qdisc;
static char lines[6];

/* ---------------------------------------------------------------------- */

static void *qinfo_start(struct seq_file *seq, loff_t *pos)
{
	if (qdev && *pos < sizeof(lines))
		return &lines[*pos];
	return NULL;
}

static void *qinfo_next(struct seq_file *seq, void *v, loff_t *pos)
{
	++*pos;
	if (*pos < sizeof(lines))
		return &lines[*pos];
	return NULL;
}

static void qinfo_stop(struct seq_file *seq, void *v)
{
}

static int qinfo_show(struct seq_file *seq, void *v)
{
	cpmac_priv_t *cpmacpriv = netdev_priv(qdev);
    struct cpmacqueues *priv = qdisc_priv(qdisc);
    char buf[256];
	char *s = buf;
	char *end = buf + sizeof(buf);
	int pos = (char *)v-lines;
	int i;

	switch (pos) {
	   case 0:
	   case 1:
	   case 2:
	     seq_printf(seq, "queue%d: free %u dmafree %u\n",
					 pos,
	                 cpmacpriv->TxPrioQueues->q[pos].Free,
	                 cpmacpriv->TxPrioQueues->q[pos].DMAFree);
		 break;

	   case 3:
	     seq_printf(seq, "dev: %s ptime %u\n", 
					netif_queue_stopped(qdev) ? "stopped" : "in action",
					CPMAC_TXPAUSE(qdev->base_addr));
		 break;
		  
	   case 4:
		 for (i=0; i < CPMAC_WAN_QUEUE_BANDS; i++) {
		    snprintf(s, end-s, " %d", skb_queue_len(&priv->wanqueues[i]));
		    s += strlen(s);
		 }
	     seq_printf(seq, "WAN: %s\n", buf);
		 break;

	   case 5:
		 for (i=0; i < CPMAC_LAN_QUEUE_BANDS; i++) {
		    snprintf(s, end-s, " %d", skb_queue_len(&priv->lanqueues[i]));
		    s += strlen(s);
		 }
	     seq_printf(seq, "LAN: %s\n", buf);
		 break;

	   default:
	     seq_printf(seq, "(%d)\n", pos);
	}

	return 0;
}

static struct seq_operations seq_qinfo_ops = {
	.start	= qinfo_start,
	.next	= qinfo_next,
	.stop	= qinfo_stop,
	.show	= qinfo_show,
};

static int seq_qinfo_open(struct inode *inode, struct file *file)
{
	return seq_open(file, &seq_qinfo_ops);
}

static struct file_operations proc_qinfo_ops = {
	.open		= seq_qinfo_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= seq_release,
};

/* ---------------------------------------------------------------------- */

int cpmac_set_queue(struct net_device *dev)
{
   static char buf[128];
   struct Qdisc *oqdisc;
   int ret;

   if (qdev)
	  return -EBUSY;

   if ((qdisc = qdisc_alloc(dev, &cpmac_queue_ops)) == 0)
      return -ENOMEM;

   if (cpmac_queue_ops.init) {
	  ret = cpmac_queue_ops.init(qdisc, 0);
	  if (ret)
		 return ret;
   }

   qdev = dev;
   snprintf(buf, sizeof(buf)-1, "cpmac_sched_%s", dev->name);
   create_seq_entry(buf, 0, &proc_qinfo_ops);

   if (dev->flags & IFF_UP)
	  dev_deactivate(dev);

   qdisc_lock_tree(dev);

   oqdisc = dev->qdisc_sleeping;
   /* Prune old scheduler */
   if (oqdisc && atomic_read(&oqdisc->refcnt) <= 1)
	   qdisc_reset(oqdisc);
   /* ... and graft new one */
   if (qdisc == NULL)
	  qdisc = &noop_qdisc;
   dev->qdisc_sleeping = qdisc;
   dev->qdisc = &noop_qdisc;

   qdisc_unlock_tree(dev);

   if (dev->flags & IFF_UP)
	   dev_activate(dev);

   DEB_INFO("cpmac_sched: added to %s.\n", dev->name);

   return 0;
}
