/*
   * Copyright (C) ST-Ericsson AB 2010
   * Author:	Sjur Brendeland/sjur.brandeland@stericsson.com
   * License terms: GNU General Public License (GPL) version 2
   */

  #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__

  #include <linux/string.h>
  #include <linux/skbuff.h>
  #include <linux/hardirq.h>

  #include <linux/export.h>

  #include <net/caif/cfpkt.h>

  #define PKT_PREFIX  48

  #define PKT_POSTFIX 2

  #define PKT_LEN_WHEN_EXTENDING 128

  #define PKT_ERROR(pkt, errmsg)		   \
  do {					   \
  	cfpkt_priv(pkt)->erronous = true;  \
  	skb_reset_tail_pointer(&pkt->skb); \
  	pr_warn(errmsg);		   \
  } while (0)

  
  struct cfpktq {
  	struct sk_buff_head head;
  	atomic_t count;
  	/* Lock protects count updates */
  	spinlock_t lock;
  };
  
  /*
   * net/caif/ is generic and does not
   * understand SKB, so we do this typecast
   */
  struct cfpkt {
  	struct sk_buff skb;
  };
  
  /* Private data inside SKB */
  struct cfpkt_priv_data {
  	struct dev_info dev_info;
  	bool erronous;
  };

  static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt)

  {
  	return (struct cfpkt_priv_data *) pkt->skb.cb;
  }

  static inline bool is_erronous(struct cfpkt *pkt)

  {
  	return cfpkt_priv(pkt)->erronous;
  }

  static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt)

  {
  	return &pkt->skb;
  }

  static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb)

  {
  	return (struct cfpkt *) skb;
  }

  struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt)
  {
  	struct cfpkt *pkt = skb_to_pkt(nativepkt);
  	cfpkt_priv(pkt)->erronous = false;
  	return pkt;
  }
  EXPORT_SYMBOL(cfpkt_fromnative);
  
  void *cfpkt_tonative(struct cfpkt *pkt)
  {
  	return (void *) pkt;
  }
  EXPORT_SYMBOL(cfpkt_tonative);
  
  static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx)
  {
  	struct sk_buff *skb;
  
  	if (likely(in_interrupt()))
  		skb = alloc_skb(len + pfx, GFP_ATOMIC);
  	else
  		skb = alloc_skb(len + pfx, GFP_KERNEL);
  
  	if (unlikely(skb == NULL))
  		return NULL;
  
  	skb_reserve(skb, pfx);
  	return skb_to_pkt(skb);
  }
  
  inline struct cfpkt *cfpkt_create(u16 len)
  {
  	return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX);
  }

  
  void cfpkt_destroy(struct cfpkt *pkt)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	kfree_skb(skb);
  }

  inline bool cfpkt_more(struct cfpkt *pkt)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	return skb->len > 0;
  }

  int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	if (skb_headlen(skb) >= len) {
  		memcpy(data, skb->data, len);
  		return 0;
  	}
  	return !cfpkt_extr_head(pkt, data, len) &&
  	    !cfpkt_add_head(pkt, data, len);
  }

  
  int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	u8 *from;
  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  
  	if (unlikely(len > skb->len)) {

  		PKT_ERROR(pkt, "read beyond end of packet
  ");

  		return -EPROTO;
  	}
  
  	if (unlikely(len > skb_headlen(skb))) {
  		if (unlikely(skb_linearize(skb) != 0)) {

  			PKT_ERROR(pkt, "linearize failed
  ");

  			return -EPROTO;
  		}
  	}
  	from = skb_pull(skb, len);
  	from -= len;

  	if (data)
  		memcpy(data, from, len);

  	return 0;
  }

  EXPORT_SYMBOL(cfpkt_extr_head);

  
  int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	u8 *data = dta;
  	u8 *from;
  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  
  	if (unlikely(skb_linearize(skb) != 0)) {

  		PKT_ERROR(pkt, "linearize failed
  ");

  		return -EPROTO;
  	}
  	if (unlikely(skb->data + len > skb_tail_pointer(skb))) {

  		PKT_ERROR(pkt, "read beyond end of packet
  ");

  		return -EPROTO;
  	}
  	from = skb_tail_pointer(skb) - len;
  	skb_trim(skb, skb->len - len);
  	memcpy(data, from, len);
  	return 0;
  }

  int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
  {
  	return cfpkt_add_body(pkt, NULL, len);
  }

  int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	struct sk_buff *lastskb;
  	u8 *to;
  	u16 addlen = 0;
  
  
  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  
  	lastskb = skb;
  
  	/* Check whether we need to add space at the tail */
  	if (unlikely(skb_tailroom(skb) < len)) {
  		if (likely(len < PKT_LEN_WHEN_EXTENDING))
  			addlen = PKT_LEN_WHEN_EXTENDING;
  		else
  			addlen = len;
  	}
  
  	/* Check whether we need to change the SKB before writing to the tail */
  	if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) {
  
  		/* Make sure data is writable */
  		if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) {

  			PKT_ERROR(pkt, "cow failed
  ");

  			return -EPROTO;
  		}
  		/*
  		 * Is the SKB non-linear after skb_cow_data()? If so, we are
  		 * going to add data to the last SKB, so we need to adjust
  		 * lengths of the top SKB.
  		 */
  		if (lastskb != skb) {

  			pr_warn("Packet is non-linear
  ");

  			skb->len += len;
  			skb->data_len += len;
  		}
  	}
  
  	/* All set to put the last SKB and optionally write data there. */
  	to = skb_put(lastskb, len);
  	if (likely(data))
  		memcpy(to, data, len);
  	return 0;
  }

  
  inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
  {
  	return cfpkt_add_body(pkt, &data, 1);
  }

  
  int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	struct sk_buff *lastskb;
  	u8 *to;
  	const u8 *data = data2;

  	int ret;

  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  	if (unlikely(skb_headroom(skb) < len)) {

  		PKT_ERROR(pkt, "no headroom
  ");

  		return -EPROTO;
  	}
  
  	/* Make sure data is writable */

  	ret = skb_cow_data(skb, 0, &lastskb);
  	if (unlikely(ret < 0)) {

  		PKT_ERROR(pkt, "cow failed
  ");

  		return ret;

  	}
  
  	to = skb_push(skb, len);
  	memcpy(to, data, len);
  	return 0;
  }

  EXPORT_SYMBOL(cfpkt_add_head);

  
  inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
  {
  	return cfpkt_add_body(pkt, data, len);
  }

  inline u16 cfpkt_getlen(struct cfpkt *pkt)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  	return skb->len;
  }

  inline u16 cfpkt_iterate(struct cfpkt *pkt,
  			    u16 (*iter_func)(u16, void *, u16),
  			    u16 data)
  {
  	/*
  	 * Don't care about the performance hit of linearizing,
  	 * Checksum should not be used on high-speed interfaces anyway.
  	 */
  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  	if (unlikely(skb_linearize(&pkt->skb) != 0)) {

  		PKT_ERROR(pkt, "linearize failed
  ");

  		return -EPROTO;
  	}
  	return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt));
  }

  int cfpkt_setlen(struct cfpkt *pkt, u16 len)
  {
  	struct sk_buff *skb = pkt_to_skb(pkt);
  
  
  	if (unlikely(is_erronous(pkt)))
  		return -EPROTO;
  
  	if (likely(len <= skb->len)) {
  		if (unlikely(skb->data_len))
  			___pskb_trim(skb, len);
  		else
  			skb_trim(skb, len);
  
  			return cfpkt_getlen(pkt);
  	}
  
  	/* Need to expand SKB */
  	if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len)))

  		PKT_ERROR(pkt, "skb_pad_trail failed
  ");

  
  	return cfpkt_getlen(pkt);
  }

  struct cfpkt *cfpkt_append(struct cfpkt *dstpkt,
  			     struct cfpkt *addpkt,
  			     u16 expectlen)
  {
  	struct sk_buff *dst = pkt_to_skb(dstpkt);
  	struct sk_buff *add = pkt_to_skb(addpkt);
  	u16 addlen = skb_headlen(add);
  	u16 neededtailspace;
  	struct sk_buff *tmp;
  	u16 dstlen;
  	u16 createlen;
  	if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) {

  		return dstpkt;
  	}
  	if (expectlen > addlen)
  		neededtailspace = expectlen;
  	else
  		neededtailspace = addlen;
  
  	if (dst->tail + neededtailspace > dst->end) {
  		/* Create a dumplicate of 'dst' with more tail space */

  		struct cfpkt *tmppkt;

  		dstlen = skb_headlen(dst);
  		createlen = dstlen + neededtailspace;

  		tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
  		if (tmppkt == NULL)

  			return NULL;

  		tmp = pkt_to_skb(tmppkt);

  		skb_set_tail_pointer(tmp, dstlen);
  		tmp->len = dstlen;
  		memcpy(tmp->data, dst->data, dstlen);
  		cfpkt_destroy(dstpkt);
  		dst = tmp;
  	}
  	memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add));
  	cfpkt_destroy(addpkt);
  	dst->tail += addlen;
  	dst->len += addlen;
  	return skb_to_pkt(dst);
  }

  
  struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
  {
  	struct sk_buff *skb2;
  	struct sk_buff *skb = pkt_to_skb(pkt);

  	struct cfpkt *tmppkt;

  	u8 *split = skb->data + pos;
  	u16 len2nd = skb_tail_pointer(skb) - split;
  
  	if (unlikely(is_erronous(pkt)))
  		return NULL;
  
  	if (skb->data + pos > skb_tail_pointer(skb)) {

  		PKT_ERROR(pkt, "trying to split beyond end of packet
  ");

  		return NULL;
  	}
  
  	/* Create a new packet for the second part of the data */

  	tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX,
  				  PKT_PREFIX);
  	if (tmppkt == NULL)
  		return NULL;
  	skb2 = pkt_to_skb(tmppkt);

  
  	if (skb2 == NULL)
  		return NULL;
  
  	/* Reduce the length of the original packet */
  	skb_set_tail_pointer(skb, pos);
  	skb->len = pos;
  
  	memcpy(skb2->data, split, len2nd);
  	skb2->tail += len2nd;
  	skb2->len += len2nd;
  	return skb_to_pkt(skb2);
  }

  bool cfpkt_erroneous(struct cfpkt *pkt)

  {
  	return cfpkt_priv(pkt)->erronous;
  }

  
  struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
  {
  	return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
  }

  EXPORT_SYMBOL(cfpkt_info);