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