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; }