static enum ofp_return_code ofp_ip_output_add_eth(odp_packet_t pkt, struct ip_out *odata) { uint8_t l2_size = 0; void *l2_addr; if (!odata->gw) /* link local */ odata->gw = odata->ip->ip_dst.s_addr; if (ETH_WITHOUT_VLAN(odata->vlan, odata->out_port)) l2_size = sizeof(struct ofp_ether_header); else l2_size = sizeof(struct ofp_ether_vlan_header); if (odp_packet_l2_offset(pkt) + l2_size == odp_packet_l3_offset(pkt)) { l2_addr = odp_packet_l2_ptr(pkt, NULL); } else if (odp_packet_l3_offset(pkt) >= l2_size) { odp_packet_l2_offset_set(pkt, odp_packet_l3_offset(pkt) - l2_size); l2_addr = odp_packet_l2_ptr(pkt, NULL); } else { l2_addr = odp_packet_push_head(pkt, l2_size - odp_packet_l3_offset(pkt)); odp_packet_l2_offset_set(pkt, 0); odp_packet_l3_offset_set(pkt, l2_size); odp_packet_l4_offset_set(pkt, l2_size + (odata->ip->ip_hl<<2)); } if (odp_unlikely(l2_addr == NULL)) { OFP_DBG("l2_addr == NULL"); return OFP_PKT_DROP; } if (ETH_WITHOUT_VLAN(odata->vlan, odata->out_port)) { struct ofp_ether_header *eth = (struct ofp_ether_header *)l2_addr; uint32_t addr = odp_be_to_cpu_32(odata->ip->ip_dst.s_addr); if (OFP_IN_MULTICAST(addr)) { eth->ether_dhost[0] = 0x01; eth->ether_dhost[1] = 0x00; eth->ether_dhost[2] = 0x5e; eth->ether_dhost[3] = (addr >> 16) & 0x7f; eth->ether_dhost[4] = (addr >> 8) & 0xff; eth->ether_dhost[5] = addr & 0xff; } else if (odata->dev_out->ip_addr == odata->ip->ip_dst.s_addr) { odata->is_local_address = 1; ofp_copy_mac(eth->ether_dhost, &(odata->dev_out->mac[0])); } else if (ofp_get_mac(odata->dev_out, odata->gw, eth->ether_dhost) < 0) { send_arp_request(odata->dev_out, odata->gw); return ofp_arp_save_ipv4_pkt(pkt, odata->nh, odata->gw, odata->dev_out); } ofp_copy_mac(eth->ether_shost, odata->dev_out->mac); eth->ether_type = odp_cpu_to_be_16(OFP_ETHERTYPE_IP); } else {
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); }
/** * Set up an icmp packet * * @param pool Buffer pool to create packet in * * @return Handle of created packet * @retval ODP_PACKET_INVALID Packet could not be created */ static odp_packet_t pack_icmp_pkt(odp_pool_t pool) { odp_packet_t pkt; char *buf; odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_icmphdr_t *icmp; struct timeval tval; uint8_t *tval_d; unsigned short seq; args->appl.payload = 56; pkt = odp_packet_alloc(pool, args->appl.payload + ODPH_ICMPHDR_LEN + ODPH_IPV4HDR_LEN + ODPH_ETHHDR_LEN); if (pkt == ODP_PACKET_INVALID) return pkt; buf = odp_packet_data(pkt); /* ether */ odp_packet_l2_offset_set(pkt, 0); eth = (odph_ethhdr_t *)buf; memcpy((char *)eth->src.addr, args->appl.srcmac.addr, ODPH_ETHADDR_LEN); memcpy((char *)eth->dst.addr, args->appl.dstmac.addr, ODPH_ETHADDR_LEN); eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); /* ip */ odp_packet_l3_offset_set(pkt, ODPH_ETHHDR_LEN); ip = (odph_ipv4hdr_t *)(buf + ODPH_ETHHDR_LEN); ip->dst_addr = odp_cpu_to_be_32(args->appl.dstip); ip->src_addr = odp_cpu_to_be_32(args->appl.srcip); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; ip->tot_len = odp_cpu_to_be_16(args->appl.payload + ODPH_ICMPHDR_LEN + ODPH_IPV4HDR_LEN); ip->proto = ODPH_IPPROTO_ICMP; seq = odp_atomic_fetch_add_u64(&counters.seq, 1) % 0xffff; ip->id = odp_cpu_to_be_16(seq); ip->chksum = 0; odph_ipv4_csum_update(pkt); /* icmp */ icmp = (odph_icmphdr_t *)(buf + ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); icmp->type = ICMP_ECHO; icmp->code = 0; icmp->un.echo.id = 0; icmp->un.echo.sequence = ip->id; tval_d = (uint8_t *)(buf + ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN + ODPH_ICMPHDR_LEN); /* TODO This should be changed to use an * ODP timer API once one exists. */ gettimeofday(&tval, NULL); memcpy(tval_d, &tval, sizeof(struct timeval)); icmp->chksum = 0; icmp->chksum = odp_chksum(icmp, args->appl.payload + ODPH_ICMPHDR_LEN); return 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 odp_packet_t pack_pkt(odp_pool_t pool, odph_ethaddr_t eth_src, odph_ethaddr_t eth_dst, uint32_t ip_src, uint32_t ip_dst, int payload, int is_ip) { odp_packet_t pkt; char *buf; odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_udphdr_t *udp; static unsigned short seq = 0; pkt = odp_packet_alloc(pool, payload + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN + ODPH_ETHHDR_LEN); if (pkt == ODP_PACKET_INVALID) return pkt; buf = odp_packet_data(pkt); /* ether */ odp_packet_l2_offset_set(pkt, 0); eth = (odph_ethhdr_t *)buf; memcpy((char *)eth->src.addr, eth_src.addr, ODPH_ETHADDR_LEN); memcpy((char *)eth->dst.addr, eth_dst.addr, ODPH_ETHADDR_LEN); if ( is_ip ) { eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); odp_packet_l3_offset_set(pkt, ODPH_ETHHDR_LEN); ip = (odph_ipv4hdr_t *)(buf + ODPH_ETHHDR_LEN); ip->dst_addr = odp_cpu_to_be_32(ip_dst); ip->src_addr = odp_cpu_to_be_32(ip_src); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; ip->tot_len = odp_cpu_to_be_16(payload + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN); ip->proto = ODPH_IPPROTO_UDP; seq++; ip->id = odp_cpu_to_be_16(seq); ip->chksum = 0; odph_ipv4_csum_update(pkt); odp_packet_l4_offset_set(pkt, ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp = (odph_udphdr_t *)(buf + ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp->src_port = 0; udp->dst_port = 0; udp->length = odp_cpu_to_be_16(payload + ODPH_UDPHDR_LEN); udp->chksum = 0; udp->chksum = odp_cpu_to_be_16(odph_ipv4_udp_chksum(pkt)); } else { eth->type = odp_cpu_to_be_16(0xbeef); } return pkt; }
/** * set up an udp packet * * @param pool Buffer pool to create packet in * * @return Handle of created packet * @retval ODP_PACKET_INVALID Packet could not be created */ static odp_packet_t pack_udp_pkt(odp_pool_t pool) { odp_packet_t pkt; char *buf; odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_udphdr_t *udp; unsigned short seq; pkt = odp_packet_alloc(pool, args->appl.payload + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN + ODPH_ETHHDR_LEN); if (pkt == ODP_PACKET_INVALID) return pkt; buf = odp_packet_data(pkt); /* ether */ odp_packet_l2_offset_set(pkt, 0); eth = (odph_ethhdr_t *)buf; memcpy((char *)eth->src.addr, args->appl.srcmac.addr, ODPH_ETHADDR_LEN); memcpy((char *)eth->dst.addr, args->appl.dstmac.addr, ODPH_ETHADDR_LEN); eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); /* ip */ odp_packet_l3_offset_set(pkt, ODPH_ETHHDR_LEN); ip = (odph_ipv4hdr_t *)(buf + ODPH_ETHHDR_LEN); ip->dst_addr = odp_cpu_to_be_32(args->appl.dstip); ip->src_addr = odp_cpu_to_be_32(args->appl.srcip); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; ip->tot_len = odp_cpu_to_be_16(args->appl.payload + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN); ip->proto = ODPH_IPPROTO_UDP; seq = odp_atomic_fetch_add_u64(&counters.seq, 1) % 0xFFFF; ip->id = odp_cpu_to_be_16(seq); ip->chksum = 0; odph_ipv4_csum_update(pkt); /* udp */ odp_packet_l4_offset_set(pkt, ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp = (odph_udphdr_t *)(buf + ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp->src_port = 0; udp->dst_port = 0; udp->length = odp_cpu_to_be_16(args->appl.payload + ODPH_UDPHDR_LEN); udp->chksum = 0; udp->chksum = odp_cpu_to_be_16(odph_ipv4_udp_chksum(pkt)); return pkt; }
static uint32_t pktio_init_packet(odp_packet_t pkt) { odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_udphdr_t *udp; char *buf; uint16_t seq; uint8_t mac[ODPH_ETHADDR_LEN] = {0}; int pkt_len = odp_packet_len(pkt); buf = odp_packet_data(pkt); /* Ethernet */ odp_packet_l2_offset_set(pkt, 0); eth = (odph_ethhdr_t *)buf; memcpy(eth->src.addr, mac, ODPH_ETHADDR_LEN); memcpy(eth->dst.addr, mac, ODPH_ETHADDR_LEN); eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); /* IP */ odp_packet_l3_offset_set(pkt, ODPH_ETHHDR_LEN); ip = (odph_ipv4hdr_t *)(buf + ODPH_ETHHDR_LEN); ip->dst_addr = odp_cpu_to_be_32(0x0a000064); ip->src_addr = odp_cpu_to_be_32(0x0a000001); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; ip->tot_len = odp_cpu_to_be_16(pkt_len - ODPH_ETHHDR_LEN); ip->ttl = 128; ip->proto = ODPH_IPPROTO_UDP; seq = odp_atomic_fetch_inc_u32(&ip_seq); ip->id = odp_cpu_to_be_16(seq); ip->chksum = 0; odph_ipv4_csum_update(pkt); /* UDP */ odp_packet_l4_offset_set(pkt, ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp = (odph_udphdr_t *)(buf + ODPH_ETHHDR_LEN + ODPH_IPV4HDR_LEN); udp->src_port = odp_cpu_to_be_16(12049); udp->dst_port = odp_cpu_to_be_16(12050); udp->length = odp_cpu_to_be_16(pkt_len - ODPH_ETHHDR_LEN - ODPH_IPV4HDR_LEN); udp->chksum = 0; return pktio_pkt_set_seq(pkt); }
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; }
static enum ofp_return_code ofp_fragment_pkt(odp_packet_t pkt, struct ip_out *odata) { struct ofp_ip *ip, *ip_new; int pl_len, seg_len, pl_pos, flen, hwlen; uint16_t frag, frag_new; uint8_t *payload_new; uint32_t payload_offset; odp_packet_t pkt_new; int ret = OFP_PKT_PROCESSED; ip = (struct ofp_ip *)odp_packet_l3_ptr(pkt, NULL); /* * Copy fragment IP options into a separate buffer, which is * copied into each fragment, except the first one. */ int ip_hlen = ip->ip_hl<<2; int iopts_len = ip_hlen - sizeof(struct ofp_ip); uint8_t fopts[(iopts_len+3)&0xfffc]; uint8_t *iopts = (uint8_t *)(ip + 1); int iopts_pos = 0, fopts_len = 0; while (iopts_pos < iopts_len) { int opt_len = 1; switch (OFP_IPOPT_NUMBER(iopts[iopts_pos])) { case OFP_IPOPT_EOL: case OFP_IPOPT_NOP: break; default: opt_len = iopts[iopts_pos+1]; if (opt_len > iopts_len - iopts_pos) opt_len = iopts_len - iopts_pos; if (OFP_IPOPT_COPIED(iopts[iopts_pos])) { memcpy(fopts + fopts_len, iopts + iopts_pos, opt_len); fopts_len += opt_len; } } iopts_pos += opt_len; } while (fopts_len & 3) fopts[fopts_len++] = 0; pl_len = odp_be_to_cpu_16(ip->ip_len) - ip_hlen; pl_pos = 0; frag = odp_be_to_cpu_16(ip->ip_off); payload_offset = odp_packet_l3_offset(pkt) + ip_hlen; OFP_UPDATE_PACKET_STAT(tx_eth_frag, 1); int first = 1; while (pl_pos < pl_len) { int f_ip_hl = ip->ip_hl; if (!first) f_ip_hl = (sizeof(struct ofp_ip) + fopts_len) >> 2; int f_ip_hlen = f_ip_hl<<2; seg_len = (odata->dev_out->if_mtu - f_ip_hlen) & 0xfff8; flen = (pl_len - pl_pos) > seg_len ? seg_len : (pl_len - pl_pos); hwlen = flen + f_ip_hlen; pkt_new = ofp_packet_alloc(hwlen); if (pkt_new == ODP_PACKET_INVALID) { OFP_ERR("ofp_packet_alloc failed"); return OFP_PKT_DROP; } odp_packet_user_ptr_set(pkt_new, odp_packet_user_ptr(pkt)); *ofp_packet_user_area(pkt_new) = *ofp_packet_user_area(pkt); odp_packet_l2_offset_set(pkt_new, 0); odp_packet_l3_offset_set(pkt_new, 0); ip_new = odp_packet_l3_ptr(pkt_new, NULL); *ip_new = *ip; if (first) memcpy(ip_new + 1, ip + 1, ip_hlen - sizeof(struct ofp_ip)); else memcpy(ip_new + 1, fopts, fopts_len); ip_new->ip_hl = f_ip_hl; payload_new = (uint8_t *)ip_new + f_ip_hlen; if (odp_packet_copy_to_mem(pkt, payload_offset + pl_pos, flen, payload_new) < 0) { OFP_ERR("odp_packet_copy_to_mem failed"); odp_packet_free(pkt_new); return OFP_PKT_DROP; }; ip_new->ip_len = odp_cpu_to_be_16(flen + f_ip_hlen); frag_new = frag + pl_pos/8; pl_pos += flen; if (pl_pos < pl_len) frag_new |= OFP_IP_MF; ip_new->ip_off = odp_cpu_to_be_16(frag_new); odata->ip = ip_new; odata->insert_checksum = 1; ret = ofp_ip_output_continue(pkt_new, odata); if (ret == OFP_PKT_DROP) { odp_packet_free(pkt_new); return OFP_PKT_DROP; } first = 0; } odp_packet_free(pkt); return OFP_PKT_PROCESSED; }
odp_packet_t create_packet(odp_pool_t pool, bool vlan, odp_atomic_u32_t *seq, bool flag_udp) { uint32_t seqno; odph_ethhdr_t *ethhdr; odph_udphdr_t *udp; odph_tcphdr_t *tcp; odph_ipv4hdr_t *ip; uint8_t payload_len; char src_mac[ODPH_ETHADDR_LEN] = {0}; char dst_mac[ODPH_ETHADDR_LEN] = {0}; uint32_t addr = 0; uint32_t mask; int offset; odp_packet_t pkt; int packet_len = 0; payload_len = sizeof(cls_test_packet_t); packet_len += ODPH_ETHHDR_LEN; packet_len += ODPH_IPV4HDR_LEN; if (flag_udp) packet_len += ODPH_UDPHDR_LEN; else packet_len += ODPH_TCPHDR_LEN; packet_len += payload_len; if (vlan) packet_len += ODPH_VLANHDR_LEN; pkt = odp_packet_alloc(pool, packet_len); CU_ASSERT_FATAL(pkt != ODP_PACKET_INVALID); /* Ethernet Header */ offset = 0; odp_packet_l2_offset_set(pkt, offset); ethhdr = (odph_ethhdr_t *)odp_packet_l2_ptr(pkt, NULL); memcpy(ethhdr->src.addr, src_mac, ODPH_ETHADDR_LEN); memcpy(ethhdr->dst.addr, dst_mac, ODPH_ETHADDR_LEN); offset += sizeof(odph_ethhdr_t); if (vlan) { /* Default vlan header */ uint8_t *parseptr; odph_vlanhdr_t *vlan; vlan = (odph_vlanhdr_t *)(ðhdr->type); parseptr = (uint8_t *)vlan; vlan->tci = odp_cpu_to_be_16(0); vlan->tpid = odp_cpu_to_be_16(ODPH_ETHTYPE_VLAN); offset += sizeof(odph_vlanhdr_t); parseptr += sizeof(odph_vlanhdr_t); uint16be_t *type = (uint16be_t *)(void *)parseptr; *type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); } else { ethhdr->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); } odp_packet_l3_offset_set(pkt, offset); /* ipv4 */ ip = (odph_ipv4hdr_t *)odp_packet_l3_ptr(pkt, NULL); parse_ipv4_string(CLS_DEFAULT_SADDR, &addr, &mask); ip->dst_addr = odp_cpu_to_be_32(addr); parse_ipv4_string(CLS_DEFAULT_DADDR, &addr, &mask); ip->src_addr = odp_cpu_to_be_32(addr); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; if (flag_udp) ip->tot_len = odp_cpu_to_be_16(ODPH_UDPHDR_LEN + payload_len + ODPH_IPV4HDR_LEN); else ip->tot_len = odp_cpu_to_be_16(ODPH_TCPHDR_LEN + payload_len + ODPH_IPV4HDR_LEN); ip->ttl = 128; if (flag_udp) ip->proto = ODPH_IPPROTO_UDP; else ip->proto = ODPH_IPPROTO_TCP; seqno = odp_atomic_fetch_inc_u32(seq); ip->id = odp_cpu_to_be_16(seqno); ip->chksum = 0; ip->chksum = odph_ipv4_csum_update(pkt); offset += ODPH_IPV4HDR_LEN; /* udp */ if (flag_udp) { odp_packet_l4_offset_set(pkt, offset); udp = (odph_udphdr_t *)odp_packet_l4_ptr(pkt, NULL); udp->src_port = odp_cpu_to_be_16(CLS_DEFAULT_SPORT); udp->dst_port = odp_cpu_to_be_16(CLS_DEFAULT_DPORT); udp->length = odp_cpu_to_be_16(payload_len + ODPH_UDPHDR_LEN); udp->chksum = 0; } else { odp_packet_l4_offset_set(pkt, offset); tcp = (odph_tcphdr_t *)odp_packet_l4_ptr(pkt, NULL); tcp->src_port = odp_cpu_to_be_16(CLS_DEFAULT_SPORT); tcp->dst_port = odp_cpu_to_be_16(CLS_DEFAULT_DPORT); tcp->hl = ODPH_TCPHDR_LEN / 4; /* TODO: checksum field has to be updated */ tcp->cksm = 0; } /* set pkt sequence number */ cls_pkt_set_seq(pkt); return pkt; }
/* * Generate a single test packet for transmission. */ static odp_packet_t pktio_create_packet(void) { odp_packet_t pkt; odph_ethhdr_t *eth; odph_ipv4hdr_t *ip; odph_udphdr_t *udp; char *buf; uint16_t seq; uint32_t offset; pkt_head_t pkt_hdr; size_t payload_len; uint8_t mac[ODPH_ETHADDR_LEN] = {0}; payload_len = sizeof(pkt_hdr) + gbl_args->args.pkt_len; pkt = odp_packet_alloc(transmit_pkt_pool, payload_len + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN + ODPH_ETHHDR_LEN); if (pkt == ODP_PACKET_INVALID) return ODP_PACKET_INVALID; buf = odp_packet_data(pkt); /* Ethernet */ offset = 0; odp_packet_l2_offset_set(pkt, offset); eth = (odph_ethhdr_t *)buf; memcpy(eth->src.addr, mac, ODPH_ETHADDR_LEN); memcpy(eth->dst.addr, mac, ODPH_ETHADDR_LEN); eth->type = odp_cpu_to_be_16(ODPH_ETHTYPE_IPV4); /* IP */ offset += ODPH_ETHHDR_LEN; odp_packet_l3_offset_set(pkt, ODPH_ETHHDR_LEN); ip = (odph_ipv4hdr_t *)(buf + ODPH_ETHHDR_LEN); ip->dst_addr = odp_cpu_to_be_32(0); ip->src_addr = odp_cpu_to_be_32(0); ip->ver_ihl = ODPH_IPV4 << 4 | ODPH_IPV4HDR_IHL_MIN; ip->tot_len = odp_cpu_to_be_16(payload_len + ODPH_UDPHDR_LEN + ODPH_IPV4HDR_LEN); ip->ttl = 128; ip->proto = ODPH_IPPROTO_UDP; seq = odp_atomic_fetch_inc_u32(&ip_seq); ip->id = odp_cpu_to_be_16(seq); ip->chksum = 0; odph_ipv4_csum_update(pkt); /* UDP */ offset += ODPH_IPV4HDR_LEN; odp_packet_l4_offset_set(pkt, offset); udp = (odph_udphdr_t *)(buf + offset); udp->src_port = odp_cpu_to_be_16(0); udp->dst_port = odp_cpu_to_be_16(0); udp->length = odp_cpu_to_be_16(payload_len + ODPH_UDPHDR_LEN); udp->chksum = 0; /* payload */ offset += ODPH_UDPHDR_LEN; pkt_hdr.magic = TEST_HDR_MAGIC; if (odp_packet_copydata_in(pkt, offset, sizeof(pkt_hdr), &pkt_hdr) != 0) LOG_ABORT("Failed to generate test packet.\n"); 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; }
static enum ofp_return_code ofp_fragment_pkt(odp_packet_t pkt, struct ofp_ifnet *dev_out, uint8_t is_local_address) { struct ofp_ip *ip, *ip_new; uint16_t vlan = dev_out->vlan; int tot_len, pl_len, seg_len, pl_pos, flen, hwlen; uint16_t frag, frag_new; uint8_t *payload_new; uint32_t payload_offset; odp_pool_t pkt_pool; odp_packet_t pkt_new; struct ofp_ether_header *eth, *eth_new; struct ofp_ether_vlan_header *eth_vlan, *eth_new_vlan; int ret = OFP_PKT_PROCESSED; if (!vlan) eth = odp_packet_l2_ptr(pkt, NULL); else eth_vlan = odp_packet_l2_ptr(pkt, NULL); ip = (struct ofp_ip *)odp_packet_l3_ptr(pkt, NULL); pkt_pool = ofp_packet_pool; tot_len = odp_be_to_cpu_16(ip->ip_len); pl_len = tot_len - (ip->ip_hl<<2); seg_len = (dev_out->if_mtu - sizeof(struct ofp_ip)) & 0xfff8; pl_pos = 0; frag = odp_be_to_cpu_16(ip->ip_off); payload_offset = odp_packet_l3_offset(pkt) + (ip->ip_hl<<2); OFP_UPDATE_PACKET_STAT(tx_eth_frag, 1); while (pl_pos < pl_len) { flen = (pl_len - pl_pos) > seg_len ? seg_len : (pl_len - pl_pos); hwlen = flen + sizeof(struct ofp_ip) + (vlan ? sizeof(struct ofp_ether_vlan_header) : sizeof(struct ofp_ether_header)); pkt_new = odp_packet_alloc(pkt_pool, hwlen); if (pkt_new == ODP_PACKET_INVALID) { OFP_ERR("odp_packet_alloc failed"); return OFP_PKT_DROP; } odp_packet_user_ptr_set(pkt_new, odp_packet_user_ptr(pkt)); odp_packet_l2_offset_set(pkt_new, 0); if (vlan) { eth_new_vlan = odp_packet_l2_ptr(pkt_new, NULL); *eth_new_vlan = *eth_vlan; ip_new = (struct ofp_ip *)(eth_new_vlan + 1); odp_packet_l3_offset_set(pkt_new, OFP_ETHER_HDR_LEN + OFP_ETHER_VLAN_ENCAP_LEN); } else { eth_new = odp_packet_l2_ptr(pkt_new, NULL); *eth_new = *eth; ip_new = (struct ofp_ip *)(eth_new + 1); odp_packet_l3_offset_set(pkt_new, OFP_ETHER_HDR_LEN); } *ip_new = *ip; payload_new = (uint8_t *)(ip_new + 1); if (odp_packet_copydata_out(pkt, payload_offset + pl_pos, flen, payload_new) < 0) { OFP_ERR("odp_packet_copydata_out failed"); return OFP_PKT_DROP; }; ip_new->ip_len = odp_cpu_to_be_16(flen + sizeof(*ip_new)); frag_new = frag + pl_pos/8; pl_pos += flen; if (pl_pos < pl_len) frag_new |= OFP_IP_MF; ip_new->ip_off = odp_cpu_to_be_16(frag_new); ip_new->ip_sum = 0; ip_new->ip_sum = ofp_cksum_buffer((uint16_t *)ip_new, sizeof(*ip_new)); if (is_local_address) ret = send_pkt_loop(dev_out, pkt_new); else ret = send_pkt_out(dev_out, pkt_new); if (ret == OFP_PKT_DROP) { odp_packet_free(pkt_new); return OFP_PKT_DROP; } } odp_packet_free(pkt); return OFP_PKT_PROCESSED; }