From: Dobrica Pavlinusic <dpavlin@rot13.org>
Date: Sun, 4 Oct 2020 06:49:57 +0000 (+0200)
Subject: protocol_decode
X-Git-Url: http://git.rot13.org/?p=zc;a=commitdiff_plain;h=27514330ca51d73a786df6533336bb61d17f391d

protocol_decode
---

diff --git a/Protocol.pm b/Protocol.pm
index 2db4ddb..829b7f0 100644
--- a/Protocol.pm
+++ b/Protocol.pm
@@ -5,7 +5,7 @@ use strict;
 require Exporter;
 
 our @ISA = qw(Exporter);
-our @EXPORT = qw( modbus_crc16 $protocol read_parameter_frame write_parameter_frame );
+our @EXPORT = qw( modbus_crc16 $protocol read_parameter_frame write_parameter_frame $function_code_description hex_dump protocol_decode );
 
 use Data::Dump qw(dump);
 
@@ -123,6 +123,148 @@ sub write_parameter_frame {
 
 	return $frame . modbus_crc16($frame);
 }
+
+our $function_code_description = {
+	up => {
+		0x07 => 'Upstream Heartbeat Frame',
+		0x08 => 'Upstream alarm frame',
+		0x03 => 'Upstream Read Parameter Frame',
+		0x06 => 'Upstream return write parameter frame',
+	},
+	down => {
+		0x03 => 'Downstream read parameter frame',
+		0x06 => 'Downstream write parameter frame',
+	}
+};
+
+sub hex_dump {
+	my $bin = shift;
+	my $hex = unpack('H*', $bin);
+	$hex =~ s{(..)}{$1 }g;
+	return $hex;
+}
+
+sub protocol_decode {
+	my ( $up_down, $bin ) = @_;
+
+	$SIG{__WARN__} = sub {
+		return unless $debug;
+		print STDERR @_;
+	};
+
+	my $hash = {
+		up_down => $up_down,
+		bin => $bin,
+	};
+
+	my $cksum = substr($bin, -2, 2);
+
+	if ( my $crc = modbus_crc16( substr($bin,0,-2) ) ) {
+		if ( $crc ne $cksum ) {
+			$hash->{error}->{crc} = "got " . unpack('H*',$crc) . " expected " . unpack('H*',$cksum);
+		}
+	}
+
+	my ( $header, $ver, $function_code, $len ) = unpack("CCCv", $bin);
+
+	$hash->{function_code} = $function_code;
+	$hash->{ver} = $ver;
+	$hash->{len} = $len;
+
+	my $data = substr($bin,5,-2);
+
+	warn "header = $header 0x", hex_dump($header)," ver = $ver function_code = $function_code len = $len == ",length($data), " data = ",hex_dump($data), " cksum = ", unpack('H*',$cksum);
+
+	if ( $header != 0x5a ) {
+		$hash->{error}->{header} = "$header expected 0x5a";
+	}
+
+	my $length_data = length($data);
+	if ( $length_data != $len ) {
+		$hash->{error}->{length} = "$length_data expected $len";
+	}
+
+
+	while ( $data ) {
+
+		warn "XXX data = ", hex_dump($data);
+
+		my $data_id = unpack( 'C', substr($data,0,1) );
+
+		if ( ! exists( $protocol->{$data_id} ) ) {
+			my $len = unpack('C', substr($data,1,1));
+			push @{ $hash->{error}->{data_id} }, sprintf "data_id %d 0x%2x len %d [%s]", $data_id, $data_id, $len, unpack('H*', substr($data,2,$len));
+			$data = substr($data,2 + $len);
+			next;
+		}
+		my $pack_fmt = $protocol->{$data_id}->{pack_fmt} || die "can't find pack_fmt for data_id $data_id";
+
+		my $data_range;
+		my $data_len;
+
+		if ( $data_id == 0x00 ) {
+			# sequence number
+
+			$data_range = substr($data,2,4);
+			$data = substr($data,6);
+
+		} elsif ( $up_down eq 'down' && $function_code == 0x03 ) {
+			# type A
+
+			$data_range = substr($data,0,1);
+			$data = substr($data,1);
+
+		} elsif ( $up_down eq 'up'   && ( $function_code == 0x07 || $function_code == 0x08 || $function_code == 0x03 ) ) {
+			# type B
+
+			$data_len = unpack('C', substr($data,1,1));
+			$data_range = substr($data,2, $data_len);
+
+			$data = substr($data,2 + $data_len);
+
+		} elsif ( $up_down eq 'down' && $function_code == 0x06 ) {
+			# type B
+
+			$data_len = unpack('C', substr($data,1,1));
+			$data_range = substr($data,2, $data_len);
+
+			$data = substr($data,2 + $data_len);
+
+		} elsif ( $up_down eq 'up' && $function_code == 0x06 ) {
+			# type C
+
+			# FIXME check first byte?
+			my $data_range = substr($data,1,1);
+
+			$data = substr($data,2);
+
+		} else {
+			$hash->{error}->{function_code_unknown} = $function_code;
+			warn "ERROR unknown function_code = $function_code";
+		}
+
+		my @v = unpack($pack_fmt, $data_range);
+
+		my $v = join(' ', @v);
+
+		if ( $data_id == 0x0c ) {
+			$v = $v / 100;
+		} elsif ( $data_id == 0x0d ) {
+			$v = $v / 100;
+		}
+
+		push @{ $hash->{data_id_order} }, $data_id;
+		$hash->{data_id}->{$data_id} = $v;
+		$hash->{data_len}->{$data_id} = $data_len;
+		$hash->{data_range}->{$data_id} = $data_range;
+
+	}
+
+	return $hash;
+
+} # protocol_decode
+
+
 1;
 
 # 6. Protocol Format