int
Layer7_Connection::X_Memory_Write_Block (memaddr_t addr, const CArray & data)
{
CArray prev;
unsigned int i, j;
int k, res = 0;
const unsigned blocksize = 12;
if (X_Memory_Read_Block (addr, data (), prev) == -1)
return -1;
for (i = 0; i < data (); i++)
{
if (data[i] == prev[i])
continue;
j = 0;
while (data[i + j] != prev[i + j] && j < blocksize && i + j < data ())
j++;
k = X_Memory_Write (addr + i, CArray (data.array () + i, j));
if (k == -1)
return -1;
if (k == -2)
res = -2;
i += j - 1;
}
return res;
}
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
Layer7_Connection::A_Memory_Write (memaddr_t addr, const CArray & data)
{
A_Memory_Write_PDU r;
r.addr = addr;
r.count = data () & 0x0f;
r.data.set (data.array (), data () & 0x0f);
l4->Send (r.ToPacket ());
return 0;
}
CArray *
USBConverterInterface::Get_Packet (pth_event_t stop)
{
CArray *res1 = i->Get_Packet (stop);
if (res1)
{
CArray res = *res1;
if (res () != 64)
goto out;
if (res[0] != 0x01)
goto out;
if ((res[1] & 0x0f) != 3)
goto out;
if (res[2] > 64 - 8)
goto out;
if (res[3] != 0)
goto out;
if (res[4] != 8)
goto out;
if (res[5] != 0)
goto out;
if (res[6] + 11 > 64)
goto out;
if (res[7] != 1)
goto out;
switch (v)
{
case vEMI1:
if (res[8] != 1)
goto out;
break;
case vEMI2:
if (res[8] != 2)
goto out;
break;
case vCEMI:
if (res[8] != 3)
goto out;
break;
default:
goto out;
}
res1->set (res.array () + 11, res[6]);
t->TracePacket (0, this, "RecvEMI", *res1);
}
return res1;
out:
delete res1;
return 0;
}
int
Layer7_Connection::A_Memory_Write_Block (memaddr_t addr, const CArray & data)
{
CArray prev;
int i, j, k, res = 0;
const unsigned blocksize = 12;
for (i = 0; i < data (); i += blocksize)
{
j = blocksize;
if (i + j > data ())
j = data () - i;
k = A_Memory_Write (addr + i, CArray (data.array () + i, j));
if (k == -1)
return -1;
}
return res;
}
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
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
setcontent (xmlNodePtr n, CArray data)
{
CArray d = encode_hex (data);
xmlNodeSetContent (n, (const xmlChar *) d.array ());
}
