lwip_pppol2tp.c

00001 /**
00002  * @file
00003  * Network Point to Point Protocol over Layer 2 Tunneling Protocol program file.
00004  *
00005  */
00006 
00007 /*
00008  * Redistribution and use in source and binary forms, with or without modification,
00009  * are permitted provided that the following conditions are met:
00010  *
00011  * 1. Redistributions of source code must retain the above copyright notice,
00012  *    this list of conditions and the following disclaimer.
00013  * 2. Redistributions in binary form must reproduce the above copyright notice,
00014  *    this list of conditions and the following disclaimer in the documentation
00015  *    and/or other materials provided with the distribution.
00016  * 3. The name of the author may not be used to endorse or promote products
00017  *    derived from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
00020  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
00022  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00023  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00024  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
00027  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
00028  * OF SUCH DAMAGE.
00029  *
00030  * This file is part of the lwIP TCP/IP stack.
00031  *
00032  */
00033 
00034 /*
00035  * L2TP Support status:
00036  *
00037  * Supported:
00038  * - L2TPv2 (PPP over L2TP, a.k.a. UDP tunnels)
00039  * - LAC
00040  *
00041  * Not supported:
00042  * - LNS (require PPP server support)
00043  * - L2TPv3 ethernet pseudowires
00044  * - L2TPv3 VLAN pseudowire
00045  * - L2TPv3 PPP pseudowires
00046  * - L2TPv3 IP encapsulation
00047  * - L2TPv3 IP pseudowire
00048  * - L2TP tunnel switching - http://tools.ietf.org/html/draft-ietf-l2tpext-tunnel-switching-08
00049  * - Multiple tunnels per UDP socket, as well as multiple sessions per tunnel
00050  * - Hidden AVPs
00051  */
00052 
00053 #include "netif/ppp/ppp_opts.h"
00054 #if PPP_SUPPORT && PPPOL2TP_SUPPORT /* don't build if not configured for use in lwipopts.h */
00055 
00056 #include "lwip/err.h"
00057 #include "lwip/memp.h"
00058 #include "lwip/netif.h"
00059 #include "lwip/udp.h"
00060 #include "lwip/snmp.h"
00061 
00062 #include "netif/ppp/ppp_impl.h"
00063 #include "netif/ppp/lcp.h"
00064 #include "netif/ppp/ipcp.h"
00065 #include "netif/ppp/pppol2tp.h"
00066 #include "netif/ppp/pppcrypt.h"
00067 #include "netif/ppp/magic.h"
00068 
00069 /* Memory pool */
00070 LWIP_MEMPOOL_DECLARE(PPPOL2TP_PCB, MEMP_NUM_PPPOL2TP_INTERFACES, sizeof(pppol2tp_pcb), "PPPOL2TP_PCB")
00071 
00072 /* callbacks called from PPP core */
00073 static err_t pppol2tp_write(ppp_pcb *ppp, void *ctx, struct pbuf *p);
00074 static err_t pppol2tp_netif_output(ppp_pcb *ppp, void *ctx, struct pbuf *p, u_short protocol);
00075 static err_t pppol2tp_destroy(ppp_pcb *ppp, void *ctx);    /* Destroy a L2TP control block */
00076 static err_t pppol2tp_connect(ppp_pcb *ppp, void *ctx);    /* Be a LAC, connect to a LNS. */
00077 static void pppol2tp_disconnect(ppp_pcb *ppp, void *ctx);  /* Disconnect */
00078 
00079  /* Prototypes for procedures local to this file. */
00080 static void pppol2tp_input(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port);
00081 static void pppol2tp_dispatch_control_packet(pppol2tp_pcb *l2tp, u16_t port, struct pbuf *p, u16_t ns, u16_t nr);
00082 static void pppol2tp_timeout(void *arg);
00083 static void pppol2tp_abort_connect(pppol2tp_pcb *l2tp);
00084 static err_t pppol2tp_send_sccrq(pppol2tp_pcb *l2tp);
00085 static err_t pppol2tp_send_scccn(pppol2tp_pcb *l2tp, u16_t ns);
00086 static err_t pppol2tp_send_icrq(pppol2tp_pcb *l2tp, u16_t ns);
00087 static err_t pppol2tp_send_iccn(pppol2tp_pcb *l2tp, u16_t ns);
00088 static err_t pppol2tp_send_zlb(pppol2tp_pcb *l2tp, u16_t ns);
00089 static err_t pppol2tp_send_stopccn(pppol2tp_pcb *l2tp, u16_t ns);
00090 static err_t pppol2tp_xmit(pppol2tp_pcb *l2tp, struct pbuf *pb);
00091 static err_t pppol2tp_udp_send(pppol2tp_pcb *l2tp, struct pbuf *pb);
00092 
00093 /* Callbacks structure for PPP core */
00094 static const struct link_callbacks pppol2tp_callbacks = {
00095   pppol2tp_connect,
00096 #if PPP_SERVER
00097   NULL,
00098 #endif /* PPP_SERVER */
00099   pppol2tp_disconnect,
00100   pppol2tp_destroy,
00101   pppol2tp_write,
00102   pppol2tp_netif_output,
00103   NULL,
00104   NULL
00105 };
00106 
00107 
00108 /* Create a new L2TP session. */
00109 ppp_pcb *pppol2tp_create(struct netif *pppif,
00110        struct netif *netif, const ip_addr_t *ipaddr, u16_t port,
00111        const u8_t *secret, u8_t secret_len,
00112        ppp_link_status_cb_fn link_status_cb, void *ctx_cb) {
00113   ppp_pcb *ppp;
00114   pppol2tp_pcb *l2tp;
00115   struct udp_pcb *udp;
00116 #if !PPPOL2TP_AUTH_SUPPORT
00117   LWIP_UNUSED_ARG(secret);
00118   LWIP_UNUSED_ARG(secret_len);
00119 #endif /* !PPPOL2TP_AUTH_SUPPORT */
00120 
00121   if (ipaddr == NULL) {
00122     goto ipaddr_check_failed;
00123   }
00124 
00125   l2tp = (pppol2tp_pcb *)LWIP_MEMPOOL_ALLOC(PPPOL2TP_PCB);
00126   if (l2tp == NULL) {
00127     goto memp_malloc_l2tp_failed;
00128   }
00129 
00130   udp = udp_new_ip_type(IP_GET_TYPE(ipaddr));
00131   if (udp == NULL) {
00132     goto udp_new_failed;
00133   }
00134   udp_recv(udp, pppol2tp_input, l2tp);
00135 
00136   ppp = ppp_new(pppif, &pppol2tp_callbacks, l2tp, link_status_cb, ctx_cb);
00137   if (ppp == NULL) {
00138     goto ppp_new_failed;
00139   }
00140 
00141   memset(l2tp, 0, sizeof(pppol2tp_pcb));
00142   l2tp->phase = PPPOL2TP_STATE_INITIAL;
00143   l2tp->ppp = ppp;
00144   l2tp->udp = udp;
00145   l2tp->netif = netif;
00146   ip_addr_copy(l2tp->remote_ip, *ipaddr);
00147   l2tp->remote_port = port;
00148 #if PPPOL2TP_AUTH_SUPPORT
00149   l2tp->secret = secret;
00150   l2tp->secret_len = secret_len;
00151 #endif /* PPPOL2TP_AUTH_SUPPORT */
00152 
00153   return ppp;
00154 
00155 ppp_new_failed:
00156   udp_remove(udp);
00157 udp_new_failed:
00158   LWIP_MEMPOOL_FREE(PPPOL2TP_PCB, l2tp);
00159 memp_malloc_l2tp_failed:
00160 ipaddr_check_failed:
00161   return NULL;
00162 }
00163 
00164 /* Called by PPP core */
00165 static err_t pppol2tp_write(ppp_pcb *ppp, void *ctx, struct pbuf *p) {
00166   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
00167   struct pbuf *ph; /* UDP + L2TP header */
00168   err_t ret;
00169 #if MIB2_STATS
00170   u16_t tot_len;
00171 #else /* MIB2_STATS */
00172   LWIP_UNUSED_ARG(ppp);
00173 #endif /* MIB2_STATS */
00174 
00175   ph = pbuf_alloc(PBUF_TRANSPORT, (u16_t)(PPPOL2TP_OUTPUT_DATA_HEADER_LEN), PBUF_RAM);
00176   if(!ph) {
00177     LINK_STATS_INC(link.memerr);
00178     LINK_STATS_INC(link.proterr);
00179     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
00180     pbuf_free(p);
00181     return ERR_MEM;
00182   }
00183 
00184   pbuf_header(ph, -(s16_t)PPPOL2TP_OUTPUT_DATA_HEADER_LEN); /* hide L2TP header */
00185   pbuf_cat(ph, p);
00186 #if MIB2_STATS
00187   tot_len = ph->tot_len;
00188 #endif /* MIB2_STATS */
00189 
00190   ret = pppol2tp_xmit(l2tp, ph);
00191   if (ret != ERR_OK) {
00192     LINK_STATS_INC(link.err);
00193     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
00194     return ret;
00195   }
00196 
00197   MIB2_STATS_NETIF_ADD(ppp->netif, ifoutoctets, (u16_t)tot_len);
00198   MIB2_STATS_NETIF_INC(ppp->netif, ifoutucastpkts);
00199   LINK_STATS_INC(link.xmit);
00200   return ERR_OK;
00201 }
00202 
00203 /* Called by PPP core */
00204 static err_t pppol2tp_netif_output(ppp_pcb *ppp, void *ctx, struct pbuf *p, u_short protocol) {
00205   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
00206   struct pbuf *pb;
00207   u8_t *pl;
00208   err_t err;
00209 #if MIB2_STATS
00210   u16_t tot_len;
00211 #else /* MIB2_STATS */
00212   LWIP_UNUSED_ARG(ppp);
00213 #endif /* MIB2_STATS */
00214 
00215   /* @todo: try to use pbuf_header() here! */
00216   pb = pbuf_alloc(PBUF_TRANSPORT, PPPOL2TP_OUTPUT_DATA_HEADER_LEN + sizeof(protocol), PBUF_RAM);
00217   if(!pb) {
00218     LINK_STATS_INC(link.memerr);
00219     LINK_STATS_INC(link.proterr);
00220     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
00221     return ERR_MEM;
00222   }
00223 
00224   pbuf_header(pb, -(s16_t)PPPOL2TP_OUTPUT_DATA_HEADER_LEN);
00225 
00226   pl = (u8_t*)pb->payload;
00227   PUTSHORT(protocol, pl);
00228 
00229   pbuf_chain(pb, p);
00230 #if MIB2_STATS
00231   tot_len = pb->tot_len;
00232 #endif /* MIB2_STATS */
00233 
00234   if( (err = pppol2tp_xmit(l2tp, pb)) != ERR_OK) {
00235     LINK_STATS_INC(link.err);
00236     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
00237     return err;
00238   }
00239 
00240   MIB2_STATS_NETIF_ADD(ppp->netif, ifoutoctets, tot_len);
00241   MIB2_STATS_NETIF_INC(ppp->netif, ifoutucastpkts);
00242   LINK_STATS_INC(link.xmit);
00243   return ERR_OK;
00244 }
00245 
00246 /* Destroy a L2TP control block */
00247 static err_t pppol2tp_destroy(ppp_pcb *ppp, void *ctx) {
00248   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
00249   LWIP_UNUSED_ARG(ppp);
00250 
00251   sys_untimeout(pppol2tp_timeout, l2tp);
00252   udp_remove(l2tp->udp);
00253   LWIP_MEMPOOL_FREE(PPPOL2TP_PCB, l2tp);
00254   return ERR_OK;
00255 }
00256 
00257 /* Be a LAC, connect to a LNS. */
00258 static err_t pppol2tp_connect(ppp_pcb *ppp, void *ctx) {
00259   err_t err;
00260   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
00261   lcp_options *lcp_wo;
00262   lcp_options *lcp_ao;
00263 #if PPP_IPV4_SUPPORT && VJ_SUPPORT
00264   ipcp_options *ipcp_wo;
00265   ipcp_options *ipcp_ao;
00266 #endif /* PPP_IPV4_SUPPORT && VJ_SUPPORT */
00267 
00268   l2tp->tunnel_port = l2tp->remote_port;
00269   l2tp->our_ns = 0;
00270   l2tp->peer_nr = 0;
00271   l2tp->peer_ns = 0;
00272   l2tp->source_tunnel_id = 0;
00273   l2tp->remote_tunnel_id = 0;
00274   l2tp->source_session_id = 0;
00275   l2tp->remote_session_id = 0;
00276   /* l2tp->*_retried are cleared when used */
00277 
00278   lcp_wo = &ppp->lcp_wantoptions;
00279   lcp_wo->mru = PPPOL2TP_DEFMRU;
00280   lcp_wo->neg_asyncmap = 0;
00281   lcp_wo->neg_pcompression = 0;
00282   lcp_wo->neg_accompression = 0;
00283   lcp_wo->passive = 0;
00284   lcp_wo->silent = 0;
00285 
00286   lcp_ao = &ppp->lcp_allowoptions;
00287   lcp_ao->mru = PPPOL2TP_DEFMRU;
00288   lcp_ao->neg_asyncmap = 0;
00289   lcp_ao->neg_pcompression = 0;
00290   lcp_ao->neg_accompression = 0;
00291 
00292 #if PPP_IPV4_SUPPORT && VJ_SUPPORT
00293   ipcp_wo = &ppp->ipcp_wantoptions;
00294   ipcp_wo->neg_vj = 0;
00295   ipcp_wo->old_vj = 0;
00296 
00297   ipcp_ao = &ppp->ipcp_allowoptions;
00298   ipcp_ao->neg_vj = 0;
00299   ipcp_ao->old_vj = 0;
00300 #endif /* PPP_IPV4_SUPPORT && VJ_SUPPORT */
00301 
00302   /* Listen to a random source port, we need to do that instead of using udp_connect()
00303    * because the L2TP LNS might answer with its own random source port (!= 1701)
00304    */
00305 #if LWIP_IPV6
00306   if (IP_IS_V6_VAL(l2tp->udp->local_ip)) {
00307     udp_bind(l2tp->udp, IP6_ADDR_ANY, 0);
00308   } else
00309 #endif /* LWIP_IPV6 */
00310   udp_bind(l2tp->udp, IP_ADDR_ANY, 0);
00311 
00312 #if PPPOL2TP_AUTH_SUPPORT
00313   /* Generate random vector */
00314   if (l2tp->secret != NULL) {
00315     magic_random_bytes(l2tp->secret_rv, sizeof(l2tp->secret_rv));
00316   }
00317 #endif /* PPPOL2TP_AUTH_SUPPORT */
00318 
00319   do {
00320     l2tp->remote_tunnel_id = magic();
00321   } while(l2tp->remote_tunnel_id == 0);
00322   /* save state, in case we fail to send SCCRQ */
00323   l2tp->sccrq_retried = 0;
00324   l2tp->phase = PPPOL2TP_STATE_SCCRQ_SENT;
00325   if ((err = pppol2tp_send_sccrq(l2tp)) != 0) {
00326     PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCRQ, error=%d\n", err));
00327   }
00328   sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00329   return err;
00330 }
00331 
00332 /* Disconnect */
00333 static void pppol2tp_disconnect(ppp_pcb *ppp, void *ctx) {
00334   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
00335 
00336   l2tp->our_ns++;
00337   pppol2tp_send_stopccn(l2tp, l2tp->our_ns);
00338 
00339   /* stop any timer, disconnect can be called while initiating is in progress */
00340   sys_untimeout(pppol2tp_timeout, l2tp);
00341   l2tp->phase = PPPOL2TP_STATE_INITIAL;
00342   ppp_link_end(ppp); /* notify upper layers */
00343 }
00344 
00345 /* UDP Callback for incoming IPv4 L2TP frames */
00346 static void pppol2tp_input(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) {
00347   pppol2tp_pcb *l2tp = (pppol2tp_pcb*)arg;
00348   u16_t hflags, hlen, len=0, tunnel_id=0, session_id=0, ns=0, nr=0, offset=0;
00349   u8_t *inp;
00350   LWIP_UNUSED_ARG(pcb);
00351 
00352   /* we can't unbound a UDP pcb, thus we can still receive UDP frames after the link is closed */
00353   if (l2tp->phase < PPPOL2TP_STATE_SCCRQ_SENT) {
00354     goto free_and_return;
00355   }
00356 
00357   if (!ip_addr_cmp(&l2tp->remote_ip, addr)) {
00358     goto free_and_return;
00359   }
00360 
00361   /* discard packet if port mismatch, but only if we received a SCCRP */
00362   if (l2tp->phase > PPPOL2TP_STATE_SCCRQ_SENT && l2tp->tunnel_port != port) {
00363     goto free_and_return;
00364   }
00365 
00366   /* printf("-----------\nL2TP INPUT, %d\n", p->len); */
00367 
00368   /* L2TP header */
00369   if (p->len < sizeof(hflags) + sizeof(tunnel_id) + sizeof(session_id) ) {
00370     goto packet_too_short;
00371   }
00372 
00373   inp = (u8_t*)p->payload;
00374   GETSHORT(hflags, inp);
00375 
00376   if (hflags & PPPOL2TP_HEADERFLAG_CONTROL) {
00377     /* check mandatory flags for a control packet */
00378     if ( (hflags & PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY) != PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY ) {
00379       PPPDEBUG(LOG_DEBUG, ("pppol2tp: mandatory header flags for control packet not set\n"));
00380       goto free_and_return;
00381     }
00382     /* check forbidden flags for a control packet */
00383     if (hflags & PPPOL2TP_HEADERFLAG_CONTROL_FORBIDDEN) {
00384       PPPDEBUG(LOG_DEBUG, ("pppol2tp: forbidden header flags for control packet found\n"));
00385       goto free_and_return;
00386     }
00387   } else {
00388     /* check mandatory flags for a data packet */
00389     if ( (hflags & PPPOL2TP_HEADERFLAG_DATA_MANDATORY) != PPPOL2TP_HEADERFLAG_DATA_MANDATORY) {
00390       PPPDEBUG(LOG_DEBUG, ("pppol2tp: mandatory header flags for data packet not set\n"));
00391       goto free_and_return;
00392     }
00393   }
00394 
00395   /* Expected header size  */
00396   hlen = sizeof(hflags) + sizeof(tunnel_id) + sizeof(session_id);
00397   if (hflags & PPPOL2TP_HEADERFLAG_LENGTH) {
00398     hlen += sizeof(len);
00399   }
00400   if (hflags & PPPOL2TP_HEADERFLAG_SEQUENCE) {
00401     hlen += sizeof(ns) + sizeof(nr);
00402   }
00403   if (hflags & PPPOL2TP_HEADERFLAG_OFFSET) {
00404     hlen += sizeof(offset);
00405   }
00406   if (p->len < hlen) {
00407     goto packet_too_short;
00408   }
00409 
00410   if (hflags & PPPOL2TP_HEADERFLAG_LENGTH) {
00411     GETSHORT(len, inp);
00412     if (p->len < len || len < hlen) {
00413       goto packet_too_short;
00414     }
00415   }
00416   GETSHORT(tunnel_id, inp);
00417   GETSHORT(session_id, inp);
00418   if (hflags & PPPOL2TP_HEADERFLAG_SEQUENCE) {
00419     GETSHORT(ns, inp);
00420     GETSHORT(nr, inp);
00421   }
00422   if (hflags & PPPOL2TP_HEADERFLAG_OFFSET) {
00423     GETSHORT(offset, inp)
00424     if (offset > 4096) { /* don't be fooled with large offset which might overflow hlen */
00425       PPPDEBUG(LOG_DEBUG, ("pppol2tp: strange packet received, offset=%d\n", offset));
00426       goto free_and_return;
00427     }
00428     hlen += offset;
00429     if (p->len < hlen) {
00430       goto packet_too_short;
00431     }
00432     INCPTR(offset, inp);
00433   }
00434 
00435   /* printf("HLEN = %d\n", hlen); */
00436 
00437   /* skip L2TP header */
00438   if (pbuf_header(p, -(s16_t)hlen) != 0) {
00439     goto free_and_return;
00440   }
00441 
00442   /* printf("LEN=%d, TUNNEL_ID=%d, SESSION_ID=%d, NS=%d, NR=%d, OFFSET=%d\n", len, tunnel_id, session_id, ns, nr, offset); */
00443   PPPDEBUG(LOG_DEBUG, ("pppol2tp: input packet, len=%"U16_F", tunnel=%"U16_F", session=%"U16_F", ns=%"U16_F", nr=%"U16_F"\n",
00444     len, tunnel_id, session_id, ns, nr));
00445 
00446   /* Control packet */
00447   if (hflags & PPPOL2TP_HEADERFLAG_CONTROL) {
00448     pppol2tp_dispatch_control_packet(l2tp, port, p, ns, nr);
00449     goto free_and_return;
00450   }
00451 
00452   /* Data packet */
00453   if(l2tp->phase != PPPOL2TP_STATE_DATA) {
00454     goto free_and_return;
00455   }
00456   if(tunnel_id != l2tp->remote_tunnel_id) {
00457      PPPDEBUG(LOG_DEBUG, ("pppol2tp: tunnel ID mismatch, assigned=%d, received=%d\n", l2tp->remote_tunnel_id, tunnel_id));
00458      goto free_and_return;
00459   }
00460   if(session_id != l2tp->remote_session_id) {
00461      PPPDEBUG(LOG_DEBUG, ("pppol2tp: session ID mismatch, assigned=%d, received=%d\n", l2tp->remote_session_id, session_id));
00462      goto free_and_return;
00463   }
00464   /*
00465    * skip address & flags if necessary
00466    *
00467    * RFC 2661 does not specify whether the PPP frame in the L2TP payload should
00468    * have a HDLC header or not. We handle both cases for compatibility.
00469    */
00470   if (p->len >= 2) {
00471     GETSHORT(hflags, inp);
00472     if (hflags == 0xff03) {
00473       pbuf_header(p, -(s16_t)2);
00474     }
00475   }
00476   /* Dispatch the packet thereby consuming it. */
00477   ppp_input(l2tp->ppp, p);
00478   return;
00479 
00480 packet_too_short:
00481   PPPDEBUG(LOG_DEBUG, ("pppol2tp: packet too short: %d\n", p->len));
00482 free_and_return:
00483   pbuf_free(p);
00484 }
00485 
00486 /* L2TP Control packet entry point */
00487 static void pppol2tp_dispatch_control_packet(pppol2tp_pcb *l2tp, u16_t port, struct pbuf *p, u16_t ns, u16_t nr) {
00488   u8_t *inp;
00489   u16_t avplen, avpflags, vendorid, attributetype, messagetype=0;
00490   err_t err;
00491 #if PPPOL2TP_AUTH_SUPPORT
00492   lwip_md5_context md5_ctx;
00493   u8_t md5_hash[16];
00494   u8_t challenge_id = 0;
00495 #endif /* PPPOL2TP_AUTH_SUPPORT */
00496 
00497   l2tp->peer_nr = nr;
00498   l2tp->peer_ns = ns;
00499   /* printf("L2TP CTRL INPUT, ns=%d, nr=%d, len=%d\n", ns, nr, p->len); */
00500 
00501   /* Handle the special case of the ICCN acknowledge */
00502   if (l2tp->phase == PPPOL2TP_STATE_ICCN_SENT && l2tp->peer_nr > l2tp->our_ns) {
00503     l2tp->phase = PPPOL2TP_STATE_DATA;
00504   }
00505 
00506   /* ZLB packets */
00507   if (p->tot_len == 0) {
00508     return;
00509   }
00510 
00511   p = ppp_singlebuf(p);
00512   inp = (u8_t*)p->payload;
00513   /* Decode AVPs */
00514   while (p->len > 0) {
00515     if (p->len < sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype) ) {
00516       goto packet_too_short;
00517     }
00518     GETSHORT(avpflags, inp);
00519     avplen = avpflags & PPPOL2TP_AVPHEADERFLAG_LENGTHMASK;
00520     /* printf("AVPLEN = %d\n", avplen); */
00521     if (p->len < avplen || avplen < sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype)) {
00522       goto packet_too_short;
00523     }
00524     GETSHORT(vendorid, inp);
00525     GETSHORT(attributetype, inp);
00526     avplen -= sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype);
00527 
00528     /* Message type must be the first AVP */
00529     if (messagetype == 0) {
00530       if (attributetype != 0 || vendorid != 0 || avplen != sizeof(messagetype) ) {
00531         PPPDEBUG(LOG_DEBUG, ("pppol2tp: message type must be the first AVP\n"));
00532         return;
00533       }
00534       GETSHORT(messagetype, inp);
00535       /* printf("Message type = %d\n", messagetype); */
00536       switch(messagetype) {
00537         /* Start Control Connection Reply */
00538         case PPPOL2TP_MESSAGETYPE_SCCRP:
00539           /* Only accept SCCRP packet if we sent a SCCRQ */
00540           if (l2tp->phase != PPPOL2TP_STATE_SCCRQ_SENT) {
00541             goto send_zlb;
00542           }
00543           break;
00544         /* Incoming Call Reply */
00545         case PPPOL2TP_MESSAGETYPE_ICRP:
00546           /* Only accept ICRP packet if we sent a IRCQ */
00547           if (l2tp->phase != PPPOL2TP_STATE_ICRQ_SENT) {
00548             goto send_zlb;
00549           }
00550           break;
00551         /* Stop Control Connection Notification */
00552         case PPPOL2TP_MESSAGETYPE_STOPCCN:
00553           pppol2tp_send_zlb(l2tp, l2tp->our_ns); /* Ack the StopCCN before we switch to down state */
00554           if (l2tp->phase < PPPOL2TP_STATE_DATA) {
00555             pppol2tp_abort_connect(l2tp);
00556           } else if (l2tp->phase == PPPOL2TP_STATE_DATA) {
00557             /* Don't disconnect here, we let the LCP Echo/Reply find the fact
00558              * that PPP session is down. Asking the PPP stack to end the session
00559              * require strict checking about the PPP phase to prevent endless
00560              * disconnection loops.
00561              */
00562           }
00563           return;
00564         default:
00565           break;
00566       }
00567       goto nextavp;
00568     }
00569 
00570     /* Skip proprietary L2TP extensions */
00571     if (vendorid != 0) {
00572       goto skipavp;
00573     }
00574 
00575     switch (messagetype) {
00576       /* Start Control Connection Reply */
00577       case PPPOL2TP_MESSAGETYPE_SCCRP:
00578        switch (attributetype) {
00579           case PPPOL2TP_AVPTYPE_TUNNELID:
00580             if (avplen != sizeof(l2tp->source_tunnel_id) ) {
00581                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Assign tunnel ID length check failed\n"));
00582                return;
00583             }
00584             GETSHORT(l2tp->source_tunnel_id, inp);
00585             PPPDEBUG(LOG_DEBUG, ("pppol2tp: Assigned tunnel ID %"U16_F"\n", l2tp->source_tunnel_id));
00586             goto nextavp;
00587 #if PPPOL2TP_AUTH_SUPPORT
00588           case PPPOL2TP_AVPTYPE_CHALLENGE:
00589             if (avplen == 0) {
00590                PPPDEBUG(LOG_DEBUG, ("pppol2tp: Challenge length check failed\n"));
00591                return;
00592             }
00593             if (l2tp->secret == NULL) {
00594               PPPDEBUG(LOG_DEBUG, ("pppol2tp: Received challenge from peer and no secret key available\n"));
00595               pppol2tp_abort_connect(l2tp);
00596               return;
00597             }
00598             /* Generate hash of ID, secret, challenge */
00599             lwip_md5_init(&md5_ctx);
00600             lwip_md5_starts(&md5_ctx);
00601             challenge_id = PPPOL2TP_MESSAGETYPE_SCCCN;
00602             lwip_md5_update(&md5_ctx, &challenge_id, 1);
00603             lwip_md5_update(&md5_ctx, l2tp->secret, l2tp->secret_len);
00604             lwip_md5_update(&md5_ctx, inp, avplen);
00605             lwip_md5_finish(&md5_ctx, l2tp->challenge_hash);
00606             lwip_md5_free(&md5_ctx);
00607             l2tp->send_challenge = 1;
00608             goto skipavp;
00609           case PPPOL2TP_AVPTYPE_CHALLENGERESPONSE:
00610             if (avplen != PPPOL2TP_AVPTYPE_CHALLENGERESPONSE_SIZE) {
00611                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Challenge Response length check failed\n"));
00612                return;
00613             }
00614             /* Generate hash of ID, secret, challenge */
00615             lwip_md5_init(&md5_ctx);
00616             lwip_md5_starts(&md5_ctx);
00617             challenge_id = PPPOL2TP_MESSAGETYPE_SCCRP;
00618             lwip_md5_update(&md5_ctx, &challenge_id, 1);
00619             lwip_md5_update(&md5_ctx, l2tp->secret, l2tp->secret_len);
00620             lwip_md5_update(&md5_ctx, l2tp->secret_rv, sizeof(l2tp->secret_rv));
00621             lwip_md5_finish(&md5_ctx, md5_hash);
00622             lwip_md5_free(&md5_ctx);
00623             if ( memcmp(inp, md5_hash, sizeof(md5_hash)) ) {
00624               PPPDEBUG(LOG_DEBUG, ("pppol2tp: Received challenge response from peer and secret key do not match\n"));
00625               pppol2tp_abort_connect(l2tp);
00626               return;
00627             }
00628             goto skipavp;
00629 #endif /* PPPOL2TP_AUTH_SUPPORT */
00630           default:
00631             break;
00632         }
00633         break;
00634       /* Incoming Call Reply */
00635       case PPPOL2TP_MESSAGETYPE_ICRP:
00636         switch (attributetype) {
00637          case PPPOL2TP_AVPTYPE_SESSIONID:
00638             if (avplen != sizeof(l2tp->source_session_id) ) {
00639                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Assign session ID length check failed\n"));
00640                return;
00641             }
00642             GETSHORT(l2tp->source_session_id, inp);
00643             PPPDEBUG(LOG_DEBUG, ("pppol2tp: Assigned session ID %"U16_F"\n", l2tp->source_session_id));
00644             goto nextavp;
00645           default:
00646             break;
00647         }
00648         break;
00649       default:
00650         break;
00651     }
00652 
00653 skipavp:
00654     INCPTR(avplen, inp);
00655 nextavp:
00656     /* printf("AVP Found, vendor=%d, attribute=%d, len=%d\n", vendorid, attributetype, avplen); */
00657     /* next AVP */
00658     if (pbuf_header(p, -(s16_t)(avplen + sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype)) ) != 0) {
00659       return;
00660     }
00661   }
00662 
00663   switch(messagetype) {
00664     /* Start Control Connection Reply */
00665     case PPPOL2TP_MESSAGETYPE_SCCRP:
00666       do {
00667         l2tp->remote_session_id = magic();
00668       } while(l2tp->remote_session_id == 0);
00669       l2tp->tunnel_port = port; /* LNS server might have chosen its own local port */
00670       l2tp->icrq_retried = 0;
00671       l2tp->phase = PPPOL2TP_STATE_ICRQ_SENT;
00672       l2tp->our_ns++;
00673       if ((err = pppol2tp_send_scccn(l2tp, l2tp->our_ns)) != 0) {
00674         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCCN, error=%d\n", err));
00675       }
00676       l2tp->our_ns++;
00677       if ((err = pppol2tp_send_icrq(l2tp, l2tp->our_ns)) != 0) {
00678         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICRQ, error=%d\n", err));
00679       }
00680       sys_untimeout(pppol2tp_timeout, l2tp);
00681       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00682       break;
00683     /* Incoming Call Reply */
00684     case PPPOL2TP_MESSAGETYPE_ICRP:
00685       l2tp->iccn_retried = 0;
00686       l2tp->phase = PPPOL2TP_STATE_ICCN_SENT;
00687       l2tp->our_ns++;
00688       ppp_start(l2tp->ppp); /* notify upper layers */
00689       if ((err = pppol2tp_send_iccn(l2tp, l2tp->our_ns)) != 0) {
00690         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICCN, error=%d\n", err));
00691       }
00692       sys_untimeout(pppol2tp_timeout, l2tp);
00693       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00694       break;
00695     /* Unhandled packet, send ZLB ACK */
00696     default:
00697       goto send_zlb;
00698   }
00699   return;
00700 
00701 send_zlb:
00702   pppol2tp_send_zlb(l2tp, l2tp->our_ns);
00703   return;
00704 packet_too_short:
00705   PPPDEBUG(LOG_DEBUG, ("pppol2tp: packet too short: %d\n", p->len));
00706 }
00707 
00708 /* L2TP Timeout handler */
00709 static void pppol2tp_timeout(void *arg) {
00710   pppol2tp_pcb *l2tp = (pppol2tp_pcb*)arg;
00711   err_t err;
00712   u32_t retry_wait;
00713 
00714   PPPDEBUG(LOG_DEBUG, ("pppol2tp: timeout\n"));
00715 
00716   switch (l2tp->phase) {
00717     case PPPOL2TP_STATE_SCCRQ_SENT:
00718       /* backoff wait */
00719       if (l2tp->sccrq_retried < 0xff) {
00720         l2tp->sccrq_retried++;
00721       }
00722       if (!l2tp->ppp->settings.persist && l2tp->sccrq_retried >= PPPOL2TP_MAXSCCRQ) {
00723         pppol2tp_abort_connect(l2tp);
00724         return;
00725       }
00726       retry_wait = LWIP_MIN(PPPOL2TP_CONTROL_TIMEOUT * l2tp->sccrq_retried, PPPOL2TP_SLOW_RETRY);
00727       PPPDEBUG(LOG_DEBUG, ("pppol2tp: sccrq_retried=%d\n", l2tp->sccrq_retried));
00728       if ((err = pppol2tp_send_sccrq(l2tp)) != 0) {
00729         l2tp->sccrq_retried--;
00730         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCRQ, error=%d\n", err));
00731       }
00732       sys_timeout(retry_wait, pppol2tp_timeout, l2tp);
00733       break;
00734 
00735     case PPPOL2TP_STATE_ICRQ_SENT:
00736       l2tp->icrq_retried++;
00737       if (l2tp->icrq_retried >= PPPOL2TP_MAXICRQ) {
00738         pppol2tp_abort_connect(l2tp);
00739         return;
00740       }
00741       PPPDEBUG(LOG_DEBUG, ("pppol2tp: icrq_retried=%d\n", l2tp->icrq_retried));
00742       if (l2tp->peer_nr <= l2tp->our_ns -1) { /* the SCCCN was not acknowledged */
00743         if ((err = pppol2tp_send_scccn(l2tp, l2tp->our_ns -1)) != 0) {
00744       l2tp->icrq_retried--;
00745       PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCCN, error=%d\n", err));
00746       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00747       break;
00748     }
00749       }
00750       if ((err = pppol2tp_send_icrq(l2tp, l2tp->our_ns)) != 0) {
00751     l2tp->icrq_retried--;
00752     PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICRQ, error=%d\n", err));
00753       }
00754       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00755       break;
00756 
00757     case PPPOL2TP_STATE_ICCN_SENT:
00758       l2tp->iccn_retried++;
00759       if (l2tp->iccn_retried >= PPPOL2TP_MAXICCN) {
00760         pppol2tp_abort_connect(l2tp);
00761         return;
00762       }
00763       PPPDEBUG(LOG_DEBUG, ("pppol2tp: iccn_retried=%d\n", l2tp->iccn_retried));
00764       if ((err = pppol2tp_send_iccn(l2tp, l2tp->our_ns)) != 0) {
00765     l2tp->iccn_retried--;
00766     PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICCN, error=%d\n", err));
00767       }
00768       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
00769       break;
00770 
00771     default:
00772       return;  /* all done, work in peace */
00773   }
00774 }
00775 
00776 /* Connection attempt aborted */
00777 static void pppol2tp_abort_connect(pppol2tp_pcb *l2tp) {
00778   PPPDEBUG(LOG_DEBUG, ("pppol2tp: could not establish connection\n"));
00779   l2tp->phase = PPPOL2TP_STATE_INITIAL;
00780   ppp_link_failed(l2tp->ppp); /* notify upper layers */
00781 }
00782 
00783 /* Initiate a new tunnel */
00784 static err_t pppol2tp_send_sccrq(pppol2tp_pcb *l2tp) {
00785   struct pbuf *pb;
00786   u8_t *p;
00787   u16_t len;
00788 
00789   /* calculate UDP packet length */
00790   len = 12 +8 +8 +10 +10 +6+sizeof(PPPOL2TP_HOSTNAME)-1 +6+sizeof(PPPOL2TP_VENDORNAME)-1 +8 +8;
00791 #if PPPOL2TP_AUTH_SUPPORT
00792   if (l2tp->secret != NULL) {
00793     len += 6 + sizeof(l2tp->secret_rv);
00794   }
00795 #endif /* PPPOL2TP_AUTH_SUPPORT */
00796 
00797   /* allocate a buffer */
00798   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
00799   if (pb == NULL) {
00800     return ERR_MEM;
00801   }
00802   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
00803 
00804   p = (u8_t*)pb->payload;
00805   /* fill in pkt */
00806   /* L2TP control header */
00807   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
00808   PUTSHORT(len, p); /* Length */
00809   PUTSHORT(0, p); /* Tunnel Id */
00810   PUTSHORT(0, p); /* Session Id */
00811   PUTSHORT(0, p); /* NS Sequence number - to peer */
00812   PUTSHORT(0, p); /* NR Sequence number - expected for peer */
00813 
00814   /* AVP - Message type */
00815   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00816   PUTSHORT(0, p); /* Vendor ID */
00817   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
00818   PUTSHORT(PPPOL2TP_MESSAGETYPE_SCCRQ, p); /* Attribute value: Message type: SCCRQ */
00819 
00820   /* AVP - L2TP Version */
00821   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00822   PUTSHORT(0, p); /* Vendor ID */
00823   PUTSHORT(PPPOL2TP_AVPTYPE_VERSION, p); /* Attribute type: Version */
00824   PUTSHORT(PPPOL2TP_VERSION, p); /* Attribute value: L2TP Version */
00825 
00826   /* AVP - Framing capabilities */
00827   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
00828   PUTSHORT(0, p); /* Vendor ID */
00829   PUTSHORT(PPPOL2TP_AVPTYPE_FRAMINGCAPABILITIES, p); /* Attribute type: Framing capabilities */
00830   PUTLONG(PPPOL2TP_FRAMINGCAPABILITIES, p); /* Attribute value: Framing capabilities */
00831 
00832   /* AVP - Bearer capabilities */
00833   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
00834   PUTSHORT(0, p); /* Vendor ID */
00835   PUTSHORT(PPPOL2TP_AVPTYPE_BEARERCAPABILITIES, p); /* Attribute type: Bearer capabilities */
00836   PUTLONG(PPPOL2TP_BEARERCAPABILITIES, p); /* Attribute value: Bearer capabilities */
00837 
00838   /* AVP - Host name */
00839   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6+sizeof(PPPOL2TP_HOSTNAME)-1, p); /* Mandatory flag + len field */
00840   PUTSHORT(0, p); /* Vendor ID */
00841   PUTSHORT(PPPOL2TP_AVPTYPE_HOSTNAME, p); /* Attribute type: Hostname */
00842   MEMCPY(p, PPPOL2TP_HOSTNAME, sizeof(PPPOL2TP_HOSTNAME)-1); /* Attribute value: Hostname */
00843   INCPTR(sizeof(PPPOL2TP_HOSTNAME)-1, p);
00844 
00845   /* AVP - Vendor name */
00846   PUTSHORT(6+sizeof(PPPOL2TP_VENDORNAME)-1, p); /* len field */
00847   PUTSHORT(0, p); /* Vendor ID */
00848   PUTSHORT(PPPOL2TP_AVPTYPE_VENDORNAME, p); /* Attribute type: Vendor name */
00849   MEMCPY(p, PPPOL2TP_VENDORNAME, sizeof(PPPOL2TP_VENDORNAME)-1); /* Attribute value: Vendor name */
00850   INCPTR(sizeof(PPPOL2TP_VENDORNAME)-1, p);
00851 
00852   /* AVP - Assign tunnel ID */
00853   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00854   PUTSHORT(0, p); /* Vendor ID */
00855   PUTSHORT(PPPOL2TP_AVPTYPE_TUNNELID, p); /* Attribute type: Tunnel ID */
00856   PUTSHORT(l2tp->remote_tunnel_id, p); /* Attribute value: Tunnel ID */
00857 
00858   /* AVP - Receive window size */
00859   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00860   PUTSHORT(0, p); /* Vendor ID */
00861   PUTSHORT(PPPOL2TP_AVPTYPE_RECEIVEWINDOWSIZE, p); /* Attribute type: Receive window size */
00862   PUTSHORT(PPPOL2TP_RECEIVEWINDOWSIZE, p); /* Attribute value: Receive window size */
00863 
00864 #if PPPOL2TP_AUTH_SUPPORT
00865   /* AVP - Challenge */
00866   if (l2tp->secret != NULL) {
00867     PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6 + sizeof(l2tp->secret_rv), p); /* Mandatory flag + len field */
00868     PUTSHORT(0, p); /* Vendor ID */
00869     PUTSHORT(PPPOL2TP_AVPTYPE_CHALLENGE, p); /* Attribute type: Challenge */
00870     MEMCPY(p, l2tp->secret_rv, sizeof(l2tp->secret_rv)); /* Attribute value: Random vector */
00871     INCPTR(sizeof(l2tp->secret_rv), p);
00872   }
00873 #endif /* PPPOL2TP_AUTH_SUPPORT */
00874 
00875   return pppol2tp_udp_send(l2tp, pb);
00876 }
00877 
00878 /* Complete tunnel establishment */
00879 static err_t pppol2tp_send_scccn(pppol2tp_pcb *l2tp, u16_t ns) {
00880   struct pbuf *pb;
00881   u8_t *p;
00882   u16_t len;
00883 
00884   /* calculate UDP packet length */
00885   len = 12 +8;
00886 #if PPPOL2TP_AUTH_SUPPORT
00887   if (l2tp->send_challenge) {
00888     len += 6 + sizeof(l2tp->challenge_hash);
00889   }
00890 #endif /* PPPOL2TP_AUTH_SUPPORT */
00891 
00892   /* allocate a buffer */
00893   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
00894   if (pb == NULL) {
00895     return ERR_MEM;
00896   }
00897   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
00898 
00899   p = (u8_t*)pb->payload;
00900   /* fill in pkt */
00901   /* L2TP control header */
00902   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
00903   PUTSHORT(len, p); /* Length */
00904   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
00905   PUTSHORT(0, p); /* Session Id */
00906   PUTSHORT(ns, p); /* NS Sequence number - to peer */
00907   PUTSHORT(l2tp->peer_ns+1, p); /* NR Sequence number - expected for peer */
00908 
00909   /* AVP - Message type */
00910   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00911   PUTSHORT(0, p); /* Vendor ID */
00912   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
00913   PUTSHORT(PPPOL2TP_MESSAGETYPE_SCCCN, p); /* Attribute value: Message type: SCCCN */
00914 
00915 #if PPPOL2TP_AUTH_SUPPORT
00916   /* AVP - Challenge response */
00917   if (l2tp->send_challenge) {
00918     PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6 + sizeof(l2tp->challenge_hash), p); /* Mandatory flag + len field */
00919     PUTSHORT(0, p); /* Vendor ID */
00920     PUTSHORT(PPPOL2TP_AVPTYPE_CHALLENGERESPONSE, p); /* Attribute type: Challenge response */
00921     MEMCPY(p, l2tp->challenge_hash, sizeof(l2tp->challenge_hash)); /* Attribute value: Computed challenge */
00922     INCPTR(sizeof(l2tp->challenge_hash), p);
00923   }
00924 #endif /* PPPOL2TP_AUTH_SUPPORT */
00925 
00926   return pppol2tp_udp_send(l2tp, pb);
00927 }
00928 
00929 /* Initiate a new session */
00930 static err_t pppol2tp_send_icrq(pppol2tp_pcb *l2tp, u16_t ns) {
00931   struct pbuf *pb;
00932   u8_t *p;
00933   u16_t len;
00934   u32_t serialnumber;
00935 
00936   /* calculate UDP packet length */
00937   len = 12 +8 +8 +10;
00938 
00939   /* allocate a buffer */
00940   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
00941   if (pb == NULL) {
00942     return ERR_MEM;
00943   }
00944   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
00945 
00946   p = (u8_t*)pb->payload;
00947   /* fill in pkt */
00948   /* L2TP control header */
00949   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
00950   PUTSHORT(len, p); /* Length */
00951   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
00952   PUTSHORT(0, p); /* Session Id */
00953   PUTSHORT(ns, p); /* NS Sequence number - to peer */
00954   PUTSHORT(l2tp->peer_ns+1, p); /* NR Sequence number - expected for peer */
00955 
00956   /* AVP - Message type */
00957   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00958   PUTSHORT(0, p); /* Vendor ID */
00959   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
00960   PUTSHORT(PPPOL2TP_MESSAGETYPE_ICRQ, p); /* Attribute value: Message type: ICRQ */
00961 
00962   /* AVP - Assign session ID */
00963   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
00964   PUTSHORT(0, p); /* Vendor ID */
00965   PUTSHORT(PPPOL2TP_AVPTYPE_SESSIONID, p); /* Attribute type: Session ID */
00966   PUTSHORT(l2tp->remote_session_id, p); /* Attribute value: Session ID */
00967 
00968   /* AVP - Call Serial Number */
00969   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
00970   PUTSHORT(0, p); /* Vendor ID */
00971   PUTSHORT(PPPOL2TP_AVPTYPE_CALLSERIALNUMBER, p); /* Attribute type: Serial number */
00972   serialnumber = magic();
00973   PUTLONG(serialnumber, p); /* Attribute value: Serial number */
00974 
00975   return pppol2tp_udp_send(l2tp, pb);
00976 }
00977 
00978 /* Complete tunnel establishment */
00979 static err_t pppol2tp_send_iccn(pppol2tp_pcb *l2tp, u16_t ns) {
00980   struct pbuf *pb;
00981   u8_t *p;
00982   u16_t len;
00983 
00984   /* calculate UDP packet length */
00985   len = 12 +8 +10 +10;
00986 
00987   /* allocate a buffer */
00988   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
00989   if (pb == NULL) {
00990     return ERR_MEM;
00991   }
00992   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
00993 
00994   p = (u8_t*)pb->payload;
00995   /* fill in pkt */
00996   /* L2TP control header */
00997   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
00998   PUTSHORT(len, p); /* Length */
00999   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
01000   PUTSHORT(l2tp->source_session_id, p); /* Session Id */
01001   PUTSHORT(ns, p); /* NS Sequence number - to peer */
01002   PUTSHORT(l2tp->peer_ns+1, p); /* NR Sequence number - expected for peer */
01003 
01004   /* AVP - Message type */
01005   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
01006   PUTSHORT(0, p); /* Vendor ID */
01007   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
01008   PUTSHORT(PPPOL2TP_MESSAGETYPE_ICCN, p); /* Attribute value: Message type: ICCN */
01009 
01010   /* AVP - Framing type */
01011   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
01012   PUTSHORT(0, p); /* Vendor ID */
01013   PUTSHORT(PPPOL2TP_AVPTYPE_FRAMINGTYPE, p); /* Attribute type: Framing type */
01014   PUTLONG(PPPOL2TP_FRAMINGTYPE, p); /* Attribute value: Framing type */
01015 
01016   /* AVP - TX Connect speed */
01017   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
01018   PUTSHORT(0, p); /* Vendor ID */
01019   PUTSHORT(PPPOL2TP_AVPTYPE_TXCONNECTSPEED, p); /* Attribute type: TX Connect speed */
01020   PUTLONG(PPPOL2TP_TXCONNECTSPEED, p); /* Attribute value: TX Connect speed */
01021 
01022   return pppol2tp_udp_send(l2tp, pb);
01023 }
01024 
01025 /* Send a ZLB ACK packet */
01026 static err_t pppol2tp_send_zlb(pppol2tp_pcb *l2tp, u16_t ns) {
01027   struct pbuf *pb;
01028   u8_t *p;
01029   u16_t len;
01030 
01031   /* calculate UDP packet length */
01032   len = 12;
01033 
01034   /* allocate a buffer */
01035   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
01036   if (pb == NULL) {
01037     return ERR_MEM;
01038   }
01039   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
01040 
01041   p = (u8_t*)pb->payload;
01042   /* fill in pkt */
01043   /* L2TP control header */
01044   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
01045   PUTSHORT(len, p); /* Length */
01046   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
01047   PUTSHORT(0, p); /* Session Id */
01048   PUTSHORT(ns, p); /* NS Sequence number - to peer */
01049   PUTSHORT(l2tp->peer_ns+1, p); /* NR Sequence number - expected for peer */
01050 
01051   return pppol2tp_udp_send(l2tp, pb);
01052 }
01053 
01054 /* Send a StopCCN packet */
01055 static err_t pppol2tp_send_stopccn(pppol2tp_pcb *l2tp, u16_t ns) {
01056   struct pbuf *pb;
01057   u8_t *p;
01058   u16_t len;
01059 
01060   /* calculate UDP packet length */
01061   len = 12 +8 +8 +8;
01062 
01063   /* allocate a buffer */
01064   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
01065   if (pb == NULL) {
01066     return ERR_MEM;
01067   }
01068   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
01069 
01070   p = (u8_t*)pb->payload;
01071   /* fill in pkt */
01072   /* L2TP control header */
01073   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
01074   PUTSHORT(len, p); /* Length */
01075   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
01076   PUTSHORT(0, p); /* Session Id */
01077   PUTSHORT(ns, p); /* NS Sequence number - to peer */
01078   PUTSHORT(l2tp->peer_ns+1, p); /* NR Sequence number - expected for peer */
01079 
01080   /* AVP - Message type */
01081   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
01082   PUTSHORT(0, p); /* Vendor ID */
01083   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
01084   PUTSHORT(PPPOL2TP_MESSAGETYPE_STOPCCN, p); /* Attribute value: Message type: StopCCN */
01085 
01086   /* AVP - Assign tunnel ID */
01087   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
01088   PUTSHORT(0, p); /* Vendor ID */
01089   PUTSHORT(PPPOL2TP_AVPTYPE_TUNNELID, p); /* Attribute type: Tunnel ID */
01090   PUTSHORT(l2tp->remote_tunnel_id, p); /* Attribute value: Tunnel ID */
01091 
01092   /* AVP - Result code */
01093   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
01094   PUTSHORT(0, p); /* Vendor ID */
01095   PUTSHORT(PPPOL2TP_AVPTYPE_RESULTCODE, p); /* Attribute type: Result code */
01096   PUTSHORT(PPPOL2TP_RESULTCODE, p); /* Attribute value: Result code */
01097 
01098   return pppol2tp_udp_send(l2tp, pb);
01099 }
01100 
01101 static err_t pppol2tp_xmit(pppol2tp_pcb *l2tp, struct pbuf *pb) {
01102   u8_t *p;
01103 
01104   /* make room for L2TP header - should not fail */
01105   if (pbuf_header(pb, (s16_t)PPPOL2TP_OUTPUT_DATA_HEADER_LEN) != 0) {
01106     /* bail out */
01107     PPPDEBUG(LOG_ERR, ("pppol2tp: pppol2tp_pcb: could not allocate room for L2TP header\n"));
01108     LINK_STATS_INC(link.lenerr);
01109     pbuf_free(pb);
01110     return ERR_BUF;
01111   }
01112 
01113   p = (u8_t*)pb->payload;
01114   PUTSHORT(PPPOL2TP_HEADERFLAG_DATA_MANDATORY, p);
01115   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
01116   PUTSHORT(l2tp->source_session_id, p); /* Session Id */
01117 
01118   return pppol2tp_udp_send(l2tp, pb);
01119 }
01120 
01121 static err_t pppol2tp_udp_send(pppol2tp_pcb *l2tp, struct pbuf *pb) {
01122   err_t err;
01123   if (l2tp->netif) {
01124     err = udp_sendto_if(l2tp->udp, pb, &l2tp->remote_ip, l2tp->tunnel_port, l2tp->netif);
01125   } else {
01126     err = udp_sendto(l2tp->udp, pb, &l2tp->remote_ip, l2tp->tunnel_port);
01127   }
01128   pbuf_free(pb);
01129   return err;
01130 }
01131 
01132 #endif /* PPP_SUPPORT && PPPOL2TP_SUPPORT */