diff options
Diffstat (limited to 'epan/dissectors/packet-rk512.c')
| -rw-r--r-- | epan/dissectors/packet-rk512.c | 376 |
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: +*/ |