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