[mirror_ubuntu-jammy-kernel.git] / net / ife / ife.c

/*
 * net/ife/ife.c - Inter-FE protocol based on ForCES WG InterFE LFB
 * Copyright (c) 2015 Jamal Hadi Salim <jhs@mojatatu.com>
 * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
 *
 * Refer to: draft-ietf-forces-interfelfb-03 and netdev01 paper:
 * "Distributing Linux Traffic Control Classifier-Action Subsystem"
 * Authors: Jamal Hadi Salim and Damascene M. Joachimpillai
 *
 * 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.
 */

#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/skbuff.h>
#include <linux/rtnetlink.h>
#include <linux/module.h>
#include <linux/init.h>
#include <net/net_namespace.h>
#include <net/netlink.h>
#include <net/pkt_sched.h>
#include <linux/etherdevice.h>
#include <net/ife.h>

struct ifeheadr {
	__be16 metalen;
	u8 tlv_data[];
};

void *ife_encode(struct sk_buff *skb, u16 metalen)
{
	/* OUTERHDR:TOTMETALEN:{TLVHDR:Metadatum:TLVHDR..}:ORIGDATA
	 * where ORIGDATA = original ethernet header ...
	 */
	int hdrm = metalen + IFE_METAHDRLEN;
	int total_push = hdrm + skb->dev->hard_header_len;
	struct ifeheadr *ifehdr;
	struct ethhdr *iethh;	/* inner ether header */
	int skboff = 0;
	int err;

	err = skb_cow_head(skb, total_push);
	if (unlikely(err))
		return NULL;

	iethh = (struct ethhdr *) skb->data;

	__skb_push(skb, total_push);
	memcpy(skb->data, iethh, skb->dev->hard_header_len);
	skb_reset_mac_header(skb);
	skboff += skb->dev->hard_header_len;

	/* total metadata length */
	ifehdr = (struct ifeheadr *) (skb->data + skboff);
	metalen += IFE_METAHDRLEN;
	ifehdr->metalen = htons(metalen);

	return ifehdr->tlv_data;
}
EXPORT_SYMBOL_GPL(ife_encode);

void *ife_decode(struct sk_buff *skb, u16 *metalen)
{
	struct ifeheadr *ifehdr;
	int total_pull;
	u16 ifehdrln;

	if (!pskb_may_pull(skb, skb->dev->hard_header_len + IFE_METAHDRLEN))
		return NULL;

	ifehdr = (struct ifeheadr *) (skb->data + skb->dev->hard_header_len);
	ifehdrln = ntohs(ifehdr->metalen);
	total_pull = skb->dev->hard_header_len + ifehdrln;

	if (unlikely(ifehdrln < 2))
		return NULL;

	if (unlikely(!pskb_may_pull(skb, total_pull)))
		return NULL;

	skb_set_mac_header(skb, total_pull);
	__skb_pull(skb, total_pull);
	*metalen = ifehdrln - IFE_METAHDRLEN;

	return &ifehdr->tlv_data;
}
EXPORT_SYMBOL_GPL(ife_decode);

struct meta_tlvhdr {
	__be16 type;
	__be16 len;
};

static bool __ife_tlv_meta_valid(const unsigned char *skbdata,
				 const unsigned char *ifehdr_end)
{
	const struct meta_tlvhdr *tlv;
	u16 tlvlen;

	if (unlikely(skbdata + sizeof(*tlv) > ifehdr_end))
		return false;

	tlv = (const struct meta_tlvhdr *)skbdata;
	tlvlen = ntohs(tlv->len);

	/* tlv length field is inc header, check on minimum */
	if (tlvlen < NLA_HDRLEN)
		return false;

	/* overflow by NLA_ALIGN check */
	if (NLA_ALIGN(tlvlen) < tlvlen)
		return false;

	if (unlikely(skbdata + NLA_ALIGN(tlvlen) > ifehdr_end))
		return false;

	return true;
}

/* Caller takes care of presenting data in network order
 */
void *ife_tlv_meta_decode(void *skbdata, const void *ifehdr_end, u16 *attrtype,
			  u16 *dlen, u16 *totlen)
{
	struct meta_tlvhdr *tlv;

	if (!__ife_tlv_meta_valid(skbdata, ifehdr_end))
		return NULL;

	tlv = (struct meta_tlvhdr *)skbdata;
	*dlen = ntohs(tlv->len) - NLA_HDRLEN;
	*attrtype = ntohs(tlv->type);

	if (totlen)
		*totlen = nla_total_size(*dlen);

	return skbdata + sizeof(struct meta_tlvhdr);
}
EXPORT_SYMBOL_GPL(ife_tlv_meta_decode);

void *ife_tlv_meta_next(void *skbdata)
{
	struct meta_tlvhdr *tlv = (struct meta_tlvhdr *) skbdata;
	u16 tlvlen = ntohs(tlv->len);

	tlvlen = NLA_ALIGN(tlvlen);

	return skbdata + tlvlen;
}
EXPORT_SYMBOL_GPL(ife_tlv_meta_next);

/* Caller takes care of presenting data in network order
 */
int ife_tlv_meta_encode(void *skbdata, u16 attrtype, u16 dlen, const void *dval)
{
	__be32 *tlv = (__be32 *) (skbdata);
	u16 totlen = nla_total_size(dlen);	/*alignment + hdr */
	char *dptr = (char *) tlv + NLA_HDRLEN;
	u32 htlv = attrtype << 16 | (dlen + NLA_HDRLEN);

	*tlv = htonl(htlv);
	memset(dptr, 0, totlen - NLA_HDRLEN);
	memcpy(dptr, dval, dlen);

	return totlen;
}
EXPORT_SYMBOL_GPL(ife_tlv_meta_encode);

MODULE_AUTHOR("Jamal Hadi Salim <jhs@mojatatu.com>");
MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
MODULE_DESCRIPTION("Inter-FE LFB action");
MODULE_LICENSE("GPL");
Commit	Line	Data
1ce84604 YG	1	/*
	2	* net/ife/ife.c - Inter-FE protocol based on ForCES WG InterFE LFB
	3	* Copyright (c) 2015 Jamal Hadi Salim <jhs@mojatatu.com>
	4	* Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
	5	*
	6	* Refer to: draft-ietf-forces-interfelfb-03 and netdev01 paper:
	7	* "Distributing Linux Traffic Control Classifier-Action Subsystem"
	8	* Authors: Jamal Hadi Salim and Damascene M. Joachimpillai
	9	*
	10	* This program is free software; you can redistribute it and/or modify
	11	* it under the terms of the GNU General Public License as published by
	12	* the Free Software Foundation.
	13	*/
	14
	15	#include <linux/types.h>
	16	#include <linux/kernel.h>
	17	#include <linux/string.h>
	18	#include <linux/errno.h>
	19	#include <linux/skbuff.h>
	20	#include <linux/rtnetlink.h>
	21	#include <linux/module.h>
	22	#include <linux/init.h>
	23	#include <net/net_namespace.h>
	24	#include <net/netlink.h>
	25	#include <net/pkt_sched.h>
	26	#include <linux/etherdevice.h>
	27	#include <net/ife.h>
	28
	29	struct ifeheadr {
	30	__be16 metalen;
	31	u8 tlv_data[];
	32	};
	33
	34	void ife_encode(struct sk_buff skb, u16 metalen)
	35	{
	36	/* OUTERHDR:TOTMETALEN:{TLVHDR:Metadatum:TLVHDR..}:ORIGDATA
	37	* where ORIGDATA = original ethernet header ...
	38	*/
	39	int hdrm = metalen + IFE_METAHDRLEN;
	40	int total_push = hdrm + skb->dev->hard_header_len;
	41	struct ifeheadr *ifehdr;
	42	struct ethhdr iethh; / inner ether header */
	43	int skboff = 0;
	44	int err;
	45
	46	err = skb_cow_head(skb, total_push);
	47	if (unlikely(err))
	48	return NULL;
	49
	50	iethh = (struct ethhdr *) skb->data;
	51
	52	__skb_push(skb, total_push);
	53	memcpy(skb->data, iethh, skb->dev->hard_header_len);
	54	skb_reset_mac_header(skb);
	55	skboff += skb->dev->hard_header_len;
	56
	57	/* total metadata length */
	58	ifehdr = (struct ifeheadr *) (skb->data + skboff);
	59	metalen += IFE_METAHDRLEN;
	60	ifehdr->metalen = htons(metalen);
	61
	62	return ifehdr->tlv_data;
	63	}
	64	EXPORT_SYMBOL_GPL(ife_encode);
65
66	void ife_decode(struct sk_buff skb, u16 *metalen)
67	{
68	struct ifeheadr *ifehdr;
69	int total_pull;
70	u16 ifehdrln;
71
d57493d6 AA	72	if (!pskb_may_pull(skb, skb->dev->hard_header_len + IFE_METAHDRLEN))
	73	return NULL;
	74
1ce84604 YG	75	ifehdr = (struct ifeheadr *) (skb->data + skb->dev->hard_header_len);
	76	ifehdrln = ntohs(ifehdr->metalen);
	77	total_pull = skb->dev->hard_header_len + ifehdrln;
	78
	79	if (unlikely(ifehdrln < 2))
	80	return NULL;
	81
	82	if (unlikely(!pskb_may_pull(skb, total_pull)))
	83	return NULL;
	84
	85	skb_set_mac_header(skb, total_pull);
	86	__skb_pull(skb, total_pull);
	87	*metalen = ifehdrln - IFE_METAHDRLEN;
	88
	89	return &ifehdr->tlv_data;
	90	}
	91	EXPORT_SYMBOL_GPL(ife_decode);
	92
	93	struct meta_tlvhdr {
	94	__be16 type;
	95	__be16 len;
	96	};
	97
cc74eddd AA	98	static bool __ife_tlv_meta_valid(const unsigned char *skbdata,
	99	const unsigned char *ifehdr_end)
	100	{
	101	const struct meta_tlvhdr *tlv;
	102	u16 tlvlen;
	103
	104	if (unlikely(skbdata + sizeof(*tlv) > ifehdr_end))
	105	return false;
	106
	107	tlv = (const struct meta_tlvhdr *)skbdata;
	108	tlvlen = ntohs(tlv->len);
	109
	110	/* tlv length field is inc header, check on minimum */
	111	if (tlvlen < NLA_HDRLEN)
	112	return false;
	113
	114	/* overflow by NLA_ALIGN check */
	115	if (NLA_ALIGN(tlvlen) < tlvlen)
	116	return false;
	117
	118	if (unlikely(skbdata + NLA_ALIGN(tlvlen) > ifehdr_end))
	119	return false;
	120
	121	return true;
	122	}
	123
1ce84604 YG	124	/* Caller takes care of presenting data in network order
1ce84604 YG	125	*/
cc74eddd AA	126	void ife_tlv_meta_decode(void skbdata, const void ifehdr_end, u16 attrtype,
cc74eddd AA	127	u16 dlen, u16 totlen)
1ce84604	128	{
cc74eddd AA	129	struct meta_tlvhdr *tlv;
	130
	131	if (!__ife_tlv_meta_valid(skbdata, ifehdr_end))
	132	return NULL;
1ce84604	133
cc74eddd	134	tlv = (struct meta_tlvhdr *)skbdata;
1ce84604 YG	135	*dlen = ntohs(tlv->len) - NLA_HDRLEN;
	136	*attrtype = ntohs(tlv->type);
	137
	138	if (totlen)
	139	totlen = nla_total_size(dlen);
	140
	141	return skbdata + sizeof(struct meta_tlvhdr);
	142	}
	143	EXPORT_SYMBOL_GPL(ife_tlv_meta_decode);
	144
	145	void ife_tlv_meta_next(void skbdata)
	146	{
	147	struct meta_tlvhdr tlv = (struct meta_tlvhdr ) skbdata;
	148	u16 tlvlen = ntohs(tlv->len);
	149
	150	tlvlen = NLA_ALIGN(tlvlen);
	151
	152	return skbdata + tlvlen;
	153	}
	154	EXPORT_SYMBOL_GPL(ife_tlv_meta_next);
	155
	156	/* Caller takes care of presenting data in network order
	157	*/
	158	int ife_tlv_meta_encode(void skbdata, u16 attrtype, u16 dlen, const void dval)
	159	{
	160	__be32 tlv = (__be32 ) (skbdata);
	161	u16 totlen = nla_total_size(dlen); /alignment + hdr /
	162	char dptr = (char ) tlv + NLA_HDRLEN;
	163	u32 htlv = attrtype << 16 \| (dlen + NLA_HDRLEN);
	164
	165	*tlv = htonl(htlv);
	166	memset(dptr, 0, totlen - NLA_HDRLEN);
	167	memcpy(dptr, dval, dlen);
	168
	169	return totlen;
	170	}
	171	EXPORT_SYMBOL_GPL(ife_tlv_meta_encode);
	172
	173	MODULE_AUTHOR("Jamal Hadi Salim <jhs@mojatatu.com>");
f1fd20c3	174	MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
1ce84604 YG	175	MODULE_DESCRIPTION("Inter-FE LFB action");
1ce84604 YG	176	MODULE_LICENSE("GPL");