void TunTapComponent::rxThreadFunction()
{
//LOG(LINFO) << "RX thread started, listening on tun/tap device " << x_tunTapDevice;
fd_set socketSet;
struct timeval selectTimeout;
char buffer[MAX_BUF_SIZE];
int nread;
try
{
// read data coming from the kernel
while(true)
{
boost::this_thread::interruption_point();
// reset socket set and add tap descriptor
FD_ZERO(&socketSet);
FD_SET(tunFd_, &socketSet);
// initialize timeout
selectTimeout.tv_sec = 1;
selectTimeout.tv_usec = 0;
// suspend thread until we receive a packet or timeout
if (select(tunFd_ + 1, &socketSet, NULL, NULL, &selectTimeout) == 0) {
//LOG(LDEBUG) << "Timeout while waiting for incoming packet.";
} else {
if (FD_ISSET(tunFd_, &socketSet)) {
if ((nread = read(tunFd_, buffer, MAX_BUF_SIZE)) < 0)
{
LOG(LFATAL) << "Error while reading from tun/tap interface.";
continue;
}
LOG(LDEBUG) << "Read " << nread << " bytes from device "
<< tunName_;
// copy received data into new StackDataSet
shared_ptr<StackDataSet> packetBuffer(new StackDataSet);
packetBuffer->data.assign(buffer, buffer + nread);
// send downwards
sendDownwards(packetBuffer);
}
}
} // while (true)
throw SystemException("Rx thread stopped unexpectedly.");
}
catch(IrisException& ex)
{
LOG(LFATAL) << "Error in TunTap component: " << ex.what()
<< " - TX thread exiting.";
}
catch(boost::thread_interrupted)
{
LOG(LINFO) << "Thread " << boost::this_thread::get_id()
<< " in stack component interrupted.";
}
}
unsigned int WINAPI RecvThreadProc(LPVOID argv)
{
SOCKET hServSock = (SOCKET) argv;
CustomBuffer packetBuffer(BUF_SIZE);
char* message = new char[BUF_SIZE];
unsigned short packetSize = 0;
unsigned short messageLength = 0;
PacketType packetType = PACKET_NONE;
while(true)
{
memset(message, 0, sizeof(char)*BUF_SIZE);
packetSize = GetPacketSize(hServSock);
if(RecvMessage(hServSock, packetBuffer.GetBuffer(), packetSize) == SOCKET_ERROR)
{
ErrorHandling("RecvMessage(): error!", GetLastError());
getchar();
exit(1);
}
packetBuffer.Commit(packetSize);
packetBuffer.Read((char*)&packetType, 1);
packetBuffer.Read(message, packetSize - 1);
switch(packetType)
{
case PACKET_ENTER:
Log("Enter CSTalk");
break;
case PACKET_CHAT:
Log(message);
break;
case PACKET_EXIT:
break;
default:
break;
}
}
return 0;
}
void TopicSender::sendWithFEC()
{
#if WITH_OPENFEC
uint16_t msg_id = m_sender->allocateMessageID();
uint64_t dataSize = sizeof(FECHeader) + m_buf.size();
// If the message fits in a single packet, use that as the buffer size
uint64_t symbolSize;
uint64_t sourceSymbols;
if(dataSize <= FECPacket::MaxDataSize)
{
sourceSymbols = 1;
symbolSize = dataSize;
}
else
{
// We need to pad the data to a multiple of our packet payload size.
sourceSymbols = div_round_up(dataSize, FECPacket::MaxDataSize);
symbolSize = FECPacket::MaxDataSize;
}
ROS_DEBUG("dataSize: %lu, symbol size: %lu, sourceSymbols: %lu", dataSize, symbolSize, sourceSymbols);
uint64_t packetSize = sizeof(FECPacket::Header) + symbolSize;
ROS_DEBUG("=> packetSize: %lu", packetSize);
uint64_t repairSymbols = std::ceil(m_sender->fec() * sourceSymbols);
uint64_t numPackets = sourceSymbols + repairSymbols;
of_session_t* ses = 0;
uint32_t prng_seed = rand();
if(sourceSymbols >= MIN_PACKETS_LDPC)
{
ROS_DEBUG("%s: Choosing LDPC-Staircase codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 1) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create LDPC codec instance", m_topicName.c_str());
return;
}
of_ldpc_parameters_t params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.prng_seed = prng_seed;
params.N1 = 7;
ROS_DEBUG("LDPC seed: 7, 0x%X", params.prng_seed);
if(of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
else
{
ROS_DEBUG("%s: Choosing Reed-Solomon codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create REED_SOLOMON codec instance", m_topicName.c_str());
return;
}
of_rs_2_m_parameters params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.m = 8;
if(of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
std::vector<uint8_t> packetBuffer(numPackets * packetSize);
std::vector<void*> symbols(sourceSymbols + repairSymbols);
uint64_t writtenData = 0;
// Fill the source packets
for(uint64_t i = 0; i < sourceSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
uint64_t remainingSpace = symbolSize;
symbols[i] = dataPtr;
if(i == 0)
{
// First packet includes the FECHeader
FECHeader* msgHeader = reinterpret_cast<FECHeader*>(dataPtr);
// Fill in header fields
msgHeader->flags = m_flags;
msgHeader->topic_msg_counter = m_inputMsgCounter;
strncpy(msgHeader->topic_name, m_topicName.c_str(), sizeof(msgHeader->topic_name));
if(msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] != 0)
{
ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] = 0;
}
strncpy(msgHeader->topic_type, m_topicType.c_str(), sizeof(msgHeader->topic_type));
if(msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] != 0)
{
ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] = 0;
}
for(int i = 0; i < 4; ++i)
msgHeader->topic_md5[i] = m_md5[i];
dataPtr += sizeof(FECHeader);
remainingSpace -= sizeof(FECHeader);
}
uint64_t chunkSize = std::min(remainingSpace, m_buf.size() - writtenData);
memcpy(dataPtr, m_buf.data() + writtenData, chunkSize);
writtenData += chunkSize;
// Set any padding to zero
if(chunkSize < remainingSpace)
memset(dataPtr + chunkSize, 0, remainingSpace - chunkSize);
}
// Fill the repair packets
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
symbols[i] = dataPtr;
}
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
if(of_build_repair_symbol(ses, symbols.data(), i) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not build repair symbol", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
// FEC work is done
of_release_codec_instance(ses);
std::vector<unsigned int> packetOrder(numPackets);
std::iota(packetOrder.begin(), packetOrder.end(), 0);
// Send the packets in random order
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::mt19937 mt(seed);
std::shuffle(packetOrder.begin(), packetOrder.end(), mt);
ROS_DEBUG("Sending %d packets", (int)packetOrder.size());
for(unsigned int idx : packetOrder)
{
if(!m_sender->send(packetBuffer.data() + idx * packetSize, packetSize, m_topicName))
return;
}
#else
throw std::runtime_error("Forward error correction requested, but I was not compiled with FEC support...");
#endif
}
