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 enum ofp_return_code fastpath_local_hook(odp_packet_t pkt,
void *arg)
{
int protocol = *(int *)arg;
(void) pkt;
if (my_test_val == TEST_LOCAL_HOOK) {
CU_ASSERT_EQUAL(protocol, IS_IPV4);
CU_ASSERT_EQUAL(odp_packet_len(pkt), sizeof(test_frame));
if (memcmp((uint8_t *)odp_packet_data(pkt) +
odp_packet_l3_offset(pkt),
in_pkt_data + OFP_ETHER_HDR_LEN,
odp_packet_len(pkt) - OFP_ETHER_HDR_LEN))
CU_FAIL("Corrupt data");
return OFP_TEST_LOCAL_HOOK;
} else if (my_test_val == TEST_LOCAL_HOOK_GRE) {
/* GRE packet is offered to local hook, then
after processing to forward hook */
my_test_val = TEST_FORWARD_HOOK;
return OFP_PKT_CONTINUE;
} else if (my_test_val == TEST_LOCAL_HOOK_GRE_APP) {
/* GRE packet is offered to local hook, then
after tunnel is not found to GRE hook */
my_test_val = TEST_GRE_HOOK;
return OFP_PKT_CONTINUE;
} else if (my_test_val == TEST_LOCAL_IPv4_HOOK)
return OFP_PKT_CONTINUE;
else if (my_test_val == TEST_LOCAL_UDPv4_HOOK)
return OFP_PKT_CONTINUE;
else
return OFP_TEST_FAIL;
}
/*
* Trim packet data beyond the end of L3 payload
* (i.e. from the offset (L3 offset + l3_len) to the end of the packet).
*/
static inline void trim_tail(odp_packet_t pkt, uint32_t l3_len)
{
uint32_t l3_offset = odp_packet_l3_offset(pkt);
uint32_t len = odp_packet_len(pkt);
if (len > l3_offset + l3_len)
odp_packet_pull_tail(pkt, len - l3_offset - l3_len);
}
/**
* Print odp packets
*
* @param thr worker id
* @param pkt_tbl packets to be print
* @param len packet number
*/
static void print_pkts(int thr, odp_packet_t pkt_tbl[], unsigned len)
{
odp_packet_t pkt;
char *buf;
odph_ipv4hdr_t *ip;
odph_icmphdr_t *icmp;
struct timeval tvrecv;
struct timeval tvsend;
double rtt;
unsigned i;
size_t offset;
char msg[1024];
int rlen;
for (i = 0; i < len; ++i) {
pkt = pkt_tbl[i];
rlen = 0;
/* only ip pkts */
if (!odp_packet_has_ipv4(pkt))
continue;
odp_atomic_inc_u64(&counters.ip);
buf = odp_packet_data(pkt);
ip = (odph_ipv4hdr_t *)(buf + odp_packet_l3_offset(pkt));
offset = odp_packet_l4_offset(pkt);
/* udp */
if (ip->proto == ODPH_IPPROTO_UDP) {
odp_atomic_inc_u64(&counters.udp);
}
/* icmp */
if (ip->proto == ODPH_IPPROTO_ICMP) {
icmp = (odph_icmphdr_t *)(buf + offset);
/* echo reply */
if (icmp->type == ICMP_ECHOREPLY) {
odp_atomic_inc_u64(&counters.icmp);
memcpy(&tvsend, buf + offset + ODPH_ICMPHDR_LEN,
sizeof(struct timeval));
/* TODO This should be changed to use an
* ODP timer API once one exists. */
gettimeofday(&tvrecv, NULL);
tv_sub(&tvrecv, &tvsend);
rtt = tvrecv.tv_sec*1000 + tvrecv.tv_usec/1000;
rlen += sprintf(msg + rlen,
"ICMP Echo Reply seq %d time %.1f ",
odp_be_to_cpu_16(icmp->un.echo.sequence)
, rtt);
} else if (icmp->type == ICMP_ECHO) {
rlen += sprintf(msg + rlen,
"Icmp Echo Request");
}
msg[rlen] = '\0';
printf(" [%02i] %s\n", thr, msg);
}
}
}
uint8_t *odp_packet_l3(odp_packet_t pkt)
{
const size_t offset = odp_packet_l3_offset(pkt);
if (odp_unlikely(offset == ODP_PACKET_OFFSET_INVALID))
return NULL;
return odp_packet_buf_addr(pkt) + offset;
}
static enum ofp_return_code ofp_output_ipv4_to_gre(
odp_packet_t pkt, struct ofp_ifnet *dev_gre,
uint16_t vrfid, struct ofp_nh_entry **nh_new)
{
struct ofp_ip *ip;
struct ofp_greip *greip;
uint32_t flags;
uint8_t l2_size = 0;
int32_t offset;
*nh_new = ofp_get_next_hop(vrfid, dev_gre->ip_remote, &flags);
if (*nh_new == NULL)
return OFP_PKT_DROP;
ip = odp_packet_l3_ptr(pkt, NULL);
/* Remove eth header, prepend gre + ip */
if (odp_packet_has_l2(pkt))
l2_size = odp_packet_l3_offset(pkt) - odp_packet_l2_offset(pkt);
offset = sizeof(*greip) - l2_size;
if (offset >= 0)
greip = odp_packet_push_head(pkt, offset);
else
greip = odp_packet_pull_head(pkt, -offset);
odp_packet_has_l2_set(pkt, 0);
odp_packet_l3_offset_set(pkt, 0);
if (!greip)
return OFP_PKT_DROP;
greip->gi_flags = 0;
greip->gi_ptype = odp_cpu_to_be_16(OFP_GREPROTO_IP);
greip->gi_i.ip_hl = 5;
greip->gi_i.ip_v = 4;
greip->gi_i.ip_tos = ip->ip_tos;
greip->gi_i.ip_len =
odp_cpu_to_be_16(odp_be_to_cpu_16(ip->ip_len) +
sizeof(*greip));
greip->gi_i.ip_id = ip->ip_id;
greip->gi_i.ip_off = 0;
greip->gi_i.ip_ttl = ip->ip_ttl;
greip->gi_i.ip_p = OFP_IPPROTO_GRE;
greip->gi_i.ip_sum = 0;
greip->gi_i.ip_src.s_addr = dev_gre->ip_local;
greip->gi_i.ip_dst.s_addr = dev_gre->ip_remote;
return OFP_PKT_CONTINUE;
}
/*
* Prepare a packet for L2 header prepend and output. The packet is pulled
* or pushed as necessary so that there is exactly l2_size bytes in the
* beginning of the packet before the data pointed to by the L3 offset.
*
* After return
* - L2 offset is undefined
* - L3 offset points to the same data as before the call
* - Value of L3 offset is l2_size
* - If packet was pushed or pulled, L4 offset is set to l2size + hlen
*
* Returns pointer to the L3 data or NULL if trimming failed.
*
*/
static inline void *trim_for_output(odp_packet_t pkt, uint32_t l2_size,
uint32_t hlen)
{
void *l2_addr;
uint32_t l3_offset = odp_packet_l3_offset(pkt);
if (l3_offset == l2_size) {
l2_addr = odp_packet_data(pkt);
} else if (l3_offset > l2_size) {
l2_addr = odp_packet_pull_head(pkt, l3_offset - l2_size);
odp_packet_l3_offset_set(pkt, l2_size);
odp_packet_l4_offset_set(pkt, l2_size + hlen);
} else {
l2_addr = odp_packet_push_head(pkt, l2_size - l3_offset);
odp_packet_l3_offset_set(pkt, l2_size);
odp_packet_l4_offset_set(pkt, l2_size + hlen);
}
return l2_addr;
}
int ofp_in6_cksum(odp_packet_t m, uint8_t nxt, uint32_t off, uint32_t len)
{
int sum;
int tmp;
union {
uint16_t s[2];
uint32_t l;
} l_util;
struct ofp_ip6_hdr *ip6 = odp_packet_l3_ptr(m, NULL);
/*Pseudo header*/
sum = _ofp_in6_cksum_pseudo(ip6, len, nxt, 0);
/* Payload*/
tmp = ofp_getsum(m, odp_packet_l3_offset(m) +
off, len);
sum += tmp;
REDUCE;
return (~sum & 0xffff);
}
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;
}
enum ofp_return_code ofp_send_frame(struct ofp_ifnet *dev, odp_packet_t pkt)
{
struct ofp_ether_header *eth, eth_tmp;
struct ofp_ether_vlan_header *eth_vlan, eth_vlan_tmp;
uint32_t pkt_len, eth_hdr_len;
enum ofp_return_code rc;
if (ofp_if_type(dev) == OFP_IFT_GRE) {
OFP_ERR("Send frame on GRE port");
return OFP_PKT_DROP;
}
ofp_packet_user_area_reset(pkt);
/* Contsruct ethernet header */
eth = odp_packet_l2_ptr(pkt, NULL);
eth_vlan = odp_packet_l2_ptr(pkt, NULL);
if (odp_be_to_cpu_16(eth->ether_type) == OFP_ETHERTYPE_VLAN) {
if (dev->vlan) {
/* change vlan */
eth_vlan->evl_tag = odp_cpu_to_be_16(dev->vlan);
} else {
/* remove existing vlan */
eth_vlan_tmp = *eth_vlan;
eth = odp_packet_pull_head(pkt, 4);
if (!eth) {
OFP_ERR("odp_packet_pull_head failed");
return OFP_PKT_DROP;
}
odp_packet_l3_offset_set(pkt,
odp_packet_l3_offset(pkt) - 4);
ofp_copy_mac(eth->ether_dhost, eth_vlan_tmp.evl_dhost);
ofp_copy_mac(eth->ether_shost, eth_vlan_tmp.evl_shost);
eth->ether_type = eth_vlan_tmp.evl_proto;
}
} else {
if (dev->vlan) {
/* insert vlan */
eth_tmp = *eth;
eth_vlan = odp_packet_push_head(pkt, 4);
if (!eth_vlan) {
OFP_ERR("odp_packet_push_head failed");
return OFP_PKT_DROP;
}
odp_packet_l3_offset_set(pkt,
odp_packet_l3_offset(pkt) + 4);
ofp_copy_mac(eth_vlan->evl_dhost, eth_tmp.ether_dhost);
ofp_copy_mac(eth_vlan->evl_shost, eth_tmp.ether_shost);
eth_vlan->evl_encap_proto =
odp_cpu_to_be_16(OFP_ETHERTYPE_VLAN);
eth_vlan->evl_tag = odp_cpu_to_be_16(dev->vlan);
eth_vlan->evl_proto = eth_tmp.ether_type;
}
}
if (dev->vlan)
eth_hdr_len = OFP_ETHER_HDR_LEN + OFP_ETHER_VLAN_ENCAP_LEN;
else
eth_hdr_len = OFP_ETHER_HDR_LEN;
pkt_len = odp_packet_len(pkt) - eth_hdr_len;
if (pkt_len > dev->if_mtu) {
OFP_ERR("Packet size bigger than MTU: %d %d", pkt_len,
dev->if_mtu);
return OFP_PKT_DROP;
}
rc = send_pkt_out(dev, pkt);
if (rc != OFP_PKT_PROCESSED)
return rc;
return ofp_send_pending_pkt();
}
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;
}
static void
test_ofp_packet_input_forwarding_to_output(void)
{
odp_packet_t pkt;
odp_event_t ev;
int res;
/* Call ofp_packet_input using a pkt with destination ip
* that does NOT match the local ip on ifnet and a route is found.
* ARP is found for gateway IP.
* Function returns OFP_PKT_PROCESSED and
* packet is forwarded to ofp_ip_output.*/
unsigned char ll_addr[13] = "123456789012";
my_test_val = TEST_FORWARD_HOOK;
CU_ASSERT_EQUAL(
ofp_ipv4_lookup_mac(dst_ipaddr + 1, ll_addr, ifnet), -1);
CU_ASSERT_EQUAL(
ofp_arp_ipv4_insert(dst_ipaddr + 1, ll_addr, ifnet), 0);
if (create_odp_packet_ip4(&pkt, test_frame, sizeof(test_frame),
dst_ipaddr, 0)) {
CU_FAIL("Fail to create packet");
return;
}
res = ofp_packet_input(pkt, interface_queue[port],
ofp_eth_vlan_processing);
CU_ASSERT_EQUAL(res, OFP_PKT_PROCESSED);
CU_ASSERT_NOT_EQUAL(ev = odp_queue_deq(ifnet->outq_def),
ODP_EVENT_INVALID);
CU_ASSERT_EQUAL(odp_queue_deq(ifnet->outq_def), ODP_EVENT_INVALID);
#ifdef SP
CU_ASSERT_EQUAL(odp_queue_deq(ifnet->spq_def), ODP_EVENT_INVALID);
#endif /* SP */
CU_ASSERT_EQUAL(odp_packet_len(pkt), sizeof(test_frame));
pkt = odp_packet_from_event(ev);
struct ofp_ip *ip_in_pkt_data =
(struct ofp_ip *)(in_pkt_data + OFP_ETHER_HDR_LEN);
ip_in_pkt_data->ip_ttl--;
#ifdef OFP_PERFORMANCE
/*checksum is not filled on ip_output*/
ip_in_pkt_data->ip_sum =
((struct ofp_ip *)odp_packet_l3_ptr(pkt, NULL))->ip_sum;
#else
ip_in_pkt_data->ip_sum = 0;
ip_in_pkt_data->ip_sum = ofp_cksum_buffer((uint16_t *)ip_in_pkt_data,
ip_in_pkt_data->ip_hl<<2);
#endif
if (memcmp((uint8_t *)odp_packet_data(pkt) + odp_packet_l3_offset(pkt),
in_pkt_data + OFP_ETHER_HDR_LEN,
sizeof(test_frame) - OFP_ETHER_HDR_LEN))
CU_FAIL("corrupt l3 + data forwarded");
struct ofp_ether_header *eth =
(struct ofp_ether_header *)odp_packet_l2_ptr(pkt, NULL);
if (memcmp(eth->ether_dhost, ll_addr, OFP_ETHER_ADDR_LEN))
CU_FAIL("Bad destination mac address on the forwarded packet");
CU_ASSERT_EQUAL(eth->ether_type, odp_cpu_to_be_16(OFP_ETHERTYPE_IP));
CU_PASS("ofp_packet_input_forwarding_to_output");
}
