eibd/libserver/eibnetip.cpp

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