[OpenSPARC-T2-DV] / verif / env / common / vera / niu_ippktgen / C / wrapper / pgLibnetL2.c

#include "include/libnet.h"
#include "include/pg.h"
#include "include/packet.h"
extern unsigned char * genPayload(int data_type, unsigned int seed,
				  int data_length);


int libnet_gen_l2_packet(struct packet_desc *packet_vars, libnet_t *l) {

	libnet_ptag_t t;
	char *payload;
	u_short payload_s;
	u_char *cp, oui[3];

	u_short priority, cfi;
	u_short frame_type;
	int is_llc_snap, is_arp, is_802_3;
	u_short len_proto;

	/* TMP CODE ONLY TO TEST OTHER LAYERS  */
	if (packet_vars->l2_pkt_type == 0) {
		printf("***TEST PROGRAM ERROR !! \n");
		return(-1);
	}

	is_llc_snap = is_arp = is_802_3 = 0;
  
	payload = NULL;
	payload_s = 0;
	if (packet_vars->l2_len) {
		payload = genPayload(packet_vars->payload.data_type,
				     packet_vars->payload.data_seed,
				     packet_vars->l2_len);
		payload_s = packet_vars->l2_len;
	} else {
		payload = NULL;
		payload_s = 0;
	}

	frame_type = 0;
	if (packet_vars->l3_pkt_type) {
		if (packet_vars->l3_pkt_type & L3_PKT_TYPE_IPV4) {
			frame_type = ETHERTYPE_IP;
		} else if (packet_vars->l3_pkt_type & L3_PKT_TYPE_IPV6) {
			frame_type = ETHERTYPE_IPV6;
		} else {
			frame_type = 0;
		}
		printf("L3 protocol chosen - %x\n", frame_type);
	} else if (packet_vars->l2_pkt_type & L2_PKT_TYPE_ARP) {
		frame_type = ETHERTYPE_ARP;
	} else if (packet_vars->l2_pkt_type & L2_PKT_TYPE_RARP) {
		frame_type = ETHERTYPE_REVARP;
#if 0	/* Needs to be implemented to support user defined frame types */
	} else if (packet_vars->ether.proto) {
		frame_type = packet_vars->ether.proto;
#endif
	} else {
		is_802_3 = 1;
	}

	if ((packet_vars->l2_pkt_type & L2_PKT_TYPE_ARP) ||
	    (packet_vars->l2_pkt_type & L2_PKT_TYPE_RARP)) {

		printf("---Generating ARP/RARP header \n");
		t = libnet_build_arp(packet_vars->arp.hw_type,
				     packet_vars->arp.proto_type,
				     packet_vars->arp.hw_len,
				     packet_vars->arp.proto_len,
				     packet_vars->arp.op_code,
				     packet_vars->arp.src_ha,
				     packet_vars->arp.src_ip,
				     packet_vars->arp.tgt_ha,
				     packet_vars->arp.tgt_ip,
				     payload,		/* payload */
				     payload_s,		/* payload size */
				     l,			/* libnet handle */
				     0);		/* libnet id */
		if (t == -1) {
			fprintf(stderr, "***Can't build ARP header: %s\n",
				libnet_geterror(l));
			return (NULL);
		}

		payload = NULL;
		payload_s = 0;
		is_arp = 1;
	}

	if (packet_vars->l2_pkt_type & L2_PKT_TYPE_LLCSNAP) {

		memset(&oui, 0, 3);
		printf("---Generating LLC header \n");
		t = libnet_build_802_2snap (
					    0xaa,	/* SNAP DSAP */
					    0xaa,	/* SNAP SSAP */
					    0x3,	/* control */
					    oui,	/* oui */
					    frame_type,	/* protocol */
					    payload,	/* payload */
					    payload_s,	/* payload size */
					    l,		/* libnet handle */
					    0);		/* libnet id */


		if (t == -1) {
			fprintf(stderr, "***Can't build LLC SNAP header: "
				"%s\n", libnet_geterror(l));
			return(NULL);
		}

		payload = NULL;
		payload_s = 0;
		is_llc_snap = 1;
	}
	if(packet_vars->payload.error_code & PG_TYPE_LEN_USER_MODE) {
          len_proto = (packet_vars->payload.error_data & 0xffff0000)>>16;
	}else if(packet_vars->payload.error_code & PG_TYPE_LEN_ERR) {
          len_proto = (packet_vars->payload.error_data & 0xffff0000)>>16;
	} else {	
	  if (is_llc_snap || is_802_3) {
		  len_proto = packet_vars->payload.data_length +  packet_vars->payload.l2_pad_length  - 4;
	  } else {
		  len_proto = frame_type;
	  }
	}

	if (packet_vars->l2_pkt_type & L2_PKT_TYPE_TAGGED) {

		printf("---Generating VLAN tagged ethernet header \n");
		priority = (packet_vars->ether.tci & 0xe000) >> 13;
		cfi = (packet_vars->ether.tci & 0x1000) >> 12;
		t = libnet_build_802_1q(
					packet_vars->ether.dst_addr,
					packet_vars->ether.src_addr,
					ETHERTYPE_VLAN,	/* TPI */
					priority,	/* priority (0 - 7) */
					cfi,		/* CFI flag */
					(packet_vars->ether.tci) & 0xfff,	/* vid (0 - 4095) */
					len_proto,	/* frame size */
					payload,	/* payload */
					payload_s,	/* payload size */
					l,		/* libnet handle */
					0);		/* libnet id */

		if (t == -1) {
			fprintf(stderr, "***Can't build ethernet VLAN header: "
				"%s\n", libnet_geterror(l));
			return(NULL);
		}
	} else {

		printf("---Generating ethernet header \n");
		t = libnet_build_ethernet(
					  packet_vars->ether.dst_addr,
					  packet_vars->ether.src_addr,
					  len_proto,
					  payload,	/* payload */
					  payload_s,	/* payload size */
					  l,		/* libnet handle */
					  0);		/* libnet id */
		if (t == -1) {
			fprintf(stderr, "***Can't build ethernet header: %s\n",
				libnet_geterror(l));
			return(NULL);
		}
	}


	return(1);
}
Commit	Line	Data
86530b38 AT	1	#include "include/libnet.h"
	2	#include "include/pg.h"
	3	#include "include/packet.h"
	4	extern unsigned char * genPayload(int data_type, unsigned int seed,
	5	int data_length);
	6
	7
	8	int libnet_gen_l2_packet(struct packet_desc packet_vars, libnet_t l) {
	9
	10	libnet_ptag_t t;
	11	char *payload;
	12	u_short payload_s;
	13	u_char *cp, oui[3];
	14
	15	u_short priority, cfi;
	16	u_short frame_type;
	17	int is_llc_snap, is_arp, is_802_3;
	18	u_short len_proto;
	19
	20	/* TMP CODE ONLY TO TEST OTHER LAYERS */
	21	if (packet_vars->l2_pkt_type == 0) {
	22	printf("***TEST PROGRAM ERROR !! \n");
	23	return(-1);
	24	}
	25
	26	is_llc_snap = is_arp = is_802_3 = 0;
	27
	28	payload = NULL;
	29	payload_s = 0;
	30	if (packet_vars->l2_len) {
	31	payload = genPayload(packet_vars->payload.data_type,
	32	packet_vars->payload.data_seed,
	33	packet_vars->l2_len);
	34	payload_s = packet_vars->l2_len;
	35	} else {
	36	payload = NULL;
	37	payload_s = 0;
	38	}
	39
	40	frame_type = 0;
	41	if (packet_vars->l3_pkt_type) {
	42	if (packet_vars->l3_pkt_type & L3_PKT_TYPE_IPV4) {
	43	frame_type = ETHERTYPE_IP;
	44	} else if (packet_vars->l3_pkt_type & L3_PKT_TYPE_IPV6) {
	45	frame_type = ETHERTYPE_IPV6;
	46	} else {
	47	frame_type = 0;
	48	}
	49	printf("L3 protocol chosen - %x\n", frame_type);
	50	} else if (packet_vars->l2_pkt_type & L2_PKT_TYPE_ARP) {
	51	frame_type = ETHERTYPE_ARP;
	52	} else if (packet_vars->l2_pkt_type & L2_PKT_TYPE_RARP) {
	53	frame_type = ETHERTYPE_REVARP;
	54	#if 0 /* Needs to be implemented to support user defined frame types */
	55	} else if (packet_vars->ether.proto) {
	56	frame_type = packet_vars->ether.proto;
	57	#endif
	58	} else {
	59	is_802_3 = 1;
	60	}
	61
	62	if ((packet_vars->l2_pkt_type & L2_PKT_TYPE_ARP) \|\|
	63	(packet_vars->l2_pkt_type & L2_PKT_TYPE_RARP)) {
	64
65	printf("---Generating ARP/RARP header \n");
66	t = libnet_build_arp(packet_vars->arp.hw_type,
67	packet_vars->arp.proto_type,
68	packet_vars->arp.hw_len,
69	packet_vars->arp.proto_len,
70	packet_vars->arp.op_code,
71	packet_vars->arp.src_ha,
72	packet_vars->arp.src_ip,
73	packet_vars->arp.tgt_ha,
74	packet_vars->arp.tgt_ip,
75	payload, /* payload */
76	payload_s, /* payload size */
77	l, /* libnet handle */
78	0); /* libnet id */
79	if (t == -1) {
80	fprintf(stderr, "***Can't build ARP header: %s\n",
81	libnet_geterror(l));
82	return (NULL);
83	}
84
85	payload = NULL;
86	payload_s = 0;
87	is_arp = 1;
88	}
89
90	if (packet_vars->l2_pkt_type & L2_PKT_TYPE_LLCSNAP) {
91
92	memset(&oui, 0, 3);
93	printf("---Generating LLC header \n");
94	t = libnet_build_802_2snap (
95	0xaa, /* SNAP DSAP */
96	0xaa, /* SNAP SSAP */
97	0x3, /* control */
98	oui, /* oui */
99	frame_type, /* protocol */
100	payload, /* payload */
101	payload_s, /* payload size */
102	l, /* libnet handle */
103	0); /* libnet id */
104
105
106	if (t == -1) {
107	fprintf(stderr, "***Can't build LLC SNAP header: "
108	"%s\n", libnet_geterror(l));
109	return(NULL);
110	}
111
112	payload = NULL;
113	payload_s = 0;
114	is_llc_snap = 1;
115	}
116	if(packet_vars->payload.error_code & PG_TYPE_LEN_USER_MODE) {
117	len_proto = (packet_vars->payload.error_data & 0xffff0000)>>16;
118	}else if(packet_vars->payload.error_code & PG_TYPE_LEN_ERR) {
119	len_proto = (packet_vars->payload.error_data & 0xffff0000)>>16;
120	} else {
121	if (is_llc_snap \|\| is_802_3) {
122	len_proto = packet_vars->payload.data_length + packet_vars->payload.l2_pad_length - 4;
123	} else {
124	len_proto = frame_type;
125	}
126	}
127
128	if (packet_vars->l2_pkt_type & L2_PKT_TYPE_TAGGED) {
129
130	printf("---Generating VLAN tagged ethernet header \n");
131	priority = (packet_vars->ether.tci & 0xe000) >> 13;
132	cfi = (packet_vars->ether.tci & 0x1000) >> 12;
133	t = libnet_build_802_1q(
134	packet_vars->ether.dst_addr,
135	packet_vars->ether.src_addr,
136	ETHERTYPE_VLAN, /* TPI */
137	priority, /* priority (0 - 7) */
138	cfi, /* CFI flag */
139	(packet_vars->ether.tci) & 0xfff, /* vid (0 - 4095) */
140	len_proto, /* frame size */
141	payload, /* payload */
142	payload_s, /* payload size */
143	l, /* libnet handle */
144	0); /* libnet id */
145
146	if (t == -1) {
147	fprintf(stderr, "***Can't build ethernet VLAN header: "
148	"%s\n", libnet_geterror(l));
149	return(NULL);
150	}
151	} else {
152
153	printf("---Generating ethernet header \n");
154	t = libnet_build_ethernet(
155	packet_vars->ether.dst_addr,
156	packet_vars->ether.src_addr,
157	len_proto,
158	payload, /* payload */
159	payload_s, /* payload size */
160	l, /* libnet handle */
161	0); /* libnet id */
162	if (t == -1) {
163	fprintf(stderr, "***Can't build ethernet header: %s\n",
164	libnet_geterror(l));
165	return(NULL);
166	}
167	}
168
169
170	return(1);
171	}