ife.c 4.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176
  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. #include <linux/types.h>
  15. #include <linux/kernel.h>
  16. #include <linux/string.h>
  17. #include <linux/errno.h>
  18. #include <linux/skbuff.h>
  19. #include <linux/rtnetlink.h>
  20. #include <linux/module.h>
  21. #include <linux/init.h>
  22. #include <net/net_namespace.h>
  23. #include <net/netlink.h>
  24. #include <net/pkt_sched.h>
  25. #include <linux/etherdevice.h>
  26. #include <net/ife.h>
  27. struct ifeheadr {
  28. __be16 metalen;
  29. u8 tlv_data[];
  30. };
  31. void *ife_encode(struct sk_buff *skb, u16 metalen)
  32. {
  33. /* OUTERHDR:TOTMETALEN:{TLVHDR:Metadatum:TLVHDR..}:ORIGDATA
  34. * where ORIGDATA = original ethernet header ...
  35. */
  36. int hdrm = metalen + IFE_METAHDRLEN;
  37. int total_push = hdrm + skb->dev->hard_header_len;
  38. struct ifeheadr *ifehdr;
  39. struct ethhdr *iethh; /* inner ether header */
  40. int skboff = 0;
  41. int err;
  42. err = skb_cow_head(skb, total_push);
  43. if (unlikely(err))
  44. return NULL;
  45. iethh = (struct ethhdr *) skb->data;
  46. __skb_push(skb, total_push);
  47. memcpy(skb->data, iethh, skb->dev->hard_header_len);
  48. skb_reset_mac_header(skb);
  49. skboff += skb->dev->hard_header_len;
  50. /* total metadata length */
  51. ifehdr = (struct ifeheadr *) (skb->data + skboff);
  52. metalen += IFE_METAHDRLEN;
  53. ifehdr->metalen = htons(metalen);
  54. return ifehdr->tlv_data;
  55. }
  56. EXPORT_SYMBOL_GPL(ife_encode);
  57. void *ife_decode(struct sk_buff *skb, u16 *metalen)
  58. {
  59. struct ifeheadr *ifehdr;
  60. int total_pull;
  61. u16 ifehdrln;
  62. if (!pskb_may_pull(skb, skb->dev->hard_header_len + IFE_METAHDRLEN))
  63. return NULL;
  64. ifehdr = (struct ifeheadr *) (skb->data + skb->dev->hard_header_len);
  65. ifehdrln = ntohs(ifehdr->metalen);
  66. total_pull = skb->dev->hard_header_len + ifehdrln;
  67. if (unlikely(ifehdrln < 2))
  68. return NULL;
  69. if (unlikely(!pskb_may_pull(skb, total_pull)))
  70. return NULL;
  71. skb_set_mac_header(skb, total_pull);
  72. __skb_pull(skb, total_pull);
  73. *metalen = ifehdrln - IFE_METAHDRLEN;
  74. return &ifehdr->tlv_data;
  75. }
  76. EXPORT_SYMBOL_GPL(ife_decode);
  77. struct meta_tlvhdr {
  78. __be16 type;
  79. __be16 len;
  80. };
  81. static bool __ife_tlv_meta_valid(const unsigned char *skbdata,
  82. const unsigned char *ifehdr_end)
  83. {
  84. const struct meta_tlvhdr *tlv;
  85. u16 tlvlen;
  86. if (unlikely(skbdata + sizeof(*tlv) > ifehdr_end))
  87. return false;
  88. tlv = (const struct meta_tlvhdr *)skbdata;
  89. tlvlen = ntohs(tlv->len);
  90. /* tlv length field is inc header, check on minimum */
  91. if (tlvlen < NLA_HDRLEN)
  92. return false;
  93. /* overflow by NLA_ALIGN check */
  94. if (NLA_ALIGN(tlvlen) < tlvlen)
  95. return false;
  96. if (unlikely(skbdata + NLA_ALIGN(tlvlen) > ifehdr_end))
  97. return false;
  98. return true;
  99. }
  100. /* Caller takes care of presenting data in network order
  101. */
  102. void *ife_tlv_meta_decode(void *skbdata, const void *ifehdr_end, u16 *attrtype,
  103. u16 *dlen, u16 *totlen)
  104. {
  105. struct meta_tlvhdr *tlv;
  106. if (!__ife_tlv_meta_valid(skbdata, ifehdr_end))
  107. return NULL;
  108. tlv = (struct meta_tlvhdr *)skbdata;
  109. *dlen = ntohs(tlv->len) - NLA_HDRLEN;
  110. *attrtype = ntohs(tlv->type);
  111. if (totlen)
  112. *totlen = nla_total_size(*dlen);
  113. return skbdata + sizeof(struct meta_tlvhdr);
  114. }
  115. EXPORT_SYMBOL_GPL(ife_tlv_meta_decode);
  116. void *ife_tlv_meta_next(void *skbdata)
  117. {
  118. struct meta_tlvhdr *tlv = (struct meta_tlvhdr *) skbdata;
  119. u16 tlvlen = ntohs(tlv->len);
  120. tlvlen = NLA_ALIGN(tlvlen);
  121. return skbdata + tlvlen;
  122. }
  123. EXPORT_SYMBOL_GPL(ife_tlv_meta_next);
  124. /* Caller takes care of presenting data in network order
  125. */
  126. int ife_tlv_meta_encode(void *skbdata, u16 attrtype, u16 dlen, const void *dval)
  127. {
  128. __be32 *tlv = (__be32 *) (skbdata);
  129. u16 totlen = nla_total_size(dlen); /*alignment + hdr */
  130. char *dptr = (char *) tlv + NLA_HDRLEN;
  131. u32 htlv = attrtype << 16 | (dlen + NLA_HDRLEN);
  132. *tlv = htonl(htlv);
  133. memset(dptr, 0, totlen - NLA_HDRLEN);
  134. memcpy(dptr, dval, dlen);
  135. return totlen;
  136. }
  137. EXPORT_SYMBOL_GPL(ife_tlv_meta_encode);
  138. MODULE_AUTHOR("Jamal Hadi Salim <jhs@mojatatu.com>");
  139. MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
  140. MODULE_DESCRIPTION("Inter-FE LFB action");
  141. MODULE_LICENSE("GPL");