API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 26th, 2015
273
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.41 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. #include <limits.h>
  25.  
  26. #define MAX_PACKET_LENGTH 4192
  27. #define BLOCKCACHE_SIZE 10
  28.  
  29. // this is where we store a summary of the
  30. // information from the radiotap header
  31.  
  32. typedef struct {
  33. int m_nChannel;
  34. int m_nChannelFlags;
  35. int m_nRate;
  36. int m_nAntenna;
  37. int m_nRadiotapFlags;
  38. } __attribute__((packed)) PENUMBRA_RADIOTAP_DATA;
  39.  
  40.  
  41.  
  42. int flagHelp = 0;
  43. int param_port = 0;
  44. int param_retransmission_block_size = 1;
  45. int last_block_num = -1;
  46. int num_sent = 0, num_lost = 0;
  47. packet_buffer_t *packet_buffer_list_cache[BLOCKCACHE_SIZE];
  48. int block_nums[BLOCKCACHE_SIZE];
  49. int last_block_written = 0;
  50. pthread_mutex_t thread_mutex;
  51.  
  52. void
  53. usage(void)
  54. {
  55. printf(
  56. "(c)2015 befinitiv. Based on packetspammer by Andy Green. Licensed under GPL2\n"
  57. "\n"
  58. "Usage: rx [options] <interfaces>\n\nOptions\n"
  59. "-p <port> Port number 0-255 (default 0)\n"
  60. "-b <block_numsize> Number of packets in a retransmission block (default 1). Needs to match with tx.\n"
  61. "Example:\n"
  62. " echo -n mon0 > /sys/class/ieee80211/phy0/add_iface\n"
  63. " iwconfig mon0 mode monitor\n"
  64. " ifconfig mon0 up\n"
  65. " rx mon0 Receive raw packets on mon0 and output the payload to stdout\n"
  66. "\n");
  67. exit(1);
  68. }
  69.  
  70. typedef struct {
  71. pcap_t *ppcap;
  72. int selectable_fd;
  73. int n80211HeaderLength;
  74. char name[16];
  75. } monitor_interface_t;
  76.  
  77. void open_and_configure_interface(const char *name, int port, monitor_interface_t *interface) {
  78. struct bpf_program bpfprogram;
  79. char szProgram[512];
  80. char szErrbuf[PCAP_ERRBUF_SIZE];
  81. // open the interface in pcap
  82.  
  83. szErrbuf[0] = '\0';
  84. strcpy(interface->name, name);
  85. interface->ppcap = pcap_open_live(name, 2048, 1, 0, szErrbuf);
  86. if (interface->ppcap == NULL) {
  87. fprintf(stderr, "Unable to open interface %s in pcap: %s\n",
  88. name, szErrbuf);
  89. exit(1);
  90. }
  91.  
  92.  
  93. if(pcap_setnonblock(interface->ppcap, 0, szErrbuf) < 0) {
  94. fprintf(stderr, "Error setting %s to nonblocking mode: %s\n", name, szErrbuf);
  95. }
  96.  
  97. int nLinkEncap = pcap_datalink(interface->ppcap);
  98.  
  99. switch (nLinkEncap) {
  100.  
  101. case DLT_PRISM_HEADER:
  102. fprintf(stderr, "DLT_PRISM_HEADER Encap\n");
  103. interface->n80211HeaderLength = 0x20; // ieee80211 comes after this
  104. sprintf(szProgram, "radio[0x4a:4]==0x13223344 && radio[0x4e:2] == 0x55%.2x", port);
  105. break;
  106.  
  107. case DLT_IEEE802_11_RADIO:
  108. fprintf(stderr, "DLT_IEEE802_11_RADIO Encap\n");
  109. interface->n80211HeaderLength = 0x18; // ieee80211 comes after this
  110. sprintf(szProgram, "ether[0x0a:4]==0x13223344 && ether[0x0e:2] == 0x55%.2x", port);
  111. break;
  112.  
  113. default:
  114. fprintf(stderr, "!!! unknown encapsulation on %s !\n", name);
  115. exit(1);
  116.  
  117. }
  118.  
  119. if (pcap_compile(interface->ppcap, &bpfprogram, szProgram, 1, 0) == -1) {
  120. puts(szProgram);
  121. puts(pcap_geterr(interface->ppcap));
  122. exit(1);
  123. } else {
  124. if (pcap_setfilter(interface->ppcap, &bpfprogram) == -1) {
  125. fprintf(stderr, "%s\n", szProgram);
  126. fprintf(stderr, "%s\n", pcap_geterr(interface->ppcap));
  127. } else {
  128. }
  129. pcap_freecode(&bpfprogram);
  130. }
  131.  
  132. interface->selectable_fd = pcap_get_selectable_fd(interface->ppcap);
  133. }
  134.  
  135. void writeblock(monitor_interface_t *interface, packet_buffer_t *packet_buffer_list) {
  136. int i;
  137. for(i=0; i<param_retransmission_block_size; ++i) {
  138. packet_buffer_t *p = packet_buffer_list + i;
  139. if(p->valid) {
  140. write(STDOUT_FILENO, p->data, p->len);
  141. }
  142. else {
  143. //fprintf(stderr, "%s Lost a packet %x! Lossrate: %f\t(%d / %d)\n", interface->name, i+(block_num-1)*param_retransmission_block_size, 1.0 * num_lost/num_sent, num_lost, num_sent);
  144. num_lost++;
  145. }
  146. p->valid = 0;
  147. p->crc_correct = 0;
  148. p->len = 0;
  149. }
  150. }
  151.  
  152. void process_packet(monitor_interface_t *interface) {
  153.  
  154. struct pcap_pkthdr * ppcapPacketHeader = NULL;
  155. struct ieee80211_radiotap_iterator rti;
  156. PENUMBRA_RADIOTAP_DATA prd;
  157. u8 payloadBuffer[MAX_PACKET_LENGTH];
  158. u8 *pu8Payload = payloadBuffer;
  159. int bytes;
  160. int n;
  161. uint32_t seq_nr;
  162. int block_num;
  163. int packet_num;
  164. int checksum_correct;
  165. int retval;
  166. int u16HeaderLen;
  167.  
  168. // receive
  169.  
  170. retval = pcap_next_ex(interface->ppcap, &ppcapPacketHeader,
  171. (const u_char**)&pu8Payload);
  172. if (retval < 0) {
  173. fprintf(stderr, "Socket broken\n");
  174. fprintf(stderr, "%s\n", pcap_geterr(interface->ppcap));
  175. exit(1);
  176. }
  177. if (retval != 1) {
  178. return;
  179. }
  180.  
  181. u16HeaderLen = (pu8Payload[2] + (pu8Payload[3] << 8));
  182.  
  183. if (ppcapPacketHeader->len <
  184. (u16HeaderLen + interface->n80211HeaderLength))
  185. return;
  186.  
  187. bytes = ppcapPacketHeader->len -
  188. (u16HeaderLen + interface->n80211HeaderLength);
  189. if (bytes < 0)
  190. return;
  191.  
  192. if (ieee80211_radiotap_iterator_init(&rti,
  193. (struct ieee80211_radiotap_header *)pu8Payload,
  194. ppcapPacketHeader->len) < 0)
  195. return;
  196.  
  197. while ((n = ieee80211_radiotap_iterator_next(&rti)) == 0) {
  198.  
  199. switch (rti.this_arg_index) {
  200. case IEEE80211_RADIOTAP_RATE:
  201. prd.m_nRate = (*rti.this_arg);
  202. break;
  203.  
  204. case IEEE80211_RADIOTAP_CHANNEL:
  205. prd.m_nChannel =
  206. le16_to_cpu(*((u16 *)rti.this_arg));
  207. prd.m_nChannelFlags =
  208. le16_to_cpu(*((u16 *)(rti.this_arg + 2)));
  209. break;
  210.  
  211. case IEEE80211_RADIOTAP_ANTENNA:
  212. prd.m_nAntenna = (*rti.this_arg) + 1;
  213. break;
  214.  
  215. case IEEE80211_RADIOTAP_FLAGS:
  216. prd.m_nRadiotapFlags = *rti.this_arg;
  217. break;
  218. }
  219. }
  220. pu8Payload += u16HeaderLen + interface->n80211HeaderLength;
  221.  
  222. if (prd.m_nRadiotapFlags & IEEE80211_RADIOTAP_F_FCS)
  223. bytes -= 4;
  224.  
  225. checksum_correct = (prd.m_nRadiotapFlags & 0x40) == 0;
  226.  
  227. //first 4 bytes are the sequence number
  228. seq_nr = *(uint32_t*)pu8Payload;
  229. pu8Payload += 4;
  230. bytes -= 4;
  231. 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
  232.  
  233. //sometimes block_num can arrive malformed so handle this error by ignoring it
  234. if(abs((block_num - last_block_written)) > 1000 //TODO: what if we stopped receiving data for a while on all adapters??
  235. && last_block_written != 0) return;
  236.  
  237. //if we received the start of a new block, we need to write out the old one
  238. if(block_num > last_block_num && last_block_num >= 0 && checksum_correct) {
  239. //write out old block
  240.  
  241. if(block_num > last_block_num + 1) {
  242. int lost_block_nums = block_num - last_block_num - 1;
  243. num_lost += lost_block_nums * param_retransmission_block_size;
  244. num_sent += lost_block_nums * param_retransmission_block_size;
  245. fprintf(stderr, "%s Lost %d blocks! Lossrate %f\t(%d / %d) %d %d\n",
  246. interface->name, block_num - last_block_num - 1, 1.0 * num_lost/num_sent, num_lost, num_sent, block_num, last_block_num);
  247. }
  248. }
  249.  
  250. //safety first: we only go to the next block if the FCS is correct
  251. if(checksum_correct && block_num > last_block_num)
  252. last_block_num = block_num;
  253.  
  254. 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
  255.  
  256. //only overwrite packets where the checksum is not yet correct. otherwise the packets are already received correctly
  257.  
  258. pthread_mutex_lock(&thread_mutex);
  259.  
  260. packet_buffer_t *packet_buffer_list = NULL;
  261. int a;
  262. int o;
  263. int lb = INT_MAX;
  264. for(a=0; a<BLOCKCACHE_SIZE; a++) {
  265. if(block_nums[a] == block_num) {
  266. //fprintf(stderr, "%s match block=%d last_block_written=%d\n", interface->name, block_num, last_block_written);
  267. packet_buffer_list = packet_buffer_list_cache[a];
  268. break;
  269. }
  270. if(block_nums[a] < lb) {
  271. lb = block_nums[a];
  272. o = a;
  273. }
  274. }
  275. if(packet_buffer_list == NULL) {
  276. if(block_nums[o] > last_block_written) {
  277. //fprintf(stderr, "%s writing block=%d, cblock=%d\n", interface->name, block_nums[o], block_num);
  278. writeblock(interface, packet_buffer_list_cache[o]);
  279. last_block_written = block_nums[o];
  280. }
  281. packet_buffer_list = packet_buffer_list_cache[o];
  282. block_nums[o] = block_num;
  283. }
  284. if(packet_buffer_list[packet_num].crc_correct == 0) {
  285. memcpy(packet_buffer_list[packet_num].data, pu8Payload, bytes);
  286. packet_buffer_list[packet_num].len = bytes;
  287. packet_buffer_list[packet_num].valid = 1;
  288. packet_buffer_list[packet_num].crc_correct = checksum_correct;
  289. }
  290.  
  291. pthread_mutex_unlock(&thread_mutex);
  292.  
  293. return;
  294. }
  295.  
  296. void *capture(void *i) {
  297. monitor_interface_t *interface = (monitor_interface_t*)i;
  298. for(;;) {
  299. process_packet(interface);
  300. }
  301. }
  302.  
  303. int
  304. main(int argc, char *argv[])
  305. {
  306. monitor_interface_t interfaces[MAX_PENUMBRA_INTERFACES];
  307. int num_interfaces = 0;
  308. memset(block_nums, 0, sizeof(block_nums));
  309. while (1) {
  310. int nOptionIndex;
  311. static const struct option optiona[] = {
  312. { "help", no_argument, &flagHelp, 1 },
  313. { 0, 0, 0, 0 }
  314. };
  315. int c = getopt_long(argc, argv, "hp:b:",
  316. optiona, &nOptionIndex);
  317.  
  318. if (c == -1)
  319. break;
  320. switch (c) {
  321. case 0: // long option
  322. break;
  323.  
  324. case 'h': // help
  325. usage();
  326.  
  327. case 'p': //port
  328. param_port = atoi(optarg);
  329. break;
  330.  
  331. case 'b': //retransmission block size
  332. param_retransmission_block_size = atoi(optarg);
  333. break;
  334.  
  335. default:
  336. fprintf(stderr, "unknown switch %c\n", c);
  337. usage();
  338. break;
  339. }
  340. }
  341.  
  342. if (optind >= argc)
  343. usage();
  344.  
  345. int a;
  346. for(a=0; a<BLOCKCACHE_SIZE; a++) {
  347. packet_buffer_list_cache[a] = lib_alloc_packet_buffer_list(param_retransmission_block_size, MAX_PACKET_LENGTH);
  348. }
  349.  
  350. int x = optind;
  351. while(x < argc && num_interfaces < MAX_PENUMBRA_INTERFACES) {
  352. open_and_configure_interface(argv[x], param_port, interfaces + num_interfaces);
  353. pthread_t tid;
  354. pthread_create(&tid, NULL, capture, interfaces + num_interfaces);
  355. ++num_interfaces;
  356. ++x;
  357. }
  358. sleep(1000000L);
  359. return (0);
  360. }
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!