/*******************************************************************
* Packet implementation
******************************************************************/
void packetWork(void)
{
auto inPort0 = this->input(0);
const auto msg = inPort0->peekMessage();
const auto &pkt = msg.extract<Pothos::Packet>();
int flags = SOAPY_SDR_ONE_PACKET;
long long timeNs = 0;
const size_t numElems = pkt.payload.elements();
//convert to target data type if not already
const auto outBuff = pkt.payload.convert(inPort0->dtype());
//parse metadata from packet
const auto txTimeMeta = pkt.metadata.find("txTime");
const auto txEndMeta = pkt.metadata.find("txEnd");
if (txTimeMeta != pkt.metadata.end())
{
flags |= SOAPY_SDR_HAS_TIME;
timeNs = txTimeMeta->second.convert<long long>();
}
if (txEndMeta != pkt.metadata.end())
{
flags |= SOAPY_SDR_END_BURST;
}
//parse labels from packet
for (const auto &label : pkt.labels)
{
//found a time label
if (label.id == "txTime")
{
flags |= SOAPY_SDR_HAS_TIME;
timeNs = label.data.convert<long long>();
}
//found an end label
if (label.id == "txEnd")
{
flags |= SOAPY_SDR_END_BURST;
}
}
//write the packet data
const long timeoutUs = this->workInfo().maxTimeoutNs/1000;
const void *buffs[1]; buffs[0] = outBuff.as<const void *>();
const int ret = _device->writeStream(_stream, buffs, numElems, flags, timeNs, timeoutUs);
//handle result
if (ret > 0) inPort0->popMessage();
else if (ret == SOAPY_SDR_TIMEOUT) return this->yield();
else
{
inPort0->popMessage();
throw Pothos::Exception("SDRSink::work()", "writeStream "+std::string(SoapySDR::errToStr(ret)));
}
}
/**********************************************************************
TERMINATE
Si occupa di:
-Uccidere root e tutti i figli ricorsivamente
-Rimettere i thread nella lista dei liberi
-Togliere i thread dalle varie liste/array in cui sono presenti
-Decrementare il valore di thread_count
-Pulire la inbox da eventuali messaggi
**********************************************************************/
void terminate (tcb_t *target) {
msg_t *msg;
tcb_t *child;
/* Se ha un padre lo elimino dai suoi figli */
outChild(target);
/* Caso ricorsivo -> HA figli (su cui viene chiamata la terminate) */
while (TRUE) {
if ( (child = removeChild(target)) == NULL ) break; /* Passa al caso base e pulisce thread, liste, messaggi... */
terminate(child);
}
/* Caso base -> NON ha figli */
/* Se è in qualche lista o è il thread corrente lo elimino */
if (((outThread(&ready_queue, target)) != NULL) || (current_thread == target)) {
/* Pulisco la inbox */
while (TRUE) {
if ( (msg = popMessage(&(target->t_inbox), NULL)) == NULL ) break;
freeMsg(msg);
}
/* Se è un manager lo elimino dal trap_managers array */
delete_manager(target);
if (current_thread == target) current_thread=NULL;
/* Restituisco ai thread liberi */
freeTcb(target);
thread_count--;
}
else if (outThread(&wait_queue, target)) {
/* Decremento contatore processi in attesa I/O o SSI */
if (target->waiting_for == SSI_tcb)
soft_block_count--;
/* Tutto come caso precedente*/
while (TRUE) {
if ( (msg = popMessage(&(target->t_inbox), NULL)) == NULL ) break;
freeMsg(msg);
}
delete_manager(target);
freeTcb(target);
thread_count--;
}
else PANIC();
}
static void *
executeActions(void *arg){
Message* msg;
int client;
while(1){
pthread_mutex_lock(&mut);
msg = popMessage();
if(msg != NULL){
if(strcmp(msg->resource, "login") == 0){
if(strcmp(msg->method, "register") == 0){
registerUser(msg);
}else if(strcmp(msg->method, "login") == 0){
loginUser(msg);
}
}else if(strcmp(msg->resource, "user") == 0){
if(strcmp(msg->method, "fee") == 0){
sendUserFee(msg);
}
}else if(strcmp(msg->resource, "mail") == 0){
if(strcmp(msg->method, "receive") == 0){
sendEmails(msg);
}
}
client = getClientCond(msg->referer);
pthread_cond_signal(&newData[client]);
}
else{
/* Sleep until new Messages */
printf("Stack Empty, Sleeping\n");
pthread_cond_wait(&newMessage, &mut);
printf("Woke up to consume Stack\n");
}
pthread_mutex_unlock(&mut);
}
}
void work(void)
{
auto inputPort = this->input(0);
auto outputPort = this->output(0);
while (inputPort->hasMessage())
{
auto m = inputPort->popMessage();
outputPort->postMessage(m);
}
const auto &buffer = inputPort->buffer();
if (buffer.length != 0)
{
outputPort->postBuffer(buffer);
inputPort->consume(inputPort->elements());
for (size_t i = 0; i < inputPort->elements(); i++)
{
if (std::generate_canonical<double, 10>(_gen) <= _probability)
{
Pothos::Label label;
label.index = i;
label.width = buffer.dtype.size();
if (not _ids.empty()) label.id = _ids.at(_randomId(_gen));
outputPort->postLabel(label);
}
}
}
}
void work(void)
{
auto inPort = this->input(0);
auto outPort = this->output(0);
inPort->setReserve(_mod);
//handle packet conversion if applicable
if (inPort->hasMessage())
{
auto msg = inPort->popMessage();
if (msg.type() == typeid(Pothos::Packet))
this->msgWork(msg.extract<Pothos::Packet>());
else outPort->postMessage(msg);
return; //output buffer used, return now
}
//calculate work size
const size_t numSyms = std::min(inPort->elements() / _mod, outPort->elements());
if (numSyms == 0) return;
//perform conversion
auto in = inPort->buffer().as<const unsigned char *>();
auto out = outPort->buffer().as<unsigned char *>();
switch (_order)
{
case MSBit: ::bitsToSymbolsMSBit(_mod, in, out, numSyms); break;
case LSBit: ::bitsToSymbolsLSBit(_mod, in, out, numSyms); break;
}
//produce/consume
inPort->consume(numSyms * _mod);
outPort->produce(numSyms);
}
void RosoutPanel::setBufferSize(uint32_t size)
{
max_messages_ = size;
while (messages_.size() >= max_messages_)
{
popMessage();
}
}
void work(void)
{
auto in0 = this->input("in0");
if (in0->hasMessage())
{
in0->popMessage();
triggered++;
}
}
void ossimPlanetClientConnection::sendNextMessage()
{
OpenThreads::ScopedLock<OpenThreads::Mutex> lock1(theMessageQueueMutex);
OpenThreads::ScopedLock<OpenThreads::Mutex> lock2(theMutex);
if(!theMessageQueue.empty()&&theSocket)
{
ossimString message = popMessage();
theSocket->writestring(message.c_str());
}
}
void work(void)
{
workCount++;
auto in0 = this->input("in0");
auto out0 = this->output("out0");
if (in0->hasMessage())
{
auto msg = in0->popMessage();
out0->postMessage(msg);
}
}
void CombatMessageWindowWithHistory::pushMessage( const std::string& message )
{
if( messageBuffer[ 0 ] != NULL )
popMessage();
messageBuffer[ 0 ] = new TextMessage;
messageBuffer[ 0 ]->initTime = Timer::getTime();
messageBuffer[ 0 ]->text = ogui->CreateTextLabel( win, textXPosition, getVerticalPosition( 0 ), textXSize, textHeight, message.c_str() );
messageBuffer[ 0 ]->text->SetFont( textFont );
messageBuffer[ 0 ]->alpha = 1.0f;
}
/***********************************************************************
* work function
**********************************************************************/
void SpectrogramDisplay::work(void)
{
auto updateRate = this->height()/_timeSpan;
if (updateRate != _lastUpdateRate) this->callVoid("updateRateChanged", updateRate);
_lastUpdateRate = updateRate;
auto inPort = this->input(0);
if (not inPort->hasMessage()) return;
const auto msg = inPort->popMessage();
//label-based messages have in-line commands
if (msg.type() == typeid(Pothos::Label))
{
const auto &label = msg.convert<Pothos::Label>();
if (label.id == _freqLabelId and label.data.canConvert(typeid(double)))
{
this->setCenterFrequency(label.data.convert<double>());
}
if (label.id == _rateLabelId and label.data.canConvert(typeid(double)))
{
this->setSampleRate(label.data.convert<double>());
}
}
//packet-based messages have payloads to FFT
if (msg.type() == typeid(Pothos::Packet))
{
const auto &buff = msg.convert<Pothos::Packet>().payload;
auto floatBuff = buff.convert(Pothos::DType(typeid(std::complex<float>)), buff.elements());
//safe guard against FFT size changes, old buffers could still be in-flight
if (floatBuff.elements() != this->numFFTBins()) return;
//handle automatic FFT mode
if (_fftModeAutomatic)
{
const bool isComplex = buff.dtype.isComplex();
const bool changed = _fftModeComplex != isComplex;
_fftModeComplex = isComplex;
if (changed) QMetaObject::invokeMethod(this, "handleUpdateAxis", Qt::QueuedConnection);
}
//power bins to points on the curve
CArray fftBins(floatBuff.as<const std::complex<float> *>(), this->numFFTBins());
const auto powerBins = _fftPowerSpectrum.transform(fftBins, _fullScale);
this->appendBins(powerBins);
}
}
void ConstellationDisplay::work(void)
{
auto inPort = this->input(0);
if (not inPort->hasMessage()) return;
const auto msg = inPort->popMessage();
//packet-based messages have payloads to plot
if (msg.type() == typeid(Pothos::Packet))
{
_queueDepth++;
const auto &buff = msg.convert<Pothos::Packet>().payload;
auto floatBuff = buff.convert(Pothos::DType(typeid(std::complex<float>)), buff.elements());
QMetaObject::invokeMethod(this, "handleSamples", Qt::QueuedConnection, Q_ARG(Pothos::BufferChunk, floatBuff));
}
}
void RosoutPanel::processMessage(const rosgraph_msgs::Log::ConstPtr& message)
{
uint32_t id = message_id_counter_++;
messages_.insert(std::make_pair(id, message));
if (filter(id))
{
addMessageToTable(message, id);
}
validateOrderedMessages();
if (messages_.size() > max_messages_)
{
popMessage();
}
}
void work(void)
{
auto inputPort = this->input(0);
auto outputPort = this->output(0);
//extract message
if (not inputPort->hasMessage()) return;
auto msg = inputPort->popMessage();
//forward non-packet messages
if (msg.type() != typeid(Pothos::Packet))
{
outputPort->postMessage(msg);
return;
}
const auto &packet = msg.extract<Pothos::Packet>();
//post output labels
for (auto label : packet.labels)
{
outputPort->postLabel(label.toAdjusted(
packet.payload.dtype.size(), 1)); //elements to bytes
}
//post start of frame label
if (not _frameStartId.empty())
{
outputPort->postLabel(Pothos::Label(_frameStartId, Pothos::Object(), 0, packet.payload.dtype.size()));
}
//post end of frame label
if (not _frameEndId.empty())
{
outputPort->postLabel(Pothos::Label(_frameEndId, Pothos::Object(), packet.payload.length-1, packet.payload.dtype.size()));
}
//post the payload
outputPort->postBuffer(packet.payload);
}
void work(void)
{
auto inputPort = this->input(0);
auto outputPort = this->output(0);
while (inputPort->hasMessage() and _elementsLeft != 0)
{
auto m = inputPort->popMessage();
outputPort->postMessage(m);
_elementsLeft -= 1;
}
auto buffer = inputPort->buffer();
//input port type unspecified, inspect buffer for actual element count
const size_t elems = std::min(_elementsLeft, buffer.elements());
if (elems != 0)
{
buffer.length = elems*buffer.dtype.size();
outputPort->postBuffer(buffer);
inputPort->consume(buffer.length);
_elementsLeft -= elems;
}
}
void
xpcc::rpr::Interface<Device, N>::dropReceivedMessage()
{
popMessage(receivedMessages);
}
/* Aggiunge il tcb_t puntato da p alla lista tcbFree_h, rendendolo quindi nuovamente disponibile*/
void freeTcb(tcb_t *p) {
msg_t*m=NULL;
while((m=popMessage(&(p->t_inbox), NULL)) != NULL)
freeMsg(m);
list_add(&p->t_next, &tcbFree_h.t_next);
}
void work(void)
{
auto inputPort = this->input(0);
auto outputPort = this->output(0);
//forward messages
while (inputPort->hasMessage())
{
auto msg = inputPort->popMessage();
outputPort->postMessage(msg);
}
//is there any input buffer available?
if (inputPort->elements() == 0) return;
//start frame mode has its own work implementation
if (_startFrameMode) return this->startFrameModeWork();
//drop until start of frame label
if (_fullFrameMode and not _inFrame)
{
for (const auto &label : inputPort->labels())
{
//end of input buffer labels, exit loop
if (label.index >= inputPort->elements()) break;
//ignore labels that are not start of frame
if (label.id != _frameStartId) continue;
//in position 0, set in frame, done loop
if (label.index == 0)
{
_inFrame = true;
break;
}
//otherwise consume up to but not including
//done work, will be in-frame for next work
else
{
inputPort->consume(label.index);
_inFrame = true;
return;
}
}
//start of frame not found, consume everything, exit this work
if (not _inFrame)
{
inputPort->consume(inputPort->elements());
return;
}
}
//grab the input buffer
Pothos::Packet packet;
packet.payload = inputPort->buffer();
if (_mtu != 0)
{
const auto elemSize = packet.payload.dtype.size();
const auto mtuElems = (_mtu/elemSize)*elemSize;
packet.payload.length = std::min(mtuElems, packet.payload.length);
}
//grab the input labels
for (const auto &label : inputPort->labels())
{
const auto pktLabel = label.toAdjusted(
1, packet.payload.dtype.size()); //bytes to elements
if (pktLabel.index >= packet.payload.elements()) break;
packet.labels.push_back(pktLabel);
//end of frame found, truncate payload and leave loop
if (_fullFrameMode and label.id == _frameEndId)
{
packet.payload.length = label.index+label.width;
_inFrame = false;
break;
}
}
//consume the input and produce the packet
inputPort->consume(packet.payload.length);
outputPort->postMessage(packet);
}
/**********************************************************************
RECV
Mette in wait per un messaggio (settando prima gli opportuni campi
ausiliari nella struttura del tcb richiedente) se non vi è il
messaggio cercato nella inbox (o un qualsiasi messaggio per
ANYMESSAGE). Caso BLOCCANTE.
Altrimenti viene creata l'astrazione del messaggio ricevuto
restituendo il payload inviato al thread all'indirizzo indicato
nel registro a2 (reply) ---> CASO NON BLOCCANTE.
**********************************************************************/
tcb_t *recv (tcb_t *receiver, tcb_t *sender, U32 *reply){
msg_t *msg;
/* Caso ANYMESSAGE, attesa di un qualsiasi messaggio */
if (sender == ANYMESSAGE) {
/* Cerco di estrarre il primo messaggio, se c'è */
msg = popMessage(&(receiver->t_inbox), NULL);
/* Inbox vuota -> wait */
if (msg == NULL) {
/* Per chi sono fermo */
receiver->waiting_for = ANYMESSAGE;
/* Dove aspetto la risposta */
receiver->reply = reply;
/* Metto in wait_queue */
insertThread(&wait_queue, receiver);
current_thread = NULL;
return NULL;
}
/* Inbox NON vuota -> preleva messaggio */
else {
*reply = msg->m_message;
sender = msg->m_sender;
/* Restituisco il messaggio alla lista dei liberi */
freeMsg(msg);
/* Restituisco il mittente del messaggio */
return(sender);
}
}
/* Caso THREAD SPECIFICATO */
else if (sender != ANYMESSAGE) {
/* Cerco di estrarre il messaggio inviato dal thread specificato */
msg = popMessage(&(receiver->t_inbox), sender);
/* Messaggio non trovato -> wait */
if (msg == NULL) {
/* Per chi sono fermo */
receiver->waiting_for = sender;
/* Dove aspetto la risposta */
receiver->reply = reply;
/* Metto in wait_queue */
insertThread(&wait_queue, receiver);
current_thread = NULL;
return NULL;
}
/* Messaggio trovato -> preleva messaggio */
else {
*reply = msg->m_message;
/* Se prelevo risposta ad un servizio decremento soft_block_count */
if (sender == (tcb_t *)MAGIC_SSI) soft_block_count--;
/* Restituisco il messaggio alla lista dei liberi */
freeMsg(msg);
/* Restituisco il mittente del messaggio */
return(sender);
}
}
return NULL;
}
void
xpcc::rpr::Interface<Device, N>::update()
{
uint8_t data;
if (status & STATUS_END_DELIMITER_RECEIVED && !messagesToSend.isEmpty())
{
writeMessage(getMessage(messagesToSend));
popMessage(messagesToSend);
}
while (Device::read(data))
{
XPCC_RPR_LOG("receiving raw " << xpcc::hex << data << xpcc::ascii);
if (data == startDelimiterByte)
{
status &= ~STATUS_END_DELIMITER_RECEIVED;
status |= STATUS_START_DELIMITER_RECEIVED;
XPCC_RPR_LOG("start delimiter");
crc = crcInitialValue;
length = 0;
nextEscaped = false;
// we do not send the frame boundaries here, but wait for the AC.
}
else if (data == endDelimiterByte)
{
if (length >= 6 && (status & STATUS_START_DELIMITER_RECEIVED))
{
status &= ~STATUS_START_DELIMITER_RECEIVED;
status |= STATUS_END_DELIMITER_RECEIVED;
if (!(status & STATUS_SOURCE_RECOGNISED) && (receiveBuffer.type != MESSAGE_TYPE_UNICAST))
{
XPCC_RPR_LOG("tx: forwarding endDelimiterByte");
Device::write(endDelimiterByte);
}
XPCC_RPR_LOG("end delimiter with length=" << length);
if (status & STATUS_DESTINATION_RECOGNISED)
{
if (crc == 0)
{
XPCC_RPR_LOG("crc check success");
if (receiveBuffer.length > 1)
{
receiveBuffer.command = receiveBuffer.payload[0];
receiveBuffer.payload += 1;
receiveBuffer.length -= 1;
}
pushMessage(receivedMessages, &receiveBuffer);
receiveBuffer.payload = rx_buffer;
}
else {
XPCC_RPR_LOG("crc check failure");
}
}
}
crc = crcInitialValue;
length = 0;
nextEscaped = false;
}
else if (data == controlEscapeByte)
{
// the next byte is escaped
nextEscaped = true;
XPCC_RPR_LOG("escape sequence");
continue;
}
else
{
if (nextEscaped)
{
nextEscaped = false;
// toggle bit 5
data = data ^ 0x20;
XPCC_RPR_LOG("data escaped");
}
// all data is now escaped
// make sure we actually received a start delimiter before the payload
if (!(status & STATUS_START_DELIMITER_RECEIVED))
return;
switch (length++)
{
// LSB of destination address
case 0:
XPCC_RPR_LOG("rx: LSB dest");
addressBuffer = data;
break;
// MSB of destination address
case 1:
{
XPCC_RPR_LOG("rx: MSB dest");
// check the destination address against our own
uint16_t dest = (data << 8) | addressBuffer;
status &= ~(STATUS_DESTINATION_RECOGNISED | STATUS_RX_BUFFER_OVERFLOW | STATUS_SOURCE_RECOGNISED);
receiveBuffer.type = MESSAGE_TYPE_ANY;
receiveBuffer.destination = dest;
receiveBuffer.length = 0;
// it is a broadcast, we need to listen
if (receiveBuffer.destination == ADDRESS_BROADCAST)
{
XPCC_RPR_LOG("rx: broadcast");
receiveBuffer.type = MESSAGE_TYPE_BROADCAST;
status |= STATUS_DESTINATION_RECOGNISED;
}
else
{
if (receiveBuffer.destination & ADDRESS_INDIVIDUAL_GROUP)
{
// group address
if (_groupAddress == (receiveBuffer.destination & ADDRESS_VALUE))
{
XPCC_RPR_LOG("rx: my group");
receiveBuffer.type = MESSAGE_TYPE_MULTICAST;
status |= STATUS_DESTINATION_RECOGNISED;
}
}
else {
// individual address
if (_address == (receiveBuffer.destination & ADDRESS_VALUE))
{
XPCC_RPR_LOG("rx: my address");
receiveBuffer.type = MESSAGE_TYPE_UNICAST;
status |= STATUS_DESTINATION_RECOGNISED;
}
}
}
if (status & STATUS_DESTINATION_RECOGNISED)
{
crc = crcUpdate(crc, addressBuffer);
crc = crcUpdate(crc, data);
}
}
break;
// LSB of source address
case 2:
XPCC_RPR_LOG("rx: LSB source");
addressBuffer = data;
break;
// MSB of Source Address
case 3:
{
XPCC_RPR_LOG("rx: MSB source");
// check the source address against our own
uint16_t source = (data << 8) | addressBuffer;
receiveBuffer.source = (source & ADDRESS_VALUE);
if (_address == receiveBuffer.source)
{
status |= STATUS_SOURCE_RECOGNISED;
}
if (status & STATUS_DESTINATION_RECOGNISED)
{
crc = crcUpdate(crc, addressBuffer);
crc = crcUpdate(crc, data);
}
if (!(status & STATUS_SOURCE_RECOGNISED) && (receiveBuffer.type != MESSAGE_TYPE_UNICAST))
{
XPCC_RPR_LOG("tx: forwarding destination");
Device::write(startDelimiterByte);
writeByteEscaped(receiveBuffer.destination);
writeByteEscaped(receiveBuffer.destination >> 8);
XPCC_RPR_LOG("tx: forwarding source");
writeByteEscaped(addressBuffer);
writeByteEscaped(data);
}
else {
XPCC_RPR_LOG("rx: no forwarding");
}
}
break;
default:
if (status & STATUS_DESTINATION_RECOGNISED)
{
if (length <= N+8)
{
XPCC_RPR_LOG("rx: buffering payload");
receiveBuffer.payload[length-5] = data;
receiveBuffer.length++;
crc = crcUpdate(crc, data);
}
else {
// really, really bad programmer !
// now go sit in the corner and increase dat payload buffer
status |= STATUS_RX_BUFFER_OVERFLOW;
XPCC_RPR_LOG("rx: buffer overflow!!!");
}
}
if (!(status & STATUS_SOURCE_RECOGNISED) && (receiveBuffer.type != MESSAGE_TYPE_UNICAST))
{
XPCC_RPR_LOG("forwarding payload");
writeByteEscaped(data);
}
break;
}
}
