f804b0e5167bc4d1b139df60086758efd8cad773
[l2tpns.git] / pppoe.c
1 /*
2 * Fernando ALVES 2013
3 * Add functionality "server pppoe" to l2tpns.
4 * inspiration pppoe.c of accel-ppp
5 * GPL licenced
6 */
7
8 #include <unistd.h>
9 #include <assert.h>
10 #include <stdlib.h>
11 #include <stdio.h>
12 #include <stdarg.h>
13 #include <errno.h>
14 #include <string.h>
15 #include <pthread.h>
16 #include <fcntl.h>
17 #include <sys/socket.h>
18 #include <sys/ioctl.h>
19 #include <net/if.h>
20 #include <net/ethernet.h>
21 #include <netpacket/packet.h>
22 #include <arpa/inet.h>
23 #include <linux/if_pppox.h>
24 #include <linux/rtnetlink.h>
25
26 #include "l2tpns.h"
27 #include "cluster.h"
28 #include "constants.h"
29 #include "md5.h"
30 #include "util.h"
31
32 int pppoediscfd = -1;
33 int pppoesessfd = -1;
34
35 static uint8_t bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
36
37 /* PPPoE codes */
38 #define CODE_PADI 0x09
39 #define CODE_PADO 0x07
40 #define CODE_PADR 0x19
41 #define CODE_PADS 0x65
42 #define CODE_PADT 0xA7
43 #define CODE_SESS 0x00
44
45 /* PPPoE Tags */
46 #define TAG_END_OF_LIST 0x0000
47 #define TAG_SERVICE_NAME 0x0101
48 #define TAG_AC_NAME 0x0102
49 #define TAG_HOST_UNIQ 0x0103
50 #define TAG_AC_COOKIE 0x0104
51 #define TAG_VENDOR_SPECIFIC 0x0105
52 #define TAG_RELAY_SESSION_ID 0x0110
53 #define TAG_SERVICE_NAME_ERROR 0x0201
54 #define TAG_AC_SYSTEM_ERROR 0x0202
55 #define TAG_GENERIC_ERROR 0x0203
56
57 static char *code_pad[] = {
58 "PADI",
59 "PADO",
60 "PADR",
61 "PADS",
62 "PADT",
63 "SESS",
64 NULL
65 };
66
67 enum
68 {
69 INDEX_PADI = 0,
70 INDEX_PADO,
71 INDEX_PADR,
72 INDEX_PADS,
73 INDEX_PADT,
74 INDEX_SESS
75 };
76
77 // set up pppoe discovery socket
78 static void init_pppoe_disc(void)
79 {
80 int on = 1;
81 struct ifreq ifr;
82 struct sockaddr_ll sa;
83
84 memset(&ifr, 0, sizeof(ifr));
85 memset(&sa, 0, sizeof(sa));
86
87 pppoediscfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_DISC));
88 if (pppoediscfd < 0)
89 {
90 LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
91 exit(1);
92 }
93
94 fcntl(pppoediscfd, F_SETFD, fcntl(pppoediscfd, F_GETFD) | FD_CLOEXEC);
95
96 if (setsockopt(pppoediscfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
97 {
98 LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
99 exit(1);
100 }
101
102 assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_to_bind) - 1);
103 if (*config->pppoe_if_to_bind)
104 strncpy(ifr.ifr_name, config->pppoe_if_to_bind, IFNAMSIZ);
105
106 if (ioctl(pppoediscfd, SIOCGIFHWADDR, &ifr))
107 {
108 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
109 exit(1);
110 }
111
112 if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
113 {
114 LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_to_bind);
115 exit(1);
116 }
117
118 memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
119
120 if (ioctl(pppoediscfd, SIOCGIFMTU, &ifr))
121 {
122 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
123 exit(1);
124 }
125
126 if (ifr.ifr_mtu < ETH_DATA_LEN)
127 LOG(0, 0, 0, "Error pppoe: interface %s has MTU of %i, should be %i\n", config->pppoe_if_to_bind, ifr.ifr_mtu, ETH_DATA_LEN);
128
129 if (ioctl(pppoediscfd, SIOCGIFINDEX, &ifr))
130 {
131 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
132 exit(1);
133 }
134
135 memset(&sa, 0, sizeof(sa));
136 sa.sll_family = AF_PACKET;
137 sa.sll_protocol = htons(ETH_P_PPP_DISC);
138 sa.sll_ifindex = ifr.ifr_ifindex;
139
140 if (bind(pppoediscfd, (struct sockaddr *)&sa, sizeof(sa)))
141 {
142 LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
143 exit(1);
144 }
145
146 if (fcntl(pppoediscfd, F_SETFL, O_NONBLOCK))
147 {
148 LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
149 exit(1);
150 }
151
152 }
153
154 // set up pppoe session socket
155 static void init_pppoe_sess(void)
156 {
157 int on = 1;
158 struct ifreq ifr;
159 struct sockaddr_ll sa;
160
161 memset(&ifr, 0, sizeof(ifr));
162 memset(&sa, 0, sizeof(sa));
163
164 pppoesessfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PPP_SES));
165 if (pppoesessfd < 0)
166 {
167 LOG(0, 0, 0, "Error pppoe: socket: %s\n", strerror(errno));
168 exit(1);
169 }
170
171 fcntl(pppoesessfd, F_SETFD, fcntl(pppoesessfd, F_GETFD) | FD_CLOEXEC);
172
173 if (setsockopt(pppoesessfd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)))
174 {
175 LOG(0, 0, 0, "Error pppoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
176 exit(1);
177 }
178
179 assert(strlen(ifr.ifr_name) < sizeof(config->pppoe_if_to_bind) - 1);
180 if (*config->pppoe_if_to_bind)
181 strncpy(ifr.ifr_name, config->pppoe_if_to_bind, IFNAMSIZ);
182
183 if (ioctl(pppoesessfd, SIOCGIFHWADDR, &ifr))
184 {
185 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFHWADDR): %s\n", strerror(errno));
186 exit(1);
187 }
188
189 if ((ifr.ifr_hwaddr.sa_data[0] & 1) != 0)
190 {
191 LOG(0, 0, 0, "Error pppoe: interface %s has not unicast address\n", config->pppoe_if_to_bind);
192 exit(1);
193 }
194
195 memcpy(config->pppoe_hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
196
197 if (ioctl(pppoesessfd, SIOCGIFMTU, &ifr))
198 {
199 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFMTU): %s\n", strerror(errno));
200 exit(1);
201 }
202
203 if (ifr.ifr_mtu < ETH_DATA_LEN)
204 LOG(0, 0, 0, "Error pppoe: interface %s has MTU of %i, should be %i\n", config->pppoe_if_to_bind, ifr.ifr_mtu, ETH_DATA_LEN);
205
206 if (ioctl(pppoesessfd, SIOCGIFINDEX, &ifr))
207 {
208 LOG(0, 0, 0, "Error pppoe: ioctl(SIOCGIFINDEX): %s\n", strerror(errno));
209 exit(1);
210 }
211
212 memset(&sa, 0, sizeof(sa));
213 sa.sll_family = AF_PACKET;
214 sa.sll_protocol = htons(ETH_P_PPP_SES);
215 sa.sll_ifindex = ifr.ifr_ifindex;
216
217 if (bind(pppoesessfd, (struct sockaddr *)&sa, sizeof(sa)))
218 {
219 LOG(0, 0, 0, "Error pppoe: bind: %s\n", strerror(errno));
220 exit(1);
221 }
222
223 if (fcntl(pppoesessfd, F_SETFL, O_NONBLOCK))
224 {
225 LOG(0, 0, 0, "Error pppoe: failed to set nonblocking mode: %s\n", strerror(errno));
226 exit(1);
227 }
228 }
229
230 // set up pppoe discovery/session socket
231 void init_pppoe(void)
232 {
233 tunnelidt t = TUNNEL_ID_PPPOE;
234
235 init_pppoe_disc();
236 init_pppoe_sess();
237
238 // Reserve the a pseudo tunnel for pppoe server
239 if (t > config->cluster_highest_tunnelid)
240 config->cluster_highest_tunnelid = t;
241
242 memset(&tunnel[t], 0, sizeof(tunnel[t]));
243 tunnel[t].state = TUNNELOPEN;
244 STAT(tunnel_created);
245 }
246
247 char * get_string_codepad(uint8_t codepad)
248 {
249 char * ptrch = NULL;
250 switch(codepad)
251 {
252 case CODE_PADI:
253 ptrch = code_pad[INDEX_PADI];
254 break;
255
256 case CODE_PADO:
257 ptrch = code_pad[INDEX_PADO];
258 break;
259
260 case CODE_PADR:
261 ptrch = code_pad[INDEX_PADR];
262 break;
263
264 case CODE_PADS:
265 ptrch = code_pad[INDEX_PADS];
266 break;
267
268 case CODE_PADT:
269 ptrch = code_pad[INDEX_PADT];
270 break;
271
272 case CODE_SESS:
273 ptrch = code_pad[INDEX_SESS];
274 break;
275 }
276
277 return ptrch;
278 }
279
280 static uint8_t * setup_header(uint8_t *pack, const uint8_t *src, const uint8_t *dst, int code, uint16_t sid, uint16_t h_proto)
281 {
282 uint8_t * p;
283
284 // 14 bytes ethernet Header + 6 bytes header pppoe
285 struct ethhdr *ethhdr = (struct ethhdr *)pack;
286 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
287
288 memcpy(ethhdr->h_source, src, ETH_ALEN);
289 memcpy(ethhdr->h_dest, dst, ETH_ALEN);
290 ethhdr->h_proto = htons(h_proto);
291
292 hdr->ver = 1;
293 hdr->type = 1;
294 hdr->code = code;
295 hdr->sid = htons(sid);
296 hdr->length = 0;
297
298 p = (uint8_t *)(pack + ETH_HLEN + sizeof(*hdr));
299
300 return p;
301 }
302
303 static void add_tag(uint8_t *pack, int type, const uint8_t *data, int len)
304 {
305 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
306 struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
307
308 tag->tag_type = htons(type);
309 tag->tag_len = htons(len);
310 memcpy(tag->tag_data, data, len);
311
312 hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + len);
313 }
314
315 static void add_tag2(uint8_t *pack, const struct pppoe_tag *t)
316 {
317 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
318 struct pppoe_tag *tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length));
319
320 memcpy(tag, t, sizeof(*t) + ntohs(t->tag_len));
321
322 hdr->length = htons(ntohs(hdr->length) + sizeof(*tag) + ntohs(t->tag_len));
323 }
324
325 static void pppoe_disc_send(const uint8_t *pack)
326 {
327 struct ethhdr *ethhdr = (struct ethhdr *)pack;
328 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
329 int n, s;
330
331 s = ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length);
332
333 LOG(3, 0, 0, "SENT pppoe_disc: Code %s to %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_dest));
334 LOG_HEX(5, "pppoe_disc_send", pack, s);
335
336 n = write(pppoediscfd, pack, s);
337 if (n < 0 )
338 LOG(0, 0, 0, "pppoe: write: %s\n", strerror(errno));
339 else if (n != s) {
340 LOG(0, 0, 0, "pppoe: short write %i/%i\n", n,s);
341 }
342 }
343
344 void pppoe_sess_send(const uint8_t *pack, uint16_t l, tunnelidt t)
345 {
346 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
347 int n;
348 uint16_t sizeppp;
349 sessionidt s;
350
351 if (t != TUNNEL_ID_PPPOE)
352 {
353 LOG(3, 0, t, "ERROR pppoe_sess_send: Tunnel %d is not a tunnel pppoe\n", t);
354 return;
355 }
356
357 s = ntohs(hdr->sid);
358 if (session[s].tunnel != t)
359 {
360 LOG(3, s, t, "ERROR pppoe_sess_send: Session is not a session pppoe\n");
361 return;
362 }
363
364 if (l < (ETH_HLEN + sizeof(*hdr) + 3))
365 {
366 LOG(0, s, t, "ERROR pppoe_sess_send: packet too small for pppoe sent (size=%d)\n", l);
367 return;
368 }
369
370 // recalculate the ppp frame length
371 sizeppp = l - (ETH_HLEN + sizeof(*hdr));
372 hdr->length = htons(sizeppp);
373
374 LOG_HEX(5, "pppoe_sess_send", pack, l);
375
376 n = write(pppoesessfd, pack, l);
377 if (n < 0 )
378 LOG(0, s, t, "pppoe_sess_send: write: %s\n", strerror(errno));
379 else if (n != l)
380 LOG(0, s, t, "pppoe_sess_send: short write %i/%i\n", n,l);
381 }
382
383 static void pppoe_send_err(const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, int code, int tag_type)
384 {
385 uint8_t pack[ETHER_MAX_LEN];
386
387 setup_header(pack, config->pppoe_hwaddr, addr, code, 0, ETH_P_PPP_DISC);
388
389 add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
390 add_tag(pack, tag_type, NULL, 0);
391
392 if (host_uniq)
393 add_tag2(pack, host_uniq);
394
395 if (relay_sid)
396 add_tag2(pack, relay_sid);
397
398 pppoe_disc_send(pack);
399 }
400
401 // generate cookie
402 static void pppoe_gen_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *out)
403 {
404 MD5_CTX ctx;
405
406 MD5_Init(&ctx);
407 MD5_Update(&ctx, config->l2tp_secret, strlen(config->l2tp_secret));
408 MD5_Update(&ctx, (void *) serv_hwaddr, ETH_ALEN);
409 MD5_Update(&ctx, (void *) client_hwaddr, ETH_ALEN);
410 MD5_Final(out, &ctx);
411 }
412
413 // check cookie
414 static int pppoe_check_cookie(const uint8_t *serv_hwaddr, const uint8_t *client_hwaddr, uint8_t *cookie)
415 {
416 hasht hash;
417
418 pppoe_gen_cookie(serv_hwaddr, client_hwaddr, hash);
419
420 return memcmp(hash, cookie, 16);
421 }
422
423 static void pppoe_send_PADO(const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, const struct pppoe_tag *service_name)
424 {
425 uint8_t pack[ETHER_MAX_LEN];
426 hasht hash;
427
428 setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADO, 0, ETH_P_PPP_DISC);
429
430 add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
431
432 if (service_name)
433 add_tag2(pack, service_name);
434
435 pppoe_gen_cookie(config->pppoe_hwaddr, addr, hash);
436 add_tag(pack, TAG_AC_COOKIE, hash, 16);
437
438 if (host_uniq)
439 add_tag2(pack, host_uniq);
440
441 if (relay_sid)
442 add_tag2(pack, relay_sid);
443
444 pppoe_disc_send(pack);
445 }
446
447 static void pppoe_send_PADS(uint16_t sid, const uint8_t *addr, const struct pppoe_tag *host_uniq, const struct pppoe_tag *relay_sid, const struct pppoe_tag *service_name)
448 {
449 uint8_t pack[ETHER_MAX_LEN];
450
451 setup_header(pack, config->pppoe_hwaddr, addr, CODE_PADS, sid, ETH_P_PPP_DISC);
452
453 add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
454
455 add_tag2(pack, service_name);
456
457 if (host_uniq)
458 add_tag2(pack, host_uniq);
459
460 if (relay_sid)
461 add_tag2(pack, relay_sid);
462
463 pppoe_disc_send(pack);
464 }
465
466 static void pppoe_send_PADT(uint16_t sid)
467 {
468 uint8_t pack[ETHER_MAX_LEN];
469
470 setup_header(pack, config->pppoe_hwaddr, session[sid].src_hwaddr, CODE_PADT, sid, ETH_P_PPP_DISC);
471
472 add_tag(pack, TAG_AC_NAME, (uint8_t *)config->pppoe_ac_name, strlen(config->pppoe_ac_name));
473
474 LOG(3, sid, session[sid].tunnel, "pppoe: Sent PADT\n");
475
476 pppoe_disc_send(pack);
477 }
478
479 void pppoe_shutdown_session(sessionidt s)
480 {
481
482 if (session[s].tunnel != TUNNEL_ID_PPPOE)
483 {
484 LOG(3, s, session[s].tunnel, "ERROR pppoe_shutdown_session: Session is not a session pppoe\n");
485 return;
486 }
487
488 pppoe_send_PADT(s);
489 }
490
491 static void pppoe_recv_PADI(uint8_t *pack, int size)
492 {
493 struct ethhdr *ethhdr = (struct ethhdr *)pack;
494 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
495 struct pppoe_tag *tag;
496 struct pppoe_tag *host_uniq_tag = NULL;
497 struct pppoe_tag *relay_sid_tag = NULL;
498 struct pppoe_tag *service_name_tag = NULL;
499 int n, service_match = 0;
500 int len;
501
502 if (hdr->sid)
503 return;
504
505 len = ntohs(hdr->length);
506 for (n = 0; n < len; n += sizeof(*tag) + ntohs(tag->tag_len)) {
507 tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
508 if (n + sizeof(*tag) + ntohs(tag->tag_len) > len)
509 return;
510 switch (ntohs(tag->tag_type)) {
511 case TAG_END_OF_LIST:
512 break;
513 case TAG_SERVICE_NAME:
514 if (*config->pppoe_service_name && tag->tag_len)
515 {
516 if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
517 break;
518 if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
519 break;
520 service_match = 1;
521 }
522 else
523 {
524 service_name_tag = tag;
525 service_match = 1;
526 }
527 break;
528 case TAG_HOST_UNIQ:
529 host_uniq_tag = tag;
530 break;
531 case TAG_RELAY_SESSION_ID:
532 relay_sid_tag = tag;
533 break;
534 }
535 }
536
537 if (!service_match)
538 {
539 LOG(3, 0, 0, "pppoe: discarding PADI packet (Service-Name mismatch)\n");
540 return;
541 }
542
543 pppoe_send_PADO(ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
544 }
545
546 static void pppoe_recv_PADR(uint8_t *pack, int size)
547 {
548 struct ethhdr *ethhdr = (struct ethhdr *)pack;
549 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
550 struct pppoe_tag *tag;
551 struct pppoe_tag *host_uniq_tag = NULL;
552 struct pppoe_tag *relay_sid_tag = NULL;
553 struct pppoe_tag *ac_cookie_tag = NULL;
554 struct pppoe_tag *service_name_tag = NULL;
555 int n, service_match = 0;
556 uint16_t sid;
557
558 if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
559 {
560 LOG(1, 0, 0, "Rcv pppoe: discard PADR (destination address is broadcast)\n");
561 return;
562 }
563
564 if (hdr->sid)
565 {
566 LOG(1, 0, 0, "Rcv pppoe: discarding PADR packet (sid is not zero)\n");
567 return;
568 }
569
570 for (n = 0; n < ntohs(hdr->length); n += sizeof(*tag) + ntohs(tag->tag_len))
571 {
572 tag = (struct pppoe_tag *)(pack + ETH_HLEN + sizeof(*hdr) + n);
573 switch (ntohs(tag->tag_type))
574 {
575 case TAG_END_OF_LIST:
576 break;
577 case TAG_SERVICE_NAME:
578 service_name_tag = tag;
579 if (tag->tag_len == 0)
580 service_match = 1;
581 else if (*config->pppoe_service_name)
582 {
583 if (ntohs(tag->tag_len) != strlen(config->pppoe_service_name))
584 break;
585 if (memcmp(tag->tag_data, config->pppoe_service_name, ntohs(tag->tag_len)))
586 break;
587 service_match = 1;
588 }
589 else
590 {
591 service_match = 1;
592 }
593 break;
594 case TAG_HOST_UNIQ:
595 host_uniq_tag = tag;
596 break;
597 case TAG_AC_COOKIE:
598 ac_cookie_tag = tag;
599 break;
600 case TAG_RELAY_SESSION_ID:
601 relay_sid_tag = tag;
602 break;
603 }
604 }
605
606 if (!service_match)
607 {
608 LOG(3, 0, 0, "pppoe: Service-Name mismatch\n");
609 pppoe_send_err(ethhdr->h_source, host_uniq_tag, relay_sid_tag, CODE_PADS, TAG_SERVICE_NAME_ERROR);
610 return;
611 }
612
613 if (!ac_cookie_tag)
614 {
615 LOG(3, 0, 0, "pppoe: discard PADR packet (no AC-Cookie tag present)\n");
616 return;
617 }
618
619 if (ntohs(ac_cookie_tag->tag_len) != 16)
620 {
621 LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie tag length)\n");
622 return;
623 }
624
625 if (pppoe_check_cookie(ethhdr->h_dest, ethhdr->h_source, (uint8_t *) ac_cookie_tag->tag_data))
626 {
627 LOG(3, 0, 0, "pppoe: discard PADR packet (incorrect AC-Cookie)\n");
628 return;
629 }
630
631 sid = sessionfree;
632 sessionfree = session[sid].next;
633 memset(&session[sid], 0, sizeof(session[0]));
634
635 if (sid > config->cluster_highest_sessionid)
636 config->cluster_highest_sessionid = sid;
637
638 session[sid].opened = time_now;
639 session[sid].tunnel = TUNNEL_ID_PPPOE;
640 session[sid].last_packet = session[sid].last_data = time_now;
641
642 //strncpy(session[sid].called, called, sizeof(session[sid].called) - 1);
643 //strncpy(session[sid].calling, calling, sizeof(session[sid].calling) - 1);
644
645 session[sid].ppp.phase = Establish;
646 session[sid].ppp.lcp = Starting;
647
648 session[sid].magic = time_now; // set magic number
649 session[sid].mru = PPPoE_MRU; // default
650
651 // start LCP
652 sess_local[sid].lcp_authtype = config->radius_authprefer;
653 sess_local[sid].ppp_mru = MRU;
654
655 // Set multilink options before sending initial LCP packet
656 sess_local[sid].mp_mrru = 1614;
657 sess_local[sid].mp_epdis = ntohl(config->iftun_address ? config->iftun_address : my_address);
658
659 memcpy(session[sid].src_hwaddr, ethhdr->h_source, ETH_ALEN);
660 pppoe_send_PADS(sid, ethhdr->h_source, host_uniq_tag, relay_sid_tag, service_name_tag);
661
662 sendlcp(sid, session[sid].tunnel);
663 change_state(sid, lcp, RequestSent);
664
665 }
666
667 static void pppoe_recv_PADT(uint8_t *pack)
668 {
669 struct ethhdr *ethhdr = (struct ethhdr *)pack;
670 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
671
672 if (!memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN))
673 {
674 LOG(3, 0, 0, "pppoe: discard PADT (destination address is broadcast)\n");
675 return;
676 }
677
678 if (hdr->sid)
679 {
680 if ((hdr->sid < MAXSESSION) && (session[hdr->sid].tunnel == TUNNEL_ID_PPPOE))
681 sessionshutdown(hdr->sid, "Client shutdown", CDN_ADMIN_DISC, 0);
682 }
683 }
684
685 // fill in a PPPOE message with a PPP frame,
686 // returns start of PPP frame
687 uint8_t *pppoe_makeppp(uint8_t *b, int size, uint8_t *p, int l, sessionidt s, tunnelidt t,
688 uint16_t mtype, uint8_t prio, bundleidt bid, uint8_t mp_bits)
689 {
690 uint16_t type = mtype;
691 uint8_t *start = b;
692 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(b + ETH_HLEN);
693
694 if (t != TUNNEL_ID_PPPOE)
695 return NULL;
696
697 if (size < 28) // Need more space than this!!
698 {
699 LOG(0, s, t, "pppoe_makeppp buffer too small for pppoe header (size=%d)\n", size);
700 return NULL;
701 }
702
703 // 14 bytes ethernet Header + 6 bytes header pppoe
704 b = setup_header(b, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
705
706 // Check whether this session is part of multilink
707 if (bid)
708 {
709 if (bundle[bid].num_of_links > 1)
710 type = PPPMP; // Change PPP message type to the PPPMP
711 else
712 bid = 0;
713 }
714
715 *(uint16_t *) b = htons(type);
716 b += 2;
717 hdr->length += 2;
718
719 if (bid)
720 {
721 // Set the sequence number and (B)egin (E)nd flags
722 if (session[s].mssf)
723 {
724 // Set the multilink bits
725 uint16_t bits_send = mp_bits;
726 *(uint16_t *) b = htons((bundle[bid].seq_num_t & 0x0FFF)|bits_send);
727 b += 2;
728 hdr->length += 2;
729 }
730 else
731 {
732 *(uint32_t *) b = htonl(bundle[bid].seq_num_t);
733 // Set the multilink bits
734 *b = mp_bits;
735 b += 4;
736 hdr->length += 4;
737 }
738
739 bundle[bid].seq_num_t++;
740
741 // Add the message type if this fragment has the begin bit set
742 if (mp_bits & MP_BEGIN)
743 {
744 //*b++ = mtype; // The next two lines are instead of this
745 *(uint16_t *) b = htons(mtype); // Message type
746 b += 2;
747 hdr->length += 2;
748 }
749 }
750
751 if ((b - start) + l > size)
752 {
753 LOG(0, s, t, "pppoe_makeppp would overflow buffer (size=%d, header+payload=%td)\n", size, (b - start) + l);
754 return NULL;
755 }
756
757 // Copy the payload
758 if (p && l)
759 {
760 memcpy(b, p, l);
761 hdr->length += l;
762 }
763
764 return b;
765 }
766
767 // fill in a PPPOE message with a PPP frame,
768 // returns start of PPP frame
769 //(note: THIS ROUTINE WRITES TO p[-28]).
770 uint8_t *opt_pppoe_makeppp(uint8_t *p, int l, sessionidt s, tunnelidt t, uint16_t mtype, uint8_t prio, bundleidt bid, uint8_t mp_bits)
771 {
772 uint16_t type = mtype;
773 uint16_t hdrlen = l;
774 uint8_t *b = p;
775 struct pppoe_hdr *hdr;
776
777 if (t != TUNNEL_ID_PPPOE)
778 return NULL;
779
780 // Check whether this session is part of multilink
781 if (bid)
782 {
783 if (bundle[bid].num_of_links > 1)
784 type = PPPMP; // Change PPP message type to the PPPMP
785 else
786 bid = 0;
787 }
788
789 if (bid)
790 {
791 // Add the message type if this fragment has the begin bit set
792 if (mp_bits & MP_BEGIN)
793 {
794 b -= 2;
795 *(uint16_t *) b = htons(mtype); // Message type
796 }
797
798 // Set the sequence number and (B)egin (E)nd flags
799 if (session[s].mssf)
800 {
801 // Set the multilink bits
802 uint16_t bits_send = mp_bits;
803 b -= 2;
804 *(uint16_t *) b = htons((bundle[bid].seq_num_t & 0x0FFF)|bits_send);
805 }
806 else
807 {
808 b -= 4;
809 *(uint32_t *) b = htonl(bundle[bid].seq_num_t);
810 // Set the multilink bits
811 *b = mp_bits;
812 }
813
814 bundle[bid].seq_num_t++;
815 }
816
817 b -= 2;
818 *(uint16_t *) b = htons(type);
819
820 // Size ppp packet
821 hdrlen += (p - b);
822
823 // 14 bytes ethernet Header + 6 bytes header pppoe
824 b -= (ETH_HLEN + sizeof(*hdr));
825 setup_header(b, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
826 hdr = (struct pppoe_hdr *)(b + ETH_HLEN);
827 // Store length on header pppoe
828 hdr->length = hdrlen;
829
830 return b;
831 }
832
833 // pppoe discovery recv data
834 void process_pppoe_disc(uint8_t *pack, int size)
835 {
836 struct ethhdr *ethhdr = (struct ethhdr *)pack;
837 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
838
839 LOG(3, 0, 0, "RCV pppoe_disc: Code %s from %s\n", get_string_codepad(hdr->code), fmtMacAddr(ethhdr->h_source));
840 LOG_HEX(5, "PPPOE Disc", pack, size);
841
842 if (!config->cluster_iam_master)
843 {
844 if (hdr->code == CODE_PADI)
845 return; // Discard because the PADI is received by all (PADI is a broadcast diffusion)
846
847 master_forward_pppoe_packet(pack, size, hdr->code);
848 return;
849 }
850
851 if (size < (ETH_HLEN + sizeof(*hdr)))
852 {
853 LOG(1, 0, 0, "Error pppoe_disc: short packet received (%i)\n", size);
854 return;
855 }
856
857 if (memcmp(ethhdr->h_dest, bc_addr, ETH_ALEN) && memcmp(ethhdr->h_dest, config->pppoe_hwaddr, ETH_ALEN))
858 {
859 LOG(1, 0, 0, "Error pppoe_disc: h_dest != bc_addr and h_dest != config->pppoe_hwaddr\n");
860 return;
861 }
862
863 if (!memcmp(ethhdr->h_source, bc_addr, ETH_ALEN))
864 {
865 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (source address is broadcast)\n");
866 return;
867 }
868
869 if ((ethhdr->h_source[0] & 1) != 0)
870 {
871 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (host address is not unicast)\n");
872 return;
873 }
874
875 if (size < ETH_HLEN + sizeof(*hdr) + ntohs(hdr->length))
876 {
877 LOG(1, 0, 0, "Error pppoe_disc: short packet received\n");
878 return;
879 }
880
881 if (hdr->ver != 1)
882 {
883 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported version %i)\n", hdr->ver);
884 return;
885 }
886
887 if (hdr->type != 1)
888 {
889 LOG(1, 0, 0, "Error pppoe_disc: discarding packet (unsupported type %i)\n", hdr->type);
890 return;
891 }
892
893 switch (hdr->code) {
894 case CODE_PADI:
895 pppoe_recv_PADI(pack, size);
896 break;
897 case CODE_PADR:
898 pppoe_recv_PADR(pack, size);
899 break;
900 case CODE_PADT:
901 pppoe_recv_PADT(pack);
902 break;
903 }
904 }
905
906 // Forward from pppoe to l2tp remote LNS
907 static void pppoe_forwardto_session_rmlns(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
908 {
909 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
910 uint16_t lppp = ntohs(hdr->length);
911 uint16_t ll2tp = lppp + 6;
912 uint8_t *pppdata = (uint8_t *) hdr->tag;
913 uint8_t *pl2tp = pppdata - 6;
914 uint8_t *p = pl2tp;
915 uint16_t t = 0, s = 0;
916
917 s = session[sess].forwardtosession;
918 if (session[s].forwardtosession != sess)
919 {
920 LOG(3, sess, session[sess].tunnel, "pppoe: Link Session (%u) broken\n", s);
921 return;
922 }
923
924 t = session[s].tunnel;
925 if (t >= MAXTUNNEL)
926 {
927 LOG(1, s, t, "pppoe: Session with invalid tunnel ID\n");
928 return;
929 }
930
931 if (!tunnel[t].isremotelns)
932 {
933 LOG(3, sess, session[sess].tunnel, "pppoe: Link Tunnel/Session (%u/%u) broken\n", s, t);
934 return;
935 }
936
937 // First word L2TP options (with no options)
938 *(uint16_t *) p = htons(0x0002);
939 p += 2;
940 *(uint16_t *) p = htons(tunnel[t].far); // tunnel
941 p += 2;
942 *(uint16_t *) p = htons(session[s].far); // session
943 p += 2;
944
945 if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
946 {
947 session[sess].last_packet = session[sess].last_data = time_now;
948 // Update STAT IN
949 increment_counter(&session[sess].cin, &session[sess].cin_wrap, ll2tp);
950 session[sess].cin_delta += ll2tp;
951 session[sess].pin++;
952 sess_local[sess].cin += ll2tp;
953 sess_local[sess].pin++;
954
955 session[s].last_data = time_now;
956 // Update STAT OUT
957 increment_counter(&session[s].cout, &session[s].cout_wrap, ll2tp); // byte count
958 session[s].cout_delta += ll2tp;
959 session[s].coutgrp_delta += ll2tp;
960 session[s].pout++;
961 sess_local[s].cout += ll2tp;
962 sess_local[s].pout++;
963 }
964 else
965 session[sess].last_packet = time_now;
966
967 tunnelsend(pl2tp, ll2tp, t); // send it...
968 }
969
970 // Forward from l2tp to pppoe
971 // (note: THIS ROUTINE WRITES TO pack[-20]).
972 void pppoe_forwardto_session_pppoe(uint8_t *pack, int size, sessionidt sess, uint16_t proto)
973 {
974 uint16_t t = 0, s = 0;
975 uint16_t lpppoe = size - 2;
976 uint8_t *p = pack + 2; // First word L2TP options
977
978 LOG(5, sess, session[sess].tunnel, "Forwarding data session to pppoe session %u\n", session[sess].forwardtosession);
979
980 s = session[sess].forwardtosession;
981 t = session[s].tunnel;
982
983 if (*pack & 0x40)
984 { // length
985 p += 2;
986 lpppoe -= 2;
987 }
988
989 *(uint16_t *) p = htons(tunnel[t].far); // tunnel
990 p += 2;
991 *(uint16_t *) p = htons(session[s].far); // session
992 p += 2;
993 lpppoe -= 4;
994
995 if (*pack & 0x08)
996 { // ns/nr
997 *(uint16_t *) p = htons(tunnel[t].ns); // sequence
998 p += 2;
999 *(uint16_t *) p = htons(tunnel[t].nr); // sequence
1000 p += 2;
1001 lpppoe -= 4;
1002 }
1003
1004 if (lpppoe > 2 && p[0] == 0xFF && p[1] == 0x03)
1005 {
1006 // HDLC address header, discard in pppoe
1007 p += 2;
1008 lpppoe -= 2;
1009 }
1010
1011 lpppoe += (ETH_HLEN + sizeof(struct pppoe_hdr));
1012 p -= (ETH_HLEN + sizeof(struct pppoe_hdr));
1013
1014 // 14 bytes ethernet Header + 6 bytes header pppoe
1015 setup_header(p, config->pppoe_hwaddr, session[s].src_hwaddr, CODE_SESS, s, ETH_P_PPP_SES);
1016
1017 if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
1018 {
1019 session[sess].last_packet = session[sess].last_data = time_now;
1020 // Update STAT IN
1021 increment_counter(&session[sess].cin, &session[sess].cin_wrap, lpppoe);
1022 session[sess].cin_delta += lpppoe;
1023 session[sess].pin++;
1024 sess_local[sess].cin += lpppoe;
1025 sess_local[sess].pin++;
1026
1027 session[s].last_data = time_now;
1028 // Update STAT OUT
1029 increment_counter(&session[s].cout, &session[s].cout_wrap, lpppoe); // byte count
1030 session[s].cout_delta += lpppoe;
1031 session[s].coutgrp_delta += lpppoe;
1032 session[s].pout++;
1033 sess_local[s].cout += lpppoe;
1034 sess_local[s].pout++;
1035 }
1036 else
1037 session[sess].last_packet = time_now;
1038
1039 tunnelsend(p, lpppoe, t); // send it....
1040 }
1041
1042 void process_pppoe_sess(uint8_t *pack, int size)
1043 {
1044 //struct ethhdr *ethhdr = (struct ethhdr *)pack;
1045 struct pppoe_hdr *hdr = (struct pppoe_hdr *)(pack + ETH_HLEN);
1046 uint16_t lppp = ntohs(hdr->length);
1047 uint8_t *pppdata = (uint8_t *) hdr->tag;
1048 uint16_t proto, sid, t;
1049
1050 sid = ntohs(hdr->sid);
1051 t = TUNNEL_ID_PPPOE;
1052
1053 LOG_HEX(5, "RCV PPPOE Sess", pack, size);
1054
1055 if (sid >= MAXSESSION)
1056 {
1057 LOG(0, sid, t, "Received pppoe packet with invalid session ID\n");
1058 STAT(tunnel_rx_errors);
1059 return;
1060 }
1061
1062 if (session[sid].tunnel != t)
1063 {
1064 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1065
1066 LOG(1, sid, t, "ERROR process_pppoe_sess: Session is not a session pppoe\n");
1067 return;
1068 }
1069
1070 if (hdr->ver != 1)
1071 {
1072 LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported version %i)\n", hdr->ver);
1073 return;
1074 }
1075
1076 if (hdr->type != 1)
1077 {
1078 LOG(3, sid, t, "Error process_pppoe_sess: discarding packet (unsupported type %i)\n", hdr->type);
1079 return;
1080 }
1081
1082 if (lppp > 2 && pppdata[0] == 0xFF && pppdata[1] == 0x03)
1083 { // HDLC address header, discard
1084 LOG(5, sid, t, "pppoe_sess: HDLC address header, discard\n");
1085 pppdata += 2;
1086 lppp -= 2;
1087 }
1088 if (lppp < 2)
1089 {
1090 LOG(3, sid, t, "Error process_pppoe_sess: Short ppp length %d\n", lppp);
1091 return;
1092 }
1093 if (*pppdata & 1)
1094 {
1095 proto = *pppdata++;
1096 lppp--;
1097 }
1098 else
1099 {
1100 proto = ntohs(*(uint16_t *) pppdata);
1101 pppdata += 2;
1102 lppp -= 2;
1103 }
1104
1105 if (session[sid].forwardtosession)
1106 { // Must be forwaded to a remote lns tunnel l2tp
1107 pppoe_forwardto_session_rmlns(pack, size, sid, proto);
1108 return;
1109 }
1110
1111 if (proto == PPPPAP)
1112 {
1113 session[sid].last_packet = time_now;
1114 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1115 processpap(sid, t, pppdata, lppp);
1116 }
1117 else if (proto == PPPCHAP)
1118 {
1119 session[sid].last_packet = time_now;
1120 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1121 processchap(sid, t, pppdata, lppp);
1122 }
1123 else if (proto == PPPLCP)
1124 {
1125 session[sid].last_packet = time_now;
1126 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1127 processlcp(sid, t, pppdata, lppp);
1128 }
1129 else if (proto == PPPIPCP)
1130 {
1131 session[sid].last_packet = time_now;
1132 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1133 processipcp(sid, t, pppdata, lppp);
1134 }
1135 else if (proto == PPPIPV6CP && config->ipv6_prefix.s6_addr[0])
1136 {
1137 session[sid].last_packet = time_now;
1138 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1139 processipv6cp(sid, t, pppdata, lppp);
1140 }
1141 else if (proto == PPPCCP)
1142 {
1143 session[sid].last_packet = time_now;
1144 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1145 processccp(sid, t, pppdata, lppp);
1146 }
1147 else if (proto == PPPIP)
1148 {
1149 session[sid].last_packet = session[sid].last_data = time_now;
1150 if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1151 processipin(sid, t, pppdata, lppp);
1152 }
1153 else if (proto == PPPMP)
1154 {
1155 session[sid].last_packet = session[sid].last_data = time_now;
1156 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1157 processmpin(sid, t, pppdata, lppp);
1158 }
1159 else if (proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0])
1160 {
1161 session[sid].last_packet = session[sid].last_data = time_now;
1162 if (session[sid].walled_garden && !config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1163 processipv6in(sid, t, pppdata, lppp);
1164 }
1165 else if (session[sid].ppp.lcp == Opened)
1166 {
1167 session[sid].last_packet = time_now;
1168 if (!config->cluster_iam_master) { master_forward_pppoe_packet(pack, size, hdr->code); return; }
1169 protoreject(sid, t, pppdata, lppp, proto);
1170 }
1171 else
1172 {
1173 LOG(3, sid, t, "process_pppoe_sess: Unknown PPP protocol 0x%04X received in LCP %s state\n",
1174 proto, ppp_state(session[sid].ppp.lcp));
1175 }
1176 }
1177
1178 void pppoe_send_garp()
1179 {
1180 int s;
1181 struct ifreq ifr;
1182 uint8_t mac[6];
1183
1184 if (!*config->pppoe_if_to_bind)
1185 return;
1186
1187 s = socket(PF_INET, SOCK_DGRAM, 0);
1188 if (s < 0)
1189 {
1190 LOG(0, 0, 0, "Error creating socket for GARP: %s\n", strerror(errno));
1191 return;
1192 }
1193 memset(&ifr, 0, sizeof(ifr));
1194 strncpy(ifr.ifr_name, config->pppoe_if_to_bind, sizeof(ifr.ifr_name) - 1);
1195 if (ioctl(s, SIOCGIFHWADDR, &ifr) < 0)
1196 {
1197 LOG(0, 0, 0, "Error getting eth0 hardware address for GARP: %s\n", strerror(errno));
1198 close(s);
1199 return;
1200 }
1201 memcpy(mac, &ifr.ifr_hwaddr.sa_data, 6*sizeof(char));
1202 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0)
1203 {
1204 LOG(0, 0, 0, "Error getting eth0 interface index for GARP: %s\n", strerror(errno));
1205 close(s);
1206 return;
1207 }
1208 close(s);
1209
1210 sendarp(ifr.ifr_ifindex, mac, config->iftun_address);
1211 }
1212
1213 // rcv pppoe data from slave
1214 void pppoe_process_forward(uint8_t *pack, int size, in_addr_t addr)
1215 {
1216 struct ethhdr *ethhdr = (struct ethhdr *)pack;
1217
1218 if (ethhdr->h_proto == htons(ETH_P_PPP_DISC))
1219 process_pppoe_disc(pack, size);
1220 else if (ethhdr->h_proto == htons(ETH_P_PPP_SES))
1221 process_pppoe_sess(pack, size);
1222 else
1223 LOG(0, 0, 0, "pppoe_process_forward: I got a C_PPPOE_FORWARD from %s, but not a PPPOE data?\n", fmtaddr(addr, 0));
1224 }