uint32_t cls_pkt_get_seq(odp_packet_t pkt)
{
uint32_t offset;
cls_test_packet_t data;
odph_ipv4hdr_t *ip;
odph_tcphdr_t *tcp;
ip = (odph_ipv4hdr_t *)odp_packet_l3_ptr(pkt, NULL);
offset = odp_packet_l4_offset(pkt);
if (!offset && !ip)
return TEST_SEQ_INVALID;
if (ip->proto == ODPH_IPPROTO_UDP)
odp_packet_copydata_out(pkt, offset + ODPH_UDPHDR_LEN,
sizeof(data), &data);
else {
tcp = (odph_tcphdr_t *)odp_packet_l4_ptr(pkt, NULL);
odp_packet_copydata_out(pkt, offset + tcp->hl * 4,
sizeof(data), &data);
}
if (data.magic == DATA_MAGIC)
return data.seq;
return TEST_SEQ_INVALID;
}
void test_pktio_pmr_match_set_cos(void)
{
uint32_t addr = 0;
uint32_t mask;
odph_ipv4hdr_t *ip;
odph_udphdr_t *udp;
odp_packet_t pkt;
odp_queue_t queue;
uint32_t seq;
pkt = create_packet(false);
seq = cls_pkt_get_seq(pkt);
ip = (odph_ipv4hdr_t *)odp_packet_l3_ptr(pkt, NULL);
parse_ipv4_string(CLS_PMR_SET_SADDR, &addr, &mask);
ip->src_addr = odp_cpu_to_be_32(addr);
ip->chksum = 0;
ip->chksum = odp_cpu_to_be_16(odph_ipv4_csum_update(pkt));
udp = (odph_udphdr_t *)odp_packet_l4_ptr(pkt, NULL);
udp->src_port = odp_cpu_to_be_16(CLS_PMR_SET_SPORT);
enqueue_loop_interface(pkt);
pkt = receive_packet(&queue, ODP_TIME_SEC);
CU_ASSERT(queue == queue_list[CLS_PMR_SET]);
CU_ASSERT(seq == cls_pkt_get_seq(pkt));
odp_packet_free(pkt);
}
int cls_pkt_set_seq(odp_packet_t pkt)
{
static uint32_t seq;
cls_test_packet_t data;
uint32_t offset;
odph_ipv4hdr_t *ip;
odph_tcphdr_t *tcp;
int status;
data.magic = DATA_MAGIC;
data.seq = ++seq;
ip = (odph_ipv4hdr_t *)odp_packet_l3_ptr(pkt, NULL);
offset = odp_packet_l4_offset(pkt);
CU_ASSERT_FATAL(offset != 0);
if (ip->proto == ODPH_IPPROTO_UDP)
status = odp_packet_copydata_in(pkt, offset + ODPH_UDPHDR_LEN,
sizeof(data), &data);
else {
tcp = (odph_tcphdr_t *)odp_packet_l4_ptr(pkt, NULL);
status = odp_packet_copydata_in(pkt, offset + tcp->hl * 4,
sizeof(data), &data);
}
return status;
}
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;
}
int get_payload(struct rte_mbuf *pkt, unsigned char **payload, int *len)
{
odph_ipv4hdr_t *ip;
int payload_offset;
ip = (odph_ipv4hdr_t*)odp_packet_l3_ptr(pkt, NULL);
if(ip == NULL)
{
fprintf(stderr, "recv non-ip packet!\n");
return -1;
}
payload_offset = (ip->ver_ihl & 0xf) << 2;
if(ip->proto == 6)//TCP
{
odph_tcphdr_t *tcp;
tcp = (odph_tcphdr_t*)odp_packet_l4_ptr(pkt, NULL);
//printf("%d\n", (tcp->hl & 0xf) << 2);
payload_offset += (tcp->hl & 0xf) << 2;
*len = ntohs(ip->tot_len) - payload_offset;
*payload = (unsigned char*)ip + payload_offset;
//printf("payload %s\n", (char*)*payload);
//printf("len %d\n", *len);
return 0;
}
else if(ip->proto == 17)
{
payload_offset += 8;
*len = ntohs(ip->tot_len) - payload_offset;
*payload = (unsigned char*)ip + payload_offset;
return 0;
}
fprintf(stderr, "recv non-tcp/udp packet!\n");
return -1;
}
void test_pmr_cos(void)
{
odp_packet_t pkt;
odph_udphdr_t *udp;
odp_queue_t queue;
uint32_t seq;
pkt = create_packet(false);
seq = cls_pkt_get_seq(pkt);
udp = (odph_udphdr_t *)odp_packet_l4_ptr(pkt, NULL);
udp->src_port = odp_cpu_to_be_16(CLS_PMR_SPORT);
enqueue_loop_interface(pkt);
pkt = receive_packet(&queue, ODP_TIME_SEC);
CU_ASSERT(queue == queue_list[CLS_PMR]);
CU_ASSERT(seq == cls_pkt_get_seq(pkt));
odp_packet_free(pkt);
}
proc_result_t pktinf_input(odp_packet_t pkt,void* args){
proc_result_t result;
struct pkt_info *info = args;
result.packet_handle = pkt;
result.next_action = NA_CONTINUE;
if(odp_packet_has_ipv4(pkt)){
odph_ipv4hdr_t* hdr = odp_packet_l3_ptr(pkt,NULL);
*((uint32be_t*)info->src_ip) = hdr->src_addr;
*((uint32be_t*)info->dst_ip) = hdr->dst_addr;
}else if(odp_packet_has_ipv6(pkt)){
odph_ipv6hdr_t* hdr = odp_packet_l3_ptr(pkt,NULL);
*((ipv6addr*)info->src_ip) = *((ipv6addr*)hdr->src_addr);
*((ipv6addr*)info->dst_ip) = *((ipv6addr*)hdr->dst_addr);
}
if(odp_packet_has_tcp(pkt)||odp_packet_has_udp(pkt)||odp_packet_has_sctp(pkt)){
ports p = *((ports*)odp_packet_l4_ptr(pkt,NULL));
info->src_port = p.src;
info->dst_port = p.dest;
}
return result;
}
static int pktio_fixup_checksums(odp_packet_t pkt)
{
odph_ipv4hdr_t *ip;
odph_udphdr_t *udp;
uint32_t len;
ip = (odph_ipv4hdr_t *)odp_packet_l3_ptr(pkt, &len);
if (ip->proto != ODPH_IPPROTO_UDP) {
CU_FAIL("unexpected L4 protocol");
return -1;
}
udp = (odph_udphdr_t *)odp_packet_l4_ptr(pkt, &len);
ip->chksum = 0;
odph_ipv4_csum_update(pkt);
udp->chksum = 0;
udp->chksum = odp_cpu_to_be_16(odph_ipv4_udp_chksum(pkt));
return 0;
}
int extract_tuple(odp_packet_t pkt, uint32_t ft[5])
{
odph_ipv4hdr_t *ip;
odph_udphdr_t *udp;
ip = (odph_ipv4hdr_t*)odp_packet_l3_ptr(pkt, NULL);
if(ip == NULL)
{
fprintf(stderr, "recv an invalid packet(is not ip packet)!\n");
return -1;
}
udp = (odph_udphdr_t*)odp_packet_l4_ptr(pkt, NULL);
if(udp == NULL)
{
fprintf(stderr, "recv an invalid packet(is not tcp/udp packet)!\n");
return -1;
}
ft[0] = ntohl(ip->src_addr);
ft[1] = ntohl(ip->dst_addr);
ft[2] = ntohs(udp->src_port);
ft[3] = ntohs(udp->dst_port);
ft[4] = ip->proto;
return 0;
}
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;
}
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;
}
