void RData::printStructure (const QString &prefix) {
switch (datatype) {
case NoData:
qDebug ("%s: NoData, length %d", prefix.toLatin1().data(), getDataLength ());
break;
case IntVector:
qDebug ("%s: IntVector, length %d", prefix.toLatin1().data(), getDataLength ());
for (unsigned int i = 0; i < getDataLength (); ++i) {
qDebug ("%s%d: %d", prefix.toLatin1().data(), i, getIntVector ()[i]);
}
break;
case RealVector:
qDebug ("%s: RealVector, length %d", prefix.toLatin1().data(), getDataLength ());
for (unsigned int i = 0; i < getDataLength (); ++i) {
qDebug ("%s%d: %f", prefix.toLatin1().data(), i, getRealVector ()[i]);
}
break;
case StringVector:
qDebug ("%s: StringVector, length %d", prefix.toLatin1().data(), getDataLength ());
for (unsigned int i = 0; i < getDataLength (); ++i) {
qDebug ("%s%d: %s", prefix.toLatin1().data(), i, getStringVector ()[i].toLatin1().data());
}
break;
case StructureVector:
qDebug ("%s: StructureVector, length %d", prefix.toLatin1().data(), getDataLength ());
for (unsigned int i = 0; i < getDataLength (); ++i) {
QString sub_prefix = prefix + QString::number (i);
getStructureVector ()[i]->printStructure (sub_prefix);
}
break;
default:
qDebug ("%s: INVALID %d, length %d", prefix.toLatin1().data(), datatype, getDataLength ());
}
qDebug ("%s: END\n\n", prefix.toLatin1 ().data());
}
void RawData::readInto(uint8_t* buffer, size_t len) {
if (m_index_current + len > getDataLength()) {
FL_LOG(_log, LMsg("RawData") << m_index_current << " : " << len << " : " << getDataLength());
throw IndexOverflow(__FUNCTION__);
}
m_datasource->readInto(buffer, m_index_current, len);
m_index_current += len;
}
bool RawData::getLine(std::string& buffer) {
if (getCurrentIndex() >= getDataLength())
return false;
buffer = "";
char c;
while (getCurrentIndex() < getDataLength() && (c = read8()) != '\n')
buffer += c;
return true;
}
void RawData::read(std::string& outbuffer, int32_t size) {
if ((size < 0) || ((size + m_index_current) > getDataLength())) {
size = getDataLength() - m_index_current;
}
if (size == 0) {
outbuffer = "";
return;
}
outbuffer.resize(size);
// read directly into string
readInto(reinterpret_cast<uint8_t*>(&outbuffer[0]), size);
}
bool ParityDataField::checkSkeleton(std::shared_ptr<DataField> field) const
{
bool ret = false;
if (field)
{
std::shared_ptr<ParityDataField> pField = std::dynamic_pointer_cast<ParityDataField>(field);
if (pField)
{
std::vector<unsigned int> bitsPositions = pField->getBitsUsePositions();
ret = (pField->getDataLength() == getDataLength() &&
pField->getPosition() == getPosition() &&
pField->getParityType() == getParityType() &&
bitsPositions.size() == d_bitsUsePositions.size()
);
if (ret)
{
for (std::vector<unsigned int>::const_iterator i = d_bitsUsePositions.begin(), fi = bitsPositions.begin(); ret && i != d_bitsUsePositions.end() && fi != bitsPositions.end(); ++i, ++fi)
{
ret = (*i) == (*fi);
}
}
}
}
return ret;
}
size_t AOS_Bitstream_PDU::addFill(const size_t fillIdx, const ACE_UINT8 fillVal /* = 0xE0 */) {
long fillCount = getDataLength() - fillIdx;
if ( fillCount > 0 ) {
ACE_OS::memset(ptrData() + fillIdx, fillVal, fillCount);
return fillCount;
}
return 0;
}
unsigned long __RSPacketGetDataLength(__RSPacket* packet)
{
if (packet && isValidPacket(packet))
{
return getDataLength(packet);
}
return 0;
}
XMLCh* HexBin::getCanonicalRepresentation(const XMLCh* const hexData
, MemoryManager* const manager)
{
if (getDataLength(hexData) == -1)
return 0;
XMLCh* retStr = XMLString::replicate(hexData, manager);
XMLString::upperCaseASCII(retStr);
return retStr;
}
unsigned long __RSPacketGetData(__RSPacket* packet, unsigned char* databuf, unsigned long bufferSize)
{
if (packet && isValidPacket(packet) && databuf && bufferSize)
{
databuf[bufferSize - 1] = 0; // test the data buffer size is ture or NO.
bufferSize = min(bufferSize, getDataLength(packet));
if (bufferSize)
{
memcpy(databuf, packet->payload.data, bufferSize);
}
return bufferSize;
}
return 0;
}
std::vector<uint8_t> RawData::getDataInBytes() {
// get the total file size
uint32_t size = getDataLength();
// create output vector
std::vector<uint8_t> target;
// resize vector to file size
target.resize(size);
// read bytes directly into vector
readInto(&target[0], target.size());
return target;
}
void Packet15UpdatePlayerScore::pack() const
{
if (!packedData)
{
packedData = new char[getDataLength()];
}
packHeader();
char* dataP = &packedData[OFFSET_DATA];
dataP += util::pack(&playerID, 1, dataP);
dataP += util::pack(&newScore, 1, dataP);
packed = true;
}
bool BinaryDataField::checkSkeleton(std::shared_ptr<DataField> field) const
{
bool ret = false;
if (field)
{
std::shared_ptr<BinaryDataField> pField = std::dynamic_pointer_cast<BinaryDataField>(field);
if (pField)
{
ret = (pField->getDataLength() == getDataLength() &&
pField->getPaddingChar() == getPaddingChar() &&
pField->getPosition() == getPosition() &&
(pField->getIsFixedField() || getValue().size() == 0 || pField->getValue() == getValue())
);
}
}
return ret;
}
U32 SFXWavStream::read( U8 *buffer, U32 bytes )
{
AssertFatal( mStream, "SFXWavStream::seek() - Stream is null!" );
// Read in even sample chunks.
bytes -= bytes % mFormat.getBytesPerSample();
// Read the data and determine how much we've read.
// FileStreams apparently report positions past
// the actual stream length, so manually cap the
// numbers here.
const U32 oldPosition = mStream->getPosition();
mStream->read( bytes, buffer );
U32 newPosition = mStream->getPosition();
const U32 maxPosition = getDataLength() + mDataStart;
if( newPosition > maxPosition )
newPosition = maxPosition;
const U32 numBytesRead = newPosition - oldPosition;
// TODO: Is it *just* 16 bit samples that needs to
// be flipped? What about 32 bit samples?
#ifdef TORQUE_BIG_ENDIAN
// We need to endian-flip 16-bit data.
if ( getFormat().getBytesPerChannel() == 2 )
{
U16 *ds = (U16*)buffer;
U16 *de = (U16*)(buffer+bytes);
while (ds<de)
{
*ds = convertLEndianToHost(*ds);
ds++;
}
}
#endif
return numBytesRead;
}
bool CustomFormat::checkSkeleton(std::shared_ptr<Format> format) const
{
bool ret = false;
if (format)
{
std::shared_ptr<CustomFormat> pFormat = std::dynamic_pointer_cast<CustomFormat>(format);
if (pFormat && pFormat->getDataLength() == getDataLength())
{
std::list<std::shared_ptr<DataField> > fields = pFormat->getFieldList();
if (fields.size() == d_fieldList.size())
{
ret = true;
for (std::list<std::shared_ptr<DataField> >::const_iterator i = d_fieldList.cbegin(), fi = fields.cbegin(); ret && i != d_fieldList.cend() && fi != fields.cend(); ++i, ++fi)
{
ret = (*i)->checkSkeleton(*fi);
}
}
}
}
return ret;
}
bool PlayStream::play()
{
#ifdef RPLAYSTREAM_SHOW_SLOTS
printf("play() -- Card: %d Stream: %d\n",card_number,stream_number);
#endif // RPLAYSTREAM_SHOW_SLOTS
if(!is_open) {
printf("FAIL1\n");
return false;
}
if((!playing)&&(!is_paused)) {
HPICall(HPI_OutStreamSetTimeScale(NULL,hpi_stream,
(uint16_t)((1000.0/(double)play_speed)*
HPI_OSTREAM_TIMESCALE_UNITS)));
if(!HPICall(HPI_OutStreamGetInfoEx(NULL,hpi_stream,
&state,&buffer_size,&data_to_play,
&samples_played,&reserved))) {
printf("FAIL3\n");
return false;
}
fragment_size=buffer_size/4;
if(fragment_size>MAX_FRAGMENT_SIZE) {
fragment_size=MAX_FRAGMENT_SIZE;
}
if(pdata!=NULL) {
delete pdata;
}
pdata=(uint8_t *)malloc(fragment_size);
if(pdata==NULL) {
printf("FAIL4\n");
return false;
}
switch(getFormatTag()) {
case WAVE_FORMAT_PCM:
case WAVE_FORMAT_VORBIS:
switch(getBitsPerSample()) {
case 8:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_PCM8_UNSIGNED,
getSamplesPerSec(),0,0));
break;
case 16:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_PCM16_SIGNED,
getSamplesPerSec(),0,0));
break;
case 32:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_PCM32_SIGNED,
getSamplesPerSec(),0,0));
break;
default:
HPICall(HPI_AdapterClose(NULL,card_number));
return false;
break;
}
break;
case WAVE_FORMAT_MPEG:
switch(getHeadLayer()) {
case 1:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_MPEG_L1,getSamplesPerSec(),
getHeadBitRate(),getHeadFlags()));
break;
case 2:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_MPEG_L2,getSamplesPerSec(),
getHeadBitRate(),getHeadFlags()));
break;
case 3:
HPICall(HPI_FormatCreate(&format,getChannels(),
HPI_FORMAT_MPEG_L3,getSamplesPerSec(),
getHeadBitRate(),getHeadFlags()));
break;
default:
HPI_AdapterClose(NULL,card_number);
return false;
}
break;
default:
return false;
}
#if HPI_VER < 0x00030500
if(HPI_DataCreate(&hpi_data,&format,pdata,fragment_size)!=0) {
return false;
}
#endif
}
if(!is_paused) {
memset(pdata,0,fragment_size);
left_to_write=getDataLength()-seekWave(0,SEEK_CUR);
if(left_to_write<fragment_size) {
read_bytes = left_to_write;
left_to_write=0;
stopping=true;
}
else {
read_bytes=fragment_size;
left_to_write-=fragment_size;
}
readWave(pdata,read_bytes);
#if HPI_VER > 0x00030500
HPICall(HPI_OutStreamWriteBuf(NULL,hpi_stream,pdata,read_bytes,&format));
#else
HPICall(HPI_DataCreate(&hpi_data,&format,pdata,read_bytes));
HPICall(HPI_OutStreamWrite(NULL,hpi_stream,&hpi_data));
#endif
if(HPI_OutStreamStart(NULL,hpi_stream)!=0) {
printf("FAIL11\n");
return false;
}
clock->start(50);
clock->start(FRAGMENT_TIME);
playing=true;
is_paused=false;
stopping=false;
if(play_length>0) {
play_timer->start(play_length,true);
start_time=QTime::currentTime();
}
stream_state=PlayStream::Playing;
if(!restart_transport) {
emit isStopped(false);
emit played();
emit stateChanged(card_number,stream_number,(int)stream_state);
}
}
if((!playing)&(is_paused|repositioned)) {
HPICall(HPI_OutStreamStart(NULL,hpi_stream));
clock->start(FRAGMENT_TIME);
playing=true;
stopping=false;
is_paused=false;
stream_state=PlayStream::Playing;
if(!restart_transport) {
emit isStopped(false);
emit played();
emit stateChanged(card_number,stream_number,(int)stream_state);
}
}
return true;
}
void FASCN200BitFormat::setLinearData(const void* data, size_t dataLengthBytes)
{
unsigned int pos = 0;
unsigned char c;
if (dataLengthBytes * 8 < getDataLength())
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "Data length too small.");
}
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_SS)
{
char tmpmsg[64];
sprintf(tmpmsg, "The FASC-N Start Sentinel doesn't match (%x).", c);
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, tmpmsg);
}
setAgencyCode((unsigned short)revertField(data, dataLengthBytes, &pos, 16));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_FS)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N Field Separator doesn't match after the Agency Code.");
}
setSystemCode((unsigned short)revertField(data, dataLengthBytes, &pos, 16));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_FS)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N Field Separator doesn't match after the System Code.");
}
setUid(revertField(data, dataLengthBytes, &pos, 24));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_FS)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N Field Separator doesn't match after the Credential.");
}
setSerieCode((unsigned char)revertField(data, dataLengthBytes, &pos, 4));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_FS)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N Field Separator doesn't match after the Credential Series.");
}
setCredentialCode((unsigned char)revertField(data, dataLengthBytes, &pos, 4));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_FS)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N Field Separator doesn't match after the Credential Issue.");
}
setPersonIdentifier(revertField(data, dataLengthBytes, &pos, 40));
setOrganizationalCategory((FASCNOrganizationalCategory)revertField(data, dataLengthBytes, &pos, 4));
setOrganizationalIdentifier((unsigned char)revertField(data, dataLengthBytes, &pos, 16));
setPOACategory((FASCNPOAssociationCategory)revertField(data, dataLengthBytes, &pos, 4));
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != FASCN_ES)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The End Sentinel doesn't match.");
}
unsigned char lrc = calculateLRC(data, pos);
c = (unsigned char)revertField(data, dataLengthBytes, &pos, 4);
if (c != lrc)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The FASC-N LRC seems wrong.");
}
}
bool AcParser::parseState(struct AcState* dest, uint8_t parseBuffer[], int pbLen) {
if (pbLen != getDataLength()) {
Spark.publish("PARSE_ERROR", "BAD LENGTH");
// Something is wrong, skip parsing
return false;
}
// Verify the header bytes in the parser buffer match the header array
for (int i = 0; i < headerLength; i++) {
if (parseBuffer[i] != headerAndMask[i]) {
return false;
}
}
// Check the rest of the buffer for the unit being OFF (all bytes 0xFF) or verify the rest of the
// bytes match the expected mask. This ensures that bits that should alwasy be 1 are without
// having to add that logic into the various parsing bits
bool isOff = true;
bool maskMatches = true;
for (int i = headerLength; i < pbLen; i++) {
isOff = isOff && parseBuffer[i] == 0xFF;
maskMatches = maskMatches && ((parseBuffer[i] & headerAndMask[i]) == headerAndMask[i]);
}
if (isOff) {
updateStates(dest, 0, 0, FAN_OFF, MODE_OFF, false);
return true;
}
if (!maskMatches) {
char msg[20];
sprintf(msg, "INVALID MASKED BITS"); // TODO print parseBuffer
Spark.publish("PARSE_ERROR", msg);
return false;
}
bool timer = isTimer(parseBuffer, pbLen);
uint8_t tensBits = parseBuffer[headerLength];
uint8_t onesBits = parseBuffer[headerLength + 1];
double display = decodeDisplayNumber(tensBits, onesBits, timer);
if (display == -1) {
// Display digits were invalid, ignore buffer
char msg[20];
sprintf(msg, "INVALID DISPLAY: hl:%d %02x %02x", headerLength, tensBits, onesBits);
Spark.publish("PARSE_ERROR", msg);
return false;
}
uint8_t acModeBits = parseBuffer[acModeByteIndex];
enum AcModes acMode = decodeAcMode(acModeBits);
if (acMode == MODE_INVALID) {
// AC Mode was invalid, ignore buffer
char msg[20];
sprintf(msg, "INVALID MODE: %02x", acModeBits);
Spark.publish("PARSE_ERROR", msg);
return false;
}
uint8_t fanSpeedBits = parseBuffer[fanSpeedByteIndex];
enum FanSpeeds fanSpeed = decodeFanSpeed(fanSpeedBits);
if (fanSpeed == FAN_INVALID) {
// Fan Speed was invalid, ignore buffer
char msg[20];
sprintf(msg, "INVALID FAN: %02x", fanSpeedBits);
Spark.publish("PARSE_ERROR", msg);
return false;
}
if (timer) {
updateStates(dest, 0, display, fanSpeed, acMode, false);
} else {
updateStates(dest, (int) display, 0, fanSpeed, acMode, false);
}
return true;
}
void
cgicc::FormFile::writeToStream(std::ostream& out) const
{
out.write(getData().data(), getDataLength());
}
std::string DnsResource::getDataAsString()
{
uint8_t* resourceRawData = getRawData() + m_NameLength + 3*sizeof(uint16_t) + sizeof(uint32_t);
size_t dataLength = getDataLength();
DnsType dnsType = getDnsType();
std::string result = "";
switch (dnsType)
{
case DNS_TYPE_A:
{
if (dataLength != 4)
{
LOG_ERROR("DNS type is A but resource length is not 4 - packet is malformed");
break;
}
uint32_t addrAsInt = *(uint32_t*)resourceRawData;
IPv4Address ip4AddrElad(addrAsInt);
if (!ip4AddrElad.isValid())
{
LOG_ERROR("Invalid IPv4 address for DNS resource of type A");
break;
}
result = ip4AddrElad.toString();
break;
}
case DNS_TYPE_AAAA:
{
if (dataLength != 16)
{
LOG_ERROR("DNS type is AAAA but resource length is not 16 - packet is malformed");
break;
}
IPv6Address ip6Addr(resourceRawData);
if (!ip6Addr.isValid())
{
LOG_ERROR("Invalid IPv6 address for DNS resource of type AAAA");
break;
}
result = ip6Addr.toString();
break;
}
case DNS_TYPE_NS:
case DNS_TYPE_CNAME:
case DNS_TYPE_DNAM:
case DNS_TYPE_PTR:
case DNS_TYPE_MX:
{
decodeName((const char*)resourceRawData, result);
break;
}
default:
{
std::stringstream sstream;
sstream << "0x" << std::hex;
for(size_t i = 0; i < dataLength; i++)
sstream << std::setw(2) << std::setfill('0') << (int)resourceRawData[i];
result = sstream.str();
break;
}
}
return result;
}
/*
* Called for data received from tty
*
* The input data is expected to be in garmin usb-packet format.
*
* buf contains the data read, it may span more than one packet
* or even incomplete packets
*/
static int nat_receive(struct garmin_data *garmin_data_p,
const unsigned char *buf, int count)
{
unsigned long flags;
__u8 *dest;
int offs = 0;
int result = count;
int len;
while (offs < count) {
/* if buffer contains header, copy rest of data */
if (garmin_data_p->insize >= GARMIN_PKTHDR_LENGTH)
len = GARMIN_PKTHDR_LENGTH
+getDataLength(garmin_data_p->inbuffer);
else
len = GARMIN_PKTHDR_LENGTH;
if (len >= GPS_IN_BUFSIZ) {
/* seems to be an invalid packet, ignore rest
of input */
dev_dbg(&garmin_data_p->port->dev,
"%s - packet size too large: %d\n",
__func__, len);
garmin_data_p->insize = 0;
count = 0;
result = -EINVPKT;
} else {
len -= garmin_data_p->insize;
if (len > (count-offs))
len = (count-offs);
if (len > 0) {
dest = garmin_data_p->inbuffer
+ garmin_data_p->insize;
memcpy(dest, buf+offs, len);
garmin_data_p->insize += len;
offs += len;
}
}
/* do we have a complete packet ? */
if (garmin_data_p->insize >= GARMIN_PKTHDR_LENGTH) {
len = GARMIN_PKTHDR_LENGTH+
getDataLength(garmin_data_p->inbuffer);
if (garmin_data_p->insize >= len) {
garmin_write_bulk(garmin_data_p->port,
garmin_data_p->inbuffer,
len, 0);
garmin_data_p->insize = 0;
/* if this was an abort-transfer command,
flush all queued data. */
if (isAbortTrfCmnd(garmin_data_p->inbuffer)) {
spin_lock_irqsave(&garmin_data_p->lock,
flags);
garmin_data_p->flags |= FLAGS_DROP_DATA;
spin_unlock_irqrestore(
&garmin_data_p->lock, flags);
pkt_clear(garmin_data_p);
}
}
}
}
return result;
}
BOOL __RSPacketIsEmpty(__RSPacket* packet)
{
if (packet)
return (getDataLength(packet) == 0);
return YES;
}
bool DnsResource::setData(const std::string& dataAsString)
{
// convert data to byte array according to the DNS type
size_t dataLength = 0;
uint8_t dataAsByteArr[256];
switch (getDnsType())
{
case DNS_TYPE_A:
{
IPv4Address ip4Addr((std::string)dataAsString);
if (!ip4Addr.isValid())
{
LOG_ERROR("Requested DNS type is A but data '%s' is an illegal IPv4 address. Couldn't set data for resource", dataAsString.c_str());
return false;
}
dataLength = 4;
uint32_t addrAsInt = ip4Addr.toInt();
memcpy(dataAsByteArr, &addrAsInt, dataLength);
break;
}
case DNS_TYPE_AAAA:
{
IPv6Address ip6Addr((std::string)dataAsString);
if (!ip6Addr.isValid())
{
LOG_ERROR("Requested DNS type is AAAA but data '%s' is an illegal IPv6 address. Couldn't set data for resource", dataAsString.c_str());
return false;
}
dataLength = 16;
ip6Addr.copyTo(dataAsByteArr);
break;
}
case DNS_TYPE_NS:
case DNS_TYPE_CNAME:
case DNS_TYPE_DNAM:
case DNS_TYPE_PTR:
case DNS_TYPE_MX:
{
encodeName(dataAsString, (char*)dataAsByteArr, dataLength);
break;
}
default:
{
if (dataAsString.substr(0, 2) != "0x")
{
LOG_ERROR("DNS data for DNS type %d should be an hex stream and begin with '0x'", getDnsType());
return false;
}
if (dataAsString.length() % 2 != 0)
{
LOG_ERROR("DNS data for DNS type %d should be an hex stream with an even number of character. "
"Current character count is an odd number: %d", getDnsType(), dataAsString.length());
return false;
}
char* dataAsCharPtr = (char*)dataAsString.c_str();
dataAsCharPtr += 2; //skip the '0x' prefix
char strtolBuf[5] = { '0', 'x', 0, 0, 0 };
char* strtolEndPtr;
while (*dataAsCharPtr != 0)
{
strtolBuf[2] = dataAsCharPtr[0];
strtolBuf[3] = dataAsCharPtr[1];
dataAsByteArr[dataLength] = strtol(strtolBuf, &strtolEndPtr, 0);
if (strtolEndPtr[0] != '\0') {
//non-hexadecimal character encountered
LOG_ERROR("DNS data for DNS type %d should be a valid hex stream", getDnsType());
return false;
}
dataAsCharPtr += 2 * sizeof(char);
dataLength++;
}
break;
}
}
size_t dataLengthOffset = m_NameLength + (2*sizeof(uint16_t)) + sizeof(uint32_t);
size_t dataOffset = dataLengthOffset + sizeof(uint16_t);
if (m_DnsLayer != NULL)
{
size_t curLength = getDataLength();
if (dataLength > curLength)
{
if (!m_DnsLayer->extendLayer(m_OffsetInLayer + dataOffset, dataLength-curLength, this))
{
LOG_ERROR("Couldn't set data for DNS query, unable to extend layer");
return false;
}
}
else if (dataLength < curLength)
{
if (!m_DnsLayer->shortenLayer(m_OffsetInLayer + dataOffset, curLength-dataLength, this))
{
LOG_ERROR("Couldn't set data for DNS query, unable to shorten layer");
return false;
}
}
}
// write data to resource
memcpy(getRawData() + dataOffset, dataAsByteArr, dataLength);
//update data length in resource
dataLength = htons(dataLength);
memcpy(getRawData() + dataLengthOffset, &dataLength, sizeof(uint16_t));
return true;
}
void RawData::setIndex(uint32_t index) {
if (index > getDataLength())
throw IndexOverflow(__FUNCTION__);
m_index_current = index;
}
/*
* Sends a usb packet to the tty
*
* Assumes, that all packages and at an usb-packet boundary.
*
* return <0 on error, 0 if packet is incomplete or > 0 if packet was sent
*/
static int gsp_send(struct garmin_data *garmin_data_p,
const unsigned char *buf, int count)
{
struct device *dev = &garmin_data_p->port->dev;
const unsigned char *src;
unsigned char *dst;
int pktid = 0;
int datalen = 0;
int cksum = 0;
int i = 0;
int k;
dev_dbg(dev, "%s - state %d - %d bytes.\n", __func__,
garmin_data_p->state, count);
k = garmin_data_p->outsize;
if ((k+count) > GPS_OUT_BUFSIZ) {
dev_dbg(dev, "packet too large\n");
garmin_data_p->outsize = 0;
return -4;
}
memcpy(garmin_data_p->outbuffer+k, buf, count);
k += count;
garmin_data_p->outsize = k;
if (k >= GARMIN_PKTHDR_LENGTH) {
pktid = getPacketId(garmin_data_p->outbuffer);
datalen = getDataLength(garmin_data_p->outbuffer);
i = GARMIN_PKTHDR_LENGTH + datalen;
if (k < i)
return 0;
} else {
return 0;
}
dev_dbg(dev, "%s - %d bytes in buffer, %d bytes in pkt.\n", __func__, k, i);
/* garmin_data_p->outbuffer now contains a complete packet */
usb_serial_debug_data(&garmin_data_p->port->dev, __func__, k,
garmin_data_p->outbuffer);
garmin_data_p->outsize = 0;
if (GARMIN_LAYERID_APPL != getLayerId(garmin_data_p->outbuffer)) {
dev_dbg(dev, "not an application packet (%d)\n",
getLayerId(garmin_data_p->outbuffer));
return -1;
}
if (pktid > 255) {
dev_dbg(dev, "packet-id %d too large\n", pktid);
return -2;
}
if (datalen > 255) {
dev_dbg(dev, "packet-size %d too large\n", datalen);
return -3;
}
/* the serial protocol should be able to handle this packet */
k = 0;
src = garmin_data_p->outbuffer+GARMIN_PKTHDR_LENGTH;
for (i = 0; i < datalen; i++) {
if (*src++ == DLE)
k++;
}
src = garmin_data_p->outbuffer+GARMIN_PKTHDR_LENGTH;
if (k > (GARMIN_PKTHDR_LENGTH-2)) {
/* can't add stuffing DLEs in place, move data to end
of buffer ... */
dst = garmin_data_p->outbuffer+GPS_OUT_BUFSIZ-datalen;
memcpy(dst, src, datalen);
src = dst;
}
dst = garmin_data_p->outbuffer;
*dst++ = DLE;
*dst++ = pktid;
cksum += pktid;
*dst++ = datalen;
cksum += datalen;
if (datalen == DLE)
*dst++ = DLE;
for (i = 0; i < datalen; i++) {
__u8 c = *src++;
*dst++ = c;
cksum += c;
if (c == DLE)
*dst++ = DLE;
}
cksum = 0xFF & -cksum;
*dst++ = cksum;
if (cksum == DLE)
*dst++ = DLE;
*dst++ = DLE;
*dst++ = ETX;
i = dst-garmin_data_p->outbuffer;
send_to_tty(garmin_data_p->port, garmin_data_p->outbuffer, i);
garmin_data_p->pkt_id = pktid;
garmin_data_p->state = STATE_WAIT_TTY_ACK;
return i;
}
static int garmin_write(struct tty_struct *tty, struct usb_serial_port *port,
const unsigned char *buf, int count)
{
struct device *dev = &port->dev;
int pktid, pktsiz, len;
struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
__le32 *privpkt = (__le32 *)garmin_data_p->privpkt;
usb_serial_debug_data(dev, __func__, count, buf);
if (garmin_data_p->state == STATE_RESET)
return -EIO;
/* check for our private packets */
if (count >= GARMIN_PKTHDR_LENGTH) {
len = PRIVPKTSIZ;
if (count < len)
len = count;
memcpy(garmin_data_p->privpkt, buf, len);
pktsiz = getDataLength(garmin_data_p->privpkt);
pktid = getPacketId(garmin_data_p->privpkt);
if (count == (GARMIN_PKTHDR_LENGTH+pktsiz)
&& GARMIN_LAYERID_PRIVATE ==
getLayerId(garmin_data_p->privpkt)) {
dev_dbg(dev, "%s - processing private request %d\n",
__func__, pktid);
/* drop all unfinished transfers */
garmin_clear(garmin_data_p);
switch (pktid) {
case PRIV_PKTID_SET_MODE:
if (pktsiz != 4)
return -EINVPKT;
garmin_data_p->mode = __le32_to_cpu(privpkt[3]);
dev_dbg(dev, "%s - mode set to %d\n",
__func__, garmin_data_p->mode);
break;
case PRIV_PKTID_INFO_REQ:
priv_status_resp(port);
break;
case PRIV_PKTID_RESET_REQ:
process_resetdev_request(port);
break;
case PRIV_PKTID_SET_DEF_MODE:
if (pktsiz != 4)
return -EINVPKT;
initial_mode = __le32_to_cpu(privpkt[3]);
dev_dbg(dev, "%s - initial_mode set to %d\n",
__func__,
garmin_data_p->mode);
break;
}
return count;
}
}
if (garmin_data_p->mode == MODE_GARMIN_SERIAL) {
return gsp_receive(garmin_data_p, buf, count);
} else { /* MODE_NATIVE */
return nat_receive(garmin_data_p, buf, count);
}
}
