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                // payload[4] = method byte: 0x11 = UserData request
95                if payload.len() >= 5 && payload[4] == 0x11 {
96                    handle_user_data(&mut transport, header.pdu_ref, &payload, &store).await?;
97                } else {
98                    send_simple_ack(&mut transport, header.pdu_ref).await?;
99                }
100            }
101            _ => {
102                send_error_response(&mut transport, header.pdu_ref, 0x81, 0x04).await?;
103            }
104        }
105    }
106}
107
108// -- Read / Write handlers --------------------------------------------------
109
110fn handle_read_var(payload: &mut Bytes, store: &DataStore) -> std::result::Result<(u8, Bytes), ()> {
111    let req = ReadVarRequest::decode(payload).map_err(|_| ())?;
112
113    let items: Vec<DataItem> = req
114        .items
115        .iter()
116        .map(|item| {
117            let area_byte = item.area as u8;
118            let data = store.read_area(area_byte, item.db_number, item.start, item.length as u32);
119            DataItem { return_code: 0xFF, data: Bytes::from(data) }
120        })
121        .collect();
122
123    let item_count = items.len() as u8;
124    let resp = ReadVarResponse { items };
125    let mut buf = BytesMut::new();
126    resp.encode(&mut buf);
127    Ok((item_count, buf.freeze()))
128}
129
130fn handle_write_var(payload: &mut Bytes, store: &DataStore) -> std::result::Result<(u8, Bytes), ()> {
131    let req = WriteVarRequest::decode(payload).map_err(|_| ())?;
132
133    for item in &req.items {
134        let area_byte = item.address.area as u8;
135        store.write_area(area_byte, item.address.db_number, item.address.start, &item.data);
136    }
137
138    let item_count = req.items.len() as u8;
139    let mut buf = BytesMut::new();
140    for _ in 0..item_count {
141        buf.put_u8(0xFF);
142    }
143    Ok((item_count, buf.freeze()))
144}
145
146// -- UserData : SZL responses -----------------------------------------------
147
148async fn handle_user_data<T: AsyncWrite + Unpin>(
149    transport: &mut T,
150    pdu_ref: u16,
151    payload: &[u8],
152    store: &DataStore,
153) -> Result<()> {
154    // payload[5] = Tg byte: low nibble = function group
155    // 0x44 = grSZL, 0x47 = grClock
156    let tg = if payload.len() >= 6 { payload[5] } else { 0 };
157    let group = tg & 0x0F;
158
159    match group {
160        0x07 => handle_clock_user_data(transport, pdu_ref, payload, store).await,
161        _ => handle_szl_user_data(transport, pdu_ref, payload).await,
162    }
163}
164
165async fn handle_clock_user_data<T: AsyncWrite + Unpin>(
166    transport: &mut T,
167    pdu_ref: u16,
168    payload: &[u8],
169    store: &DataStore,
170) -> Result<()> {
171    // payload[6] = subfn: 0x01 = read clock, 0x02 = set clock
172    let subfn = if payload.len() >= 7 { payload[6] } else { 0 };
173
174    if subfn == 0x02 {
175        // Set clock: datetime bytes start at payload[16] (after 8-byte param + 4-byte envelope + 4 skipped)
176        // Actual layout from client: param(8) + envelope[0xFF,0x09,0x00,0x08] + datetime(8)
177        if payload.len() >= 20 {
178            let mut dt_bytes = [0u8; 8];
179            dt_bytes.copy_from_slice(&payload[12..20]);
180            store.set_clock(dt_bytes);
181        }
182        // Respond: AckData with empty body
183        let header = S7Header {
184            pdu_type: PduType::AckData,
185            reserved: 0,
186            pdu_ref,
187            param_len: 0,
188            data_len: 0,
189            error_class: Some(0),
190            error_code: Some(0),
191        };
192        let mut buf = BytesMut::new();
193        header.encode(&mut buf);
194        return send_cotp_data(transport, buf.freeze()).await;
195    }
196
197    // Read clock: respond with real PLC layout:
198    // pdu_type=UserData, param_len=12, data_len=4
199    // datetime bytes span body[8..16] = param[8..12] + data[0..4]
200    let clock = store.get_clock();
201    let mut buf = BytesMut::new();
202    let header = S7Header {
203        pdu_type: PduType::UserData,
204        reserved: 0,
205        pdu_ref,
206        param_len: 12,
207        data_len: 4,
208        error_class: None,
209        error_code: None,
210    };
211    header.encode(&mut buf);
212    // param: 8-byte echo (method=0x12=response, Tg=0x87) + first 4 datetime bytes
213    buf.extend_from_slice(&[0x00, 0x01, 0x12, 0x08, 0x12, 0x87, 0x01, 0x00]);
214    buf.extend_from_slice(&clock[..4]);
215    // data: last 4 datetime bytes
216    buf.extend_from_slice(&clock[4..]);
217    send_cotp_data(transport, buf.freeze()).await
218}
219
220async fn handle_szl_user_data<T: AsyncWrite + Unpin>(
221    transport: &mut T,
222    pdu_ref: u16,
223    payload: &[u8],
224) -> Result<()> {
225    // payload = param(8) + data_envelope(4) + [szl_id:2][szl_index:2]
226    let szl_id = if payload.len() >= 14 {
227        u16::from_be_bytes([payload[12], payload[13]])
228    } else {
229        0
230    };
231
232    let response_data = build_szl_response(szl_id);
233    let param_len = 12u16;
234    let data_len = response_data.len() as u16;
235
236    let header = S7Header {
237        pdu_type: PduType::AckData,
238        reserved: 0,
239        pdu_ref,
240        param_len,
241        data_len,
242        error_class: Some(0),
243        error_code: Some(0),
244    };
245    let mut buf = BytesMut::new();
246    header.encode(&mut buf);
247    if payload.len() >= 12 {
248        buf.extend_from_slice(&payload[..12]);
249    } else {
250        buf.resize(buf.len() + param_len as usize, 0);
251    }
252    buf.put_u8(0xFF);
253    buf.put_u8(0x04);
254    buf.put_u16(data_len);
255    buf.extend_from_slice(&response_data);
256    send_cotp_data(transport, buf.freeze()).await
257}
258
259fn szl_block(szl_id: u16, szl_index: u16, entry_len: u16, entries: &[u8]) -> Vec<u8> {
260    let entry_count = if entry_len > 0 { (entries.len() / entry_len as usize) as u16 } else { 0 };
261    let mut v = Vec::with_capacity(8 + entries.len());
262    v.extend_from_slice(&szl_id.to_be_bytes());
263    v.extend_from_slice(&szl_index.to_be_bytes());
264    v.extend_from_slice(&entry_len.to_be_bytes());
265    v.extend_from_slice(&entry_count.to_be_bytes());
266    v.extend_from_slice(entries);
267    v
268}
269
270fn build_szl_response(szl_id: u16) -> Vec<u8> {
271    match szl_id {
272        // Order code: entry_len=28 (2-byte index + 20-byte string + 6 version bytes)
273        0x0011 => {
274            let mut entry = vec![0u8; 28];
275            entry[0] = 0x00; entry[1] = 0x01; // entry index 0x0001
276            let s = b"Simulated PLC       "; // 20 chars
277            entry[2..2 + s.len()].copy_from_slice(s);
278            // version: v1.v2.v3 at offsets 23,24,25 (after 2-idx + 20-str + 1-pad)
279            entry[23] = 1; entry[24] = 0; entry[25] = 0;
280            szl_block(0x0011, 0x0000, 28, &entry)
281        }
282        // Protection level
283        0x0032 => {
284            let mut entry = vec![0u8; 16];
285            // scheme_szl=3, scheme_module=3, scheme_bus=3, level=0
286            entry[0] = 3; entry[2] = 3; entry[4] = 3;
287            szl_block(0x0032, 0x0000, 16, &entry)
288        }
289        // CPU info: entry_len=34 (2-byte index + 32-byte string), 7 entries
290        0x001C => {
291            const SLEN: usize = 32;
292            const ELEN: usize = 2 + SLEN;
293            let entry_len = ELEN as u16;
294
295            let make = |idx: u16, s: &[u8]| -> [u8; ELEN] {
296                let mut e = [b' '; ELEN];
297                e[0] = (idx >> 8) as u8;
298                e[1] = idx as u8;
299                let n = s.len().min(SLEN);
300                e[2..2 + n].copy_from_slice(&s[..n]);
301                e
302            };
303
304            let mut entries = Vec::with_capacity(7 * ELEN);
305            entries.extend_from_slice(&make(0x0001, b"SimPLC"));           // AS name
306            entries.extend_from_slice(&make(0x0002, b"CPU Simulated"));    // module type (S7-300 style)
307            entries.extend_from_slice(&make(0x0003, b"SimPLC"));           // module name
308            entries.extend_from_slice(&make(0x0004, b"(C) Simulated"));    // copyright
309            entries.extend_from_slice(&make(0x0005, b"SIM-0000000001"));   // serial number
310            entries.extend_from_slice(&make(0x0007, b"CPU Simulated"));    // canonical module type
311            entries.extend_from_slice(&make(0x0008, b"SimPLC"));           // module name (dup)
312            szl_block(0x001C, 0x0000, entry_len, &entries)
313        }
314        // CP info: index(2) + max_pdu(2) + max_conn(2) + max_mpi(4) + max_bus(4) = 14 bytes
315        0x0131 => {
316            let mut entry = vec![0u8; 14];
317            entry[0] = 0x00; entry[1] = 0x01; // index 0x0001
318            entry[2] = 0x01; entry[3] = 0xE0; // max_pdu=480
319            entry[4] = 0x00; entry[5] = 0x20; // max_connections=32
320            entry[6] = 0x00; entry[7] = 0x02; entry[8] = 0xDC; entry[9] = 0x6C; // max_mpi=187500
321            entry[10] = 0x00; entry[11] = 0x00; entry[12] = 0x61; entry[13] = 0xA8; // max_bus=25000
322            szl_block(0x0131, 0x0001, 14, &entry)
323        }
324        // PLC status: entries[3]=0x08 → payload[11]=0x08 (RUN), matching get_plc_status logic
325        0x0424 => {
326            let mut data = vec![0u8; 12];
327            data[3] = 0x08; // RUN — payload[8+3]=payload[11]
328            szl_block(0x0424, 0x0000, 12, &data)
329        }
330        _ => szl_block(szl_id, 0x0000, 0, &[]),
331    }
332}
333
334// -- Helper functions -------------------------------------------------------
335
336async fn send_simple_ack<T: AsyncWrite + Unpin>(transport: &mut T, pdu_ref: u16) -> Result<()> {
337    let header = S7Header {
338        pdu_type: PduType::AckData,
339        reserved: 0,
340        pdu_ref,
341        param_len: 0,
342        data_len: 0,
343        error_class: Some(0),
344        error_code: Some(0),
345    };
346    let mut buf = BytesMut::new();
347    header.encode(&mut buf);
348    send_cotp_data(transport, buf.freeze()).await
349}
350
351async fn send_ack_data<T: AsyncWrite + Unpin>(
352    transport: &mut T,
353    pdu_ref: u16,
354    func: u8,
355    item_count: u8,
356    data: Bytes,
357) -> Result<()> {
358    let param: Bytes = Bytes::copy_from_slice(&[func, item_count]);
359    let header = S7Header {
360        pdu_type: PduType::AckData,
361        reserved: 0,
362        pdu_ref,
363        param_len: 2,
364        data_len: data.len() as u16,
365        error_class: Some(0),
366        error_code: Some(0),
367    };
368    let mut buf = BytesMut::new();
369    header.encode(&mut buf);
370    buf.extend_from_slice(&param);
371    buf.extend_from_slice(&data);
372    send_cotp_data(transport, buf.freeze()).await
373}
374
375async fn send_error_response<T: AsyncWrite + Unpin>(
376    transport: &mut T,
377    pdu_ref: u16,
378    error_class: u8,
379    error_code: u8,
380) -> Result<()> {
381    let header = S7Header {
382        pdu_type: PduType::AckData,
383        reserved: 0,
384        pdu_ref,
385        param_len: 0,
386        data_len: 0,
387        error_class: Some(error_class),
388        error_code: Some(error_code),
389    };
390    let mut buf = BytesMut::new();
391    header.encode(&mut buf);
392    send_cotp_data(transport, buf.freeze()).await
393}
snap7_server/dispatch.rs

snap7_server/
dispatch.rs
