rogue
Loading...
Searching...
No Matches
SrpV3Emulation.cpp
Go to the documentation of this file.
1
17#include "rogue/Directives.h"
18
20
21#include <inttypes.h>
22#include <stdint.h>
23
24#include <cstring>
25#include <memory>
26#include <random>
27#include <utility>
28#include <vector>
29
30#include "rogue/GilRelease.h"
31#include "rogue/Logging.h"
32#include "rogue/ScopedGil.h"
38
39namespace rps = rogue::protocols::srp;
41
42#ifndef NO_PYTHON
43 #include <boost/python.hpp>
44namespace bp = boost::python;
45#endif
46
48rps::SrpV3EmulationPtr rps::SrpV3Emulation::create() {
49 rps::SrpV3EmulationPtr p = std::make_shared<rps::SrpV3Emulation>();
50 return (p);
51}
52
54void rps::SrpV3Emulation::setup_python() {
55#ifndef NO_PYTHON
56 bp::class_<rps::SrpV3Emulation, rps::SrpV3EmulationPtr, bp::bases<ris::Master, ris::Slave>, boost::noncopyable>(
57 "SrpV3Emulation",
58 bp::init<>());
59 bp::implicitly_convertible<rps::SrpV3EmulationPtr, ris::MasterPtr>();
60 bp::implicitly_convertible<rps::SrpV3EmulationPtr, ris::SlavePtr>();
61#endif
62}
63
65rps::SrpV3Emulation::SrpV3Emulation() : ris::Master(), ris::Slave() {
66 log_ = rogue::Logging::create("SrpV3Emulation");
67 totAlloc_ = 0;
68 threadEn_ = true;
69 thread_ = std::thread(&SrpV3Emulation::runThread, this);
70}
71
73rps::SrpV3Emulation::~SrpV3Emulation() {
74 stop();
75
76 MemoryMap::iterator it = memMap_.begin();
77 while (it != memMap_.end()) {
78 free(it->second);
79 ++it;
80 }
81}
82
84void rps::SrpV3Emulation::stop() {
85 {
86 std::lock_guard<std::mutex> lock(queMtx_);
87 threadEn_ = false;
88 }
89 queCond_.notify_all();
90
91 // Release the GIL: worker may be mid-sendFrame to a Python slave.
92 {
94 if (thread_.joinable()) thread_.join();
95 }
96
97 ris::Master::stop();
98}
99
101void rps::SrpV3Emulation::runThread() {
102 ris::FramePtr frame;
103 log_->debug("Worker thread started");
104
105 while (threadEn_) {
106 {
107 std::unique_lock<std::mutex> lock(queMtx_);
108 queCond_.wait(lock, [this] { return !queue_.empty() || !threadEn_; });
109
110 if (!threadEn_) break;
111
112 frame = queue_.front();
113 queue_.pop();
114 }
115
116 processFrame(frame);
117
118 // acceptFrame() queues the caller's frame uncopied, so a Python-owned
119 // FramePtr carries a Boost.Python deleter that calls Py_DECREF without the
120 // GIL, which is fatal on this thread. Release here under the GIL instead of
121 // at the next queue_.front() overwrite or thread exit.
122#ifndef NO_PYTHON
123 if (Py_IsInitialized()) {
125 frame.reset();
126 } else {
127 frame.reset();
128 }
129#else
130 frame.reset();
131#endif
132 }
133
134 log_->debug("Worker thread stopped");
135}
136
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);
142 return nullptr;
143 }
144
145 // Fill with random data to emulate uninitialized hardware memory
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();
149
150 memMap_.insert(std::make_pair(addr4k, page));
151 totAlloc_++;
152 log_->debug("Allocating page at 0x%" PRIx64 ". Total pages %" PRIu32, addr4k, totAlloc_);
153 return page;
154}
155
157void rps::SrpV3Emulation::readMemory(uint64_t address, uint8_t* data, uint32_t size) {
158 uint64_t addr4k;
159 uint64_t off4k;
160 uint64_t size4k;
161
162 while (size > 0) {
163 addr4k = (address / 0x1000) * 0x1000;
164 off4k = address % 0x1000;
165 size4k = (addr4k + 0x1000) - (addr4k + off4k);
166
167 if (size4k > size) size4k = size;
168
169 auto it = memMap_.find(addr4k);
170 if (it == memMap_.end()) {
171 // Allocate page with random data on first read (like uninitialized SRAM)
172 uint8_t* page = allocatePage(addr4k);
173 if (page == nullptr) return;
174 memcpy(data, page + off4k, size4k);
175 } else {
176 memcpy(data, it->second + off4k, size4k);
177 }
178
179 size -= size4k;
180 address += size4k;
181 data += size4k;
182 }
183}
184
186void rps::SrpV3Emulation::writeMemory(uint64_t address, const uint8_t* data, uint32_t size) {
187 uint64_t addr4k;
188 uint64_t off4k;
189 uint64_t size4k;
190
191 while (size > 0) {
192 addr4k = (address / 0x1000) * 0x1000;
193 off4k = address % 0x1000;
194 size4k = (addr4k + 0x1000) - (addr4k + off4k);
195
196 if (size4k > size) size4k = size;
197
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);
203 }
204
205 memcpy(it->second + off4k, data, size4k);
206
207 size -= size4k;
208 address += size4k;
209 data += size4k;
210 }
211}
212
214void rps::SrpV3Emulation::acceptFrame(ris::FramePtr frame) {
215 {
216 std::lock_guard<std::mutex> lock(queMtx_);
217 queue_.push(frame);
218 }
219 queCond_.notify_one();
220}
221
223void rps::SrpV3Emulation::processFrame(ris::FramePtr frame) {
224 uint32_t header[HeadLen / 4];
225 uint32_t tail[TailLen / 4];
226 uint32_t fSize;
227 uint32_t opCode;
228 uint32_t id;
229 uint64_t address;
230 uint32_t reqSize;
231 bool doWrite;
232 bool isPost;
233
234 rogue::GilRelease noGil;
235 ris::FrameLockPtr frLock = frame->lock();
236
237 if (frame->getError()) {
238 log_->warning("Got errored frame = 0x%" PRIx8, frame->getError());
239 return;
240 }
241
242 // Check minimum frame size
243 fSize = frame->getPayload();
244 if (fSize < HeadLen) {
245 log_->warning("Got undersized frame size = %" PRIu32, fSize);
246 return;
247 }
248
249 // Read header
250 ris::FrameIterator fIter = frame->begin();
251 ris::fromFrame(fIter, HeadLen, header);
252
253 // Verify SRPv3 version
254 if ((header[0] & 0xFF) != 0x03) {
255 log_->warning("Got non-SRPv3 frame, version = 0x%" PRIx32, header[0] & 0xFF);
256 return;
257 }
258
259 // Extract fields
260 opCode = (header[0] >> 8) & 0x3;
261 id = header[1];
262 address = (static_cast<uint64_t>(header[3]) << 32) | header[2];
263 reqSize = header[4] + 1;
264
265 // Validate opCode: 0=read, 1=write, 2=posted write
266 if (opCode == 0x3) {
267 log_->warning("Got invalid SRPv3 opcode 0x3 for id=%" PRIu32, id);
268 return;
269 }
270
271 // Match the public SrpV3 client contract: request size is encoded as N-1
272 // and only valid for aligned sizes in the inclusive range [4, 4096].
273 if (reqSize < 4 || reqSize > 4096 || (reqSize & 0x3) != 0) {
274 log_->warning("Got invalid SRPv3 request size=%" PRIu32 " for id=%" PRIu32, reqSize, id);
275 return;
276 }
277
278 doWrite = (opCode == 0x1); // Write
279 isPost = (opCode == 0x2); // Posted write
280
281 log_->debug("Got request id=%" PRIu32 ", opCode=%" PRIu32 ", addr=0x%" PRIx64 ", size=%" PRIu32,
282 id,
283 opCode,
284 address,
285 reqSize);
286
287 // Release the incoming frame lock before building/sending response
288 frLock.reset();
289
290 {
291 std::lock_guard<std::mutex> lock(memMtx_);
292
293 if (doWrite || isPost) {
294 // Verify frame contains write data
295 if (fSize < HeadLen + reqSize) {
296 log_->warning("Write frame too small: got %" PRIu32 ", need %" PRIu32, fSize, HeadLen + reqSize);
297 return;
298 }
299
300 // Re-lock and read write data from frame
301 frLock = frame->lock();
302 fIter = frame->begin() + HeadLen;
303 std::vector<uint8_t> wrData(reqSize);
304 ris::fromFrame(fIter, reqSize, wrData.data());
305 frLock.reset();
306
307 writeMemory(address, wrData.data(), reqSize);
308
309 log_->debug("Write complete id=%" PRIu32 ", addr=0x%" PRIx64 ", size=%" PRIu32, id, address, reqSize);
310 }
311
312 // Posted writes do not get a response
313 if (isPost) return;
314
315 // Build response frame
316 uint32_t respLen = HeadLen + reqSize + TailLen;
317 ris::FramePtr respFrame = reqFrame(respLen, true);
318 respFrame->setPayload(respLen);
319 ris::FrameIterator rIter = respFrame->begin();
320
321 // Write header (echo back the request header)
322 ris::toFrame(rIter, HeadLen, header);
323
324 // Write data payload (read from internal memory)
325 std::vector<uint8_t> rdData(reqSize);
326 readMemory(address, rdData.data(), reqSize);
327 ris::toFrame(rIter, reqSize, rdData.data());
328
329 // Write tail (status = 0 = success)
330 tail[0] = 0;
331 ris::toFrame(rIter, TailLen, tail);
332
333 log_->debug("Sending response id=%" PRIu32 ", size=%" PRIu32, id, respLen);
334
335 sendFrame(respFrame);
336 }
337}
RAII helper that releases the Python GIL for a scope.
Definition GilRelease.h:36
static std::shared_ptr< rogue::Logging > create(const std::string &name, bool quiet=false)
Creates a logger instance.
Definition Logging.cpp:95
RAII helper that acquires the Python GIL for a scope.
Definition ScopedGil.h:35
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.
Definition Frame.h:549
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.
Definition FrameLock.h:110
std::shared_ptr< rogue::protocols::srp::SrpV3Emulation > SrpV3EmulationPtr