eibd/libserver/eibnetip.cpp

   1 /*
   2     EIBD eib bus access and management daemon
   3     Copyright (C) 2005-2009 Martin Koegler <mkoegler@auto.tuwien.ac.at>
   4
   5     This program is free software; you can redistribute it and/or modify
   6     it under the terms of the GNU General Public License as published by
   7     the Free Software Foundation; either version 2 of the License, or
   8     (at your option) any later version.
   9
  10     This program is distributed in the hope that it will be useful,
  11     but WITHOUT ANY WARRANTY; without even the implied warranty of
  12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13     GNU General Public License for more details.
  14
  15     You should have received a copy of the GNU General Public License
  16     along with this program; if not, write to the Free Software
  17     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18 */
  19
  20 #include <string.h>
  21 #include <netdb.h>
  22 #include <sys/socket.h>
  23 #include <unistd.h>
  24 #include "eibnetip.h"
  25 #include "config.h"
  26 #ifdef HAVE_LINUX_NETLINK
  27 #include <asm/types.h>
  28 #include <linux/netlink.h>
  29 #include <linux/rtnetlink.h>
  30 #endif
  31 #ifdef HAVE_WINDOWS_IPHELPER
  32 #define Array XArray
  33 #include <windows.h>
  34 #include <iphlpapi.h>
  35 #undef Array
  36 #endif
  37 #if HAVE_BSD_SOURCEINFO
  38 #include <net/if.h>
  39 #include <net/route.h>
  40 #endif
  41
  42 int
  43 GetHostIP (struct sockaddr_in *sock, const char *Name)
  44 {
  45   struct hostent *h;
  46   if (!Name)
  47     return 0;
  48   memset (sock, 0, sizeof (*sock));
  49   h = gethostbyname (Name);
  50   if (!h)
  51     return 0;
  52 #ifdef HAVE_SOCKADDR_IN_LEN
  53   sock->sin_len = sizeof (*sock);
  54 #endif
  55   sock->sin_family = h->h_addrtype;
  56   sock->sin_addr.s_addr = (*((unsigned long *) h->h_addr_list[0]));
  57   return 1;
  58 }
  59
  60 #ifdef HAVE_LINUX_NETLINK
  61 typedef struct
  62 {
  63   struct nlmsghdr n;
  64   struct rtmsg r;
  65   char data[1000];
  66 } r_req;
  67
  68 int
  69 GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)
  70 {
  71   int s;
  72   int l;
  73   r_req req;
  74   struct rtattr *a;
  75   memset (&req, 0, sizeof (req));
  76   memset (src, 0, sizeof (*src));
  77   s = socket (PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
  78   if (s == -1)
  79     return 0;
  80   req.n.nlmsg_len =
  81     NLMSG_SPACE (sizeof (req.r)) + RTA_LENGTH (sizeof (*dest));
  82   req.n.nlmsg_flags = NLM_F_REQUEST;
  83   req.n.nlmsg_type = RTM_GETROUTE;
  84   req.r.rtm_family = AF_INET;
  85   req.r.rtm_dst_len = 32;
  86   a = (rtattr *) ((char *) &req + NLMSG_SPACE (sizeof (req.r)));
  87   a->rta_type = RTA_DST;
  88   a->rta_len = RTA_LENGTH (sizeof (dest->sin_addr.s_addr));
  89   memcpy (RTA_DATA (a), &dest->sin_addr.s_addr,
  90           sizeof (dest->sin_addr.s_addr));
  91   if (write (s, &req, req.n.nlmsg_len) < 0)
  92     return 0;
  93   if (read (s, &req, sizeof (req)) < 0)
  94     return 0;
  95   close (s);
  96   if (req.n.nlmsg_type == NLMSG_ERROR)
  97     return 0;
  98   l = ((struct nlmsghdr *) &req)->nlmsg_len;
  99   while (RTA_OK (a, l))
 100     {
 101       if (a->rta_type == RTA_PREFSRC
 102           && RTA_PAYLOAD (a) == sizeof (src->sin_addr.s_addr))
 103         {
 104           src->sin_family = AF_INET;
 105           memcpy (&src->sin_addr.s_addr, RTA_DATA (a), RTA_PAYLOAD (a));
 106           return 1;
 107         }
 108       a = RTA_NEXT (a, l);
 109     }
 110   return 0;
 111 }
 112 #endif
 113
 114 #ifdef HAVE_WINDOWS_IPHELPER
 115 int
 116 GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)
 117 {
 118   DWORD d = 0;
 119   PMIB_IPADDRTABLE tab;
 120   DWORD s = 0;
 121
 122   memset (src, 0, sizeof (*src));
 123   if (GetBestInterface (dest->sin_addr.s_addr, &d) != NO_ERROR)
 124     return 0;
 125
 126   tab = (MIB_IPADDRTABLE *) malloc (sizeof (MIB_IPADDRTABLE));
 127   if (!tab)
 128     return 0;
 129   if (GetIpAddrTable (tab, &s, 0) == ERROR_INSUFFICIENT_BUFFER)
 130     {
 131       tab = (MIB_IPADDRTABLE *) realloc (tab, s);
 132       if (!tab)
 133         return 0;
 134     }
 135   if (GetIpAddrTable (tab, &s, 0) != NO_ERROR)
 136     {
 137       if (tab)
 138         free (tab);
 139       return 0;
 140     }
 141   for (int i = 0; i < tab->dwNumEntries; i++)
 142     if (tab->table[i].dwIndex == d)
 143       {
 144         src->sin_family = AF_INET;
 145         src->sin_addr.s_addr = tab->table[i].dwAddr;
 146         free (tab);
 147         return 1;
 148       }
 149   free (tab);
 150   return 0;
 151 }
 152 #endif
 153
 154 #if HAVE_BSD_SOURCEINFO
 155 typedef struct
 156 {
 157   struct rt_msghdr hdr;
 158   char data[1000];
 159 } r_req;
 160
 161 #ifndef HAVE_SA_SIZE
 162 static int
 163 SA_SIZE (struct sockaddr *sa)
 164 {
 165   int align = sizeof (long);
 166   int len = (sa->sa_len ? sa->sa_len : align);
 167   if (len & (align - 1))
 168     {
 169       len += align - (len & (align - 1));
 170     }
 171   return len;
 172 }
 173 #endif
 174
 175 int
 176 GetSourceAddress (const struct sockaddr_in *dest, struct sockaddr_in *src)
 177 {
 178   int s;
 179   r_req req;
 180   char *cp = req.data;
 181   memset (&req, 0, sizeof (req));
 182   memset (src, 0, sizeof (*src));
 183   s = socket (PF_ROUTE, SOCK_RAW, 0);
 184   if (s == -1)
 185     return 0;
 186   req.hdr.rtm_msglen = sizeof (req) + sizeof (*dest);
 187   req.hdr.rtm_version = RTM_VERSION;
 188   req.hdr.rtm_flags = RTF_UP;
 189   req.hdr.rtm_type = RTM_GET;
 190   req.hdr.rtm_addrs = RTA_DST | RTA_IFP;
 191   memcpy (cp, dest, sizeof (*dest));
 192   if (write (s, (char *) &req, req.hdr.rtm_msglen) < 0)
 193     return 0;
 194   if (read (s, (char *) &req, sizeof (req)) < 0)
 195     return 0;
 196   close (s);
 197   int i;
 198   cp = (char *) (&req.hdr + 1);
 199   for (i = 1; i; i <<= 1)
 200     if (i & req.hdr.rtm_addrs)
 201       {
 202         struct sockaddr *sa = (struct sockaddr *) cp;
 203         if (i == RTA_IFA)
 204           {
 205             src->sin_len = sizeof (*src);
 206             src->sin_family = AF_INET;
 207             src->sin_addr.s_addr =
 208               ((struct sockaddr_in *) sa)->sin_addr.s_addr;
 209             return 1;
 210           }
 211         cp += SA_SIZE (sa);
 212       }
 213   return 0;
 214 }
 215 #endif
 216
 217 EIBNetIPPacket::EIBNetIPPacket ()
 218 {
 219   service = 0;
 220   memset (&src, 0, sizeof (src));
 221 }
 222
 223 EIBNetIPPacket *
 224 EIBNetIPPacket::fromPacket (const CArray & c, const struct sockaddr_in src)
 225 {
 226   EIBNetIPPacket *p;
 227   unsigned len;
 228   if (c () < 6)
 229     return 0;
 230   if (c[0] != 0x6 || c[1] != 0x10)
 231     return 0;
 232   len = (c[4] << 8) | c[5];
 233   if (len != c ())
 234     return 0;
 235   p = new EIBNetIPPacket;
 236   p->service = (c[2] << 8) | c[3];
 237   p->data.set (c.array () + 6, len - 6);
 238   p->src = src;
 239   return p;
 240 }
 241
 242 CArray
 243 EIBNetIPPacket::ToPacket ()
 244   CONST
 245 {
 246   CArray c;
 247   c.resize (6 + data ());
 248   c[0] = 0x06;
 249   c[1] = 0x10;
 250   c[2] = (service >> 8) & 0xff;
 251   c[3] = (service) & 0xff;
 252   c[4] = ((data () + 6) >> 8) & 0xff;
 253   c[5] = ((data () + 6)) & 0xff;
 254   c.setpart (data, 6);
 255   return c;
 256 }
 257
 258 CArray
 259 IPtoEIBNetIP (const struct sockaddr_in * a)
 260 {
 261   CArray buf;
 262   buf.resize (8);
 263   buf[0] = 0x08;
 264   buf[1] = 0x01;
 265   buf[2] = (ntohl (a->sin_addr.s_addr) >> 24) & 0xff;
 266   buf[3] = (ntohl (a->sin_addr.s_addr) >> 16) & 0xff;
 267   buf[4] = (ntohl (a->sin_addr.s_addr) >> 8) & 0xff;
 268   buf[5] = (ntohl (a->sin_addr.s_addr) >> 0) & 0xff;
 269   buf[6] = (ntohs (a->sin_port) >> 8) & 0xff;
 270   buf[7] = (ntohs (a->sin_port) >> 0) & 0xff;
 271   return buf;
 272 }
 273
 274 int
 275 EIBnettoIP (const CArray & buf, struct sockaddr_in *a)
 276 {
 277   int ip, port;
 278   memset (a, 0, sizeof (*a));
 279   if (buf[0] != 0x8 || buf[1] != 0x1)
 280     return 1;
 281   ip = (buf[2] << 24) | (buf[3] << 16) | (buf[4] << 8) | (buf[5]);
 282   port = (buf[6] << 8) | (buf[7]);
 283 #ifdef HAVE_SOCKADDR_IN_LEN
 284   a->sin_len = sizeof (*a);
 285 #endif
 286   a->sin_family = AF_INET;
 287   a->sin_port = htons (port);
 288   a->sin_addr.s_addr = htonl (ip);
 289   return 0;
 290 }
 291
 292 EIBNetIPSocket::EIBNetIPSocket (struct sockaddr_in bindaddr, bool reuseaddr,
 293                                 Trace * tr)
 294 {
 295   int i;
 296   t = tr;
 297   TRACEPRINTF (t, 0, this, "Open");
 298   multicast = 0;
 299   pth_sem_init (&insignal);
 300   pth_sem_init (&outsignal);
 301   getwait = pth_event (PTH_EVENT_SEM, &outsignal);
 302   memset (&maddr, 0, sizeof (maddr));
 303   memset (&sendaddr, 0, sizeof (sendaddr));
 304   memset (&recvaddr, 0, sizeof (recvaddr));
 305   recvall = 0;
 306
 307   fd = socket (AF_INET, SOCK_DGRAM, 0);
 308   if (fd == -1)
 309     return;
 310
 311   if (reuseaddr)
 312     {
 313       i = 1;
 314       if (setsockopt (fd, SOL_SOCKET, SO_REUSEADDR, &i, sizeof (i)) == -1)
 315         {
 316           close (fd);
 317           fd = -1;
 318           return;
 319         }
 320     }
 321   if (bind (fd, (struct sockaddr *) &bindaddr, sizeof (bindaddr)) == -1)
 322     {
 323       close (fd);
 324       fd = -1;
 325       return;
 326     }
 327
 328   Start ();
 329   TRACEPRINTF (t, 0, this, "Openend");
 330 }
 331
 332 EIBNetIPSocket::~EIBNetIPSocket ()
 333 {
 334   TRACEPRINTF (t, 0, this, "Close");
 335   Stop ();
 336   pth_event_free (getwait, PTH_FREE_THIS);
 337   if (fd != -1)
 338     {
 339       if (multicast)
 340         setsockopt (fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &maddr,
 341                     sizeof (maddr));
 342       close (fd);
 343     }
 344 }
 345
 346 bool
 347 EIBNetIPSocket::init ()
 348 {
 349   return fd != -1;
 350 }
 351
 352 bool
 353 EIBNetIPSocket::SetMulticast (struct ip_mreq multicastaddr)
 354 {
 355   if (multicast)
 356     return false;
 357   maddr = multicastaddr;
 358   if (setsockopt (fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &maddr, sizeof (maddr))
 359       == -1)
 360     return false;
 361   multicast = 1;
 362   return true;
 363 }
 364
 365 void
 366 EIBNetIPSocket::Send (EIBNetIPPacket p)
 367 {
 368   struct _EIBNetIP_Send s;
 369   t->TracePacket (1, this, "Send", p.data);
 370   s.data = p;
 371   s.addr = sendaddr;
 372   inqueue.put (s);
 373   pth_sem_inc (&insignal, 1);
 374 }
 375
 376 EIBNetIPPacket *
 377 EIBNetIPSocket::Get (pth_event_t stop)
 378 {
 379   if (stop != NULL)
 380     pth_event_concat (getwait, stop, NULL);
 381
 382   pth_wait (getwait);
 383
 384   if (stop)
 385     pth_event_isolate (getwait);
 386
 387   if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
 388     {
 389       pth_sem_dec (&outsignal);
 390       t->TracePacket (1, this, "Recv", outqueue.top ().data);
 391       return new EIBNetIPPacket (outqueue.get ());
 392     }
 393   else
 394     return 0;
 395 }
 396
 397 void
 398 EIBNetIPSocket::Run (pth_sem_t * stop1)
 399 {
 400   int i;
 401   int error = 0;
 402   uchar buf[255];
 403   socklen_t rl;
 404   sockaddr_in r;
 405   pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
 406   pth_event_t input = pth_event (PTH_EVENT_SEM, &insignal);
 407   while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
 408     {
 409       pth_event_concat (stop, input, NULL);
 410       rl = sizeof (r);
 411       memset (&r, 0, sizeof (r));
 412       i =
 413         pth_recvfrom_ev (fd, buf, sizeof (buf), 0, (struct sockaddr *) &r,
 414                          &rl, stop);
 415       if (i > 0 && rl == sizeof (r))
 416         {
 417           if (recvall == 1 || !memcmp (&r, &recvaddr, sizeof (r)) ||
 418               (recvall == 2 && memcmp (&r, &localaddr, sizeof (r))))
 419             {
 420               t->TracePacket (0, this, "Recv", i, buf);
 421               EIBNetIPPacket *p =
 422                 EIBNetIPPacket::fromPacket (CArray (buf, i), r);
 423               if (p)
 424                 {
 425                   outqueue.put (*p);
 426                   delete p;
 427                   pth_sem_inc (&outsignal, 1);
 428                 }
 429             }
 430         }
 431       pth_event_isolate (stop);
 432       if (!inqueue.isempty ())
 433         {
 434           const struct _EIBNetIP_Send s = inqueue.top ();
 435           CArray p = s.data.ToPacket ();
 436           t->TracePacket (0, this, "Send", p);
 437           i =
 438             pth_sendto_ev (fd, p.array (), p (), 0,
 439                            (const struct sockaddr *) &s.addr, sizeof (s.addr),
 440                            stop);
 441           if (i > 0)
 442             {
 443               pth_sem_dec (&insignal);
 444               inqueue.get ();
 445               error = 0;
 446             }
 447           else
 448             error++;
 449           if (error > 5)
 450             {
 451               t->TracePacket (0, this, "Drop EIBnetSocket", p);
 452               pth_sem_dec (&insignal);
 453               inqueue.get ();
 454               error = 0;
 455             }
 456         }
 457     }
 458   pth_event_free (stop, PTH_FREE_THIS);
 459   pth_event_free (input, PTH_FREE_THIS);
 460 }
 461
 462 EIBnet_ConnectRequest::EIBnet_ConnectRequest ()
 463 {
 464   memset (&caddr, 0, sizeof (caddr));
 465   memset (&daddr, 0, sizeof (daddr));
 466 }
 467
 468 EIBNetIPPacket EIBnet_ConnectRequest::ToPacket ()CONST
 469 {
 470   EIBNetIPPacket
 471     p;
 472   CArray
 473     ca,
 474     da;
 475   ca = IPtoEIBNetIP (&caddr);
 476   da = IPtoEIBNetIP (&daddr);
 477   p.service = CONNECTION_REQUEST;
 478   p.data.resize (ca () + da () + 1 + CRI ());
 479   p.data.setpart (ca, 0);
 480   p.data.setpart (da, ca ());
 481   p.data[ca () + da ()] = CRI () + 1;
 482   p.data.setpart (CRI, ca () + da () + 1);
 483   return p;
 484 }
 485
 486 int
 487 parseEIBnet_ConnectRequest (const EIBNetIPPacket & p,
 488                             EIBnet_ConnectRequest & r)
 489 {
 490   if (p.service != CONNECTION_REQUEST)
 491     return 1;
 492   if (p.data () < 18)
 493     return 1;
 494   if (EIBnettoIP (CArray (p.data.array (), 8), &r.caddr))
 495     return 1;
 496   if (EIBnettoIP (CArray (p.data.array () + 8, 8), &r.daddr))
 497     return 1;
 498   if (p.data () - 16 != p.data[16])
 499     return 1;
 500   r.CRI = CArray (p.data.array () + 17, p.data () - 17);
 501   return 0;
 502 }
 503
 504 EIBnet_ConnectResponse::EIBnet_ConnectResponse ()
 505 {
 506   memset (&daddr, 0, sizeof (daddr));
 507   channel = 0;
 508   status = 0;
 509 }
 510
 511 EIBNetIPPacket EIBnet_ConnectResponse::ToPacket ()CONST
 512 {
 513   EIBNetIPPacket
 514     p;
 515   CArray
 516     da = IPtoEIBNetIP (&daddr);
 517   p.service = CONNECTION_RESPONSE;
 518   p.data.resize (da () + CRD () + 3);
 519   p.data[0] = channel;
 520   p.data[1] = status;
 521   p.data.setpart (da, 2);
 522   p.data[da () + 2] = CRD () + 1;
 523   p.data.setpart (CRD, da () + 3);
 524   return p;
 525 }
 526
 527 int
 528 parseEIBnet_ConnectResponse (const EIBNetIPPacket & p,
 529                              EIBnet_ConnectResponse & r)
 530 {
 531   if (p.service != CONNECTION_RESPONSE)
 532     return 1;
 533   if (p.data () < 12)
 534     return 1;
 535   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.daddr))
 536     return 1;
 537   if (p.data () - 10 != p.data[10])
 538     return 1;
 539   r.channel = p.data[0];
 540   r.status = p.data[1];
 541   r.CRD = CArray (p.data.array () + 11, p.data () - 11);
 542   return 0;
 543 }
 544
 545 EIBnet_ConnectionStateRequest::EIBnet_ConnectionStateRequest ()
 546 {
 547   memset (&caddr, 0, sizeof (caddr));
 548   channel = 0;
 549 }
 550
 551 EIBNetIPPacket EIBnet_ConnectionStateRequest::ToPacket ()CONST
 552 {
 553   EIBNetIPPacket
 554     p;
 555   CArray
 556     ca = IPtoEIBNetIP (&caddr);
 557   p.service = CONNECTIONSTATE_REQUEST;
 558   p.data.resize (ca () + 2);
 559   p.data[0] = channel;
 560   p.data[1] = 0;
 561   p.data.setpart (ca, 2);
 562   return p;
 563 }
 564
 565 int
 566 parseEIBnet_ConnectionStateRequest (const EIBNetIPPacket & p,
 567                                     EIBnet_ConnectionStateRequest & r)
 568 {
 569   if (p.service != CONNECTIONSTATE_REQUEST)
 570     return 1;
 571   if (p.data () != 10)
 572     return 1;
 573   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr))
 574     return 1;
 575   r.channel = p.data[0];
 576   return 0;
 577 }
 578
 579 EIBnet_ConnectionStateResponse::EIBnet_ConnectionStateResponse ()
 580 {
 581   channel = 0;
 582   status = 0;
 583 }
 584
 585 EIBNetIPPacket EIBnet_ConnectionStateResponse::ToPacket ()CONST
 586 {
 587   EIBNetIPPacket
 588     p;
 589   p.service = CONNECTIONSTATE_RESPONSE;
 590   p.data.resize (2);
 591   p.data[0] = channel;
 592   p.data[1] = status;
 593   return p;
 594 }
 595
 596 int
 597 parseEIBnet_ConnectionStateResponse (const EIBNetIPPacket & p,
 598                                      EIBnet_ConnectionStateResponse & r)
 599 {
 600   if (p.service != CONNECTIONSTATE_RESPONSE)
 601     return 1;
 602   if (p.data () != 2)
 603     return 1;
 604   r.channel = p.data[0];
 605   r.status = p.data[1];
 606   return 0;
 607 }
 608
 609 EIBnet_DisconnectRequest::EIBnet_DisconnectRequest ()
 610 {
 611   memset (&caddr, 0, sizeof (caddr));
 612   channel = 0;
 613 }
 614
 615 EIBNetIPPacket EIBnet_DisconnectRequest::ToPacket ()CONST
 616 {
 617   EIBNetIPPacket
 618     p;
 619   CArray
 620     ca = IPtoEIBNetIP (&caddr);
 621   p.service = DISCONNECT_REQUEST;
 622   p.data.resize (ca () + 2);
 623   p.data[0] = channel;
 624   p.data[1] = 0;
 625   p.data.setpart (ca, 2);
 626   return p;
 627 }
 628
 629 int
 630 parseEIBnet_DisconnectRequest (const EIBNetIPPacket & p,
 631                                EIBnet_DisconnectRequest & r)
 632 {
 633   if (p.service != DISCONNECT_REQUEST)
 634     return 1;
 635   if (p.data () != 10)
 636     return 1;
 637   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr))
 638     return 1;
 639   r.channel = p.data[0];
 640   return 0;
 641 }
 642
 643 EIBnet_DisconnectResponse::EIBnet_DisconnectResponse ()
 644 {
 645   channel = 0;
 646   status = 0;
 647 }
 648
 649 EIBNetIPPacket EIBnet_DisconnectResponse::ToPacket ()CONST
 650 {
 651   EIBNetIPPacket
 652     p;
 653   p.service = DISCONNECT_RESPONSE;
 654   p.data.resize (2);
 655   p.data[0] = channel;
 656   p.data[1] = status;
 657   return p;
 658 }
 659
 660 int
 661 parseEIBnet_DisconnectResponse (const EIBNetIPPacket & p,
 662                                 EIBnet_DisconnectResponse & r)
 663 {
 664   if (p.service != DISCONNECT_RESPONSE)
 665     return 1;
 666   if (p.data () != 2)
 667     return 1;
 668   r.channel = p.data[0];
 669   r.status = p.data[1];
 670   return 0;
 671 }
 672
 673 EIBnet_TunnelRequest::EIBnet_TunnelRequest ()
 674 {
 675   channel = 0;
 676   seqno = 0;
 677 }
 678
 679 EIBNetIPPacket EIBnet_TunnelRequest::ToPacket ()CONST
 680 {
 681   EIBNetIPPacket
 682     p;
 683   p.service = TUNNEL_REQUEST;
 684   p.data.resize (CEMI () + 4);
 685   p.data[0] = 4;
 686   p.data[1] = channel;
 687   p.data[2] = seqno;
 688   p.data[3] = 0;
 689   p.data.setpart (CEMI, 4);
 690   return p;
 691 }
 692
 693 int
 694 parseEIBnet_TunnelRequest (const EIBNetIPPacket & p, EIBnet_TunnelRequest & r)
 695 {
 696   if (p.service != TUNNEL_REQUEST)
 697     return 1;
 698   if (p.data () < 6)
 699     return 1;
 700   if (p.data[0] != 4)
 701     return 1;
 702   r.channel = p.data[1];
 703   r.seqno = p.data[2];
 704   r.CEMI.set (p.data.array () + 4, p.data () - 4);
 705   return 0;
 706 }
 707
 708 EIBnet_TunnelACK::EIBnet_TunnelACK ()
 709 {
 710   channel = 0;
 711   seqno = 0;
 712   status = 0;
 713 }
 714
 715 EIBNetIPPacket EIBnet_TunnelACK::ToPacket ()CONST
 716 {
 717   EIBNetIPPacket
 718     p;
 719   p.service = TUNNEL_RESPONSE;
 720   p.data.resize (4);
 721   p.data[0] = 4;
 722   p.data[1] = channel;
 723   p.data[2] = seqno;
 724   p.data[3] = status;
 725   return p;
 726 }
 727
 728 int
 729 parseEIBnet_TunnelACK (const EIBNetIPPacket & p, EIBnet_TunnelACK & r)
 730 {
 731   if (p.service != TUNNEL_RESPONSE)
 732     return 1;
 733   if (p.data () != 4)
 734     return 1;
 735   if (p.data[0] != 4)
 736     return 1;
 737   r.channel = p.data[1];
 738   r.seqno = p.data[2];
 739   r.status = p.data[3];
 740   return 0;
 741 }
 742
 743 EIBnet_DescriptionRequest::EIBnet_DescriptionRequest ()
 744 {
 745   memset (&caddr, 0, sizeof (caddr));
 746 }
 747
 748 EIBNetIPPacket EIBnet_DescriptionRequest::ToPacket ()CONST
 749 {
 750   EIBNetIPPacket
 751     p;
 752   CArray
 753     ca = IPtoEIBNetIP (&caddr);
 754   p.service = DESCRIPTION_REQUEST;
 755   p.data = ca;
 756   return p;
 757 }
 758
 759 int
 760 parseEIBnet_DescriptionRequest (const EIBNetIPPacket & p,
 761                                 EIBnet_DescriptionRequest & r)
 762 {
 763   if (p.service != DESCRIPTION_REQUEST)
 764     return 1;
 765   if (p.data () != 8)
 766     return 1;
 767   if (EIBnettoIP (p.data, &r.caddr))
 768     return 1;
 769   return 0;
 770 }
 771
 772
 773 EIBnet_DescriptionResponse::EIBnet_DescriptionResponse ()
 774 {
 775   KNXmedium = 0;
 776   devicestatus = 0;
 777   individual_addr = 0;
 778   installid = 0;
 779   memset (&serial, 0, sizeof (serial));
 780   multicastaddr.s_addr = 0;
 781   memset (&MAC, 0, sizeof (MAC));
 782   memset (&name, 0, sizeof (name));
 783 }
 784
 785 EIBNetIPPacket EIBnet_DescriptionResponse::ToPacket ()CONST
 786 {
 787   EIBNetIPPacket
 788     p;
 789   p.service = DESCRIPTION_RESPONSE;
 790   p.data.resize (56 + services () * 2);
 791   p.data[0] = 54;
 792   p.data[1] = 1;
 793   p.data[2] = KNXmedium;
 794   p.data[3] = devicestatus;
 795   p.data[4] = (individual_addr >> 8) & 0xff;
 796   p.data[5] = (individual_addr) & 0xff;
 797   p.data[6] = (installid >> 8) & 0xff;
 798   p.data[7] = (installid) & 0xff;
 799   memcpy (p.data.array () + 18, &serial, 6);
 800   memcpy (p.data.array () + 14, &multicastaddr, 4);
 801   memcpy (p.data.array () + 18, &MAC, 6);
 802   memcpy (p.data.array () + 24, &name, 30);
 803   p.data[53] = 0;
 804   p.data[54] = services () * 2 + 2;
 805   p.data[55] = 2;
 806   for (int i = 0; i < services (); i++)
 807     {
 808       p.data[56 + i * 2] = services[i].family;
 809       p.data[57 + i * 2] = services[i].version;
 810     }
 811   p.data.setpart (optional, 56 + services () * 2);
 812   return p;
 813 }
 814
 815 int
 816 parseEIBnet_DescriptionResponse (const EIBNetIPPacket & p,
 817                                  EIBnet_DescriptionResponse & r)
 818 {
 819   if (p.service != DESCRIPTION_RESPONSE)
 820     return 1;
 821   if (p.data () < 56)
 822     return 1;
 823   if (p.data[0] != 54)
 824     return 1;
 825   if (p.data[1] != 1)
 826     return 1;
 827   r.KNXmedium = p.data[2];
 828   r.devicestatus = p.data[3];
 829   r.individual_addr = (p.data[4] << 8) | p.data[5];
 830   r.installid = (p.data[6] << 8) | p.data[7];
 831   memcpy (&r.serial, p.data.array () + 8, 6);
 832   memcpy (&r.multicastaddr, p.data.array () + 14, 4);
 833   memcpy (&r.MAC, p.data.array () + 18, 6);
 834   memcpy (&r.name, p.data.array () + 24, 30);
 835   r.name[29] = 0;
 836   if (p.data[55] != 2)
 837     return 1;
 838   if (p.data[54] % 2)
 839     return 1;
 840   if (p.data[54] + 54 > p.data ())
 841     return 1;
 842   r.services.resize ((p.data[54] / 2) - 1);
 843   for (int i = 0; i < (p.data[54] / 2) - 1; i++)
 844     {
 845       r.services[i].family = p.data[56 + 2 * i];
 846       r.services[i].version = p.data[57 + 2 * i];
 847     }
 848   r.optional.set (p.data.array () + p.data[54] + 54,
 849                   p.data () - p.data[54] - 54);
 850   return 0;
 851 }
 852
 853 EIBnet_SearchRequest::EIBnet_SearchRequest ()
 854 {
 855   memset (&caddr, 0, sizeof (caddr));
 856 }
 857
 858 EIBNetIPPacket EIBnet_SearchRequest::ToPacket ()CONST
 859 {
 860   EIBNetIPPacket
 861     p;
 862   CArray
 863     ca = IPtoEIBNetIP (&caddr);
 864   p.service = SEARCH_REQUEST;
 865   p.data = ca;
 866   return p;
 867 }
 868
 869 int
 870 parseEIBnet_SearchRequest (const EIBNetIPPacket & p, EIBnet_SearchRequest & r)
 871 {
 872   if (p.service != SEARCH_REQUEST)
 873     return 1;
 874   if (p.data () != 8)
 875     return 1;
 876   if (EIBnettoIP (p.data, &r.caddr))
 877     return 1;
 878   return 0;
 879 }
 880
 881
 882 EIBnet_SearchResponse::EIBnet_SearchResponse ()
 883 {
 884   KNXmedium = 0;
 885   devicestatus = 0;
 886   individual_addr = 0;
 887   installid = 0;
 888   memset (&serial, 0, sizeof (serial));
 889   multicastaddr.s_addr = 0;
 890   memset (&MAC, 0, sizeof (MAC));
 891   memset (&name, 0, sizeof (name));
 892 }
 893
 894 EIBNetIPPacket EIBnet_SearchResponse::ToPacket ()CONST
 895 {
 896   EIBNetIPPacket
 897     p;
 898   CArray
 899     ca = IPtoEIBNetIP (&caddr);
 900   p.service = SEARCH_RESPONSE;
 901   p.data.resize (64 + services () * 2);
 902   p.data.setpart (ca, 0);
 903   p.data[8] = 54;
 904   p.data[9] = 1;
 905   p.data[10] = KNXmedium;
 906   p.data[11] = devicestatus;
 907   p.data[12] = (individual_addr >> 8) & 0xff;
 908   p.data[13] = (individual_addr) & 0xff;
 909   p.data[14] = (installid >> 8) & 0xff;
 910   p.data[15] = (installid) & 0xff;
 911   memcpy (p.data.array () + 16, &serial, 6);
 912   memcpy (p.data.array () + 22, &multicastaddr, 4);
 913   memcpy (p.data.array () + 26, &MAC, 6);
 914   memcpy (p.data.array () + 32, &name, 30);
 915   p.data[61] = 0;
 916   p.data[62] = services () * 2 + 2;
 917   p.data[63] = 2;
 918   for (int i = 0; i < services (); i++)
 919     {
 920       p.data[64 + i * 2] = services[i].family;
 921       p.data[65 + i * 2] = services[i].version;
 922     }
 923   return p;
 924 }
 925
 926 int
 927 parseEIBnet_SearchResponse (const EIBNetIPPacket & p,
 928                             EIBnet_SearchResponse & r)
 929 {
 930   if (p.service != SEARCH_RESPONSE)
 931     return 1;
 932   if (p.data () < 64)
 933     return 1;
 934   if (EIBnettoIP (CArray (p.data.array () + 0, 8), &r.caddr))
 935     return 1;
 936   if (p.data[8] != 54)
 937     return 1;
 938   if (p.data[9] != 1)
 939     return 1;
 940   r.KNXmedium = p.data[10];
 941   r.devicestatus = p.data[11];
 942   r.individual_addr = (p.data[12] << 8) | p.data[13];
 943   r.installid = (p.data[14] << 8) | p.data[15];
 944   memcpy (&r.serial, p.data.array () + 16, 6);
 945   memcpy (&r.multicastaddr, p.data.array () + 22, 4);
 946   memcpy (&r.MAC, p.data.array () + 26, 6);
 947   memcpy (&r.name, p.data.array () + 32, 30);
 948   r.name[29] = 0;
 949   if (p.data[63] != 2)
 950     return 1;
 951   if (p.data[62] % 2)
 952     return 1;
 953   if (p.data[62] + 62 > p.data ())
 954     return 1;
 955   r.services.resize ((p.data[62] / 2) - 1);
 956   for (int i = 0; i < (p.data[62] / 2) - 1; i++)
 957     {
 958       r.services[i].family = p.data[64 + 2 * i];
 959       r.services[i].version = p.data[65 + 2 * i];
 960     }
 961   return 0;
 962 }