1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
#include <malloc.h>
#include <string.h>
#include <math.h>
#include <sys/socket.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/ip.h>
#include <linux/in.h>
#include <linux/udp.h>
#include <linux/if_arp.h>
#include <stdio.h>
unsigned short in_cksum(unsigned short *addr, int len)
{
int nleft = len;
int sum = 0;
unsigned short *w = addr;
unsigned short answer = 0;
while (nleft > 1) {
sum += *w++;
nleft -= 2;
}
if (nleft == 1) {
*(unsigned char *) (&answer) = *(unsigned char *) w;
sum += answer;
}
sum = (sum >> 16) + (sum & 0xFFFF);
sum += (sum >> 16);
answer = ~sum;
return (answer);
}
unsigned char hex(const unsigned char c) {
if ('0' <= c && c <= '9') return c - '0';
if ('a' <= c && c <= 'f') return c - 'a' + 10;
if ('A' <= c && c <= 'F') return c - 'A' + 10;
return -1;
}
unsigned char hex2(const unsigned char *p) {
int i;
unsigned char c;
i = hex(*p++);
if (i < 0) return i;
c = (i << 4);
i = hex(*p);
if (i < 0) return i;
return c | i;
}
void etherAddrton(unsigned char *dest, const unsigned char *mac) {
const unsigned char *p = mac;
int i = 0;
do {
if (*p == ':') {
continue;
}
dest[i++] = hex2(p++);
} while (*p++ && *p);
}
int sendCustomUDP(const int socket, const char *sourcemac, const char *destmac, const char *sourceip, const int sourceport, const char *destip, const int destport, const char *data, const int datalen) {
struct sockaddr_ll socket_address;
struct in_addr srcipaddr;
struct in_addr dstipaddr;
void* buffer = (void*)malloc(ETH_FRAME_LEN);
struct ethhdr *eh = (struct ethhdr *)buffer;
struct iphdr *ip = (struct iphdr *)(buffer+14);
struct udphdr *udp = (struct udphdr *)(buffer+14+20);
unsigned char *resten = (unsigned char *)(buffer+20+14+sizeof(struct udphdr));
static unsigned int id = 1;
int send_result = 0;
// Ethernet header
etherAddrton(eh->h_source, sourcemac);
etherAddrton(eh->h_dest, destmac);
eh->h_proto = 8;
// SendTo struct
socket_address.sll_family = PF_PACKET;
socket_address.sll_protocol = htons(ETH_P_IP);
socket_address.sll_ifindex = 2;
socket_address.sll_hatype = ARPHRD_ETHER;
socket_address.sll_pkttype = PACKET_OTHERHOST;
socket_address.sll_halen = ETH_ALEN;
memcpy(socket_address.sll_addr, eh->h_source, 6);
socket_address.sll_addr[6] = 0x00;/*not used*/
socket_address.sll_addr[7] = 0x00;/*not used*/
// TODO: errorhandling
inet_aton(sourceip, &srcipaddr);
inet_aton(destip, &dstipaddr);
// IP Header
ip->version = 4;
ip->ihl = 5;
ip->tos = 0x10;
ip->tot_len = htons(datalen+8+20);
ip->id = htons(id++);
ip->frag_off = 0x0040;
ip->ttl = 64;
ip->protocol = 17; // UDP
ip->check = 0x0000;
ip->saddr = srcipaddr.s_addr;
ip->daddr = dstipaddr.s_addr;
ip->check = in_cksum((unsigned short *)ip, sizeof(struct iphdr));
// UDP Header
udp->source = htons(20561);
udp->dest = htons(20561);
udp->check = 0;
udp->len = htons(sizeof(struct udphdr) + datalen);
memcpy(resten, data, datalen);
/*send the packet*/
send_result = sendto(socket, buffer, datalen+8+14+20, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
free(buffer);
return send_result-8-14-20;
}
|
