void TPUARTwrap::send_L_Data (LDataPtr l) { assert(out.size() == 0); out = l->ToPacket (); send_again(); }
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; }