Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@alessiomargan
alessiomargan committed Jan 23, 2025
1 parent 4b8f849 commit 2243f96
Show file tree
Hide file tree
Showing 4 changed files with 73 additions and 32 deletions.
5 changes: 2 additions & 3 deletions soes/ecat_slv.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,7 +305,7 @@ void ecat_slv_worker (uint32_t event_mask)
void ecat_slv_poll (void)
{
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, 4);
ESCvar.Time = etohl (ESCvar.Time);

/* Check the state machine */
Expand Down Expand Up @@ -364,8 +364,7 @@ void ecat_slv_init (esc_cfg_t * config)
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus, 2);
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
}

Expand Down
82 changes: 57 additions & 25 deletions soes/esc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
#include "esc_coe.h"
#include "esc_foe.h"

#ifdef F28P65x
#ifdef F28P65X
#include "ethercat_subdevice_cpu1_hal.h"
//
// Read and Write APIs from SSC
Expand Down Expand Up @@ -155,10 +155,17 @@ uint32_t ESC_ALeventread (void)
*
* @param[in] n = Read Sync Manager no. n
*/

void ESC_SMack (uint8_t n)
{
#ifndef F28P65X
uint8_t dummy;
ESC_read ((uint16_t)(ESCREG_SM0ACTIVATE + (n << 3)), &dummy, 1);
#else
volatile uint16_t smStatus = 0x0000;
uint16_t Offset = (ESCREG_SM0ACTIVATE + (SIZEOF_SM_REGISTER*n));
HW_EscReadWord(smStatus,Offset);
#endif
}

/** Read SM Status register 0x805(+ offset to SyncManager n) and save the
Expand All @@ -168,9 +175,16 @@ void ESC_SMack (uint8_t n)
*/
void ESC_SMstatus (uint8_t n)
{
#ifndef F28P65X
_ESCsm2 *sm;
sm = (_ESCsm2 *)&ESCvar.SM[n];
ESC_read ((uint16_t)(ESCREG_SM0STATUS + (n << 3)), &(sm->Status), 1);
#else
volatile uint16_t smControlStatusReg = 0x0000;
uint16_t Offset = (ESCREG_SM0CONTROL + (SIZEOF_SM_REGISTER*n));
HW_EscReadWord(smControlStatusReg,Offset);
ESCvar.SM[n].SmRegs[0] = smControlStatusReg;
#endif
}

/** Write ESCvar.SM[n] data to ESC PDI control register 0x807(+ offset to SyncManager n).
Expand All @@ -179,9 +193,12 @@ void ESC_SMstatus (uint8_t n)
*/
void ESC_SMwritepdi (uint8_t n)
{
#ifndef F28P65X
_ESCsm2 *sm;
sm = (_ESCsm2 *)&ESCvar.SM[n];
ESC_write ((uint16_t)(ESCREG_SM0PDI + (n << 3)), &(sm->ActPDI), 1);
#else
#endif
}

/** Read ESC PDI control register 0x807(+ offset to SyncManager n) to ESCvar.SM[n] data.
Expand All @@ -190,9 +207,13 @@ void ESC_SMwritepdi (uint8_t n)
*/
void ESC_SMreadpdi (uint8_t n)
{
#ifndef F28P65X
_ESCsm2* sm;
sm = (_ESCsm2*)&ESCvar.SM[n];
ESC_read ((uint16_t)(ESCREG_SM0PDI + (n << 3)), &(sm->ActPDI), 1);
#else
#endif

}

/** Write 0 to Bit0 in SM PDI control register 0x807(+ offset to SyncManager n) to Activate the Sync Manager n.
Expand All @@ -201,7 +222,7 @@ void ESC_SMreadpdi (uint8_t n)
*/
void ESC_SMenable (uint8_t n)
{
#ifndef F28P65x
#ifndef F28P65X
_ESCsm2 *sm;
sm = (_ESCsm2 *)&ESCvar.SM[n];
sm->ActPDI &= (uint8_t)~ESCREG_SMENABLE_BIT;
Expand All @@ -227,7 +248,7 @@ void ESC_SMenable (uint8_t n)
*/
void ESC_SMdisable (uint8_t n)
{
#ifndef F28P65x
#ifndef F28P65X
_ESCsm2 *sm;
sm = (_ESCsm2 *)&ESCvar.SM[n];
sm->ActPDI |= ESCREG_SMENABLE_BIT;
Expand Down Expand Up @@ -351,8 +372,8 @@ uint8_t ESC_checkmbx (uint8_t state)
ESC_read (ESCREG_SM0, (void *) &ESCvar.SM[0], SIZEOF_SM_REGISTER);
ESC_read (ESCREG_SM1, (void *) &ESCvar.SM[1], SIZEOF_SM_REGISTER);
SM = (_ESCsm2 *) & ESCvar.SM[0];
if ((etohs (SM->PSA) != ESC_MBX0_sma) || (etohs (SM->Length) != ESC_MBX0_sml)
|| (SM->Command != ESC_MBX0_smc) || (ESCvar.SM[0].ECsm == 0))
if ((etohs (SM->PSA) != ESC_MBX0_sma) || (etohs (SM->Length) != ESC_MBX0_sml))
//|| (SM->Command != ESC_MBX0_smc) || (ESCvar.SM[0].ECsm == 0))
{
DPRINT("FAIL: 0x%04X 0x%04X|%d %d|0x%02X 0x%02X|%d == 0\n\r",
etohs (SM->PSA), ESC_MBX0_sma,
Expand All @@ -365,8 +386,8 @@ uint8_t ESC_checkmbx (uint8_t state)
return (uint8_t) (ESCinit | ESCerror); //fail state change
}
SM = (_ESCsm2 *) & ESCvar.SM[1];
if ((etohs (SM->PSA) != ESC_MBX1_sma) || (etohs (SM->Length) != ESC_MBX1_sml)
|| (SM->Command != ESC_MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
if ((etohs (SM->PSA) != ESC_MBX1_sma) || (etohs (SM->Length) != ESC_MBX1_sml))
//|| (SM->Command != ESC_MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM1;
ESC_SMdisable (0);
Expand Down Expand Up @@ -403,8 +424,11 @@ uint8_t ESC_startmbx (uint8_t state)
}
else
{
ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
//ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
//ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
ESCvar.toggle = (ESCvar.SM[1].SmRegs[1] & SM_SETTING_REPAET_REQ_MASK); //sync repeat request toggle state
ESCvar.SM[1].SmRegs[1] = ESCvar.toggle & 0x1U;
//HW_EscWriteWord(sm1Activate,(ESC_SYNCMAN_ACTIVE_OFFSET + SIZEOF_SM_REGISTER));
ESC_SMwritepdi (1);
ESCvar.MBXrun = 1;
}
Expand Down Expand Up @@ -438,8 +462,8 @@ uint8_t ESC_startmbxboot (uint8_t state)
}
else
{
ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
//ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
//ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
ESC_SMwritepdi (1);
ESCvar.MBXrun = 1;
}
Expand Down Expand Up @@ -673,7 +697,8 @@ uint8_t ESC_mbxprocess (void)
}

/* repeat request */
if (ESCvar.SM[1].ECrep != ESCvar.toggle)
//if (ESCvar.SM[1].ECrep != ESCvar.toggle)
if ((ESCvar.SM[1].SmRegs[1] & SM_SETTING_REPAET_REQ_MASK) != ESCvar.toggle)
{
if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
{
Expand All @@ -694,15 +719,18 @@ uint8_t ESC_mbxprocess (void)
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
}
ESCvar.toggle = ESCvar.SM[1].ECrep;
ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
//ESCvar.toggle = ESCvar.SM[1].ECrep;
//ESCvar.SM[1].PDIrep = ESCvar.toggle & 0x1U;
ESCvar.toggle = (ESCvar.SM[1].SmRegs[1] & SM_SETTING_REPAET_REQ_MASK);
ESCvar.SM[1].SmRegs[1] = ESCvar.toggle & 0x1U;
ESC_SMwritepdi (1);
}
return 0;
}

/* if the outmailbox is free check if we have something to send */
if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
//if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
if (ESCvar.txcue && (ESCvar.mbxfree || !(ESCvar.SM[1].SmRegs[0] & SM_STATUS_MBX_BUFFER_FULL)))
{
/* check out request mbx */
mbxhandle = ESC_outreqbuffer ();
Expand All @@ -723,11 +751,13 @@ uint8_t ESC_mbxprocess (void)
}

/* read mailbox if full and no xoe in progress */
if ((ESCvar.SM[0].MBXstat != 0) && (MBXcontrol[0].state == 0)
&& (ESCvar.mbxoutpost == 0) && (ESCvar.xoe == 0))

//if ((ESCvar.SM[0].MBXstat != 0) && (MBXcontrol[0].state == 0)
if (((ESCvar.SM[0].SmRegs[0] & SM_STATUS_MBX_BUFFER_FULL) != 0) && (MBXcontrol[0].state == 0)
&& (ESCvar.mbxoutpost == 0) && (ESCvar.xoe == 0))
{
ESC_readmbx ();
ESCvar.SM[0].MBXstat = 0;
//ESCvar.SM[0].MBXstat = 0;
if (etohs (MBh->length) == 0)
{
MBX_error (MBXERR_INVALIDHEADER);
Expand Down Expand Up @@ -785,7 +815,8 @@ uint8_t ESC_checkSM23 (uint8_t state)

/* Check SM settings */
if ((etohs (SM->PSA) != ESC_SM2_sma) ||
(SM->Command != ESC_SM2_smc))
//(SM->Command != ESC_SM2_smc))
((SM->SmRegs[0] & 0x00FF) != ESC_SM2_smc))
{
ESCvar.SMtestresult = SMRESULT_ERRSM2;
/* fail state change */
Expand All @@ -801,15 +832,15 @@ uint8_t ESC_checkSM23 (uint8_t state)
}
/* SM disabled and (SM activated or length > 0) set by master */
else if (((ESC_SM2_act & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) || (ESCvar.ESC_SM2_sml > 0)))
(/*(SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED)*/ 0 || (ESCvar.ESC_SM2_sml > 0)))
{
ESCvar.SMtestresult = SMRESULT_ERRSM2;
/* fail state change */
return (ESCpreop | ESCerror);
}
/* SM enabled and (length > 0 but SM disabled) set by master */
else if (((ESC_SM2_act & ESCREG_SYNC_ACT_ACTIVATED) > 0) &&
((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
//((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
(ESCvar.ESC_SM2_sml > 0))
{
DPRINT("%s: 0x%04X 0x%04X|%d %d|0x%02X 0x%02X|%d %d\n\r", __FUNCTION__,
Expand All @@ -833,7 +864,8 @@ uint8_t ESC_checkSM23 (uint8_t state)
SM = (_ESCsm2 *) & ESCvar.SM[3];
/* Check SM settings */
if ((etohs (SM->PSA) != ESC_SM3_sma) ||
(SM->Command != ESC_SM3_smc))
//(SM->Command != ESC_SM3_smc))
((SM->SmRegs[0] & 0x00FF) != ESC_SM3_smc))
{
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
Expand All @@ -849,16 +881,16 @@ uint8_t ESC_checkSM23 (uint8_t state)
}
/* SM disabled and (SM activated or length > 0) set by master */
else if (((ESC_SM3_act & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) || (ESCvar.ESC_SM3_sml > 0)))
(/*(SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED)*/ 0 || (ESCvar.ESC_SM3_sml > 0)))
{
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
return (ESCpreop | ESCerror);
}
/* SM enabled and (length > 0 but SM disabled) set by master */
else if (((ESC_SM3_act & ESCREG_SYNC_ACT_ACTIVATED) > 0) &&
((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
(ESCvar.ESC_SM3_sml > 0))
//((SM->ActESC & ESCREG_SYNC_ACT_ACTIVATED) == 0) &&
(ESCvar.ESC_SM3_sml > 0))
{
DPRINT("%s: 0x%04X 0x%04X|%d %d|0x%02X 0x%02X|%d %d\n\r", __FUNCTION__,
etohs (SM->PSA), ESC_SM3_sma,
Expand Down
13 changes: 11 additions & 2 deletions soes/esc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#define ESCREG_EECONTSTAT 0x0502
#define ESCREG_EEDATA 0x0508
#define ESCREG_SM0 0x0800
#define ESCREG_SM0CONTROL (ESCREG_SM0 + 4)
#define ESCREG_SM0STATUS (ESCREG_SM0 + 5)
#define ESCREG_SM0ACTIVATE (ESCREG_SM0 + 6)
#define ESCREG_SM0PDI (ESCREG_SM0 + 7)
Expand Down Expand Up @@ -354,7 +355,9 @@ typedef struct CC_PACKED
{
uint16_t PSA; // PhysicalStartAddress
uint16_t Length;

#ifdef F28P65X
uint16_t SmRegs[2];
#else
#if defined(EC_LITTLE_ENDIAN)
uint8_t Mode:2;
uint8_t Direction:2;
Expand All @@ -380,7 +383,6 @@ typedef struct CC_PACKED
uint8_t PDIrep:1;
uint8_t PDIr5:6;
#endif

#if defined(EC_BIG_ENDIAN)
uint8_t R1:1;
uint8_t WTE:1;
Expand All @@ -406,6 +408,7 @@ typedef struct CC_PACKED
uint8_t PDIrep:1;
uint8_t PDIsm:1;
#endif
#endif
} _ESCsm;
CC_PACKED_END

Expand All @@ -415,13 +418,18 @@ typedef struct CC_PACKED
{
uint16_t PSA;
uint16_t Length;
#ifdef F28P65X
uint16_t SmRegs[2];
#else
uint8_t Command;
uint8_t Status;
uint8_t ActESC;
uint8_t ActPDI;
#endif
} _ESCsm2;
CC_PACKED_END

/*
CC_PACKED_BEGIN
typedef struct CC_PACKED
{
Expand All @@ -430,6 +438,7 @@ typedef struct CC_PACKED
uint8_t Command;
} _ESCsmCompact;
CC_PACKED_END
*/

#define SIZEOF_SM_REGISTER 8 /**< \brief Each SyncManger has 8Byte Configuration/Status Registers*/

Expand Down
5 changes: 3 additions & 2 deletions soes/hal/advr_esc/esc_hw_tiESC.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,13 @@ void ESC_read (uint16_t address, void *buf, uint16_t len)
} else {
ESC_readBlock(buf, address, len);
}
ESCvar.ALevent = ESC_readWord(ESCREG_ALEVENT);
}
/** ESC write function used by the Slave stack.
*
* @param[in] address = address of ESC register to write
* @param[out] buf = pointer to buffer to write from
* @param[in] len = number of word to write
* @param[in] len = number of bytes to write
*/
//__attribute__((section(".RamFunc")))
void ESC_write (uint16_t address, void *buf, uint16_t len)
Expand All @@ -49,7 +50,7 @@ void ESC_write (uint16_t address, void *buf, uint16_t len)
} else {
ESC_writeBlock(buf, address, len);
}

ESCvar.ALevent = ESC_readWord(ESCREG_ALEVENT);
}

/* Un-used due to evb-lan9252-digio not havning any possability to
Expand Down

0 comments on commit 2243f96

Please sign in to comment.