void FT12LowLevelDriver::Send_Packet (CArray l) { CArray pdu; uchar c; unsigned i; t->TracePacket (1, this, "Send", l); assert (l () <= 32); pdu.resize (l () + 7); pdu[0] = 0x68; pdu[1] = l () + 1; pdu[2] = l () + 1; pdu[3] = 0x68; if (sendflag) pdu[4] = 0x53; else pdu[4] = 0x73; sendflag = !sendflag; pdu.setpart (l.array (), 5, l ()); c = pdu[4]; for (i = 0; i < l (); i++) c += l[i]; pdu[pdu () - 2] = c; pdu[pdu () - 1] = 0x16; inqueue.put (pdu); pth_sem_set_value (&send_empty, 0); pth_sem_inc (&in_signal, TRUE); }
int main (int ac, char *ag[]) { CArray p; uchar buf[200]; int set = 0; if (ac != 4) die (_("%s input file output"), ag[0]); { FILE *f = fopen (ag[2], "r"); if (!f) die (_("open of %s failed"), ag[1]); while (!feof (f)) { int i = fread (buf, 1, sizeof (buf), f); p.setpart (buf, p (), i); } fclose (f); } xmlDocPtr f; f = xmlParseFile (ag[1]); if (!f) die (_("can not parse %s"), ag[1]); xmlNodePtr n = xmlDocGetRootElement (f); if (n->type != XML_ELEMENT_NODE || (strcmp ((const char *) n->name, "DeviceConfig") && strcmp ((const char *) n->name, "DeviceDesc"))) die (_("wrong format")); xmlNodePtr cld = n->children; while (cld) { if (cld->type == XML_ELEMENT_NODE) { const char *name = (const char *) cld->name; if (!strcmp (name, "ProgramID")) { setcontent (cld, p); set = 1; } } cld = cld->next; } if (!set) die (_("ProgramID not found")); if (xmlSaveFormatFileEnc (ag[3], f, "UTF-8", 1) < 0) die (_("write to %s failed"), ag[3]); xmlFreeDoc (f); }
int Layer7_Connection::X_Memory_Read_Block (memaddr_t addr, unsigned int len, CArray & erg) { unsigned blocksize = 12; CArray e; erg.resize (len); for (unsigned i = 0; i < len; i += blocksize) { rt: if (A_Memory_Read (addr + i, (len - i > blocksize ? blocksize : len - i), e) == -1) { if (blocksize == 12) { blocksize = 2; goto rt; } return -1; } erg.setpart (e, i); } 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 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); }