/* Socks Server 5
 * Copyright (C) 2002 - 2006 by Matteo Ricchetti - <matteo.ricchetti@libero.it>

 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */


#include"SS5Main.h"
#include"SS5Defs.h"
#include"SS5Mod_socks5.h"
#include"SS5OpenLdap.h"
#include"SS5Mod_log.h"

S5RetCode InitModule( struct _module *m )
{
  m->MethodParsing=MethodParsing;
  m->RequestParsing=RequestParsing;
  m->UpstreamServing=UpstreamServing;
  m->ConnectServing=ConnectServing;
  m->BindServing=BindServing;
  m->UdpAssociateServing=UdpAssociateServing;
  m->UdpAssociateResponse=UdpAssociateResponse;
  m->AddMethod=AddMethod;
  m->FreeMethod=FreeMethod;
  m->GetMethod=GetMethod;
  m->AddRoute=AddRoute;
  m->FreeRoute=FreeRoute;
  m->GetRoute=GetRoute;
  m->AddProxy=AddProxy;
  m->FreeProxy=FreeProxy;
  m->GetProxy=GetProxy;

  return OK;
}

S5RetCode MethodParsing(struct _SS5MethodInfo *mi, struct _SS5ClientInfo *ci, struct _SS5Socks5Data *sd)
{
  register unsigned int i;

  char logString[128];

  pid_t pid;

  /*
  * Get child/thread pid
  */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  /*
   * Receive socks V5 method or V4 request
   */
  if( (sd->MethodBytesReceived=recv(ci->Socket,sd->MethodRequest,sizeof(sd->MethodRequest),0)) <= 0 ) {
    ERRNO(pid)
    return ERR;
  }

  if( (unsigned char)sd->MethodRequest[0] == SOCKS4_VERSION ) {
    mi->Ver=SOCKS4_VERSION;
    mi->NMeth=0;
    mi->NoAuth=ERR;
    mi->BasicAuth=ERR;
    memcpy(sd->TcpRequest,sd->MethodRequest, sd->MethodBytesReceived);
    sd->TcpRBytesReceived=sd->MethodBytesReceived;

    return OK;
  }
  else if( (unsigned char)sd->MethodRequest[0] == SOCKS5_VERSION ) {
    mi->Ver=SOCKS5_VERSION;
    mi->NMeth=(unsigned char)sd->MethodRequest[1];

    /*    TODO:
     *    Bisogna valorrizzare i campi noauth e basicauth a NO oppure SI
     *    in modo da evidenziare nessuna valorizzazione e quindi nessun
     *    metodo supportato
     */
    for(i=0;i<(mi->NMeth);i++) {
      if( (unsigned char)sd->MethodRequest[2+i] == 0 )
        mi->NoAuth=OK;
      else if ( (unsigned char)sd->MethodRequest[2+i] == 2 )
        mi->BasicAuth=OK;
    }
  }
  else {
    return ERR;
  }
  /*
   * Send response V5 to client containing supported methods
   */
  sd->MethodResponse[0]=5;

  switch( GetMethod(inet_network(ci->SrcAddr), ci->SrcPort) ) {
    case NOAUTH:    sd->MethodResponse[1] = NOAUTH;    mi->Method = (unsigned char)sd->MethodResponse[1];    break;
    case USRPWD:    sd->MethodResponse[1] = USRPWD;    mi->Method = (unsigned char)sd->MethodResponse[1];    break;
    case FAKEPWD:   sd->MethodResponse[1] = USRPWD;    mi->Method = FAKEPWD;                                 break;
    default:
      sd->MethodResponse[1] = (unsigned char)NOMETHOD;
      mi->Method = (unsigned char)sd->MethodResponse[1];
      if( (sd->MethodBytesSent = send(ci->Socket,sd->MethodResponse,sizeof(sd->MethodResponse),SS5_SEND_OPT)) == -1) {
        ERRNO(pid)
        return ERR;
      }
      return ERR;
    break;
  }

  if( (sd->MethodBytesSent = send(ci->Socket,sd->MethodResponse,sizeof(sd->MethodResponse),SS5_SEND_OPT)) == -1) {
    ERRNO(pid);
    return ERR;
  }
  return OK;
}


S5RetCode RequestParsing(struct _SS5ClientInfo *ci, struct _SS5Socks5Data *sd, struct _SS5RequestInfo *ri)
{
  unsigned int i,
               c,
	       len;

  char logString[128];

  pid_t pid;

  /*
  * Get child/thread pid
  */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  
  memset((char *)sd->TcpRequest, 0, sizeof(sd->TcpRequest));

  if( (sd->TcpRBytesReceived=recv(ci->Socket,sd->TcpRequest,sizeof(sd->TcpRequest),0)) <= 0 ) {
    ERRNO(pid)
    return ERR;
  }

  ri->Ver=(unsigned char)sd->TcpRequest[0];
  ri->Cmd=(unsigned char)sd->TcpRequest[1];

  switch( sd->TcpRequest[3] ) {
    case IPV4:
      /*
       * Destination address is dot notation
       */
      ri->ATyp=IPV4;

      ri->DstPort=0;
      ri->DstPort +=(unsigned char)sd->TcpRequest[8];
      ri->DstPort <<=8;
      ri->DstPort +=(unsigned char)sd->TcpRequest[9];

      snprintf(ri->DstAddr,sizeof(ri->DstAddr),"%hu.%hu.%hu.%hu",(unsigned char)sd->TcpRequest[4],
                                                                 (unsigned char)sd->TcpRequest[5],
                                                                 (unsigned char)sd->TcpRequest[6],
                                                                 (unsigned char)sd->TcpRequest[7]);
    break;

    case IPV6: /* Not supported */
      return (-1 * S5REQUEST_ADDNOTSUPPORT);
    break;

    case DOMAIN:
      /*
       * Destination address is FQDN
       */
      len=(unsigned char)sd->TcpRequest[4] + 5;
      ri->DstPort=0;
      ri->DstPort +=(unsigned char)sd->TcpRequest[len];
      ri->DstPort <<=8;
      ri->DstPort +=(unsigned char)sd->TcpRequest[len+1];

      for(c=0,i=5;i<len;i++,c++ )
        ri->DstAddr[c]=(unsigned char)sd->TcpRequest[i];

      ri->DstAddr[c]='\0';
      ri->ATyp=DOMAIN;
    break;
  }

  return OK;
}


S5RetCode UpstreamServing(struct _SS5UpstreamInfo *ui, struct _SS5ClientInfo *ci, struct _SS5RequestInfo *ri, int *applicationSocket, 
                          struct _SS5Socks5Data *sd, struct _SS5AuthInfo *ai)
{
  struct sockaddr_in applicationSsin,
                     bindInterfaceSsin;

  char logString[128];

  pid_t pid;

  struct in_addr in;

  unsigned short ip_a,
                 ip_b,
		 ip_c,
		 ip_d;

  /*
   *    Get child/thread pid
   */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  if ( (*applicationSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
    ERRNO(pid)
    return( -1 * S5REQUEST_ISERROR );
  }

  /*
   * SS5: set bind interface if configured
   */
  if( ROUTE() ) {
    if( (in.s_addr=(unsigned long int)GetRoute(inet_network(ci->SrcAddr), ai->Username)) ) {

      memset((char *)&bindInterfaceSsin, 0, sizeof(struct sockaddr_in));

      if( (bindInterfaceSsin.sin_addr.s_addr=in.s_addr) ) {
        bindInterfaceSsin.sin_family      = AF_INET;
        bindInterfaceSsin.sin_port        = htons(0);

        if ( bind(*applicationSocket, (struct sockaddr *)&bindInterfaceSsin, sizeof(struct sockaddr_in)) == -1 ) {
          ERRNO(pid)
          return( -1 * S5REQUEST_ISERROR );
        }
      }
    }
  }

  memset((char *)&applicationSsin, 0, sizeof(struct sockaddr_in));
  applicationSsin.sin_family      = AF_INET;
  applicationSsin.sin_port        = htons(ui->DstPort);
  applicationSsin.sin_addr.s_addr = (unsigned long int)ui->DstAddr;

  if( connect(*applicationSocket,(struct sockaddr *)&applicationSsin,sizeof(struct sockaddr_in)) != -1 ) {
    /* 
     * Proxy client method towards upstream socks server
     */
    if( send(*applicationSocket,sd->MethodRequest,sd->MethodBytesReceived,SS5_SEND_OPT) == -1) {
      ERRNO(pid)
      return( -1 * S5REQUEST_ISERROR );
    }
    if( recv(*applicationSocket,sd->MethodResponse,sizeof(sd->MethodResponse),0) <= 0 ) {
      ERRNO(pid)
      return( -1 * S5REQUEST_ISERROR );
    }
    /* 
     * Proxy client connect request towards upstream socks server
     */
    if( send(*applicationSocket,sd->TcpRequest,sd->TcpRBytesReceived,SS5_SEND_OPT) == -1) {
      ERRNO(pid)
      return( -1 * S5REQUEST_ISERROR );
    }
    if( ri->Cmd == BIND ) {
      /* 
       * Proxy client bind request towards upstream socks server
       */
      if( (sd->TcpRBytesReceived=recv(*applicationSocket,sd->Response,sizeof(sd->Response),0)) <= 0 ) {
        ERRNO(pid)
        return( -1 * S5REQUEST_ISERROR );
      }
      /* 
       * Intercept bind ip address: if equal 0.0.0.0, replace it
       */
      if( sd->Response[4] == 0 && sd->Response[5] == 0 && sd->Response[6] == 0 && sd->Response[7] == 0 ) {
        sscanf((const char *)ui->DstAddr,"%hu.%hu.%hu.%hu",&ip_a,&ip_b,&ip_c,&ip_d);
        sd->Response[4]=ip_a;
        sd->Response[5]=ip_b;
        sd->Response[6]=ip_c;
        sd->Response[7]=ip_d;
      }
      if( send(ci->Socket,sd->Response,sd->TcpRBytesReceived,SS5_SEND_OPT) == -1) {
        ERRNO(pid)
        return( -1 * S5REQUEST_ISERROR );
      }
    }
    return OK;
  }
  else {
    return( -1 * S5REQUEST_HOSTUNREACH );
  }
  return OK;
}


S5RetCode ConnectServing(struct _SS5ClientInfo *ci, struct _SS5RequestInfo *ri, struct _SS5AuthInfo *ai, int *applicationSocket, struct _SS5Socks5Data *sd)
{
  register unsigned int index;

  char logString[128];

  S5RetCode err=S5REQUEST_SUCCEDED;

  S5Limit resolvedHostNumber=0;

  struct _S5HostList resolvedHostList[MAXDNS_RESOLV];

  struct sockaddr_in applicationSsin,
                     bindInterfaceSsin;

  struct in_addr in;

  unsigned short ipA,
                 ipB,
                 ipC,
                 ipD;

  pid_t pid;

  /*
  * Get child/thread pid
  */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  /*
   * SS5: Resolve hostname before connecting
   */ 
  if( ri->ATyp == DOMAIN ) {
    if( S5ResolvHostName(ri, (struct _S5HostList *)resolvedHostList, &resolvedHostNumber) == ERR )
      err=S5REQUEST_ISERROR;
  }

  if( err == S5REQUEST_SUCCEDED ) {
    if ( (*applicationSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
      ERRNO(pid)
      err=S5REQUEST_ISERROR;
    }
    else { 
      /*
       * SS5: set bind interface if configured
       */ 
      if( ROUTE() ) {
        if( (in.s_addr=(unsigned long int)GetRoute(inet_network(ci->SrcAddr), ai->Username)) ) {

          memset((char *)&bindInterfaceSsin, 0, sizeof(struct sockaddr_in));
          if( (bindInterfaceSsin.sin_addr.s_addr=in.s_addr) )
          {
            bindInterfaceSsin.sin_family      = AF_INET;
            bindInterfaceSsin.sin_port        = htons(0);
    
            if ( bind(*applicationSocket, (struct sockaddr *)&bindInterfaceSsin, sizeof(struct sockaddr_in)) == -1 ) {
              ERRNO(pid)
              err=S5REQUEST_ISERROR;
            }
          }
        }
      }
    
      if( err == S5REQUEST_SUCCEDED ) {
        bzero((char *)&applicationSsin, sizeof(struct sockaddr_in));
        applicationSsin.sin_family      = AF_INET;
        applicationSsin.sin_port        = htons(ri->DstPort);
        applicationSsin.sin_addr.s_addr = inet_addr(ri->DstAddr);
    
        if( connect(*applicationSocket,(struct sockaddr *)&applicationSsin,sizeof(struct sockaddr_in)) == -1 ) {
          ERRNO(pid)
          err=S5REQUEST_CONNREFUSED;
          /*
           * Try connecting to other destinations in case of multiple dns answers
           */
          for(index=1;index<resolvedHostNumber;index++) {
            strncpy(ri->DstAddr,resolvedHostList[index].NextHost,sizeof(ri->DstAddr));
            applicationSsin.sin_addr.s_addr = inet_addr(ri->DstAddr);
     
            if( connect(*applicationSocket,(struct sockaddr *)&applicationSsin,sizeof(struct sockaddr_in)) != -1 ) {
              err=S5REQUEST_SUCCEDED;
              break;
            }
          }
        }
      }
    }
  }
    
  /*
   * Prepare and send socks V5 response
   */
  memcpy(sd->Response,sd->TcpRequest,32);

  sscanf((const char *)ri->DstAddr,"%hu.%hu.%hu.%hu",&ipA,&ipB,&ipC,&ipD);

  sd->Response[0]=5;
  sd->Response[1]=err;
  sd->Response[2]=0;
  sd->Response[3]=IPV4;
  sd->Response[4]=ipA;
  sd->Response[5]=ipB;
  sd->Response[6]=ipC;
  sd->Response[7]=ipD;
  sd->Response[8]=ri->DstPort >> 8;
  sd->Response[9]=(ri->DstPort << 8) >> 8;

  switch( ri->ATyp ) {
    /*
     *    Socks V5 Header is 10 bytes
     */
    case IPV4:
    case DOMAIN:
      if( send(ci->Socket,sd->Response,10,SS5_SEND_OPT) == -1) {
        ERRNO(pid)
        err=S5REQUEST_ISERROR;
      }
      break;
    /*
     *    Socks V5 Header is 22 bytes but IPV6 is not supported
     */
    case IPV6:    err=S5REQUEST_ADDNOTSUPPORT;    break;
  }

  if( err != S5REQUEST_SUCCEDED )
     return (-1 * err);
  else
    return OK;
}

S5RetCode BindServing(struct _SS5ClientInfo *ci, struct _SS5RequestInfo *ri, struct _SS5AuthInfo *ai, int *applicationSocket, struct _SS5Socks5Data *sd)
{
  unsigned int len,
               fd;
  int cb = 0;

  unsigned short ipA,
                 ipB,
                 ipC,
                 ipD;

  char addr[16];

  struct timeval tv;

  fd_set fdset;

  S5Limit resolvedHostNumber=1;

  struct _S5HostList resolvedHostList[MAXDNS_RESOLV];

  struct in_addr in;

  struct sockaddr_in applicationSsin,
                     clientBindSsin;

  char logString[128];

  S5RetCode err=S5REQUEST_SUCCEDED;

  pid_t pid;

  /*
  * Get child/thread pid
  */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  /*
   * SS5: Resolve hostname before binding 
   */ 
  if( ri->ATyp == DOMAIN ) {
    if( S5ResolvHostName(ri, (struct _S5HostList *)resolvedHostList, &resolvedHostNumber) == ERR ) {
      err = S5REQUEST_ISERROR;
    }
  }

  if( err == S5REQUEST_SUCCEDED ) {
    /*
     * Create application socket
     */
    if ((*applicationSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
      ERRNO(pid)
      err = S5REQUEST_ISERROR;
    }
    else {
      /*
       * Create client socket for bind
       */
      if ((cb = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
        ERRNO(pid)
        err = S5REQUEST_ISERROR;
      }
      else {
        memset((char *)&clientBindSsin, 0, sizeof(struct sockaddr_in));
        clientBindSsin.sin_family      = AF_INET;
        clientBindSsin.sin_port        = htons(0);
      
        /*
         * Look for the right interface for binding
         */
        if( S5GetBindIf(ri->DstAddr,addr) == ERR ) {
          /* Match with destination address in socks request */
          clientBindSsin.sin_addr.s_addr = htonl(INADDR_ANY);
        }
        else
          clientBindSsin.sin_addr.s_addr = inet_addr(addr);
      
        /*
         * SS5: set bind interface if configured
         */ 
        if( ROUTE() ) {
          if( (in.s_addr=(unsigned long int)GetRoute(inet_network(ci->SrcAddr), ai->Username)) )
            clientBindSsin.sin_addr.s_addr = in.s_addr;
        } 
        if (bind(cb, (struct sockaddr *)&clientBindSsin, sizeof(struct sockaddr_in)) == -1) {
          ERRNO(pid)
          err = S5REQUEST_ISERROR;
        }
        else {
          /*
           * Get clientbind info
           */
          len=sizeof(struct sockaddr);
          getsockname(cb,(struct sockaddr *)&clientBindSsin,&len);
        
          /*
           * SS5: listen for a queue length equal to one
           */ 
          if (listen(cb, 1) == -1) {
            ERRNO(pid)
            err = S5REQUEST_ISERROR;
          }
        }
      }
    }
  }

  in.s_addr=clientBindSsin.sin_addr.s_addr;
  strncpy(addr,(char *)inet_ntoa(in),sizeof(addr));

  sscanf((const char *)addr,"%hu.%hu.%hu.%hu",&ipA,&ipB,&ipC,&ipD);

  /*
   * Send socks response
   */
  memcpy(sd->Response,sd->TcpRequest,32);

  sd->Response[0]=5;
  sd->Response[1]=err;
  sd->Response[2]=0;
  sd->Response[3]=IPV4;
  sd->Response[4]=ipA;
  sd->Response[5]=ipB;
  sd->Response[6]=ipC;
  sd->Response[7]=ipD;
  sd->Response[8]=(ntohs(clientBindSsin.sin_port) >> 8);
  sd->Response[9]=((ntohs(clientBindSsin.sin_port) << 8) >> 8);

  switch( ri->ATyp ) {
    /*
     *    Socks V5 Header is 10 bytes
     */
    case IPV4:
    case DOMAIN:
      if( send(ci->Socket,sd->Response,10,SS5_SEND_OPT) == -1) {
        ERRNO(pid)
        return(-1 * S5REQUEST_ISERROR);
      }
      break;
    /*
     *    Socks V5 Header is 22 bytes but IPV6 is not supported
     */
    case IPV6:    return (-1 * S5REQUEST_ADDNOTSUPPORT);    break;
  }

  if( err == S5REQUEST_SUCCEDED ) {
    /* 
     * Wait for BIND_TIMEOUT before closing listen port
     */
    bzero((char *)&applicationSsin, sizeof(struct sockaddr_in));
    len = sizeof (struct sockaddr_in);
  
    FD_ZERO(&fdset);
    FD_SET(cb,&fdset);
  
    tv.tv_sec=BIND_TIMEOUT;
    tv.tv_usec=0;
  
    if( (fd=select(cb+1,&fdset,NULL,NULL,&tv)) ) {
      if( FD_ISSET(cb,&fdset) ) {
        if ((*applicationSocket = accept(cb, (struct sockaddr *)&applicationSsin, &len)) == -1) {
          ERRNO(pid)

          close(cb);
          return (-1 * S5REQUEST_TTLEXPIRED);
        }
      }
    }
    else {
      /*
       * Timeout expired accepting connection from remote application
       */
      close(cb);
      return (-1 * S5REQUEST_TTLEXPIRED);
    }
    /*
     * Socks response packet
     */
    sd->Response[1]=S5REQUEST_SUCCEDED;

    switch( ri->ATyp ) {
      /*
       *    Socks V5 Header is 10 bytes
       */
      case IPV4:
      case DOMAIN:
        if( send(ci->Socket,sd->Response,10,SS5_SEND_OPT) == -1) {
          ERRNO(pid)
          return(-1 * S5REQUEST_ISERROR);
        }
      break;
  
      /*
       *    Socks V5 Header is 22 bytes but IPV6 is not supported
       */
      case IPV6:    return (-1 * S5REQUEST_ADDNOTSUPPORT);    break;
    }
  }

  if( err != S5REQUEST_SUCCEDED )
     return (-1 * err);
  else
    return OK;

}


S5RetCode UdpAssociateServing(struct _SS5ClientInfo *ci, struct _SS5RequestInfo *ri, struct _SS5UdpRequestInfo *uri, struct _SS5UdpClientInfo *uci, int *applicationSocket, struct _SS5Socks5Data *sd, struct _SS5ProxyData *pd)
{
  register unsigned int count,index;

  unsigned int len,
               fd;

  unsigned short ipA,
                 ipB,
                 ipC,
                 ipD;

  char addr[16];

  char logString[128];

  pid_t pid;

  fd_set fdset;

  struct timeval tv;

  S5Limit resolvedHostNumber=1;

  struct _S5HostList resolvedHostList[MAXDNS_RESOLV];

  struct in_addr in;

  struct sockaddr_in serverbind_ssin,
                     clientBindSsin;

  S5RetCode err=S5REQUEST_SUCCEDED;

  /*
   *    Get child/thread pid
   */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  /*
   * SS5: Resolve hostname before binding
   */
  if( ri->ATyp == DOMAIN ) {
    if( S5ResolvHostName(ri, (struct _S5HostList *)resolvedHostList, &resolvedHostNumber) == ERR ) {
      err=S5REQUEST_ISERROR;
    }
  }

  if( err == S5REQUEST_SUCCEDED ) {
    /*
     * Create server socket vs client
     */
    if ((uci->Socket = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
      ERRNO(pid)
      err=S5REQUEST_ISERROR;
    }
    else {
      memset((char *)&serverbind_ssin, 0, sizeof(struct sockaddr_in));
      serverbind_ssin.sin_family      = AF_INET;
      serverbind_ssin.sin_port        = htons(0);
    
      /*
       * Look for the right interface to bind before
       * receiving socks request from client (UDP)
       */
      if( S5GetBindIf(ri->DstAddr,addr) ) {                
        /* Match with destination address in socks request */
        serverbind_ssin.sin_addr.s_addr = inet_addr(addr);	
      }
      else if( S5GetBindIf(ci->SrcAddr,addr) ) {
        /* Match with client source address*/
        serverbind_ssin.sin_addr.s_addr = inet_addr(addr);
      }
      else
        /* Bind ANY (if option set) */
        serverbind_ssin.sin_addr.s_addr = htonl(INADDR_ANY);
    
      /*
       * SS5: bind socket to manage request vs client
       */
      if( bind(uci->Socket,(struct sockaddr *)&serverbind_ssin,sizeof(struct sockaddr_in)) ) {
        ERRNO(pid)
        err=S5REQUEST_ISERROR;
      }
      else {
        /*
         * Get information about ip and port after bind operation
         * to send to client
         */
        len = sizeof (struct sockaddr_in);
        getsockname(uci->Socket,(struct sockaddr *)&serverbind_ssin,&len);
      }
    }
  }

  in.s_addr=serverbind_ssin.sin_addr.s_addr;
  strncpy(addr,(char *)inet_ntoa(in),sizeof(addr));

  sscanf((const char *)addr,"%hu.%hu.%hu.%hu",&ipA,&ipB,&ipC,&ipD);

  /*
   * SS5: create response to send to client
   */
  memset(sd->Response,0,sizeof(sd->Response));
  sd->Response[0]=5;
  sd->Response[1]=err;
  sd->Response[2]=0;
  sd->Response[4]=ipA;
  sd->Response[5]=ipB;
  sd->Response[6]=ipC;
  sd->Response[7]=ipD;
  sd->Response[8]=(ntohs(serverbind_ssin.sin_port) >> 8);
  sd->Response[9]=((ntohs(serverbind_ssin.sin_port) << 8) >> 8);

  switch( ri->ATyp ) {
    /* Socks V5 Header 10 Bytes */
    case IPV4:
      sd->Response[3]=IPV4;
      if( send(ci->Socket,sd->Response,10,SS5_SEND_OPT) == -1) {
        ERRNO(pid)
        return( -1 * S5REQUEST_ISERROR);
      }
    break;
    case DOMAIN:
    /* Socks V5 Header 10 Bytes */
      sd->Response[3]=DOMAIN;
      if( send(ci->Socket,sd->Response,10,SS5_SEND_OPT) == -1) {
        ERRNO(pid)
        return( -1 * S5REQUEST_ISERROR);
      }
    break;
    /*
     *    Socks V5 Header is 22 Bytesi but is not supported
     */
    case IPV6:    return (-1 * S5REQUEST_ADDNOTSUPPORT);    break;
  }

  if( err == S5REQUEST_SUCCEDED ) {
    /*
     * Wait for receiving socks V5 request from client
     * until UDP_TIMEOUT value
     */
    FD_ZERO(&fdset);
    FD_SET(uci->Socket,&fdset);
  
    tv.tv_sec=UDP_TIMEOUT;
    tv.tv_usec=0;
  
    len=sizeof(struct sockaddr_in);
    memset(sd->UdpRequest,0,sizeof(sd->UdpRequest));
  
    if( (fd=select(uci->Socket+1,&fdset,NULL,NULL,&tv)) ) {
      if( FD_ISSET(uci->Socket,&fdset) ) {
        if( (sd->UdpRBytesReceived=recvfrom(uci->Socket,sd->UdpRequest,sizeof(sd->UdpRequest),0,(struct sockaddr *)&clientBindSsin,(socklen_t *)&len)) == -1 ) {
          ERRNO(pid)
          return( -1 * S5REQUEST_ISERROR);
        }
      }
    }
    else {
      /* UDP timeout expired */
      return( -1 * S5REQUEST_TTLEXPIRED);
    }
  
    /*
     * Set udp request info
     */
    uri->ATyp=(unsigned char)sd->UdpRequest[3]; 
    uri->Frag=(unsigned char)sd->UdpRequest[2];
  
    /*
     * Check for fragmentation bit set
     */
    if( uri->Frag ) {
      return( -1 * S5REQUEST_ISERROR);
    } 
    /*
     * Get remote peer address and port 
     * to set udp client info
     */
    len=sizeof(struct sockaddr);
    getpeername(uci->Socket,(struct sockaddr *)&clientBindSsin,&len);
  
    in.s_addr=clientBindSsin.sin_addr.s_addr;
    strncpy(addr,(char *)inet_ntoa(in),sizeof(addr));
  
    sscanf((const char *)addr,"%hu.%hu.%hu.%hu",&ipA,&ipB,&ipC,&ipD);
  
    strncpy(uci->SrcAddr,addr,sizeof(uci->SrcAddr));
    uci->SrcPort=ntohs(clientBindSsin.sin_port);
  
    /*
     * Set udp request info
     */ 
    switch( uri->ATyp ) {
      case IPV4:
        uri->DstPort=0;
        uri->DstPort +=(unsigned char)sd->UdpRequest[8];
        uri->DstPort <<=8;
        uri->DstPort +=(unsigned char)sd->UdpRequest[9];
  
        snprintf(uri->DstAddr,sizeof(ri->DstAddr),"%hu.%hu.%hu.%hu",(unsigned char)sd->UdpRequest[4],
                                                                    (unsigned char)sd->UdpRequest[5],
                                                                    (unsigned char)sd->UdpRequest[6],
                                                                    (unsigned char)sd->UdpRequest[7]);
  
        /*
         * Move data after socks header to proxy buffer
         */
        for(count=0;count<((unsigned int)sd->UdpRBytesReceived-10);count++)
          pd->UdpSend[count]=sd->UdpRequest[count+10];
  
        pd->UdpSBufLen=sd->UdpRBytesReceived-10;
      break;
  
      case IPV6: 
        /* IPV6 is not supported */
        return (-1 * S5REQUEST_ADDNOTSUPPORT);
      break;
  
      case DOMAIN:
        len=(unsigned char)sd->UdpRequest[4] + 5;
  
        uri->DstPort=0;
        uri->DstPort +=(unsigned char)sd->UdpRequest[len];
        uri->DstPort <<=8;
        uri->DstPort +=(unsigned char)sd->UdpRequest[len+1];
  
        for(count=0,index=5;index<len;index++,count++ )
          uri->DstAddr[count]=sd->UdpRequest[index+2];
        uri->DstAddr[count]='\0';
  
        /*
         * Move data after socks header to proxy buffer
         */
        len=7+sd->UdpRequest[4];
        for(count=0;count<(sd->UdpRBytesReceived-len);count++)
          pd->UdpSend[count]=sd->UdpRequest[count+len];
        pd->UdpSBufLen=sd->UdpRBytesReceived-len;
      break;
    }
  
    /*
     * Verify client source address and port
     * are the same as specified in socks request
     */
    if( S5VerifyBind(uci,ri) == ERR ) {
      return( -1 * S5REQUEST_ACLDENY);
    }
  
    /*
     * SS5: Resolve hostname of udp destination address
     */ 
    if( ri->ATyp == DOMAIN )
      if( S5ResolvHostName((struct _SS5RequestInfo *)uri, (struct _S5HostList *)resolvedHostList, &resolvedHostNumber) == ERR )
        return( -1 * S5REQUEST_ISERROR);
  
    /*
     * SS5: create socket for application
     */ 
    if ((*applicationSocket = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
      ERRNO(pid)
      return( -1 * S5REQUEST_ISERROR);
    }
  }

  if( err != S5REQUEST_SUCCEDED )
     return (-1 * err);
  else
    return OK;
}


S5RetCode UdpAssociateResponse(struct _SS5UdpRequestInfo *uri, struct _SS5UdpClientInfo *uci, struct _SS5Socks5Data *sd, struct _SS5ProxyData *pd)
{
  register int count;

  unsigned int len;

  pid_t pid;

  char logString[128];

  unsigned short ipA,
                 ipB,
                 ipC,
                 ipD;

  struct sockaddr_in clientBindSsin;

  /*
   *    Get child/thread pid
   */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  memset((char *)&clientBindSsin, 0, sizeof(struct sockaddr_in));
  clientBindSsin.sin_family      = AF_INET;
  clientBindSsin.sin_port        = htons(uci->SrcPort);
  clientBindSsin.sin_addr.s_addr = inet_addr(uci->SrcAddr);

  sscanf(uci->SrcAddr,"%hu.%hu.%hu.%hu",&ipA,&ipB,&ipC,&ipD);

  memset(sd->UdpRequest,0,sizeof(sd->UdpRequest));

  switch( uri->ATyp ) {
    case IPV4:
    case DOMAIN:
      sd->UdpRequest[0]=0;
      sd->UdpRequest[1]=0;
      sd->UdpRequest[2]=uri->Frag;
      sd->UdpRequest[3]=uri->ATyp;
      sd->UdpRequest[4]=ipA;
      sd->UdpRequest[5]=ipB;
      sd->UdpRequest[6]=ipC;
      sd->UdpRequest[7]=ipD;
      sd->UdpRequest[8] = ((uci->SrcPort) >> 8);
      sd->UdpRequest[9] = ((uci->SrcPort<< 8) >> 8);
    break;
    /*
     *    Socks V5 Header is 22 bytes but IPV6 is not supported
     */
    case IPV6:    return (-1 * S5REQUEST_ADDNOTSUPPORT);    break;
  }

  /*
   * Send response to client
   */
  for(count=0;count<(pd->UdpRBufLen);count++)
    sd->UdpRequest[count+10]=pd->UdpRecv[count];

  /*
   * Relay application response to client
   */
  len=sizeof(struct sockaddr_in);

  if( (sd->UdpRBytesSent=sendto(uci->Socket,sd->UdpRequest,pd->UdpRBufLen+10,0,(struct sockaddr *)&clientBindSsin,(socklen_t)len)) == -1 ) {
    ERRNO(pid)
    return (-1 * S5REQUEST_ISERROR);
  }

  return OK;
}


S5RetCode S5GetBindIf( char *applicationIp, char *clientBind )
{
  int index;

  if( (index=S5IfMatch(applicationIp)) != -1 ) {
    strncpy(clientBind,S5Interface[index]->IP,sizeof(S5Interface[index]->IP) - 1);
    return OK;
  }
  return ERR;
}


S5RetCode S5ResolvHostName( struct _SS5RequestInfo *ri, struct _S5HostList *resolvedHostList, S5Limit *resolvedHostNumber)
{
  register unsigned int index,count;

  struct hostent *dhost;

  char logString[128];

  pid_t pid;

  /*
  * Get child/thread pid
  */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  if( (dhost=(struct hostent *)gethostbyname(ri->DstAddr)) == NULL ) {
    /* TODO: handle h_error */
    return ERR;
  }
  /* 
   * In case of multiple answers, save all 
   */
  *resolvedHostNumber=0;
  for (index=0; dhost->h_addr_list[index] && index < MAXDNS_RESOLV; index++) {
    strncpy(resolvedHostList[index].NextHost,(char *)inet_ntoa(*(struct in_addr *)dhost->h_addr_list[index]),sizeof(resolvedHostList[index].NextHost));
    *resolvedHostNumber=*resolvedHostNumber + 1;
  }
  if( index == MAXDNS_RESOLV ) {
    if( VERBOSE() ) {
      snprintf(logString,128,"[%u] [VERB] Max number of multiple dns records reached while resolving destination: %d.",pid, MAXDNS_RESOLV);
      LOGUPDATE()
    }
  }
  /* 
   * If request, order dns answers
   */
  if( SS5SocksOpt.DnsOrder ) {
    S5OrderIP(resolvedHostList, resolvedHostNumber);

    if( VERBOSE() ) {
      snprintf(logString,128,"[%u] [VERB] Ordering multiple records dns.",pid);
      LOGUPDATE()

      for(count=0;count<*resolvedHostNumber; count++) {
        snprintf(logString,128,"[%u] [VERB] Resolved %s to %s.",pid,ri->DstAddr,resolvedHostList[count].NextHost);
        LOGUPDATE()
      }
    }
  }

  strncpy(ri->DstAddr,resolvedHostList[0].NextHost,sizeof(ri->DstAddr));

  return OK;
}

S5RetCode S5OrderIP( struct _S5HostList *resolvedHostList, S5Limit *resolvedHostNumber )
{
  register unsigned int index;

  unsigned int swap;
  
  char ip_tmp[16];

  do {
    swap=0;
    for(index=0;index<(*resolvedHostNumber)-1;index++)
      if( S5CompIP(resolvedHostList[index].NextHost,resolvedHostList[index+1].NextHost) ) {
        strncpy(ip_tmp,resolvedHostList[index+1].NextHost,sizeof(resolvedHostList[index+1].NextHost) - 1);
        strncpy(resolvedHostList[index+1].NextHost,resolvedHostList[index].NextHost,sizeof(resolvedHostList[index].NextHost) - 1);
	strncpy(resolvedHostList[index].NextHost,ip_tmp,sizeof(ip_tmp) - 1);
	swap=1;
      }
  } while(swap);

  return OK;
}

S5RetCode S5CompIP(char src[16],char dst[16] )
{
  unsigned short ips_a,
                 ips_b,
		 ips_c,
		 ips_d,
                 ipd_a,
		 ipd_b,
		 ipd_c,
		 ipd_d;

  sscanf(src,"%hu.%hu.%hu.%hu",&ips_a,&ips_b,&ips_c,&ips_d);
  sscanf(dst,"%hu.%hu.%hu.%hu",&ipd_a,&ipd_b,&ipd_c,&ipd_d);

  if( (ips_a > ipd_a) )
    return OK;
  else if( (ipd_a >ips_a) )
    return ERR;
  else if( (ips_b >ipd_b) )
    return OK;
  else if( (ipd_b >ips_b) )
    return ERR;
  else if( (ips_c >ipd_c) )
    return OK;
  else if( (ipd_c >ips_c) )
    return ERR;
  else if( (ips_d >ipd_d) )
    return OK;
  else if( (ipd_d >ips_d) )
    return ERR;

  return ERR;
}

S5RetCode S5VerifyBind(struct _SS5UdpClientInfo *uci, struct _SS5RequestInfo *ri )
{
  if( STREQ(uci->SrcAddr,ri->DstAddr,sizeof(uci->SrcAddr) - 1) && (uci->SrcPort == ri->DstPort) )
    return OK;
  else if ( STREQ(ri->DstAddr,"0.0.0.0",sizeof("0.0.0.0") - 1) && (uci->SrcPort == ri->DstPort) )
    return OK;
  else
    return ERR;
}

S5RetCode S5IfMatch(char ip[16])
{
  unsigned int count;

  unsigned short ipb_a,
                 ipb_b,
                 ipb_c,
                 ipb_d;
  unsigned short ips_a,
                 ips_b,
                 ips_c,
                 ips_d;
  unsigned short msk_a,
                 msk_b,
                 msk_c,
                 msk_d;

  sscanf((const char *)ip,"%hu.%hu.%hu.%hu",&ipb_a,&ipb_b,&ipb_c,&ipb_d);

  for(count=0;count<NInterF;count++) {
    sscanf((const char *)S5Interface[count]->IP,"%hu.%hu.%hu.%hu",&ips_a,&ips_b,&ips_c,&ips_d);
    sscanf(S5Interface[count]->NetMask,"%hu.%hu.%hu.%hu",&msk_a,&msk_b,&msk_c,&msk_d);

    if( !((ips_a & msk_a) - (ipb_a & msk_a) + (ips_b & msk_b) - (ipb_b & msk_b) + (ips_c & msk_c) -  \
          (ipb_c & msk_c) + (ips_d & msk_d) - (ipb_d & msk_d)) )
      return count;
  }
  return -1;
}

inline S5RetCode FileCheck( char *group, char *user )
{
  FILE *groupFile;

  pid_t pid;

  char groupFileName[192];
  char userName[64];

  char logString[128];

   /*
   *    Get child/thread pid
   */
  if( NOTTHREADED() )
    pid=getpid();
  else
    pid=(unsigned int)pthread_self();

  if( SS5SocksOpt.Profiling == FILE_PROFILING ) {
    strncpy(groupFileName,S5ProfilePath,sizeof(groupFileName));
    strncat(groupFileName,"/",strlen("/"));
    strncat(groupFileName,group,strlen(group));
    strncat(groupFileName,"\0",strlen("\0"));
    if( (groupFile = fopen(groupFileName,"r")) == NULL ) {
      ERRNO(pid)
      return ERR;
    }

    /*
     *    Check for username into configuration file for access profile
     */
    while( fscanf(groupFile,"%64s",userName) != EOF ) {
      if( userName[0] != '#' )
        if( STRCASEEQ(userName,user,64) ) {
          fclose(groupFile);
          return OK;
        }
    }
    fclose(groupFile);
    return ERR;
  }
  return ERR;
}


/* ***************************** HASH for ROUTING TABLE **************************** */
inline S5Limit S5RouteHash( unsigned long int sa )
{
  return ( sa % MAXROUTELIST );
}

unsigned long int GetRoute(unsigned long int sa, char *uname)
{
  S5Limit index,nm;
  S5RetCode err = ERR;
  struct _S5RouteNode *node;
  unsigned long int n_sa;

  for(nm=0;nm<=32;nm +=1) {
    if( nm < 32)
      n_sa=((sa >> nm) << nm);
    else
      n_sa=0;
    index=S5RouteHash( n_sa );

    if( S5RouteList[index] != NULL ) {
      node=S5RouteList[index];

      do {
        if( (node->SrcAddr == n_sa) && (node->Mask == (nm)) ) {
  
          if( node->Group[0] != '-' ) {
            /*
             * Look for username into group (file or directory) defined in permit line
             */
            if( SS5SocksOpt.Profiling == FILE_PROFILING )
              err=FileCheck(node->Group,uname);
            else if( SS5SocksOpt.Profiling == LDAP_PROFILING )
              err=DirectoryCheck(node->Group,uname);
            if( err ) {
              return node->SrcIf;
            }
          }
          else
            return node->SrcIf;
        }
        node=node->next;
      } while(node != NULL );
    }
  }

  return ERR;
}

S5RetCode AddRoute(unsigned long int sa, unsigned long int si, char *group, unsigned int mask )
{
  int index;
  struct _S5RouteNode *node;

  index=S5RouteHash( sa );

  if( _tmp_S5RouteList[index]== NULL ) {
    _tmp_S5RouteList[index]=(struct _S5RouteNode *)calloc(1,sizeof(struct _S5RouteNode));
    _tmp_S5RouteList[index]->Mask=mask;
    _tmp_S5RouteList[index]->SrcAddr=sa;
    _tmp_S5RouteList[index]->SrcIf=si;
    strncpy(_tmp_S5RouteList[index]->Group,group,sizeof(_tmp_S5RouteList[index]->Group));
    _tmp_S5RouteList[index]->next=NULL;
  }
  else {
    node=_tmp_S5RouteList[index];
    while( node->next != NULL ){
      node=node->next;
    }
    node->next=(struct _S5RouteNode *)calloc(1,sizeof(struct _S5RouteNode));
    node->next->Mask=mask;
    node->next->SrcAddr=sa;
    node->next->SrcIf=si;
    strncpy(node->next->Group,group,sizeof(node->next->Group));
    node->next->next=NULL;
  }
  return OK;
}

S5RetCode FreeRoute( struct _S5RouteNode **node )
{
  struct _S5RouteNode *lnode;
  struct _S5RouteNode *lnode_prev=NULL;

  lnode=*node;

  if( lnode != NULL ) {
    do {
      while( lnode->next != NULL ) {
        lnode_prev=lnode;
        lnode=lnode->next;
      }
      free(lnode);
      if( lnode_prev != NULL ) {
        lnode_prev->next=NULL;
        lnode=lnode_prev;
        lnode_prev=NULL;
      }
      else
        lnode=NULL;
    } while( (lnode) != NULL );
  }
  *node=NULL;

  return OK;
}

S5RetCode S5BrowseRouteList( struct _S5RouteNode *node )
{
  struct _S5RouteNode *lnode;

  int found=0;
  lnode=node;
  do {
    if(lnode != NULL ) {
      printf("Browse: %lu %u %lu\f", lnode->SrcAddr,lnode->Mask,lnode->SrcIf); 
      found++;
      lnode=lnode->next;
    }
  } while( lnode != NULL );
  return found;
}

/* ***************************** HASH for UPSTREAM PROXY **************************** */
inline S5Limit S5ProxyHash( unsigned long int da, unsigned int dp )
{
  register unsigned int i;
  register long int hashVal = 0;
  register unsigned int len;
  char s[24];

  snprintf(s,sizeof(s),"%lu%u",da,dp);

  len=strlen(s);
  for(i=0; i<len;i++)
    hashVal= 37*hashVal + s[i];

  hashVal %= 997;
  if(hashVal < 0)
    hashVal +=997;

  return hashVal;

}

S5RetCode GetProxy(unsigned long int da, unsigned int dp, struct _SS5UpstreamInfo *ui)
{
  register unsigned int index,nm;
  register unsigned long int n_da;
  struct _S5ProxyNode *node;

  for(nm=0;nm<=32;nm++) {
    if( nm < 32)
      n_da=((da >> nm) << nm);
    else
      n_da=0;

    index=S5ProxyHash( n_da, dp );

    if( S5ProxyList[index]!= NULL ) {
      node=S5ProxyList[index];
      do {
        if( (node->DstAddr == n_da) && (node->Mask == nm) && (node->DstPort == dp) ) {
          ui->DstAddr=node->ProxyAddr;
          ui->DstPort=node->ProxyPort;
          if( node->Type == PROXY ) {
            return OK;
          }
          return ERR_NOPROXY;
        }
        node=node->next;
      } while(node != NULL );
    }
  }

  for(nm=0;nm<=32;nm++) {
    if( nm < 32)
      n_da=((da >> nm) << nm);
    else
      n_da=0;

    index=S5ProxyHash( n_da, 0 );

    if( S5ProxyList[index]!= NULL ) {
      node=S5ProxyList[index];
      do {
        if( (node->DstAddr == n_da ) && (node->Mask == nm) && (dp >= node->DstRangeMin) && (dp <= node->DstRangeMax) ) {
          ui->DstAddr=node->ProxyAddr;
          ui->DstPort=node->ProxyPort;
          if( node->Type == PROXY ) {
            return OK;
          }
          return ERR_NOPROXY; 
        }
        node=node->next;
      } while(node != NULL );
    }
  }

  return ERR;
}

S5RetCode AddProxy(unsigned int type, unsigned long int da, unsigned long int dp, unsigned long int pa, unsigned int pp, unsigned int mask )
{
  int index;
  struct _S5ProxyNode *node;

  if( dp > 65535 )
    index=S5ProxyHash( da, 0 );
  else
    index=S5ProxyHash( da, dp );

  if( _tmp_S5ProxyList[index]== NULL ) {
    _tmp_S5ProxyList[index]=(struct _S5ProxyNode *)calloc(1,sizeof(struct _S5ProxyNode));
    _tmp_S5ProxyList[index]->Mask=mask;
    _tmp_S5ProxyList[index]->DstAddr=da;
    _tmp_S5ProxyList[index]->Type=type;

    if( dp > 65535 ) {
      _tmp_S5ProxyList[index]->DstPort=0;
      _tmp_S5ProxyList[index]->DstRangeMax=dp;
      _tmp_S5ProxyList[index]->DstRangeMax >>= 16;
      _tmp_S5ProxyList[index]->DstRangeMax <<= 16;
      _tmp_S5ProxyList[index]->DstRangeMin = dp - _tmp_S5ProxyList[index]->DstRangeMax;
      _tmp_S5ProxyList[index]->DstRangeMax >>= 16;

    }
    else
      _tmp_S5ProxyList[index]->DstPort=dp;

    _tmp_S5ProxyList[index]->ProxyAddr=pa;
    _tmp_S5ProxyList[index]->ProxyPort=pp;
    _tmp_S5ProxyList[index]->next=NULL;
  }
  else {
    node=_tmp_S5ProxyList[index];
    while( node->next != NULL ){
      node=node->next;
    }
    node->next=(struct _S5ProxyNode *)calloc(1,sizeof(struct _S5ProxyNode));
    node->next->Mask=mask;
    node->next->DstAddr=da;
    node->next->Type=type;

  if( dp > 65535 ) {
      node->next->DstPort=0;
      node->next->DstRangeMax=dp;
      node->next->DstRangeMax >>= 16;
      node->next->DstRangeMax <<= 16;
      node->next->DstRangeMin = dp - node->next->DstRangeMax;
      node->next->DstRangeMax >>= 16;
    }
    else
      node->next->DstPort=dp;

    node->next->ProxyAddr=pa;
    node->next->ProxyPort=pp;
    node->next->next=NULL;
  }
  return OK;
}

S5RetCode FreeProxy( struct _S5ProxyNode **node )
{
  struct _S5ProxyNode *lnode;
  struct _S5ProxyNode *lnode_prev=NULL;

  lnode=*node;

  if( lnode != NULL ) {
    do {
      while( lnode->next != NULL ) {
        lnode_prev=lnode;
        lnode=lnode->next;
      }
      free(lnode);
      if( lnode_prev != NULL ) {
        lnode_prev->next=NULL;
        lnode=lnode_prev;
        lnode_prev=NULL;
      }
      else
        lnode=NULL;
    } while( (lnode) != NULL );
  }
  *node=NULL;
  
  return OK;

}

S5RetCode S5BrowseProxyList( struct _S5ProxyNode *node )
{
  struct _S5ProxyNode *lnode;
  int found=0;

  lnode=node;
  do {
    if(lnode != NULL ) {
      printf("%lu %u %lu %lu %u\f", lnode->DstAddr,lnode->Mask,lnode->DstPort,lnode->ProxyAddr,lnode->ProxyPort); 
      lnode=lnode->next;
      found++;
    }
  } while( lnode != NULL );

  return found;
}

/* ***************************** HASH for METHOD **************************** */
inline S5Limit S5MethodHash( unsigned long int sa, unsigned int sp )
{
  register unsigned int i,len;
  register int hashVal = 0;
  char s[24];

  snprintf(s,sizeof(s),"%lu%u",sa,sp);

  len=strlen(s);
  for(i=0; i<len;i++)
    hashVal= 37*hashVal + s[i];

  hashVal %= 997;
  if(hashVal < 0)
    hashVal +=997;

  return hashVal;

}

unsigned char GetMethod(unsigned long int sa, unsigned int sp)
{
  register unsigned int index,nm;
  register unsigned long int n_sa;
  struct _S5MethodNode *node;

  for(nm=0;nm<=32;nm++) {
    if( nm < 32)
      n_sa=((sa >> nm) << nm);
    else
      n_sa=0;

    index=S5MethodHash( n_sa, 0 );

    if( S5MethodList[index]!= NULL ) {
      node=S5MethodList[index];
      do {
        if( (node->SrcAddr == n_sa) && (node->Mask == nm) && (sp >= node->SrcRangeMin) && (sp <= node->SrcRangeMax) ) {
          return node->Method;
        }
        node=node->next;
      } while(node != NULL );
    }
  }

  for(nm=0;nm<=32;nm++) {
    if( nm < 32)
      n_sa=((sa >> nm) << nm);
    else
      n_sa=0;

    index=S5MethodHash( n_sa, sp );

    if( S5MethodList[index]!= NULL ) {
      node=S5MethodList[index];
      do {
        if( (node->SrcAddr == n_sa) && (node->Mask == nm) && (node->SrcPort == sp) ) {
          return node->Method;
        }
        node=node->next;
      } while(node != NULL );
    }
  }

  return ERR;
}

S5RetCode AddMethod(unsigned long int sa, unsigned long int sp, unsigned int me, unsigned int mask)
{
  int index;
  struct _S5MethodNode *node;

  if( sp > 65535 )
    index=S5MethodHash( sa, 0 );
  else
    index=S5MethodHash( sa, sp );

  if( _tmp_S5MethodList[index]== NULL ) {
    _tmp_S5MethodList[index]=(struct _S5MethodNode *)calloc(1,sizeof(struct _S5MethodNode));
    _tmp_S5MethodList[index]->Mask=mask;
    _tmp_S5MethodList[index]->SrcAddr=sa;

    if( sp > 65535 ) {
      _tmp_S5MethodList[index]->SrcPort=0;
      _tmp_S5MethodList[index]->SrcRangeMax=sp;
      _tmp_S5MethodList[index]->SrcRangeMax >>= 16;
      _tmp_S5MethodList[index]->SrcRangeMax <<= 16;
      _tmp_S5MethodList[index]->SrcRangeMin = sp - _tmp_S5MethodList[index]->SrcRangeMax;
      _tmp_S5MethodList[index]->SrcRangeMax >>= 16;
    }
    else
      _tmp_S5MethodList[index]->SrcPort=sp;

    _tmp_S5MethodList[index]->Method=me;
    _tmp_S5MethodList[index]->next=NULL;
  }
  else {
    node=_tmp_S5MethodList[index];
    while( node->next != NULL ){
      node=node->next;
    }
    node->next=(struct _S5MethodNode *)calloc(1,sizeof(struct _S5MethodNode));
    node->next->Mask=mask;
    node->next->SrcAddr=sa;

    if( sp > 65535 ) {
      node->next->SrcPort=0;
      node->next->SrcRangeMax=sp;
      node->next->SrcRangeMax >>= 16;
      node->next->SrcRangeMax <<= 16;
      node->next->SrcRangeMin = sp - node->next->SrcRangeMax;
      node->next->SrcRangeMax >>= 16;
    }
    else
      node->next->SrcPort=sp;

    node->next->Method=me;
    node->next->next=NULL;
  }
  return OK;
}

S5RetCode FreeMethod( struct _S5MethodNode **node )
{
  struct _S5MethodNode *lnode;
  struct _S5MethodNode *lnode_prev=NULL;

  lnode=*node;

  if( lnode != NULL ) {
    do {
      while( lnode->next != NULL ) {
        lnode_prev=lnode;
        lnode=lnode->next;
      }
      free(lnode);
      if( lnode_prev != NULL ) {
        lnode_prev->next=NULL;
        lnode=lnode_prev;
        lnode_prev=NULL;
      }
      else
        lnode=NULL;
    } while( (lnode) != NULL );
  }
  *node=NULL;
  
  return OK;

}

S5RetCode S5BrowseMethodList( struct _S5MethodNode *node )
{
  struct _S5MethodNode *lnode;
  int found=0;

  lnode=node;
  do {
    if(lnode != NULL ) {
      printf("Browse - P: %p - %lu %u %lu %u\f",(void *)lnode,lnode->SrcAddr,lnode->Mask,lnode->SrcPort,lnode->Method); 
      lnode=lnode->next;
      found++;
    }
  } while( lnode != NULL );

  return found;
}