void task_sleep (task_ticks_t ticks)
{
#ifdef PTHDEBUG2
printf ("task_sleep(%d)\n", ticks);
#endif
pth_nap (pth_time (0, ticks * PTH_USECS_PER_TICK));
}
APDU *
Layer7_Individual::Request_Response (APDU * r)
{
APDU *a;
CArray *c;
l4->Send (r->ToPacket ());
pth_event_t t = pth_event (PTH_EVENT_RTIME, pth_time (6, 100));
while (pth_event_status (t) != PTH_STATUS_OCCURRED)
{
c = l4->Get (t);
if (c)
{
if (c->len () == 0)
{
delete c;
pth_event_free (t, PTH_FREE_THIS);
return 0;
}
a = APDU::fromPacket (*c, this->t);
delete c;
if (a->isResponse (r))
{
pth_event_free (t, PTH_FREE_THIS);
return a;
}
delete a;
pth_event_free (t, PTH_FREE_THIS);
return 0;
}
}
pth_event_free (t, PTH_FREE_THIS);
return 0;
}
Array < eibaddr_t >
Layer7_Broadcast::A_IndividualAddress_Read (unsigned timeout)
{
Array < eibaddr_t > addrs;
A_IndividualAddress_Read_PDU r;
APDU *a;
l4->Send (r.ToPacket ());
pth_event_t t = pth_event (PTH_EVENT_RTIME, pth_time (timeout, 0));
while (pth_event_status (t) != PTH_STATUS_OCCURRED)
{
BroadcastComm *c = l4->Get (t);
if (c)
{
a = APDU::fromPacket (c->data);
if (a->isResponse (&r))
{
addrs.resize (addrs () + 1);
addrs[addrs () - 1] = c->src;
}
delete a;
delete c;
}
}
pth_event_free (t, PTH_FREE_THIS);
return addrs;
}
Array < eibaddr_t > GroupCache::LastUpdates (uint16_t start, uint8_t Timeout,
uint16_t & end, pth_event_t stop)
{
Array < eibaddr_t > a;
pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (Timeout, 0));
do
{
if (pos < 0x100)
{
if (pos < start && start < ((pos - 0x100) & 0xffff))
start = (pos - 0x100) & 0xffff;
}
else
{
if (start < ((pos - 0x100) & 0xffff) || start > pos)
start = (pos - 0x100) & 0xffff;
}
TRACEPRINTF (t, 8, this, "LastUpdates start: %d pos: %d", start, pos);
while (start != pos && !updates[start & 0xff])
start++;
if (start != pos)
{
while (start != pos)
{
if (updates[start & 0xff])
a.add (updates[start & 0xff]);
start++;
}
end = pos;
pth_event_free (timeout, PTH_FREE_THIS);
return a;
}
if (pth_event_status (timeout) == PTH_STATUS_OCCURRED)
{
end = pos;
pth_event_free (timeout, PTH_FREE_THIS);
return a;
}
pth_event_concat (timeout, stop, NULL);
pth_mutex_acquire (&mutex, 0, 0);
pth_cond_await (&cond, &mutex, timeout);
pth_mutex_release (&mutex);
pth_event_isolate (timeout);
}
while (1);
}
/*!
* \brief close connection with eibnetmux socket server
*
* \param conn connection handle as returned by enmx_open()
*/
void enmx_close( ENMX_HANDLE conn )
{
sConnectionInfo **connInfo, *temp;
SOCKET_CMD_HEAD cmd_head;
pth_event_t ev_wakeup;
time_t secs;
temp = NULL;
for( connInfo = &enmx_connections; *connInfo != NULL; connInfo = &(*connInfo)->next ) {
if( (*connInfo)->socket == conn ) {
temp = *connInfo;
*connInfo = (*connInfo)->next;
break;
}
}
if( temp != NULL ) {
cmd_head.cmd = SOCKET_CMD_EXIT;
cmd_head.address = 0xffff;
secs = time( NULL ) + TIMEOUT;
switch( temp->mode ) {
case ENMX_MODE_STANDARD:
while( write( conn, &cmd_head, sizeof( cmd_head )) == -1 ) {
if( errno == EAGAIN ) {
if( secs <= time( NULL )) {
break;
}
usleep( 10 * 1000 );
continue;
}
}
break;
case ENMX_MODE_PTH:
ev_wakeup = pth_event( PTH_EVENT_TIME, pth_time( secs, 0 ));
pth_write_ev( conn, &cmd_head, sizeof( cmd_head ), ev_wakeup );
// either the request has been sent or timeout reached
// in any case, we've finished
pth_event_free( ev_wakeup, PTH_FREE_ALL );
break;
}
if( temp->hostname ) free( temp->hostname );
if( temp->name ) free( temp->name );
free( temp );
}
close( conn );
}
int
glthread_cond_timedwait_multithreaded (gl_cond_t *cond,
gl_lock_t *lock,
struct timespec *abstime)
{
int ret, status;
pth_event_t ev;
ev = pth_event (PTH_EVENT_TIME, pth_time (abstime->tv_sec, abstime->tv_nsec / 1000));
ret = pth_cond_await (cond, lock, ev);
status = pth_event_status (ev);
pth_event_free (ev, PTH_FREE_THIS);
if (status == PTH_STATUS_OCCURRED)
return ETIMEDOUT;
return ret;
}
GroupCacheEntry
GroupCache::Read (eibaddr_t addr, unsigned Timeout, uint16_t age)
{
TRACEPRINTF (t, 4, this, "GroupCacheRead %s %d %d",
FormatGroupAddr (addr)(), Timeout, age);
bool rm = false;
GroupCacheEntry *c;
if (!enable)
{
GroupCacheEntry f;
f.src = 0;
f.dst = 0;
TRACEPRINTF (t, 4, this, "GroupCache not enabled");
return f;
}
c = find (addr);
if (c && age && c->recvtime + age < time (0))
rm = true;
if (c && !rm)
{
TRACEPRINTF (t, 4, this, "GroupCache found: %s",
FormatEIBAddr (c->src)());
return *c;
}
if (!Timeout)
{
GroupCacheEntry f;
f.src = 0;
f.dst = addr;
TRACEPRINTF (t, 4, this, "GroupCache no entry");
return f;
}
A_GroupValue_Read_PDU apdu;
T_DATA_XXX_REQ_PDU tpdu;
L_Data_PDU *l;
pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (Timeout, 0));
tpdu.data = apdu.ToPacket ();
l = new L_Data_PDU (FakeL2);
l->data = tpdu.ToPacket ();
l->source = 0;
l->dest = addr;
l->AddrType = GroupAddress;
layer3->send_L_Data (l);
do
{
c = find (addr);
rm = false;
if (c && age && c->recvtime + age < time (0))
rm = true;
if (c && !rm)
{
TRACEPRINTF (t, 4, this, "GroupCache found: %s",
FormatEIBAddr (c->src)());
pth_event_free (timeout, PTH_FREE_THIS);
return *c;
}
if (pth_event_status (timeout) == PTH_STATUS_OCCURRED && c)
{
GroupCacheEntry gc;
gc.src = 0;
gc.dst = addr;
TRACEPRINTF (t, 4, this, "GroupCache reread timeout");
pth_event_free (timeout, PTH_FREE_THIS);
return gc;
}
if (pth_event_status (timeout) == PTH_STATUS_OCCURRED)
{
c = new GroupCacheEntry;
c->src = 0;
c->dst = addr;
c->recvtime = time (0);
add (c);
TRACEPRINTF (t, 4, this, "GroupCache timeout");
pth_event_free (timeout, PTH_FREE_THIS);
return *c;
}
pth_mutex_acquire (&mutex, 0, 0);
pth_cond_await (&cond, &mutex, timeout);
pth_mutex_release (&mutex);
}
while (1);
}
void
BCU1SerialLowLevelDriver::Run (pth_sem_t * stop1)
{
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
pth_event_t input = pth_event (PTH_EVENT_SEM, &in_signal);
pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (0, 10));
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
int error;
timeout =
pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
pth_time (0, 150));
pth_event_concat (stop, input, timeout, NULL);
pth_wait (stop);
pth_event_isolate (stop);
pth_event_isolate (input);
timeout =
pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
pth_time (0, 200));
pth_event_concat (stop, timeout, NULL);
struct timeval v1, v2;
gettimeofday (&v1, 0);
CArray e;
CArray r;
uchar s;
if (!inqueue.isempty ())
{
const CArray & c = inqueue.top ();
e.resize (c () + 1);
s = c () & 0x1f;
s |= 0x20;
s |= 0x80 * bitcount (s);
e[0] = s;
e.setpart (c, 1);
}
else
{
e.resize (1);
e[0] = 0xff;
}
if (!startsync ())
{
error = 1;
goto err;
}
if (!exchange (e[0], s, stop))
{
error = 3;
goto err;
}
if (!endsync ())
{
error = 2;
goto err;
}
if (s == 0xff && e[0] != 0xff)
{
for (unsigned i = 1; i < e (); i++)
{
if (!startsync ())
{
error = 1;
goto err;
}
if (!exchange (e[i], s, stop))
{
error = 3;
goto err;
}
if (endsync ())
{
error = 2;
goto err;
}
}
if (s != 0x00)
{
error = 10;
goto err;
}
inqueue.get ();
TRACEPRINTF (t, 0, this, "Sent");
pth_sem_dec (&in_signal);
if (inqueue.isempty ())
pth_sem_set_value (&send_empty, 1);
}
else if (s != 0xff)
{
r.resize ((s & 0x1f));
for (unsigned i = 0; i < (s & 0x1f); i++)
{
if (!startsync ())
{
error = 1;
goto err;
}
if (!exchange (0, r[i], stop))
{
error = 3;
goto err;
}
if (!endsync ())
{
error = 2;
goto err;
}
}
TRACEPRINTF (t, 0, this, "Recv");
outqueue.put (new CArray (r));
pth_sem_inc (&out_signal, 1);
}
gettimeofday (&v2, 0);
TRACEPRINTF (t, 1, this, "Recvtime: %d",
v2.tv_sec * 1000000L + v2.tv_usec -
(v1.tv_sec * 1000000L + v1.tv_usec));
if (0)
{
err:
gettimeofday (&v2, 0);
TRACEPRINTF (t, 1, this, "ERecvtime: %d",
v2.tv_sec * 1000000L + v2.tv_usec -
(v1.tv_sec * 1000000L + v1.tv_usec));
setstat (getstat () & ~(TIOCM_RTS | TIOCM_CTS));
pth_usleep (2000);
while ((getstat () & TIOCM_CTS));
TRACEPRINTF (t, 0, this, "Restart %d", error);
}
pth_event_isolate (timeout);
}
pth_event_free (timeout, PTH_FREE_THIS);
pth_event_free (stop, PTH_FREE_THIS);
pth_event_free (input, PTH_FREE_THIS);
}
void
FT12LowLevelDriver::Run (pth_sem_t * stop1)
{
CArray last;
int i;
uchar buf[255];
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
pth_event_t input = pth_event (PTH_EVENT_SEM, &in_signal);
pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (0, 100000));
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
pth_event_isolate (input);
pth_event_isolate (timeout);
if (mode == 0)
pth_event_concat (stop, input, NULL);
if (mode == 1)
pth_event_concat (stop, timeout, NULL);
i = pth_read_ev (fd, buf, sizeof (buf), stop);
if (i > 0)
{
t->TracePacket (0, this, "Recv", i, buf);
akt.setpart (buf, akt (), i);
}
while (akt.len () > 0)
{
if (akt[0] == 0xE5 && mode == 1)
{
pth_sem_dec (&in_signal);
inqueue.get ();
if (inqueue.isempty ())
pth_sem_set_value (&send_empty, 1);
akt.deletepart (0, 1);
mode = 0;
repeatcount = 0;
}
else if (akt[0] == 0x10)
{
if (akt () < 4)
break;
if (akt[1] == akt[2] && akt[3] == 0x16)
{
uchar c1 = 0xE5;
t->TracePacket (0, this, "Send Ack", 1, &c1);
write (fd, &c1, 1);
if ((akt[1] == 0xF3 && !recvflag) ||
(akt[1] == 0xD3 && recvflag))
{
//right sequence number
recvflag = !recvflag;
}
if ((akt[1] & 0x0f) == 0)
{
const uchar reset[1] = { 0xA0 };
CArray *c = new CArray (reset, sizeof (reset));
t->TracePacket (0, this, "RecvReset", *c);
outqueue.put (c);
pth_sem_inc (&out_signal, TRUE);
}
}
akt.deletepart (0, 4);
}
else if (akt[0] == 0x68)
{
int len;
uchar c1;
if (akt () < 7)
break;
if (akt[1] != akt[2] || akt[3] != 0x68)
{
//receive error, try to resume
akt.deletepart (0, 1);
continue;
}
if (akt () < akt[1] + 6)
break;
c1 = 0;
for (i = 4; i < akt[1] + 4; i++)
c1 += akt[i];
if (akt[akt[1] + 4] != c1 || akt[akt[1] + 5] != 0x16)
{
len = akt[1] + 6;
//Forget wrong short frame
akt.deletepart (0, len);
continue;
}
c1 = 0xE5;
t->TracePacket (0, this, "Send Ack", 1, &c1);
i = write (fd, &c1, 1);
if ((akt[4] == 0xF3 && recvflag) ||
(akt[4] == 0xD3 && !recvflag))
{
if (CArray (akt.array () + 5, akt[1] - 1) != last)
{
TRACEPRINTF (t, 0, this, "Sequence jump");
recvflag = !recvflag;
}
else
TRACEPRINTF (t, 0, this, "Wrong Sequence");
}
if ((akt[4] == 0xF3 && !recvflag) ||
(akt[4] == 0xD3 && recvflag))
{
recvflag = !recvflag;
CArray *c = new CArray;
len = akt[1] + 6;
c->setpart (akt.array () + 5, 0, len - 7);
last = *c;
outqueue.put (c);
pth_sem_inc (&out_signal, TRUE);
}
akt.deletepart (0, len);
}
else
//Forget unknown byte
akt.deletepart (0, 1);
}
if (mode == 1 && pth_event_status (timeout) == PTH_STATUS_OCCURRED)
mode = 0;
if (mode == 0 && !inqueue.isempty ())
{
const CArray & c = inqueue.top ();
t->TracePacket (0, this, "Send", c);
repeatcount++;
i = pth_write_ev (fd, c.array (), c (), stop);
if (i == c ())
{
mode = 1;
timeout =
pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
pth_time (0, 100000));
}
}
}
pth_event_free (stop, PTH_FREE_THIS);
pth_event_free (timeout, PTH_FREE_THIS);
pth_event_free (input, PTH_FREE_THIS);
}
void
USBLoop::Run (pth_sem_t * stop1)
{
fd_set r, w, e;
int rc, fds, i;
struct timeval tv, tv1;
const struct libusb_pollfd **usbfd, **usbfd_orig;
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
// The next two are dummy allocations which will be replaced later
pth_event_t event = pth_event (PTH_EVENT_SEM, stop1);
pth_event_t timeout = pth_event (PTH_EVENT_SEM, stop1);
tv1.tv_sec = tv1.tv_usec = 0;
TRACEPRINTF (t, 10, this, "LoopStart");
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
TRACEPRINTF (t, 10, this, "LoopBegin");
FD_ZERO (&r);
FD_ZERO (&w);
FD_ZERO (&e);
fds = 0;
rc = 0;
usbfd = libusb_get_pollfds (context);
usbfd_orig = usbfd;
if (usbfd)
while (*usbfd)
{
if ((*usbfd)->fd > fds)
fds = (*usbfd)->fd;
if ((*usbfd)->events & POLLIN)
FD_SET ((*usbfd)->fd, &r);
if ((*usbfd)->events & POLLOUT)
FD_SET ((*usbfd)->fd, &w);
usbfd++;
}
free (usbfd_orig);
i = libusb_get_next_timeout (context, &tv);
if (i < 0)
break;
if (i > 0)
{
pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE,
timeout, pth_time (tv.tv_sec, tv.tv_usec));
pth_event_concat (stop, timeout, NULL);
}
pth_event (PTH_EVENT_SELECT | PTH_MODE_REUSE, event, &rc, fds + 1, &r,
&w, &e);
pth_event_concat (stop, event, NULL);
TRACEPRINTF (t, 10, this, "LoopWait");
pth_wait (stop);
TRACEPRINTF (t, 10, this, "LoopProcess");
pth_event_isolate (event);
pth_event_isolate (timeout);
if (libusb_handle_events_timeout (context, &tv1))
break;
TRACEPRINTF (t, 10, this, "LoopEnd");
}
TRACEPRINTF (t, 10, this, "LoopStop");
pth_event_free (timeout, PTH_FREE_THIS);
pth_event_free (event, PTH_FREE_THIS);
pth_event_free (stop, PTH_FREE_THIS);
}
/**
* Implement a realtime loop on a non-realtime OS.
*
* This function never returns. It is called from a separate thread context to
* simulate what would be done at interrupt time on a realtime OS. Instead,
* it sleeps for short durations, and on wakeup invokes the target's realtime
* functions multiple times, depending on how long the sleep lasted.
*/
void realtime_loop (void)
{
struct timeval prev_time, curr_time;
int usecs_elapsed = 0;
#ifdef CONFIG_DEBUG_LATENCY
int latency;
#endif
gettimeofday (&prev_time, NULL);
for (;;)
{
/* Delay a small amount on most iterations of the loop.
If we do not ever sleep, then this task will consume all of the CPU.
Our goal is to sleep for 1ms (1000 usecs) evenly on every iteration.
However we adjust this value in two ways: First, we always try to
sleep slightly less than that, since this function will consume some
CPU time. The rough estimate for this is 100 usecs.
Second, handle actual delays that are fractional. For example, if we
actually sleep for 2.9ms, we will only simulate two interrupts, and
900 usec is ignored. So for the next iteration, don't sleep for that
extra 900 usec.
Because of the adjustments, usecs_asked can go negative; if it does,
we won't sleep at all. */
int usecs_asked = 1000 - usecs_elapsed - 100;
if (usecs_asked > 0)
{
#if defined(CONFIG_PTH)
pth_nap (pth_time (0, usecs_asked));
#elif defined(CONFIG_PTHREADS)
usleep (usecs_asked);
#else
#error "No thread library supported for realtime yet"
#endif
}
/* Now see how long we actually slept. This takes into account the
actual sleep time, which is typically longer on a multitasking OS,
and also the length of time that the previous iteration took. Even
if we did not sleep at all above, usecs_elapsed will be positive here. */
gettimeofday (&curr_time, NULL);
usecs_elapsed = curr_time.tv_usec - prev_time.tv_usec;
if (usecs_elapsed < 0)
usecs_elapsed += 1000000;
#ifdef CONFIG_DEBUG_LATENCY
/* This is for debugging only to see how good your native OS is.
Print a message when the latency is more than 0.5ms than we requested . */
latency = usecs_elapsed - usecs_asked;
if (latency > 200)
print_log ("latency %d usec\n", latency);
#endif
prev_time = curr_time;
/* Invoke realtime tick at least once every time through the loop.
So if we slept < 1ms (either the OS lied to us, or we didn't sleep at all),
we'll make forward progress. */
realtime_counter++;
realtime_tick ();
usecs_elapsed -= usecs_asked;
if (usecs_elapsed > 20000)
{
}
/* If any remaining millseconds occurred during the wait, handle them */
while (usecs_elapsed >= 1000)
{
realtime_counter++;
realtime_tick ();
usecs_elapsed -= 1000;
}
/* Whatever was left over will be subtracted from the next delay, so
that we stay on schedule */
}
}
void
EMI1Layer2::Run (pth_sem_t * stop1)
{
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
pth_event_t input = pth_event (PTH_EVENT_SEM, &in_signal);
pth_event_t timeout = pth_event (PTH_EVENT_RTIME, pth_time (0, 0));
bool wait_confirm = false;
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
if (!wait_confirm)
pth_event_concat (stop, input, NULL);
if (wait_confirm)
pth_event_concat (stop, timeout, NULL);
CArray *c = iface->Get_Packet (stop);
pth_event_isolate(input);
pth_event_isolate(timeout);
if (!wait_confirm && !inqueue.isempty())
{
pth_sem_dec (&in_signal);
Send(inqueue.get());
if (noqueue)
{
pth_event (PTH_EVENT_RTIME | PTH_MODE_REUSE, timeout,
pth_time (1, 0));
wait_confirm = true;
}
}
if (wait_confirm && pth_event_status(timeout) == PTH_STATUS_OCCURRED)
wait_confirm = false;
if (!c)
continue;
if (c->len () == 1 && (*c)[0] == 0xA0 && (mode & BUSMODE_UP))
{
TRACEPRINTF (t, 2, this, "Reopen");
busmode_t old_mode = mode;
mode = BUSMODE_DOWN;
if (Open ())
mode = old_mode; // restore VMONITOR
}
if (c->len () == 1 && (*c)[0] == 0xA0 && mode == BUSMODE_MONITOR)
{
TRACEPRINTF (t, 2, this, "Reopen Busmonitor");
mode = BUSMODE_DOWN;
enterBusmonitor ();
}
if (c->len () && (*c)[0] == 0x4E)
wait_confirm = false;
if (c->len () && (*c)[0] == 0x49 && (mode & BUSMODE_UP))
{
L_Data_PDU *p = EMI_to_L_Data (*c, this);
if (p)
{
delete c;
if (p->AddrType == IndividualAddress)
p->dest = 0;
TRACEPRINTF (t, 2, this, "Recv %s", p->Decode ()());
if (mode == BUSMODE_VMONITOR)
{
L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU (this);
l2->pdu.set (p->ToPacket ());
l3->recv_L_Data (l2);
}
l3->recv_L_Data (p);
continue;
}
}
if (c->len () > 4 && (*c)[0] == 0x49 && mode == BUSMODE_MONITOR)
{
L_Busmonitor_PDU *p = new L_Busmonitor_PDU (this);
p->status = (*c)[1];
p->timestamp = ((*c)[2] << 24) | ((*c)[3] << 16);
p->pdu.set (c->array () + 4, c->len () - 4);
delete c;
TRACEPRINTF (t, 2, this, "Recv %s", p->Decode ()());
l3->recv_L_Data (p);
continue;
}
delete c;
}
pth_event_free (stop, PTH_FREE_THIS);
pth_event_free (input, PTH_FREE_THIS);
pth_event_free (timeout, PTH_FREE_THIS);
}
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);
}
