int MFRC522::writeToTag(byte block, byte *data) {
int status, i, len;
byte buffer[18];
buffer[0] = MF1_WRITE;
buffer[1] = block;
calculateCRC(buffer, 2, &buffer[2]);
status = commandTag(MFRC522_TRANSCEIVE, buffer, 4, buffer, &len);
if ((status != MI_OK) || (len != 4) || ((buffer[0] & 0x0F) != 0x0A)) {
status = MI_ERR;
}
if (status == MI_OK) {
for (i = 0; i < 16; i++) {
buffer[i] = data[i];
}
calculateCRC(buffer, 16, &buffer[16]);
status = commandTag(MFRC522_TRANSCEIVE, buffer, 18, buffer, &len);
if ((status != MI_OK) || (len != 4) || ((buffer[0] & 0x0F) != 0x0A)) {
status = MI_ERR;
}
}
return status;
}
/******************************************************************************
* 函 数 名：write
* 功能描述：写块数据
* 输入参数：blockAddr--块地址;writeData--向块写16字节数据
* 返 回 值：成功返回MI_OK
******************************************************************************/
unsigned char RFID::write(unsigned char blockAddr, unsigned char *writeData)
{
unsigned char status;
unsigned int recvBits;
unsigned char i;
unsigned char buff[18];
buff[0] = PICC_WRITE;
buff[1] = blockAddr;
calculateCRC(buff, 2, &buff[2]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 4, buff, &recvBits);
if ((status != MI_OK) || (recvBits != 4) || ((buff[0] & 0x0F) != 0x0A))
status = MI_ERR;
if (status == MI_OK)
{
for (i=0; i<16; i++) //?FIFO?16Byte?? Datos a la FIFO 16Byte escribir
buff[i] = *(writeData+i);
calculateCRC(buff, 16, &buff[16]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 18, buff, &recvBits);
if ((status != MI_OK) || (recvBits != 4) || ((buff[0] & 0x0F) != 0x0A))
status = MI_ERR;
}
return status;
}
/*
* MFRC522Write -> write
* La escritura de datos de bloque
* blockAddr - dirección del bloque; WriteData - para escribir 16 bytes del bloque de datos
* Valor de retorno: el retorno exitoso MI_OK
*/
uint8_t RFID::write(uint8_t blockAddr, uint8_t *writeData)
{
uint8_t status;
uint16_t recvBits;
uint8_t i;
uint8_t buff[18];
buff[0] = PICC_WRITE;
buff[1] = blockAddr;
calculateCRC(buff, 2, &buff[2]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 4, buff, &recvBits);
if ((status != MI_OK) || (recvBits != 4) || ((buff[0] & 0x0F) != 0x0A))
{
status = MI_ERR;
}
if (status == MI_OK)
{
for (i=0; i<16; i++) //?FIFO?16Byte?? Datos a la FIFO 16Byte escribir
{
buff[i] = *(writeData+i);
}
calculateCRC(buff, 16, &buff[16]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 18, buff, &recvBits);
if ((status != MI_OK) || (recvBits != 4) || ((buff[0] & 0x0F) != 0x0A))
{
status = MI_ERR;
}
}
return status;
}
int sendHdlPacage(hdlDataPacage hdlPacage){
hdlDataPacage localHdlDataPacage = hdlPacage;
char txBuffer[100];
int i;
unsigned int crc;
txBuffer[0] = RS_HEADER[0];
txBuffer[1] = RS_HEADER[1];
txBuffer[2] = (char)(MIN_PACKAGE_SIZE + localHdlDataPacage.additionContent.size);
txBuffer[3] = localHdlDataPacage.orgSubnet;
txBuffer[4] = localHdlDataPacage.orgDeviceId;
txBuffer[5] = getHighByte(localHdlDataPacage.devType);
txBuffer[6] = getLowByte(localHdlDataPacage.devType);
txBuffer[7] = getHighByte(localHdlDataPacage.operCode);
txBuffer[8] = getLowByte(localHdlDataPacage.operCode);
txBuffer[9] = localHdlDataPacage.targSubnet;
txBuffer[10]= localHdlDataPacage.targId;
if (localHdlDataPacage.additionContent.size > 0){
for (i = 11; (localHdlDataPacage.additionContent.size + 10); i++){
txBuffer[i] = localHdlDataPacage.additionContent.content[i-10];
}
}
i = 10 + localHdlDataPacage.additionContent.size;
crc = calculateCRC((unsigned int)i, txBuffer);
i+=1;
txBuffer[i] = getHighByte(crc);
i+=1;
txBuffer[i] = getLowByte(crc);
open_closeUART(1);
// write(uart0_filestream, &tx_buffer, sizeof(txBuffer);
open_closeUART(0);
}
bool readblock(byte block_address, byte * data_out) override {
byte read[2] = {0x30, block_address};
byte Intermediate[18];
byte data_crc[2];
byte check_crc[2];
byte data[16];
trancieve(read, 2, Intermediate, nullptr, true, false);
for (int i = 0; i < 16; i++){
data[i] = Intermediate[i];
}
for (int i = 0; i < 2; i++){
data_crc[i] = Intermediate[i + 16];
}
calculateCRC(data, 16, check_crc);
for (int i = 0; i < 2; i++){
if(data_crc[i] != check_crc[i]){
hwlib::cout << "CRC of recieved data does not match\n";
return false;
}
}
for (int i = 0; i < 16; i++){
data_out[i] = data[i];
}
return true;
}
/******************************************************************************
* 函 数 名：selectTag
* 功能描述：选卡，读取卡存储器容量
* 输入参数：serNum--传入卡序列号
* 返 回 值：成功返回卡容量
******************************************************************************/
unsigned char RFID::selectTag(unsigned char *serNum)
{
unsigned char i;
unsigned char status;
unsigned char size;
unsigned int recvBits;
unsigned char buffer[9];
//ClearBitMask(Status2Reg, 0x08); //MFCrypto1On=0
buffer[0] = PICC_SElECTTAG;
buffer[1] = 0x70;
for (i=0; i<5; i++)
buffer[i+2] = *(serNum+i);
calculateCRC(buffer, 7, &buffer[7]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buffer, 9, buffer, &recvBits);
if ((status == MI_OK) && (recvBits == 0x18))
size = buffer[0];
else
size = 0;
return size;
}
// private functions
byte SmeSFX::insertCRC(char *crcPos, const char *payload, byte msgType, byte seqNumber, byte payloadLen) {
word crc = calculateCRC(payloadLen, msgType, seqNumber, payload);
char* tmp = (char*)&crc;
crcPos[0] = tmp[0];
crcPos[1] = tmp[1];
return 2;
}
bool ReadingData::checkCRC(uint32_t crc) {
// if (calculateCRC() == crc)
// return true;
if ((calculateCRC() ^ crc) == 0)
return true;
return false;
}
void MFRC522::haltTag() {
int status, len;
byte buffer[4];
buffer[0] = MF1_HALT;
buffer[1] = 0;
calculateCRC(buffer, 2, &buffer[2]);
clearBitMask(Status2Reg, 0x08); // turn off encryption
status = commandTag(MFRC522_TRANSCEIVE, buffer, 4, buffer, &len);
}
/*
* MFRC522Halt -> halt
* Cartas de Mando para dormir
* Los parámetros de entrada: Ninguno
* Valor devuelto: Ninguno
*/
void RFID::halt()
{
unsigned char status;
unsigned int unLen;
unsigned char buff[4];
buff[0] = PICC_HALT;
buff[1] = 0;
calculateCRC(buff, 2, &buff[2]);
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 4, buff,&unLen);
}
sfxRxFSME SmeSFX::crcCheck(void) {
word crc = calculateCRC(answer.length, answer.type,
answer.sequenceNumber, (const char*)answer.payload);
word *receivedCrc=(word *)answer.crc;
if (((word)*receivedCrc) == crc) {
return tailerRec;
}
else {
sfxError = SME_SFX_KO;
return nullState;
}
}
int MFRC522::readFromTag(byte block, byte *result) {
int status, len;
result[0] = MF1_READ;
result[1] = block;
calculateCRC(result, 2, &result[2]);
status = commandTag(MFRC522_TRANSCEIVE, result, 4, result, &len);
if ((status != MI_OK) || (len != 0x90)) {
status = MI_ERR;
}
return status;
}
/*
* MFRC522Halt -> halt
* Cartas de Mando para dormir
* Los parámetros de entrada: Ninguno
* Valor devuelto: Ninguno
*/
void RFID::halt()
{
uint8_t status;
uint16_t unLen;
uint8_t buff[4];
buff[0] = PICC_HALT;
buff[1] = 0;
calculateCRC(buff, 2, &buff[2]);
clearBitMask(Status2Reg, 0x08); // turn off encryption
status = MFRC522ToCard(PCD_TRANSCEIVE, buff, 4, buff,&unLen);
}
/******************************************************************************
* 函 数 名：read
* 功能描述：读块数据
* 输入参数：blockAddr--块地址;recvData--读出的块数据
* 返 回 值：成功返回MI_OK
******************************************************************************/
unsigned char RFID::read(unsigned char blockAddr, unsigned char *recvData)
{
unsigned char status;
unsigned int unLen;
recvData[0] = PICC_READ;
recvData[1] = blockAddr;
calculateCRC(recvData,2, &recvData[2]);
status = MFRC522ToCard(PCD_TRANSCEIVE, recvData, 4, recvData, &unLen);
if ((status != MI_OK) || (unLen != 0x90))
status = MI_ERR;
return status;
}
/* Request Cunstruct packet to send
* @param: none
* @return: none
* @private
* @comment: none
*/
void Modbus::constructPacket()
{
//Disable next transmission until get response packet or timeout
_transmission_ready_flag = false;
_packet->requests++;
_total_request++; //calculate total packets are requested
//Modbus Application Protocol v1.13b
frame[0] = _packet->id;
frame[1] = _packet->function;
frame[2] = _packet->address >> 8; //Address Hi
frame[3] = _packet->address & 0xFF; //Address Lo
//If packet is single regiser, data is what it is
if ( _packet->function == PRESET_SINGLE_REGISTER ){
_packet->data = _register_array[_packet->register_start_address];
}
//2 bytes address
frame[4] = _packet->data >> 8; //total registers Hi
frame[5] = _packet->data & 0xFF; //total registers Lo
//Frame size for function code 3, 4 & 6 = 8
uint8_t frameSize;
if (_packet->function == PRESET_MULTIPLE_REGISTERS)
frameSize = construct_F16();
else if (_packet->function == FORCE_MULTIPLE_COILS)
frameSize = construct_F15();
else // else functions 1,2,3,4,5 & 6 is assumed. They all share the exact same request format.
frameSize = 8; // the request is always 8 bytes in size for the above mentioned functions.
//Error check CRC16
uint16_t crc16 = calculateCRC(frameSize - 2); //First 6 bytes of frame is used to calculate CRC error check
frame[frameSize - 2] = crc16 >> 8; //High byte of CRC
frame[frameSize - 1] = crc16 & 0xFF; //Low byte of CRC
#if DEBUG_UART
print("Request: ");
for (uint8_t i=0;i<frameSize;i++)
print(frame[i]);
println();
#endif
//Send packet frame to slave
sendPacket(frameSize);
}
bool select_card(byte * Cardserial) override {
byte Get_UID[2] = {0x93, 0x20};
byte Sel_card[7] = {0x93, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00};
byte UID[5];
byte Sak[3];
byte CRCcheck[2];
int SAK_size;
int UID_lenght;
trancieve(Get_UID, 2, UID, &UID_lenght, false, false);
byte CRC_A = UID[0] ^ UID[1] ^ UID[2] ^ UID[3];
if (UID[4] != CRC_A){
hwlib::cout << "Could not select card. UID-CRC does not match expected.\n";
return false;
}
hwlib::wait_ms(333);
for(int i = 0; i < 5; i++){
Sel_card[i + 2] = UID[i];
}
trancieve(Sel_card, 7, Sak, &SAK_size, true, false);
calculateCRC(&Sak[0], 1, CRCcheck);
if(Sak[0] == 0x08){
if((Sak[1] != CRCcheck[0]) || (Sak[2] != CRCcheck[1])){
hwlib::cout << "Selection error. Sak CRC does not match expected";
return false;
}
} else {
hwlib::cout << "No valid SAK response";
return false;
}
for(int i = 0; i < 4; i++){
Cardserial[i] = UID[i];
}
hwlib::cout << "Card selected\n";
return true;
}
StringBuffer CacheSyncSource::getItemSignature(StringBuffer& key) {
void* content = NULL;
size_t size = 0;
if (key.length() <= 0) {
return NULL;
}
LOG.debug("[%s] Getting signature for item with key %s", getName(), key.c_str());
content = getItemContent(key, &size);
StringBuffer s;
s.sprintf("%ld", calculateCRC(content, size));
if (content) { delete [] (char*)content; content = NULL; }
return s;
}
/*
* MFRC522Read -> read
* Lectura de datos de bloque
* Los parámetros de entrada: blockAddr - dirección del bloque; recvData - leer un bloque de datos
* MI_OK Valor de retorno: el retorno exitoso MI_OK
*/
uint8_t RFID::read(uint8_t blockAddr, uint8_t *recvData)
{
uint8_t status;
uint16_t unLen;
recvData[0] = PICC_READ;
recvData[1] = blockAddr;
calculateCRC(recvData,2, &recvData[2]);
status = MFRC522ToCard(PCD_TRANSCEIVE, recvData, 4, recvData, &unLen);
if ((status != MI_OK) || (unLen != 0x90))
{
status = MI_ERR;
}
return status;
}
void constructPacket()
{
packet->requests++;
frame[0] = packet->id;
frame[1] = packet->function;
frame[2] = packet->address >> 8; // address Hi
frame[3] = packet->address & 0xFF; // address Lo
// For functions 1 & 2 data is the number of points
// For function 5 data is either ON (0xFF00) or OFF (0x0000)
// For function 6 data is exactly that, one register's data
// For functions 3, 4 & 16 data is the number of registers
// For function 15 data is the number of coils
// The data attribute needs to be intercepted by F5 & F6 because these requests
// include their data in the data register and not in the masters array
if (packet->function == FORCE_SINGLE_COIL || packet->function == PRESET_SINGLE_REGISTER)
packet->data = register_array[packet->local_start_address]; // get the data
frame[4] = packet->data >> 8; // MSB
frame[5] = packet->data & 0xFF; // LSB
unsigned char frameSize;
// construct the frame according to the modbus function
if (packet->function == PRESET_MULTIPLE_REGISTERS)
frameSize = construct_F16();
else if (packet->function == FORCE_MULTIPLE_COILS)
frameSize = construct_F15();
else // else functions 1,2,3,4,5 & 6 is assumed. They all share the exact same request format.
frameSize = 8; // the request is always 8 bytes in size for the above mentioned functions.
unsigned int crc16 = calculateCRC(frameSize - 2);
frame[frameSize - 2] = crc16 >> 8; // split crc into 2 bytes
frame[frameSize - 1] = crc16 & 0xFF;
sendPacket(frameSize);
state = WAITING_FOR_REPLY; // state change
// if broadcast is requested (id == 0) for function 5,6,15 and 16 then override
// the previous state and force a success since the slave wont respond
if (packet->id == 0)
processSuccess();
}
////////////////////////////////////////////////////
// setCache
// adds current packet to specified cache
void EQPacketStream::setCache(uint16_t serverArqSeq, EQProtocolPacket& packet)
{
// check if the entry already exists in the cache
EQPacketMap::iterator it = m_cache.find(serverArqSeq);
if (it == m_cache.end())
{
// entry doesn't exist, so insert an entry into the cache
#ifdef PACKET_PROCESS_DIAG
seqDebug("SEQ: Insert arq (%04x) stream %d into cache", serverArqSeq, m_streamid);
#endif
m_cache.insert(EQPacketMap::value_type(serverArqSeq,
new EQProtocolPacket(packet, true)));
emit cacheSize(m_cache.size(), (int)m_streamid);
}
else
{
// replacing an existing entry, make sure the new data is valid
#ifdef APPLY_CRC_CHECK
if (! packet.hasCRC() || calculateCRC(packet) == packet.crc())
#endif
{
#ifdef PACKET_PROCESS_DIAG
seqDebug("SEQ: Update arq (%04x) stream %d in cache", serverArqSeq, m_streamid);
#endif
// Free the old packet at this place and replace with the new one.
delete it->second;
it->second = new EQProtocolPacket(packet, true);
}
#if defined(PACKET_PROCESS_DIAG) && defined(APPLY_CRC_CHECK)
else
seqDebug("SEQ: Not Updating arq (%04x) stream %d into cache, CRC error!",
serverArqSeq, m_streamid);
#endif
}
#ifdef PACKET_CACHE_DIAG
if (m_cache.size() > m_maxCacheCount)
m_maxCacheCount = m_cache.size();
#endif // PACKET_CACHE_DIAG
}
void constructPacket()
{
transmission_ready_Flag = 0; // disable the next transmission
packet->requests++;
frame[0] = packet->id;
frame[1] = packet->function;
frame[2] = packet->address >> 8; // address Hi
frame[3] = packet->address & 0xFF; // address Lo
frame[4] = packet->no_of_registers >> 8; // no_of_registers Hi
frame[5] = packet->no_of_registers & 0xFF; // no_of_registers Lo
unsigned int crc16;
// construct the frame according to the modbus function
if (packet->function == PRESET_MULTIPLE_REGISTERS)
{
unsigned char no_of_bytes = packet->no_of_registers * 2;
unsigned char frameSize = 9 + no_of_bytes; // first 7 bytes of the array + 2 bytes CRC+ noOfBytes
frame[6] = no_of_bytes; // number of bytes
unsigned char index = 7; // user data starts at index 7
unsigned int temp;
unsigned char no_of_registers = packet->no_of_registers;
for (unsigned char i = 0; i < no_of_registers; i++)
{
temp = packet->register_array[i]; // get the data
frame[index] = temp >> 8;
index++;
frame[index] = temp & 0xFF;
index++;
}
crc16 = calculateCRC(frameSize - 2);
frame[frameSize - 2] = crc16 >> 8; // split crc into 2 bytes
frame[frameSize - 1] = crc16 & 0xFF;
sendPacket(frameSize);
if (packet->id == 0) // check broadcast id
{
messageOkFlag = 1; // message successful, there will be no response on a broadcast
previousPolling = millis(); // start the polling delay
}
}
hdlDataPacage recieveHdlPacage(void){
char rxBuffer[100];
hdlDataPacage localHdlPacage;
unsigned int crc;
unsigned int checkCRC;
open_closeUART(1) ;
// read(uart0_filestream, (void*)rxBuffer, 255);
open_closeUART(0);
crc = calculateCRC(strlen(rxBuffer), rxBuffer);
checkCRC = twoCharToUint((strlen(rxBuffer) - 1), strlen(rxBuffer));
if (crc == checkCRC){
localHdlPacage = hdlDataParser(strlen(rxBuffer), rxBuffer);
return localHdlPacage;
}else{
localHdlPacage.orgDeviceId = '0';
return localHdlPacage;
}
}
// returns the eqiv. array of bytes (with CRC checksum added) of a frame structure
byte* EZRoboNetDevice::frame2Bytes(EZFrame* frame) {
int framelen = 5 + frame->PayloadLength + 4; // 5 header bytes plus payload, plus starter, terminator and CRC checksum
byte* rawframe = (byte*)malloc(framelen);
rawframe[0] = framestarter;
rawframe[1] = frame->SenderID;
rawframe[2] = frame->ReceiverID;
rawframe[3] = (byte)frame->Ftype;
rawframe[4] = lowByte(frame->PayloadLength);
rawframe[5] = highByte(frame->PayloadLength);
int i;
for (i=0; i<frame->PayloadLength; i++)
rawframe[6+i] = frame->Payload[i];
rawframe[framelen-3] = frameterminator;
// now adding CRC
uint16_t crc = calculateCRC(rawframe, framelen-2);
rawframe[framelen-2] = lowByte(crc);
rawframe[framelen-1] = highByte(crc);
return rawframe;
}
byte MFRC522::selectTag(byte *serial) {
int i, status, len;
byte sak;
byte buffer[9];
buffer[0] = MF1_SELECTTAG;
buffer[1] = 0x70;
for (i = 0; i < 5; i++) {
buffer[i+2] = serial[i];
}
calculateCRC(buffer, 7, &buffer[7]);
status = commandTag(MFRC522_TRANSCEIVE, buffer, 9, buffer, &len);
if ((status == MI_OK) && (len == 0x18)) {
sak = buffer[0];
}
else {
sak = 0;
}
return sak;
}
unsigned int modbus_update()
{
if(is_update == true)
{
long t_now=millis();
Serial.print(t_now-t_getPacket);
Serial.println(" ms.");
unsigned char buffer = pointer_buffer;
// The minimum request packet is 8 bytes for function 3 & 16
if (buffer > 7)
{
unsigned char id = frame[0];
broadcastFlag = 0;
if (id == 0)
broadcastFlag = 1;
if (id == slaveID || broadcastFlag) // if the recieved ID matches the slaveID or broadcasting id (0), continue
{
unsigned int crc = ((frame[buffer - 2] << 8) | frame[buffer - 1]); // combine the crc Low & High bytes
if (calculateCRC(buffer - 2) == crc) // if the calculated crc matches the recieved crc continue
{
function = frame[1];
unsigned int startingAddress = ((frame[2] << 8) | frame[3]); // combine the starting address bytes
unsigned int no_of_registers = ((frame[4] << 8) | frame[5]); // combine the number of register bytes
unsigned int maxData = startingAddress + no_of_registers;
unsigned char index;
unsigned char address;
unsigned int crc16;
// broadcasting is not supported for function 3
if (!broadcastFlag && (function == 3))
{
if (startingAddress < holdingRegsSize) // check exception 2 ILLEGAL DATA ADDRESS
{
if (maxData <= holdingRegsSize) // check exception 3 ILLEGAL DATA VALUE
{
unsigned char noOfBytes = no_of_registers * 2;
// ID, function, noOfBytes, (dataLo + dataHi)*number of registers,
// crcLo, crcHi
unsigned char responseFrameSize = 5 + noOfBytes;
frame[0] = slaveID;
frame[1] = function;
frame[2] = noOfBytes;
address = 3; // PDU starts at the 4th byte
unsigned int temp;
for (index = startingAddress; index < maxData; index++)
{
temp = regs[index];
frame[address] = temp >> 8; // split the register into 2 bytes
address++;
frame[address] = temp & 0xFF;
address++;
}
crc16 = calculateCRC(responseFrameSize - 2);
frame[responseFrameSize - 2] = crc16 >> 8; // split crc into 2 bytes
frame[responseFrameSize - 1] = crc16 & 0xFF;
sendPacket(responseFrameSize);
}
else
exceptionResponse(3); // exception 3 ILLEGAL DATA VALUE
}
/**
\details Test the Compressed RTF decompression routine.
This function:
-# Loads some test data and checks it
-# Decompresses the test data
-# Checks that the decompressed data matches the expected result
\param mt pointer on the top-level mapitest structure
\return true on success, otherwise false
*/
_PUBLIC_ bool mapitest_noserver_lzfu(struct mapitest *mt)
{
enum MAPISTATUS retval;
DATA_BLOB uncompressed1;
DATA_BLOB uncompressed2;
uint8_t compressed_hex[1024];
uint8_t *compressed;
uint32_t compressed_length;
compressed = talloc_array(mt->mem_ctx, uint8_t, 1024);
memcpy(compressed_hex, RTF_COMPRESSED1_HEX, 98);
compressed_length = strhex_to_str((char*)compressed, 1024, (char*)compressed_hex, 98);
if (compressed_length != 49) {
mapitest_print(mt, "* %-40s: uncompress RTF - Bad length\n", "LZFU");
return false;
}
uint32_t crc = calculateCRC(compressed, 0x10, (49-0x10));
if (crc == 0xA7C7C5F1) {
mapitest_print(mt, "* CRC pass\n");
} else {
mapitest_print(mt, "* CRC failure, expected 0xA7C7C5F1, but got 0x%08X\n", crc);
}
retval = uncompress_rtf(mt->mem_ctx, compressed, compressed_length, &uncompressed1);
if (retval != MAPI_E_SUCCESS) {
mapitest_print_retval(mt, "uncompress_rtf - step 1 (bad retval)");
return false;
}
if (sizeof(RTF_UNCOMPRESSED1) != uncompressed1.length) {
mapitest_print(mt, "* %-40s: FAILED (bad length: %i vs %i)\n", "uncompress_rtf - step 1",
sizeof(RTF_UNCOMPRESSED1), uncompressed1.length);
return false;
}
if (!strncmp((char*)uncompressed1.data, RTF_UNCOMPRESSED1, uncompressed1.length)) {
mapitest_print(mt, "* %-40s: PASSED\n", "uncompress_rtf - step 1");
} else {
mapitest_print(mt, "* %-40s: FAILED - %s\n", "uncompress_rtf - step 1", (char*)uncompressed1.data);
return false;
}
memcpy(compressed_hex, RTF_COMPRESSED2_HEX, 60);
compressed_length = strhex_to_str((char*)compressed, 1024, (char*)compressed_hex, 60);
if (compressed_length != 30) {
mapitest_print(mt, "* %-40s: uncompress RTF - Bad length\n", "LZFU");
return false;
}
retval = uncompress_rtf(mt->mem_ctx, compressed, compressed_length, &uncompressed2);
if (retval != MAPI_E_SUCCESS) {
mapitest_print_retval(mt, "uncompress_rtf - step 2 (bad retval)");
return false;
}
if (!strncmp((char*)uncompressed2.data, RTF_UNCOMPRESSED2, uncompressed2.length)) {
mapitest_print(mt, "* %-40s: PASSED\n", "uncompress_rtf - step 2");
} else {
mapitest_print(mt, "* %-40s: FAILED - %s\n", "uncompress_rtf - step 2", (char*)uncompressed2.data);
return false;
}
/* TODO: add an uncompressed test here */
return true;
}
/* Request Check validity of packet and process the result
* @param: none
* @return: none
* @private
*/
void Modbus::checkPacket()
{
//Check packet response
uint8_t buffer = getPacket();
//if there is nothing received -> return
if ( buffer == 0 )
return;
#if DEBUG_UART
print(F("Response: "));
for (uint8_t i=0;i<buffer;i++)
{
print(frame[i]);
}
print('\t');
#endif
uint8_t stt = 0;
//Check exception response. Slave with OR with 0x80 if exception exists
if ( (frame[1] & 0x80) == 0x80 )
{
println(F("Packet errors"));
switch(frame[2]) //3rd is the exception code
{
case ILLEGAL_FUNCTION:
println(F("Illegal function or not support"));
break;
case ILLEGAL_DATA_VALUE:
println(F("Illegal Value"));
break;
case ILLEGAL_DATA_ADDRESS:
println(F("Illegal data address"));
break;
default:
println(F("Misc exception"));
}
packetError();
return;
}//check exception
uint16_t received_crc = ((frame[buffer - 2] << 8) | frame[buffer - 1]);
uint16_t calculated_crc = calculateCRC(buffer - 2);
if ( calculated_crc == received_crc ) //verify checksum
{
#if DEBUG_UART
print("CRC! ");
#endif
//Check packet functions
switch( frame[1] )
{
case READ_COIL_STATUS:
case READ_INPUT_STATUS:
process_F1_F2();
break;
case READ_INPUT_REGISTERS:
case READ_HOLDING_REGISTERS:
process_F3_F4();
break;
case FORCE_SINGLE_COIL:
case PRESET_SINGLE_REGISTER:
case FORCE_MULTIPLE_COILS:
case PRESET_MULTIPLE_REGISTERS:
process_F5_F6_F15_F16();
break;
default: // illegal function returned
packetError();
break;
}
return;
}//CRC check
else
{
packetError();
println(F("CRC errors"));
return;
}
}
unsigned int modbus_update()
{
if ((*ModbusPort).available())
{
unsigned char buffer = 0;
unsigned char overflow = 0;
while ((*ModbusPort).available())
{
// If more bytes is received than the BUFFER_SIZE the overflow flag will be set and the
// serial buffer will be red untill all the data is cleared from the receive buffer.
if (overflow)
(*ModbusPort).read();
else
{
if (buffer == BUFFER_SIZE)
overflow = 1;
frame[buffer] = (*ModbusPort).read();
buffer++;
}
delayMicroseconds(T1_5); // inter character time out
}
// If an overflow occurred increment the errorCount
// variable and return to the main sketch without
// responding to the request i.e. force a timeout
if (overflow)
return errorCount++;
// The minimum request packet is 8 bytes for function 3 & 16
if (buffer > 7)
{
unsigned char id = frame[0];
broadcastFlag = 0;
if (id == 0)
broadcastFlag = 1;
if (id == slaveID || broadcastFlag) // if the recieved ID matches the slaveID or broadcasting id (0), continue
{
unsigned int crc = ((frame[buffer - 2] << 8) | frame[buffer - 1]); // combine the crc Low & High bytes
if (calculateCRC(buffer - 2) == crc) // if the calculated crc matches the recieved crc continue
{
function = frame[1];
unsigned int startingAddress = ((frame[2] << 8) | frame[3]); // combine the starting address bytes
unsigned int no_of_registers = ((frame[4] << 8) | frame[5]); // combine the number of register bytes
unsigned int maxData = startingAddress + no_of_registers;
unsigned char index;
unsigned char address;
unsigned int crc16;
// broadcasting is not supported for function 3
if (!broadcastFlag && (function == 3))
{
if (startingAddress < holdingRegsSize) // check exception 2 ILLEGAL DATA ADDRESS
{
if (maxData <= holdingRegsSize) // check exception 3 ILLEGAL DATA VALUE
{
unsigned char noOfBytes = no_of_registers * 2;
// ID, function, noOfBytes, (dataLo + dataHi)*number of registers,
// crcLo, crcHi
unsigned char responseFrameSize = 5 + noOfBytes;
frame[0] = slaveID;
frame[1] = function;
frame[2] = noOfBytes;
address = 3; // PDU starts at the 4th byte
unsigned int temp;
for (index = startingAddress; index < maxData; index++)
{
temp = regs[index];
frame[address] = temp >> 8; // split the register into 2 bytes
address++;
frame[address] = temp & 0xFF;
address++;
}
crc16 = calculateCRC(responseFrameSize - 2);
frame[responseFrameSize - 2] = crc16 >> 8; // split crc into 2 bytes
frame[responseFrameSize - 1] = crc16 & 0xFF;
sendPacket(responseFrameSize);
}
else
exceptionResponse(3); // exception 3 ILLEGAL DATA VALUE
}
////////////////////////////////////////////////////
// handle a new packet on the stream
void EQPacketStream::handlePacket(EQUDPIPPacketFormat& packet)
{
emit numPacket(++m_packetCount, (int)m_streamid);
// Packet is ours now. Logging needs to know this later on.
packet.setSessionKey(getSessionKey());
// Only accept packets if we've been initialized unless they are
// initialization packets!
if (packet.getNetOpCode() != OP_SessionRequest &&
packet.getNetOpCode() != OP_SessionResponse &&
! m_sessionKey)
{
#if (defined(PACKET_PROCESS_DIAG) && (PACKET_PROCESS_DIAG > 1)) || (defined(PACKET_SESSION_DIAG) && PACKET_SESSION_DIAG > 1)
seqDebug("discarding packet %s:%d ==>%s:%d netopcode=%04x size=%d. Session not initialized. Need to zone to start picking up packets. Session tracking %s.",
(const char*)packet.getIPv4SourceA(), packet.getSourcePort(),
(const char*)packet.getIPv4DestA(), packet.getDestPort(),
packet.getNetOpCode(), packet.payloadLength(),
(m_session_tracking_enabled == 2 ? "locked on" :
(m_session_tracking_enabled == 1 ? "enabled" : "disabled")));
#endif
return;
}
// Only accept packets that correspond to our latched client port, if
// it is set. This helps filter out multiple sessions on the same physical
// host when two eq clients zone at the same time. The first one will win.
if (m_sessionClientPort != 0 &&
((dir() == DIR_Server && m_sessionClientPort != packet.getDestPort()) ||
(dir() == DIR_Client && m_sessionClientPort != packet.getSourcePort())))
{
#if (defined(PACKET_PROCESS_DIAG) && (PACKET_PROCESS_DIAG > 1)) || (defined(PACKET_SESSION_DIAG) && PACKET_SESSION_DIAG > 1)
seqDebug("discarding packet %s:%d ==>%s:%d netopcode=%04x size=%d. Multiple sessions on the same box? Ignoring all but one of them. Latched client port %d. Session tracking %s.",
(const char*)packet.getIPv4SourceA(), packet.getSourcePort(),
(const char*)packet.getIPv4DestA(), packet.getDestPort(),
packet.getNetOpCode(), packet.payloadLength(),
m_sessionClientPort,
(m_session_tracking_enabled == 2 ? "locked on" :
(m_session_tracking_enabled == 1 ? "enabled" : "disabled")));
#endif
return;
}
// Only accept packets that pass the EQ protocol-level CRC check. This helps
// weed out non-EQ packets that we might see.
#ifdef APPLY_CRC_CHECK
if (packet.hasCRC())
{
uint16_t calcedCRC = calculateCRC(packet);
if (calcedCRC != packet.crc())
{
seqWarn("INVALID PACKET: Bad CRC [%s:%d -> %s:%d] netOp %04x seq %04x len %d crc (%04x != %04x)",
(const char*)packet.getIPv4SourceA(), packet.getSourcePort(),
(const char*)packet.getIPv4DestA(), packet.getDestPort(),
packet.getNetOpCode(), packet.arqSeq(), packet.getUDPPayloadLength(),
packet.crc(), calcedCRC);
return;
}
}
#endif /* APPLY_CRC_CHECK */
// Decode the packet first
if (! packet.decode(m_maxLength))
{
seqWarn("Packet decode failed for stream %s (%d), op %04x, flags %02x packet dropped.",
EQStreamStr[m_streamid], m_streamid, packet.getNetOpCode(),
packet.getFlags());
return;
}
#ifdef PACKET_DECODE_DIAG
else if (packet.hasFlags())
{
seqDebug("Successful decode for stream %s (%d), op %04x, flags %02x.",
EQStreamStr[m_streamid], m_streamid, packet.getNetOpCode(),
packet.getFlags());
}
#endif
// Raw packet
emit rawPacket(packet.rawPayload(), packet.rawPayloadLength(), m_dir,
packet.getNetOpCode());
processPacket(packet, false); // false = isn't subpacket
// if the cache isn't empty, then process it.
if (!m_cache.empty())
processCache();
}
////////////////////////////////////////////////////
// Cache processing
void EQPacketStream::processCache()
{
#if defined(PACKET_CACHE_DIAG)
seqDebug("SEQ: START checking stream %s cache, arq %04x, cache count %04d",
EQStreamStr[m_streamid], m_arqSeqExp, m_cache.size());
#endif
EQPacketMap::iterator it;
EQPacketMap::iterator eraseIt;
EQProtocolPacket* packet;
// check if the cache has grown large enough that we should give up
// on seeing the current serverArqSeqExp
//
// If people see this a lot, they either have pathetic network cards, or
// are having problems keeping up with packets (slow computer? Too much
// net traffic?). Some possible solutions to this are to turn on session
// tracking to filter out more PF_PACKET packets from getting passed out of
// the kernel and to up the socket receive buffer sizes. See FAQ for
// more information.
if (m_cache.size() >= m_arqSeqGiveUp)
{
// ok, if the expected server arq sequence isn't here yet, give up
// attempt to find the current expencted arq seq
it = m_cache.find(m_arqSeqExp);
// keep trying to find a new serverArqSeqExp if we haven't found a good
// one yet...
while(it == m_cache.end())
{
seqWarn("SEQ: Giving up on finding arq %04x in stream %s cache, skipping!",
m_arqSeqExp, EQStreamStr[m_streamid]);
// incremente the expected arq sequence number
m_arqSeqExp++;
emit seqExpect(m_arqSeqExp, (int)m_streamid);
// attempt to find the new current expencted arq seq
it = m_cache.find(m_arqSeqExp);
}
}
else
{
// haven't given up yet, just try to find the current serverArqSeqExp
// attempt to find the current expected ARQ seq
it = m_cache.find(m_arqSeqExp);
}
// iterate over cache until we reach the end or run out of
// immediate followers
while (it != m_cache.end())
{
// get the PacketFormat for the iterator
packet = it->second;
// make sure this is the expected packet
// (we might have incremented to the one after the one returned
// by find above).
if (packet->arqSeq() != m_arqSeqExp)
break;
#ifdef PACKET_CACHE_DIAG
seqDebug("SEQ: found next arq %04x in stream %s cache, cache count %04d",
m_arqSeqExp, EQStreamStr[m_streamid], m_cache.size());
#endif
// validate the packet with a crc check. If the packet is for an old
// session, we probably shouldn't be using it!
#ifdef APPLY_CRC_CHECK
if (packet->hasCRC() && packet->crc() != calculateCRC(*packet))
{
#if defined (PACKET_CACHE_DIAG)
// Something's screwed up
seqDebug("SEQ: INVALID PACKET: Bad CRC in packet in stream %s cache with arq %04x! Droping it, but leaving expected seq as %04x",
EQStreamStr[m_streamid], packet->arqSeq(), m_arqSeqExp);
#endif
// Need to drop from the cache
eraseIt = it;
// increment the current position iterator
it++;
// erase the packet from the cache
m_cache.erase(eraseIt);
emit cacheSize(m_cache.size(), (int)m_streamid);
#ifdef PACKET_CACHE_DIAG
seqDebug("SEQ: REMOVING arq %04x from stream %s cache, cache count %04d",
packet->arqSeq(), EQStreamStr[m_streamid], m_cache.size());
#endif
// delete the packet
delete packet;
// No sense looping some more.
break;
}
else
#endif /* APPLY_CRC_CHECK */
{
#if defined (PACKET_CACHE_DIAG) && (PACKET_CACHE_DIAG > 2)
seqDebug("SEQ: Found next arq in stream %s cache, incrementing arq seq, %04x",
EQStreamStr[m_streamid], packet->arqSeq());
#endif
// Process the packet since it's next in the sequence and was just
// received out of order
processPacket(*packet, packet->isSubpacket());
// Need to drop from the cache
eraseIt = it;
// increment the current position iterator
it++;
// erase the packet from the cache
m_cache.erase(eraseIt);
emit cacheSize(m_cache.size(), (int)m_streamid);
#ifdef PACKET_CACHE_DIAG
seqDebug("SEQ: REMOVING arq %04x from stream %s cache, cache count %04d",
packet->arqSeq(), EQStreamStr[m_streamid], m_cache.size());
#endif
// delete the packet
delete packet;
if (m_arqSeqExp == 0)
it = m_cache.begin();
}
}
#ifdef PACKET_CACHE_DIAG
seqDebug("SEQ: FINISHED checking stream %s cache, arq %04x, cache count %04d",
EQStreamStr[m_streamid], m_arqSeqExp, m_cache.size());
#endif
}
