static void send_arp_request(struct ofp_ifnet *dev, uint32_t gw) { char buf[sizeof(struct ofp_ether_vlan_header) + sizeof(struct ofp_arphdr)]; struct ofp_arphdr *arp; struct ofp_ether_header *e1 = (struct ofp_ether_header *)buf; struct ofp_ether_vlan_header *e2 = (struct ofp_ether_vlan_header *)buf; size_t size; odp_packet_t pkt; memset(buf, 0, sizeof(buf)); memset(e1->ether_dhost, 0xff, OFP_ETHER_ADDR_LEN); memcpy(e1->ether_shost, dev->mac, OFP_ETHER_ADDR_LEN); if (ETH_WITH_VLAN(dev)) { arp = (struct ofp_arphdr *) (e2 + 1); e2->evl_encap_proto = odp_cpu_to_be_16(OFP_ETHERTYPE_VLAN); e2->evl_tag = odp_cpu_to_be_16(dev->vlan); e2->evl_proto = odp_cpu_to_be_16(OFP_ETHERTYPE_ARP); size = sizeof(*arp) + sizeof(*e2); } else { arp = (struct ofp_arphdr *) (e1 + 1); e1->ether_type = odp_cpu_to_be_16(OFP_ETHERTYPE_ARP); size = sizeof(*arp) + sizeof(*e1); } arp->hrd = odp_cpu_to_be_16(OFP_ARPHDR_ETHER); arp->pro = odp_cpu_to_be_16(OFP_ETHERTYPE_IP); arp->hln = OFP_ETHER_ADDR_LEN; arp->pln = sizeof(struct ofp_in_addr); arp->op = odp_cpu_to_be_16(OFP_ARPOP_REQUEST); memcpy(arp->eth_src, e1->ether_shost, OFP_ETHER_ADDR_LEN); arp->ip_src = dev->ip_addr; memcpy(arp->eth_dst, e1->ether_dhost, OFP_ETHER_ADDR_LEN); arp->ip_dst = gw; pkt = ofp_packet_alloc(size); if (pkt == ODP_PACKET_INVALID) { OFP_ERR("ofp_packet_alloc failed"); return; } memcpy(odp_packet_data(pkt), buf, size); if (odp_unlikely(ofp_if_type(dev) == OFP_IFT_VXLAN)) { ofp_vxlan_send_arp_request(pkt, dev); return; } odp_packet_has_eth_set(pkt, 1); odp_packet_has_arp_set(pkt, 1); odp_packet_l2_offset_set(pkt, 0); odp_packet_l3_offset_set(pkt, size - sizeof(struct ofp_arphdr)); if (send_pkt_out(dev, pkt) == OFP_PKT_DROP) odp_packet_free(pkt); }
static int create_odp_packet_ip4(odp_packet_t *opkt, uint8_t *pkt_data, int plen, uint32_t dst_addr) { odp_pool_t pool; uint8_t *buf; odp_packet_t pkt = ODP_PACKET_INVALID; struct ofp_ip *iphdr; memset(orig_pkt_data, 0x0, sizeof(orig_pkt_data)); pool = odp_pool_lookup("packet_pool"); if (pool == ODP_POOL_INVALID) { fail_with_odp("ODP packet_pool not found\n"); return -1; } pkt = odp_packet_alloc(pool, plen); if (pkt == ODP_PACKET_INVALID) { fail_with_odp("ODP packet alloc failed"); return -1; } buf = odp_packet_data(pkt); if (odp_packet_copy_from_mem(pkt, 0, plen, pkt_data) < 0) { fail_with_odp("Packet data copy failed\n"); return -1; }; iphdr = (struct ofp_ip *)&buf[OFP_ETHER_HDR_LEN]; /* changes to the default packet. Recalculate ip checksum */ if (dst_addr) { iphdr->ip_dst.s_addr = dst_addr; iphdr->ip_sum = 0; iphdr->ip_sum = ofp_cksum_buffer((uint16_t *)iphdr, iphdr->ip_hl<<2); } /* END OF changes to the default packet */ odp_packet_has_eth_set(pkt, 1); odp_packet_has_ipv4_set(pkt, 1); odp_packet_l2_offset_set(pkt, 0); odp_packet_l3_offset_set(pkt, OFP_ETHER_HDR_LEN); odp_packet_l4_offset_set(pkt, OFP_ETHER_HDR_LEN + (iphdr->ip_hl<<2)); *opkt = pkt; memcpy(orig_pkt_data, pkt_data, plen); return 0; }
static int create_odp_packet_ip6(odp_packet_t *opkt, uint8_t *pkt_data, int plen) { odp_pool_t pool; odp_packet_t pkt = ODP_PACKET_INVALID; memset(orig_pkt_data, 0x0, sizeof(orig_pkt_data)); pool = odp_pool_lookup("packet_pool"); if (pool == ODP_POOL_INVALID) { fail_with_odp("ODP packet_pool not found\n"); return -1; } pkt = odp_packet_alloc(pool, plen); if (pkt == ODP_PACKET_INVALID) { fail_with_odp("ODP packet alloc failed"); return -1; } if (odp_packet_copy_from_mem(pkt, 0, plen, pkt_data) < 0) { fail_with_odp("Packet data copy failed\n"); return -1; }; odp_packet_has_eth_set(pkt, 1); odp_packet_has_l2_set(pkt, 1); odp_packet_has_ipv6_set(pkt, 1); odp_packet_l2_offset_set(pkt, 0); odp_packet_l3_offset_set(pkt, OFP_ETHER_HDR_LEN); *opkt = pkt; odp_packet_copy_to_mem(pkt, 0, plen, orig_pkt_data); return 0; }
odp_packet_t create_ipv4_packet(stream_db_entry_t *stream, uint8_t *dmac, odp_pool_t pkt_pool) { ipsec_cache_entry_t *entry = NULL; odp_packet_t pkt; uint8_t *base; uint8_t *data; odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_ipv4hdr_t *inner_ip = NULL; odph_ahhdr_t *ah = NULL; odph_esphdr_t *esp = NULL; odph_icmphdr_t *icmp; stream_pkt_hdr_t *test; unsigned i; if (stream->input.entry) entry = stream->input.entry; else if (stream->output.entry) entry = stream->output.entry; /* Get packet */ pkt = odp_packet_alloc(pkt_pool, 0); if (ODP_PACKET_INVALID == pkt) return ODP_PACKET_INVALID; base = odp_packet_data(pkt); data = odp_packet_data(pkt); /* Ethernet */ odp_packet_has_eth_set(pkt, 1); eth = (odph_ethhdr_t *)data; data += sizeof(*eth); memset((char *)eth->src.addr, (0x80 | stream->id), ODPH_ETHADDR_LEN); memcpy((char *)eth->dst.addr, dmac, ODPH_ETHADDR_LEN); eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); /* IPv4 */ odp_packet_has_ipv4_set(pkt, 1); ip = (odph_ipv4hdr_t *)data; data += sizeof(*ip); /* Wait until almost finished to fill in mutable fields */ memset((char *)ip, 0, sizeof(*ip)); ip->ver_ihl = 0x45; ip->id = odp_cpu_to_be_16(stream->id); /* Outer IP header in tunnel mode */ if (entry && entry->mode == IPSEC_SA_MODE_TUNNEL && (entry == stream->input.entry)) { ip->proto = ODPH_IPV4; ip->src_addr = odp_cpu_to_be_32(entry->tun_src_ip); ip->dst_addr = odp_cpu_to_be_32(entry->tun_dst_ip); } else { ip->proto = ODPH_IPPROTO_ICMP; ip->src_addr = odp_cpu_to_be_32(stream->src_ip); ip->dst_addr = odp_cpu_to_be_32(stream->dst_ip); } /* AH (if specified) */ if (entry && (entry == stream->input.entry) && (ODP_AUTH_ALG_NULL != entry->ah.alg)) { if (ODP_AUTH_ALG_MD5_96 != entry->ah.alg) abort(); ah = (odph_ahhdr_t *)data; data += sizeof(*ah); data += entry->ah.icv_len; memset((char *)ah, 0, sizeof(*ah) + entry->ah.icv_len); ah->ah_len = 1 + (entry->ah.icv_len / 4); ah->spi = odp_cpu_to_be_32(entry->ah.spi); ah->seq_no = odp_cpu_to_be_32(stream->input.ah_seq++); } /* ESP (if specified) */ if (entry && (entry == stream->input.entry) && (ODP_CIPHER_ALG_NULL != entry->esp.alg)) { if (ODP_CIPHER_ALG_3DES_CBC != entry->esp.alg) abort(); esp = (odph_esphdr_t *)data; data += sizeof(*esp); data += entry->esp.iv_len; esp->spi = odp_cpu_to_be_32(entry->esp.spi); esp->seq_no = odp_cpu_to_be_32(stream->input.esp_seq++); RAND_bytes(esp->iv, 8); } /* Inner IP header in tunnel mode */ if (entry && (entry == stream->input.entry) && (entry->mode == IPSEC_SA_MODE_TUNNEL)) { inner_ip = (odph_ipv4hdr_t *)data; memset((char *)inner_ip, 0, sizeof(*inner_ip)); inner_ip->ver_ihl = 0x45; inner_ip->proto = ODPH_IPPROTO_ICMP; inner_ip->id = odp_cpu_to_be_16(stream->id); inner_ip->ttl = 64; inner_ip->tos = 0; inner_ip->frag_offset = 0; inner_ip->src_addr = odp_cpu_to_be_32(stream->src_ip); inner_ip->dst_addr = odp_cpu_to_be_32(stream->dst_ip); inner_ip->chksum = odp_chksum(inner_ip, sizeof(*inner_ip)); data += sizeof(*inner_ip); } /* ICMP header so we can see it on wireshark */ icmp = (odph_icmphdr_t *)data; data += sizeof(*icmp); icmp->type = ICMP_ECHO; icmp->code = 0; icmp->un.echo.id = odp_cpu_to_be_16(0x1234); icmp->un.echo.sequence = odp_cpu_to_be_16(stream->created); /* Packet payload of incrementing bytes */ test = (stream_pkt_hdr_t *)data; data += sizeof(*test); test->magic = odp_cpu_to_be_64(STREAM_MAGIC); for (i = 0; i < stream->length; i++) *data++ = (uint8_t)i; /* Close ICMP */ icmp->chksum = 0; icmp->chksum = odp_chksum(icmp, data - (uint8_t *)icmp); /* Close ESP if specified */ if (esp) { int payload_len = data - (uint8_t *)icmp; uint8_t *encrypt_start = (uint8_t *)icmp; if (entry->mode == IPSEC_SA_MODE_TUNNEL) { payload_len = data - (uint8_t *)inner_ip; encrypt_start = (uint8_t *)inner_ip; } int encrypt_len; odph_esptrl_t *esp_t; DES_key_schedule ks1, ks2, ks3; uint8_t iv[8]; memcpy(iv, esp->iv, sizeof(iv)); encrypt_len = ESP_ENCODE_LEN(payload_len + sizeof(*esp_t), entry->esp.block_len); memset(data, 0, encrypt_len - payload_len); data += encrypt_len - payload_len; esp_t = (odph_esptrl_t *)(data) - 1; esp_t->pad_len = encrypt_len - payload_len - sizeof(*esp_t); esp_t->next_header = ip->proto; ip->proto = ODPH_IPPROTO_ESP; DES_set_key((DES_cblock *)&entry->esp.key.data[0], &ks1); DES_set_key((DES_cblock *)&entry->esp.key.data[8], &ks2); DES_set_key((DES_cblock *)&entry->esp.key.data[16], &ks3); DES_ede3_cbc_encrypt(encrypt_start, encrypt_start, encrypt_len, &ks1, &ks2, &ks3, (DES_cblock *)iv, 1); } /* Since ESP can pad we can now fix IP length */ ip->tot_len = odp_cpu_to_be_16(data - (uint8_t *)ip); /* Close AH if specified */ if (ah) { uint8_t hash[EVP_MAX_MD_SIZE]; int auth_len = data - (uint8_t *)ip; ah->next_header = ip->proto; ip->proto = ODPH_IPPROTO_AH; HMAC(EVP_md5(), entry->ah.key.data, entry->ah.key.length, (uint8_t *)ip, auth_len, hash, NULL); memcpy(ah->icv, hash, 12); } /* Correct set packet length offsets */ odp_packet_push_tail(pkt, data - base); odp_packet_l2_offset_set(pkt, (uint8_t *)eth - base); odp_packet_l3_offset_set(pkt, (uint8_t *)ip - base); odp_packet_l4_offset_set(pkt, ((uint8_t *)ip - base) + sizeof(*ip)); /* Now fill in final IP header fields */ ip->ttl = 64; ip->tos = 0; ip->frag_offset = 0; ip->chksum = 0; odph_ipv4_csum_update(pkt); return pkt; }
enum ofp_return_code ofp_nd6_ns_output(struct ofp_ifnet *dev, uint8_t *daddr6, uint8_t *taddr6) { size_t size = 0; size_t iter = 0; struct ofp_ether_header *e1; struct ofp_ether_vlan_header *e2; struct ofp_ip6_hdr *ip6hdr; struct ofp_icmp6_hdr *icmp; odp_packet_t pkt; if (dev->vlan) size = sizeof(struct ofp_ether_vlan_header); else size = sizeof(struct ofp_ether_header); size += sizeof(struct ofp_ip6_hdr) + sizeof(struct ofp_icmp6_hdr) + 16 /*target addr*/ + 8; /* option*/ pkt = ofp_packet_alloc(size); if (pkt == ODP_PACKET_INVALID) return OFP_PKT_DROP; odp_packet_has_eth_set(pkt, 1); odp_packet_l2_offset_set(pkt, iter); if (dev->vlan) { e2 = (struct ofp_ether_vlan_header *)odp_packet_l2_ptr(pkt, NULL); iter += sizeof(*e2); memset(e2->evl_dhost, 0xff, OFP_ETHER_ADDR_LEN); memcpy(e2->evl_shost, dev->mac, OFP_ETHER_ADDR_LEN); e2->evl_encap_proto = odp_cpu_to_be_16(OFP_ETHERTYPE_VLAN); e2->evl_tag = odp_cpu_to_be_16(dev->vlan); e2->evl_proto = odp_cpu_to_be_16(OFP_ETHERTYPE_IPV6); } else { e1 = (struct ofp_ether_header *)odp_packet_l2_ptr(pkt, NULL); iter += sizeof(*e1); memset(e1->ether_dhost, 0xff, OFP_ETHER_ADDR_LEN); memcpy(e1->ether_shost, dev->mac, OFP_ETHER_ADDR_LEN); e1->ether_type = odp_cpu_to_be_16(OFP_ETHERTYPE_IPV6); } odp_packet_l3_offset_set(pkt, iter); ip6hdr = (struct ofp_ip6_hdr *)odp_packet_l3_ptr(pkt, NULL); iter += sizeof(*ip6hdr); ip6hdr->ofp_ip6_flow = 0; ip6hdr->ofp_ip6_vfc = OFP_IPV6_VERSION; ip6hdr->ofp_ip6_plen = odp_cpu_to_be_16(32); /*sizeof(*icmp) + sizeof taddr + 8*/ /* for checksum calculation */ ip6hdr->ofp_ip6_nxt = 0; ip6hdr->ofp_ip6_hlim = OFP_IPPROTO_ICMPV6; /* XXX should be multicast address*/ memcpy(ip6hdr->ip6_src.ofp_s6_addr, dev->ip6_addr, 16); if (ofp_ip6_is_set(daddr6)) memcpy(ip6hdr->ip6_dst.ofp_s6_addr, daddr6, 16); else { /* Solicited-node multicast address */ ip6hdr->ip6_dst.ofp_s6_addr16[0] = OFP_IPV6_ADDR_INT16_MLL; ip6hdr->ip6_dst.ofp_s6_addr16[1] = 0; ip6hdr->ip6_dst.ofp_s6_addr32[1] = 0; ip6hdr->ip6_dst.ofp_s6_addr32[2] = OFP_IPV6_ADDR_INT32_ONE; ip6hdr->ip6_dst.ofp_s6_addr32[3] = *((uint32_t *)taddr6 + 3); ip6hdr->ip6_dst.ofp_s6_addr[12] = 0xff; } odp_packet_l4_offset_set(pkt, iter); icmp = (struct ofp_icmp6_hdr *)odp_packet_l4_ptr(pkt, NULL); iter += sizeof(*icmp) + 8 /* option */; icmp->icmp6_type = OFP_ND_NEIGHBOR_SOLICIT; icmp->icmp6_code = 0; icmp->icmp6_cksum = 0; icmp->ofp_icmp6_data32[0] = 0; /* Reserved */ memcpy(&icmp->ofp_icmp6_data8[4], taddr6, 16); /* Option: Source link-layer address */ icmp->ofp_icmp6_data8[20] = OFP_ND_OPT_SOURCE_LINKADDR; icmp->ofp_icmp6_data8[21] = 1; /* 8 octets */ memcpy(&icmp->ofp_icmp6_data8[22], dev->mac, 6); icmp->icmp6_cksum = ofp_cksum_buffer(&ip6hdr->ofp_ip6_plen, 68); ip6hdr->ofp_ip6_nxt = OFP_IPPROTO_ICMPV6; ip6hdr->ofp_ip6_hlim = 255; if (send_pkt_out(dev, pkt) == OFP_PKT_DROP) { OFP_ERR("Drop packet"); odp_packet_free(pkt); return OFP_PKT_DROP; } return OFP_PKT_PROCESSED; }