net/ife/ife.c

   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
  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         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
  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
 124 /* Caller takes care of presenting data in network order
 125  */
 126 void *ife_tlv_meta_decode(void *skbdata, const void *ifehdr_end, u16 *attrtype,
 127                           u16 *dlen, u16 *totlen)
 128 {
 129         struct meta_tlvhdr *tlv;
 130
 131         if (!__ife_tlv_meta_valid(skbdata, ifehdr_end))
 132                 return NULL;
 133
 134         tlv = (struct meta_tlvhdr *)skbdata;
 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>");
 174 MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
 175 MODULE_DESCRIPTION("Inter-FE LFB action");
 176 MODULE_LICENSE("GPL");