ha.c

Go to the documentation of this file.

/* 
 * Dynamic hierarchial IP tunnel
 *
 * Copyright (C) 1998-2001, Dynamics group, Russ Dill (UDHCP),Tero 
 * Hätinen, Joni Purojärvi & Antti Pyykkönen (DYNAMO GPOUP) 
 *
 * Home Agent daemon with udhcp-client and dhcp-support UDHCP implied 
 * on November 2007 by Tero Hätinen, Joni Purojärvi & Antti Pyykkönen
 * (DYNAMO GPOUP)
 *
 * udhcp DHCP client partially implied and modified by Tero Hätinen, 
 * Joni Purojärvi, Antti Pyykkönen November 2007
 * Original source by Russ Dill <Russ.Dill@asu.edu> in July 2001
 * 
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */


#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#define DEBUG_FLAG 'H'

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <stdio.h>
#include <arpa/inet.h>
#include <sys/un.h>
#include <net/if.h>

#include <pthread.h>

#include <sys/time.h>
#include <unistd.h>
#include <syslog.h>
#ifndef __USE_BSD
#define __USE_BSD /* for sigblock et al. */
#endif
#include <signal.h>
#include <time.h>
#include <stdlib.h>
#include <assert.h>
#include <errno.h>
#include <getopt.h>
#include <sys/uio.h>

#include "ha.h"
#include "list.h"
#include "hashtable.h"
#include "fileio.h"
#include "msgparser.h"
#include "agentadv.h"
#include "proxyarp.h"
#include "binding.h"
#include "auth.h"
#include "tunnel.h"
#include "debug.h"
#include "agentapi.h"
#include "util.h"
#include "fixed_fd_zero.h"
#include "dyn_ip.h"
#include "ha_config.h"
#include "agent_utils.h"
#include "md5_mac.h"
/*UDHCP files to include*/

#include "clientpacket.h"
#include "packet.h"
#include "script.h"
#include "socket.h"
#include "pidfile.h"


#define DEFAULT_SCRIPT  "/usr/share/udhcpc/default.script" 
/*UDHCP-clients config*/
struct client_config_t client_config = {
        /* Default options. */
        abort_if_no_lease: 0,
        foreground: 0,
        quit_after_lease: 0,
        background_if_no_lease: 0,
        interface: "eth2", //read and set on dynhad.conf, set on main!
        pidfile: NULL,
        script: DEFAULT_SCRIPT,
        clientid: NULL, 
        hostname: NULL,
        ifindex: 0,
        arp: "\0\0\0\0\0\0",            /* appease gcc-3.0 */
};

#define LISTEN_NONE 0
#define LISTEN_KERNEL 1
#define LISTEN_RAW 2
static int listen_mode;

/* util.c - common command line arguments */
extern int opt_foreground;
extern char *opt_config;

/* DYNAMO: Struct for mobilenodes with home address from DHCP etc.*/
struct dhcp_mobile {
 unsigned char *nai;
 int state;
 unsigned long requested_ip; /* = 0 */
 unsigned long server_addr;
 unsigned long timeout;
 int packet_num; /* = 0 */
 int fd;
 int signal_pipe[2];
 int nai_length;
 int lease; //lease time
 int spi; //prolly not needed
 };

struct dhcp_mobile dhcp_mobile_array[HA_DEFAULT_MAX_BINDINGS]; 

int cur_mobiles=0; //how many mobile nodes are in the array
int maxmobiles=HA_DEFAULT_MAX_BINDINGS; //max mobiles to place in array



/*dynamics defines */

#define ASSERT assert
#define MIN(x, y) ( (x) < (y) ? (x) : (y))

#define LOG2(lev, fmt, args...) { DEBUG(DEBUG_FLAG, fmt, ## args); \
  syslog(lev, fmt, ## args); }

#define MAX_ADV_DELAY 200000.0 /* wait random 0 - MAX_ADV_DELAY microseconds 
                                  befora sending agent advertisement (either 
                                  broadcast or unicast, answer to agent 
                                  solicitation */


/* dynamics function prototypes */

/* called before the home agent terminates to do some cleaning */
static void clean_up();

/* functions for handling requests, creating and destroying
 * tunnels (bindings) */
static void remove_tunnel(struct bindingentry *binding);
static void destroy_binding(struct bindingentry *binding);
static struct fa_spi_entry* get_fa_spi(int spi, struct in_addr addr);
static struct spi_entry* get_mn_spi(int spi);
static void init_interfaces(void);

/* dynamics global variables */

static struct ha_config config;
struct fa_nai_ext *ha_nai = NULL;
static HASH *tunnels;
static struct bindingtable *bindings;
static int bindingcount = 0;
static char *program_name; /* the name this program was run with */
static struct dynamics_ha_status stats;



/***************************************************************************************************************/


/**************************
* DYNAMO: DHCP-Mobile array handling
***************************/


int find_dhcp_mobile(unsigned char *nai, int nai_length)
{
int i, n, found;
        found=-1;
        for(i=0;i<cur_mobiles;i++)
        {       
                if(nai_length==dhcp_mobile_array[i].nai_length)
                {
                        n=memcmp(nai, dhcp_mobile_array[i].nai,nai_length);
                        if( n==0 )
                        {
                         found=i; 
                         break;
                        }
                }       
                                
        }
        
return found;

}


int add_dhcp_mobile(unsigned long ip, unsigned char *nai, int nai_length)
{
        if (cur_mobiles==maxmobiles) return 0;
        dhcp_mobile_array[cur_mobiles].requested_ip=ip;
        dhcp_mobile_array[cur_mobiles].timeout=0;
        dhcp_mobile_array[cur_mobiles].state=DHCPDISCOVER; 
        dhcp_mobile_array[cur_mobiles].nai= xmalloc(nai_length); 
        memcpy(dhcp_mobile_array[cur_mobiles].nai, nai, nai_length); 
        dhcp_mobile_array[cur_mobiles].nai_length=nai_length;
        cur_mobiles=cur_mobiles+1;
        return 1;
}


int del_dhcp_mobile(int i)
{
if(cur_mobiles <= i || i<0) return 0; //index out of array 
        free(dhcp_mobile_array[i].nai);
  dhcp_mobile_array[i].state=0;
        dhcp_mobile_array[i].requested_ip=0; 
        dhcp_mobile_array[i].server_addr=0;
        dhcp_mobile_array[i].timeout=0;
        dhcp_mobile_array[i].packet_num=0; 
        dhcp_mobile_array[i].fd=0;
        dhcp_mobile_array[i].signal_pipe[0]=0; 
  dhcp_mobile_array[i].signal_pipe[1]=0; 
        dhcp_mobile_array[i].nai_length=0;
        dhcp_mobile_array[i].lease=0; 
        dhcp_mobile_array[i].spi=0; 
        dhcp_mobile_array[i]=dhcp_mobile_array[cur_mobiles-1];
        cur_mobiles=cur_mobiles-1;
  return 1;

}


/*****************************
* UDHCP Client starts here
* DYNAMO: changed old global variables into dhcp_mobile_array
******************************/

/* just a little helper  */
static void change_mode(int new_mode, int selected_mobile)
{
        DEBUG(LOG_INFO, "entering %s listen mode",
        new_mode ? (new_mode == 1 ? "kernel" : "raw") : "none");
        close(dhcp_mobile_array[selected_mobile].fd);
        dhcp_mobile_array[selected_mobile].fd = -1;
        listen_mode = new_mode;
}
/* Exit and cleanup */
static void exit_client(int retval)
{
        pidfile_delete(client_config.pidfile);
        CLOSE_LOG();
        exit(retval);
}


/* Signal handler */
static void signal_handler(int sig, int selected_mobile)
{
        if (send(dhcp_mobile_array[selected_mobile].signal_pipe[1], &sig, sizeof(sig), MSG_DONTWAIT) < 0) {
                LOG(LOG_ERR, "Could not send signal: %s",
                        strerror(errno));
        }
}

/* perform a renew */
static void perform_renew(int selected_mobile)
{       

        LOG(LOG_INFO, "Performing a DHCP renew");
        switch (dhcp_mobile_array[selected_mobile].state) {
        case BOUND:
                change_mode(LISTEN_KERNEL, selected_mobile);
        case RENEWING:
        case REBINDING:
                dhcp_mobile_array[selected_mobile].state = RENEW_REQUESTED;
                break;
        case RENEW_REQUESTED: /* impatient are we? fine, square 1 */
                run_script(NULL, "deconfig");
        case REQUESTING:
        case RELEASED:
                change_mode(LISTEN_RAW, selected_mobile);
                dhcp_mobile_array[selected_mobile].state = INIT_SELECTING;
                break;
        case INIT_SELECTING: NULL;
        }

        /* start things over */
        dhcp_mobile_array[selected_mobile].packet_num = 0;

        /* Kill any timeouts because the user wants this to hurry along */
        dhcp_mobile_array[selected_mobile].timeout = 0;
}


/* perform a release */
static void perform_release(int selected_mobile)
{
        char buffer[16];
        
        struct in_addr temp_addr;

        /* send release packet */
        if (dhcp_mobile_array[selected_mobile].state == BOUND || dhcp_mobile_array[selected_mobile].state == RENEWING || dhcp_mobile_array[selected_mobile].state == REBINDING) {
                temp_addr.s_addr = dhcp_mobile_array[selected_mobile].server_addr;
                sprintf(buffer, "%s", inet_ntoa(temp_addr));
                temp_addr.s_addr = dhcp_mobile_array[selected_mobile].requested_ip; 
                LOG(LOG_INFO, "Unicasting a release of %s to %s", 
                                inet_ntoa(temp_addr), buffer);
                send_release(dhcp_mobile_array[selected_mobile].server_addr, dhcp_mobile_array[selected_mobile].requested_ip); /* unicast */
                run_script(NULL, "deconfig");
        }
        LOG(LOG_INFO, "Entering released state");

        change_mode(LISTEN_NONE, selected_mobile);
        dhcp_mobile_array[selected_mobile].state = RELEASED;
        dhcp_mobile_array[selected_mobile].timeout = 0;
}

/* Never called*/
static void background(void)
{
        int pid_fd;

        pid_fd = pidfile_acquire(client_config.pidfile); /* hold lock during fork. */
        while (pid_fd >= 0 && pid_fd < 3) pid_fd = dup(pid_fd); /* don't let daemon close it */
        if (daemon(0, 0) == -1) {
                perror("fork");
                exit_client(1);
        }
        client_config.foreground = 1; /* Do not fork again. */
        pidfile_write_release(pid_fd);
}

unsigned long udhcp(int selected_mobile, int flag)
{
        
        unsigned char *temp, *message;
        unsigned long t1 = 0, t2 = 0, xid = 0;
        unsigned long start = 0, lease;
        fd_set rfds;
        int retval;
        struct timeval tv;
        int c, len;
        struct dhcpMessage packet;
        struct in_addr temp_addr;
        int pid_fd;
        time_t now;
        int max_fd;
        int sig;
        

        if (flag == REQUESTIP)
        {
        if(dhcp_mobile_array[selected_mobile].state==2) return 0;       
         listen_mode = 0;
         dhcp_mobile_array[selected_mobile].state=1;    
        }


        if (flag == RELEASEIP)
        {
        perform_release(selected_mobile); 
        return 0;
        }

        if(flag == RENEWIP)
        {       
        perform_renew(selected_mobile);
        return dhcp_mobile_array[selected_mobile].requested_ip;
        }
                
        OPEN_LOG("udhcpc");
        
        pid_fd = pidfile_acquire(client_config.pidfile);
        pidfile_write_release(pid_fd);
        if (read_interface(client_config.interface, &client_config.ifindex, 
                           NULL, client_config.arp) < 0)
                exit_client(1); //should never exit in this version!

        /* 
        DYNAMO:Set NAI into client_id field     
        */
        
        int mal_big = dhcp_mobile_array[selected_mobile].nai_length+2;
        client_config.clientid = xmalloc(mal_big); 
        client_config.clientid[OPT_CODE] = DHCP_CLIENT_ID;
        client_config.clientid[OPT_LEN] = dhcp_mobile_array[selected_mobile].nai_length; 
        client_config.clientid[OPT_DATA] = 1; 
        memcpy(client_config.clientid + 2, dhcp_mobile_array[selected_mobile].nai, dhcp_mobile_array[selected_mobile].nai_length); 

        /* setup signal handlers */
        socketpair(AF_UNIX, SOCK_STREAM, 0, dhcp_mobile_array[selected_mobile].signal_pipe);
        signal(SIGUSR1, signal_handler);
        signal(SIGUSR2, signal_handler);
        signal(SIGTERM, signal_handler);
        
        dhcp_mobile_array[selected_mobile].state = INIT_SELECTING;
        
        change_mode(LISTEN_RAW, selected_mobile);

        for (;;) {

                tv.tv_sec = dhcp_mobile_array[selected_mobile].timeout - time(0);
                tv.tv_usec = 0;
                FD_ZERO(&rfds);

                if (listen_mode != LISTEN_NONE && dhcp_mobile_array[selected_mobile].fd < 0) {
                        if (listen_mode == LISTEN_KERNEL)
                                dhcp_mobile_array[selected_mobile].fd = listen_socket(INADDR_ANY, CLIENT_PORT, client_config.interface);
                        else
                                dhcp_mobile_array[selected_mobile].fd = raw_socket(client_config.ifindex);
                        if (dhcp_mobile_array[selected_mobile].fd < 0) {
                                LOG(LOG_ERR, "FATAL: couldn't listen on socket, %s", strerror(errno));
                                exit_client(0);
                        }
                }
                if (dhcp_mobile_array[selected_mobile].fd >= 0) FD_SET(dhcp_mobile_array[selected_mobile].fd, &rfds);
                FD_SET(dhcp_mobile_array[selected_mobile].signal_pipe[0], &rfds);               

                if (tv.tv_sec > 0) {
                        DEBUG(LOG_INFO, "Waiting on select...\n");
                        max_fd = dhcp_mobile_array[selected_mobile].signal_pipe[0] > dhcp_mobile_array[selected_mobile].fd ? dhcp_mobile_array[selected_mobile].signal_pipe[0] : dhcp_mobile_array[selected_mobile].fd;
                        retval = select(max_fd + 1, &rfds, NULL, NULL, &tv);
                } else retval = 0; /* If we already timed out, fall through */

                now = time(0);
                if (retval == 0) {
                        /* timeout dropped to zero */
                        switch (dhcp_mobile_array[selected_mobile].state) {
                        case INIT_SELECTING:
                                if (dhcp_mobile_array[selected_mobile].packet_num < 3) {
                                        if (dhcp_mobile_array[selected_mobile].packet_num == 0)
                                                xid = random_xid();

                                        /* send discover packet */
        
                                        send_discover(xid, dhcp_mobile_array[selected_mobile].requested_ip); /* broadcast */
        
                                        dhcp_mobile_array[selected_mobile].timeout = now + ((dhcp_mobile_array[selected_mobile].packet_num == 2) ? 4 : 2);
                                        dhcp_mobile_array[selected_mobile].packet_num++;
                                } else {
/*
                                        if (client_config.background_if_no_lease) {
                                                LOG(LOG_INFO, "No lease, forking to background.");
                                                background();
                                        } else if (client_config.abort_if_no_lease) {
                                                LOG(LOG_INFO, "No lease, failing.");
                                                exit_client(1);
                                        }
*/
                                        /* wait to try again */
                                        dhcp_mobile_array[selected_mobile].packet_num = 0;
                                        dhcp_mobile_array[selected_mobile].timeout = 0;
          LOG(LOG_INFO, "DHCP-server did not answer after 3 DHCPDISCOVERs");
          return -1; //failed after 3 discovers!
                                }
        
                                break;
                        case RENEW_REQUESTED:
                        case REQUESTING:
                                if (dhcp_mobile_array[selected_mobile].packet_num < 3) {
                                        /* send request packet */
        
                                        if (dhcp_mobile_array[selected_mobile].state == RENEW_REQUESTED)
                                                send_renew(xid, dhcp_mobile_array[selected_mobile].server_addr, dhcp_mobile_array[selected_mobile].requested_ip); /* unicast */
                                        else send_selecting(xid, dhcp_mobile_array[selected_mobile].server_addr, dhcp_mobile_array[selected_mobile].requested_ip); /* broadcast */
                                        
                                        dhcp_mobile_array[selected_mobile].timeout = now + ((dhcp_mobile_array[selected_mobile].packet_num == 2) ? 10 : 2);
                                        dhcp_mobile_array[selected_mobile].packet_num++;
                                } else {

                                        /* timed out, go back to init state */
                                        if (dhcp_mobile_array[selected_mobile].state == RENEW_REQUESTED) run_script(NULL, "deconfig");
                                        dhcp_mobile_array[selected_mobile].state = INIT_SELECTING;
                                        dhcp_mobile_array[selected_mobile].timeout = now;
                                        dhcp_mobile_array[selected_mobile].packet_num = 0;
                                        change_mode(LISTEN_RAW, selected_mobile);
                                }
                                break;
                        case BOUND:
                                /* Lease is starting to run out, time to enter renewing state */
                                dhcp_mobile_array[selected_mobile].state = RENEWING;
                                change_mode(LISTEN_KERNEL, selected_mobile);
        
                                DEBUG(LOG_INFO, "Entering renew state");
                                /* fall right through */
                        case RENEWING:
                                /* Either set a new T1, or enter REBINDING state */
                                if ((t2 - t1) <= (lease / 14400 + 1)) {
                                        /* timed out, enter rebinding state */
                                        dhcp_mobile_array[selected_mobile].state = REBINDING;
                                        dhcp_mobile_array[selected_mobile].timeout = now + (t2 - t1);
                                        DEBUG(LOG_INFO, "Entering rebinding state");
                                } else {
                                        /* send a request packet */
                                        send_renew(xid, dhcp_mobile_array[selected_mobile].server_addr, dhcp_mobile_array[selected_mobile].requested_ip); /* unicast */
                                        
                                        t1 = (t2 - t1) / 2 + t1;
                                        dhcp_mobile_array[selected_mobile].timeout = t1 + start;
                                }
                                break;
                        case REBINDING:
                                /* Either set a new T2, or enter INIT state */
                                if ((lease - t2) <= (lease / 14400 + 1)) {
                                        /* timed out, enter init state */
                                        dhcp_mobile_array[selected_mobile].state = INIT_SELECTING;
                                        LOG(LOG_INFO, "Lease lost, entering init state");
                                        run_script(NULL, "deconfig");
                                        dhcp_mobile_array[selected_mobile].timeout = now;
                                        dhcp_mobile_array[selected_mobile].packet_num = 0;
                                        change_mode(LISTEN_RAW, selected_mobile);
                                } else {
                                        /* send a request packet */
                                        send_renew(xid, 0, dhcp_mobile_array[selected_mobile].requested_ip); /* broadcast */

                                        t2 = (lease - t2) / 2 + t2;
                                        dhcp_mobile_array[selected_mobile].timeout = t2 + start;
                                }
                                break;
                        case RELEASED:
                                /* yah, I know, *you* say it would never happen */
                                dhcp_mobile_array[selected_mobile].timeout = 0;
                                break;
                        }

        
                } else if (retval > 0 && listen_mode != LISTEN_NONE && FD_ISSET(dhcp_mobile_array[selected_mobile].fd, &rfds)) {
                        /* a packet is ready, read it */
        
                        if (listen_mode == LISTEN_KERNEL)
                                len = get_packet(&packet, dhcp_mobile_array[selected_mobile].fd);
                        else len = get_raw_packet(&packet, dhcp_mobile_array[selected_mobile].fd);
                        
                        if (len == -1 && errno != EINTR) {
                                DEBUG(LOG_INFO, "error on read, %s, reopening socket", strerror(errno));
                                change_mode(listen_mode, selected_mobile); /* just close and reopen */
                        }
                        if (len < 0) continue;

                        
                        if (packet.xid != xid) {
                                DEBUG(LOG_INFO, "Ignoring XID %lx (our xid is %lx)",
                                        (unsigned long) packet.xid, xid);
                                continue;
                        }
                        
                        if ((message = get_option(&packet, DHCP_MESSAGE_TYPE)) == NULL) {
                                DEBUG(LOG_ERR, "couldnt get option from packet -- ignoring");
                                continue;
                        }
                        
                        switch (dhcp_mobile_array[selected_mobile].state) {
                        case INIT_SELECTING:
                                /* Must be a DHCPOFFER to one of our xid's */
                                if (*message == DHCPOFFER) {
                                        if ((temp = get_option(&packet, DHCP_SERVER_ID))) {
                                                memcpy(&dhcp_mobile_array[selected_mobile].server_addr, temp, 4);
                                                xid = packet.xid;
                                                dhcp_mobile_array[selected_mobile].requested_ip = packet.yiaddr; //ip in data struct
                                                
                                                /* enter requesting state */
                                                dhcp_mobile_array[selected_mobile].state = REQUESTING;
                                                dhcp_mobile_array[selected_mobile].timeout = now;
                                                dhcp_mobile_array[selected_mobile].packet_num = 0;
                                        } else {
                                                DEBUG(LOG_ERR, "No server ID in message");
                                        }
                                }
                                break;
                        case RENEW_REQUESTED:
                        case REQUESTING:
                        case RENEWING:
                        case REBINDING:
                                if (*message == DHCPACK) {
                                        if (!(temp = get_option(&packet, DHCP_LEASE_TIME))) {
                                                LOG(LOG_ERR, "No lease time with ACK, using 1 hour lease");
                                                lease = 60 * 60;
                                        } else {
                                                memcpy(&lease, temp, 4);
                                                lease = ntohl(lease);
                                        }
                                                
                                        /* enter bound state */
                                        t1 = lease / 2;
                                        
                                        /* little fixed point for n * .875 */
                                        t2 = (lease * 0x7) >> 3;
                                        temp_addr.s_addr = packet.yiaddr;
                                        LOG(LOG_INFO, "Lease of %s obtained, lease time %ld", 
                                                inet_ntoa(temp_addr), lease);
                                        start = now;
                                        dhcp_mobile_array[selected_mobile].timeout = t1 + start;
                                        dhcp_mobile_array[selected_mobile].requested_ip = packet.yiaddr;
                                        run_script(&packet,
                                                   ((dhcp_mobile_array[selected_mobile].state == RENEWING || dhcp_mobile_array[selected_mobile].state == REBINDING) ? "renew" : "bound"));

                                        dhcp_mobile_array[selected_mobile].state = BOUND;
                                        change_mode(LISTEN_NONE, selected_mobile);
                                        if (client_config.quit_after_lease){                    
                                                return dhcp_mobile_array[selected_mobile].requested_ip; 
                                                }
                                        if (!client_config.foreground){
                        
                                                return dhcp_mobile_array[selected_mobile].requested_ip;
                                                }

                                } else if (*message == DHCPNAK) {
                                        /* return to init state */
                                        LOG(LOG_INFO, "Received DHCP NAK");
                                        run_script(&packet, "nak");
                                        if (dhcp_mobile_array[selected_mobile].state != REQUESTING)
                                                run_script(NULL, "deconfig");
                                        dhcp_mobile_array[selected_mobile].state = INIT_SELECTING;
                                        dhcp_mobile_array[selected_mobile].timeout = now;
                                        dhcp_mobile_array[selected_mobile].requested_ip = 0;
                                        dhcp_mobile_array[selected_mobile].packet_num = 0;
                                        change_mode(LISTEN_RAW, selected_mobile);
                                        sleep(3); /* avoid excessive network traffic */
                                }
                                break;
                        /* case BOUND, RELEASED: - ignore all packets */
                        }       
                } else if (retval > 0 && FD_ISSET(dhcp_mobile_array[selected_mobile].signal_pipe[0], &rfds)) {
                        if (read(dhcp_mobile_array[selected_mobile].signal_pipe[0], &sig, sizeof(signal)) < 0) {
                                DEBUG(LOG_ERR, "Could not read signal: %s", 
                                        strerror(errno));
                                continue; /* probably just EINTR */
                        }
                        switch (sig) {
                        case SIGUSR1: 
                                perform_renew(selected_mobile);
                                break;
                        case SIGUSR2:
                                perform_release(selected_mobile);
                                break;
                        case SIGTERM:
                                LOG(LOG_INFO, "Received SIGTERM");
                                exit_client(0);
                        }
                } else if (retval == -1 && errno == EINTR) {
                        /* a signal was caught */               
                } else {
                        /* An error occured */
                        DEBUG(LOG_ERR, "Error on select");
                }
                
        }
        
        return 0;
}


/*****************************************
* Dynamics HA starts here
******************************************/

static void
set_ha_nai(void)
{
        if (ha_nai != NULL) {
                free(ha_nai);
                ha_nai = NULL;
        }
        if (config.ha_nai[0] != '\0') {
                ha_nai = (struct fa_nai_ext *)
                        malloc(sizeof(struct fa_nai_ext) +
                               strlen(config.ha_nai));
                if (ha_nai == NULL) {
                        DEBUG(DEBUG_FLAG, "Not enough memory for ha_nai\n");
                        clean_up(1);
                }
                ha_nai->type = VENDOR_EXT_TYPE2;
                ha_nai->reserved = 0;
                ha_nai->length = sizeof(struct fa_nai_ext) - 2 +
                        strlen(config.ha_nai);
                ha_nai->vendor_id = htonl(VENDOR_ID_DYNAMICS);
                ha_nai->sub_type = htons(VENDOR_EXT_DYNAMICS_FA_NAI);
                memcpy(ha_nai + 1, config.ha_nai, strlen(config.ha_nai));
        }
}


static void
reload_config(int sig)
{
        LOG2(LOG_INFO, "Received signal %d, rereading configuration\n", sig);
        cleanup_config(&config);

        if (load_config(&config, program_name,
                        opt_config == NULL ? HA_GLOBAL_CONF_FILE : opt_config)
            == FALSE) {
                LOG2(LOG_ALERT, "Reloading configuration file failed\n");
                clean_up(1);
        }
        init_interfaces();
        set_ha_nai();
        /* If the Port configuration is changed in the configuration
         * file, it will have no effect on a reload. If this functionality
         * is needed, the code should close the old sockets and reopen them
         * here. */
}


/**********************************************************************
 * clean up stuff
 **********************************************************************/

static int
binding_cleanup_iterator(struct node *node, void *dummy)
{
        struct bindingentry *data = (struct bindingentry *) node;

        ASSERT(data != NULL);
        
        binding_remove(data);
        remove_tunnel(data);
        destroy_binding(data);

        return 1;
}


static void
sig_handler(int sig)
{
        LOG2(LOG_INFO, "signal %d received, cleaning up\n", sig);
        clean_up(sig); /* calls clean_up */
}       


static void
clean_up(int value)
{
        /* destroy all hashes and finish nicely */

        if (bindings != NULL) {
                binding_iterator(bindings, binding_cleanup_iterator, NULL);
                binding_destroy(bindings);
        }
        if (tunnels != NULL)
                tunnel_destroy_hash(tunnels);

        /* remove old unix sockets */
        if (config.ha_api_read_socket_path[0] != '\0')
                unlink(config.ha_api_read_socket_path);
        if (config.ha_api_admin_socket_path[0] != '\0')
                unlink(config.ha_api_admin_socket_path);

        if (unlink(HA_PID_FILE) < 0) {
                LOG2(LOG_INFO, "clean_up() could not remove PID file ("
                     HA_PID_FILE "): %s\n", strerror(errno));
        }

        cleanup_config(&config);
        if (ha_nai != NULL) {
                free(ha_nai);
                ha_nai = NULL;
        }

        syslog(LOG_INFO, "%s home agent daemon version %s stopped\n",
               PACKAGE, VERSION);

        closelog();
        exit(value);
}

/**********************************************************************
 * API handling
 **********************************************************************/

static int
binding_get_tunnels_iter(struct node *node, void *data)
{
        struct bindingentry *binding = (struct bindingentry *) node;
        struct api_msg *api_msg = (struct api_msg *) data;
        dyn_tunnel_id id;

        if (api_msg->length + sizeof(dyn_tunnel_id) > API_DATA_SIZE) {
                DEBUG(DEBUG_FLAG, "API message overflow - skipping rest of "
                      "the bindings\n");
                return 0;
        }

        id.mn_addr.s_addr = binding->mn_addr.s_addr;
        id.ha_addr.s_addr = binding->ha_addr.s_addr;
        id.priv_ha = 0;

        memcpy((char *) api_msg->params + api_msg->length, &id,
               sizeof(dyn_tunnel_id));

        api_msg->length += sizeof(dyn_tunnel_id);

        return 1;
}


static struct bindingentry*
api_fetch_binding(struct api_msg *msg)
{
        dyn_tunnel_id *tid;
        struct bindingkey bkey;
        struct bindingentry *binding;

        if (msg->length != sizeof(dyn_tunnel_id)) {
                LOG2(LOG_ERR, "API message has wrong length (%i, expected %i)"
                     "\n", msg->length, sizeof(dyn_tunnel_id));
                return NULL;
        }
        tid = (dyn_tunnel_id *) msg->params;
        bkey.mn_addr = tid->mn_addr;
        if (config.sha_addr.s_addr != 0)
                bkey.ha_addr = config.sha_addr;
        else if (tid->ha_addr.s_addr != 0)
                bkey.ha_addr = tid->ha_addr;
        else {
                struct interface_entry *iface;
                iface = (struct interface_entry *)
                        list_get_first(&config.interfaces);
                if (iface == NULL)
                        return NULL;
                bkey.ha_addr = iface->addr;
        }
        bkey.priv_ha = config.priv_ha;
        binding = binding_fetch(bindings, &bkey);
        if (!binding) {
                DEBUG(DEBUG_FLAG, "   API call could not find the request "
                      "binding\n");
        }
        return binding;
}


static int
handle_api(int socket, int priv)
{
        struct sockaddr_un cli_addr;
        socklen_t cli_len;
        int n;
        struct api_msg recv_msg;
        struct api_msg send_msg;
        struct bindingentry *binding;
        struct dynamics_tunnel_info *info;
        struct dynamics_ha_status *status;

        memset(&cli_addr, 0, sizeof(cli_addr));
        cli_len = sizeof(cli_addr);

        n = recvfrom(socket, &recv_msg, sizeof(struct api_msg), 0,
                     (struct sockaddr*)&cli_addr,
                     &cli_len);

        LOG2(LOG_DEBUG, "API: received %d bytes from %s\n", n,
             cli_addr.sun_path);

        if (n < 0) {
                LOG2(LOG_ERR, "handle_api - recvfrom failed - %s\n",
                     strerror(errno));
                return -1;
        }

        if (priv)
                stats.apicalls_admin++;
        else
                stats.apicalls_read++;

        memset(&send_msg, 0, sizeof(struct api_msg));
        send_msg.type = API_REPLY_MSG;
        send_msg.code = API_UNDEFINED;
        send_msg.length = 0;

        switch (recv_msg.code) {
        case API_GET_TUNNELS:
                DEBUG(DEBUG_FLAG, "Received API_GET_TUNNELS\n");
                if (recv_msg.length != 0) {
                        LOG2(LOG_ERR,
                             "API_GET_TUNNELS message has wrong length\n");
                        send_msg.code = API_ILLEGAL_PARAMETERS;
                        break;
                }

                /* get the list of bindings into send_msg */
                if (binding_iterator(bindings, binding_get_tunnels_iter,
                                     &send_msg))
                        send_msg.code = API_SUCCESS;
                else
                        send_msg.code = API_INSUFFICIENT_SPACE;
                break;

        case API_GET_TUNNEL_INFO:
                DEBUG(DEBUG_FLAG, "Received API_GET_TUNNEL_INFO\n");
                binding = api_fetch_binding(&recv_msg);
                if (!binding) {
                        send_msg.code = API_FAILED;
                        break;
                }
                info = (struct dynamics_tunnel_info *) send_msg.params;
                memset(info, 0, sizeof(struct dynamics_tunnel_info));
                info->id.mn_addr.s_addr = binding->mn_addr.s_addr;
                info->id.ha_addr.s_addr = binding->ha_addr.s_addr;
                info->mn_addr.s_addr = binding->mn_addr.s_addr;
                info->co_addr.s_addr = binding->lower_addr.s_addr;
                info->ha_addr.s_addr = binding->ha_addr.s_addr;
                info->expiration_time = binding->exp_time;
                info->creation_time = binding->create_time;
                info->refresh_time = binding->mod_time;
                info->last_timestamp[0] = ntohl(binding->id[0]);
                info->last_timestamp[1] = ntohl(binding->id[1]);
                info->spi = binding->spi;
                info->confirmed = 1;
                info->timeout = binding->timeout;
                send_msg.code = API_SUCCESS;
                send_msg.length = sizeof(struct dynamics_tunnel_info);
                break;

        case API_DESTROY_TUNNEL:
                DEBUG(DEBUG_FLAG, "Received API_DESTROY_TUNNEL\n");
                if (!priv) {
                        send_msg.code = API_NOT_PERMITTED;
                        break;
                }
                binding = api_fetch_binding(&recv_msg);
                if (!binding) {
                        send_msg.code = API_FAILED;
                        break;
                }
                binding_remove(binding);
                remove_tunnel(binding);
                destroy_binding(binding);
                send_msg.length = 0;
                send_msg.code = API_SUCCESS;
                LOG2(LOG_NOTICE, "API call removed binding for MN %s\n",
                     inet_ntoa(binding->mn_addr));
                break;

        case API_GET_STATUS:
                DEBUG(DEBUG_FLAG, "Received API_GET_STATUS\n");
                if (recv_msg.length != 0) {
                        LOG2(LOG_ERR, "API_GET_STATUS message has wrong "
                             "length\n");
                        break;
                }
                status = (struct dynamics_ha_status *) send_msg.params;
                memcpy(status, &stats, sizeof(struct dynamics_ha_status));
                UNALIGNED_(&status->tunnel_count, &bindingcount);
                send_msg.length = sizeof(struct dynamics_ha_status);
                send_msg.code = API_SUCCESS;
                break;

        case API_RELOAD_CONFIG:
                /* Force a reload of the configuration */
                DEBUG(DEBUG_FLAG, "Received API_RELOAD_CONFIG\n");
                if (!priv) {
                        send_msg.code = API_NOT_PERMITTED;
                        break;
                }
                reload_config(1);
                DEBUG(DEBUG_FLAG, "Configuration_reloaded\n");
                send_msg.code = API_SUCCESS;
                break;

        default:
                LOG2(LOG_ERR, "Received unknown API command: %d\n",
                     recv_msg.code);
                send_msg.code = API_NOT_SUPPORTED;
                break;
        }

        return api_send(socket, &cli_addr, cli_len, &send_msg);
}

/**********************************************************************
 * Registration request handling
 **********************************************************************/

static int
pubkey_hash_ok(struct msg_key *pubkey, struct msg_key *hash)
{
        unsigned char mac[MD5_MAC_LEN];

        if (GET_KEY_LEN(hash) != MD5_MAC_LEN) {
                DEBUG(DEBUG_FLAG, "pubkey_hash_ok: invalid hash length: %i, "
                      "expected %i\n", GET_KEY_LEN(hash), MD5_MAC_LEN);
                return 0;
        }

        md5_mac((unsigned char *) "", 0, (unsigned char *) pubkey,
                GET_KEY_EXT_LEN(pubkey), mac);
        return (memcmp(mac, MSG_KEY_DATA(hash), MD5_MAC_LEN) == 0);
}


static int
check_fa_auth_ext(struct in_addr addr, const struct msg_extensions *ext)
{

        struct fa_spi_entry *fa_spi;

        fa_spi = get_fa_spi(0, addr);
        if (fa_spi == NULL) {
                /* no security association with the sender - authentication
                 * extension not required */
                if (ext->fh_auth != NULL) {
                        DEBUG(DEBUG_FLAG, "No FA-HA security association, but "
                              "request had FA-HA authentication extension\n");
                        return 1;
                }
                return 0;
        }

        if (ext->fh_auth == NULL && ext->sha_ha_auth != NULL &&
            config.sha_addr.s_addr != 0) {
                DEBUG(DEBUG_FLAG, "Checking sha_ha_auth\n");
                if (auth_check_vendor(fa_spi->alg, fa_spi->shared_secret,
                                      fa_spi->shared_secret_len,
                                      (unsigned char *) ext->req,
                                      ext->sha_ha_auth))
                        return 0;
                return 1;
        }

        DEBUG(DEBUG_FLAG, "Checking fh_auth\n");
        if (ext->fh_auth == NULL ||
            !auth_check(fa_spi->alg, fa_spi->shared_secret,
                        fa_spi->shared_secret_len, (unsigned char *) ext->req,
                        ext->fh_auth))
                return 1;
        else
                return 0;
}


#define AUTH_MAC_RFC2002 -100

static struct spi_entry *
validate_request(struct bindingentry *binding, struct in_addr faaddr,
                 const char *msg, int msg_len,
                 const struct msg_extensions *ext, int *code, int *auth_type)
{
        struct node *node;
        struct spi_entry *spi = NULL;
        time_t current_time;
        enum { NOT_FOUND, UNICAST_FOUND, BROADCAST_FOUND } own_addr_found;
        struct authorized_entry *auth = NULL;
        char faaddrstr[16];
        char mnaddrstr[16];
        struct ha_tunnel_data *t_data = NULL;

        if (binding != NULL)
                t_data = (struct ha_tunnel_data *) binding->data;
        dynamics_strlcpy(faaddrstr, inet_ntoa(faaddr), sizeof(faaddrstr));


        *code = REGREP_ACCEPTED;
        *auth_type = 0;

        /* check that it has the right extensions */
        if (ext->req == NULL || ext->mh_auth == NULL) {
                LOG2(LOG_WARNING,
                     "FA %s: message does not contain either request "
                     "and/or message authentication\n", faaddrstr);
                if (ext->mh_auth == NULL)
                        *code = REGREP_MN_FAILED_AUTH_HA;
                else
                        *code = REGREP_BAD_REQUEST_HA;
                return NULL;
        }
        
        dynamics_strlcpy(mnaddrstr, inet_ntoa(ext->req->home_addr),
                         sizeof(mnaddrstr));


        /***********************
        * DYNAMO: New check for MN auth without IP check
        * - Need to approve 0.0.0.0 ip with any spi
        ************************/
        for (node = list_get_first(&config.authorized_list); node != NULL;
             node = list_get_next(node)) {
                auth = (struct authorized_entry *) node;
                if (ntohl(ext->mh_auth->spi) >= auth->spi_low &&
                    ntohl(ext->mh_auth->spi) <= auth->spi_high) {
                        break;
                }
        }

        if (node == NULL) {
                LOG2(LOG_WARNING, "FA %s, MN %s: Not authorized\n", 
                     faaddrstr, mnaddrstr);
                *code = REGREP_ADMIN_PROHIBITED_HA;
                return NULL;
        }

        /* check MAC (MN-HA authentication extension) */
        spi = get_mn_spi(ntohl(ext->mh_auth->spi));
        if (spi == NULL) {
                LOG2(LOG_WARNING, "FA %s, MN %s: SPI %d not found\n", 
                     faaddrstr, mnaddrstr, ntohl(ext->mh_auth->spi));
                *code = REGREP_MN_FAILED_AUTH_HA;
                return NULL;
        }

        if (!auth_check(spi->auth_alg, spi->shared_secret,
                        spi->shared_secret_len,
                        (unsigned char *) msg, ext->mh_auth)) {
                if (spi->auth_alg != AUTH_ALG_MD5 ||
                    !auth_check(AUTH_ALG_MD5_RFC2002, spi->shared_secret,
                                spi->shared_secret_len,
                                (unsigned char *) msg, ext->mh_auth)) {
                        LOG2(LOG_WARNING, "FA %s, MN %s, SPI %d: Invalid MN-HA"
                             " authentication extension\n",
                             faaddrstr, mnaddrstr, spi->spi);
                        *code = REGREP_MN_FAILED_AUTH_HA;
                        return spi;
                } else {
                        /* non-standard authentication method used */
                        DEBUG(DEBUG_FLAG, "RFC 2002 MAC calculation "
                              "detected - trying to use the same method\n");
                        *auth_type = AUTH_MAC_RFC2002;
                }
        }

        if (ext->double_auth_ext > 0) {
                LOG2(LOG_WARNING, "FA %s, MN %s: duplicate authentication "
                     "extension\n", faaddrstr, mnaddrstr);
                if (ext->double_auth_ext & DOUBLE_MH_AUTH)
                        *code = REGREP_MN_FAILED_AUTH_HA;
                else if (ext->double_auth_ext & DOUBLE_FH_AUTH)
                        *code = REGREP_FA_FAILED_AUTH_HA;
                else
                        *code = REGREP_REASON_UNSPEC_HA;
                return spi;
        }

        /* check possible Foreign-Home Authentication Extension */
        if (check_fa_auth_ext(faaddr, ext) != 0) {
                LOG2(LOG_WARNING, "FA %s, MN %s: invalid/missing fh_auth\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_FA_FAILED_AUTH_HA;
                return spi;
        }

        /* check that MN-HA auth. ext. protects correct extensions */
        if (!auth_is_protected(ext->pubkey_hash, ext->mh_auth) ||
            !auth_is_protected(ext->priv_ha, ext->mh_auth) ||
            !auth_is_protected(ext->mn_keyreq, ext->mh_auth) ||
            !auth_is_protected(ext->mn_nai, ext->mh_auth)) {
                LOG2(LOG_WARNING, "FA %s, MN %s: MN-HA auth. ext. does not "
                     "protect all the required extensions\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_MN_FAILED_AUTH_HA;
        }

        /* check that MN-AAA auth. ext. protects correct extensions */
        if (ext->mn_fa_key_req_aaa != NULL &&
            (ext->mn_aaa_auth == NULL ||
             !auth_is_protected(ext->mn_fa_key_req_aaa, ext->mn_aaa_auth))) {
                LOG2(LOG_WARNING, "FA %s, MN %s: MN-AAA auth. ext. does not "
                     "protect MN-FA keyreq from AAA extensions\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_MN_FAILED_AUTH_HA;
        }
        if (ext->mn_ha_key_req_aaa != NULL &&
            (ext->mn_aaa_auth == NULL ||
             !auth_is_protected(ext->mn_ha_key_req_aaa, ext->mn_aaa_auth))) {
                LOG2(LOG_WARNING, "FA %s, MN %s: MN-AAA auth. ext. does not "
                     "protect MN-HA keyreq from AAA extensions\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_MN_FAILED_AUTH_HA;
        }

        if (ext->fh_auth != NULL &&
            !auth_is_protected(ext->challenge, ext->fh_auth)) {
                LOG2(LOG_WARNING, "FA %s, MN %s: FA-HA auth. ext. does not "
                     "protect all the required extensions\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_FA_FAILED_AUTH_HA;
        }

        /* check own (HA) address */
        own_addr_found = NOT_FOUND;
        for (node = list_get_first(&config.interfaces); node != NULL;
             node = list_get_next(node)) {
                struct interface_entry *iface =
                        (struct interface_entry *) node;
                if (iface->addr.s_addr == ext->req->ha_addr.s_addr) {
                        own_addr_found = UNICAST_FOUND;
                        break;
                }
                if (iface->ha_disc &&
                    iface->bcaddr.s_addr == ext->req->ha_addr.s_addr) {
                        own_addr_found = BROADCAST_FOUND;
                        break;
                }
        }

        if (own_addr_found != 1 &&
            (config.sha_addr.s_addr == 0 ||
             ext->req->ha_addr.s_addr != config.sha_addr.s_addr)) {
                if (own_addr_found == BROADCAST_FOUND)
                        DEBUG(DEBUG_FLAG,
                              "==> HA discovery, deny with unknown HA\n");
                else
                        LOG2(LOG_WARNING, "FA %s, MN %s: HA address does not "
                             "match\n", faaddrstr, mnaddrstr);
                *code = REGREP_UNKNOWN_HA_HA;
                return spi;
        }

        /* check option bits */
        if (ext->req->opts & REGREQ_SIMULTANEOUS_BINDINGS) {
                LOG2(LOG_INFO,
                     "FA %s, MN %s: Received request for unsupported "
                    "simultaneous_bindings\n", faaddrstr, mnaddrstr);
                /* this is ok, but inform the MN */
                *code = REGREP_ACCEPTED_NO_SB;
        }

        current_time = time(NULL);
        if (spi->replay_method == REPLAY_PROTECTION_TIMESTAMP &&
            (ntohl(ext->req->id[0]) < current_time + UNIX_NTP_DIFF -
             spi->timestamp_tolerance ||
             ntohl(ext->req->id[0]) > current_time + UNIX_NTP_DIFF +
             spi->timestamp_tolerance)) {
                time_t t;
                t = ntohl(ext->req->id[0]) - UNIX_NTP_DIFF;
                LOG2(LOG_WARNING, "ID mismatch - timestamp difference=%i, "
                     "tolerance=%i\n", abs(t - current_time),
                     spi->timestamp_tolerance);
                *code = REGREP_ID_MISMATCH_HA;
        } else if (spi->replay_method == REPLAY_PROTECTION_NONCE &&
                   t_data != NULL && ext->req->id[0] != t_data->nonce) {
                LOG2(LOG_WARNING, "ID mismatch (nonce)\n");
                DEBUG(DEBUG_FLAG, "\treq->id: %08x, expected nonce %08x\n",
                      ext->req->id[0], t_data->nonce);
                *code = REGREP_ID_MISMATCH_HA;
        }

        if (ext->req->opts & REGREQ_BROADCAST_DATAGRAMS) {
                DEBUG(LOG_INFO,
                      "FA %s, MN %s: Received request for unsupported "
                    "broadcast message forwarding\n", faaddrstr, mnaddrstr);
                /* this is not yet implemented in Dynamics HA, but accept the
                 * connection anyway; there is no accepted reply code that
                 * would indicate this situation, so MN will think that the
                 * request was accepted.. */
        }

        /* REGREQ_VJ_HC is ignored in HA, see RFC 2002, 3.8.2 */
        if (ext->req->opts & (REGREQ_MINIMAL_ENCAPS | REGREQ_GRE_ENCAPS)) {
                LOG2(LOG_INFO, "FA %s, MN %s: Received unsupported option "
                     "request (opts=0x%02x)\n", faaddrstr, mnaddrstr,
                     ext->req->opts);
                /* RFC 2344 defines new denial code for unavailable
                 * encapsulation */
                if ((ext->req->opts & REGREQ_REVERSE_TUNNEL) != 0)
                        *code = REGREP_ENCAP_UNAVAIL_HA;
                else
                        *code = REGREP_REASON_UNSPEC_HA;
                return spi;
        }

        if (!config.enable_reverse_tunneling &&
            (ext->req->opts & REGREQ_REVERSE_TUNNEL) != 0) {
                LOG2(LOG_INFO,
                     "FA %s, MN %s: Asked forbidden reverse tunneling\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_REVERSE_TUNNEL_UNAVAIL_HA;
                return spi;
        }

        if (!config.enable_triangle_tunneling &&
            (ext->req->opts & REGREQ_REVERSE_TUNNEL) == 0) {
                LOG2(LOG_INFO,
                     "FA %s, MN %s: Reverse tunneling mandatory\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_REVERSE_TUNNEL_MANDATORY_HA;
                return spi;
        }

        if (ext->req->opts & REGREQ_RESERVED) {
                LOG2(LOG_INFO, "FA %s, MN %s: Received a request with non-zero"
                     " reserved field\n", faaddrstr, mnaddrstr);
                *code = REGREP_BAD_REQUEST_HA;
                return spi;
        }

        if (config.pubkey_hash_method == HASH_METHOD_REQUIRE &&
            ext->fa_pubkey != NULL && ext->pubkey_hash == NULL) {
                LOG2(LOG_INFO, "FA %s, MN %s: Received a request with public "
                     "key, but without public key hash extension\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_ADMIN_PROHIBITED_HA;
                return spi;
        }

        if (config.pubkey_hash_method != HASH_METHOD_NONE &&
            ext->fa_pubkey != NULL &&
            ext->pubkey_hash != NULL &&
            !pubkey_hash_ok(ext->fa_pubkey, ext->pubkey_hash)) {
                LOG2(LOG_INFO, "FA %s, MN %s: Received a request with public "
                     "key and mismatching public key hash extension\n",
                     faaddrstr, mnaddrstr);
                *code = REGREP_ADMIN_PROHIBITED_HA;
                return spi;
        }

        /* request is OK */
        return spi;
}


static int
new_session_key(struct bindingentry *binding, struct spi_entry *mn_spi)
{
#ifndef NODEBUGMODE
        int i;
#endif

        /* remove the (possibly) cached previous key reply using FA's public
         * key, because the session key changes */
        if (binding->last_sent_fa_pubkeyrep)
                free(binding->last_sent_fa_pubkeyrep);
        binding->last_sent_fa_pubkeyrep = NULL;

        /* generate a new session key */
        binding->keylen =
                generate_session_key(mn_spi->auth_alg, mn_spi->shared_secret,
                                     mn_spi->shared_secret_len, binding->key);
        if (binding->keylen == -1) {
                LOG2(LOG_ERR, "Could not generate session key\n");
                return -1;
        }

#ifndef NODEBUGMODE
        DEBUG(DEBUG_FLAG, "Session key: ");
        for (i = 0; i < binding->keylen; i++)
                DEBUG(DEBUG_FLAG, "%02x", binding->key[i]);
        DEBUG(DEBUG_FLAG, "\n");
#endif

        return 0;
}


struct gratuitous_arp_data {
        struct gratuitous_arp_data *next;
        struct timeval tv;
        struct in_addr addr;
        char interface[IFNAMSIZ + 1];
};

static struct gratuitous_arp_data *gratuitous_arp_queue = NULL;


static void
send_gratuitous_arp(struct in_addr addr, char *interface)
{
        struct gratuitous_arp_data *arp2, *arp3, *arp;

        /* HW broadcast is not usually guaranteed, so the gratuitous ARP is
         * transmitted a small number of times (see RFC2002, sec. 4.6) */

        /* send first packet immediately */
        proxyarp_gratuitous(addr, interface);

        arp2 = (struct gratuitous_arp_data *)
                malloc(sizeof(struct gratuitous_arp_data));
        arp3 = (struct gratuitous_arp_data *)
                malloc(sizeof(struct gratuitous_arp_data));
        if (arp2 == NULL || arp3 == NULL) {
                DEBUG(DEBUG_FLAG, "send_gratuitous_arp: malloc() failed\n");
                if (arp2 != NULL)
                        free(arp2);
                if (arp3 != NULL)
                        free(arp3);
                return;
        }

        /* send second packet after 100 ms delay */
        memset(arp2, 0, sizeof(struct gratuitous_arp_data));
        gettimeofday(&arp2->tv, NULL);
        add_usecs(&arp2->tv, 100000);
        arp2->addr.s_addr = addr.s_addr;
        dynamics_strlcpy(arp2->interface, interface, sizeof(arp2->interface));

        /* send third packet after 200 ms delay */
        memcpy(arp3, arp2, sizeof(struct gratuitous_arp_data));
        add_usecs(&arp3->tv, 100000);

        DEBUG(DEBUG_FLAG, "send_gratuitous_arp - adding packet to queue at "
              "%li.%06li and %li.%06li\n", arp2->tv.tv_sec, arp2->tv.tv_usec,
              arp3->tv.tv_sec, arp3->tv.tv_usec);

        if (gratuitous_arp_queue == NULL) {
                /* empty queue - make new queue of the new items */
                gratuitous_arp_queue = arp2;
                arp2->next = arp3;
                arp3->next = NULL;
                return;
        }

        if (cmp_timeval(&gratuitous_arp_queue->tv, &arp2->tv) > 0) {
                /* arp2 goes before the old head of the queue */
                arp2->next = gratuitous_arp_queue;
                gratuitous_arp_queue = arp2;
        } else {
                /* insert arp2 into queue */
                arp = gratuitous_arp_queue;
                while (arp->next != NULL &&
                       cmp_timeval(&arp->next->tv, &arp2->tv) <= 0)
                        arp = arp->next;
                arp2->next = arp->next;
                arp->next = arp2;
        }

        /* insert arp3 into queue */
        arp = arp2;
        while (arp->next != NULL &&
               cmp_timeval(&arp->next->tv, &arp3->tv) <= 0)
                arp = arp->next;
        arp3->next = arp->next;
        arp->next = arp3;
}


static void
check_queued_gratuitous_arp(void)
{
        struct timeval now;
        struct gratuitous_arp_data *arp;

        if (gratuitous_arp_queue == NULL)
                return;

        gettimeofday(&now, NULL);
        while (gratuitous_arp_queue != NULL &&
               cmp_timeval(&gratuitous_arp_queue->tv, &now) <= 0) {
                arp = gratuitous_arp_queue;
                DEBUG(DEBUG_FLAG, "Sending queued gratuitous ARP "
                      "(tv=%li.%06li addr=%s interface=%s)\n",
                      arp->tv.tv_sec, arp->tv.tv_usec, inet_ntoa(arp->addr),
                      arp->interface);
                proxyarp_gratuitous(arp->addr, arp->interface);
                gratuitous_arp_queue = arp->next;
                free(arp);
        }
        if (gratuitous_arp_queue == NULL)
                DEBUG(DEBUG_FLAG, "Gratuitous ARP queue empty.\n");
        else
                DEBUG(DEBUG_FLAG, "Entries remaining in gratuitous ARP queue "
                      "(next at %li.%06li)\n",
                      gratuitous_arp_queue->tv.tv_sec,
                      gratuitous_arp_queue->tv.tv_usec);
}


static struct bindingentry *
create_binding(struct msg_extensions *ext, struct spi_entry *mn_spi,
               int create_tunnels)
{
        struct bindingentry *binding;
        struct ha_tunnel_data *t_data;
        char mn_addrstr[17];
        char co_addrstr[17];
        int ok = TRUE;

        DEBUG(DEBUG_FLAG, "Creating a new binding for MN\n");

        ASSERT(ext != NULL && ext->req != NULL);
        ASSERT(mn_spi != NULL);
        if (bindingcount >= config.max_bindings) {
                LOG2(LOG_WARNING, "max bindings reached\n");
                return NULL;
        }

        /* create the binding */
        binding = malloc(sizeof(struct bindingentry));
        if (binding == NULL) {
                LOG2(LOG_ERR, "malloc[binding] - %s\n", strerror(errno));
                return NULL;
        }
        memset(binding, 0, sizeof(struct bindingentry));
        binding->data = malloc(sizeof(struct ha_tunnel_data));
        if (binding->data == NULL) {
                LOG2(LOG_ERR, "malloc[ha_tunnel_data] - %s\n",
                     strerror(errno));
                free(binding);
                return NULL;
        }
        memset(binding->data, 0, sizeof(struct ha_tunnel_data));
        t_data = (struct ha_tunnel_data *) binding->data;

        binding->spi = mn_spi->spi;

        if ((ext->fa_keyreq || ext->fa_pubkey || ext->mn_keyreq) &&
            new_session_key(binding, mn_spi) < 0)
                ok = FALSE;

        binding->mn_addr.s_addr = ext->req->home_addr.s_addr;
        binding->ha_addr.s_addr = ext->req->ha_addr.s_addr;
        binding->priv_ha = config.priv_ha;
        binding->lower_addr.s_addr = config.sha_addr.s_addr != 0 ?
                config.sha_addr.s_addr : ext->req->co_addr.s_addr;
        dynamics_strlcpy(mn_addrstr, inet_ntoa(ext->req->home_addr),
                         sizeof(mn_addrstr));
        dynamics_strlcpy(co_addrstr, inet_ntoa(ext->req->co_addr),
                         sizeof(co_addrstr));
        memcpy(&binding->id, ext->req->id, REG_REQ_ID_LEN);
        binding->create_time = time(NULL);

        if (ok && create_tunnels &&
            tunnel_add(tunnels, binding->lower_addr, binding->tun_dev,
                       ext->req->ha_addr, 1, TUNNEL_IPIP, 0) == NULL) {
                LOG2(LOG_ERR, "Could not add tunnel to FA %s (COA=%s) for "
                     "MN %s\n", inet_ntoa(binding->lower_addr), co_addrstr,
                     mn_addrstr);
                ok = FALSE;
        }

        if (ok && create_tunnels &&
            dyn_ip_route_get(binding->mn_addr, t_data->arp_if, IFNAMSIZ)
            != 0) {
                LOG2(LOG_ERR, "Could not get arp interface for MN %s\n",
                     mn_addrstr);
                ok = FALSE;
        }

        if (ok && create_tunnels &&
            dyn_ip_route_replace(binding->mn_addr, binding->tun_dev) != 0) {
                LOG2(LOG_ERR, "Could not add route to MN %s\n",
                     mn_addrstr);
                ok = FALSE;
        }

        if (!ok) {
                free(binding->data);
                free(binding);
                return NULL;
        }

        if (create_tunnels) {
                DEBUG(DEBUG_FLAG, "tunnel_add => tun_dev=[%s]\n",
                      binding->tun_dev);
                DEBUG(DEBUG_FLAG, "dyn_ip_route_get: %s => %s\n",
                      inet_ntoa(binding->mn_addr), t_data->arp_if);
                proxyarp_add_item(binding->mn_addr, t_data->arp_if);
                send_gratuitous_arp(binding->mn_addr, t_data->arp_if);
        }

        bindingcount++;
        return binding;
}


static void
remove_tunnel(struct bindingentry *binding)
{
        struct ha_tunnel_data *t_data;

        ASSERT(binding != NULL && binding->data != NULL);

        t_data = (struct ha_tunnel_data *) binding->data;
        if (dyn_ip_route_del(binding->mn_addr, binding->tun_dev) < 0)
                LOG2(LOG_WARNING, "remove_tunnel: dyn_ip_route_del failed\n");
        if (tunnel_delete(tunnels, binding->lower_addr, 0, TUNNEL_IPIP, 0) < 0)
                LOG2(LOG_WARNING, "remove_tunnel: tunnel_delete failed\n");
        if (proxyarp_del_item(binding->mn_addr, t_data->arp_if) < 0)
                LOG2(LOG_WARNING, "remove_tunnel: proxyarp_del_item failed\n");
}


static int
switch_tunnel(struct bindingentry *binding, struct msg_extensions *ext,
              struct spi_entry *mn_spi)
{
        char mn_addrstr[17];
        char co_addrstr[17];
        char old_lower_tunl[IFNAMSIZ];

        ASSERT(binding != NULL && ext != NULL && ext->req != NULL);

        dynamics_strlcpy(mn_addrstr, inet_ntoa(ext->req->home_addr),
                         sizeof(mn_addrstr));
        dynamics_strlcpy(co_addrstr, inet_ntoa(ext->req->co_addr),
                         sizeof(co_addrstr));
        memcpy(old_lower_tunl, binding->tun_dev, IFNAMSIZ);

        /* remove old tunnel (delayed deletion) */
        if (tunnel_delete(tunnels, binding->lower_addr, 0, TUNNEL_IPIP, 0) < 0)
        {
                LOG2(LOG_ERR,
                     "Could not delete old tunnel to FA %s for MN %s\n",
                     co_addrstr, mn_addrstr);
        }

        binding->lower_addr = config.sha_addr.s_addr != 0 ?
                config.sha_addr : ext->req->co_addr;

        /* add the new tunnel */
        if (tunnel_add(tunnels, binding->lower_addr, binding->tun_dev,
                       ext->req->ha_addr, 1, TUNNEL_IPIP, 0) == NULL) {
                LOG2(LOG_ERR, "Could not add tunnel to FA %s (COA=%s) for "
                     "MN %s\n", inet_ntoa(binding->lower_addr), co_addrstr,
                     mn_addrstr);
                destroy_binding(binding);
                return -1;
        }
        DEBUG(DEBUG_FLAG, "tunnel_add => tun_dev=[%s]\n", binding->tun_dev);

        if (dyn_ip_route_replace(binding->mn_addr, binding->tun_dev) != 0) {
                LOG2(LOG_WARNING,
                     "Route replace failed (COA=%s, MN=%s, dev=%s)\n",
                     co_addrstr, mn_addrstr, binding->tun_dev)
        }

        if ((ext->fa_keyreq || ext->fa_pubkey || ext->mn_keyreq) &&
            new_session_key(binding, mn_spi) < 0) {
                destroy_binding(binding);
                return -1;
        }

        return 0;
}


static void
destroy_binding(struct bindingentry *binding)
{
        if (binding->data) /* ccmalloc needs this */
                free(binding->data);
        if (binding->fa_pubkey)
                free(binding->fa_pubkey);
        if (binding->last_sent_fa_pubkeyrep)
                free(binding->last_sent_fa_pubkeyrep);
        free(binding);
        bindingcount--;
}


static void
check_bindings(void)
{
        struct bindingentry *binding;
        time_t diff;
        static int first_time = 1;
        static time_t advance_expire_time;
        time_t time2;

        if (first_time) {
                advance_expire_time = time(NULL);
                first_time = 0;
        }
        time2 = time(NULL);
        diff = time2 - advance_expire_time;
        advance_expire_time = time2;
        if (diff < 0) {
                DEBUG(DEBUG_FLAG,
                      "check_bindings: diff(%li) < 0 - system time changed?\n",
                      diff);
                diff = 0;
        }
        binding = binding_getexpired(bindings, diff);
        while (binding != NULL) {
                DEBUG(DEBUG_FLAG, "Binding to MN %s expired\n",
                      inet_ntoa(binding->mn_addr));
                remove_tunnel(binding);
                destroy_binding(binding);
                binding = binding_getexpired(bindings, 0);
        }
}


static struct fa_spi_entry *
get_fa_spi(int spi, struct in_addr addr)
{
        struct fa_spi_entry *s;
        struct node *node;

        for (node = list_get_first(&config.fa_spi_list); node != NULL;
             node = list_get_next(node)) {
                s = (struct fa_spi_entry *) node;
                if ((spi == 0 || s->spi == spi) &&
                    s->addr.s_addr == addr.s_addr) {
                        return s;
                }
        }
        return NULL;
}


static struct spi_entry *
get_mn_spi(int spi)
{
        struct spi_entry *s;
        struct node *node;

        for (node = list_get_first(&config.spi_list); node != NULL;
             node = list_get_next(node)) {
                s = (struct spi_entry *) node;
                if (s->spi == spi)
                        return s;
        }
        return NULL;
}


static int
ha_add_ext_start(unsigned char *start, unsigned char *pos, int left,
                 struct msg_extensions *ext, int lifetime,
                 struct spi_entry *mn_spi, struct bindingentry *binding,
                 int auth_type)
{
        int used = 0, n;

        if (config.priv_ha > 0) {
                struct priv_ha_ext *priv = (struct priv_ha_ext *) (pos + used);
                DEBUG(DEBUG_FLAG, " * priv_ha\n");
                if (left < sizeof(struct priv_ha_ext))
                        return -1;
                memset(priv, 0, sizeof(struct priv_ha_ext));
                priv->type = VENDOR_EXT_TYPE2;
                priv->length = sizeof(struct priv_ha_ext) - 2;
                priv->vendor_id = htonl(VENDOR_ID_DYNAMICS);
                priv->sub_type = htons(VENDOR_EXT_DYNAMICS_PRIV_HA);
                priv->priv_ha = htonl(config.priv_ha);
                used += GET_PRIV_HA_EXT_LEN(priv);
                left -= GET_PRIV_HA_EXT_LEN(priv);
        }

        /* RFC 2794 requires that HA copies MN NAI extension to the reply */
        if (ext != NULL && ext->mn_nai != NULL) {
                DEBUG(DEBUG_FLAG, " * mn_nai\n");
                if (left < GET_MN_NAI_EXT_LEN(ext->mn_nai))
                        return -1;
                memcpy(pos + used, ext->mn_nai,
                       GET_MN_NAI_EXT_LEN(ext->mn_nai));
                used += GET_MN_NAI_EXT_LEN(ext->mn_nai);
                left -= GET_MN_NAI_EXT_LEN(ext->mn_nai);
        }

        /* Add the session key to mobile node, if it requested for a key and
         * the reply is not a deregistration or failure reply */
        if (ext && ext->mn_keyreq != NULL && mn_spi && lifetime != 0) {
                struct msg_key *key;
                DEBUG(DEBUG_FLAG, " * mn_keyrep\n");
                if (left < sizeof(struct msg_key) + MAX_SK_LEN)
                        return -1;
                key = (struct msg_key *) (pos + used);
                n = auth_encrypt(mn_spi->auth_alg, mn_spi->shared_secret,
                                 mn_spi->shared_secret_len, binding->key,
                                 key, (struct reg_rep *) start,
                                 VENDOR_EXT_DYNAMICS_MN_KEYREP,
                                 htonl(mn_spi->spi));
                used += n;
                left -= n;
        }

        if (ext && mn_spi) {
                struct msg_auth *auth;
                DEBUG(DEBUG_FLAG, " * mh_auth\n");
                if (left < sizeof(struct msg_auth) + MAX_SK_LEN)
                        return -1;
                /* Add message authentication for MN */
                auth = (struct msg_auth *) (pos + used);
                n = auth_add((auth_type == AUTH_RFC2002BIS ?
                              AUTH_ALG_MD5_RFC2002 : mn_spi->auth_alg),
                             mn_spi->shared_secret,
                             mn_spi->shared_secret_len, start,
                             (struct msg_auth *) (pos + used), MH_AUTH,
                             htonl(mn_spi->spi));
                used += n;
                left -= n;
        }

        /* RFC 3012 requires that HA copies challenge extension to the reply
         * after Mobile-Home auth. ext.; this will be authenticated with
         * FA-HA auth. ext. if security association for this is available */
        if (ext != NULL && ext->challenge != NULL) {
                DEBUG(DEBUG_FLAG, " * challenge\n");
                n = GET_CHALLENGE_EXT_LEN(ext->challenge);
                if (left < n)
                        return -1;
                memcpy(pos + used, ext->challenge, n);
                used += n;
                left -= n;
        }

        return used;
}


static int
ha_add_ext_end(unsigned char *start, unsigned char *pos, struct in_addr addr,
               int left, struct msg_extensions *ext)
{
        struct fa_spi_entry *fa_spi;
        int used = 0;

        if (config.priv_ha && ext && ext->nonce) {
                if (ext->nonce->length != sizeof(struct nonce_ext) - 2) {
                        DEBUG(DEBUG_FLAG, " - invalid nonce ext len\n");
                } else {
                        DEBUG(DEBUG_FLAG, " * nonce\n");
                        if (left < GET_NONCE_EXT_LEN(ext->nonce))
                                return -1;
                        memcpy(pos + used, ext->nonce,
                               GET_NONCE_EXT_LEN(ext->nonce));
                        used += GET_NONCE_EXT_LEN(ext->nonce);
                        left -= GET_NONCE_EXT_LEN(ext->nonce);
                }
        }

        fa_spi = get_fa_spi(0, addr);
        if (fa_spi != NULL && config.priv_ha > 0) {
                DEBUG(DEBUG_FLAG, " * sha_ha_auth\n");
                if (left < sizeof(struct vendor_msg_auth) + MAX_SK_LEN)
                        return -1;
                used += auth_add_vendor(
                        AUTH_ALG_MD5, fa_spi->shared_secret,
                        fa_spi->shared_secret_len, start,
                        (struct vendor_msg_auth *) (pos + used),
                        VENDOR_EXT_DYNAMICS_SHA_HA_AUTH, htonl(fa_spi->spi));
        } else if (fa_spi != NULL) {
                DEBUG(DEBUG_FLAG, " * fh_auth\n");
                if (left < sizeof(struct msg_auth) + MAX_SK_LEN)
                        return -1;
                used += auth_add(fa_spi->alg, fa_spi->shared_secret,
                                 fa_spi->shared_secret_len, start,
                                 (struct msg_auth *) (pos + used), FH_AUTH,
                                 htonl(fa_spi->spi));
        }

        return used;
}


static int
send_reg_repl(int s, struct bindingentry *binding, struct spi_entry *mn_spi,
              struct msg_extensions *ext, struct interface_entry *forced_iface,
              int code)
{
        unsigned char msg[MAXMSG];
        unsigned char *msgpos = msg;
        struct reg_rep *reply;
        struct msg_key *key;
        struct sockaddr_in dest_addr;
        struct fa_spi_entry *fa_spi;
#ifdef USE_TEARDOWN
        struct registration_ext_dynamics *dyn_ext;
#endif
        int msglen, result, key_to_fa, left = MAXMSG, n;
        struct ha_tunnel_data *t_data;

        ASSERT(s >= 0 && binding != NULL && binding->data != NULL &&
               mn_spi != NULL);

        t_data = (struct ha_tunnel_data *) binding->data;

        /* Create the reply extension */
        DEBUG(DEBUG_FLAG, "Reply\n");
        reply = (struct reg_rep *) msg;
        memset(reply, 0, sizeof(struct reg_rep));
        reply->lifetime = htons(binding->timeout);
        reply->type = REG_REP;
        reply->code = code;
        reply->home_addr.s_addr = binding->mn_addr.s_addr;
        reply->ha_addr.s_addr = config.sha_addr.s_addr != 0 ?
                config.sha_addr.s_addr : binding->ha_addr.s_addr;
        memcpy(reply->id, binding->id, REG_REQ_ID_LEN);
        if (mn_spi->replay_method == REPLAY_PROTECTION_NONCE)
                t_data->nonce = reply->id[0] = get_rand32();
        msgpos += sizeof(struct reg_rep);
        left -= sizeof(struct reg_rep);

        n = ha_add_ext_start(msg, msgpos, left, ext, binding->timeout,
                             mn_spi, binding, t_data->auth_type);
        if (n < 0)
                return -1;
        msgpos += n;
        left -= n;

        /* Encrypt the session key for the FA if it requested the key with
         * fa_keyreq (use shared secret and MD5) or fa_pubkey (use RSA) */
        key_to_fa = 0;
        if (binding->fa_spi != 0 && ntohs(reply->lifetime) != 0) {
                fa_spi = get_fa_spi(binding->fa_spi, t_data->lower_saddr);
                if (fa_spi == NULL) {
                        LOG2(LOG_ERR, "unknown FA SPI %i\n", binding->fa_spi);
                        return -1;
                }
                DEBUG(DEBUG_FLAG, " * fa_keyrep\n");
                if (left < sizeof(struct msg_key) + MAX_SK_LEN)
                        return -1;
                key = (struct msg_key *) msgpos;
                n = auth_encrypt(fa_spi->alg, fa_spi->shared_secret,
                                 fa_spi->shared_secret_len, binding->key,
                                 key, reply, VENDOR_EXT_DYNAMICS_FA_KEYREP,
                                 htonl(fa_spi->spi));
                msgpos += n;
                left -= n;
                key_to_fa = 1;
        } else if (binding->fa_pubkey && ntohs(reply->lifetime) != 0 &&
                   binding->last_sent_fa_pubkeyrep) {
                DEBUG(DEBUG_FLAG, " * fa_pubkeyrep\n");
                n = GET_KEY_EXT_LEN(binding->last_sent_fa_pubkeyrep);
                if (left < n)
                        return -1;
                /* If the foreign agent included a public key, add an RSA
                 * encrypted session key to the FA */
                key = (struct msg_key *) msgpos;
                memcpy(key, binding->last_sent_fa_pubkeyrep, n);
                msgpos += n;
                left -= n;
                key_to_fa = 1;
        }

#ifdef USE_TEARDOWN
        if (teardown) {
                DEBUG(DEBUG_FLAG, " * ext_dynamics (teardown)\n");
                if (left < sizeof(struct registration_ext_dynamics))
                        return -1;
                dyn_ext = (struct registration_ext_dynamics *) msgpos;
                dyn_ext->type = VENDOR_EXT_TYPE2;
                dyn_ext->reserved = 0;
                dyn_ext->length = sizeof(struct registration_ext_dynamics) - 2;
                dyn_ext->vendor_id = htonl(VENDOR_ID_DYNAMICS);
                dyn_ext->sub_type = htons(VENDOR_EXT_DYNAMICS_OPTIONS);
                dyn_ext->version = VENDOR_EXT_VERSION;
                dyn_ext->opts = REG_EXT_OWN_TEAR_DOWN;
                dyn_ext->seq = 0;
                msgpos += sizeof(struct registration_ext_dynamics);
                left -= sizeof(struct registration_ext_dynamics);
        }
#endif

        if (key_to_fa) {
                DEBUG(DEBUG_FLAG, " * sk_auth\n");
                if (left < sizeof(struct vendor_msg_auth) + MAX_SK_LEN)
                        return -1;
                /* Add session key-based message authentication for FA */
                n = auth_add_vendor(AUTH_ALG_MD5, binding->key,
                                    binding->keylen, msg,
                                    (struct vendor_msg_auth *) msgpos,
                                    VENDOR_EXT_DYNAMICS_SK_AUTH,
                                    htonl(mn_spi->spi));
                msgpos += n;
                left -= n;
        }

        n = ha_add_ext_end(msg, msgpos, t_data->lower_saddr, left, ext);
        if (n < 0)
                return -1;
        msgpos += n;
        left -= n;

        msglen = msgpos - msg;

        dest_addr.sin_family = AF_INET;
        dest_addr.sin_addr.s_addr = t_data->lower_saddr.s_addr;
        dest_addr.sin_port = binding->lower_port;

        if (forced_iface != NULL &&
            setsockopt(s, SOL_SOCKET, SO_BINDTODEVICE, forced_iface->dev,
                       IFNAMSIZ)) {
                DEBUG(DEBUG_FLAG, "setsockopt(SOL_SOCKET, SO_BINDTODEVICE): "
                      "%s\n", strerror(errno));
        }

        DEBUG(DEBUG_FLAG, " * total %i bytes to %s:%i\n", msglen,
              inet_ntoa(dest_addr.sin_addr), ntohs(dest_addr.sin_port));
        result = sendto(s, msg, msglen, 0,
                        (struct sockaddr *) &dest_addr,
                        sizeof(struct sockaddr_in));

        if (forced_iface != NULL) {
                /* kernel seems to require optlen >= sizeof(int) */
                char dummy[sizeof(int)];
                memset(dummy, 0, sizeof(dummy));
                if (setsockopt(s, SOL_SOCKET, SO_BINDTODEVICE, dummy,
                               sizeof(dummy)))
                        DEBUG(DEBUG_FLAG, "setsockopt(SOL_SOCKET, "
                              "SO_BINDTODEVICE): %s\n", strerror(errno));
        }

        return dynamics_check_sendto(result, msglen, "send_reg_repl");
}


static int
send_reg_failure(int s, struct sockaddr_in *sock_addr,
                 struct spi_entry *mn_spi, int auth_type,
                 int code, __u32 *nonce, struct msg_extensions *ext,
                 struct interface_entry *forced_iface,
                 struct bindingentry *binding,
                 struct interface_entry *from_iface)
{
        unsigned char msg[MAXMSG];
        unsigned char *msgpos = msg;
        struct reg_rep *reply;
        int msglen, result, n, left = MAXMSG;
        struct ha_tunnel_data *t_data = NULL;
        static time_t last_failure_time = 0;
        time_t *timer, now;

        ASSERT(s >= 0 && sock_addr && ext && ext->req && code > 2);

        if (binding) {
                /* known binding - use MN's own timer */
                t_data = (struct ha_tunnel_data *) binding->data;
                timer = &t_data->last_failure_time;
        } else {
                /* no binding - use general timer */
                timer = &last_failure_time;
        }

        /* do not send failure replys too often */
        time(&now);
        if (*timer > now) {
                DEBUG(DEBUG_FLAG, "send_reg_failure: timed failure in the "
                      "future - host time changed?\n");
                *timer = now;
        } else if (now - *timer < config.reg_error_reply_interval) {
                DEBUG(DEBUG_FLAG, "Too frequent error message - skipping\n");
                return 0;
        }
        *timer = now;

        if (mn_spi == NULL && ext->mh_auth != NULL)
                mn_spi = get_mn_spi(ntohl(ext->mh_auth->spi));
        
        /* Create the reply extension */
        DEBUG(DEBUG_FLAG, "Failure reply\n");
        reply = (struct reg_rep *) msg;
        memset(reply, 0, sizeof(struct reg_rep));
        reply->lifetime = 0;
        reply->type = REG_REP;
        reply->code = code;
        reply->home_addr.s_addr = ext->req->home_addr.s_addr;
        reply->ha_addr.s_addr = ext->req->ha_addr.s_addr;
        if (config.sha_addr.s_addr != 0)
                reply->ha_addr.s_addr = config.sha_addr.s_addr;
        else if (code == REGREP_UNKNOWN_HA_HA) {
                /* use the HA's unicast IP address in case of HA discovery */
                struct node *node;
                int found = 0;
                for (node = list_get_first(&config.interfaces); node != NULL;
                     node = list_get_next(node)) {
                        struct interface_entry *iface =
                                (struct interface_entry *) node;
                        if (iface->ha_disc &&
                            iface->bcaddr.s_addr == ext->req->ha_addr.s_addr) {
                                reply->ha_addr.s_addr = iface->addr.s_addr;
                                found = 1;
                                break;
                        }
                }
                if (!found && from_iface != NULL)
                        reply->ha_addr.s_addr = from_iface->addr.s_addr;
        }
        memcpy(reply->id, ext->req->id, REG_REQ_ID_LEN);
        if (mn_spi) {
                if (mn_spi->replay_method == REPLAY_PROTECTION_TIMESTAMP &&
                    code == REGREP_ID_MISMATCH_HA) {
                        reply->id[0] = htonl(time(NULL) + UNIX_NTP_DIFF);
                } else if (mn_spi->replay_method == REPLAY_PROTECTION_NONCE) {
                        reply->id[0] = get_rand32();
                        if (nonce != NULL)
                                *nonce = reply->id[0];
                }
        }
        msgpos += sizeof(struct reg_rep);
        left -= sizeof(struct reg_rep);

        n = ha_add_ext_start(msg, msgpos, left, ext, 0, mn_spi, NULL,
                             auth_type);
        if (n < 0)
                return -1;
        msgpos += n;
        left -= n;

        n = ha_add_ext_end(msg, msgpos, sock_addr->sin_addr, left, ext);
        if (n < 0)
                return -1;
        msgpos += n;
        left -= n;

        msglen = msgpos - msg;

        if (forced_iface != NULL &&
            setsockopt(s, SOL_SOCKET, SO_BINDTODEVICE, forced_iface->dev,
                       IFNAMSIZ)) {
                DEBUG(DEBUG_FLAG, "setsockopt(SOL_SOCKET, SO_BINDTODEVICE): "
                      "%s\n", strerror(errno));
        }

        DEBUG(DEBUG_FLAG, "send_reg_failure(code=%i): "
              "sending %i bytes to %s:%i (forced_iface=%s)\n", code, msglen,
              inet_ntoa(sock_addr->sin_addr), ntohs(sock_addr->sin_port),
              forced_iface != NULL ? forced_iface->dev : "N/A");
        result = sendto(s, msg, msglen, 0,
                        (struct sockaddr *)sock_addr,
                        sizeof(struct sockaddr_in));

        if (forced_iface != NULL) {
                /* kernel seems to require optlen >= sizeof(int); in addition,
                 * 2.2 kernels seem to report error (Invalid argument) even
                 * when this unbinding success.. */
                char dummy[sizeof(int)];
                memset(dummy, 0, sizeof(dummy));
                if (setsockopt(s, SOL_SOCKET, SO_BINDTODEVICE, dummy,
                               sizeof(dummy)) < 0 &&
                    setsockopt(s, SOL_PACKET, SO_BINDTODEVICE, NULL, 0) < 0)
                        DEBUG(DEBUG_FLAG, "setsockopt(SOL_SOCKET, "
                              "SO_BINDTODEVICE): %s (this is probably "
                              "bogus)\n", strerror(errno));
        }

        return dynamics_check_sendto(result, msglen, "send_reg_failure");
}





static int
handle_reg_msg(struct interface_entry *from_iface, int s)
{
        char msg[MAXMSG];
        struct sockaddr_in cli_addr;
        struct in_addr dst_addr;
        struct msghdr mh;
        char cdata[256];
        struct iovec iov;
        struct cmsghdr *cmsg;
        struct bindingentry *binding;
        struct spi_entry *mn_spi;
        int n, res, rsock;
        struct msg_extensions ext;
        int killbinding;
        int code, auth_type;
        char mn_addrstr[17]; 
        char fa_addrstr[17];
        struct ha_tunnel_data *t_data = NULL;
        struct bindingkey bkey;
        struct node *node;
        struct interface_entry *force_iface = NULL;
        int selected_mobile; //what mobile node we're handling in the array


        memset(&cli_addr, 0, sizeof(cli_addr));
        dst_addr.s_addr = 0;
        memset(&mh, 0, sizeof(mh));
        iov.iov_base = msg;
        iov.iov_len = MAXMSG;
        mh.msg_name = &cli_addr;
        mh.msg_namelen = sizeof(cli_addr);
        mh.msg_iov = &iov;
        mh.msg_iovlen = 1;
        mh.msg_control = &cdata;
        mh.msg_controllen = sizeof(cdata);

        n = recvmsg(s, &mh, 0);
        
        LOG(LOG_INFO, "Received %d bytes from %s:%d\n", n,
              inet_ntoa(cli_addr.sin_addr), ntohs(cli_addr.sin_port));

        if (n < 0) {
                LOG(LOG_INFO "handle_reg_msg - recvfrom failed - %s\n",
                     strerror(errno));
                return -1;
        }
        

        for (cmsg = CMSG_FIRSTHDR(&mh); cmsg != NULL;
             cmsg = DYNAMICS_CMSG_NXTHDR(&mh, cmsg)) {
                if (cmsg->cmsg_level == SOL_IP &&
                    cmsg->cmsg_type == IP_PKTINFO) {
                        struct _in_pktinfo *pkt;
                        pkt = (struct _in_pktinfo *) CMSG_DATA(cmsg);
                        dst_addr = pkt->ipi_addr;
                        DEBUG(DEBUG_FLAG, "\tIP_PKTINFO: ipi_ifindex=%i "
                              "ipi_spec_dst=%s ", pkt->ipi_ifindex,
                              inet_ntoa(pkt->ipi_spec_dst));
                        DEBUG(DEBUG_FLAG, "ipi_addr=%s\n",
                              inet_ntoa(pkt->ipi_addr));
                }
        }

        /* use the matching UDP socket for the registration reply */
        rsock = -1;
        for (node = list_get_first(&config.interfaces); node != NULL;
             node = list_get_next(node)) {
                struct interface_entry *iface =
                        (struct interface_entry *) node;
                if (iface->addr.s_addr == dst_addr.s_addr ||
                        (iface->ha_disc &&
                         iface->bcaddr.s_addr == dst_addr.s_addr)) {
                        rsock = iface->udp_sock;
                }
        }
        if (rsock < 0 && from_iface != NULL &&
            dst_addr.s_addr == (unsigned int) -1) {
                DEBUG(DEBUG_FLAG, "Broadcast 255.255.255.255 destination - "
                      "using interface[%s] UDP socket[%i]\n", from_iface->dev,
                      from_iface->udp_sock);
                rsock = from_iface->udp_sock;
        }
        if (rsock < 0) {
                LOG2(LOG_WARNING, "Could not find UDP socket for "
                     "registration reply - trying recv. socket\n");
                rsock = s;
        }
        //pthread_mutex_unlock(&reg_mutex);     
        res = parse_msg(msg, n, &ext);



        if (ext.req != NULL && ntohs(ext.req->lifetime) == 0 &&
            ext.req->home_addr.s_addr == cli_addr.sin_addr.s_addr &&
            ext.req->home_addr.s_addr == ext.req->co_addr.s_addr) {
                /* RFC 2002, Ch. 3.8.3.1:
                 * When HA is replying to deregistration (lifetime == 0 &&
                 * COA == home addr) whose srcIP is MN's home address, the
                 * reply must be send directly to home network bypassing any
                 * mobility bindings */
                DEBUG(DEBUG_FLAG, "Dereg. from home => bypassing mobility "
                      "bindings in routing\n");
                force_iface = from_iface;
        }

        if (res == -3 || res == -4) {
                LOG2(LOG_WARNING, "Unknown critical vendor extension in a "
                     "request from %s:%i\n", inet_ntoa(cli_addr.sin_addr),
                     ntohs(cli_addr.sin_port));
                send_reg_failure(rsock, &cli_addr, NULL, 0,
                                 res == -3 ? REGREP_UNSUPP_VENDOR_ID_MN_HA :
                                 REGREP_UNSUPP_VENDOR_ID_FA_HA, NULL, &ext,
                                 force_iface, NULL, from_iface);
                stats.discarded_vendor_ext++;
                report_discarded_msg(msg, n, &cli_addr,
                                     "unknown vendor extension");
                return -1;
        } else if (res != 0 || ext.req == NULL) {
                char *reason = "N/A";
                if (res == -1) {
                        reason = "unknown extension";
                        stats.discarded_unknown_ext++;
                } else if (res == -2) {
                        reason = "malformed message";
                        stats.discarded_malformed_msg++;
                } else if (ext.req == NULL) {
                        reason = "not a request";
                        stats.discarded_not_request++;
                }
                LOG2(LOG_WARNING, "Message parser failed or not a request "
                     "(from %s:%i): %s\n", inet_ntoa(cli_addr.sin_addr),
                     ntohs(cli_addr.sin_port), reason);
                report_discarded_msg(msg, n, &cli_addr, reason);
                return -1;
        }

        memcpy(&bkey.mn_addr, &ext.req->home_addr, sizeof(bkey.mn_addr));
        bkey.ha_addr = config.sha_addr.s_addr != 0 ? config.sha_addr :
                ext.req->ha_addr;
        bkey.priv_ha = config.priv_ha;
        binding = binding_fetch(bindings, &bkey);
        if (binding != NULL)
                t_data = (struct ha_tunnel_data *) binding->data;
        mn_spi = validate_request(binding, cli_addr.sin_addr, msg, n, &ext,
                                  &code, &auth_type);
        if (code > 2) {
                send_reg_failure(rsock, &cli_addr, mn_spi, 0, code,
                                 (t_data != NULL ? &t_data->nonce : NULL),
                                 &ext, force_iface, binding, from_iface);
                stats.req_rejected++;
                report_discarded_msg(msg, n, &cli_addr, "invalid message");
                return -1;
        }
        if (mn_spi == NULL) {
                stats.req_rejected++;
                report_discarded_msg(msg, n, &cli_addr, "MN SPI not found");
                return -1;
        }

        dynamics_strlcpy(mn_addrstr, inet_ntoa(ext.req->home_addr),
                         sizeof(mn_addrstr));
        dynamics_strlcpy(fa_addrstr, inet_ntoa(ext.req->co_addr),
                         sizeof(fa_addrstr));

        if (binding != NULL) {
                t_data->auth_type = (auth_type == AUTH_MAC_RFC2002 ?
                                     AUTH_RFC2002BIS : AUTH_RFC2002);

                /* If the id field is identical the message was a duplicate.
                 * In order to avoid breaking the tunnel drop this message as
                 * a special case without sending a failure reply if the replay
                 * protection is used
                 */
                if (mn_spi->replay_method != REPLAY_PROTECTION_NONE &&
                    binding->id[0] == ext.req->id[0] &&
                    binding->id[1] == ext.req->id[1]) {
                        DEBUG(DEBUG_FLAG,
                              "Received duplicate request - dropping it\n");
                        stats.req_rejected++;
                        return -1;
                }

                /* Make sure that the id field is incremented if the timestamp
                 * protection is used */
                if (mn_spi->replay_method == REPLAY_PROTECTION_TIMESTAMP &&
                    (ntohl(binding->id[0]) > ntohl(ext.req->id[0]) ||
                     (ntohl(binding->id[0]) == ntohl(ext.req->id[0]) &&
                      ntohl(binding->id[1]) >= ntohl(ext.req->id[1])))) {
                        LOG2(LOG_NOTICE, "Timestamp did not increase "
                             "(MN=%s, FA=%s)\n", mn_addrstr, fa_addrstr);
                        send_reg_failure(rsock, &cli_addr, mn_spi,
                                         t_data->auth_type,
                                         REGREP_ID_MISMATCH_HA,
                                         &t_data->nonce, &ext, force_iface,
                                         binding, from_iface);
                        stats.req_rejected++;
                        return -1;
                }

                /* remove binding from list (does not remove tunnels etc.) */
                binding_remove(binding);
        } else {
                LOG2(LOG_NOTICE, "MN %s registers, FA is %s\n", 
                     mn_addrstr, fa_addrstr);
        }

        stats.req_accepted++;

        killbinding = 0;

        if (binding &&
            (binding->lower_addr.s_addr != (config.sha_addr.s_addr != 0 ?
                                            config.sha_addr.s_addr :
                                            ext.req->co_addr.s_addr) ||
             ext.req->lifetime == 0)) {
                DEBUG(DEBUG_FLAG, "Found existing binding for MN %s\n",
                      mn_addrstr);

                if (ext.req->co_addr.s_addr == ext.req->home_addr.s_addr &&
                    ext.req->lifetime == 0) {
                        DEBUG(DEBUG_FLAG, "MN is deregistering all its "
                              "simultaneous bindings\n");
                        /* Note: RFC 2002, 3.6.1.2, special case
                         * This should be taken into account if the
                         * simultaneous bindings are implemented.
                         * At the moment we just deregister the only possible
                         * binding below. */
                }

#ifdef USE_TEARDOWN
                /* if co_addr differs in request, send reply to old
                 * co_addr to purge old tunnel segment */
                if (config.sha_addr.s_addr == 0 &&
                    binding->lower_addr.s_addr != ext.req->co_addr.s_addr) {
                        binding->timeout = 0;
                        DEBUG(DEBUG_FLAG, "Sending purge msg to %s\n",
                              inet_ntoa(binding->lower_addr));
                        send_reg_repl(s, binding, mn_spi, NULL, force_iface,
                                      REGREP_ACCEPTED);
                }
#endif

                if (ext.req->lifetime > 0) {
                        /*
                        * It was a location update, 
                        * DYNAMO: renew IP from DHCP-server
                        */
                        udhcp(find_dhcp_mobile(MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length),RENEWIP);
                        
                        LOG2(LOG_INFO, "MN %s updating tunnel to FA %s\n", 
                             mn_addrstr, fa_addrstr);
                        if (switch_tunnel(binding, &ext, mn_spi) < 0)
                                return -1;
                } else {
                        /* if it is a deregistration request,
                         * destroy the binding after the reply is sent,
                         * since the binding is needed when sending the
                         * reply */
                        LOG2(LOG_NOTICE, "MN %s deregisters\n", mn_addrstr);
                        remove_tunnel(binding);
                        /*
                        * DYNAMO: Deregister, send DHCPRELEASE and delete from dhcp_mobile_array
                        */
                        udhcp(find_dhcp_mobile(MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length),RELEASEIP); 
                        /*Check if find returns -1? Should never happen in this case tho*/
                        del_dhcp_mobile(find_dhcp_mobile(MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length)); 
                                                
                        if (cli_addr.sin_addr.s_addr == binding->mn_addr.s_addr
                            && ext.req->co_addr.s_addr ==
                            ext.req->home_addr.s_addr) {
                                DEBUG(DEBUG_FLAG, "MN returned home and "
                                      "deregistered all care-of addresses\n");
                                /* FIX: also HA should send gratuitous ARP
                                 * telling that MN takes care of its own
                                 * packets from now on;
                                 * draft-ietf-mobileip-rfc2002-bis-03.txt, 4.6;
                                 * This would require that HA would get the
                                 * link-layer address of the MN from the
                                 * deregistration message and use it in the
                                 * gratuitous ARP packet */
                        }
                        killbinding = 1;
                }
        } 

    if(ext.mn_nai == NULL) return -1; //TODO send regreply with error code 97 (missing nai, this is FA's error msg tho) or code 134 (failed auth).
    if(ext.mn_nai->length==0) return -1;


        if (binding == NULL) {

                if (ntohs(ext.req->lifetime) == 0) {
                        DEBUG(DEBUG_FLAG, "Deregistration attempt, but binding"
                              " not found - creating temporary binding\n");
                        binding = create_binding(&ext, mn_spi, 0);
                        killbinding = 1;
                } else {


/**************************************
* DYNAMO: Get MN NAI and NAI lenght for dhcp_mobile struct and dhcp-call
**************************************/

        /*
         DYNAMO: Check if mobile node with current nai already exists on array
                 find returns -1 if already added!
        */
        
        if (find_dhcp_mobile(MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length)>=0) //-1 == not found
        {
         LOG(LOG_INFO, "Mobile node was already added in the data structure");  
         //return -1; //We dont want to exit even if the mobile is in dhcp_mobile array, we just write in the same place then
        
        }
        
        add_dhcp_mobile(0, MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length); 
        
        LOG(LOG_INFO, "Preparing for DHCP call for registering mobile node");
        
        /* Lets find mobile's place in array for DHCP call */
        selected_mobile=find_dhcp_mobile(MSG_MN_NAI_DATA(ext.mn_nai), ext.mn_nai->length);
        if (selected_mobile <0) return -1; //nai wasnt found, should never happen!!
        
        /* Get IP for Mobile node from DHCP with NAI as client id*/
                
        if(udhcp(selected_mobile,REQUESTIP)==-1) return -1;  //DHCP-server time-out or NAK
        
        ext.req->home_addr.s_addr=dhcp_mobile_array[selected_mobile].requested_ip; //set the IP-address to regreply extions

/**************************************
*Dynamics reg_reg handling continues
**************************************/

                        binding = create_binding(&ext, mn_spi, 1);
                }

                if (binding == NULL) {
                        LOG2(LOG_ERR, "Failed to create binding for MN: %s, "
                             "FA: %s\n", mn_addrstr, fa_addrstr);
                        send_reg_failure(rsock, &cli_addr, mn_spi,
                                         (auth_type == AUTH_MAC_RFC2002 ?
                                          1 : 0),
                                         REGREP_NO_RESOURCES_HA, NULL, &ext,
                                         force_iface, binding, from_iface);
                        return -1;
                }
                t_data = (struct ha_tunnel_data *) binding->data;
                t_data->auth_type = (auth_type == AUTH_MAC_RFC2002 ?
                                     AUTH_RFC2002BIS : AUTH_RFC2002);
        }
        ASSERT(binding != NULL);
        t_data = (struct ha_tunnel_data *) binding->data;
        ASSERT(t_data != NULL);

        binding->mod_time = time(NULL);
        t_data->reverse_tunnel = (ext.req->opts & REGREQ_REVERSE_TUNNEL) != 0;
        if (ext.req->opts & REGREQ_MINIMAL_ENCAPS)
                t_data->encapsulation = ENCAPS_MINIMAL;
        else if (ext.req->opts & REGREQ_GRE_ENCAPS)
                t_data->encapsulation = ENCAPS_GRE;
        else
                t_data->encapsulation = ENCAPS_IPIP;

        if (ext.fa_keyreq)
                binding->fa_spi = ntohl(ext.fa_keyreq->spi);
        else
                binding->fa_spi = 0;

        /* If the request has a public key add it to the binding */
        if (ext.fa_pubkey) {
                /* binding->fa_pubkey is NULL if we have generated a
                 * new SK. If the public key sent by the FA is the same 
                 * as in a previous binding, we can reuse the encrypted
                 * SK and save some CPU cycles */
                if (binding->fa_pubkey &&
                    binding->last_sent_fa_pubkeyrep != NULL &&
                    (memcmp(binding->fa_pubkey, 
                            ext.fa_pubkey, 
                            GET_KEY_EXT_LEN(ext.fa_pubkey)) == 0))
                {
                        DEBUG(DEBUG_FLAG, "Reusing encrypted SK for FA %s\n",
                              fa_addrstr);
                } else {
                        if (binding->fa_pubkey)
                                free(binding->fa_pubkey);
                        if (binding->last_sent_fa_pubkeyrep)
                                free(binding->last_sent_fa_pubkeyrep);
                        binding->last_sent_fa_pubkeyrep = NULL;
                        binding->fa_pubkey = (struct msg_key *)
                                malloc(GET_KEY_EXT_LEN(ext.fa_pubkey));
                        if (binding->fa_pubkey != NULL) {
                                memcpy(binding->fa_pubkey, ext.fa_pubkey,
                                       GET_KEY_EXT_LEN(ext.fa_pubkey));
                        } else {
                                LOG2(LOG_ERR, "Not enough memory "
                                     "for fa_pubkey\n");
                                return -1;
                        }
                        binding->last_sent_fa_pubkeyrep =
                                dynamics_do_rsa_encrypt(binding->key,
                                                        binding->keylen,
                                                        binding->fa_pubkey);
                        if (binding->last_sent_fa_pubkeyrep == NULL) {
                                LOG2(LOG_ERR, "RSA encryption failed (MN=%s, "
                                     "FA=%s)\n", mn_addrstr, fa_addrstr);
                        }
                }

        } else {
                /* remove the (possible) old FA pubkey from the binding to make
                 * sure that the next FA using RSA will get the correct key */
                if (binding->fa_pubkey)
                        free(binding->fa_pubkey);
                binding->fa_pubkey = NULL;

                if (binding->last_sent_fa_pubkeyrep)
                        free(binding->last_sent_fa_pubkeyrep);
                binding->last_sent_fa_pubkeyrep = NULL;
        }

        memcpy(&binding->id, ext.req->id, REG_REQ_ID_LEN);

        /* store where the request came from */
        t_data->lower_saddr.s_addr = cli_addr.sin_addr.s_addr;
        binding->lower_port = cli_addr.sin_port;

        binding->timeout = MIN(ntohs(ext.req->lifetime), mn_spi->max_lifetime);
        binding->exp_time = time(NULL) + binding->timeout;

        if (send_reg_repl(rsock, binding, mn_spi, &ext, force_iface,
                          code == REGREP_ACCEPTED_NO_SB ?
                          REGREP_ACCEPTED_NO_SB : REGREP_ACCEPTED) != 0) {
                LOG2(LOG_ERR, "sending of registration reply to MN: %s, "
                     "FA: %s failed\n", mn_addrstr, fa_addrstr);
                remove_tunnel(binding);
                destroy_binding(binding);
                return -1;
        }

        if (killbinding) { //del_mobile_dhcp could also be called from here...
                destroy_binding(binding);
        } else {
                /* increase timeout with one second so that the one second
                 * timer resolution doesn't cause too early expirations */
                binding->timeout++;

                /* make sure that the bindings module gets timer update just
                 * before adding the binding */
                check_bindings();
                binding_add(bindings, binding);
        }

 return 0;
}


static int
ha_send_agent_adv(struct interface_entry *iface, struct sockaddr_ll *to,
                  struct in_addr *dest)
{
        /* Set Agent Advertisement data */
        set_agent_adv_data(
                config.ha_default_tunnel_lifetime,
                iface->addr,
                iface->addr, AGENT_ADV_HOME_AGENT, 0,
                iface->interval > 21845 ?
                65535 : iface->interval * 3, NULL, 0);
        set_agent_adv_nai(ha_nai);

        return send_agent_advertisement(iface->icmp_sock,
                                        (struct sockaddr *) to, dest,
                                        iface->if_index);
}


static void
send_agent_advs(struct timeval *next_agentadv)
{
        struct interface_entry *iface;
        struct node *node;
        struct timeval now, next;
        unsigned int diff;
        int first = 1;

        /* Send ICMP Agent Advertisement message and save the timestamp */
        gettimeofday(&now, NULL);
        next.tv_sec = 0;
        next.tv_usec = 0;
        for (node = list_get_first(&config.interfaces); node != NULL;
             node = list_get_next(node)) {
                iface = (struct interface_entry *) node;
                if (iface->agentadv != INTERFACE_AGENTADV_ALL)
                        continue;

                if (now.tv_sec >= iface->last_adv.tv_sec + iface->interval) {
                        if (!ha_send_agent_adv(iface, NULL, NULL))
                                stats.adv_sent++;
                        else {
                                DEBUG(DEBUG_FLAG, "send_agent_advs: "
                                      "advertisement sending to iface[%s] "
                                      "failed\n", iface->dev);
                        }
                        iface->last_adv.tv_sec = now.tv_sec;
                        iface->last_adv.tv_usec = now.tv_usec;
                }

                next.tv_sec = iface->last_adv.tv_sec + iface->interval;
                /* Add random delay */
                next.tv_usec = iface->last_adv.tv_usec +
                        (int) (MAX_ADV_DELAY * rand() / (RAND_MAX + 1.0));
                if (next.tv_usec > 999999) {
                        next.tv_sec++;
                        next.tv_usec -= 1000000;
                }
                if (first || next.tv_sec < next_agentadv->tv_sec) {
                        DEBUG(DEBUG_FLAG, "** ");
                        *next_agentadv = next;
                }
                first = 0;

                /* for debuging */
                diff = ((next.tv_sec - now.tv_sec) * 1000000 + 
                        next.tv_usec - now.tv_usec) / 1000; 
                DEBUG(DEBUG_FLAG, "send_agent_advs: next agentadv: "
                      "%ld.%ld diff = %u msec\n", 
                      next.tv_sec, next.tv_usec, diff);
        }
}


static void
handle_icmp(struct interface_entry *iface)
{
        int r;
        struct sockaddr_ll from;
        struct in_addr to, fromaddr;

        r = check_icmp_sol(iface->icmp_sock, &from, &to, &fromaddr);

        if (r == 0 && iface->agentadv != INTERFACE_AGENTADV_NONE) {
                /* Agent solicitation */
                DEBUG(DEBUG_FLAG,
                      "\tagent solicitation => reply with agent adv\n");
                send_agent_advertisement(iface->icmp_sock,
                                         (struct sockaddr *) &from, &fromaddr,
                                         from.sll_ifindex);
        }
}


static void
set_expr_timer(struct timeval *tv, struct timeval next_agentadv)
{
        int expire_time;
        struct timeval now;

        gettimeofday(&now, NULL);
        DEBUG(DEBUG_FLAG, "set_expr_timer (now=%li.%06li)\n",
              now.tv_sec, now.tv_usec);

        if (timerisset(&next_agentadv)) {
                tv->tv_sec = next_agentadv.tv_sec - now.tv_sec;
                tv->tv_usec = next_agentadv.tv_usec - now.tv_usec;
                if (tv->tv_usec < 0) {
                        tv->tv_sec--;
                        tv->tv_usec += 1000000;
                }
                if (tv->tv_sec < 0) {
                        tv->tv_sec = 0;
                        tv->tv_usec = 0;
                }
                DEBUG(DEBUG_FLAG, "\tnext_agentadv in %li.%06li sec\n",
                      tv->tv_sec, tv->tv_usec);
        } else {
                tv->tv_sec = -1;
                tv->tv_usec = 0;
        }

        if ((expire_time = binding_nextexpiretime(bindings)) >= 0) {
                DEBUG(DEBUG_FLAG, "\tbinding next expire in %i sec\n",
                      expire_time);
                if (tv->tv_sec < 0 || expire_time < tv->tv_sec) {
                        DEBUG(DEBUG_FLAG, "\tsetting expire time to %i sec\n",
                              expire_time);
                        tv->tv_sec = (time_t)expire_time;
                        tv->tv_usec = 0;
                }
        }

        if (tv->tv_sec == -1 && tunnel_delayed_exists(tunnels)) {
                DEBUG(DEBUG_FLAG, "\tsetting expire time to %i sec (tunnel "
                      "check)\n", TUNNEL_DELAYED_CHECK_INTERVAL);
                tv->tv_sec = TUNNEL_DELAYED_CHECK_INTERVAL;
        }

        if (gratuitous_arp_queue != NULL) {
                struct timeval next;

                next.tv_sec = gratuitous_arp_queue->tv.tv_sec - now.tv_sec;
                next.tv_usec = gratuitous_arp_queue->tv.tv_usec - now.tv_usec;
                if (next.tv_usec < 0) {
                        next.tv_sec--;
                        next.tv_usec += 1000000;
                }
                if (next.tv_sec < 0) {
                        next.tv_sec = 0;
                        next.tv_usec = 0;
                }

                if (tv->tv_sec == -1 || cmp_timeval(tv, &next) > 0) {
                        tv->tv_sec = next.tv_sec;
                        tv->tv_usec = next.tv_usec;
                        DEBUG(DEBUG_FLAG, "\tnext gratuitous ARP in %li.%06li "
                              "sec\n", tv->tv_sec, tv->tv_usec);
                }
        }
}


static void
init_interfaces(void)
{
        struct interface_entry *iface;
        struct in_addr bc;

        DEBUG(DEBUG_FLAG, "Initializing interfaces\n");
        inet_aton("255.255.255.255", &bc);
        iface = (struct interface_entry *) list_get_first(&config.interfaces);
        if (iface == NULL) {
                LOG2(LOG_ALERT, "No interfaces defined\n");
                clean_up(-1);
        }
        while (iface != NULL) {
                DEBUG(DEBUG_FLAG, "\t%s: ", iface->dev);
                iface->if_index = dyn_ip_get_ifindex(iface->dev);
                if (iface->if_index < 0) {
                        LOG2(LOG_ALERT,
                             "Could not get ifindex for interface %s\n",
                             iface->dev);
                        clean_up(-1);
                }
                DEBUG(DEBUG_FLAG, "ifindex=%i ", iface->if_index);

                if (iface->force_addr.s_addr != 0) {
                        DEBUG(DEBUG_FLAG, "forcing address ");
                        iface->addr = iface->force_addr;
                } else {
                        if (dyn_ip_get_ifaddr(iface->dev, &iface->addr) != 0) {
                                LOG2(LOG_ALERT, "could not get interface[%s] "
                                     "address\n", iface->dev);
                                clean_up(-1);
                        }
                }

                /* bind unicast UDP socket into local IP address, but not
                 * interface */
                iface->udp_sock = dynamics_open_udp_socket(
                        config.socket_priority, 
                        iface->force_addr.s_addr != 0 ? iface->force_addr :
                        iface->addr, config.udpport, NULL);
                if (iface->udp_sock < 0) {
                        LOG2(LOG_ALERT, "UDP socket opening failed\n");
                        clean_up(-1);
                }

                if (iface->ha_disc) {
                        /* bind broadcast UDP sockets into broadcast IP address
                         * and interface */
                        if (dyn_ip_get_bcaddr(iface->dev, &iface->bcaddr) != 0)
                        {
                                LOG2(LOG_ALERT, "could not get interface[%s] "
                                     "broadcast address\n", iface->dev);
                                clean_up(-1);
                        }
                        iface->udp_bc_sock = dynamics_open_udp_socket(
                                config.socket_priority, iface->bcaddr,
                                config.udpport, iface->dev);
                        if (iface->udp_bc_sock < 0) {
                                LOG2(LOG_ALERT, "UDP broadcast socket opening "
                                     "failed for interface[%s]\n", iface->dev);
                                clean_up(-1);
                        }

                        iface->udp_bc_sock2 = dynamics_open_udp_socket(
                                config.socket_priority, bc, config.udpport,
                                iface->dev);
                        if (iface->udp_bc_sock2 < 0) {
                                LOG2(LOG_ALERT, "UDP broadcast "
                                     "(255.255.255.255) socket opening "
                                     "failed for interface[%s]\n", iface->dev);
                                clean_up(-1);
                        }
                }

                iface->icmp_sock =
                        open_agent_icmp_adv_socket(iface->dev,
                                                   AGENTADV_FILTER_SOL);
                if (iface->icmp_sock < 0) {
                        LOG2(LOG_ALERT, "could not open ICMP socket\n");
                        clean_up(-1);
                }

                DEBUG(DEBUG_FLAG, "%s => socket=%i\n", inet_ntoa(iface->addr),
                      iface->icmp_sock);

                iface = (struct interface_entry *) list_get_next(&iface->node);
        }
}


static void
ha_parse_command_line(int argc, char *argv[])
{
        int c, oindex = 0;
        static struct option long_options[] = {
                /* common arguments */
                {"help", no_argument, NULL, 'h'},
                {"version", no_argument, NULL, 'v'},
                {"debug", no_argument, NULL, 0},
                {"fg", no_argument, NULL, 0},
                {"config", required_argument, NULL, 'c'},
                /* HA specific arguments */
                /* end of arguments */
                {0, 0, 0, 0}
        };

        DEBUG(DEBUG_FLAG, "HA command line parsing\n");

        while ((c = getopt_long(argc, argv, "+hv", long_options, &oindex)) !=
               EOF) {
                
                switch (c) {
                case 'h':
                        /* show HA specific parameters */
                        /* N/A */
                        exit(1);
                        break;
                        
                case '?':
                        printf("Command line parsing failed - aborting\n");
                        exit(1);

                case 'v':
                case 'c':
                case 0:
                        break;
                        
                default:
                        printf("?? getopt returned character code 0%o ??\n",
                               c);
                        exit(1);
                }
        }
}


int
main(int argc, char *argv[])
{
        int api_rw_sock; /* unix socket for privileged (may modify data)
                            API calls */
        int api_ro_sock; /* unix socket for unprivileged (read only)
                            API calls */
        struct timeval next_agentadv;
        struct interface_entry *iface;

        program_name = parse_long_options(argc, argv, "home agent", PACKAGE,
                                          VERSION, dynamics_usage);

        ha_parse_command_line(argc, argv);

        memset(&config, 0, sizeof(struct ha_config));

        if (load_config(&config, program_name,
                        opt_config == NULL ? HA_GLOBAL_CONF_FILE : opt_config)
            == FALSE) {
                clean_up(1);
        }
        set_ha_nai();
        client_config.interface=config.dhcp_if; //set ethernet interface from HAs config to udhcps config
        LOG(LOG_INFO, "Sending DHCP-packets to interface: %s.\n", client_config.interface);


        check_kernel_support(CHECK_KERNEL_IPIP | CHECK_KERNEL_NETLINK);

        if (getuid() || geteuid()) {
                fprintf(stderr, "%s: This program must be run by root.\n",
                        program_name);
                exit(1);
        }

        iface = (struct interface_entry *) list_get_first(&config.interfaces);
        srand(time(NULL) ^ (iface ? iface->addr.s_addr : 0));

        allow_ipv4_forwarding();

        /* init tables */
        bindings = binding_init(config.max_bindings,
                                config.ha_default_tunnel_lifetime);
        if (bindings == NULL) {
                LOG2(LOG_ERR, "binding_init failed\n");
                clean_up(1);
        }

        init_interfaces();

        tunnels = tunnel_init("TUNL", 1, 253);
        if (tunnels == NULL) {
                LOG2(LOG_ERR, "tunnel_init failed\n");
                clean_up(1);
        }

        api_rw_sock = api_open_socket(config.ha_api_admin_socket_path,
                                      config.ha_api_admin_socket_group,
                                      config.ha_api_admin_socket_owner,
                                      config.ha_api_admin_socket_permissions);
        if (api_rw_sock < 0) {
                LOG2(LOG_ERR, "Could not create API rw-socket\n");
                clean_up(1);
        }

        api_ro_sock = api_open_socket(config.ha_api_read_socket_path,
                                      config.ha_api_read_socket_group,
                                      config.ha_api_read_socket_owner,
                                      config.ha_api_read_socket_permissions);
        if (api_ro_sock < 0) {
                LOG2(LOG_ERR, "Could not create API ro-socket\n");
                clean_up(1);
        }

        if (!opt_foreground && dynamics_fork_daemon() == -1)
                clean_up(1);
        dynamics_write_pid_file(HA_PID_FILE);

        memset(&stats, 0, sizeof(stats));
        dynamics_strlcpy(stats.version, VERSION, sizeof(stats.version));

        /* set up signal actions */
        signal(SIGTERM, sig_handler);            /* clean death */
        signal(SIGINT, sig_handler);             /* clean death */
        signal(SIGHUP, reload_config);

        syslog(LOG_INFO, "%s home agent daemon version %s started\n",
               PACKAGE, VERSION);

        next_agentadv.tv_sec = 0;
        next_agentadv.tv_usec = 0;
        send_agent_advs(&next_agentadv);

        /* loop forever waiting for messages */
        for (;;) {

        
                fd_set set;
                int oldmask;
                struct node *node;
                struct timeval tv;

                oldmask = sigblock(sigmask(SIGHUP));
                FD_ZERO(&set);
                FD_SET(api_rw_sock, &set);
                FD_SET(api_ro_sock, &set);
                for (node = list_get_first(&config.interfaces); node != NULL;
                     node = list_get_next(node)) {
                        iface = (struct interface_entry *) node;
                        FD_SET(iface->udp_sock, &set);
                        if (iface->ha_disc && iface->udp_bc_sock > -1)
                                FD_SET(iface->udp_bc_sock, &set);
                        if (iface->ha_disc && iface->udp_bc_sock2 > -1)
                                FD_SET(iface->udp_bc_sock2, &set);
                        FD_SET(iface->icmp_sock, &set);
                }

                set_expr_timer(&tv, next_agentadv);
                sigsetmask(oldmask);

                if (select(FD_SETSIZE, &set, NULL, NULL,
                           tv.tv_sec == -1 ? NULL : &tv) < 0) {
                        /* don't exit on e.g. SIGHUP */
                        if (errno != EINTR) {
                                LOG2(LOG_ERR, "select - %s", strerror(errno));
                                clean_up(1);
                        }
                        continue;
                }
                       
                /* we don't want to reread the configuration file
                 * while we are handling requests; hold the signal
                 * until we are ready */
                oldmask = sigblock(sigmask(SIGHUP));

                if (FD_ISSET(api_rw_sock, &set)) {
                        /* privileged API call */
                        handle_api(api_rw_sock, 1);
                }

                if (FD_ISSET(api_ro_sock, &set)) {
                        /* unprivileged API call */
                        handle_api(api_ro_sock, 0);
                }

                for (node = list_get_first(&config.interfaces); node != NULL;
                     node = list_get_next(node)) {
                        iface = (struct interface_entry *) node;
                        if (FD_ISSET(iface->udp_sock, &set)) {
                                LOG(LOG_INFO, "Got UDP message(%s)\n",
                                      iface->dev);

                                handle_reg_msg(iface, iface->udp_sock);
                                
                                
                        }
                        if (iface->ha_disc && iface->udp_bc_sock > -1 &&
                            FD_ISSET(iface->udp_bc_sock, &set)) {
                                DEBUG(DEBUG_FLAG, "Got UDP (BC) message(%s)\n",
                                      iface->dev);

                
                                handle_reg_msg(iface, iface->udp_bc_sock);
                        }
                        if (iface->ha_disc && iface->udp_bc_sock2 > -1 &&
                            FD_ISSET(iface->udp_bc_sock2, &set)) {
                                DEBUG(DEBUG_FLAG, "Got UDP (BC "
                                      "255.255.255.255) message(%s)\n",
                                      iface->dev);
        
                                
                                handle_reg_msg(iface, iface->udp_bc_sock2);
                        }
                        if (FD_ISSET(iface->icmp_sock, &set)) {
                                handle_icmp(iface);
                        }
                        
                }


                if (timerisset(&next_agentadv))
                        send_agent_advs(&next_agentadv);

                check_bindings();
                tunnel_check_delayed(tunnels, 0);
                check_queued_gratuitous_arp();

                sigsetmask(oldmask);
                
        }

        /* not reached */
        return 0;
}

---