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