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;
}
int pth_mutex_acquire(pth_mutex_t *mutex, int tryonly, pth_event_t ev_extra)
{
static pth_key_t ev_key = PTH_KEY_INIT;
pth_event_t ev;
pth_debug2("pth_mutex_acquire: called from thread \"%s\"", pth_current->name);
/* consistency checks */
if (mutex == NULL)
return pth_error(FALSE, EINVAL);
if (!(mutex->mx_state & PTH_MUTEX_INITIALIZED))
return pth_error(FALSE, EDEADLK);
/* still not locked, so simply acquire mutex? */
if (!(mutex->mx_state & PTH_MUTEX_LOCKED)) {
mutex->mx_state |= PTH_MUTEX_LOCKED;
mutex->mx_owner = pth_current;
mutex->mx_count = 1;
pth_ring_append(&(pth_current->mutexring), &(mutex->mx_node));
pth_debug1("pth_mutex_acquire: immediately locking mutex");
return TRUE;
}
/* already locked by caller? */
if (mutex->mx_count >= 1 && mutex->mx_owner == pth_current) {
/* recursive lock */
mutex->mx_count++;
pth_debug1("pth_mutex_acquire: recursive locking");
return TRUE;
}
/* should we just tryonly? */
if (tryonly)
return pth_error(FALSE, EBUSY);
/* else wait for mutex to become unlocked.. */
pth_debug1("pth_mutex_acquire: wait until mutex is unlocked");
for (;;) {
ev = pth_event(PTH_EVENT_MUTEX|PTH_MODE_STATIC, &ev_key, mutex);
if (ev_extra != NULL)
pth_event_concat(ev, ev_extra, NULL);
pth_wait(ev);
if (ev_extra != NULL) {
pth_event_isolate(ev);
if (pth_event_status(ev) == PTH_STATUS_PENDING)
return pth_error(FALSE, EINTR);
}
if (!(mutex->mx_state & PTH_MUTEX_LOCKED))
break;
}
/* now it's again unlocked, so acquire mutex */
pth_debug1("pth_mutex_acquire: locking mutex");
mutex->mx_state |= PTH_MUTEX_LOCKED;
mutex->mx_owner = pth_current;
mutex->mx_count = 1;
pth_ring_append(&(pth_current->mutexring), &(mutex->mx_node));
return TRUE;
}
void KnxConnection::Run (pth_sem_t * stop1)
{
if (url_m == "")
return;
stop_m = pth_event (PTH_EVENT_SEM, stop1);
bool retry = true;
while (retry)
{
con_m = EIBSocketURL(url_m.c_str());
if (con_m)
{
EIBSetEvent (con_m, stop_m);
if (EIBOpen_GroupSocket (con_m, 0) != -1)
{
logger_m.infoStream() << "KnxConnection: Group socket opened. Waiting for messages." << endlog;
// If scope reached this point, there is no doubt that the
// connection with the bus is up and ready.
isReady_m = true;
int retval;
while ((retval = checkInput()) > 0)
{
/* TODO: find another way to check if event occured
struct timeval tv;
tv.tv_sec = 1;
tv.tv_usec = 0;
pth_select_ev(0,0,0,0,&tv,stop);
*/
}
if (retval == -1)
retry = false;
}
else
logger_m.errorStream() << "Failed to open group socket." << endlog;
if (con_m)
EIBClose(con_m);
con_m = 0;
}
else
logger_m.errorStream() << "Failed to open knxConnection url." << endlog;
if (retry)
{
struct timeval tv;
tv.tv_sec = 60;
tv.tv_usec = 0;
pth_select_ev(0,0,0,0,&tv,stop_m);
if (pth_event_status (stop_m) == PTH_STATUS_OCCURRED)
retry = false;
}
}
logger_m.infoStream() << "Out of KnxConnection loop." << endlog;
pth_event_free (stop_m, PTH_FREE_THIS);
stop_m = 0;
}
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);
}
int TcpClientIOPort::get(uint8_t* buf, int len, pth_event_t stop)
{
logger_m.debugStream() << "get(buf, len=" << len << ")" << endlog;
bool retry = true;
while (retry) {
Socket so(this);
if (so.sockfd_m >= 0) {
ssize_t i = pth_read_ev(so.sockfd_m, buf, len, stop);
logger_m.debugStream() << "Out of read " << i << endlog;
if (i > 0)
{
std::string msg(reinterpret_cast<const char*>(buf), i);
logger_m.debugStream() << "Received '" << msg << "' on ioport " << getID() << endlog;
return i;
}
else {
if (pth_event_status (stop) == PTH_STATUS_OCCURRED)
retry = false;
else if (permanent_m && permanentSockfd_m >= 0)
{
close(permanentSockfd_m);
permanentSockfd_m = -1;
}
}
}
else {
struct timeval tv;
tv.tv_sec = 60;
tv.tv_usec = 0;
pth_select_ev(0,0,0,0,&tv,stop);
if (pth_event_status (stop) == PTH_STATUS_OCCURRED)
retry = false;
}
}
logger_m.debugStream() << "Abort get() on ioport " << getID() << endlog;
return -1;
}
void Layer2Runner::Run(pth_sem_t * stop1)
{
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
unsigned i;
TRACEPRINTF (l2->t, 3, this, "L2r running: %08X", l2);
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
LPDU *l = l2->Get_L_Data (stop);
if (!l)
continue;
l3->recv_L_Data(l);
}
TRACEPRINTF (l2->t, 3, this, "L2r stopped: %08X", l2);
}
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);
}
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;
}
LPDU *
TPUARTLayer2Driver::Get_L_Data (pth_event_t stop)
{
if (stop != NULL)
pth_event_concat (getwait, stop, NULL);
pth_wait (getwait);
if (stop)
pth_event_isolate (getwait);
if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
{
pth_sem_dec (&out_signal);
LPDU *l = outqueue.get ();
TRACEPRINTF (t, 2, this, "Recv %s", l->Decode ()());
return l;
}
else
return 0;
}
CArray *
BCU1SerialLowLevelDriver::Get_Packet (pth_event_t stop)
{
if (stop != NULL)
pth_event_concat (getwait, stop, NULL);
pth_wait (getwait);
if (stop)
pth_event_isolate (getwait);
if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
{
pth_sem_dec (&out_signal);
CArray *c = outqueue.get ();
t->TracePacket (1, this, "Recv", *c);
return c;
}
else
return 0;
}
LPDU *
EIBNetIPTunnel::Get_L_Data (pth_event_t stop)
{
if (stop != NULL)
pth_event_concat (getwait, stop, NULL);
pth_wait (getwait);
if (stop)
pth_event_isolate (getwait);
if (pth_event_status (getwait) == PTH_STATUS_OCCURRED)
{
pth_sem_dec (&outsignal);
LPDU *c = outqueue.get ();
if (c)
TRACEPRINTF (t, 2, this, "Recv %s", c->Decode ()());
return c;
}
else
return 0;
}
int main(int argc, char *argv[])
{
char caLine[MAXLINELEN];
pth_event_t ev = NULL;
pth_event_t evt = NULL;
pth_t t_worker = NULL;
pth_t t_ticker = NULL;
pth_attr_t t_attr;
pth_msgport_t mp = NULL;
pth_msgport_t mp_worker = NULL;
struct query *q = NULL;
int n;
if (!pth_init()) {
perror("pth_init");
exit(1);
}
/* murray added for tmp debug */
/*
pth_time_t intval, former;
printf("-------------------------\n");
pth_time_set(&former, PTH_TIME_NOW);
pth_usleep(300);
pth_time_set(&intval, PTH_TIME_NOW);
pth_time_sub(&intval, &former);
double val = pth_time_t2d(&intval);
printf("the intval is [%f]\n", val);
pth_debug2("the intval is [%f]\n", val);
return 0;
*/
fprintf(stderr, "This is TEST_MP, a Pth test using message ports.\n");
fprintf(stderr, "\n");
fprintf(stderr, "Lines on stdin are send to a worker thread via message\n");
fprintf(stderr, "ports, translated to upper case by the worker thread and\n");
fprintf(stderr, "send back to the main thread via message ports.\n");
fprintf(stderr, "Additionally a useless ticker thread awakens every 5s.\n");
fprintf(stderr, "Enter \"quit\" on stdin for stopping this test.\n");
fprintf(stderr, "\n");
t_attr = pth_attr_new();
pth_attr_set(t_attr, PTH_ATTR_NAME, "worker");
pth_attr_set(t_attr, PTH_ATTR_JOINABLE, TRUE);
pth_attr_set(t_attr, PTH_ATTR_STACK_SIZE, 16*1024);
t_worker = pth_spawn(t_attr, worker, NULL);
pth_attr_set(t_attr, PTH_ATTR_NAME, "ticker");
t_ticker = pth_spawn(t_attr, ticker, NULL);
pth_attr_destroy(t_attr);
pth_yield(NULL);
mp_worker = pth_msgport_find("worker");
mp = pth_msgport_create("main");
q = (struct query *)malloc(sizeof(struct query));
ev = pth_event(PTH_EVENT_MSG, mp);
evt = NULL;
for (;;) {
if (evt == NULL)
evt = pth_event(PTH_EVENT_TIME, pth_timeout(20,0));
else
evt = pth_event(PTH_EVENT_TIME|PTH_MODE_REUSE, evt, pth_timeout(20,0));
n = pth_readline_ev(STDIN_FILENO, caLine, MAXLINELEN, evt);
if (n == -1 && pth_event_status(evt) == PTH_STATUS_OCCURRED) {
fprintf(stderr, "main: Hey, what are you waiting for? Type in something!\n");
continue;
}
if (n < 0) {
fprintf(stderr, "main: I/O read error on stdin\n");
break;
}
if (n == 0) {
fprintf(stderr, "main: EOF on stdin\n");
break;
}
caLine[n-1] = NUL;
if (strcmp(caLine, "quit") == 0) {
fprintf(stderr, "main: quit\n");
break;
}
fprintf(stderr, "main: out --> <%s>\n", caLine);
q->string = caLine;
q->head.m_replyport = mp;
pth_msgport_put(mp_worker, (pth_message_t *)q);
pth_wait(ev);
q = (struct query *)pth_msgport_get(mp);
fprintf(stderr, "main: in <-- <%s>\n", q->string);
}
free(q);
pth_event_free(ev, PTH_FREE_THIS);
pth_event_free(evt, PTH_FREE_THIS);
pth_msgport_destroy(mp);
pth_cancel(t_worker);
pth_join(t_worker, NULL);
pth_cancel(t_ticker);
pth_join(t_ticker, NULL);
pth_kill();
return 0;
}
void
Layer3::Run (pth_sem_t * stop1)
{
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
unsigned i;
running = true;
for (i = 0; i < layer2 (); i++)
layer2[i].Start ();
TRACEPRINTF (t, 3, this, "L3 started");
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
pth_event_t bufev = pth_event (PTH_EVENT_SEM, &bufsem);
pth_event_concat (bufev, stop, NULL);
pth_wait (bufev);
pth_event_isolate (bufev);
if (pth_event_status (bufev) != PTH_STATUS_OCCURRED)
{
pth_event_free (bufev, PTH_FREE_THIS);
continue;
}
pth_event_free (bufev, PTH_FREE_THIS);
pth_sem_dec (&bufsem);
LPDU *l = buf.get ();
if (!l)
continue;
if (l->getType () == L_Busmonitor)
{
L_Busmonitor_PDU *l1, *l2;
l1 = (L_Busmonitor_PDU *) l;
TRACEPRINTF (t, 3, this, "Recv %s", l1->Decode ()());
for (i = 0; i < busmonitor (); i++)
{
l2 = new L_Busmonitor_PDU (*l1);
busmonitor[i].cb->Send_L_Busmonitor (l2);
}
for (i = 0; i < vbusmonitor (); i++)
{
l2 = new L_Busmonitor_PDU (*l1);
vbusmonitor[i].cb->Send_L_Busmonitor (l2);
}
}
if (l->getType () == L_Data)
{
L_Data_PDU *l1;
l1 = (L_Data_PDU *) l;
if (l1->repeated)
{
CArray d1 = l1->ToPacket ();
for (i = 0; i < ignore (); i++)
if (d1 == ignore[i].data)
{
TRACEPRINTF (t, 3, this, "Repeated discareded");
goto wt;
}
}
l1->repeated = 1;
ignore.resize (ignore () + 1);
ignore[ignore () - 1].data = l1->ToPacket ();
ignore[ignore () - 1].end = getTime () + 1000000;
l1->repeated = 0;
if (l1->AddrType == IndividualAddress
&& l1->dest == defaultAddr)
l1->dest = 0;
TRACEPRINTF (t, 3, this, "Recv %s", l1->Decode ()());
if (l1->AddrType == GroupAddress && l1->dest == 0)
{
for (i = 0; i < broadcast (); i++)
broadcast[i].cb->Send_L_Data (new L_Data_PDU (*l1));
}
if (l1->AddrType == GroupAddress && l1->dest != 0)
{
for (i = 0; i < group (); i++)
{
Group_Info &grp = group[i];
if (grp.dest == l1->dest || grp.dest == 0)
grp.cb->Send_L_Data (new L_Data_PDU (*l1));
}
}
if (l1->AddrType == IndividualAddress)
{
for (i = 0; i < individual (); i++)
{
Individual_Info &indiv = individual[i];
if (indiv.dest == l1->dest || indiv.dest == 0)
if (indiv.src == l1->source || indiv.src == 0)
indiv.cb->Send_L_Data (new L_Data_PDU (*l1));
}
}
// finally, send to all (other(?)) L2 interfaces
// TODO: filter by addresses
send_L_Data(l1);
}
// ignore[] is ordered, any timed-out items are at the front
for (i = 0; i < ignore (); i++)
if (ignore[i].end >= getTime ())
break;
if (i)
ignore.deletepart (0, i);
wt:
delete l;
}
TRACEPRINTF (t, 3, this, "L3 stopping");
running = false;
for (i = 0; i < layer2 (); i++)
layer2[i].Stop ();
pth_event_free (stop, PTH_FREE_THIS);
}
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
TPUARTLayer2Driver::Run (pth_sem_t * stop1)
{
struct message m;
int l;
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
pth_event_t input = pth_event (PTH_EVENT_SEM, &in_signal);
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
pth_event_concat (stop, input, NULL);
l = pth_read_ev (fd, &m, sizeof (m), stop);
if (l >= 0)
{
LPDU *l1;
if (m.length > sizeof (m.data))
m.length = sizeof (m.data);
t->TracePacket (0, this, "Recv", m.length, m.data);
if (vmode && mode == 0)
{
L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU;
l2->pdu.set (m.data, m.length);
outqueue.put (l2);
pth_sem_inc (&out_signal, 1);
}
if (mode == 0)
l1 = LPDU::fromPacket (CArray (m.data, m.length));
else
{
l1 = new L_Busmonitor_PDU;
((L_Busmonitor_PDU *) l1)->pdu.set (m.data, m.length);
}
outqueue.put (l1);
pth_sem_inc (&out_signal, 1);
}
pth_event_isolate (stop);
if (!inqueue.isempty ())
{
LPDU *l1 = inqueue.top ();
CArray c = l1->ToPacket ();
unsigned len = c ();
if (len > sizeof (m.data))
len = sizeof (m.data);
memcpy (m.data, c.array (), len);
m.length = len;
if (ver)
m.length--;
t->TracePacket (0, this, "Send", m.length, m.data);
l = pth_write_ev (fd, &m, sizeof (m), stop);
if (l >= 0)
{
if (vmode)
{
L_Busmonitor_PDU *l2 = new L_Busmonitor_PDU;
l2->pdu.set (c);
outqueue.put (l2);
pth_sem_inc (&out_signal, 1);
}
pth_sem_dec (&in_signal);
delete inqueue.get ();
}
}
}
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);
}
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);
}
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);
}
int KnxConnection::checkInput(pth_event_t ev)
{
int len;
eibaddr_t dest;
eibaddr_t src;
uint8_t buf[200];
if (!con_m)
return 0;
if (ev)
EIBSetEvent (con_m, ev);
len = EIBGetGroup_Src (con_m, sizeof (buf), buf, &src, &dest);
if (ev)
{
EIBSetEvent (con_m, stop_m);
if (pth_event_status (ev) == PTH_STATUS_OCCURRED)
return -1;
}
if (pth_event_status (stop_m) == PTH_STATUS_OCCURRED)
return -1;
if (len == -1)
{
logger_m.errorStream() << "Read failed" << endlog;
return 0;
}
if (len < 2)
{
logger_m.warnStream() << "Invalid Packet (too short)" << endlog;
return 0;
}
if (buf[0] & 0x3 || (buf[1] & 0xC0) == 0xC0)
{
logger_m.warnStream() << "Unknown APDU from "<< src << " to " << dest << endlog;
}
else
{
if (logger_m.isDebugEnabled())
{
DbgStream dbg = logger_m.debugStream();
switch (buf[1] & 0xC0)
{
case 0x00:
dbg << "Read";
break;
case 0x40:
dbg << "Response";
break;
case 0x80:
dbg << "Write";
break;
}
dbg << " from " << Object::WriteAddr(src) << " to " << Object::WriteGroupAddr(dest);
if (buf[1] & 0xC0)
{
dbg << ": " << std::hex << std::setfill ('0') << std::setw (2);
if (len == 2)
dbg << (int)(buf[1] & 0x3F);
else
{
for (uint8_t *p = buf+2; p < buf+len; p++)
dbg << (int)*p << " ";
}
}
dbg << std::dec << endlog;
}
if (listener_m)
{
switch (buf[1] & 0xC0)
{
case 0x00:
listener_m->onRead(src, dest, buf, len);
break;
case 0x40:
listener_m->onResponse(src, dest, buf, len);
break;
case 0x80:
listener_m->onWrite(src, dest, buf, len);
break;
}
}
}
return 1;
}
void
Layer3::Run (pth_sem_t * stop1)
{
pth_event_t stop = pth_event (PTH_EVENT_SEM, stop1);
unsigned i;
while (pth_event_status (stop) != PTH_STATUS_OCCURRED)
{
LPDU *l = layer2->Get_L_Data (stop);
if (!l)
continue;
if (l->getType () == L_Busmonitor)
{
L_Busmonitor_PDU *l1, *l2;
l1 = (L_Busmonitor_PDU *) l;
TRACEPRINTF (t, 3, this, "Recv %s", l1->Decode ()());
for (i = 0; i < busmonitor (); i++)
{
l2 = new L_Busmonitor_PDU (*l1);
busmonitor[i].cb->Get_L_Busmonitor (l2);
}
for (i = 0; i < vbusmonitor (); i++)
{
l2 = new L_Busmonitor_PDU (*l1);
vbusmonitor[i].cb->Get_L_Busmonitor (l2);
}
}
if (l->getType () == L_Data)
{
L_Data_PDU *l1;
l1 = (L_Data_PDU *) l;
if (l1->repeated)
{
CArray d1 = l1->ToPacket ();
for (i = 0; i < ignore (); i++)
if (d1 == ignore[i].data)
{
TRACEPRINTF (t, 3, this, "Repeated discareded");
goto wt;
}
}
l1->repeated = 1;
ignore.resize (ignore () + 1);
ignore[ignore () - 1].data = l1->ToPacket ();
ignore[ignore () - 1].end = getTime () + 1000000;
l1->repeated = 0;
if (l1->AddrType == IndividualAddress
&& l1->dest == layer2->getDefaultAddr ())
l1->dest = 0;
TRACEPRINTF (t, 3, this, "Recv %s", l1->Decode ()());
if (l1->AddrType == GroupAddress && l1->dest == 0)
{
for (i = 0; i < broadcast (); i++)
broadcast[i].cb->Get_L_Data (new L_Data_PDU (*l1));
}
if (l1->AddrType == GroupAddress && l1->dest != 0)
{
for (i = 0; i < group (); i++)
if (group[i].dest == l1->dest || group[i].dest == 0)
group[i].cb->Get_L_Data (new L_Data_PDU (*l1));
}
if (l1->AddrType == IndividualAddress)
{
for (i = 0; i < individual (); i++)
if (individual[i].dest == l1->dest
|| individual[i].dest == 0)
if (individual[i].src == l1->source
|| individual[i].src == 0)
individual[i].cb->Get_L_Data (new L_Data_PDU (*l1));
}
}
redel:
for (i = 0; i < ignore (); i++)
if (ignore[i].end < getTime ())
{
ignore.deletepart (i, 1);
goto redel;
}
wt:
delete l;
}
pth_event_free (stop, PTH_FREE_THIS);
}
