release/src/router/rc/listen.c

   1 /*
   2         listen.c -- Listen for any packet through an interface
   3         Copyright 2003, CyberTAN  Inc.  All Rights Reserved
   4
   5         This is UNPUBLISHED PROPRIETARY SOURCE CODE of CyberTAN Inc.
   6         the contents of this file may not be disclosed to third parties,
   7         copied or duplicated in any form without the prior written
   8         permission of CyberTAN Inc.
   9
  10         This software should be used as a reference only, and it not
  11         intended for production use!
  12
  13         THIS SOFTWARE IS OFFERED "AS IS", AND CYBERTAN GRANTS NO WARRANTIES OF ANY
  14         KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE.  CYBERTAN
  15         SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
  16         FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE
  17 */
  18
  19 #include <rc.h>
  20
  21 #include <sys/ioctl.h>
  22 #include <arpa/inet.h>
  23
  24 // for PF_PACKET
  25 #include <features.h>
  26 #if __GLIBC__ >=2 && __GLIBC_MINOR >= 1
  27 #include <netpacket/packet.h>
  28 #include <net/ethernet.h>
  29 #else
  30 #include <asm/types.h>
  31 #include <linux/if_packet.h>
  32 #include <linux/if_ether.h>
  33 #endif
  34
  35 #define LOG _dprintf
  36
  37 enum {L_FAIL, L_ERROR, L_UPGRADE, L_ESTABLISHED, L_SUCCESS};
  38
  39 struct iphdr {
  40         u_int8_t version;
  41         u_int8_t tos;
  42         u_int16_t tot_len;
  43         u_int16_t id;
  44         u_int16_t frag_off;
  45         u_int8_t ttl;
  46         u_int8_t protocol;
  47         u_int16_t check;
  48         u_int8_t saddr[4];
  49         u_int8_t daddr[4];
  50 } __attribute__((packed));
  51
  52 struct EthPacket {
  53         u_int8_t dst_mac[6];
  54         u_int8_t src_mac[6];
  55         u_int8_t type[2];
  56         //struct iphdr ip;      // size = 20
  57         u_int8_t version;
  58         u_int8_t tos;
  59         u_int16_t tot_len;
  60         u_int16_t id;
  61         u_int16_t frag_off;
  62         u_int8_t ttl;
  63         u_int8_t protocol;
  64         u_int16_t check;
  65         u_int8_t saddr[4];
  66         u_int8_t daddr[4];
  67         u_int8_t data[1500 - 20];
  68 } __attribute__((packed));
  69
  70
  71 static int read_interface(const char *interface, int *ifindex, unsigned char *mac)
  72 {
  73         int fd;
  74         struct ifreq ifr;
  75         int r;
  76
  77         memset(&ifr, 0, sizeof(struct ifreq));
  78         if ((fd = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) {
  79                 LOG("socket failed!: \n");
  80                 return -1;
  81         }
  82
  83         r = -1;
  84         ifr.ifr_addr.sa_family = AF_INET;
  85         strcpy(ifr.ifr_name, interface);
  86
  87         if (ioctl(fd, SIOCGIFINDEX, &ifr) == 0) {
  88                 *ifindex = ifr.ifr_ifindex;
  89                 LOG("adapter index %d \n", ifr.ifr_ifindex);
  90
  91                 if (ioctl(fd, SIOCGIFHWADDR, &ifr) == 0) {
  92                         memcpy(mac, ifr.ifr_hwaddr.sa_data, 6);
  93                         LOG("adapter hardware address %02x:%02x:%02x:%02x:%02x:%02x \n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  94                         r = 0;
  95                 }
  96                 else {
  97                         LOG("SIOCGIFHWADDR failed!\n");
  98                 }
  99         }
 100         else {
 101                 LOG("SIOCGIFINDEX failed!\n");
 102         }
 103         close(fd);
 104         return r;
 105 }
 106
 107 static int raw_socket(int ifindex)
 108 {
 109         int fd;
 110         struct sockaddr_ll sock;
 111
 112         LOG("Opening raw socket on ifindex %d\n", ifindex);
 113         if ((fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_IP))) < 0) {
 114                 LOG("socket call failed: \n");
 115                 return -1;
 116         }
 117
 118         sock.sll_family = AF_PACKET;
 119         sock.sll_protocol = htons(ETH_P_IP);
 120         sock.sll_ifindex = ifindex;
 121         if (bind(fd, (struct sockaddr *) &sock, sizeof(sock)) < 0) {
 122                 LOG("bind call failed: \n");
 123                 close(fd);
 124                 return -1;
 125         }
 126
 127         return fd;
 128 }
 129
 130 static u_int16_t checksum(void *addr, int count)
 131 {
 132         // Compute Internet Checksum for "count" bytes beginning at location "addr".
 133         register int32_t sum = 0;
 134         u_int16_t *source = (u_int16_t *) addr;
 135
 136         while (count > 1)  {
 137                 /*  This is the inner loop */
 138                 sum += *source++;
 139                 count -= 2;
 140         }
 141
 142         /*  Add left-over byte, if any */
 143         if (count > 0) {
 144                 /* Make sure that the left-over byte is added correctly both with little and big endian hosts */
 145                 u_int16_t tmp = 0;
 146                 *(unsigned char *)(&tmp) = *(unsigned char *)source;
 147                 sum += tmp;
 148         }
 149
 150         /*  Fold 32-bit sum to 16 bits */
 151         while (sum >> 16)
 152                 sum = (sum & 0xffff) + (sum >> 16);
 153         return ~sum;
 154 }
 155
 156 static int listen_interface(char *interface, int wan_proto)
 157 {
 158         int ifindex = 0;
 159         fd_set rfds;
 160         struct EthPacket packet;
 161         struct timeval tv;
 162         int retval;
 163         unsigned char mac[6];
 164         static int fd;
 165         int ret = L_SUCCESS;
 166         int bytes;
 167         u_int16_t check;
 168         struct in_addr ipaddr, netmask;
 169
 170
 171         if (read_interface(interface, &ifindex, mac) < 0) {
 172                 return L_ERROR;
 173         }
 174
 175         fd = raw_socket(ifindex);
 176         if (fd < 0) {
 177                 LOG("FATAL: couldn't listen on socket\n");
 178                 return L_ERROR;
 179         }
 180
 181         while (1) {
 182                 if (!wait_action_idle(5)) {     // Don't execute during upgrading
 183                         ret = L_UPGRADE;
 184                         break;
 185                 }
 186                 if (check_wanup()) {
 187                         ret = L_ESTABLISHED;
 188                         break;
 189                 }
 190
 191                 tv.tv_sec = 100000;
 192                 tv.tv_usec = 0;
 193                 FD_ZERO(&rfds);
 194                 FD_SET(fd, &rfds);
 195                 LOG("Waitting for select... \n");
 196                 retval = select(fd + 1, &rfds, NULL, NULL, &tv);
 197
 198                 if (retval == 0) {
 199                         printf("no packet recieved! \n\n");
 200                         continue;
 201                 }
 202
 203                 memset(&packet, 0, sizeof(struct EthPacket));
 204                 bytes = read(fd, &packet, sizeof(struct EthPacket));
 205                 if (bytes < 0) {
 206                         close(fd);
 207                         LOG("couldn't read on raw listening socket -- ignoring\n");
 208                         usleep(500000); // possible down interface, looping condition
 209                         return L_FAIL;
 210                 }
 211
 212                 if (bytes < (int) (sizeof(struct iphdr))) {
 213                         LOG("message too short, ignoring\n");
 214                         ret = L_FAIL;
 215                         goto EXIT;
 216                 }
 217
 218                 if (memcmp(mac, packet.dst_mac, 6) != 0) {
 219                         LOG("dest %02x:%02x:%02x:%02x:%02x:%02x mac not the router\n",
 220                                 packet.dst_mac[0], packet.dst_mac[1], packet.dst_mac[2],
 221                                 packet.dst_mac[3], packet.dst_mac[4], packet.dst_mac[5]);
 222                         ret = L_FAIL;
 223                         goto EXIT;
 224                 }
 225
 226                 if (inet_addr(nvram_safe_get("lan_ipaddr")) == *(u_int32_t *)packet.daddr) {
 227                         LOG("dest ip equal to lan ipaddr\n");
 228                         ret = L_FAIL;
 229                         goto EXIT;
 230                 }
 231
 232                 LOG("inet_addr=%x, packet.daddr=%x",inet_addr(nvram_safe_get("lan_ipaddr")),*(u_int32_t *)packet.daddr);
 233
 234                 //for (i=0; i<34;i++) {
 235                 //      if (i%16==0) printf("\n");
 236                 //      printf("%02x ",*(((u_int8_t *)packet)+i));
 237                 //}
 238                 //printf ("\n");
 239
 240                 LOG("%02X%02X%02X%02X%02X%02X,%02X%02X%02X%02X%02X%02X,%02X%02X\n",
 241                         packet.dst_mac[0], packet.dst_mac[1], packet.dst_mac[2],
 242                         packet.dst_mac[3], packet.dst_mac[4], packet.dst_mac[5],
 243                         packet.src_mac[0], packet.src_mac[1], packet.src_mac[2],
 244                         packet.src_mac[3], packet.src_mac[4], packet.src_mac[5],
 245                         packet.type[0],packet.type[1]);
 246
 247                 LOG("ip.version = %x", packet.version);
 248                 LOG("ip.tos = %x", packet.tos);
 249                 LOG("ip.tot_len = %x", packet.tot_len);
 250                 LOG("ip.id = %x", packet.id);
 251                 LOG("ip.ttl= %x", packet.ttl);
 252                 LOG("ip.protocol= %x", packet.protocol);
 253                 LOG("ip.check=%04x", packet.check);
 254                 LOG("ip.saddr=%08x", *(u_int32_t *)&(packet.saddr));
 255                 LOG("ip.daddr=%08x", *(u_int32_t *)&(packet.daddr));
 256
 257                 if (*(u_int16_t *)packet.type == 0x0800) {
 258                         LOG("not ip protocol");
 259                         ret = L_FAIL;
 260                         goto EXIT;
 261                 }
 262
 263                 /* ignore any extra garbage bytes */
 264                 bytes = ntohs(packet.tot_len);
 265
 266                 /* check IP checksum */
 267                 check = packet.check;
 268                 packet.check = 0;
 269
 270                 if (check != checksum(&(packet.version), sizeof(struct iphdr))) {
 271                         LOG("bad IP header checksum, ignoring\n");
 272                         LOG("check received = %X, should be %X",check, checksum(&(packet.version), sizeof(struct iphdr)));
 273                         ret = L_FAIL;
 274                         goto EXIT;
 275                 }
 276
 277                 LOG("oooooh!!! got some!\n");
 278
 279                 switch (wan_proto) {
 280                 case WP_PPTP:
 281                         inet_aton(nvram_safe_get("pptp_server_ip"), &ipaddr);
 282                         break;
 283                 case WP_L2TP:
 284 #ifdef TCONFIG_L2TP
 285                         inet_aton(nvram_safe_get("lan_ipaddr"), &ipaddr);       // checkme: why?        zzz
 286 #endif
 287                         break;
 288                 default:
 289                         inet_aton(nvram_safe_get("wan_ipaddr"), &ipaddr);
 290                         break;
 291                 }
 292                 inet_aton(nvram_safe_get("wan_netmask"), &netmask);
 293                 LOG("gateway=%08x", ipaddr.s_addr);
 294                 LOG("netmask=%08x", netmask.s_addr);
 295
 296                 if ((ipaddr.s_addr & netmask.s_addr) != (*(u_int32_t *)&(packet.daddr) & netmask.s_addr)) {
 297                         if (wan_proto == WP_L2TP) {
 298                                 ret = L_SUCCESS;
 299                                 goto EXIT;
 300                         }
 301                         else {
 302                                 ret = L_FAIL;
 303                                 goto EXIT;
 304                         }
 305                 }
 306         }
 307
 308 EXIT:
 309         if (fd) close(fd);
 310         return ret;
 311 }
 312
 313 int listen_main(int argc, char *argv[])
 314 {
 315         char *interface;
 316
 317         if (argc < 2) {
 318                 usage_exit(argv[0], "<interface>");
 319         }
 320
 321         interface = argv[1];
 322         printf("Starting listen on %s\n", interface);
 323
 324         if (fork() != 0) return 0;
 325
 326         while (1) {
 327                 switch (listen_interface(interface, get_wan_proto())) {
 328                 case L_SUCCESS:
 329                         LOG("\n*** LAN to WAN packet received\n\n");
 330                         force_to_dial();
 331
 332                         if (check_wanup()) return 0;
 333
 334                         // Connect fail, we want to re-connect session
 335                         sleep(3);
 336                         break;
 337                 case L_UPGRADE:
 338                         LOG("listen: nothing to do...\n");
 339                         return 0;
 340                 case L_ESTABLISHED:
 341                         LOG("The link had been established\n");
 342                         return 0;
 343                 case L_ERROR:
 344                         LOG("ERROR\n");
 345                         return 0;
 346 /*              case L_FAIL:
 347                         break;  */
 348                 }
 349         }
 350 }