xref: /openbmc/linux/net/ife/ife.c (revision 2839a639)
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 
ife_encode(struct sk_buff * skb,u16 metalen)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 
ife_decode(struct sk_buff * skb,u16 * metalen)66 void *ife_decode(struct sk_buff *skb, u16 *metalen)
67 {
68 	struct ifeheadr *ifehdr;
69 	int total_pull;
70 	u16 ifehdrln;
71 
72 	if (!pskb_may_pull(skb, skb->dev->hard_header_len + IFE_METAHDRLEN))
73 		return NULL;
74 
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 	ifehdr = (struct ifeheadr *)(skb->data + skb->dev->hard_header_len);
86 	skb_set_mac_header(skb, total_pull);
87 	__skb_pull(skb, total_pull);
88 	*metalen = ifehdrln - IFE_METAHDRLEN;
89 
90 	return &ifehdr->tlv_data;
91 }
92 EXPORT_SYMBOL_GPL(ife_decode);
93 
94 struct meta_tlvhdr {
95 	__be16 type;
96 	__be16 len;
97 };
98 
__ife_tlv_meta_valid(const unsigned char * skbdata,const unsigned char * ifehdr_end)99 static bool __ife_tlv_meta_valid(const unsigned char *skbdata,
100 				 const unsigned char *ifehdr_end)
101 {
102 	const struct meta_tlvhdr *tlv;
103 	u16 tlvlen;
104 
105 	if (unlikely(skbdata + sizeof(*tlv) > ifehdr_end))
106 		return false;
107 
108 	tlv = (const struct meta_tlvhdr *)skbdata;
109 	tlvlen = ntohs(tlv->len);
110 
111 	/* tlv length field is inc header, check on minimum */
112 	if (tlvlen < NLA_HDRLEN)
113 		return false;
114 
115 	/* overflow by NLA_ALIGN check */
116 	if (NLA_ALIGN(tlvlen) < tlvlen)
117 		return false;
118 
119 	if (unlikely(skbdata + NLA_ALIGN(tlvlen) > ifehdr_end))
120 		return false;
121 
122 	return true;
123 }
124 
125 /* Caller takes care of presenting data in network order
126  */
ife_tlv_meta_decode(void * skbdata,const void * ifehdr_end,u16 * attrtype,u16 * dlen,u16 * totlen)127 void *ife_tlv_meta_decode(void *skbdata, const void *ifehdr_end, u16 *attrtype,
128 			  u16 *dlen, u16 *totlen)
129 {
130 	struct meta_tlvhdr *tlv;
131 
132 	if (!__ife_tlv_meta_valid(skbdata, ifehdr_end))
133 		return NULL;
134 
135 	tlv = (struct meta_tlvhdr *)skbdata;
136 	*dlen = ntohs(tlv->len) - NLA_HDRLEN;
137 	*attrtype = ntohs(tlv->type);
138 
139 	if (totlen)
140 		*totlen = nla_total_size(*dlen);
141 
142 	return skbdata + sizeof(struct meta_tlvhdr);
143 }
144 EXPORT_SYMBOL_GPL(ife_tlv_meta_decode);
145 
ife_tlv_meta_next(void * skbdata)146 void *ife_tlv_meta_next(void *skbdata)
147 {
148 	struct meta_tlvhdr *tlv = (struct meta_tlvhdr *) skbdata;
149 	u16 tlvlen = ntohs(tlv->len);
150 
151 	tlvlen = NLA_ALIGN(tlvlen);
152 
153 	return skbdata + tlvlen;
154 }
155 EXPORT_SYMBOL_GPL(ife_tlv_meta_next);
156 
157 /* Caller takes care of presenting data in network order
158  */
ife_tlv_meta_encode(void * skbdata,u16 attrtype,u16 dlen,const void * dval)159 int ife_tlv_meta_encode(void *skbdata, u16 attrtype, u16 dlen, const void *dval)
160 {
161 	__be32 *tlv = (__be32 *) (skbdata);
162 	u16 totlen = nla_total_size(dlen);	/*alignment + hdr */
163 	char *dptr = (char *) tlv + NLA_HDRLEN;
164 	u32 htlv = attrtype << 16 | (dlen + NLA_HDRLEN);
165 
166 	*tlv = htonl(htlv);
167 	memset(dptr, 0, totlen - NLA_HDRLEN);
168 	memcpy(dptr, dval, dlen);
169 
170 	return totlen;
171 }
172 EXPORT_SYMBOL_GPL(ife_tlv_meta_encode);
173 
174 MODULE_AUTHOR("Jamal Hadi Salim <jhs@mojatatu.com>");
175 MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
176 MODULE_DESCRIPTION("Inter-FE LFB action");
177 MODULE_LICENSE("GPL");
178