Add L2 PPP and PPPoE

src/l2_ppp.c

@@ -12,22 +12,35 @@
 #else
 #pragma pack(push, 1)
 #endif
+// all in network order
 
-struct ppp {  // RFC-1662
-  uint8_t addr, ctrl;
+struct ppp {  // RFC-1661
   uint16_t proto;
 };
 
 struct lcp {  // RFC-1661
-  uint8_t code, id, len[2];
+  uint8_t code, id;
+  uint16_t len;
 };
 
-struct ipcp { // RFC-1332
-  uint8_t code;
+struct ipcp {  // RFC-1332
+  uint8_t code, id;
+  uint16_t len;
 };
 
-struct ipv6cp { // RFC-5072
-  uint8_t code;
+struct ipv6cp {  // RFC-5072
+  uint8_t code, id;
+  uint16_t len;
+};
+
+struct hdlc_ {  // RFC-1662, "PPP in HDLC-like Framing"
+  uint8_t addr, ctrl;
+};
+
+struct pppoe {  // RFC-2516, "A Method for Transmitting PPP Over Ethernet
+                // (PPPoE)"
+  uint8_t vertype, code;
+  uint16_t id, len;
 };
 
 #if defined(__DCC__)
@@ -36,52 +49,361 @@ struct ipv6cp { // RFC-5072
 #pragma pack(pop)
 #endif
 
+#define MG_PPP_ADDR 0xff
+#define MG_PPP_CTRL 0x03
 
-void mg_l2_ppp_init(struct mg_l2addr *addr, uint16_t *mtu,
-                    uint16_t *framesize) {
-  (void) addr;
-  *mtu = 1500;        // 1492 for PPPoE
-  *framesize = 1540;  // *** TODO(scaprile): actual value, check for PPPoE too
+#define MG_PPP_PROTO_IP 0x0021
+#define MG_PPP_PROTO_IPV6 0x0057
+#define MG_PPP_PROTO_LCP 0xc021
+#define MG_PPP_PROTO_IPCP 0x8021
+#define MG_PPP_PROTO_IPV6CP 0x8057
+#define MG_PPP_PROTO_PAP 0xc023
+#define MG_PPP_PROTO_CHAP 0xc223
+
+#define MG_PPP_IPCP_REQ 1
+#define MG_PPP_IPCP_ACK 2
+#define MG_PPP_IPCP_NACK 3
+#define MG_PPP_IPCP_REJECT 4
+#define MG_PPP_IPCP_IPADDR 3
+
+#define MG_PPP_LCP_CFG_REQ 1
+#define MG_PPP_LCP_CFG_ACK 2
+#define MG_PPP_LCP_CFG_NACK 3
+#define MG_PPP_LCP_CFG_REJECT 4
+#define MG_PPP_LCP_CFG_TERM_REQ 5
+#define MG_PPP_LCP_CFG_TERM_ACK 6
+#define MG_PPP_LCP_REJECT 8
+#define MG_PPP_LCP_ECHO_REQ 9
+#define MG_PPP_LCP_ECHO_REPLY 10
+
+#define MG_PPPoE_PADI 0x09
+#define MG_PPPoE_PADO 0x07
+#define MG_PPPoE_PADR 0x19
+#define MG_PPPoE_PADS 0x65
+#define MG_PPPoE_PADT 0xa7
+
+#define MG_PPPoE_ST_DISC 0  // Discovery phase, see what servers are out there
+#define MG_PPPoE_ST_REQ 1   // Chose a server, request a session and wait
+#define MG_PPPoE_ST_SESS 2  // Session established, PPP traffic is exchanged
+
+#define PDIFF(a, b) ((size_t) (((char *) (b)) - ((char *) (a))))
+
+static bool s_link = false;  // *******************************************
+static uint8_t s_state = MG_PPPoE_ST_DISC;
+static uint16_t s_id;
+
+void mg_l2_ppp_init(struct mg_tcpip_if *ifp) {
+  ifp->mtu = 1500;
+  ifp->framesize = 1500 + sizeof(struct ppp) + sizeof(struct hdlc_);
+}
+
+extern void mg_l2_eth_init(struct mg_tcpip_if *);
+
+void mg_l2_pppoe_init(struct mg_tcpip_if *ifp) {
+  mg_l2_eth_init(ifp);
+  ifp->mtu = ifp->mtu - (uint16_t) (sizeof(struct pppoe) +
+                                    sizeof(struct ppp));  // 1500 --> 1492
+}
+
+bool mg_l2_ppp_poll(struct mg_tcpip_if *ifp, bool expired_1000ms) {
+  if (expired_1000ms && ifp->state == MG_TCPIP_STATE_DOWN) s_link = false;
+  return s_link;
+}
+
+static uint8_t *hdlc_header(uint8_t *p) {
+  struct hdlc_ *hdlc = (struct hdlc_ *) p;
+  hdlc->addr = MG_PPP_ADDR;
+  hdlc->ctrl = MG_PPP_CTRL;
+  return (uint8_t *) (hdlc + 1);
+}
+
+static uint8_t *ppp_header(uint16_t proto, uint8_t *p) {
+  struct ppp *ppp = (struct ppp *) p;
+  ppp->proto = mg_htons(proto);
+  return (uint8_t *) (ppp + 1);
+}
+
+static uint8_t *l2_ppp_header(enum mg_l2proto proto, uint8_t *p) {
+  uint16_t ppp_proto = proto == MG_TCPIP_L2PROTO_IPV4   ? MG_PPP_PROTO_IP
+                       : proto == MG_TCPIP_L2PROTO_IPV4 ? MG_PPP_PROTO_IPV6
+                                                        : 0;
+  return ppp_header(ppp_proto, p);
 }
 
 uint8_t *mg_l2_ppp_header(enum mg_l2proto proto, struct mg_l2addr *src,
                           struct mg_l2addr *dst, uint8_t *frame) {
   (void) src;
   (void) dst;
-  (void) proto;
-  return frame;
+  return l2_ppp_header(proto, hdlc_header(frame));
 }
 
-size_t mg_l2_ppp_footer(size_t len, uint8_t *frame) {
-  (void) frame;
-  return len;
+extern uint8_t *mg_l2_eth_header(enum mg_l2proto proto, struct mg_l2addr *src,
+                                 struct mg_l2addr *dst, uint8_t *frame);
+
+static uint8_t *pppoe_header(enum mg_l2proto proto, uint8_t code, uint16_t id,
+                             struct mg_l2addr *src, struct mg_l2addr *dst,
+                             uint8_t *frame) {
+  struct pppoe *pppoe =
+      (struct pppoe *) mg_l2_eth_header(proto, src, dst, frame);
+  pppoe->vertype = 0x11;
+  pppoe->code = code;
+  pppoe->id = id;
+  return (uint8_t *) (pppoe + 1);
+}
+
+uint8_t *mg_l2_pppoe_header(enum mg_l2proto proto, struct mg_l2addr *src,
+                            struct mg_l2addr *dst, uint8_t *frame) {
+  (void) dst;
+  return l2_ppp_header(proto, pppoe_header(MG_TCPIP_L2PROTO_PPPoE_SESS, 0, s_id,
+                                           src, dst, frame));
+}
+
+// Calculate FCS/CRC for PPP frames. Could be implemented faster using lookup
+// tables.
+static uint16_t fcs_do(uint8_t *frame, size_t len) {
+  uint32_t fcs = 0xffff;
+  unsigned int j;
+  for (j = 0; j < len; j++) {
+    unsigned int i;
+    uint8_t x = frame[j];
+    for (i = 0; i < 8; i++) {
+      fcs = ((fcs ^ x) & 1) ? (fcs >> 1) ^ 0x8408 : fcs >> 1;
+      x >>= 1;
+    }
+  }
+  return (uint16_t) (fcs & 0xffff);
+}
+
+size_t mg_l2_ppp_footer(size_t len, uint8_t *cur) {
+  uint16_t crc;
+  uint8_t *frame;
+  len += sizeof(struct ppp) + sizeof(struct hdlc_);
+  frame = cur - len;
+  crc = fcs_do(frame, len);
+  *cur++ = (uint8_t) ~crc;  // add CRC, note the byte order
+  *cur++ = (uint8_t) (~crc >> 8);
+  // there is no len field in PPP
+  return len + 2;
+}
+
+extern size_t mg_l2_eth_footer(size_t len, uint8_t *cur);
+
+static size_t pppoe_footer(size_t len, uint8_t *cur) {
+  struct pppoe *pppoe = (struct pppoe *) (cur - len - sizeof(struct pppoe));
+  pppoe->len = mg_htons((uint16_t) len);
+  return mg_l2_eth_footer(PDIFF(pppoe, cur), cur);
+}
+
+size_t mg_l2_pppoe_footer(size_t len, uint8_t *cur) {
+  return pppoe_footer(len + sizeof(struct ppp), cur);
+}
+
+size_t mg_l2_driver_output(struct mg_tcpip_if *ifp, size_t len);
+
+static uint8_t *ppp_tx_frame_header(struct mg_tcpip_if *ifp, uint16_t proto) {
+  uint8_t *l2p = (uint8_t *) ifp->tx.buf;
+  if (ifp->l2type == MG_TCPIP_L2_PPP)
+    return ppp_header(proto, hdlc_header(l2p));
+  return ppp_header(proto, pppoe_header(MG_TCPIP_L2PROTO_PPPoE_SESS, 0, s_id,
+                                        (struct mg_l2addr *) ifp->mac,
+                                        (struct mg_l2addr *) ifp->gwmac, l2p));
+}
+static size_t ppp_tx_frame_footer(struct mg_tcpip_if *ifp, size_t len,
+                                  uint8_t *cur) {
+  return mg_l2_driver_output(ifp, ifp->l2type == MG_TCPIP_L2_PPPoE
+                                      ? mg_l2_pppoe_footer(len, cur)
+                                      : mg_l2_ppp_footer(len, cur));
+}
+
+// Transmit a single PPP frame for the given protocol
+static size_t ppp_tx_frame(struct mg_tcpip_if *ifp, uint16_t proto,
+                           uint8_t *data, size_t datasz) {
+  uint8_t *pay = ppp_tx_frame_header(ifp, proto);
+  memcpy(pay, data, datasz);
+  return ppp_tx_frame_footer(ifp, datasz, pay + datasz);
+}
+
+static void ppp_handle_lcp(struct mg_tcpip_if *ifp, uint8_t *lcpp,
+                           size_t lcpsz) {
+  uint8_t id;
+  uint16_t len;
+  struct lcp *lcp = (struct lcp *) lcpp;
+  if (lcpsz < sizeof(*lcp)) return;
+  id = lcp->id;
+  len = mg_ntohs(lcp->len);
+  switch (lcp->code) {
+    case MG_PPP_LCP_CFG_REQ: {
+      if (len == sizeof(*lcp)) {
+        MG_DEBUG(("LCP config request of %d bytes, acknowledging...", len));
+        lcp->code = MG_PPP_LCP_CFG_ACK;
+        ppp_tx_frame(ifp, MG_PPP_PROTO_LCP, lcpp, len);
+        lcp->code = MG_PPP_LCP_CFG_REQ;
+        ppp_tx_frame(ifp, MG_PPP_PROTO_LCP, lcpp, len);
+      } else {
+        MG_DEBUG(("LCP config request of %d bytes, rejecting...", len));
+        lcp->code = MG_PPP_LCP_CFG_REJECT;
+        ppp_tx_frame(ifp, MG_PPP_PROTO_LCP, lcpp, len);
+      }
+    } break;
+    case MG_PPP_LCP_CFG_ACK:
+      s_link = true;
+      break;
+    case MG_PPP_LCP_CFG_TERM_REQ: {
+      uint8_t ack[4] = {MG_PPP_LCP_CFG_TERM_ACK, id, 0, 4};
+      MG_DEBUG(("LCP termination request, acknowledging..."));
+      ppp_tx_frame(ifp, MG_PPP_PROTO_LCP, ack, sizeof(ack));
+      s_link = false;
+    } break;
+    case MG_PPP_LCP_ECHO_REQ:  // RFC-1661 5.8: must respond
+      MG_DEBUG(("LCP echo request of %d bytes, replying...", len));
+      lcp->code = MG_PPP_LCP_ECHO_REPLY;
+      ppp_tx_frame(ifp, MG_PPP_PROTO_LCP, lcpp, len);
+      break;
+  }
+}
+
+static bool find_opt(const uint8_t opt, const uint8_t optlen,
+                     const uint8_t *opts, size_t optslen, uint8_t *dest) {
+  uint8_t *p = (uint8_t *) opts;
+  while (optslen >= 2) {               // parse options for requested one
+    if (p[1] > optslen) return false;  // truncated / malformed
+    if (p[0] == opt && p[1] == optlen) {
+      memcpy(dest, p + 2, optlen - 2);
+      return true;
+    }
+    optslen -= p[1];
+    p += p[1];
+  }
+  return false;
+}
+
+static void ppp_handle_ipcp(struct mg_tcpip_if *ifp, uint8_t *ipcpp,
+                            size_t ipcpsz) {
+  uint16_t len;
+  uint8_t id;
+  struct ipcp *ipcp = (struct ipcp *) ipcpp;
+  uint8_t req[] = {
+      MG_PPP_IPCP_REQ, 0, 0, 10, MG_PPP_IPCP_IPADDR, 6, 0, 0, 0, 0};
+  if (ipcpsz < sizeof(*ipcp)) return;
+  id = ipcp->id;
+  len = mg_ntohs(ipcp->len);
+  switch (ipcp->code) {
+    case MG_PPP_IPCP_REQ:
+      MG_VERBOSE(("got IPCP config request, acknowledging..."));
+      if (len >= 10 &&
+          find_opt(MG_PPP_IPCP_IPADDR, 6, (const uint8_t *) (ipcp + 1),
+                   len - sizeof(*ipcp), (uint8_t *) &ifp->gw))
+        MG_DEBUG(("IPCP ack, GW IP: %M", mg_print_ip4, &ifp->gw));
+      ipcp->code = MG_PPP_IPCP_ACK;
+      ppp_tx_frame(ifp, MG_PPP_PROTO_IPCP, ipcpp, len);
+      req[1] = id;
+      memcpy(req + 6, &ifp->ip, 4);  // Request config IP address or 0.0.0.0
+      ppp_tx_frame(ifp, MG_PPP_PROTO_IPCP, req, sizeof(req));
+      break;
+    case MG_PPP_IPCP_ACK:
+      // Our peer accepted our static IP address (unlikely if 0.0.0.0)
+      MG_VERBOSE(("got IPCP config ack"));
+      ifp->mask = 0xffffffff;  // send to gw
+      ifp->state = MG_TCPIP_STATE_IP;
+      ifp->gw_ready = true;
+      break;
+    case MG_PPP_IPCP_NACK:
+      MG_VERBOSE(("got IPCP config nack"));
+      // NACK contains our "suggested" IP address, use it
+      if (len >= 10 &&
+          find_opt(MG_PPP_IPCP_IPADDR, 6, (const uint8_t *) (ipcp + 1),
+                   len - sizeof(*ipcp), (uint8_t *) &ifp->ip)) {
+        ifp->mask = 0xffffffff;  // send to gw
+        ifp->state = MG_TCPIP_STATE_IP;
+        ifp->gw_ready = true;
+        MG_DEBUG(("IPCP ack, IP: %M", mg_print_ip4, &ifp->ip));
+        ipcp->code = MG_PPP_IPCP_REQ;
+        ppp_tx_frame(ifp, MG_PPP_PROTO_IPCP, ipcpp, len);
+      }
+      break;
+    case MG_PPP_IPCP_REJECT:
+      MG_ERROR(("Peer rejected our IP address, need to properly set ifp->ip"));
+      break;
+  }
+}
+
+#if MG_ENABLE_IPV6
+#endif
+
+static bool ppp_rx(struct mg_tcpip_if *ifp, enum mg_l2proto *proto,
+                   struct mg_str *pay, struct mg_str *raw) {
+  struct ppp *ppp = (struct ppp *) pay->buf;
+  if (pay->len < sizeof(*ppp)) return false;  // Truncated
+  pay->buf = (char *) (ppp + 1);
+  pay->len = pay->len - sizeof(*ppp);
+  switch (mg_ntohs(ppp->proto)) {
+    case MG_PPP_PROTO_LCP:
+      ppp_handle_lcp(ifp, (uint8_t *) pay->buf, pay->len);
+      return false;
+    case MG_PPP_PROTO_IPCP:
+      if (s_link) ppp_handle_ipcp(ifp, (uint8_t *) pay->buf, pay->len);
+      return false;
+    case MG_PPP_PROTO_IP:
+      if (!s_link) return false;
+      MG_VERBOSE(("got IP packet of %d bytes", pay->len));
+      *proto = MG_TCPIP_L2PROTO_IPV4;
+      break;
+#if 0 && MG_ENABLE_IPV6
+    case MG_PPP_PROTO_IPV6CP:
+      if (s_link) ppp_handle_ipv6cp(ifp, (uint8_t *) pay->buf, pay->len);
+      return false;
+    case MG_PPP_PROTO_IPV6:
+      if (!s_link) return false;
+      MG_VERBOSE(("got IPv6 packet of %d bytes", pay->len));
+      *proto = MG_TCPIP_L2PROTO_IPV6;
+      break;
+#endif
+    default: {
+      struct lcp rej;
+      uint8_t *p;
+      size_t msglen;
+      MG_DEBUG(("unknown %u-byte PPP frame with proto 0x%04x:",
+                pay->len + sizeof(*ppp), mg_ntohs(ppp->proto)));
+      if (mg_log_level >= MG_LL_DEBUG) mg_hexdump(ppp, sizeof(*ppp) + 20);
+      if (!s_link) return false;  // RFC-1661 5.7: must reject on link up
+      if (pay->len > (size_t) (ifp->mtu - 20))
+        pay->len = (size_t) (ifp->mtu - 20);  // truncate to some safe limit
+      rej.code = MG_PPP_LCP_REJECT;
+      mg_random(&rej.id, sizeof(rej.id));
+      msglen = pay->len + sizeof(*ppp);
+      rej.len = mg_htons((uint16_t) (msglen + sizeof(rej)));
+      p = ppp_tx_frame_header(ifp, MG_PPP_PROTO_LCP);
+      memmove(p, &rej, sizeof(rej));          // LCP reject
+      memmove(p + sizeof(rej), ppp, msglen);  // rejected PPP message
+      ppp_tx_frame_footer(ifp, msglen + sizeof(rej), p + msglen + sizeof(rej));
+    }
+      return false;
+  }
+  (void) raw;
+  return true;
 }
 
 bool mg_l2_ppp_rx(struct mg_tcpip_if *ifp, enum mg_l2proto *proto,
                   struct mg_str *pay, struct mg_str *raw) {
-#if 0
-if (ppp->addr == MG_PPP_ADDR && ppp->ctrl == MG_PPP_CTRL) {
-  code = ntohs(ppp->proto);
-  payload = (uint8_t *) (ppp + 1);
-} else { // Address-and-Control-Field-Compressed PPP header
-  uint16_t *cppp = (uint16_t *) ppp;
-  code = ntohs(*cppp);
-  payload = (uint8_t *) (cppp + 1);
-}
-#endif
-  *pay = *raw;
-  *proto = MG_TCPIP_L2PROTO_IPV4;
-  (void) ifp;
-  return true;
+  struct hdlc_ *hdlc = (struct hdlc_ *) raw->buf;
+  if (raw->len < sizeof(*hdlc) + 2) return false;  // Truncated
+  if (hdlc->addr == MG_PPP_ADDR && hdlc->ctrl == MG_PPP_CTRL) {
+    pay->buf = (char *) (hdlc + 1);
+    pay->len = raw->len - sizeof(*hdlc) - 2;
+  } else {  // Address-and-Control-Field-Compressed PPP header
+    pay->buf = (char *) raw->buf;
+    pay->len = raw->len - 2;
+  }
+  return ppp_rx(ifp, proto, pay, raw);
 }
 
+extern struct mg_l2addr s_mapip;
+
 struct mg_l2addr *mg_l2_ppp_getaddr(uint8_t *frame) {
   (void) frame;
   return &s_mapip;  // bogus
 }
 
-extern struct mg_l2addr s_mapip;
-
 struct mg_l2addr *mg_l2_ppp_mapip(enum mg_l2addrtype addrtype,
                                   struct mg_addr *addr) {
   (void) addrtype;
@@ -112,4 +434,98 @@ uint8_t mg_l2_ppp_ip6put(struct mg_l2addr *addr, uint8_t *opts) {
 }
 #endif
 
+// Transmit a single PPPoE frame for the discovery phase
+static size_t pppoe_tx_frame(struct mg_tcpip_if *ifp, uint8_t code, uint16_t id,
+                             uint8_t *data, size_t datasz,
+                             struct mg_l2addr *dst) {
+  uint8_t *l2p = (uint8_t *) ifp->tx.buf;
+  uint8_t *p = pppoe_header(MG_TCPIP_L2PROTO_PPPoE_DISC, code, id,
+                            (struct mg_l2addr *) ifp->mac, dst, l2p);
+  memmove(p, data, datasz);
+  return mg_l2_driver_output(ifp, pppoe_footer(datasz, p + datasz));
+}
+
+extern struct mg_l2addr *mg_l2_eth_mapip(enum mg_l2addrtype, struct mg_addr *);
+
+bool mg_l2_pppoe_poll(struct mg_tcpip_if *ifp, bool expired_1000ms) {
+  if (expired_1000ms && s_state == MG_PPPoE_ST_DISC &&
+      ifp->state == MG_TCPIP_STATE_LINK_UP) {
+    uint16_t tags[2];
+    tags[0] = mg_htons(0x0101);  // Service Request
+    tags[1] = mg_htons(0x0000);  // Any
+    pppoe_tx_frame(ifp, MG_PPPoE_PADI, 0, (uint8_t *) tags, sizeof(tags),
+                   mg_l2_eth_mapip(MG_TCPIP_L2ADDR_BCAST, NULL));
+    MG_DEBUG(("Sent PADI"));
+  } else if (expired_1000ms && (s_state != MG_PPPoE_ST_SESS ||
+                                ifp->state == MG_TCPIP_STATE_DOWN)) {
+    s_state = MG_PPPoE_ST_DISC;
+  }
+  return mg_l2_ppp_poll(ifp, expired_1000ms);
+}
+
+extern bool mg_l2_eth_rx(struct mg_tcpip_if *ifp, enum mg_l2proto *proto,
+                         struct mg_str *pay, struct mg_str *raw);
+extern struct mg_l2addr *mg_l2_eth_getaddr(uint8_t *);
+
+bool mg_l2_pppoe_rx(struct mg_tcpip_if *ifp, enum mg_l2proto *proto,
+                    struct mg_str *pay, struct mg_str *raw) {
+  enum mg_l2proto eth_proto;  // raw is handled by eth
+  struct pppoe *pppoe;
+  if (!mg_l2_eth_rx(ifp, &eth_proto, pay, raw)) return false;
+  pppoe = (struct pppoe *) pay->buf;            // here we handle pay, not raw
+  if (pay->len < sizeof(*pppoe)) return false;  // Truncated
+  if (eth_proto == MG_TCPIP_L2PROTO_PPPoE_DISC) {
+    MG_VERBOSE(("PPPoE_DISC"));
+    if (s_state == MG_PPPoE_ST_DISC && pppoe->code == MG_PPPoE_PADO &&
+        pppoe->id == 0) {
+      uint16_t tags[2];
+      bool has_cookie = false;
+      size_t len = pay->len - sizeof(*pppoe);
+      uint8_t *p = (uint8_t *) (pppoe + 1);
+      uint16_t taglen;
+      while (len >= 4) {  // parse tags for a possible AC-Cookie
+        uint16_t curtag = *((uint16_t *) p);
+        taglen = mg_ntohs(*(((uint16_t *) p) + 1));
+        if (taglen > len - 4) return false;  // truncated / malformed
+        if (curtag == mg_htons(0x0104)) {
+          has_cookie = true;
+          break;
+        }
+        len -= 4 + taglen;
+        p += 4 + taglen;
+      }
+      tags[0] = mg_htons(0x0101);  // Service Request
+      tags[1] = mg_htons(0x0000);  // Any
+      if (has_cookie) {  // copy tags to before AC-Cookie tag in rx buffer
+        memcpy(p -= sizeof(tags), tags, sizeof(tags));  // point to all tags
+        taglen += 4 + sizeof(tags);                     // account for tags
+      } else {
+        p = (uint8_t *) tags;
+        taglen = sizeof(tags);
+      }
+      pppoe_tx_frame(ifp, MG_PPPoE_PADR, 0, p, taglen,
+                     mg_l2_eth_getaddr((uint8_t *) raw->buf));
+      MG_DEBUG(("Sent PADR"));
+      s_state = MG_PPPoE_ST_REQ;
+    } else if (s_state == MG_PPPoE_ST_REQ && pppoe->code == MG_PPPoE_PADS) {
+      s_id = pppoe->id;
+      memcpy(&ifp->gwmac, mg_l2_eth_getaddr((uint8_t *) raw->buf)->addr.mac, 6);
+      MG_DEBUG(("PPPoE session 0x%04x started", mg_ntohs(s_id)));
+      s_state = MG_PPPoE_ST_SESS;
+    } else if (s_state == MG_PPPoE_ST_SESS && pppoe->code == MG_PPPoE_PADT &&
+               pppoe->id == s_id) {
+      MG_ERROR(("Got PADT"));
+      s_id = 0;
+      s_link = false;
+      s_state = MG_PPPoE_ST_DISC;
+    }
+  } else if (eth_proto == MG_TCPIP_L2PROTO_PPPoE_SESS &&
+             s_state == MG_PPPoE_ST_SESS) {
+    pay->buf = (char *) (pppoe + 1);
+    pay->len = pay->len - sizeof(*pppoe);
+    return ppp_rx(ifp, proto, pay, raw);
+  }
+  return false;
+}
+
 #endif