/***
 *
 *  stack/rtskb.c - rtskb implementation for rtnet
 *
 *  Copyright (C) 2002      Ulrich Marx <marx@fet.uni-hannover.de>,
 *  Copyright (C) 2003-2006 Jan Kiszka <jan.kiszka@web.de>
 *  Copyright (C) 2006 Jorge Almeida <j-almeida@criticalsoftware.com>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of version 2 of the GNU General Public License as
 *  published by the Free Software Foundation.
 *
 *  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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <linux/moduleparam.h>
#include <linux/slab.h>
#include <rtnet_checksum.h>

#include <rtdev.h>
#include <rtnet_internal.h>
#include <rtskb.h>
#include <rtnet_port.h>

static unsigned int global_rtskbs = DEFAULT_GLOBAL_RTSKBS;
module_param(global_rtskbs, uint, 0444);
MODULE_PARM_DESC(global_rtskbs,
		 "Number of realtime socket buffers in global pool");

/* Linux slab pool for rtskbs */
static struct kmem_cache *rtskb_slab_pool;

/* pool of rtskbs for global use */
struct rtskb_pool global_pool;
EXPORT_SYMBOL_GPL(global_pool);

/* pool statistics */
unsigned int rtskb_pools = 0;
unsigned int rtskb_pools_max = 0;
unsigned int rtskb_amount = 0;
unsigned int rtskb_amount_max = 0;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP)
/* RTcap interface */
rtdm_lock_t rtcap_lock;
EXPORT_SYMBOL_GPL(rtcap_lock);

void (*rtcap_handler)(struct rtskb *skb) = NULL;
EXPORT_SYMBOL_GPL(rtcap_handler);
#endif

/***
 *  rtskb_copy_and_csum_bits
 */
unsigned int rtskb_copy_and_csum_bits(const struct rtskb *skb, int offset,
				      u8 *to, int len, unsigned int csum)
{
	int copy;

	/* Copy header. */
	if ((copy = skb->len - offset) > 0) {
		if (copy > len)
			copy = len;
		csum = rtnet_csum_copy(skb->data + offset, to, copy, csum);
		if ((len -= copy) == 0)
			return csum;
		offset += copy;
		to += copy;
	}

	RTNET_ASSERT(len == 0, );
	return csum;
}

EXPORT_SYMBOL_GPL(rtskb_copy_and_csum_bits);

/***
 *  rtskb_copy_and_csum_dev
 */
void rtskb_copy_and_csum_dev(const struct rtskb *skb, u8 *to)
{
	unsigned int csum;
	unsigned int csstart;

	if (skb->ip_summed == CHECKSUM_PARTIAL) {
		csstart = skb->h.raw - skb->data;

		if (csstart > skb->len)
			BUG();
	} else
		csstart = skb->len;

	memcpy(to, skb->data, csstart);

	csum = 0;
	if (csstart != skb->len)
		csum = rtskb_copy_and_csum_bits(skb, csstart, to + csstart,
						skb->len - csstart, 0);

	if (skb->ip_summed == CHECKSUM_PARTIAL) {
		unsigned int csstuff = csstart + skb->csum;

		*((unsigned short *)(to + csstuff)) = csum_fold(csum);
	}
}

EXPORT_SYMBOL_GPL(rtskb_copy_and_csum_dev);

#ifdef CONFIG_XENO_DRIVERS_NET_CHECKED
/**
 *  skb_over_panic - private function
 *  @skb: buffer
 *  @sz: size
 *  @here: address
 *
 *  Out of line support code for rtskb_put(). Not user callable.
 */
void rtskb_over_panic(struct rtskb *skb, int sz, void *here)
{
	rtdm_printk("RTnet: rtskb_put :over: %p:%d put:%d dev:%s\n", here,
		    skb->len, sz, (skb->rtdev) ? skb->rtdev->name : "<NULL>");
}

EXPORT_SYMBOL_GPL(rtskb_over_panic);

/**
 *  skb_under_panic - private function
 *  @skb: buffer
 *  @sz: size
 *  @here: address
 *
 *  Out of line support code for rtskb_push(). Not user callable.
 */
void rtskb_under_panic(struct rtskb *skb, int sz, void *here)
{
	rtdm_printk("RTnet: rtskb_push :under: %p:%d put:%d dev:%s\n", here,
		    skb->len, sz, (skb->rtdev) ? skb->rtdev->name : "<NULL>");
}

EXPORT_SYMBOL_GPL(rtskb_under_panic);
#endif /* CONFIG_XENO_DRIVERS_NET_CHECKED */

static struct rtskb *__rtskb_pool_dequeue(struct rtskb_pool *pool)
{
	struct rtskb_queue *queue = &pool->queue;
	struct rtskb *skb;

	if (pool->lock_ops && !pool->lock_ops->trylock(pool->lock_cookie))
		return NULL;
	skb = __rtskb_dequeue(queue);
	if (skb == NULL && pool->lock_ops)
		pool->lock_ops->unlock(pool->lock_cookie);

	return skb;
}

struct rtskb *rtskb_pool_dequeue(struct rtskb_pool *pool)
{
	struct rtskb_queue *queue = &pool->queue;
	rtdm_lockctx_t context;
	struct rtskb *skb;

	rtdm_lock_get_irqsave(&queue->lock, context);
	skb = __rtskb_pool_dequeue(pool);
	rtdm_lock_put_irqrestore(&queue->lock, context);

	return skb;
}
EXPORT_SYMBOL_GPL(rtskb_pool_dequeue);

static void __rtskb_pool_queue_tail(struct rtskb_pool *pool, struct rtskb *skb)
{
	struct rtskb_queue *queue = &pool->queue;

	__rtskb_queue_tail(queue, skb);
	if (pool->lock_ops)
		pool->lock_ops->unlock(pool->lock_cookie);
}

void rtskb_pool_queue_tail(struct rtskb_pool *pool, struct rtskb *skb)
{
	struct rtskb_queue *queue = &pool->queue;
	rtdm_lockctx_t context;

	rtdm_lock_get_irqsave(&queue->lock, context);
	__rtskb_pool_queue_tail(pool, skb);
	rtdm_lock_put_irqrestore(&queue->lock, context);
}
EXPORT_SYMBOL_GPL(rtskb_pool_queue_tail);

/***
 *  alloc_rtskb - allocate an rtskb from a pool
 *  @size: required buffer size (to check against maximum boundary)
 *  @pool: pool to take the rtskb from
 */
struct rtskb *alloc_rtskb(unsigned int size, struct rtskb_pool *pool)
{
	struct rtskb *skb;

	RTNET_ASSERT(size <= SKB_DATA_ALIGN(RTSKB_SIZE), return NULL;);

	skb = rtskb_pool_dequeue(pool);
	if (!skb)
		return NULL;

	/* Load the data pointers. */
	skb->data = skb->buf_start;
	skb->tail = skb->buf_start;
	skb->end = skb->buf_start + size;

	/* Set up other states */
	skb->chain_end = skb;
	skb->len = 0;
	skb->pkt_type = PACKET_HOST;
	skb->xmit_stamp = NULL;
	skb->ip_summed = CHECKSUM_NONE;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP)
	skb->cap_flags = 0;
#endif

	return skb;
}

EXPORT_SYMBOL_GPL(alloc_rtskb);

/***
 *  kfree_rtskb
 *  @skb    rtskb
 */
void kfree_rtskb(struct rtskb *skb)
{
#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP)
	rtdm_lockctx_t context;
	struct rtskb *comp_skb;
	struct rtskb *next_skb;
	struct rtskb *chain_end;
#endif

	RTNET_ASSERT(skb != NULL, return;);
	RTNET_ASSERT(skb->pool != NULL, return;);

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP)
	next_skb = skb;
	chain_end = skb->chain_end;

	do {
		skb = next_skb;
		next_skb = skb->next;

		rtdm_lock_get_irqsave(&rtcap_lock, context);

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

			comp_skb = skb->cap_comp_skb;
			skb->pool = xchg(&comp_skb->pool, skb->pool);

			rtdm_lock_put_irqrestore(&rtcap_lock, context);

			rtskb_pool_queue_tail(comp_skb->pool, comp_skb);
		} else {
			rtdm_lock_put_irqrestore(&rtcap_lock, context);

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

	} while (chain_end != skb);

#else /* CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP */

	rtskb_pool_queue_tail(skb->pool, skb);

#endif /* CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP */
}

EXPORT_SYMBOL_GPL(kfree_rtskb);

/***
 *  rtskb_pool_init
 *  @pool: pool to be initialized
 *  @initial_size: number of rtskbs to allocate
 *  return: number of actually allocated rtskbs
 */
unsigned int rtskb_pool_init(struct rtskb_pool *pool, unsigned int initial_size,
			     const struct rtskb_pool_lock_ops *lock_ops,
			     void *lock_cookie)
{
	unsigned int i;

	rtskb_queue_init(&pool->queue);

	i = rtskb_pool_extend(pool, initial_size);

	rtskb_pools++;
	if (rtskb_pools > rtskb_pools_max)
		rtskb_pools_max = rtskb_pools;

	pool->lock_ops = lock_ops;
	pool->lock_cookie = lock_cookie;

	return i;
}

EXPORT_SYMBOL_GPL(rtskb_pool_init);

static int rtskb_module_pool_trylock(void *cookie)
{
	int err = 1;
	if (cookie)
		err = try_module_get(cookie);
	return err;
}

static void rtskb_module_pool_unlock(void *cookie)
{
	if (cookie)
		module_put(cookie);
}

static const struct rtskb_pool_lock_ops rtskb_module_lock_ops = {
	.trylock = rtskb_module_pool_trylock,
	.unlock = rtskb_module_pool_unlock,
};

unsigned int __rtskb_module_pool_init(struct rtskb_pool *pool,
				      unsigned int initial_size,
				      struct module *module)
{
	return rtskb_pool_init(pool, initial_size, &rtskb_module_lock_ops,
			       module);
}
EXPORT_SYMBOL_GPL(__rtskb_module_pool_init);

/***
 *  __rtskb_pool_release
 *  @pool: pool to release
 */
void rtskb_pool_release(struct rtskb_pool *pool)
{
	struct rtskb *skb;

	while ((skb = rtskb_dequeue(&pool->queue)) != NULL) {
		rtdev_unmap_rtskb(skb);
		kmem_cache_free(rtskb_slab_pool, skb);
		rtskb_amount--;
	}

	rtskb_pools--;
}

EXPORT_SYMBOL_GPL(rtskb_pool_release);

unsigned int rtskb_pool_extend(struct rtskb_pool *pool, unsigned int add_rtskbs)
{
	unsigned int i;
	struct rtskb *skb;

	RTNET_ASSERT(pool != NULL, return -EINVAL;);

	for (i = 0; i < add_rtskbs; i++) {
		/* get rtskb from slab pool */
		if (!(skb = kmem_cache_alloc(rtskb_slab_pool, GFP_KERNEL))) {
			printk(KERN_ERR
			       "RTnet: rtskb allocation from slab pool failed\n");
			break;
		}

		/* fill the header with zero */
		memset(skb, 0, sizeof(struct rtskb));

		skb->chain_end = skb;
		skb->pool = pool;
		skb->buf_start =
			((unsigned char *)skb) + ALIGN_RTSKB_STRUCT_LEN;
#ifdef CONFIG_XENO_DRIVERS_NET_CHECKED
		skb->buf_end = skb->buf_start + SKB_DATA_ALIGN(RTSKB_SIZE) - 1;
#endif

		if (rtdev_map_rtskb(skb) < 0) {
			kmem_cache_free(rtskb_slab_pool, skb);
			break;
		}

		rtskb_queue_tail(&pool->queue, skb);

		rtskb_amount++;
		if (rtskb_amount > rtskb_amount_max)
			rtskb_amount_max = rtskb_amount;
	}

	return i;
}

unsigned int rtskb_pool_shrink(struct rtskb_pool *pool, unsigned int rem_rtskbs)
{
	unsigned int i;
	struct rtskb *skb;

	for (i = 0; i < rem_rtskbs; i++) {
		if ((skb = rtskb_dequeue(&pool->queue)) == NULL)
			break;

		rtdev_unmap_rtskb(skb);
		kmem_cache_free(rtskb_slab_pool, skb);
		rtskb_amount--;
	}

	return i;
}

/* Note: acquires only the first skb of a chain! */
int rtskb_acquire(struct rtskb *rtskb, struct rtskb_pool *comp_pool)
{
	struct rtskb *comp_rtskb;
	struct rtskb_pool *release_pool;
	rtdm_lockctx_t context;

	rtdm_lock_get_irqsave(&comp_pool->queue.lock, context);

	comp_rtskb = __rtskb_pool_dequeue(comp_pool);
	if (!comp_rtskb) {
		rtdm_lock_put_irqrestore(&comp_pool->queue.lock, context);
		return -ENOMEM;
	}

	rtdm_lock_put(&comp_pool->queue.lock);

	comp_rtskb->chain_end = comp_rtskb;
	comp_rtskb->pool = release_pool = rtskb->pool;

	rtdm_lock_get(&release_pool->queue.lock);

	__rtskb_pool_queue_tail(release_pool, comp_rtskb);

	rtdm_lock_put_irqrestore(&release_pool->queue.lock, context);

	rtskb->pool = comp_pool;

	return 0;
}

EXPORT_SYMBOL_GPL(rtskb_acquire);

/* clone rtskb to another, allocating the new rtskb from pool */
struct rtskb *rtskb_clone(struct rtskb *rtskb, struct rtskb_pool *pool)
{
	struct rtskb *clone_rtskb;
	unsigned int total_len;

	clone_rtskb = alloc_rtskb(rtskb->end - rtskb->buf_start, pool);
	if (clone_rtskb == NULL)
		return NULL;

	/* Note: We don't clone
	- rtskb.sk
	- rtskb.xmit_stamp
       until real use cases show up. */

	clone_rtskb->priority = rtskb->priority;
	clone_rtskb->rtdev = rtskb->rtdev;
	clone_rtskb->time_stamp = rtskb->time_stamp;

	clone_rtskb->mac.raw = clone_rtskb->buf_start;
	clone_rtskb->nh.raw = clone_rtskb->buf_start;
	clone_rtskb->h.raw = clone_rtskb->buf_start;

	clone_rtskb->data += rtskb->data - rtskb->buf_start;
	clone_rtskb->tail += rtskb->tail - rtskb->buf_start;
	clone_rtskb->mac.raw += rtskb->mac.raw - rtskb->buf_start;
	clone_rtskb->nh.raw += rtskb->nh.raw - rtskb->buf_start;
	clone_rtskb->h.raw += rtskb->h.raw - rtskb->buf_start;

	clone_rtskb->protocol = rtskb->protocol;
	clone_rtskb->pkt_type = rtskb->pkt_type;

	clone_rtskb->ip_summed = rtskb->ip_summed;
	clone_rtskb->csum = rtskb->csum;

	total_len = rtskb->len + rtskb->data - rtskb->mac.raw;
	memcpy(clone_rtskb->mac.raw, rtskb->mac.raw, total_len);
	clone_rtskb->len = rtskb->len;

	return clone_rtskb;
}

EXPORT_SYMBOL_GPL(rtskb_clone);

int rtskb_pools_init(void)
{
	rtskb_slab_pool = kmem_cache_create_usercopy("rtskb_slab_pool",
						ALIGN_RTSKB_STRUCT_LEN +
						    SKB_DATA_ALIGN(RTSKB_SIZE),
						0, SLAB_HWCACHE_ALIGN,
						0, SKB_DATA_ALIGN(RTSKB_SIZE), NULL);
	if (rtskb_slab_pool == NULL)
		return -ENOMEM;

	/* reset the statistics (cache is accounted separately) */
	rtskb_pools = 0;
	rtskb_pools_max = 0;
	rtskb_amount = 0;
	rtskb_amount_max = 0;

	/* create the global rtskb pool */
	if (rtskb_module_pool_init(&global_pool, global_rtskbs) < global_rtskbs)
		goto err_out;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP)
	rtdm_lock_init(&rtcap_lock);
#endif

	return 0;

err_out:
	rtskb_pool_release(&global_pool);
	kmem_cache_destroy(rtskb_slab_pool);

	return -ENOMEM;
}

void rtskb_pools_release(void)
{
	rtskb_pool_release(&global_pool);
	kmem_cache_destroy(rtskb_slab_pool);
}