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