kernel/netgraph7: Replace usage of MALLOC/FREE with kmalloc/kfree here too.
[dragonfly.git] / sys / netgraph7 / ng_l2tp.c
1 /*-
2  * Copyright (c) 2001-2002 Packet Design, LLC.
3  * All rights reserved.
4  * 
5  * Subject to the following obligations and disclaimer of warranty,
6  * use and redistribution of this software, in source or object code
7  * forms, with or without modifications are expressly permitted by
8  * Packet Design; provided, however, that:
9  * 
10  *    (i)  Any and all reproductions of the source or object code
11  *         must include the copyright notice above and the following
12  *         disclaimer of warranties; and
13  *    (ii) No rights are granted, in any manner or form, to use
14  *         Packet Design trademarks, including the mark "PACKET DESIGN"
15  *         on advertising, endorsements, or otherwise except as such
16  *         appears in the above copyright notice or in the software.
17  * 
18  * THIS SOFTWARE IS BEING PROVIDED BY PACKET DESIGN "AS IS", AND
19  * TO THE MAXIMUM EXTENT PERMITTED BY LAW, PACKET DESIGN MAKES NO
20  * REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED, REGARDING
21  * THIS SOFTWARE, INCLUDING WITHOUT LIMITATION, ANY AND ALL IMPLIED
22  * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
23  * OR NON-INFRINGEMENT.  PACKET DESIGN DOES NOT WARRANT, GUARANTEE,
24  * OR MAKE ANY REPRESENTATIONS REGARDING THE USE OF, OR THE RESULTS
25  * OF THE USE OF THIS SOFTWARE IN TERMS OF ITS CORRECTNESS, ACCURACY,
26  * RELIABILITY OR OTHERWISE.  IN NO EVENT SHALL PACKET DESIGN BE
27  * LIABLE FOR ANY DAMAGES RESULTING FROM OR ARISING OUT OF ANY USE
28  * OF THIS SOFTWARE, INCLUDING WITHOUT LIMITATION, ANY DIRECT,
29  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, PUNITIVE, OR CONSEQUENTIAL
30  * DAMAGES, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES, LOSS OF
31  * USE, DATA OR PROFITS, HOWEVER CAUSED AND UNDER ANY THEORY OF
32  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
33  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
34  * THE USE OF THIS SOFTWARE, EVEN IF PACKET DESIGN IS ADVISED OF
35  * THE POSSIBILITY OF SUCH DAMAGE.
36  * 
37  * Author: Archie Cobbs <archie@freebsd.org>
38  *
39  * $FreeBSD: src/sys/netgraph/ng_l2tp.c,v 1.25 2008/03/16 21:33:12 mav Exp $
40  * $DragonFly: src/sys/netgraph7/ng_l2tp.c,v 1.2 2008/06/26 23:05:35 dillon Exp $
41  */
42
43 /*
44  * L2TP netgraph node type.
45  *
46  * This node type implements the lower layer of the
47  * L2TP protocol as specified in RFC 2661.
48  */
49
50 #include <sys/param.h>
51 #include <sys/systm.h>
52 #include <sys/kernel.h>
53 #include <sys/time.h>
54 #include <sys/conf.h>
55 #include <sys/mbuf.h>
56 #include <sys/malloc.h>
57 #include <sys/errno.h>
58 #include <sys/libkern.h>
59
60 #include "ng_message.h"
61 #include "netgraph.h"
62 #include "ng_parse.h"
63 #include "ng_l2tp.h"
64
65 #ifdef NG_SEPARATE_MALLOC
66 MALLOC_DEFINE(M_NETGRAPH_L2TP, "netgraph_l2tp", "netgraph l2tp node");
67 #else
68 #define M_NETGRAPH_L2TP M_NETGRAPH
69 #endif
70
71 /* L2TP header format (first 2 bytes only) */
72 #define L2TP_HDR_CTRL           0x8000                  /* control packet */
73 #define L2TP_HDR_LEN            0x4000                  /* has length field */
74 #define L2TP_HDR_SEQ            0x0800                  /* has ns, nr fields */
75 #define L2TP_HDR_OFF            0x0200                  /* has offset field */
76 #define L2TP_HDR_PRIO           0x0100                  /* give priority */
77 #define L2TP_HDR_VERS_MASK      0x000f                  /* version field mask */
78 #define L2TP_HDR_VERSION        0x0002                  /* version field */
79
80 /* Bits that must be zero or one in first two bytes of header */
81 #define L2TP_CTRL_0BITS         0x030d                  /* ctrl: must be 0 */
82 #define L2TP_CTRL_1BITS         0xc802                  /* ctrl: must be 1 */
83 #define L2TP_DATA_0BITS         0x800d                  /* data: must be 0 */
84 #define L2TP_DATA_1BITS         0x0002                  /* data: must be 1 */
85
86 /* Standard xmit ctrl and data header bits */
87 #define L2TP_CTRL_HDR           (L2TP_HDR_CTRL | L2TP_HDR_LEN \
88                                     | L2TP_HDR_SEQ | L2TP_HDR_VERSION)
89 #define L2TP_DATA_HDR           (L2TP_HDR_VERSION)      /* optional: len, seq */
90
91 /* Some hard coded values */
92 #define L2TP_MAX_XWIN           128                     /* my max xmit window */
93 #define L2TP_MAX_REXMIT         5                       /* default max rexmit */
94 #define L2TP_MAX_REXMIT_TO      30                      /* default rexmit to */
95 #define L2TP_DELAYED_ACK        ((hz + 19) / 20)        /* delayed ack: 50 ms */
96
97 /* Default data sequence number configuration for new sessions */
98 #define L2TP_CONTROL_DSEQ       1                       /* we are the lns */
99 #define L2TP_ENABLE_DSEQ        1                       /* enable data seq # */
100
101 /* Compare sequence numbers using circular math */
102 #define L2TP_SEQ_DIFF(x, y)     ((int)((int16_t)(x) - (int16_t)(y)))
103
104 #define SESSHASHSIZE            0x0020
105 #define SESSHASH(x)             (((x) ^ ((x) >> 8)) & (SESSHASHSIZE - 1))
106
107 /* Hook private data (data session hooks only) */
108 struct ng_l2tp_hook_private {
109         struct ng_l2tp_sess_config      conf;   /* hook/session config */
110         struct ng_l2tp_session_stats    stats;  /* per sessions statistics */
111         hook_p                          hook;   /* hook reference */
112         u_int16_t                       ns;     /* data ns sequence number */
113         u_int16_t                       nr;     /* data nr sequence number */
114         LIST_ENTRY(ng_l2tp_hook_private) sessions;
115 };
116 typedef struct ng_l2tp_hook_private *hookpriv_p;
117
118 /*
119  * Sequence number state
120  *
121  * Invariants:
122  *    - If cwnd < ssth, we're doing slow start, otherwise congestion avoidance
123  *    - The number of unacknowledged xmit packets is (ns - rack) <= seq->wmax
124  *    - The first (ns - rack) mbuf's in xwin[] array are copies of these
125  *      unacknowledged packets; the remainder of xwin[] consists first of
126  *      zero or more further untransmitted packets in the transmit queue
127  *    - We try to keep the peer's receive window as full as possible.
128  *      Therefore, (i < cwnd && xwin[i] != NULL) implies (ns - rack) > i.
129  *    - rack_timer is running iff (ns - rack) > 0 (unack'd xmit'd pkts)
130  *    - If xack != nr, there are unacknowledged recv packet(s) (delayed ack)
131  *    - xack_timer is running iff xack != nr (unack'd rec'd pkts)
132  */
133 struct l2tp_seq {
134         u_int16_t               ns;             /* next xmit seq we send */
135         u_int16_t               nr;             /* next recv seq we expect */
136         u_int16_t               inproc;         /* packet is in processing */
137         u_int16_t               rack;           /* last 'nr' we rec'd */
138         u_int16_t               xack;           /* last 'nr' we sent */
139         u_int16_t               wmax;           /* peer's max recv window */
140         u_int16_t               cwnd;           /* current congestion window */
141         u_int16_t               ssth;           /* slow start threshold */
142         u_int16_t               acks;           /* # consecutive acks rec'd */
143         u_int16_t               rexmits;        /* # retransmits sent */
144         struct callout          rack_timer;     /* retransmit timer */
145         struct callout          xack_timer;     /* delayed ack timer */
146         struct mbuf             *xwin[L2TP_MAX_XWIN];   /* transmit window */
147         struct mtx              mtx;                    /* seq mutex */
148 };
149
150 /* Node private data */
151 struct ng_l2tp_private {
152         node_p                  node;           /* back pointer to node */
153         hook_p                  ctrl;           /* hook to upper layers */
154         hook_p                  lower;          /* hook to lower layers */
155         struct ng_l2tp_config   conf;           /* node configuration */
156         struct ng_l2tp_stats    stats;          /* node statistics */
157         struct l2tp_seq         seq;            /* ctrl sequence number state */
158         ng_ID_t                 ftarget;        /* failure message target */
159         LIST_HEAD(, ng_l2tp_hook_private) sesshash[SESSHASHSIZE];
160 };
161 typedef struct ng_l2tp_private *priv_p;
162
163 /* Netgraph node methods */
164 static ng_constructor_t ng_l2tp_constructor;
165 static ng_rcvmsg_t      ng_l2tp_rcvmsg;
166 static ng_shutdown_t    ng_l2tp_shutdown;
167 static ng_newhook_t     ng_l2tp_newhook;
168 static ng_rcvdata_t     ng_l2tp_rcvdata;
169 static ng_rcvdata_t     ng_l2tp_rcvdata_lower;
170 static ng_rcvdata_t     ng_l2tp_rcvdata_ctrl;
171 static ng_disconnect_t  ng_l2tp_disconnect;
172
173 /* Internal functions */
174 static int      ng_l2tp_xmit_ctrl(priv_p priv, struct mbuf *m, u_int16_t ns);
175
176 static void     ng_l2tp_seq_init(priv_p priv);
177 static int      ng_l2tp_seq_set(priv_p priv,
178                         const struct ng_l2tp_seq_config *conf);
179 static int      ng_l2tp_seq_adjust(priv_p priv,
180                         const struct ng_l2tp_config *conf);
181 static void     ng_l2tp_seq_reset(priv_p priv);
182 static void     ng_l2tp_seq_failure(priv_p priv);
183 static void     ng_l2tp_seq_recv_nr(priv_p priv, u_int16_t nr);
184 static void     ng_l2tp_seq_xack_timeout(node_p node, hook_p hook,
185                     void *arg1, int arg2);
186 static void     ng_l2tp_seq_rack_timeout(node_p node, hook_p hook,
187                     void *arg1, int arg2);
188
189 static hookpriv_p       ng_l2tp_find_session(priv_p privp, u_int16_t sid);
190 static ng_fn_eachhook   ng_l2tp_reset_session;
191
192 #ifdef INVARIANTS
193 static void     ng_l2tp_seq_check(struct l2tp_seq *seq);
194 #endif
195
196 /* Parse type for struct ng_l2tp_seq_config. */
197 static const struct ng_parse_struct_field
198         ng_l2tp_seq_config_fields[] = NG_L2TP_SEQ_CONFIG_TYPE_INFO;
199 static const struct ng_parse_type ng_l2tp_seq_config_type = {
200         &ng_parse_struct_type,
201         &ng_l2tp_seq_config_fields
202 };
203
204 /* Parse type for struct ng_l2tp_config */
205 static const struct ng_parse_struct_field
206         ng_l2tp_config_type_fields[] = NG_L2TP_CONFIG_TYPE_INFO;
207 static const struct ng_parse_type ng_l2tp_config_type = {
208         &ng_parse_struct_type,
209         &ng_l2tp_config_type_fields,
210 };
211
212 /* Parse type for struct ng_l2tp_sess_config */
213 static const struct ng_parse_struct_field
214         ng_l2tp_sess_config_type_fields[] = NG_L2TP_SESS_CONFIG_TYPE_INFO;
215 static const struct ng_parse_type ng_l2tp_sess_config_type = {
216         &ng_parse_struct_type,
217         &ng_l2tp_sess_config_type_fields,
218 };
219
220 /* Parse type for struct ng_l2tp_stats */
221 static const struct ng_parse_struct_field
222         ng_l2tp_stats_type_fields[] = NG_L2TP_STATS_TYPE_INFO;
223 static const struct ng_parse_type ng_l2tp_stats_type = {
224         &ng_parse_struct_type,
225         &ng_l2tp_stats_type_fields
226 };
227
228 /* Parse type for struct ng_l2tp_session_stats. */
229 static const struct ng_parse_struct_field
230         ng_l2tp_session_stats_type_fields[] = NG_L2TP_SESSION_STATS_TYPE_INFO;
231 static const struct ng_parse_type ng_l2tp_session_stats_type = {
232         &ng_parse_struct_type,
233         &ng_l2tp_session_stats_type_fields
234 };
235
236 /* List of commands and how to convert arguments to/from ASCII */
237 static const struct ng_cmdlist ng_l2tp_cmdlist[] = {
238         {
239           NGM_L2TP_COOKIE,
240           NGM_L2TP_SET_CONFIG,
241           "setconfig",
242           &ng_l2tp_config_type,
243           NULL
244         },
245         {
246           NGM_L2TP_COOKIE,
247           NGM_L2TP_GET_CONFIG,
248           "getconfig",
249           NULL,
250           &ng_l2tp_config_type
251         },
252         {
253           NGM_L2TP_COOKIE,
254           NGM_L2TP_SET_SESS_CONFIG,
255           "setsessconfig",
256           &ng_l2tp_sess_config_type,
257           NULL
258         },
259         {
260           NGM_L2TP_COOKIE,
261           NGM_L2TP_GET_SESS_CONFIG,
262           "getsessconfig",
263           &ng_parse_hint16_type,
264           &ng_l2tp_sess_config_type
265         },
266         {
267           NGM_L2TP_COOKIE,
268           NGM_L2TP_GET_STATS,
269           "getstats",
270           NULL,
271           &ng_l2tp_stats_type
272         },
273         {
274           NGM_L2TP_COOKIE,
275           NGM_L2TP_CLR_STATS,
276           "clrstats",
277           NULL,
278           NULL
279         },
280         {
281           NGM_L2TP_COOKIE,
282           NGM_L2TP_GETCLR_STATS,
283           "getclrstats",
284           NULL,
285           &ng_l2tp_stats_type
286         },
287         {
288           NGM_L2TP_COOKIE,
289           NGM_L2TP_GET_SESSION_STATS,
290           "getsessstats",
291           &ng_parse_int16_type,
292           &ng_l2tp_session_stats_type
293         },
294         {
295           NGM_L2TP_COOKIE,
296           NGM_L2TP_CLR_SESSION_STATS,
297           "clrsessstats",
298           &ng_parse_int16_type,
299           NULL
300         },
301         {
302           NGM_L2TP_COOKIE,
303           NGM_L2TP_GETCLR_SESSION_STATS,
304           "getclrsessstats",
305           &ng_parse_int16_type,
306           &ng_l2tp_session_stats_type
307         },
308         {
309           NGM_L2TP_COOKIE,
310           NGM_L2TP_ACK_FAILURE,
311           "ackfailure",
312           NULL,
313           NULL
314         },
315         {
316           NGM_L2TP_COOKIE,
317           NGM_L2TP_SET_SEQ,
318           "setsequence",
319           &ng_l2tp_seq_config_type,
320           NULL
321         },
322         { 0 }
323 };
324
325 /* Node type descriptor */
326 static struct ng_type ng_l2tp_typestruct = {
327         .version =      NG_ABI_VERSION,
328         .name =         NG_L2TP_NODE_TYPE,
329         .constructor =  ng_l2tp_constructor,
330         .rcvmsg =       ng_l2tp_rcvmsg,
331         .shutdown =     ng_l2tp_shutdown,
332         .newhook =      ng_l2tp_newhook,
333         .rcvdata =      ng_l2tp_rcvdata,
334         .disconnect =   ng_l2tp_disconnect,
335         .cmdlist =      ng_l2tp_cmdlist,
336 };
337 NETGRAPH_INIT(l2tp, &ng_l2tp_typestruct);
338
339 /* Sequence number state sanity checking */
340 #ifdef INVARIANTS
341 #define L2TP_SEQ_CHECK(seq)     ng_l2tp_seq_check(seq)
342 #else
343 #define L2TP_SEQ_CHECK(x)       do { } while (0)
344 #endif
345
346 /* memmove macro */
347 #define memmove(d, s, l)        bcopy(s, d, l)
348
349 /* Whether to use m_copypacket() or m_dup() */
350 #define L2TP_COPY_MBUF          m_copypacket
351
352 #define ERROUT(x)       do { error = (x); goto done; } while (0)
353
354 /************************************************************************
355                         NETGRAPH NODE STUFF
356 ************************************************************************/
357
358 /*
359  * Node type constructor
360  */
361 static int
362 ng_l2tp_constructor(node_p node)
363 {
364         priv_p priv;
365         int     i;
366
367         /* Allocate private structure */
368         priv = kmalloc(sizeof(*priv), M_NETGRAPH_L2TP,
369                        M_WAITOK | M_NULLOK | M_ZERO);
370         if (priv == NULL)
371                 return (ENOMEM);
372         NG_NODE_SET_PRIVATE(node, priv);
373         priv->node = node;
374
375         /* Apply a semi-reasonable default configuration */
376         priv->conf.peer_win = 1;
377         priv->conf.rexmit_max = L2TP_MAX_REXMIT;
378         priv->conf.rexmit_max_to = L2TP_MAX_REXMIT_TO;
379
380         /* Initialize sequence number state */
381         ng_l2tp_seq_init(priv);
382
383         for (i = 0; i < SESSHASHSIZE; i++)
384             LIST_INIT(&priv->sesshash[i]);
385
386         /* Done */
387         return (0);
388 }
389
390 /*
391  * Give our OK for a hook to be added.
392  */
393 static int
394 ng_l2tp_newhook(node_p node, hook_p hook, const char *name)
395 {
396         const priv_p priv = NG_NODE_PRIVATE(node);
397
398         /* Check hook name */
399         if (strcmp(name, NG_L2TP_HOOK_CTRL) == 0) {
400                 if (priv->ctrl != NULL)
401                         return (EISCONN);
402                 priv->ctrl = hook;
403                 NG_HOOK_SET_RCVDATA(hook, ng_l2tp_rcvdata_ctrl);
404         } else if (strcmp(name, NG_L2TP_HOOK_LOWER) == 0) {
405                 if (priv->lower != NULL)
406                         return (EISCONN);
407                 priv->lower = hook;
408                 NG_HOOK_SET_RCVDATA(hook, ng_l2tp_rcvdata_lower);
409         } else {
410                 static const char hexdig[16] = "0123456789abcdef";
411                 u_int16_t session_id;
412                 hookpriv_p hpriv;
413                 uint16_t hash;
414                 const char *hex;
415                 int i;
416                 int j;
417
418                 /* Parse hook name to get session ID */
419                 if (strncmp(name, NG_L2TP_HOOK_SESSION_P,
420                     sizeof(NG_L2TP_HOOK_SESSION_P) - 1) != 0)
421                         return (EINVAL);
422                 hex = name + sizeof(NG_L2TP_HOOK_SESSION_P) - 1;
423                 for (session_id = i = 0; i < 4; i++) {
424                         for (j = 0; j < 16 && hex[i] != hexdig[j]; j++);
425                         if (j == 16)
426                                 return (EINVAL);
427                         session_id = (session_id << 4) | j;
428                 }
429                 if (hex[i] != '\0')
430                         return (EINVAL);
431
432                 /* Create hook private structure */
433                 hpriv = kmalloc(sizeof(*hpriv), M_NETGRAPH_L2TP,
434                                 M_WAITOK | M_NULLOK | M_ZERO);
435                 if (hpriv == NULL)
436                         return (ENOMEM);
437                 hpriv->conf.session_id = htons(session_id);
438                 hpriv->conf.control_dseq = L2TP_CONTROL_DSEQ;
439                 hpriv->conf.enable_dseq = L2TP_ENABLE_DSEQ;
440                 hpriv->hook = hook;
441                 NG_HOOK_SET_PRIVATE(hook, hpriv);
442                 hash = SESSHASH(hpriv->conf.session_id);
443                 LIST_INSERT_HEAD(&priv->sesshash[hash], hpriv, sessions);
444         }
445
446         /* Done */
447         return (0);
448 }
449
450 /*
451  * Receive a control message.
452  */
453 static int
454 ng_l2tp_rcvmsg(node_p node, item_p item, hook_p lasthook)
455 {
456         const priv_p priv = NG_NODE_PRIVATE(node);
457         struct ng_mesg *resp = NULL;
458         struct ng_mesg *msg;
459         int error = 0;
460
461         NGI_GET_MSG(item, msg);
462         switch (msg->header.typecookie) {
463         case NGM_L2TP_COOKIE:
464                 switch (msg->header.cmd) {
465                 case NGM_L2TP_SET_CONFIG:
466                     {
467                         struct ng_l2tp_config *const conf =
468                                 (struct ng_l2tp_config *)msg->data;
469
470                         /* Check for invalid or illegal config */
471                         if (msg->header.arglen != sizeof(*conf)) {
472                                 error = EINVAL;
473                                 break;
474                         }
475                         conf->enabled = !!conf->enabled;
476                         conf->match_id = !!conf->match_id;
477                         conf->tunnel_id = htons(conf->tunnel_id);
478                         conf->peer_id = htons(conf->peer_id);
479                         if (priv->conf.enabled
480                             && ((priv->conf.tunnel_id != 0
481                                && conf->tunnel_id != priv->conf.tunnel_id)
482                               || ((priv->conf.peer_id != 0
483                                && conf->peer_id != priv->conf.peer_id)))) {
484                                 error = EBUSY;
485                                 break;
486                         }
487
488                         /* Save calling node as failure target */
489                         priv->ftarget = NGI_RETADDR(item);
490
491                         /* Adjust sequence number state */
492                         if ((error = ng_l2tp_seq_adjust(priv, conf)) != 0)
493                                 break;
494
495                         /* Update node's config */
496                         priv->conf = *conf;
497                         break;
498                     }
499                 case NGM_L2TP_GET_CONFIG:
500                     {
501                         struct ng_l2tp_config *conf;
502
503                         NG_MKRESPONSE(resp, msg, sizeof(*conf), M_WAITOK | M_NULLOK);
504                         if (resp == NULL) {
505                                 error = ENOMEM;
506                                 break;
507                         }
508                         conf = (struct ng_l2tp_config *)resp->data;
509                         *conf = priv->conf;
510
511                         /* Put ID's in host order */
512                         conf->tunnel_id = ntohs(conf->tunnel_id);
513                         conf->peer_id = ntohs(conf->peer_id);
514                         break;
515                     }
516                 case NGM_L2TP_SET_SESS_CONFIG:
517                     {
518                         struct ng_l2tp_sess_config *const conf =
519                             (struct ng_l2tp_sess_config *)msg->data;
520                         hookpriv_p hpriv;
521
522                         /* Check for invalid or illegal config. */
523                         if (msg->header.arglen != sizeof(*conf)) {
524                                 error = EINVAL;
525                                 break;
526                         }
527
528                         /* Put ID's in network order */
529                         conf->session_id = htons(conf->session_id);
530                         conf->peer_id = htons(conf->peer_id);
531
532                         /* Find matching hook */
533                         hpriv = ng_l2tp_find_session(priv, conf->session_id);
534                         if (hpriv == NULL) {
535                                 error = ENOENT;
536                                 break;
537                         }
538
539                         /* Update hook's config */
540                         hpriv->conf = *conf;
541                         break;
542                     }
543                 case NGM_L2TP_GET_SESS_CONFIG:
544                     {
545                         struct ng_l2tp_sess_config *conf;
546                         u_int16_t session_id;
547                         hookpriv_p hpriv;
548
549                         /* Get session ID */
550                         if (msg->header.arglen != sizeof(session_id)) {
551                                 error = EINVAL;
552                                 break;
553                         }
554                         memcpy(&session_id, msg->data, 2);
555                         session_id = htons(session_id);
556
557                         /* Find matching hook */
558                         hpriv = ng_l2tp_find_session(priv, session_id);
559                         if (hpriv == NULL) {
560                                 error = ENOENT;
561                                 break;
562                         }
563
564                         /* Send response */
565                         NG_MKRESPONSE(resp, msg, sizeof(hpriv->conf), M_WAITOK | M_NULLOK);
566                         if (resp == NULL) {
567                                 error = ENOMEM;
568                                 break;
569                         }
570                         conf = (struct ng_l2tp_sess_config *)resp->data;
571                         *conf = hpriv->conf;
572
573                         /* Put ID's in host order */
574                         conf->session_id = ntohs(conf->session_id);
575                         conf->peer_id = ntohs(conf->peer_id);
576                         break;
577                     }
578                 case NGM_L2TP_GET_STATS:
579                 case NGM_L2TP_CLR_STATS:
580                 case NGM_L2TP_GETCLR_STATS:
581                     {
582                         if (msg->header.cmd != NGM_L2TP_CLR_STATS) {
583                                 NG_MKRESPONSE(resp, msg,
584                                     sizeof(priv->stats), M_WAITOK | M_NULLOK);
585                                 if (resp == NULL) {
586                                         error = ENOMEM;
587                                         break;
588                                 }
589                                 memcpy(resp->data,
590                                     &priv->stats, sizeof(priv->stats));
591                         }
592                         if (msg->header.cmd != NGM_L2TP_GET_STATS)
593                                 memset(&priv->stats, 0, sizeof(priv->stats));
594                         break;
595                     }
596                 case NGM_L2TP_GET_SESSION_STATS:
597                 case NGM_L2TP_CLR_SESSION_STATS:
598                 case NGM_L2TP_GETCLR_SESSION_STATS:
599                     {
600                         uint16_t session_id;
601                         hookpriv_p hpriv;
602
603                         /* Get session ID. */
604                         if (msg->header.arglen != sizeof(session_id)) {
605                                 error = EINVAL;
606                                 break;
607                         }
608                         bcopy(msg->data, &session_id, sizeof(uint16_t));
609                         session_id = htons(session_id);
610
611                         /* Find matching hook. */
612                         hpriv = ng_l2tp_find_session(priv, session_id);
613                         if (hpriv == NULL) {
614                                 error = ENOENT;
615                                 break;
616                         }
617
618                         if (msg->header.cmd != NGM_L2TP_CLR_SESSION_STATS) {
619                                 NG_MKRESPONSE(resp, msg,
620                                     sizeof(hpriv->stats), M_WAITOK | M_NULLOK);
621                                 if (resp == NULL) {
622                                         error = ENOMEM;
623                                         break;
624                                 }
625                                 bcopy(&hpriv->stats, resp->data,
626                                         sizeof(hpriv->stats));
627                         }
628                         if (msg->header.cmd != NGM_L2TP_GET_SESSION_STATS)
629                                 bzero(&hpriv->stats, sizeof(hpriv->stats));
630                         break;
631                     }
632                 case NGM_L2TP_SET_SEQ:
633                     {
634                         struct ng_l2tp_seq_config *const conf =
635                                 (struct ng_l2tp_seq_config *)msg->data;
636
637                         /* Check for invalid or illegal seq config. */
638                         if (msg->header.arglen != sizeof(*conf)) {
639                                 error = EINVAL;
640                                 break;
641                         }
642                         conf->ns = htons(conf->ns);
643                         conf->nr = htons(conf->nr);
644                         conf->rack = htons(conf->rack);
645                         conf->xack = htons(conf->xack);
646
647                         /* Set sequence numbers. */
648                         error = ng_l2tp_seq_set(priv, conf);
649                         break;
650                     }
651                 default:
652                         error = EINVAL;
653                         break;
654                 }
655                 break;
656         default:
657                 error = EINVAL;
658                 break;
659         }
660
661         /* Done */
662         NG_RESPOND_MSG(error, node, item, resp);
663         NG_FREE_MSG(msg);
664         return (error);
665 }
666
667 /*
668  * Destroy node
669  */
670 static int
671 ng_l2tp_shutdown(node_p node)
672 {
673         const priv_p priv = NG_NODE_PRIVATE(node);
674         struct l2tp_seq *const seq = &priv->seq;
675
676         /* Sanity check */
677         L2TP_SEQ_CHECK(seq);
678
679         /* Reset sequence number state */
680         ng_l2tp_seq_reset(priv);
681
682         /* Free private data if neither timer is running */
683         ng_uncallout(&seq->rack_timer, node);
684         ng_uncallout(&seq->xack_timer, node);
685
686         mtx_destroy(&seq->mtx);
687
688         kfree(priv, M_NETGRAPH_L2TP);
689
690         /* Unref node */
691         NG_NODE_UNREF(node);
692         return (0);
693 }
694
695 /*
696  * Hook disconnection
697  */
698 static int
699 ng_l2tp_disconnect(hook_p hook)
700 {
701         const node_p node = NG_HOOK_NODE(hook);
702         const priv_p priv = NG_NODE_PRIVATE(node);
703
704         /* Zero out hook pointer */
705         if (hook == priv->ctrl)
706                 priv->ctrl = NULL;
707         else if (hook == priv->lower)
708                 priv->lower = NULL;
709         else {
710                 const hookpriv_p hpriv = NG_HOOK_PRIVATE(hook);
711                 LIST_REMOVE(hpriv, sessions);
712                 kfree(hpriv, M_NETGRAPH_L2TP);
713                 NG_HOOK_SET_PRIVATE(hook, NULL);
714         }
715
716         /* Go away if no longer connected to anything */
717         if (NG_NODE_NUMHOOKS(node) == 0 && NG_NODE_IS_VALID(node))
718                 ng_rmnode_self(node);
719         return (0);
720 }
721
722 /*************************************************************************
723                         INTERNAL FUNCTIONS
724 *************************************************************************/
725
726 /*
727  * Find the hook with a given session ID (in network order).
728  */
729 static hookpriv_p
730 ng_l2tp_find_session(priv_p privp, u_int16_t sid)
731 {
732         uint16_t        hash = SESSHASH(sid);
733         hookpriv_p      hpriv = NULL;
734
735         LIST_FOREACH(hpriv, &privp->sesshash[hash], sessions) {
736                 if (hpriv->conf.session_id == sid)
737                         break;
738         }
739
740         return (hpriv);
741 }
742
743 /*
744  * Reset a hook's session state.
745  */
746 static int
747 ng_l2tp_reset_session(hook_p hook, void *arg)
748 {
749         const hookpriv_p hpriv = NG_HOOK_PRIVATE(hook);
750
751         if (hpriv != NULL) {
752                 hpriv->conf.control_dseq = 0;
753                 hpriv->conf.enable_dseq = 0;
754                 bzero(&hpriv->conf, sizeof(struct ng_l2tp_session_stats));
755                 hpriv->nr = 0;
756                 hpriv->ns = 0;
757         }
758         return (-1);
759 }
760
761 /*
762  * Handle an incoming frame from below.
763  */
764 static int
765 ng_l2tp_rcvdata_lower(hook_p h, item_p item)
766 {
767         static const u_int16_t req_bits[2][2] = {
768                 { L2TP_DATA_0BITS, L2TP_DATA_1BITS },
769                 { L2TP_CTRL_0BITS, L2TP_CTRL_1BITS },
770         };
771         const node_p node = NG_HOOK_NODE(h);
772         const priv_p priv = NG_NODE_PRIVATE(node);
773         hookpriv_p hpriv = NULL;
774         hook_p hook = NULL;
775         u_int16_t ids[2];
776         struct mbuf *m;
777         u_int16_t hdr;
778         u_int16_t ns;
779         u_int16_t nr;
780         int is_ctrl;
781         int error;
782         int len, plen;
783
784         /* Sanity check */
785         L2TP_SEQ_CHECK(&priv->seq);
786
787         /* If not configured, reject */
788         if (!priv->conf.enabled) {
789                 NG_FREE_ITEM(item);
790                 ERROUT(ENXIO);
791         }
792
793         /* Grab mbuf */
794         NGI_GET_M(item, m);
795
796         /* Remember full packet length; needed for per session accounting. */
797         plen = m->m_pkthdr.len;
798
799         /* Update stats */
800         priv->stats.recvPackets++;
801         priv->stats.recvOctets += plen;
802
803         /* Get initial header */
804         if (m->m_pkthdr.len < 6) {
805                 priv->stats.recvRunts++;
806                 NG_FREE_ITEM(item);
807                 NG_FREE_M(m);
808                 ERROUT(EINVAL);
809         }
810         if (m->m_len < 2 && (m = m_pullup(m, 2)) == NULL) {
811                 priv->stats.memoryFailures++;
812                 NG_FREE_ITEM(item);
813                 ERROUT(EINVAL);
814         }
815         hdr = ntohs(*mtod(m, u_int16_t *));
816         m_adj(m, 2);
817
818         /* Check required header bits and minimum length */
819         is_ctrl = (hdr & L2TP_HDR_CTRL) != 0;
820         if ((hdr & req_bits[is_ctrl][0]) != 0
821             || (~hdr & req_bits[is_ctrl][1]) != 0) {
822                 priv->stats.recvInvalid++;
823                 NG_FREE_ITEM(item);
824                 NG_FREE_M(m);
825                 ERROUT(EINVAL);
826         }
827         if (m->m_pkthdr.len < 4                         /* tunnel, session id */
828             + (2 * ((hdr & L2TP_HDR_LEN) != 0))         /* length field */
829             + (4 * ((hdr & L2TP_HDR_SEQ) != 0))         /* seq # fields */
830             + (2 * ((hdr & L2TP_HDR_OFF) != 0))) {      /* offset field */
831                 priv->stats.recvRunts++;
832                 NG_FREE_ITEM(item);
833                 NG_FREE_M(m);
834                 ERROUT(EINVAL);
835         }
836
837         /* Get and validate length field if present */
838         if ((hdr & L2TP_HDR_LEN) != 0) {
839                 if (m->m_len < 2 && (m = m_pullup(m, 2)) == NULL) {
840                         priv->stats.memoryFailures++;
841                         NG_FREE_ITEM(item);
842                         ERROUT(EINVAL);
843                 }
844                 len = (u_int16_t)ntohs(*mtod(m, u_int16_t *)) - 4;
845                 m_adj(m, 2);
846                 if (len < 0 || len > m->m_pkthdr.len) {
847                         priv->stats.recvInvalid++;
848                         NG_FREE_ITEM(item);
849                         NG_FREE_M(m);
850                         ERROUT(EINVAL);
851                 }
852                 if (len < m->m_pkthdr.len)              /* trim extra bytes */
853                         m_adj(m, -(m->m_pkthdr.len - len));
854         }
855
856         /* Get tunnel ID and session ID */
857         if (m->m_len < 4 && (m = m_pullup(m, 4)) == NULL) {
858                 priv->stats.memoryFailures++;
859                 NG_FREE_ITEM(item);
860                 ERROUT(EINVAL);
861         }
862         memcpy(ids, mtod(m, u_int16_t *), 4);
863         m_adj(m, 4);
864
865         /* Check tunnel ID */
866         if (ids[0] != priv->conf.tunnel_id
867             && (priv->conf.match_id || ids[0] != 0)) {
868                 priv->stats.recvWrongTunnel++;
869                 NG_FREE_ITEM(item);
870                 NG_FREE_M(m);
871                 ERROUT(EADDRNOTAVAIL);
872         }
873
874         /* Check session ID (for data packets only) */
875         if ((hdr & L2TP_HDR_CTRL) == 0) {
876                 hpriv = ng_l2tp_find_session(priv, ids[1]);
877                 if (hpriv == NULL) {
878                         priv->stats.recvUnknownSID++;
879                         NG_FREE_ITEM(item);
880                         NG_FREE_M(m);
881                         ERROUT(ENOTCONN);
882                 }
883                 hook = hpriv->hook;
884         }
885
886         /* Get Ns, Nr fields if present */
887         if ((hdr & L2TP_HDR_SEQ) != 0) {
888                 if (m->m_len < 4 && (m = m_pullup(m, 4)) == NULL) {
889                         priv->stats.memoryFailures++;
890                         NG_FREE_ITEM(item);
891                         ERROUT(EINVAL);
892                 }
893                 memcpy(&ns, &mtod(m, u_int16_t *)[0], 2);
894                 ns = ntohs(ns);
895                 memcpy(&nr, &mtod(m, u_int16_t *)[1], 2);
896                 nr = ntohs(nr);
897                 m_adj(m, 4);
898         }
899
900         /* Strip offset padding if present */
901         if ((hdr & L2TP_HDR_OFF) != 0) {
902                 u_int16_t offset;
903
904                 /* Get length of offset padding */
905                 if (m->m_len < 2 && (m = m_pullup(m, 2)) == NULL) {
906                         priv->stats.memoryFailures++;
907                         NG_FREE_ITEM(item);
908                         ERROUT(EINVAL);
909                 }
910                 memcpy(&offset, mtod(m, u_int16_t *), 2);
911                 offset = ntohs(offset);
912
913                 /* Trim offset padding */
914                 if ((2+offset) > m->m_pkthdr.len) {
915                         priv->stats.recvInvalid++;
916                         NG_FREE_ITEM(item);
917                         NG_FREE_M(m);
918                         ERROUT(EINVAL);
919                 }
920                 m_adj(m, 2+offset);
921         }
922
923         /* Handle control packets */
924         if ((hdr & L2TP_HDR_CTRL) != 0) {
925                 struct l2tp_seq *const seq = &priv->seq;
926
927                 /* Handle receive ack sequence number Nr */
928                 ng_l2tp_seq_recv_nr(priv, nr);
929
930                 /* Discard ZLB packets */
931                 if (m->m_pkthdr.len == 0) {
932                         priv->stats.recvZLBs++;
933                         NG_FREE_ITEM(item);
934                         NG_FREE_M(m);
935                         ERROUT(0);
936                 }
937
938                 mtx_lock(&seq->mtx);
939                 /*
940                  * If not what we expect or we are busy, drop packet and
941                  * send an immediate ZLB ack.
942                  */
943                 if (ns != seq->nr || seq->inproc) {
944                         if (L2TP_SEQ_DIFF(ns, seq->nr) <= 0)
945                                 priv->stats.recvDuplicates++;
946                         else
947                                 priv->stats.recvOutOfOrder++;
948                         mtx_unlock(&seq->mtx);
949                         ng_l2tp_xmit_ctrl(priv, NULL, seq->ns);
950                         NG_FREE_ITEM(item);
951                         NG_FREE_M(m);
952                         ERROUT(0);
953                 }
954                 /*
955                  * Until we deliver this packet we can't receive next one as
956                  * we have no information for sending ack.
957                  */
958                 seq->inproc = 1;
959                 mtx_unlock(&seq->mtx);
960
961                 /* Prepend session ID to packet. */
962                 M_PREPEND(m, 2, MB_DONTWAIT);
963                 if (m == NULL) {
964                         seq->inproc = 0;
965                         priv->stats.memoryFailures++;
966                         NG_FREE_ITEM(item);
967                         ERROUT(ENOBUFS);
968                 }
969                 memcpy(mtod(m, u_int16_t *), &ids[1], 2);
970
971                 /* Deliver packet to upper layers */
972                 NG_FWD_NEW_DATA(error, item, priv->ctrl, m);
973                 
974                 mtx_lock(&seq->mtx);
975                 /* Ready to process next packet. */
976                 seq->inproc = 0;
977
978                 /* If packet was successfully delivered send ack. */
979                 if (error == 0) {
980                         /* Update recv sequence number */
981                         seq->nr++;
982                         /* Start receive ack timer, if not already running */
983                         if (!callout_active(&seq->xack_timer)) {
984                                 ng_callout(&seq->xack_timer, priv->node, NULL,
985                                     L2TP_DELAYED_ACK, ng_l2tp_seq_xack_timeout,
986                                     NULL, 0);
987                         }
988                 }
989                 mtx_unlock(&seq->mtx);
990
991                 ERROUT(error);
992         }
993
994         /* Per session packet, account it. */
995         hpriv->stats.recvPackets++;
996         hpriv->stats.recvOctets += plen;
997
998         /* Follow peer's lead in data sequencing, if configured to do so */
999         if (!hpriv->conf.control_dseq)
1000                 hpriv->conf.enable_dseq = ((hdr & L2TP_HDR_SEQ) != 0);
1001
1002         /* Handle data sequence numbers if present and enabled */
1003         if ((hdr & L2TP_HDR_SEQ) != 0) {
1004                 if (hpriv->conf.enable_dseq
1005                     && L2TP_SEQ_DIFF(ns, hpriv->nr) < 0) {
1006                         NG_FREE_ITEM(item);     /* duplicate or out of order */
1007                         NG_FREE_M(m);
1008                         priv->stats.recvDataDrops++;
1009                         ERROUT(0);
1010                 }
1011                 hpriv->nr = ns + 1;
1012         }
1013
1014         /* Drop empty data packets */
1015         if (m->m_pkthdr.len == 0) {
1016                 NG_FREE_ITEM(item);
1017                 NG_FREE_M(m);
1018                 ERROUT(0);
1019         }
1020
1021         /* Deliver data */
1022         NG_FWD_NEW_DATA(error, item, hook, m);
1023 done:
1024         /* Done */
1025         L2TP_SEQ_CHECK(&priv->seq);
1026         return (error);
1027 }
1028
1029 /*
1030  * Handle an outgoing control frame.
1031  */
1032 static int
1033 ng_l2tp_rcvdata_ctrl(hook_p hook, item_p item)
1034 {
1035         const node_p node = NG_HOOK_NODE(hook);
1036         const priv_p priv = NG_NODE_PRIVATE(node);
1037         struct l2tp_seq *const seq = &priv->seq;
1038         struct mbuf *m;
1039         int error;
1040         int i;
1041         u_int16_t       ns;
1042
1043         /* Sanity check */
1044         L2TP_SEQ_CHECK(&priv->seq);
1045
1046         /* If not configured, reject */
1047         if (!priv->conf.enabled) {
1048                 NG_FREE_ITEM(item);
1049                 ERROUT(ENXIO);
1050         }
1051
1052         /* Grab mbuf and discard other stuff XXX */
1053         NGI_GET_M(item, m);
1054         NG_FREE_ITEM(item);
1055
1056         /* Packet should have session ID prepended */
1057         if (m->m_pkthdr.len < 2) {
1058                 priv->stats.xmitInvalid++;
1059                 m_freem(m);
1060                 ERROUT(EINVAL);
1061         }
1062
1063         /* Check max length */
1064         if (m->m_pkthdr.len >= 0x10000 - 14) {
1065                 priv->stats.xmitTooBig++;
1066                 m_freem(m);
1067                 ERROUT(EOVERFLOW);
1068         }
1069
1070         mtx_lock(&seq->mtx);
1071
1072         /* Find next empty slot in transmit queue */
1073         for (i = 0; i < L2TP_MAX_XWIN && seq->xwin[i] != NULL; i++);
1074         if (i == L2TP_MAX_XWIN) {
1075                 mtx_unlock(&seq->mtx);
1076                 priv->stats.xmitDrops++;
1077                 m_freem(m);
1078                 ERROUT(ENOBUFS);
1079         }
1080         seq->xwin[i] = m;
1081
1082         /* If peer's receive window is already full, nothing else to do */
1083         if (i >= seq->cwnd) {
1084                 mtx_unlock(&seq->mtx);
1085                 ERROUT(0);
1086         }
1087
1088         /* Start retransmit timer if not already running */
1089         if (!callout_active(&seq->rack_timer))
1090                 ng_callout(&seq->rack_timer, node, NULL,
1091                     hz, ng_l2tp_seq_rack_timeout, NULL, 0);
1092         
1093         ns = seq->ns++;
1094         
1095         mtx_unlock(&seq->mtx);
1096
1097         /* Copy packet */
1098         if ((m = L2TP_COPY_MBUF(m, MB_DONTWAIT)) == NULL) {
1099                 priv->stats.memoryFailures++;
1100                 ERROUT(ENOBUFS);
1101         }
1102
1103         /* Send packet and increment xmit sequence number */
1104         error = ng_l2tp_xmit_ctrl(priv, m, ns);
1105 done:
1106         /* Done */
1107         L2TP_SEQ_CHECK(&priv->seq);
1108         return (error);
1109 }
1110
1111 /*
1112  * Handle an outgoing data frame.
1113  */
1114 static int
1115 ng_l2tp_rcvdata(hook_p hook, item_p item)
1116 {
1117         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
1118         const hookpriv_p hpriv = NG_HOOK_PRIVATE(hook);
1119         struct mbuf *m;
1120         u_int16_t hdr;
1121         int error;
1122         int i = 1;
1123
1124         /* Sanity check */
1125         L2TP_SEQ_CHECK(&priv->seq);
1126
1127         /* If not configured, reject */
1128         if (!priv->conf.enabled) {
1129                 NG_FREE_ITEM(item);
1130                 ERROUT(ENXIO);
1131         }
1132
1133         /* Get mbuf */
1134         NGI_GET_M(item, m);
1135
1136         /* Check max length */
1137         if (m->m_pkthdr.len >= 0x10000 - 12) {
1138                 priv->stats.xmitDataTooBig++;
1139                 NG_FREE_ITEM(item);
1140                 NG_FREE_M(m);
1141                 ERROUT(EOVERFLOW);
1142         }
1143
1144         /* Prepend L2TP header */
1145         M_PREPEND(m, 6
1146             + (2 * (hpriv->conf.include_length != 0))
1147             + (4 * (hpriv->conf.enable_dseq != 0)),
1148             MB_DONTWAIT);
1149         if (m == NULL) {
1150                 priv->stats.memoryFailures++;
1151                 NG_FREE_ITEM(item);
1152                 ERROUT(ENOBUFS);
1153         }
1154         hdr = L2TP_DATA_HDR;
1155         if (hpriv->conf.include_length) {
1156                 hdr |= L2TP_HDR_LEN;
1157                 mtod(m, u_int16_t *)[i++] = htons(m->m_pkthdr.len);
1158         }
1159         mtod(m, u_int16_t *)[i++] = priv->conf.peer_id;
1160         mtod(m, u_int16_t *)[i++] = hpriv->conf.peer_id;
1161         if (hpriv->conf.enable_dseq) {
1162                 hdr |= L2TP_HDR_SEQ;
1163                 mtod(m, u_int16_t *)[i++] = htons(hpriv->ns);
1164                 mtod(m, u_int16_t *)[i++] = htons(hpriv->nr);
1165                 hpriv->ns++;
1166         }
1167         mtod(m, u_int16_t *)[0] = htons(hdr);
1168
1169         /* Update per session stats. */
1170         hpriv->stats.xmitPackets++;
1171         hpriv->stats.xmitOctets += m->m_pkthdr.len;
1172
1173         /* And the global one. */
1174         priv->stats.xmitPackets++;
1175         priv->stats.xmitOctets += m->m_pkthdr.len;
1176
1177         /* Send packet */
1178         NG_FWD_NEW_DATA(error, item, priv->lower, m);
1179 done:
1180         /* Done */
1181         L2TP_SEQ_CHECK(&priv->seq);
1182         return (error);
1183 }
1184
1185 /*
1186  * Send a message to our controlling node that we've failed.
1187  */
1188 static void
1189 ng_l2tp_seq_failure(priv_p priv)
1190 {
1191         struct ng_mesg *msg;
1192         int error;
1193
1194         NG_MKMESSAGE(msg, NGM_L2TP_COOKIE, NGM_L2TP_ACK_FAILURE, 0, M_WAITOK | M_NULLOK);
1195         if (msg == NULL)
1196                 return;
1197         NG_SEND_MSG_ID(error, priv->node, msg, priv->ftarget, 0);
1198 }
1199
1200 /************************************************************************
1201                         SEQUENCE NUMBER HANDLING
1202 ************************************************************************/
1203
1204 /*
1205  * Initialize sequence number state.
1206  */
1207 static void
1208 ng_l2tp_seq_init(priv_p priv)
1209 {
1210         struct l2tp_seq *const seq = &priv->seq;
1211
1212         KASSERT(priv->conf.peer_win >= 1,
1213             ("%s: peer_win is zero", __func__));
1214         memset(seq, 0, sizeof(*seq));
1215         seq->cwnd = 1;
1216         seq->wmax = priv->conf.peer_win;
1217         if (seq->wmax > L2TP_MAX_XWIN)
1218                 seq->wmax = L2TP_MAX_XWIN;
1219         seq->ssth = seq->wmax;
1220         ng_callout_init(&seq->rack_timer);
1221         ng_callout_init(&seq->xack_timer);
1222         mtx_init(&seq->mtx, "ng_l2tp", NULL, MTX_DEF);
1223         L2TP_SEQ_CHECK(seq);
1224 }
1225
1226 /*
1227  * Set sequence number state as given from user.
1228  */
1229 static int
1230 ng_l2tp_seq_set(priv_p priv, const struct ng_l2tp_seq_config *conf)
1231 {
1232         struct l2tp_seq *const seq = &priv->seq;
1233
1234         /* If node is enabled, deny update to sequence numbers. */
1235         if (priv->conf.enabled)
1236                 return (EBUSY);
1237
1238         /* We only can handle the simple cases. */
1239         if (conf->xack != conf->nr || conf->ns != conf->rack)
1240                 return (EINVAL);
1241
1242         /* Set ns,nr,rack,xack parameters. */
1243         seq->ns = conf->ns;
1244         seq->nr = conf->nr;
1245         seq->rack = conf->rack;
1246         seq->xack = conf->xack;
1247
1248         return (0);
1249 }
1250
1251 /*
1252  * Adjust sequence number state accordingly after reconfiguration.
1253  */
1254 static int
1255 ng_l2tp_seq_adjust(priv_p priv, const struct ng_l2tp_config *conf)
1256 {
1257         struct l2tp_seq *const seq = &priv->seq;
1258         u_int16_t new_wmax;
1259
1260         /* If disabling node, reset state sequence number */
1261         if (!conf->enabled) {
1262                 ng_l2tp_seq_reset(priv);
1263                 return (0);
1264         }
1265
1266         /* Adjust peer's max recv window; it can only increase */
1267         new_wmax = conf->peer_win;
1268         if (new_wmax > L2TP_MAX_XWIN)
1269                 new_wmax = L2TP_MAX_XWIN;
1270         if (new_wmax == 0)
1271                 return (EINVAL);
1272         if (new_wmax < seq->wmax)
1273                 return (EBUSY);
1274         seq->wmax = new_wmax;
1275
1276         /* Done */
1277         return (0);
1278 }
1279
1280 /*
1281  * Reset sequence number state.
1282  */
1283 static void
1284 ng_l2tp_seq_reset(priv_p priv)
1285 {
1286         struct l2tp_seq *const seq = &priv->seq;
1287         hook_p hook;
1288         int i;
1289
1290         /* Sanity check */
1291         L2TP_SEQ_CHECK(seq);
1292
1293         /* Stop timers */
1294         ng_uncallout(&seq->rack_timer, priv->node);
1295         ng_uncallout(&seq->xack_timer, priv->node);
1296
1297         /* Free retransmit queue */
1298         for (i = 0; i < L2TP_MAX_XWIN; i++) {
1299                 if (seq->xwin[i] == NULL)
1300                         break;
1301                 m_freem(seq->xwin[i]);
1302         }
1303
1304         /* Reset session hooks' sequence number states */
1305         NG_NODE_FOREACH_HOOK(priv->node, ng_l2tp_reset_session, NULL, hook);
1306
1307         /* Reset node's sequence number state */
1308         seq->ns = 0;
1309         seq->nr = 0;
1310         seq->rack = 0;
1311         seq->xack = 0;
1312         seq->wmax = L2TP_MAX_XWIN;
1313         seq->cwnd = 1;
1314         seq->ssth = seq->wmax;
1315         seq->acks = 0;
1316         seq->rexmits = 0;
1317         bzero(seq->xwin, sizeof(seq->xwin));
1318
1319         /* Done */
1320         L2TP_SEQ_CHECK(seq);
1321 }
1322
1323 /*
1324  * Handle receipt of an acknowledgement value (Nr) from peer.
1325  */
1326 static void
1327 ng_l2tp_seq_recv_nr(priv_p priv, u_int16_t nr)
1328 {
1329         struct l2tp_seq *const seq = &priv->seq;
1330         struct mbuf     *xwin[L2TP_MAX_XWIN];   /* partial local copy */
1331         int             nack;
1332         int             i, j;
1333         uint16_t        ns;
1334
1335         mtx_lock(&seq->mtx);
1336
1337         /* Verify peer's ACK is in range */
1338         if ((nack = L2TP_SEQ_DIFF(nr, seq->rack)) <= 0) {
1339                 mtx_unlock(&seq->mtx);
1340                 return;                         /* duplicate ack */
1341         }
1342         if (L2TP_SEQ_DIFF(nr, seq->ns) > 0) {
1343                 mtx_unlock(&seq->mtx);
1344                 priv->stats.recvBadAcks++;      /* ack for packet not sent */
1345                 return;
1346         }
1347         KASSERT(nack <= L2TP_MAX_XWIN,
1348             ("%s: nack=%d > %d", __func__, nack, L2TP_MAX_XWIN));
1349
1350         /* Update receive ack stats */
1351         seq->rack = nr;
1352         seq->rexmits = 0;
1353
1354         /* Free acknowledged packets and shift up packets in the xmit queue */
1355         for (i = 0; i < nack; i++)
1356                 m_freem(seq->xwin[i]);
1357         memmove(seq->xwin, seq->xwin + nack,
1358             (L2TP_MAX_XWIN - nack) * sizeof(*seq->xwin));
1359         memset(seq->xwin + (L2TP_MAX_XWIN - nack), 0,
1360             nack * sizeof(*seq->xwin));
1361
1362         /*
1363          * Do slow-start/congestion avoidance windowing algorithm described
1364          * in RFC 2661, Appendix A. Here we handle a multiple ACK as if each
1365          * ACK had arrived separately.
1366          */
1367         if (seq->cwnd < seq->wmax) {
1368
1369                 /* Handle slow start phase */
1370                 if (seq->cwnd < seq->ssth) {
1371                         seq->cwnd += nack;
1372                         nack = 0;
1373                         if (seq->cwnd > seq->ssth) {    /* into cg.av. phase */
1374                                 nack = seq->cwnd - seq->ssth;
1375                                 seq->cwnd = seq->ssth;
1376                         }
1377                 }
1378
1379                 /* Handle congestion avoidance phase */
1380                 if (seq->cwnd >= seq->ssth) {
1381                         seq->acks += nack;
1382                         while (seq->acks >= seq->cwnd) {
1383                                 seq->acks -= seq->cwnd;
1384                                 if (seq->cwnd < seq->wmax)
1385                                         seq->cwnd++;
1386                         }
1387                 }
1388         }
1389
1390         /* Stop xmit timer */
1391         if (callout_active(&seq->rack_timer))
1392                 ng_uncallout(&seq->rack_timer, priv->node);
1393
1394         /* If transmit queue is empty, we're done for now */
1395         if (seq->xwin[0] == NULL) {
1396                 mtx_unlock(&seq->mtx);
1397                 return;
1398         }
1399
1400         /* Start restransmit timer again */
1401         ng_callout(&seq->rack_timer, priv->node, NULL,
1402             hz, ng_l2tp_seq_rack_timeout, NULL, 0);
1403
1404         /*
1405          * Send more packets, trying to keep peer's receive window full.
1406          * Make copy of everything we need before lock release.
1407          */
1408         ns = seq->ns;
1409         j = 0;
1410         while ((i = L2TP_SEQ_DIFF(seq->ns, seq->rack)) < seq->cwnd
1411             && seq->xwin[i] != NULL) {
1412                 xwin[j++] = seq->xwin[i];
1413                 seq->ns++;
1414         }
1415
1416         mtx_unlock(&seq->mtx);
1417
1418         /*
1419          * Send prepared.
1420          * If there is a memory error, pretend packet was sent, as it
1421          * will get retransmitted later anyway.
1422          */
1423         for (i = 0; i < j; i++) {
1424                 struct mbuf     *m;
1425                 if ((m = L2TP_COPY_MBUF(xwin[i], MB_DONTWAIT)) == NULL)
1426                         priv->stats.memoryFailures++;
1427                 else
1428                         ng_l2tp_xmit_ctrl(priv, m, ns);
1429                 ns++;
1430         }
1431 }
1432
1433 /*
1434  * Handle an ack timeout. We have an outstanding ack that we
1435  * were hoping to piggy-back, but haven't, so send a ZLB.
1436  */
1437 static void
1438 ng_l2tp_seq_xack_timeout(node_p node, hook_p hook, void *arg1, int arg2)
1439 {
1440         const priv_p priv = NG_NODE_PRIVATE(node);
1441         struct l2tp_seq *const seq = &priv->seq;
1442
1443         /* Make sure callout is still active before doing anything */
1444         if (callout_pending(&seq->xack_timer) ||
1445             (!callout_active(&seq->xack_timer)))
1446                 return;
1447
1448         /* Sanity check */
1449         L2TP_SEQ_CHECK(seq);
1450
1451         /* Send a ZLB */
1452         ng_l2tp_xmit_ctrl(priv, NULL, seq->ns);
1453
1454         /* callout_deactivate() is not needed here 
1455             as ng_uncallout() was called by ng_l2tp_xmit_ctrl() */
1456
1457         /* Sanity check */
1458         L2TP_SEQ_CHECK(seq);
1459 }
1460
1461 /* 
1462  * Handle a transmit timeout. The peer has failed to respond
1463  * with an ack for our packet, so retransmit it.
1464  */
1465 static void
1466 ng_l2tp_seq_rack_timeout(node_p node, hook_p hook, void *arg1, int arg2)
1467 {
1468         const priv_p priv = NG_NODE_PRIVATE(node);
1469         struct l2tp_seq *const seq = &priv->seq;
1470         struct mbuf *m;
1471         u_int delay;
1472
1473         /* Make sure callout is still active before doing anything */
1474         if (callout_pending(&seq->rack_timer) ||
1475             (!callout_active(&seq->rack_timer)))
1476                 return;
1477
1478         /* Sanity check */
1479         L2TP_SEQ_CHECK(seq);
1480
1481         priv->stats.xmitRetransmits++;
1482
1483         /* Have we reached the retransmit limit? If so, notify owner. */
1484         if (seq->rexmits++ >= priv->conf.rexmit_max)
1485                 ng_l2tp_seq_failure(priv);
1486
1487         /* Restart timer, this time with an increased delay */
1488         delay = (seq->rexmits > 12) ? (1 << 12) : (1 << seq->rexmits);
1489         if (delay > priv->conf.rexmit_max_to)
1490                 delay = priv->conf.rexmit_max_to;
1491         ng_callout(&seq->rack_timer, node, NULL,
1492             hz * delay, ng_l2tp_seq_rack_timeout, NULL, 0);
1493
1494         /* Do slow-start/congestion algorithm windowing algorithm */
1495         seq->ns = seq->rack;
1496         seq->ssth = (seq->cwnd + 1) / 2;
1497         seq->cwnd = 1;
1498         seq->acks = 0;
1499
1500         /* Retransmit oldest unack'd packet */
1501         if ((m = L2TP_COPY_MBUF(seq->xwin[0], MB_DONTWAIT)) == NULL)
1502                 priv->stats.memoryFailures++;
1503         else
1504                 ng_l2tp_xmit_ctrl(priv, m, seq->ns++);
1505
1506         /* callout_deactivate() is not needed here 
1507             as ng_callout() is getting called each time */
1508
1509         /* Sanity check */
1510         L2TP_SEQ_CHECK(seq);
1511 }
1512
1513 /*
1514  * Transmit a control stream packet, payload optional.
1515  * The transmit sequence number is not incremented.
1516  */
1517 static int
1518 ng_l2tp_xmit_ctrl(priv_p priv, struct mbuf *m, u_int16_t ns)
1519 {
1520         struct l2tp_seq *const seq = &priv->seq;
1521         u_int16_t session_id = 0;
1522         int error;
1523
1524         mtx_lock(&seq->mtx);
1525
1526         /* Stop ack timer: we're sending an ack with this packet.
1527            Doing this before to keep state predictable after error. */
1528         if (callout_active(&seq->xack_timer))
1529                 ng_uncallout(&seq->xack_timer, priv->node);
1530
1531         seq->xack = seq->nr;
1532
1533         mtx_unlock(&seq->mtx);
1534
1535         /* If no mbuf passed, send an empty packet (ZLB) */
1536         if (m == NULL) {
1537
1538                 /* Create a new mbuf for ZLB packet */
1539                 MGETHDR(m, MB_DONTWAIT, MT_DATA);
1540                 if (m == NULL) {
1541                         priv->stats.memoryFailures++;
1542                         return (ENOBUFS);
1543                 }
1544                 m->m_len = m->m_pkthdr.len = 12;
1545                 m->m_pkthdr.rcvif = NULL;
1546                 priv->stats.xmitZLBs++;
1547         } else {
1548
1549                 /* Strip off session ID */
1550                 if (m->m_len < 2 && (m = m_pullup(m, 2)) == NULL) {
1551                         priv->stats.memoryFailures++;
1552                         return (ENOBUFS);
1553                 }
1554                 memcpy(&session_id, mtod(m, u_int16_t *), 2);
1555                 m_adj(m, 2);
1556
1557                 /* Make room for L2TP header */
1558                 M_PREPEND(m, 12, MB_DONTWAIT);
1559                 if (m == NULL) {
1560                         priv->stats.memoryFailures++;
1561                         return (ENOBUFS);
1562                 }
1563         }
1564
1565         /* Fill in L2TP header */
1566         mtod(m, u_int16_t *)[0] = htons(L2TP_CTRL_HDR);
1567         mtod(m, u_int16_t *)[1] = htons(m->m_pkthdr.len);
1568         mtod(m, u_int16_t *)[2] = priv->conf.peer_id;
1569         mtod(m, u_int16_t *)[3] = session_id;
1570         mtod(m, u_int16_t *)[4] = htons(ns);
1571         mtod(m, u_int16_t *)[5] = htons(seq->nr);
1572
1573         /* Update sequence number info and stats */
1574         priv->stats.xmitPackets++;
1575         priv->stats.xmitOctets += m->m_pkthdr.len;
1576
1577         /* Send packet */
1578         NG_SEND_DATA_ONLY(error, priv->lower, m);
1579         return (error);
1580 }
1581
1582 #ifdef INVARIANTS
1583 /*
1584  * Sanity check sequence number state.
1585  */
1586 static void
1587 ng_l2tp_seq_check(struct l2tp_seq *seq)
1588 {
1589         int self_unack, peer_unack;
1590         int i;
1591
1592 #define CHECK(p)        KASSERT((p), ("%s: not: %s", __func__, #p))
1593
1594         mtx_lock(&seq->mtx);
1595
1596         self_unack = L2TP_SEQ_DIFF(seq->nr, seq->xack);
1597         peer_unack = L2TP_SEQ_DIFF(seq->ns, seq->rack);
1598         CHECK(seq->wmax <= L2TP_MAX_XWIN);
1599         CHECK(seq->cwnd >= 1);
1600         CHECK(seq->cwnd <= seq->wmax);
1601         CHECK(seq->ssth >= 1);
1602         CHECK(seq->ssth <= seq->wmax);
1603         if (seq->cwnd < seq->ssth)
1604                 CHECK(seq->acks == 0);
1605         else
1606                 CHECK(seq->acks <= seq->cwnd);
1607         CHECK(self_unack >= 0);
1608         CHECK(peer_unack >= 0);
1609         CHECK(peer_unack <= seq->wmax);
1610         CHECK((self_unack == 0) ^ callout_active(&seq->xack_timer));
1611         CHECK((peer_unack == 0) ^ callout_active(&seq->rack_timer));
1612         for (i = 0; i < peer_unack; i++)
1613                 CHECK(seq->xwin[i] != NULL);
1614         for ( ; i < seq->cwnd; i++)         /* verify peer's recv window full */
1615                 CHECK(seq->xwin[i] == NULL);
1616
1617         mtx_unlock(&seq->mtx);
1618
1619 #undef CHECK
1620 }
1621 #endif  /* INVARIANTS */