diff --git a/f28p65x_cpu1_echoback_solution/ethercat_subdevice_cpu1_hal.c b/f28p65x_cpu1_echoback_solution/ethercat_subdevice_cpu1_hal.c index 10158e3..bd48d2d 100644 --- a/f28p65x_cpu1_echoback_solution/ethercat_subdevice_cpu1_hal.c +++ b/f28p65x_cpu1_echoback_solution/ethercat_subdevice_cpu1_hal.c @@ -415,6 +415,65 @@ ESC_writeWord(uint16_t wordValue, uint16_t address) EINT; } + + +void +ESC_WriteOddWordISR(uint16_t wordValue, uint16_t address ) +{ + + volatile uint16_t first_write , second_write , byte1 , byte2; + + // 0xff00 to get higher byte , example 0x1234 & 0xff00 = 1200 r + + // lower value of wordValue , this will be the higher byte for the (addr -1) word. + byte1 = (wordValue & 0x00ff)<<8; + + + //get lower byte of previous word and or with lower byte of word + first_write = ((ESC_readWordISR(address-1) & 0x00ff) | byte1); + + // higher msb byte of wordValue , this will be lower byte of (addr +1) word + byte2 = ((wordValue & 0xff00))>>8; + + //get higher byte of next word and OR with higher byte of addr+1 word. + second_write = (((ESC_readWordISR(address+1) & 0xff00)) | byte2 ); + + + + HWREGH(ESC_BASE + ((address-1) / 2U)) = first_write; + HWREGH(ESC_BASE + ((address+1) / 2U)) = second_write; + + + +} + +//***************************************************************************** +// +// ESC_writeWord +// +//***************************************************************************** + +void +ESC_WriteOddWord(uint16_t wordValue , uint16_t address) +{ + // + // Disables interrupts + // + DINT; + + // + // Write one 16-word into ESC memory + // + ESC_WriteOddWordISR(wordValue, address); + + // + // Enable interrupts + // + EINT; +} + + + //***************************************************************************** // // ESC_setLed diff --git a/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_hw.h b/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_hw.h index 11fb727..403477a 100644 --- a/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_hw.h +++ b/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_hw.h @@ -62,6 +62,12 @@ #define HW_EscWriteDWord(DWordValue, Address) ESC_writeDWord(DWordValue, Address) #define HW_EscWriteDWordIsr(DWordValue, Address) ESC_writeDWordISR(DWordValue, Address) #define HW_EscWriteWord(WordValue, Address) ESC_writeWord(WordValue, Address) + +//new +#define HW_EscWriteOddWord(byte, Address) ESC_WriteOddWord(byte, Address) + + +// new end. #define HW_EscWriteWordIsr(WordValue, Address) ESC_writeWordISR(WordValue, Address) #define HW_EscWriteMbxMem(pData, Address, Len) ESC_writeBlock(pData, Address, Len) diff --git a/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/mailbox.c b/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/mailbox.c index ae94564..3881664 100644 --- a/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/mailbox.c +++ b/f28p65x_cpu1_echoback_solution/f28p65x_cpu1_echoback_solution/mailbox.c @@ -820,15 +820,31 @@ void MBX_CheckAndCopyMailbox( void ) VARVOLATILE UINT16 smstate = 0x00; HW_EscReadWord(smstate,ESC_SYNCMAN_CONTROL_OFFSET); smstate = SWAPWORD(smstate); + + if(smstate & SM_STATUS_MBX_BUFFER_FULL){ - if(smstate & SM_STATUS_MBX_BUFFER_FULL) - { - /*Unlock the mailbox SyncManger buffer*/ u16dummy = 0; - HW_EscReadWord(u16dummy,(u16EscAddrReceiveMbx + u16ReceiveMbxSize - 2)); + /*Unlock the mailbox SyncManger buffer*/ + + if ((u16EscAddrReceiveMbx + u16ReceiveMbxSize - 2) & 1) + { + /* Odd mailbox size: addr+size-2 is odd, last byte is at addr+size-1 (even). + Must read that word to cover the last byte and unlock SM0. */ + HW_EscReadWord(u16dummy, (u16EscAddrReceiveMbx + u16ReceiveMbxSize - 1)); + } + else + { + + HW_EscReadWord(u16dummy, (u16EscAddrReceiveMbx + u16ReceiveMbxSize - 2)); + } + } + + + + } /* in MBX_MailboxWriteInd the mailbox protocol will be processed */ @@ -872,8 +888,8 @@ UINT8 MBX_CopyToSendMailbox( TMBX MBXMEM *pMbx ) /*ECATCHANGE_START(V5.13) MBX2*/ /*Reset the not used mailbox memory*/ { - UINT16 LastUsedAddr = u16EscAddrSendMbx + mbxSize + MBX_HEADER_SIZE; - UINT16 LastAddrToReset = (u16EscAddrSendMbx + u16SendMbxSize); + volatile UINT16 LastUsedAddr = u16EscAddrSendMbx + mbxSize + MBX_HEADER_SIZE; + volatile UINT16 LastAddrToReset = (u16EscAddrSendMbx + u16SendMbxSize); /*round down to last even 16bit address*/ LastUsedAddr = LastUsedAddr & 0xFFFE; LastAddrToReset = (LastAddrToReset - 2) & 0xFFFE; @@ -885,6 +901,18 @@ UINT8 MBX_CopyToSendMailbox( TMBX MBXMEM *pMbx ) HW_EscWriteWord(u16dummy, LastUsedAddr); LastUsedAddr = LastUsedAddr + 2; } + + // changed added to handle odd case. + if((u16EscAddrSendMbx + u16SendMbxSize - 2) & 0001){ + + LastUsedAddr = LastUsedAddr - 1; + HW_EscWriteOddWord(u16dummy, LastUsedAddr); + + } + + + + } /*ECATCHANGE_END(V5.13) MBX2*/ @@ -916,7 +944,14 @@ UINT8 MBX_CopyToSendMailbox( TMBX MBXMEM *pMbx ) MEMCPY((UINT16 *)&u16dummy,(((UINT16 *)pMbx) + LastDataAddress),(2 - BytesLeft)); } - HW_EscWriteWord(u16dummy, (u16EscAddrSendMbx + u16SendMbxSize - 2)); + if((u16EscAddrSendMbx + u16SendMbxSize - 2) & 0001){ + // odd length box. + HW_EscWriteOddWord(u16dummy, (u16EscAddrSendMbx + u16SendMbxSize - 2)); + + }else{ + HW_EscWriteWord(u16dummy, (u16EscAddrSendMbx + u16SendMbxSize - 2)); + } + } }