1/* PPPoE support library "libpppoe"
2 *
3 * Copyright 2000 Michal Ostrowski <mostrows@styx.uwaterloo.ca>,
4 *		  Jamal Hadi Salim <hadi@cyberus.ca>
5 *
6 *  This program is free software; you can redistribute it and/or
7 *  modify it under the terms of the GNU General Public License
8 *  as published by the Free Software Foundation; either version
9 *  2 of the License, or (at your option) any later version.
10 */
11
12#include <pppoe.h>
13
14static int tag_map[] = { PTT_SRV_NAME,
15			 PTT_AC_NAME,
16			 PTT_HOST_UNIQ,
17			 PTT_AC_COOKIE,
18			 PTT_VENDOR,
19			 PTT_RELAY_SID,
20			 PTT_SRV_ERR,
21			 PTT_SYS_ERR,
22			 PTT_GEN_ERR,
23			 PTT_EOL
24};
25
26int disc_sock=-1;
27
28int verify_packet( struct session *ses, struct pppoe_packet *p);
29
30#define TAG_DATA(type,tag_ptr) ((type *) ((struct pppoe_tag*)tag_ptr)->tag_data)
31
32
33/***************************************************************************
34 *
35 * Return the location where the next tag can be pu
36 *
37 **************************************************************************/
38static  struct pppoe_tag *next_tag(struct pppoe_hdr *ph)
39{
40    return (struct pppoe_tag *)
41	(((char *) &ph->tag) + ntohs(ph->length));
42}
43
44/**************************************************************************
45 *
46 * Update header to reflect the addition of a new tag
47 *
48 **************************************************************************/
49static  void add_tag(struct pppoe_hdr *ph, struct pppoe_tag *pt)
50{
51    int len = (ntohs(ph->length) +
52	       ntohs(pt->tag_len) +
53	       sizeof(struct pppoe_tag));
54
55    if (pt != next_tag(ph))
56	printf("PPPoE add_tag caller is buggy\n");
57
58    ph->length = htons(len);
59}
60
61/*************************************************************************
62 *
63 * Look for a tag of a specific type
64 *
65 ************************************************************************/
66struct pppoe_tag *get_tag(struct pppoe_hdr *ph, u_int16_t idx)
67{
68    char *end = (char *) next_tag(ph);
69    char *ptn = NULL;
70    struct pppoe_tag *pt = &ph->tag[0];
71
72    /*
73     * Keep processing tags while a tag header will still fit.
74     *
75     * This check will ensure that the entire tag header pointed
76     * to by pt will fit inside the message, and thus it will be
77     * valid to check the tag_type and tag_len fields.
78     */
79    while ((char *)(pt + 1) <= end) {
80	/*
81	 * If the tag data would go past the end of the packet, abort.
82	 */
83	ptn = (((char *) (pt + 1)) + ntohs(pt->tag_len));
84	if (ptn > end)
85	    return NULL;
86
87	if (pt->tag_type == idx)
88	    return pt;
89
90	pt = (struct pppoe_tag *) ptn;
91    }
92
93    return NULL;
94}
95
96/* We want to use tag names to reference into arrays  containing the tag data.
97   This takes an RFC 2516 tag identifier and maps it into a local one.
98   The reverse mapping is accomplished via the tag_map array */
99#define UNMAP_TAG(x) case PTT_##x : return TAG_##x
100static inline int tag_index(int tag){
101    switch(tag){
102	UNMAP_TAG(SRV_NAME);
103	UNMAP_TAG(AC_NAME);
104	UNMAP_TAG(HOST_UNIQ);
105	UNMAP_TAG(AC_COOKIE);
106	UNMAP_TAG(VENDOR);
107	UNMAP_TAG(RELAY_SID);
108	UNMAP_TAG(SRV_ERR);
109	UNMAP_TAG(SYS_ERR);
110	UNMAP_TAG(GEN_ERR);
111	UNMAP_TAG(EOL);
112    };
113    return -1;
114}
115
116/*************************************************************************
117 *
118 * Makes a copy of a tag into a PPPoE packe
119 *
120 ************************************************************************/
121void copy_tag(struct pppoe_packet *dest, struct pppoe_tag *pt)
122{
123    struct pppoe_tag *end_tag = get_tag(dest->hdr, PTT_EOL);
124    int tagid;
125    int tag_len;
126    if( !pt ) {
127	return;
128    }
129    tagid = tag_index(pt->tag_type);
130
131    tag_len = sizeof(struct pppoe_tag) + ntohs(pt->tag_len);
132
133    if( end_tag ){
134	memcpy(((char*)end_tag)+tag_len ,
135	       end_tag, sizeof(struct pppoe_tag));
136
137	dest->tags[tagid]=end_tag;
138	dest->tags[TAG_EOL] = (struct pppoe_tag*)((char*)dest->tags[TAG_EOL] + tag_len);
139	memcpy(end_tag, pt, tag_len);
140	dest->hdr->length = htons(ntohs(dest->hdr->length) + tag_len);
141
142    }else{
143	memcpy(next_tag(dest->hdr),pt, tag_len);
144	dest->tags[tagid]=next_tag(dest->hdr);
145	add_tag(dest->hdr,next_tag(dest->hdr));
146    }
147
148
149}
150
151
152/*************************************************************************
153 *
154 * Put tags from a packet into a nice array
155 *
156 ************************************************************************/
157static void extract_tags(struct pppoe_hdr *ph, struct pppoe_tag** buf){
158    int i=0;
159    for(;i<MAX_TAGS;++i){
160	buf[i] = get_tag(ph,tag_map[i]);
161    }
162}
163
164
165/*************************************************************************
166 *
167 * Verify that a packet has a tag containint a specific value
168 *
169 ************************************************************************/
170static int verify_tag(struct session* ses,
171		      struct pppoe_packet* p,
172		      unsigned short id,
173		      char* data,
174		      int data_len)
175{
176    int len;
177    struct pppoe_tag *pt = p->tags[id];
178
179    if( !pt ){
180	poe_info(ses,"Missing tag %d. Expected %s\n",
181		 id,data);
182	return 0;
183    }
184    len = ntohs(pt->tag_len);
185    if(len != data_len){
186	poe_info(ses,"Length mismatch on tag %d: expect: %d got: %d\n",
187		 id, data_len, len);
188	return 0;
189    }
190
191    if( 0!=memcmp(pt->tag_data,data,data_len)){
192	poe_info(ses,"Tag data mismatch on tag %d: expect: %s vs %s\n",
193		 id, data,pt->tag_data);
194	return 0;
195    }
196    return 1;
197}
198
199
200/*************************************************************************
201 *
202 * Verify the existence of an ethernet device.
203 * Construct an AF_PACKET address struct to match.
204 *
205 ************************************************************************/
206int get_sockaddr_ll(const char *devnam,struct sockaddr_ll* sll){
207    struct ifreq ifr;
208    int retval;
209
210    if(disc_sock<0){
211
212	disc_sock = socket(PF_PACKET, SOCK_DGRAM, 0);
213	if( disc_sock < 0 ){
214	    return -1;
215	}
216    }
217
218    strncpy(ifr.ifr_name, devnam, sizeof(ifr.ifr_name));
219
220    retval = ioctl( disc_sock , SIOCGIFINDEX, &ifr);
221
222    if( retval < 0 ){
223//	error("Bad device name: %s  (%m)",devnam);
224	return 0;
225    }
226
227    if(sll) sll->sll_ifindex = ifr.ifr_ifindex;
228
229    retval = ioctl (disc_sock, SIOCGIFHWADDR, &ifr);
230    if( retval < 0 ){
231//	error("Bad device name: %s  (%m)",devnam);
232	return 0;
233    }
234
235    if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
236	error("Interface %s is not Ethernet!", devnam);
237	return 0;
238    }
239    if(sll){
240	sll->sll_family	= AF_PACKET;
241	sll->sll_protocol= ntohs(ETH_P_PPP_DISC);
242	sll->sll_hatype	= ARPHRD_ETHER;
243	sll->sll_pkttype = PACKET_BROADCAST;
244	sll->sll_hatype	= ETH_ALEN;
245	memcpy( sll->sll_addr , ifr.ifr_hwaddr.sa_data, ETH_ALEN);
246    }
247    return 1;
248}
249
250
251
252
253/*************************************************************************
254 *
255 * Construct and send a discovery message.
256 *
257 ************************************************************************/
258int send_disc(struct session *ses, struct pppoe_packet *p)
259{
260    char buf[MAX_PAYLOAD + sizeof(struct pppoe_hdr)];
261    int data_len = sizeof(struct pppoe_hdr);
262
263    struct pppoe_hdr *ph = NULL;
264    struct pppoe_tag *tag = NULL;
265    int i, err = 0;
266    int got_host_uniq = 0;
267    int got_srv_name = 0;
268    int got_ac_name = 0;
269
270    for (i = 0; i < MAX_TAGS; i++) {
271	if (!p->tags[i])
272	    continue;
273
274	got_host_uniq |= (p->tags[i]->tag_type == PTT_HOST_UNIQ);
275
276	/* Relay identifiers qualify as HOST_UNIQ's:
277	   we need HOST_UNIQ to uniquely identify the packet,
278	   PTT_RELAY_SID is sufficient for us for outgoing packets */
279	got_host_uniq |= (p->tags[i]->tag_type == PTT_RELAY_SID);
280
281	got_srv_name |= (p->tags[i]->tag_type == PTT_SRV_NAME);
282	got_ac_name  |= (p->tags[i]->tag_type == PTT_AC_NAME);
283
284	data_len += (ntohs(p->tags[i]->tag_len) +
285		     sizeof(struct pppoe_tag));
286    }
287
288    ph = (struct pppoe_hdr *) buf;
289
290
291    memcpy(ph, p->hdr, sizeof(struct pppoe_hdr));
292    ph->length = __constant_htons(0);
293
294    /* if no HOST_UNIQ tags --- add one with process id */
295    if (!got_host_uniq){
296	data_len += (sizeof(struct pppoe_tag) +
297		     sizeof(struct session *));
298	tag = next_tag(ph);
299	tag->tag_type = PTT_HOST_UNIQ;
300	tag->tag_len = htons(sizeof(struct session *));
301	memcpy(tag->tag_data,
302	       &ses,
303	       sizeof(struct session *));
304
305	add_tag(ph, tag);
306    }
307
308    if( !got_srv_name ){
309	data_len += sizeof(struct pppoe_tag);
310	tag = next_tag(ph);
311	tag->tag_type = PTT_SRV_NAME;
312	tag->tag_len = 0;
313	add_tag(ph, tag);
314    }
315
316    if(!got_ac_name && ph->code==PADO_CODE){
317	data_len += sizeof(struct pppoe_tag);
318	tag = next_tag(ph);
319	tag->tag_type = PTT_AC_NAME;
320	tag->tag_len = 0;
321	add_tag(ph, tag);
322    }
323
324    for (i = 0; i < MAX_TAGS; i++) {
325	if (!p->tags[i])
326	    continue;
327
328	tag = next_tag(ph);
329	memcpy(tag, p->tags[i],
330	       sizeof(struct pppoe_tag) + ntohs(p->tags[i]->tag_len));
331
332	add_tag(ph, tag);
333    }
334
335    /* Now fixup the packet struct to make sure all of its pointers
336       are self-contained */
337    memcpy( p->hdr , ph, data_len );
338    extract_tags( p->hdr, p->tags);
339
340    err = sendto(disc_sock, buf, data_len, 0,
341		   (struct sockaddr*) &p->addr,
342		   sizeof(struct sockaddr_ll));
343
344    if(err < 0)
345	poe_error(ses,"sendto returned: %m\n");
346
347    return err;
348}
349
350/*************************************************************************
351 *
352 * Verify that a packet is legal
353 *
354 *************************************************************************/
355int verify_packet( struct session *ses, struct pppoe_packet *p){
356    struct session * hu_val;
357
358    /* This code here should do all of the error checking and
359       validation on the incoming packet */
360
361
362    /* If we receive any error tags, abort */
363#define CHECK_TAG(name, val)					\
364    if((NULL==p->tags[name])== val){				\
365	poe_error(ses,"Tag error: " #name );			\
366	return -1;						\
367    }
368
369
370    /* If packet is not directed to our MAC address, forget it */
371    if (ses->state == PADS_CODE) {
372	    if (memcmp(p->addr.sll_addr, ses->remote.sll_addr, ETH_ALEN)) {
373		poe_info(ses,"ETH_DEST mismatch: %E %E \n",p->addr.sll_addr, ses->remote.sll_addr);
374		return -1;
375	    }
376    }
377
378    CHECK_TAG(TAG_SRV_ERR,0);
379    CHECK_TAG(TAG_SYS_ERR,0);
380    CHECK_TAG(TAG_GEN_ERR,0);
381
382    /* A HOST_UNIQ must be present */
383    CHECK_TAG(TAG_HOST_UNIQ,1);
384
385    hu_val = *TAG_DATA(struct session* ,p->tags[TAG_HOST_UNIQ]);
386
387    if( hu_val != ses ){
388	poe_info(ses,"HOST_UNIQ mismatch: %08x %i\n",(int)hu_val,getpid());
389	return -1;
390    }
391
392    if(ses->filt->htag &&
393       !verify_tag(ses,p,TAG_HOST_UNIQ,ses->filt->htag->tag_data,(int)ntohs(ses->filt->htag->tag_len))) {
394	poe_info(ses,"HOST_UNIQ failure");
395	return -1;
396    }
397
398
399    if(ses->filt->ntag && ses->state == PADO_CODE &&
400       !verify_tag(ses,p,TAG_AC_NAME,ses->filt->ntag->tag_data,(int)ntohs(ses->filt->ntag->tag_len))){
401	poe_info(ses,"AC_NAME failure");
402	return -1;
403    }
404
405    if(ses->filt->stag &&
406       !verify_tag(ses,p,TAG_SRV_NAME,ses->filt->stag->tag_data,(int)ntohs(ses->filt->stag->tag_len))){
407	poe_info(ses,"SRV_NAME failure");
408	return -1;
409    }
410
411    return 0;
412}
413
414
415/*************************************************************************
416 *
417 * Receive and verify an incoming packet.
418 *
419 *************************************************************************/
420static int recv_disc( struct session *ses,
421		      struct pppoe_packet *p){
422    int error = 0;
423    unsigned int from_len = sizeof(struct sockaddr_ll);
424
425    p->hdr = (struct pppoe_hdr*)p->buf;
426
427    error = recvfrom( disc_sock, p->buf, 1500, 0,
428		      (struct sockaddr*)&p->addr, &from_len);
429
430    if(error < 0) return error;
431
432    extract_tags(p->hdr,p->tags);
433
434    return 1;
435}
436
437
438/*************************************************************************
439 *
440 * Send a PADT
441 *
442 *************************************************************************/
443int session_disconnect(struct session *ses){
444    struct pppoe_packet padt;
445
446    memset(&padt,0,sizeof(struct pppoe_packet));
447    memcpy(&padt.addr, &ses->remote, sizeof(struct sockaddr_ll));
448
449    padt.hdr = (struct pppoe_hdr*) ses->curr_pkt.buf;
450    padt.hdr->ver  = 1;
451    padt.hdr->type = 1;
452    padt.hdr->code = PADT_CODE;
453    padt.hdr->sid  = ses->sp.sa_addr.pppoe.sid;
454
455    send_disc(ses,&padt);
456    ses->sp.sa_addr.pppoe.sid = 0 ;
457    ses->state = PADO_CODE;
458    return 0;
459
460}
461
462
463/*************************************************************************
464 *
465 * Make a connection -- behaviour depends on callbacks specified in "ses"
466 *
467 *************************************************************************/
468int session_connect(struct session *ses)
469{
470
471    struct pppoe_packet *p_out=NULL;
472    struct pppoe_packet rcv_packet;
473    int ret;
474
475
476    if(ses->init_disc){
477	ret = (*ses->init_disc)(ses, NULL, &p_out);
478	if( ret != 0 ) return ret;
479    }
480
481    /* main discovery loop */
482
483
484    while(ses->retransmits < ses->retries || ses->retries==-1 ){
485
486	fd_set in;
487	struct timeval tv;
488	FD_ZERO(&in);
489
490	FD_SET(disc_sock,&in);
491
492	if(ses->retransmits < 0)
493	    ret = select(disc_sock+1, &in, NULL, NULL, NULL);
494	else {
495	    ++ses->retransmits;
496	    tv.tv_sec = 1 << ses->retransmits;
497	    tv.tv_usec = 0;
498again:
499	    ret = select(disc_sock+1, &in, NULL, NULL, &tv);
500	}
501	if( ret < 0 && errno != EINTR){
502	    return -1;
503	}
504	else 	if( ret == 0 ) {
505	    if( DEB_DISC )
506		poe_dbglog(ses, "Re-sending ...");
507
508	    if( ses->timeout ) {
509		ret = (*ses->timeout)(ses, NULL, &p_out);
510		if( ret != 0 )
511		    return ret;
512	    }
513	    else if(p_out)
514		send_disc(ses,p_out);
515
516	    continue;
517	}
518
519	ret = recv_disc(ses, &rcv_packet);
520	/* Should differentiate between system errors and
521	   bad packets and the like... */
522	if( ret < 0 && errno )
523	    return -1;
524
525	switch (rcv_packet.hdr->code) {
526
527	case PADI_CODE:
528	{
529	    if(ses->rcv_padi){
530		ret = (*ses->rcv_padi)(ses,&rcv_packet,&p_out);
531
532		if( ret != 0){
533		    return ret;
534		}
535	    }
536	    break;
537	}
538
539	case PADO_CODE:		/* wait for PADO */
540	{
541	    if(ses->rcv_pado){
542		ret = (*ses->rcv_pado)(ses,&rcv_packet,&p_out);
543
544		if( ret != 0){
545		    return ret;
546		}
547		else
548		    goto again;
549	    }
550	    break;
551	}
552
553	case PADR_CODE:
554	{
555	    if(ses->rcv_padr){
556		ret = (*ses->rcv_padr)(ses,&rcv_packet,&p_out);
557
558		if( ret != 0){
559		    return ret;
560		}
561	    }
562	    break;
563	}
564
565	case PADS_CODE:		/* wait for PADS */
566	{
567	    if(ses->rcv_pads){
568		ret = (*ses->rcv_pads)(ses,&rcv_packet,&p_out);
569
570		if( ret != 0){
571		    return ret;
572		}
573		else
574		    goto again;
575	    }
576	    break;
577	}
578
579	case PADT_CODE:
580	{
581	    if( rcv_packet.hdr->sid != ses->sp.sa_addr.pppoe.sid ){
582		--ses->retransmits;
583		continue;
584	    }
585	    if(ses->rcv_padt){
586		ret = (*ses->rcv_padt)(ses,&rcv_packet,&p_out);
587
588		if( ret != 0){
589		    return ret;
590		}
591		else
592		    goto again;
593	    }else{
594		poe_error (ses,"connection terminated");
595		return (-1);
596	    }
597	    break;
598	}
599	default:
600	    poe_error(ses,"invalid packet %P",&rcv_packet);
601	    return (-1);
602	}
603    }
604    return (0);
605}
606
607
608/*************************************************************************
609 *
610 * Register an ethernet address as a client of relaying services.
611 *
612 *************************************************************************/
613int add_client(char *addr)
614{
615    struct pppoe_con* pc = (struct pppoe_con*)malloc(sizeof(struct pppoe_con));
616    int ret;
617    if(!pc)
618	return -ENOMEM;
619
620    memset(pc, 0 , sizeof(struct pppoe_con));
621
622    memcpy(pc->client,addr, ETH_ALEN);
623    memcpy(pc->key, addr, ETH_ALEN);
624
625    pc->key_len = ETH_ALEN;
626
627    if( (ret=store_con(pc)) < 0 ){
628	free(pc);
629    }
630    return ret;
631
632}
633
634struct pppoe_tag *make_filter_tag(short type, short length, char* data){
635    struct pppoe_tag *pt =
636	(struct pppoe_tag* )malloc( sizeof(struct pppoe_tag) + length );
637
638    if(pt == NULL) return NULL;
639
640    pt->tag_len=htons(length);
641    pt->tag_type=type;
642
643    if(length>0 && data){
644	memcpy( pt+1, data, length);
645    }
646    return pt;
647}
648