void Send::miscData(const char* const charArray) {
char packet[64];
int packetLength = 0;
IntUnion intUnion;
packet[packetLength++] = 'M';
for(int i = 0; i < 8; i++) {
packet[packetLength++] = charArray[i];
}
packet[packetLength++] = calcChecksum(packet, packetLength);
Serial.write((uint8_t*)packet, packetLength);
}
void RplethDataTransport::sendPing()
{
std::vector<unsigned char> data;
data.push_back(Device::RPLETH);
data.push_back(0x0A);
data.push_back(0x00);
data.push_back(calcChecksum(data));
TcpDataTransport::send(data);
}
virtual void *forgeHeader(std::map<std::string, std::string> &map, unsigned int *size) {
(void)map;
header = new header_s;
*size = ICMP_SIZE;
header->type = 8;
header->code = 0;
header->id = getpid();
header->seq = 0;
header->checksum = calcChecksum();
return header;
}
// Recursively scans a specified directory, calculates file checksums, and
// adds the entries to a linked list
void scanHooliDir(char *dir, const int base_path_length, hooli_file** file,
int* count)
{
DIR* dirp; // Pointer to directory stream
struct dirent* dp; // Pointer to current object in directory stream
// Open directory
if(NULL == (dirp = opendir(dir))) {
// If the directory can't be opened, log an error and move on
syslog(LOG_WARNING, "Unable to open directory '%s'", dir);
return;
}
// Iterate over files/folders in directory
while(NULL != (dp = readdir(dirp))) {
// Don't scan directories like '.' or '..'
if(isCurrentOrParentDir(dp->d_name)) {
continue;
}
// Scan subdirectories recursively
if(DT_DIR == dp->d_type) {
// Concatenate parent directory and current directory names
char* current_path = getFullPathName(dir, dp->d_name, true);
scanHooliDir(current_path, base_path_length, file, count);
free(current_path);
} else {
// Concatenate parent directory and current directory names
char* current_path = getFullPathName(dir, dp->d_name, false);
// int file_length; // Length of file in bytes
// char* buffer; // Contents of file
// readFileToBuffer(current_path, &buffer, &file_length);
int checksum = calcChecksum(current_path);
if(-1 != checksum) {
// uLong checksum = calcChecksum(buffer, file_length); // checksum
// Copy relative path to new string
char* filepath = getStrCopy(current_path + base_path_length);
// Add hooli_file entry
addHooliFile(checksum, filepath, file, *count);
(*count)++;
// free(buffer);
}
free(current_path);
}
}
if(0 != errno) {
syslog(LOG_WARNING, "Error reading file/directory within %s", dir);
}
closedir(dirp);
}
void Send::quaternionData() {
char packet[64];
int packetLength = 0;
#ifdef BINARY_PACKETS
IntUnion intUnion;
packet[packetLength++] = 'Q';
intUnion.intVal= (int)(10000.0f * ahrs.q0);
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= (int)(-10000.0f * ahrs.q1); // negated complex elements for quaternion conjugate
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= (int)(-10000.0f * ahrs.q2);
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= (int)(-10000.0f * ahrs.q3);
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
packet[packetLength++] = calcChecksum(packet, packetLength);
packet[packetLength++] = intUnion.lsb;
#else
packet[packetLength++] = 'Q';
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, (int)(10000.0f * ahrs.q0));
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, (int)(10000.0f * ahrs.q1));
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, (int)(10000.0f * ahrs.q2));
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, (int)(10000.0f * ahrs.q3));
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, calcChecksum(packet, packetLength));
packet[packetLength++] = '\r';
#endif
Serial.write((uint8_t*)packet, packetLength);
}
void
FormMain::browse() // slot
{
QSettings settings;
const QString fileName = QFileDialog::getOpenFileName( this, "Исходный файл",
settings.value( LAST_DIR, QDir::homePath() ).toString() );
if ( fileName.isEmpty() )
return;
m_editPath->setText( fileName );
calcChecksum( fileName );
}
void Scanner2d::stop()
{
ssize_t msg_length = 5;
ssize_t bytes_written;
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_STOP;
command_buffer_[3] = 0; // payload length
command_buffer_[4] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("stop command failed!");
}
}
int displayInfo(int szb_path,szb_header_t *szb_header)
{
char str[8192];
lseek(szb_path,0,SEEK_SET);
read(szb_path,str,8192);
//printf("%s\n",szb_header->magic);
printf("checksum is :%x\n",szb_header->checksum);
printf("%d\n",szb_header->filesize);
//printf("%s\n",szb_header->author);
//printf("%s\n",szb_header->version);
printf("%x\n",szb_header->timestamp);
printf("%d\n",szb_header->imagecount);
///printf("%d\n",sizeof(unsigned int));
int off_set=CFG_FATBOOT;
printf("%x\n",calcChecksum((unsigned int *)(str+12),8192-12));
printf("%d\n",sizeof(szb_header_t));
}
void Scanner2d::setSampleRejectionMode(const bool enabled)
{
ssize_t msg_length = 6;
ssize_t bytes_written;
ROS_DEBUG("Sending setSampleRejectionMode: %d command", enabled);
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_SET_SAMPLE_REJECTION;
command_buffer_[3] = 1; // payload length
command_buffer_[4] = (enabled & 0x01);
command_buffer_[5] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("setSamplesPerScan command failed!");
}
}
void Scanner2d::setScanPeriod(const uint16_t period)
{
ssize_t msg_length = 7;
ssize_t bytes_written;
ROS_DEBUG("Sending setScanPeriod: %d command", period);
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_SET_SCAN_PERIOD;
command_buffer_[3] = 2; // payload length
command_buffer_[4] = (period & 0xFF);
command_buffer_[5] = (period & 0xFF00) >> 8;
command_buffer_[6] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("setScanPeriod command failed!");
}
}
void Scanner2d::setParkTrim(const int16_t trim)
{
ssize_t msg_length = 7;
ssize_t bytes_written;
ROS_DEBUG("Sending setParkTrim: %d command", trim);
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_SET_PARK_TRIM;
command_buffer_[3] = 2; // payload length
command_buffer_[4] = (trim & 0xFF);
command_buffer_[5] = (trim & 0xFF00) >> 8;
command_buffer_[6] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("setParkTrim command failed!");
}
}
void Scanner2d::setSamplesPerScan(const uint16_t samples_per_scan)
{
ssize_t msg_length = 7;
ssize_t bytes_written;
ROS_DEBUG("Sending setSamplesPerScan: %d command", samples_per_scan);
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_SET_SAMPLES_PER_SCAN;
command_buffer_[3] = 2; // payload length
command_buffer_[4] = (samples_per_scan & 0xFF);
command_buffer_[5] = (samples_per_scan & 0xFF00) >> 8;
command_buffer_[6] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("setSamplesPerScan command failed!");
}
}
/**
* Send pose to tracker SPU over UDP.
*/
void CUDP::sendPose(const Vector3 &pos, const Matrix3 &rot) {
std::stringstream ss;
// Send pose as homogenous 4x4 matrix in row major order
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
ss << rot.m[i][j] << " ";
ss << pos.v[i] << " ";
}
ss << "0 0 0 1 ";
ss << "c" << std::ends;
std::string s = ss.str();
int l = s.length();
s[l - 2] = 0;
s[l - 2] = calcChecksum(s);
_sock.writeDatagram(s.c_str(), l, _IP, _Port);
}
static asynStatus sendCommand(void* ppvt,asynUser* pasynUser,int retry)
{
unsigned char cs;
asynStatus sts;
int addr;
size_t len,bytesXfer;
Port* plov = (Port*)ppvt;
Serport* pser = plov->pserport;
asynPrint(pasynUser,ASYN_TRACE_FLOW,"drvLove::sendCommand - retries(%d)\n",retry);
if( retry == 0 )
{
sts = pasynManager->getAddr(pasynUser,&addr);
if( ISNOTOK(sts) )
return( sts );
sprintf(plov->tmpMsg,"%02X%s",addr,plov->outMsg);
calcChecksum(strlen(plov->tmpMsg),plov->tmpMsg,&cs);
sprintf(plov->outMsg,"\002L%s%2X",plov->tmpMsg,cs);
}
len = strlen(plov->outMsg);
sts = pser->pasynOctet->write(pser->pasynOctetPvt,pser->pasynUser,plov->outMsg,len,&bytesXfer);
if( ISOK(sts) )
asynPrint(pasynUser,ASYN_TRACEIO_FILTER,"drvLove::sendCommand - retries(%d),data \"%s\" \"%s\"\n",retry,plov->outMsg);
else
{
if( sts == asynTimeout )
asynPrint(pasynUser,ASYN_TRACE_ERROR,"drvLove::sendCommand - retries(%d) asynTimeout\n",retry);
else if( sts == asynOverflow )
asynPrint(pasynUser,ASYN_TRACE_ERROR,"drvLove::sendCommand - retries(%d) asynOverflow\n",retry);
else if( sts == asynError )
asynPrint(pasynUser,ASYN_TRACE_ERROR,"drvLove::sendCommand - retries(%d) asynError\n",retry);
else
asynPrint(pasynUser,ASYN_TRACE_ERROR,"drvLove::sendCommand - retries(%d) failed - unknown Asyn error\n",retry);
}
return( sts );
}
static boolean_e generateFilePlayer( const player_s *player, const int season, const int player_idx, fileplayer_s *players_file )
{
int2word( players_file[player_idx].acc_stats.action.id_info.player_id, player->player_id );
int2byte( players_file[player_idx].acc_stats.action.id_info.checksum, calcChecksum( player->player_id ) );
memcpy( players_file[player_idx].first_name, player->first_name, sizeof(players_file[player_idx].first_name) );
memcpy( players_file[player_idx].last_name, player->last_name, sizeof(players_file[player_idx].last_name) );
memcpy( players_file[player_idx].first_phoen, player->first_phoenetic, sizeof(players_file[player_idx].first_phoen) );
memcpy( players_file[player_idx].last_phoen, player->last_phoenetic, sizeof(players_file[player_idx].last_phoen) );
termName( players_file[player_idx].first_name, sizeof(players_file[player_idx].first_name) );
termName( players_file[player_idx].last_name, sizeof(players_file[player_idx].last_name) );
termName( players_file[player_idx].first_phoen, sizeof(players_file[player_idx].first_phoen) );
termName( players_file[player_idx].last_phoen, sizeof(players_file[player_idx].last_phoen) );
int2byte( players_file[player_idx].year, player->rookie_season + YEAR_SEASON_OFFSET );
if ( player->player_type == pt_Pitcher ) { generatePitcher( player, season, &(players_file[player_idx]) ); }
else { generateBatter( player, season, &(players_file[player_idx]) ); }
return bl_True;
}
void Scanner2d::setMinMaxAngle(const uint16_t min, const uint16_t max)
{
ssize_t msg_length = 9;
ssize_t bytes_written;
ROS_DEBUG("Sending setMinMaxAngle: %d, %d command", min, max);
command_buffer_[0] = 0xFF;
command_buffer_[1] = 0xFF;
command_buffer_[2] = SCANNER2D_CMD_SET_MIN_MAX_ANGLE;
command_buffer_[3] = 4; // payload length
command_buffer_[4] = (min & 0xFF);
command_buffer_[5] = (min & 0xFF00) >> 8;
command_buffer_[6] = (max & 0xFF);
command_buffer_[7] = (max & 0xFF00) >> 8;
command_buffer_[8] = calcChecksum(command_buffer_, msg_length-1);
bytes_written = ::write(port_fd_, command_buffer_, msg_length);
if (bytes_written != msg_length)
{
ROS_ERROR("setMinMaxAngle command failed!");
}
}
/* ======================================================================
Function: addCustomValue
Purpose : let user add custom values (mainly for testing)
Input : Pointer to the label name
pointer to the value
pointer on flag state of the label
Output : pointer to the new node (or founded one)
Comments: checksum is calculated before adding, no need to bother with
====================================================================== */
ValueList * TInfo::addCustomValue(char * name, char * value, uint8_t * flags)
{
// Little check
if (name && *name && value && *value) {
ValueList * me;
// Same as if we really received this line
customLabel(name, value, flags);
me = valueAdd(name, value, calcChecksum(name,value), flags);
if ( me ) {
// something to do with new datas
if (*flags & (TINFO_FLAGS_UPDATED | TINFO_FLAGS_ADDED | TINFO_FLAGS_ALERT) ) {
// this frame will for sure be updated
_frame_updated = true;
}
return (me);
}
}
// Error or Already Exists
return ( (ValueList *) NULL);
}
void Receive::doTasks() {
while(Serial.available() > 0) {
static bool inSync = false;
static int byteCount = 0;
// Add new byte to buffer
rxBuf[rxBufIndex++] = Serial.read();
// Check if out of sync
if (++byteCount > MAX_PACKET_LENGTH) {
byteCount = MAX_PACKET_LENGTH; // prevent overflow
inSync = false;
}
// Decode LED packet
/*
if (rxBufIndex >= LED_PACKET_LENGTH) {
if ((inSync ? rxBuf[0] : rxBuf[rxBufIndex - LED_PACKET_LENGTH]) == 'L') {
if (calcChecksum(LED_PACKET_LENGTH) == 0) {
unsigned char red = (unsigned char)rxBuf[rxBufIndex - (LED_PACKET_LENGTH - 1)];
unsigned char green = (unsigned char)rxBuf[rxBufIndex - (LED_PACKET_LENGTH - 2)];
unsigned char blue = (unsigned char)rxBuf[rxBufIndex - (LED_PACKET_LENGTH - 3)];
I2CBus::setRGB(red, green, blue);
rxBufIndex = 0;
byteCount = 0;
inSync = true;
}
}
}*/
// Decode vibration motor packet
if (rxBufIndex >= VIB_PACKET_LEGNTH) {
if ((inSync ? rxBuf[0] : rxBuf[rxBufIndex - VIB_PACKET_LEGNTH]) == 'V') {
if (calcChecksum(VIB_PACKET_LEGNTH) == 0) {
char value = (unsigned char)rxBuf[rxBufIndex - (VIB_PACKET_LEGNTH - 1)];
VibrationMotor::pulse(value);
rxBufIndex = 0;
byteCount = 0;
inSync = true;
}
}
}
// Decode misc packet
if (rxBufIndex >= MISC_PACKET_LEGNTH) {
if ((inSync ? rxBuf[0] : rxBuf[rxBufIndex - MISC_PACKET_LEGNTH]) == 'M') {
if (calcChecksum(MISC_PACKET_LEGNTH) == 0) {
char charArray[8];
charArray[0] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 1)];
charArray[1] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 2)];
charArray[2] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 3)];
charArray[3] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 4)];
charArray[4] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 5)];
charArray[5] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 6)];
charArray[6] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 7)];
charArray[7] = (unsigned char)rxBuf[rxBufIndex - (MISC_PACKET_LEGNTH - 8)];
// TODO: process data
rxBufIndex = 0;
byteCount = 0;
inSync = true;
}
}
}
}
}
std::vector<unsigned char> RplethDataTransport::receive(long int timeout)
{
// Force timeout minimum
if (timeout < 2000)
{
timeout = 2000;
}
INFO_("Receiving... %d", timeout);
std::vector<unsigned char> buf;
if (d_trashedData.size() > 0)
{
COM_("Adding existing trashed data: %s", BufferHelper::getHex(d_trashedData).c_str());
buf = d_trashedData;
}
/*#ifdef _WINDOWS
Sleep(200);
#elif defined(__unix__)
usleep(200000);
#endif*/
std::clock_t begin;
std::vector<unsigned char> ansbuf;
std::clock_t end;
long diff = 0;
size_t exceptedlen = 0;
while (timeout > 0 && exceptedlen == 0)
{
ansbuf.clear();
begin = std::clock();
ansbuf = TcpDataTransport::receive(timeout);
COM_("Answer from reader %s", BufferHelper::getHex(ansbuf).c_str());
end = std::clock();
diff = end - begin;
buf.insert(buf.end(), ansbuf.begin(), ansbuf.end());
if (buf.size() >= 4)
{
exceptedlen = 4 + buf[3] + 1;
if (buf.size() < exceptedlen)
{
exceptedlen = 0;
}
}
if (diff >= 0 && diff < timeout)
{
timeout -= diff;
}
else
{
timeout = 0;
}
//EXCEPTION_ASSERT_WITH_LOG(buf.size() >= 4, std::invalid_argument, "A valid answer buffer size must be at least 4 bytes long");
}
if (buf.size() > exceptedlen)
{
d_trashedData.clear();
d_trashedData.insert(d_trashedData.end(), buf.begin()+exceptedlen, buf.end());
buf.erase(buf.begin()+exceptedlen, buf.end());
COM_(" -> Actual trashed data %s...", BufferHelper::getHex(d_trashedData).c_str());
}
else if (buf.size() < exceptedlen)
{
std::cout << "Buf size : " << buf.size()<< "exceptedlen" << exceptedlen << std::endl;
d_trashedData.clear();
d_trashedData.insert(d_trashedData.end(), buf.begin(), buf.end());
THROW_EXCEPTION_WITH_LOG(std::invalid_argument, "The answer buffer doesn't match the excepted data length.");
}
else
{
d_trashedData.clear();
}
EXCEPTION_ASSERT_WITH_LOG(buf[0] != 0x01, std::invalid_argument, "The supplied answer buffer get the state : Command failure");
EXCEPTION_ASSERT_WITH_LOG(buf[0] != 0x02, std::invalid_argument, "The supplied answer buffer get the state : Bad checksum in command");
EXCEPTION_ASSERT_WITH_LOG(buf[0] != 0x03, LibLogicalAccessException, "The supplied answer buffer get the state : Timeout");
EXCEPTION_ASSERT_WITH_LOG(buf[0] != 0x04, std::invalid_argument, "The supplied answer buffer get the state : Bad size of command");
EXCEPTION_ASSERT_WITH_LOG(buf[0] != 0x05, std::invalid_argument, "The supplied answer buffer get the state : Bad device in command");
EXCEPTION_ASSERT_WITH_LOG(buf[0] == 0x00, std::invalid_argument, "The supplied answer buffer is corrupted");
std::vector<unsigned char> res;
std::vector<unsigned char> bufnoc = std::vector<unsigned char>(buf.begin(), buf.end() - 1);
unsigned char checksum_receive = buf[buf.size()-1];
EXCEPTION_ASSERT_WITH_LOG(calcChecksum(bufnoc) == checksum_receive, std::invalid_argument, "The supplied answer buffer get the state : Bad checksum in answer");
if (bufnoc.size() > 4)
{
res = std::vector<unsigned char> (bufnoc.begin() + 4, bufnoc.end());
}
return res;
}
void Send::sensorData() {
char packet[128];
int packetLength = 0;
IntUnion intUnion;
Serial.write((uint8_t*)packet, packetLength);
#ifdef BINARY_PACKETS
packet[packetLength++] = 'S';
intUnion.intVal= Calibration::gyrX;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::gyrY;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::gyrZ;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::accX;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::accY;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::accZ;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::magX;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::magY;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
intUnion.intVal= Calibration::magZ;
packet[packetLength++] = intUnion.msb;
packet[packetLength++] = intUnion.lsb;
packet[packetLength++] = calcChecksum(packet, packetLength);
#else
packet[packetLength++] = 'S';
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::gyrX);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::gyrY);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::gyrZ);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::accX);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::accY);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::accZ);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::magX);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::magY);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, Calibration::magZ);
packet[packetLength++] = ',';
IntValToChars(packet, &packetLength, calcChecksum(packet, packetLength));
packet[packetLength++] = '\r';
#endif
Serial.write((uint8_t*)packet, packetLength);
}
/**
* Calculate and print hash for pic, compare to picture_digest SEI if
* present in seis. seis may be NULL. Hash is printed to stdout, in
* a manner suitable for the status line. Theformat is:
* [Hash_type:xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx,(yyy)]
* Where, x..x is the hash
* yyy has the following meanings:
* OK - calculated hash matches the SEI message
* ***ERROR*** - calculated hash does not match the SEI message
* unk - no SEI message was available for comparison
*/
static void calcAndPrintHashStatus(TComPicYuv& pic, const SEIDecodedPictureHash* pictureHashSEI)
{
/* calculate MD5sum for entire reconstructed picture */
UChar recon_digest[3][16];
Int numChar=0;
const Char* hashType = "\0";
if (pictureHashSEI)
{
switch (pictureHashSEI->method)
{
case SEIDecodedPictureHash::MD5:
{
hashType = "MD5";
calcMD5(pic, recon_digest);
numChar = 16;
break;
}
case SEIDecodedPictureHash::CRC:
{
hashType = "CRC";
calcCRC(pic, recon_digest);
numChar = 2;
break;
}
case SEIDecodedPictureHash::CHECKSUM:
{
hashType = "Checksum";
calcChecksum(pic, recon_digest);
numChar = 4;
break;
}
default:
{
assert (!"unknown hash type");
}
}
}
/* compare digest against received version */
const Char* ok = "(unk)";
Bool mismatch = false;
if (pictureHashSEI)
{
ok = "(OK)";
for(Int yuvIdx = 0; yuvIdx < 3; yuvIdx++)
{
for (UInt i = 0; i < numChar; i++)
{
if (recon_digest[yuvIdx][i] != pictureHashSEI->digest[yuvIdx][i])
{
ok = "(***ERROR***)";
mismatch = true;
}
}
}
}
printf("[%s:%s,%s] ", hashType, digestToString(recon_digest, numChar), ok);
if (mismatch)
{
g_md5_mismatch = true;
printf("[rx%s:%s] ", hashType, digestToString(pictureHashSEI->digest, numChar));
}
}
const uint8_t *
woffEncode(const uint8_t * sfntData, uint32_t sfntLen,
uint16_t majorVersion, uint16_t minorVersion,
uint32_t * woffLen, uint32_t * pStatus)
{
uint8_t * woffData = NULL;
tableOrderRec * tableOrder = NULL;
uint32_t tableOffset;
uint32_t totalSfntSize;
uint16_t numOrigTables;
uint16_t numTables;
uint16_t tableIndex;
uint16_t order;
const sfntDirEntry * sfntDir;
uint32_t tableBase;
uint32_t checkSumAdjustment = 0;
woffHeader * newHeader;
uint32_t tag = 0;
uint32_t removedDsigSize = 0;
uint32_t status = eWOFF_ok;
const sfntHeader * header = (const sfntHeader *) (sfntData);
const sfntHeadTable * head = NULL;
if (pStatus && WOFF_FAILURE(*pStatus)) {
return NULL;
}
if (READ32BE(header->version) != SFNT_VERSION_TT &&
READ32BE(header->version) != SFNT_VERSION_CFF &&
READ32BE(header->version) != SFNT_VERSION_true) {
status |= eWOFF_warn_unknown_version;
}
numOrigTables = READ16BE(header->numTables);
sfntDir = (const sfntDirEntry *) (sfntData + sizeof(sfntHeader));
for (tableIndex = 0; tableIndex < numOrigTables; ++tableIndex) {
/* validate table checksums, to figure out if we need to drop DSIG;
also check that table directory is correctly sorted */
uint32_t prevTag = tag;
uint32_t csum = calcChecksum(&sfntDir[tableIndex], sfntData, sfntLen);
if (csum != READ32BE(sfntDir[tableIndex].checksum)) {
status |= eWOFF_warn_checksum_mismatch;
}
checkSumAdjustment += csum;
tag = READ32BE(sfntDir[tableIndex].tag);
if (tag <= prevTag) {
FAIL(eWOFF_invalid);
}
if (tag == TABLE_TAG_head || tag == TABLE_TAG_bhed) {
if (READ32BE(sfntDir[tableIndex].length) < HEAD_TABLE_SIZE) {
FAIL(eWOFF_invalid);
}
head = (const sfntHeadTable *)(sfntData +
READ32BE(sfntDir[tableIndex].offset));
}
}
if (!head) {
FAIL(eWOFF_invalid);
}
if ((status & eWOFF_warn_checksum_mismatch) == 0) {
/* no point even checking if we already have an error,
as fixing that will change the overall checksum too */
const uint32_t * csumPtr = (const uint32_t *) sfntData;
const uint32_t * csumEnd = csumPtr + 3 + 4 * numOrigTables;
while (csumPtr < csumEnd) {
checkSumAdjustment += READ32BE(*csumPtr);
++csumPtr;
}
checkSumAdjustment = 0xB1B0AFBA - checkSumAdjustment;
if (checkSumAdjustment != READ32BE(head->checkSumAdjustment)) {
status |= eWOFF_warn_checksum_mismatch;
}
}
/* Fixing checkSumAdjustment is tricky, because if there's a DSIG table,
we're going to have to remove that, which in turn means that table
offsets in the directory will all change.
And recalculating checkSumAdjustment requires taking account of any
individual table checksum corrections, but they have not actually been
applied to the sfnt data at this point.
And finally, we'd need to get the corrected checkSumAdjustment into the
encoded head table (but we can't modify the original sfnt data).
An easier way out seems to be to go ahead and encode the font, knowing
that checkSumAdjustment will be wrong; then (if the status flag
eWOFF_warn_checksum_mismatch is set) we'll decode the font back to
sfnt format. This will fix up the checkSumAdjustment (and return a
warning status). We'll ignore that warning, and then re-encode the
new, cleaned-up sfnt to get the final WOFF data. Perhaps not the most
efficient approach, but it seems simpler than trying to predict the
correct final checkSumAdjustment and incorporate it into the head
table on the fly. */
tableOrder = (tableOrderRec *) malloc(numOrigTables * sizeof(tableOrderRec));
if (!tableOrder) {
FAIL(eWOFF_out_of_memory);
}
for (tableIndex = 0, numTables = 0;
tableIndex < numOrigTables; ++tableIndex) {
if ((status & eWOFF_warn_checksum_mismatch) != 0) {
/* check for DSIG table that we must drop if we're fixing checksums */
tag = READ32BE(sfntDir[tableIndex].tag);
if (tag == TABLE_TAG_DSIG) {
status |= eWOFF_warn_removed_DSIG;
removedDsigSize = READ32BE(sfntDir[tableIndex].length);
continue;
}
}
tableOrder[numTables].offset = READ32BE(sfntDir[tableIndex].offset);
tableOrder[numTables].oldIndex = tableIndex;
tableOrder[numTables].newIndex = numTables;
++numTables;
}
qsort(tableOrder, numTables, sizeof(tableOrderRec), compareOffsets);
/* initially, allocate space for header and directory */
tableOffset = sizeof(woffHeader) + numTables * sizeof(woffDirEntry);
woffData = (uint8_t *) malloc(tableOffset);
if (!woffData) {
FAIL(eWOFF_out_of_memory);
}
/* accumulator for total expected size of decoded font */
totalSfntSize = sizeof(sfntHeader) + numTables * sizeof(sfntDirEntry);
/*
* We use a macro for this rather than creating a variable because woffData
* will get reallocated during encoding. The macro avoids the risk of using a
* stale pointer, and the compiler should optimize multiple successive uses.
*/
#define WOFFDIR ((woffDirEntry *) (woffData + sizeof(woffHeader)))
for (order = 0; order < numTables; ++order) {
uLong sourceLen, destLen;
uint32_t sourceOffset;
uint16_t oldIndex = tableOrder[order].oldIndex;
uint16_t newIndex = tableOrder[order].newIndex;
WOFFDIR[newIndex].tag = sfntDir[oldIndex].tag;
if ((status & eWOFF_warn_checksum_mismatch) != 0) {
uint32_t csum = calcChecksum(&sfntDir[oldIndex], sfntData, sfntLen);
WOFFDIR[newIndex].checksum = READ32BE(csum);
} else {
WOFFDIR[newIndex].checksum = sfntDir[oldIndex].checksum;
}
WOFFDIR[newIndex].origLen = sfntDir[oldIndex].length;
WOFFDIR[newIndex].offset = READ32BE(tableOffset);
/* allocate enough space for upper bound of compressed size */
sourceOffset = READ32BE(sfntDir[oldIndex].offset);
if ((sourceOffset & 3) != 0) {
status |= eWOFF_warn_misaligned_table;
}
sourceLen = READ32BE(sfntDir[oldIndex].length);
if (sourceLen > sfntLen || sourceOffset > sfntLen - sourceLen) {
FAIL(eWOFF_invalid);
}
destLen = LONGALIGN(compressBound(sourceLen));
woffData = (uint8_t *) realloc(woffData, tableOffset + destLen);
if (!woffData) {
FAIL(eWOFF_out_of_memory);
}
/* do the compression directly into the WOFF data block */
if (compress2((Bytef *) (woffData + tableOffset), &destLen,
(const Bytef *) (sfntData + sourceOffset),
sourceLen, 9) != Z_OK) {
FAIL(eWOFF_compression_failure);
}
if (destLen < sourceLen) {
/* compressed table was smaller */
tableOffset += destLen;
WOFFDIR[newIndex].compLen = READ32BE(destLen);
} else {
/* compression didn't make it smaller, so store original data instead */
destLen = sourceLen;
/* reallocate to ensure enough space for the table,
plus potential padding after it */
woffData = (uint8_t *) realloc(woffData,
tableOffset + LONGALIGN(sourceLen));
if (!woffData) {
FAIL(eWOFF_out_of_memory);
}
/* copy the original data into place */
memcpy(woffData + tableOffset,
sfntData + READ32BE(sfntDir[oldIndex].offset), sourceLen);
tableOffset += sourceLen;
WOFFDIR[newIndex].compLen = WOFFDIR[newIndex].origLen;
}
/* we always realloc woffData to a long-aligned size, so this is safe */
while ((tableOffset & 3) != 0) {
woffData[tableOffset++] = 0;
}
/* update total size of uncompressed OpenType with table size */
totalSfntSize += sourceLen;
totalSfntSize = LONGALIGN(totalSfntSize);
}
if (totalSfntSize > sfntLen) {
if (totalSfntSize > LONGALIGN(sfntLen)) {
FAIL(eWOFF_invalid);
} else {
status |= eWOFF_warn_unpadded_table;
}
} else if (totalSfntSize < sfntLen) {
/* check if the remaining data is a DSIG we're removing;
if so, we're already warning about that */
if ((status & eWOFF_warn_removed_DSIG) != 0 ||
sfntLen - totalSfntSize >
LONGALIGN(removedDsigSize) + sizeof(sfntDirEntry)) {
status |= eWOFF_warn_trailing_data;
}
}
/* write the header */
newHeader = (woffHeader *) (woffData);
newHeader->signature = WOFF_SIGNATURE;
newHeader->signature = READ32BE(newHeader->signature);
newHeader->flavor = header->version;
newHeader->length = READ32BE(tableOffset);
newHeader->numTables = READ16BE(numTables);
newHeader->reserved = 0;
newHeader->totalSfntSize = READ32BE(totalSfntSize);
newHeader->majorVersion = READ16BE(majorVersion);
newHeader->minorVersion = READ16BE(minorVersion);
newHeader->metaOffset = 0;
newHeader->metaCompLen = 0;
newHeader->metaOrigLen = 0;
newHeader->privOffset = 0;
newHeader->privLen = 0;
free(tableOrder);
if ((status & eWOFF_warn_checksum_mismatch) != 0) {
/* The original font had checksum errors, so we now decode our WOFF data
back to sfnt format (which fixes checkSumAdjustment), then re-encode
to get a clean copy. */
const uint8_t * cleanSfnt = woffDecode(woffData, tableOffset,
&sfntLen, &status);
if (WOFF_FAILURE(status)) {
FAIL(status);
}
free(woffData);
woffData = (uint8_t *) woffEncode(cleanSfnt, sfntLen,
majorVersion, minorVersion,
&tableOffset, &status);
free((void *) cleanSfnt);
if (WOFF_FAILURE(status)) {
FAIL(status);
}
}
if (woffLen) {
*woffLen = tableOffset;
}
if (pStatus) {
*pStatus |= status;
}
return woffData;
failure:
if (tableOrder) {
free(tableOrder);
}
if (woffData) {
free(woffData);
}
if (pStatus) {
*pStatus = status;
}
return NULL;
}
boolean_e formatDosToAmiga( fileplayer_s *players_file )
{
clearErrorMessage();
for ( int i = 0; i < TOTAL_PLAYERS; ++i )
{
if ( EMPTY( players_file[i].last_name ) ) continue;
unsigned int pos = nibble( players_file[i].position[0], n_High );
if ( pos != fpos_Pitcher )
{
acc_bat_stats_s *action = &(players_file[i].acc_stats.action.batting);
if ( byte2int( action->acc_rbi ) >= STAT_CAP_AMOUNT ||
byte2int( action->acc_so ) >= STAT_CAP_AMOUNT )
{
acc_pch_stats_s *pch = &(players_file[i].acc_stats.simulated.pitching);
unsigned int act_inn = word2int( pch->acc_innings );
unsigned int act_hits = word2int( pch->acc_hits );
if ( act_inn > 0 || act_hits > 0 )
{
sprintf( error_message, "ERROR: Player %.*s, %.*s requires overflow and has pitching stats.",
sizeof(players_file[i].last_name), players_file[i].last_name,
sizeof(players_file[i].first_name), players_file[i].first_name );
return bl_False;
}
}
acc_stats_s *acc_stats = &(players_file[i].acc_stats);
acc_stats_u sim = acc_stats->simulated;
acc_stats->simulated = acc_stats->action;
acc_stats->action = sim;
capStat( acc_stats->simulated.batting.acc_rbi, acc_stats->action.batting.acc_rbi );
capStat( acc_stats->simulated.batting.acc_so, acc_stats->action.batting.acc_so );
}
else
{
acc_bat_stats_s *act = &(players_file[i].acc_stats.action.batting);
acc_pch_stats_s *sim = &(players_file[i].acc_stats.simulated.pitching);
unsigned char games = act->acc_games[0];
act->acc_games[0] = 0;
sim->acc_starts[0] = games;
}
acc_player_id_s *id_info = &(players_file[i].acc_stats.action.id_info);
int player_id = word2int( id_info->player_id );
if ( player_id == 0 || calcChecksum( player_id ) != byte2int( id_info->checksum ) )
{
sprintf( error_message, "ERROR: Player %.*s, %.*s has a player id checksum mismatch.",
sizeof(players_file[i].last_name), players_file[i].last_name,
sizeof(players_file[i].first_name), players_file[i].first_name );
return bl_False;
}
}
return bl_True;
}
void updateProgram(void)
{
int clear = 0;
int cmdByte, i, temp;
FLASH_EraseResult result;
uint32_t addr, maxAddr = 0;
uint16_t endSector = 0xFFFF;
_ee_getReserved(_AI_EE_RES_ADDR_MAX_SECTOR, &endSector);
if (endSector > APPLICATION_END_SECTOR || !IS_FLASH_SECTOR(endSector))
endSector = APPLICATION_END_SECTOR;
lcd_clear();
lcd_printf("Aithon Board\nProgramming...");
// Unlock the Flash Program Erase controller
FLASH_If_Init();
while (TRUE)
{
led_toggle(0);
cmdByte = getByte();
debugPrintCmd(cmdByte);
switch (cmdByte)
{
case SYNC:
// sync
flushInterface();
sendResponse(SYNC, ACK);
break;
case ERASE_FLASH_START:
if (FLASH_If_Erase_Start() == FLASH_ERASE_IN_PROGRESS)
sendResponse(ERASE_FLASH_START, ACK);
else
sendResponse(ERASE_FLASH_START, NACK);
break;
case ERASE_FLASH_STATUS:
result = FLASH_If_Erase_Status(endSector);
if (result == FLASH_ERASE_COMPLETE)
sendResponse(ERASE_FLASH_STATUS, ACK);
else if (result == FLASH_ERASE_IN_PROGRESS)
sendResponse(ERASE_FLASH_STATUS, BUSY);
else
sendResponse(ERASE_FLASH_STATUS, NACK);
break;
case SET_ADDR:
// Read in the address, MSB first.
addr = 0;
for (i = 0; i < 4; i++)
{
if ((temp = getByte()) == Q_TIMEOUT)
break;
addr |= (((uint8_t) temp) & 0xFF) << (i * 8);
}
// Check for errors.
if (temp == Q_TIMEOUT)
sendResponse(SET_ADDR, NACK);
else
{
sendResponse(SET_ADDR, ACK);
// We'll get relative addresses, so add the start address.
addr += APPLICATION_START_ADDRESS;
}
break;
case CHECK_ADDR:
// Get the checksum
temp = getByte();
if (temp == Q_TIMEOUT)
sendResponse(CHECK_ADDR, NACK);
else
{
// Subtract the start address before calculating the checksum
addr -= APPLICATION_START_ADDRESS;
if (temp == calcChecksum((uint8_t *)&addr, 4))
sendResponse(CHECK_ADDR, ACK);
else
sendResponse(CHECK_ADDR, NACK);
addr += APPLICATION_START_ADDRESS;
}
break;
case FILL_BUFFER:
for (i = 0; i < PACKET_LEN; i++)
{
if ((temp = getByte()) == Q_TIMEOUT)
break;
_buffer[i] = (uint8_t) (temp & 0xFF);
}
if (temp == Q_TIMEOUT)
sendResponse(FILL_BUFFER, NACK);
else
sendResponse(FILL_BUFFER, ACK);
break;
case CHECK_BUFFER:
// Get the checksum
temp = getByte();
if (temp != Q_TIMEOUT && temp == calcChecksum(_buffer, PACKET_LEN))
sendResponse(CHECK_BUFFER, ACK);
else
sendResponse(CHECK_BUFFER, NACK);
break;
case COMMIT_BUFFER:
maxAddr = addr + PACKET_LEN - 1;
if (FLASH_If_Write((__IO uint32_t *)&addr, (uint32_t *)_buffer, PACKET_LEN/4))
sendResponse(COMMIT_BUFFER, NACK);
else
sendResponse(COMMIT_BUFFER, ACK);
break;
case START_PROGRAM:
sendResponse(START_PROGRAM, ACK);
flushInterface();
_ee_putReserved(_AI_EE_RES_ADDR_MAX_SECTOR, FLASH_Addr_To_Sector(maxAddr));
delayS(1);
startProgram();
// ...should never get here
return;
case Q_TIMEOUT:
default:
if (clear == 0) {
lcd_clear();
clear = 1;
}
lcd_printf ("0%x ", cmdByte);
break;
}
}
}
uint32 Sword2Engine::restoreFromBuffer(byte *buffer, uint32 size) {
Common::MemoryReadStream readS(buffer, size);
// Calc checksum & check that aginst the value stored in the header
if (readS.readUint32LE() != calcChecksum(buffer + 4, size - 4)) {
free(buffer);
return SR_ERR_INCOMPATIBLE;
}
readS.seek(SAVE_DESCRIPTION_LEN, SEEK_CUR);
// Check savegame against length of current global variables resource
// This would most probably be trapped by the checksum test anyway,
// but it doesn't do any harm to check this as well.
// Historical note: During development, earlier savegames would often
// be shorter than the current expected length.
if (readS.readUint32LE() != _resman->fetchLen(1)) {
free(buffer);
return SR_ERR_INCOMPATIBLE;
}
byte *globalVars = _resman->openResource(1);
byte *objectHub = _resman->openResource(CUR_PLAYER_ID) + ResHeader::size();
uint32 screenId = readS.readUint32LE();
uint32 runListId = readS.readUint32LE();
uint32 feetX = readS.readUint32LE();
uint32 feetY = readS.readUint32LE();
uint32 musicId = readS.readUint32LE();
// Trash all resources from memory except player object & global vars
_resman->killAll(false);
_logic->resetKillList();
readS.read(objectHub, ObjectHub::size());
readS.read(_logic->_saveLogic, ObjectLogic::size());
readS.read(_logic->_saveGraphic, ObjectGraphic::size());
readS.read(_logic->_saveMega, ObjectMega::size());
// Fill out the player object structures from the savegame structures.
// Also run the appropriate scripts to set up George's anim tables and
// walkdata, and Nico's anim tables.
// Script no. 8 - 'george_savedata_return' calls fnGetPlayerSaveData
_logic->runResScript(CUR_PLAYER_ID, 8);
// Script no. 14 - 'set_up_nico_anim_tables'
_logic->runResScript(CUR_PLAYER_ID, 14);
// Which megaset was the player at the time of saving?
ObjectMega obMega(_logic->_saveMega);
uint32 scriptNo = 0;
switch (obMega.getMegasetRes()) {
case 36: // GeoMega:
scriptNo = 9; // script no.9 - 'player_is_george'
break;
case 2003: // GeoMegaB:
scriptNo = 13; // script no.13 - 'player_is_georgeB'
break;
case 1366: // NicMegaA:
scriptNo = 11; // script no.11 - 'player_is_nicoA'
break;
case 1437: // NicMegaB:
scriptNo = 12; // script no.12 - 'player_is_nicoB'
break;
case 1575: // NicMegaC:
scriptNo = 10; // script no.10 - 'player_is_nicoC'
break;
}
_logic->runResScript(CUR_PLAYER_ID, scriptNo);
// Copy variables from savegame buffer to memory
readS.read(globalVars, _resman->fetchLen(1));
_resman->closeResource(CUR_PLAYER_ID);
_resman->closeResource(1);
free(buffer);
int32 pars[2];
pars[0] = screenId;
pars[1] = 1;
_logic->fnInitBackground(pars);
ScreenInfo *screenInfo = _screen->getScreenInfo();
// So palette not restored immediately after control panel - we want to
// fade up instead!
screenInfo->new_palette = 99;
// These need setting after the defaults get set in fnInitBackground.
// Remember that these can change through the game, so need saving &
// restoring too.
screenInfo->feet_x = feetX;
screenInfo->feet_y = feetY;
// Start the new run list
_logic->expressChangeSession(runListId);
// Force in the new scroll position, so unsightly scroll-catch-up does
// not occur when screen first draws after returning from restore panel
// Set the screen record of player position - ready for setScrolling()
screenInfo->player_feet_x = obMega.getFeetX();
screenInfo->player_feet_y = obMega.getFeetY();
// if this screen is wide, recompute the scroll offsets now
if (screenInfo->scroll_flag)
_screen->setScrolling();
// Any music required will be started after we've returned from
// restoreControl() - see systemMenuMouse() in mouse.cpp!
// Restart any looping music. Originally this was - and still is - done
// in systemMenuMouse(), but with ScummVM we have other ways of
// restoring savegames so it's easier to put it here as well.
if (musicId) {
pars[0] = musicId;
pars[1] = FX_LOOP;
_logic->fnPlayMusic(pars);
} else
_logic->fnStopMusic(NULL);
return SR_OK;
}
//calculate the current checksum and save it to the proper place
void PrefHandler::saveChecksum() {
uint8_t csum;
csum = calcChecksum();
memCache->Write(EE_CHECKSUM + base_address + lkg_address, csum);
}
void Modem::readXChar(int)
{
uchar c;
static uchar crc_hi, block, cblock;
while(read(fd, (void *) &c, 1) == 1)
{
switch(xstate)
{
case 1: /* 1st 'C' sent */
case 2: /* 2nd 'C' sent */
case 3: /* 3rd 'C' sent */
case 4: /* 4th 'C' sent */
case 5: /* 1st <NAK> sent */
case 6: /* 2nd <NAK> sent */
case 7: /* 3rd <NAK> sent */
case 8: /* 4th <NAK> sent */
case 9: /* 5th <NAK> sent */
case 10: /* 6th <NAK> sent */
if(c == CSOH)
{
timerStart(1000);
xsize = 128;
xstate = 11;
}
else if(c == CSTX)
{
timerStart(1000);
xsize = 1024;
xstate = 11;
}
else if(c == CEOT)
{
timer->stop();
writeChar(CACK);
xreset();
emit xmodemDone(true);
}
else
timerStart(1000);
break;
case 11: /* <SOH> or <STX> received */
timerStart(1000);
block = c;
xstate++;
break;
case 12: /* block number received */
timerStart(1000);
cblock = c;
xstate++;
bufpos = 0;
break;
case 13: /* complement block number received */
timerStart(1000);
buffer[ bufpos++ ] = c;
if(bufpos == xsize)
{
bufpos = 0;
xstate++;
if(!xcrc)
xstate++;
}
break;
case 14: /* data block received */
timerStart(1000);
crc_hi = c;
xstate++;
break;
case 15: /* crc high-byte received */
timerStart(10000);
xstate = 4;
if((uchar)(block ^ cblock) != 0xff)
{
writeChar(CNAK);
break;
}
if(block + 1 == xblock)
{
writeChar(CACK);
break;
}
if(block != xblock)
{
timer->stop();
writeChar(CCAN);
xreset();
emit xmodemDone(false);
break;
}
if(xcrc)
{
if(((ushort) crc_hi << 8 | (ushort) c) != calcCRC())
{
writeChar(CNAK);
break;
}
}
else
{
if(c != calcChecksum())
{
writeChar(CNAK);
break;
}
}
writeChar(CACK);
xblock++;
emit gotXBlock(buffer, xsize);
break;
default:
break;
}
}
}
/* ======================================================================
Function: checkLine
Purpose : check one line of teleinfo received
Input : -
Output : pointer to the data object in the linked list if OK else NULL
Comments:
====================================================================== */
ValueList * TInfo::checkLine(char * pline)
{
char * p;
char * ptok;
char * pend;
char * pvalue;
char checksum;
char buff[TINFO_BUFSIZE];
uint8_t flags = TINFO_FLAGS_NONE;
boolean err = true ; // Assume error
int len ; // Group len
if (pline==NULL)
return NULL;
len = strlen(pline);
// a line should be at least 7 Char
// 2 Label + Space + 1 etiquette + space + checksum + \r
if ( len < 7 )
return NULL;
// Get our own working copy
strncpy( buff, _recv_buff, len+1);
p = &buff[0];
ptok = p; // for sure we start with token name
pend = p + len; // max size
// Init values
pvalue = NULL;
checksum = 0;
//TI_Debug("Got [");
//TI_Debug(len);
//TI_Debug("] ");
// Loop in buffer
while ( p < pend ) {
// start of token value
if ( *p==' ' && ptok) {
// Isolate token name
*p++ = '\0';
// 1st space, it's the label value
if (!pvalue)
pvalue = p;
else
// 2nd space, so it's the checksum
checksum = *p;
}
// new line ? ok we got all we need ?
if ( *p=='\r' ) {
*p='\0';
// Good format ?
if ( ptok && pvalue && checksum ) {
// Always check to avoid bad behavior
if(strlen(ptok) && strlen(pvalue)) {
// Is checksum is OK
if ( calcChecksum(ptok,pvalue) == checksum) {
// In case we need to do things on specific labels
customLabel(ptok, pvalue, &flags);
// Add value to linked lists of values
ValueList * me = valueAdd(ptok, pvalue, checksum, &flags);
// value correctly added/changed
if ( me ) {
// something to do with new datas
if (flags & (TINFO_FLAGS_UPDATED | TINFO_FLAGS_ADDED | TINFO_FLAGS_ALERT) ) {
// this frame will for sure be updated
_frame_updated = true;
// Do we need to advertise user callback
if (_fn_data)
_fn_data(me, flags);
}
}
}
}
}
}
// Next char
p++;
} // While
return NULL;
}
int main(int argc, char** argv)
{
printf("(UDP) File Transfer Server\n");
printf("Anthony Phelps, aphelps\n");
char* port;
port = malloc(sizeof(char*)*5);
//Argument parsing
if(argc > 1)
{
strcpy(port, argv[1]);
}
else
{
strcpy(port, "54323");
}
struct addrinfo hints, *res;
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_flags = AI_PASSIVE;
//Get network info about the server
int addrStats = getaddrinfo(NULL, port, &hints, &res);
if(addrStats < 0)
{
perror("[ERROR] Could not get address info");
exit(-1);
}
//create socket
int sockFd = socket(res->ai_family, res->ai_socktype, res->ai_protocol);
if(sockFd < 1)
{
perror("[ERROR] Could not create socket");
exit(-1);
}
//Bind to port
int binRes = bind(sockFd, res->ai_addr, res->ai_addrlen);
if(binRes < 0)
{
perror("[ERROR] Failed to bind to port");
exit(-1);
}
int curTrans = -1;
int curUIN = -1;
int totalBytes = -1;
int bad = 0;
int lastgood = 0;
Packet * packs = malloc(sizeof(Packet)*5);
int packSize = 5;
int packC = 0;
int save = 0;
//Loop forever because is a server
while(1)
{
struct sockaddr_in remaddr;
socklen_t addrlen = sizeof(remaddr);
Packet response;
response.header = packs[packC].header;
//Receive initial data from client
printf("[INFO] Waiting for new packet...\n");
int recvB = recvfrom(sockFd, &packs[packC], sizeof(Packet), 0, (struct sockaddr *)&remaddr, &addrlen);
if(recvB <= 0)
{
perror("[ERROR] Could not correctly receive bytes");
exit(-1);
}
toH(&(packs[packC].header));
if(calcChecksum(&packs[packC], sizeof(Packet)) == 0)
{
printf("[INFO] Packet received: %i.\n", packs[packC].header.sequenceNumber);
lastgood = packs[packC].header.sequenceNumber;
totalBytes -= packs[packC].header.nbytes;
bad = 0;
}
else
{
printf("[INFO] Bad Packets.\n");
bad = 1;
}
if((curTrans == -1 || curUIN == -1) && bad == 0 && packs[packC].header.sequenceNumber == 1)
{
printf("[INFO] Packet for new file received.\n");
curTrans = packs[packC].header.transactionNumber;
curUIN = packs[packC].header.UIN;
totalBytes = packs[packC].header.nTotalBytes-packs[packC].header.nbytes;
save = packs[packC].header.saveFile;
packC = 0;
if(packC >= packSize)
{
packSize *= 2;
packs = realloc(packs, sizeof(Packet)*packSize);
}
}
else if(bad == 0 && curTrans != packs[packC].header.transactionNumber && curUIN != packs[packC].header.UIN)
{
printf("[WARN] Received packet from different transaction.\n");
}
else if(bad == 0 && curTrans == packs[packC].header.transactionNumber && curUIN == packs[packC].header.UIN)
{
packC += 1;
if(packC >= packSize)
{
packSize *= 2;
packs = realloc(packs, sizeof(Packet)*packSize);
}
}
//Setup response packet
response.header.ackNumber = lastgood;
response.header.garbleChance = packs[packC].header.garbleChance;
response.header.packetType = 2;
response.header.checksum = 0;
response.header.checksum = calcChecksum(&response, sizeof(Packet));
//Send response to client
toN(&(response.header));
int sentb = sendto(sockFd, &response, sizeof(Packet), 0, (struct sockaddr *) &remaddr, addrlen);
if(sentb < 0)
{
perror("[ERROR] Failed to send response packet");
exit(1);
}
if(totalBytes <= 0)
{
//Write contents to file
if(save == 1)
{
printf("[INFO] Writing data to file.\n");
int file = open(packs[0].header.filename, O_WRONLY | O_CREAT);
if(file == -1)
{
perror("[ERROR] Couldn't create file descriptor");
exit(1);
}
int i;
for(i = 0; i < packC; i++)
{
write(file, packs[i].data, packs[i].header.nbytes);
}
close(file);
}
totalBytes = -1;
curTrans = -1;
curUIN = -1;
}
}
//Close and free unneeded stuff (Not really ever possible to run this though because the loop is infintie)
close(sockFd);
freeaddrinfo(res);
return 0;
}
/* ======================================================================
Function: valueAdd
Purpose : Add element to the Linked List of values
Input : Pointer to the label name
pointer to the value
checksum value
flag state of the label (modified by function)
Output : pointer to the new node (or founded one)
Comments: - state of the label changed by the function
====================================================================== */
ValueList * TInfo::valueAdd(char * name, char * value, uint8_t checksum, uint8_t * flags)
{
// Get our linked list
ValueList * me = &_valueslist;
uint8_t lgname = strlen(name);
uint8_t lgvalue = strlen(value);
uint8_t thischeck = calcChecksum(name,value);
// just some paranoia
if (thischeck != checksum ) {
TI_Debug(name);
TI_Debug('=');
TI_Debug(value);
TI_Debug(F(" '"));
TI_Debug((char) cheksum);
TI_Debug(F("' Not added bad checksum calculated '"));
TI_Debug((char) thischeck);
TI_Debugln(F("'"));
} else {
// Got one and all seems good ?
if (me && lgname && lgvalue && checksum) {
// Create pointer on the new node
ValueList *newNode = NULL;
ValueList *parNode = NULL ;
// Loop thru the node
while (me->next) {
// save parent node
parNode = me ;
// go to next node
me = me->next;
// Check if we already have this LABEL
if (strncmp(me->name, name, lgname) == 0) {
// Already got also this value, return US
if (strncmp(me->value, value, lgvalue) == 0) {
*flags |= TINFO_FLAGS_EXIST;
me->flags = *flags;
return ( me );
} else {
// We changed the value
*flags |= TINFO_FLAGS_UPDATED;
me->flags = *flags ;
// Do we have enought space to hold new value ?
if (strlen(me->value) >= lgvalue ) {
// Copy it
strncpy(me->value, value , lgvalue );
me->checksum = checksum ;
// That's all
return (me);
} else {
// indicate our parent node that the next node
// is not us anymore but the next we have
parNode->next = me->next;
// free up this node
free (me);
// Return to parent (that will now point on next node and not us)
// and continue loop just in case we have sevral with same name
me = parNode;
}
}
}
}
// Our linked list structure sizeof(ValueList)
// + Name + '\0'
// + Value + '\0'
size_t size = sizeof(ValueList) + lgname + 1 + lgvalue + 1 ;
// Create new node with size to store strings
if ((newNode = (ValueList *) malloc(size) ) == NULL)
return ( (ValueList *) NULL );
else
// get our buffer Safe
memset(newNode, 0, size);
// Put the new node on the list
me->next = newNode;
// First String located after last struct element
// Second String located after the First + \0
newNode->checksum = checksum;
newNode->name = (char *) newNode + sizeof(ValueList);
newNode->value = (char *) newNode->name + lgname + 1;
// Copy the string data
memcpy(newNode->name , name , lgname );
memcpy(newNode->value, value , lgvalue );
// So we just created this node but was it new
// or was matter of text size ?
if ( (*flags & TINFO_FLAGS_UPDATED) == 0) {
// so we added this node !
*flags |= TINFO_FLAGS_ADDED ;
newNode->flags = *flags;
}
TI_Debug(F("Added '"));
TI_Debug(name);
TI_Debug('=');
TI_Debug(value);
TI_Debug(F("' '"));
TI_Debug((char) cheksum);
TI_Debugln(F("'"));
// return pointer on the new node
return (newNode);
}
} // Checksum OK
// Error or Already Exists
return ( (ValueList *) NULL);
}