modbus slave

Cargo.toml

@@ -26,7 +26,7 @@ object-id = "0.1.3"
 oneshot = { version = "0.1.6", default-features = false, features = ["std"] }
 parking_lot = "0.12.1"
 pin-project = "1.1.5"
-rmodbus = { version = "0.9.3", optional = true }
+rmodbus = { version = "0.9.4", optional = true }
 roboplc-derive = { version = "0.1.5" }
 serde = { version = "1.0.197", features = ["derive", "rc"] }
 serial = "0.4.0"
@@ -57,6 +57,11 @@ name = "plc-modbus"
 path = "examples/plc-modbus.rs"
 required-features = ["modbus"]
 
+[[example]]
+name = "modbus-slave"
+path = "examples/modbus-slave.rs"
+required-features = ["modbus"]
+
 [[example]]
 name = "raw-udp"
 path = "examples/raw-udp.rs"

examples/modbus-slave.rs

+use roboplc::comm::Protocol;
+use roboplc::io::modbus::prelude::*;
+use roboplc::{prelude::*, time::interval};
+use tracing::info;
+
+/// Modbus slave storage context size for each register type
+const HOLDINGS: usize = 2;
+const DISCRETES: usize = 0;
+const INPUTS: usize = 13;
+const COILS: usize = 2;
+
+/// A server mapping alias
+type ServerMapping = ModbusServerMapping<COILS, DISCRETES, INPUTS, HOLDINGS>;
+
+/// First data type. For Modbus slave context data types must be split into booleans and others
+#[derive(Clone, Debug, Default)]
+#[binrw]
+struct Data {
+    counter: u16, // available in the input register 0 (i0)
+}
+
+/// Booleans data type
+#[derive(Clone, Debug, Default)]
+#[binrw]
+struct Relays {
+    relay1: u8, // available in the coil 0 (c0)
+    relay2: u8, // available in the coil 1 (c1)
+}
+
+#[derive(Default)]
+#[binrw]
+struct Input {
+    value: u32,
+}
+
+// This example does not use controller's data hub
+type Message = ();
+
+// Controller's shared variables
+#[derive(Default)]
+struct Variables {
+    data: Data,
+    relays: Relays,
+    input: Input,
+}
+
+#[allow(clippy::struct_field_names)]
+#[derive(WorkerOpts)]
+struct Worker1 {
+    // Modbus server context and controller variables/data hub are not synchronized automatically,
+    // workers must write to the Modbus server context and read from the controller variables/data
+    // hub
+    env_mapping: ServerMapping,
+    relay_mapping: ServerMapping,
+    input_mapping: ServerMapping,
+}
+
+#[allow(clippy::cast_lossless)]
+impl Worker<Message, Variables> for Worker1 {
+    fn run(&mut self, context: &Context<Message, Variables>) -> WResult {
+        for _ in interval(Duration::from_secs(2)) {
+            let mut vars = context.variables().write();
+            vars.data.counter += 1;
+            vars.relays.relay1 = u8::from(vars.data.counter % 2 == 0);
+            vars.relays.relay2 = u8::from(vars.data.counter % 2 != 0);
+            self.env_mapping.write(&vars.data)?;
+            self.relay_mapping.write(&vars.relays)?;
+            vars.input = self.input_mapping.read()?;
+            info!(%vars.data.counter, "i0(1)");
+            info!(%vars.relays.relay1, "c0(1)");
+            info!(%vars.relays.relay2, "c1(1)");
+            info!(%vars.input.value, "h0(2)");
+            if !context.is_online() {
+                break;
+            }
+        }
+        Ok(())
+    }
+}
+
+/// Modbus server requires a worker to run with
+#[derive(WorkerOpts)]
+#[worker_opts(blocking = true)]
+struct ModbusSrv {
+    server: ModbusServer<COILS, DISCRETES, INPUTS, HOLDINGS>,
+}
+
+impl Worker<Message, Variables> for ModbusSrv {
+    fn run(&mut self, _context: &Context<Message, Variables>) -> WResult {
+        self.server.serve()?;
+        Ok(())
+    }
+}
+
+fn main() -> std::result::Result<(), Box<dyn std::error::Error>> {
+    // for TCP
+    let addr = "0.0.0.0:5552";
+    // for RTU
+    //let addr = "/dev/ttyS0:9600:8:N:1";
+    // Modbus Unit ID
+    let unit = 1;
+    let timeout = Duration::from_secs(5);
+    env_logger::builder()
+        .filter_level(log::LevelFilter::Info)
+        .init();
+    let server = ModbusServer::bind(Protocol::Tcp, unit, addr, timeout, 1)?;
+    // Modbus server register mapping, can be specified with '@' or without
+    let env_mapping = server.mapping("i@0".parse()?, 13);
+    let relay_mapping = server.mapping("c@0".parse()?, 2);
+    let input_mapping = server.mapping("h@0".parse()?, 2);
+    let mut controller = Controller::<Message, Variables>::new();
+    controller.register_signals(Duration::from_secs(5))?;
+    controller.spawn_worker(Worker1 {
+        env_mapping,
+        relay_mapping,
+        input_mapping,
+    })?;
+    controller.spawn_worker(ModbusSrv { server })?;
+    info!(addr, unit, "started");
+    controller.block();
+    info!("exiting");
+    Ok(())
+}

src/io/modbus/mod.rs

@@ -7,13 +7,19 @@ use binrw::{BinRead, BinWrite};
 pub use regs::{Kind as ModbusRegisterKind, Register as ModbusRegister};
 use rmodbus::guess_response_frame_len;
 use rmodbus::{client::ModbusRequest as RModbusRequest, ModbusProto};
+#[allow(clippy::module_name_repetitions)]
+pub use server::{ModbusServer, ModbusServerMapping};
 
 use super::IoMapping;
 
 mod regs;
+mod server;
 
 pub mod prelude {
-    pub use super::{ModbusMapping, ModbusMappingOptions, ModbusRegister, ModbusRegisterKind};
+    pub use super::{
+        ModbusMapping, ModbusMappingOptions, ModbusRegister, ModbusRegisterKind, ModbusServer,
+        ModbusServerMapping,
+    };
 }
 
 pub trait SwapModbusEndianess {

src/io/modbus/server.rs

+use crate::io::{modbus::ModbusRegister, IoMapping};
+use crate::{
+    comm::{self, Protocol},
+    semaphore::Semaphore,
+    Error, Result,
+};
+use binrw::{BinRead, BinWrite};
+use parking_lot::Mutex;
+use rmodbus::{
+    server::{context::ModbusContext, storage::ModbusStorage, ModbusFrame},
+    ModbusFrameBuf, ModbusProto,
+};
+use serial::SystemPort;
+use std::time::Duration;
+use std::{
+    io::{Cursor, Read, Write},
+    net::{TcpListener, TcpStream},
+    sync::Arc,
+    thread,
+};
+use tracing::error;
+
+use super::ModbusRegisterKind;
+
+enum Server {
+    Tcp(TcpListener),
+    Serial(SystemPort),
+}
+
+fn handle_client<
+    T: Read + Write,
+    const C: usize,
+    const D: usize,
+    const I: usize,
+    const H: usize,
+>(
+    mut client: T,
+    unit: u8,
+    storage: Arc<Mutex<ModbusStorage<C, D, I, H>>>,
+    modbus_proto: ModbusProto,
+) -> Result<()> {
+    let mut buf: ModbusFrameBuf = [0; 256];
+    let mut response = Vec::with_capacity(256);
+    loop {
+        if client.read(&mut buf).unwrap_or(0) == 0 {
+            break;
+        }
+        response.truncate(0);
+        let mut frame = ModbusFrame::new(unit, &buf, modbus_proto, &mut response);
+        frame.parse().map_err(Error::io)?;
+        if frame.processing_required {
+            if frame.readonly {
+                frame.process_read(&*storage.lock()).map_err(Error::io)?;
+            } else {
+                frame
+                    .process_write(&mut *storage.lock())
+                    .map_err(Error::io)?;
+            }
+        }
+        if frame.response_required {
+            frame.finalize_response().map_err(Error::io)?;
+            client.write_all(&response).map_err(Error::io)?;
+        }
+    }
+    Ok(())
+}
+
+#[allow(clippy::module_name_repetitions)]
+pub struct ModbusServer<const C: usize, const D: usize, const I: usize, const H: usize> {
+    storage: Arc<Mutex<ModbusStorage<C, D, I, H>>>,
+    unit: u8,
+    server: Server,
+    timeout: Duration,
+    semaphore: Semaphore,
+}
+impl<const C: usize, const D: usize, const I: usize, const H: usize> ModbusServer<C, D, I, H> {
+    pub fn bind(
+        protocol: Protocol,
+        unit: u8,
+        path: &str,
+        timeout: Duration,
+        max_workers: usize,
+    ) -> Result<Self> {
+        let server = match protocol {
+            Protocol::Tcp => Server::Tcp(TcpListener::bind(path)?),
+            Protocol::Rtu => Server::Serial(comm::serial::open(&path.parse()?, timeout)?),
+        };
+        Ok(Self {
+            storage: <_>::default(),
+            unit,
+            server,
+            timeout,
+            semaphore: Semaphore::new(max_workers),
+        })
+    }
+    pub fn mapping(&self, register: ModbusRegister, count: u16) -> ModbusServerMapping<C, D, I, H> {
+        let buf_capacity = match register.kind {
+            ModbusRegisterKind::Coil | ModbusRegisterKind::Discrete => usize::from(count),
+            ModbusRegisterKind::Input | ModbusRegisterKind::Holding => usize::from(count) * 2,
+        };
+        ModbusServerMapping {
+            storage: self.storage.clone(),
+            register,
+            count,
+            data_buf: Vec::with_capacity(buf_capacity),
+        }
+    }
+    pub fn storage(&self) -> Arc<Mutex<ModbusStorage<C, D, I, H>>> {
+        self.storage.clone()
+    }
+    pub fn serve(&mut self) -> Result<()> {
+        let timeout = self.timeout;
+        let unit = self.unit;
+        match self.server {
+            Server::Tcp(ref server) => loop {
+                let permission = self.semaphore.acquire();
+                let (stream, addr) = server.accept()?;
+                if let Err(e) = prepare_tcp_stream(&stream, timeout) {
+                    error!(%addr, %e, "error preparing tcp stream");
+                    continue;
+                }
+                let storage = self.storage.clone();
+                thread::spawn(move || {
+                    let _permission = permission;
+                    if let Err(error) = handle_client(stream, unit, storage, ModbusProto::TcpUdp) {
+                        error!(%addr, %error, "error handling Modbus client");
+                    }
+                });
+            },
+            Server::Serial(ref mut serial) => loop {
+                if let Err(e) =
+                    handle_client(&mut *serial, unit, self.storage.clone(), ModbusProto::Rtu)
+                {
+                    error!(%e, "error handling Modbus client");
+                }
+            },
+        }
+    }
+}
+
+fn prepare_tcp_stream(stream: &TcpStream, timeout: Duration) -> Result<()> {
+    stream.set_read_timeout(Some(timeout))?;
+    stream.set_write_timeout(Some(timeout))?;
+    stream.set_nodelay(true)?;
+    Ok(())
+}
+
+pub struct ModbusServerMapping<const C: usize, const D: usize, const I: usize, const H: usize> {
+    storage: Arc<Mutex<ModbusStorage<C, D, I, H>>>,
+    register: ModbusRegister,
+    count: u16,
+    data_buf: Vec<u8>,
+}
+
+impl<const C: usize, const D: usize, const I: usize, const H: usize> IoMapping
+    for ModbusServerMapping<C, D, I, H>
+{
+    type Options = ();
+
+    fn read<T>(&mut self) -> Result<T>
+    where
+        T: for<'a> BinRead<Args<'a> = ()>,
+    {
+        self.data_buf.truncate(0);
+        match self.register.kind {
+            ModbusRegisterKind::Coil => self
+                .storage
+                .lock()
+                .get_coils_as_u8_bytes(self.register.offset, self.count, &mut self.data_buf)
+                .map_err(Error::io)?,
+            ModbusRegisterKind::Discrete => self
+                .storage
+                .lock()
+                .get_discretes_as_u8_bytes(self.register.offset, self.count, &mut self.data_buf)
+                .map_err(Error::io)?,
+            ModbusRegisterKind::Input => self
+                .storage
+                .lock()
+                .get_inputs_as_u8(self.register.offset, self.count, &mut self.data_buf)
+                .map_err(Error::io)?,
+            ModbusRegisterKind::Holding => self
+                .storage
+                .lock()
+                .get_holdings_as_u8(self.register.offset, self.count, &mut self.data_buf)
+                .map_err(Error::io)?,
+        };
+        let mut reader = Cursor::new(&self.data_buf);
+        T::read_be(&mut reader).map_err(Into::into)
+    }
+
+    fn write<T>(&mut self, value: T) -> Result<()>
+    where
+        T: for<'a> BinWrite<Args<'a> = ()>,
+    {
+        let mut data_buf = Cursor::new(&mut self.data_buf);
+        value.write_be(&mut data_buf)?;
+        macro_rules! check_data_len_bool {
+            () => {
+                if self.data_buf.len() > self.count.into() {
+                    return Err(Error::io("invalid data length"));
+                }
+            };
+        }
+        macro_rules! check_data_len_u16 {
+            () => {
+                if self.data_buf.len() > usize::from(self.count) * 2 {
+                    return Err(Error::io("invalid data length"));
+                }
+            };
+        }
+        match self.register.kind {
+            ModbusRegisterKind::Coil => {
+                check_data_len_bool!();
+                self.storage
+                    .lock()
+                    .set_coils_from_u8_bytes(self.register.offset, &self.data_buf)
+                    .map_err(Error::io)?;
+            }
+            ModbusRegisterKind::Discrete => {
+                check_data_len_bool!();
+                self.storage
+                    .lock()
+                    .set_discretes_from_u8_bytes(self.register.offset, &self.data_buf)
+                    .map_err(Error::io)?;
+            }
+            ModbusRegisterKind::Input => {
+                check_data_len_u16!();
+                self.storage
+                    .lock()
+                    .set_inputs_from_u8(self.register.offset, &self.data_buf)
+                    .map_err(Error::io)?;
+            }
+            ModbusRegisterKind::Holding => {
+                check_data_len_u16!();
+                self.storage
+                    .lock()
+                    .set_holdings_from_u8(self.register.offset, &self.data_buf)
+                    .map_err(Error::io)?;
+            }
+        };
+        Ok(())
+    }
+}