eibd/backend/eibnettunnel.cpp

   1 /*
   2     EIBD eib bus access and management daemon
   3     Copyright (C) 2005-2011 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18 */
  19
  20 #include "eibnettunnel.h"
  21 #include "emi.h"
  22
  23 bool
  24 EIBNetIPTunnel::addAddress (eibaddr_t addr)
  25 {
  26   return 0;
  27 }
  28
  29 bool
  30 EIBNetIPTunnel::removeAddress (eibaddr_t addr)
  31 {
  32   return 0;
  33 }
  34
  35 bool
  36 EIBNetIPTunnel::addGroupAddress (eibaddr_t addr)
  37 {
  38   return 1;
  39 }
  40
  41 bool
  42 EIBNetIPTunnel::removeGroupAddress (eibaddr_t addr)
  43 {
  44   return 1;
  45 }
  46
  47 eibaddr_t
  48 EIBNetIPTunnel::getDefaultAddr ()
  49 {
  50   return 0;
  51 }
  52
  53 EIBNetIPTunnel::EIBNetIPTunnel (const char *dest, int port, int sport,
  54                                 const char *srcip, int Dataport, int flags,
  55                                 Trace * tr)
  56 {
  57   t = tr;
  58   TRACEPRINTF (t, 2, this, "Open");
  59   pth_sem_init (&insignal);
  60   pth_sem_init (&outsignal);
  61   getwait = pth_event (PTH_EVENT_SEM, &outsignal);
  62   noqueue = flags & FLAG_B_TUNNEL_NOQUEUE;
  63   sock = 0;
  64   if (!GetHostIP (&caddr, dest))
  65     return;
  66   caddr.sin_port = htons (port);
  67   if (!GetSourceAddress (&caddr, &raddr))
  68     return;
  69   raddr.sin_port = htons (sport);
  70   NAT = false;
  71   dataport = Dataport;
  72   sock = new EIBNetIPSocket (raddr, 0, t);
  73   if (!sock->init ())
  74     {
  75       delete sock;
  76       sock = 0;
  77       return;
  78     }
  79   if (srcip)
  80     {
  81       if (!GetHostIP (&saddr, srcip))
  82         {
  83           delete sock;
  84           sock = 0;
  85           return;
  86         }
  87       saddr.sin_port = htons (sport);
  88       NAT = true;
  89     }
  90   else
  91     saddr = raddr;
  92   sock->sendaddr = caddr;
  93   sock->recvaddr = caddr;
  94   sock->recvall = 0;
  95   mode = 0;
  96   vmode = 0;
  97   support_busmonitor = 1;
  98   connect_busmonitor = 0;
  99   Start ();
 100   TRACEPRINTF (t, 2, this, "Opened");
 101 }
 102
 103 EIBNetIPTunnel::~EIBNetIPTunnel ()
 104 {
 105   TRACEPRINTF (t, 2, this, "Close");
 106   Stop ();
 107   while (!outqueue.isempty ())
 108     delete outqueue.get ();
 109   pth_event_free (getwait, PTH_FREE_THIS);
 110   if (sock)
 111     delete sock;
 112 }
 113
 114 bool EIBNetIPTunnel::init ()
 115 {
 116   return sock != 0;
 117 }
 118
 119 void
 120 EIBNetIPTunnel::Send_L_Data (LPDU * l)
 121 {
 122   TRACEPRINTF (t, 2, this, "Send %s", l->Decode ()());
 123   if (l->getType () != L_Data)
 124     {
 125       delete l;
 126       return;
 127     }
 128   L_Data_PDU *l1 = (L_Data_PDU *) l;
 129   inqueue.put (L_Data_ToCEMI (0x11, *l1));
 130   pth_sem_inc (&insignal, 1);
 131   if (vmode)
 132     {
 133       L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU;
 134       l2->pdu.set (l->ToPacket ());
 135       outqueue.put (l2);
 136       pth_sem_inc (&outsignal, 1);
 137     }
 138   outqueue.put (l);
 139   pth_sem_inc (&outsignal, 1);
 140 }
 141
 142 LPDU *
 143 EIBNetIPTunnel::Get_L_Data (pth_event_t stop)
 144 {
 145   if (stop != NULL)
 146     pth_event_concat (getwait, stop, NULL);
 147
 148   pth_wait (getwait);
 149
 150   if (stop)
 151     pth_event_isolate (getwait);
 152
 153   if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
 154     {
 155       pth_sem_dec (&outsignal);
 156       LPDU *c = outqueue.get ();
 157       if (c)
 158         TRACEPRINTF (t, 2, this, "Recv %s", c->Decode ()());
 159       return c;
 160     }
 161   else
 162     return 0;
 163 }
 164
 165 bool
 166 EIBNetIPTunnel::Send_Queue_Empty ()
 167 {
 168   return inqueue.isempty ();
 169 }
 170
 171 bool
 172 EIBNetIPTunnel::openVBusmonitor ()
 173 {
 174   vmode = 1;
 175   return 1;
 176 }
 177
 178 bool
 179 EIBNetIPTunnel::closeVBusmonitor ()
 180 {
 181   vmode = 0;
 182   return 1;
 183 }
 184
 185 bool
 186 EIBNetIPTunnel::enterBusmonitor ()
 187 {
 188   mode = 1;
 189   if (support_busmonitor)
 190     connect_busmonitor = 1;
 191   inqueue.put (CArray ());
 192   pth_sem_inc (&insignal, 1);
 193   return 1;
 194 }
 195
 196 bool
 197 EIBNetIPTunnel::leaveBusmonitor ()
 198 {
 199   mode = 0;
 200   connect_busmonitor = 0;
 201   inqueue.put (CArray ());
 202   pth_sem_inc (&insignal, 1);
 203   return 1;
 204 }
 205
 206 bool
 207 EIBNetIPTunnel::Open ()
 208 {
 209   return 1;
 210 }
 211
 212 bool
 213 EIBNetIPTunnel::Close ()
 214 {
 215   return 1;
 216 }
 217
 218 bool
 219 EIBNetIPTunnel::Connection_Lost ()
 220 {
 221   return 0;
 222 }
 223
 224 void
 225 EIBNetIPTunnel::Run (pth_sem_t * stop1)
 226 {
 227   int channel = -1;
 228   int mod = 0;
 229   int rno = 0;
 230   int sno = 0;
 231   int retry = 0;
 232   int heartbeat = 0;
 233   int drop = 0;
 234   eibaddr_t myaddr;
 235   pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
 236   pth_event_t input = pth_event (PTH_EVENT_SEM, &insignal);
 237   pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (0, 0));
 238   pth_event_t timeout1 = pth_event (PTH_EVENT_RTIME, pth_time (10, 0));
 239   L_Data_PDU *c;
 240
 241   EIBNetIPPacket p;
 242   EIBNetIPPacket *p1;
 243   EIBnet_ConnectRequest creq;
 244   creq.nat = saddr.sin_addr.s_addr == 0;
 245   EIBnet_ConnectResponse cresp;
 246   EIBnet_ConnectionStateRequest csreq;
 247   csreq.nat = saddr.sin_addr.s_addr == 0;
 248   EIBnet_ConnectionStateResponse csresp;
 249   EIBnet_TunnelRequest treq;
 250   EIBnet_TunnelACK tresp;
 251   EIBnet_DisconnectRequest dreq;
 252   dreq.nat = saddr.sin_addr.s_addr == 0;
 253   EIBnet_DisconnectResponse dresp;
 254   creq.caddr = saddr;
 255   creq.daddr = saddr;
 256   creq.CRI.resize (3);
 257   creq.CRI[0] = 0x04;
 258   creq.CRI[1] = 0x02;
 259   creq.CRI[2] = 0x00;
 260   p = creq.ToPacket ();
 261   sock->sendaddr = caddr;
 262   sock->Send (p);
 263
 264   while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
 265     {
 266       if (mod == 1)
 267         pth_event_concat (stop, input, NULL);
 268       if (mod == 2 || mod == 3)
 269         pth_event_concat (stop, timeout, NULL);
 270
 271       pth_event_concat (stop, timeout1, NULL);
 272
 273       p1 = sock->Get (stop);
 274       pth_event_isolate (stop);
 275       pth_event_isolate (timeout);
 276       pth_event_isolate (timeout1);
 277       if (p1)
 278         {
 279           switch (p1->service)
 280             {
 281             case CONNECTION_RESPONSE:
 282               if (mod)
 283                 goto err;
 284               if (parseEIBnet_ConnectResponse (*p1, cresp))
 285                 {
 286                   TRACEPRINTF (t, 1, this, "Recv wrong connection response");
 287                   break;
 288                 }
 289               if (cresp.status != 0)
 290                 {
 291                   TRACEPRINTF (t, 1, this, "Connect failed with error %02X",
 292                                cresp.status);
 293                   if (cresp.status == 0x23 && support_busmonitor == 1
 294                       && connect_busmonitor == 1)
 295                     {
 296                       TRACEPRINTF (t, 1, this, "Disable busmonitor support");
 297                       support_busmonitor = 0;
 298                       connect_busmonitor = 0;
 299                       creq.CRI[1] = 0x02;
 300                       pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1,
 301                                  pth_time (10, 0));
 302                       p = creq.ToPacket ();
 303                       TRACEPRINTF (t, 1, this, "Connectretry");
 304                       sock->sendaddr = caddr;
 305                       sock->Send (p);
 306                     }
 307                   break;
 308                 }
 309               if (cresp.CRD () != 3)
 310                 {
 311                   TRACEPRINTF (t, 1, this, "Recv wrong connection response");
 312                   break;
 313                 }
 314               myaddr = (cresp.CRD[1] << 8) | cresp.CRD[2];
 315               daddr = cresp.daddr;
 316               if (!cresp.nat)
 317                 {
 318                   if (NAT)
 319                     daddr.sin_addr = caddr.sin_addr;
 320                   if (dataport != -1)
 321                     daddr.sin_port = htons (dataport);
 322                 }
 323               channel = cresp.channel;
 324               mod = 1;
 325               sno = 0;
 326               rno = 0;
 327               sock->recvaddr2 = daddr;
 328               sock->recvall = 3;
 329               pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1,
 330                          pth_time (30, 0));
 331               heartbeat = 0;
 332               break;
 333
 334             case TUNNEL_REQUEST:
 335               if (mod == 0)
 336                 {
 337                   TRACEPRINTF (t, 1, this, "Not connected");
 338                   goto err;
 339                 }
 340               if (parseEIBnet_TunnelRequest (*p1, treq))
 341                 {
 342                   TRACEPRINTF (t, 1, this, "Invalid request");
 343                   break;
 344                 }
 345               if (treq.channel != channel)
 346                 {
 347                   TRACEPRINTF (t, 1, this, "Not for us");
 348                   break;
 349                 }
 350               if (((treq.seqno + 1) & 0xff) == rno)
 351                 {
 352                   tresp.status = 0;
 353                   tresp.channel = channel;
 354                   tresp.seqno = treq.seqno;
 355                   p = tresp.ToPacket ();
 356                   sock->sendaddr = daddr;
 357                   sock->Send (p);
 358                   sock->recvall = 0;
 359                   break;
 360                 }
 361               if (treq.seqno != rno)
 362                 {
 363                   TRACEPRINTF (t, 1, this, "Wrong sequence %d<->%d",
 364                                treq.seqno, rno);
 365                   if (treq.seqno < rno)
 366                     treq.seqno += 0x100;
 367                   if (treq.seqno >= rno + 5)
 368                     {
 369                       dreq.caddr = saddr;
 370                       dreq.channel = channel;
 371                       p = dreq.ToPacket ();
 372                       sock->sendaddr = caddr;
 373                       sock->Send (p);
 374                       sock->recvall = 0;
 375                       mod = 0;
 376                     }
 377                   break;
 378                 }
 379               rno++;
 380               if (rno > 0xff)
 381                 rno = 0;
 382               tresp.status = 0;
 383               tresp.channel = channel;
 384               tresp.seqno = treq.seqno;
 385               p = tresp.ToPacket ();
 386               sock->sendaddr = daddr;
 387               sock->Send (p);
 388               //Confirmation
 389               if (treq.CEMI[0] == 0x2E)
 390                 {
 391                   if (mod == 3)
 392                     mod = 1;
 393                   break;
 394                 }
 395               if (treq.CEMI[0] == 0x2B)
 396                 {
 397                   L_Busmonitor_PDU *l2 = CEMI_to_Busmonitor (treq.CEMI);
 398                   outqueue.put (l2);
 399                   pth_sem_inc (&outsignal, 1);
 400                   break;
 401                 }
 402               if (treq.CEMI[0] != 0x29)
 403                 {
 404                   TRACEPRINTF (t, 1, this, "Unexpected CEMI Type %02X",
 405                                treq.CEMI[0]);
 406                   break;
 407                 }
 408               c = CEMI_to_L_Data (treq.CEMI);
 409               if (c)
 410                 {
 411
 412                   TRACEPRINTF (t, 1, this, "Recv %s", c->Decode ()());
 413                   if (mode == 0)
 414                     {
 415                       if (vmode)
 416                         {
 417                           L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU;
 418                           l2->pdu.set (c->ToPacket ());
 419                           outqueue.put (l2);
 420                           pth_sem_inc (&outsignal, 1);
 421                         }
 422                       if (c->AddrType == IndividualAddress
 423                           && c->dest == myaddr)
 424                         c->dest = 0;
 425                       outqueue.put (c);
 426                       pth_sem_inc (&outsignal, 1);
 427                       break;
 428                     }
 429                   L_Busmonitor_PDU *p1 = new L_Busmonitor_PDU;
 430                   p1->pdu = c->ToPacket ();
 431                   delete c;
 432                   outqueue.put (p1);
 433                   pth_sem_inc (&outsignal, 1);
 434                   break;
 435                 }
 436               TRACEPRINTF (t, 1, this, "Unknown CEMI");
 437               break;
 438             case TUNNEL_RESPONSE:
 439               if (mod == 0)
 440                 {
 441                   TRACEPRINTF (t, 1, this, "Not connected");
 442                   goto err;
 443                 }
 444               if (parseEIBnet_TunnelACK (*p1, tresp))
 445                 {
 446                   TRACEPRINTF (t, 1, this, "Invalid response");
 447                   break;
 448                 }
 449               if (tresp.channel != channel)
 450                 {
 451                   TRACEPRINTF (t, 1, this, "Not for us");
 452                   break;
 453                 }
 454               if (tresp.seqno != sno)
 455                 {
 456                   TRACEPRINTF (t, 1, this, "Wrong sequence %d<->%d",
 457                                tresp.seqno, sno);
 458                   break;
 459                 }
 460               if (tresp.status)
 461                 {
 462                   TRACEPRINTF (t, 1, this, "Error in ACK %d", tresp.status);
 463                   break;
 464                 }
 465               if (mod == 2)
 466                 {
 467                   sno++;
 468                   if (sno > 0xff)
 469                     sno = 0;
 470                   pth_sem_dec (&insignal);
 471                   inqueue.get ();
 472                   if (noqueue)
 473                     {
 474                       mod = 3;
 475                       pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
 476                                  pth_time (1, 0));
 477                     }
 478                   else
 479                     mod = 1;
 480                   retry = 0;
 481                   drop = 0;
 482                 }
 483               else
 484                 TRACEPRINTF (t, 1, this, "Unexpected ACK");
 485               break;
 486             case CONNECTIONSTATE_RESPONSE:
 487               if (parseEIBnet_ConnectionStateResponse (*p1, csresp))
 488                 {
 489                   TRACEPRINTF (t, 1, this, "Invalid response");
 490                   break;
 491                 }
 492               if (csresp.channel != channel)
 493                 {
 494                   TRACEPRINTF (t, 1, this, "Not for us");
 495                   break;
 496                 }
 497               if (csresp.status == 0)
 498                 {
 499                   if (heartbeat > 0)
 500                     heartbeat--;
 501                   else
 502                     TRACEPRINTF (t, 1, this,
 503                                  "Duplicate Connection State Response");
 504                 }
 505               else if (csresp.status == 0x21)
 506                 {
 507                   TRACEPRINTF (t, 1, this,
 508                                "Connection State Response not connected");
 509                   dreq.caddr = saddr;
 510                   dreq.channel = channel;
 511                   p = dreq.ToPacket ();
 512                   sock->sendaddr = caddr;
 513                   sock->Send (p);
 514                   sock->recvall = 0;
 515                   mod = 0;
 516                 }
 517               else
 518                 TRACEPRINTF (t, 1, this,
 519                              "Connection State Response Error %02x",
 520                              csresp.status);
 521               break;
 522             case DISCONNECT_REQUEST:
 523               if (mod == 0)
 524                 {
 525                   TRACEPRINTF (t, 1, this, "Not connected");
 526                   goto err;
 527                 }
 528               if (parseEIBnet_DisconnectRequest (*p1, dreq))
 529                 {
 530                   TRACEPRINTF (t, 1, this, "Invalid request");
 531                   break;
 532                 }
 533               if (dreq.channel != channel)
 534                 {
 535                   TRACEPRINTF (t, 1, this, "Not for us");
 536                   break;
 537                 }
 538               dresp.channel = channel;
 539               dresp.status = 0;
 540               p = dresp.ToPacket ();
 541               t->TracePacket (1, this, "SendDis", p.data);
 542               sock->sendaddr = caddr;
 543               sock->Send (p);
 544               sock->recvall = 0;
 545               mod = 0;
 546               break;
 547             case DISCONNECT_RESPONSE:
 548               if (mod == 0)
 549                 {
 550                   TRACEPRINTF (t, 1, this, "Not connected");
 551                   break;
 552                 }
 553               if (parseEIBnet_DisconnectResponse (*p1, dresp))
 554                 {
 555                   TRACEPRINTF (t, 1, this, "Invalid request");
 556                   break;
 557                 }
 558               if (dresp.channel != channel)
 559                 {
 560                   TRACEPRINTF (t, 1, this, "Not for us");
 561                   break;
 562                 }
 563               mod = 0;
 564               sock->recvall = 0;
 565               TRACEPRINTF (t, 1, this, "Disconnected");
 566               pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1,
 567                          pth_time (0, 100));
 568               break;
 569             default:
 570             err:
 571               TRACEPRINTF (t, 1, this, "Recv unexpected service %04X",
 572                            p1->service);
 573             }
 574           delete p1;
 575         }
 576       if (mod == 2 && pth_event_status (timeout) == PTH_STATUS_OCCURRED)
 577         {
 578           mod = 1;
 579           retry++;
 580           if (retry > 3)
 581             {
 582               TRACEPRINTF (t, 1, this, "Drop");
 583               pth_sem_dec (&insignal);
 584               inqueue.get ();
 585               retry = 0;
 586               drop++;
 587               if (drop >= 3)
 588                 {
 589                   dreq.caddr = saddr;
 590                   dreq.channel = channel;
 591                   p = dreq.ToPacket ();
 592                   sock->sendaddr = caddr;
 593                   sock->Send (p);
 594                   sock->recvall = 0;
 595                   mod = 0;
 596                 }
 597             }
 598         }
 599       if (mod == 3 && pth_event_status (timeout) == PTH_STATUS_OCCURRED)
 600         mod = 1;
 601       if (mod != 0 && pth_event_status (timeout1) == PTH_STATUS_OCCURRED)
 602         {
 603           pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1,
 604                      pth_time (30, 0));
 605           if (heartbeat < 5)
 606             {
 607               csreq.caddr = saddr;
 608               csreq.channel = channel;
 609               p = csreq.ToPacket ();
 610               TRACEPRINTF (t, 1, this, "Heartbeat");
 611               sock->sendaddr = caddr;
 612               sock->Send (p);
 613               heartbeat++;
 614             }
 615           else
 616             {
 617               TRACEPRINTF (t, 1, this, "Disconnection because of errors");
 618               dreq.caddr = saddr;
 619               dreq.channel = channel;
 620               p = dreq.ToPacket ();
 621               sock->sendaddr = caddr;
 622               if (channel != -1)
 623                 sock->Send (p);
 624               sock->recvall = 0;
 625               mod = 0;
 626             }
 627         }
 628       if (mod == 0 && pth_event_status (timeout1) == PTH_STATUS_OCCURRED)
 629         {
 630           pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1,
 631                      pth_time (10, 0));
 632           creq.CRI[1] =
 633             ((connect_busmonitor && support_busmonitor) ? 0x80 : 0x02);
 634           p = creq.ToPacket ();
 635           TRACEPRINTF (t, 1, this, "Connectretry");
 636           sock->sendaddr = caddr;
 637           sock->Send (p);
 638         }
 639
 640       if (!inqueue.isempty () && inqueue.top ()() == 0)
 641         {
 642           pth_sem_dec (&insignal);
 643           inqueue.get ();
 644           if (support_busmonitor)
 645             {
 646               dreq.caddr = saddr;
 647               dreq.channel = channel;
 648               p = dreq.ToPacket ();
 649               sock->sendaddr = caddr;
 650               sock->Send (p);
 651             }
 652         }
 653
 654       if (!inqueue.isempty () && mod == 1)
 655         {
 656           treq.channel = channel;
 657           treq.seqno = sno;
 658           treq.CEMI = inqueue.top ();
 659           p = treq.ToPacket ();
 660           t->TracePacket (1, this, "SendTunnel", p.data);
 661           sock->sendaddr = daddr;
 662           sock->Send (p);
 663           mod = 2;
 664           pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
 665                      pth_time (1, 0));
 666         }
 667     }
 668 out:
 669   dreq.caddr = saddr;
 670   dreq.channel = channel;
 671   p = dreq.ToPacket ();
 672   sock->sendaddr = caddr;
 673   if (channel != -1)
 674     sock->Send (p);
 675
 676   pth_event_free (stop, PTH_FREE_THIS);
 677   pth_event_free (input, PTH_FREE_THIS);
 678   pth_event_free (timeout, PTH_FREE_THIS);
 679   pth_event_free (timeout1, PTH_FREE_THIS);
 680 }