1 /**
2  * @file
3  * Network Point to Point Protocol over Layer 2 Tunneling Protocol program file.
4  *
5  */
6 
7 /*
8  * Redistribution and use in source and binary forms, with or without modification,
9  * are permitted provided that the following conditions are met:
10  *
11  * 1. Redistributions of source code must retain the above copyright notice,
12  *    this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright notice,
14  *    this list of conditions and the following disclaimer in the documentation
15  *    and/or other materials provided with the distribution.
16  * 3. The name of the author may not be used to endorse or promote products
17  *    derived from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
20  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
22  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
23  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
24  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
27  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
28  * OF SUCH DAMAGE.
29  *
30  * This file is part of the lwIP TCP/IP stack.
31  *
32  */
33 
34 /*
35  * L2TP Support status:
36  *
37  * Supported:
38  * - L2TPv2 (PPP over L2TP, a.k.a. UDP tunnels)
39  * - LAC
40  *
41  * Not supported:
42  * - LNS (require PPP server support)
43  * - L2TPv3 ethernet pseudowires
44  * - L2TPv3 VLAN pseudowire
45  * - L2TPv3 PPP pseudowires
46  * - L2TPv3 IP encapsulation
47  * - L2TPv3 IP pseudowire
48  * - L2TP tunnel switching - http://tools.ietf.org/html/draft-ietf-l2tpext-tunnel-switching-08
49  * - Multiple tunnels per UDP socket, as well as multiple sessions per tunnel
50  * - Hidden AVPs
51  */
52 
53 #include "netif/ppp/ppp_opts.h"
54 #if PPP_SUPPORT && PPPOL2TP_SUPPORT /* don't build if not configured for use in lwipopts.h */
55 
56 #include "lwip/err.h"
57 #include "lwip/memp.h"
58 #include "lwip/netif.h"
59 #include "lwip/udp.h"
60 #include "lwip/snmp.h"
61 
62 #include "netif/ppp/ppp_impl.h"
63 #include "netif/ppp/lcp.h"
64 #include "netif/ppp/ipcp.h"
65 #include "netif/ppp/pppol2tp.h"
66 #include "netif/ppp/pppcrypt.h"
67 #include "netif/ppp/magic.h"
68 
69 /* Memory pool */
70 LWIP_MEMPOOL_DECLARE(PPPOL2TP_PCB, MEMP_NUM_PPPOL2TP_INTERFACES, sizeof(pppol2tp_pcb), "PPPOL2TP_PCB")
71 
72 /* callbacks called from PPP core */
73 static err_t pppol2tp_write(ppp_pcb *ppp, void *ctx, struct pbuf *p);
74 static err_t pppol2tp_netif_output(ppp_pcb *ppp, void *ctx, struct pbuf *p, u_short protocol);
75 static err_t pppol2tp_destroy(ppp_pcb *ppp, void *ctx);    /* Destroy a L2TP control block */
76 static void pppol2tp_connect(ppp_pcb *ppp, void *ctx);    /* Be a LAC, connect to a LNS. */
77 static void pppol2tp_disconnect(ppp_pcb *ppp, void *ctx);  /* Disconnect */
78 
79  /* Prototypes for procedures local to this file. */
80 static void pppol2tp_input(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port);
81 static void pppol2tp_dispatch_control_packet(pppol2tp_pcb *l2tp, u16_t port, struct pbuf *p, u16_t ns, u16_t nr);
82 static void pppol2tp_timeout(void *arg);
83 static void pppol2tp_abort_connect(pppol2tp_pcb *l2tp);
84 static err_t pppol2tp_send_sccrq(pppol2tp_pcb *l2tp);
85 static err_t pppol2tp_send_scccn(pppol2tp_pcb *l2tp, u16_t ns);
86 static err_t pppol2tp_send_icrq(pppol2tp_pcb *l2tp, u16_t ns);
87 static err_t pppol2tp_send_iccn(pppol2tp_pcb *l2tp, u16_t ns);
88 static err_t pppol2tp_send_zlb(pppol2tp_pcb *l2tp, u16_t ns, u16_t nr);
89 static err_t pppol2tp_send_stopccn(pppol2tp_pcb *l2tp, u16_t ns);
90 static err_t pppol2tp_xmit(pppol2tp_pcb *l2tp, struct pbuf *pb);
91 static err_t pppol2tp_udp_send(pppol2tp_pcb *l2tp, struct pbuf *pb);
92 
93 /* Callbacks structure for PPP core */
94 static const struct link_callbacks pppol2tp_callbacks = {
95   pppol2tp_connect,
96 #if PPP_SERVER
97   NULL,
98 #endif /* PPP_SERVER */
99   pppol2tp_disconnect,
100   pppol2tp_destroy,
101   pppol2tp_write,
102   pppol2tp_netif_output,
103   NULL,
104   NULL
105 };
106 
107 
108 /* Create a new L2TP session. */
pppol2tp_create(struct netif * pppif,struct netif * netif,const ip_addr_t * ipaddr,u16_t port,const u8_t * secret,u8_t secret_len,ppp_link_status_cb_fn link_status_cb,void * ctx_cb)109 ppp_pcb *pppol2tp_create(struct netif *pppif,
110        struct netif *netif, const ip_addr_t *ipaddr, u16_t port,
111        const u8_t *secret, u8_t secret_len,
112        ppp_link_status_cb_fn link_status_cb, void *ctx_cb) {
113   ppp_pcb *ppp;
114   pppol2tp_pcb *l2tp;
115   struct udp_pcb *udp;
116 #if !PPPOL2TP_AUTH_SUPPORT
117   LWIP_UNUSED_ARG(secret);
118   LWIP_UNUSED_ARG(secret_len);
119 #endif /* !PPPOL2TP_AUTH_SUPPORT */
120 
121   if (ipaddr == NULL) {
122     goto ipaddr_check_failed;
123   }
124 
125   l2tp = (pppol2tp_pcb *)LWIP_MEMPOOL_ALLOC(PPPOL2TP_PCB);
126   if (l2tp == NULL) {
127     goto memp_malloc_l2tp_failed;
128   }
129 
130   udp = udp_new_ip_type(IP_GET_TYPE(ipaddr));
131   if (udp == NULL) {
132     goto udp_new_failed;
133   }
134   udp_recv(udp, pppol2tp_input, l2tp);
135 
136   ppp = ppp_new(pppif, &pppol2tp_callbacks, l2tp, link_status_cb, ctx_cb);
137   if (ppp == NULL) {
138     goto ppp_new_failed;
139   }
140 
141   memset(l2tp, 0, sizeof(pppol2tp_pcb));
142   l2tp->phase = PPPOL2TP_STATE_INITIAL;
143   l2tp->ppp = ppp;
144   l2tp->udp = udp;
145   l2tp->netif = netif;
146   ip_addr_copy(l2tp->remote_ip, *ipaddr);
147   l2tp->remote_port = port;
148 #if PPPOL2TP_AUTH_SUPPORT
149   l2tp->secret = secret;
150   l2tp->secret_len = secret_len;
151 #endif /* PPPOL2TP_AUTH_SUPPORT */
152 
153   return ppp;
154 
155 ppp_new_failed:
156   udp_remove(udp);
157 udp_new_failed:
158   LWIP_MEMPOOL_FREE(PPPOL2TP_PCB, l2tp);
159 memp_malloc_l2tp_failed:
160 ipaddr_check_failed:
161   return NULL;
162 }
163 
164 /* Called by PPP core */
pppol2tp_write(ppp_pcb * ppp,void * ctx,struct pbuf * p)165 static err_t pppol2tp_write(ppp_pcb *ppp, void *ctx, struct pbuf *p) {
166   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
167   struct pbuf *ph; /* UDP + L2TP header */
168   err_t ret;
169 #if MIB2_STATS
170   u16_t tot_len;
171 #else /* MIB2_STATS */
172   LWIP_UNUSED_ARG(ppp);
173 #endif /* MIB2_STATS */
174 
175   /* skip address & flags */
176   pbuf_remove_header(p, 2);
177 
178   ph = pbuf_alloc(PBUF_TRANSPORT, (u16_t)(PPPOL2TP_OUTPUT_DATA_HEADER_LEN), PBUF_RAM);
179   if(!ph) {
180     LINK_STATS_INC(link.memerr);
181     LINK_STATS_INC(link.proterr);
182     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
183     pbuf_free(p);
184     return ERR_MEM;
185   }
186 
187   pbuf_remove_header(ph, PPPOL2TP_OUTPUT_DATA_HEADER_LEN); /* hide L2TP header */
188   pbuf_cat(ph, p);
189 #if MIB2_STATS
190   tot_len = ph->tot_len;
191 #endif /* MIB2_STATS */
192 
193   ret = pppol2tp_xmit(l2tp, ph);
194   if (ret != ERR_OK) {
195     LINK_STATS_INC(link.err);
196     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
197     return ret;
198   }
199 
200   MIB2_STATS_NETIF_ADD(ppp->netif, ifoutoctets, (u16_t)tot_len);
201   MIB2_STATS_NETIF_INC(ppp->netif, ifoutucastpkts);
202   LINK_STATS_INC(link.xmit);
203   return ERR_OK;
204 }
205 
206 /* Called by PPP core */
pppol2tp_netif_output(ppp_pcb * ppp,void * ctx,struct pbuf * p,u_short protocol)207 static err_t pppol2tp_netif_output(ppp_pcb *ppp, void *ctx, struct pbuf *p, u_short protocol) {
208   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
209   struct pbuf *pb;
210   u8_t *pl;
211   err_t err;
212 #if MIB2_STATS
213   u16_t tot_len;
214 #else /* MIB2_STATS */
215   LWIP_UNUSED_ARG(ppp);
216 #endif /* MIB2_STATS */
217 
218   /* @todo: try to use pbuf_header() here! */
219   pb = pbuf_alloc(PBUF_TRANSPORT, PPPOL2TP_OUTPUT_DATA_HEADER_LEN + sizeof(protocol), PBUF_RAM);
220   if(!pb) {
221     LINK_STATS_INC(link.memerr);
222     LINK_STATS_INC(link.proterr);
223     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
224     return ERR_MEM;
225   }
226 
227   pbuf_remove_header(pb, PPPOL2TP_OUTPUT_DATA_HEADER_LEN);
228 
229   pl = (u8_t*)pb->payload;
230   PUTSHORT(protocol, pl);
231 
232   pbuf_chain(pb, p);
233 #if MIB2_STATS
234   tot_len = pb->tot_len;
235 #endif /* MIB2_STATS */
236 
237   if( (err = pppol2tp_xmit(l2tp, pb)) != ERR_OK) {
238     LINK_STATS_INC(link.err);
239     MIB2_STATS_NETIF_INC(ppp->netif, ifoutdiscards);
240     return err;
241   }
242 
243   MIB2_STATS_NETIF_ADD(ppp->netif, ifoutoctets, tot_len);
244   MIB2_STATS_NETIF_INC(ppp->netif, ifoutucastpkts);
245   LINK_STATS_INC(link.xmit);
246   return ERR_OK;
247 }
248 
249 /* Destroy a L2TP control block */
pppol2tp_destroy(ppp_pcb * ppp,void * ctx)250 static err_t pppol2tp_destroy(ppp_pcb *ppp, void *ctx) {
251   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
252   LWIP_UNUSED_ARG(ppp);
253 
254   sys_untimeout(pppol2tp_timeout, l2tp);
255   udp_remove(l2tp->udp);
256   LWIP_MEMPOOL_FREE(PPPOL2TP_PCB, l2tp);
257   return ERR_OK;
258 }
259 
260 /* Be a LAC, connect to a LNS. */
pppol2tp_connect(ppp_pcb * ppp,void * ctx)261 static void pppol2tp_connect(ppp_pcb *ppp, void *ctx) {
262   err_t err;
263   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
264   lcp_options *lcp_wo;
265   lcp_options *lcp_ao;
266 #if PPP_IPV4_SUPPORT && VJ_SUPPORT
267   ipcp_options *ipcp_wo;
268   ipcp_options *ipcp_ao;
269 #endif /* PPP_IPV4_SUPPORT && VJ_SUPPORT */
270 
271   l2tp->tunnel_port = l2tp->remote_port;
272   l2tp->our_ns = 0;
273   l2tp->peer_nr = 0;
274   l2tp->peer_ns = 0;
275   l2tp->source_tunnel_id = 0;
276   l2tp->remote_tunnel_id = 0;
277   l2tp->source_session_id = 0;
278   l2tp->remote_session_id = 0;
279   /* l2tp->*_retried are cleared when used */
280 
281   lcp_wo = &ppp->lcp_wantoptions;
282   lcp_wo->mru = PPPOL2TP_DEFMRU;
283   lcp_wo->neg_asyncmap = 0;
284   lcp_wo->neg_pcompression = 0;
285   lcp_wo->neg_accompression = 0;
286   lcp_wo->passive = 0;
287   lcp_wo->silent = 0;
288 
289   lcp_ao = &ppp->lcp_allowoptions;
290   lcp_ao->mru = PPPOL2TP_DEFMRU;
291   lcp_ao->neg_asyncmap = 0;
292   lcp_ao->neg_pcompression = 0;
293   lcp_ao->neg_accompression = 0;
294 
295 #if PPP_IPV4_SUPPORT && VJ_SUPPORT
296   ipcp_wo = &ppp->ipcp_wantoptions;
297   ipcp_wo->neg_vj = 0;
298   ipcp_wo->old_vj = 0;
299 
300   ipcp_ao = &ppp->ipcp_allowoptions;
301   ipcp_ao->neg_vj = 0;
302   ipcp_ao->old_vj = 0;
303 #endif /* PPP_IPV4_SUPPORT && VJ_SUPPORT */
304 
305   /* Listen to a random source port, we need to do that instead of using udp_connect()
306    * because the L2TP LNS might answer with its own random source port (!= 1701)
307    */
308 #if LWIP_IPV6
309   if (IP_IS_V6_VAL(l2tp->udp->local_ip)) {
310     udp_bind(l2tp->udp, IP6_ADDR_ANY, 0);
311   } else
312 #endif /* LWIP_IPV6 */
313   udp_bind(l2tp->udp, IP_ADDR_ANY, 0);
314 
315 #if PPPOL2TP_AUTH_SUPPORT
316   /* Generate random vector */
317   if (l2tp->secret != NULL) {
318     magic_random_bytes(l2tp->secret_rv, sizeof(l2tp->secret_rv));
319   }
320 #endif /* PPPOL2TP_AUTH_SUPPORT */
321 
322   do {
323     l2tp->remote_tunnel_id = magic();
324   } while(l2tp->remote_tunnel_id == 0);
325   /* save state, in case we fail to send SCCRQ */
326   l2tp->sccrq_retried = 0;
327   l2tp->phase = PPPOL2TP_STATE_SCCRQ_SENT;
328   if ((err = pppol2tp_send_sccrq(l2tp)) != 0) {
329     PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCRQ, error=%d\n", err));
330   }
331   sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
332 }
333 
334 /* Disconnect */
pppol2tp_disconnect(ppp_pcb * ppp,void * ctx)335 static void pppol2tp_disconnect(ppp_pcb *ppp, void *ctx) {
336   pppol2tp_pcb *l2tp = (pppol2tp_pcb *)ctx;
337 
338   l2tp->our_ns++;
339   pppol2tp_send_stopccn(l2tp, l2tp->our_ns);
340 
341   /* stop any timer, disconnect can be called while initiating is in progress */
342   sys_untimeout(pppol2tp_timeout, l2tp);
343   l2tp->phase = PPPOL2TP_STATE_INITIAL;
344   ppp_link_end(ppp); /* notify upper layers */
345 }
346 
347 /* UDP Callback for incoming IPv4 L2TP frames */
pppol2tp_input(void * arg,struct udp_pcb * pcb,struct pbuf * p,const ip_addr_t * addr,u16_t port)348 static void pppol2tp_input(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) {
349   pppol2tp_pcb *l2tp = (pppol2tp_pcb*)arg;
350   u16_t hflags, hlen, len=0, tunnel_id=0, session_id=0, ns=0, nr=0, offset=0;
351   u8_t *inp;
352   LWIP_UNUSED_ARG(pcb);
353 
354   /* we can't unbound a UDP pcb, thus we can still receive UDP frames after the link is closed */
355   if (l2tp->phase < PPPOL2TP_STATE_SCCRQ_SENT) {
356     goto free_and_return;
357   }
358 
359   if (!ip_addr_eq(&l2tp->remote_ip, addr)) {
360     goto free_and_return;
361   }
362 
363   /* discard packet if port mismatch, but only if we received a SCCRP */
364   if (l2tp->phase > PPPOL2TP_STATE_SCCRQ_SENT && l2tp->tunnel_port != port) {
365     goto free_and_return;
366   }
367 
368   /* printf("-----------\nL2TP INPUT, %d\n", p->len); */
369 
370   /* L2TP header */
371   if (p->len < sizeof(hflags) + sizeof(tunnel_id) + sizeof(session_id) ) {
372     goto packet_too_short;
373   }
374 
375   inp = (u8_t*)p->payload;
376   GETSHORT(hflags, inp);
377 
378   if (hflags & PPPOL2TP_HEADERFLAG_CONTROL) {
379     /* check mandatory flags for a control packet */
380     if ( (hflags & PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY) != PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY ) {
381       PPPDEBUG(LOG_DEBUG, ("pppol2tp: mandatory header flags for control packet not set\n"));
382       goto free_and_return;
383     }
384     /* check forbidden flags for a control packet */
385     if (hflags & PPPOL2TP_HEADERFLAG_CONTROL_FORBIDDEN) {
386       PPPDEBUG(LOG_DEBUG, ("pppol2tp: forbidden header flags for control packet found\n"));
387       goto free_and_return;
388     }
389   } else {
390     /* check mandatory flags for a data packet */
391     if ( (hflags & PPPOL2TP_HEADERFLAG_DATA_MANDATORY) != PPPOL2TP_HEADERFLAG_DATA_MANDATORY) {
392       PPPDEBUG(LOG_DEBUG, ("pppol2tp: mandatory header flags for data packet not set\n"));
393       goto free_and_return;
394     }
395   }
396 
397   /* Expected header size  */
398   hlen = sizeof(hflags) + sizeof(tunnel_id) + sizeof(session_id);
399   if (hflags & PPPOL2TP_HEADERFLAG_LENGTH) {
400     hlen += sizeof(len);
401   }
402   if (hflags & PPPOL2TP_HEADERFLAG_SEQUENCE) {
403     hlen += sizeof(ns) + sizeof(nr);
404   }
405   if (hflags & PPPOL2TP_HEADERFLAG_OFFSET) {
406     hlen += sizeof(offset);
407   }
408   if (p->len < hlen) {
409     goto packet_too_short;
410   }
411 
412   if (hflags & PPPOL2TP_HEADERFLAG_LENGTH) {
413     GETSHORT(len, inp);
414     if (p->len < len || len < hlen) {
415       goto packet_too_short;
416     }
417   }
418   GETSHORT(tunnel_id, inp);
419   GETSHORT(session_id, inp);
420   if (hflags & PPPOL2TP_HEADERFLAG_SEQUENCE) {
421     GETSHORT(ns, inp);
422     GETSHORT(nr, inp);
423   }
424   if (hflags & PPPOL2TP_HEADERFLAG_OFFSET) {
425     GETSHORT(offset, inp)
426     if (offset > 4096) { /* don't be fooled with large offset which might overflow hlen */
427       PPPDEBUG(LOG_DEBUG, ("pppol2tp: strange packet received, offset=%d\n", offset));
428       goto free_and_return;
429     }
430     hlen += offset;
431     if (p->len < hlen) {
432       goto packet_too_short;
433     }
434     INCPTR(offset, inp);
435   }
436 
437   /* printf("HLEN = %d\n", hlen); */
438 
439   /* skip L2TP header */
440   if (pbuf_remove_header(p, hlen) != 0) {
441     goto free_and_return;
442   }
443 
444   /* printf("LEN=%d, TUNNEL_ID=%d, SESSION_ID=%d, NS=%d, NR=%d, OFFSET=%d\n", len, tunnel_id, session_id, ns, nr, offset); */
445   PPPDEBUG(LOG_DEBUG, ("pppol2tp: input packet, len=%"U16_F", tunnel=%"U16_F", session=%"U16_F", ns=%"U16_F", nr=%"U16_F"\n",
446     p->tot_len, tunnel_id, session_id, ns, nr));
447 
448   /* Control packet */
449   if (hflags & PPPOL2TP_HEADERFLAG_CONTROL) {
450     pppol2tp_dispatch_control_packet(l2tp, port, p, ns, nr);
451     goto free_and_return;
452   }
453 
454   /* Data packet */
455   if(l2tp->phase != PPPOL2TP_STATE_DATA) {
456     goto free_and_return;
457   }
458   if(tunnel_id != l2tp->remote_tunnel_id) {
459      PPPDEBUG(LOG_DEBUG, ("pppol2tp: tunnel ID mismatch, assigned=%d, received=%d\n", l2tp->remote_tunnel_id, tunnel_id));
460      goto free_and_return;
461   }
462   if(session_id != l2tp->remote_session_id) {
463      PPPDEBUG(LOG_DEBUG, ("pppol2tp: session ID mismatch, assigned=%d, received=%d\n", l2tp->remote_session_id, session_id));
464      goto free_and_return;
465   }
466   /*
467    * skip address & flags if necessary
468    *
469    * RFC 2661 (L2TPv2) does not specify whether the PPP frame in the L2TP payload
470    * should have a HDLC header or not, both behaviors are seen in the wild.
471    *
472    * On the other hand, L2TPv3 draft-ietf-l2tpext-l2tp-ppp versions 00 and 01 say
473    * it must be included, versions 02 to 05 say it must be omitted, and versions
474    * 06 and onwards say it should be omitted so it changed along the path when
475    * L2TPv3 was designed.  Latest versions state that receivers must handle both
476    * cases.
477    *
478    * We handle both cases for compatibility.
479    */
480   if (p->len >= 2) {
481     GETSHORT(hflags, inp);
482     if (hflags == 0xff03) {
483       pbuf_remove_header(p, 2);
484     }
485   }
486   /* Dispatch the packet thereby consuming it. */
487   ppp_input(l2tp->ppp, p);
488   return;
489 
490 packet_too_short:
491   PPPDEBUG(LOG_DEBUG, ("pppol2tp: packet too short: %d\n", p->len));
492 free_and_return:
493   pbuf_free(p);
494 }
495 
496 /* L2TP Control packet entry point */
pppol2tp_dispatch_control_packet(pppol2tp_pcb * l2tp,u16_t port,struct pbuf * p,u16_t ns,u16_t nr)497 static void pppol2tp_dispatch_control_packet(pppol2tp_pcb *l2tp, u16_t port, struct pbuf *p, u16_t ns, u16_t nr) {
498   u8_t *inp;
499   u16_t avplen, avpflags, vendorid, attributetype, messagetype=0;
500   err_t err;
501 #if PPPOL2TP_AUTH_SUPPORT
502   lwip_md5_context md5_ctx;
503   u8_t md5_hash[16];
504   u8_t challenge_id = 0;
505 #endif /* PPPOL2TP_AUTH_SUPPORT */
506 
507   /* printf("L2TP CTRL INPUT, ns=%d, nr=%d, len=%d\n", ns, nr, p->len); */
508 
509   /* Drop unexpected packet */
510   if (ns != l2tp->peer_ns) {
511     PPPDEBUG(LOG_DEBUG, ("pppol2tp: drop unexpected packet: received NS=%d, expected NS=%d\n", ns, l2tp->peer_ns));
512     /*
513      * In order to ensure that all messages are acknowledged properly
514      * (particularly in the case of a lost ZLB ACK message), receipt
515      * of duplicate messages MUST be acknowledged.
516      *
517      * In this very special case we Ack a packet we previously received.
518      * Therefore our NS is the NR we just received. And our NR is the
519      * NS we just received plus one.
520      */
521     if ((s16_t)(ns - l2tp->peer_ns) < 0) {
522       pppol2tp_send_zlb(l2tp, nr, ns+1);
523     }
524     return;
525   }
526 
527   l2tp->peer_nr = nr;
528 
529   /* Handle the special case of the ICCN acknowledge */
530   if (l2tp->phase == PPPOL2TP_STATE_ICCN_SENT && (s16_t)(l2tp->peer_nr - l2tp->our_ns) > 0) {
531     l2tp->phase = PPPOL2TP_STATE_DATA;
532     sys_untimeout(pppol2tp_timeout, l2tp);
533     ppp_start(l2tp->ppp); /* notify upper layers */
534   }
535 
536   /* ZLB packets */
537   if (p->tot_len == 0) {
538     return;
539   }
540   /* A ZLB packet does not consume a NS slot thus we don't record the NS value for ZLB packets */
541   l2tp->peer_ns = ns+1;
542 
543   p = pbuf_coalesce(p, PBUF_RAW);
544   if (p->next != NULL) {
545     PPPDEBUG(LOG_DEBUG, ("pppol2tp: pbuf_coalesce failed: %d\n", p->tot_len));
546     return;
547   }
548 
549   inp = (u8_t*)p->payload;
550   /* Decode AVPs */
551   while (p->len > 0) {
552     if (p->len < sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype) ) {
553       goto packet_too_short;
554     }
555     GETSHORT(avpflags, inp);
556     avplen = avpflags & PPPOL2TP_AVPHEADERFLAG_LENGTHMASK;
557     /* printf("AVPLEN = %d\n", avplen); */
558     if (p->len < avplen || avplen < sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype)) {
559       goto packet_too_short;
560     }
561     GETSHORT(vendorid, inp);
562     GETSHORT(attributetype, inp);
563     avplen -= sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype);
564 
565     /* Message type must be the first AVP */
566     if (messagetype == 0) {
567       if (attributetype != 0 || vendorid != 0 || avplen != sizeof(messagetype) ) {
568         PPPDEBUG(LOG_DEBUG, ("pppol2tp: message type must be the first AVP\n"));
569         return;
570       }
571       GETSHORT(messagetype, inp);
572       /* printf("Message type = %d\n", messagetype); */
573       switch(messagetype) {
574         /* Start Control Connection Reply */
575         case PPPOL2TP_MESSAGETYPE_SCCRP:
576           /* Only accept SCCRP packet if we sent a SCCRQ */
577           if (l2tp->phase != PPPOL2TP_STATE_SCCRQ_SENT) {
578             goto send_zlb;
579           }
580           break;
581         /* Incoming Call Reply */
582         case PPPOL2TP_MESSAGETYPE_ICRP:
583           /* Only accept ICRP packet if we sent a IRCQ */
584           if (l2tp->phase != PPPOL2TP_STATE_ICRQ_SENT) {
585             goto send_zlb;
586           }
587           break;
588         /* Stop Control Connection Notification */
589         case PPPOL2TP_MESSAGETYPE_STOPCCN:
590           pppol2tp_send_zlb(l2tp, l2tp->our_ns+1, l2tp->peer_ns); /* Ack the StopCCN before we switch to down state */
591           if (l2tp->phase < PPPOL2TP_STATE_DATA) {
592             pppol2tp_abort_connect(l2tp);
593           } else if (l2tp->phase == PPPOL2TP_STATE_DATA) {
594             /* Don't disconnect here, we let the LCP Echo/Reply find the fact
595              * that PPP session is down. Asking the PPP stack to end the session
596              * require strict checking about the PPP phase to prevent endless
597              * disconnection loops.
598              */
599           }
600           return;
601         default:
602           break;
603       }
604       goto nextavp;
605     }
606 
607     /* Skip proprietary L2TP extensions */
608     if (vendorid != 0) {
609       goto skipavp;
610     }
611 
612     switch (messagetype) {
613       /* Start Control Connection Reply */
614       case PPPOL2TP_MESSAGETYPE_SCCRP:
615        switch (attributetype) {
616           case PPPOL2TP_AVPTYPE_TUNNELID:
617             if (avplen != sizeof(l2tp->source_tunnel_id) ) {
618                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Assign tunnel ID length check failed\n"));
619                return;
620             }
621             GETSHORT(l2tp->source_tunnel_id, inp);
622             PPPDEBUG(LOG_DEBUG, ("pppol2tp: Assigned tunnel ID %"U16_F"\n", l2tp->source_tunnel_id));
623             goto nextavp;
624 #if PPPOL2TP_AUTH_SUPPORT
625           case PPPOL2TP_AVPTYPE_CHALLENGE:
626             if (avplen == 0) {
627                PPPDEBUG(LOG_DEBUG, ("pppol2tp: Challenge length check failed\n"));
628                return;
629             }
630             if (l2tp->secret == NULL) {
631               PPPDEBUG(LOG_DEBUG, ("pppol2tp: Received challenge from peer and no secret key available\n"));
632               pppol2tp_abort_connect(l2tp);
633               return;
634             }
635             /* Generate hash of ID, secret, challenge */
636             lwip_md5_init(&md5_ctx);
637             lwip_md5_starts(&md5_ctx);
638             challenge_id = PPPOL2TP_MESSAGETYPE_SCCCN;
639             lwip_md5_update(&md5_ctx, &challenge_id, 1);
640             lwip_md5_update(&md5_ctx, l2tp->secret, l2tp->secret_len);
641             lwip_md5_update(&md5_ctx, inp, avplen);
642             lwip_md5_finish(&md5_ctx, l2tp->challenge_hash);
643             lwip_md5_free(&md5_ctx);
644             l2tp->send_challenge = 1;
645             goto skipavp;
646           case PPPOL2TP_AVPTYPE_CHALLENGERESPONSE:
647             if (avplen != PPPOL2TP_AVPTYPE_CHALLENGERESPONSE_SIZE) {
648                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Challenge Response length check failed\n"));
649                return;
650             }
651             /* Generate hash of ID, secret, challenge */
652             lwip_md5_init(&md5_ctx);
653             lwip_md5_starts(&md5_ctx);
654             challenge_id = PPPOL2TP_MESSAGETYPE_SCCRP;
655             lwip_md5_update(&md5_ctx, &challenge_id, 1);
656             lwip_md5_update(&md5_ctx, l2tp->secret, l2tp->secret_len);
657             lwip_md5_update(&md5_ctx, l2tp->secret_rv, sizeof(l2tp->secret_rv));
658             lwip_md5_finish(&md5_ctx, md5_hash);
659             lwip_md5_free(&md5_ctx);
660             if ( memcmp(inp, md5_hash, sizeof(md5_hash)) ) {
661               PPPDEBUG(LOG_DEBUG, ("pppol2tp: Received challenge response from peer and secret key do not match\n"));
662               pppol2tp_abort_connect(l2tp);
663               return;
664             }
665             goto skipavp;
666 #endif /* PPPOL2TP_AUTH_SUPPORT */
667           default:
668             break;
669         }
670         break;
671       /* Incoming Call Reply */
672       case PPPOL2TP_MESSAGETYPE_ICRP:
673         switch (attributetype) {
674          case PPPOL2TP_AVPTYPE_SESSIONID:
675             if (avplen != sizeof(l2tp->source_session_id) ) {
676                PPPDEBUG(LOG_DEBUG, ("pppol2tp: AVP Assign session ID length check failed\n"));
677                return;
678             }
679             GETSHORT(l2tp->source_session_id, inp);
680             PPPDEBUG(LOG_DEBUG, ("pppol2tp: Assigned session ID %"U16_F"\n", l2tp->source_session_id));
681             goto nextavp;
682           default:
683             break;
684         }
685         break;
686       default:
687         break;
688     }
689 
690 skipavp:
691     INCPTR(avplen, inp);
692 nextavp:
693     /* printf("AVP Found, vendor=%d, attribute=%d, len=%d\n", vendorid, attributetype, avplen); */
694     /* next AVP */
695     if (pbuf_remove_header(p, avplen + sizeof(avpflags) + sizeof(vendorid) + sizeof(attributetype)) != 0) {
696       return;
697     }
698   }
699 
700   switch(messagetype) {
701     /* Start Control Connection Reply */
702     case PPPOL2TP_MESSAGETYPE_SCCRP:
703       do {
704         l2tp->remote_session_id = magic();
705       } while(l2tp->remote_session_id == 0);
706       l2tp->tunnel_port = port; /* LNS server might have chosen its own local port */
707       l2tp->icrq_retried = 0;
708       l2tp->phase = PPPOL2TP_STATE_ICRQ_SENT;
709       l2tp->our_ns++;
710       if ((err = pppol2tp_send_scccn(l2tp, l2tp->our_ns)) != 0) {
711         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCCN, error=%d\n", err));
712         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
713       }
714       l2tp->our_ns++;
715       if ((err = pppol2tp_send_icrq(l2tp, l2tp->our_ns)) != 0) {
716         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICRQ, error=%d\n", err));
717         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
718       }
719       sys_untimeout(pppol2tp_timeout, l2tp);
720       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
721       break;
722     /* Incoming Call Reply */
723     case PPPOL2TP_MESSAGETYPE_ICRP:
724       l2tp->iccn_retried = 0;
725       l2tp->phase = PPPOL2TP_STATE_ICCN_SENT;
726       l2tp->our_ns++;
727       if ((err = pppol2tp_send_iccn(l2tp, l2tp->our_ns)) != 0) {
728         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICCN, error=%d\n", err));
729         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
730       }
731       sys_untimeout(pppol2tp_timeout, l2tp);
732       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
733       break;
734     /* Unhandled packet, send ZLB ACK */
735     default:
736       goto send_zlb;
737   }
738   return;
739 
740 send_zlb:
741   pppol2tp_send_zlb(l2tp, l2tp->our_ns+1, l2tp->peer_ns);
742   return;
743 packet_too_short:
744   PPPDEBUG(LOG_DEBUG, ("pppol2tp: packet too short: %d\n", p->len));
745 }
746 
747 /* L2TP Timeout handler */
pppol2tp_timeout(void * arg)748 static void pppol2tp_timeout(void *arg) {
749   pppol2tp_pcb *l2tp = (pppol2tp_pcb*)arg;
750   err_t err;
751   u32_t retry_wait;
752 
753   PPPDEBUG(LOG_DEBUG, ("pppol2tp: timeout\n"));
754 
755   switch (l2tp->phase) {
756     case PPPOL2TP_STATE_SCCRQ_SENT:
757       /* backoff wait */
758       if (l2tp->sccrq_retried < 0xff) {
759         l2tp->sccrq_retried++;
760       }
761       if (!l2tp->ppp->settings.persist && l2tp->sccrq_retried >= PPPOL2TP_MAXSCCRQ) {
762         pppol2tp_abort_connect(l2tp);
763         return;
764       }
765       retry_wait = LWIP_MIN(PPPOL2TP_CONTROL_TIMEOUT * l2tp->sccrq_retried, PPPOL2TP_SLOW_RETRY);
766       PPPDEBUG(LOG_DEBUG, ("pppol2tp: sccrq_retried=%d\n", l2tp->sccrq_retried));
767       if ((err = pppol2tp_send_sccrq(l2tp)) != 0) {
768         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCRQ, error=%d\n", err));
769         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
770       }
771       sys_timeout(retry_wait, pppol2tp_timeout, l2tp);
772       break;
773 
774     case PPPOL2TP_STATE_ICRQ_SENT:
775       l2tp->icrq_retried++;
776       if (l2tp->icrq_retried >= PPPOL2TP_MAXICRQ) {
777         pppol2tp_abort_connect(l2tp);
778         return;
779       }
780       PPPDEBUG(LOG_DEBUG, ("pppol2tp: icrq_retried=%d\n", l2tp->icrq_retried));
781       if ((s16_t)(l2tp->peer_nr - l2tp->our_ns) < 0) { /* the SCCCN was not acknowledged */
782         if ((err = pppol2tp_send_scccn(l2tp, l2tp->our_ns -1)) != 0) {
783           PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send SCCCN, error=%d\n", err));
784           LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
785           sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
786           break;
787         }
788       }
789       if ((err = pppol2tp_send_icrq(l2tp, l2tp->our_ns)) != 0) {
790         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICRQ, error=%d\n", err));
791         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
792       }
793       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
794       break;
795 
796     case PPPOL2TP_STATE_ICCN_SENT:
797       l2tp->iccn_retried++;
798       if (l2tp->iccn_retried >= PPPOL2TP_MAXICCN) {
799         pppol2tp_abort_connect(l2tp);
800         return;
801       }
802       PPPDEBUG(LOG_DEBUG, ("pppol2tp: iccn_retried=%d\n", l2tp->iccn_retried));
803       if ((err = pppol2tp_send_iccn(l2tp, l2tp->our_ns)) != 0) {
804         PPPDEBUG(LOG_DEBUG, ("pppol2tp: failed to send ICCN, error=%d\n", err));
805         LWIP_UNUSED_ARG(err); /* if PPPDEBUG is disabled */
806       }
807       sys_timeout(PPPOL2TP_CONTROL_TIMEOUT, pppol2tp_timeout, l2tp);
808       break;
809 
810     default:
811       return;  /* all done, work in peace */
812   }
813 }
814 
815 /* Connection attempt aborted */
pppol2tp_abort_connect(pppol2tp_pcb * l2tp)816 static void pppol2tp_abort_connect(pppol2tp_pcb *l2tp) {
817   PPPDEBUG(LOG_DEBUG, ("pppol2tp: could not establish connection\n"));
818   l2tp->phase = PPPOL2TP_STATE_INITIAL;
819   ppp_link_failed(l2tp->ppp); /* notify upper layers */
820 }
821 
822 /* Initiate a new tunnel */
pppol2tp_send_sccrq(pppol2tp_pcb * l2tp)823 static err_t pppol2tp_send_sccrq(pppol2tp_pcb *l2tp) {
824   struct pbuf *pb;
825   u8_t *p;
826   u16_t len;
827 
828   /* calculate UDP packet length */
829   len = 12 +8 +8 +10 +10 +6+sizeof(PPPOL2TP_HOSTNAME)-1 +6+sizeof(PPPOL2TP_VENDORNAME)-1 +8 +8;
830 #if PPPOL2TP_AUTH_SUPPORT
831   if (l2tp->secret != NULL) {
832     len += 6 + sizeof(l2tp->secret_rv);
833   }
834 #endif /* PPPOL2TP_AUTH_SUPPORT */
835 
836   /* allocate a buffer */
837   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
838   if (pb == NULL) {
839     return ERR_MEM;
840   }
841   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
842 
843   p = (u8_t*)pb->payload;
844   /* fill in pkt */
845   /* L2TP control header */
846   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
847   PUTSHORT(len, p); /* Length */
848   PUTSHORT(0, p); /* Tunnel Id */
849   PUTSHORT(0, p); /* Session Id */
850   PUTSHORT(0, p); /* NS Sequence number - to peer */
851   PUTSHORT(0, p); /* NR Sequence number - expected for peer */
852 
853   /* AVP - Message type */
854   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
855   PUTSHORT(0, p); /* Vendor ID */
856   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
857   PUTSHORT(PPPOL2TP_MESSAGETYPE_SCCRQ, p); /* Attribute value: Message type: SCCRQ */
858 
859   /* AVP - L2TP Version */
860   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
861   PUTSHORT(0, p); /* Vendor ID */
862   PUTSHORT(PPPOL2TP_AVPTYPE_VERSION, p); /* Attribute type: Version */
863   PUTSHORT(PPPOL2TP_VERSION, p); /* Attribute value: L2TP Version */
864 
865   /* AVP - Framing capabilities */
866   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
867   PUTSHORT(0, p); /* Vendor ID */
868   PUTSHORT(PPPOL2TP_AVPTYPE_FRAMINGCAPABILITIES, p); /* Attribute type: Framing capabilities */
869   PUTLONG(PPPOL2TP_FRAMINGCAPABILITIES, p); /* Attribute value: Framing capabilities */
870 
871   /* AVP - Bearer capabilities */
872   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
873   PUTSHORT(0, p); /* Vendor ID */
874   PUTSHORT(PPPOL2TP_AVPTYPE_BEARERCAPABILITIES, p); /* Attribute type: Bearer capabilities */
875   PUTLONG(PPPOL2TP_BEARERCAPABILITIES, p); /* Attribute value: Bearer capabilities */
876 
877   /* AVP - Host name */
878   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6+sizeof(PPPOL2TP_HOSTNAME)-1, p); /* Mandatory flag + len field */
879   PUTSHORT(0, p); /* Vendor ID */
880   PUTSHORT(PPPOL2TP_AVPTYPE_HOSTNAME, p); /* Attribute type: Hostname */
881   MEMCPY(p, PPPOL2TP_HOSTNAME, sizeof(PPPOL2TP_HOSTNAME)-1); /* Attribute value: Hostname */
882   INCPTR(sizeof(PPPOL2TP_HOSTNAME)-1, p);
883 
884   /* AVP - Vendor name */
885   PUTSHORT(6+sizeof(PPPOL2TP_VENDORNAME)-1, p); /* len field */
886   PUTSHORT(0, p); /* Vendor ID */
887   PUTSHORT(PPPOL2TP_AVPTYPE_VENDORNAME, p); /* Attribute type: Vendor name */
888   MEMCPY(p, PPPOL2TP_VENDORNAME, sizeof(PPPOL2TP_VENDORNAME)-1); /* Attribute value: Vendor name */
889   INCPTR(sizeof(PPPOL2TP_VENDORNAME)-1, p);
890 
891   /* AVP - Assign tunnel ID */
892   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
893   PUTSHORT(0, p); /* Vendor ID */
894   PUTSHORT(PPPOL2TP_AVPTYPE_TUNNELID, p); /* Attribute type: Tunnel ID */
895   PUTSHORT(l2tp->remote_tunnel_id, p); /* Attribute value: Tunnel ID */
896 
897   /* AVP - Receive window size */
898   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
899   PUTSHORT(0, p); /* Vendor ID */
900   PUTSHORT(PPPOL2TP_AVPTYPE_RECEIVEWINDOWSIZE, p); /* Attribute type: Receive window size */
901   PUTSHORT(PPPOL2TP_RECEIVEWINDOWSIZE, p); /* Attribute value: Receive window size */
902 
903 #if PPPOL2TP_AUTH_SUPPORT
904   /* AVP - Challenge */
905   if (l2tp->secret != NULL) {
906     PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6 + sizeof(l2tp->secret_rv), p); /* Mandatory flag + len field */
907     PUTSHORT(0, p); /* Vendor ID */
908     PUTSHORT(PPPOL2TP_AVPTYPE_CHALLENGE, p); /* Attribute type: Challenge */
909     MEMCPY(p, l2tp->secret_rv, sizeof(l2tp->secret_rv)); /* Attribute value: Random vector */
910     INCPTR(sizeof(l2tp->secret_rv), p);
911   }
912 #endif /* PPPOL2TP_AUTH_SUPPORT */
913 
914   return pppol2tp_udp_send(l2tp, pb);
915 }
916 
917 /* Complete tunnel establishment */
pppol2tp_send_scccn(pppol2tp_pcb * l2tp,u16_t ns)918 static err_t pppol2tp_send_scccn(pppol2tp_pcb *l2tp, u16_t ns) {
919   struct pbuf *pb;
920   u8_t *p;
921   u16_t len;
922 
923   /* calculate UDP packet length */
924   len = 12 +8;
925 #if PPPOL2TP_AUTH_SUPPORT
926   if (l2tp->send_challenge) {
927     len += 6 + sizeof(l2tp->challenge_hash);
928   }
929 #endif /* PPPOL2TP_AUTH_SUPPORT */
930 
931   /* allocate a buffer */
932   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
933   if (pb == NULL) {
934     return ERR_MEM;
935   }
936   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
937 
938   p = (u8_t*)pb->payload;
939   /* fill in pkt */
940   /* L2TP control header */
941   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
942   PUTSHORT(len, p); /* Length */
943   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
944   PUTSHORT(0, p); /* Session Id */
945   PUTSHORT(ns, p); /* NS Sequence number - to peer */
946   PUTSHORT(l2tp->peer_ns, p); /* NR Sequence number - expected for peer */
947 
948   /* AVP - Message type */
949   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
950   PUTSHORT(0, p); /* Vendor ID */
951   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
952   PUTSHORT(PPPOL2TP_MESSAGETYPE_SCCCN, p); /* Attribute value: Message type: SCCCN */
953 
954 #if PPPOL2TP_AUTH_SUPPORT
955   /* AVP - Challenge response */
956   if (l2tp->send_challenge) {
957     PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 6 + sizeof(l2tp->challenge_hash), p); /* Mandatory flag + len field */
958     PUTSHORT(0, p); /* Vendor ID */
959     PUTSHORT(PPPOL2TP_AVPTYPE_CHALLENGERESPONSE, p); /* Attribute type: Challenge response */
960     MEMCPY(p, l2tp->challenge_hash, sizeof(l2tp->challenge_hash)); /* Attribute value: Computed challenge */
961     INCPTR(sizeof(l2tp->challenge_hash), p);
962   }
963 #endif /* PPPOL2TP_AUTH_SUPPORT */
964 
965   return pppol2tp_udp_send(l2tp, pb);
966 }
967 
968 /* Initiate a new session */
pppol2tp_send_icrq(pppol2tp_pcb * l2tp,u16_t ns)969 static err_t pppol2tp_send_icrq(pppol2tp_pcb *l2tp, u16_t ns) {
970   struct pbuf *pb;
971   u8_t *p;
972   u16_t len;
973   u32_t serialnumber;
974 
975   /* calculate UDP packet length */
976   len = 12 +8 +8 +10;
977 
978   /* allocate a buffer */
979   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
980   if (pb == NULL) {
981     return ERR_MEM;
982   }
983   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
984 
985   p = (u8_t*)pb->payload;
986   /* fill in pkt */
987   /* L2TP control header */
988   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
989   PUTSHORT(len, p); /* Length */
990   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
991   PUTSHORT(0, p); /* Session Id */
992   PUTSHORT(ns, p); /* NS Sequence number - to peer */
993   PUTSHORT(l2tp->peer_ns, p); /* NR Sequence number - expected for peer */
994 
995   /* AVP - Message type */
996   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
997   PUTSHORT(0, p); /* Vendor ID */
998   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
999   PUTSHORT(PPPOL2TP_MESSAGETYPE_ICRQ, p); /* Attribute value: Message type: ICRQ */
1000 
1001   /* AVP - Assign session ID */
1002   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
1003   PUTSHORT(0, p); /* Vendor ID */
1004   PUTSHORT(PPPOL2TP_AVPTYPE_SESSIONID, p); /* Attribute type: Session ID */
1005   PUTSHORT(l2tp->remote_session_id, p); /* Attribute value: Session ID */
1006 
1007   /* AVP - Call Serial Number */
1008   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
1009   PUTSHORT(0, p); /* Vendor ID */
1010   PUTSHORT(PPPOL2TP_AVPTYPE_CALLSERIALNUMBER, p); /* Attribute type: Serial number */
1011   serialnumber = magic();
1012   PUTLONG(serialnumber, p); /* Attribute value: Serial number */
1013 
1014   return pppol2tp_udp_send(l2tp, pb);
1015 }
1016 
1017 /* Complete tunnel establishment */
pppol2tp_send_iccn(pppol2tp_pcb * l2tp,u16_t ns)1018 static err_t pppol2tp_send_iccn(pppol2tp_pcb *l2tp, u16_t ns) {
1019   struct pbuf *pb;
1020   u8_t *p;
1021   u16_t len;
1022 
1023   /* calculate UDP packet length */
1024   len = 12 +8 +10 +10;
1025 
1026   /* allocate a buffer */
1027   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
1028   if (pb == NULL) {
1029     return ERR_MEM;
1030   }
1031   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
1032 
1033   p = (u8_t*)pb->payload;
1034   /* fill in pkt */
1035   /* L2TP control header */
1036   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
1037   PUTSHORT(len, p); /* Length */
1038   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
1039   PUTSHORT(l2tp->source_session_id, p); /* Session Id */
1040   PUTSHORT(ns, p); /* NS Sequence number - to peer */
1041   PUTSHORT(l2tp->peer_ns, p); /* NR Sequence number - expected for peer */
1042 
1043   /* AVP - Message type */
1044   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
1045   PUTSHORT(0, p); /* Vendor ID */
1046   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
1047   PUTSHORT(PPPOL2TP_MESSAGETYPE_ICCN, p); /* Attribute value: Message type: ICCN */
1048 
1049   /* AVP - Framing type */
1050   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
1051   PUTSHORT(0, p); /* Vendor ID */
1052   PUTSHORT(PPPOL2TP_AVPTYPE_FRAMINGTYPE, p); /* Attribute type: Framing type */
1053   PUTLONG(PPPOL2TP_FRAMINGTYPE, p); /* Attribute value: Framing type */
1054 
1055   /* AVP - TX Connect speed */
1056   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 10, p); /* Mandatory flag + len field */
1057   PUTSHORT(0, p); /* Vendor ID */
1058   PUTSHORT(PPPOL2TP_AVPTYPE_TXCONNECTSPEED, p); /* Attribute type: TX Connect speed */
1059   PUTLONG(PPPOL2TP_TXCONNECTSPEED, p); /* Attribute value: TX Connect speed */
1060 
1061   return pppol2tp_udp_send(l2tp, pb);
1062 }
1063 
1064 /* Send a ZLB ACK packet */
pppol2tp_send_zlb(pppol2tp_pcb * l2tp,u16_t ns,u16_t nr)1065 static err_t pppol2tp_send_zlb(pppol2tp_pcb *l2tp, u16_t ns, u16_t nr) {
1066   struct pbuf *pb;
1067   u8_t *p;
1068   u16_t len;
1069 
1070   /* calculate UDP packet length */
1071   len = 12;
1072 
1073   /* allocate a buffer */
1074   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
1075   if (pb == NULL) {
1076     return ERR_MEM;
1077   }
1078   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
1079 
1080   p = (u8_t*)pb->payload;
1081   /* fill in pkt */
1082   /* L2TP control header */
1083   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
1084   PUTSHORT(len, p); /* Length */
1085   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
1086   PUTSHORT(0, p); /* Session Id */
1087   PUTSHORT(ns, p); /* NS Sequence number - to peer */
1088   PUTSHORT(nr, p); /* NR Sequence number - expected for peer */
1089 
1090   return pppol2tp_udp_send(l2tp, pb);
1091 }
1092 
1093 /* Send a StopCCN packet */
pppol2tp_send_stopccn(pppol2tp_pcb * l2tp,u16_t ns)1094 static err_t pppol2tp_send_stopccn(pppol2tp_pcb *l2tp, u16_t ns) {
1095   struct pbuf *pb;
1096   u8_t *p;
1097   u16_t len;
1098 
1099   /* calculate UDP packet length */
1100   len = 12 +8 +8 +8;
1101 
1102   /* allocate a buffer */
1103   pb = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
1104   if (pb == NULL) {
1105     return ERR_MEM;
1106   }
1107   LWIP_ASSERT("pb->tot_len == pb->len", pb->tot_len == pb->len);
1108 
1109   p = (u8_t*)pb->payload;
1110   /* fill in pkt */
1111   /* L2TP control header */
1112   PUTSHORT(PPPOL2TP_HEADERFLAG_CONTROL_MANDATORY, p);
1113   PUTSHORT(len, p); /* Length */
1114   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
1115   PUTSHORT(0, p); /* Session Id */
1116   PUTSHORT(ns, p); /* NS Sequence number - to peer */
1117   PUTSHORT(l2tp->peer_ns, p); /* NR Sequence number - expected for peer */
1118 
1119   /* AVP - Message type */
1120   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
1121   PUTSHORT(0, p); /* Vendor ID */
1122   PUTSHORT(PPPOL2TP_AVPTYPE_MESSAGE, p); /* Attribute type: Message Type */
1123   PUTSHORT(PPPOL2TP_MESSAGETYPE_STOPCCN, p); /* Attribute value: Message type: StopCCN */
1124 
1125   /* AVP - Assign tunnel ID */
1126   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
1127   PUTSHORT(0, p); /* Vendor ID */
1128   PUTSHORT(PPPOL2TP_AVPTYPE_TUNNELID, p); /* Attribute type: Tunnel ID */
1129   PUTSHORT(l2tp->remote_tunnel_id, p); /* Attribute value: Tunnel ID */
1130 
1131   /* AVP - Result code */
1132   PUTSHORT(PPPOL2TP_AVPHEADERFLAG_MANDATORY + 8, p); /* Mandatory flag + len field */
1133   PUTSHORT(0, p); /* Vendor ID */
1134   PUTSHORT(PPPOL2TP_AVPTYPE_RESULTCODE, p); /* Attribute type: Result code */
1135   PUTSHORT(PPPOL2TP_RESULTCODE, p); /* Attribute value: Result code */
1136 
1137   return pppol2tp_udp_send(l2tp, pb);
1138 }
1139 
pppol2tp_xmit(pppol2tp_pcb * l2tp,struct pbuf * pb)1140 static err_t pppol2tp_xmit(pppol2tp_pcb *l2tp, struct pbuf *pb) {
1141   u8_t *p;
1142 
1143   /* make room for L2TP header - should not fail */
1144   if (pbuf_add_header(pb, PPPOL2TP_OUTPUT_DATA_HEADER_LEN) != 0) {
1145     /* bail out */
1146     PPPDEBUG(LOG_ERR, ("pppol2tp: pppol2tp_pcb: could not allocate room for L2TP header\n"));
1147     LINK_STATS_INC(link.lenerr);
1148     pbuf_free(pb);
1149     return ERR_BUF;
1150   }
1151 
1152   p = (u8_t*)pb->payload;
1153   PUTSHORT(PPPOL2TP_HEADERFLAG_DATA_MANDATORY, p);
1154   PUTSHORT(l2tp->source_tunnel_id, p); /* Tunnel Id */
1155   PUTSHORT(l2tp->source_session_id, p); /* Session Id */
1156 
1157   return pppol2tp_udp_send(l2tp, pb);
1158 }
1159 
pppol2tp_udp_send(pppol2tp_pcb * l2tp,struct pbuf * pb)1160 static err_t pppol2tp_udp_send(pppol2tp_pcb *l2tp, struct pbuf *pb) {
1161   err_t err;
1162   if (l2tp->netif) {
1163     err = udp_sendto_if(l2tp->udp, pb, &l2tp->remote_ip, l2tp->tunnel_port, l2tp->netif);
1164   } else {
1165     err = udp_sendto(l2tp->udp, pb, &l2tp->remote_ip, l2tp->tunnel_port);
1166   }
1167   pbuf_free(pb);
1168   return err;
1169 }
1170 
1171 #endif /* PPP_SUPPORT && PPPOL2TP_SUPPORT */
1172