SHARE
TWEET

Wifibroadcast with diversity

a guest May 22nd, 2015 262 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data
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
 
Top