/* 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 #include #include #include 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: */