/* 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 }

};

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(pinfo->pool, 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_)

{

  unsigned signature_start, offset;

  uint16_t block_type;

  tvbuff_t* rk512_tvb;

  if (!tvb_find_tvb_remaining(tvb, tvb_header_signature, 0, &signature_start)) {

    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, VALS(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:

*/