snap7_server/
dispatch.rs

1use bytes::{BufMut, Bytes, BytesMut};
2use snap7_client::proto::s7::{
3    header::{PduType, S7Header},
4    read_var::{DataItem, ReadVarRequest, ReadVarResponse, FUNC_READ_VAR},
5    write_var::{WriteVarRequest, FUNC_WRITE_VAR},
6};
7use tokio::io::{AsyncRead, AsyncWrite};
8
9use crate::{
10    error::Result,
11    handshake::{recv_cotp_data, send_cotp_data},
12    store::DataStore,
13};
14
15/// Run the S7 request dispatch loop over an established transport.
16///
17/// Reads COTP Data PDUs, decodes S7 requests, executes them against
18/// `store`, and sends S7 AckData responses. Runs until the transport
19/// closes (EOF) or a fatal I/O error occurs.
20pub async fn dispatch_loop<T>(mut transport: T, _pdu_size: u16, store: DataStore) -> Result<()>
21where
22    T: AsyncRead + AsyncWrite + Unpin,
23{
24    loop {
25        let mut payload = match recv_cotp_data(&mut transport).await {
26            Ok(p) => p,
27            Err(_) => return Ok(()),
28        };
29
30        let header = match S7Header::decode(&mut payload) {
31            Ok(h) => h,
32            Err(_) => {
33                send_error_response(&mut transport, 0, 0x81, 0x04).await?;
34                continue;
35            }
36        };
37
38        if payload.is_empty() {
39            send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?;
40            continue;
41        }
42
43        // Dispatch based on PDU type
44        match header.pdu_type {
45            PduType::Job => {
46                let func = payload[0];
47                match func {
48                    FUNC_READ_VAR => {
49                        match handle_read_var(&mut payload, &store) {
50                            Ok((item_count, response)) => {
51                                send_ack_data(&mut transport, header.pdu_ref, FUNC_READ_VAR, item_count, response).await?;
52                            }
53                            Err(()) => send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?,
54                        }
55                    }
56                    FUNC_WRITE_VAR => {
57                        match handle_write_var(&mut payload, &store) {
58                            Ok((item_count, response)) => {
59                                send_ack_data(&mut transport, header.pdu_ref, FUNC_WRITE_VAR, item_count, response).await?;
60                            }
61                            Err(()) => send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?,
62                        }
63                    }
64                    // PLC control commands
65                    0x28 | 0x29 | 0x2A | 0x31 => {
66                        let hdr = S7Header {
67                            pdu_type: PduType::AckData,
68                            reserved: 0,
69                            pdu_ref: header.pdu_ref,
70                            param_len: 2,
71                            data_len: if func == 0x31 { 1 } else { 0 },
72                            error_class: Some(0),
73                            error_code: Some(0),
74                        };
75                        let mut buf = BytesMut::new();
76                        hdr.encode(&mut buf);
77                        buf.extend_from_slice(&[func, 0x00]);
78                        if func == 0x31 {
79                            buf.put_u8(0x08); // status = RUN
80                        }
81                        send_cotp_data(&mut transport, buf.freeze()).await?;
82                    }
83                    // Password commands
84                    0x11 | 0x12 => {
85                        send_simple_ack(&mut transport, header.pdu_ref).await?;
86                    }
87                    _ => {
88                        send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?;
89                    }
90                }
91            }
92            PduType::UserData => {
93                // UserData: SZL, clock, block info, etc.
94                if payload.len() >= 5 && (payload[4] == 0x11 || payload[4] == 0xF5) {
95                    handle_user_data(&mut transport, header.pdu_ref, &payload).await?;
96                } else {
97                    send_simple_ack(&mut transport, header.pdu_ref).await?;
98                }
99            }
100            _ => {
101                send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?;
102            }
103        }
104    }
105}
106
107// -- Read / Write handlers --------------------------------------------------
108
109fn handle_read_var(payload: &mut Bytes, store: &DataStore) -> std::result::Result<(u8, Bytes), ()> {
110    let req = ReadVarRequest::decode(payload).map_err(|_| ())?;
111
112    let items: Vec<DataItem> = req
113        .items
114        .iter()
115        .map(|item| {
116            let area_byte = item.area as u8;
117            let data = store.read_area(area_byte, item.db_number, item.start, item.length as u32);
118            DataItem { return_code: 0xFF, data: Bytes::from(data) }
119        })
120        .collect();
121
122    let item_count = items.len() as u8;
123    let resp = ReadVarResponse { items };
124    let mut buf = BytesMut::new();
125    resp.encode(&mut buf);
126    Ok((item_count, buf.freeze()))
127}
128
129fn handle_write_var(payload: &mut Bytes, store: &DataStore) -> std::result::Result<(u8, Bytes), ()> {
130    let req = WriteVarRequest::decode(payload).map_err(|_| ())?;
131
132    for item in &req.items {
133        let area_byte = item.address.area as u8;
134        store.write_area(area_byte, item.address.db_number, item.address.start, &item.data);
135    }
136
137    let item_count = req.items.len() as u8;
138    let mut buf = BytesMut::new();
139    for _ in 0..item_count {
140        buf.put_u8(0xFF);
141    }
142    Ok((item_count, buf.freeze()))
143}
144
145// -- UserData : SZL responses -----------------------------------------------
146
147async fn handle_user_data<T: AsyncWrite + Unpin>(
148    transport: &mut T,
149    pdu_ref: u16,
150    payload: &[u8],
151) -> Result<()> {
152    let szl_id = if payload.len() >= 10 {
153        u16::from_be_bytes([payload[8], payload[9]])
154    } else {
155        0
156    };
157
158    let response_data = build_szl_response(szl_id);
159    let param_len = 12u16;
160    let data_len = response_data.len() as u16;
161
162    let header = S7Header {
163        pdu_type: PduType::AckData,
164        reserved: 0,
165        pdu_ref,
166        param_len,
167        data_len,
168        error_class: Some(0),
169        error_code: Some(0),
170    };
171    let mut buf = BytesMut::new();
172    header.encode(&mut buf);
173    if payload.len() >= 12 {
174        buf.extend_from_slice(&payload[..12]);
175    } else {
176        buf.resize(buf.len() + param_len as usize, 0);
177    }
178    buf.put_u8(0xFF);
179    buf.put_u8(0x04);
180    buf.put_u16(data_len);
181    buf.extend_from_slice(&response_data);
182    send_cotp_data(transport, buf.freeze()).await
183}
184
185fn build_szl_response(szl_id: u16) -> Vec<u8> {
186    match szl_id {
187        0x0011 => {
188            let d = vec![b' '; 20];
189            let blk = (4 + d.len()) as u16;
190            let mut v = Vec::with_capacity(6 + d.len());
191            v.extend_from_slice(&blk.to_be_bytes());
192            v.extend_from_slice(&szl_id.to_be_bytes());
193            v.extend_from_slice(&[0x00, 0x00]);
194            v.extend_from_slice(&d);
195            v
196        }
197        0x0032 => {
198            let pl: Vec<u8> = {
199                let mut v = Vec::with_capacity(16);
200                v.extend_from_slice(&[0x00; 8]); // scheme_szl + scheme_module + scheme_bus + level
201                v.extend_from_slice(b"        "); // pass_word
202                v
203            };
204            let blk = (4 + pl.len()) as u16;
205            let mut v = Vec::with_capacity(6 + pl.len());
206            v.extend_from_slice(&blk.to_be_bytes());
207            v.extend_from_slice(&szl_id.to_be_bytes());
208            v.extend_from_slice(&[0x00, 0x04]);
209            v.extend_from_slice(&pl);
210            v
211        }
212        0x001C => {
213            let mut pl = vec![b' '; 122];
214            let name = b"Simulated PLC";
215            pl[..name.len().min(24)].copy_from_slice(&name[..name.len().min(24)]);
216            let blk = (4 + pl.len()) as u16;
217            let mut v = Vec::with_capacity(6 + pl.len());
218            v.extend_from_slice(&blk.to_be_bytes());
219            v.extend_from_slice(&szl_id.to_be_bytes());
220            v.extend_from_slice(&[0x00, 0x00]);
221            v.extend_from_slice(&pl);
222            v
223        }
224        _ => {
225            let pl: Vec<u8> = Vec::new();
226            let blk = (4 + pl.len()) as u16;
227            let mut v = Vec::with_capacity(6 + pl.len());
228            v.extend_from_slice(&blk.to_be_bytes());
229            v.extend_from_slice(&szl_id.to_be_bytes());
230            v.extend_from_slice(&[0x00, 0x00]);
231            v.extend_from_slice(&pl);
232            v
233        }
234    }
235}
236
237// -- Helper functions -------------------------------------------------------
238
239async fn send_simple_ack<T: AsyncWrite + Unpin>(transport: &mut T, pdu_ref: u16) -> Result<()> {
240    let header = S7Header {
241        pdu_type: PduType::AckData,
242        reserved: 0,
243        pdu_ref,
244        param_len: 0,
245        data_len: 0,
246        error_class: Some(0),
247        error_code: Some(0),
248    };
249    let mut buf = BytesMut::new();
250    header.encode(&mut buf);
251    send_cotp_data(transport, buf.freeze()).await
252}
253
254async fn send_ack_data<T: AsyncWrite + Unpin>(
255    transport: &mut T,
256    pdu_ref: u16,
257    func: u8,
258    item_count: u8,
259    data: Bytes,
260) -> Result<()> {
261    let param: Bytes = Bytes::copy_from_slice(&[func, item_count]);
262    let header = S7Header {
263        pdu_type: PduType::AckData,
264        reserved: 0,
265        pdu_ref,
266        param_len: 2,
267        data_len: data.len() as u16,
268        error_class: Some(0),
269        error_code: Some(0),
270    };
271    let mut buf = BytesMut::new();
272    header.encode(&mut buf);
273    buf.extend_from_slice(&param);
274    buf.extend_from_slice(&data);
275    send_cotp_data(transport, buf.freeze()).await
276}
277
278async fn send_error_response<T: AsyncWrite + Unpin>(
279    transport: &mut T,
280    pdu_ref: u16,
281    error_class: u8,
282    error_code: u8,
283) -> Result<()> {
284    let header = S7Header {
285        pdu_type: PduType::AckData,
286        reserved: 0,
287        pdu_ref,
288        param_len: 0,
289        data_len: 0,
290        error_class: Some(error_class),
291        error_code: Some(error_code),
292    };
293    let mut buf = BytesMut::new();
294    header.encode(&mut buf);
295    send_cotp_data(transport, buf.freeze()).await
296}
snap7_server/dispatch.rs

snap7_server/
dispatch.rs
