43 #include <boost/python.hpp>
44namespace bp = boost::python;
54void rps::SrpV3Emulation::setup_python() {
56 bp::class_<rps::SrpV3Emulation, rps::SrpV3EmulationPtr, bp::bases<ris::Master, ris::Slave>, boost::noncopyable>(
59 bp::implicitly_convertible<rps::SrpV3EmulationPtr, ris::MasterPtr>();
60 bp::implicitly_convertible<rps::SrpV3EmulationPtr, ris::SlavePtr>();
65rps::SrpV3Emulation::SrpV3Emulation() :
ris::Master(),
ris::Slave() {
69 thread_ = std::thread(&SrpV3Emulation::runThread,
this);
73rps::SrpV3Emulation::~SrpV3Emulation() {
76 MemoryMap::iterator it = memMap_.begin();
77 while (it != memMap_.end()) {
84void rps::SrpV3Emulation::stop() {
86 std::lock_guard<std::mutex> lock(queMtx_);
89 queCond_.notify_all();
94 if (thread_.joinable()) thread_.join();
101void rps::SrpV3Emulation::runThread() {
103 log_->debug(
"Worker thread started");
107 std::unique_lock<std::mutex> lock(queMtx_);
108 queCond_.wait(lock, [
this] {
return !queue_.empty() || !threadEn_; });
110 if (!threadEn_)
break;
112 frame = queue_.front();
123 if (Py_IsInitialized()) {
134 log_->debug(
"Worker thread stopped");
138uint8_t* rps::SrpV3Emulation::allocatePage(uint64_t addr4k) {
139 uint8_t* page =
reinterpret_cast<uint8_t*
>(malloc(0x1000));
140 if (page ==
nullptr) {
141 log_->error(
"Failed to allocate page at 0x%" PRIx64, addr4k);
146 std::mt19937 gen(std::random_device {}());
147 uint32_t* p32 =
reinterpret_cast<uint32_t*
>(page);
148 for (
size_t i = 0; i < 0x1000 / 4; i++) p32[i] = gen();
150 memMap_.insert(std::make_pair(addr4k, page));
152 log_->debug(
"Allocating page at 0x%" PRIx64
". Total pages %" PRIu32, addr4k, totAlloc_);
157void rps::SrpV3Emulation::readMemory(uint64_t address, uint8_t* data, uint32_t size) {
163 addr4k = (address / 0x1000) * 0x1000;
164 off4k = address % 0x1000;
165 size4k = (addr4k + 0x1000) - (addr4k + off4k);
167 if (size4k > size) size4k = size;
169 auto it = memMap_.find(addr4k);
170 if (it == memMap_.end()) {
172 uint8_t* page = allocatePage(addr4k);
173 if (page ==
nullptr)
return;
174 memcpy(data, page + off4k, size4k);
176 memcpy(data, it->second + off4k, size4k);
186void rps::SrpV3Emulation::writeMemory(uint64_t address,
const uint8_t* data, uint32_t size) {
192 addr4k = (address / 0x1000) * 0x1000;
193 off4k = address % 0x1000;
194 size4k = (addr4k + 0x1000) - (addr4k + off4k);
196 if (size4k > size) size4k = size;
198 auto it = memMap_.find(addr4k);
199 if (it == memMap_.end()) {
200 uint8_t* page = allocatePage(addr4k);
201 if (page ==
nullptr)
return;
202 it = memMap_.find(addr4k);
205 memcpy(it->second + off4k, data, size4k);
216 std::lock_guard<std::mutex> lock(queMtx_);
219 queCond_.notify_one();
224 uint32_t header[HeadLen / 4];
225 uint32_t tail[TailLen / 4];
237 if (frame->getError()) {
238 log_->warning(
"Got errored frame = 0x%" PRIx8, frame->getError());
243 fSize = frame->getPayload();
244 if (fSize < HeadLen) {
245 log_->warning(
"Got undersized frame size = %" PRIu32, fSize);
254 if ((header[0] & 0xFF) != 0x03) {
255 log_->warning(
"Got non-SRPv3 frame, version = 0x%" PRIx32, header[0] & 0xFF);
260 opCode = (header[0] >> 8) & 0x3;
262 address = (
static_cast<uint64_t
>(header[3]) << 32) | header[2];
263 reqSize = header[4] + 1;
267 log_->warning(
"Got invalid SRPv3 opcode 0x3 for id=%" PRIu32,
id);
273 if (reqSize < 4 || reqSize > 4096 || (reqSize & 0x3) != 0) {
274 log_->warning(
"Got invalid SRPv3 request size=%" PRIu32
" for id=%" PRIu32, reqSize,
id);
278 doWrite = (opCode == 0x1);
279 isPost = (opCode == 0x2);
281 log_->debug(
"Got request id=%" PRIu32
", opCode=%" PRIu32
", addr=0x%" PRIx64
", size=%" PRIu32,
291 std::lock_guard<std::mutex> lock(memMtx_);
293 if (doWrite || isPost) {
295 if (fSize < HeadLen + reqSize) {
296 log_->warning(
"Write frame too small: got %" PRIu32
", need %" PRIu32, fSize, HeadLen + reqSize);
301 frLock = frame->lock();
302 fIter = frame->begin() + HeadLen;
303 std::vector<uint8_t> wrData(reqSize);
307 writeMemory(address, wrData.data(), reqSize);
309 log_->debug(
"Write complete id=%" PRIu32
", addr=0x%" PRIx64
", size=%" PRIu32,
id, address, reqSize);
316 uint32_t respLen = HeadLen + reqSize + TailLen;
318 respFrame->setPayload(respLen);
325 std::vector<uint8_t> rdData(reqSize);
326 readMemory(address, rdData.data(), reqSize);
333 log_->debug(
"Sending response id=%" PRIu32
", size=%" PRIu32,
id, respLen);
335 sendFrame(respFrame);
RAII helper that releases the Python GIL for a scope.
static std::shared_ptr< rogue::Logging > create(const std::string &name, bool quiet=false)
Creates a logger instance.
RAII helper that acquires the Python GIL for a scope.
Random-access byte iterator across a Frame payload.
static void fromFrame(rogue::interfaces::stream::FrameIterator &iter, uint32_t size, void *dst)
Copies bytes from a frame iterator to a destination pointer.
std::shared_ptr< rogue::interfaces::stream::Frame > FramePtr
Shared pointer alias for Frame.
static void toFrame(rogue::interfaces::stream::FrameIterator &iter, uint32_t size, void *src)
Copies bytes from a source pointer into a frame iterator.
std::shared_ptr< rogue::interfaces::stream::FrameLock > FrameLockPtr
Shared pointer alias for FrameLock.
std::shared_ptr< rogue::protocols::srp::SrpV3Emulation > SrpV3EmulationPtr