src/plugins/lisp/lisp-cp/packets.c

   1 /*
   2  * Copyright (c) 2016 Cisco and/or its affiliates.
   3  * Licensed under the Apache License, Version 2.0 (the "License");
   4  * you may not use this file except in compliance with the License.
   5  * You may obtain a copy of the License at:
   6  *
   7  *     http://www.apache.org/licenses/LICENSE-2.0
   8  *
   9  * Unless required by applicable law or agreed to in writing, software
  10  * distributed under the License is distributed on an "AS IS" BASIS,
  11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12  * See the License for the specific language governing permissions and
  13  * limitations under the License.
  14  */
  15
  16 #include <lisp/lisp-cp/packets.h>
  17 #include <lisp/lisp-cp/lisp_cp_messages.h>
  18 #include <vnet/udp/udp_packet.h>
  19 #include <vnet/ip/ip4_inlines.h>
  20 #include <vnet/ip/ip6_inlines.h>
  21
  22 /* Returns IP ID for the packet */
  23 /* static u16 ip_id = 0;
  24 static inline u16
  25 get_IP_ID()
  26 {
  27     ip_id++;
  28     return (ip_id);
  29 } */
  30
  31 u16
  32 udp_ip4_checksum (const void *b, u32 len, u8 * src, u8 * dst)
  33 {
  34   const u16 *buf = b;
  35   u16 *ip_src = (u16 *) src;
  36   u16 *ip_dst = (u16 *) dst;
  37   u32 length = len;
  38   u32 sum = 0;
  39
  40   while (len > 1)
  41     {
  42       sum += *buf++;
  43       if (sum & 0x80000000)
  44         sum = (sum & 0xFFFF) + (sum >> 16);
  45       len -= 2;
  46     }
  47
  48   /* Add the padding if the packet length is odd */
  49   if (len & 1)
  50     sum += *((u8 *) buf);
  51
  52   /* Add the pseudo-header */
  53   sum += *(ip_src++);
  54   sum += *ip_src;
  55
  56   sum += *(ip_dst++);
  57   sum += *ip_dst;
  58
  59   sum += clib_host_to_net_u16 (IP_PROTOCOL_UDP);
  60   sum += clib_host_to_net_u16 (length);
  61
  62   /* Add the carries */
  63   while (sum >> 16)
  64     sum = (sum & 0xFFFF) + (sum >> 16);
  65
  66   /* Return the one's complement of sum */
  67   return ((u16) (~sum));
  68 }
  69
  70 u16
  71 udp_ip6_checksum (ip6_header_t * ip6, udp_header_t * up, u32 len)
  72 {
  73   size_t i;
  74   register const u16 *sp;
  75   u32 sum;
  76   union
  77   {
  78     struct
  79     {
  80       ip6_address_t ph_src;
  81       ip6_address_t ph_dst;
  82       u32 ph_len;
  83       u8 ph_zero[3];
  84       u8 ph_nxt;
  85     } ph;
  86     u16 pa[20];
  87   } phu;
  88
  89   /* pseudo-header */
  90   clib_memset (&phu, 0, sizeof (phu));
  91   phu.ph.ph_src = ip6->src_address;
  92   phu.ph.ph_dst = ip6->dst_address;
  93   phu.ph.ph_len = clib_host_to_net_u32 (len);
  94   phu.ph.ph_nxt = IP_PROTOCOL_UDP;
  95
  96   sum = 0;
  97   for (i = 0; i < sizeof (phu.pa) / sizeof (phu.pa[0]); i++)
  98     sum += phu.pa[i];
  99
 100   sp = (const u16 *) up;
 101
 102   for (i = 0; i < (len & ~1); i += 2)
 103     sum += *sp++;
 104
 105   if (len & 1)
 106     sum += clib_host_to_net_u16 ((*(const u8 *) sp) << 8);
 107
 108   while (sum > 0xffff)
 109     sum = (sum & 0xffff) + (sum >> 16);
 110   sum = ~sum & 0xffff;
 111
 112   return (sum);
 113 }
 114
 115 u16
 116 udp_checksum (udp_header_t * uh, u32 udp_len, void *ih, u8 version)
 117 {
 118   switch (version)
 119     {
 120     case AF_IP4:
 121       return (udp_ip4_checksum (uh, udp_len,
 122                                 ((ip4_header_t *) ih)->src_address.as_u8,
 123                                 ((ip4_header_t *) ih)->dst_address.as_u8));
 124     case AF_IP6:
 125       return (udp_ip6_checksum (ih, uh, udp_len));
 126     default:
 127       return ~0;
 128     }
 129 }
 130
 131 void *
 132 pkt_push_udp (vlib_main_t * vm, vlib_buffer_t * b, u16 sp, u16 dp)
 133 {
 134   udp_header_t *uh;
 135   u16 udp_len = sizeof (udp_header_t) + vlib_buffer_length_in_chain (vm, b);
 136
 137   uh = vlib_buffer_push_uninit (b, sizeof (*uh));
 138
 139   uh->src_port = clib_host_to_net_u16 (sp);
 140   uh->dst_port = clib_host_to_net_u16 (dp);
 141   uh->length = clib_host_to_net_u16 (udp_len);
 142   uh->checksum = 0;
 143   return uh;
 144 }
 145
 146 void *
 147 pkt_push_ip (vlib_main_t * vm, vlib_buffer_t * b, ip_address_t * src,
 148              ip_address_t * dst, u32 proto, u8 csum_offload)
 149 {
 150   if (ip_addr_version (src) != ip_addr_version (dst))
 151     {
 152       clib_warning ("src %U and dst %U IP have different AFI! Discarding!",
 153                     format_ip_address, src, format_ip_address, dst);
 154       return 0;
 155     }
 156
 157   switch (ip_addr_version (src))
 158     {
 159     case AF_IP4:
 160       return vlib_buffer_push_ip4 (vm, b, &ip_addr_v4 (src),
 161                                    &ip_addr_v4 (dst), proto, csum_offload);
 162       break;
 163     case AF_IP6:
 164       return vlib_buffer_push_ip6 (vm, b, &ip_addr_v6 (src),
 165                                    &ip_addr_v6 (dst), proto);
 166       break;
 167     }
 168
 169   return 0;
 170 }
 171
 172 void *
 173 pkt_push_udp_and_ip (vlib_main_t * vm, vlib_buffer_t * b, u16 sp, u16 dp,
 174                      ip_address_t * sip, ip_address_t * dip, u8 csum_offload)
 175 {
 176   u16 udpsum;
 177   udp_header_t *uh;
 178   void *ih;
 179
 180   uh = pkt_push_udp (vm, b, sp, dp);
 181
 182   if (csum_offload)
 183     {
 184       ih = pkt_push_ip (vm, b, sip, dip, IP_PROTOCOL_UDP, 1);
 185       vnet_buffer_offload_flags_set (b, VNET_BUFFER_OFFLOAD_F_UDP_CKSUM);
 186       vnet_buffer (b)->l3_hdr_offset = (u8 *) ih - b->data;
 187       vnet_buffer (b)->l4_hdr_offset = (u8 *) uh - b->data;
 188       uh->checksum = 0;
 189     }
 190   else
 191     {
 192       ih = pkt_push_ip (vm, b, sip, dip, IP_PROTOCOL_UDP, 0);
 193       udpsum = udp_checksum (uh, clib_net_to_host_u16 (uh->length), ih,
 194                              ip_addr_version (sip));
 195       if (udpsum == (u16) ~ 0)
 196         {
 197           clib_warning ("Failed UDP checksum! Discarding");
 198           return 0;
 199         }
 200       /* clear flags used for csum since we're not offloading */
 201       b->flags &= ~(VNET_BUFFER_F_IS_IP4 | VNET_BUFFER_F_IS_IP6);
 202       uh->checksum = udpsum;
 203     }
 204   return ih;
 205 }
 206
 207 void *
 208 pkt_push_ecm_hdr (vlib_buffer_t * b)
 209 {
 210   ecm_hdr_t *h;
 211   h = vlib_buffer_push_uninit (b, sizeof (h[0]));
 212
 213   clib_memset (h, 0, sizeof (h[0]));
 214   h->type = LISP_ENCAP_CONTROL_TYPE;
 215   clib_memset (h->reserved2, 0, sizeof (h->reserved2));
 216
 217   return h;
 218 }
 219
 220 /* *INDENT-ON* */
 221
 222 /*
 223  * fd.io coding-style-patch-verification: ON
 224  *
 225  * Local Variables:
 226  * eval: (c-set-style "gnu")
 227  * End:
 228  */