144 lines
3.9 KiB
C++
144 lines
3.9 KiB
C++
#include <cstring>
|
|
#include <sys/socket.h>
|
|
#include <boost/system/error_code.hpp>
|
|
#include <livoxProto1/device.h>
|
|
#include "ioUringAssemblyEngine.h"
|
|
#include "pcloudStimulusBuffer.h"
|
|
#include "livoxGen1.h"
|
|
|
|
namespace smo {
|
|
namespace stim_buff {
|
|
|
|
struct DummyLivoxEthHeader
|
|
{
|
|
enum : uint32_t {
|
|
INVALID_ERR_CODE = 0xFFFFFFFFu
|
|
};
|
|
enum : uint8_t {
|
|
INVALID_TIMESTAMP_TYPE = 0xFFu,
|
|
INVALID_DATA_TYPE = 0xFFu
|
|
};
|
|
|
|
uint8_t version, slot, id, rsvd;
|
|
uint32_t err_code;
|
|
uint8_t timestamp_type, data_type;
|
|
uint8_t timestamp[8];
|
|
};
|
|
|
|
IoUringAssemblyEngine::IoUringAssemblyEngine(PcloudStimulusBuffer& parent_)
|
|
: parent(parent_),
|
|
frameAssemblyDesc(nullptr), ring{},
|
|
isSetup(false),
|
|
stallTimer(parent_.device->componentThread->getIoService())
|
|
{}
|
|
|
|
bool IoUringAssemblyEngine::setup()
|
|
{
|
|
if (isSetup)
|
|
{ return false; }
|
|
|
|
// Get FrameAssemblyDesc from staging buffer
|
|
frameAssemblyDesc = static_cast<std::shared_ptr<FrameAssemblyDesc>>(
|
|
parent.stagingBuffer);
|
|
|
|
if (!frameAssemblyDesc || frameAssemblyDesc->slots.empty())
|
|
{ return false; }
|
|
|
|
// Get UDP socket file descriptor
|
|
int udpFd = (*livoxProto1.livoxProto1_getPcloudDataFdDesc)()
|
|
->native_handle();
|
|
if (udpFd < 0)
|
|
{ return false; }
|
|
|
|
/** EXPLANATION:
|
|
* Initialize io_uring ring - allocate SQEs and CQEs for one frame assembly
|
|
* One SQE per slot (one datagram per slot)
|
|
*/
|
|
int ret = io_uring_queue_init(
|
|
static_cast<unsigned int>(frameAssemblyDesc->numSlots), &ring, 0);
|
|
if (ret < 0)
|
|
{ return false; }
|
|
|
|
isSetup = true;
|
|
return true;
|
|
}
|
|
|
|
void IoUringAssemblyEngine::finalize()
|
|
{
|
|
// Call stop() to cancel in-flight operations (stop() already cancels the timer)
|
|
stop();
|
|
|
|
// Clean up io_uring ring if it was initialized
|
|
if (isSetup)
|
|
{
|
|
io_uring_queue_exit(&ring);
|
|
isSetup = false;
|
|
}
|
|
|
|
// Reset state to allow setup() to be called again
|
|
frameAssemblyDesc = nullptr;
|
|
}
|
|
|
|
void IoUringAssemblyEngine::resetAndAssembleFrame()
|
|
{
|
|
// Design/stub: This method should:
|
|
// 1. Submit frameAssemblyDesc->numSlots RECVMSG SQEs using io_uring_prep_recvmsg()
|
|
// - Each SQE receives into frameAssemblyDesc->slots[i].vaddr
|
|
// - With size frameAssemblyDesc->slots[i].nBytes
|
|
// - Socket FD from parent.device->pcloudDataSocketDesc->native_handle()
|
|
// 2. Submit batch via io_uring_submit(&ring)
|
|
// 3. Set up stall timer using stallTimer with appropriate timeout
|
|
// - SQEs are independent and can arrive out of order
|
|
// - Timer detects if SQEs get stalled
|
|
}
|
|
|
|
void IoUringAssemblyEngine::stop()
|
|
{
|
|
// Design/stub: This method should:
|
|
// 1. Cancel all pending SQEs using io_uring cancellation mechanisms
|
|
// 2. Cancel in-flight stall timeout timer via stallTimer.cancel()
|
|
// 3. Set appropriate state flags
|
|
}
|
|
|
|
void IoUringAssemblyEngine::cancelIncompleteAndFillDummies()
|
|
{
|
|
if (!frameAssemblyDesc)
|
|
{ return; }
|
|
|
|
for (size_t i = 0; i < frameAssemblyDesc->numSlots; ++i)
|
|
{
|
|
// In the real path, decide from CQE accounting whether slot i completed.
|
|
// Here, demonstrate dummy header insertion API.
|
|
auto* hdr = reinterpret_cast<DummyLivoxEthHeader*>(frameAssemblyDesc->slots[i].vaddr);
|
|
hdr->err_code = DummyLivoxEthHeader::INVALID_ERR_CODE;
|
|
hdr->timestamp_type = DummyLivoxEthHeader::INVALID_TIMESTAMP_TYPE;
|
|
hdr->data_type = DummyLivoxEthHeader::INVALID_DATA_TYPE;
|
|
}
|
|
}
|
|
|
|
size_t IoUringAssemblyEngine::computePointsPerDgram(int returnMode)
|
|
{
|
|
/*
|
|
* Map modes to points per datagram based on Livox docs
|
|
* 1: first, 2: strongest -> 96 samples => 96 points
|
|
* 3: dual -> 48 samples * 2 points = 96
|
|
* 4: triple -> 30 samples * 3 points = 90
|
|
*/
|
|
switch (returnMode)
|
|
{
|
|
case static_cast<int>(livoxProto1::Device::ReturnMode::SingleFirst):
|
|
case static_cast<int>(livoxProto1::Device::ReturnMode::SingleStrongest):
|
|
case static_cast<int>(livoxProto1::Device::ReturnMode::Dual):
|
|
return 96u;
|
|
case static_cast<int>(livoxProto1::Device::ReturnMode::Triple):
|
|
return 90u;
|
|
default:
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": Unknown returnMode "
|
|
+ std::to_string(returnMode));
|
|
}
|
|
}
|
|
|
|
} // namespace stim_buff
|
|
} // namespace smo
|