bool MnistDataset::LoadImages(const char *images_path) {
DeleteImages();
FILE* images_file = fopen(images_path, "rb");
if (images_file == NULL)
return false;
/// Чтение информации из заголовка
fread(&images_magic_number, sizeof(__int32), 1, images_file);
SwapEndian(images_magic_number);
if (images_magic_number != kMagicTrainImages && images_magic_number != kMagicTestImages) {
fclose(images_file);
return false;
}
fread(&num_images_, sizeof(__int32), 1, images_file);
SwapEndian(num_images_);
fread(&num_rows_, sizeof(__int32), 1, images_file);
SwapEndian(num_rows_);
fread(&num_cols_, sizeof(__int32), 1, images_file);
SwapEndian(num_cols_);
/// Инициализация массива данных
images_ = new ImageData*[num_images_];
int buff_size = num_cols_ * num_rows_;
unsigned char buffer[buff_size];
for (int i = 0; i < num_images_; ++i) {
if (feof(images_file)) {
fclose(images_file);
return false;
}
fread(buffer, sizeof(unsigned char), buff_size, images_file);
images_[i] = new ImageData(num_cols_, num_rows_);
images_[i]->Load(buffer, buff_size);
}
fclose(images_file);
is_images_loaded = true;
return true;
}
bool PolyGon::WritePolyGon(FILE* & fp)
{
void* intbuf;
int tempRecNum = recNum;
intbuf = (char*)&tempRecNum;
SwapEndian((char*)intbuf,sizeof(int));
fwrite(intbuf,sizeof(char),sizeof(int),fp);
int tempLength = length;
intbuf = (char*)&tempLength;
SwapEndian((char*)intbuf,sizeof(int));
fwrite(intbuf,sizeof(char),sizeof(int),fp);
fwrite(&shapeType,sizeof(int),1,fp);
double mybounds[4];
bounds.getBounds(mybounds);
fwrite(mybounds,sizeof(double),4,fp);
fwrite(&numParts,sizeof(int),1,fp);
fwrite(&numPoints,sizeof(int),1,fp);
fwrite(parts,sizeof(int),numParts,fp);
//Input points
double x,y;
for(int i=0;i<numPoints;i++) {
points[i].getXY(x,y);
fwrite(&x,sizeof(double),1,fp);
fwrite(&y,sizeof(double),1,fp);
}
return true;
}
// ****************************************************************
// ReadShapeData()
// ****************************************************************
// Reads a single shape from the shapefile
char* CShapefileReader::ReadShapeData(int& offset, int& recordLength)
{
recordLength = 0;
// index records are 8 bytes;
char* data = _indexData + offset*8;
SwapEndian(data);
int readOffset = (*(int*)data) * 2;
data = _indexData + offset * 8 + 4;
SwapEndian(data);
int contentLength = (*(int*)data);
if (contentLength > 0)
{
CSingleLock lock(_readLock);
lock.Lock();
int ret = fseek(_shpfile, (long)readOffset + 2 * sizeof(int), SEEK_SET);
if (ret != 0) return NULL;
// *2: for conversion from 16-bit words to 8-bit words
int length = contentLength * 2;
char* shapeData = new char[length];
int count = (int)fread(shapeData, sizeof(char), length, _shpfile);
recordLength = length;
return shapeData;
}
return NULL;
}
void XDBF::writeSettingEntryPrivate(void *data, int len, Entry *e)
{
// seek to the entry data position
openedFile->setPosition(e->address + 0x10);
// change the endian of the value for writing
SwapEndian(data, 1, len);
// write the data to the entry
openedFile->write(data, len);
// change the endian back to the original so it's not screwed up
SwapEndian(data, 1, len);
}
void shape::writeRecordHeader( FILE * out, int recordNumber )
{ void* intbuf;
intbuf = (char*)&recordNumber;
SwapEndian((char*)intbuf,sizeof(int));
fwrite(intbuf,sizeof(char),sizeof(int), out);
//Record Lengths are measured in 16 bit words
int contentLength = recordbyteLength();
contentLength -= 2*sizeof(int);
contentLength = contentLength/2;
intbuf = (char*)&contentLength;
SwapEndian((char*)intbuf,sizeof(int));
fwrite(intbuf,sizeof(char),sizeof(int), out);
}
void XDBF::writeEntry(Entry *entry, Achievement_Entry *chiev)
{
Achievement_Entry temp = *chiev;
SwapEndian(&temp.size);
SwapEndian(&temp.id);
SwapEndian(&temp.imageID);
SwapEndian(&temp.gamerscore);
SwapEndian((unsigned long long*)&temp.unlockedTime);
openedFile->setPosition(entry->address);
openedFile->write(&temp, 0x1C);
updateSyncListEntry(*getSync(ET_ACHIEVEMENT, entry->address), ET_ACHIEVEMENT, Enqueue);
}
void XDBF::injectStringEntry(wstring wstr, unsigned long long id)
{
// if no id was provided, then we need to get the next available one
if (id == 0)
id = getNextId(ET_STRING);
// create a character array to hold the data to write, we need to
// make a copy so that we can reverse the endian of the wstring
unsigned short *dataToWrite = new unsigned short[wstr.length() + 1];
// copy the characters to the array
for (int i = 0; i < wstr.length(); i++)
{
dataToWrite[i] = (unsigned short)wstr.at(i);
SwapEndian(&dataToWrite[i], 1, 2);
}
dataToWrite[wstr.length()] = 0;
// inject the new string entry
injectEntry_private(ET_STRING, (char*)dataToWrite, WSTRING_BYTES(wstr.length()), id);
// give the memory back
delete[] dataToWrite;
}
//unsigned short
void NifStream( unsigned short & val, istream& in, const NifInfo & info ) {
if ( info.endian == sys_endian ) {
val = ReadUShort( in );
} else {
val = SwapEndian( ReadUShort( in ) );
}
}
inline
void
#ifdef LITTLE_ENDIAN
MakeBigEndian( T * pT )
{
SwapEndian( pT );
}
inline
void
#ifdef BIG_ENDIAN
MakeLittleEndian( T * pT )
{
SwapEndian( pT );
}
uint32_t SwapEndianIfBig(uint32_t x) {
if (IsBigEndian()) {
return SwapEndian(x);
} else {
return x;
}
}
//float
void NifStream( float & val, istream& in, const NifInfo & info ) {
if ( info.endian == sys_endian ) {
val = ReadFloat( in );
} else {
val = SwapEndian( ReadFloat( in ) );
}
}
void WP_Message::PrepareReply( const WP_Packet& req, UINT32 flags, UINT32 payloadSize, UINT8* payload )
{
memcpy( m_header.m_signature, m_parent->m_szMarker ? m_parent->m_szMarker : MARKER_PACKET_V1, sizeof(m_header.m_signature) );
m_header.m_crcData = SUPPORT_ComputeCRC( payload, payloadSize, 0 );
m_header.m_cmd = req.m_cmd;
m_header.m_seq = m_parent->m_lastOutboundMessage++;
m_header.m_seqReply = req.m_seq;
m_header.m_flags = flags | WP_Flags::c_Reply;
m_header.m_size = payloadSize;
m_payload = payload;
//
// The CRC for the header is computed setting the CRC field to zero and then running the CRC algorithm.
//
m_header.m_crcHeader = 0;
#if defined(BIG_ENDIAN)
SwapEndian();
#endif
m_header.m_crcHeader = SUPPORT_ComputeCRC( (UINT8*)&m_header, sizeof(m_header), 0 );
#if defined(BIG_ENDIAN)
m_header.m_crcHeader = ::SwapEndian( m_header.m_crcHeader );
#endif
}
void NifStream( unsigned short const & val, ostream& out, const NifInfo & info ) {
if ( info.endian == sys_endian ) {
WriteUShort( val, out );
} else {
WriteUShort( SwapEndian(val), out );
}
}
void NifStream( float const & val, ostream& out, const NifInfo & info ) {
if ( info.endian == sys_endian ) {
WriteFloat( val, out );
} else {
WriteFloat( SwapEndian(val), out );
}
}
bool shape::read( FILE * f, int & bytesRead )
{
char intbuf[sizeof(int)];
//get record number -- BIG ENDIAN
fread(intbuf,sizeof(char),sizeof(int),f);
SwapEndian(intbuf,sizeof(int));
//recNum = *(int*)intbuf;
//get content length -- BIG ENDIAN
fread(intbuf,sizeof(char),sizeof(int),f);
SwapEndian(intbuf,sizeof(int));
int recordLen = *(int*)intbuf;
bytesRead = sizeof(int);
bytesRead += sizeof(int);
return true;
}
void XDBF::writeSettingMetaData(char *buffer, unsigned long long id, unsigned char type)
{
// copy the entry id to the data buffer
SwapEndian(&id);
memcpy(buffer, &id, 8);
// copy the type to the setting data
memcpy(&buffer[8], &type, 1);
}
void XDBF::injectSettingEntry_private(void* value, int len, Setting_Entry *entry, unsigned long long id)
{
// allocate enough memory for an integral/floating point setting entry
char *data = new char[0x18];
// zero out the write buffer
memset(data, 0, 0x18);
// write the setting meta data, so the id and type
writeSettingMetaData(data, id, entry->type);
// copy the data to the write buffer
SwapEndian(value, 1, len);
memcpy(data + 0x10, value, len);
SwapEndian(value, 1, len);
// inject the new entry
entry->entry = injectEntry_private(ET_SETTING, data, 0x18, id);
}
void XDBF::swapTitleEndianness(Title_Entry *entry)
{
SwapEndian(&entry->titleID);
SwapEndian((unsigned int*)&entry->achievementCount);
SwapEndian((unsigned int*)&entry->achievementUnlockedCount);
SwapEndian(&entry->totalGamerscore);
SwapEndian(&entry->gamerscoreUnlocked);
SwapEndian(&entry->flags);
SwapEndian(&entry->lastPlayed.dwHighDateTime);
SwapEndian(&entry->lastPlayed.dwLowDateTime);
}
int16_t TMP102_ReadRawTemperature()
{
int16_t temp = 0;
SystemTimerDelay(SystemTimeDiff(200)); //Wait until system runing 125ms for conversion
temp = i2c_read_smbus_word(TMP102_I2C_ADDR, TMP102_TEMPERATURE_REG);
SwapEndian(&temp);
if(temp & 0x0001) {
temp >>= 3;
}
bool MnistDataset::LoadLabels(const char *labels_path) {
DeleteLabels();
FILE* labels_file = fopen(labels_path, "rb");
if (labels_file == NULL)
return false;
fread(&labels_magic_number, sizeof(__int32), 1, labels_file);
SwapEndian(labels_magic_number);
if (labels_magic_number != kMagicTestLabels && labels_magic_number != kMagicTrainLabels) {
fclose(labels_file);
return false;
}
fread(&num_labels_, sizeof(__int32), 1, labels_file);
SwapEndian(num_labels_);
labels_ = new unsigned char[num_labels_];
int labels_read = 0;
labels_read = fread(labels_, sizeof(unsigned char), num_labels_, labels_file);
fclose(labels_file);
if (labels_read != num_labels_)
return false;
is_labels_loaded = true;
return true;
}
void XDBF::swapAchievementEndianness(Achievement_Entry *entry)
{
SwapEndian(&entry->size);
SwapEndian(&entry->id);
SwapEndian(&entry->imageID);
SwapEndian(&entry->gamerscore);
SwapEndian(&entry->flags);
SwapEndian((unsigned long long*)&entry->unlockedTime);
}
inline bool PBFParser::readPBFBlobHeader(std::fstream &stream, ParserThreadData *thread_data)
{
int size(0);
stream.read((char *)&size, sizeof(int));
size = SwapEndian(size);
if (stream.eof())
{
return false;
}
if (size > MAX_BLOB_HEADER_SIZE || size < 0)
{
return false;
}
char *data = new char[size];
stream.read(data, size * sizeof(data[0]));
bool dataSuccessfullyParsed = (thread_data->PBFBlobHeader).ParseFromArray(data, size);
delete[] data;
return dataSuccessfullyParsed;
}
uint16_t TMP102_init()
{
TMP102_Config Config;
//uint16_t *ptr_tmp = (uint16_t*) &Config;
Config.OS = 0;
Config.R = TMP102_RESOLUTION_12BIT;
Config.F = TMP102_FAULT_QUEUE_6;
Config.POL = TMP102_POLARITY_LOW;
Config.TM = TMP102_INTERRUPT_MODE;
Config.SD = TMP102_NORMAL_OPERATION;
Config.CR = TMP102_CONVER_RATE_125MS;
Config.AL = TMP102_ALERT_ZERO;
Config.EM = TMP102_EXENDED_MODE;
SwapEndian(&Config);
i2c_write_smbus_word(TMP102_I2C_ADDR, TMP102_CONFIG_REG, *(uint16_t *)&Config);
return 0;
}
//Attempt to load a ninepatch from file
NinePatchBinary::NinePatchBinary(string filename) {
vector<unsigned char> ninepatchData;
lodepng::load_file(ninepatchData,filename);
valid = false;
//Check that the file opened
if (ninepatchData.size() < sizeof(savedNinepatch)) {
cout << "Failed to open 9patch: " << filename << "\n";
return;
}
//Read in bytes until the struct is populated
memcpy((char *)&savedNinepatch,ninepatchData.data(),sizeof(savedNinepatch));
//Check file signature
if (string(savedNinepatch.fileSignature,4) != "PTCH") {
cout << "Corrupted 9patch: " << filename << "\n";
return;
}
//Check if the endian-ness of this machine is different than the file save type
if (savedNinepatch.endianByte != 1) {
//Swap required
SwapEndian(&savedNinepatch.height);
SwapEndian(&savedNinepatch.left);
SwapEndian(&savedNinepatch.top);
SwapEndian(&savedNinepatch.width);
}
//else all done loading binary file. Now load the texture into opengl
//Derive texture name from ninepatch file name
if (filename.size() <= 6) {
cout << "Invalid 9patch filename: " << filename << "\n";
return;
}
string imageFilename = filename.substr(0,filename.size()-6) + ".cropped.png";
unsigned int width;
unsigned int height;
vector<unsigned char> image;
//Read the file
unsigned int error = lodepng::decode(image,width,height,imageFilename);
if (error) {
cout << "Failed to load texture for 9patch: " << filename << "\n";
return;
}
glGenTextures( 1, (GLuint*)&openglTextureId );
glBindTexture(GL_TEXTURE_2D, openglTextureId);
glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, width,
height, 0, GL_RGBA, GL_UNSIGNED_BYTE,
&image[0] );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); //GL_NEAREST FOR SPEED
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); //GL_NEAREST FOR SPEED
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
//record image size
imageSize = vec2(width,height);
{
//Build the texture coordinates
//This part copied and modified from the control.cpp file
//that's what I use "vat" instead of modifying coordiantes directly
vector<vec2> vat(22);
float top = (float)savedNinepatch.top/imageSize.y;
float left = (float)savedNinepatch.left/imageSize.x;
float width = (float)savedNinepatch.width/imageSize.x;
float height = (float)savedNinepatch.height/imageSize.y;
//Draw the textured nine patch
//by resizing the texture coordinates
//using the 9patch system
//Coordinates arranged like this:
//A = 0, B = 1, C = 2, etc.
//A C E G
//BN DL FJ H
//O MQ KS IU
//P R T V
vat[0] = vec2(0,0);
vat[1] = vec2(0,top);
vat[2] = vec2(left,0);
vat[3] = vec2(left,top);
vat[4] = vec2(left+width,0);
vat[5] = vec2(left+width,top);
vat[6] = vec2(1,0);
vat[7] = vec2(1,top);
vat[8] = vec2(1,top+height);
vat[9] = vat[5];
vat[10] = vec2(left+width,top+height);
vat[11] = vat[3];
vat[12] = vec2(left,top+height);
vat[13] = vat[1];
vat[14] = vec2(0,top+height);
vat[15] = vec2(0,1);
vat[16] = vat[12];
vat[17] = vec2(left,1);
vat[18] = vat[10];
vat[19] = vec2(left+width,1);
vat[20] = vat[8];
vat[21] = vec2(1,1);
coordinates = vat;
}
valid = true;
cout << "Successfully loaded 9patch: " << filename << "\n";
}
/****************************************************************************************************
* @fn FormatSensorDataPacket
* Using the sensor sample data, this function creates the HIF Sensor Data Packet for
* sending to Host in the buffer provided.
*
* @param [OUT]pDestPacket - Destination buffer supplied by the caller. This buffer size must be at least
* sizeof(HifSensorDataRaw_t) in length
* @param [IN]pSrc - Data structure carrying the sensor sample (typically from driver)
* @param [IN]sensorPacketType - Sensor Packet type ID corresponding to members in the type union
* @param [IN]metaData - Meta data for the sensor type used (not applicable for all sensor types)
* @param [IN]sType - Sensor Type for the sensor data presented
* @param [IN]subType - Sub type for the sensor type used (not applicable for all sensor types)
*
* @return Size of the formatted packet or -Error code enum corresponding to the error encountered
*
***************************************************************************************************/
int32_t FormatSensorDataPacket( HostIFPackets_t *pDestPacket, const uint8_t *pSrc,
uint8_t sensorPacketType, uint8_t metaData,
ASensorType_t sType, uint8_t subType )
{
const SensorPktDesc_t *pSPD = &(sensorPacketDescriptions[sensorPacketType]);
uint8_t *pDest = (uint8_t *) pDestPacket;
uint8_t tsSize, elemSize;
uint32_t i, j; // i = dest index, j = src index.
/* Sanity checks... */
if (pDest == NULL || pSrc == NULL)
{
return SET_ERROR( OSP_STATUS_NULL_POINTER );
}
/* clear header before setting bits in it */
pDestPacket->GenericControlPkt.Q.ControlByte = 0;
pDestPacket->GenericControlPkt.Q.SensorIdByte = 0;
pDestPacket->GenericControlPkt.Q.AttributeByte = 0;
/* Set Sensor enumeration type Android or User defined */
if ( IsPrivateNotAndroid( sType ) )
{
SetPrivateField( &(pDestPacket->GenericControlPkt.Q.ControlByte) );
}
/* Setup Control Byte */
SetPacketID( pDest, PKID_SENSOR_DATA );
SetDataFormatSensor( pDest, pSPD->DataFormat );
SetTimeFormatSensor( pDest, pSPD->TimeFormat );
/* Setup Sensor ID Byte */
SetMetadata( pDest, metaData );
SetSensorTypeField( pDest, sType );
/* Setup Attribute Byte */
SetSensorSubType( pDest, subType );
SetSensorDataDataSize( pDest, pSPD->DataSz );
SetSensorDataTimeStampSize( pDest, pSPD->TStampSz );
i = PKT_TIMESTAMP_OFFSET; // offset of timestamp in destination packet
j = LOCAL_PACKET_TIMESTAMP_OFFSET; // offset of timestamp in source packet
/* Copy timestamp */
tsSize = (pSPD->TStampSz == TIME_STAMP_32_BIT ? sizeof(int32_t) : sizeof(int64_t));
/* NOTE: TODO-BUGFIX - The logic below will fail for BE machine for 32-bit timestamp as it
will copy the higher 32-bit of the 64-bit TS */
SwapEndian(pDest + i, pSrc + j, tsSize);
i += tsSize; // dest packet TS 32 or 64 bits.
j += sizeof(int64_t); // src packet TS 64 bits, or TS 32 + spare 32.
/* Copy payload */
elemSize = pSPD->ElementSz;
if (elemSize != 0)
{
/* Data bytes */
uint16_t nElem = pSPD->NumElements;
uint16_t nBytesToCopy;
if ( SENSOR_DOUBLE_PAYLOAD_SIZE_FOR_METADATA( sensorPacketType, metaData ) )
{
/* Special handling for SENSOR_DATA_UNCALIBRATED_FIXP packet */
/* Check if metadata is available if it is available then add it to HIF packet */
/* Add it by doubling the payload size before endian swap/copy. */
nElem *= 2; // double the payload size by doubling the number of elements.
}
nBytesToCopy = nElem * elemSize;
if (pSPD->IsBigEndian)
{
SwapEndianX( pDest + i, pSrc + j, elemSize, nElem );
}
else
{
SH_MEMCPY( pDest + i, pSrc + j, nBytesToCopy );
}
i += nBytesToCopy;
}
/* set CRC flag and append CRC, if indicated */
if (FORMAT_WITH_CRC_ENABLED)
{
i += CRC_SIZE;
FormatPacketCRC( pDestPacket, i );
}
return i;
}
/****************************************************************************************************
* @fn ParseSensorDataPacket
* Top level parser for Sensor Data Packets.
*
* @param [OUT]pOut - Sensor structure that will return the parsed values
* @param [IN]pSrc - Packet buffer containing the packet to parse
* @param [OUT]pPktSizeByType - Size of the packet as determined by its type and other parameters
* @param [IN]pktBufferSize - Size of the packet buffer provided. Sensor Data packet buffer can
* have multiple HIF packets.
*
* @return OSP_STATUS_OK or negative Error code enum corresponding to the error encountered
*
***************************************************************************************************/
static int32_t ParseSensorDataPacket( LocalPacketTypes_t *pOut, const uint8_t *pSrc, uint16_t *pPktSizeByType, uint16_t pktBufferSize )
{
/* Aliases. */
uint8_t dSize;
uint8_t dFormat;
uint8_t timeFormat;
uint8_t tSize;
uint8_t sensSubType;
HostIFPackets_t *pHif = (HostIFPackets_t*) pSrc;
int32_t errCode = OSP_STATUS_UNSPECIFIED_ERROR;
ASensorType_t sensorTypeRaw;
/* Get header fields */
ParsePacketHeader( pOut, pSrc, TRUE );
dSize = pOut->SCP.SDP.DataSize;
dFormat = pOut->SCP.SDP.DataFormatSensor;
timeFormat = pOut->SCP.SDP.TimeStampFormat;
tSize = pOut->SCP.SDP.TimeStampSize;
sensSubType = pOut->SubType;
*pPktSizeByType = 0; /* Init to 0 in case we return error */
LocalSensorPacketTypes_t *pSDPOut = &(pOut->SCP.SDP);
sensorTypeRaw = AndroidSensorClearPrivateBase( pOut->SType );
switch ( (ASensorType_t) sensorTypeRaw )
{
case SENSOR_ACCELEROMETER:
case SENSOR_MAGNETIC_FIELD:
case SENSOR_GYROSCOPE:
if ((dSize == DATA_SIZE_32_BIT) && (dFormat == DATA_FORMAT_SENSOR_FIXPOINT) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract sensor data from packet */
SwapEndianBufferToInt32X3(pSDPOut->P.CalFixP.Axis, pHif->CalPktFixP.Data);
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.CalFixP.TimeStamp.TS64), pHif->CalPktFixP.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_CALIBRATED_FIXP].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_ROTATION_VECTOR:
case SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
case SENSOR_GAME_ROTATION_VECTOR:
if ((dSize == DATA_SIZE_32_BIT) && (dFormat == DATA_FORMAT_SENSOR_FIXPOINT) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract Quaternion data from packet */
SwapEndianBufferToInt32X4(pSDPOut->P.QuatFixP.Quat, pHif->QuatPktFixP.Data);
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.QuatFixP.TimeStamp.TS64), pHif->QuatPktFixP.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_QUATERNION_FIXP].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_GYROSCOPE_UNCALIBRATED:
case PSENSOR_ACCELEROMETER_UNCALIBRATED:
if ((dSize == DATA_SIZE_32_BIT) && (dFormat == DATA_FORMAT_SENSOR_FIXPOINT) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract Quaternion data from packet */
SwapEndianBufferToInt32X3(pSDPOut->P.UncalFixP.Axis, pHif->UncalPktFixP.Data);
/* Check if META_DATA is set to 0x01 then read offset */
if (pOut->Metadata == META_DATA_OFFSET_CHANGE)
{
SwapEndianBufferToInt32X3(pSDPOut->P.UncalFixP.Offset, pHif->UncalPktFixP.Offset);
*pPktSizeByType = sizeof(pHif->UncalPktFixP.Offset);
}
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.UncalFixP.TimeStamp.TS64), pHif->UncalPktFixP.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType += sensorPacketDescriptions[SENSOR_DATA_UNCALIBRATED_FIXP].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_SIGNIFICANT_MOTION:
if ((dSize == DATA_SIZE_8_BIT) && (dFormat == DATA_FORMAT_SENSOR_RAW) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract SignificantMotion data from packet */
pSDPOut->P.SigMotion.MotionDetected = pHif->SignificantMotion.significantMotionDetected;
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.SigMotion.TimeStamp.TS64), pHif->SignificantMotion.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_SIGNIFICANT_MOTION].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_STEP_DETECTOR:
if ((dSize == DATA_SIZE_8_BIT) && (dFormat == DATA_FORMAT_SENSOR_RAW) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract StepDetector data from packet */
pSDPOut->P.StepDetector.StepDetected = pHif->StepDetector.stepDetected;
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.StepDetector.TimeStamp.TS64), pHif->StepDetector.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_STEP_DETECTOR].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_STEP_COUNTER:
if ((dSize == DATA_SIZE_64_BIT) && (dFormat == DATA_FORMAT_SENSOR_RAW) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract StepCounter data from packet */
SwapEndian( &(pSDPOut->P.StepCount.NumStepsTotal),
(const void *) &(pHif->StepCounter.NumStepsTotal), sizeof(uint64_t));
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.StepCount.TimeStamp.TS64), pHif->StepCounter.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_STEP_COUNTER].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_ORIENTATION:
if ((dSize == DATA_SIZE_32_BIT) && (dFormat == DATA_FORMAT_SENSOR_FIXPOINT) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract OrientationFixP data from packet */
SwapEndianBufferToInt32(&(pSDPOut->P.OrientFixP.Pitch), pHif->OrientationFixP.Pitch);
SwapEndianBufferToInt32(&(pSDPOut->P.OrientFixP.Roll), pHif->OrientationFixP.Roll);
SwapEndianBufferToInt32(&(pSDPOut->P.OrientFixP.Yaw), pHif->OrientationFixP.Yaw);
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.OrientFixP.TimeStamp.TS64), pHif->OrientationFixP.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_ORIENTATION_FIXP].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SENSOR_LINEAR_ACCELERATION:
case SENSOR_GRAVITY:
if ((dSize == DATA_SIZE_32_BIT) && (dFormat == DATA_FORMAT_SENSOR_FIXPOINT) &&
(timeFormat == TIME_FORMAT_SENSOR_FIXPOINT) && (tSize == TIME_STAMP_64_BIT))
{
/* Extract ThreeAxisFixp data from packet */
SwapEndianBufferToInt32X3(pSDPOut->P.ThreeAxisFixP.Axis, pHif->ThreeAxisFixp.Data);
//TODO: How to handle the Accuracy here ? Leave it in the Metadeta ?
/* Extract fixed point time stamp */
SwapEndianBufferToTimeStamp64(&(pSDPOut->P.ThreeAxisFixP.TimeStamp.TS64), pHif->ThreeAxisFixp.TimeStamp);
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_THREE_AXIS_FIXP].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case PSENSOR_ACCELEROMETER_RAW:
case PSENSOR_MAGNETIC_FIELD_RAW:
case PSENSOR_GYROSCOPE_RAW:
if ((sensSubType == SENSOR_SUBTYPE_UNUSED) && (dSize == DATA_SIZE_16_BIT) &&
(dFormat == DATA_FORMAT_SENSOR_RAW) && (timeFormat == TIME_FORMAT_SENSOR_RAW))
{
/* Extract Raw sensor data from packet */
pSDPOut->P.RawSensor.Axis[0] = (int32_t)BYTES_TO_SHORT(pHif->SensPktRaw.DataRaw[0], pHif->SensPktRaw.DataRaw[1]);
pSDPOut->P.RawSensor.Axis[1] = (int32_t)BYTES_TO_SHORT(pHif->SensPktRaw.DataRaw[2], pHif->SensPktRaw.DataRaw[3]);
pSDPOut->P.RawSensor.Axis[2] = (int32_t)BYTES_TO_SHORT(pHif->SensPktRaw.DataRaw[4], pHif->SensPktRaw.DataRaw[5]);
#ifdef DEBUG_SENSOR_PACKETS
if (g_logging & 0x10)
{
DPRINTF("HIF: %d, %02X, %02X, %04X\r\n", sizeof(HostIFPackets_t),
pHif->SensPktRaw.DataRaw[0], pHif->SensPktRaw.DataRaw[1], pSDPOut->P.RawSensor.Axis[0]);
}
#endif
/* Extract raw 32-bit time stamp. Timestamp extension maybe done if needed by higher layer */
SwapEndianBufferToTimeStamp32(&(pSDPOut->P.RawSensor.TStamp), (void *) &(pHif->SensPktRaw.TimeStamp));
/* Get packet size for this known type from our lookup table */
*pPktSizeByType = sensorPacketDescriptions[SENSOR_DATA_RAW].PktSzSansCRC;
errCode = OSP_STATUS_OK;
}
else
{
errCode = OSP_STATUS_SENSOR_UNSUPPORTED;
}
break;
case SYSTEM_REAL_TIME_CLOCK:
// TODO: to be implement!
errCode = OSP_STATUS_UNSUPPORTED_FEATURE;
break;
default:
errCode = OSP_STATUS_UNSUPPORTED_FEATURE;
break;
}
/* Check for CRC option and do CRC check now that we know the packet size */
if ( *pPktSizeByType != 0 )
{
if (GetCRCFlag(pSrc))
{
/* Note that pPktSizeByType does not include CRC size at this point */
const uint16_t crcCalc = Crc16_CCITT( pSrc, *pPktSizeByType );
const uint16_t crcPacket = BYTES_TO_SHORT( pSrc[*pPktSizeByType], pSrc[*pPktSizeByType + 1] );
if (crcCalc != crcPacket)
{
errCode = OSP_STATUS_INVALID_CRC;
}
else
{
errCode = OSP_STATUS_OK;
}
/* Increase packet size to account for CRC. We do that even if CRC failed so that application
layer can skip over this packet */
*pPktSizeByType += CRC_SIZE;
}
}
/* Check if we are exceeding buffer size provided */
if (*pPktSizeByType > pktBufferSize)
{
errCode = OSP_STATUS_BUFFER_TOO_SMALL;
/* Note: purposely not resetting the pPktSizeByType to zero here */
}
return SET_ERROR( errCode );
}
int main() {
int x; double d; foo f;
SwapEndian( x );
SwapEndian( d );
SwapEndian( f );
}
void SwapEndian(T& t) {
SwapEndian(t, std::is_pod<T>{}, get_sizeof_t<T>{});
}
void XDBF::injectSettingEntry(Setting_Entry *entry, unsigned long long id)
{
char *data;
if (id == 0)
id = getNextId(ET_SETTING);
switch (entry->type)
{
case SET_INT32:
injectSettingEntry_private(&entry->i32_data, 4, entry, id);
break;
case SET_INT64:
injectSettingEntry_private(&entry->i64_data, 8, entry, id);
break;
case SET_DOUBLE:
injectSettingEntry_private(&entry->double_data, 8, entry, id);
break;
case SET_FLOAT:
injectSettingEntry_private(&entry->float_data, 4, entry, id);
break;
case SET_UNICODE:
{
// allocate the perfect amount of memory for the entry
data = new char[entry->unicode_string.str_len_in_bytes + 24];
// zero the memory
memset(data, 0, entry->unicode_string.str_len_in_bytes + 24);
// write the entry meta data
writeSettingMetaData(data, id, SET_UNICODE);
// copy the characters to the array
for (int i = 0; i < entry->unicode_string.str->length(); i++)
{
((unsigned short*)data)[i + 12] = (unsigned short)entry->unicode_string.str->at(i);
SwapEndian(&((unsigned short*)data)[i + 12], 1, 2);
}
data[entry->unicode_string.str->length()] = 0;
// write the entry
injectEntry_private(ET_SETTING, data, entry->unicode_string.str_len_in_bytes + 24, id);
// give that string back to memory
delete[] data;
break;
}
case SET_DATETIME:
// allocate the correct amount of memory for the entry
data = new char[0x18];
// zero the memory
memset(data, 0, 0x18);
// write the entry meta data
writeSettingMetaData(data, id, SET_DATETIME);
// swap the endian for writing
SwapEndian(&entry->time_stamp.dwHighDateTime);
SwapEndian(&entry->time_stamp.dwLowDateTime);
// copy the data time to the write buffer
memcpy(data + 0x10, &entry->time_stamp, 8);
// swap the endian back so the value isn't screwed up
SwapEndian(&entry->time_stamp.dwHighDateTime);
SwapEndian(&entry->time_stamp.dwLowDateTime);
// inject the entry
injectEntry_private(ET_SETTING, data, 0x18, id);
break;
case SET_BINARY:
if(entry->binary.size > 0x3E8)
throw "Binary data too large to inject. Max = 1000(0x3E8)";
int size = entry->binary.size + 0x18;
char *buffer = new char[size];
memset(buffer, 0, size);
writeSettingMetaData(buffer, id, SET_BINARY);
SwapEndian(&entry->binary.size);
*(int*)&buffer[0x10] = entry->binary.size;
SwapEndian(&entry->binary.size);
memcpy(&buffer[0x18], entry->binary.data, entry->binary.size);
injectEntry_private(ET_SETTING, buffer, size, id);
break;
}
}
