//=============================================================================
// eFdd::AddFile
//-----------------------------------------------------------------------------
bool eFdd::AddFile(const byte* hdr, const byte* data)
{
eUdi::eTrack::eSector* s = GetSector(0, 0, 9);
if(!s)
return false;
int len = hdr[13];
int pos = s->data[0xe4] * 0x10;
eUdi::eTrack::eSector* dir = GetSector(0, 0, 1 + pos / 0x100);
if(!dir)
return false;
if(SectorDataW(s, 0xe5) < len) //disk full
return false;
memcpy(dir->data + (pos & 0xff), hdr, 14);
SectorDataW(dir, (pos & 0xff) + 14, SectorDataW(s, 0xe1));
UpdateCRC(dir);
pos = s->data[0xe1] + 16 * s->data[0xe2];
s->data[0xe1] = (pos + len) & 0x0f, s->data[0xe2] = (pos + len) >> 4;
s->data[0xe4]++;
SectorDataW(s, 0xe5, SectorDataW(s, 0xe5) - len);
UpdateCRC(s);
// goto next track. s8 become invalid
for(int i = 0; i < len; ++i, ++pos)
{
int cyl = pos / 32;
int side = (pos / 16) & 1;
if(!WriteSector(cyl, side, (pos & 0x0f) + 1, data + i * 0x100))
return false;
}
return true;
}
//=============================================================================
// eFdd::ReadScl
//-----------------------------------------------------------------------------
bool eFdd::ReadScl(const void* data, size_t data_size)
{
if(data_size < 9)
return false;
const byte* buf = (const byte*)data;
if(memcmp(data, "SINCLAIR", 8) || int(data_size) < 9 + (0x100 + 14)*buf[8])
return false;
CreateTrd();
int size = 0;
for(int i = 0; i < buf[8]; ++i)
{
size += buf[9 + 14 * i + 13];
}
if(size > 2544)
{
eUdi::eTrack::eSector* s = GetSector(0, 0, 9);
SectorDataW(s, 0xe5, size); // free sec
UpdateCRC(s);
}
const byte* d = buf + 9 + 14 * buf[8];
for(int i = 0; i < buf[8]; ++i)
{
if(!AddFile(buf + 9 + 14*i, d))
return false;
d += buf[9 + 14*i + 13]*0x100;
}
return true;
}
/*
* Adds the CRC checksum. The entire data packet up until the
* CRC checksum must already be set, because the value is a function
* of all the preceding bytes.
* Returns the total number of bytes in the packet.
*/
void SetChecksum(uint8_t* data, uint16_t num_parameters) {
// The prefix size is 8 bytes. Check overflow.
assert(num_parameters <= MAX_PACKET_BYTES - PREFIX_SIZE);
uint16_t num_bytes_before_checksum = num_parameters + PREFIX_SIZE;
uint16_t crc = UpdateCRC(0, data, num_bytes_before_checksum);
data[num_bytes_before_checksum] = LowBits(crc);
data[num_bytes_before_checksum + 1] = HighBits(crc);
}
gcc_pure
static inline uint16_t
UpdateCRC(const void *data, size_t length, uint16_t crc)
{
const uint8_t *p = (const uint8_t *)data, *end = p + length;
while (p < end)
crc = UpdateCRC(*p++, crc);
return crc;
}
// Caller is resonsible for the received_data buffer's appropriate size.
// Returns the total numberSet bytes received for packet.
// If receive timed out, value of num_bytes_received is undefined.
bool RXPacket(uint8_t* received_data, int packet_length, int* num_bytes_received) {
dxl_hal_set_timeout(packet_length);
// Repeatedly attempt to read until either we have all of the bytes
// requested or the timeout expires.
// TODO replace this logic with something that isn't so CPU intensive.
// like interrupt driven...
int bytes_read = 0;
while (bytes_read < packet_length) {
if (dxl_hal_timeout() == 1) {
return false;
}
assert(packet_length < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length);
}
// PrintPacket(data, num_bytes);
// Now attempt to match the header to the data received.
int header_match_count = 0;
int buffer_index = 0;
while (header_match_count < HEADER_SIZE) {
if (buffer_index >= bytes_read) {
return false;
}
if (received_data[buffer_index++] == HEADER[header_match_count]) {
header_match_count++;
} else {
header_match_count = 0;
}
}
// Now, the header is matched
// Move bytes down to the beginning of the data buffer.
int packet_start_index = buffer_index - HEADER_SIZE;
bytes_read -= packet_start_index;
memmove(received_data, &(received_data[packet_start_index]), bytes_read);
// Now read the remaining bytes if necessary.
while (bytes_read < packet_length) {
assert((packet_length - bytes_read) < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length - bytes_read);
if (dxl_hal_timeout() == 1) {
return false;
}
}
uint16_t expected_crc = UpdateCRC(0, received_data, bytes_read - CHECKSUM_SIZE);
uint16_t received_crc = MakeWord(received_data[bytes_read - 2], received_data[bytes_read - 1]);
if (expected_crc != received_crc) {
// CRC check failed.
return false;
}
*num_bytes_received = bytes_read;
return true;
}
//=============================================================================
// eFdd::WriteSector
//-----------------------------------------------------------------------------
bool eFdd::WriteSector(int cyl, int side, int sec, const byte* data)
{
eUdi::eTrack::eSector* s = GetSector(cyl, side, sec);
if(!s || !s->data)
return false;
int len = s->Len();
memcpy(s->data, data, len);
UpdateCRC(s);
return true;
}
int32_t DDS_A::SubmitRequest(AbstractUnit* pUnit)
{
// DDS step four (4)
// Keep sending messages until all data is send.
if(m_eState == sSendingDataOn) {
// Get the data message that we want to transmit
Register rData;
DataType eDataType;
// Ask for the download data to be transfered.
if(pUnit->GetDownloadData(m_uiDataId, m_byIndex, eDataType, rData)) {
// Compose the message.
MessageAerospace msg(
Identifier(lPrimary, idNS_C00_Request, pUnit->GetNodeId(), m_byNodeB),
eDataType,
sDataDownload,
m_byIndex,
rData
);
// The is no harm if the PostMessage returns false. This
// means that the outgoing message buffer is full and that
// we should wait a little.
if(pUnit->PostMessage(msg)) {
// If the message is posted successfully,
UpdateCRC(rData);
// check if we reached the last message.
if(m_byIndex == m_bySize) {
// In this case we should wait for the CRC from node B.
// We temporary stop sending (there is nothing to send anymore)
// Note that the timeout is still counting down.
m_eState = sSendingDataOff;
}
else {
// No, move index to next message. Next message will be
// posted in another loop.
m_byIndex++;
}
return 1; // One message posted.
}
}
// The data is not available, but it should be. This is an error.
else {
PRINTF("DDS A: Download msg data not available for DataId=%u, index=%i\n",
m_uiDataId, (int32_t)m_byIndex
);
// Report error and stop sending data.
m_eState = sInvalidDataA;
}
}
return 0;
}
UInt32 GetPresetCrc(PresetElement* pPreset)
{
Crc32 crc;
Int32 l, lCount;
UpdateCRC(crc, pPreset->strOrigin);
UpdateCRC(crc, pPreset->strDestination);
UpdateCRC(crc, pPreset->strName);
UpdateCRC(crc, pPreset->strPassword);
crc.Update(&pPreset->bCreateZipFile, sizeof(pPreset->bCreateZipFile));
crc.Update(&pPreset->lCreateZipCompressionLevel, sizeof(pPreset->lCreateZipCompressionLevel));
crc.Update(&pPreset->bCheckVersion, sizeof(pPreset->bCheckVersion));
crc.Update(&pPreset->bParseSymbols, sizeof(pPreset->bParseSymbols));
crc.Update(&pPreset->bWriteBuildInfo, sizeof(pPreset->bWriteBuildInfo));
crc.Update(&pPreset->bBatch, sizeof(pPreset->bBatch));
crc.Update(&pPreset->bRemoveSCC, sizeof(pPreset->bRemoveSCC));
lCount = pPreset->arFilters.GetElementCount();
for (l = 0; l < lCount; l++)
{
FilterElement* pFilter = pPreset->arFilters[l];
if (!pFilter)
continue;
crc.Update(&pFilter->lCondition, sizeof(pFilter->lCondition));
crc.Update(&pFilter->lAction, sizeof(pFilter->lAction));
UpdateCRC(crc, pFilter->str);
UpdateCRC(crc, pFilter->strRename);
crc.Update(&pFilter->bSetXBit, sizeof(pFilter->bSetXBit));
}
return crc.GetCrc();
}
unsigned short int calculatesCRC(unsigned char * data , int sizeOfData)
{
unsigned short int oCRC ;
unsigned int iCRC = 0xFFFFFFFF ;
int incr ;
for (incr = 0 ; incr < sizeOfData ; incr++)
{
UpdateCRC(&iCRC, data[incr]);
//fprintf (&__debug, "CRC = %x\n", iCRC) ; //à enlever, juste pour débuguer
}
oCRC = iCRC %0xFFFF ; // modulo pour mettre sur 16 bits
return(oCRC) ;
}
WORD XCalcCheck(PXVar xv, PCHAR PktBuf)
{
int i;
WORD Check;
if (xv->CheckLen == 1) { /* CheckSum */
/* Calc sum */
Check = 0;
for (i = 0; i <= xv->DataLen - 1; i++)
Check = Check + (BYTE) (PktBuf[3 + i]);
return (Check & 0xff);
} else { /* CRC */
Check = 0;
for (i = 0; i <= xv->DataLen - 1; i++)
Check = UpdateCRC(PktBuf[3 + i], Check);
return (Check);
}
}
void FinalPacketConsistencyCheck(uint8_t* data, uint16_t num_bytes) {
assert(num_bytes >= BASE_PACKET_SIZE);
// Check header
int i;
for (i = 0; i < HEADER_SIZE; i++) {
assert(data[i] == HEADER[i]);
}
// Check packet length setting
assert(MakeWord(data[LENGTH_L_ADDR], data[LENGTH_H_ADDR]) == num_bytes - PREFIX_SIZE + 1);
// Check Instruction
assert(data[INST_ADDR] > 0 && data[INST_ADDR] <= 11);
// CRC Check
uint16_t expected_crc = UpdateCRC(0, data, num_bytes - CHECKSUM_SIZE);
assert(MakeWord(data[num_bytes - 2], data[num_bytes - 1]) == expected_crc);
}
WORD YCalcCheck(PYVar yv, const PCHAR PktBuf, const WORD len)
{
int i;
WORD Check;
// CheckSum.
if (1 == yv->CheckLen)
{
// Calc sum.
Check = 0;
for (i = 0 ; i <= len - 1 ; i++)
Check = Check + (BYTE)(PktBuf[3 + i]);
return (Check & 0xff);
}
else
{
// CRC.
Check = 0;
for (i = 0 ; i <= len - 1 ; i++)
Check = UpdateCRC(PktBuf[3 + i],Check);
return (Check);
}
}
void DDS_B::ProcessRequest(AbstractUnit* pUnit, const MessageAerospace& msg)
{
// DDS step two (2)
// dtMemId defines the initial situation.
if(msg.GetDataType()==dtMemId) {
// We must send something back to node A.
// We will return one of three possibilities.
// XOn ... the transfer is accepted.
// Abort ... the transfer is busy (can't accept another).
// Invalid ... this data structure id is not supported.
int32_t iRet = ddsXOn;
if(IsBusy())
iRet = ddsAbort; // data id, number of messages
else if(!pUnit->AcceptDownload(msg.GetData().ui32, msg.GetMessageCode()))
iRet = ddsInvalid;
// Make the response message - all nodes will hear this.
MessageAerospace msgOut(
Identifier(lPrimary, idNS_C00_Response, pUnit->GetNodeId(), msg.GetSender()),
dtLong, sDataDownload, msg.GetMessageCode(),
Register(iRet)
);
// Remember some data if post was successful.
if(pUnit->PostMessage(msgOut)) {
PRINTF("DDS B - Step 2: iRet = %i\n", iRet);
if(iRet==ddsXOn) {
pUnit->SetBDownloadTimeout(0, &m_bTimeout); // clear any pending timeout.
Reset();
m_eState = sReceivingData;
m_bySize = msg.GetMessageCode(); // number of messages to accept
m_uiDataId = msg.GetData().ui32; // data structure id
m_bySender = msg.GetSender(); // remember the sender.
// We do not want to listen forever. Set the timeout.
pUnit->SetBDownloadTimeout(TRANSFER_TIMEOUT_MS(m_bySize), &m_bTimeout);
return;
}
}
// Can't post a message.
else {
PRINTF("DDS B: Unable to post a confirmation message.\n");
}
}
// DDS step five (5)
// Receiving data messages and storing them. We may return
// a message that will pause/resume or abort the download.
// If this is the last message to be received, a CRC is sent
// back. See step seven.
else if(m_eState == sReceivingData) {
//ASSERT(msg.GetMessageCode() == m_byIndex+1);
if(msg.GetMessageCode() != m_byIndex+1) {
// TODO verify this.
m_eState = sInvalidIndex;
}
else {
int32_t iRet = pUnit->StoreDownloadMessage(m_uiDataId, msg);
// The data msg was accepted.
if(iRet == ddsXOn) {
m_byIndex++;
UpdateCRC(msg.GetData());
// DDS step seven (7)
// This message termiates the download service by
// returning CRC value of downloaded data.
if(m_byIndex == m_bySize) {
MessageAerospace msgOut(
Identifier(lPrimary, idNS_C00_Response, pUnit->GetNodeId(), msg.GetSender()),
dtChksum, sDataDownload, msg.GetMessageCode(),
Register(m_crc.Get())
);
if(pUnit->PostMessage(msgOut)==false) {
PRINTF("DDS B: Unable to post final checksum message.\n");
return;
}
// We are ready to accept new download service.
pUnit->SetBDownloadTimeout(0, &m_bTimeout);
PRINTF("DDS B: Checksum = 0x%x\n", m_crc.Get());
Reset();
}
}
// We need to send iRet, because it is not rXOn.
// Here we actually implementing step five.
else {
// TODO Če je xOn po xOff, potem moramo poslati s katerim
// indeksom naj node A nadaljuje.
MessageAerospace msgOut(
Identifier(lPrimary, idNS_C00_Response, pUnit->GetNodeId(), msg.GetSender()),
dtLong, sDataDownload, msg.GetMessageCode(),
Register(iRet)
);
// Is posted successfully, remember some data
if(pUnit->PostMessage(msgOut)==false) {
PRINTF("DDS B: Unable to post message.\n");
}
}
}
}
// We are not in receiving mode anymore - most probaby due timeout.
else {
PRINTF("DDS B: Message received in sReady mode.\n");
}
}
//=============================================================================
// eFdd::CreateTrd
//-----------------------------------------------------------------------------
void eFdd::CreateTrd()
{
SAFE_DELETE(disk);
disk = new eUdi(eUdi::MAX_CYL, eUdi::MAX_SIDE);
for(int i = 0; i < disk->Cyls(); ++i)
{
for(int j = 0; j < disk->Sides(); ++j)
{
Seek(i, j);
int id_len = Track().data_len / 8 + ((Track().data_len & 7) ? 1 : 0);
memset(Track().data, 0, Track().data_len + id_len);
int pos = 0;
WriteBlock(pos, 0x4e, 80); //gap4a
WriteBlock(pos, 0, 12); //sync
WriteBlock(pos, 0xc2, 3, true); //iam
Write(pos++, 0xfc);
const int max_trd_sectors = 16;
static const byte lv[3][max_trd_sectors] =
{
{ 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 },
{ 1,9,2,10,3,11,4,12,5,13,6,14,7,15,8,16 },
{ 1,12,7,2,13,8,3,14,9,4,15,10,5,16,11,6 }
};
Track().sectors_amount = max_trd_sectors;
for(int i = 0; i < max_trd_sectors; ++i)
{
WriteBlock(pos, 0x4e, 40); //gap1 50 fixme: recalculate gap1 only for non standard formats
WriteBlock(pos, 0, 12); //sync
WriteBlock(pos, 0xa1, 3, true); //id am
Write(pos++, 0xfe);
eUdi::eTrack::eSector& sec = Sector(i);
sec.id = Track().data + pos;
Write(pos++, cyl);
Write(pos++, side);
Write(pos++, lv[trdos_interleave][i]);
Write(pos++, 1); //256byte
word crc = Crc(Track().data + pos - 5, 5);
Write(pos++, crc >> 8);
Write(pos++, (byte)crc);
WriteBlock(pos, 0x4e, 22); //gap2
WriteBlock(pos, 0, 12); //sync
WriteBlock(pos, 0xa1, 3, true); //data am
Write(pos++, 0xfb);
sec.data = Track().data + pos;
int len = sec.Len();
crc = Crc(Track().data + pos - 1, len + 1);
pos += len;
Write(pos++, crc >> 8);
Write(pos++, (byte)crc);
}
if(pos > Track().data_len)
{
assert(0); //track too long
}
WriteBlock(pos, 0x4e, Track().data_len - pos - 1); //gap3
}
}
eUdi::eTrack::eSector* s = GetSector(0, 0, 9);
if(!s)
return;
s->data[0xe2] = 1; // first free track
s->data[0xe3] = 0x16; // 80T,DS
SectorDataW(s, 0xe5, 2544); // free sec
s->data[0xe7] = 0x10; // trdos flag
UpdateCRC(s);
}
int _tmain(int argc, _TCHAR* argv[])
{
unsigned short crc = UpdateCRC(0, buffer, 6);
return 0;
}
