API tools faq
paste
Advertisement
Guest User

Wifibroadcast with diversity

a guest
May 22nd, 2015
308
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.15 KB | None | 0 0
raw download clone embed print report
  1. // (c)2015 befinitiv
  2.  
  3. /*
  4. * This program is free software; you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation; version 2.
  7. *
  8. * This program is distributed in the hope that it will be useful,
  9. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. * GNU General Public License for more details.
  12. *
  13. * You should have received a copy of the GNU General Public License along
  14. * with this program; if not, write to the Free Software Foundation, Inc.,
  15. * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  16. */
  17.  
  18.  
  19.  
  20. #include "lib.h"
  21. #include "wifibroadcast.h"
  22. #include "radiotap.h"
  23. #include <pthread.h>
  24.  
  25. #define MAX_PACKET_LENGTH 4192
  26. #define DIVERSITY_CHECK_INTERVAL 256
  27. #define MIN_BLOCK_LOSS_ERROR 3
  28.  
  29. typedef struct {
  30. int m_nChannel;
  31. int m_nChannelFlags;
  32. int m_nRate;
  33. int m_nAntenna;
  34. int m_nRadiotapFlags;
  35. } __attribute__((packed)) PENUMBRA_RADIOTAP_DATA;
  36.  
  37. int flagHelp = 0;
  38. int errors[MAX_PENUMBRA_INTERFACES];
  39. char *adaptors[MAX_PENUMBRA_INTERFACES];
  40. int selected_adapter = 0;
  41. int num_adapters = 0;
  42. int param_port = 0;
  43. int param_retransmission_block_size = 4;
  44.  
  45. void
  46. usage(void) {
  47. printf(
  48. "(c)2015 befinitiv. Based on packetspammer by Andy Green. Licensed under GPL2\n"
  49. "\n"
  50. "Usage: rx [options] <interface>\n\nOptions\n"
  51. "-p <port> Port number 0-255 (default 0)\n"
  52. "-b <blocksize> Number of packets in a retransmission block (default 1). Needs to match with tx.\n"
  53. "Example:\n"
  54. " echo -n mon0 > /sys/class/ieee80211/phy0/add_iface\n"
  55. " iwconfig mon0 mode monitor\n"
  56. " ifconfig mon0 up\n"
  57. " rx mon0 Receive raw packets on mon0 and output the payload to stdout\n"
  58. "\n");
  59. exit(1);
  60. }
  61.  
  62. void *capture(void *p);
  63.  
  64. int main(int argc, char *argv[]) {
  65.  
  66. while(1) {
  67. int nOptionIndex = 0;
  68. static const struct option optiona[] = {
  69. { "help", no_argument, &flagHelp, 1 },
  70. { 0, 0, 0, 0 }
  71. };
  72. int c = getopt_long(argc, argv, "hp:b:",
  73. optiona, &nOptionIndex);
  74.  
  75. if(c == -1)
  76. break;
  77.  
  78. switch (c) {
  79. case 0: // long option
  80. break;
  81.  
  82. case 'h': // help
  83. usage();
  84.  
  85. case 'p': //port
  86. param_port = atoi(optarg);
  87. break;
  88.  
  89. case 'b': //retransmission block size
  90. param_retransmission_block_size = atoi(optarg);
  91. break;
  92.  
  93. default:
  94. fprintf(stderr, "unknown switch %c\n", c);
  95. usage();
  96. break;
  97. }
  98. }
  99. int x = optind;
  100. int i = 0;
  101. while(x < argc && i < MAX_PENUMBRA_INTERFACES) {
  102. char *iface = malloc(16);
  103. strcpy(&iface[1], argv[x]);
  104. iface[0] = (char)(i&0xff); //for index
  105. pthread_t tid;
  106. pthread_create(&tid, NULL, capture, iface);
  107. x++;
  108. i++;
  109. num_adapters++;
  110. sleep(1);
  111. }
  112. //TODO: handle freeing *iface mallocs here
  113. memset(errors, 0, sizeof(errors));
  114. sleep(1000000L);
  115. return 0;
  116. }
  117.  
  118. void *capture(void *p) {
  119. char *iface = (char*)p;
  120. int index = (int)(iface[0]&0xff);
  121. iface++;
  122. int argc = 6;
  123. adaptors[index] = iface;
  124. u8 u8aSendBuffer[MAX_PACKET_LENGTH];
  125. char szErrbuf[PCAP_ERRBUF_SIZE];
  126. int n80211HeaderLength = 0, nLinkEncap = 0;
  127. int retval, bytes;
  128. pcap_t *ppcap = NULL;
  129. struct bpf_program bpfprogram;
  130. char szProgram[512], fBrokenSocket = 0;
  131. u16 u16HeaderLen;
  132. packet_buffer_t *packet_buffer_list = 0;
  133. int last_block_num = -1;
  134. int num_sent = 0;
  135. int num_lost = 0;
  136. int i;
  137. int last_block_num_err_check = 0;
  138.  
  139. if(optind >= argc)
  140. usage();
  141.  
  142. szErrbuf[0] = '\0';
  143. ppcap = pcap_open_live(iface, 2048, 1, 20, szErrbuf);
  144. if(ppcap == NULL) {
  145. fprintf(stderr, "Unable to open interface %s in pcap: %s\n",
  146. iface, szErrbuf);
  147. return 1;
  148. }
  149.  
  150. nLinkEncap = pcap_datalink(ppcap);
  151.  
  152. switch(nLinkEncap) {
  153.  
  154. case DLT_PRISM_HEADER:
  155. fprintf(stderr, "DLT_PRISM_HEADER Encap\n");
  156. n80211HeaderLength = 0x20; // ieee80211 comes after this
  157. sprintf(szProgram, "radio[0x4a:4]==0x13223344 && radio[0x4e:2] == 0x55%.2x", param_port);
  158. break;
  159.  
  160. case DLT_IEEE802_11_RADIO:
  161. fprintf(stderr, "DLT_IEEE802_11_RADIO Encap\n");
  162. n80211HeaderLength = 0x18; // ieee80211 comes after this
  163. sprintf(szProgram, "ether[0x0a:4]==0x13223344 && ether[0x0e:2] == 0x55%.2x", param_port);
  164. break;
  165.  
  166. default:
  167. fprintf(stderr, "!!! unknown encapsulation on %s !\n", iface);
  168. return 1;
  169.  
  170. }
  171.  
  172. if(pcap_compile(ppcap, &bpfprogram, szProgram, 1, 0) == -1) {
  173. puts(szProgram);
  174. puts(pcap_geterr(ppcap));
  175. return 1;
  176. } else {
  177. if(pcap_setfilter(ppcap, &bpfprogram) == -1) {
  178. fprintf(stderr, "%s\n", szProgram);
  179. fprintf(stderr, "%s\n", pcap_geterr(ppcap));
  180. } else {
  181. }
  182. pcap_freecode(&bpfprogram);
  183. }
  184.  
  185. packet_buffer_list = lib_alloc_packet_buffer_list(param_retransmission_block_size, MAX_PACKET_LENGTH);
  186.  
  187. while(!fBrokenSocket) {
  188. struct pcap_pkthdr * ppcapPacketHeader = NULL;
  189. struct ieee80211_radiotap_iterator rti;
  190. PENUMBRA_RADIOTAP_DATA prd;
  191. u8 * pu8Payload = u8aSendBuffer;
  192. int n;
  193. uint32_t seq_nr;
  194. int block_num;
  195. int packet_num;
  196. int checksum_correct;
  197.  
  198. retval = pcap_next_ex(ppcap, &ppcapPacketHeader,
  199. (const u_char**)&pu8Payload);
  200.  
  201. if(retval < 0) {
  202. fBrokenSocket = 1;
  203. continue;
  204. }
  205.  
  206. if(retval != 1) {
  207. errors[index]++;
  208. continue;
  209. }
  210.  
  211. u16HeaderLen = (pu8Payload[2] + (pu8Payload[3] << 8));
  212.  
  213. if(ppcapPacketHeader->len <
  214. (u16HeaderLen + n80211HeaderLength))
  215. continue;
  216.  
  217. bytes = ppcapPacketHeader->len -
  218. (u16HeaderLen + n80211HeaderLength);
  219. if(bytes < 0)
  220. continue;
  221.  
  222. if(ieee80211_radiotap_iterator_init(&rti,
  223. (struct ieee80211_radiotap_header *)pu8Payload,
  224. ppcapPacketHeader->len) < 0)
  225. continue;
  226.  
  227. while((n = ieee80211_radiotap_iterator_next(&rti)) == 0) {
  228.  
  229. switch(rti.this_arg_index) {
  230. case IEEE80211_RADIOTAP_RATE:
  231. prd.m_nRate = (*rti.this_arg);
  232.  
  233. case IEEE80211_RADIOTAP_CHANNEL:
  234. prd.m_nChannel =
  235. le16_to_cpu(*((u16 *)rti.this_arg));
  236. prd.m_nChannelFlags =
  237. le16_to_cpu(*((u16 *)(rti.this_arg + 2)));
  238. break;
  239.  
  240. case IEEE80211_RADIOTAP_ANTENNA:
  241. prd.m_nAntenna = (*rti.this_arg) + 1;
  242. break;
  243.  
  244. case IEEE80211_RADIOTAP_FLAGS:
  245. prd.m_nRadiotapFlags = *rti.this_arg;
  246. break;
  247. }
  248. }
  249. pu8Payload += u16HeaderLen + n80211HeaderLength;
  250.  
  251. if(prd.m_nRadiotapFlags & IEEE80211_RADIOTAP_F_FCS)
  252. bytes -= 4;
  253.  
  254. checksum_correct = (prd.m_nRadiotapFlags & 0x40) == 0;
  255.  
  256. //first 4 bytes are the sequence number
  257. seq_nr = *(uint32_t*)pu8Payload;
  258. pu8Payload += 4;
  259. bytes -= 4;
  260.  
  261. block_num = seq_nr / param_retransmission_block_size; //if retr_block_size would be limited to powers of two, this could be replaced by a logical AND operation
  262.  
  263. //if we received the start of a new block, we need to write out the old one
  264.  
  265. if(block_num != last_block_num && last_block_num >= 0 && checksum_correct) {
  266. for(i=0; i<param_retransmission_block_size; ++i) {
  267. packet_buffer_t *p = packet_buffer_list + i;
  268. num_sent++;
  269. if(p->valid && index == selected_adapter) {
  270. write(STDOUT_FILENO, p->data, p->len);
  271. } else if(!p->valid) {
  272. fprintf(stderr, "Lost a packet! %s Lossrate: %f\t(%d / %d)\n", iface, 1.0 * num_lost/num_sent, num_lost, num_sent);
  273. num_lost++;
  274. errors[index]++;
  275. }
  276. p->valid = 0;
  277. p->len = 0;
  278. }
  279. if(block_num > last_block_num + 1) {
  280. int lost_blocks = block_num - last_block_num - 1;
  281. num_lost += lost_blocks * param_retransmission_block_size;
  282. num_sent += lost_blocks * param_retransmission_block_size;
  283. fprintf(stderr, "Lost %d blocks! %s Lossrate %f\t(%d / %d) bn=%d lbn=%d\n", block_num - last_block_num - 1, iface, 1.0 * num_lost/num_sent, num_lost, num_sent, block_num, last_block_num);
  284. if((block_num - last_block_num - 1) >= MIN_BLOCK_LOSS_ERROR) errors[index]++;
  285. }
  286. }
  287.  
  288. //safety first: we only go to the next block if the FCS is correct
  289. if(checksum_correct)
  290. last_block_num = block_num;
  291. packet_num = seq_nr % param_retransmission_block_size; //if retr_block_size would be limited to powers of two, this could be replace by a locical and operation
  292.  
  293. //if the checksum is correct than it is safe to overwrite a packet without checking the vadility. if however the checksum is wrong, we only write to unitialized packets. this avoids overwriting frames with correct checksum with corrupted ones
  294. if(checksum_correct || packet_buffer_list[packet_num].valid == 0) {
  295. memcpy(packet_buffer_list[packet_num].data, pu8Payload, bytes);
  296. packet_buffer_list[packet_num].len = bytes;
  297. packet_buffer_list[packet_num].valid = 1;
  298. }
  299.  
  300. if((block_num % DIVERSITY_CHECK_INTERVAL == 0)
  301. && last_block_num_err_check != block_num) {
  302. last_block_num_err_check = block_num;
  303. int e = 99999;
  304. int h;
  305. int s = 0;
  306. int pe = errors[selected_adapter];
  307. for(h=0; h<num_adapters; h++) {
  308. if(errors[h] < e) {
  309. e = errors[h];
  310. s = h; //adaptor with lowest error rate within the last [DIVERSITY_CHECK_INTERVAL] blocks
  311. }
  312. errors[h] = 0;
  313. }
  314. if(e < pe
  315. && pe > 5) {
  316. selected_adapter = s;
  317. fprintf(stderr, "diversity: [%s]\n", adaptors[s]);
  318. }
  319. memset(errors, sizeof(errors), 0);
  320. }
  321. }
  322.  
  323. return 0;
  324.  
  325. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!