// ====================================================================== // \title UdpReceiverImpl.cpp // \author tcanham // \brief cpp file for UdpReceiver component implementation class // // \copyright // Copyright 2009-2015, by the California Institute of Technology. // ALL RIGHTS RESERVED. United States Government Sponsorship // acknowledged. // // ====================================================================== #include #include #include #include #include #include #include #include #include #include //#define DEBUG_PRINT(...) printf(##__VA_ARGS__) #define DEBUG_PRINT(...) namespace Svc { // ---------------------------------------------------------------------- // Construction, initialization, and destruction // ---------------------------------------------------------------------- UdpReceiverComponentImpl :: UdpReceiverComponentImpl( const char *const compName ) : UdpReceiverComponentBase(compName), m_fd(-1), m_packetsReceived(0), m_bytesReceived(0), m_packetsDropped(0), m_decodeErrors(0), m_firstSeq(true), m_currSeq(0) { } void UdpReceiverComponentImpl :: init( const NATIVE_INT_TYPE instance ) { UdpReceiverComponentBase::init(instance); } UdpReceiverComponentImpl :: ~UdpReceiverComponentImpl() { if (this->m_fd != -1) { close(this->m_fd); } } void UdpReceiverComponentImpl::open( const char* port ) { //create a UDP socket this->m_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); if (-1 == this->m_fd) { Fw::LogStringArg arg(strerror(errno)); this->log_WARNING_HI_UR_SocketError(arg); } sockaddr_in saddr; // zero out the structure memset(&saddr, 0, sizeof(saddr)); saddr.sin_family = AF_INET; saddr.sin_port = htons(atoi(port)); saddr.sin_addr.s_addr = htonl(INADDR_ANY); //bind socket to port NATIVE_INT_TYPE status = bind(this->m_fd , (struct sockaddr*)&saddr, sizeof(saddr)); if (-1 == status) { Fw::LogStringArg arg(strerror(errno)); this->log_WARNING_HI_UR_BindError(arg); close(this->m_fd); this->m_fd = -1; } else { this->log_ACTIVITY_HI_UR_PortOpened(atoi(port)); } } void UdpReceiverComponentImpl::startThread( NATIVE_UINT_TYPE priority, /*!< read task priority */ NATIVE_UINT_TYPE stackSize, /*!< stack size */ NATIVE_UINT_TYPE affinity /*!< cpu affinity */ ) { Os::TaskString name(this->getObjName()); Os::Task::TaskStatus stat = this->m_socketTask.start( name, 0, priority, stackSize, UdpReceiverComponentImpl::workerTask, this, affinity); FW_ASSERT(Os::Task::TASK_OK == stat,stat); } // ---------------------------------------------------------------------- // Handler implementations for user-defined typed input ports // ---------------------------------------------------------------------- void UdpReceiverComponentImpl :: Sched_handler( const NATIVE_INT_TYPE portNum, U32 context ) { this->tlmWrite_UR_BytesReceived(this->m_bytesReceived); this->tlmWrite_UR_PacketsReceived(this->m_packetsReceived); this->tlmWrite_UR_PacketsDropped(this->m_packetsDropped); } void UdpReceiverComponentImpl::workerTask(void* ptr) { UdpReceiverComponentImpl *compPtr = static_cast(ptr); while (true) { compPtr->doRecv(); } } void UdpReceiverComponentImpl::doRecv() { // wait for data from the socket NATIVE_INT_TYPE psize = recvfrom( this->m_fd, this->m_recvBuff.getBuffAddr(), this->m_recvBuff.getBuffCapacity(), MSG_WAITALL, 0, 0); if (-1 == psize) { if (errno != EINTR) { Fw::LogStringArg arg(strerror(errno)); this->log_WARNING_HI_UR_RecvError(arg); } return; } // reset buffer for deserialization Fw::SerializeStatus stat = this->m_recvBuff.setBuffLen(psize); FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat); // get sequence number U8 seqNum; stat = this->m_recvBuff.deserialize(seqNum); // check for deserialization error or port number too high if (stat != Fw::FW_SERIALIZE_OK) { this->log_WARNING_HI_UR_DecodeError(DECODE_SEQ,stat); this->m_decodeErrors++; return; } // track sequence number if (this->m_firstSeq) { // first time, set tracked sequence number equal to the one received this->m_currSeq = seqNum; this->m_firstSeq = false; } else { // make sure sequence number has gone up by one if (seqNum != ++this->m_currSeq) { // will only be right if it rolls over only once, but better than nothing U8 diff = seqNum - this->m_currSeq; this->m_packetsDropped += diff; // send EVR this->log_WARNING_HI_UR_DroppedPacket(diff); // reset to current sequence this->m_currSeq = seqNum; } } // get port number U8 portNum; stat = this->m_recvBuff.deserialize(portNum); // check for deserialization error or port number too high if (stat != Fw::FW_SERIALIZE_OK or portNum > this->getNum_PortsOut_OutputPorts()) { this->log_WARNING_HI_UR_DecodeError(DECODE_PORT,stat); this->m_decodeErrors++; return; } // get buffer for port stat = this->m_recvBuff.deserialize(this->m_portBuff); if (stat != Fw::FW_SERIALIZE_OK) { this->log_WARNING_HI_UR_DecodeError(DECODE_BUFFER,stat); this->m_decodeErrors++; return; } // call output port DEBUG_PRINT("Calling port %d with %d bytes.\n",portNum,this->m_portBuff.getBuffLength()); if (this->isConnected_PortsOut_OutputPort(portNum)) { Fw::SerializeStatus stat = this->PortsOut_out(portNum,this->m_portBuff); // If had issues deserializing the data, then report it: if (stat != Fw::FW_SERIALIZE_OK) { this->log_WARNING_HI_UR_DecodeError(PORT_SEND,stat); this->m_decodeErrors++; } this->m_packetsReceived++; this->m_bytesReceived += psize; } } #ifdef BUILD_UT UdpReceiverComponentImpl::UdpSerialBuffer& UdpReceiverComponentImpl::UdpSerialBuffer::operator=(const UdpReceiverComponentImpl::UdpSerialBuffer& other) { this->resetSer(); this->serialize(other.getBuffAddr(),other.getBuffLength(),true); return *this; } UdpReceiverComponentImpl::UdpSerialBuffer::UdpSerialBuffer( const Fw::SerializeBufferBase& other) : Fw::SerializeBufferBase() { FW_ASSERT(sizeof(this->m_buff)>= other.getBuffLength(),sizeof(this->m_buff),other.getBuffLength()); memcpy(this->m_buff,other.getBuffAddr(),other.getBuffLength()); this->setBuffLen(other.getBuffLength()); } UdpReceiverComponentImpl::UdpSerialBuffer::UdpSerialBuffer( const UdpReceiverComponentImpl::UdpSerialBuffer& other) : Fw::SerializeBufferBase() { FW_ASSERT(sizeof(this->m_buff)>= other.getBuffLength(),sizeof(this->m_buff),other.getBuffLength()); memcpy(this->m_buff,other.m_buff,other.getBuffLength()); this->setBuffLen(other.getBuffLength()); } UdpReceiverComponentImpl::UdpSerialBuffer::UdpSerialBuffer(): Fw::SerializeBufferBase() { } #endif } // end namespace Svc