Uint16 modbus_func(Uint16 *Buffer, Uint16 len, Uint16 ModbusAddress)
{
Uint16 tmp;
// отвечаем только если нас спрашивают
if (*Buffer != ModbusAddress)
return 0;
// проверяем целосность посылки
if (Crc16(Buffer,len) != 0)
return 0;
// определяем функцию
switch (Buffer[1])
{
// чтение
case 0x03:
len = modbus_0x03_func(Buffer, len);
break;
// запись
case 0x06:
len = modbus_0x06_func(Buffer, len);
break;
default:
len = modbus_error(Buffer,MODBUS_FUNCTION_ERROR);
}
// добавляем к посылке CRC
tmp = Crc16(Buffer,len);
Buffer[len] = tmp & 0xFF;
Buffer[len+1] = tmp >> 8;
return len+2;
}
/*-----------------------------------------------------------*/
UINT8_T WriteBlockLink(UINT8_T index, BlockLink *bl)
{
UINT16_T crc;
UINT8_T *buf;
UINT8_T i;
buf=(UINT8_T*)local_malloc(sl->sector[index].bl_size);
if(buf==NULL)
return ERR_LOCAL_MALLOC;
memset((void*)buf,0x00,sl->sector[index].bl_size);
//сформировать пакет для записи и сгенерить crc16 если требуется
//копируем заголовок сегмента в массив для записи
i=0;
memcpy((void*)buf,(void*)&bl->body.pxNextFreeBlock,sl->sector[index].StartAddrLen);
i=sl->sector[index].StartAddrLen;
memcpy((void*)(buf+i),(void*)&bl->body.xBlockSize,sl->sector[index].SectorSizeLen);
i+=sl->sector[index].SectorSizeLen;
if((sl->sector[index].Type & SECTOR_CRC)>0)
{
Crc16_Clear();
crc=Crc16(buf,i);
memcpy((void*)(buf+i),(void*)&crc,sizeof(UINT16_T));
}
i=ERR_OK;
i=sector_write(index, bl->pxCurrentAddr, (void*)buf, sl->sector[index].bl_size);
local_free(buf);
return i;
}
UI C_UART_ReceivePacket::add(U8 data)
{
if ((data == START_PAKET_SIMBOL) && (_esc_flag == 0) )
{
_size = 0;
_flag = LOCKED;
}
else if( _flag == LOCKED )
{
if (data == END_PAKET_SIMBOL)
{
if (_size >= PAKET_MIN_SIZE)
{
U16 crc_calc = Crc16(_data,_size - 2) ;
U16 crc_rec = ((UI)_data[_size - 1] << 8) | (UI)_data[_size - 2];
_size -= 2;
if (crc_calc == crc_rec )
_flag = DATA_READY;
else
_flag = COMPLETE;
}
else
_flag = COMPLETE;
}
else if ( (_size < (UART_MAX_PACKET_SIZE - 1) ))
{
if ( _esc_flag )
{
_esc_flag = 0;
if (data == ALT_ESC)
_data[_size] = ESC_SIMBOL;
else if (data == ALT_START)
_data[_size] = START_PAKET_SIMBOL;
else if (data == ALT_END)
_data[_size] = END_PAKET_SIMBOL;
else
_flag = COMPLETE;
_size++;
}
else
{
if (data == ESC_SIMBOL)
{
_esc_flag = 1;
}
else
{
_data[_size++] = data;
}
}
}
else
{
_flag = COMPLETE;
}
}
return _flag;
}
/*-----------------------------------------------------------*/
UINT8_T sector_MainSave()
{
UINT8_T i;
UINT16_T size,size1;
UINT8_T err;
if(sl==NULL)
return ERR_SL_NULL;
//найти сектор main
for(i=0;i<(sl->sector_counter);i++)
{
if((sl->sector[i].Type & SECTOR_MAIN)>0)
{
//сгенерировать CRC16
sl->crc=~sl->sector_counter;
Crc16_Clear();
Crc16((UINT8_T*)&sl->sector_counter,sizeof(UINT8_T));
Crc16((UINT8_T*)&sl->crc,sizeof(UINT8_T));
sl->crc16=Crc16((UINT8_T*)sl->sector, (sizeof(SectorInfo)*sl->sector_counter) );
//размер структуры с учетом выравнивания
size=( (sizeof(SectorList)-sizeof(SectorInfo*) + (sl->sector[i].ByteAligment-1)) & ~(sl->sector[i].ByteAligment-1) );
err=sector_write(i, sl->sector[i].StartAddr, (void*)sl, size);
if(err!=ERR_OK)
return err;
size1=( (sizeof(SectorInfo)*sl->sector_counter + (sl->sector[i].ByteAligment-1)) & ~(sl->sector[i].ByteAligment-1) );
err=sector_write(i, sl->sector[i].StartAddr+size, (void*)sl->sector , size1);
if(err!=ERR_OK)
return err;
return ERR_OK;
}
}
return ERR_NO_MAIN;
}
unsigned char C1WireByKernel::ThreadReadRawData8ChannelAddressableSwitch(const std::string& deviceFileName) const
{
static const unsigned char CmdReadPioRegisters[]={0xF5};
unsigned char answer[33];
if (!sendAndReceiveByRwFile(deviceFileName,CmdReadPioRegisters,sizeof(CmdReadPioRegisters),answer,sizeof(answer)))
throw OneWireReadErrorException(deviceFileName);
// Now check the Crc
unsigned char crcData[33];
crcData[0]=CmdReadPioRegisters[0];
for(int i=0;i<32;i++)
crcData[i+1]=answer[i];
if (Crc16(crcData,sizeof(crcData))!=answer[32])
throw OneWireReadErrorException(deviceFileName);
return answer[0];
}
/*-----------------------------------------------------------*/
UINT8_T ReadBlockLink(UINT8_T index , BlockLink *bl)
{
UINT16_T crc;
UINT8_T *buf;
UINT8_T i;
UINT8_T err=ERR_OK;
buf=(UINT8_T*)local_malloc(sl->sector[index].bl_size);
if(buf==NULL)
return ERR_LOCAL_MALLOC;
err=sector_read(index, bl->pxCurrentAddr, (void*)buf, sl->sector[index].bl_size);
if(err!=ERR_OK)
goto exit;
//проверка CRC если требуется
if((sl->sector[index].Type & SECTOR_CRC)>0)
{
Crc16_Clear();
i=sl->sector[index].StartAddrLen + sl->sector[index].SectorSizeLen; //размер структуры BlockLink
crc=Crc16(buf,i);
err=memcmp((void*)(buf+i),(void*)&crc,sizeof(UINT16_T));
if(err!=0x00)
{
err=ERR_CRC;
goto exit;
}
}
//загружаем в структуру
i=sl->sector[index].StartAddrLen;
bl->body.pxNextFreeBlock=0;
memcpy((void*)&bl->body.pxNextFreeBlock,(void*)buf,i);
bl->body.xBlockSize=0;
memcpy((void*)&bl->body.xBlockSize,(void*)(buf+i),sl->sector[index].SectorSizeLen);
exit:
local_free(buf);
return err;
}
int CfgLoadHashs (char *pszFilter, char *pszFolder)
{
nDefaultHash = Crc32 (0, (unsigned char *) "m4d1", 4);
if (hashList.nHashs)
return hashList.nHashs;
if (!CfgCountHashs (pszFilter, pszFolder))
return 0;
hashList.hashs = (uint *) D2_ALLOC (hashList.nHashs * sizeof (int));
FFS ffs;
char szTag [FILENAME_LEN];
int i = 0;
sprintf (szTag, "%s/%s", pszFolder, pszFilter);
for (i = 0; i ? !FFN (&ffs, 0) : !FFF (szTag, &ffs, 0); i++) {
ffs.name [4] = '\0';
strlwr (ffs.name);
strcompress (ffs.name);
hashList.hashs [i] = Crc16 (0, (const unsigned char *) &ffs.name [0], 4);
}
return i;
}
// Given grid of recognized dots, extracts saved information. Returns number of
// corrected erorrs (0..16) on success and 17 if information is not readable.
static int Recognizebits(t_data *result,uchar grid[NDOT][NDOT],
t_procdata *pdata) {
int i,j,k,q,r,factor,lcorr,c,cmin,cmax,limit;
int grid1[NDOT][NDOT],answer,bestanswer;
static int lastgood;
ushort crc;
t_data uncorrected,bestresult;
cmin=pdata->cmin;
cmax=pdata->cmax;
bestanswer=17;
// If orientation is not yet known, try all possible orientations + mirroring.
for (r=0; r<8; r++) {
if (pdata->orientation>=0 && r!=pdata->orientation) continue;
// Try 3 different point overlapping factors, combined with 3 different
// thresholds. Usually all cells are alike, so I remember the last known
// good combination and start with it.
for (k=0; k<9; k++) {
q=(k+lastgood)%9;
switch (q) {
case 0: factor=1000; lcorr=0; break;
case 1: factor=32; lcorr=0; break;
case 2: factor=16; lcorr=0; break;
case 3: factor=1000; lcorr=(cmin-cmax)/16; break;
case 4: factor=32; lcorr=(cmin-cmax)/16; break;
case 5: factor=16; lcorr=(cmin-cmax)/16; break;
case 6: factor=1000; lcorr=(cmax-cmin)/16; break;
case 7: factor=32; lcorr=(cmax-cmin)/16; break;
case 8: factor=16; lcorr=(cmax-cmin)/16; break;
default: factor=1000; lcorr=0; lastgood=0; break; };
// Correct grid for overlapping dots and calculate limit between black
// and white. I take into account only adjacent dots; the influence of
// diagonals is significantly lower.
limit=0;
for (j=0; j<NDOT; j++) {
for (i=0; i<NDOT; i++) {
c=grid[i][j]*factor;
if (i>0) c-=grid[j][i-1]; else c-=cmax;
if (i<31) c-=grid[j][i+1]; else c-=cmax;
if (j>0) c-=grid[j-1][i]; else c-=cmax;
if (j<31) c-=grid[j+1][i]; else c-=cmax;
grid1[j][i]=c;
limit+=c;
};
};
limit=limit/1024+lcorr*factor;
// Extract data according to the selected orientation.
memset(result,0,sizeof(t_data));
for (j=0; j<NDOT; j++) {
for (i=0; i<NDOT; i++) {
switch (r) {
case 0: c=grid1[j][i]; break;
case 1: c=grid1[i][NDOT-1-j]; break;
case 2: c=grid1[NDOT-1-j][NDOT-1-i]; break;
case 3: c=grid1[NDOT-1-i][j]; break;
case 4: c=grid1[i][j]; break;
case 5: c=grid1[j][NDOT-1-i]; break;
case 6: c=grid1[NDOT-1-i][NDOT-1-j]; break;
case 7: c=grid1[NDOT-1-j][i]; break;
};
if (c<limit) {
((ulong *)result)[j]|=1<<i;
};
};
};
// XOR with grid that corrects mean brightness.
for (j=0; j<NDOT; j++) {
((ulong *)result)[j]^=(j & 1?0xAAAAAAAA:0x55555555); };
// Apply ECC to restore invalid data.
if (pdata->mode & M_BEST)
memcpy(&uncorrected,result,sizeof(t_data));
else
memcpy(&pdata->uncorrected,result,sizeof(t_data));
answer=Decode8((uchar *)result,NULL,0,127);
if (answer<0) answer=17;
// Verify data for correctness by calculating CRC.
if (answer<=16) {
crc=(ushort)(Crc16((uchar *)result,NDATA+4)^0x55AA);
if (crc==result->crc) {
// Data recognized correctly, save orientation of actually processed
// page and factoring.
pdata->orientation=r;
// Report success.
if ((pdata->mode & M_BEST)==0) {
lastgood=q;
return answer; }
else if (answer<bestanswer) {
bestanswer=answer;
bestresult=*result;
memcpy(&pdata->uncorrected,&uncorrected,sizeof(t_data));
};
};
};
};
};
if (pdata->mode & M_BEST)
*result=bestresult;
return bestanswer;
};
// Saves file with specified index and closes file descriptor (if force is 1,
// attempts to save data even if file is not yet complete). Returns 0 on
// success and -1 on error.
int Saverestoredfile(int slot,int force) {
int n,success;
ushort filecrc;
ulong l,length;
uchar *bufout,*data,*tempdata;
t_fproc *pf;
aes_context ctx;
HANDLE hfile;
if (slot<0 || slot>=NFILE)
return -1; // Invalid index of file descriptor
pf=fproc+slot;
if (pf->busy==0 || pf->nblock==0)
return -1; // Index points to unused descriptor
if (pf->ndata!=pf->nblock && force==0)
return -1; // Still incomplete data
Message("",0);
// If data is encrypted, decrypt it to temporary buffer. Decryption in place
// is possible, but the whole data would be lost if password is incorrect.
if (pf->mode & PBM_ENCRYPTED) {
if (pf->datasize & 0x0000000F) {
Reporterror("Encrypted data is not aligned");
return -1;
};
if (Getpassword()!=0)
return -1; // User cancelled decryption
tempdata=(uchar *)GlobalAlloc(GMEM_FIXED,pf->datasize);
if (tempdata==NULL) {
Reporterror("Low memory, can't decrypt data");
return -1;
};
n=strlen(password);
while (n<PASSLEN) password[n++]=0;
memset(&ctx,0,sizeof(ctx));
aes_set_key(&ctx,(uchar *)password,256);
for (l=0; l<pf->datasize; l+=16)
aes_decrypt(&ctx,pf->data+l,tempdata+l);
filecrc=Crc16(tempdata,pf->datasize);
if (filecrc!=pf->filecrc) {
Reporterror("Invalid password, please try again");
GlobalFree((HGLOBAL)tempdata);
return -1;
}
else {
GlobalFree((HGLOBAL)pf->data);
pf->data=tempdata;
pf->mode&=~PBM_ENCRYPTED;
};
};
// If data is compressed, unpack it to temporary buffer.
if ((pf->mode & PBM_COMPRESSED)==0) {
// Data is not compressed.
data=pf->data;
length=pf->origsize;
bufout=NULL;
}
else {
// Data is compressed. Create temporary buffer.
if (pf->origsize==0)
pf->origsize=pf->datasize*4; // Weak attempt to recover
bufout=(uchar *)GlobalAlloc(GMEM_FIXED,pf->origsize);
if (bufout==NULL) {
Reporterror("Low memory");
return -1;
};
// Unpack data.
length=pf->origsize;
success=BZ2_bzBuffToBuffDecompress((char *)bufout,(uint *)&length,
pf->data,pf->datasize,0,0);
if (success!=BZ_OK) {
GlobalFree((HGLOBAL)bufout);
Reporterror("Unable to unpack data");
return -1;
};
data=bufout;
};
// Ask user for file name.
if (Selectoutfile(pf->name)!=0) { // Cancelled by user
if (bufout!=NULL) GlobalFree((HGLOBAL)bufout);
return -1;
};
// Open file and save data.
hfile=CreateFile(outfile,GENERIC_WRITE,0,NULL,
CREATE_ALWAYS,FILE_ATTRIBUTE_NORMAL,NULL);
if (hfile==INVALID_HANDLE_VALUE) {
if (bufout!=NULL) GlobalFree((HGLOBAL)bufout);
Reporterror("Unable to create file");
return -1;
};
WriteFile(hfile,data,length,&l,NULL);
// Restore old modification date and time.
SetFileTime(hfile,&pf->modified,&pf->modified,&pf->modified);
// Close file and restore old basic attributes.
CloseHandle(hfile);
SetFileAttributes(outfile,pf->attributes);
if (bufout!=NULL) GlobalFree((HGLOBAL)bufout);
if (l!=length) {
Reporterror("I/O error");
return -1;
};
// Close file descriptor and report success.
Closefproc(slot);
Message("File saved",0);
return 0;
};
static void enumerator_handler(RETRANSLATOR *pRetranslator, void *ctx)
{
if (pRetranslator->sock == -1)
return;
ENUMCONTEXT *pContext = (ENUMCONTEXT *)ctx;
time_t now = time(NULL);
if (FD_ISSET(pRetranslator->sock, &pContext->fdReadSet)) {
api_log_printf("[EGTS-TATARSTAN] socket #%d is readble\r\n", pRetranslator->sock);
for (;;) {
int status = recv(pRetranslator->sock, (char *)&AckPacket, sizeof(AckPacket), 0);
if (status <= 0) {
api_log_printf("[EGTS-TATARSTAN] socket #%d is closed\r\n", pRetranslator->sock);
closesocket(pRetranslator->sock);
pRetranslator->sock = -1;
pRetranslator->status = RETRANSLATOR_STATUS_INIT;
return;
}
if (status > 0) {
api_log_printf("[EGTS-TATARSTAN] Received #%d bytes from socket #%d\r\n", status, pRetranslator->sock);
if (AckPacket.hcs != CRC8((unsigned char *)&AckPacket, 10)) {
api_log_printf("[EGTS-TATARSTAN] CRC8 invalid\r\n");
break;
}
if (*((unsigned short *)((unsigned char *)&AckPacket.record_id + AckPacket.dataLength)) != Crc16((unsigned char *)&AckPacket.record_id, AckPacket.dataLength)) {
api_log_printf("[EGTS-TATARSTAN] CRC16 invalid\r\n");
break;
}
if (AckPacket.record_id != pRetranslator->packet_id) {
api_log_printf("[EGTS-TATARSTAN] Wrong record_id\r\n");
break;
}
if (AckPacket.result != 0) {
api_log_printf("[EGTS-TATARSTAN] Wrong result code\r\n");
break;
}
spinlock_lock(&pRetranslator->spinlock);
pRetranslator->records_queue.pop();
spinlock_unlock(&pRetranslator->spinlock);
pRetranslator->status = RETRANSLATOR_STATUS_CONNECTED;
pRetranslator->timeout = 0;
pRetranslator->packet_id++;
}
break;
}
}
if (FD_ISSET(pRetranslator->sock, &pContext->fdWriteSet)) {
api_log_printf("[EGTS-TATARSTAN] socket #%d is writible\r\n", pRetranslator->sock);
switch (pRetranslator->status) {
case RETRANSLATOR_STATUS_INIT:
api_log_printf("[EGTS-TATARSTAN] ERROR, socket #%d is in intial state\r\n", pRetranslator->sock);
break;
case RETRANSLATOR_STATUS_CONNECTING:
api_log_printf("[EGTS-TATARSTAN] socket #%d connected\r\n", pRetranslator->sock);
pRetranslator->packet_id = 0;
pRetranslator->status = RETRANSLATOR_STATUS_CONNECTED;
case RETRANSLATOR_STATUS_CONNECTED:
spinlock_lock(&pRetranslator->spinlock);
if (!pRetranslator->records_queue.empty()) {
RETRANSLATOR_RECORD rr = pRetranslator->records_queue.front();
spinlock_unlock(&pRetranslator->spinlock);
TatarPacket.version = 0x01;
TatarPacket.securityKeyId = 0x01;
TatarPacket.packet_flags = 0x01;
TatarPacket.packet_length = 0x0b;
TatarPacket.encoding = 0x00;
TatarPacket.dataLength = 35;
TatarPacket.packet_id = pRetranslator->packet_id;
TatarPacket.type = 1; // EGTS_PT_APPDATA
TatarPacket.record_length = 24;
TatarPacket.record_num = pRetranslator->packet_id;
TatarPacket.record_flags = 0x09; // OID – (Object Identifier) | PRIORITY 01
TatarPacket.record_oid = pRetranslator->oid; // Last 8 digits of imei
TatarPacket.record_sst = 0x02; // EGTS_TELEDATA_SERVICE
TatarPacket.record_rst = 0x02; // EGTS_TELEDATA_SERVICE
TatarPacket.subrecord_type = 0x10; // EGTS_SR_POS_DATA
TatarPacket.subrecord_length = 0x15;
TatarPacket.sr_pos_data_timestamp = rr.t - 1262304000;
float lat = (float)rr.latitude / 10000000;
float lon = (float)rr.longitude / 10000000;
TatarPacket.sr_pos_data_latitude = (unsigned int)((fabs(lat) / 90) * 0xFFFFFFFF);
TatarPacket.sr_pos_data_longitude = (unsigned int)((fabs(lon) / 180) * 0xFFFFFFFF);
TatarPacket.sr_pos_data_flags = 0;
if ((rr.latitude != 0)&&(rr.longitude != 0))
TatarPacket.sr_pos_data_flags |= 0x01;
if (rr.flags1 & RECORD_FLAG1_MOVE)
TatarPacket.sr_pos_data_flags |= 0x10;
if (lon < 0)
TatarPacket.sr_pos_data_flags |= 0x40;
if (lat < 0)
TatarPacket.sr_pos_data_flags |= 0x20;
TatarPacket.sr_pos_data_speed = rr.speed & 0x3FFF;
TatarPacket.sr_pos_data_direction = (rr.cog & 0xFF);
if (rr.cog > 255)
TatarPacket.sr_pos_data_speed |= 0x8000;
TatarPacket.sr_pos_data_odometer[0] = 0xcf;
TatarPacket.sr_pos_data_odometer[1] = 0xf6;
TatarPacket.sr_pos_data_odometer[2] = 0x27;
TatarPacket.sr_pos_data_digitalInputs = rr.flags2 & 0xF0;
TatarPacket.sr_pos_data_source = (rr.flags1 & RECORD_FLAG1_IGNITION) ? 0 : 5;
TatarPacket.hcs = CRC8((unsigned char *)&TatarPacket, 10);
TatarPacket.data_crc = Crc16((unsigned char *)&TatarPacket.record_length, TatarPacket.dataLength);
send(pRetranslator->sock, (char *)&TatarPacket, sizeof(TatarPacket), 0);
api_log_printf("[EGTS-TATARSTAN] socket #%d send record\r\n", pRetranslator->sock);
pRetranslator->status = RETRANSLATOR_STATUS_WAITACK;
pRetranslator->timeout = now + 30;
}
else {
spinlock_unlock(&pRetranslator->spinlock);
}
break;
case RETRANSLATOR_STATUS_WAITACK:
api_log_printf("[EGTS-TATARSTAN] ERROR, socket #%d is in waitack state\r\n", pRetranslator->sock);
break;
}
}
}
unsigned short Crc16(unsigned short crcInit, unsigned char buffer[], int size) {
for (int i = 0; i < size; i++)
crcInit = Crc16(crcInit, buffer[i]);
return crcInit;
}
/*-----------------------------------------------------------*/
UINT8_T sector_Open(UINT8_T index, UINT8_T aligment ,UINT32_T addr)
{
UINT16_T size;
UINT8_T err=ERR_OK;
SectorList *header=NULL;
UINT8_T i;
//подготовить
if(aligment==0)
return ERR_WRONG_ALIGMENT;
size=(sizeof(SectorList)+(aligment-1)) & ~(aligment-1);
header=(SectorList*)local_malloc( size );
if(header==NULL)
return ERR_LOCAL_MALLOC;
//чтение заголовка. Учитываем выравнивание
size=(sizeof(SectorList)-sizeof(SectorInfo*)+(aligment-1)) & ~(aligment-1);
err=sector_read(index, addr, (void*)header, size);
if(err==ERR_OK)
{
i=~header->sector_counter;
if(i == header->crc)
{
//создание структур
err=sector_Create(header->sector_counter,aligment);
if(err==ERR_OK)
{
err=sector_read(index, (addr+size), (void*)sl->sector, (sizeof(SectorInfo)*sl->sector_counter + (aligment-1)) & ~(aligment-1) );
if(err!=ERR_OK)
{
sector_ResourceFree();
}
else
{
sl->crc=header->crc;
//проверка CRC16
Crc16_Clear();
Crc16((UINT8_T*)&sl->sector_counter,sizeof(UINT8_T));
Crc16((UINT8_T*)&sl->crc,sizeof(UINT8_T));
sl->crc16=Crc16((UINT8_T*)sl->sector,sizeof(SectorInfo)*sl->sector_counter);
if(header->crc16!=sl->crc16)
{
sector_ResourceFree();
err=ERR_CRC;
}
else
{
for(i=0;i<sl->sector_counter;i++)
{
if(sl->sector[i].Type!=SECTOR_FREE)
{
size=(sl->sector[index].pxEnd_Addr+sl->sector[index].bl_size)-sl->sector[index].StartAddr;
ApplicationSectorOpenHook(i, sl->sector[i].StartAddr,size);
}
}
}
}
}
}
else
{
err=ERR_CRC;
}
}
local_free(header);
return err;
}
