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 "router.h"
  21  
  22  #include <iostream>
  23  #include <math.h>
  24  #include <sys/socket.h>
  25  #include <typeinfo>
  26  
  27  #include <ev++.h>
  28  #ifdef HAVE_SYSTEMD
  29  #include <systemd/sd-daemon.h>
  30  #endif
  31  
  32  #include "cm_tp1.h"
  33  #ifdef HAVE_GROUPCACHE
  34  #include "groupcacheclient.h"
  35  #endif
  36  #include "link.h"
  37  #include "lowlevel.h"
  38  #include "server.h"
  39  #include "systemdserver.h"
  40  #include "tcptunserver.h"
  41  
  42  /** global filter adapter, sending end */
  43  class RouterHigh : public Driver
  44  {
  45    Router* router;
  46  public:
  47    RouterHigh(Router& r, const RouterLowPtr& rl);
  48    virtual ~RouterHigh() = default;
  49  
  50    virtual void recv_L_Data (LDataPtr l);
  51    virtual void recv_L_Busmonitor (LBusmonPtr l);
  52  
  53    virtual void send_L_Data (LDataPtr l)
  54    {
  55      router->send_L_Data(std::move(l));
  56    }
  57  
  58    virtual bool checkAddress (eibaddr_t addr) const override
  59    {
  60      LinkConnectPtr link = nullptr;
  61      return router->checkAddress(addr, link);
  62    }
  63  
  64    virtual bool checkGroupAddress (eibaddr_t addr) const override
  65    {
  66      LinkConnectPtr link = nullptr;
  67      return router->checkGroupAddress(addr, link);
  68    }
  69  
  70    virtual bool hasAddress (eibaddr_t addr) const override
  71    {
  72      LinkConnectPtr link = nullptr;
  73      return router->hasAddress(addr, link);
  74    }
  75  
  76    virtual void addAddress (eibaddr_t addr) override
  77    {
  78      if (addr != router->addr)
  79        ERRORPRINTF (t, E_ERROR | 80, "%s filter: Trying to add address %s", router->main, FormatEIBAddr(addr));
  80    }
  81  
  82    virtual void start()
  83    {
  84      router->start_();
  85    }
  86  
  87    virtual void stop(bool err)
  88    {
  89      router->stop_(err);
  90    }
  91  
  92    /** prevent dup calls to started() */
  93    bool is_started = false;
  94    virtual void started();
  95    virtual void stopped(bool err);
  96  
  97  };
  98  
  99  /** global filter adapter, receiving end */
 100  class RouterLow : public LinkConnect_
 101  {
 102  public:
 103    Router* router;
 104    RouterLow(Router& r);
 105    virtual ~RouterLow() = default;
 106  
 107    virtual void recv_L_Data (LDataPtr l)
 108    {
 109      router->queue_L_Data (std::move(l));
 110    }
 111    virtual void recv_L_Busmonitor (LBusmonPtr l)
 112    {
 113      router->queue_L_Busmonitor (std::move(l));
 114    }
 115    virtual void send_Next ();
 116  
 117    virtual void start()
 118    {
 119      send->start();
 120    }
 121    virtual void stop(bool err)
 122    {
 123      send->stop(err);
 124    }
 125  
 126    virtual void started()
 127    {
 128      router->started();
 129    }
 130    virtual void stopped(bool err)
 131    {
 132      router->stopped(err);
 133    }
 134  };
 135  
 136  static Factory<Server> _servers;
 137  static Factory<Driver> _drivers;
 138  static Factory<Filter> _filters;
 139  
 140  bool unseen_lister(void *user, const IniSection& section, const std::string& name, const std::string& value);
 141  
 142  /** parses an EIB individual address */
 143  bool
 144  Router::readaddr (const std::string& addr, eibaddr_t& parsed)
 145  {
 146    int a, b, c;
 147    if (sscanf (addr.c_str(), "%d.%d.%d", &a, &b, &c) != 3)
 148      {
 149        ERRORPRINTF (t, E_ERROR | 81, "'%s' is not a device address. Use X.Y.Z format.", addr);
 150        return false;
 151      }
 152    parsed = ((a & 0x0f) << 12) | ((b & 0x0f) << 8) | (c & 0xff);
 153    return true;
 154  }
 155  
 156  bool
 157  Router::readaddrblock (const std::string& addr, eibaddr_t& parsed, int &len)
 158  {
 159    int a, b, c;
 160    if (sscanf (addr.c_str(), "%d.%d.%d:%d", &a, &b, &c, &len) != 4)
 161      {
 162        ERRORPRINTF (t, E_ERROR | 82, "An address block needs to look like X.Y.Z:N, not '%s'.", addr);
 163        return false;
 164      }
 165    parsed = ((a & 0x0f) << 12) | ((b & 0x0f) << 8) | (c & 0xff);
 166    return true;
 167  }
 168  
 169  Router::Router (IniData& d, std::string sn) : BaseRouter(d)
 170    , servers(_servers.Instance())
 171    , filters(_filters.Instance())
 172    , drivers(_drivers.Instance())
 173    , main(sn)
 174  {
 175    IniSectionPtr s = ini[main];
 176    t = TracePtr(new Trace(s, s->value("name","")));
 177    servername = s->value("name","knxd");
 178  
 179    r_low = RouterLowPtr(new RouterLow(*this));
 180    r_high = RouterHighPtr(new RouterHigh(*this, r_low));
 181    r_low->set_driver(std::dynamic_pointer_cast<Driver>(r_high));
 182  
 183    trigger.set<Router, &Router::trigger_cb>(this);
 184    mtrigger.set<Router, &Router::mtrigger_cb>(this);
 185    state_trigger.set<Router, &Router::state_trigger_cb>(this);
 186    trigger.start();
 187    mtrigger.start();
 188    state_trigger.start();
 189  
 190    TRACEPRINTF (t, 4, "initialized");
 191  }
 192  
 193  bool
 194  Router::setup()
 195  {
 196    std::string x;
 197    IniSectionPtr s = ini[main];
 198    TRACEPRINTF (t, 4, "setting up");
 199  
 200    force_broadcast = s->value("force-broadcast", false);
 201    unknown_ok = s->value("unknown-ok", false);
 202  
 203    x = s->value("addr","");
 204    if (!x.size())
 205      {
 206        ERRORPRINTF (t, E_ERROR | 84, "There is no KNX addr= in section '%s'.", main);
 207        goto ex;
 208      }
 209    if (!readaddr(x,addr))
 210      goto ex;
 211  
 212    x = s->value("client-addrs","");
 213    if (x.size())
 214      {
 215        if (!readaddrblock(x,client_addrs_start,client_addrs_len))
 216          goto ex;
 217        client_addrs_pos = client_addrs_len-1;
 218        client_addrs.resize(client_addrs_len);
 219        ITER(i,client_addrs)
 220        *i = false;
 221      }
 222  
 223  #ifdef HAVE_GROUPCACHE
 224    {
 225      IniSectionPtr gc = s->sub("cache",false);
 226      if (gc->name.size() > 0)
 227        {
 228          if (!CreateGroupCache(*this, gc))
 229            goto ex;
 230        }
 231    }
 232  #endif
 233  
 234    if (!r_low->setup())
 235      return false;
 236  
 237  #ifdef HAVE_SYSTEMD
 238    {
 239      std::string sd_name_knxd = s->value("systemd","");
 240      std::string sd_name_tun = s->value("systemd_tcptunsrv","");
 241      if (sd_name_knxd.size() > 0 || sd_name_tun.size() > 0)
 242        {
 243          // IniSectionPtr sd = ini[sd_name];
 244          // set the section's "referenced" bit, for error checking
 245          if (sd_name_knxd.size() > 0)
 246            (void) ini[sd_name_knxd];
 247          if (sd_name_tun.size() > 0)
 248            (void) ini[sd_name_tun];
 249  
 250          char** fd_names_c = nullptr;
 251          int num_fds = sd_listen_fds_with_names(0, &fd_names_c);
 252          if( num_fds < 0 )
 253            {
 254              ERRORPRINTF (t, E_ERROR | 85, "Error getting fds from systemd.");
 255              goto ex;
 256            }
 257  
 258          std::vector<std::string> fd_names;
 259          if (fd_names_c)
 260            {
 261              for (char** name = fd_names_c; *name; name++)
 262                {
 263                  fd_names.push_back(*name);
 264                  free(*name);
 265                }
 266              free(fd_names_c);
 267            }
 268          if (fd_names.size() < num_fds)
 269            {
 270              ERRORPRINTF (t, E_ERROR | 147, "Got too few fds names from systemd.");
 271              goto ex;
 272            }
 273  
 274          // zero FDs from systemd is not a bug
 275          for( int fd = SD_LISTEN_FDS_START; fd < SD_LISTEN_FDS_START+num_fds; ++fd )
 276            {
 277              std::string name = fd_names[fd - SD_LISTEN_FDS_START];
 278              TRACEPRINTF (t, 4, "Got fd %d with name '%s'", fd, name);
 279              std::string sd_name;
 280              bool is_tun;
 281              if (name == "knxnet") {
 282                sd_name = sd_name_tun;
 283                is_tun = true;
 284              } else {
 285                sd_name = sd_name_knxd;
 286                is_tun = false;
 287              }
 288  
 289              IniSectionPtr sds = ini.add_auto(sd_name);
 290  
 291              (*sds)["use"] = sd_name;
 292              if( sd_is_socket(fd, AF_UNSPEC, SOCK_STREAM, 1) <= 0 )
 293                {
 294                  ERRORPRINTF (t, E_ERROR | 86, "systemd socket %d is not a socket.", fd);
 295                  goto ex;
 296                }
 297  
 298              ServerPtr sdp;
 299              if (is_tun) {
 300                sdp = ServerPtr(new TcpTunSystemdServer(*this, sds, fd));
 301              } else {
 302                sdp = ServerPtr(new SystemdServer(*this, sds, fd));
 303              }
 304              if (!sdp->setup())
 305                goto ex;
 306              registerLink(sdp);
 307  
 308              using_systemd = true;
 309            }
 310        }
 311    }
 312  
 313  #endif
 314  
 315    x = s->value("connections","");
 316    {
 317      size_t pos = 0;
 318      size_t comma = 0;
 319      while(true)
 320        {
 321          comma = x.find(',',pos);
 322          std::string name = x.substr(pos,comma-pos);
 323          if (name.size())
 324            {
 325              LinkConnectPtr link = setup_link(name);
 326              if (link == nullptr)
 327                goto ex;
 328              registerLink(link);
 329            }
 330          if (comma == std::string::npos)
 331            break;
 332          pos = comma+1;
 333        }
 334    }
 335  
 336    if (links.size() == 0)
 337      {
 338        ERRORPRINTF (t, E_ERROR | 87, "No connections in section '%s'.", main);
 339        goto ex;
 340      }
 341    if (links.size() == 1)
 342      {
 343        ERRORPRINTF (t, E_ERROR | 88, "Only one connection in section '%s'.", main);
 344        goto ex;
 345      }
 346  
 347    if (ini.list_unseen(&unseen_lister, (void *)this))
 348      goto ex;
 349  
 350    ITER(i,links)
 351    ERRORPRINTF (t, E_INFO | 129, "Connected: %s.", i->second->info(2));
 352  
 353    TRACEPRINTF (t, 4, "setup OK");
 354    return true;
 355  ex:
 356    TRACEPRINTF (t, 4, "setup BROKEN");
 357    return false;
 358  }
 359  
 360  bool
 361  unseen_lister(void *user,
 362                const IniSection& section, const std::string& name, const std::string& value)
 363  {
 364    Router *r = (Router *)user;
 365    ERRORPRINTF (r->t, (r->unknown_ok ? E_WARNING : E_FATAL) | 104, "Section '%s': unrecognized argument '%s = %s'",
 366                 section.name, name, value);
 367    return !r->unknown_ok;
 368  }
 369  
 370  bool
 371  Router::do_driver(LinkConnectPtr &link, IniSectionPtr& s, const std::string& drivername, bool quiet)
 372  {
 373    DriverPtr driver = nullptr;
 374  
 375    link = LinkConnectPtr(new LinkConnect(*this, s, t));
 376    driver = DriverPtr(drivers.create(drivername, link, s));
 377    if (driver == nullptr)
 378      {
 379        if(!quiet)
 380          ERRORPRINTF (t, E_ERROR | 89, "Driver '%s' not found.", drivername);
 381        link = nullptr;
 382        return false;
 383      }
 384    link->set_driver(driver);
 385    if (!link->setup())
 386      link = nullptr;
 387    return true;
 388  }
 389  
 390  
 391  FilterPtr
 392  Router::get_filter(const LinkConnectPtr_& link, IniSectionPtr& s, const std::string& filtername)
 393  {
 394    return FilterPtr(filters.create(filtername, link, s));
 395  }
 396  
 397  bool
 398  Router::do_server(ServerPtr &link, IniSectionPtr& s, const std::string& servername, bool quiet)
 399  {
 400    link = ServerPtr(servers.create(servername, *this, s));
 401    if (link == nullptr)
 402      {
 403        if(!quiet)
 404          ERRORPRINTF (t, E_ERROR | 90, "Server '%s' not found.", servername);
 405        return false;
 406      }
 407    if (!link->setup())
 408      link = nullptr;
 409    return true;
 410  }
 411  
 412  LinkConnectPtr
 413  Router::setup_link(std::string& name)
 414  {
 415    IniSectionPtr s = ini[name];
 416    std::string servername = s->value("server","");
 417    std::string drivername = s->value("driver","");
 418    LinkConnectPtr link = nullptr;
 419    ServerPtr server = nullptr;
 420  
 421    if (servername.size() && do_server(server, s,servername))
 422      return server;
 423    if (drivername.size() && do_driver(link, s,drivername))
 424      return link;
 425    if (do_server(server, s,s->name, true))
 426      return server;
 427    if (do_driver(link, s,s->name, true))
 428      return link;
 429    ERRORPRINTF (t, E_ERROR | 91, "Section '%s' has no known server nor driver.", name);
 430    return nullptr;
 431  }
 432  
 433  void
 434  Router::start()
 435  {
 436    if (want_up)
 437      return;
 438    want_up = true;
 439  
 440    TRACEPRINTF (t, 4, "trigger going up");
 441    r_low->start();
 442  }
 443  
 444  void
 445  Router::start_()
 446  {
 447    some_running = true;
 448    bool seen = true;
 449  
 450    in_link_loop += 1;
 451    seq++;
 452    while (seen)
 453      {
 454        seen = false;
 455        links_changed = false;
 456        ITER(i,links)
 457        {
 458          auto ii = i->second;
 459          if (ii->ignore || ii->transient)
 460            continue;
 461          if (ii->seq >= seq)
 462            continue;
 463          seen = true;
 464          ii->seq = seq;
 465          TRACEPRINTF (ii->t, 3, "Start: %s", ii->info(0));
 466          ii->setState(L_going_up);
 467          if (links_changed)
 468            break;
 469        }
 470      }
 471    in_link_loop -= 1;
 472    TRACEPRINTF (t, 4, "going up triggered");
 473  }
 474  
 475  void
 476  Router::send_Next()
 477  {
 478    if (high_sending)
 479      {
 480        TRACEPRINTF (t, 6, "sending set");
 481        return;
 482      }
 483    if (high_send_more)
 484      {
 485        TRACEPRINTF (t, 6, "send_more set");
 486        return;
 487      }
 488    ITER(i,links)
 489    {
 490      auto ii = i->second;
 491      if (ii->state != L_up)
 492        {
 493          TRACEPRINTF (ii->t, 6, "not up");
 494          continue;
 495        }
 496      if (!ii->send_more)
 497        {
 498          TRACEPRINTF (ii->t, 6, "still waiting");
 499          return;
 500        }
 501      TRACEPRINTF (ii->t, 6, "is OK");
 502    }
 503    TRACEPRINTF (t, 6, "OK");
 504    high_send_more = true;
 505    r_high->send_Next();
 506  }
 507  
 508  void
 509  Router::linkStateChanged(const LinkConnectPtr& link)
 510  {
 511    TRACEPRINTF (link->t, 4, "link state changed: %s", link->stateName());
 512    linkChanges.push(link);
 513    state_trigger.send();
 514  }
 515  
 516  void
 517  Router::state_trigger_cb (ev::async &, int)
 518  {
 519    bool oarn = all_running;
 520    bool osrn = some_running;
 521  
 522    int n_up = 0;
 523    int n_going = 0;
 524    int n_down = 0;
 525    int n_error = 0;
 526  
 527    TRACEPRINTF (t, 4, "check start");
 528  
 529    while (!linkChanges.empty())
 530      {
 531        LinkConnectPtr l = linkChanges.get();
 532        if (!want_up && l->state >= L_up)
 533          l->setState(L_going_down);
 534      }
 535  
 536    ITER(i,links)
 537    {
 538      auto ii = i->second;
 539      LConnState lcs = ii->state;
 540  
 541      if (ii->transient)
 542        {
 543          if (!want_up && lcs >= L_up)
 544            {
 545              ii->setState(L_going_down);
 546              n_going ++;
 547            }
 548          continue;
 549        }
 550  
 551      lcs = ii->state;
 552      switch(lcs)
 553        {
 554        case L_down:
 555          TRACEPRINTF (ii->t, 4, "is %s", ii->stateName());
 556          if (!ii->ignore)
 557            n_down++;
 558          break;
 559        case L_error:
 560          if (!ii->ignore)
 561            {
 562              n_down++;
 563              n_error++;
 564            }
 565          break;
 566        case L_up:
 567          n_up++;
 568          break;
 569        default:
 570          TRACEPRINTF (ii->t, 4, "state is %s", ii->stateName());
 571          n_going++;
 572        }
 573    }
 574  
 575    if (!n_going && !n_down && n_up)
 576      {
 577        all_running = true;
 578        some_running = true;
 579      }
 580    else if (!n_going && !n_up)
 581      {
 582        some_running = false;
 583        all_running = false;
 584      }
 585    else
 586      {
 587        some_running = true;
 588        all_running = false;
 589      }
 590  
 591    TRACEPRINTF (t, 4, "check end: want_up %d some %d>%d all %d>%d, going %d up %d down %d", want_up, osrn,some_running, oarn,all_running, n_going,n_up,n_down);
 592    if (want_up && n_error)
 593      {
 594        // Hard report errors
 595        ITER(i,links)
 596        {
 597          auto ii = i->second;
 598          LConnState lcs = ii->state;
 599          switch(lcs)
 600            {
 601            case L_error:
 602              if (!ii->ignore)
 603                ERRORPRINTF (ii->t, E_FATAL | 105, "Link down, terminating");
 604            }
 605        }
 606        exitcode = 1;
 607        stop(true);
 608      }
 609  
 610    if (osrn && !some_running)
 611      r_high->stopped(n_error > 0);
 612    else if (!oarn && all_running)
 613      r_high->started();
 614  }
 615  
 616  void
 617  Router::stop(bool err)
 618  {
 619    if (!want_up)
 620      return;
 621    want_up = false;
 622  
 623    TRACEPRINTF (t, 4, "trigger Going down");
 624    r_low->stop(err);
 625  }
 626  
 627  void
 628  Router::stop_(bool err)
 629  {
 630    all_running = false;
 631    bool seen = true;
 632  
 633    if (want_up)
 634      {
 635        // we get here when there's a failure to start the global filter chain
 636        stop(err);
 637        return;
 638      }
 639  
 640    in_link_loop += 1;
 641    seq++;
 642    while(seen)
 643      {
 644        links_changed = false;
 645        seen = false;
 646        ITER(i,links)
 647        {
 648          auto ii = i->second;
 649          if (ii->seq >= seq)
 650            continue; // already told it
 651          TRACEPRINTF (ii->t, 4, "R Stopping");
 652          seen = true;
 653          ii->seq = seq;
 654          ii->setState(L_going_down);
 655          if (links_changed)
 656            break;
 657        }
 658      }
 659    in_link_loop -= 1;
 660  }
 661  
 662  
 663  void
 664  Router::started()
 665  {
 666    low_send_more = true;
 667    high_send_more = true;
 668    trigger.send();
 669    mtrigger.send();
 670  
 671    if (all_running && !running_signal)
 672      {
 673        running_signal = true;
 674        TRACEPRINTF (t, 4, "all drivers up");
 675  #ifdef HAVE_SYSTEMD
 676        sd_notify(0,"READY=1");
 677  #endif
 678      }
 679  
 680    TRACEPRINTF (t, 4, "up");
 681  }
 682  
 683  void
 684  Router::stopped(bool err)
 685  {
 686    TRACEPRINTF (t, 4, "down");
 687    if (want_up)
 688      stop(err);
 689    else
 690      ev_break (EV_A_ EVBREAK_ALL);
 691  }
 692  
 693  Router::~Router()
 694  {
 695    TRACEPRINTF (t, 4, "deleting");
 696    cache = nullptr;
 697    trigger.stop();
 698    mtrigger.stop();
 699    state_trigger.stop();
 700  
 701    R_ITER(i,vbusmonitor)
 702    ERRORPRINTF (t, E_WARNING | 55, "VBusmonitor '%s' didn't de-register!", i->cb->name);
 703    vbusmonitor.clear();
 704  
 705    R_ITER(i,busmonitor)
 706    ERRORPRINTF (t, E_WARNING | 56, "Busmonitor '%s' didn't de-register!", i->cb->name);
 707    busmonitor.clear();
 708  
 709  //  ITER(i,links)
 710  //    delete i->second;
 711    links.clear();
 712  
 713    TRACEPRINTF (t, 4, "deleted.");
 714  }
 715  
 716  void
 717  Router::recv_L_Data (LDataPtr l, LinkConnect& link)
 718  {
 719    LinkConnectPtr l2x = nullptr;
 720  
 721    if (l->address_type == IndividualAddress && l->destination_address == 0)
 722      {
 723        // Common problem with things that are not true gateways
 724        ERRORPRINTF (link.t, E_WARNING | 57, "Message without destination. Use the single-node filter ('-B single')?");
 725        return;
 726      }
 727  
 728    // Unassigned source: set to link's, or our, address
 729    if (l->source_address == 0)
 730      {
 731        l->source_address = link.addr;
 732        if (l->source_address == 0)
 733          l->source_address = addr;
 734      }
 735  
 736    if (l->source_address == addr)
 737      {
 738        // locally generated?
 739        if (!link.is_local)
 740          {
 741            // Nope. Reject.
 742            TRACEPRINTF (link.t, 3, "Packet not from us");
 743            return;
 744          }
 745      }
 746    else if (hasAddress (l->source_address, l2x))
 747      {
 748        // check if from the correct interface
 749        if (&*l2x != &link)
 750          {
 751            TRACEPRINTF (link.t, 3, "Packet not from %d:%s: %s", l2x->t->seq, l2x->t->name, l->Decode (t));
 752            return;
 753          }
 754      }
 755    else if (client_addrs_len && l->source_address >= client_addrs_start && l->source_address < client_addrs_start+client_addrs_len)
 756      {
 757        TRACEPRINTF (link.t, 3, "Packet originally from closed local interface");
 758        return;
 759      }
 760    else if (l->source_address != 0xFFFF)   // don't assign the "unprogrammed" address
 761      {
 762        link.addAddress (l->source_address);
 763      }
 764  
 765    l->source = &link;
 766    r_high->recv_L_Data(std::move(l));
 767  }
 768  
 769  void
 770  Router::recv_L_Busmonitor (LBusmonPtr l)
 771  {
 772    r_high->recv_L_Busmonitor(std::move(l));
 773  }
 774  
 775  void
 776  Router::queue_L_Data (LDataPtr l)
 777  {
 778    if (some_running || want_up)
 779      {
 780        buf.emplace (std::move(l));
 781        if (running_signal)
 782          trigger.send();
 783      }
 784    else
 785      TRACEPRINTF (t, 9, "Queue: discard (not running) %s", l->Decode (t));
 786  }
 787  
 788  void
 789  Router::queue_L_Busmonitor (LBusmonPtr l)
 790  {
 791    if (some_running || want_up)
 792      {
 793        mbuf.push (std::move(l));
 794        if (running_signal)
 795          mtrigger.send();
 796      }
 797    else
 798      TRACEPRINTF (t, 9, "MonQueue: discard (not running) %s", l->Decode (t));
 799  }
 800  
 801  bool
 802  Router::deregisterBusmonitor (L_Busmonitor_CallBack * c)
 803  {
 804    ITER(i, busmonitor)
 805    if (i->cb == c)
 806      {
 807        busmonitor.erase(i);
 808        TRACEPRINTF (t, 3, "deregisterBusmonitor");
 809        return true;
 810      }
 811    TRACEPRINTF (t, 3, "deregisterBusmonitor failed");
 812    return false;
 813  }
 814  
 815  bool
 816  Router::deregisterVBusmonitor (L_Busmonitor_CallBack * c)
 817  {
 818    ITER(i,vbusmonitor)
 819    if (i->cb == c)
 820      {
 821        TRACEPRINTF (t, 3, "deregisterVBusmonitor");
 822        vbusmonitor.erase(i);
 823        return true;
 824      }
 825    TRACEPRINTF (t, 3, "deregisterVBusmonitor failed");
 826    return false;
 827  }
 828  
 829  bool
 830  Router::registerBusmonitor (L_Busmonitor_CallBack * c)
 831  {
 832    busmonitor.push_back((Busmonitor_Info)
 833    {
 834      .cb=c
 835    });
 836    TRACEPRINTF (t, 3, "registerBusmonitor");
 837    return true;
 838  }
 839  
 840  bool
 841  Router::registerVBusmonitor (L_Busmonitor_CallBack * c)
 842  {
 843    vbusmonitor.push_back((Busmonitor_Info)
 844    {
 845      .cb=c
 846    });
 847    TRACEPRINTF (t, 3, "registerVBusmonitor");
 848    return true;
 849  }
 850  
 851  bool
 852  Router::registerLink(const LinkConnectPtr& link, bool transient)
 853  {
 854    const std::string& n = link->name();
 855  #if 1 // TODO tracing
 856    link->pos = link->t->seq;
 857  #else
 858    static int pos = 0;
 859    if (link->pos == 0)
 860      link->pos = ++pos;
 861  #endif
 862    auto res = links.emplace(std::piecewise_construct,
 863                             std::forward_as_tuple(link->pos),
 864                             std::forward_as_tuple(link));
 865    if (! res.second)
 866      {
 867        ERRORPRINTF (link->t, E_ERROR | 93, "registerLink: %d:%s: already present", link->pos,n);
 868        return false;
 869      }
 870    TRACEPRINTF (link->t, 3, "registerLink: %d:%s", link->pos,n);
 871    links_changed = true;
 872    if (transient)
 873      link->transient = true;
 874    if (want_up)
 875      {
 876        all_running = false;
 877        TRACEPRINTF (link->t, 3, "Start: %s", link->info(0));
 878        link->seq = seq;
 879        link->setState(L_going_up);
 880      }
 881    return true;
 882  }
 883  
 884  bool
 885  Router::unregisterLink(const LinkConnectPtr& link)
 886  {
 887    const std::string& n = link->name();
 888    auto res = links.find(link->pos);
 889    if (res == links.end())
 890      {
 891        ERRORPRINTF (link->t, E_ERROR | 94, "unregisterLink: %d:%s: not present", link->pos,n);
 892        return false;
 893      }
 894    links.erase(res);
 895    TRACEPRINTF (link->t, 3, "unregisterLink: %s", n);
 896    links_changed = true;
 897    if (!in_link_loop)
 898      state_trigger.send();
 899    return true;
 900  }
 901  
 902  bool
 903  Router::hasAddress (eibaddr_t addr, LinkConnectPtr& link, bool quiet) const
 904  {
 905    if (addr == this->addr)
 906      {
 907        if (!quiet)
 908          TRACEPRINTF (t, 8, "default addr %s", FormatEIBAddr (addr));
 909        return true;
 910      }
 911  
 912    if (link && link->hasAddress(addr))
 913      {
 914        if (!quiet)
 915          TRACEPRINTF (t, 8, "own addr %s", FormatEIBAddr (addr));
 916        return false;
 917      }
 918  
 919    C_ITER(i, links)
 920    {
 921      if (i->second == link)
 922        continue;
 923      if (i->second->hasAddress (addr))
 924        {
 925          if (i->second == link)
 926            {
 927              if (!quiet)
 928                TRACEPRINTF (link->t, 8, "local addr %s", FormatEIBAddr (addr));
 929              return false;
 930            }
 931          if (!quiet)
 932            TRACEPRINTF (i->second->t, 8, "found addr %s", FormatEIBAddr (addr));
 933          link = i->second;
 934          return true;
 935        }
 936    }
 937  
 938    if (!quiet)
 939      TRACEPRINTF (t, 8, "unknown addr %s", FormatEIBAddr (addr));
 940    return false;
 941  }
 942  
 943  bool
 944  Router::checkAddress (eibaddr_t addr, LinkConnectPtr link) const
 945  {
 946    if (addr == 0) // always accept broadcast
 947      return true;
 948  
 949    C_ITER(i, links)
 950    {
 951      if (i->second == link)
 952        continue;
 953      if (i->second->checkAddress (addr))
 954        return true;
 955    }
 956  
 957    return false;
 958  }
 959  
 960  bool
 961  Router::checkGroupAddress (eibaddr_t addr, LinkConnectPtr link) const
 962  {
 963    if (addr == 0) // always accept broadcast
 964      return true;
 965  
 966    C_ITER(i, links)
 967    {
 968      if (i->second == link)
 969        continue;
 970      if (i->second->checkGroupAddress (addr))
 971        return true;
 972    }
 973  
 974    return false;
 975  }
 976  
 977  eibaddr_t
 978  Router::get_client_addr (TracePtr t)
 979  {
 980    /*
 981     * Start allocating after the last-assigned address.
 982     * This leaves a buffer for delayed replies so that they don't get sent
 983     * to a new client.
 984     *
 985     * client_addrs_pos is set to len-1 in set_client_block() so that allocation
 986     * still starts at the first free address when starting up.
 987     */
 988    for (int i = 1; i <= client_addrs_len; i++)
 989      {
 990        unsigned int pos = (client_addrs_pos + i) % client_addrs_len;
 991        if (client_addrs[pos])
 992          continue;
 993        eibaddr_t a = client_addrs_start + pos;
 994        LinkConnectPtr link = nullptr;
 995        if (a != addr && !hasAddress (a, link, true))
 996          {
 997            TRACEPRINTF (t, 3, "Allocate %s", FormatEIBAddr (a));
 998            /* remember for next pass */
 999            client_addrs_pos = pos;
1000            client_addrs[pos] = true;
1001            return a;
1002          }
1003      }
1004  
1005    /* no more … */
1006    ERRORPRINTF (t, E_WARNING | 59, "Allocate: no more free addresses!");
1007    return 0;
1008  }
1009  
1010  void
1011  Router::release_client_addr(eibaddr_t addr)
1012  {
1013    if (addr < client_addrs_start)
1014      {
1015        ERRORPRINTF (t, E_ERROR | 3, "Release BAD1 %s", FormatEIBAddr (addr));
1016        return;
1017      }
1018    int pos = addr - client_addrs_start;
1019    if (pos >= client_addrs_len)
1020      {
1021        ERRORPRINTF (t, E_ERROR | 95, "Release BAD2 %s", FormatEIBAddr (addr));
1022        return;
1023      }
1024    if (!client_addrs[pos])
1025      {
1026        ERRORPRINTF (t, E_ERROR | 96, "Release free addr %s", FormatEIBAddr (addr));
1027        return;
1028      }
1029  
1030    TRACEPRINTF (t, 3, "Release %s", FormatEIBAddr (addr));
1031    client_addrs[pos] = false;
1032  }
1033  
1034  void
1035  Router::trigger_cb (ev::async &, int)
1036  {
1037    while (!buf.empty() && low_send_more)
1038      {
1039        LDataPtr l1 = buf.get ();
1040  
1041        if (vbusmonitor.size())
1042          {
1043            LBusmonPtr l2 = LBusmonPtr(new L_Busmon_PDU ());
1044            l2->lpdu.set (L_Data_to_CM_TP1 (l1));
1045  
1046            ITER(i,vbusmonitor)
1047            i->cb->send_L_Busmonitor (LBusmonPtr(new L_Busmon_PDU (*l2)));
1048          }
1049        if (!l1->hop_count)
1050          {
1051            TRACEPRINTF (t, 3, "Hopcount zero: %s", l1->Decode (t));
1052            goto next;
1053          }
1054        if (l1->hop_count < 7 || !force_broadcast)
1055          l1->hop_count--;
1056  
1057        if (l1->repeated)
1058          {
1059            CArray d1 = L_Data_to_CM_TP1 (l1);
1060            ITER (i,ignore)
1061            if (d1 == i->data)
1062              {
1063                TRACEPRINTF (t, 9, "Drop: %s", l1->Decode (t));
1064                goto next;
1065              }
1066          }
1067        l1->repeated = 1;
1068        ignore.push_back((IgnoreInfo)
1069        {
1070          .data = L_Data_to_CM_TP1 (l1), .end = getTime () + 1000000
1071        });
1072        l1->repeated = 0;
1073  
1074        if (l1->address_type == IndividualAddress
1075            && l1->destination_address == this->addr)
1076          l1->destination_address = 0;
1077  
1078        low_send_more = false;
1079        r_low->send_L_Data(std::move(l1));
1080  next:
1081        ;
1082      }
1083  
1084    if (!low_send_more)
1085      TRACEPRINTF (t, 6, "wait L");
1086  
1087    // Timestamps are ordered, so we scan for the first
1088    timestamp_t tm = getTime ();
1089    ITER (i, ignore)
1090    if (i->end >= tm)
1091      {
1092        ignore.erase (ignore.begin(), i);
1093        break;
1094      }
1095  }
1096  
1097  bool
1098  Router::has_send_more(LinkConnectPtr i)
1099  {
1100    if (i->send_more)
1101      return true;
1102    ERRORPRINTF (i->t, E_FATAL | 106, "internal error: send_more is not set");
1103    return false;
1104  }
1105  
1106  void
1107  Router::send_L_Data(LDataPtr l1)
1108  {
1109    assert(high_send_more);
1110    high_sending = true;
1111    high_send_more = false;
1112  
1113    auto source = l1->source;
1114    l1->source = nullptr;
1115  
1116    if (l1->address_type == GroupAddress)
1117      {
1118        // This is easy: send to all other L2 which subscribe to the
1119        // group.
1120        ITER(i, links)
1121        {
1122          auto ii = i->second;
1123          if (ii->state != L_up)
1124            continue;
1125          if ((l1->source_address == 0xFFFF) // programming
1126               ? &*ii == source
1127               : ii->hasAddress(l1->source_address))
1128            continue; // don't return to same interface
1129          if(!has_send_more(ii))
1130            continue; // internal error if not
1131          if (ii->checkGroupAddress(l1->destination_address))
1132            ii->send_L_Data (LDataPtr(new L_Data_PDU (*l1)));
1133        }
1134      }
1135    else if (l1->address_type == IndividualAddress)
1136      {
1137        // we want to send to the interface on which the destination address
1138        // has appeared. If it hasn't been seen yet, we send to all
1139        // interfaces.
1140        // Address ~0 is special; it's used for programming
1141        // so can be on different interfaces. Always broadcast these.
1142        bool found = (l1->destination_address == this->addr);
1143        if (l1->destination_address != 0xFFFF)
1144          ITER(i, links)
1145          {
1146            auto ii = i->second;
1147            if (ii->hasAddress (l1->source_address))
1148              continue;
1149            if (ii->hasAddress (l1->destination_address))
1150              {
1151                found = true;
1152                break;
1153              }
1154          }
1155        ITER (i, links)
1156        {
1157          auto ii = i->second;
1158          if (ii->state != L_up)
1159            continue;
1160          if (ii->hasAddress (l1->source_address))
1161            continue; // don't return to same interface
1162          if(!has_send_more(ii))
1163            continue; // internal error if not
1164          if (l1->hop_count == 7 || found ? ii->hasAddress (l1->destination_address) : ii->checkAddress (l1->destination_address))
1165            i->second->send_L_Data (LDataPtr(new L_Data_PDU (*l1)));
1166        }
1167      }
1168    high_sending = false;
1169    send_Next(); // check readiness
1170  }
1171  
1172  void
1173  Router::mtrigger_cb (ev::async &, int)
1174  {
1175    while (!mbuf.empty())
1176      {
1177        LBusmonPtr l1 = mbuf.get ();
1178  
1179        TRACEPRINTF (t, 3, "RecvMon %s", l1->Decode (t));
1180        ITER (i, busmonitor)
1181        i->cb->send_L_Busmonitor (LBusmonPtr(new L_Busmon_PDU (*l1)));
1182      }
1183  }
1184  
1185  bool
1186  Router::checkStack(IniSectionPtr& cfg)
1187  {
1188    LinkConnectPtr conn = LinkConnectPtr(new LinkConnect(*this, cfg, t));
1189    DriverPtr dummy = DriverPtr(drivers.create("dummy",conn,cfg));
1190    if (dummy == nullptr)
1191      return false;
1192    conn->set_driver(dummy);
1193    if (!conn->setup())
1194      return false;
1195    return true;
1196  }
1197  
1198  bool
1199  Router::hasClientAddrs(bool complain) const
1200  {
1201    if (client_addrs_len > 0)
1202      return true;
1203    if (complain)
1204      ERRORPRINTF (t, E_ERROR | 51, "You need a client-addrs=X.Y.Z:N option in your %s section.", main);
1205    return false;
1206  
1207  }
1208  
1209  void
1210  RouterHigh::started()
1211  {
1212    if (is_started)
1213      return;
1214    is_started = true;
1215    Driver::started();
1216  }
1217  
1218  void
1219  RouterHigh::stopped(bool err)
1220  {
1221    is_started = false;
1222    Driver::stopped(err);
1223  }
1224  
1225  void
1226  RouterHigh::recv_L_Data (LDataPtr l)
1227  {
1228    auto r = recv.lock();
1229    if (r != nullptr)
1230      r->recv_L_Data(std::move(l));
1231  }
1232  
1233  void
1234  RouterHigh::recv_L_Busmonitor (LBusmonPtr l)
1235  {
1236    auto r = recv.lock();
1237    if (r != nullptr)
1238      r->recv_L_Busmonitor(std::move(l));
1239  }
1240  
1241  RouterHigh::RouterHigh(Router& r, const RouterLowPtr& rl)
1242    : Driver(std::dynamic_pointer_cast<LinkConnect_>(rl), r.ini[r.main]), router(&r)
1243  {
1244    t->setAuxName("H");
1245  }
1246  
1247  RouterLow::RouterLow(Router& r)
1248    : LinkConnect_(r, r.ini[r.main], r.t), router(&r)
1249  {
1250    t->setAuxName("L");
1251  }
1252  
1253  void
1254  RouterLow::send_Next()
1255  {
1256    router->low_send_more = true;
1257    TRACEPRINTF (t, 6, "OK L");
1258    router->trigger.send();
1259  }