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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
|
#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>
//#define ETH_FRAME_LEN 1518
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->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;
}
int main (int argc, char **argv) {
int s; /*socketdescriptor*/
int j;
int result;
char data[11] = "1234567890";
s = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
printf("Sockid: %d\n", s);
result = sendCustomUDP(s, "00:19:db:66:e5::bf", "04:1e:64:ea:e5:15", "10.0.0.100", 20561, "255.255.255.255", 20561, data, 10);
printf("Send result: %d\n", result);
close(s);
return 0;
}
|
