void BioloidBus::SendAction()
{
BLD_BUS_LOG( "Sending ACTION\n" );
SendCmdHeader( Bioloid::BROADCAST_ID, 0, Bioloid::CMD_ACTION );
SendCheckSum();
}
void CopyData()
{
struct HEADER {
Uint16 BlockSize;
Uint32 DestAddr;
Uint32 ProgBuffAddr;
} BlockHeader;
Uint16 wordData;
Uint16 status;
Uint16 i,j;
//Make sure code security is disabled
CsmUnlock();
EALLOW;
Flash_CPUScaleFactor = SCALE_FACTOR;
Flash_CallbackPtr = NULL;
EDIS;
status = Flash_Erase((SECTORA | SECTORB | SECTORC | SECTORD), &FlashStatus);
if(status != STATUS_SUCCESS)
{
//TODO fix so that it returns a serial error and reboot device
return;
}
// After Flash Erase, send the checksum to PC program.
SendCheckSum();
// Get the size in words of the first block
BlockHeader.BlockSize = (*GetOnlyWordData)();
// While the block size is > 0 copy the data
// to the DestAddr. There is no error checking
// as it is assumed the DestAddr is a valid
// memory location
while(BlockHeader.BlockSize != (Uint16)0x0000)
{
if(BlockHeader.BlockSize > PROG_BUFFER_LENGTH){
//Block is to big to fit into our buffer so we must program it in chunks
BlockHeader.DestAddr = GetLongData();
//Program as many full buffers as possible
for(j = 0; j < (BlockHeader.BlockSize / PROG_BUFFER_LENGTH); j++){
BlockHeader.ProgBuffAddr = (Uint32)progBuf;
for(i = 1; i <= PROG_BUFFER_LENGTH; i++)
{
wordData = (*GetOnlyWordData)();
*(Uint16 *)BlockHeader.ProgBuffAddr++ = wordData;
}
status = Flash_Program((Uint16 *) BlockHeader.DestAddr, (Uint16 *)progBuf, PROG_BUFFER_LENGTH, &FlashStatus);
if(status != STATUS_SUCCESS)
{
return;
}
BlockHeader.DestAddr += PROG_BUFFER_LENGTH;
// After Flash program, send the checksum to PC program.
SendCheckSum();
}
//Program the leftovers
BlockHeader.ProgBuffAddr = (Uint32)progBuf;
for(i = 1; i <= (BlockHeader.BlockSize % PROG_BUFFER_LENGTH); i++)
{
wordData = (*GetOnlyWordData)();
*(Uint16 *)BlockHeader.ProgBuffAddr++ = wordData;
}
status = Flash_Program((Uint16 *) BlockHeader.DestAddr, (Uint16 *)progBuf, (BlockHeader.BlockSize % PROG_BUFFER_LENGTH), &FlashStatus);
if(status != STATUS_SUCCESS)
{
return;
}
// After Flash program, send the checksum to PC program.
SendCheckSum();
}else{
//Block will fit into our buffer so we'll program it all at once
BlockHeader.DestAddr = GetLongData();
BlockHeader.ProgBuffAddr = (Uint32)progBuf;
for(i = 1; i <= BlockHeader.BlockSize; i++)
{
wordData = (*GetOnlyWordData)();
*(Uint16 *)BlockHeader.ProgBuffAddr++ = wordData;
}
status = Flash_Program((Uint16 *) BlockHeader.DestAddr, (Uint16 *)progBuf, BlockHeader.BlockSize, &FlashStatus);
if(status != STATUS_SUCCESS)
{
return;
}
// After Flash program, send the checksum to PC program.
SendCheckSum();
}
// Get the size of the next block
BlockHeader.BlockSize = (*GetOnlyWordData)();
}
return;
}
