Orange Pi5 kernel

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