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