void EIBNetIPRouter::Run (pth_sem_t * stop1) { pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1); while (pth_event_status (stop) != PTH_STATUS_OCCURRED) { EIBNetIPPacket *p = sock->Get (stop); if (p) { if (p->service != ROUTING_INDICATION) { delete p; continue; } if (p->data () < 2 || p->data[0] != 0x29) { if (p->data () < 2) { TRACEPRINTF (t, 2, this, "No payload (%d)", p->data ()); } else { TRACEPRINTF (t, 2, this, "Payload not L_Data.ind (%02x)", p->data[0]); } delete p; continue; } const CArray data = p->data; delete p; L_Data_PDU *c = CEMI_to_L_Data (data, this); if (c) { TRACEPRINTF (t, 2, this, "Recv %s", c->Decode ()()); if (mode & BUSMODE_UP) { if (mode == BUSMODE_VMONITOR) { L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU (this); l2->pdu.set (c->ToPacket ()); l3->recv_L_Data (l2); } l3->recv_L_Data (c); continue; } L_Busmonitor_PDU *p1 = new L_Busmonitor_PDU (this); p1->pdu = c->ToPacket (); delete c; l3->recv_L_Data (p1); continue; } } } pth_event_free (stop, PTH_FREE_THIS); }
void EIBNetIPRouter::read_cb(EIBNetIPPacket *p) { if (p->service != ROUTING_INDICATION) { delete p; return; } if (p->data.size() < 2 || p->data[0] != 0x29) { if (p->data.size() < 2) { TRACEPRINTF (t, 2, "No payload (%d)", p->data.size()); } else { TRACEPRINTF (t, 2, "Payload not L_Data.ind (%02x)", p->data[0]); } delete p; return; } LDataPtr c = CEMI_to_L_Data (p->data, t); delete p; if (c) { if (!monitor) recv_L_Data (std::move(c)); else { LBusmonPtr p1 = LBusmonPtr(new L_Busmonitor_PDU ()); p1->pdu = c->ToPacket (); recv_L_Busmonitor (std::move(p1)); } } }
void EIBNetIPTunnel::read_cb (EIBNetIPPacket *p1) { LDataPtr c; switch (p1->service) { case CONNECTION_RESPONSE: { EIBnet_ConnectResponse cresp; if (mod) goto err; if (parseEIBnet_ConnectResponse (*p1, cresp)) { TRACEPRINTF (t, 1, "Recv wrong connection response"); break; } if (cresp.status != 0) { TRACEPRINTF (t, 1, "Connect failed with error %02X", cresp.status); if (cresp.status == 0x23 && support_busmonitor && monitor) { TRACEPRINTF (t, 1, "Disable busmonitor support"); restart(); return; // support_busmonitor = false; // connect_busmonitor = false; // EIBnet_ConnectRequest creq = get_creq(); // creq.CRI[1] = 0x02; // EIBNetIPPacket p = creq.ToPacket (); // TRACEPRINTF (t, 1, "Connectretry"); // sock->Send (p, caddr); // conntimeout.start(10,0); } break; } if (cresp.CRD.size() != 3) { TRACEPRINTF (t, 1, "Recv wrong connection response"); break; } auto cn = std::dynamic_pointer_cast<LinkConnect>(conn.lock()); if (cn != nullptr) cn->setAddress((cresp.CRD[1] << 8) | cresp.CRD[2]); auto f = findFilter("single"); if (f != nullptr) std::dynamic_pointer_cast<NatL2Filter>(f)->setAddress((cresp.CRD[1] << 8) | cresp.CRD[2]); // TODO else reject daddr = cresp.daddr; if (!cresp.nat) { if (NAT) { daddr.sin_addr = caddr.sin_addr; if (dataport != 0) daddr.sin_port = htons (dataport); } } channel = cresp.channel; mod = 1; trigger.send(); sno = 0; rno = 0; sock->recvaddr2 = daddr; sock->recvall = 3; if (heartbeat_time) conntimeout.start(heartbeat_time,0); heartbeat = 0; BusDriver::start(); break; } case TUNNEL_REQUEST: { EIBnet_TunnelRequest treq; if (mod == 0) { TRACEPRINTF (t, 1, "Not connected"); goto err; } if (parseEIBnet_TunnelRequest (*p1, treq)) { TRACEPRINTF (t, 1, "Invalid request"); break; } if (treq.channel != channel) { TRACEPRINTF (t, 1, "Not for us (treq.chan %d != %d)", treq.channel,channel); break; } if (((treq.seqno + 1) & 0xff) == rno) { EIBnet_TunnelACK tresp; tresp.status = 0; tresp.channel = channel; tresp.seqno = treq.seqno; EIBNetIPPacket p = tresp.ToPacket (); sock->Send (p, daddr); sock->recvall = 0; break; } if (treq.seqno != rno) { TRACEPRINTF (t, 1, "Wrong sequence %d<->%d", treq.seqno, rno); if (treq.seqno < rno) treq.seqno += 0x100; if (treq.seqno >= rno + 5) restart(); break; } rno++; if (rno > 0xff) rno = 0; EIBnet_TunnelACK tresp; tresp.status = 0; tresp.channel = channel; tresp.seqno = treq.seqno; EIBNetIPPacket p = tresp.ToPacket (); sock->Send (p, daddr); //Confirmation if (treq.CEMI[0] == 0x2E) { if (mod == 3) { mod = 1; trigger.send(); } break; } if (treq.CEMI[0] == 0x2B) { LBusmonPtr l2 = CEMI_to_Busmonitor (treq.CEMI, std::dynamic_pointer_cast<Driver>(shared_from_this())); recv_L_Busmonitor (std::move(l2)); break; } if (treq.CEMI[0] != 0x29) { TRACEPRINTF (t, 1, "Unexpected CEMI Type %02X", treq.CEMI[0]); break; } c = CEMI_to_L_Data (treq.CEMI, t); if (c) { if (!monitor) recv_L_Data (std::move(c)); else { LBusmonPtr p1 = LBusmonPtr(new L_Busmonitor_PDU ()); p1->pdu = c->ToPacket (); recv_L_Busmonitor (std::move(p1)); } break; } TRACEPRINTF (t, 1, "Unknown CEMI"); break; } case TUNNEL_RESPONSE: { EIBnet_TunnelACK tresp; if (mod == 0) { TRACEPRINTF (t, 1, "Not connected"); goto err; } if (parseEIBnet_TunnelACK (*p1, tresp)) { TRACEPRINTF (t, 1, "Invalid response"); break; } if (tresp.channel != channel) { TRACEPRINTF (t, 1, "Not for us (tresp.chan %d != %d)", tresp.channel,channel); break; } if (tresp.seqno != sno) { TRACEPRINTF (t, 1, "Wrong sequence %d<->%d", tresp.seqno, sno); break; } if (tresp.status) { TRACEPRINTF (t, 1, "Error in ACK %d", tresp.status); break; } if (mod == 2) { sno++; if (sno > 0xff) sno = 0; out.clear(); send_Next(); mod = 1; trigger.send(); retry = 0; } else TRACEPRINTF (t, 1, "Unexpected ACK mod=%d",mod); break; } case CONNECTIONSTATE_RESPONSE: { EIBnet_ConnectionStateResponse csresp; if (parseEIBnet_ConnectionStateResponse (*p1, csresp)) { TRACEPRINTF (t, 1, "Invalid response"); break; } if (csresp.channel != channel) { TRACEPRINTF (t, 1, "Not for us (csresp.chan %d != %d)", csresp.channel,channel); break; } if (csresp.status == 0) { if (heartbeat > 0) { heartbeat = 0; TRACEPRINTF (t, 1, "got Connection State Response"); } else TRACEPRINTF (t, 1, "Duplicate Connection State Response"); } else if (csresp.status == 0x21) { TRACEPRINTF (t, 1, "Connection State Response: not connected"); restart(); } else { TRACEPRINTF (t, 1, "Connection State Response Error %02x", csresp.status); errored(); } break; } case DISCONNECT_REQUEST: { EIBnet_DisconnectRequest dreq; if (mod == 0) { TRACEPRINTF (t, 1, "Not connected"); goto err; } if (parseEIBnet_DisconnectRequest (*p1, dreq)) { TRACEPRINTF (t, 1, "Invalid request"); break; } if (dreq.channel != channel) { TRACEPRINTF (t, 1, "Not for us (dreq.chan %d != %d)", dreq.channel,channel); break; } EIBnet_DisconnectResponse dresp; dresp.channel = channel; dresp.status = 0; EIBNetIPPacket p = dresp.ToPacket (); t->TracePacket (1, "SendDis", p.data); sock->Send (p, caddr); sock->recvall = 0; mod = 0; conntimeout.start(0.1,0); break; } case DISCONNECT_RESPONSE: { EIBnet_DisconnectResponse dresp; if (mod == 0) { TRACEPRINTF (t, 1, "Not connected"); break; } if (parseEIBnet_DisconnectResponse (*p1, dresp)) { TRACEPRINTF (t, 1, "Invalid request"); break; } if (dresp.channel != channel) { TRACEPRINTF (t, 1, "Not for us (dresp.chan %d != %d)", dresp.channel,channel); break; } mod = 0; sock->recvall = 0; TRACEPRINTF (t, 1, "Disconnected"); restart(); conntimeout.start(0.1,0); break; } default: err: TRACEPRINTF (t, 1, "Recv unexpected service %04X", p1->service); } delete p1; }
void EIBNetIPTunnel::Run (pth_sem_t * stop1) { int channel = -1; int mod = 0; int rno = 0; int sno = 0; int retry = 0; int heartbeat = 0; int drop = 0; eibaddr_t myaddr; pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1); pth_event_t input = pth_event (PTH_EVENT_SEM, &insignal); pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (0, 0)); pth_event_t timeout1 = pth_event (PTH_EVENT_RTIME, pth_time (10, 0)); L_Data_PDU *c; EIBNetIPPacket p; EIBNetIPPacket *p1; EIBnet_ConnectRequest creq; creq.nat = saddr.sin_addr.s_addr == 0; EIBnet_ConnectResponse cresp; EIBnet_ConnectionStateRequest csreq; csreq.nat = saddr.sin_addr.s_addr == 0; EIBnet_ConnectionStateResponse csresp; EIBnet_TunnelRequest treq; EIBnet_TunnelACK tresp; EIBnet_DisconnectRequest dreq; dreq.nat = saddr.sin_addr.s_addr == 0; EIBnet_DisconnectResponse dresp; creq.caddr = saddr; creq.daddr = saddr; creq.CRI.resize (3); creq.CRI[0] = 0x04; creq.CRI[1] = 0x02; creq.CRI[2] = 0x00; p = creq.ToPacket (); sock->sendaddr = caddr; sock->Send (p); while (pth_event_status (stop) != PTH_STATUS_OCCURRED) { if (mod == 1) pth_event_concat (stop, input, NULL); if (mod == 2 || mod == 3) pth_event_concat (stop, timeout, NULL); pth_event_concat (stop, timeout1, NULL); p1 = sock->Get (stop); pth_event_isolate (stop); pth_event_isolate (timeout); pth_event_isolate (timeout1); if (p1) { switch (p1->service) { case CONNECTION_RESPONSE: if (mod) goto err; if (parseEIBnet_ConnectResponse (*p1, cresp)) { TRACEPRINTF (t, 1, this, "Recv wrong connection response"); break; } if (cresp.status != 0) { TRACEPRINTF (t, 1, this, "Connect failed with error %02X", cresp.status); if (cresp.status == 0x23 && support_busmonitor == 1 && connect_busmonitor == 1) { TRACEPRINTF (t, 1, this, "Disable busmonitor support"); support_busmonitor = 0; connect_busmonitor = 0; creq.CRI[1] = 0x02; pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1, pth_time (10, 0)); p = creq.ToPacket (); TRACEPRINTF (t, 1, this, "Connectretry"); sock->sendaddr = caddr; sock->Send (p); } break; } if (cresp.CRD () != 3) { TRACEPRINTF (t, 1, this, "Recv wrong connection response"); break; } myaddr = (cresp.CRD[1] << 8) | cresp.CRD[2]; daddr = cresp.daddr; if (!cresp.nat) { if (NAT) daddr.sin_addr = caddr.sin_addr; if (dataport != -1) daddr.sin_port = htons (dataport); } channel = cresp.channel; mod = 1; sno = 0; rno = 0; sock->recvaddr2 = daddr; sock->recvall = 3; pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1, pth_time (30, 0)); heartbeat = 0; break; case TUNNEL_REQUEST: if (mod == 0) { TRACEPRINTF (t, 1, this, "Not connected"); goto err; } if (parseEIBnet_TunnelRequest (*p1, treq)) { TRACEPRINTF (t, 1, this, "Invalid request"); break; } if (treq.channel != channel) { TRACEPRINTF (t, 1, this, "Not for us"); break; } if (((treq.seqno + 1) & 0xff) == rno) { tresp.status = 0; tresp.channel = channel; tresp.seqno = treq.seqno; p = tresp.ToPacket (); sock->sendaddr = daddr; sock->Send (p); sock->recvall = 0; break; } if (treq.seqno != rno) { TRACEPRINTF (t, 1, this, "Wrong sequence %d<->%d", treq.seqno, rno); if (treq.seqno < rno) treq.seqno += 0x100; if (treq.seqno >= rno + 5) { dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; sock->Send (p); sock->recvall = 0; mod = 0; } break; } rno++; if (rno > 0xff) rno = 0; tresp.status = 0; tresp.channel = channel; tresp.seqno = treq.seqno; p = tresp.ToPacket (); sock->sendaddr = daddr; sock->Send (p); //Confirmation if (treq.CEMI[0] == 0x2E) { if (mod == 3) mod = 1; break; } if (treq.CEMI[0] == 0x2B) { L_Busmonitor_PDU *l2 = CEMI_to_Busmonitor (treq.CEMI); outqueue.put (l2); pth_sem_inc (&outsignal, 1); break; } if (treq.CEMI[0] != 0x29) { TRACEPRINTF (t, 1, this, "Unexpected CEMI Type %02X", treq.CEMI[0]); break; } c = CEMI_to_L_Data (treq.CEMI); if (c) { TRACEPRINTF (t, 1, this, "Recv %s", c->Decode ()()); if (mode == 0) { if (vmode) { L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU; l2->pdu.set (c->ToPacket ()); outqueue.put (l2); pth_sem_inc (&outsignal, 1); } if (c->AddrType == IndividualAddress && c->dest == myaddr) c->dest = 0; outqueue.put (c); pth_sem_inc (&outsignal, 1); break; } L_Busmonitor_PDU *p1 = new L_Busmonitor_PDU; p1->pdu = c->ToPacket (); delete c; outqueue.put (p1); pth_sem_inc (&outsignal, 1); break; } TRACEPRINTF (t, 1, this, "Unknown CEMI"); break; case TUNNEL_RESPONSE: if (mod == 0) { TRACEPRINTF (t, 1, this, "Not connected"); goto err; } if (parseEIBnet_TunnelACK (*p1, tresp)) { TRACEPRINTF (t, 1, this, "Invalid response"); break; } if (tresp.channel != channel) { TRACEPRINTF (t, 1, this, "Not for us"); break; } if (tresp.seqno != sno) { TRACEPRINTF (t, 1, this, "Wrong sequence %d<->%d", tresp.seqno, sno); break; } if (tresp.status) { TRACEPRINTF (t, 1, this, "Error in ACK %d", tresp.status); break; } if (mod == 2) { sno++; if (sno > 0xff) sno = 0; pth_sem_dec (&insignal); inqueue.get (); if (noqueue) { mod = 3; pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout, pth_time (1, 0)); } else mod = 1; retry = 0; drop = 0; } else TRACEPRINTF (t, 1, this, "Unexpected ACK"); break; case CONNECTIONSTATE_RESPONSE: if (parseEIBnet_ConnectionStateResponse (*p1, csresp)) { TRACEPRINTF (t, 1, this, "Invalid response"); break; } if (csresp.channel != channel) { TRACEPRINTF (t, 1, this, "Not for us"); break; } if (csresp.status == 0) { if (heartbeat > 0) heartbeat--; else TRACEPRINTF (t, 1, this, "Duplicate Connection State Response"); } else if (csresp.status == 0x21) { TRACEPRINTF (t, 1, this, "Connection State Response not connected"); dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; sock->Send (p); sock->recvall = 0; mod = 0; } else TRACEPRINTF (t, 1, this, "Connection State Response Error %02x", csresp.status); break; case DISCONNECT_REQUEST: if (mod == 0) { TRACEPRINTF (t, 1, this, "Not connected"); goto err; } if (parseEIBnet_DisconnectRequest (*p1, dreq)) { TRACEPRINTF (t, 1, this, "Invalid request"); break; } if (dreq.channel != channel) { TRACEPRINTF (t, 1, this, "Not for us"); break; } dresp.channel = channel; dresp.status = 0; p = dresp.ToPacket (); t->TracePacket (1, this, "SendDis", p.data); sock->sendaddr = caddr; sock->Send (p); sock->recvall = 0; mod = 0; break; case DISCONNECT_RESPONSE: if (mod == 0) { TRACEPRINTF (t, 1, this, "Not connected"); break; } if (parseEIBnet_DisconnectResponse (*p1, dresp)) { TRACEPRINTF (t, 1, this, "Invalid request"); break; } if (dresp.channel != channel) { TRACEPRINTF (t, 1, this, "Not for us"); break; } mod = 0; sock->recvall = 0; TRACEPRINTF (t, 1, this, "Disconnected"); pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1, pth_time (0, 100)); break; default: err: TRACEPRINTF (t, 1, this, "Recv unexpected service %04X", p1->service); } delete p1; } if (mod == 2 && pth_event_status (timeout) == PTH_STATUS_OCCURRED) { mod = 1; retry++; if (retry > 3) { TRACEPRINTF (t, 1, this, "Drop"); pth_sem_dec (&insignal); inqueue.get (); retry = 0; drop++; if (drop >= 3) { dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; sock->Send (p); sock->recvall = 0; mod = 0; } } } if (mod == 3 && pth_event_status (timeout) == PTH_STATUS_OCCURRED) mod = 1; if (mod != 0 && pth_event_status (timeout1) == PTH_STATUS_OCCURRED) { pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1, pth_time (30, 0)); if (heartbeat < 5) { csreq.caddr = saddr; csreq.channel = channel; p = csreq.ToPacket (); TRACEPRINTF (t, 1, this, "Heartbeat"); sock->sendaddr = caddr; sock->Send (p); heartbeat++; } else { TRACEPRINTF (t, 1, this, "Disconnection because of errors"); dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; if (channel != -1) sock->Send (p); sock->recvall = 0; mod = 0; } } if (mod == 0 && pth_event_status (timeout1) == PTH_STATUS_OCCURRED) { pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout1, pth_time (10, 0)); creq.CRI[1] = ((connect_busmonitor && support_busmonitor) ? 0x80 : 0x02); p = creq.ToPacket (); TRACEPRINTF (t, 1, this, "Connectretry"); sock->sendaddr = caddr; sock->Send (p); } if (!inqueue.isempty () && inqueue.top ()() == 0) { pth_sem_dec (&insignal); inqueue.get (); if (support_busmonitor) { dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; sock->Send (p); } } if (!inqueue.isempty () && mod == 1) { treq.channel = channel; treq.seqno = sno; treq.CEMI = inqueue.top (); p = treq.ToPacket (); t->TracePacket (1, this, "SendTunnel", p.data); sock->sendaddr = daddr; sock->Send (p); mod = 2; pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout, pth_time (1, 0)); } } out: dreq.caddr = saddr; dreq.channel = channel; p = dreq.ToPacket (); sock->sendaddr = caddr; if (channel != -1) sock->Send (p); pth_event_free (stop, PTH_FREE_THIS); pth_event_free (input, PTH_FREE_THIS); pth_event_free (timeout, PTH_FREE_THIS); pth_event_free (timeout1, PTH_FREE_THIS); }