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;
}
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;
}
