/*
    EIBD eib bus access and management daemon
    Copyright (C) 2005-2007 Martin Koegler <mkoegler@auto.tuwien.ac.at>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <netdb.h>
#include <sys/socket.h>
#include <unistd.h>
#include "eibnetip.h"
#include "config.h"
#ifdef HAVE_LINUX_NETLINK
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#endif
#ifdef HAVE_WINDOWS_IPHELPER
#define Array XArray
#include <windows.h>
#include <iphlpapi.h>
#undef Array
#endif
#if defined(__FreeBSD__)
#include <netinet/in.h>
#include <net/if.h>
#include <net/route.h>
#endif

int
GetHostIP (struct sockaddr_in *sock, const char *Name)
{
  struct hostent *h;
  if (!Name)
    return 0;
  memset (sock, 0, sizeof (*sock));
  h = gethostbyname (Name);
  if (!h)
    return 0;
  sock->sin_len = sizeof (*sock);
  sock->sin_family = h->h_addrtype;
  sock->sin_addr.s_addr = (*((unsigned long *) h->h_addr_list[0]));
  return 1;
}

#ifdef HAVE_LINUX_NETLINK
typedef struct
{
  struct nlmsghdr n;
  struct rtmsg r;
  char data[1000];
} r_req;

int
GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)
{
  int s;
  int l;
  r_req req;
  struct rtattr *a;
  memset (&req, 0, sizeof (req));
  memset (src, 0, sizeof (*src));
  s = socket (PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
  if (s == -1)
    return 0;
  req.n.nlmsg_len =
    NLMSG_SPACE (sizeof (req.r)) + RTA_LENGTH (sizeof (*dest));
  req.n.nlmsg_flags = NLM_F_REQUEST;
  req.n.nlmsg_type = RTM_GETROUTE;
  req.r.rtm_family = AF_INET;
  req.r.rtm_dst_len = 32;
  a = (rtattr *) ((char *) &req + NLMSG_SPACE (sizeof (req.r)));
  a->rta_type = RTA_DST;
  a->rta_len = RTA_LENGTH (sizeof (dest->sin_addr.s_addr));
  memcpy (RTA_DATA (a), &dest->sin_addr.s_addr,
	  sizeof (dest->sin_addr.s_addr));
  if (write (s, &req, req.n.nlmsg_len) < 0)
    return 0;
  if (read (s, &req, sizeof (req)) < 0)
    return 0;
  close (s);
  if (req.n.nlmsg_type == NLMSG_ERROR)
    return 0;
  l = ((struct nlmsghdr *) &req)->nlmsg_len;
  while (RTA_OK (a, l))
    {
      if (a->rta_type == RTA_PREFSRC
	  && RTA_PAYLOAD (a) == sizeof (src->sin_addr.s_addr))
	{
	  src->sin_family = AF_INET;
	  memcpy (&src->sin_addr.s_addr, RTA_DATA (a), RTA_PAYLOAD (a));
	  return 1;
	}
      a = RTA_NEXT (a, l);
    }
  return 0;
}
#endif

#ifdef HAVE_WINDOWS_IPHELPER
int
GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)
{
  DWORD d = 0;
  PMIB_IPADDRTABLE tab;
  DWORD s = 0;

  memset (src, 0, sizeof (*src));
  if (GetBestInterface (dest->sin_addr.s_addr, &d) != NO_ERROR)
    return 0;

  tab = (MIB_IPADDRTABLE *) malloc (sizeof (MIB_IPADDRTABLE));
  if (!tab)
    return 0;
  if (GetIpAddrTable (tab, &s, 0) == ERROR_INSUFFICIENT_BUFFER)
    {
      tab = (MIB_IPADDRTABLE *) realloc (tab, s);
      if (!tab)
	return 0;
    }
  if (GetIpAddrTable (tab, &s, 0) != NO_ERROR)
    {
      if (tab)
	free (tab);
      return 0;
    }
  for (int i = 0; i < tab->dwNumEntries; i++)
    if (tab->table[i].dwIndex == d)
      {
	src->sin_family = AF_INET;
	src->sin_addr.s_addr = tab->table[i].dwAddr;
	free (tab);
	return 1;
      }
  free (tab);
  return 0;
}
#endif

#if defined(__FreeBSD__)
typedef struct                                                                                            
{
  struct rt_msghdr hdr;
  char data[1000];                                                                                       
} r_req;

int
GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)                                
{
  int s;
  r_req req; 
  char *cp = req.data;                                                                                    
  memset(&req, 0, sizeof(req));                                                                           
  memset(src, 0, sizeof (*src));                                                                          
  s = socket(PF_ROUTE, SOCK_RAW, 0);
  if (s == -1)                                                                                            
    return 0; 
  req.hdr.rtm_msglen = sizeof(req) + sizeof(*dest);                                                       
  req.hdr.rtm_version = RTM_VERSION;                                                                      
  req.hdr.rtm_flags = RTF_UP;                                                                             
  req.hdr.rtm_type = RTM_GET;
  req.hdr.rtm_addrs = RTA_DST | RTA_IFP;                                                                  
  memcpy(cp, dest, sizeof(*dest));                                                                        
  if (write (s, (char *)&req, req.hdr.rtm_msglen) < 0)                                                    
    return 0;
  if (read(s, (char *)&req, sizeof(req)) < 0)                                                             
    return 0;
  close(s);
  int i;
  cp = (char *)(&req.hdr + 1);                                                                            
  for (i = 1; i; i <<= 1)                                                                                 
    if (i & req.hdr.rtm_addrs)                                                                            
    { 
      struct sockaddr *sa = (struct sockaddr *) cp;                                                       
      if (i == RTA_IFA)                                                                                   
      {                                                                                                   
        src->sin_len = sizeof (*src);
        src->sin_family = AF_INET;                                                                        
        src->sin_addr.s_addr = ((struct sockaddr_in *)sa)->sin_addr.s_addr;                               
        return 1;
      }
      cp += SA_SIZE(sa);                                                                                  
    } 
  return 0;
}
#endif

EIBNetIPPacket::EIBNetIPPacket ()
{
  service = 0;
}

EIBNetIPPacket *
EIBNetIPPacket::fromPacket (const CArray & c)
{
  EIBNetIPPacket *p;
  unsigned len;
  if (c () < 6)
    return 0;
  if (c[0] != 0x6 || c[1] != 0x10)
    return 0;
  len = (c[4] << 8) | c[5];
  if (len != c ())
    return 0;
  p = new EIBNetIPPacket;
  p->service = (c[2] << 8) | c[3];
  p->data.set (c.array () + 6, len - 6);
  return p;
}

CArray
EIBNetIPPacket::ToPacket ()
  CONST
{
  CArray c;
  c.resize (6 + data ());
  c[0] = 0x06;
  c[1] = 0x10;
  c[2] = (service >> 8) & 0xff;
  c[3] = (service) & 0xff;
  c[4] = ((data () + 6) >> 8) & 0xff;
  c[5] = ((data () + 6)) & 0xff;
  c.setpart (data, 6);
  return c;
}

CArray
IPtoEIBNetIP (const struct sockaddr_in * a)
{
  CArray buf;
  buf.resize (8);
  buf[0] = 0x08;
  buf[1] = 0x01;
  buf[2] = (ntohl (a->sin_addr.s_addr) >> 24) & 0xff;
  buf[3] = (ntohl (a->sin_addr.s_addr) >> 16) & 0xff;
  buf[4] = (ntohl (a->sin_addr.s_addr) >> 8) & 0xff;
  buf[5] = (ntohl (a->sin_addr.s_addr) >> 0) & 0xff;
  buf[6] = (ntohs (a->sin_port) >> 8) & 0xff;
  buf[7] = (ntohs (a->sin_port) >> 0) & 0xff;
  return buf;
}

int
EIBnettoIP (const CArray & buf, struct sockaddr_in *a)
{
  int ip, port;
  memset (a, 0, sizeof (*a));
  if (buf[0] != 0x8 || buf[1] != 0x1)
    return 1;
  ip = (buf[2] << 24) | (buf[3] << 16) | (buf[4] << 8) | (buf[5]);
  port = (buf[6] << 8) | (buf[7]);
  a->sin_len = sizeof (*a);
  a->sin_family = AF_INET;
  a->sin_port = htons (port);
  a->sin_addr.s_addr = htonl (ip);
  return 0;
}

EIBNetIPSocket::EIBNetIPSocket (struct sockaddr_in bindaddr, bool reuseaddr,
				Trace * tr)
{
  int i;
  t = tr;
  TRACEPRINTF (t, 0, this, "Open");
  multicast = 0;
  pth_sem_init (&insignal);
  pth_sem_init (&outsignal);
  getwait = pth_event (PTH_EVENT_SEM, &outsignal);
  memset (&maddr, 0, sizeof (maddr));
  memset (&sendaddr, 0, sizeof (sendaddr));
  memset (&recvaddr, 0, sizeof (recvaddr));
  recvall = 0;

  fd = socket (AF_INET, SOCK_DGRAM, 0);
  if (fd == -1)
    throw Exception (DEV_OPEN_FAIL);

  if (reuseaddr)
    {
      i = 1;
      if (setsockopt (fd, SOL_SOCKET, SO_REUSEADDR, &i, sizeof (i)) == -1)
	throw Exception (DEV_OPEN_FAIL);
    }
  if (bind (fd, (struct sockaddr *) &bindaddr, sizeof (bindaddr)) == -1)
    throw Exception (DEV_OPEN_FAIL);

  Start ();
  TRACEPRINTF (t, 0, this, "Openend");
}

EIBNetIPSocket::~EIBNetIPSocket ()
{
  TRACEPRINTF (t, 0, this, "Close");
  Stop ();
  pth_event_free (getwait, PTH_FREE_THIS);
  if (fd != -1)
    {
      if (multicast)
	setsockopt (fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &maddr, sizeof (maddr));
      close (fd);
    }
}

void
EIBNetIPSocket::SetMulticast (struct ip_mreq multicastaddr)
{
  if (multicast)
    throw Exception (DEV_OPEN_FAIL);
  maddr = multicastaddr;
  if (setsockopt (fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &maddr, sizeof (maddr)) ==
      -1)
    throw Exception (DEV_OPEN_FAIL);
  multicast = 1;
}

void
EIBNetIPSocket::Send (EIBNetIPPacket p)
{
  struct _EIBNetIP_Send s;
  t->TracePacket (1, this, "Send", p.data);
  s.data = p;
  s.addr = sendaddr;
  inqueue.put (s);
  pth_sem_inc (&insignal, 1);
}

EIBNetIPPacket *
EIBNetIPSocket::Get (pth_event_t stop)
{
  if (stop != NULL)
    pth_event_concat (getwait, stop, NULL);

  pth_wait (getwait);

  if (stop)
    pth_event_isolate (getwait);

  if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
    {
      pth_sem_dec (&outsignal);
      t->TracePacket (1, this, "Recv", outqueue.top ().data);
      return new EIBNetIPPacket (outqueue.get ());
    }
  else
    return 0;
}

void
EIBNetIPSocket::Run (pth_sem_t * stop1)
{
  int i;
  uchar buf[255];
  socklen_t rl;
  sockaddr_in r;
  pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
  pth_event_t input = pth_event (PTH_EVENT_SEM, &insignal);
  while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
    {
      pth_event_concat (stop, input, NULL);
      rl = sizeof (r);
      memset (&r, 0, sizeof (r));
      i =
	pth_recvfrom_ev (fd, buf, sizeof (buf), 0, (struct sockaddr *) &r,
			 &rl, stop);
      if (i > 0 && rl == sizeof (r))
	{
	  if (recvall == 1 || !memcmp (&r, &recvaddr, sizeof (r)) ||
	      (recvall == 2 && memcmp (&r, &localaddr, sizeof (r))))
	    {
	      t->TracePacket (0, this, "Recv", i, buf);
	      EIBNetIPPacket *p =
		EIBNetIPPacket::fromPacket (CArray (buf, i));
	      if (p)
		{
		  outqueue.put (*p);
		  delete p;
		  pth_sem_inc (&outsignal, 1);
		}
	    }
	}
      pth_event_isolate (stop);
      if (!inqueue.isempty ())
	{
	  const struct _EIBNetIP_Send s = inqueue.top ();
	  CArray p = s.data.ToPacket ();
	  t->TracePacket (0, this, "Send", p);
	  i =
	    pth_sendto_ev (fd, p.array (), p (), 0,
			   (const struct sockaddr *) &s.addr, sizeof (s.addr),
			   stop);
	  if (i > 0)
	    {
	      pth_sem_dec (&insignal);
	      inqueue.get ();
	    }
	}
    }
  pth_event_free (stop, PTH_FREE_THIS);
  pth_event_free (input, PTH_FREE_THIS);
}

EIBnet_ConnectRequest::EIBnet_ConnectRequest ()
{
  memset (&caddr, 0, sizeof (caddr));
  memset (&daddr, 0, sizeof (daddr));
}

EIBNetIPPacket EIBnet_ConnectRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca,
    da;
  ca = IPtoEIBNetIP (&caddr);
  da = IPtoEIBNetIP (&daddr);
  p.service = CONNECTION_REQUEST;
  p.data.resize (ca () + da () + 1 + CRI ());
  p.data.setpart (ca, 0);
  p.data.setpart (da, ca ());
  p.data[ca () + da ()] = CRI () + 1;
  p.data.setpart (CRI, ca () + da () + 1);
  return p;
}

int
parseEIBnet_ConnectRequest (const EIBNetIPPacket & p,
			    EIBnet_ConnectRequest & r)
{
  if (p.service != CONNECTION_REQUEST)
    return 1;
  if (p.data () < 18)
    return 1;
  if (EIBnettoIP (CArray (p.data.array (), 8), &r.caddr))
    return 1;
  if (EIBnettoIP (CArray (p.data.array () + 8, 8), &r.daddr))
    return 1;
  if (p.data () - 16 != p.data[16])
    return 1;
  r.CRI = CArray (p.data.array () + 17, p.data () - 17);
  return 0;
}

EIBnet_ConnectResponse::EIBnet_ConnectResponse ()
{
  memset (&daddr, 0, sizeof (daddr));
  channel = 0;
  status = 0;
}

EIBNetIPPacket EIBnet_ConnectResponse::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    da = IPtoEIBNetIP (&daddr);
  p.service = CONNECTION_RESPONSE;
  p.data.resize (da () + CRD () + 3);
  p.data[0] = channel;
  p.data[1] = status;
  p.data.setpart (da, 2);
  p.data[da () + 2] = CRD () + 1;
  p.data.setpart (CRD, da () + 3);
  return p;
}

int
parseEIBnet_ConnectResponse (const EIBNetIPPacket & p,
			     EIBnet_ConnectResponse & r)
{
  if (p.service != CONNECTION_RESPONSE)
    return 1;
  if (p.data () < 12)
    return 1;
  if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.daddr))
    return 1;
  if (p.data () - 10 != p.data[10])
    return 1;
  r.channel = p.data[0];
  r.status = p.data[1];
  r.CRD = CArray (p.data.array () + 11, p.data () - 11);
  return 0;
}

EIBnet_ConnectionStateRequest::EIBnet_ConnectionStateRequest ()
{
  memset (&caddr, 0, sizeof (caddr));
  channel = 0;
}

EIBNetIPPacket EIBnet_ConnectionStateRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca = IPtoEIBNetIP (&caddr);
  p.service = CONNECTIONSTATE_REQUEST;
  p.data.resize (ca () + 2);
  p.data[0] = channel;
  p.data[1] = 0;
  p.data.setpart (ca, 2);
  return p;
}

int
parseEIBnet_ConnectionStateRequest (const EIBNetIPPacket & p,
				    EIBnet_ConnectionStateRequest & r)
{
  if (p.service != CONNECTIONSTATE_REQUEST)
    return 1;
  if (p.data () != 10)
    return 1;
  if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr))
    return 1;
  r.channel = p.data[0];
  return 0;
}

EIBnet_ConnectionStateResponse::EIBnet_ConnectionStateResponse ()
{
  channel = 0;
  status = 0;
}

EIBNetIPPacket EIBnet_ConnectionStateResponse::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  p.service = CONNECTIONSTATE_RESPONSE;
  p.data.resize (2);
  p.data[0] = channel;
  p.data[1] = status;
  return p;
}

int
parseEIBnet_ConnectionStateResponse (const EIBNetIPPacket & p,
				     EIBnet_ConnectionStateResponse & r)
{
  if (p.service != CONNECTIONSTATE_RESPONSE)
    return 1;
  if (p.data () != 2)
    return 1;
  r.channel = p.data[0];
  r.status = p.data[1];
  return 0;
}

EIBnet_DisconnectRequest::EIBnet_DisconnectRequest ()
{
  memset (&caddr, 0, sizeof (caddr));
  channel = 0;
}

EIBNetIPPacket EIBnet_DisconnectRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca = IPtoEIBNetIP (&caddr);
  p.service = DISCONNECT_REQUEST;
  p.data.resize (ca () + 2);
  p.data[0] = channel;
  p.data[1] = 0;
  p.data.setpart (ca, 2);
  return p;
}

int
parseEIBnet_DisconnectRequest (const EIBNetIPPacket & p,
			       EIBnet_DisconnectRequest & r)
{
  if (p.service != DISCONNECT_REQUEST)
    return 1;
  if (p.data () != 10)
    return 1;
  if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr))
    return 1;
  r.channel = p.data[0];
  return 0;
}

EIBnet_DisconnectResponse::EIBnet_DisconnectResponse ()
{
  channel = 0;
  status = 0;
}

EIBNetIPPacket EIBnet_DisconnectResponse::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  p.service = DISCONNECT_RESPONSE;
  p.data.resize (2);
  p.data[0] = channel;
  p.data[1] = status;
  return p;
}

int
parseEIBnet_DisconnectResponse (const EIBNetIPPacket & p,
				EIBnet_DisconnectResponse & r)
{
  if (p.service != DISCONNECT_RESPONSE)
    return 1;
  if (p.data () != 2)
    return 1;
  r.channel = p.data[0];
  r.status = p.data[1];
  return 0;
}

EIBnet_TunnelRequest::EIBnet_TunnelRequest ()
{
  channel = 0;
  seqno = 0;
}

EIBNetIPPacket EIBnet_TunnelRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  p.service = TUNNEL_REQUEST;
  p.data.resize (CEMI () + 4);
  p.data[0] = 4;
  p.data[1] = channel;
  p.data[2] = seqno;
  p.data[3] = 0;
  p.data.setpart (CEMI, 4);
  return p;
}

int
parseEIBnet_TunnelRequest (const EIBNetIPPacket & p, EIBnet_TunnelRequest & r)
{
  if (p.service != TUNNEL_REQUEST)
    return 1;
  if (p.data () < 6)
    return 1;
  if (p.data[0] != 4)
    return 1;
  r.channel = p.data[1];
  r.seqno = p.data[2];
  r.CEMI.set (p.data.array () + 4, p.data () - 4);
  return 0;
}

EIBnet_TunnelACK::EIBnet_TunnelACK ()
{
  channel = 0;
  seqno = 0;
  status = 0;
}

EIBNetIPPacket EIBnet_TunnelACK::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  p.service = TUNNEL_RESPONSE;
  p.data.resize (4);
  p.data[0] = 4;
  p.data[1] = channel;
  p.data[2] = seqno;
  p.data[3] = status;
  return p;
}

int
parseEIBnet_TunnelACK (const EIBNetIPPacket & p, EIBnet_TunnelACK & r)
{
  if (p.service != TUNNEL_RESPONSE)
    return 1;
  if (p.data () != 4)
    return 1;
  if (p.data[0] != 4)
    return 1;
  r.channel = p.data[1];
  r.seqno = p.data[2];
  r.status = p.data[3];
  return 0;
}

EIBnet_DescriptionRequest::EIBnet_DescriptionRequest ()
{
  memset (&caddr, 0, sizeof (caddr));
}

EIBNetIPPacket EIBnet_DescriptionRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca = IPtoEIBNetIP (&caddr);
  p.service = DESCRIPTION_REQUEST;
  p.data = ca;
  return p;
}

int
parseEIBnet_DescriptionRequest (const EIBNetIPPacket & p,
				EIBnet_DescriptionRequest & r)
{
  if (p.service != DESCRIPTION_REQUEST)
    return 1;
  if (p.data () != 8)
    return 1;
  if (EIBnettoIP (p.data, &r.caddr))
    return 1;
  return 0;
}


EIBnet_DescriptionResponse::EIBnet_DescriptionResponse ()
{
  KNXmedium = 0;
  devicestatus = 0;
  individual_addr = 0;
  installid = 0;
  memset (&serial, 0, sizeof (serial));
  multicastaddr.s_addr = 0;
  memset (&MAC, 0, sizeof (MAC));
  memset (&name, 0, sizeof (name));
}

EIBNetIPPacket EIBnet_DescriptionResponse::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  p.service = DESCRIPTION_RESPONSE;
  p.data.resize (56 + services () * 2);
  p.data[0] = 54;
  p.data[1] = 1;
  p.data[2] = KNXmedium;
  p.data[3] = devicestatus;
  p.data[4] = (individual_addr >> 8) & 0xff;
  p.data[5] = (individual_addr) & 0xff;
  p.data[6] = (installid >> 8) & 0xff;
  p.data[7] = (installid) & 0xff;
  memcpy (p.data.array () + 18, &serial, 6);
  memcpy (p.data.array () + 14, &multicastaddr, 4);
  memcpy (p.data.array () + 18, &MAC, 6);
  memcpy (p.data.array () + 24, &name, 30);
  p.data[53] = 0;
  p.data[54] = services () * 2 + 2;
  p.data[55] = 2;
  for (int i = 0; i < services (); i++)
    {
      p.data[56 + i * 2] = services[i].family;
      p.data[57 + i * 2] = services[i].version;
    }
  p.data.setpart (optional, 56 + services () * 2);
  return p;
}

int
parseEIBnet_DescriptionResponse (const EIBNetIPPacket & p,
				 EIBnet_DescriptionResponse & r)
{
  if (p.service != DESCRIPTION_RESPONSE)
    return 1;
  if (p.data () < 56)
    return 1;
  if (p.data[0] != 54)
    return 1;
  if (p.data[1] != 1)
    return 1;
  r.KNXmedium = p.data[2];
  r.devicestatus = p.data[3];
  r.individual_addr = (p.data[4] << 8) | p.data[5];
  r.installid = (p.data[6] << 8) | p.data[7];
  memcpy (&r.serial, p.data.array () + 8, 6);
  memcpy (&r.multicastaddr, p.data.array () + 14, 4);
  memcpy (&r.MAC, p.data.array () + 18, 6);
  memcpy (&r.name, p.data.array () + 24, 30);
  r.name[29] = 0;
  if (p.data[55] != 2)
    return 1;
  if (p.data[54] % 2)
    return 1;
  if (p.data[54] + 54 > p.data ())
    return 1;
  r.services.resize ((p.data[54] / 2) - 1);
  for (int i = 0; i < (p.data[54] / 2) - 1; i++)
    {
      r.services[i].family = p.data[56 + 2 * i];
      r.services[i].version = p.data[57 + 2 * i];
    }
  r.optional.set (p.data.array () + p.data[54] + 54,
		  p.data () - p.data[54] - 54);
  return 0;
}

EIBnet_SearchRequest::EIBnet_SearchRequest ()
{
  memset (&caddr, 0, sizeof (caddr));
}

EIBNetIPPacket EIBnet_SearchRequest::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca = IPtoEIBNetIP (&caddr);
  p.service = SEARCH_REQUEST;
  p.data = ca;
  return p;
}

int
parseEIBnet_SearchRequest (const EIBNetIPPacket & p, EIBnet_SearchRequest & r)
{
  if (p.service != SEARCH_REQUEST)
    return 1;
  if (p.data () != 8)
    return 1;
  if (EIBnettoIP (p.data, &r.caddr))
    return 1;
  return 0;
}


EIBnet_SearchResponse::EIBnet_SearchResponse ()
{
  KNXmedium = 0;
  devicestatus = 0;
  individual_addr = 0;
  installid = 0;
  memset (&serial, 0, sizeof (serial));
  multicastaddr.s_addr = 0;
  memset (&MAC, 0, sizeof (MAC));
  memset (&name, 0, sizeof (name));
}

EIBNetIPPacket EIBnet_SearchResponse::ToPacket ()CONST
{
  EIBNetIPPacket
    p;
  CArray
    ca = IPtoEIBNetIP (&caddr);
  p.service = SEARCH_RESPONSE;
  p.data.resize (64 + services () * 2);
  p.data.setpart (ca, 0);
  p.data[8] = 54;
  p.data[9] = 1;
  p.data[10] = KNXmedium;
  p.data[11] = devicestatus;
  p.data[12] = (individual_addr >> 8) & 0xff;
  p.data[13] = (individual_addr) & 0xff;
  p.data[14] = (installid >> 8) & 0xff;
  p.data[15] = (installid) & 0xff;
  memcpy (p.data.array () + 16, &serial, 6);
  memcpy (p.data.array () + 22, &multicastaddr, 4);
  memcpy (p.data.array () + 26, &MAC, 6);
  memcpy (p.data.array () + 32, &name, 30);
  p.data[61] = 0;
  p.data[62] = services () * 2 + 2;
  p.data[63] = 2;
  for (int i = 0; i < services (); i++)
    {
      p.data[64 + i * 2] = services[i].family;
      p.data[65 + i * 2] = services[i].version;
    }
  return p;
}

int
parseEIBnet_SearchResponse (const EIBNetIPPacket & p,
			    EIBnet_SearchResponse & r)
{
  if (p.service != SEARCH_RESPONSE)
    return 1;
  if (p.data () < 64)
    return 1;
  if (EIBnettoIP (CArray (p.data.array () + 0, 8), &r.caddr))
    return 1;
  if (p.data[8] != 54)
    return 1;
  if (p.data[9] != 1)
    return 1;
  r.KNXmedium = p.data[10];
  r.devicestatus = p.data[11];
  r.individual_addr = (p.data[12] << 8) | p.data[13];
  r.installid = (p.data[14] << 8) | p.data[15];
  memcpy (&r.serial, p.data.array () + 16, 6);
  memcpy (&r.multicastaddr, p.data.array () + 22, 4);
  memcpy (&r.MAC, p.data.array () + 26, 6);
  memcpy (&r.name, p.data.array () + 32, 30);
  r.name[29] = 0;
  if (p.data[63] != 2)
    return 1;
  if (p.data[62] % 2)
    return 1;
  if (p.data[62] + 62 > p.data ())
    return 1;
  r.services.resize ((p.data[62] / 2) - 1);
  for (int i = 0; i < (p.data[62] / 2) - 1; i++)
    {
      r.services[i].family = p.data[64 + 2 * i];
      r.services[i].version = p.data[65 + 2 * i];
    }
  return 0;
}