Merging upstream version 4.4.0.

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 376 insertions, 0 deletions

diff --git a/epan/dissectors/packet-rk512.c b/epan/dissectors/packet-rk512.c
new file mode 100644
index 00000000..eae30bd9
--- /dev/null
+++ b/epan/dissectors/packet-rk512.c
@@ -0,0 +1,376 @@
+/* packet-rk512.c
+ * Routines for RK 512 protocol dissection
+ * Copyright 2022 Michael Mann
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include "config.h"
+
+#include <epan/packet.h>
+#include <epan/expert.h>
+#include <epan/dissectors/packet-tcp.h>
+#include <wsutil/crc16.h>
+
+void proto_register_rk512(void);
+void proto_reg_handoff_rk512(void);
+
+static int proto_rk512;
+
+//static int hf_rk512_garbage_data;
+static int hf_rk512_reply_header;
+static int hf_rk512_data_block_type;
+static int hf_rk512_size;
+static int hf_rk512_coordination_flag;
+static int hf_rk512_device_code;
+static int hf_rk512_protocol_version;
+static int hf_rk512_status;
+static int hf_rk512_scan_number;
+static int hf_rk512_telegram_number;
+static int hf_rk512_data_type;
+static int hf_rk512_measurement_data_type;
+static int hf_rk512_measurement_data;
+static int hf_rk512_measurement_data_distance;
+static int hf_rk512_measurement_data_flags;
+static int hf_rk512_checksum;
+static int hf_rk512_checksum_status;
+
+static int ett_rk512;
+static int ett_rk512_measurement_data;
+static int ett_rk512_measurement_data_value;
+static int ett_rk512_continuous_data;
+
+static expert_field ei_rk512_reply_header;
+static expert_field ei_rk512_data_type;
+static expert_field ei_rk512_checksum;
+
+//Preferences
+static unsigned rk512_num_measurements_pts = 541;
+
+#define RK512_HEADER_SIZE   4
+#define RK512_CRC_SIZE   2
+//Used to find the start of packets
+static const uint8_t HEADER_SEQUENCE[RK512_HEADER_SIZE] = { 0, 0, 0, 0 };
+static tvbuff_t* tvb_header_signature = NULL;
+
+#define MEASUREMENT_DATA		0xBBBB
+#define REFLECTOR_DATA			0xCCCC
+
+
+static const value_string device_code_vals[] = {
+	{ 7, "Host" },
+	{ 8, "Guest" },
+	{ 0, NULL }
+};
+
+static const value_string status_vals[] = {
+	{ 0, "Normal" },
+	{ 1, "Lockout" },
+	{ 0, NULL }
+};
+
+#define BLOCKNUM_CONTINUOUSDATA		0x0000		// 0
+#define BLOCKNUM_SCID				0x0017		// 23
+#define BLOCKNUM_TOKEN				0x0019		// 25
+#define BLOCKNUM_DATAMODE			0x0067		// 103
+
+static const value_string data_block_type_vals[] = {
+	{ BLOCKNUM_CONTINUOUSDATA, "Continuous Data" },
+	{ BLOCKNUM_SCID, "SCID" },
+	{ BLOCKNUM_TOKEN, "Token" },
+	{ BLOCKNUM_DATAMODE, "Data Mode" },
+	{ 0, NULL }
+};
+
+static const value_string datatype_vals[] = {
+	{ MEASUREMENT_DATA, "Measurement data" },
+	{ REFLECTOR_DATA, "Reflector data" },
+	{ 0, NULL }
+};
+
+/* Copied and renamed from proto.c because global value_strings don't work for plugins */
+static const value_string plugin_proto_checksum_vals[] = {
+	{ PROTO_CHECKSUM_E_BAD,        "Bad"  },
+	{ PROTO_CHECKSUM_E_GOOD,       "Good" },
+	{ PROTO_CHECKSUM_E_UNVERIFIED, "Unverified" },
+	{ PROTO_CHECKSUM_E_NOT_PRESENT, "Not present" },
+	{ 0, NULL }
+};
+
+static int* const rk512_measurement_data_fields[] = {
+	&hf_rk512_measurement_data_distance,
+	&hf_rk512_measurement_data_flags,
+	NULL,
+};
+
+static unsigned
+get_rk512_pdu_len(packet_info *pinfo _U_, tvbuff_t *tvb, int offset, void *data _U_)
+{
+	uint32_t len = 0;
+
+	len = tvb_get_ntohs(tvb, offset + RK512_HEADER_SIZE + 2);	//length in words
+
+	//Yes, this is a horrible hack but works with the captures seen
+	if ((rk512_num_measurements_pts == 381) && (len == 392))
+		len = 390;
+
+	return ((len*2) + RK512_HEADER_SIZE + 2 + RK512_CRC_SIZE);
+}
+
+static int
+dissect_rk512_pdu(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _U_)
+{
+	proto_tree		*rk512_tree, *continous_data_tree, *measurement_tree;
+	proto_item		*ti, *header_item, *continous_data_item, *data_item;
+	int				offset = 0, start_offset;
+	uint32_t			tag, block_type, data_type, sub_data_type, size;
+
+	col_set_str(pinfo->cinfo, COL_PROTOCOL, "RK512");
+	col_clear(pinfo->cinfo, COL_INFO);
+
+	ti = proto_tree_add_item(tree, proto_rk512, tvb, 0, -1, ENC_NA);
+	rk512_tree = proto_item_add_subtree(ti, ett_rk512);
+
+	header_item = proto_tree_add_item_ret_uint(rk512_tree, hf_rk512_reply_header, tvb, offset, 4, ENC_BIG_ENDIAN, &tag);
+	offset += 4;
+	if (tag != 0)
+		expert_add_info(pinfo, header_item, &ei_rk512_reply_header);
+
+	proto_tree_add_item_ret_uint(rk512_tree, hf_rk512_data_block_type, tvb, offset, 2, ENC_BIG_ENDIAN, &block_type);
+	offset += 2;
+
+	col_set_str(pinfo->cinfo, COL_INFO, val_to_str_const(block_type, data_block_type_vals, "Unknown"));
+
+	switch (block_type)
+	{
+	case BLOCKNUM_CONTINUOUSDATA:
+		start_offset = offset;
+		continous_data_tree = proto_tree_add_subtree(rk512_tree, tvb, offset, -1, ett_rk512_continuous_data, &continous_data_item, "Continuous Data");
+
+		proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_size, tvb, offset, 2, ENC_BIG_ENDIAN, &size);
+		size *= 2;		//size is in words
+		offset += 2;
+		proto_tree_add_item(continous_data_tree, hf_rk512_coordination_flag, tvb, offset, 1, ENC_BIG_ENDIAN);
+		offset += 1;
+		proto_tree_add_item(continous_data_tree, hf_rk512_device_code, tvb, offset, 1, ENC_BIG_ENDIAN);
+		offset += 1;
+		proto_tree_add_item(continous_data_tree, hf_rk512_protocol_version, tvb, offset, 2, ENC_BIG_ENDIAN);
+		offset += 2;
+		proto_tree_add_item(continous_data_tree, hf_rk512_status, tvb, offset, 2, ENC_BIG_ENDIAN);
+		offset += 2;
+		proto_tree_add_item(continous_data_tree, hf_rk512_scan_number, tvb, offset, 4, ENC_LITTLE_ENDIAN);
+		offset += 4;
+		proto_tree_add_item(continous_data_tree, hf_rk512_telegram_number, tvb, offset, 2, ENC_LITTLE_ENDIAN);
+		offset += 2;
+		data_item = proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_data_type, tvb, offset, 2, ENC_LITTLE_ENDIAN, &data_type);
+		offset += 2;
+
+		switch (data_type)
+		{
+		case MEASUREMENT_DATA:
+		{
+			//Yes, this is a horrible hack but works with the captures seen
+			if ((rk512_num_measurements_pts == 381) && (size == 784))
+				size = 780;
+
+			uint8_t* crc_data;
+			proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_measurement_data_type, tvb, offset, 2, ENC_LITTLE_ENDIAN, &sub_data_type);
+			offset += 2;
+			measurement_tree = proto_tree_add_subtree(continous_data_tree, tvb, offset, rk512_num_measurements_pts * 2, ett_rk512_measurement_data, NULL, "Measurement Data");
+			for (unsigned i = 0; i < rk512_num_measurements_pts; i++)
+			{
+				proto_tree_add_bitmask(measurement_tree, tvb, offset, hf_rk512_measurement_data, ett_rk512_measurement_data_value, rk512_measurement_data_fields, ENC_BIG_ENDIAN);
+				offset += 2;
+			}
+
+			//Create data to compute the CRC
+			crc_data = (uint8_t*)wmem_alloc(wmem_packet_scope(), size + 2);
+			//start with a word with value 0
+			crc_data[0] = 0;
+			crc_data[1] = 0;
+			//copy the rest of the packet
+			tvb_memcpy(tvb, &crc_data[2], start_offset, size);
+
+			proto_tree_add_checksum(rk512_tree, tvb, offset,
+				hf_rk512_checksum, hf_rk512_checksum_status, &ei_rk512_checksum, pinfo,
+				crc16_x25_ccitt_seed(crc_data, size + 2, 0xFFFF), ENC_LITTLE_ENDIAN, PROTO_CHECKSUM_VERIFY);
+			offset += 2;
+			break;
+		}
+		case REFLECTOR_DATA:
+			break;
+		default:
+			expert_add_info(pinfo, data_item, &ei_rk512_data_type);
+			break;
+		}
+
+		proto_item_set_len(continous_data_item, offset - start_offset);
+		break;
+	case BLOCKNUM_SCID:
+		break;
+	case BLOCKNUM_TOKEN:
+		break;
+	case BLOCKNUM_DATAMODE:
+		break;
+	}
+
+	return tvb_captured_length(tvb);
+}
+
+static int
+dissect_rk512(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data)
+{
+	tcp_dissect_pdus(tvb, pinfo, tree, true, 8, get_rk512_pdu_len, dissect_rk512_pdu, data);
+	return tvb_captured_length(tvb);
+}
+
+static bool
+dissect_rk512_heur(tvbuff_t* tvb, packet_info* pinfo, proto_tree* tree, void* data _U_)
+{
+	int signature_start, offset;
+	uint16_t block_type;
+	tvbuff_t* rk512_tvb;
+
+	signature_start = tvb_find_tvb(tvb, tvb_header_signature, 0);
+	if (signature_start == -1) {
+		return false;
+	}
+
+	//keep track of the offset for verification of upcoming fields
+	offset = signature_start + RK512_HEADER_SIZE;
+
+	//Make sure there are enough bytes for the rest of the field checks
+	if (tvb_captured_length_remaining(tvb, offset) < 2)
+		return false;
+
+	//Make sure it's supported block type
+	block_type = tvb_get_ntohs(tvb, offset);
+	if (try_val_to_str(block_type, data_block_type_vals) == NULL)
+		return false;
+
+	offset += 2;
+	if (block_type == BLOCKNUM_CONTINUOUSDATA)
+	{
+		uint16_t datatype;
+		if (tvb_captured_length_remaining(tvb, offset) < 18)
+			return false;
+
+		datatype = tvb_get_ntohs(tvb, offset+14);
+		if (try_val_to_str(datatype, datatype_vals) == NULL)
+			return false;
+	}
+
+	rk512_tvb = tvb_new_subset_remaining(tvb, signature_start);
+	dissect_rk512(rk512_tvb, pinfo, tree, data);
+	return true;
+}
+
+static void
+rk512_shutdown(void)
+{
+	tvb_free(tvb_header_signature);
+}
+
+static void
+rk512_fmt_version( char *result, uint32_t revision )
+{
+		snprintf( result, ITEM_LABEL_LENGTH, "%d.%d",
+		(uint8_t)(revision & 0xFF), (uint8_t)(( revision & 0xFF00 ) >> 8));
+}
+
+void
+proto_register_rk512(void)
+{
+	expert_module_t* expert_rk512;
+
+	static hf_register_info hf[] = {
+		//{ &hf_rk512_garbage_data,
+		//	{ "Garbage Data", "rk512.garbage_data", FT_BYTES, BASE_NONE, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_reply_header,
+			{ "Reply Header", "rk512.reply_header", FT_UINT32, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_data_block_type,
+			{ "Data Block Type", "rk512.data_block_type", FT_UINT16, BASE_DEC, VALS(data_block_type_vals), 0x0, NULL, HFILL } },
+		{ &hf_rk512_size,
+			{ "Message Size", "rk512.size", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_coordination_flag,
+			{ "Coordination Flag", "rk512.coordination_flag", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_device_code,
+			{ "Device Code", "rk512.device_code", FT_UINT8, BASE_HEX, VALS(device_code_vals), 0x0, NULL, HFILL } },
+		{ &hf_rk512_protocol_version,
+			{ "Protocol Version", "rk512.protocol_version.version", FT_UINT16, BASE_CUSTOM, CF_FUNC(rk512_fmt_version), 0x0, NULL, HFILL } },
+		{ &hf_rk512_status,
+			{ "Status", "rk512.status", FT_UINT16, BASE_HEX, VALS(status_vals), 0x0, NULL, HFILL } },
+		{ &hf_rk512_scan_number,
+			{ "Scan number", "rk512.scan_number", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_telegram_number,
+			{ "Telegram number", "rk512.telegram_number", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_data_type,
+			{ "Data type", "rk512.data_type", FT_UINT16, BASE_HEX, VALS(datatype_vals), 0x0, NULL, HFILL } },
+		{ &hf_rk512_measurement_data_type,
+			{ "Measurement datatype", "rk512.measurement.data_type", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_measurement_data,
+			{ "Measurement data", "rk512.measurement.data", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_measurement_data_distance,
+			{ "Distance", "rk512.measurement.data.distance", FT_UINT16, BASE_DEC, NULL, 0x1FFF, NULL, HFILL } },
+		{ &hf_rk512_measurement_data_flags,
+			{ "Flags", "rk512.measurement.data.flags", FT_UINT16, BASE_HEX, NULL, 0xE000, NULL, HFILL } },
+		{ &hf_rk512_checksum,
+			{ "Checksum", "rk512.checksum", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+		{ &hf_rk512_checksum_status,
+			{ "CRC Status", "rk512.checksum_status", FT_UINT8, BASE_NONE, &plugin_proto_checksum_vals, 0x0, NULL, HFILL } },
+
+	};
+
+	static int *ett[] = {
+		&ett_rk512,
+		&ett_rk512_continuous_data,
+		&ett_rk512_measurement_data,
+		&ett_rk512_measurement_data_value
+	};
+
+	static ei_register_info ei[] = {
+		{ &ei_rk512_reply_header, { "rk512.reply_header.not_zero", PI_PROTOCOL, PI_WARN, "Reply header not zero", EXPFILL }},
+		{ &ei_rk512_data_type, { "rk512.data_type.unknown", PI_PROTOCOL, PI_WARN, "Unknown data type", EXPFILL }},
+		{ &ei_rk512_checksum, { "rk512.checksum.incorrect", PI_CHECKSUM, PI_WARN, "Checksum incorrect", EXPFILL }},
+	};
+
+
+	proto_rk512 = proto_register_protocol("SICK RK512", "RK512", "rk512");
+	proto_register_field_array(proto_rk512, hf, array_length(hf));
+	proto_register_subtree_array(ett, array_length(ett));
+	expert_rk512 = expert_register_protocol(proto_rk512);
+	expert_register_field_array(expert_rk512, ei, array_length(ei));
+
+	module_t* rk512_module = prefs_register_protocol(proto_rk512, NULL);
+	prefs_register_uint_preference(rk512_module, "num_measurement_pts",
+			"Number of measurement points",
+			"Number of measurement points within the packet",
+			10, &rk512_num_measurements_pts);
+
+	tvb_header_signature = tvb_new_real_data(HEADER_SEQUENCE, sizeof(HEADER_SEQUENCE), sizeof(HEADER_SEQUENCE));
+
+	register_shutdown_routine(rk512_shutdown);
+}
+
+void
+proto_reg_handoff_rk512(void)
+{
+	dissector_handle_t rk512_handle;
+
+	rk512_handle = create_dissector_handle(dissect_rk512, proto_rk512);
+	dissector_add_for_decode_as("tcp.port", rk512_handle);
+	heur_dissector_add("tcp", dissect_rk512_heur, "RK512 over TCP", "rk512_tcp", proto_rk512, HEURISTIC_ENABLE);
+}
+
+/*
+* Editor modelines  -  http://www.wireshark.org/tools/modelines.html
+*
+* Local variables:
+* c-basic-offset: 4
+* tab-width: 8
+* indent-tabs-mode: t
+* End:
+*
+* vi: set shiftwidth=4 tabstop=8 expandtab:
+* :indentSize=4:tabSize=8:noTabs=false:
+*/