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         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
  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  */
 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
 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  */
 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");