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