release/src/router/ppp/pppd/plugins/pppoe/libpppoe.c

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