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