void SendServoTargetPos(uint8_t ServoNo, int16_t targetPos,uint16_t targetSpeed)
{
//printf("setting servo %d pos to %d @ %d\n", ServoNo, targetPos, targetSpeed);
dxl_set_txpacket_id(ServoNo);
dxl_set_txpacket_instruction(INST_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, LO_BYTE(targetPos));
dxl_set_txpacket_parameter(2, HI_BYTE(targetPos));
dxl_set_txpacket_parameter(3, LO_BYTE(targetSpeed));
dxl_set_txpacket_parameter(4, HI_BYTE(targetSpeed));
dxl_set_txpacket_length(7);
dxl_txrx_packet();
}
PRGArchive *
PRGArchive::archiveFromArchive(Archive *otherArchive)
{
if (otherArchive == NULL || otherArchive->getNumberOfItems() == 0)
return NULL;
PRGArchive *archive = new PRGArchive();
archive->debug(1, "Creating PRG archive from %s archive...\n", otherArchive->getTypeAsString());
// Determine container size and allocate memory
archive->size = 2 + otherArchive->getSizeOfItem(0);
if ((archive->data = (uint8_t *)malloc(archive->size)) == NULL) {
archive->warn("Failed to allocate %d bytes of memory\n", archive->size);
delete archive;
return NULL;
}
// Load address
uint8_t* ptr = archive->data;
*ptr++ = LO_BYTE(otherArchive->getDestinationAddrOfItem(0));
*ptr++ = HI_BYTE(otherArchive->getDestinationAddrOfItem(0));
// File data
int byte;
otherArchive->selectItem(0);
while ((byte = otherArchive->getByte()) != EOF) {
*ptr++ = (uint8_t)byte;
}
return archive;
}
//-----------------------------------------------------------------
// set_address:
//-----------------------------------------------------------------
static void set_address(struct device_request *request)
{
unsigned char addr = (LO_BYTE(request->wValue)) & USB_ADDRESS_MASK;
usbhw_set_address(addr);
usbhw_control_endpoint_ack();
log_printf(USBLOG_INFO, "USB: Set address %x\n", addr);
}
static void snap_received_packet(
volatile struct mstp_port_struct_t *mstp_port,
int sockfd)
{
uint16_t mtu_len = 0; /* number of octets of packet saved in file */
unsigned i = 0; /* counter */
static uint8_t mtu[1500] = { 0 };
uint16_t max_data = 0;
mtu[0] = 0;
mtu[1] = 0;
mtu[2] = 0;
mtu[3] = 0;
mtu[4] = 0;
mtu[5] = mstp_port->DestinationAddress;
mtu[6] = 0;
mtu[7] = 0;
mtu[8] = 0;
mtu[9] = 0;
mtu[10] = 0;
mtu[11] = mstp_port->SourceAddress;
/* length - 12, 13 */
mtu[14] = 0xaa; /* DSAP for SNAP */
mtu[15] = 0xaa; /* SSAP for SNAP */
mtu[16] = 0x03; /* Control Field for SNAP */
mtu[17] = 0x00; /* Organization Code: Cimetrics */
mtu[18] = 0x10; /* Organization Code: Cimetrics */
mtu[19] = 0x90; /* Organization Code: Cimetrics */
mtu[20] = 0x00; /* Protocol ID */
mtu[21] = 0x01; /* Protocol ID */
mtu[22] = 0x00; /* delta time */
mtu[23] = 0x00; /* delta time */
mtu[24] = 0x80; /* unknown byte */
mtu[25] = mstp_port->FrameType;
mtu[26] = mstp_port->DestinationAddress;
mtu[27] = mstp_port->SourceAddress;
mtu[28] = HI_BYTE(mstp_port->DataLength);
mtu[29] = LO_BYTE(mstp_port->DataLength);
mtu[30] = mstp_port->HeaderCRCActual;
mtu_len = 31;
if (mstp_port->DataLength) {
max_data = min(mstp_port->InputBufferSize, mstp_port->DataLength);
for (i = 0; i < max_data; i++) {
mtu[31 + i] = mstp_port->InputBuffer[i];
}
mtu[31 + max_data] = mstp_port->DataCRCActualMSB;
mtu[31 + max_data + 1] = mstp_port->DataCRCActualLSB;
mtu_len += (max_data + 2);
}
/* Ethernet length is data only - not address or length bytes */
encode_unsigned16(&mtu[12], mtu_len - 14);
write(sockfd, &mtu[0], mtu_len);
}
int lbp_write(u16 addr, void *buffer) {
int send, recv;
lbp_cmd_addr_data packet;
packet.cmd = LBP_CMD_WRITE | LBP_ARGS_32BIT;
packet.addr_hi = LO_BYTE(addr);
packet.addr_lo = HI_BYTE(addr);
memcpy(&packet.data, buffer, 4);
send = lbp_send(&packet, sizeof(lbp_cmd_addr_data));
if (LBP_SENDRECV_DEBUG)
printf("lbp_write(%02X:%04X)\n", packet.cmd, addr);
return 0;
}
int lbp_read(u16 addr, void *buffer) {
int send, recv;
lbp_cmd_addr packet;
u32 *ptr = buffer;
packet.cmd = LBP_CMD_READ | LBP_ARGS_32BIT;
packet.addr_hi = LO_BYTE(addr);
packet.addr_lo = HI_BYTE(addr);
send = lbp_send(&packet, sizeof(lbp_cmd_addr));
recv = lbp_recv(buffer, 4);
if (LBP_SENDRECV_DEBUG)
printf("lbp_read(%02X:%04X): %08X\n", packet.cmd, addr, *ptr);
return 0;
}
//-----------------------------------------------------------------
// get_descriptor:
//-----------------------------------------------------------------
static void get_descriptor(struct device_request *request)
{
unsigned char bDescriptorType = HI_BYTE(request->wValue);
unsigned char bDescriptorIndex = LO_BYTE( request->wValue );
unsigned short wLength = request->wLength;
unsigned char bCount = 0;
unsigned char *desc_ptr;
desc_ptr = usb_get_descriptor(bDescriptorType, bDescriptorIndex, wLength, &bCount);
if (desc_ptr)
usb_control_send(desc_ptr, bCount);
else
usbhw_control_endpoint_stall();
}
static void write_received_packet(
volatile struct mstp_port_struct_t *mstp_port)
{
uint32_t ts_sec; /* timestamp seconds */
uint32_t ts_usec; /* timestamp microseconds */
uint32_t incl_len; /* number of octets of packet saved in file */
uint32_t orig_len; /* actual length of packet */
uint8_t header[8]; /* MS/TP header */
struct timeval tv;
size_t max_data = 0;
if (pFile) {
gettimeofday(&tv, NULL);
ts_sec = tv.tv_sec;
ts_usec = tv.tv_usec;
if ((mstp_port->ReceivedValidFrame) ||
(mstp_port->ReceivedValidFrameNotForUs)) {
packet_statistics(&tv, mstp_port);
}
(void) data_write(&ts_sec, sizeof(ts_sec), 1);
(void) data_write(&ts_usec, sizeof(ts_usec), 1);
if (mstp_port->DataLength) {
max_data = min(mstp_port->InputBufferSize, mstp_port->DataLength);
incl_len = orig_len = 8 + max_data + 2;
} else {
incl_len = orig_len = 8;
}
(void) data_write(&incl_len, sizeof(incl_len), 1);
(void) data_write(&orig_len, sizeof(orig_len), 1);
header[0] = 0x55;
header[1] = 0xFF;
header[2] = mstp_port->FrameType;
header[3] = mstp_port->DestinationAddress;
header[4] = mstp_port->SourceAddress;
header[5] = HI_BYTE(mstp_port->DataLength);
header[6] = LO_BYTE(mstp_port->DataLength);
header[7] = mstp_port->HeaderCRCActual;
(void) data_write(header, sizeof(header), 1);
if (mstp_port->DataLength) {
(void) data_write(mstp_port->InputBuffer, max_data, 1);
(void) data_write((char *) &mstp_port->DataCRCActualMSB, 1, 1);
(void) data_write((char *) &mstp_port->DataCRCActualLSB, 1, 1);
}
} else {
fprintf(stderr, "mstpcap[packet]: failed to open %s: %s\n",
Capture_Filename, strerror(errno));
}
}
/** Read PDO assign structure */
int si_PDOassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset)
{
uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
uint8 subcnt;
int wkc, bsize = 0, rdl;
int32 rdat2;
uint8 bitlen, obj_subidx;
uint16 obj_idx;
int abs_offset, abs_bit;
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign subindex 0 ( = number of PDO's) */
wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
rdat = etohs(rdat);
/* positive result from slave ? */
if ((wkc > 0) && (rdat > 0))
{
/* number of available sub indexes */
nidx = rdat;
bsize = 0;
/* read all PDO's */
for (idxloop = 1; idxloop <= nidx; idxloop++)
{
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign */
wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
/* result is index of PDO */
idx = etohl(rdat);
if (idx > 0)
{
rdl = sizeof(subcnt); subcnt = 0;
/* read number of subindexes of PDO */
wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
subidx = subcnt;
/* for each subindex */
for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
{
rdl = sizeof(rdat2); rdat2 = 0;
/* read SDO that is mapped in PDO */
wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
rdat2 = etohl(rdat2);
/* extract bitlength of SDO */
bitlen = LO_BYTE(rdat2);
bsize += bitlen;
obj_idx = (uint16)(rdat2 >> 16);
obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff);
abs_offset = mapoffset + (bitoffset / 8);
abs_bit = bitoffset % 8;
ODlist.Slave = slave;
ODlist.Index[0] = obj_idx;
OElist.Entries = 0;
wkc = 0;
/* read object entry from dictionary if not a filler (0x0000:0x00) */
if(obj_idx || obj_subidx)
wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist);
printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen);
if((wkc > 0) && OElist.Entries)
{
printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx]), OElist.Name[obj_subidx]);
}
else
printf("\n");
bitoffset += bitlen;
};
};
};
/** Enumerate and init all slaves.
*
* @param[in] context = context struct
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
* @return Workcounter of slave discover datagram = number of slaves found
*/
int ecx_config_init(ecx_contextt *context, uint8 usetable)
{
uint16 w, slave, ADPh, configadr, ssigen;
uint16 topology, estat;
int16 topoc, slavec, aliasadr;
uint8 b,h;
uint8 zbuf[64];
uint8 SMc;
uint32 eedat;
int wkc, cindex, nSM, lp;
EC_PRINT("ec_config_init %d\n",usetable);
*(context->slavecount) = 0;
/* clean ec_slave array */
memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave);
memset(&zbuf, 0x00, sizeof(zbuf));
memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup);
/* clear slave eeprom cache */
ecx_siigetbyte(context, 0, EC_MAXEEPBUF);
for(lp = 0; lp < context->maxgroup; lp++)
{
context->grouplist[lp].logstartaddr = lp << 16; /* default start address per group entry */
}
/* make special pre-init register writes to enable MAC[1] local administered bit *
* setting for old netX100 slaves */
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
/* netX100 should now be happy */
wkc = ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
printf("wkc = %d\n",wkc);
w = 0x0000;
wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
if (wkc > 0)
{
*(context->slavecount) = wkc;
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLPORT, sizeof(b), &b, EC_TIMEOUTRET3); /* deact loop manual */
w = htoes(0x0004);
ecx_BWR(context->port, 0x0000, ECT_REG_IRQMASK, sizeof(w), &w, EC_TIMEOUTRET3); /* set IRQ mask */
ecx_BWR(context->port, 0x0000, ECT_REG_RXERR, 8, &zbuf, EC_TIMEOUTRET3); /* reset CRC counters */
ecx_BWR(context->port, 0x0000, ECT_REG_FMMU0, 16 * 3, &zbuf, EC_TIMEOUTRET3); /* reset FMMU's */
ecx_BWR(context->port, 0x0000, ECT_REG_SM0, 8 * 4, &zbuf, EC_TIMEOUTRET3); /* reset SyncM */
ecx_BWR(context->port, 0x0000, ECT_REG_DCSYSTIME, 4, &zbuf, EC_TIMEOUTRET3); /* reset system time+ofs */
w = htoes(0x1000);
ecx_BWR(context->port, 0x0000, ECT_REG_DCSPEEDCNT, sizeof(w), &w, EC_TIMEOUTRET3); /* DC speedstart */
w = htoes(0x0c00);
ecx_BWR(context->port, 0x0000, ECT_REG_DCTIMEFILT, sizeof(w), &w, EC_TIMEOUTRET3); /* DC filt expr */
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
b = 2;
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET3); /* force Eeprom from PDI */
b = 0;
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET3); /* set Eeprom to master */
for (slave = 1; slave <= *(context->slavecount); slave++)
{
ADPh = (uint16)(1 - slave);
context->slavelist[slave].Itype =
etohs(ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3)); /* read interface type of slave */
/* a node offset is used to improve readibility of network frames */
/* this has no impact on the number of addressable slaves (auto wrap around) */
ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET3); /* set node address of slave */
if (slave == 1)
{
b = 1; /* kill non ecat frames for first slave */
}
else
{
b = 0; /* pass all frames for following slaves */
}
ecx_APWRw(context->port, ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET3); /* set non ecat frame behaviour */
configadr = etohs(ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3));
context->slavelist[slave].configadr = configadr;
ecx_FPRD(context->port, configadr, ECT_REG_ALIAS, sizeof(aliasadr), &aliasadr, EC_TIMEOUTRET3);
context->slavelist[slave].aliasadr = etohs(aliasadr);
ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3);
estat = etohs(estat);
if (estat & EC_ESTAT_R64) /* check if slave can read 8 byte chunks */
{
context->slavelist[slave].eep_8byte = 1;
}
ecx_readeeprom1(context, slave, ECT_SII_MANUF); /* Manuf */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_man =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* Manuf */
ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_id =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* ID */
ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_rev =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* revision */
ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */
context->slavelist[slave].mbx_wo = (uint16)LO_WORD(eedat);
context->slavelist[slave].mbx_l = (uint16)HI_WORD(eedat);
if (context->slavelist[slave].mbx_l > 0)
{
ecx_readeeprom1(context, slave, ECT_SII_TXMBXADR); /* read mailbox offset */
}
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
if (context->slavelist[slave].mbx_l > 0)
{
eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* read mailbox offset */
context->slavelist[slave].mbx_ro = (uint16)LO_WORD(eedat); /* read mailbox offset */
context->slavelist[slave].mbx_rl = (uint16)HI_WORD(eedat); /*read mailbox length */
if (context->slavelist[slave].mbx_rl == 0)
{
context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l;
}
}
configadr = context->slavelist[slave].configadr;
if ((etohs(ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3)) & 0x04) > 0) /* Support DC? */
{
context->slavelist[slave].hasdc = TRUE;
}
else
{
context->slavelist[slave].hasdc = FALSE;
}
topology = etohs(ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3)); /* extract topology from DL status */
h = 0;
b = 0;
if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */
{
h++;
b |= 0x01;
}
if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */
{
h++;
b |= 0x02;
}
if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */
{
h++;
b |= 0x04;
}
if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */
{
h++;
b |= 0x08;
}
/* ptype = Physical type*/
context->slavelist[slave].ptype =
LO_BYTE(etohs(ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3)));
context->slavelist[slave].topology = h;
context->slavelist[slave].activeports = b;
/* 0=no links, not possible */
/* 1=1 link , end of line */
/* 2=2 links , one before and one after */
/* 3=3 links , split point */
/* 4=4 links , cross point */
/* search for parent */
context->slavelist[slave].parent = 0; /* parent is master */
if (slave > 1)
{
topoc = 0;
slavec = slave - 1;
do
{
topology = context->slavelist[slavec].topology;
if (topology == 1)
{
topoc--; /* endpoint found */
}
if (topology == 3)
{
topoc++; /* split found */
}
if (topology == 4)
{
topoc += 2; /* cross found */
}
if (((topoc >= 0) && (topology > 1)) ||
(slavec == 1)) /* parent found */
{
context->slavelist[slave].parent = slavec;
slavec = 1;
}
slavec--;
}
while (slavec > 0);
}
w = ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */
/* set default mailbox configuration if slave has mailbox */
if (context->slavelist[slave].mbx_l>0)
{
context->slavelist[slave].SMtype[0] = 1;
context->slavelist[slave].SMtype[1] = 2;
context->slavelist[slave].SMtype[2] = 3;
context->slavelist[slave].SMtype[3] = 4;
context->slavelist[slave].SM[0].StartAddr = htoes(context->slavelist[slave].mbx_wo);
context->slavelist[slave].SM[0].SMlength = htoes(context->slavelist[slave].mbx_l);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
context->slavelist[slave].SM[1].StartAddr = htoes(context->slavelist[slave].mbx_ro);
context->slavelist[slave].SM[1].SMlength = htoes(context->slavelist[slave].mbx_rl);
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
context->slavelist[slave].mbx_proto =
ecx_readeeprom(context, slave, ECT_SII_MBXPROTO, EC_TIMEOUTEEP);
}
cindex = 0;
#ifdef EC_VER1
/* use configuration table ? */
if (usetable)
{
cindex = ec_findconfig( context->slavelist[slave].eep_man, context->slavelist[slave].eep_id );
context->slavelist[slave].configindex= cindex;
}
/* slave found in configuration table ? */
if (cindex)
{
context->slavelist[slave].Dtype = ec_configlist[cindex].Dtype;
strcpy(context->slavelist[slave].name ,ec_configlist[cindex].name);
context->slavelist[slave].Ibits = ec_configlist[cindex].Ibits;
context->slavelist[slave].Obits = ec_configlist[cindex].Obits;
if (context->slavelist[slave].Obits)
{
context->slavelist[slave].FMMU0func = 1;
}
if (context->slavelist[slave].Ibits)
{
context->slavelist[slave].FMMU1func = 2;
}
context->slavelist[slave].FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
context->slavelist[slave].FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
context->slavelist[slave].SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
context->slavelist[slave].SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
/* simple (no mailbox) output slave found ? */
if (context->slavelist[slave].Obits && !context->slavelist[slave].SM[2].StartAddr)
{
context->slavelist[slave].SM[0].StartAddr = htoes(0x0f00);
context->slavelist[slave].SM[0].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
context->slavelist[slave].FMMU[0].FMMUactive = 1;
context->slavelist[slave].FMMU[0].FMMUtype = 2;
context->slavelist[slave].SMtype[0] = 3;
}
/* complex output slave */
else
{
context->slavelist[slave].SM[2].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SMtype[2] = 3;
}
context->slavelist[slave].SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
context->slavelist[slave].SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
/* simple (no mailbox) input slave found ? */
if (context->slavelist[slave].Ibits && !context->slavelist[slave].SM[3].StartAddr)
{
context->slavelist[slave].SM[1].StartAddr = htoes(0x1000);
context->slavelist[slave].SM[1].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SM[1].SMflags = htoel(0x00000000);
context->slavelist[slave].FMMU[1].FMMUactive = 1;
context->slavelist[slave].FMMU[1].FMMUtype = 1;
context->slavelist[slave].SMtype[1] = 4;
}
/* complex input slave */
else
{
context->slavelist[slave].SM[3].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SMtype[3] = 4;
}
}
/* slave not in configuration table, find out via SII */
else
#endif
{
ssigen = ecx_siifind(context, slave, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
context->slavelist[slave].CoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x07);
context->slavelist[slave].FoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x08);
context->slavelist[slave].EoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x09);
context->slavelist[slave].SoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x0a);
if((ecx_siigetbyte(context, slave, ssigen + 0x0d) & 0x02) > 0)
{
context->slavelist[slave].blockLRW = 1;
context->slavelist[0].blockLRW++;
}
context->slavelist[slave].Ebuscurrent = ecx_siigetbyte(context, slave, ssigen + 0x0e);
context->slavelist[slave].Ebuscurrent += ecx_siigetbyte(context, slave, ssigen + 0x0f) << 8;
context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
}
/* SII strings section */
if (ecx_siifind(context, slave, ECT_SII_STRING) > 0)
{
ecx_siistring(context, context->slavelist[slave].name, slave, 1);
}
/* no name for slave found, use constructed name */
else
{
sprintf(context->slavelist[slave].name, "? M:%8.8x I:%8.8x",
(unsigned int)context->slavelist[slave].eep_man,
(unsigned int)context->slavelist[slave].eep_id);
}
/* SII SM section */
nSM = ecx_siiSM(context, slave, context->eepSM);
if (nSM>0)
{
context->slavelist[slave].SM[0].StartAddr = htoes(context->eepSM->PhStart);
context->slavelist[slave].SM[0].SMlength = htoes(context->eepSM->Plength);
context->slavelist[slave].SM[0].SMflags =
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
SMc = 1;
while ((SMc < EC_MAXSM) && ecx_siiSMnext(context, slave, context->eepSM, SMc))
{
context->slavelist[slave].SM[SMc].StartAddr = htoes(context->eepSM->PhStart);
context->slavelist[slave].SM[SMc].SMlength = htoes(context->eepSM->Plength);
context->slavelist[slave].SM[SMc].SMflags =
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
SMc++;
}
}
/* SII FMMU section */
if (ecx_siiFMMU(context, slave, context->eepFMMU))
{
if (context->eepFMMU->FMMU0 !=0xff)
{
context->slavelist[slave].FMMU0func = context->eepFMMU->FMMU0;
}
if (context->eepFMMU->FMMU1 !=0xff)
{
context->slavelist[slave].FMMU1func = context->eepFMMU->FMMU1;
}
if (context->eepFMMU->FMMU2 !=0xff)
{
context->slavelist[slave].FMMU2func = context->eepFMMU->FMMU2;
}
if (context->eepFMMU->FMMU3 !=0xff)
{
context->slavelist[slave].FMMU3func = context->eepFMMU->FMMU3;
}
}
}
if (context->slavelist[slave].mbx_l > 0)
{
if (context->slavelist[slave].SM[0].StartAddr == 0x0000) /* should never happen */
{
EC_PRINT("Slave %d has no proper mailbox in configuration, try default.\n", slave);
context->slavelist[slave].SM[0].StartAddr = htoes(0x1000);
context->slavelist[slave].SM[0].SMlength = htoes(0x0080);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
context->slavelist[slave].SMtype[0] = 1;
}
if (context->slavelist[slave].SM[1].StartAddr == 0x0000) /* should never happen */
{
EC_PRINT("Slave %d has no proper mailbox out configuration, try default.\n", slave);
context->slavelist[slave].SM[1].StartAddr = htoes(0x1080);
context->slavelist[slave].SM[1].SMlength = htoes(0x0080);
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
context->slavelist[slave].SMtype[1] = 2;
}
/* program SM0 mailbox in and SM1 mailbox out for slave */
/* writing both SM in one datagram will solve timing issue in old NETX */
ecx_FPWR(context->port, configadr, ECT_REG_SM0, sizeof(ec_smt) * 2,
&(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3);
}
/* request pre_op for slave */
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP | EC_STATE_ACK) , EC_TIMEOUTRET3); /* set preop status */
}
}
return wkc;
}
T64File *
T64File::makeT64ArchiveWithAnyArchive(AnyArchive *otherArchive)
{
if (otherArchive == NULL)
return NULL;
T64File *archive = new T64File();
// Determine file size and allocate memory
unsigned currentFiles = otherArchive->numberOfItems();
unsigned maxFiles = (currentFiles < 30) ? 30 : currentFiles;
archive->size = 64 /* header */ + maxFiles * 32 /* tape entries */;
for (unsigned i = 0; i < currentFiles; i++) {
archive->selectItem(i);
archive->size += otherArchive->getSizeOfItem();
}
if ((archive->data = new uint8_t[archive->size]) == NULL) {
archive->warn("Failed to allocate %d bytes of memory\n", archive->size);
delete archive;
return NULL;
}
// Magic bytes (32 bytes)
uint8_t *ptr = archive->data;
strncpy((char *)ptr, "C64 tape image file", 32);
ptr += 32;
// Version (2 bytes)
*ptr++ = 0x00;
*ptr++ = 0x01;
// Max files (2 bytes)
*ptr++ = LO_BYTE(maxFiles);
*ptr++ = HI_BYTE(maxFiles);
// Current files (2 bytes)
*ptr++ = LO_BYTE(currentFiles);
*ptr++ = HI_BYTE(currentFiles);
// Reserved (2 bytes)
*ptr++ = 0x00;
*ptr++ = 0x00;
// User description (24 bytes)
strncpy((char *)ptr, (char *)otherArchive->getName(), 24);
for (unsigned i = 0; i < 24; i++, ptr++)
*ptr = ascii2pet(*ptr);
// Tape entries
uint32_t tapePosition = 64 + maxFiles * 32; // data of item 0 starts here
memset(ptr, 0, 32 * maxFiles);
for (unsigned n = 0; n < maxFiles; n++) {
if (n >= currentFiles) {
// Empty tape slot
ptr += 32;
continue;
}
otherArchive->selectItem(n);
// Entry used (1 byte)
*ptr++ = 0x01;
// File type (1 byte)
*ptr++ = 0x82;
// Start address (2 bytes)
uint16_t startAddr = otherArchive->getDestinationAddrOfItem();
*ptr++ = LO_BYTE(startAddr);
*ptr++ = HI_BYTE(startAddr);
// Start address (2 bytes)
uint16_t endAddr = startAddr + otherArchive->getSizeOfItem();
*ptr++ = LO_BYTE(endAddr);
*ptr++ = HI_BYTE(endAddr);
// Reserved (2 bytes)
ptr += 2;
// Tape position (4 bytes)
*ptr++ = LO_BYTE(tapePosition);
*ptr++ = LO_BYTE(tapePosition >> 8);
*ptr++ = LO_BYTE(tapePosition >> 16);
*ptr++ = LO_BYTE(tapePosition >> 24);
tapePosition += otherArchive->getSizeOfItem();
// Reserved (4 bytes)
ptr += 4;
// File name (16 bytes)
strncpy((char *)ptr, (char *)otherArchive->getNameOfItem(), 16);
for (unsigned i = 0; i < 16; i++, ptr++)
*ptr = ascii2pet(*ptr);
}
// File data
for (unsigned n = 0; n < currentFiles; n++) {
int byte;
otherArchive->selectItem(n);
while ((byte = otherArchive->readItem()) != EOF) {
*ptr++ = (uint8_t)byte;
}
}
otherArchive->dumpDirectory();
archive->dumpDirectory();
archive->debug(1, "T64 archive created with other archive of type %s.\n",
otherArchive->typeAsString());
return archive;
}
bool
T64File::repair()
{
unsigned i, n;
uint16_t noOfItems = numberOfItems();
//
// 1. Repair number of items, if this value is zero
//
if (noOfItems == 0) {
while (directoryItemIsPresent(noOfItems))
noOfItems++;
uint16_t noOfItemsStatedInHeader = numberOfItems();
if (noOfItems != noOfItemsStatedInHeader) {
debug(1, "Repairing corrupted T64 archive: Changing number of items from %d to %d.\n",
noOfItemsStatedInHeader, noOfItems);
data[0x24] = LO_BYTE(noOfItems);
data[0x25] = HI_BYTE(noOfItems);
}
assert(noOfItems == numberOfItems());
}
for (i = 0; i < noOfItems; i++) {
//
// 2. Check relative offset information for each item
//
// Compute start address in file
n = 0x48 + (i * 0x20);
uint16_t startAddrInContainer = LO_LO_HI_HI(data[n], data[n+1], data[n+2], data[n+3]);
if (startAddrInContainer >= size) {
warn("T64 archive is corrupt (offset mismatch). Sorry, can't repair.\n");
return false;
}
//
// 3. Check for file end address mismatches (as created by CONVC64)
//
// Compute start address in memory
n = 0x42 + (i * 0x20);
uint16_t startAddrInMemory = LO_HI(data[n], data[n+1]);
// Compute end address in memory
n = 0x44 + (i * 0x20);
uint16_t endAddrInMemory = LO_HI(data[n], data[n+1]);
if (endAddrInMemory == 0xC3C6) {
// Let's assume that the rest of the file data belongs to this file ...
uint16_t fixedEndAddrInMemory = startAddrInMemory + (size - startAddrInContainer);
debug(1, "Repairing corrupted T64 archive: Changing end address of item %d from %04X to %04X.\n",
i, endAddrInMemory, fixedEndAddrInMemory);
data[n] = LO_BYTE(fixedEndAddrInMemory);
data[n+1] = HI_BYTE(fixedEndAddrInMemory);
}
}
return 1; // Archive repaired successfully
}
static BOOL UI_WriteLogoFileName(UINT32 logoId)
{
FST_FILE pFile;
UINT32 uiFileSize;
UINT8 *BgBuf;
char *fileName;
URECT Rect = {0, 0, 320, 240};
ER erReturn = E_OK;
PSTORE_SECTION_HANDLE* pSecHdl; // to save the handle of PStore section
char *psecnameId;
if (logoId == UI_LOGO_POWERON)
{
fileName = "A:\\logo.jpg";
psecnameId = PS_POWERON_LOGO;
}
else //if (logoId == UI_LOGO_POWEROFF)
{
fileName = "A:\\logo2.jpg";
psecnameId = PS_POWEROFF_LOGO;
}
FileSys_WaitFinish();
pFile = FileSys_OpenFile(fileName, FST_OPEN_READ);
if (pFile == NULL)
{
debug_err(("open file error\r\n"));
return FALSE;
}
uiFileSize = FileSys_GetFileLen(fileName); // for small page nand
USE_MSG(("file size = %d\r\n",uiFileSize));
if ((uiFileSize < 1024) || (uiFileSize > LOGO_MAX_FILESIZE))
{
Cal_FillRect(&Rect, _OSD_INDEX_BLACK);
Cal_ShowStringByColor("jpg size is too big or too small\n", &Rect, _OSD_INDEX_YELLOW, _OSD_INDEX_BLACK);
Delay_DelayMs(2000);
Cal_FillRect(&Rect, _OSD_INDEX_BLACK);
debug_err(("Write logo error\n\r"));
return FALSE;
}
BgBuf = (UINT8 *)OS_GetMempoolAddr(POOL_ID_APP);
if (FileSys_ReadFile(pFile,(UINT8 *)(BgBuf+4),&uiFileSize,FST_FLAG_NONE,NULL)!=FST_STA_OK)
{
USE_MSG(("file read failed\r\n"));
return FALSE;
}
FileSys_CloseFile(pFile);
*BgBuf = LO_BYTE(LO_WORD(uiFileSize));
*(BgBuf+1) = HI_BYTE(LO_WORD(uiFileSize));
*(BgBuf+2) = LO_BYTE(HI_WORD(uiFileSize));
*(BgBuf+3) = HI_BYTE(HI_WORD(uiFileSize));
pSecHdl = PStore_OpenSection(psecnameId, PS_RDWR | PS_CREATE);
if (pSecHdl == E_PS_SECHDLER)
{
debug_err(("Section open fail\r\n"));
goto err_ret;
}
erReturn = PStore_WriteSection(BgBuf, 0, SYSPARAM_LOGO_LEN, pSecHdl);
if (erReturn != E_PS_OK)
{
debug_err(("PStore program error\r\n"));
goto err_ret;
}
//#PIC#2010/07/23#Creator -begin
PStore_CloseSection(pSecHdl);
//#PIC#2010/07/23#Creator -end
return TRUE;
err_ret:
PStore_CloseSection(pSecHdl);
//PStore_DisablePS();
return FALSE;
}
void UI_UpdateCfgFile(void)
{
#if 0
UINT32 uiKeyAct, uiKeyCode;
FLGPTN uiFlag;
#endif
char* str;
FST_FILE pFile;
UINT32 uiFileSize;
UINT8 *BgBuf;
char *fileName;
URECT Rect = {0, 0, 320, 240};
BOOL Ret =TRUE ;
ER erReturn;
PSTORE_SECTION_HANDLE* pSecHdl; // to save the handle of PStore section
fileName = "A:\\NTCONFIG.bin";
FileSys_WaitFinish();
pFile = FileSys_OpenFile(fileName, FST_OPEN_READ);
if (pFile == NULL)
{
debug_err(("open file error\r\n"));
Ret =FALSE;
}
uiFileSize = FileSys_GetFileLen(fileName); // for small page nand
USE_MSG(("file size = %d\r\n",uiFileSize));
//if file is null
if (uiFileSize == 0)
{
USE_MSG(("file size is 0\r\n"));
Ret =FALSE;
}
if (uiFileSize > CFG_MAX_FILESIZE)
{
USE_MSG(("file size is bigger = %d\r\n",uiFileSize));
Ret =FALSE;
}
//get_blk((void *)&BgBuf, POOL_ID_SICD);
//rel_blk(POOL_ID_SICD, BgBuf);
//SysGetSICDAddr((UINT32*)&BgBuf);
BgBuf = (UINT8 *)OS_GetMempoolAddr(POOL_ID_APP);
if (FileSys_ReadFile(pFile,BgBuf,&uiFileSize,FST_FLAG_NONE,NULL)!=FST_STA_OK)
{
USE_MSG(("file read failed\r\n"));
Ret =FALSE;
}
FileSys_CloseFile(pFile);
*BgBuf = LO_BYTE(LO_WORD(uiFileSize));
*(BgBuf+1) = HI_BYTE(LO_WORD(uiFileSize));
*(BgBuf+2) = LO_BYTE(HI_WORD(uiFileSize));
*(BgBuf+3) = HI_BYTE(HI_WORD(uiFileSize));
//PStore_EnablePS();
pSecHdl = PStore_OpenSection(PS_BG_CFG, PS_RDWR | PS_CREATE);
if (pSecHdl == E_PS_SECHDLER)
{
debug_err(("Section open fail\r\n"));
PStore_CloseSection(pSecHdl);
//PStore_DisablePS();
Ret =FALSE;
}
erReturn = PStore_WriteSection(BgBuf, 0, SYSPARAM_SYSFLAG_LEN, pSecHdl);
PStore_CloseSection(pSecHdl);
if (erReturn != E_PS_OK)
{
debug_err(("PStore program error\r\n"));
Ret =FALSE;
}
Cal_FillRect(&Rect, _OSD_INDEX_BLACK);
Rect.x = 56;
Rect.y = 108;
Rect.w = 212;
Rect.h = 24;
if (Ret == FALSE)
{
str = "Update Cfg file error";
Cal_ShowStringByColor(str, &Rect, _OSD_INDEX_YELLOW, _OSD_INDEX_BLACK);
}else
{
str = "Update Cfg file ok";
Cal_ShowStringByColor(str, &Rect, _OSD_INDEX_YELLOW, _OSD_INDEX_BLACK);
}
Delay_DelayMs(1000);
if (Ret)
{
System_PowerOff(SYS_POWEROFF_NORMAL);
}
#if 0
clr_flg(FLG_ID_CALIBRATION, 0xffffffff);
while (1)
{
wai_flg(&uiFlag, FLG_ID_CALIBRATION, 0xffffffff, TWF_ORW | TWF_CLR);
debug_msg("^GuiFlag:%x\r\n",uiFlag);
if (uiFlag)
break;
}
#else
Delay_DelayMs(1500);
#endif
}
/** Enumerate and init all slaves.
*
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
* @return Workcounter of slave discover datagram = number of slaves found
*/
int ec_config_init(uint8 usetable)
{
uint16 w, slave, ADPh, configadr, mbx_wo, mbx_ro, mbx_l, ssigen;
uint16 topology, estat;
int16 topoc, slavec;
uint8 b,h;
uint8 zbuf[64];
uint8 SMc;
uint32 eedat;
int wkc, cindex, nSM;
ec_slavecount = 0;
/* clean ec_slave array */
memset(&ec_slave, 0x00, sizeof(ec_slave));
memset(&zbuf, 0x00, sizeof(zbuf));
w = 0x0000;
wkc = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
if (wkc > 0)
{
ec_slavecount = wkc;
b = 0x00;
ec_BWR(0x0000, ECT_REG_DLPORT, sizeof(b), &b, EC_TIMEOUTRET); /* deact loop manual */
w = htoes(0x0004);
ec_BWR(0x0000, ECT_REG_IRQMASK, sizeof(w), &w, EC_TIMEOUTRET); /* set IRQ mask */
ec_BWR(0x0000, ECT_REG_RXERR, 8, &zbuf, EC_TIMEOUTRET); /* reset CRC counters */
ec_BWR(0x0000, ECT_REG_FMMU0, 16 * 3, &zbuf, EC_TIMEOUTRET); /* reset FMMU's */
ec_BWR(0x0000, ECT_REG_SM0, 8 * 4, &zbuf, EC_TIMEOUTRET); /* reset SyncM */
ec_BWR(0x0000, ECT_REG_DCSYSTIME, 4, &zbuf, EC_TIMEOUTRET); /* reset system time+ofs */
w = htoes(0x1000);
ec_BWR(0x0000, ECT_REG_DCSPEEDCNT, sizeof(w), &w, EC_TIMEOUTRET); /* DC speedstart */
w = htoes(0x0c00);
ec_BWR(0x0000, ECT_REG_DCTIMEFILT, sizeof(w), &w, EC_TIMEOUTRET); /* DC filt expr */
b = 0x00;
ec_BWR(0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ec_BWR(0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET); /* Reset all slaves to Init */
b = 2;
ec_BWR(0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET); /* force Eeprom from PDI */
b = 0;
ec_BWR(0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET); /* set Eeprom to master */
for (slave = 1; slave <= ec_slavecount; slave++)
{
ADPh = (uint16)(1 - slave);
ec_slave[slave].Itype = etohs(ec_APRDw(ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET)); /* read interface type of slave */
/* a node offset is used to improve readibility of network frames */
/* this has no impact on the number of addressable slaves (auto wrap around) */
ec_APWRw(ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET); /* set node address of slave */
if (slave == 1)
{
b = 1; /* kill non ecat frames for first slave */
}
else
{
b = 0; /* pass all frames for following slaves */
}
ec_APWRw(ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET); /* set non ecat frame behaviour */
configadr = etohs(ec_APRDw(ADPh, ECT_REG_STADR, EC_TIMEOUTRET));
ec_slave[slave].configadr = configadr;
ec_FPRD(configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET);
estat = etohs(estat);
if ((estat & (1 << 6))) /* check if slave can read 8 byte chunks */
{
ec_slave[slave].eep_8byte = 1;
}
ec_readeeprom1(slave, ECT_SII_MANUF); /* Manuf */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_man = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* Manuf */
ec_readeeprom1(slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_id = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* ID */
ec_readeeprom1(slave, ECT_SII_REV); /* revision */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_rev = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* revision */
ec_readeeprom1(slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
eedat = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */
ec_slave[slave].mbx_wo = (uint16)LO_WORD(eedat);
ec_slave[slave].mbx_l = (uint16)HI_WORD(eedat);
if (ec_slave[slave].mbx_l > 0)
{
ec_readeeprom1(slave, ECT_SII_TXMBXADR); /* read mailbox offset */
}
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
if (ec_slave[slave].mbx_l > 0)
{
ec_slave[slave].mbx_ro = (uint16)etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* read mailbox offset */
}
configadr = ec_slave[slave].configadr;
mbx_ro = ec_slave[slave].mbx_ro;
mbx_wo = ec_slave[slave].mbx_wo;
mbx_l = ec_slave[slave].mbx_l;
if ((etohs(ec_FPRDw(configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET)) & 0x04) > 0) /* Support DC? */
{
ec_slave[slave].hasdc = TRUE;
}
else
{
ec_slave[slave].hasdc = FALSE;
}
topology = etohs(ec_FPRDw(configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET)); /* extract topology from DL status */
h = 0;
b = 0;
if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */
{
h++;
b |= 0x01;
}
if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */
{
h++;
b |= 0x02;
}
if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */
{
h++;
b |= 0x04;
}
if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */
{
h++;
b |= 0x08;
}
/* ptype = Physical type*/
ec_slave[slave].ptype = LO_BYTE(etohs(ec_FPRDw(configadr, ECT_REG_PORTDES, EC_TIMEOUTRET)));
ec_slave[slave].topology = h;
ec_slave[slave].activeports = b;
/* 0=no links, not possible */
/* 1=1 link , end of line */
/* 2=2 links , one before and one after */
/* 3=3 links , split point */
/* 4=4 links , cross point */
/* search for parent */
ec_slave[slave].parent = 0; /* parent is master */
if (slave > 1)
{
topoc = 0;
slavec = slave - 1;
do
{
topology = ec_slave[slavec].topology;
if (topology == 1)
{
topoc--; /* endpoint found */
}
if (topology == 3)
{
topoc++; /* split found */
}
if (topology == 4)
{
topoc+=2; /* cross found */
}
if (((topoc >= 0) && (topology > 1)) ||
(slavec == 1)) /* parent found */
{
ec_slave[slave].parent = slavec;
slavec = 1;
}
slavec--;
}
while (slavec > 0);
}
w = ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */
/* set default mailbox configuration if slave has mailbox */
if (ec_slave[slave].mbx_l>0)
{
ec_slave[slave].SMtype[0] = 0;
ec_slave[slave].SMtype[1] = 1;
ec_slave[slave].SMtype[2] = 2;
ec_slave[slave].SMtype[3] = 3;
ec_slave[slave].SM[0].StartAddr = htoes(ec_slave[slave].mbx_wo);
ec_slave[slave].SM[0].SMlength = htoes(ec_slave[slave].mbx_l);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
ec_slave[slave].SM[1].StartAddr = htoes(ec_slave[slave].mbx_ro);
ec_slave[slave].SM[1].SMlength = htoes(ec_slave[slave].mbx_l);
ec_slave[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
ec_slave[slave].mbx_proto = ec_readeeprom (slave, ECT_SII_MBXPROTO, EC_TIMEOUTEEP);
}
cindex = 0;
/* use configuration table ? */
if (usetable)
{
cindex = ec_findconfig( ec_slave[slave].eep_man, ec_slave[slave].eep_id );
ec_slave[slave].configindex= cindex;
}
/* slave found in configuration table ? */
if (cindex)
{
ec_slave[slave].Dtype = ec_configlist[cindex].Dtype;
strcpy( ec_slave[slave].name ,ec_configlist[cindex].name);
ec_slave[slave].Ibits = ec_configlist[cindex].Ibits;
ec_slave[slave].Obits = ec_configlist[cindex].Obits;
if (ec_slave[slave].Obits)
{
ec_slave[slave].FMMU0func = 1;
}
if (ec_slave[slave].Ibits)
{
ec_slave[slave].FMMU1func = 2;
}
ec_slave[slave].FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
ec_slave[slave].FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
ec_slave[slave].SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
ec_slave[slave].SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
/* simple (no mailbox) output slave found ? */
if (ec_slave[slave].Obits && !ec_slave[slave].SM[2].StartAddr)
{
ec_slave[slave].SM[0].StartAddr = htoes(0x0f00);
ec_slave[slave].SM[0].SMlength = htoes((ec_slave[slave].Obits + 7) / 8);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
ec_slave[slave].FMMU[0].FMMUactive = 1;
ec_slave[slave].FMMU[0].FMMUtype = 2;
ec_slave[slave].SMtype[0] = 2;
}
/* complex output slave */
else
{
ec_slave[slave].SM[2].SMlength = htoes((ec_slave[slave].Obits + 7) / 8);
ec_slave[slave].SMtype[2] = 2;
}
ec_slave[slave].SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
ec_slave[slave].SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
/* simple (no mailbox) input slave found ? */
if (ec_slave[slave].Ibits && !ec_slave[slave].SM[3].StartAddr)
{
ec_slave[slave].SM[1].StartAddr = htoes(0x1000);
ec_slave[slave].SM[1].SMlength = htoes((ec_slave[slave].Ibits + 7) / 8);
ec_slave[slave].SM[1].SMflags = htoel(0x00000000);
ec_slave[slave].FMMU[1].FMMUactive = 1;
ec_slave[slave].FMMU[1].FMMUtype = 1;
ec_slave[slave].SMtype[1] = 3;
}
/* complex input slave */
else
{
ec_slave[slave].SM[3].SMlength = htoes((ec_slave[slave].Ibits + 7) / 8);
ec_slave[slave].SMtype[3] = 3;
}
}
/* slave not in configuration table, find out via SII */
else
{
ssigen = ec_siifind(slave, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
ec_slave[slave].CoEdetails = ec_siigetbyte(slave, ssigen + 0x07);
ec_slave[slave].FoEdetails = ec_siigetbyte(slave, ssigen + 0x08);
ec_slave[slave].EoEdetails = ec_siigetbyte(slave, ssigen + 0x09);
ec_slave[slave].SoEdetails = ec_siigetbyte(slave, ssigen + 0x0a);
if((ec_siigetbyte(slave, ssigen + 0x0d) & 0x02) > 0)
{
ec_slave[slave].blockLRW = 1;
ec_slave[0].blockLRW++;
}
ec_slave[slave].Ebuscurrent = ec_siigetbyte(slave, ssigen + 0x0e);
ec_slave[slave].Ebuscurrent += ec_siigetbyte(slave, ssigen + 0x0f) << 8;
ec_slave[0].Ebuscurrent += ec_slave[slave].Ebuscurrent;
}
/* SII strings section */
if (ec_siifind(slave, ECT_SII_STRING) > 0)
{
ec_siistring(ec_slave[slave].name, slave, 1);
}
/* no name for slave found, use constructed name */
else
{
sprintf(ec_slave[slave].name, "? M:%8.8x I:%8.8x",
(unsigned int)ec_slave[slave].eep_man, (unsigned int)ec_slave[slave].eep_id);
}
/* SII SM section */
nSM = ec_siiSM (slave,&ec_SM);
if (nSM>0)
{
ec_slave[slave].SM[0].StartAddr = htoes(ec_SM.PhStart);
ec_slave[slave].SM[0].SMlength = htoes(ec_SM.Plength);
ec_slave[slave].SM[0].SMflags = htoel((ec_SM.Creg) + (ec_SM.Activate << 16));
SMc = 1;
while ((SMc < EC_MAXSM) && ec_siiSMnext(slave, &ec_SM, SMc))
{
ec_slave[slave].SM[SMc].StartAddr = htoes(ec_SM.PhStart);
ec_slave[slave].SM[SMc].SMlength = htoes(ec_SM.Plength);
ec_slave[slave].SM[SMc].SMflags = htoel((ec_SM.Creg) + (ec_SM.Activate << 16));
SMc++;
}
}
/* SII FMMU section */
if (ec_siiFMMU(slave, &ec_FMMU))
{
if (ec_FMMU.FMMU0 !=0xff)
ec_slave[slave].FMMU0func = ec_FMMU.FMMU0;
if (ec_FMMU.FMMU1 !=0xff)
ec_slave[slave].FMMU1func = ec_FMMU.FMMU1;
if (ec_FMMU.FMMU2 !=0xff)
ec_slave[slave].FMMU2func = ec_FMMU.FMMU2;
if (ec_FMMU.FMMU3 !=0xff)
ec_slave[slave].FMMU3func = ec_FMMU.FMMU3;
}
}
if (ec_slave[slave].mbx_l > 0)
{
if (ec_slave[slave].SM[0].StartAddr == 0x0000) /* should never happen */
{
ec_slave[slave].SM[0].StartAddr = htoes(0x1000);
ec_slave[slave].SM[0].SMlength = htoes(0x0080);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
ec_slave[slave].SMtype[0] = 0;
}
if (ec_slave[slave].SM[1].StartAddr == 0x0000) /* should never happen */
{
ec_slave[slave].SM[1].StartAddr = htoes(0x1080);
ec_slave[slave].SM[1].SMlength = htoes(0x0080);
ec_slave[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
ec_slave[slave].SMtype[1] = 1;
}
/* program SM0 mailbox in for slave */
ec_FPWR (configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET);
/* program SM1 mailbox out for slave */
// usleep(1000); // was needed for NETX (needs internal time after SM update)
ec_FPWR (configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET);
}
ec_FPWRw(configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP | EC_STATE_ACK) , EC_TIMEOUTRET); /* set preop status */
}
}
return wkc;
}
UINT32 UserPS_WriteUniFontFile(CHAR *pFilename)
{
PPSTORE_SECTION_HANDLE pSection;
HNVT_FILE *pFile;
UINT32 uiFileSize, uiWriteSize;
UINT8 *pMemPool;
FilesysWaitInitFinish(FST_TIME_INFINITE);
FilesysWaitCmdFinish(FST_TIME_INFINITE);
pFile = Filesys_fopen((char *)pFilename, "R");
if(pFile == NULL)
{
debug_err(("UserPS: Error opening %s\r\n", pFilename));
return E_USERPS_FILE;
}
uiFileSize = pFile->fileSize;
uiWriteSize = uiFileSize;
debug_err(("UserPS: File size = %ld\r\n", uiFileSize));
FilesysCloseActFile();
if(uiFileSize > POOL_SIZE_UNIFONT)
{
debug_err(("UserPS: File size too large!\r\n"));
return E_USERPS_FILE;
}
get_blk((void *)&pMemPool, POOL_ID_CAPTURE);
rel_blk(POOL_ID_CAPTURE, pMemPool);
pMemPool = (pMemPool + POOL_SIZE_CAPTURE - POOL_SIZE_UNIFONT);
FilesysReadWriteByName2(FST_CMD_READ_BY_NAME, pFilename, (UINT8 *)(pMemPool+4),
&uiWriteSize, 0, FST_TIME_INFINITE);
FilesysWaitCmdFinish(FST_TIME_INFINITE);
debug_err(("UserPS: Gotta write file size = %ld\r\n", uiWriteSize));
*pMemPool = LO_BYTE(LO_WORD(uiWriteSize));
*(pMemPool+1) = HI_BYTE(LO_WORD(uiWriteSize));
*(pMemPool+2) = LO_BYTE(HI_WORD(uiWriteSize));
*(pMemPool+3) = HI_BYTE(HI_WORD(uiWriteSize));
//PStore_EnablePS();
if((pSection = PStore_OpenSection(PS_UNIFONT_DATA, PS_RDWR | PS_CREATE)) != E_PS_SECHDLER)
{
if(PStore_WriteSection(pMemPool, 0, (uiWriteSize+4), pSection) != E_PS_OK)
{
debug_err(("UserPS: PStore write Unicode font data fail\r\n"));
}
PStore_CloseSection(pSection);
//PStore_DisablePS();
}
else
{
debug_err(("UserPS: PStore open section fail\r\n"));
//PStore_DisablePS();
return E_USERPS_PSECTION;
}
return E_USERPS_OK;
}
