c90d1ed4f21c0ecd3eeaf086f64e9cfbf659df18
[ltp2tcp] / sll_encap.c
1 /******************************************************************************
2 Author: Samuel Jero
3
4 Date: 5/2011
5
6 Description:  <SLL, IPv4, UDP> encapsulation functions
7
8 ******************************************************************************/
9 #include "ltp2tcp.h"
10 #include <pcap/sll.h>
11 #include <netinet/udp.h>
12
13 extern int debug;
14
15
16
17 /*SLL encapsulation private data structure*/
18 struct sll_en_p{
19         int first;
20         struct pcap_pkthdr      header;
21         u_char                          od[sizeof(struct sll_header)+sizeof(struct iphdr)+sizeof(struct udphdr)];
22 };
23
24
25 /*Fill the encapsulation structure*/
26 int fill_sllip4_encap(struct sll_en_p *slip, const u_char* data, int dlen, struct pcap_pkthdr *h){
27         /*safety check*/
28         if(slip==NULL || data==NULL || h==NULL || dlen < sizeof(struct sll_header)+sizeof(struct iphdr)){
29                 dbgprintf(1, "Error: SLL, IPv4 Encapsulation method given bad data!\n");
30                 return -1;
31         }
32
33         if(slip->first==0){
34                 /* First time, allocate memory and copy libpcap header and encap headers
35                  * this guarantees the IP "direction" of the encap headers */
36                 memcpy(&slip->header, h, sizeof(struct pcap_pkthdr));
37                 memcpy(slip->od, data,sizeof(struct sll_header)+sizeof(struct iphdr));
38                 slip->first=1;
39         }else{
40                 /* Just update the libpcap header (and associated timestamp)*/
41                 memcpy(&slip->header, h, sizeof(struct pcap_pkthdr));
42         }
43         return 0;
44 }
45
46 /* encapsulation manipulation previous to packet conversion */
47 int sll_encap_pre(struct pcap_pkthdr *h, const u_char **odata, u_char **ndata, int* olength, int* nlength)
48 {
49         struct iphdr                    *iph;
50         struct sll_header               *slh;
51         struct udphdr                   *udph;
52
53         /*Safety checks*/
54         if(!h || !odata || !ndata || !*odata || !*ndata || !olength || !nlength){
55                 dbgprintf(0,"Error: SLL Encapsulation given bad data!\n");
56                 exit(1);
57         }
58         if(*olength < sizeof(struct ether_header)+sizeof(struct iphdr)+sizeof(struct udphdr)
59                         || *nlength < sizeof(struct ether_header)+sizeof(struct iphdr)+sizeof(struct tcphdr)){
60                         dbgprintf(0, "Error: SLL Encapsulation given packet of wrong size!\n");
61                         return -1;
62         }
63
64         /*initialize encapsulation private data*/
65         if(state.en_priv==NULL){
66                 /* First time, allocate memory and copy libpcap header and encap headers
67                  * this guarantees the IP "direction" of the encap headers */
68                 state.en_priv=malloc(sizeof(struct sll_en_p));
69                 if(state.en_priv==NULL){
70                         dbgprintf(0,"Error: Couldn't allocate Memory\n");
71                         exit(1);
72                 }
73         }
74         if(fill_sllip4_encap((struct sll_en_p*)state.en_priv, *odata, *olength, h)<0){
75                 return -1;
76         }
77
78         /*Copy SLL and IPv4 headers over*/
79         memcpy(*ndata, *odata, sizeof(struct sll_header)+sizeof(struct iphdr));
80         *odata+=sizeof(struct sll_header)+ sizeof(struct iphdr);
81         *ndata+=sizeof(struct sll_header)+ sizeof(struct iphdr);
82
83         /*Confirm that this is Ethernet and that IPv4 is next*/
84         slh=(struct sll_header*)(*odata -sizeof(struct sll_header)- sizeof(struct iphdr));
85         if(slh->sll_protocol!=htons(ETHERTYPE_IP)){
86                 dbgprintf(1, "Note: Packet not SLL or Not IPv4 next\n");
87                 return -1;
88         }
89
90         /* Check That this is IPv4 and that UDP is next*/
91         iph= (struct iphdr *) (*ndata - sizeof(struct iphdr));
92         if(iph->version!=4){
93                 dbgprintf(1, "Note: Packet is not IPv4\n");
94                 return -1;
95         }
96         if(iph->protocol!=0x11){
97                 dbgprintf(1, "Note: Packet is not UDP\n");
98                 return -1;
99         }
100
101         /*set ip to indicate that tcp is next protocol*/
102         iph->protocol=6;
103         iph->check=htonl(0);
104
105         /* Adjust libpcap headers*/
106         h->caplen=sizeof(struct sll_header) +sizeof(struct iphdr);
107         h->len=sizeof(struct sll_header) +sizeof(struct iphdr);
108
109         /*Adjust packet length*/
110         udph=(struct udphdr*)*odata;
111         *olength=ntohs(udph->len);
112
113         /*Adjust New Packet Length*/
114         *nlength-=sizeof(struct sll_header) +sizeof(struct iphdr);
115
116         /*Move Packet Pointer past UDP header*/
117         *odata+=sizeof(struct udphdr);
118 return 0;
119 }
120
121 /* encapsulation manipulation after conversion */
122 int sll_encap_post(int tlen, u_char *data)
123 {
124         struct iphdr *iph;
125
126         /* Move data pointer to start of IPv4 header*/
127         data+=sizeof(struct sll_header);
128
129         /*Determine if the given length is reasonable*/
130         if((tlen+sizeof(struct iphdr)) > 0xFFFF){
131                         dbgprintf(1, "Error: Given TCP header+data length is too large for an IPv4 packet!\n");
132                         return -1;
133         }
134
135         /*Adjust IPv4 header to account for packet's total length*/
136         iph=(struct iphdr*)data;
137         iph->tot_len=htons(sizeof(struct iphdr)+tlen);
138         return 0;
139 }
140
141 /* Create a TCP three-way handshake */
142 int sll_encap_handshake(struct pcap_pkthdr *h)
143 {
144         struct iphdr            *iph;
145         struct tcphdr           *tcph;
146         u_char                          *data;
147         u_char                          *ptr;
148         struct pcap_pkthdr      nh;
149         u_int32_t                       temp;
150         struct sll_en_p         *slip=(struct sll_en_p*)state.en_priv;
151
152
153         /*Safety Check*/
154         if(h==NULL || state.en_priv==NULL){
155                 dbgprintf(1, "Error: SLL, IPv4 Encapsulation handshake method given bad data!\n");
156                 return -1;
157         }
158
159         /*create new libpcap header*/
160         memcpy(&nh, h, sizeof(struct pcap_pkthdr));
161
162         /*create buffer for new packet*/
163         ptr=data=malloc(MAX_PACKET);
164         if(data==NULL){
165                 dbgprintf(0,"Error: Couldn't allocate Memory\n");
166                 exit(1);
167         }
168
169         /* 1)Create Syn Packet*/
170                 /*make sure the packet is all zero*/
171                 memset(data, 0, MAX_PACKET);
172                 ptr=data;
173
174                 /*Set the libpcap header*/
175                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr)+4;
176                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr)+4;
177                 nh.ts.tv_usec-=3000; /*Time comes from the first packet received, so make these packets earlier*/
178
179                 /* Copy SLL and IP headers from private data area*/
180                 /* These are headers from the first packet in the capture*/
181                 memcpy(ptr, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
182
183                 /*Adjust IP header*/
184                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
185                 iph->protocol=6;
186                 iph->check=htonl(0);
187                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr)+4);
188
189                 /*Build TCP header*/
190                 ptr+=sizeof(struct sll_header)+ sizeof(struct iphdr);
191                 tcph=(struct tcphdr*)ptr;
192                 tcph->source=htons(1113);
193                 tcph->dest=htons(1113);
194                 tcph->doff=6;
195                 tcph->check=htonl(0);
196                 tcph->urg_ptr=0;
197                 tcph->urg=0;
198                 tcph->psh=0;
199                 tcph->fin=0;
200                 tcph->syn=1;
201                 tcph->rst=0;
202                 tcph->ack=0;
203
204                 /*Initialize Sequence and Acknowledgment Numbers and Window*/
205                 tcph->seq=htonl(state.seq_num++);
206                 tcph->ack_seq=htonl(0);
207                 tcph->window=htons(WIN_FACTOR);
208
209                 /* Add SACK permitted option*/
210                 ptr+=sizeof(struct tcphdr);
211                 *ptr=4;
212                 ptr++;
213                 *ptr=2;
214
215                 /*Save To Packet Capture*/
216                 pcap_dump((u_char*)state.out,&nh, data);
217
218
219         /* 2)Create Syn,Ack Packet*/
220                 /*make sure the packet is all zero*/
221                 memset(data, 0, MAX_PACKET);
222                 ptr=data;
223
224                 /*Set the libpcap header*/
225                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr)+4;
226                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr)+4;
227                 nh.ts.tv_usec+=1000; /*This packet is 1/3rd closer to the first packet then the previous packet created*/
228
229                 /* Copy SLL and IP headers from private data area*/
230                 /* These are headers from the first packet in the capture*/
231                 memcpy(data, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
232
233                 /*Adjust IP header, including swapping source and destination*/
234                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
235                 iph->protocol=6;
236                 iph->check=htonl(0);
237                 temp=iph->saddr;
238                 iph->saddr=iph->daddr;
239                 iph->daddr=temp;
240                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr)+4);
241
242                 /*Build TCP header*/
243                 ptr+=sizeof(struct sll_header)+ sizeof(struct iphdr);
244                 tcph=(struct tcphdr*)ptr;
245                 tcph->source=htons(1113);
246                 tcph->dest=htons(1113);
247                 tcph->doff=6;
248                 tcph->check=htonl(0);
249                 tcph->urg_ptr=0;
250                 tcph->urg=0;
251                 tcph->psh=0;
252                 tcph->fin=0;
253                 tcph->syn=1;
254                 tcph->rst=0;
255                 tcph->ack=1;
256
257                 /*Initialize Sequence and Acknowledgement Numbers and Window*/
258                 tcph->seq=htonl(state.ack_num++);
259                 tcph->ack_seq=htonl(state.seq_num);
260                 tcph->window=htons(WIN_FACTOR);
261
262                 /* Add SACK permitted option*/
263                 ptr+=sizeof(struct tcphdr);
264                 *ptr=4;
265                 ptr++;
266                 *ptr=2;
267
268                 /*Save To Packet Capture*/
269                 pcap_dump((u_char*)state.out,&nh, data);
270
271         /* 3)Create Ack Packet*/
272                 /*make sure the packet is all zero*/
273                 memset(data, 0, MAX_PACKET);
274                 ptr=data;
275
276                 /*Set the libpcap header*/
277                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
278                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
279                 nh.ts.tv_usec+=1000; /*This packet is 2/3rds between SYN and first packet*/
280
281                 /* Copy SLL and IP headers from private data area*/
282                 /* These are headers from the first packet in the capture*/
283                 memcpy(data, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
284
285                 /*Adjust IP header*/
286                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
287                 iph->protocol=6;
288                 iph->check=htonl(0);
289                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr));
290
291                 /*Build TCP header*/
292                 ptr+=sizeof(struct sll_header)+ sizeof(struct iphdr);
293                 tcph=(struct tcphdr*)ptr;
294                 tcph->source=htons(1113);
295                 tcph->dest=htons(1113);
296                 tcph->doff=5;
297                 tcph->check=htonl(0);
298                 tcph->urg_ptr=0;
299                 tcph->urg=0;
300                 tcph->psh=0;
301                 tcph->fin=0;
302                 tcph->syn=0;
303                 tcph->rst=0;
304                 tcph->ack=1;
305
306                 /*Initialize Sequence and Acknowledgement numbers and window*/
307                 tcph->seq=htonl(state.seq_num++);
308                 tcph->ack_seq=htonl(state.ack_num);
309                 tcph->window=htons(WIN_FACTOR);
310
311                 /*Save To Packet Capture*/
312                 pcap_dump((u_char*)state.out,&nh, data);
313                 return 0;
314 }
315
316 /* Create a TCP ending handshake */
317 int sll_encap_fin()
318 {
319         struct iphdr            *iph;
320         struct tcphdr           *tcph;
321         u_char                          *data;
322         u_char                          *ptr;
323         struct pcap_pkthdr      nh;
324         u_int32_t                       temp;
325         struct sll_en_p         *slip=(struct sll_en_p*)state.en_priv;
326
327         /*Safety Check*/
328         if(slip==NULL){
329                 dbgprintf(1,"Error: SLL, IPv4 Encapsulation Finish method given invalid data!\n");
330                 return -1;
331         }
332
333         /*copy the libpcap header from private data area*/
334         memcpy(&nh, &slip->header, sizeof(struct pcap_pkthdr));
335
336         /*create buffer for new packet*/
337         ptr=data=malloc(MAX_PACKET);
338         if(data==NULL){
339                 dbgprintf(0,"Error: Couldn't allocate Memory\n");
340                 exit(1);
341         }
342
343         /* 1)Create Fin Packet*/
344                 /*make sure the packet is all zero*/
345                 memset(data, 0, MAX_PACKET);
346                 ptr=data;
347
348                 /*Set the libpcap header*/
349                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
350                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
351                 nh.ts.tv_usec+=1000; /*Time is from the last packet in the capture; make this packet after that packet*/
352
353                 /* Copy Ethernet and IP headers from private data area*/
354                 /* These are headers from the first packet in the capture*/
355                 memcpy(ptr, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
356
357                 /*Adjust IP header*/
358                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
359                 iph->protocol=6;
360                 iph->check=htonl(0);
361                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr));
362
363                 /*Build TCP header*/
364                 ptr+=sizeof(struct sll_header)+ sizeof(struct iphdr);
365                 tcph=(struct tcphdr*)ptr;
366                 tcph->source=htons(1113);
367                 tcph->dest=htons(1113);
368                 tcph->doff=5;
369                 tcph->check=htonl(0);
370                 tcph->urg_ptr=0;
371                 tcph->urg=0;
372                 tcph->psh=0;
373                 tcph->fin=1;
374                 tcph->syn=0;
375                 tcph->rst=0;
376                 tcph->ack=1;
377
378                 /* Adjust Sequence and Acknowledgment numbers and window*/
379                 tcph->seq=htonl(++state.seq_num);
380                 tcph->ack_seq=htonl(state.ack_num);
381                 tcph->window=htons(WIN_FACTOR);
382
383                 /*Update Sequence Number to include the fin packet in the sequence number space*/
384                 state.seq_num++;
385
386                 /* Save To Packet Capture*/
387                 pcap_dump((u_char*)state.out,&nh, data);
388
389         /* 2)Create Fin,Ack Packet*/
390                 /*make sure the packet is all zero*/
391                 memset(data, 0, MAX_PACKET);
392                 ptr=data;
393
394                 /*Set the libpcap header*/
395                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
396                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
397                 nh.ts.tv_usec+=1000; /*After the previous packet*/
398
399                 /* Copy Ethernet and IP headers from private data area*/
400                 /* These are headers from the first packet in the capture*/
401                 memcpy(ptr, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
402
403                 /*Update IP header, including swapping source and destination addresses*/
404                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
405                 iph->protocol=6;
406                 iph->check=htonl(0);
407                 temp=iph->saddr;
408                 iph->saddr=iph->daddr;
409                 iph->daddr=temp;
410                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr));
411
412                 /*Build TCP header*/
413                 ptr+=sizeof(struct sll_header)+ sizeof(struct iphdr);
414                 tcph=(struct tcphdr*)ptr;
415                 tcph->source=htons(1113);
416                 tcph->dest=htons(1113);
417                 tcph->doff=5;
418                 tcph->check=htonl(0);
419                 tcph->urg_ptr=0;
420                 tcph->urg=0;
421                 tcph->psh=0;
422                 tcph->fin=1;
423                 tcph->syn=0;
424                 tcph->rst=0;
425                 tcph->ack=1;
426
427                 /*Adjust Sequence and Acknowledgment numbers and window*/
428                 tcph->seq=htonl(state.ack_num++);
429                 tcph->ack_seq=htonl(state.seq_num);
430                 tcph->window=htons(WIN_FACTOR);
431
432                 /*Save To Packet Capture*/
433                 pcap_dump((u_char*)state.out,&nh, data);
434
435         /* 3)Create Ack Packet*/
436                 /*make sure the packet is all zero*/
437                 memset(data, 0, MAX_PACKET);
438                 ptr=data;
439
440                 /*Set the libpcap header*/
441                 nh.caplen=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
442                 nh.len=sizeof(struct sll_header) +sizeof(struct iphdr)+sizeof(struct tcphdr);
443                 nh.ts.tv_usec+=1000; /*After the previous packet*/
444
445                 /* Copy Ethernet and IP headers from private data area*/
446                 /* These are headers from the first packet in the capture*/
447                 memcpy(ptr, slip->od, sizeof(struct sll_header)+ sizeof(struct iphdr));
448
449                 /*Update IP header*/
450                 iph= (struct iphdr *) (ptr + sizeof(struct sll_header));
451                 iph->protocol=6;
452                 iph->check=htonl(0);
453                 iph->tot_len=htons(sizeof(struct iphdr)+sizeof(struct tcphdr));
454
455                 /*Build TCP header*/
456                 ptr+=sizeof(struct ether_header)+ sizeof(struct iphdr);
457                 tcph=(struct tcphdr*)ptr;
458                 tcph->source=htons(1113);
459                 tcph->dest=htons(1113);
460                 tcph->doff=5;
461                 tcph->check=htonl(0);
462                 tcph->urg_ptr=0;
463                 tcph->urg=0;
464                 tcph->psh=0;
465                 tcph->fin=0;
466                 tcph->syn=0;
467                 tcph->rst=0;
468                 tcph->ack=1;
469
470                 /*Adjust Sequence and Acknowledgment numbers and window*/
471                 tcph->seq=htonl(state.seq_num++);
472                 tcph->ack_seq=htonl(state.ack_num);
473                 tcph->window=htons(WIN_FACTOR);
474
475                 /*Save To Packet Capture*/
476                 pcap_dump((u_char*)state.out,&nh, data);
477                 return 0;
478 }
479
480
481
482
483 /* The UDP Encapsulation Structure*/
484 struct encap_ops sll_encap = {
485         .pre=sll_encap_pre,
486         .post=sll_encap_post,
487         .handshake=sll_encap_handshake,
488         .fin=sll_encap_fin,
489 };