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);
}