src/vnet/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 <vnet/lisp-cp/packets.h>
  17 #include <vnet/lisp-cp/lisp_cp_messages.h>
  18 #include <vnet/ip/udp_packet.h>
  19
  20 /* Returns IP ID for the packet */
  21 /* static u16 ip_id = 0;
  22 static inline u16
  23 get_IP_ID()
  24 {
  25     ip_id++;
  26     return (ip_id);
  27 } */
  28
  29 u16
  30 udp_ip4_checksum (const void *b, u32 len, u8 * src, u8 * dst)
  31 {
  32   const u16 *buf = b;
  33   u16 *ip_src = (u16 *) src;
  34   u16 *ip_dst = (u16 *) dst;
  35   u32 length = len;
  36   u32 sum = 0;
  37
  38   while (len > 1)
  39     {
  40       sum += *buf++;
  41       if (sum & 0x80000000)
  42         sum = (sum & 0xFFFF) + (sum >> 16);
  43       len -= 2;
  44     }
  45
  46   /* Add the padding if the packet length is odd */
  47   if (len & 1)
  48     sum += *((u8 *) buf);
  49
  50   /* Add the pseudo-header */
  51   sum += *(ip_src++);
  52   sum += *ip_src;
  53
  54   sum += *(ip_dst++);
  55   sum += *ip_dst;
  56
  57   sum += clib_host_to_net_u16 (IP_PROTOCOL_UDP);
  58   sum += clib_host_to_net_u16 (length);
  59
  60   /* Add the carries */
  61   while (sum >> 16)
  62     sum = (sum & 0xFFFF) + (sum >> 16);
  63
  64   /* Return the one's complement of sum */
  65   return ((u16) (~sum));
  66 }
  67
  68 u16
  69 udp_ip6_checksum (ip6_header_t * ip6, udp_header_t * up, u32 len)
  70 {
  71   size_t i;
  72   register const u16 *sp;
  73   u32 sum;
  74   union
  75   {
  76     struct
  77     {
  78       ip6_address_t ph_src;
  79       ip6_address_t ph_dst;
  80       u32 ph_len;
  81       u8 ph_zero[3];
  82       u8 ph_nxt;
  83     } ph;
  84     u16 pa[20];
  85   } phu;
  86
  87   /* pseudo-header */
  88   memset (&phu, 0, sizeof (phu));
  89   phu.ph.ph_src = ip6->src_address;
  90   phu.ph.ph_dst = ip6->dst_address;
  91   phu.ph.ph_len = clib_host_to_net_u32 (len);
  92   phu.ph.ph_nxt = IP_PROTOCOL_UDP;
  93
  94   sum = 0;
  95   for (i = 0; i < sizeof (phu.pa) / sizeof (phu.pa[0]); i++)
  96     sum += phu.pa[i];
  97
  98   sp = (const u16 *) up;
  99
 100   for (i = 0; i < (len & ~1); i += 2)
 101     sum += *sp++;
 102
 103   if (len & 1)
 104     sum += clib_host_to_net_u16 ((*(const u8 *) sp) << 8);
 105
 106   while (sum > 0xffff)
 107     sum = (sum & 0xffff) + (sum >> 16);
 108   sum = ~sum & 0xffff;
 109
 110   return (sum);
 111 }
 112
 113 u16
 114 udp_checksum (udp_header_t * uh, u32 udp_len, void *ih, u8 version)
 115 {
 116   switch (version)
 117     {
 118     case IP4:
 119       return (udp_ip4_checksum (uh, udp_len,
 120                                 ((ip4_header_t *) ih)->src_address.as_u8,
 121                                 ((ip4_header_t *) ih)->dst_address.as_u8));
 122     case IP6:
 123       return (udp_ip6_checksum (ih, uh, udp_len));
 124     default:
 125       return ~0;
 126     }
 127 }
 128
 129 void *
 130 pkt_push_udp (vlib_main_t * vm, vlib_buffer_t * b, u16 sp, u16 dp)
 131 {
 132   udp_header_t *uh;
 133   u16 udp_len = sizeof (udp_header_t) + vlib_buffer_length_in_chain (vm, b);
 134
 135   uh = vlib_buffer_push_uninit (b, sizeof (*uh));
 136
 137   uh->src_port = clib_host_to_net_u16 (sp);
 138   uh->dst_port = clib_host_to_net_u16 (dp);
 139   uh->length = clib_host_to_net_u16 (udp_len);
 140   uh->checksum = 0;
 141   return uh;
 142 }
 143
 144 void *
 145 pkt_push_ipv4 (vlib_main_t * vm, vlib_buffer_t * b, ip4_address_t * src,
 146                ip4_address_t * dst, int proto)
 147 {
 148   ip4_header_t *ih;
 149
 150   /* make some room */
 151   ih = vlib_buffer_push_uninit (b, sizeof (ip4_header_t));
 152
 153   ih->ip_version_and_header_length = 0x45;
 154   ih->tos = 0;
 155   ih->length = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b));
 156
 157   /* iph->fragment_id = clib_host_to_net_u16(get_IP_ID ()); */
 158
 159   /* TODO: decide if we allow fragments in case of control */
 160   ih->flags_and_fragment_offset = clib_host_to_net_u16 (IP_DF);
 161   ih->ttl = 255;
 162   ih->protocol = proto;
 163   ih->src_address.as_u32 = src->as_u32;
 164   ih->dst_address.as_u32 = dst->as_u32;
 165
 166   ih->checksum = ip4_header_checksum (ih);
 167   return ih;
 168 }
 169
 170 void *
 171 pkt_push_ipv6 (vlib_main_t * vm, vlib_buffer_t * b, ip6_address_t * src,
 172                ip6_address_t * dst, int proto)
 173 {
 174   ip6_header_t *ip6h;
 175   u16 payload_length;
 176
 177   /* make some room */
 178   ip6h = vlib_buffer_push_uninit (b, sizeof (ip6_header_t));
 179
 180   ip6h->ip_version_traffic_class_and_flow_label =
 181     clib_host_to_net_u32 (0x6 << 28);
 182
 183   /* calculate ip6 payload length */
 184   payload_length = vlib_buffer_length_in_chain (vm, b);
 185   payload_length -= sizeof (*ip6h);
 186
 187   ip6h->payload_length = clib_host_to_net_u16 (payload_length);
 188
 189   ip6h->hop_limit = 0xff;
 190   ip6h->protocol = proto;
 191   clib_memcpy (ip6h->src_address.as_u8, src->as_u8,
 192                sizeof (ip6h->src_address));
 193   clib_memcpy (ip6h->dst_address.as_u8, dst->as_u8,
 194                sizeof (ip6h->src_address));
 195
 196   return ip6h;
 197 }
 198
 199 void *
 200 pkt_push_ip (vlib_main_t * vm, vlib_buffer_t * b, ip_address_t * src,
 201              ip_address_t * dst, u32 proto)
 202 {
 203   if (ip_addr_version (src) != ip_addr_version (dst))
 204     {
 205       clib_warning ("src %U and dst %U IP have different AFI! Discarding!",
 206                     format_ip_address, src, format_ip_address, dst);
 207       return 0;
 208     }
 209
 210   switch (ip_addr_version (src))
 211     {
 212     case IP4:
 213       return pkt_push_ipv4 (vm, b, &ip_addr_v4 (src), &ip_addr_v4 (dst),
 214                             proto);
 215       break;
 216     case IP6:
 217       return pkt_push_ipv6 (vm, b, &ip_addr_v6 (src), &ip_addr_v6 (dst),
 218                             proto);
 219       break;
 220     }
 221
 222   return 0;
 223 }
 224
 225 void *
 226 pkt_push_udp_and_ip (vlib_main_t * vm, vlib_buffer_t * b, u16 sp, u16 dp,
 227                      ip_address_t * sip, ip_address_t * dip)
 228 {
 229   u16 udpsum;
 230   udp_header_t *uh;
 231   void *ih;
 232
 233   uh = pkt_push_udp (vm, b, sp, dp);
 234
 235   ih = pkt_push_ip (vm, b, sip, dip, IP_PROTOCOL_UDP);
 236
 237   udpsum = udp_checksum (uh, clib_net_to_host_u16 (uh->length), ih,
 238                          ip_addr_version (sip));
 239   if (udpsum == (u16) ~ 0)
 240     {
 241       clib_warning ("Failed UDP checksum! Discarding");
 242       return 0;
 243     }
 244   uh->checksum = udpsum;
 245   return ih;
 246 }
 247
 248 void *
 249 pkt_push_ecm_hdr (vlib_buffer_t * b)
 250 {
 251   ecm_hdr_t *h;
 252   h = vlib_buffer_push_uninit (b, sizeof (h[0]));
 253
 254   memset (h, 0, sizeof (h[0]));
 255   h->type = LISP_ENCAP_CONTROL_TYPE;
 256   memset (h->reserved2, 0, sizeof (h->reserved2));
 257
 258   return h;
 259 }
 260
 261 /* *INDENT-ON* */
 262
 263 /*
 264  * fd.io coding-style-patch-verification: ON
 265  *
 266  * Local Variables:
 267  * eval: (c-set-style "gnu")
 268  * End:
 269  */