epan/dissectors/packet-rk512.c

   1 /* packet-rk512.c
   2  * Routines for RK 512 protocol dissection
   3  * Copyright 2022 Michael Mann
   4  *
   5  * SPDX-License-Identifier: GPL-2.0-or-later
   6  */
   7
   8 #include "config.h"
   9
  10 #include <epan/packet.h>
  11 #include <epan/expert.h>
  12 #include <epan/dissectors/packet-tcp.h>
  13 #include <wsutil/crc16.h>
  14
  15 void proto_register_rk512(void);
  16 void proto_reg_handoff_rk512(void);
  17
  18 static int proto_rk512;
  19
  20 //static int hf_rk512_garbage_data;
  21 static int hf_rk512_reply_header;
  22 static int hf_rk512_data_block_type;
  23 static int hf_rk512_size;
  24 static int hf_rk512_coordination_flag;
  25 static int hf_rk512_device_code;
  26 static int hf_rk512_protocol_version;
  27 static int hf_rk512_status;
  28 static int hf_rk512_scan_number;
  29 static int hf_rk512_telegram_number;
  30 static int hf_rk512_data_type;
  31 static int hf_rk512_measurement_data_type;
  32 static int hf_rk512_measurement_data;
  33 static int hf_rk512_measurement_data_distance;
  34 static int hf_rk512_measurement_data_flags;
  35 static int hf_rk512_checksum;
  36 static int hf_rk512_checksum_status;
  37
  38 static int ett_rk512;
  39 static int ett_rk512_measurement_data;
  40 static int ett_rk512_measurement_data_value;
  41 static int ett_rk512_continuous_data;
  42
  43 static expert_field ei_rk512_reply_header;
  44 static expert_field ei_rk512_data_type;
  45 static expert_field ei_rk512_checksum;
  46
  47 //Preferences
  48 static unsigned rk512_num_measurements_pts = 541;
  49
  50 #define RK512_HEADER_SIZE   4
  51 #define RK512_CRC_SIZE   2
  52 //Used to find the start of packets
  53 static const uint8_t HEADER_SEQUENCE[RK512_HEADER_SIZE] = { 0, 0, 0, 0 };
  54 static tvbuff_t* tvb_header_signature = NULL;
  55
  56 #define MEASUREMENT_DATA                0xBBBB
  57 #define REFLECTOR_DATA                  0xCCCC
  58
  59
  60 static const value_string device_code_vals[] = {
  61         { 7, "Host" },
  62         { 8, "Guest" },
  63         { 0, NULL }
  64 };
  65
  66 static const value_string status_vals[] = {
  67         { 0, "Normal" },
  68         { 1, "Lockout" },
  69         { 0, NULL }
  70 };
  71
  72 #define BLOCKNUM_CONTINUOUSDATA         0x0000          // 0
  73 #define BLOCKNUM_SCID                           0x0017          // 23
  74 #define BLOCKNUM_TOKEN                          0x0019          // 25
  75 #define BLOCKNUM_DATAMODE                       0x0067          // 103
  76
  77 static const value_string data_block_type_vals[] = {
  78         { BLOCKNUM_CONTINUOUSDATA, "Continuous Data" },
  79         { BLOCKNUM_SCID, "SCID" },
  80         { BLOCKNUM_TOKEN, "Token" },
  81         { BLOCKNUM_DATAMODE, "Data Mode" },
  82         { 0, NULL }
  83 };
  84
  85 static const value_string datatype_vals[] = {
  86         { MEASUREMENT_DATA, "Measurement data" },
  87         { REFLECTOR_DATA, "Reflector data" },
  88         { 0, NULL }
  89 };
  90
  91 /* Copied and renamed from proto.c because global value_strings don't work for plugins */
  92 static const value_string plugin_proto_checksum_vals[] = {
  93         { PROTO_CHECKSUM_E_BAD,        "Bad"  },
  94         { PROTO_CHECKSUM_E_GOOD,       "Good" },
  95         { PROTO_CHECKSUM_E_UNVERIFIED, "Unverified" },
  96         { PROTO_CHECKSUM_E_NOT_PRESENT, "Not present" },
  97         { 0, NULL }
  98 };
  99
 100 static int* const rk512_measurement_data_fields[] = {
 101         &hf_rk512_measurement_data_distance,
 102         &hf_rk512_measurement_data_flags,
 103         NULL,
 104 };
 105
 106 static unsigned
 107 get_rk512_pdu_len(packet_info *pinfo _U_, tvbuff_t *tvb, int offset, void *data _U_)
 108 {
 109         uint32_t len = 0;
 110
 111         len = tvb_get_ntohs(tvb, offset + RK512_HEADER_SIZE + 2);       //length in words
 112
 113         //Yes, this is a horrible hack but works with the captures seen
 114         if ((rk512_num_measurements_pts == 381) && (len == 392))
 115                 len = 390;
 116
 117         return ((len*2) + RK512_HEADER_SIZE + 2 + RK512_CRC_SIZE);
 118 }
 119
 120 static int
 121 dissect_rk512_pdu(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data _U_)
 122 {
 123         proto_tree              *rk512_tree, *continous_data_tree, *measurement_tree;
 124         proto_item              *ti, *header_item, *continous_data_item, *data_item;
 125         int                             offset = 0, start_offset;
 126         uint32_t                        tag, block_type, data_type, sub_data_type, size;
 127
 128         col_set_str(pinfo->cinfo, COL_PROTOCOL, "RK512");
 129         col_clear(pinfo->cinfo, COL_INFO);
 130
 131         ti = proto_tree_add_item(tree, proto_rk512, tvb, 0, -1, ENC_NA);
 132         rk512_tree = proto_item_add_subtree(ti, ett_rk512);
 133
 134         header_item = proto_tree_add_item_ret_uint(rk512_tree, hf_rk512_reply_header, tvb, offset, 4, ENC_BIG_ENDIAN, &tag);
 135         offset += 4;
 136         if (tag != 0)
 137                 expert_add_info(pinfo, header_item, &ei_rk512_reply_header);
 138
 139         proto_tree_add_item_ret_uint(rk512_tree, hf_rk512_data_block_type, tvb, offset, 2, ENC_BIG_ENDIAN, &block_type);
 140         offset += 2;
 141
 142         col_set_str(pinfo->cinfo, COL_INFO, val_to_str_const(block_type, data_block_type_vals, "Unknown"));
 143
 144         switch (block_type)
 145         {
 146         case BLOCKNUM_CONTINUOUSDATA:
 147                 start_offset = offset;
 148                 continous_data_tree = proto_tree_add_subtree(rk512_tree, tvb, offset, -1, ett_rk512_continuous_data, &continous_data_item, "Continuous Data");
 149
 150                 proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_size, tvb, offset, 2, ENC_BIG_ENDIAN, &size);
 151                 size *= 2;              //size is in words
 152                 offset += 2;
 153                 proto_tree_add_item(continous_data_tree, hf_rk512_coordination_flag, tvb, offset, 1, ENC_BIG_ENDIAN);
 154                 offset += 1;
 155                 proto_tree_add_item(continous_data_tree, hf_rk512_device_code, tvb, offset, 1, ENC_BIG_ENDIAN);
 156                 offset += 1;
 157                 proto_tree_add_item(continous_data_tree, hf_rk512_protocol_version, tvb, offset, 2, ENC_BIG_ENDIAN);
 158                 offset += 2;
 159                 proto_tree_add_item(continous_data_tree, hf_rk512_status, tvb, offset, 2, ENC_BIG_ENDIAN);
 160                 offset += 2;
 161                 proto_tree_add_item(continous_data_tree, hf_rk512_scan_number, tvb, offset, 4, ENC_LITTLE_ENDIAN);
 162                 offset += 4;
 163                 proto_tree_add_item(continous_data_tree, hf_rk512_telegram_number, tvb, offset, 2, ENC_LITTLE_ENDIAN);
 164                 offset += 2;
 165                 data_item = proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_data_type, tvb, offset, 2, ENC_LITTLE_ENDIAN, &data_type);
 166                 offset += 2;
 167
 168                 switch (data_type)
 169                 {
 170                 case MEASUREMENT_DATA:
 171                 {
 172                         //Yes, this is a horrible hack but works with the captures seen
 173                         if ((rk512_num_measurements_pts == 381) && (size == 784))
 174                                 size = 780;
 175
 176                         uint8_t* crc_data;
 177                         proto_tree_add_item_ret_uint(continous_data_tree, hf_rk512_measurement_data_type, tvb, offset, 2, ENC_LITTLE_ENDIAN, &sub_data_type);
 178                         offset += 2;
 179                         measurement_tree = proto_tree_add_subtree(continous_data_tree, tvb, offset, rk512_num_measurements_pts * 2, ett_rk512_measurement_data, NULL, "Measurement Data");
 180                         for (unsigned i = 0; i < rk512_num_measurements_pts; i++)
 181                         {
 182                                 proto_tree_add_bitmask(measurement_tree, tvb, offset, hf_rk512_measurement_data, ett_rk512_measurement_data_value, rk512_measurement_data_fields, ENC_BIG_ENDIAN);
 183                                 offset += 2;
 184                         }
 185
 186                         //Create data to compute the CRC
 187                         crc_data = (uint8_t*)wmem_alloc(wmem_packet_scope(), size + 2);
 188                         //start with a word with value 0
 189                         crc_data[0] = 0;
 190                         crc_data[1] = 0;
 191                         //copy the rest of the packet
 192                         tvb_memcpy(tvb, &crc_data[2], start_offset, size);
 193
 194                         proto_tree_add_checksum(rk512_tree, tvb, offset,
 195                                 hf_rk512_checksum, hf_rk512_checksum_status, &ei_rk512_checksum, pinfo,
 196                                 crc16_x25_ccitt_seed(crc_data, size + 2, 0xFFFF), ENC_LITTLE_ENDIAN, PROTO_CHECKSUM_VERIFY);
 197                         offset += 2;
 198                         break;
 199                 }
 200                 case REFLECTOR_DATA:
 201                         break;
 202                 default:
 203                         expert_add_info(pinfo, data_item, &ei_rk512_data_type);
 204                         break;
 205                 }
 206
 207                 proto_item_set_len(continous_data_item, offset - start_offset);
 208                 break;
 209         case BLOCKNUM_SCID:
 210                 break;
 211         case BLOCKNUM_TOKEN:
 212                 break;
 213         case BLOCKNUM_DATAMODE:
 214                 break;
 215         }
 216
 217         return tvb_captured_length(tvb);
 218 }
 219
 220 static int
 221 dissect_rk512(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data)
 222 {
 223         tcp_dissect_pdus(tvb, pinfo, tree, true, 8, get_rk512_pdu_len, dissect_rk512_pdu, data);
 224         return tvb_captured_length(tvb);
 225 }
 226
 227 static bool
 228 dissect_rk512_heur(tvbuff_t* tvb, packet_info* pinfo, proto_tree* tree, void* data _U_)
 229 {
 230         int signature_start, offset;
 231         uint16_t block_type;
 232         tvbuff_t* rk512_tvb;
 233
 234         signature_start = tvb_find_tvb(tvb, tvb_header_signature, 0);
 235         if (signature_start == -1) {
 236                 return false;
 237         }
 238
 239         //keep track of the offset for verification of upcoming fields
 240         offset = signature_start + RK512_HEADER_SIZE;
 241
 242         //Make sure there are enough bytes for the rest of the field checks
 243         if (tvb_captured_length_remaining(tvb, offset) < 2)
 244                 return false;
 245
 246         //Make sure it's supported block type
 247         block_type = tvb_get_ntohs(tvb, offset);
 248         if (try_val_to_str(block_type, data_block_type_vals) == NULL)
 249                 return false;
 250
 251         offset += 2;
 252         if (block_type == BLOCKNUM_CONTINUOUSDATA)
 253         {
 254                 uint16_t datatype;
 255                 if (tvb_captured_length_remaining(tvb, offset) < 18)
 256                         return false;
 257
 258                 datatype = tvb_get_ntohs(tvb, offset+14);
 259                 if (try_val_to_str(datatype, datatype_vals) == NULL)
 260                         return false;
 261         }
 262
 263         rk512_tvb = tvb_new_subset_remaining(tvb, signature_start);
 264         dissect_rk512(rk512_tvb, pinfo, tree, data);
 265         return true;
 266 }
 267
 268 static void
 269 rk512_shutdown(void)
 270 {
 271         tvb_free(tvb_header_signature);
 272 }
 273
 274 static void
 275 rk512_fmt_version( char *result, uint32_t revision )
 276 {
 277                 snprintf( result, ITEM_LABEL_LENGTH, "%d.%d",
 278                 (uint8_t)(revision & 0xFF), (uint8_t)(( revision & 0xFF00 ) >> 8));
 279 }
 280
 281 void
 282 proto_register_rk512(void)
 283 {
 284         expert_module_t* expert_rk512;
 285
 286         static hf_register_info hf[] = {
 287                 //{ &hf_rk512_garbage_data,
 288                 //      { "Garbage Data", "rk512.garbage_data", FT_BYTES, BASE_NONE, NULL, 0x0, NULL, HFILL } },
 289                 { &hf_rk512_reply_header,
 290                         { "Reply Header", "rk512.reply_header", FT_UINT32, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 291                 { &hf_rk512_data_block_type,
 292                         { "Data Block Type", "rk512.data_block_type", FT_UINT16, BASE_DEC, VALS(data_block_type_vals), 0x0, NULL, HFILL } },
 293                 { &hf_rk512_size,
 294                         { "Message Size", "rk512.size", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL } },
 295                 { &hf_rk512_coordination_flag,
 296                         { "Coordination Flag", "rk512.coordination_flag", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 297                 { &hf_rk512_device_code,
 298                         { "Device Code", "rk512.device_code", FT_UINT8, BASE_HEX, VALS(device_code_vals), 0x0, NULL, HFILL } },
 299                 { &hf_rk512_protocol_version,
 300                         { "Protocol Version", "rk512.protocol_version.version", FT_UINT16, BASE_CUSTOM, CF_FUNC(rk512_fmt_version), 0x0, NULL, HFILL } },
 301                 { &hf_rk512_status,
 302                         { "Status", "rk512.status", FT_UINT16, BASE_HEX, VALS(status_vals), 0x0, NULL, HFILL } },
 303                 { &hf_rk512_scan_number,
 304                         { "Scan number", "rk512.scan_number", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL } },
 305                 { &hf_rk512_telegram_number,
 306                         { "Telegram number", "rk512.telegram_number", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL } },
 307                 { &hf_rk512_data_type,
 308                         { "Data type", "rk512.data_type", FT_UINT16, BASE_HEX, VALS(datatype_vals), 0x0, NULL, HFILL } },
 309                 { &hf_rk512_measurement_data_type,
 310                         { "Measurement datatype", "rk512.measurement.data_type", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 311                 { &hf_rk512_measurement_data,
 312                         { "Measurement data", "rk512.measurement.data", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 313                 { &hf_rk512_measurement_data_distance,
 314                         { "Distance", "rk512.measurement.data.distance", FT_UINT16, BASE_DEC, NULL, 0x1FFF, NULL, HFILL } },
 315                 { &hf_rk512_measurement_data_flags,
 316                         { "Flags", "rk512.measurement.data.flags", FT_UINT16, BASE_HEX, NULL, 0xE000, NULL, HFILL } },
 317                 { &hf_rk512_checksum,
 318                         { "Checksum", "rk512.checksum", FT_UINT16, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 319                 { &hf_rk512_checksum_status,
 320                         { "CRC Status", "rk512.checksum_status", FT_UINT8, BASE_NONE, &plugin_proto_checksum_vals, 0x0, NULL, HFILL } },
 321
 322         };
 323
 324         static int *ett[] = {
 325                 &ett_rk512,
 326                 &ett_rk512_continuous_data,
 327                 &ett_rk512_measurement_data,
 328                 &ett_rk512_measurement_data_value
 329         };
 330
 331         static ei_register_info ei[] = {
 332                 { &ei_rk512_reply_header, { "rk512.reply_header.not_zero", PI_PROTOCOL, PI_WARN, "Reply header not zero", EXPFILL }},
 333                 { &ei_rk512_data_type, { "rk512.data_type.unknown", PI_PROTOCOL, PI_WARN, "Unknown data type", EXPFILL }},
 334                 { &ei_rk512_checksum, { "rk512.checksum.incorrect", PI_CHECKSUM, PI_WARN, "Checksum incorrect", EXPFILL }},
 335         };
 336
 337
 338         proto_rk512 = proto_register_protocol("SICK RK512", "RK512", "rk512");
 339         proto_register_field_array(proto_rk512, hf, array_length(hf));
 340         proto_register_subtree_array(ett, array_length(ett));
 341         expert_rk512 = expert_register_protocol(proto_rk512);
 342         expert_register_field_array(expert_rk512, ei, array_length(ei));
 343
 344         module_t* rk512_module = prefs_register_protocol(proto_rk512, NULL);
 345         prefs_register_uint_preference(rk512_module, "num_measurement_pts",
 346                         "Number of measurement points",
 347                         "Number of measurement points within the packet",
 348                         10, &rk512_num_measurements_pts);
 349
 350         tvb_header_signature = tvb_new_real_data(HEADER_SEQUENCE, sizeof(HEADER_SEQUENCE), sizeof(HEADER_SEQUENCE));
 351
 352         register_shutdown_routine(rk512_shutdown);
 353 }
 354
 355 void
 356 proto_reg_handoff_rk512(void)
 357 {
 358         dissector_handle_t rk512_handle;
 359
 360         rk512_handle = create_dissector_handle(dissect_rk512, proto_rk512);
 361         dissector_add_for_decode_as("tcp.port", rk512_handle);
 362         heur_dissector_add("tcp", dissect_rk512_heur, "RK512 over TCP", "rk512_tcp", proto_rk512, HEURISTIC_ENABLE);
 363 }
 364
 365 /*
 366 * Editor modelines  -  http://www.wireshark.org/tools/modelines.html
 367 *
 368 * Local variables:
 369 * c-basic-offset: 4
 370 * tab-width: 8
 371 * indent-tabs-mode: t
 372 * End:
 373 *
 374 * vi: set shiftwidth=4 tabstop=8 expandtab:
 375 * :indentSize=4:tabSize=8:noTabs=false:
 376 */