--- /dev/null
+/*
+ * Copyright (c) 2016 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <vnet/lisp-cp/packets.h>
+#include <vnet/lisp-cp/lisp_cp_messages.h>
+#include <vnet/ip/udp_packet.h>
+
+///* Returns IP ID for the packet */
+//static u16 ip_id = 0;
+//static inline u16
+//get_IP_ID()
+//{
+// ip_id++;
+// return (ip_id);
+//}
+
+u16
+udp_ip4_checksum (const void *b, u32 len, u8 *src, u8 *dst)
+{
+ const u16 *buf = b;
+ u16 *ip_src = (u16 *) src;
+ u16 *ip_dst = (u16 *) dst;
+ u32 length = len;
+ u32 sum = 0;
+
+ while (len > 1)
+ {
+ sum += *buf++;
+ if (sum & 0x80000000)
+ sum = (sum & 0xFFFF) + (sum >> 16);
+ len -= 2;
+ }
+
+ /* Add the padding if the packet length is odd */
+ if (len & 1)
+ sum += *((u8 *) buf);
+
+ /* Add the pseudo-header */
+ sum += *(ip_src++);
+ sum += *ip_src;
+
+ sum += *(ip_dst++);
+ sum += *ip_dst;
+
+ sum += clib_host_to_net_u16 (IP_PROTOCOL_UDP);
+ sum += clib_host_to_net_u16 (length);
+
+ /* Add the carries */
+ while (sum >> 16)
+ sum = (sum & 0xFFFF) + (sum >> 16);
+
+ /* Return the one's complement of sum */
+ return ((u16) (~sum));
+}
+
+u16
+udp_ip6_checksum (ip6_header_t *ip6, udp_header_t *up, u32 len)
+{
+ size_t i;
+ register const u16 *sp;
+ u32 sum;
+ union
+ {
+ struct
+ {
+ ip6_address_t ph_src;
+ ip6_address_t ph_dst;
+ u32 ph_len;
+ u8 ph_zero[3];
+ u8 ph_nxt;
+ } ph;
+ u16 pa[20];
+ } phu;
+
+ /* pseudo-header */
+ memset (&phu, 0, sizeof(phu));
+ phu.ph.ph_src = ip6->src_address;
+ phu.ph.ph_dst = ip6->dst_address;
+ phu.ph.ph_len = clib_host_to_net_u32 (len);
+ phu.ph.ph_nxt = IP_PROTOCOL_UDP;
+
+ sum = 0;
+ for (i = 0; i < sizeof(phu.pa) / sizeof(phu.pa[0]); i++)
+ sum += phu.pa[i];
+
+ sp = (const u16 *) up;
+
+ for (i = 0; i < (len & ~1); i += 2)
+ sum += *sp++;
+
+ if (len & 1)
+ sum += clib_host_to_net_u16 ((*(const u8 *) sp) << 8);
+
+ while (sum > 0xffff)
+ sum = (sum & 0xffff) + (sum >> 16);
+ sum = ~sum & 0xffff;
+
+ return (sum);
+}
+
+u16
+udp_checksum (udp_header_t *uh, u32 udp_len, void *ih, u8 version)
+{
+ switch (version)
+ {
+ case IP4:
+ return (udp_ip4_checksum (uh, udp_len,
+ ((ip4_header_t *) ih)->src_address.as_u8,
+ ((ip4_header_t *) ih)->dst_address.as_u8));
+ case IP6:
+ return (udp_ip6_checksum (ih, uh, udp_len));
+ default:
+ return ~0;
+ }
+}
+
+void *
+pkt_push_udp (vlib_main_t * vm, vlib_buffer_t *b, u16 sp, u16 dp)
+{
+ udp_header_t * uh;
+ u16 udp_len = sizeof(udp_header_t) + vlib_buffer_length_in_chain (vm, b);
+
+ uh = vlib_buffer_push_uninit (b, sizeof(*uh));
+
+ uh->src_port = clib_host_to_net_u16 (sp);
+ uh->dst_port = clib_host_to_net_u16 (dp);
+ uh->length = clib_host_to_net_u16 (udp_len);
+ uh->checksum = 0;
+ return uh;
+}
+
+void *
+pkt_push_ipv4 (vlib_main_t * vm, vlib_buffer_t *b, ip4_address_t *src,
+ ip4_address_t *dst, int proto)
+{
+ ip4_header_t * ih;
+
+ /* make some room */
+ ih = vlib_buffer_push_uninit(b, sizeof(ip4_header_t));
+
+ ih->ip_version_and_header_length = 0x45;
+ ih->tos = 0;
+ ih->length = clib_host_to_net_u16(vlib_buffer_length_in_chain (vm, b));
+
+ // iph->fragment_id = clib_host_to_net_u16(get_IP_ID ());
+
+ /* TODO: decide if we allow fragments in case of control */
+ ih->flags_and_fragment_offset = clib_host_to_net_u16(IP_DF);
+ ih->ttl = 255;
+ ih->protocol = proto;
+ ih->src_address.as_u32 = src->as_u32;
+ ih->dst_address.as_u32 = dst->as_u32;
+
+ ih->checksum = ip4_header_checksum (ih);
+ return ih;
+}
+
+void *
+pkt_push_ipv6 (vlib_main_t * vm, vlib_buffer_t *b, ip6_address_t *src,
+ ip6_address_t *dst, int proto)
+{
+ return 0;
+// struct ip6_hdr *ip6h;
+// int len;
+//
+// len = lbuf_size(b);
+// ip6h = lbuf_push_uninit(b, sizeof(struct ip6_hdr));
+//
+// ip6h->ip6_hops = 255;
+// ip6h->ip6_vfc = (IP6VERSION << 4);
+// ip6h->ip6_nxt = proto;
+// ip6h->ip6_plen = clib_host_to_net_u16(len);
+// memcpy(ip6h->ip6_src.s6_addr, src->s6_addr, sizeof(struct in6_addr));
+// memcpy(ip6h->ip6_dst.s6_addr, dst->s6_addr, sizeof(struct in6_addr));
+// return(ip6h);
+}
+
+void *
+pkt_push_ip (vlib_main_t * vm, vlib_buffer_t *b, ip_address_t *src,
+ ip_address_t *dst, u32 proto)
+{
+ if (ip_addr_version (src) != ip_addr_version(dst))
+ {
+ clib_warning("src %s and dst %s IP have different AFI! Discarding!",
+ format_ip_address, src, format_ip_address, dst);
+ return 0;
+ }
+
+ switch (ip_addr_version(src))
+ {
+ case IP4:
+ return pkt_push_ipv4 (vm, b, &ip_addr_v4(src), &ip_addr_v4(dst), proto);
+ break;
+ case IP6:
+ return pkt_push_ipv6 (vm, b, &ip_addr_v6(src), &ip_addr_v6(dst), proto);
+ break;
+ }
+
+ return 0;
+}
+
+void *
+pkt_push_udp_and_ip (vlib_main_t * vm, vlib_buffer_t *b, u16 sp, u16 dp,
+ ip_address_t *sip, ip_address_t *dip)
+{
+ u16 udpsum;
+ udp_header_t * uh;
+ void * ih;
+
+ uh = pkt_push_udp (vm, b, sp, dp);
+
+ ih = pkt_push_ip (vm, b, sip, dip, IP_PROTOCOL_UDP);
+
+ udpsum = udp_checksum (uh, clib_net_to_host_u16 (uh->length), ih,
+ ip_addr_version(sip));
+ if (udpsum == -1)
+ {
+ clib_warning("Failed UDP checksum! Discarding");
+ return 0;
+ }
+ uh->checksum = udpsum;
+ return ih;
+}
+
+void *
+pkt_push_ecm_hdr (vlib_buffer_t *b)
+{
+ ecm_hdr_t * h;
+ h = vlib_buffer_push_uninit (b, sizeof (h[0]));
+
+ memset(h, 0, sizeof(h[0]));
+ h->type = LISP_ENCAP_CONTROL_TYPE;
+ memset (h->reserved2, 0, sizeof(h->reserved2));
+
+ return h;
+}
Code Review / vpp.git / blobdiff