pppoe.c

   1 /*
   2  * Fernando ALVES 2013
   3  * Add functionality "server pppoe" to l2tpns.
   4  * GPL licenced
   5  */
   6
   7 #include <unistd.h>
   8 #include <assert.h>
   9 #include <stdlib.h>
  10 #include <stdio.h>
  11 #include <stdarg.h>
  12 #include <errno.h>
  13 #include <string.h>
  14 #include <pthread.h>
  15 #include <fcntl.h>
  16 #include <sys/socket.h>
  17 #include <sys/ioctl.h>
  18 #include <net/if.h>
  19 #include <net/ethernet.h>
  20 #include <netpacket/packet.h>
  21 #include <arpa/inet.h>
  22 #include <linux/if_pppox.h>
  23
  24 #include "l2tpns.h"
  25 #include "cluster.h"
  26 #include "constants.h"
  27 #include "md5.h"
  28 #include "util.h"
  29
  30 int pppoediscfd = -1;
  31 int pppoesessfd = -1;
  32
  33 static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  34
  35 /* PPPoE codes */
  36 #define CODE_PADI           0x09
  37 #define CODE_PADO           0x07
  38 #define CODE_PADR           0x19
  39 #define CODE_PADS           0x65
  40 #define CODE_PADT           0xA7
  41 #define CODE_SESS           0x00
  42
  43 /* PPPoE Tags */
  44 #define TAG_END_OF_LIST        0x0000
  45 #define TAG_SERVICE_NAME       0x0101
  46 #define TAG_AC_NAME            0x0102
  47 #define TAG_HOST_UNIQ          0x0103
  48 #define TAG_AC_COOKIE          0x0104
  49 #define TAG_VENDOR_SPECIFIC    0x0105
  50 #define TAG_RELAY_SESSION_ID   0x0110
  51 #define TAG_SERVICE_NAME_ERROR 0x0201
  52 #define TAG_AC_SYSTEM_ERROR    0x0202
  53 #define TAG_GENERIC_ERROR      0x0203
  54
  55 static char *code_pad[] = {
  56         "PADI",
  57         "PADO",
  58         "PADR",
  59         "PADS",
  60         "PADT",
  61         "SESS",
  62         NULL
  63 };
  64
  65 enum
  66 {
  67         INDEX_PADI = 0,
  68         INDEX_PADO,
  69         INDEX_PADR,
  70         INDEX_PADS,
  71         INDEX_PADT,
  72         INDEX_SESS
  73 };
  74
  75 // set up pppoe discovery socket
  76 static void init_pppoe_disc(void)
  77 {
  78         int on = 1;
  79         struct ifreq ifr;
  80         struct sockaddr_ll sa;
  81
  82         memset(&ifr, 0, sizeof(ifr));
  83         memset(&sa, 0, sizeof(sa));
  84
  85         pppoediscfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_DISC));
  86         if (pppoediscfd < 0)
  87         {
  88                 LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
  89                 exit(1);
  90         }
  91
  92         fcntl(pppoediscfd, F_SETFD, fcntl(pppoediscfd, F_GETFD) | FD_CLOEXEC);
  93
  94         if (setsockopt(pppoediscfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
  95         {
  96                 LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
  97                 exit(1);
  98         }
  99
 100         assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_name) - 1);
 101         if (*config->pppoe_if_name)
 102                 strncpy(ifr.ifr_name, config->pppoe_if_name, IFNAMSIZ);
 103
 104         if (ioctl(pppoediscfd, SIOCGIFHWADDR, &ifr))
 105         {
 106                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
 107                 exit(1);
 108         }
 109
 110         if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
 111         {
 112                 LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_name);
 113                 exit(1);
 114         }
 115
 116         memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 117
 118         if (ioctl(pppoediscfd, SIOCGIFMTU, &ifr))
 119         {
 120                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
 121                 exit(1);
 122         }
 123
 124         if (ifr.ifr_mtu < ETH_DATA_LEN)
 125                 LOG(0, 0, 0, "Error pppoe: interface %s has MTU of %i, should be %i\n", config->pppoe_if_name, ifr.ifr_mtu, ETH_DATA_LEN);
 126
 127         if (ioctl(pppoediscfd, SIOCGIFINDEX, &ifr))
 128         {
 129                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
 130                 exit(1);
 131         }
 132
 133         memset(&sa, 0, sizeof(sa));
 134         sa.sll_family = AF_PACKET;
 135         sa.sll_protocol = htons(ETH_P_PPP_DISC);
 136         sa.sll_ifindex = ifr.ifr_ifindex;
 137
 138         if (bind(pppoediscfd, (struct sockaddr *)&sa, sizeof(sa)))
 139         {
 140                 LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
 141                 exit(1);
 142         }
 143
 144         if (fcntl(pppoediscfd, F_SETFL, O_NONBLOCK))
 145         {
 146                 LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
 147                 exit(1);
 148         }
 149
 150 }
 151
 152 // set up pppoe session socket
 153 static void init_pppoe_sess(void)
 154 {
 155         int on = 1;
 156         struct ifreq ifr;
 157         struct sockaddr_ll sa;
 158
 159         memset(&ifr, 0, sizeof(ifr));
 160         memset(&sa, 0, sizeof(sa));
 161
 162         pppoesessfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_SES));
 163         if (pppoesessfd < 0)
 164         {
 165                 LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
 166                 exit(1);
 167         }
 168
 169         fcntl(pppoesessfd, F_SETFD, fcntl(pppoesessfd, F_GETFD) | FD_CLOEXEC);
 170
 171         if (setsockopt(pppoesessfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
 172         {
 173                 LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
 174                 exit(1);
 175         }
 176
 177         assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_name) - 1);
 178         if (*config->pppoe_if_name)
 179                 strncpy(ifr.ifr_name, config->pppoe_if_name, IFNAMSIZ);
 180
 181         if (ioctl(pppoesessfd, SIOCGIFHWADDR, &ifr))
 182         {
 183                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
 184                 exit(1);
 185         }
 186
 187         if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
 188         {
 189                 LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_name);
 190                 exit(1);
 191         }
 192
 193         memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 194
 195         if (ioctl(pppoesessfd, SIOCGIFMTU, &ifr))
 196         {
 197                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
 198                 exit(1);
 199         }
 200
 201         if (ifr.ifr_mtu < ETH_DATA_LEN)
 202                 LOG(0, 0, 0, "Error pppoe: interface %s has MTU of %i, should be %i\n", config->pppoe_if_name, ifr.ifr_mtu, ETH_DATA_LEN);
 203
 204         if (ioctl(pppoesessfd, SIOCGIFINDEX, &ifr))
 205         {
 206                 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
 207                 exit(1);
 208         }
 209
 210         memset(&sa, 0, sizeof(sa));
 211         sa.sll_family = AF_PACKET;
 212         sa.sll_protocol = htons(ETH_P_PPP_SES);
 213         sa.sll_ifindex = ifr.ifr_ifindex;
 214
 215         if (bind(pppoesessfd, (struct sockaddr *)&sa, sizeof(sa)))
 216         {
 217                 LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
 218                 exit(1);
 219         }
 220
 221         if (fcntl(pppoesessfd, F_SETFL, O_NONBLOCK))
 222         {
 223                 LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
 224                 exit(1);
 225         }
 226 }
 227
 228 // set up pppoe discovery/session socket
 229 void init_pppoe(void)
 230 {
 231         tunnelidt t = TUNNEL_ID_PPPOE;
 232
 233         init_pppoe_disc();
 234         init_pppoe_sess();
 235
 236         // Reserve the a pseudo tunnel for pppoe server
 237         if (t > config->cluster_highest_tunnelid)
 238                 config->cluster_highest_tunnelid = t;
 239
 240         memset(&tunnel[t], 0, sizeof(tunnel[t]));
 241         tunnel[t].state = TUNNELOPEN;
 242         STAT(tunnel_created);
 243 }
 244
 245 char * get_string_codepad(uint8_t codepad)
 246 {
 247         char * ptrch = NULL;
 248         switch(codepad)
 249         {
 250                 case CODE_PADI:
 251                 ptrch = code_pad[INDEX_PADI];
 252                 break;
 253
 254                 case CODE_PADO:
 255                 ptrch = code_pad[INDEX_PADO];
 256                 break;
 257
 258                 case CODE_PADR:
 259                 ptrch = code_pad[INDEX_PADR];
 260                 break;
 261
 262                 case CODE_PADS:
 263                 ptrch = code_pad[INDEX_PADS];
 264                 break;
 265
 266                 case CODE_PADT:
 267                 ptrch = code_pad[INDEX_PADT];
 268                 break;
 269
 270                 case CODE_SESS:
 271                 ptrch = code_pad[INDEX_SESS];
 272                 break;
 273         }
 274
 275         return ptrch;
 276 }
 277
 278 static uint8_t * setup_header(uint8_t *pack, const uint8_t *src, const uint8_t *dst, int code, uint16_t sid, uint16_t h_proto)
 279 {
 280         uint8_t * p;
 281
 282         // 14 bytes ethernet Header + 6 bytes header pppoe
 283         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 284         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 285
 286         memcpy(ethhdr->h_source, src, ETH_ALEN);
 287         memcpy(ethhdr->h_dest, dst, ETH_ALEN);
 288         ethhdr->h_proto = htons(h_proto);
 289
 290         hdr->ver = 1;
 291         hdr->type = 1;
 292         hdr->code = code;
 293         hdr->sid = htons(sid);
 294         hdr->length = 0;
 295
 296         p = (uint8_t *)(pack + ETH_HLEN + sizeof(*hdr));
 297
 298         return p;
 299 }
 300
 301 static void add_tag(uint8_t *pack, int type, const uint8_t *data, int len)
 302 {
 303         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 304         struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
 305
 306         tag->tag_type = htons(type);
 307         tag->tag_len = htons(len);
 308         memcpy(tag->tag_data, data, len);
 309
 310         hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + len);
 311 }
 312
 313 static void add_tag2(uint8_t *pack, const struct pppoe_tag *t)
 314 {
 315         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 316         struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
 317
 318         memcpy(tag, t, sizeof(*t) + ntohs(t->tag_len));
 319
 320         hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + ntohs(t->tag_len));
 321 }
 322
 323 static void pppoe_disc_send(const uint8_t *pack)
 324 {
 325         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 326         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 327         int n, s;
 328
 329         s = ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length);
 330
 331         LOG(3, 0, 0, "SENT pppoe_disc: Code %s to %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_dest));
 332         LOG_HEX(5, "pppoe_disc_send", pack, s);
 333
 334         n = write(pppoediscfd, pack, s);
 335         if (n < 0 )
 336                 LOG(0, 0, 0, "pppoe: write: %s\n", strerror(errno));
 337         else if (n != s) {
 338                 LOG(0, 0, 0, "pppoe: short write %i/%i\n", n,s);
 339         }
 340 }
 341
 342 void pppoe_sess_send(const uint8_t *pack, uint16_t l, tunnelidt t)
 343 {
 344         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 345         int n;
 346         uint16_t sizeppp;
 347         sessionidt s;
 348
 349         if (t != TUNNEL_ID_PPPOE)
 350         {
 351                 LOG(3, 0, t, "ERROR pppoe_sess_send: Tunnel %d is not a tunnel pppoe\n", t);
 352                 return;
 353         }
 354
 355         s = ntohs(hdr->sid);
 356         if (session[s].tunnel != t)
 357         {
 358                 LOG(3, s, t, "ERROR pppoe_sess_send: Session is not a session pppoe\n");
 359                 return;
 360         }
 361
 362         if (l < (ETH_HLEN + sizeof(*hdr) + 3))
 363         {
 364                 LOG(0, s, t, "ERROR pppoe_sess_send: packet too small for pppoe sent (size=%d)\n", l);
 365                 return;
 366         }
 367
 368         // recalculate the ppp frame length
 369         sizeppp = l - (ETH_HLEN + sizeof(*hdr));
 370         hdr->length = htons(sizeppp);
 371
 372         LOG_HEX(5, "pppoe_sess_send", pack, l);
 373
 374         n = write(pppoesessfd, pack, l);
 375         if (n < 0 )
 376                 LOG(0, s, t, "pppoe_sess_send: write: %s\n", strerror(errno));
 377         else if (n != l)
 378                 LOG(0, s, t, "pppoe_sess_send: short write %i/%i\n", n,l);
 379 }
 380
 381 static void pppoe_send_err(const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, int code, int tag_type)
 382 {
 383         uint8_t pack[ETHER_MAX_LEN];
 384
 385         setup_header(pack, config->pppoe_hwaddr, addr, code, 0, ETH_P_PPP_DISC);
 386
 387         add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
 388         add_tag(pack, tag_type, NULL, 0);
 389
 390         if (host_uniq)
 391                 add_tag2(pack, host_uniq);
 392
 393         if (relay_sid)
 394                 add_tag2(pack, relay_sid);
 395
 396         pppoe_disc_send(pack);
 397 }
 398
 399 // generate cookie
 400 static void pppoe_gen_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *out)
 401 {
 402         MD5_CTX ctx;
 403
 404         MD5_Init(&ctx);
 405         MD5_Update(&ctx, config->l2tp_secret, strlen(config->l2tp_secret));
 406         MD5_Update(&ctx, (void *) serv_hwaddr, ETH_ALEN);
 407         MD5_Update(&ctx, (void *) client_hwaddr, ETH_ALEN);
 408         MD5_Final(out, &ctx);
 409 }
 410
 411 // check cookie
 412 static int pppoe_check_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *cookie)
 413 {
 414         hasht hash;
 415
 416         pppoe_gen_cookie(serv_hwaddr, client_hwaddr, hash);
 417
 418         return memcmp(hash, cookie, 16);
 419 }
 420
 421 static void pppoe_send_PADO(const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, const struct pppoe_tag *service_name)
 422 {
 423         uint8_t pack[ETHER_MAX_LEN];
 424         hasht hash;
 425
 426         setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADO, 0, ETH_P_PPP_DISC);
 427
 428         add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
 429
 430         if (service_name)
 431                 add_tag2(pack, service_name);
 432
 433         pppoe_gen_cookie(config->pppoe_hwaddr, addr, hash);
 434         add_tag(pack, TAG_AC_COOKIE, hash, 16);
 435
 436         if (host_uniq)
 437                 add_tag2(pack, host_uniq);
 438
 439         if (relay_sid)
 440                 add_tag2(pack, relay_sid);
 441
 442         pppoe_disc_send(pack);
 443 }
 444
 445 static void pppoe_send_PADS(uint16_t sid, const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, const struct pppoe_tag *service_name)
 446 {
 447         uint8_t pack[ETHER_MAX_LEN];
 448
 449         setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADS, sid, ETH_P_PPP_DISC);
 450
 451         add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
 452
 453         add_tag2(pack, service_name);
 454
 455         if (host_uniq)
 456                 add_tag2(pack, host_uniq);
 457
 458         if (relay_sid)
 459                 add_tag2(pack, relay_sid);
 460
 461         pppoe_disc_send(pack);
 462 }
 463
 464 static void pppoe_send_PADT(uint16_t sid)
 465 {
 466         uint8_t pack[ETHER_MAX_LEN];
 467
 468         setup_header(pack, config->pppoe_hwaddr, session[sid].src_hwaddr, CODE_PADT, sid, ETH_P_PPP_DISC);
 469
 470         add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
 471
 472         LOG(3, sid, session[sid].tunnel, "pppoe: Sent PADT\n");
 473
 474         pppoe_disc_send(pack);
 475 }
 476
 477 void pppoe_shutdown_session(sessionidt s)
 478 {
 479
 480         if (session[s].tunnel != TUNNEL_ID_PPPOE)
 481         {
 482                 LOG(3, s, session[s].tunnel, "ERROR pppoe_shutdown_session: Session is not a session pppoe\n");
 483                 return;
 484         }
 485
 486         pppoe_send_PADT(s);
 487 }
 488
 489 static void pppoe_recv_PADI(uint8_t *pack, int size)
 490 {
 491         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 492         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 493         struct pppoe_tag *tag;
 494         struct pppoe_tag *host_uniq_tag = NULL;
 495         struct pppoe_tag *relay_sid_tag = NULL;
 496         struct pppoe_tag *service_name_tag = NULL;
 497         int n, service_match = 0;
 498         int len;
 499
 500         if (hdr->sid)
 501                 return;
 502
 503         len = ntohs(hdr->length);
 504         for (n = 0; n < len; n += sizeof(*tag) + ntohs(tag->tag_len)) {
 505                 tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
 506                 if (n + sizeof(*tag) + ntohs(tag->tag_len) > len)
 507                         return;
 508                 switch (ntohs(tag->tag_type)) {
 509                         case TAG_END_OF_LIST:
 510                                 break;
 511                         case TAG_SERVICE_NAME:
 512                                 if (*config->pppoe_service_name && tag->tag_len)
 513                                 {
 514                                         if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
 515                                                 break;
 516                                         if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
 517                                                 break;
 518                                         service_match = 1;
 519                                 }
 520                                 else
 521                                 {
 522                                         service_name_tag = tag;
 523                                         service_match = 1;
 524                                 }
 525                                 break;
 526                         case TAG_HOST_UNIQ:
 527                                 host_uniq_tag = tag;
 528                                 break;
 529                         case TAG_RELAY_SESSION_ID:
 530                                 relay_sid_tag = tag;
 531                                 break;
 532                 }
 533         }
 534
 535         if (!service_match)
 536         {
 537                 LOG(3, 0, 0, "pppoe: discarding PADI packet (Service-Name mismatch)\n");
 538                 return;
 539         }
 540
 541         pppoe_send_PADO(ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
 542 }
 543
 544 static void pppoe_recv_PADR(uint8_t *pack, int size)
 545 {
 546         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 547         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 548         struct pppoe_tag *tag;
 549         struct pppoe_tag *host_uniq_tag = NULL;
 550         struct pppoe_tag *relay_sid_tag = NULL;
 551         struct pppoe_tag *ac_cookie_tag = NULL;
 552         struct pppoe_tag *service_name_tag = NULL;
 553         int n, service_match = 0;
 554         uint16_t sid;
 555
 556         if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
 557         {
 558                 LOG(1, 0, 0, "Rcv pppoe: discard PADR (destination address is broadcast)\n");
 559                 return;
 560         }
 561
 562         if (hdr->sid)
 563         {
 564                 LOG(1, 0, 0, "Rcv pppoe: discarding PADR packet (sid is not zero)\n");
 565                 return;
 566         }
 567
 568         for (n = 0; n < ntohs(hdr->length); n += sizeof(*tag) + ntohs(tag->tag_len))
 569         {
 570                 tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
 571                 switch (ntohs(tag->tag_type))
 572                 {
 573                         case TAG_END_OF_LIST:
 574                                 break;
 575                         case TAG_SERVICE_NAME:
 576                                 service_name_tag = tag;
 577                                 if (tag->tag_len == 0)
 578                                         service_match = 1;
 579                                 else if (*config->pppoe_service_name)
 580                                 {
 581                                         if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
 582                                                 break;
 583                                         if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
 584                                                 break;
 585                                         service_match = 1;
 586                                 }
 587                                 else
 588                                 {
 589                                         service_match = 1;
 590                                 }
 591                                 break;
 592                         case TAG_HOST_UNIQ:
 593                                 host_uniq_tag = tag;
 594                                 break;
 595                         case TAG_AC_COOKIE:
 596                                 ac_cookie_tag = tag;
 597                                 break;
 598                         case TAG_RELAY_SESSION_ID:
 599                                 relay_sid_tag = tag;
 600                                 break;
 601                 }
 602         }
 603
 604         if (!service_match)
 605         {
 606                 LOG(3, 0, 0, "pppoe: Service-Name mismatch\n");
 607                 pppoe_send_err(ethhdr->h_source, host_uniq_tag, relay_sid_tag, CODE_PADS, TAG_SERVICE_NAME_ERROR);
 608                 return;
 609         }
 610
 611         if (!ac_cookie_tag)
 612         {
 613                 LOG(3, 0, 0, "pppoe: discard PADR packet (no AC-Cookie tag present)\n");
 614                 return;
 615         }
 616
 617         if (ntohs(ac_cookie_tag->tag_len) != 16)
 618         {
 619                 LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie tag length)\n");
 620                 return;
 621         }
 622
 623         if (pppoe_check_cookie(ethhdr->h_dest, ethhdr->h_source, (uint8_t *) ac_cookie_tag->tag_data))
 624         {
 625                 LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie)\n");
 626                 return;
 627         }
 628
 629         sid = sessionfree;
 630         sessionfree = session[sid].next;
 631         memset(&session[sid], 0, sizeof(session[0]));
 632
 633         if (sid > config->cluster_highest_sessionid)
 634                 config->cluster_highest_sessionid = sid;
 635
 636         session[sid].opened = time_now;
 637         session[sid].tunnel = TUNNEL_ID_PPPOE;
 638         session[sid].last_packet = session[sid].last_data = time_now;
 639
 640         //strncpy(session[sid].called, called, sizeof(session[sid].called) - 1);
 641         //strncpy(session[sid].calling, calling, sizeof(session[sid].calling) - 1);
 642
 643         session[sid].ppp.phase = Establish;
 644         session[sid].ppp.lcp = Starting;
 645
 646         session[sid].magic = time_now; // set magic number
 647         session[sid].mru = PPPoE_MRU; // default
 648
 649         // start LCP
 650         sess_local[sid].lcp_authtype = config->radius_authprefer;
 651         sess_local[sid].ppp_mru = MRU;
 652
 653         // Set multilink options before sending initial LCP packet
 654         sess_local[sid].mp_mrru = 1614;
 655         sess_local[sid].mp_epdis = ntohl(config->iftun_address ? config->iftun_address : my_address);
 656
 657         memcpy(session[sid].src_hwaddr, ethhdr->h_source, ETH_ALEN);
 658         pppoe_send_PADS(sid, ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
 659
 660         sendlcp(sid, session[sid].tunnel);
 661         change_state(sid, lcp, RequestSent);
 662
 663 }
 664
 665 static void pppoe_recv_PADT(uint8_t *pack)
 666 {
 667         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 668         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 669
 670         if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
 671         {
 672                 LOG(3, 0, 0, "pppoe: discard PADT (destination address is broadcast)\n");
 673                 return;
 674         }
 675
 676         if (hdr->sid)
 677         {
 678                 if ((hdr->sid < MAXSESSION) && (session[hdr->sid].tunnel == TUNNEL_ID_PPPOE))
 679                         sessionshutdown(hdr->sid, "Client shutdown", CDN_ADMIN_DISC, 0);
 680         }
 681 }
 682
 683 // fill in a PPPOE message with a PPP frame,
 684 // returns start of PPP frame
 685 uint8_t *pppoe_makeppp(uint8_t *b, int size, uint8_t *p, int l, sessionidt s, tunnelidt t,
 686                                                 uint16_t mtype, uint8_t prio, bundleidt bid, uint8_t mp_bits)
 687 {
 688         uint16_t type = mtype;
 689         uint8_t *start = b;
 690         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(b + ETH_HLEN);
 691
 692         if (t != TUNNEL_ID_PPPOE)
 693                 return NULL;
 694
 695         if (size < 28) // Need more space than this!!
 696         {
 697                 LOG(0, s, t, "pppoe_makeppp buffer too small for pppoe header (size=%d)\n", size);
 698                 return NULL;
 699         }
 700
 701         // 14 bytes ethernet Header + 6 bytes header pppoe
 702         b = setup_header(b, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
 703
 704         // Check whether this session is part of multilink
 705         if (bid)
 706         {
 707                 if (bundle[bid].num_of_links > 1)
 708                         type = PPPMP; // Change PPP message type to the PPPMP
 709                 else
 710                         bid = 0;
 711         }
 712
 713         *(uint16_t *) b = htons(type);
 714         b += 2;
 715         hdr->length += 2;
 716
 717         if (bid)
 718         {
 719                 // Set the sequence number and (B)egin (E)nd flags
 720                 if (session[s].mssf)
 721                 {
 722                         // Set the multilink bits
 723                         uint16_t bits_send = mp_bits;
 724                         *(uint16_t *) b = htons((bundle[bid].seq_num_t & 0x0FFF)|bits_send);
 725                         b += 2;
 726                         hdr->length += 2;
 727                 }
 728                 else
 729                 {
 730                         *(uint32_t *) b = htonl(bundle[bid].seq_num_t);
 731                         // Set the multilink bits
 732                         *b = mp_bits;
 733                         b += 4;
 734                         hdr->length += 4;
 735                 }
 736
 737                 bundle[bid].seq_num_t++;
 738
 739                 // Add the message type if this fragment has the begin bit set
 740                 if (mp_bits & MP_BEGIN)
 741                 {
 742                         //*b++ = mtype; // The next two lines are instead of this
 743                         *(uint16_t *) b = htons(mtype); // Message type
 744                         b += 2;
 745                         hdr->length += 2;
 746                 }
 747         }
 748
 749         if ((b - start) + l > size)
 750         {
 751                 LOG(0, s, t, "pppoe_makeppp would overflow buffer (size=%d, header+payload=%td)\n", size, (b - start) + l);
 752                 return NULL;
 753         }
 754
 755         // Copy the payload
 756         if (p && l)
 757         {
 758                 memcpy(b, p, l);
 759                 hdr->length += l;
 760         }
 761
 762         return b;
 763 }
 764
 765 // pppoe discovery recv data
 766 void process_pppoe_disc(uint8_t *pack, int size)
 767 {
 768         struct ethhdr *ethhdr = (struct ethhdr *)pack;
 769         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 770
 771         LOG(3, 0, 0, "RCV pppoe_disc: Code %s from %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_source));
 772         LOG_HEX(5, "PPPOE Disc", pack, size);
 773
 774         if (!config->cluster_iam_master)
 775         {
 776                 if (hdr->code == CODE_PADI)
 777                         return; // Discard because the PADI is received by all (PADI is a broadcast diffusion)
 778
 779                 master_forward_pppoe_packet(pack, size, hdr->code);
 780                 return;
 781         }
 782
 783         if (size < (ETH_HLEN + sizeof(*hdr)))
 784         {
 785                 LOG(1, 0, 0, "Error pppoe_disc: short packet received (%i)\n", size);
 786                 return;
 787         }
 788
 789         if (memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) && memcmp(ethhdr->h_dest, config->pppoe_hwaddr, ETH_ALEN))
 790         {
 791                 LOG(1, 0, 0, "Error pppoe_disc: h_dest != bc_addr and  h_dest != config->pppoe_hwaddr\n");
 792                 return;
 793         }
 794
 795         if (!memcmp(ethhdr->h_source, bc_addr, ETH_ALEN))
 796         {
 797                 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (source address is broadcast)\n");
 798                 return;
 799         }
 800
 801         if ((ethhdr->h_source[0] & 1) != 0)
 802         {
 803                 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (host address is not unicast)\n");
 804                 return;
 805         }
 806
 807         if (size < ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length))
 808         {
 809                 LOG(1, 0, 0, "Error pppoe_disc: short packet received\n");
 810                 return;
 811         }
 812
 813         if (hdr->ver != 1)
 814         {
 815                 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported version %i)\n", hdr->ver);
 816                 return;
 817         }
 818
 819         if (hdr->type != 1)
 820         {
 821                 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported type %i)\n", hdr->type);
 822                 return;
 823         }
 824
 825         switch (hdr->code) {
 826                 case CODE_PADI:
 827                         pppoe_recv_PADI(pack, size);
 828                         break;
 829                 case CODE_PADR:
 830                         pppoe_recv_PADR(pack, size);
 831                         break;
 832                 case CODE_PADT:
 833                         pppoe_recv_PADT(pack);
 834                         break;
 835         }
 836 }
 837
 838 #ifdef LAC
 839 // Forward from pppoe to l2tp remote LNS
 840 static void pppoe_forwardto_session_rmlns(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
 841 {
 842         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 843         uint16_t lppp = ntohs(hdr->length);
 844         uint16_t ll2tp = lppp + 6;
 845         uint8_t *pppdata = (uint8_t *) hdr->tag;
 846         uint8_t *pl2tp = pppdata - 6;
 847         uint8_t *p = pl2tp;
 848         uint16_t t = 0, s = 0;
 849
 850         s = session[sess].forwardtosession;
 851         if (session[s].forwardtosession != sess)
 852         {
 853                 LOG(3, sess, session[sess].tunnel, "pppoe: Link Session (%u) broken\n", s);
 854                 return;
 855         }
 856
 857         t = session[s].tunnel;
 858         if (t >= MAXTUNNEL)
 859         {
 860                 LOG(1, s, t, "pppoe: Session with invalid tunnel ID\n");
 861                 return;
 862         }
 863
 864         if (!tunnel[t].isremotelns)
 865         {
 866                 LOG(3, sess, session[sess].tunnel, "pppoe: Link Tunnel/Session (%u/%u) broken\n", s, t);
 867                 return;
 868         }
 869
 870         // First word L2TP options (with no options)
 871         *(uint16_t *) p = htons(0x0002);
 872         p += 2;
 873         *(uint16_t *) p = htons(tunnel[t].far); // tunnel
 874         p += 2;
 875         *(uint16_t *) p = htons(session[s].far); // session
 876         p += 2;
 877
 878         if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
 879         {
 880                 session[sess].last_packet = session[sess].last_data = time_now;
 881                 // Update STAT IN
 882                 increment_counter(&session[sess].cin, &session[sess].cin_wrap, ll2tp);
 883                 session[sess].cin_delta += ll2tp;
 884                 session[sess].pin++;
 885                 sess_local[sess].cin += ll2tp;
 886                 sess_local[sess].pin++;
 887
 888                 session[s].last_data = time_now;
 889                 // Update STAT OUT
 890                 increment_counter(&session[s].cout, &session[s].cout_wrap, ll2tp); // byte count
 891                 session[s].cout_delta += ll2tp;
 892                 session[s].pout++;
 893                 sess_local[s].cout += ll2tp;
 894                 sess_local[s].pout++;
 895         }
 896         else
 897                 session[sess].last_packet = time_now;
 898
 899         tunnelsend(pl2tp, ll2tp, t); // send it...
 900 }
 901
 902 // Forward from l2tp to pppoe
 903 // (note: THIS ROUTINE WRITES TO pack[-20]).
 904 void pppoe_forwardto_session_pppoe(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
 905 {
 906         uint16_t t = 0, s = 0;
 907         uint16_t lpppoe = size - 2;
 908         uint8_t *p = pack + 2; // First word L2TP options
 909
 910         LOG(5, sess, session[sess].tunnel, "Forwarding data session to pppoe session %u\n", session[sess].forwardtosession);
 911
 912         s = session[sess].forwardtosession;
 913         t = session[s].tunnel;
 914
 915         if (*pack & 0x40)
 916         {       // length
 917                 p += 2;
 918                 lpppoe -= 2;
 919         }
 920
 921         *(uint16_t *) p = htons(tunnel[t].far); // tunnel
 922         p += 2;
 923         *(uint16_t *) p = htons(session[s].far); // session
 924         p += 2;
 925         lpppoe -= 4;
 926
 927         if (*pack & 0x08)
 928         {   // ns/nr
 929                 *(uint16_t *) p = htons(tunnel[t].ns); // sequence
 930                 p += 2;
 931                 *(uint16_t *) p = htons(tunnel[t].nr); // sequence
 932                 p += 2;
 933                 lpppoe -= 4;
 934         }
 935
 936         if (lpppoe > 2 && p[0] == 0xFF && p[1] == 0x03)
 937         {
 938                 // HDLC address header, discard in pppoe
 939                 p += 2;
 940                 lpppoe -= 2;
 941         }
 942
 943         lpppoe += (ETH_HLEN + sizeof(struct pppoe_hdr));
 944         p -= (ETH_HLEN + sizeof(struct pppoe_hdr));
 945
 946         // 14 bytes ethernet Header + 6 bytes header pppoe
 947         setup_header(p, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
 948
 949         if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
 950         {
 951                 session[sess].last_packet = session[sess].last_data = time_now;
 952                 // Update STAT IN
 953                 increment_counter(&session[sess].cin, &session[sess].cin_wrap, lpppoe);
 954                 session[sess].cin_delta += lpppoe;
 955                 session[sess].pin++;
 956                 sess_local[sess].cin += lpppoe;
 957                 sess_local[sess].pin++;
 958
 959                 session[s].last_data = time_now;
 960                 // Update STAT OUT
 961                 increment_counter(&session[s].cout, &session[s].cout_wrap, lpppoe); // byte count
 962                 session[s].cout_delta += lpppoe;
 963                 session[s].pout++;
 964                 sess_local[s].cout += lpppoe;
 965                 sess_local[s].pout++;
 966         }
 967         else
 968                 session[sess].last_packet = time_now;
 969
 970         tunnelsend(p, lpppoe, t); // send it....
 971 }
 972 #endif
 973
 974 void process_pppoe_sess(uint8_t *pack, int size)
 975 {
 976         //struct ethhdr *ethhdr = (struct ethhdr *)pack;
 977         struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
 978         uint16_t lppp = ntohs(hdr->length);
 979         uint8_t *pppdata = (uint8_t *) hdr->tag;
 980         uint16_t proto, sid, t;
 981
 982         sid = ntohs(hdr->sid);
 983         t = TUNNEL_ID_PPPOE;
 984
 985         LOG_HEX(5, "RCV PPPOE Sess", pack, size);
 986
 987         if (sid >= MAXSESSION)
 988         {
 989                 LOG(0, sid, t, "Received pppoe packet with invalid session ID\n");
 990                 STAT(tunnel_rx_errors);
 991                 return;
 992         }
 993
 994         if (session[sid].tunnel != t)
 995         {
 996                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
 997
 998                 LOG(1, sid, t, "ERROR process_pppoe_sess: Session is not a session pppoe\n");
 999                 return;
1000         }
1001
1002         if (hdr->ver != 1)
1003         {
1004                 LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported version %i)\n", hdr->ver);
1005                 return;
1006         }
1007
1008         if (hdr->type != 1)
1009         {
1010                 LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported type %i)\n", hdr->type);
1011                 return;
1012         }
1013
1014         if (lppp > 2 && pppdata[0] == 0xFF && pppdata[1] == 0x03)
1015         {       // HDLC address header, discard
1016                 LOG(5, sid, t, "pppoe_sess: HDLC address header, discard\n");
1017                 pppdata += 2;
1018                 lppp -= 2;
1019         }
1020         if (lppp < 2)
1021         {
1022                 LOG(3, sid, t, "Error process_pppoe_sess: Short ppp length %d\n", lppp);
1023                 return;
1024         }
1025         if (*pppdata & 1)
1026         {
1027                 proto = *pppdata++;
1028                 lppp--;
1029         }
1030         else
1031         {
1032                 proto = ntohs(*(uint16_t *) pppdata);
1033                 pppdata += 2;
1034                 lppp -= 2;
1035         }
1036
1037 #ifdef LAC
1038         if (session[sid].forwardtosession)
1039         {       // Must be forwaded to a remote lns tunnel l2tp
1040                 pppoe_forwardto_session_rmlns(pack, size, sid, proto);
1041                 return;
1042         }
1043 #endif
1044
1045         if (proto == PPPPAP)
1046         {
1047                 session[sid].last_packet = time_now;
1048                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1049                 processpap(sid, t, pppdata, lppp);
1050         }
1051         else if (proto == PPPCHAP)
1052         {
1053                 session[sid].last_packet = time_now;
1054                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1055                 processchap(sid, t, pppdata, lppp);
1056         }
1057         else if (proto == PPPLCP)
1058         {
1059                 session[sid].last_packet = time_now;
1060                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1061                 processlcp(sid, t, pppdata, lppp);
1062         }
1063         else if (proto == PPPIPCP)
1064         {
1065                 session[sid].last_packet = time_now;
1066                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1067                 processipcp(sid, t, pppdata, lppp);
1068         }
1069         else if (proto == PPPIPV6CP && config->ipv6_prefix.s6_addr[0])
1070         {
1071                 session[sid].last_packet = time_now;
1072                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1073                 processipv6cp(sid, t, pppdata, lppp);
1074         }
1075         else if (proto == PPPCCP)
1076         {
1077                 session[sid].last_packet = time_now;
1078                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1079                 processccp(sid, t, pppdata, lppp);
1080         }
1081         else if (proto == PPPIP)
1082         {
1083                 session[sid].last_packet = session[sid].last_data = time_now;
1084                 if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1085                 processipin(sid, t, pppdata, lppp);
1086         }
1087         else if (proto == PPPMP)
1088         {
1089                 session[sid].last_packet = session[sid].last_data = time_now;
1090                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1091                 processmpin(sid, t, pppdata, lppp);
1092         }
1093         else if (proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0])
1094         {
1095                 session[sid].last_packet = session[sid].last_data = time_now;
1096                 if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1097                 processipv6in(sid, t, pppdata, lppp);
1098         }
1099         else if (session[sid].ppp.lcp == Opened)
1100         {
1101                 session[sid].last_packet = time_now;
1102                 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1103                 protoreject(sid, t, pppdata, lppp, proto);
1104         }
1105         else
1106         {
1107                 LOG(3, sid, t, "process_pppoe_sess: Unknown PPP protocol 0x%04X received in LCP %s state\n",
1108                         proto, ppp_state(session[sid].ppp.lcp));
1109         }
1110 }
1111
1112 void pppoe_send_garp()
1113 {
1114         int s;
1115         struct ifreq ifr;
1116         uint8_t mac[6];
1117
1118         if (!*config->pppoe_if_name)
1119                 return;
1120
1121         s = socket(PF_INET, SOCK_DGRAM, 0);
1122         if (s < 0)
1123         {
1124                 LOG(0, 0, 0, "Error creating socket for GARP: %s\n", strerror(errno));
1125                 return;
1126         }
1127         memset(&ifr, 0, sizeof(ifr));
1128         strncpy(ifr.ifr_name, config->pppoe_if_name, sizeof(ifr.ifr_name) - 1);
1129         if (ioctl(s, SIOCGIFHWADDR, &ifr) < 0)
1130         {
1131                 LOG(0, 0, 0, "Error getting eth0 hardware address for GARP: %s\n", strerror(errno));
1132                 close(s);
1133                 return;
1134         }
1135         memcpy(mac, &ifr.ifr_hwaddr.sa_data, 6*sizeof(char));
1136         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0)
1137         {
1138                 LOG(0, 0, 0, "Error getting eth0 interface index for GARP: %s\n", strerror(errno));
1139                 close(s);
1140                 return;
1141         }
1142         close(s);
1143
1144         sendarp(ifr.ifr_ifindex, mac, config->iftun_address);
1145 }
1146
1147 // rcv pppoe data from slave
1148 void pppoe_process_forward(uint8_t *pack, int size, in_addr_t addr)
1149 {
1150         struct ethhdr *ethhdr = (struct ethhdr *)pack;
1151
1152         if (ethhdr->h_proto == htons(ETH_P_PPP_DISC))
1153                 process_pppoe_disc(pack, size);
1154         else if (ethhdr->h_proto == htons(ETH_P_PPP_SES))
1155                 process_pppoe_sess(pack, size);
1156         else
1157                 LOG(0, 0, 0, "pppoe_process_forward: I got a C_PPPOE_FORWARD from %s, but not a PPPOE data?\n", fmtaddr(addr, 0));
1158 }