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