diff options
author | Star Rauchenberger <fefferburbia@gmail.com> | 2025-07-13 10:07:59 -0400 |
---|---|---|
committer | Star Rauchenberger <fefferburbia@gmail.com> | 2025-07-13 10:07:59 -0400 |
commit | 27e0133d40f7b328482b501a18e22b12a55564ea (patch) | |
tree | ccc7f5876fcf11840836d13532aebbd223cff62d | |
parent | f66fc94340bdf6a30260a1932e3f2f22d8822304 (diff) | |
download | gen4uploader-27e0133d40f7b328482b501a18e22b12a55564ea.tar.gz gen4uploader-27e0133d40f7b328482b501a18e22b12a55564ea.tar.bz2 gen4uploader-27e0133d40f7b328482b501a18e22b12a55564ea.zip |
Associate master
-rw-r--r-- | CMakeLists.txt | 4 | ||||
-rw-r--r-- | src/addr_list.c | 17 | ||||
-rw-r--r-- | src/addr_list.h | 4 | ||||
-rw-r--r-- | src/main.c | 219 | ||||
-rw-r--r-- | src/retransmit.c | 169 | ||||
-rw-r--r-- | src/retransmit.h | 32 | ||||
-rw-r--r-- | src/transmit.c | 55 | ||||
-rw-r--r-- | src/transmit.h | 6 |
8 files changed, 378 insertions, 128 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index d784ebc..0ac3833 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt | |||
@@ -5,8 +5,10 @@ add_subdirectory(vendor/libwifi) | |||
5 | 5 | ||
6 | set(SOURCE_FILES | 6 | set(SOURCE_FILES |
7 | src/main.c | 7 | src/main.c |
8 | src/beacon_data.c | ||
9 | src/addr_list.c | 8 | src/addr_list.c |
9 | src/beacon_data.c | ||
10 | src/retransmit.c | ||
11 | src/transmit.c | ||
10 | ) | 12 | ) |
11 | 13 | ||
12 | include_directories( | 14 | include_directories( |
diff --git a/src/addr_list.c b/src/addr_list.c index 759907e..e8ba571 100644 --- a/src/addr_list.c +++ b/src/addr_list.c | |||
@@ -9,7 +9,7 @@ void addr_list_init(struct addr_list* list) { | |||
9 | list->top = NULL; | 9 | list->top = NULL; |
10 | } | 10 | } |
11 | 11 | ||
12 | void addr_list_add(struct addr_list* list, unsigned char addr[6]) { | 12 | void addr_list_add(struct addr_list* list, unsigned char addr[6], void* userdata) { |
13 | struct addr_list_node* cur = list->top; | 13 | struct addr_list_node* cur = list->top; |
14 | struct addr_list_node* prev = NULL; | 14 | struct addr_list_node* prev = NULL; |
15 | 15 | ||
@@ -25,6 +25,7 @@ void addr_list_add(struct addr_list* list, unsigned char addr[6]) { | |||
25 | struct addr_list_node* next = (struct addr_list_node*)malloc(sizeof(struct addr_list_node)); | 25 | struct addr_list_node* next = (struct addr_list_node*)malloc(sizeof(struct addr_list_node)); |
26 | next->next = NULL; | 26 | next->next = NULL; |
27 | memcpy(next->value, addr, 6); | 27 | memcpy(next->value, addr, 6); |
28 | next->userdata = userdata; | ||
28 | 29 | ||
29 | if (prev == NULL) { | 30 | if (prev == NULL) { |
30 | list->top = next; | 31 | list->top = next; |
@@ -47,6 +48,20 @@ bool addr_list_contains(struct addr_list* list, unsigned char addr[6]) { | |||
47 | return false; | 48 | return false; |
48 | } | 49 | } |
49 | 50 | ||
51 | void* addr_list_get(struct addr_list* list, unsigned char addr[6]) { | ||
52 | struct addr_list_node* cur = list->top; | ||
53 | |||
54 | while (cur != NULL) { | ||
55 | if (!strncmp(cur->value, addr, 6)) { | ||
56 | return cur->userdata; | ||
57 | } | ||
58 | |||
59 | cur = cur->next; | ||
60 | } | ||
61 | |||
62 | return NULL; | ||
63 | } | ||
64 | |||
50 | void addr_list_remove(struct addr_list* list, unsigned char addr[6]) { | 65 | void addr_list_remove(struct addr_list* list, unsigned char addr[6]) { |
51 | struct addr_list_node* cur = list->top; | 66 | struct addr_list_node* cur = list->top; |
52 | struct addr_list_node* prev = NULL; | 67 | struct addr_list_node* prev = NULL; |
diff --git a/src/addr_list.h b/src/addr_list.h index 66e217e..769d450 100644 --- a/src/addr_list.h +++ b/src/addr_list.h | |||
@@ -5,6 +5,7 @@ | |||
5 | 5 | ||
6 | struct addr_list_node { | 6 | struct addr_list_node { |
7 | unsigned char value[6]; | 7 | unsigned char value[6]; |
8 | void* userdata; | ||
8 | struct addr_list_node* next; | 9 | struct addr_list_node* next; |
9 | }; | 10 | }; |
10 | 11 | ||
@@ -13,8 +14,9 @@ struct addr_list { | |||
13 | }; | 14 | }; |
14 | 15 | ||
15 | void addr_list_init(struct addr_list* list); | 16 | void addr_list_init(struct addr_list* list); |
16 | void addr_list_add(struct addr_list* list, unsigned char addr[6]); | 17 | void addr_list_add(struct addr_list* list, unsigned char addr[6], void* userdata); |
17 | bool addr_list_contains(struct addr_list* list, unsigned char addr[6]); | 18 | bool addr_list_contains(struct addr_list* list, unsigned char addr[6]); |
19 | void* addr_list_get(struct addr_list* list, unsigned char addr[6]); | ||
18 | void addr_list_remove(struct addr_list* list, unsigned char addr[6]); | 20 | void addr_list_remove(struct addr_list* list, unsigned char addr[6]); |
19 | void addr_list_free(struct addr_list* list); | 21 | void addr_list_free(struct addr_list* list); |
20 | 22 | ||
diff --git a/src/main.c b/src/main.c index b74907d..8eeb21f 100644 --- a/src/main.c +++ b/src/main.c | |||
@@ -14,10 +14,12 @@ | |||
14 | 14 | ||
15 | #include "addr_list.h" | 15 | #include "addr_list.h" |
16 | #include "beacon_data.h" | 16 | #include "beacon_data.h" |
17 | #include "retransmit.h" | ||
18 | #include "transmit.h" | ||
17 | 19 | ||
18 | static unsigned char kBroadcastAddress[] = "\xFF\xFF\xFF\xFF\xFF\xFF"; | 20 | #define CHANNEL_NUM 13 |
19 | 21 | ||
20 | static pthread_mutex_t socket_mutex = PTHREAD_MUTEX_INITIALIZER; | 22 | static unsigned char kBroadcastAddress[] = "\xFF\xFF\xFF\xFF\xFF\xFF"; |
21 | 23 | ||
22 | static pthread_mutex_t seq_mutex = PTHREAD_MUTEX_INITIALIZER; | 24 | static pthread_mutex_t seq_mutex = PTHREAD_MUTEX_INITIALIZER; |
23 | static int seq_number = 0; | 25 | static int seq_number = 0; |
@@ -50,7 +52,8 @@ int get_interface_index(int sock, const char *interface, int *index) | |||
50 | return 0; | 52 | return 0; |
51 | } | 53 | } |
52 | 54 | ||
53 | int get_next_seq_number() { | 55 | int get_next_seq_number() |
56 | { | ||
54 | pthread_mutex_lock(&seq_mutex); | 57 | pthread_mutex_lock(&seq_mutex); |
55 | int next = seq_number++; | 58 | int next = seq_number++; |
56 | pthread_mutex_unlock(&seq_mutex); | 59 | pthread_mutex_unlock(&seq_mutex); |
@@ -62,7 +65,7 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number, | |||
62 | { | 65 | { |
63 | struct libwifi_beacon beacon = {0}; | 66 | struct libwifi_beacon beacon = {0}; |
64 | 67 | ||
65 | if (libwifi_create_beacon(&beacon, kBroadcastAddress, tx_addr, tx_addr, NULL, 13)) | 68 | if (libwifi_create_beacon(&beacon, kBroadcastAddress, tx_addr, tx_addr, NULL, CHANNEL_NUM)) |
66 | { | 69 | { |
67 | printf("Could not create beacon frame.\n"); | 70 | printf("Could not create beacon frame.\n"); |
68 | return -3; | 71 | return -3; |
@@ -80,7 +83,8 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number, | |||
80 | } | 83 | } |
81 | 84 | ||
82 | static const unsigned char tim_bitmap[] = {0x01, 0x02, 0x00, 0x00}; | 85 | static const unsigned char tim_bitmap[] = {0x01, 0x02, 0x00, 0x00}; |
83 | if (libwifi_quick_add_tag(&beacon.tags, TAG_TIM, tim_bitmap, 4)) { | 86 | if (libwifi_quick_add_tag(&beacon.tags, TAG_TIM, tim_bitmap, 4)) |
87 | { | ||
84 | printf("Could not add TIM tag.\n"); | 88 | printf("Could not add TIM tag.\n"); |
85 | return -7; | 89 | return -7; |
86 | } | 90 | } |
@@ -134,88 +138,17 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number, | |||
134 | return beacon_size; | 138 | return beacon_size; |
135 | } | 139 | } |
136 | 140 | ||
137 | int prepend_radiotap(const unsigned char *input, int input_size, unsigned char **output) | ||
138 | { | ||
139 | static const unsigned char radiotap[] = "\x00\x00\x08\x00\x00\x00\x00\x00"; | ||
140 | static const int radiotap_size = 8; | ||
141 | |||
142 | int output_size = input_size + radiotap_size; | ||
143 | |||
144 | unsigned char *buf = (unsigned char *)malloc(output_size); | ||
145 | if (buf == NULL) | ||
146 | { | ||
147 | return -1; | ||
148 | } | ||
149 | |||
150 | memcpy(buf, radiotap, radiotap_size); | ||
151 | memcpy(buf + radiotap_size, input, input_size); | ||
152 | |||
153 | *output = buf; | ||
154 | |||
155 | return output_size; | ||
156 | } | ||
157 | |||
158 | int send_packet(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size) | ||
159 | { | ||
160 | struct sockaddr_ll socket_address; | ||
161 | socket_address.sll_ifindex = device_index; | ||
162 | socket_address.sll_halen = ETH_ALEN; | ||
163 | memcpy(socket_address.sll_addr, dst_addr, 6); | ||
164 | |||
165 | pthread_mutex_lock(&socket_mutex); | ||
166 | |||
167 | int ret = 0; | ||
168 | if (sendto(sock, packet, packet_size, 0, (struct sockaddr *)&socket_address, sizeof(struct sockaddr_ll)) < 0) | ||
169 | { | ||
170 | printf("Could not send packet: %d\n", errno); | ||
171 | ret = errno; | ||
172 | } | ||
173 | |||
174 | pthread_mutex_unlock(&socket_mutex); | ||
175 | |||
176 | return ret; | ||
177 | } | ||
178 | |||
179 | int send_packet_with_radiotap(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size) | ||
180 | { | ||
181 | struct libwifi_radiotap_info radiotap_info = {0}; | ||
182 | radiotap_info.present = (1 << IEEE80211_RADIOTAP_FLAGS) | (1 << IEEE80211_RADIOTAP_RATE); | ||
183 | radiotap_info.flags = IEEE80211_RADIOTAP_F_FCS | IEEE80211_RADIOTAP_F_SHORTPRE; | ||
184 | radiotap_info.rate_raw = 4; | ||
185 | |||
186 | unsigned char radiotap_buffer[256]; | ||
187 | int radiotap_size = libwifi_create_radiotap(&radiotap_info, radiotap_buffer); | ||
188 | |||
189 | uint32_t fcs = libwifi_calculate_fcs(packet, packet_size); | ||
190 | |||
191 | int buffer_size = radiotap_size + packet_size + sizeof(uint32_t); | ||
192 | unsigned char *buffer = malloc(buffer_size); | ||
193 | if (buffer == NULL) | ||
194 | { | ||
195 | printf("Could not prepend radiotap.\n"); | ||
196 | return -2; | ||
197 | } | ||
198 | |||
199 | memcpy(buffer, radiotap_buffer, radiotap_size); | ||
200 | memcpy(buffer + radiotap_size, packet, packet_size); | ||
201 | memcpy(buffer + radiotap_size + packet_size, (unsigned char*)&fcs, sizeof(uint32_t)); | ||
202 | |||
203 | int ret = send_packet(sock, device_index, dst_addr, buffer, buffer_size); | ||
204 | free(buffer); | ||
205 | |||
206 | return ret; | ||
207 | } | ||
208 | |||
209 | int send_ack(int sock, int device_index, const unsigned char dst_addr[6]) | 141 | int send_ack(int sock, int device_index, const unsigned char dst_addr[6]) |
210 | { | 142 | { |
211 | struct libwifi_cts ack_frame = {0}; | 143 | struct libwifi_cts ack_frame = {0}; |
212 | if (libwifi_create_cts(&ack_frame, dst_addr, 0)) { | 144 | if (libwifi_create_cts(&ack_frame, dst_addr, 0)) |
145 | { | ||
213 | printf("Could not create ack packet.\n"); | 146 | printf("Could not create ack packet.\n"); |
214 | } | 147 | } |
215 | 148 | ||
216 | ack_frame.frame_header.frame_control.subtype = SUBTYPE_ACK; | 149 | ack_frame.frame_header.frame_control.subtype = SUBTYPE_ACK; |
217 | 150 | ||
218 | int ret = send_packet_with_radiotap(sock, device_index, dst_addr, (unsigned char*)&ack_frame, sizeof(struct libwifi_cts)); | 151 | int ret = send_packet_with_radiotap(sock, device_index, dst_addr, (unsigned char *)&ack_frame, sizeof(struct libwifi_cts)); |
219 | 152 | ||
220 | return ret; | 153 | return ret; |
221 | } | 154 | } |
@@ -232,10 +165,22 @@ struct thread_info | |||
232 | int sock; | 165 | int sock; |
233 | unsigned char tx_addr[6]; | 166 | unsigned char tx_addr[6]; |
234 | int device_index; | 167 | int device_index; |
168 | struct retransmitter *retransmitter; | ||
235 | 169 | ||
236 | struct addr_list authenticated; | 170 | struct addr_list authenticated; |
237 | }; | 171 | }; |
238 | 172 | ||
173 | enum auth_state | ||
174 | { | ||
175 | kAuthStateAuthenticated, | ||
176 | kAuthStateAssociated, | ||
177 | }; | ||
178 | |||
179 | struct auth_info | ||
180 | { | ||
181 | enum auth_state auth_state; | ||
182 | }; | ||
183 | |||
239 | void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const u_char *bytes) | 184 | void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const u_char *bytes) |
240 | { | 185 | { |
241 | struct thread_info *tinfo = (struct thread_info *)userdata; | 186 | struct thread_info *tinfo = (struct thread_info *)userdata; |
@@ -249,7 +194,8 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const | |||
249 | 194 | ||
250 | if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_AUTH) | 195 | if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_AUTH) |
251 | { | 196 | { |
252 | if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6)) { | 197 | if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6)) |
198 | { | ||
253 | // This is not for me. Misdelivery. | 199 | // This is not for me. Misdelivery. |
254 | return; | 200 | return; |
255 | } | 201 | } |
@@ -271,11 +217,14 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const | |||
271 | return; | 217 | return; |
272 | } | 218 | } |
273 | 219 | ||
274 | //usleep(frame.header.ctrl.duration / 2 * 100); | 220 | // usleep(frame.header.ctrl.duration / 2 * 100); |
275 | 221 | ||
276 | send_ack(tinfo->sock, tinfo->device_index, frame.header.data.addr2); | 222 | send_ack(tinfo->sock, tinfo->device_index, frame.header.data.addr2); |
277 | 223 | ||
278 | addr_list_add(&tinfo->authenticated, frame.header.data.addr2); | 224 | struct auth_info *ainfo = (struct auth_info *)malloc(sizeof(struct auth_info)); |
225 | ainfo->auth_state = kAuthStateAuthenticated; | ||
226 | |||
227 | addr_list_add(&tinfo->authenticated, frame.header.data.addr2, ainfo); | ||
279 | 228 | ||
280 | struct libwifi_auth auth_resp = {0}; | 229 | struct libwifi_auth auth_resp = {0}; |
281 | if (libwifi_create_auth(&auth_resp, frame.header.data.addr2, frame.header.data.addr1, frame.header.data.addr1, AUTH_OPEN, 2, STATUS_SUCCESS)) | 230 | if (libwifi_create_auth(&auth_resp, frame.header.data.addr2, frame.header.data.addr1, frame.header.data.addr1, AUTH_OPEN, 2, STATUS_SUCCESS)) |
@@ -287,8 +236,8 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const | |||
287 | auth_resp.frame_header.duration = frame.header.ctrl.duration; | 236 | auth_resp.frame_header.duration = frame.header.ctrl.duration; |
288 | auth_resp.frame_header.seq_control.sequence_number = get_next_seq_number(); | 237 | auth_resp.frame_header.seq_control.sequence_number = get_next_seq_number(); |
289 | 238 | ||
290 | //static const unsigned char broadcom_tag_value[] = {0x00, 0x10, 0x18, 0x02, 0x00, 0xf0, 0x00, 0x00, 0x00}; | 239 | // static const unsigned char broadcom_tag_value[] = {0x00, 0x10, 0x18, 0x02, 0x00, 0xf0, 0x00, 0x00, 0x00}; |
291 | //libwifi_quick_add_tag(&auth_resp.tags, TAG_VENDOR_SPECIFIC, broadcom_tag_value, 9); | 240 | // libwifi_quick_add_tag(&auth_resp.tags, TAG_VENDOR_SPECIFIC, broadcom_tag_value, 9); |
292 | 241 | ||
293 | size_t auth_resp_size = libwifi_get_auth_length(&auth_resp); | 242 | size_t auth_resp_size = libwifi_get_auth_length(&auth_resp); |
294 | unsigned char *auth_resp_output = (unsigned char *)malloc(auth_resp_size); | 243 | unsigned char *auth_resp_output = (unsigned char *)malloc(auth_resp_size); |
@@ -306,23 +255,73 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const | |||
306 | 255 | ||
307 | libwifi_free_auth(&auth_resp); | 256 | libwifi_free_auth(&auth_resp); |
308 | 257 | ||
309 | //usleep(frame.header.ctrl.duration * 100); | 258 | // usleep(frame.header.ctrl.duration * 100); |
259 | |||
260 | send_until_acked(tinfo->retransmitter, frame.header.data.addr2, auth_resp_output, auth_resp_size); | ||
310 | 261 | ||
311 | if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size)) | 262 | printf("Successfully authenticated %s!\n", in_addr); |
263 | } | ||
264 | else if (frame.frame_control.type == TYPE_CONTROL && frame.frame_control.subtype == SUBTYPE_ACK) | ||
265 | { | ||
266 | if (strncmp(frame.body, tinfo->tx_addr, 6)) | ||
312 | { | 267 | { |
313 | printf("Could not send auth response.\n"); | 268 | // This is not for me. Misdelivery. |
314 | return; | 269 | return; |
315 | } | 270 | } |
316 | 271 | ||
317 | usleep(5000); | 272 | handle_ack(tinfo->retransmitter); |
273 | } | ||
274 | else if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_ASSOC_REQ) | ||
275 | { | ||
276 | if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6)) | ||
277 | { | ||
278 | // This is not for me. Misdelivery. | ||
279 | return; | ||
280 | } | ||
318 | 281 | ||
319 | if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size)) | 282 | unsigned char in_addr[18]; |
283 | format_mac_address(frame.header.data.addr2, in_addr); | ||
284 | printf("Association request from %s\n", in_addr); | ||
285 | |||
286 | if (!addr_list_contains(&tinfo->authenticated, frame.header.data.addr2)) | ||
320 | { | 287 | { |
321 | printf("Could not send auth response.\n"); | 288 | // Not authenticated. |
322 | return; | 289 | return; |
323 | } | 290 | } |
324 | 291 | ||
325 | printf("Successfully authenticated %s!\n", in_addr); | 292 | struct auth_info *ainfo = addr_list_get(&tinfo->authenticated, frame.header.data.addr2); |
293 | if (ainfo->auth_state != kAuthStateAuthenticated) | ||
294 | { | ||
295 | // Already associated. | ||
296 | return; | ||
297 | } | ||
298 | |||
299 | send_ack(tinfo->sock, tinfo->device_index, frame.header.data.addr2); | ||
300 | |||
301 | ainfo->auth_state = kAuthStateAssociated; | ||
302 | |||
303 | struct libwifi_assoc_resp assoc_resp = {0}; | ||
304 | if (libwifi_create_assoc_resp(&assoc_resp, frame.header.data.addr2, tinfo->tx_addr, tinfo->tx_addr, CHANNEL_NUM)) | ||
305 | { | ||
306 | printf("Could not create assoc response.\n"); | ||
307 | return; | ||
308 | } | ||
309 | |||
310 | assoc_resp.frame_header.seq_control.sequence_number = get_next_seq_number(); | ||
311 | |||
312 | size_t assoc_resp_length = libwifi_get_assoc_resp_length(&assoc_resp); | ||
313 | unsigned char *assoc_resp_buffer = malloc(assoc_resp_length); | ||
314 | if (libwifi_dump_assoc_resp(&assoc_resp, assoc_resp_buffer, assoc_resp_length) < 0) | ||
315 | { | ||
316 | printf("Could not dump assoc response.\n"); | ||
317 | return; | ||
318 | } | ||
319 | |||
320 | libwifi_free_assoc_resp(&assoc_resp); | ||
321 | |||
322 | send_until_acked(tinfo->retransmitter, frame.header.data.addr2, assoc_resp_buffer, assoc_resp_length); | ||
323 | |||
324 | printf("Successfully associated with %s!\n", in_addr); | ||
326 | } | 325 | } |
327 | } | 326 | } |
328 | 327 | ||
@@ -330,31 +329,6 @@ void *polling_thread(void *arg) | |||
330 | { | 329 | { |
331 | struct thread_info *tinfo = (struct thread_info *)arg; | 330 | struct thread_info *tinfo = (struct thread_info *)arg; |
332 | 331 | ||
333 | /*for (;;) { | ||
334 | struct pollfd pfd; | ||
335 | pfd.fd = tinfo->sock; | ||
336 | pfd.events = POLLIN; | ||
337 | |||
338 | if (poll(&pfd, 1, -1) < 0) { | ||
339 | printf("Error polling.\n"); | ||
340 | } else { | ||
341 | if (pfd.revents & POLLIN) { | ||
342 | unsigned char message[1024]; | ||
343 | int msglen = read(tinfo->sock, message, 1024); | ||
344 | if (msglen < 0) { | ||
345 | printf("Could not read from socket.\n"); | ||
346 | } else { | ||
347 | struct libwifi_frame frame = {0}; | ||
348 | if (libwifi_get_wifi_frame(&frame, message, msglen, true)) { | ||
349 | printf("Could not parse frame.\n"); | ||
350 | } else { | ||
351 | printf("Frame type: %d:%d\n", frame.frame_control.type, frame.frame_control.subtype); | ||
352 | } | ||
353 | } | ||
354 | } | ||
355 | } | ||
356 | }*/ | ||
357 | |||
358 | char errbuf[PCAP_ERRBUF_SIZE]; | 332 | char errbuf[PCAP_ERRBUF_SIZE]; |
359 | pcap_t *sniffer = pcap_create("mon0", errbuf); | 333 | pcap_t *sniffer = pcap_create("mon0", errbuf); |
360 | if (sniffer == NULL) | 334 | if (sniffer == NULL) |
@@ -396,21 +370,16 @@ int main(int argc, char **argv) | |||
396 | return 2; | 370 | return 2; |
397 | } | 371 | } |
398 | 372 | ||
373 | struct retransmitter *retransmitter = start_retransmit_thread(sock, device_index); | ||
374 | |||
399 | struct thread_info *tinfo = (struct thread_info *)malloc(sizeof(struct thread_info *)); | 375 | struct thread_info *tinfo = (struct thread_info *)malloc(sizeof(struct thread_info *)); |
400 | tinfo->sock = sock; | 376 | tinfo->sock = sock; |
401 | memcpy(tinfo->tx_addr, tx_addr, 6); | 377 | memcpy(tinfo->tx_addr, tx_addr, 6); |
402 | tinfo->device_index = device_index; | 378 | tinfo->device_index = device_index; |
379 | tinfo->retransmitter = retransmitter; | ||
403 | addr_list_init(&tinfo->authenticated); | 380 | addr_list_init(&tinfo->authenticated); |
404 | 381 | ||
405 | pthread_attr_t thread_attr; | 382 | pthread_create(&tinfo->thread_id, NULL, &polling_thread, tinfo); |
406 | if (pthread_attr_init(&thread_attr)) | ||
407 | { | ||
408 | printf("Could not initialize thread attr.\n"); | ||
409 | return 6; | ||
410 | } | ||
411 | |||
412 | pthread_create(&tinfo->thread_id, &thread_attr, &polling_thread, tinfo); | ||
413 | pthread_attr_destroy(&thread_attr); | ||
414 | 383 | ||
415 | int beacon_index = 0; | 384 | int beacon_index = 0; |
416 | for (;;) | 385 | for (;;) |
diff --git a/src/retransmit.c b/src/retransmit.c new file mode 100644 index 0000000..44f609c --- /dev/null +++ b/src/retransmit.c | |||
@@ -0,0 +1,169 @@ | |||
1 | #include "retransmit.h" | ||
2 | |||
3 | #include <libwifi.h> | ||
4 | #include <stdio.h> | ||
5 | #include <stdlib.h> | ||
6 | #include <string.h> | ||
7 | #include <unistd.h> | ||
8 | |||
9 | #include "transmit.h" | ||
10 | |||
11 | static pthread_mutex_t retransmitter_mutex = PTHREAD_MUTEX_INITIALIZER; | ||
12 | |||
13 | void ack_map_init(struct ack_map *map) | ||
14 | { | ||
15 | map->top = NULL; | ||
16 | } | ||
17 | |||
18 | void ack_map_add(struct ack_map *map, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size) | ||
19 | { | ||
20 | struct ack_map_node *cur = map->top; | ||
21 | struct ack_map_node *prev = NULL; | ||
22 | |||
23 | while (cur != NULL) | ||
24 | { | ||
25 | prev = cur; | ||
26 | cur = cur->next; | ||
27 | } | ||
28 | |||
29 | struct ack_map_node *next = (struct ack_map_node *)malloc(sizeof(struct ack_map_node)); | ||
30 | next->next = NULL; | ||
31 | memcpy(next->dst_addr, dst_addr, 6); | ||
32 | next->packet = (unsigned char *)malloc(packet_size); | ||
33 | memcpy(next->packet, packet, packet_size); | ||
34 | next->packet_size = packet_size; | ||
35 | next->retries_left = 7; | ||
36 | |||
37 | if (prev == NULL) | ||
38 | { | ||
39 | map->top = next; | ||
40 | } | ||
41 | else | ||
42 | { | ||
43 | prev->next = next; | ||
44 | } | ||
45 | } | ||
46 | |||
47 | void ack_map_pop(struct ack_map *map) | ||
48 | { | ||
49 | if (map->top != NULL) | ||
50 | { | ||
51 | struct ack_map_node *cur = map->top; | ||
52 | map->top = cur->next; | ||
53 | |||
54 | free(cur->packet); | ||
55 | free(cur); | ||
56 | } | ||
57 | } | ||
58 | |||
59 | int ack_map_length(struct ack_map *map) | ||
60 | { | ||
61 | int result = 0; | ||
62 | |||
63 | struct ack_map_node *cur = map->top; | ||
64 | while (cur != NULL) | ||
65 | { | ||
66 | result++; | ||
67 | |||
68 | cur = cur->next; | ||
69 | } | ||
70 | |||
71 | return result; | ||
72 | } | ||
73 | |||
74 | void ack_map_clear(struct ack_map *map) | ||
75 | { | ||
76 | while (map->top != NULL) | ||
77 | { | ||
78 | ack_map_pop(map); | ||
79 | } | ||
80 | } | ||
81 | |||
82 | void *retransmit_thread(void *arg) | ||
83 | { | ||
84 | struct retransmitter *retransmitter = (struct retransmitter *)arg; | ||
85 | |||
86 | for (;;) | ||
87 | { | ||
88 | pthread_mutex_lock(&retransmitter_mutex); | ||
89 | |||
90 | if (retransmitter->quit) | ||
91 | { | ||
92 | pthread_mutex_unlock(&retransmitter_mutex); | ||
93 | |||
94 | break; | ||
95 | } | ||
96 | |||
97 | struct ack_map_node *cur = retransmitter->messages.top; | ||
98 | if (cur != NULL) | ||
99 | { | ||
100 | send_packet_with_radiotap(retransmitter->sock, retransmitter->device_index, cur->dst_addr, cur->packet, cur->packet_size); | ||
101 | |||
102 | // Set retransmit flag for future. | ||
103 | cur->packet[1] |= (1 << 3); | ||
104 | |||
105 | cur->retries_left--; | ||
106 | |||
107 | if (cur->retries_left <= 0) | ||
108 | { | ||
109 | ack_map_pop(&retransmitter->messages); | ||
110 | } | ||
111 | } | ||
112 | |||
113 | pthread_mutex_unlock(&retransmitter_mutex); | ||
114 | |||
115 | usleep(5000); | ||
116 | } | ||
117 | |||
118 | return NULL; | ||
119 | } | ||
120 | |||
121 | struct retransmitter *start_retransmit_thread(int sock, int device_index) | ||
122 | { | ||
123 | struct retransmitter *retransmitter = (struct retransmitter *)malloc(sizeof(struct retransmitter)); | ||
124 | retransmitter->sock = sock; | ||
125 | retransmitter->device_index = device_index; | ||
126 | ack_map_init(&retransmitter->messages); | ||
127 | retransmitter->quit = false; | ||
128 | |||
129 | pthread_create(&retransmitter->thread_id, NULL, &retransmit_thread, retransmitter); | ||
130 | |||
131 | return retransmitter; | ||
132 | } | ||
133 | |||
134 | void send_until_acked(struct retransmitter *retransmitter, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size) | ||
135 | { | ||
136 | pthread_mutex_lock(&retransmitter_mutex); | ||
137 | |||
138 | ack_map_add(&retransmitter->messages, dst_addr, packet, packet_size); | ||
139 | |||
140 | printf("Adding message for " MACSTR ". Current length: %d\n", MAC2STR(dst_addr), ack_map_length(&retransmitter->messages)); | ||
141 | |||
142 | pthread_mutex_unlock(&retransmitter_mutex); | ||
143 | } | ||
144 | |||
145 | void handle_ack(struct retransmitter *retransmitter) | ||
146 | { | ||
147 | pthread_mutex_lock(&retransmitter_mutex); | ||
148 | |||
149 | if (retransmitter->messages.top != NULL) | ||
150 | { | ||
151 | retransmitter->messages.top->retries_left = 0; | ||
152 | } | ||
153 | |||
154 | printf("Received ACK. Current length: %d\n", ack_map_length(&retransmitter->messages)); | ||
155 | |||
156 | pthread_mutex_unlock(&retransmitter_mutex); | ||
157 | } | ||
158 | |||
159 | void free_retransmitter(struct retransmitter *retransmitter) | ||
160 | { | ||
161 | pthread_mutex_lock(&retransmitter_mutex); | ||
162 | retransmitter->quit = true; | ||
163 | pthread_mutex_unlock(&retransmitter_mutex); | ||
164 | |||
165 | pthread_join(retransmitter->thread_id, NULL); | ||
166 | |||
167 | ack_map_clear(&retransmitter->messages); | ||
168 | free(retransmitter); | ||
169 | } | ||
diff --git a/src/retransmit.h b/src/retransmit.h new file mode 100644 index 0000000..0d2a90b --- /dev/null +++ b/src/retransmit.h | |||
@@ -0,0 +1,32 @@ | |||
1 | #ifndef G4U_RETRANSMIT_H | ||
2 | #define G4U_RETRANSMIT_H | ||
3 | |||
4 | #include <pthread.h> | ||
5 | #include <stdbool.h> | ||
6 | |||
7 | struct ack_map_node { | ||
8 | unsigned char dst_addr[6]; | ||
9 | unsigned char* packet; | ||
10 | int packet_size; | ||
11 | int retries_left; | ||
12 | struct ack_map_node* next; | ||
13 | }; | ||
14 | |||
15 | struct ack_map { | ||
16 | struct ack_map_node* top; | ||
17 | }; | ||
18 | |||
19 | struct retransmitter { | ||
20 | pthread_t thread_id; | ||
21 | int sock; | ||
22 | int device_index; | ||
23 | struct ack_map messages; | ||
24 | bool quit; | ||
25 | }; | ||
26 | |||
27 | struct retransmitter* start_retransmit_thread(int sock, int device_index); | ||
28 | void send_until_acked(struct retransmitter* retransmitter, const unsigned char dst_addr[6], const unsigned char* packet, int packet_size); | ||
29 | void handle_ack(struct retransmitter* retransmitter); | ||
30 | void free_retransmitter(struct retransmitter* retransmitter); | ||
31 | |||
32 | #endif /* G4U_RETRANSMIT_H */ | ||
diff --git a/src/transmit.c b/src/transmit.c new file mode 100644 index 0000000..1978ff2 --- /dev/null +++ b/src/transmit.c | |||
@@ -0,0 +1,55 @@ | |||
1 | #include "transmit.h" | ||
2 | |||
3 | #include <libwifi.h> | ||
4 | #include <linux/if_packet.h> | ||
5 | #include <netinet/ether.h> | ||
6 | #include <pthread.h> | ||
7 | #include <stdio.h> | ||
8 | #include <stdlib.h> | ||
9 | |||
10 | static pthread_mutex_t socket_mutex = PTHREAD_MUTEX_INITIALIZER; | ||
11 | |||
12 | int send_packet(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size) | ||
13 | { | ||
14 | struct sockaddr_ll socket_address; | ||
15 | socket_address.sll_ifindex = device_index; | ||
16 | socket_address.sll_halen = ETH_ALEN; | ||
17 | memcpy(socket_address.sll_addr, dst_addr, 6); | ||
18 | |||
19 | pthread_mutex_lock(&socket_mutex); | ||
20 | |||
21 | int ret = 0; | ||
22 | if (sendto(sock, packet, packet_size, 0, (struct sockaddr *)&socket_address, sizeof(struct sockaddr_ll)) < 0) | ||
23 | { | ||
24 | printf("Could not send packet: %d\n", errno); | ||
25 | ret = errno; | ||
26 | } | ||
27 | |||
28 | pthread_mutex_unlock(&socket_mutex); | ||
29 | |||
30 | return ret; | ||
31 | } | ||
32 | |||
33 | int send_packet_with_radiotap(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char* packet, int packet_size) | ||
34 | { | ||
35 | struct libwifi_radiotap_info radiotap_info = {0}; | ||
36 | radiotap_info.present = (1 << IEEE80211_RADIOTAP_FLAGS) | (1 << IEEE80211_RADIOTAP_RATE); | ||
37 | radiotap_info.flags = IEEE80211_RADIOTAP_F_FCS | IEEE80211_RADIOTAP_F_SHORTPRE; | ||
38 | radiotap_info.rate_raw = 4; | ||
39 | |||
40 | unsigned char radiotap_buffer[256]; | ||
41 | int radiotap_size = libwifi_create_radiotap(&radiotap_info, radiotap_buffer); | ||
42 | |||
43 | uint32_t fcs = libwifi_calculate_fcs(packet, packet_size); | ||
44 | |||
45 | int buffer_size = radiotap_size + packet_size + sizeof(uint32_t); | ||
46 | unsigned char* buffer = (unsigned char*)malloc(buffer_size); | ||
47 | |||
48 | memcpy(buffer, radiotap_buffer, radiotap_size); | ||
49 | memcpy(buffer + radiotap_size, packet, packet_size); | ||
50 | memcpy(buffer + radiotap_size + packet_size, (unsigned char*)&fcs, sizeof(uint32_t)); | ||
51 | |||
52 | int ret = send_packet(sock, device_index, dst_addr, buffer, buffer_size); | ||
53 | |||
54 | return ret; | ||
55 | } | ||
diff --git a/src/transmit.h b/src/transmit.h new file mode 100644 index 0000000..f095586 --- /dev/null +++ b/src/transmit.h | |||
@@ -0,0 +1,6 @@ | |||
1 | #ifndef G4U_TRANSMIT_H | ||
2 | #define G4U_TRANSMIT_H | ||
3 | |||
4 | int send_packet_with_radiotap(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char* packet, int packet_size); | ||
5 | |||
6 | #endif /* G4U_TRANSMIT_H */ | ||