1use crate::{ModbusService, ServiceError};
2use rustmod_core::encoding::Writer;
3use rustmod_core::pdu::{DecodedRequest, FunctionCode};
4use rustmod_core::EncodeError;
5use std::sync::RwLock;
6
7#[derive(Debug, Clone, PartialEq, Eq)]
8pub struct CoilBank {
9 values: Vec<bool>,
10}
11
12impl CoilBank {
13 pub fn new(size: usize) -> Self {
14 Self {
15 values: vec![false; size],
16 }
17 }
18
19 pub fn len(&self) -> usize {
20 self.values.len()
21 }
22
23 pub fn is_empty(&self) -> bool {
24 self.values.is_empty()
25 }
26
27 pub fn get(&self, index: usize) -> Option<bool> {
28 self.values.get(index).copied()
29 }
30
31 pub fn set(&mut self, index: usize, value: bool) -> Result<(), ServiceError> {
32 let slot = self
33 .values
34 .get_mut(index)
35 .ok_or(ServiceError::InvalidRequest("coil address out of range"))?;
36 *slot = value;
37 Ok(())
38 }
39}
40
41#[derive(Debug, Clone, PartialEq, Eq)]
42pub struct RegisterBank {
43 values: Vec<u16>,
44}
45
46impl RegisterBank {
47 pub fn new(size: usize) -> Self {
48 Self {
49 values: vec![0u16; size],
50 }
51 }
52
53 pub fn len(&self) -> usize {
54 self.values.len()
55 }
56
57 pub fn is_empty(&self) -> bool {
58 self.values.is_empty()
59 }
60
61 pub fn get(&self, index: usize) -> Option<u16> {
62 self.values.get(index).copied()
63 }
64
65 pub fn set(&mut self, index: usize, value: u16) -> Result<(), ServiceError> {
66 let slot = self
67 .values
68 .get_mut(index)
69 .ok_or(ServiceError::InvalidRequest("register address out of range"))?;
70 *slot = value;
71 Ok(())
72 }
73}
74
75#[derive(Debug, Clone, PartialEq, Eq)]
76pub struct InMemoryPointModel {
77 pub coils: CoilBank,
78 pub discrete_inputs: CoilBank,
79 pub holding_registers: RegisterBank,
80 pub input_registers: RegisterBank,
81}
82
83impl InMemoryPointModel {
84 pub fn new(
85 coil_count: usize,
86 discrete_input_count: usize,
87 holding_register_count: usize,
88 input_register_count: usize,
89 ) -> Self {
90 Self {
91 coils: CoilBank::new(coil_count),
92 discrete_inputs: CoilBank::new(discrete_input_count),
93 holding_registers: RegisterBank::new(holding_register_count),
94 input_registers: RegisterBank::new(input_register_count),
95 }
96 }
97}
98
99#[derive(Debug)]
100pub struct InMemoryModbusService {
101 model: RwLock<InMemoryPointModel>,
102}
103
104impl InMemoryModbusService {
105 pub fn new(
106 coil_count: usize,
107 discrete_input_count: usize,
108 holding_register_count: usize,
109 input_register_count: usize,
110 ) -> Self {
111 Self::with_model(InMemoryPointModel::new(
112 coil_count,
113 discrete_input_count,
114 holding_register_count,
115 input_register_count,
116 ))
117 }
118
119 pub fn with_model(model: InMemoryPointModel) -> Self {
120 Self {
121 model: RwLock::new(model),
122 }
123 }
124
125 pub fn snapshot(&self) -> InMemoryPointModel {
126 self.model
127 .read()
128 .expect("in-memory point model lock poisoned")
129 .clone()
130 }
131
132 pub fn set_coil(&self, address: u16, value: bool) -> Result<(), ServiceError> {
133 self.model
134 .write()
135 .expect("in-memory point model lock poisoned")
136 .coils
137 .set(usize::from(address), value)
138 }
139
140 pub fn set_discrete_input(&self, address: u16, value: bool) -> Result<(), ServiceError> {
141 self.model
142 .write()
143 .expect("in-memory point model lock poisoned")
144 .discrete_inputs
145 .set(usize::from(address), value)
146 }
147
148 pub fn set_holding_register(&self, address: u16, value: u16) -> Result<(), ServiceError> {
149 self.model
150 .write()
151 .expect("in-memory point model lock poisoned")
152 .holding_registers
153 .set(usize::from(address), value)
154 }
155
156 pub fn set_input_register(&self, address: u16, value: u16) -> Result<(), ServiceError> {
157 self.model
158 .write()
159 .expect("in-memory point model lock poisoned")
160 .input_registers
161 .set(usize::from(address), value)
162 }
163
164 pub fn coil(&self, address: u16) -> Option<bool> {
165 self.model
166 .read()
167 .expect("in-memory point model lock poisoned")
168 .coils
169 .get(usize::from(address))
170 }
171
172 pub fn holding_register(&self, address: u16) -> Option<u16> {
173 self.model
174 .read()
175 .expect("in-memory point model lock poisoned")
176 .holding_registers
177 .get(usize::from(address))
178 }
179}
180
181impl ModbusService for InMemoryModbusService {
182 fn handle(
183 &self,
184 unit_id: u8,
185 request: DecodedRequest<'_>,
186 response_pdu: &mut [u8],
187 ) -> Result<usize, ServiceError> {
188 let mut model = self
189 .model
190 .write()
191 .expect("in-memory point model lock poisoned");
192
193 let mut w = Writer::new(response_pdu);
194
195 match request {
196 DecodedRequest::ReadCoils(req) => {
197 let range = checked_range(req.start_address, req.quantity, model.coils.len())
198 .ok_or(ServiceError::Exception(
199 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
200 ))?;
201 let byte_count = range.len().div_ceil(8);
202 let byte_count_u8 = u8::try_from(byte_count)
203 .map_err(|_| ServiceError::Internal("coil response too large"))?;
204
205 w.write_u8(FunctionCode::ReadCoils.as_u8())
206 .map_err(map_encode)?;
207 w.write_u8(byte_count_u8).map_err(map_encode)?;
208
209 let mut packed = [0u8; 250];
210 for (i, address) in range.enumerate() {
211 if model.coils.get(address).unwrap_or(false) {
212 packed[i / 8] |= 1u8 << (i % 8);
213 }
214 }
215 w.write_all(&packed[..byte_count]).map_err(map_encode)?;
216 }
217 DecodedRequest::ReadDiscreteInputs(req) => {
218 let range = checked_range(
219 req.start_address,
220 req.quantity,
221 model.discrete_inputs.len(),
222 )
223 .ok_or(ServiceError::Exception(
224 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
225 ))?;
226 let byte_count = range.len().div_ceil(8);
227 let byte_count_u8 = u8::try_from(byte_count)
228 .map_err(|_| ServiceError::Internal("discrete input response too large"))?;
229
230 w.write_u8(FunctionCode::ReadDiscreteInputs.as_u8())
231 .map_err(map_encode)?;
232 w.write_u8(byte_count_u8).map_err(map_encode)?;
233
234 let mut packed = [0u8; 250];
235 for (i, address) in range.enumerate() {
236 if model.discrete_inputs.get(address).unwrap_or(false) {
237 packed[i / 8] |= 1u8 << (i % 8);
238 }
239 }
240 w.write_all(&packed[..byte_count]).map_err(map_encode)?;
241 }
242 DecodedRequest::ReadHoldingRegisters(req) => {
243 let range = checked_range(
244 req.start_address,
245 req.quantity,
246 model.holding_registers.len(),
247 )
248 .ok_or(ServiceError::Exception(
249 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
250 ))?;
251
252 let byte_count = range.len() * 2;
253 let byte_count_u8 = u8::try_from(byte_count)
254 .map_err(|_| ServiceError::Internal("register response too large"))?;
255
256 w.write_u8(FunctionCode::ReadHoldingRegisters.as_u8())
257 .map_err(map_encode)?;
258 w.write_u8(byte_count_u8).map_err(map_encode)?;
259 for address in range {
260 w.write_be_u16(model.holding_registers.get(address).unwrap_or(0))
261 .map_err(map_encode)?;
262 }
263 }
264 DecodedRequest::ReadInputRegisters(req) => {
265 let range = checked_range(req.start_address, req.quantity, model.input_registers.len())
266 .ok_or(ServiceError::Exception(
267 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
268 ))?;
269
270 let byte_count = range.len() * 2;
271 let byte_count_u8 = u8::try_from(byte_count)
272 .map_err(|_| ServiceError::Internal("input register response too large"))?;
273
274 w.write_u8(FunctionCode::ReadInputRegisters.as_u8())
275 .map_err(map_encode)?;
276 w.write_u8(byte_count_u8).map_err(map_encode)?;
277 for address in range {
278 w.write_be_u16(model.input_registers.get(address).unwrap_or(0))
279 .map_err(map_encode)?;
280 }
281 }
282 DecodedRequest::WriteSingleCoil(req) => {
283 model
284 .coils
285 .set(usize::from(req.address), req.value)
286 .map_err(|_| {
287 ServiceError::Exception(rustmod_core::pdu::ExceptionCode::IllegalDataAddress)
288 })?;
289 w.write_u8(FunctionCode::WriteSingleCoil.as_u8())
290 .map_err(map_encode)?;
291 w.write_be_u16(req.address).map_err(map_encode)?;
292 w.write_be_u16(if req.value { 0xFF00 } else { 0x0000 })
293 .map_err(map_encode)?;
294 }
295 DecodedRequest::WriteSingleRegister(req) => {
296 model
297 .holding_registers
298 .set(usize::from(req.address), req.value)
299 .map_err(|_| {
300 ServiceError::Exception(rustmod_core::pdu::ExceptionCode::IllegalDataAddress)
301 })?;
302 w.write_u8(FunctionCode::WriteSingleRegister.as_u8())
303 .map_err(map_encode)?;
304 w.write_be_u16(req.address).map_err(map_encode)?;
305 w.write_be_u16(req.value).map_err(map_encode)?;
306 }
307 DecodedRequest::WriteMultipleCoils(req) => {
308 let range = checked_range(req.start_address, req.quantity, model.coils.len()).ok_or(
309 ServiceError::Exception(rustmod_core::pdu::ExceptionCode::IllegalDataAddress),
310 )?;
311
312 for (i, address) in range.enumerate() {
313 let value = req.coil(i).ok_or(ServiceError::InvalidRequest(
314 "invalid packed coil write payload",
315 ))?;
316 model.coils.set(address, value)?;
317 }
318
319 w.write_u8(FunctionCode::WriteMultipleCoils.as_u8())
320 .map_err(map_encode)?;
321 w.write_be_u16(req.start_address).map_err(map_encode)?;
322 w.write_be_u16(req.quantity).map_err(map_encode)?;
323 }
324 DecodedRequest::WriteMultipleRegisters(req) => {
325 let quantity = req.quantity();
326 let quantity_u16 = u16::try_from(quantity)
327 .map_err(|_| ServiceError::InvalidRequest("register quantity too large"))?;
328 let range = checked_range(
329 req.start_address,
330 quantity_u16,
331 model.holding_registers.len(),
332 )
333 .ok_or(ServiceError::Exception(
334 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
335 ))?;
336
337 for (i, address) in range.enumerate() {
338 let value = req
339 .register(i)
340 .ok_or(ServiceError::InvalidRequest("invalid register payload"))?;
341 model.holding_registers.set(address, value)?;
342 }
343
344 w.write_u8(FunctionCode::WriteMultipleRegisters.as_u8())
345 .map_err(map_encode)?;
346 w.write_be_u16(req.start_address).map_err(map_encode)?;
347 w.write_be_u16(quantity_u16).map_err(map_encode)?;
348 }
349 DecodedRequest::MaskWriteRegister(req) => {
350 let address = usize::from(req.address);
351 let current = model.holding_registers.get(address).ok_or(ServiceError::Exception(
352 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
353 ))?;
354 let next = (current & req.and_mask) | (req.or_mask & !req.and_mask);
355 model.holding_registers.set(address, next).map_err(|_| {
356 ServiceError::Exception(rustmod_core::pdu::ExceptionCode::IllegalDataAddress)
357 })?;
358
359 w.write_u8(FunctionCode::MaskWriteRegister.as_u8())
360 .map_err(map_encode)?;
361 w.write_be_u16(req.address).map_err(map_encode)?;
362 w.write_be_u16(req.and_mask).map_err(map_encode)?;
363 w.write_be_u16(req.or_mask).map_err(map_encode)?;
364 }
365 DecodedRequest::ReadWriteMultipleRegisters(req) => {
366 let write_quantity = req.write_quantity();
367 let write_quantity_u16 = u16::try_from(write_quantity)
368 .map_err(|_| ServiceError::InvalidRequest("write quantity too large"))?;
369
370 let write_range = checked_range(
371 req.write_start_address,
372 write_quantity_u16,
373 model.holding_registers.len(),
374 )
375 .ok_or(ServiceError::Exception(
376 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
377 ))?;
378
379 for (i, address) in write_range.enumerate() {
380 let value = req
381 .register(i)
382 .ok_or(ServiceError::InvalidRequest("invalid register payload"))?;
383 model.holding_registers.set(address, value)?;
384 }
385
386 let read_range = checked_range(
387 req.read_start_address,
388 req.read_quantity,
389 model.holding_registers.len(),
390 )
391 .ok_or(ServiceError::Exception(
392 rustmod_core::pdu::ExceptionCode::IllegalDataAddress,
393 ))?;
394
395 let byte_count = read_range.len() * 2;
396 let byte_count_u8 = u8::try_from(byte_count)
397 .map_err(|_| ServiceError::Internal("register response too large"))?;
398 w.write_u8(FunctionCode::ReadWriteMultipleRegisters.as_u8())
399 .map_err(map_encode)?;
400 w.write_u8(byte_count_u8).map_err(map_encode)?;
401 for address in read_range {
402 w.write_be_u16(model.holding_registers.get(address).unwrap_or(0))
403 .map_err(map_encode)?;
404 }
405 }
406 DecodedRequest::Custom(req) => {
407 if req.function_code == 0x11 {
408 w.write_u8(0x11).map_err(map_encode)?;
410 w.write_u8(0x02).map_err(map_encode)?;
411 w.write_u8(unit_id).map_err(map_encode)?;
412 w.write_u8(0xFF).map_err(map_encode)?;
413 } else if req.function_code == 0x2B {
414 if req.data.len() != 3 || req.data[0] != 0x0E {
416 return Err(ServiceError::Exception(
417 rustmod_core::pdu::ExceptionCode::IllegalDataValue,
418 ));
419 }
420 let read_code = req.data[1];
421 w.write_u8(0x2B).map_err(map_encode)?;
422 w.write_u8(0x0E).map_err(map_encode)?;
423 w.write_u8(read_code).map_err(map_encode)?;
424 w.write_u8(0x01).map_err(map_encode)?; w.write_u8(0x00).map_err(map_encode)?; w.write_u8(0x00).map_err(map_encode)?; let objects = [
429 (0x00u8, b"rust-mod-sim".as_slice()),
430 (0x01u8, b"in-memory".as_slice()),
431 (0x02u8, b"0.1".as_slice()),
432 ];
433 w.write_u8(objects.len() as u8).map_err(map_encode)?;
434 for (id, value) in objects {
435 let value_len = u8::try_from(value.len()).map_err(|_| {
436 ServiceError::Internal("device identification object too large")
437 })?;
438 w.write_u8(id).map_err(map_encode)?;
439 w.write_u8(value_len).map_err(map_encode)?;
440 w.write_all(value).map_err(map_encode)?;
441 }
442 } else {
443 return Err(ServiceError::Exception(
444 rustmod_core::pdu::ExceptionCode::IllegalFunction,
445 ));
446 }
447 }
448 }
449
450 Ok(w.position())
451 }
452}
453
454fn checked_range(start: u16, quantity: u16, len: usize) -> Option<std::ops::Range<usize>> {
455 let start = usize::from(start);
456 let quantity = usize::from(quantity);
457 let end = start.checked_add(quantity)?;
458 if quantity == 0 || end > len {
459 return None;
460 }
461 Some(start..end)
462}
463
464fn map_encode(err: EncodeError) -> ServiceError {
465 let msg = match err {
466 EncodeError::BufferTooSmall => "response buffer too small",
467 EncodeError::ValueOutOfRange => "response value out of range",
468 EncodeError::InvalidLength => "response length invalid",
469 EncodeError::Unsupported => "response operation unsupported",
470 EncodeError::Message(_) => "response encode message",
471 };
472 ServiceError::Internal(msg)
473}
474
475#[cfg(test)]
476mod tests {
477 use super::{InMemoryModbusService, InMemoryPointModel};
478 use crate::ModbusService;
479 use rustmod_core::encoding::Reader;
480 use rustmod_core::pdu::{DecodedRequest, Response};
481
482 #[test]
483 fn in_memory_service_reads_and_writes() {
484 let service = InMemoryModbusService::with_model(InMemoryPointModel::new(16, 16, 16, 16));
485 service.set_holding_register(0, 42).unwrap();
486
487 let mut pdu = [0u8; 260];
488 let request = {
489 let mut r = Reader::new(&[0x03, 0x00, 0x00, 0x00, 0x01]);
490 DecodedRequest::decode(&mut r).unwrap()
491 };
492 let len = service.handle(1, request, &mut pdu).unwrap();
493
494 let mut rr = Reader::new(&pdu[..len]);
495 match Response::decode(&mut rr).unwrap() {
496 Response::ReadHoldingRegisters(resp) => assert_eq!(resp.register(0), Some(42)),
497 other => panic!("unexpected response: {other:?}"),
498 }
499
500 let write_req = {
501 let mut r = Reader::new(&[0x06, 0x00, 0x01, 0x12, 0x34]);
502 DecodedRequest::decode(&mut r).unwrap()
503 };
504 let _ = service.handle(1, write_req, &mut pdu).unwrap();
505 assert_eq!(service.holding_register(1), Some(0x1234));
506
507 let mask_req = {
508 let mut r = Reader::new(&[0x16, 0x00, 0x01, 0xFF, 0x00, 0x00, 0x12]);
509 DecodedRequest::decode(&mut r).unwrap()
510 };
511 let _ = service.handle(1, mask_req, &mut pdu).unwrap();
512 assert_eq!(service.holding_register(1), Some(0x1212));
513
514 let rw_req = {
515 let mut r = Reader::new(&[
516 0x17, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0x04, 0xBE, 0xEF, 0xCA,
517 0xFE,
518 ]);
519 DecodedRequest::decode(&mut r).unwrap()
520 };
521 let len = service.handle(1, rw_req, &mut pdu).unwrap();
522 let mut rr = Reader::new(&pdu[..len]);
523 match Response::decode(&mut rr).unwrap() {
524 Response::ReadWriteMultipleRegisters(resp) => {
525 assert_eq!(resp.register(0), Some(0xBEEF));
526 assert_eq!(resp.register(1), Some(0xCAFE));
527 }
528 other => panic!("unexpected response: {other:?}"),
529 }
530 }
531
532 #[test]
533 fn in_memory_service_supports_report_server_id() {
534 let service = InMemoryModbusService::with_model(InMemoryPointModel::new(4, 4, 4, 4));
535 let mut pdu = [0u8; 260];
536
537 let request = {
538 let mut r = Reader::new(&[0x11]);
539 DecodedRequest::decode(&mut r).unwrap()
540 };
541 let len = service.handle(0x2A, request, &mut pdu).unwrap();
542
543 let mut rr = Reader::new(&pdu[..len]);
544 match Response::decode(&mut rr).unwrap() {
545 Response::Custom(resp) => {
546 assert_eq!(resp.function_code, 0x11);
547 assert_eq!(resp.data, &[0x02, 0x2A, 0xFF]);
548 }
549 other => panic!("unexpected response: {other:?}"),
550 }
551 }
552
553 #[test]
554 fn in_memory_service_supports_read_device_identification() {
555 let service = InMemoryModbusService::with_model(InMemoryPointModel::new(4, 4, 4, 4));
556 let mut pdu = [0u8; 260];
557
558 let request = {
559 let mut r = Reader::new(&[0x2B, 0x0E, 0x01, 0x00]);
560 DecodedRequest::decode(&mut r).unwrap()
561 };
562 let len = service.handle(0x2A, request, &mut pdu).unwrap();
563
564 let mut rr = Reader::new(&pdu[..len]);
565 match Response::decode(&mut rr).unwrap() {
566 Response::Custom(resp) => {
567 assert_eq!(resp.function_code, 0x2B);
568 assert_eq!(resp.data[0], 0x0E);
569 assert_eq!(resp.data[1], 0x01);
570 assert_eq!(resp.data[5], 0x03);
571 }
572 other => panic!("unexpected response: {other:?}"),
573 }
574 }
575}