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