From 75329749f7d82b9b74e3b02a7a8b176fb72e42eb Mon Sep 17 00:00:00 2001 From: manbo Date: Mon, 22 Dec 2025 05:05:46 +0000 Subject: [PATCH] Update parse_sawp_function --- network/src/types/modbus.rs | 171 +++++++++++++++++------------------- 1 file changed, 81 insertions(+), 90 deletions(-) diff --git a/network/src/types/modbus.rs b/network/src/types/modbus.rs index 677c1d6..33f9c8b 100644 --- a/network/src/types/modbus.rs +++ b/network/src/types/modbus.rs @@ -3,7 +3,7 @@ use bytes::{Bytes, BytesMut}; use sawp::error::Error as SawpError; use sawp::error::ErrorKind; use sawp::parser::{Direction, Parse}; -use sawp_modbus::{Data, Message, Modbus}; +use sawp_modbus::{Data, Exception, ExceptionCode, Message, Modbus, Read, Write}; use serde::Deserialize; use serde_json::{Value, json}; use std::convert::TryInto; @@ -124,109 +124,114 @@ fn get_tcp_data_v6(buf: Bytes) -> Result { )) } -pub fn get_tcp_payload_from_eth(eth: &EthernetPacket) -> Result { - let pkt = eth.packet(); // returns &[u8] - match eth.get_ethertype() { - EtherTypes::Ipv4 => { - let ipv4 = Ipv4Packet::new(pkt).ok_or_else(|| anyhow!("failed to parse ipv4"))?; - if ipv4.get_next_level_protocol() != IpNextHeaderProtocols::Tcp { - return Err(anyhow!("not a TCP packet")); - } - let ipv4_payload = ipv4.payload(); - let tcp = TcpPacket::new(ipv4_payload).ok_or_else(|| anyhow!("failed to parse tcp"))?; - let tcp_payload = tcp.payload(); - - // compute byte offsets relative to the original packet slice - let base = pkt.as_ptr() as usize; - let start = tcp_payload.as_ptr() as usize - base; - let end = start + tcp_payload.len(); - Ok(Bytes::copy_from_slice(&pkt[start..end]).slice(0..tcp_payload.len())) - } - - EtherTypes::Ipv6 => { - let ipv6 = Ipv6Packet::new(pkt).ok_or_else(|| anyhow!("failed to parse ipv6"))?; - if ipv6.get_next_header() != IpNextHeaderProtocols::Tcp { - return Err(anyhow!("not a TCP packet")); - } - let ipv6_payload = ipv6.payload(); - let tcp = TcpPacket::new(ipv6_payload).ok_or_else(|| anyhow!("failed to parse tcp"))?; - let tcp_payload = tcp.payload(); - - let base = pkt.as_ptr() as usize; - let start = tcp_payload.as_ptr() as usize - base; - let end = start + tcp_payload.len(); - Ok(Bytes::copy_from_slice(&pkt[start..end]).slice(0..tcp_payload.len())) - } - - _ => Err(anyhow!("not IPv4/IPv6")), - } -} - -/// Top-level parse entry: takes sawp_modbus::Message and descriptor map. -/// Returns JSON object with parsed fields. pub fn parse_sawp_message( msg: &Message, map: &FuncMap, is_response: bool, ) -> Result { - // obtain function code and exception flag from msg.function let mut fn_code = msg.function.raw; let is_exception_fn = (fn_code & 0x80) != 0; if is_exception_fn { fn_code &= 0x7F; } - // Extract a byte-slice to feed generic PDU parser. - // Different Data variants carry bytes differently. let pdu_bytes = match &msg.data { Data::Exception(exc) => { - // Exception contains an exception code byte or similar; convert to vec - // If Exception type exposes code() or .0, adapt accordingly. - // Here we try to obtain a single byte; if Exception is an enum with u8 inside: - let code_byte = exc.raw; // if Exception(pub u8) - vec![code_byte] + // Exception typically wraps a u8 code; adapt if Exception is different. + // try to extract a u8 from Exception; if it exposes .code() use that. + // Here we attempt pattern if Exception(pub u8) + let code: u8 = exc.code as u8; + vec![code] } Data::Diagnostic { data, .. } => data.clone(), Data::MEI { data, .. } => data.clone(), - Data::Read(read) => { - // If Read provides a bytes() method or inner Vec, extract it. - // Adjust depending on actual Read struct — common shape: Read { byte_count, data: Vec } - // Try common fields: - if let Some(bytes) = try_extract_read_bytes(read) { - bytes - } else { - return Err("unsupported Read variant layout; adapt extraction".into()); + Data::Read(read) => match read { + Read::Request { address, quantity } => { + let mut v = Vec::with_capacity(4); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&quantity.to_be_bytes()); + v } - } - Data::Write(write) => { - if let Some(bytes) = try_extract_write_bytes(write) { - bytes - } else { - return Err("unsupported Write variant layout; adapt extraction".into()); + Read::Response(bytes) => bytes.clone(), + }, + Data::Write(write) => match write { + Write::MultReq { + address, + quantity, + data, + } => { + let mut v = Vec::with_capacity(4 + data.len()); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&quantity.to_be_bytes()); + v.extend_from_slice(data); + v } - } + Write::Mask { + address, + and_mask, + or_mask, + } => { + let mut v = Vec::with_capacity(6); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&and_mask.to_be_bytes()); + v.extend_from_slice(&or_mask.to_be_bytes()); + v + } + Write::Other { address, data } => { + let mut v = Vec::with_capacity(4); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&data.to_be_bytes()); + v + } + }, Data::ReadWrite { read, write } => { - // For ReadWrite, decide which side to parse based on is_response. - // If is_response==true, prefer read bytes; else use write bytes. if is_response { - if let Some(bytes) = try_extract_read_bytes(read) { - bytes - } else { - return Err("unsupported Read variant in ReadWrite".into()); + match read { + Read::Request { address, quantity } => { + let mut v = Vec::with_capacity(4); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&quantity.to_be_bytes()); + v + } + Read::Response(bytes) => bytes.clone(), } } else { - if let Some(bytes) = try_extract_write_bytes(write) { - bytes - } else { - return Err("unsupported Write variant in ReadWrite".into()); + match write { + Write::MultReq { + address, + quantity, + data, + } => { + let mut v = Vec::with_capacity(4 + data.len()); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&quantity.to_be_bytes()); + v.extend_from_slice(data); + v + } + Write::Mask { + address, + and_mask, + or_mask, + } => { + let mut v = Vec::with_capacity(6); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&and_mask.to_be_bytes()); + v.extend_from_slice(&or_mask.to_be_bytes()); + v + } + Write::Other { address, data } => { + let mut v = Vec::with_capacity(4); + v.extend_from_slice(&address.to_be_bytes()); + v.extend_from_slice(&data.to_be_bytes()); + v + } } } } - Data::ByteVec(v) => v.clone(), + Data::ByteVec(b) => b.clone(), Data::Empty => vec![], }; - // If exception function: treat as exception if is_exception_fn { if pdu_bytes.is_empty() { return Err("exception message missing exception code".into()); @@ -238,7 +243,6 @@ pub fn parse_sawp_message( })); } - // Lookup descriptor and parse using generic byte-slice parser let desc = map .get(&fn_code) .ok_or_else(|| format!("unknown function code: {}", fn_code))?; @@ -252,7 +256,6 @@ pub fn parse_sawp_message( parse_with_descriptor(&pdu_bytes, msg.unit_id, msg.function.raw, fields) } -/// Generic parser: parse bytes per FieldDescriptor sequence. fn parse_with_descriptor( pdu: &[u8], unit: u8, @@ -362,15 +365,3 @@ fn insert_mapped( } Ok(()) } - -/// Helper extraction functions: adapt these to the real Read/Write structs in your sawp_modbus version. -/// These try common shapes; replace with direct field access if necessary. -fn try_extract_read_bytes(_read: &T) -> Option> { - // Replace with actual extraction, for example: - // Some(read.data.clone()) or Some(read.bytes.clone()) - None -} -fn try_extract_write_bytes(_write: &T) -> Option> { - // Replace with actual extraction - None -}