int SerialIOPort::get(uint8_t* buf, int len, pth_event_t stop) { logger_m.debugStream() << "get(buf, len=" << len << ")" << endlog; if (fd_m >= 0) { ssize_t i = pth_read_ev(fd_m, buf, len, stop); // logger_m.debugStream() << "Out of recvfrom " << i << " rl=" << rl << endlog; if (i > 0) { std::string msg(reinterpret_cast<const char*>(buf), i); if (logger_m.isDebugEnabled()) { DbgStream dbg = logger_m.debugStream(); dbg << "Received message on ioport " << getID() << ": "; if (modeRaw_m) { dbg << std::hex << std::setfill ('0') << std::setw (2); for (uint8_t *p = buf; p < buf+i; p++) dbg << (int)*p << " "; dbg << std::dec << endlog; } else dbg << msg << endlog; } return i; } } return -1; }
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 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); }