void *ofp_udp_packet_parse(odp_packet_t pkt, int *length,
struct ofp_sockaddr *addr,
ofp_socklen_t *addrlen)
{
struct ofp_sockaddr *src_addr = NULL;
ofp_socklen_t src_len = 0;
struct ofp_udphdr *uh =
(struct ofp_udphdr *)odp_packet_l4_ptr(pkt, NULL);
int udplen = odp_be_to_cpu_16(uh->uh_ulen) - sizeof(*uh);
uint8_t *data = (uint8_t *)(uh + 1);
uint8_t *start = odp_packet_data(pkt);
if (addr && addrlen) {
src_addr = (struct ofp_sockaddr *)odp_packet_l2_ptr(pkt,
NULL);
if (src_addr->sa_family == OFP_AF_INET)
src_len = sizeof(struct ofp_sockaddr_in);
else if (src_addr->sa_family == OFP_AF_INET6)
src_len = sizeof(struct ofp_sockaddr_in6);
else
return NULL;
memcpy(addr, src_addr, min(*addrlen, src_len));
*addrlen = src_len;
}
if (data > start)
odp_packet_pull_head(pkt, (uint32_t)(data - start));
int pktlen = odp_packet_len(pkt);
if (pktlen > udplen)
odp_packet_pull_tail(pkt, (uint32_t)(pktlen - udplen));
if (length)
*length = udplen;
return data;
}
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;
}
/*
* Drop data from (the front of) a sockbuf.
*/
static void
sbdrop_internal(struct sockbuf *sb, int len)
{
odp_packet_t pkt;
while (len > 0) {
pkt = ofp_sockbuf_get_first(sb);
if (pkt == ODP_PACKET_INVALID)
return;
int buflen = odp_packet_len(pkt);
if (buflen > len) {
odp_packet_pull_head(pkt, len);
sb->sb_cc -= len;
if (sb->sb_sndptroff != 0)
sb->sb_sndptroff -= len;
break;
}
len -= buflen;
pkt = ofp_sockbuf_remove_first(sb);
sbfree(sb, pkt);
ofp_sockbuf_packet_free(pkt);
}
}
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();
}
