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