void FOE_write()
{
_FOE *foembx;
uint16_t data_len;
uint32_t password;
int16_t res;
if(ESCvar.foestate != FOE_READY)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
FOE_init();
foembx = (_FOE*)&MBX[0];
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
password = etohl(foembx->foeheader.x.password);
/* Get an address we can write the file to, if possible. */
res = FOE_fopen(foembx->y.filename, data_len, password, FOE_OP_WRQ);
if(res==0)
{
res = FOE_send_ack();
if(res)
FOE_abort(res);
else
ESCvar.foestate = FOE_WAIT_FOR_DATA;
}
else
FOE_abort(res);
}
void FOE_read()
{
_FOE *foembx;
uint16_t data_len;
uint32_t password;
uint16_t res;
if(ESCvar.foestate != FOE_READY)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
FOE_init();
foembx = (_FOE*)&MBX[0];
//Get the length of the file name in octets.
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
password = etohl(foembx->foeheader.x.password);
res = FOE_fopen(foembx->y.filename, data_len, password, FOE_OP_RRQ);
if(res==0)
{
ESCvar.foepacket=1;
/* Attempt to send the packet */
res = FOE_send_data_packet();
if(res <= FOE_DATA_SIZE)
{
ESCvar.foestate = FOE_WAIT_FOR_ACK;
}
else
FOE_abort(res);
}
else
FOE_abort(res);
}
/**
* Initialize the slave stack.
*/
void ecat_slv_init (esc_cfg_t * config)
{
DPRINT ("Slave stack init started\n");
ESCvar.TXPDOsize = ESCvar.ESC_SM3_sml = sizeOfPDO(TX_PDO_OBJIDX);
ESCvar.RXPDOsize = ESCvar.ESC_SM2_sml = sizeOfPDO(RX_PDO_OBJIDX);
/* Init watchdog */
watchdog = config->watchdog_cnt;
/* Call stack configuration */
ESC_config (config);
/* Call HW init */
ESC_init (config);
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
}
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx();
ESC_stopinput();
ESC_stopoutput();
}
void FOE_data()
{
_FOE *foembx;
uint32_t packet;
// uint16_t data_len, ncopied;
uint16_t data_len;
int16_t res;
if(ESCvar.foestate != FOE_WAIT_FOR_DATA)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
foembx =(_FOE*)&MBX[0];
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
packet = etohl(foembx->foeheader.x.packetnumber);
if(packet != ESCvar.foepacket)
FOE_abort(FOE_ERR_PACKETNO);
else if(ESCvar.fposition + data_len > ESCvar.fend)
FOE_abort(FOE_ERR_DISKFULL);
else
{
// ncopied = FOE_fwrite(foembx->y.data, data_len);
FOE_fwrite(foembx->y.data, data_len);
if (foe_mode == FOE_WRITE) res = FOE_send_ack(); else res=0;
if(data_len == FOE_DATA_SIZE)
{
//res = FOE_send_ack();
if(res) FOE_abort(res);
}
else
{
FOE_fclose();
FOE_init();
}
}
}
void FOE_abort(uint32_t code)
{
_FOE *foembx;
uint8_t mbxhandle;
if(code)
{
/* Send back an error packet. */
mbxhandle = ESC_claimbuffer();
if(mbxhandle)
{
foembx = (_FOE*)&MBX[mbxhandle];
foembx->mbxheader.length = htoes(FOEHSIZE); /* Don't bother with error text for now. */
foembx->mbxheader.mbxtype = MBXFOE;
foembx->foeheader.opcode = FOE_OP_ERR;
foembx->foeheader.x.errorcode = htoel(code);
MBXcontrol[mbxhandle].state = MBXstate_outreq;
}
/* Nothing we can do if we can't get an outbound mailbox. */
}
FOE_init();
}
void FOE_ack()
{
uint16_t res;
// Make sure we're able to take this.
if(ESCvar.foestate == FOE_WAIT_FOR_FINAL_ACK)
{
// Move us back to ready state.
FOE_init();
return;
}
else if(ESCvar.foestate != FOE_WAIT_FOR_ACK)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
res = FOE_send_data_packet();
if(res < FOE_DATA_SIZE)
{
ESCvar.foestate = FOE_WAIT_FOR_FINAL_ACK;
}
else if(res >= FOE_ERR_NOTDEFINED)
FOE_abort(FOE_ERR_PROGERROR);
}
/** SOES main loop. Start by initializing the stack software followed by
* the application loop for cyclic read the EtherCAT state and staus, update
* of I/O.
*/
void soes (void *arg)
{
DPRINT ("SOES (Simple Open EtherCAT Slave)\n");
TXPDOsize = SM3_sml = sizeTXPDO ();
RXPDOsize = SM2_sml = sizeRXPDO ();
/* Setup post config hooks */
static esc_cfg_t config =
{
.pre_state_change_hook = NULL,
.post_state_change_hook = post_state_change_hook
};
ESC_config ((esc_cfg_t *)&config);
ESC_reset();
ESC_init (spi_name);
task_delay (tick_from_ms (200));
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
}
/* Pre FoE to set up Application information */
bootstrap_foe_init ();
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
DPRINT ("Application_loop GO\n");
/* application run loop */
while (1)
{
/* On init restore PDO mappings to default size */
if((ESCvar.ALstatus & 0x0f) == ESCinit)
{
txpdomap = DEFAULTTXPDOMAP;
rxpdomap = DEFAULTRXPDOMAP;
txpdoitems = DEFAULTTXPDOITEMS;
rxpdoitems = DEFAULTTXPDOITEMS;
}
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
/* Check the state machine */
ESC_state ();
/* If else to two separate execution paths
* If we're running BOOSTRAP
* - MailBox
* - FoE
* Else we're running normal execution
* - MailBox
* - CoE
*/
if(local_boot_state)
{
if (ESC_mbxprocess ())
{
ESC_foeprocess ();
ESC_xoeprocess ();
}
bootstrap_state ();
}
else
{
if (ESC_mbxprocess ())
{
ESC_coeprocess ();
ESC_xoeprocess ();
}
DIG_process ();
}
};
}
