+/*
+ * Fernando ALVES 2013
+ * Add functionality "server pppoe" to l2tpns.
+ * GPL licenced
+ */
+
+#include <unistd.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <errno.h>
+#include <string.h>
+#include <pthread.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <net/ethernet.h>
+#include <netpacket/packet.h>
+#include <arpa/inet.h>
+#include <linux/if_pppox.h>
+
+#include "l2tpns.h"
+#include "cluster.h"
+#include "constants.h"
+#include "md5.h"
+#include "util.h"
+
+int pppoediscfd = -1;
+int pppoesessfd = -1;
+
+static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+/* PPPoE codes */
+#define CODE_PADI 0x09
+#define CODE_PADO 0x07
+#define CODE_PADR 0x19
+#define CODE_PADS 0x65
+#define CODE_PADT 0xA7
+#define CODE_SESS 0x00
+
+/* PPPoE Tags */
+#define TAG_END_OF_LIST 0x0000
+#define TAG_SERVICE_NAME 0x0101
+#define TAG_AC_NAME 0x0102
+#define TAG_HOST_UNIQ 0x0103
+#define TAG_AC_COOKIE 0x0104
+#define TAG_VENDOR_SPECIFIC 0x0105
+#define TAG_RELAY_SESSION_ID 0x0110
+#define TAG_SERVICE_NAME_ERROR 0x0201
+#define TAG_AC_SYSTEM_ERROR 0x0202
+#define TAG_GENERIC_ERROR 0x0203
+
+static char *code_pad[] = {
+ "PADI",
+ "PADO",
+ "PADR",
+ "PADS",
+ "PADT",
+ "SESS",
+ NULL
+};
+
+enum
+{
+ INDEX_PADI = 0,
+ INDEX_PADO,
+ INDEX_PADR,
+ INDEX_PADS,
+ INDEX_PADT,
+ INDEX_SESS
+};
+
+// set up pppoe discovery socket
+static void init_pppoe_disc(void)
+{
+ int on = 1;
+ struct ifreq ifr;
+ struct sockaddr_ll sa;
+
+ memset(&ifr, 0, sizeof(ifr));
+ memset(&sa, 0, sizeof(sa));
+
+ pppoediscfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_DISC));
+ if (pppoediscfd < 0)
+ {
+ LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
+ exit(1);
+ }
+
+ fcntl(pppoediscfd, F_SETFD, fcntl(pppoediscfd, F_GETFD) | FD_CLOEXEC);
+
+ if (setsockopt(pppoediscfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
+ {
+ LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_name) - 1);
+ if (*config->pppoe_if_name)
+ strncpy(ifr.ifr_name, config->pppoe_if_name, IFNAMSIZ);
+
+ if (ioctl(pppoediscfd, SIOCGIFHWADDR, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
+ {
+ LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_name);
+ exit(1);
+ }
+
+ memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+ if (ioctl(pppoediscfd, SIOCGIFMTU, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if (ifr.ifr_mtu < ETH_DATA_LEN)
+ 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);
+
+ if (ioctl(pppoediscfd, SIOCGIFINDEX, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ memset(&sa, 0, sizeof(sa));
+ sa.sll_family = AF_PACKET;
+ sa.sll_protocol = htons(ETH_P_PPP_DISC);
+ sa.sll_ifindex = ifr.ifr_ifindex;
+
+ if (bind(pppoediscfd, (struct sockaddr *)&sa, sizeof(sa)))
+ {
+ LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if (fcntl(pppoediscfd, F_SETFL, O_NONBLOCK))
+ {
+ LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
+ exit(1);
+ }
+
+}
+
+// set up pppoe session socket
+static void init_pppoe_sess(void)
+{
+ int on = 1;
+ struct ifreq ifr;
+ struct sockaddr_ll sa;
+
+ memset(&ifr, 0, sizeof(ifr));
+ memset(&sa, 0, sizeof(sa));
+
+ pppoesessfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_SES));
+ if (pppoesessfd < 0)
+ {
+ LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
+ exit(1);
+ }
+
+ fcntl(pppoesessfd, F_SETFD, fcntl(pppoesessfd, F_GETFD) | FD_CLOEXEC);
+
+ if (setsockopt(pppoesessfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
+ {
+ LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_name) - 1);
+ if (*config->pppoe_if_name)
+ strncpy(ifr.ifr_name, config->pppoe_if_name, IFNAMSIZ);
+
+ if (ioctl(pppoesessfd, SIOCGIFHWADDR, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
+ {
+ LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_name);
+ exit(1);
+ }
+
+ memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+ if (ioctl(pppoesessfd, SIOCGIFMTU, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if (ifr.ifr_mtu < ETH_DATA_LEN)
+ 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);
+
+ if (ioctl(pppoesessfd, SIOCGIFINDEX, &ifr))
+ {
+ LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
+ exit(1);
+ }
+
+ memset(&sa, 0, sizeof(sa));
+ sa.sll_family = AF_PACKET;
+ sa.sll_protocol = htons(ETH_P_PPP_SES);
+ sa.sll_ifindex = ifr.ifr_ifindex;
+
+ if (bind(pppoesessfd, (struct sockaddr *)&sa, sizeof(sa)))
+ {
+ LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
+ exit(1);
+ }
+
+ if (fcntl(pppoesessfd, F_SETFL, O_NONBLOCK))
+ {
+ LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
+ exit(1);
+ }
+}
+
+// set up pppoe discovery/session socket
+void init_pppoe(void)
+{
+ tunnelidt t = TUNNEL_ID_PPPOE;
+
+ init_pppoe_disc();
+ init_pppoe_sess();
+
+ // Reserve the a pseudo tunnel for pppoe server
+ if (t > config->cluster_highest_tunnelid)
+ config->cluster_highest_tunnelid = t;
+
+ memset(&tunnel[t], 0, sizeof(tunnel[t]));
+ tunnel[t].state = TUNNELOPEN;
+ STAT(tunnel_created);
+}
+
+char * get_string_codepad(uint8_t codepad)
+{
+ char * ptrch = NULL;
+ switch(codepad)
+ {
+ case CODE_PADI:
+ ptrch = code_pad[INDEX_PADI];
+ break;
+
+ case CODE_PADO:
+ ptrch = code_pad[INDEX_PADO];
+ break;
+
+ case CODE_PADR:
+ ptrch = code_pad[INDEX_PADR];
+ break;
+
+ case CODE_PADS:
+ ptrch = code_pad[INDEX_PADS];
+ break;
+
+ case CODE_PADT:
+ ptrch = code_pad[INDEX_PADT];
+ break;
+
+ case CODE_SESS:
+ ptrch = code_pad[INDEX_SESS];
+ break;
+ }
+
+ return ptrch;
+}
+
+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)
+{
+ uint8_t * p;
+
+ // 14 bytes ethernet Header + 6 bytes header pppoe
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+
+ memcpy(ethhdr->h_source, src, ETH_ALEN);
+ memcpy(ethhdr->h_dest, dst, ETH_ALEN);
+ ethhdr->h_proto = htons(h_proto);
+
+ hdr->ver = 1;
+ hdr->type = 1;
+ hdr->code = code;
+ hdr->sid = htons(sid);
+ hdr->length = 0;
+
+ p = (uint8_t *)(pack + ETH_HLEN + sizeof(*hdr));
+
+ return p;
+}
+
+static void add_tag(uint8_t *pack, int type, const uint8_t *data, int len)
+{
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
+
+ tag->tag_type = htons(type);
+ tag->tag_len = htons(len);
+ memcpy(tag->tag_data, data, len);
+
+ hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + len);
+}
+
+static void add_tag2(uint8_t *pack, const struct pppoe_tag *t)
+{
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
+
+ memcpy(tag, t, sizeof(*t) + ntohs(t->tag_len));
+
+ hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + ntohs(t->tag_len));
+}
+
+static void pppoe_disc_send(const uint8_t *pack)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ int n, s;
+
+ s = ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length);
+
+ LOG(3, 0, 0, "SENT pppoe_disc: Code %s to %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_dest));
+ LOG_HEX(5, "pppoe_disc_send", pack, s);
+
+ n = write(pppoediscfd, pack, s);
+ if (n < 0 )
+ LOG(0, 0, 0, "pppoe: write: %s\n", strerror(errno));
+ else if (n != s) {
+ LOG(0, 0, 0, "pppoe: short write %i/%i\n", n,s);
+ }
+}
+
+void pppoe_sess_send(const uint8_t *pack, uint16_t l, tunnelidt t)
+{
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ int n;
+ uint16_t sizeppp;
+ sessionidt s;
+
+ if (t != TUNNEL_ID_PPPOE)
+ {
+ LOG(3, 0, t, "ERROR pppoe_sess_send: Tunnel %d is not a tunnel pppoe\n", t);
+ return;
+ }
+
+ s = ntohs(hdr->sid);
+ if (session[s].tunnel != t)
+ {
+ LOG(3, s, t, "ERROR pppoe_sess_send: Session is not a session pppoe\n");
+ return;
+ }
+
+ if (l < (ETH_HLEN + sizeof(*hdr) + 3))
+ {
+ LOG(0, s, t, "ERROR pppoe_sess_send: packet too small for pppoe sent (size=%d)\n", l);
+ return;
+ }
+
+ // recalculate the ppp frame length
+ sizeppp = l - (ETH_HLEN + sizeof(*hdr));
+ hdr->length = htons(sizeppp);
+
+ LOG_HEX(5, "pppoe_sess_send", pack, l);
+
+ n = write(pppoesessfd, pack, l);
+ if (n < 0 )
+ LOG(0, s, t, "pppoe_sess_send: write: %s\n", strerror(errno));
+ else if (n != l)
+ LOG(0, s, t, "pppoe_sess_send: short write %i/%i\n", n,l);
+}
+
+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)
+{
+ uint8_t pack[ETHER_MAX_LEN];
+
+ setup_header(pack, config->pppoe_hwaddr, addr, code, 0, ETH_P_PPP_DISC);
+
+ add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
+ add_tag(pack, tag_type, NULL, 0);
+
+ if (host_uniq)
+ add_tag2(pack, host_uniq);
+
+ if (relay_sid)
+ add_tag2(pack, relay_sid);
+
+ pppoe_disc_send(pack);
+}
+
+// generate cookie
+static void pppoe_gen_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *out)
+{
+ MD5_CTX ctx;
+
+ MD5_Init(&ctx);
+ MD5_Update(&ctx, config->l2tp_secret, strlen(config->l2tp_secret));
+ MD5_Update(&ctx, (void *) serv_hwaddr, ETH_ALEN);
+ MD5_Update(&ctx, (void *) client_hwaddr, ETH_ALEN);
+ MD5_Final(out, &ctx);
+}
+
+// check cookie
+static int pppoe_check_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *cookie)
+{
+ hasht hash;
+
+ pppoe_gen_cookie(serv_hwaddr, client_hwaddr, hash);
+
+ return memcmp(hash, cookie, 16);
+}
+
+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)
+{
+ uint8_t pack[ETHER_MAX_LEN];
+ hasht hash;
+
+ setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADO, 0, ETH_P_PPP_DISC);
+
+ add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
+
+ if (service_name)
+ add_tag2(pack, service_name);
+
+ pppoe_gen_cookie(config->pppoe_hwaddr, addr, hash);
+ add_tag(pack, TAG_AC_COOKIE, hash, 16);
+
+ if (host_uniq)
+ add_tag2(pack, host_uniq);
+
+ if (relay_sid)
+ add_tag2(pack, relay_sid);
+
+ pppoe_disc_send(pack);
+}
+
+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)
+{
+ uint8_t pack[ETHER_MAX_LEN];
+
+ setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADS, sid, ETH_P_PPP_DISC);
+
+ add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
+
+ add_tag2(pack, service_name);
+
+ if (host_uniq)
+ add_tag2(pack, host_uniq);
+
+ if (relay_sid)
+ add_tag2(pack, relay_sid);
+
+ pppoe_disc_send(pack);
+}
+
+static void pppoe_send_PADT(uint16_t sid)
+{
+ uint8_t pack[ETHER_MAX_LEN];
+
+ setup_header(pack, config->pppoe_hwaddr, session[sid].src_hwaddr, CODE_PADT, sid, ETH_P_PPP_DISC);
+
+ add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
+
+ LOG(3, sid, session[sid].tunnel, "pppoe: Sent PADT\n");
+
+ pppoe_disc_send(pack);
+}
+
+void pppoe_shutdown_session(sessionidt s)
+{
+
+ if (session[s].tunnel != TUNNEL_ID_PPPOE)
+ {
+ LOG(3, s, session[s].tunnel, "ERROR pppoe_shutdown_session: Session is not a session pppoe\n");
+ return;
+ }
+
+ pppoe_send_PADT(s);
+}
+
+static void pppoe_recv_PADI(uint8_t *pack, int size)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ struct pppoe_tag *tag;
+ struct pppoe_tag *host_uniq_tag = NULL;
+ struct pppoe_tag *relay_sid_tag = NULL;
+ struct pppoe_tag *service_name_tag = NULL;
+ int n, service_match = 0;
+ int len;
+
+ if (hdr->sid)
+ return;
+
+ len = ntohs(hdr->length);
+ for (n = 0; n < len; n += sizeof(*tag) + ntohs(tag->tag_len)) {
+ tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
+ if (n + sizeof(*tag) + ntohs(tag->tag_len) > len)
+ return;
+ switch (ntohs(tag->tag_type)) {
+ case TAG_END_OF_LIST:
+ break;
+ case TAG_SERVICE_NAME:
+ if (*config->pppoe_service_name && tag->tag_len)
+ {
+ if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
+ break;
+ if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
+ break;
+ service_match = 1;
+ }
+ else
+ {
+ service_name_tag = tag;
+ service_match = 1;
+ }
+ break;
+ case TAG_HOST_UNIQ:
+ host_uniq_tag = tag;
+ break;
+ case TAG_RELAY_SESSION_ID:
+ relay_sid_tag = tag;
+ break;
+ }
+ }
+
+ if (!service_match)
+ {
+ LOG(3, 0, 0, "pppoe: discarding PADI packet (Service-Name mismatch)\n");
+ return;
+ }
+
+ pppoe_send_PADO(ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
+}
+
+static void pppoe_recv_PADR(uint8_t *pack, int size)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ struct pppoe_tag *tag;
+ struct pppoe_tag *host_uniq_tag = NULL;
+ struct pppoe_tag *relay_sid_tag = NULL;
+ struct pppoe_tag *ac_cookie_tag = NULL;
+ struct pppoe_tag *service_name_tag = NULL;
+ int n, service_match = 0;
+ uint16_t sid;
+
+ if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
+ {
+ LOG(1, 0, 0, "Rcv pppoe: discard PADR (destination address is broadcast)\n");
+ return;
+ }
+
+ if (hdr->sid)
+ {
+ LOG(1, 0, 0, "Rcv pppoe: discarding PADR packet (sid is not zero)\n");
+ return;
+ }
+
+ for (n = 0; n < ntohs(hdr->length); n += sizeof(*tag) + ntohs(tag->tag_len))
+ {
+ tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
+ switch (ntohs(tag->tag_type))
+ {
+ case TAG_END_OF_LIST:
+ break;
+ case TAG_SERVICE_NAME:
+ service_name_tag = tag;
+ if (tag->tag_len == 0)
+ service_match = 1;
+ else if (*config->pppoe_service_name)
+ {
+ if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
+ break;
+ if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
+ break;
+ service_match = 1;
+ }
+ else
+ {
+ service_match = 1;
+ }
+ break;
+ case TAG_HOST_UNIQ:
+ host_uniq_tag = tag;
+ break;
+ case TAG_AC_COOKIE:
+ ac_cookie_tag = tag;
+ break;
+ case TAG_RELAY_SESSION_ID:
+ relay_sid_tag = tag;
+ break;
+ }
+ }
+
+ if (!service_match)
+ {
+ LOG(3, 0, 0, "pppoe: Service-Name mismatch\n");
+ pppoe_send_err(ethhdr->h_source, host_uniq_tag, relay_sid_tag, CODE_PADS, TAG_SERVICE_NAME_ERROR);
+ return;
+ }
+
+ if (!ac_cookie_tag)
+ {
+ LOG(3, 0, 0, "pppoe: discard PADR packet (no AC-Cookie tag present)\n");
+ return;
+ }
+
+ if (ntohs(ac_cookie_tag->tag_len) != 16)
+ {
+ LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie tag length)\n");
+ return;
+ }
+
+ if (pppoe_check_cookie(ethhdr->h_dest, ethhdr->h_source, (uint8_t *) ac_cookie_tag->tag_data))
+ {
+ LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie)\n");
+ return;
+ }
+
+ sid = sessionfree;
+ sessionfree = session[sid].next;
+ memset(&session[sid], 0, sizeof(session[0]));
+
+ if (sid > config->cluster_highest_sessionid)
+ config->cluster_highest_sessionid = sid;
+
+ session[sid].opened = time_now;
+ session[sid].tunnel = TUNNEL_ID_PPPOE;
+ session[sid].last_packet = session[sid].last_data = time_now;
+
+ //strncpy(session[sid].called, called, sizeof(session[sid].called) - 1);
+ //strncpy(session[sid].calling, calling, sizeof(session[sid].calling) - 1);
+
+ session[sid].ppp.phase = Establish;
+ session[sid].ppp.lcp = Starting;
+
+ session[sid].magic = time_now; // set magic number
+ session[sid].mru = PPPoE_MRU; // default
+
+ // start LCP
+ sess_local[sid].lcp_authtype = config->radius_authprefer;
+ sess_local[sid].ppp_mru = MRU;
+
+ // Set multilink options before sending initial LCP packet
+ sess_local[sid].mp_mrru = 1614;
+ sess_local[sid].mp_epdis = ntohl(config->iftun_address ? config->iftun_address : my_address);
+
+ memcpy(session[sid].src_hwaddr, ethhdr->h_source, ETH_ALEN);
+ pppoe_send_PADS(sid, ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
+
+ sendlcp(sid, session[sid].tunnel);
+ change_state(sid, lcp, RequestSent);
+
+}
+
+static void pppoe_recv_PADT(uint8_t *pack)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+
+ if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
+ {
+ LOG(3, 0, 0, "pppoe: discard PADT (destination address is broadcast)\n");
+ return;
+ }
+
+ if (hdr->sid)
+ {
+ if ((hdr->sid < MAXSESSION) && (session[hdr->sid].tunnel == TUNNEL_ID_PPPOE))
+ sessionshutdown(hdr->sid, "Client shutdown", CDN_ADMIN_DISC, 0);
+ }
+}
+
+// fill in a PPPOE message with a PPP frame,
+// returns start of PPP frame
+uint8_t *pppoe_makeppp(uint8_t *b, int size, uint8_t *p, int l, sessionidt s, tunnelidt t,
+ uint16_t mtype, uint8_t prio, bundleidt bid, uint8_t mp_bits)
+{
+ uint16_t type = mtype;
+ uint8_t *start = b;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(b + ETH_HLEN);
+
+ if (t != TUNNEL_ID_PPPOE)
+ return NULL;
+
+ if (size < 28) // Need more space than this!!
+ {
+ LOG(0, s, t, "pppoe_makeppp buffer too small for pppoe header (size=%d)\n", size);
+ return NULL;
+ }
+
+ // 14 bytes ethernet Header + 6 bytes header pppoe
+ b = setup_header(b, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
+
+ // Check whether this session is part of multilink
+ if (bid)
+ {
+ if (bundle[bid].num_of_links > 1)
+ type = PPPMP; // Change PPP message type to the PPPMP
+ else
+ bid = 0;
+ }
+
+ *(uint16_t *) b = htons(type);
+ b += 2;
+ hdr->length += 2;
+
+ if (bid)
+ {
+ // Set the sequence number and (B)egin (E)nd flags
+ if (session[s].mssf)
+ {
+ // Set the multilink bits
+ uint16_t bits_send = mp_bits;
+ *(uint16_t *) b = htons((bundle[bid].seq_num_t & 0x0FFF)|bits_send);
+ b += 2;
+ hdr->length += 2;
+ }
+ else
+ {
+ *(uint32_t *) b = htonl(bundle[bid].seq_num_t);
+ // Set the multilink bits
+ *b = mp_bits;
+ b += 4;
+ hdr->length += 4;
+ }
+
+ bundle[bid].seq_num_t++;
+
+ // Add the message type if this fragment has the begin bit set
+ if (mp_bits & MP_BEGIN)
+ {
+ //*b++ = mtype; // The next two lines are instead of this
+ *(uint16_t *) b = htons(mtype); // Message type
+ b += 2;
+ hdr->length += 2;
+ }
+ }
+
+ if ((b - start) + l > size)
+ {
+ LOG(0, s, t, "pppoe_makeppp would overflow buffer (size=%d, header+payload=%td)\n", size, (b - start) + l);
+ return NULL;
+ }
+
+ // Copy the payload
+ if (p && l)
+ {
+ memcpy(b, p, l);
+ hdr->length += l;
+ }
+
+ return b;
+}
+
+// pppoe discovery recv data
+void process_pppoe_disc(uint8_t *pack, int size)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+
+ LOG(3, 0, 0, "RCV pppoe_disc: Code %s from %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_source));
+ LOG_HEX(5, "PPPOE Disc", pack, size);
+
+ if (!config->cluster_iam_master)
+ {
+ if (hdr->code == CODE_PADI)
+ return; // Discard because the PADI is received by all (PADI is a broadcast diffusion)
+
+ master_forward_pppoe_packet(pack, size, hdr->code);
+ return;
+ }
+
+ if (size < (ETH_HLEN + sizeof(*hdr)))
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: short packet received (%i)\n", size);
+ return;
+ }
+
+ if (memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) && memcmp(ethhdr->h_dest, config->pppoe_hwaddr, ETH_ALEN))
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: h_dest != bc_addr and h_dest != config->pppoe_hwaddr\n");
+ return;
+ }
+
+ if (!memcmp(ethhdr->h_source, bc_addr, ETH_ALEN))
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: discarding packet (source address is broadcast)\n");
+ return;
+ }
+
+ if ((ethhdr->h_source[0] & 1) != 0)
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: discarding packet (host address is not unicast)\n");
+ return;
+ }
+
+ if (size < ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length))
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: short packet received\n");
+ return;
+ }
+
+ if (hdr->ver != 1)
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported version %i)\n", hdr->ver);
+ return;
+ }
+
+ if (hdr->type != 1)
+ {
+ LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported type %i)\n", hdr->type);
+ return;
+ }
+
+ switch (hdr->code) {
+ case CODE_PADI:
+ pppoe_recv_PADI(pack, size);
+ break;
+ case CODE_PADR:
+ pppoe_recv_PADR(pack, size);
+ break;
+ case CODE_PADT:
+ pppoe_recv_PADT(pack);
+ break;
+ }
+}
+
+#ifdef LAC
+// Forward from pppoe to l2tp remote LNS
+static void pppoe_forwardto_session_rmlns(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
+{
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ uint16_t lppp = ntohs(hdr->length);
+ uint16_t ll2tp = lppp + 6;
+ uint8_t *pppdata = (uint8_t *) hdr->tag;
+ uint8_t *pl2tp = pppdata - 6;
+ uint8_t *p = pl2tp;
+ uint16_t t = 0, s = 0;
+
+ s = session[sess].forwardtosession;
+ if (session[s].forwardtosession != sess)
+ {
+ LOG(3, sess, session[sess].tunnel, "pppoe: Link Session (%u) broken\n", s);
+ return;
+ }
+
+ t = session[s].tunnel;
+ if (t >= MAXTUNNEL)
+ {
+ LOG(1, s, t, "pppoe: Session with invalid tunnel ID\n");
+ return;
+ }
+
+ if (!tunnel[t].isremotelns)
+ {
+ LOG(3, sess, session[sess].tunnel, "pppoe: Link Tunnel/Session (%u/%u) broken\n", s, t);
+ return;
+ }
+
+ // First word L2TP options (with no options)
+ *(uint16_t *) p = htons(0x0002);
+ p += 2;
+ *(uint16_t *) p = htons(tunnel[t].far); // tunnel
+ p += 2;
+ *(uint16_t *) p = htons(session[s].far); // session
+ p += 2;
+
+ if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
+ {
+ session[sess].last_packet = session[sess].last_data = time_now;
+ // Update STAT IN
+ increment_counter(&session[sess].cin, &session[sess].cin_wrap, ll2tp);
+ session[sess].cin_delta += ll2tp;
+ session[sess].pin++;
+ sess_local[sess].cin += ll2tp;
+ sess_local[sess].pin++;
+
+ session[s].last_data = time_now;
+ // Update STAT OUT
+ increment_counter(&session[s].cout, &session[s].cout_wrap, ll2tp); // byte count
+ session[s].cout_delta += ll2tp;
+ session[s].pout++;
+ sess_local[s].cout += ll2tp;
+ sess_local[s].pout++;
+ }
+ else
+ session[sess].last_packet = time_now;
+
+ tunnelsend(pl2tp, ll2tp, t); // send it...
+}
+
+// Forward from l2tp to pppoe
+// (note: THIS ROUTINE WRITES TO pack[-20]).
+void pppoe_forwardto_session_pppoe(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
+{
+ uint16_t t = 0, s = 0;
+ uint16_t lpppoe = size - 2;
+ uint8_t *p = pack + 2; // First word L2TP options
+
+ LOG(5, sess, session[sess].tunnel, "Forwarding data session to pppoe session %u\n", session[sess].forwardtosession);
+
+ s = session[sess].forwardtosession;
+ t = session[s].tunnel;
+
+ if (*pack & 0x40)
+ { // length
+ p += 2;
+ lpppoe -= 2;
+ }
+
+ *(uint16_t *) p = htons(tunnel[t].far); // tunnel
+ p += 2;
+ *(uint16_t *) p = htons(session[s].far); // session
+ p += 2;
+ lpppoe -= 4;
+
+ if (*pack & 0x08)
+ { // ns/nr
+ *(uint16_t *) p = htons(tunnel[t].ns); // sequence
+ p += 2;
+ *(uint16_t *) p = htons(tunnel[t].nr); // sequence
+ p += 2;
+ lpppoe -= 4;
+ }
+
+ if (lpppoe > 2 && p[0] == 0xFF && p[1] == 0x03)
+ {
+ // HDLC address header, discard in pppoe
+ p += 2;
+ lpppoe -= 2;
+ }
+
+ lpppoe += (ETH_HLEN + sizeof(struct pppoe_hdr));
+ p -= (ETH_HLEN + sizeof(struct pppoe_hdr));
+
+ // 14 bytes ethernet Header + 6 bytes header pppoe
+ setup_header(p, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
+
+ if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
+ {
+ session[sess].last_packet = session[sess].last_data = time_now;
+ // Update STAT IN
+ increment_counter(&session[sess].cin, &session[sess].cin_wrap, lpppoe);
+ session[sess].cin_delta += lpppoe;
+ session[sess].pin++;
+ sess_local[sess].cin += lpppoe;
+ sess_local[sess].pin++;
+
+ session[s].last_data = time_now;
+ // Update STAT OUT
+ increment_counter(&session[s].cout, &session[s].cout_wrap, lpppoe); // byte count
+ session[s].cout_delta += lpppoe;
+ session[s].pout++;
+ sess_local[s].cout += lpppoe;
+ sess_local[s].pout++;
+ }
+ else
+ session[sess].last_packet = time_now;
+
+ tunnelsend(p, lpppoe, t); // send it....
+}
+#endif
+
+void process_pppoe_sess(uint8_t *pack, int size)
+{
+ //struct ethhdr *ethhdr = (struct ethhdr *)pack;
+ struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
+ uint16_t lppp = ntohs(hdr->length);
+ uint8_t *pppdata = (uint8_t *) hdr->tag;
+ uint16_t proto, sid, t;
+
+ sid = ntohs(hdr->sid);
+ t = TUNNEL_ID_PPPOE;
+
+ LOG_HEX(5, "RCV PPPOE Sess", pack, size);
+
+ if (sid >= MAXSESSION)
+ {
+ LOG(0, sid, t, "Received pppoe packet with invalid session ID\n");
+ STAT(tunnel_rx_errors);
+ return;
+ }
+
+ if (session[sid].tunnel != t)
+ {
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+
+ LOG(1, sid, t, "ERROR process_pppoe_sess: Session is not a session pppoe\n");
+ return;
+ }
+
+ if (hdr->ver != 1)
+ {
+ LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported version %i)\n", hdr->ver);
+ return;
+ }
+
+ if (hdr->type != 1)
+ {
+ LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported type %i)\n", hdr->type);
+ return;
+ }
+
+ if (lppp > 2 && pppdata[0] == 0xFF && pppdata[1] == 0x03)
+ { // HDLC address header, discard
+ LOG(5, sid, t, "pppoe_sess: HDLC address header, discard\n");
+ pppdata += 2;
+ lppp -= 2;
+ }
+ if (lppp < 2)
+ {
+ LOG(3, sid, t, "Error process_pppoe_sess: Short ppp length %d\n", lppp);
+ return;
+ }
+ if (*pppdata & 1)
+ {
+ proto = *pppdata++;
+ lppp--;
+ }
+ else
+ {
+ proto = ntohs(*(uint16_t *) pppdata);
+ pppdata += 2;
+ lppp -= 2;
+ }
+
+#ifdef LAC
+ if (session[sid].forwardtosession)
+ { // Must be forwaded to a remote lns tunnel l2tp
+ pppoe_forwardto_session_rmlns(pack, size, sid, proto);
+ return;
+ }
+#endif
+
+ if (proto == PPPPAP)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processpap(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPCHAP)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processchap(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPLCP)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processlcp(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPIPCP)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processipcp(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPIPV6CP && config->ipv6_prefix.s6_addr[0])
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processipv6cp(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPCCP)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processccp(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPIP)
+ {
+ session[sid].last_packet = session[sid].last_data = time_now;
+ if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processipin(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPMP)
+ {
+ session[sid].last_packet = session[sid].last_data = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processmpin(sid, t, pppdata, lppp);
+ }
+ else if (proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0])
+ {
+ session[sid].last_packet = session[sid].last_data = time_now;
+ if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ processipv6in(sid, t, pppdata, lppp);
+ }
+ else if (session[sid].ppp.lcp == Opened)
+ {
+ session[sid].last_packet = time_now;
+ if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
+ protoreject(sid, t, pppdata, lppp, proto);
+ }
+ else
+ {
+ LOG(3, sid, t, "process_pppoe_sess: Unknown PPP protocol 0x%04X received in LCP %s state\n",
+ proto, ppp_state(session[sid].ppp.lcp));
+ }
+}
+
+void pppoe_send_garp()
+{
+ int s;
+ struct ifreq ifr;
+ uint8_t mac[6];
+
+ if (!*config->pppoe_if_name)
+ return;
+
+ s = socket(PF_INET, SOCK_DGRAM, 0);
+ if (s < 0)
+ {
+ LOG(0, 0, 0, "Error creating socket for GARP: %s\n", strerror(errno));
+ return;
+ }
+ memset(&ifr, 0, sizeof(ifr));
+ strncpy(ifr.ifr_name, config->pppoe_if_name, sizeof(ifr.ifr_name) - 1);
+ if (ioctl(s, SIOCGIFHWADDR, &ifr) < 0)
+ {
+ LOG(0, 0, 0, "Error getting eth0 hardware address for GARP: %s\n", strerror(errno));
+ close(s);
+ return;
+ }
+ memcpy(mac, &ifr.ifr_hwaddr.sa_data, 6*sizeof(char));
+ if (ioctl(s, SIOCGIFINDEX, &ifr) < 0)
+ {
+ LOG(0, 0, 0, "Error getting eth0 interface index for GARP: %s\n", strerror(errno));
+ close(s);
+ return;
+ }
+ close(s);
+
+ sendarp(ifr.ifr_ifindex, mac, config->iftun_address);
+}
+
+// rcv pppoe data from slave
+void pppoe_process_forward(uint8_t *pack, int size, in_addr_t addr)
+{
+ struct ethhdr *ethhdr = (struct ethhdr *)pack;
+
+ if (ethhdr->h_proto == htons(ETH_P_PPP_DISC))
+ process_pppoe_disc(pack, size);
+ else if (ethhdr->h_proto == htons(ETH_P_PPP_SES))
+ process_pppoe_sess(pack, size);
+ else
+ LOG(0, 0, 0, "pppoe_process_forward: I got a C_PPPOE_FORWARD from %s, but not a PPPOE data?\n", fmtaddr(addr, 0));
+}
projects / l2tpns.git / commitdiff