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             const struct sockaddr_in *src)
 277 {
 278   int ip, port;
 279   memset (a, 0, sizeof (*a));
 280   if (buf[0] != 0x8 || buf[1] != 0x1)
 281     return 1;
 282   ip = (buf[2] << 24) | (buf[3] << 16) | (buf[4] << 8) | (buf[5]);
 283   port = (buf[6] << 8) | (buf[7]);
 284 #ifdef HAVE_SOCKADDR_IN_LEN
 285   a->sin_len = sizeof (*a);
 286 #endif
 287   a->sin_family = AF_INET;
 288   if (port == 0 && ip == 0)
 289     {
 290       a->sin_port = src->sin_port;
 291       a->sin_addr.s_addr = src->sin_addr.s_addr;
 292     }
 293   else
 294     {
 295       a->sin_port = htons (port);
 296       a->sin_addr.s_addr = htonl (ip);
 297     }
 298   return 0;
 299 }
 300
 301 EIBNetIPSocket::EIBNetIPSocket (struct sockaddr_in bindaddr, bool reuseaddr,
 302                                 Trace * tr)
 303 {
 304   int i;
 305   t = tr;
 306   TRACEPRINTF (t, 0, this, "Open");
 307   multicast = 0;
 308   pth_sem_init (&insignal);
 309   pth_sem_init (&outsignal);
 310   getwait = pth_event (PTH_EVENT_SEM, &outsignal);
 311   memset (&maddr, 0, sizeof (maddr));
 312   memset (&sendaddr, 0, sizeof (sendaddr));
 313   memset (&recvaddr, 0, sizeof (recvaddr));
 314   recvall = 0;
 315
 316   fd = socket (AF_INET, SOCK_DGRAM, 0);
 317   if (fd == -1)
 318     return;
 319
 320   if (reuseaddr)
 321     {
 322       i = 1;
 323       if (setsockopt (fd, SOL_SOCKET, SO_REUSEADDR, &i, sizeof (i)) == -1)
 324         {
 325           close (fd);
 326           fd = -1;
 327           return;
 328         }
 329     }
 330   if (bind (fd, (struct sockaddr *) &bindaddr, sizeof (bindaddr)) == -1)
 331     {
 332       close (fd);
 333       fd = -1;
 334       return;
 335     }
 336
 337   Start ();
 338   TRACEPRINTF (t, 0, this, "Openend");
 339 }
 340
 341 EIBNetIPSocket::~EIBNetIPSocket ()
 342 {
 343   TRACEPRINTF (t, 0, this, "Close");
 344   Stop ();
 345   pth_event_free (getwait, PTH_FREE_THIS);
 346   if (fd != -1)
 347     {
 348       if (multicast)
 349         setsockopt (fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &maddr,
 350                     sizeof (maddr));
 351       close (fd);
 352     }
 353 }
 354
 355 bool
 356 EIBNetIPSocket::init ()
 357 {
 358   return fd != -1;
 359 }
 360
 361 bool
 362 EIBNetIPSocket::SetMulticast (struct ip_mreq multicastaddr)
 363 {
 364   if (multicast)
 365     return false;
 366   maddr = multicastaddr;
 367   if (setsockopt (fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &maddr, sizeof (maddr))
 368       == -1)
 369     return false;
 370   multicast = 1;
 371   return true;
 372 }
 373
 374 void
 375 EIBNetIPSocket::Send (EIBNetIPPacket p)
 376 {
 377   struct _EIBNetIP_Send s;
 378   t->TracePacket (1, this, "Send", p.data);
 379   s.data = p;
 380   s.addr = sendaddr;
 381   inqueue.put (s);
 382   pth_sem_inc (&insignal, 1);
 383 }
 384
 385 EIBNetIPPacket *
 386 EIBNetIPSocket::Get (pth_event_t stop)
 387 {
 388   if (stop != NULL)
 389     pth_event_concat (getwait, stop, NULL);
 390
 391   pth_wait (getwait);
 392
 393   if (stop)
 394     pth_event_isolate (getwait);
 395
 396   if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
 397     {
 398       pth_sem_dec (&outsignal);
 399       t->TracePacket (1, this, "Recv", outqueue.top ().data);
 400       return new EIBNetIPPacket (outqueue.get ());
 401     }
 402   else
 403     return 0;
 404 }
 405
 406 void
 407 EIBNetIPSocket::Run (pth_sem_t * stop1)
 408 {
 409   int i;
 410   int error = 0;
 411   uchar buf[255];
 412   socklen_t rl;
 413   sockaddr_in r;
 414   pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
 415   pth_event_t input = pth_event (PTH_EVENT_SEM, &insignal);
 416   while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
 417     {
 418       pth_event_concat (stop, input, NULL);
 419       rl = sizeof (r);
 420       memset (&r, 0, sizeof (r));
 421       i =
 422         pth_recvfrom_ev (fd, buf, sizeof (buf), 0, (struct sockaddr *) &r,
 423                          &rl, stop);
 424       if (i > 0 && rl == sizeof (r))
 425         {
 426           if (recvall == 1 || !memcmp (&r, &recvaddr, sizeof (r)) ||
 427               (recvall == 2 && memcmp (&r, &localaddr, sizeof (r))))
 428             {
 429               t->TracePacket (0, this, "Recv", i, buf);
 430               EIBNetIPPacket *p =
 431                 EIBNetIPPacket::fromPacket (CArray (buf, i), r);
 432               if (p)
 433                 {
 434                   outqueue.put (*p);
 435                   delete p;
 436                   pth_sem_inc (&outsignal, 1);
 437                 }
 438             }
 439         }
 440       pth_event_isolate (stop);
 441       if (!inqueue.isempty ())
 442         {
 443           const struct _EIBNetIP_Send s = inqueue.top ();
 444           CArray p = s.data.ToPacket ();
 445           t->TracePacket (0, this, "Send", p);
 446           i =
 447             pth_sendto_ev (fd, p.array (), p (), 0,
 448                            (const struct sockaddr *) &s.addr, sizeof (s.addr),
 449                            stop);
 450           if (i > 0)
 451             {
 452               pth_sem_dec (&insignal);
 453               inqueue.get ();
 454               error = 0;
 455             }
 456           else
 457             error++;
 458           if (error > 5)
 459             {
 460               t->TracePacket (0, this, "Drop EIBnetSocket", p);
 461               pth_sem_dec (&insignal);
 462               inqueue.get ();
 463               error = 0;
 464             }
 465         }
 466     }
 467   pth_event_free (stop, PTH_FREE_THIS);
 468   pth_event_free (input, PTH_FREE_THIS);
 469 }
 470
 471 EIBnet_ConnectRequest::EIBnet_ConnectRequest ()
 472 {
 473   memset (&caddr, 0, sizeof (caddr));
 474   memset (&daddr, 0, sizeof (daddr));
 475 }
 476
 477 EIBNetIPPacket EIBnet_ConnectRequest::ToPacket ()CONST
 478 {
 479   EIBNetIPPacket
 480     p;
 481   CArray
 482     ca,
 483     da;
 484   ca = IPtoEIBNetIP (&caddr);
 485   da = IPtoEIBNetIP (&daddr);
 486   p.service = CONNECTION_REQUEST;
 487   p.data.resize (ca () + da () + 1 + CRI ());
 488   p.data.setpart (ca, 0);
 489   p.data.setpart (da, ca ());
 490   p.data[ca () + da ()] = CRI () + 1;
 491   p.data.setpart (CRI, ca () + da () + 1);
 492   return p;
 493 }
 494
 495 int
 496 parseEIBnet_ConnectRequest (const EIBNetIPPacket & p,
 497                             EIBnet_ConnectRequest & r)
 498 {
 499   if (p.service != CONNECTION_REQUEST)
 500     return 1;
 501   if (p.data () < 18)
 502     return 1;
 503   if (EIBnettoIP (CArray (p.data.array (), 8), &r.caddr, &p.src))
 504     return 1;
 505   if (EIBnettoIP (CArray (p.data.array () + 8, 8), &r.daddr, &p.src))
 506     return 1;
 507   if (p.data () - 16 != p.data[16])
 508     return 1;
 509   r.CRI = CArray (p.data.array () + 17, p.data () - 17);
 510   return 0;
 511 }
 512
 513 EIBnet_ConnectResponse::EIBnet_ConnectResponse ()
 514 {
 515   memset (&daddr, 0, sizeof (daddr));
 516   channel = 0;
 517   status = 0;
 518 }
 519
 520 EIBNetIPPacket EIBnet_ConnectResponse::ToPacket ()CONST
 521 {
 522   EIBNetIPPacket
 523     p;
 524   CArray
 525     da = IPtoEIBNetIP (&daddr);
 526   p.service = CONNECTION_RESPONSE;
 527   p.data.resize (da () + CRD () + 3);
 528   p.data[0] = channel;
 529   p.data[1] = status;
 530   p.data.setpart (da, 2);
 531   p.data[da () + 2] = CRD () + 1;
 532   p.data.setpart (CRD, da () + 3);
 533   return p;
 534 }
 535
 536 int
 537 parseEIBnet_ConnectResponse (const EIBNetIPPacket & p,
 538                              EIBnet_ConnectResponse & r)
 539 {
 540   if (p.service != CONNECTION_RESPONSE)
 541     return 1;
 542   if (p.data () < 12)
 543     return 1;
 544   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.daddr, &p.src))
 545     return 1;
 546   if (p.data () - 10 != p.data[10])
 547     return 1;
 548   r.channel = p.data[0];
 549   r.status = p.data[1];
 550   r.CRD = CArray (p.data.array () + 11, p.data () - 11);
 551   return 0;
 552 }
 553
 554 EIBnet_ConnectionStateRequest::EIBnet_ConnectionStateRequest ()
 555 {
 556   memset (&caddr, 0, sizeof (caddr));
 557   channel = 0;
 558 }
 559
 560 EIBNetIPPacket EIBnet_ConnectionStateRequest::ToPacket ()CONST
 561 {
 562   EIBNetIPPacket
 563     p;
 564   CArray
 565     ca = IPtoEIBNetIP (&caddr);
 566   p.service = CONNECTIONSTATE_REQUEST;
 567   p.data.resize (ca () + 2);
 568   p.data[0] = channel;
 569   p.data[1] = 0;
 570   p.data.setpart (ca, 2);
 571   return p;
 572 }
 573
 574 int
 575 parseEIBnet_ConnectionStateRequest (const EIBNetIPPacket & p,
 576                                     EIBnet_ConnectionStateRequest & r)
 577 {
 578   if (p.service != CONNECTIONSTATE_REQUEST)
 579     return 1;
 580   if (p.data () != 10)
 581     return 1;
 582   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr, &p.src))
 583     return 1;
 584   r.channel = p.data[0];
 585   return 0;
 586 }
 587
 588 EIBnet_ConnectionStateResponse::EIBnet_ConnectionStateResponse ()
 589 {
 590   channel = 0;
 591   status = 0;
 592 }
 593
 594 EIBNetIPPacket EIBnet_ConnectionStateResponse::ToPacket ()CONST
 595 {
 596   EIBNetIPPacket
 597     p;
 598   p.service = CONNECTIONSTATE_RESPONSE;
 599   p.data.resize (2);
 600   p.data[0] = channel;
 601   p.data[1] = status;
 602   return p;
 603 }
 604
 605 int
 606 parseEIBnet_ConnectionStateResponse (const EIBNetIPPacket & p,
 607                                      EIBnet_ConnectionStateResponse & r)
 608 {
 609   if (p.service != CONNECTIONSTATE_RESPONSE)
 610     return 1;
 611   if (p.data () != 2)
 612     return 1;
 613   r.channel = p.data[0];
 614   r.status = p.data[1];
 615   return 0;
 616 }
 617
 618 EIBnet_DisconnectRequest::EIBnet_DisconnectRequest ()
 619 {
 620   memset (&caddr, 0, sizeof (caddr));
 621   channel = 0;
 622 }
 623
 624 EIBNetIPPacket EIBnet_DisconnectRequest::ToPacket ()CONST
 625 {
 626   EIBNetIPPacket
 627     p;
 628   CArray
 629     ca = IPtoEIBNetIP (&caddr);
 630   p.service = DISCONNECT_REQUEST;
 631   p.data.resize (ca () + 2);
 632   p.data[0] = channel;
 633   p.data[1] = 0;
 634   p.data.setpart (ca, 2);
 635   return p;
 636 }
 637
 638 int
 639 parseEIBnet_DisconnectRequest (const EIBNetIPPacket & p,
 640                                EIBnet_DisconnectRequest & r)
 641 {
 642   if (p.service != DISCONNECT_REQUEST)
 643     return 1;
 644   if (p.data () != 10)
 645     return 1;
 646   if (EIBnettoIP (CArray (p.data.array () + 2, 8), &r.caddr, &p.src))
 647     return 1;
 648   r.channel = p.data[0];
 649   return 0;
 650 }
 651
 652 EIBnet_DisconnectResponse::EIBnet_DisconnectResponse ()
 653 {
 654   channel = 0;
 655   status = 0;
 656 }
 657
 658 EIBNetIPPacket EIBnet_DisconnectResponse::ToPacket ()CONST
 659 {
 660   EIBNetIPPacket
 661     p;
 662   p.service = DISCONNECT_RESPONSE;
 663   p.data.resize (2);
 664   p.data[0] = channel;
 665   p.data[1] = status;
 666   return p;
 667 }
 668
 669 int
 670 parseEIBnet_DisconnectResponse (const EIBNetIPPacket & p,
 671                                 EIBnet_DisconnectResponse & r)
 672 {
 673   if (p.service != DISCONNECT_RESPONSE)
 674     return 1;
 675   if (p.data () != 2)
 676     return 1;
 677   r.channel = p.data[0];
 678   r.status = p.data[1];
 679   return 0;
 680 }
 681
 682 EIBnet_TunnelRequest::EIBnet_TunnelRequest ()
 683 {
 684   channel = 0;
 685   seqno = 0;
 686 }
 687
 688 EIBNetIPPacket EIBnet_TunnelRequest::ToPacket ()CONST
 689 {
 690   EIBNetIPPacket
 691     p;
 692   p.service = TUNNEL_REQUEST;
 693   p.data.resize (CEMI () + 4);
 694   p.data[0] = 4;
 695   p.data[1] = channel;
 696   p.data[2] = seqno;
 697   p.data[3] = 0;
 698   p.data.setpart (CEMI, 4);
 699   return p;
 700 }
 701
 702 int
 703 parseEIBnet_TunnelRequest (const EIBNetIPPacket & p, EIBnet_TunnelRequest & r)
 704 {
 705   if (p.service != TUNNEL_REQUEST)
 706     return 1;
 707   if (p.data () < 6)
 708     return 1;
 709   if (p.data[0] != 4)
 710     return 1;
 711   r.channel = p.data[1];
 712   r.seqno = p.data[2];
 713   r.CEMI.set (p.data.array () + 4, p.data () - 4);
 714   return 0;
 715 }
 716
 717 EIBnet_TunnelACK::EIBnet_TunnelACK ()
 718 {
 719   channel = 0;
 720   seqno = 0;
 721   status = 0;
 722 }
 723
 724 EIBNetIPPacket EIBnet_TunnelACK::ToPacket ()CONST
 725 {
 726   EIBNetIPPacket
 727     p;
 728   p.service = TUNNEL_RESPONSE;
 729   p.data.resize (4);
 730   p.data[0] = 4;
 731   p.data[1] = channel;
 732   p.data[2] = seqno;
 733   p.data[3] = status;
 734   return p;
 735 }
 736
 737 int
 738 parseEIBnet_TunnelACK (const EIBNetIPPacket & p, EIBnet_TunnelACK & r)
 739 {
 740   if (p.service != TUNNEL_RESPONSE)
 741     return 1;
 742   if (p.data () != 4)
 743     return 1;
 744   if (p.data[0] != 4)
 745     return 1;
 746   r.channel = p.data[1];
 747   r.seqno = p.data[2];
 748   r.status = p.data[3];
 749   return 0;
 750 }
 751
 752 EIBnet_DescriptionRequest::EIBnet_DescriptionRequest ()
 753 {
 754   memset (&caddr, 0, sizeof (caddr));
 755 }
 756
 757 EIBNetIPPacket EIBnet_DescriptionRequest::ToPacket ()CONST
 758 {
 759   EIBNetIPPacket
 760     p;
 761   CArray
 762     ca = IPtoEIBNetIP (&caddr);
 763   p.service = DESCRIPTION_REQUEST;
 764   p.data = ca;
 765   return p;
 766 }
 767
 768 int
 769 parseEIBnet_DescriptionRequest (const EIBNetIPPacket & p,
 770                                 EIBnet_DescriptionRequest & r)
 771 {
 772   if (p.service != DESCRIPTION_REQUEST)
 773     return 1;
 774   if (p.data () != 8)
 775     return 1;
 776   if (EIBnettoIP (p.data, &r.caddr, &p.src))
 777     return 1;
 778   return 0;
 779 }
 780
 781
 782 EIBnet_DescriptionResponse::EIBnet_DescriptionResponse ()
 783 {
 784   KNXmedium = 0;
 785   devicestatus = 0;
 786   individual_addr = 0;
 787   installid = 0;
 788   memset (&serial, 0, sizeof (serial));
 789   multicastaddr.s_addr = 0;
 790   memset (&MAC, 0, sizeof (MAC));
 791   memset (&name, 0, sizeof (name));
 792 }
 793
 794 EIBNetIPPacket EIBnet_DescriptionResponse::ToPacket ()CONST
 795 {
 796   EIBNetIPPacket
 797     p;
 798   p.service = DESCRIPTION_RESPONSE;
 799   p.data.resize (56 + services () * 2);
 800   p.data[0] = 54;
 801   p.data[1] = 1;
 802   p.data[2] = KNXmedium;
 803   p.data[3] = devicestatus;
 804   p.data[4] = (individual_addr >> 8) & 0xff;
 805   p.data[5] = (individual_addr) & 0xff;
 806   p.data[6] = (installid >> 8) & 0xff;
 807   p.data[7] = (installid) & 0xff;
 808   memcpy (p.data.array () + 18, &serial, 6);
 809   memcpy (p.data.array () + 14, &multicastaddr, 4);
 810   memcpy (p.data.array () + 18, &MAC, 6);
 811   memcpy (p.data.array () + 24, &name, 30);
 812   p.data[53] = 0;
 813   p.data[54] = services () * 2 + 2;
 814   p.data[55] = 2;
 815   for (int i = 0; i < services (); i++)
 816     {
 817       p.data[56 + i * 2] = services[i].family;
 818       p.data[57 + i * 2] = services[i].version;
 819     }
 820   p.data.setpart (optional, 56 + services () * 2);
 821   return p;
 822 }
 823
 824 int
 825 parseEIBnet_DescriptionResponse (const EIBNetIPPacket & p,
 826                                  EIBnet_DescriptionResponse & r)
 827 {
 828   if (p.service != DESCRIPTION_RESPONSE)
 829     return 1;
 830   if (p.data () < 56)
 831     return 1;
 832   if (p.data[0] != 54)
 833     return 1;
 834   if (p.data[1] != 1)
 835     return 1;
 836   r.KNXmedium = p.data[2];
 837   r.devicestatus = p.data[3];
 838   r.individual_addr = (p.data[4] << 8) | p.data[5];
 839   r.installid = (p.data[6] << 8) | p.data[7];
 840   memcpy (&r.serial, p.data.array () + 8, 6);
 841   memcpy (&r.multicastaddr, p.data.array () + 14, 4);
 842   memcpy (&r.MAC, p.data.array () + 18, 6);
 843   memcpy (&r.name, p.data.array () + 24, 30);
 844   r.name[29] = 0;
 845   if (p.data[55] != 2)
 846     return 1;
 847   if (p.data[54] % 2)
 848     return 1;
 849   if (p.data[54] + 54 > p.data ())
 850     return 1;
 851   r.services.resize ((p.data[54] / 2) - 1);
 852   for (int i = 0; i < (p.data[54] / 2) - 1; i++)
 853     {
 854       r.services[i].family = p.data[56 + 2 * i];
 855       r.services[i].version = p.data[57 + 2 * i];
 856     }
 857   r.optional.set (p.data.array () + p.data[54] + 54,
 858                   p.data () - p.data[54] - 54);
 859   return 0;
 860 }
 861
 862 EIBnet_SearchRequest::EIBnet_SearchRequest ()
 863 {
 864   memset (&caddr, 0, sizeof (caddr));
 865 }
 866
 867 EIBNetIPPacket EIBnet_SearchRequest::ToPacket ()CONST
 868 {
 869   EIBNetIPPacket
 870     p;
 871   CArray
 872     ca = IPtoEIBNetIP (&caddr);
 873   p.service = SEARCH_REQUEST;
 874   p.data = ca;
 875   return p;
 876 }
 877
 878 int
 879 parseEIBnet_SearchRequest (const EIBNetIPPacket & p, EIBnet_SearchRequest & r)
 880 {
 881   if (p.service != SEARCH_REQUEST)
 882     return 1;
 883   if (p.data () != 8)
 884     return 1;
 885   if (EIBnettoIP (p.data, &r.caddr, &p.src))
 886     return 1;
 887   return 0;
 888 }
 889
 890
 891 EIBnet_SearchResponse::EIBnet_SearchResponse ()
 892 {
 893   KNXmedium = 0;
 894   devicestatus = 0;
 895   individual_addr = 0;
 896   installid = 0;
 897   memset (&serial, 0, sizeof (serial));
 898   multicastaddr.s_addr = 0;
 899   memset (&MAC, 0, sizeof (MAC));
 900   memset (&name, 0, sizeof (name));
 901 }
 902
 903 EIBNetIPPacket EIBnet_SearchResponse::ToPacket ()CONST
 904 {
 905   EIBNetIPPacket
 906     p;
 907   CArray
 908     ca = IPtoEIBNetIP (&caddr);
 909   p.service = SEARCH_RESPONSE;
 910   p.data.resize (64 + services () * 2);
 911   p.data.setpart (ca, 0);
 912   p.data[8] = 54;
 913   p.data[9] = 1;
 914   p.data[10] = KNXmedium;
 915   p.data[11] = devicestatus;
 916   p.data[12] = (individual_addr >> 8) & 0xff;
 917   p.data[13] = (individual_addr) & 0xff;
 918   p.data[14] = (installid >> 8) & 0xff;
 919   p.data[15] = (installid) & 0xff;
 920   memcpy (p.data.array () + 16, &serial, 6);
 921   memcpy (p.data.array () + 22, &multicastaddr, 4);
 922   memcpy (p.data.array () + 26, &MAC, 6);
 923   memcpy (p.data.array () + 32, &name, 30);
 924   p.data[61] = 0;
 925   p.data[62] = services () * 2 + 2;
 926   p.data[63] = 2;
 927   for (int i = 0; i < services (); i++)
 928     {
 929       p.data[64 + i * 2] = services[i].family;
 930       p.data[65 + i * 2] = services[i].version;
 931     }
 932   return p;
 933 }
 934
 935 int
 936 parseEIBnet_SearchResponse (const EIBNetIPPacket & p,
 937                             EIBnet_SearchResponse & r)
 938 {
 939   if (p.service != SEARCH_RESPONSE)
 940     return 1;
 941   if (p.data () < 64)
 942     return 1;
 943   if (EIBnettoIP (CArray (p.data.array () + 0, 8), &r.caddr, &p.src))
 944     return 1;
 945   if (p.data[8] != 54)
 946     return 1;
 947   if (p.data[9] != 1)
 948     return 1;
 949   r.KNXmedium = p.data[10];
 950   r.devicestatus = p.data[11];
 951   r.individual_addr = (p.data[12] << 8) | p.data[13];
 952   r.installid = (p.data[14] << 8) | p.data[15];
 953   memcpy (&r.serial, p.data.array () + 16, 6);
 954   memcpy (&r.multicastaddr, p.data.array () + 22, 4);
 955   memcpy (&r.MAC, p.data.array () + 26, 6);
 956   memcpy (&r.name, p.data.array () + 32, 30);
 957   r.name[29] = 0;
 958   if (p.data[63] != 2)
 959     return 1;
 960   if (p.data[62] % 2)
 961     return 1;
 962   if (p.data[62] + 62 > p.data ())
 963     return 1;
 964   r.services.resize ((p.data[62] / 2) - 1);
 965   for (int i = 0; i < (p.data[62] / 2) - 1; i++)
 966     {
 967       r.services[i].family = p.data[64 + 2 * i];
 968       r.services[i].version = p.data[65 + 2 * i];
 969     }
 970   return 0;
 971 }