#include <pthread.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <arpa/inet.h>
#include <errno.h>
#include <unistd.h>
#include <signal.h>
#include <pcap.h>
#include <net/if_ether.h>
#include <netinet/udp.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <net/if_arp.h>
#include <net/ethertypes.h>
#include <netdb.h>
#include <net/if_dl.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#include "etashead/etas_typ.h"
#include "etasThreadPrio.h"
#include "udpErr.h"
#include "udpBuffer.h"
#include "ethIpUdp.h"
#include "libetas.h"

#include "udpSess.h"


#define UDP_SERVER_SESSION_ATTRIBUTES_START_KEY    0x63528472
#define UDP_SERVER_SESSION_ATTRIBUTES_END_KEY      0x78452374

#define NETWORK_DEVICE_NAME               "tsec1"
/** Snap capture length for pcap library , choose 1500 bytes (full Ethernet frame) */
#define PCAP_SNAPLEN            (1500u)


#define FILTER_STRING_LEN        64

#define ETH_IP_UDP_HEADER_SIZE 42

#define PROTO_ICMP             1

#define PROTO_TYPE_ARP  0x0800


#define CONNECT_TIMEOUT_MS                     5000
#define ARP_REPLY_TIMEOUT_MS                    5000



typedef struct
{
   struct ether_header eh;
   
   struct arphdr arph;
   
   uint8_t ethernetSrc[ETHER_ADDR_LEN];
   uint32_t ipAddressSrc;
   uint8_t ethernetDest[ETHER_ADDR_LEN];
   uint32_t ipAddressDest;
   
}__attribute__((__packed__))
ArpPacket;



typedef struct
{
   uint8_t zero;
   uint8_t protocolId;
   uint16   udpLen;
   uint32_t ipAddressSrc;
   uint32_t ipAddressDest;      
}__attribute__((__packed__))
PseudoPseudoHeader;








/**************************************** global variables *********************************************/


/**************************************** local variables **********************************************/
static UdpsSessionAttributes l_udpsSessionAttributes[MAX_NOF_UDPS_SESSIONS];


/*************************************** local function declarations ***********************************/
static int get_mac_address(pcap_if_t *pcap_ifs, char *devname, struct ether_addr *mac);
static void *f_udpsTsReceiveThread_pv(void *p_arg_pv);
static void f_packetHandler_v(u_char *p_user_pu8,
                                 const struct pcap_pkthdr *p_packetHeader_pst,
                                 const u_char *p_data_pu8);
static void *f_udpsTsSocketReceiveThread_pv(void *p_arg_v);


/*************************************** global function declarations ***********************************/
extern int delayInitExitTask(uint32 durationMs);


/************************************** global functions ************************************************/
UdpsError f_udpsTsInitSessionAttributes_e(void)
{
   UdpsError l_error_e = UdpsSuccess;
   uint32 l_sessionIndex_u32;
   UdpsSessionAttributes *l_session_p;
   pthread_mutexattr_t l_attr_st;

   /* Set Recursive attribute for Receive Locking because Lock Function could be called by several OSEK Tasks concurrently */
   pthread_mutexattr_init(&l_attr_st);
   pthread_mutexattr_setrecursive(&l_attr_st, PTHREAD_RECURSIVE_ENABLE); 


   for(l_sessionIndex_u32 = 0; l_sessionIndex_u32 < MAX_NOF_UDPS_SESSIONS; l_sessionIndex_u32++)
   {
      l_session_p = &l_udpsSessionAttributes[l_sessionIndex_u32];      

      l_session_p->inUse_b = false;
      l_session_p->sessionNo_u32 = l_sessionIndex_u32;
      l_session_p->clientRef_pv = 0;
      l_session_p->rcvCallback = NULL;
      l_session_p->asyncErrorCallback = NULL;
      l_session_p->localIpAddress_u32 = 0;
      l_session_p->remoteIpAddress_u32 = 0;
      l_session_p->localPort_u16 = 0;
      l_session_p->remotePort_u16 = 0;
#if 0
      l_session_p->sfd = -1;
#endif
      l_session_p->pcapDesc_p = 0;
      l_session_p->xcpCtr_u16 = 0;      

      /* Initialize Receive Locking Mutex */
      pthread_mutex_init(&l_session_p->receiveLock, &l_attr_st);

      l_session_p->startKeyValue_u32 = UDP_SERVER_SESSION_ATTRIBUTES_START_KEY;
      l_session_p->endKeyValue_u32 = UDP_SERVER_SESSION_ATTRIBUTES_END_KEY;
   }

   return l_error_e;   
}


UdpsError f_udpsTsOpenSession_e(uint32 p_localIpAddress_u32,
                           uint16 p_localPort_u16,
                           uint32 p_remoteIpAddress_u32,
                           uint16 p_remotePort_u16,
                           void *p_clientRef_pv,
                           RcvCallback p_udpRcvCallback,
                           AsyncErrorCallback p_asyncErrorCallback,
                           UdpsSessionHandle *p_sessionHandle_p)
{
   UdpsError l_error_e = UdpsSuccess;
   uint32 l_sessionIndex_u32;
   UdpsSessionAttributes *l_session_p;
#if 0
   struct sockaddr_in l_sin_st;
   struct timespec l_startTime_st;
   struct timespec l_currentTime_st;
   uint32 l_elapsedTimeMs_u32;
   int sfd = -1;
 #endif     

   int l_retVal_i;
   char l_errbuf_c[PCAP_ERRBUF_SIZE];
   int l_promisc_i = 0;
   int l_timeoutMs_i;
   bpf_u_int32 l_networkAddress_u32;
   bpf_u_int32 l_subnetMask_u32;
   char l_filterString_c[FILTER_STRING_LEN];
   char l_ipAddressString_c[INET_ADDRSTRLEN];
   struct bpf_program l_program_st;
/*
   struct pcap_pkthdr l_header_st;
*/
   pcap_if_t *l_interfaceList_pst = 0;
   
/*
   struct ether_header *l_etherHeader_pst;
   char *l_deviceName_pc;
*/
/* 
   struct icmp l_icmpHeader_st;
   struct arphdr l_arpHeader_st;
*/

   ArpPacket l_arpRequest_st;
   ArpPacket *l_arpResponse_pst;
   struct pcap_pkthdr *l_pcapHeader_pst;
   uint32 l_value_u32;
   int l_fd_i;
   struct timeval l_timeout_st;
   
   /* Check if a new session can be allocated */
   for(l_sessionIndex_u32 = 0; l_sessionIndex_u32 < MAX_NOF_UDPS_SESSIONS; l_sessionIndex_u32++)
   {      
      l_session_p = &l_udpsSessionAttributes[l_sessionIndex_u32];      
      
      if(false == l_session_p->inUse_b)
      {
         break;
      }         
   }
   
   if(MAX_NOF_UDPS_SESSIONS == l_sessionIndex_u32)
   {
      l_error_e = ErrUdpsAllSessionsInUse;      
   }
   
   /* Initialize Session Attributes */
   if(UdpsSuccess == l_error_e)
   {
      l_session_p->clientRef_pv = p_clientRef_pv; 
      l_session_p->rcvCallback = p_udpRcvCallback;
      l_session_p->asyncErrorCallback = p_asyncErrorCallback;
      l_session_p->xcpCtr_u16 = 0;
      l_session_p->sessionNo_u32 = l_sessionIndex_u32;
      l_session_p->localIpAddress_u32 = p_localIpAddress_u32;
      l_session_p->remoteIpAddress_u32 = p_remoteIpAddress_u32;
      l_session_p->localPort_u16 = p_localPort_u16;
      l_session_p->remotePort_u16 = p_remotePort_u16;

#if 0
      l_session_p->sfd = -1;      
#endif
      l_session_p->pcapDesc_p = 0;

      l_session_p->inUse_b = true;
   }



   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_lookupnet(NETWORK_DEVICE_NAME, &l_networkAddress_u32, &l_subnetMask_u32, l_errbuf_c);

      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsPcapLookupNetFailed;         
      }
      else
      {
         /* check if local IP address is matching */
         if(p_localIpAddress_u32 != l_networkAddress_u32)
         {
            l_error_e = ErrUdpsIpAddressMismatch;            
         }    
         
#if 0
         /* Omit check because subnet mask is not returned correctly from pcap_lookupnet */

         if(UdpsSuccess == l_error_e)
         {
            /* check if remote address is in the subnet */
            if((p_remoteIpAddress_u32 & l_subnetMask_u32) !=
               (p_localIpAddress_u32 & l_subnetMask_u32))
            {
               l_error_e = ErrUdpsInvalidRemoteIpAddress;                           
            }            
         }
#endif

      }
   }

   /* Open network device for capturing */
   if(UdpsSuccess == l_error_e)
   {
      /* Set timeout because thread would hang if target does not respond */
      l_timeoutMs_i = ARP_REPLY_TIMEOUT_MS;

      l_session_p->pcapDesc_p = pcap_open_live(NETWORK_DEVICE_NAME,
                                                      PCAP_SNAPLEN,
                                                      l_promisc_i,
                                                      l_timeoutMs_i,
                                                      l_errbuf_c); 
      
      if(0 == l_session_p->pcapDesc_p)
      {
         l_error_e = ErrUdpsPcapOpenLiveFailed;         
      }            
   }


   /* Set BIOCIMMEDIATE for capturing device */
   if(UdpsSuccess == l_error_e)
   {
      l_fd_i = pcap_fileno(l_session_p->pcapDesc_p);
   
      if(-1 == l_fd_i)
      {
         /* File descriptor not available */         
         l_error_e = ErrUdpsPcapFilenoFailed;         
      }


      if(UdpsSuccess == l_error_e)
      {
         l_value_u32 = 1;
         l_retVal_i = ioctl(l_fd_i, BIOCIMMEDIATE, &l_value_u32);         

         if(-1 == l_retVal_i)
         {
            l_error_e = ErrUdpsPcapIoctlBiocimmediateFailed;               
         }
      }
   }


   /* Generate Filter String */
   /* First, we want to filter ARP replies in order to obtain the slaves Ethernet address */
   /* Filter als UDP packets that come from the remote node (IP address and Port number) */
   if(UdpsSuccess == l_error_e)
   {
      inet_ntop(AF_INET, &p_remoteIpAddress_u32, l_ipAddressString_c, INET_ADDRSTRLEN);

      snprintf(l_filterString_c, FILTER_STRING_LEN, "arp src host %s", l_ipAddressString_c); 
      
      /* Compile Filter String */
      l_retVal_i = pcap_compile(l_session_p->pcapDesc_p,
                                        &l_program_st,
                                        l_filterString_c,
                                        0,
                                        l_subnetMask_u32); 

      if(-1 == l_retVal_i)
      {
         /* Error while compiling filter string */         
         l_error_e = ErrUdpsPcapCompileFailed;         
      }
   }

   /* Load filter program into packet capture device */
   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_setfilter(l_session_p->pcapDesc_p, &l_program_st);

      if(-1 == l_retVal_i)
      {
         /* Error while setup the filter */         
         l_error_e = ErrUdpsPcapSetFilterFailed;                           
      }

      /* Free filter program */
      pcap_freecode(&l_program_st);

   }   



   /* We must generate an ARP request in order to get the slave MAC address */

   /* Get local MAC address */
   /* Find all devices for capturing. This is done to get the MAC address of the selected capturing device */
   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_findalldevs(&l_interfaceList_pst, l_errbuf_c);

      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsPcapFindAllDevsFailed;         
      }
   }   


   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = get_mac_address(l_interfaceList_pst,
                                       NETWORK_DEVICE_NAME,
                                       (struct ether_addr *)&l_session_p->etherHeaderOut_st.ether_shost);


      
      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsGetMacAddressFailed;         
      }      
   }

   /* Free capturing interface list */
   if(0 != l_interfaceList_pst)
   {
      pcap_freealldevs(l_interfaceList_pst);
   }


   /* Write ARP Frame */
   if(UdpsSuccess == l_error_e)
   {
      /* Broadcast */
      memset(l_arpRequest_st.eh.ether_dhost, 0xFF, ETHER_ADDR_LEN);
      memcpy(l_arpRequest_st.eh.ether_shost, &l_session_p->etherHeaderOut_st.ether_shost, ETHER_ADDR_LEN);
      l_arpRequest_st.eh.ether_type = ETHERTYPE_ARP;
   
   
      l_arpRequest_st.arph.ar_hrd = ARPHRD_ETHER;
      l_arpRequest_st.arph.ar_pro = PROTO_TYPE_ARP;
      l_arpRequest_st.arph.ar_hln = ETHER_ADDR_LEN;
      l_arpRequest_st.arph.ar_pln = 4;
      l_arpRequest_st.arph.ar_op = ARPOP_REQUEST;

      memcpy(l_arpRequest_st.ethernetSrc, &l_session_p->etherHeaderOut_st.ether_shost, ETHER_ADDR_LEN); 
      l_arpRequest_st.ipAddressSrc = l_session_p->localIpAddress_u32;
      memset(l_arpRequest_st.ethernetDest, 0, ETHER_ADDR_LEN); 
      l_arpRequest_st.ipAddressDest = l_session_p->remoteIpAddress_u32;      
   }

   /* Send ARP Request */
   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_inject(l_session_p->pcapDesc_p, &l_arpRequest_st, sizeof(l_arpRequest_st));

      if(sizeof(l_arpRequest_st) != l_retVal_i)
      {
         l_error_e = ErrUdpsPcapInjectFailed;  
      }
   }

   if(UdpsSuccess == l_error_e)
   {
      /* read ARP response packet to obtain the remote MAC address */      
      l_retVal_i = pcap_next_ex(l_session_p->pcapDesc_p, &l_pcapHeader_pst, (const u_char **)&l_arpResponse_pst);
      if(1 != l_retVal_i)
      {
         l_error_e = ErrUdpsPcapNextExFailed;            
      }         
      else
      {
         /* Extract Remote MAC Address from ARP response */             
         /* check if this is an ARP response */         
         if(ARPOP_REPLY != l_arpResponse_pst->arph.ar_op)
         {
            l_error_e = ErrUdpsInvalidArpReplyFormat;            
         }
         else if(l_session_p->remoteIpAddress_u32 != l_arpResponse_pst->ipAddressSrc)
         {
            l_error_e = ErrUdpsIpAddressMismatch;            
            
         }
         else
         {
            /* Complete local Ethernet Header structure for later sending of Ethernet Packets */            
            memcpy(l_session_p->etherHeaderOut_st.ether_dhost, l_arpResponse_pst->ethernetSrc, ETHER_ADDR_LEN);             
            l_session_p->etherHeaderOut_st.ether_type = ETHERTYPE_IP;            
         }         
      }         
   }


   if(UdpsSuccess == l_error_e)
   {
      /* close pcap capturing device, because timeout > 0 is not suitable when pcap_next should return after every
      * single packet */
      pcap_close(l_session_p->pcapDesc_p);
   }     
    
   /* Reopen device */  
   if(UdpsSuccess == l_error_e)
   {
      /* Timeout 0 is necessary to let pcap_next return immidiately after capturing of a single frame */
      l_timeoutMs_i = 0;

      l_session_p->pcapDesc_p = pcap_open_live(NETWORK_DEVICE_NAME,
                                                      PCAP_SNAPLEN,
                                                      l_promisc_i,
                                                      l_timeoutMs_i,
                                                      l_errbuf_c); 
      
      if(0 == l_session_p->pcapDesc_p)
      {
         l_error_e = ErrUdpsPcapOpenLiveFailed;         
      }            
   }


   /* Set BIOCIMMEDIATE for capturing device */
   if(UdpsSuccess == l_error_e)
   {
      l_fd_i = pcap_fileno(l_session_p->pcapDesc_p);
   
      if(-1 == l_fd_i)
      {
         /* File descriptor not available */         
         l_error_e = ErrUdpsPcapFilenoFailed;         
      }


      if(UdpsSuccess == l_error_e)
      {
         l_value_u32 = 1;
         l_retVal_i = ioctl(l_fd_i, BIOCIMMEDIATE, &l_value_u32);         

         if(-1 == l_retVal_i)
         {
            l_error_e = ErrUdpsPcapIoctlBiocimmediateFailed;               
         }
      }
   }




#if 0
   /* Use ICMP socket for this purpose */
   if(UdpsSuccess == l_error_e)
   {
      sfd = socket(AF_INET, SOCK_RAW, PROTO_ICMP);
   
      if(-1 == sfd)
      {
         /* socket creation failed */
         l_error_e = ErrUdpsSocketCreationFailed;      
      }
   }


   /* Send a ping message */
   l_sin_st.sin_family = AF_INET;
   l_sin_st.sin_port = htons(p_remotePort_u16);
   l_sin_st.sin_addr.s_addr = htonl(p_remoteIpAddress_u32);  

   l_icmpHeader_st.icmp_type = ICMP_ECHO;
   l_icmpHeader_st.icmp_code = 0;
   l_icmpHeader_st.icmp_cksum = 0;
   l_icmpHeader_st.icmp_hun.ih_idseq.icd_id = 0;
   l_icmpHeader_st.icmp_hun.ih_idseq.icd_seq = 0;
   

   


   /* Bind Socket to local address */
   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(p_localPort_u16);

      /* todo check why master address cannot be specified */
      l_sin_st.sin_addr.s_addr = INADDR_ANY /* htonl(p_localIpAddress_u32) */;  
      
      l_retVal_i = bind(sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));
   
      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsSocketBindingFailed;
      }
   }


   /* connect to remote address */
   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(p_remotePort_u16);
      l_sin_st.sin_addr.s_addr = htonl(p_remoteIpAddress_u32);  

      /* The Network System is started after the Simulation Model -> we might have to wait here */
      clock_gettime(CLOCK_REALTIME, &l_startTime_st);

      while(1)
      {
         l_retVal_i = connect(sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));

         if(-1 != l_retVal_i)
         {
            break;
         }

         clock_gettime(CLOCK_REALTIME, &l_currentTime_st);
 
         l_elapsedTimeMs_u32 = (uint32)((double)(l_currentTime_st.tv_sec - l_startTime_st.tv_sec) * 1000.0 +
                                  (double)(l_currentTime_st.tv_nsec - l_startTime_st.tv_nsec) / 1e6);  

         if(l_elapsedTimeMs_u32 > CONNECT_TIMEOUT_MS)
         {
            break;
         }

         delayInitExitTask(10);
      }

      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsSocketConnectFailed;
      }
   }

   if(-1 != sfd)
   {
      /* The socket is no longer needed -> close it */
      close(sfd);      
   }


   /* Evaluate the ARP reply to get ethernet addresses */
   if(UdpsSuccess == l_error_e)
   {
      l_etherHeader_pst = (struct ether_header *)pcap_next(l_session_p->pcapDesc_p, &l_header_st);

      if(0 == l_etherHeader_pst)
      {
         /* Error while reading ARP reply */
         l_error_e = ErrUdpsPcapReadArpReplyFailed;         
      }
      else
      {
         /* set ethernet header for outgoing packets */ 
         set_ether_header( &l_session_p->etherHeaderOut_st, 
                       (struct ether_addr *)l_etherHeader_pst->ether_shost,
                       (struct ether_addr *)l_etherHeader_pst->ether_dhost);
      }
   }

#endif



   /* Replace filter in order to capture XCP Packets */
   if(UdpsSuccess == l_error_e)
   {
      inet_ntop(AF_INET, &p_remoteIpAddress_u32, l_ipAddressString_c, INET_ADDRSTRLEN);

      snprintf(l_filterString_c, FILTER_STRING_LEN, "ip src host %s and udp src port %u",
                                                       l_ipAddressString_c,
                                                       p_remotePort_u16); 
      
      /* Compile Filter String */
      l_retVal_i = pcap_compile(l_session_p->pcapDesc_p,
                                        &l_program_st,
                                        l_filterString_c,
                                        0,
                                        l_subnetMask_u32); 

      if(-1 == l_retVal_i)
      {
         /* Error while compiling filter string */         
         l_error_e = ErrUdpsPcapCompileFailed;         
      }
   }


   /* Load filter program into packet capture device */
   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_setfilter(l_session_p->pcapDesc_p, &l_program_st);

      if(-1 == l_retVal_i)
      {
         /* Error while setup the filter */         
         l_error_e = ErrUdpsPcapSetFilterFailed;                           
      }

      /* Free filter program */
      pcap_freecode(&l_program_st);

   }   


   /* verify timeout value */
   if(UdpsSuccess == l_error_e)
   {
      l_fd_i = pcap_fileno(l_session_p->pcapDesc_p);
   
      if(-1 == l_fd_i)
      {
         /* File descriptor not available */         
         l_error_e = ErrUdpsPcapFilenoFailed;         
      }


      if(UdpsSuccess == l_error_e)
      {
         l_value_u32 = 1;
         l_retVal_i = ioctl(l_fd_i, BIOCGRTIMEOUT , &l_timeout_st);         

         if(-1 == l_retVal_i)
         {
            l_error_e = ErrUdpsPcapIoctlBiocimmediateFailed;               
         }
      }
   }


   /* verify blocking state */
   if(UdpsSuccess == l_error_e)
   {
      l_retVal_i = pcap_getnonblock(l_session_p->pcapDesc_p, NULL);
      printf("nonblock = %d\n", l_retVal_i);
   }



#if 0

   /* Create an UDP socket for this session */
   if(UdpsSuccess == l_error_e)
   {
      l_session_p->sfd = socket(AF_INET, SOCK_DGRAM, 0);

      if(-1 == l_session_p->sfd)
      {
         l_error_e = ErrUdpsSocketCreationFailed;         
      }
   }

   /* Bind Socket to local address */
   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(p_localPort_u16);

      /* todo check why master address cannot be specified */
      l_sin_st.sin_addr.s_addr = INADDR_ANY /* htonl(p_localIpAddress_u32) */;  
      
      l_retVal_i = bind(l_session_p->sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));
   
      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsSocketBindingFailed;
      }
   }


   /* connect to remote address */
   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(p_remotePort_u16);
      l_sin_st.sin_addr.s_addr = htonl(p_remoteIpAddress_u32);  

      /* The Network System is started after the Simulation Model -> we might have to wait here */
      clock_gettime(CLOCK_REALTIME, &l_startTime_st);

      while(1)
      {
         l_retVal_i = connect(l_session_p->sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));

         if(-1 != l_retVal_i)
         {
            break;
         }

         clock_gettime(CLOCK_REALTIME, &l_currentTime_st);
 
         l_elapsedTimeMs_u32 = (uint32)((double)(l_currentTime_st.tv_sec - l_startTime_st.tv_sec) * 1000.0 +
                                  (double)(l_currentTime_st.tv_nsec - l_startTime_st.tv_nsec) / 1e6);  

         if(l_elapsedTimeMs_u32 > CONNECT_TIMEOUT_MS)
         {
            break;
         }

         delayInitExitTask(10);
      }

      if(-1 == l_retVal_i)
      {
         l_error_e = ErrUdpsSocketConnectFailed;
      }
   }

#endif

   /* create socket receive thread to avoid ICMP "Port unreachable" messages */
   pthread_create(NULL, NULL, f_udpsTsSocketReceiveThread_pv, (void *)l_session_p);

   sleep(1);


   /* create a receive thread for this session */
   if(UdpsSuccess == l_error_e)
   {
      if(EOK != pthread_create(&l_session_p->tid, NULL, f_udpsTsReceiveThread_pv, (void *)l_session_p))
      {
         l_error_e = ErrUdpsReceiveThreadCreationFailed;           
      }
      else
      {
         pthread_setname_np(l_session_p->tid, "UDP_RECEIVE");
      }
   }

   sleep(1);

   /* Return Session Handle */
   if(UdpsSuccess == l_error_e)
   {
      *p_sessionHandle_p = (UdpsSessionHandle)l_session_p;
   }
   
   return l_error_e;   
}                           


UdpsError f_udpsTsCloseSession_e(UdpsSessionHandle p_sessionHandle)
{
   UdpsError l_error_e = UdpsSuccess;
   UdpsSessionAttributes *l_session_p;

   l_error_e = f_udpsTsGetSessionAttributesFromHandle_e(p_sessionHandle, &l_session_p);

   /* Terminate Receive Thread */
   if(UdpsSuccess == l_error_e)
   {
      if(EOK != pthread_cancel(l_session_p->tid))
      {
         l_error_e = ErrUdpsReceiveThreadTerminationFailed;         
      }   
   }

   /* Wait for thread termination */
   if(UdpsSuccess == l_error_e)
   {
      if(EOK != pthread_join(l_session_p->tid, NULL))
      {   
         l_error_e = ErrUdpsReceiveThreadTerminationFailed;         
      }
   }



#if 0
   /* Close the socket */
   if(UdpsSuccess == l_error_e)
   {
      if(-1 == close(l_session_p->sfd))
      {
         l_error_e = ErrUdpsSocketClosingFailed;         
      }
   }
#endif

   if(UdpsSuccess == l_error_e)
   {
      /* Mark Session as invalid */
      l_session_p->inUse_b = false;
   }
   
   return l_error_e;  
}                               




UdpsError f_udpsTsGetSessionAttributesFromHandle_e(UdpsSessionHandle p_sessionHandle,
                                                      UdpsSessionAttributes **p_sessionAttributes_pp)
{
   UdpsError l_error_e = UdpsSuccess;

   /* Check session handle plausibility */
   if((p_sessionHandle < (UdpsSessionHandle)l_udpsSessionAttributes) ||
                              (p_sessionHandle >= (UdpsSessionHandle)((uint32)l_udpsSessionAttributes + sizeof(l_udpsSessionAttributes))))
   {
      l_error_e = ErrUdpsInvalidSessionHandle;      
   }

   if(UdpsSuccess == l_error_e)
   {
      if((((UdpsSessionAttributes *)p_sessionHandle)->startKeyValue_u32 != UDP_SERVER_SESSION_ATTRIBUTES_START_KEY)||
            (((UdpsSessionAttributes *)p_sessionHandle)->endKeyValue_u32 != UDP_SERVER_SESSION_ATTRIBUTES_END_KEY))
      {
         l_error_e = ErrUdpsInvalidSessionAttributeKey;         
      }
   }

   if(UdpsSuccess == l_error_e)
   {
      *p_sessionAttributes_pp = (UdpsSessionAttributes *)p_sessionHandle;   
   }
      
   return l_error_e;   
} 


UdpsSessionAttributes *f_udpsTsGetSessionAttributePtrFromSessionNo_p(uint32 p_sessionNo_u32)
{
   UdpsSessionAttributes *l_session_p;

   if(p_sessionNo_u32 < MAX_NOF_UDPS_SESSIONS)
   {
      l_session_p = &l_udpsSessionAttributes[p_sessionNo_u32];     
      
      if(true == l_session_p->inUse_b)
      {
         return l_session_p;         
      }
      else
      {
         return 0;
      }
   }
   else
   {
      return 0;    
   }     
}




void f_udpsCallAyncErrorCallbackForAllSessions_v(UdpsError p_errorCode_e,
                                                   char *p_errorString_pc)
{
   uint32 l_sessionIndex_u32;
   UdpsSessionAttributes *l_session_p;

   for(l_sessionIndex_u32 = 0; l_sessionIndex_u32 < MAX_NOF_UDPS_SESSIONS; l_sessionIndex_u32++)
   {
      l_session_p = &l_udpsSessionAttributes[l_sessionIndex_u32];      

      if((true == l_session_p->inUse_b) && (0 != l_session_p->asyncErrorCallback))
      {
         
         (l_session_p->asyncErrorCallback)((UdpsSessionHandle)l_session_p,
                                                            l_session_p->clientRef_pv,
                                                            p_errorCode_e,
                                                            p_errorString_pc);         
      }   
   }   
   
   return;
}





/******************************** local functions *****************************/

/**
 * Get the MAC address out of an interface that has been listed with pcap_findalldevs.
 * This routine looks for an interface addreess of family type AF_LINK on the given
 * device and casts that to a low level address using the LLADDR macro voodoo. 
 */
static int get_mac_address(pcap_if_t * pcap_ifs, char * devname, struct ether_addr * mac)
{
    char hbuf[NI_MAXHOST];
    
    /* Loop through all the interfaces */
    while(pcap_ifs)
    {
        pcap_addr_t *addr;
        
        /* Select interface by name */
        if( 0 == strncmp(pcap_ifs->name, devname, IFNAMSIZ-1))
        {
            addr = pcap_ifs->addresses;
            while(addr)
            {
                /* Look for the AF_LINK type address on this interface */
                if(AF_LINK == addr->addr->sa_family)
                {
                    if( EOK == getnameinfo(addr->addr, addr->addr->sa_len,
                                           hbuf, sizeof(hbuf), NULL, 0, 
                                           NI_NUMERICHOST)
                        && hbuf[0] != '\0')
                    {
                        debugLog("%s: AF_LINK address %s", devname, hbuf);

                        /* Copy the LL address */                        
                        memcpy(mac->ether_addr_octet,  
                               LLADDR((const struct sockaddr_dl *)addr->addr),
                               ETHER_ADDR_LEN);
                        return EOK;                        
                    }                                                                   
                }                
                addr = addr->next;
            }
            break;
        }                    
        pcap_ifs = pcap_ifs->next;            
    }
    
    debugLog("%s: AF_LINK address not found.", devname);
    return -1;
}    





static void *f_udpsTsReceiveThread_pv(void *p_arg_pv)
{
   pthread_t tid;
   int policy;
   struct sched_param sched_param;
   sigset_t l_sigset_st;
   UdpsSessionAttributes *l_session_p;
#if 0
   ssize_t nofBytes;
   uint8 *l_buffer_pu8;
   UdpsError l_error_e = UdpsSuccess;
   struct pcap_pkthdr *l_header_pst;
   uint8 *l_data_pu8;
   uint32 l_checksum_u32;
   struct ip *l_ipHeader_pst;
   struct udphdr *l_udpHeader_pst;
   PseudoPseudoHeader *l_pseudoHeader_pst;
#endif
   int l_retVal_i;

   l_session_p = (UdpsSessionAttributes *)p_arg_pv;

   /* adjust thread priority */
   tid = pthread_self();
   pthread_getschedparam(tid, &policy, &sched_param );
   sched_param.sched_priority = XCPOE_RP_RECEIVE_THREAD;
   pthread_setschedparam(tid, SCHED_FIFO, &sched_param ); 

   /* block all signals for this thread */
   sigfillset(&l_sigset_st);
   sigprocmask(SIG_BLOCK,&l_sigset_st, NULL); 


   /* install packet capturing handler */
   l_retVal_i = pcap_loop(l_session_p->pcapDesc_p, -1, f_packetHandler_v, (u_char *)l_session_p); 

   /* Function should never return */
   if(-1 == l_retVal_i)
   {
      printf("Error in pcap_loop\n");      
   }


#ifdef PCAP_NEXT_VARIANT
   while(1)
   {
      /* Get a buffer from the Pool */      
      l_buffer_pu8 = 0;
      l_error_e = f_udpsTsGetBufferFromPool_e(&g_receiveBufferPool_st, &l_buffer_pu8, true);

      /* Obtain next packet from capture device */
      if(UdpsSuccess == l_error_e)
      {
/*
         do
         {
*/
            l_retVal_i = pcap_next_ex(l_session_p->pcapDesc_p, &l_header_pst,
                                                         (const u_char **)&l_data_pu8); 
/*
         }
         while(0 == l_retVal_i); 
*/

         switch(l_retVal_i)
         {
            case 1:
               /* Success */
               break;            

            case 0:
               /* Read Timeout */
               l_error_e = ErrUdpsPcapNextExTimeout;            
               printf("errno = %s\n", strerror(errno));
               break;
            
            default:
               /* an error occured while reading */
               l_error_e = ErrUdpsPcapNextExFailed;            
               break;            
         }
      }
      
      /* Verify IP Header Checksum */
      if(UdpsSuccess == l_error_e)
      {
         /* Verify IP Header checksum */
         l_ipHeader_pst = (struct ip *)(l_data_pu8 + sizeof(struct ether_header));

         l_checksum_u32 = checksum((uint8 *)l_ipHeader_pst, sizeof(struct ip), 0);

         l_checksum_u32 = wrapsum(l_checksum_u32);

         if(0 != l_checksum_u32)
         {
            /* IP Header checksum verification failed */
            l_error_e = ErrUdpsIpHeaderChecksumError;
         }
      }      

      /* Verify UDP checksum if applicable */
      if(UdpsSuccess == l_error_e)
      {
         l_udpHeader_pst = (struct udphdr *)(l_data_pu8 + sizeof(struct ether_header) + sizeof(struct ip));         
         
         if(0 != l_udpHeader_pst->uh_sum)
         {
            /* Prepend pseudo Pseudo Header for checksum calculation - this overwrites parts of the UDP header */
            l_pseudoHeader_pst = (PseudoPseudoHeader *)((uint8 *)l_udpHeader_pst - sizeof(PseudoPseudoHeader));            

            l_pseudoHeader_pst->zero = 0;
            l_pseudoHeader_pst->protocolId = l_ipHeader_pst->ip_p;
            l_pseudoHeader_pst->udpLen = l_ipHeader_pst->ip_len - sizeof(struct ip);


            /* checksum is used -> verify it */
            l_checksum_u32 = checksum((uint8 *)l_pseudoHeader_pst, l_udpHeader_pst->uh_ulen + sizeof(PseudoPseudoHeader), 0);
            
            l_checksum_u32 = wrapsum(l_checksum_u32);
   
            if(0 != l_checksum_u32)
            {
               /* UDP checksum verification failed */
               l_error_e = ErrUdpsUdpChecksumError;
            }
         }
      }
     

      /* Process UDP data */
      if(UdpsSuccess == l_error_e)
      { 
         /* copy frame to allocated buffer, because the buffer returned by l_data_pu8 is overwritten
          * at the next call of pcap_next */
         /* Put only the UDP data into the receive callback */
         nofBytes = l_header_pst->caplen - ETH_IP_UDP_HEADER_SIZE;        
         memcpy(l_buffer_pu8, l_data_pu8 + ETH_IP_UDP_HEADER_SIZE, nofBytes);         
         

         /* Synchronize Receive Processing in case of Debugging */
         pthread_mutex_lock(&l_session_p->receiveLock);

         /* Frame has been received -> execute callback function */
         l_session_p->rcvCallback((UdpsSessionHandle)l_session_p,
                                       l_session_p->clientRef_pv,
                                       l_buffer_pu8, nofBytes);         

         /* Buffer must be freed by the receive callback function */

          pthread_mutex_unlock(&l_session_p->receiveLock);         
      }


      if(UdpsSuccess != l_error_e)
      { 
         /* An error occured */         
         /* Call the Async Error Handler */         
         l_session_p->asyncErrorCallback((UdpsSessionHandle)l_session_p,
                                             l_session_p->clientRef_pv,
                                             l_error_e,
                                             0);         
         
         /* Free allocated buffer */
         if(0 != l_buffer_pu8)
         {
            f_udpsTsReturnBufferToPool_e(&g_receiveBufferPool_st, l_buffer_pu8);
            l_buffer_pu8 = 0;
         }

      }      

#if   0
      /* Receive Message */
      nofBytes = recv(l_session_p->sfd, l_buffer_pu8, XCPOE_UDP_DATA_BUFFER_SIZE, 0); 
      
      if(nofBytes > 0)
      {
         /* Synchronize Receive Processing in case of Debugging */
         pthread_mutex_lock(&l_session_p->receiveLock);

         /* Frame has been received -> execute callback function */
         l_session_p->rcvCallback((UdpsSessionHandle)l_session_p,
                                       l_session_p->clientRef_pv,
                                       l_buffer_pu8, nofBytes);         

         /* Buffer must be freed by the receive callback function */

          pthread_mutex_unlock(&l_session_p->receiveLock);
      }
      else
      {
         /* An error occured */         
         /* Call the Async Error Handler */         
         if(- 1 == l_error_e)
         {
            l_session_p->asyncErrorCallback((UdpsSessionHandle)l_session_p,
                                             l_session_p->clientRef_pv,
                                             ErrUdpsSocketReceiveFailed,
                                             strerror(errno));         
         }
         
         /* Free allocated buffer */
         f_udpsTsReturnBufferToPool_e(&g_receiveBufferPool_st, l_buffer_pu8);
      }      

#endif

   }
#endif /* PCAP_NEXT_VARIANT */
   return NULL;
}



static uint8 l_receiveBuffer_u8[XCPOE_UDP_DATA_BUFFER_SIZE];
static uint32 socketRcvCnt = 0;

static void *f_udpsTsSocketReceiveThread_pv(void *p_arg_v)
{
   UdpsSessionAttributes *l_session_p;
   UdpsError l_error_e = UdpsSuccess;
   struct sockaddr_in l_sin_st;
   int l_retVal_i;
   ssize_t l_nofBytes_s32;

   l_session_p = (UdpsSessionAttributes *)p_arg_v;
   
   /* Create an UDP socket for this session */
   l_session_p->sfd = socket(AF_INET, SOCK_DGRAM, 0);

   if(-1 == l_session_p->sfd)
   {
      l_error_e = 1;         
   }

   /* Bind Socket to local address */
   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(l_session_p->localPort_u16);

      /* todo check why master address cannot be specified */
      l_sin_st.sin_addr.s_addr = INADDR_ANY /* htonl(p_localIpAddress_u32) */;  
      
      l_retVal_i = bind(l_session_p->sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));
   
      if(-1 == l_retVal_i)
      {
         l_error_e = 2;
      }
   }


   if(UdpsSuccess == l_error_e)
   {
      l_sin_st.sin_family = AF_INET;
      l_sin_st.sin_port = htons(l_session_p->remotePort_u16);
      l_sin_st.sin_addr.s_addr = htonl(l_session_p->remoteIpAddress_u32);  

      l_retVal_i = connect(l_session_p->sfd, (struct sockaddr *)&l_sin_st, sizeof(l_sin_st));

      if(-1 == l_retVal_i)
      {
         l_error_e = 3;         
      }
   }


   /* Dummy receive from socket */   
   while(1)     
   {
      l_nofBytes_s32 = recv(l_session_p->sfd, l_receiveBuffer_u8, XCPOE_UDP_DATA_BUFFER_SIZE, 0); 
         
      if(l_nofBytes_s32 > 0)
      {
         socketRcvCnt++;
      }                     
   }      
      
   
   return NULL;   
}



static unsigned int packetCounter = 0;


static void f_packetHandler_v(u_char *p_user_pu8, const struct pcap_pkthdr *p_packetHeader_pst, const u_char *p_data_pu8)
{
   UdpsSessionAttributes *l_session_p;
   ssize_t l_nofBytes_u32;
   uint8 *l_buffer_pu8;
   UdpsError l_error_e = UdpsSuccess;
   uint32 l_checksum_u32;
   struct ip *l_ipHeader_pst = NULL;
   struct udphdr *l_udpHeader_pst;
   PseudoPseudoHeader *l_pseudoHeader_pst;
   
   l_session_p = (UdpsSessionAttributes *)p_user_pu8;

   packetCounter++;

   if (p_packetHeader_pst->caplen < ETH_IP_UDP_HEADER_SIZE)
   {
      l_error_e = ErrUdpsInvalidPacketCaptureLength;
      printf("Invalid Capture Length. Counter = %u\n", packetCounter);
      exit( -1);
   }      

   /* Verify IP Header Checksum */
   if(UdpsSuccess == l_error_e)
   {
      /* Verify IP Header checksum */
      l_ipHeader_pst = (struct ip *)(p_data_pu8 + sizeof(struct ether_header));

      l_checksum_u32 = checksum((uint8 *)l_ipHeader_pst, sizeof(struct ip), 0);

      l_checksum_u32 = wrapsum(l_checksum_u32);

      if(0 != l_checksum_u32)
      {
         /* IP Header checksum verification failed */
         l_error_e = ErrUdpsIpHeaderChecksumError;
      }
   }      

   /* Verify UDP checksum if applicable */
   if(UdpsSuccess == l_error_e)
   {
      l_udpHeader_pst = (struct udphdr *)(p_data_pu8 + sizeof(struct ether_header) + sizeof(struct ip));         
      
      if(0 != l_udpHeader_pst->uh_sum)
      {
         /* Prepend pseudo Pseudo Header for checksum calculation - this overwrites parts of the UDP header */
         l_pseudoHeader_pst = (PseudoPseudoHeader *)((uint8 *)l_udpHeader_pst - sizeof(PseudoPseudoHeader));            

         l_pseudoHeader_pst->zero = 0;
         l_pseudoHeader_pst->protocolId = l_ipHeader_pst->ip_p;
         l_pseudoHeader_pst->udpLen = l_ipHeader_pst->ip_len - sizeof(struct ip);


         /* checksum is used -> verify it */
         l_checksum_u32 = checksum((uint8 *)l_pseudoHeader_pst, l_udpHeader_pst->uh_ulen + sizeof(PseudoPseudoHeader), 0);
         
         l_checksum_u32 = wrapsum(l_checksum_u32);

         if(0 != l_checksum_u32)
         {
            /* UDP checksum verification failed */
            l_error_e = ErrUdpsUdpChecksumError;
         }
      }
   }
  

   /* Process UDP data */
   if(UdpsSuccess == l_error_e)
   { 
      /* allocate buffer for further UDP frame processing */
      /* Get a buffer from the Pool */      
      l_buffer_pu8 = 0;
      l_error_e = f_udpsTsGetBufferFromPool_e(&g_receiveBufferPool_st, &l_buffer_pu8, true);
   }

   if(UdpsSuccess == l_error_e)
   {
      /* copy frame to allocated buffer, because the buffer returned by l_data_pu8 is overwritten
       * at the next call of pcap_next */
      /* Put only the UDP data into the receive callback */
      l_nofBytes_u32 = p_packetHeader_pst->caplen - ETH_IP_UDP_HEADER_SIZE;        
      memcpy(l_buffer_pu8, p_data_pu8 + ETH_IP_UDP_HEADER_SIZE, l_nofBytes_u32);         
      

      /* Synchronize Receive Processing in case of Debugging */
      pthread_mutex_lock(&l_session_p->receiveLock);

      /* Frame has been received -> execute callback function */
      l_session_p->rcvCallback((UdpsSessionHandle)l_session_p,
                                    l_session_p->clientRef_pv,
                                    l_buffer_pu8, l_nofBytes_u32);         

      /* Buffer must be freed by the receive callback function */

       pthread_mutex_unlock(&l_session_p->receiveLock);         
   }


   if(UdpsSuccess != l_error_e)
   { 
      /* An error occured */         
      /* Call the Async Error Handler */         
      l_session_p->asyncErrorCallback((UdpsSessionHandle)l_session_p,
                                          l_session_p->clientRef_pv,
                                          l_error_e,
                                          0);         
      
      /* Free allocated buffer */
      if(0 != l_buffer_pu8)
      {
         f_udpsTsReturnBufferToPool_e(&g_receiveBufferPool_st, l_buffer_pu8);
         l_buffer_pu8 = 0;
      }

   }      
   
   return;   
}