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