IBR-DTNSuite
0.8
|
00001 00002 #include <unistd.h> 00003 #include <stdio.h> 00004 #include <stdlib.h> 00005 #include <string.h> 00006 #include <inttypes.h> 00007 00008 #include "receiver.h" 00009 #include "ip.h" 00010 00011 00012 void process_ipv4( uint16_t size, char *packet); 00013 void process_icmp( uint16_t size, char *packet, uint16_t icmp_offset); 00014 uint16_t ip_checksum(void *start, unsigned int count, uint32_t initial_value); 00015 00016 00017 static int tun_fd = -1; 00018 00019 void process_packet(uint16_t size, char *packet) { 00020 if (size < 20) //minimal IPv4 Header 00021 return; 00022 00023 if ( (packet[IP_VERSION] & 0xF0) == 0x40 ) { 00024 process_ipv4(size, packet); 00025 } 00026 else { 00027 printf("IP version %x not implemented\n",(packet[IP_VERSION] % 0xF0 )); 00028 } 00029 } 00030 00031 00032 void print_packet(uint16_t size, char *packet ) { 00033 int i; 00034 for (i=0; i<size; i++) 00035 printf("%02X ",*(uint8_t *)(packet+i)); 00036 puts(""); 00037 } 00038 00039 void process_ipv4(uint16_t size, char *packet) { 00040 uint8_t protocol=packet[IPV4_PROTOCOL]; 00041 switch(protocol) { 00042 case IP_PROT_ICMP: printf("ICMP package, size %i\n",size-20); 00043 process_icmp(size,packet,20); 00044 break; 00045 case IP_PROT_UDP: printf("UDP package, size %i\n",size-20); 00046 break; 00047 case IP_PROT_TCP: printf("TCP package, size %i\n",size-20); 00048 break; 00049 default: printf("Unknown IP payload %x\n",protocol); 00050 } 00051 } 00052 00053 00054 00055 /* icmp_checksum=SAP_ip_request_Checksum(toSend->data,buffer->length-20,0); //Same length 00056 as request, subtract IP headers 00057 00058 //SET_UINT16(toSend->data,ICMP_CRC,icmp_checksum); 00059 buffer->data-=20; //Roll back for IP 00060 toSend->length=buffer->length-20; //as long as Echo Request, don't count I Header here 00061 SAP_ip_request_transmit(toSend->id,src_ip,PROTO_ICMP); 00062 00063 */ 00064 00065 00066 #define PING_BOOSTER 00067 void process_icmp( uint16_t size, char *packet, uint16_t icmp_offset) { 00068 #ifdef PING_BOOSTER 00069 uint8_t *icmp_frame= (uint8_t *) packet+icmp_offset; 00070 uint16_t checksum; 00071 00072 00073 if ( icmp_frame[ICMP_TYPE] != ICMP_ECHO_REQUEST ) 00074 return; 00075 00076 puts("PingBoost!"); 00077 uint8_t *ping_reply=malloc(size); 00078 uint32_t src,dst; 00079 00080 if (ping_reply==NULL) { 00081 puts("Malloc failed"); 00082 return; 00083 } 00084 00085 memcpy(ping_reply, packet, size); 00086 icmp_frame= (uint8_t *) ping_reply+icmp_offset; 00087 dst=GET_UINT32(packet,IPV4_DESTINATION); 00088 src=GET_UINT32(packet,IPV4_SOURCE); 00089 00090 icmp_frame[ICMP_TYPE]=ICMP_ECHO_REPLY; 00091 icmp_frame[ICMP_CODE]=ICMP_ECHO_REPLY; 00092 00093 //Reset checksum 00094 icmp_frame[ICMP_CRC] =0x00; 00095 icmp_frame[ICMP_CRC+1]=0x00; 00096 00097 checksum=ip_checksum(icmp_frame,size-20,0); 00098 printf("ICMP Check over size %i: %x",size-20,checksum); 00099 SET_UINT16(icmp_frame,ICMP_CRC, checksum); 00100 00101 //ICMP is done, change adressing in IP: 00102 SET_UINT32(ping_reply,IPV4_SOURCE,dst); 00103 SET_UINT32(ping_reply,IPV4_DESTINATION,src); 00104 00105 SET_UINT16(ping_reply,IPV4_CHECKSUM,0); 00106 checksum=ip_checksum(ping_reply,20,0); 00107 SET_UINT16(ping_reply,IPV4_CHECKSUM,checksum); 00108 00109 print_packet(size, (char *) ping_reply); 00110 00111 //ugly, won't be threadsafe later 00112 if ( write(tun_fd,ping_reply,size) == -1) { 00113 perror("PingBooster write error: "); 00114 } 00115 00116 00117 free(ping_reply); 00118 00119 00120 00121 00122 #endif 00123 } 00124 00125 00126 00127 00134 uint16_t ip_checksum(void *start, unsigned int count, uint32_t initial_value) 00135 { 00136 uint32_t sum = initial_value; //might be not 0 if protcol includes pseudo header (udp, tcp) 00137 00138 uint16_t * piStart; 00139 00140 piStart = start; 00141 while (count > 1) { 00142 sum += *piStart++; 00143 count -= 2; 00144 } 00145 00146 if (count) // add left-over byte, if any 00147 sum += *(unsigned char *)piStart; 00148 00149 while (sum >> 16) // fold 32-bit sum to 16 bits 00150 sum = (sum & 0xFFFF) + (sum >> 16); 00151 00152 sum=htons(sum); 00153 return ~sum; 00154 } 00155 00156 00157 00158 00159 void * receiver_run(void *arg) { 00160 char buf[BUFSIZE]; 00161 ssize_t bytes_read; 00162 tun_fd = *((int *)arg); 00163 int running=1; 00164 00165 while(running) { 00166 bytes_read=read(tun_fd, buf, BUFSIZE); 00167 if (bytes_read == 0) { //EOF 00168 running=0; 00169 continue; 00170 } 00171 else if (bytes_read == -1) { 00172 perror("Error reading from TUN: "); 00173 running=1; 00174 continue; 00175 } 00176 printf("Read %i bytes\n", bytes_read); 00177 print_packet(bytes_read,buf); 00178 process_packet(bytes_read, buf); 00179 } 00180 puts("Receiver thread down."); 00181 return 0; 00182 }