Wireshark · Wireshark-dev: [Wireshark-dev] New dissector: packet-genisys.c

[Bruce Keeler <bruce@xxxxxxxxxxx>]

Hi,

I've written a dissector (attached) for the Genisys protocol.  It's a 
pretty obscure protocol used in the Railway industry for SCADA control 
of signaling systems and interlockings.  The company I work for 
(Tri-Met, http://www.trimet.org/) has given the OK to contribute it.

One thing to note: the default TCP port I put in there is 10001 which is 
what we use in our organization.  It's not an IANA assigned one though, 
so I'm really not sure what should be there.

Bruce

/* packet-genisys.c
 * Routines for Genisys(TM)
 * Copyright 2009 Bruce Keeler <bruce@xxxxxxxxxxx>
 *
 * $Id$
 *
 * Wireshark - Network traffic analyzer
 * By Gerald Combs <gerald@xxxxxxxxxxxxx>
 * Copyright 1998 Gerald Combs
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

/*
 * OVERVIEW
 * --------
 *
 * Genisys is a protocol defined by Union Switch and Signal for communicating with
 * SCADA field devices, commonly used in the Railway industry.
 * It is similar in purpose to modbus.
 *
 * "Genisys" is a trademark of Union Switch & Signal.
 *
 * Genisys was designed for use over serial connections, but is commonly transported
 * over TCP as well.
 *
 * The protocol enables one master to communicate with one or more slave devices
 * over the same connection.  The slaves are identified by a one-octet slave address.
 *
 * MESSAGE FORMAT
 * --------------
 *
 * Genisys messages consist of:
 *   * a header octet in the range 0xF1 to 0xFE
 *   * the slave address (one byte)
 *   * sometimes a variable-length payload
 *   * sometimes a two-byte CRC-16
 *   * a terminator octet (0xF6)
 *
 * ESCAPING
 * --------
 *
 * Any octets between the header and terminator that are above 0xF0 must be escaped.
 * The escaped octet produces two octets: 0xF0 followed by the low nybble of the escaped byte.
 *
 *  e.g.   F4  ->  F0 04
 *
 * CRC
 * ---
 *
 * Some messages include a CRC (cyclic redundancy check).  This is a two-octet CRC-16
 * with the polynomial x^16 + x^15 + x^2 + 1, transmitted little-endian.
 * It is applied to all octets in the message including the header byte, but NOT including the
 * terminator byte.  It is calculated BEFORE any escaping is done, and the CRC bytes themselves
 * must be escaped if necessary.
 *
 * PAYLOAD
 * -------
 *
 * For messages that carry a payload, it is always a sequence of address/data byte pairs,
 * escaped if necessary.
 * 
 *  e.g. | addr1  data1 | addr2  data2 | addr3  data3
 *       |   00     C3  |   01   F0 02 |   E0     03
 *                                ^^^
 *                                 \___ escaped!
 */

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <glib.h>
#include <epan/packet.h>
#include <epan/prefs.h>

/* Forward function declarations */
void proto_register_genisys(void);

void proto_reg_handoff_genisys(void);

static void dissect_genisys(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree);

static void analyze_genisys_message(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
				    guint offset, guint length);

void
decode_pairs(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
          guint offset, guint length);

static guint
check_crc(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
          guint offset, guint length);

static guint16
crcsum(guint16 crc, const guint8* message, guint length);

/* Static data */
static dissector_handle_t genisys_handle;
static int proto_genisys = -1;

/* This is not an IANA-assigned port. */
static guint gPORT_PREF = 10001;

/* Initialize the subtree pointers */
static gint ett_genisys = -1;
static gint ett_crc     = -1;
static gint ett_payload = -1;

static int hf_field_header       = -1;
static int hf_field_terminator   = -1;
static int hf_field_slave_addr   = -1;
static int hf_field_crc          = -1;
static int hf_field_crc_expected = -1;
static int hf_field_byte_addr    = -1;
static int hf_field_byte_data    = -1;
static int hf_field_payload      = -1;
static int hf_field_junk         = -1;


/* Main dissector entry point */
static void
dissect_genisys(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree)
{
  guint offset = 0;

  col_set_str(pinfo->cinfo, COL_PROTOCOL, "GENISYS");

  /* Genisys messages start with a header octet in the range 0xF1 to 0xFE,
   * and are terminated with an 0xF6 end message octet.
   * Here, we use the 0xF6 octet as a delimiter, requesting more packets
   * until we have a complete message.  The main dissector is called with
   * everything up to and including the terminator octet.
   */
  while(offset < tvb_reported_length(tvb)) {
    gint available = tvb_reported_length_remaining(tvb, offset);
    gint end_pos = tvb_find_guint8(tvb, offset, available, 0xF6);

    if (-1 == end_pos) {
      pinfo->desegment_offset = offset;
      pinfo->desegment_len = DESEGMENT_ONE_MORE_SEGMENT;
      return;
    }
    ++end_pos;
    analyze_genisys_message(tvb, pinfo, tree, offset, end_pos - offset);
    offset += (guint) end_pos;
  }
}

static void analyze_genisys_message(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
				    guint offset, guint length)
{
  proto_item *item;
  proto_tree *genisys_tree;
  const guint8 *buf;
  guint payload_offset;
  guint junk_len = 0;
  gint payload_length;
  guint8 slave_addr;

  /* Clear out the Info column. */
  col_clear(pinfo->cinfo, COL_INFO);

  buf = tvb_get_ptr(tvb, offset, length);

  /* First, try to skip over any random junk.  There shouldn't be anything between
   * the terminator and next header though.
   */
  while (length > 0 && (buf[0] < 0xF1 || (buf[0] > 0xF3 && buf[0] < 0xF9) || buf[0] > 0xFE)) {
    offset++;
    length--;
    buf++;
    junk_len++;
  }

  if (junk_len) {
    col_set_str(pinfo->cinfo,COL_INFO, "[Random Junk] ");
    item = proto_tree_add_bytes(tree, hf_field_junk, tvb, offset - junk_len, 
				junk_len, buf - junk_len);
  }
  if (length == 0) {
    return;
  }

  item = proto_tree_add_item(tree, proto_genisys, tvb, offset, length, FALSE);
  genisys_tree = proto_item_add_subtree(item, ett_genisys);

  /* A high level look at the header octet.  Low range numbers are slave-to-master
   * messages, high numbers are master-to-slave.  If it starts with anything else, it's
   * junk, probably a partial packet
   */
  if (buf[0] >= 0xF1 && buf[0] <= 0xF3) {
    col_append_str(pinfo->cinfo,COL_INFO, "S->M ");
    proto_item_append_text(item, " S->M ");
  } else if (buf[0] >= 0xF9 && buf[0] <= 0xFE) {
    col_append_str(pinfo->cinfo,COL_INFO, "M->S ");
    proto_item_append_text(item, " M->S ");
  }

  proto_tree_add_item(genisys_tree, hf_field_header, tvb, offset, 1, FALSE);

  /* Deal with escaped slave addr */
  if (buf[1] == 0xF0) {
    slave_addr = buf[2] | 0xF0;
    payload_offset = offset + 3;
    payload_length = length - 4;
    proto_tree_add_uint(genisys_tree, hf_field_slave_addr, tvb, offset + 1, 2, slave_addr);
  } else {
    slave_addr = buf[1];
    payload_offset = offset + 2;
    payload_length = length - 3;
    proto_tree_add_item(genisys_tree, hf_field_slave_addr, tvb, offset + 1, 1, FALSE);
  }
  if (payload_length < 0) {
    proto_item_append_text(item, "[TOO SHORT] ");
    col_append_str(pinfo->cinfo,COL_INFO, "[TOO SHORT] ");
    return;
  }
  proto_item_append_text(item, "(%02x) ", buf[1]);
  col_append_fstr(pinfo->cinfo,COL_INFO, "(%02x) ", buf[1]);

  /* at this point, payload_offset points to the byte following the slave addr
   * and payload_length includes the CRC, but not the terminator byte.
   */
  switch(buf[0]) {
    case 0xF1:
      /* no crc */
      proto_item_append_text(item, "ACK ");
      col_append_str(pinfo->cinfo,COL_INFO, "ACK ");
      if (payload_length > 0) {
        proto_item_append_text(item, "(with extra junk)");
      }
      break;

    case 0xF2:
      proto_item_append_text(item, "Indication Data ");
      col_append_str(pinfo->cinfo,COL_INFO, "IND ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      if (payload_length < 0) {
        proto_item_append_text(item, "[TOO SHORT] ");
        col_append_str(pinfo->cinfo,COL_INFO, "[TOO SHORT] ");
        break;
      }
      decode_pairs(tvb, pinfo, genisys_tree, payload_offset, payload_length);
      
      break;

    case 0xF3:
      proto_item_append_text(item, "Control Checkback ");
      col_append_str(pinfo->cinfo,COL_INFO, "CHECK ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      decode_pairs(tvb, pinfo, genisys_tree, payload_offset, payload_length);
      break;

    case 0xF9:
      proto_item_append_text(item, "Common Control ");
      col_append_str(pinfo->cinfo,COL_INFO, "COMMON CTRL ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      decode_pairs(tvb, pinfo, genisys_tree, payload_offset, payload_length);
      break;

    case 0xFA:
      proto_item_append_text(item, "ACK/POLL ");
      col_append_str(pinfo->cinfo,COL_INFO, "ACK/POLL ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      if (payload_length > 0) {
        proto_item_append_text(item, "(with extra junk)");
      }
      break;

    case 0xFB:
      /* crc is optional here */
      proto_item_append_text(item, "POLL ");
      col_append_str(pinfo->cinfo,COL_INFO, "POLL ");
      if (payload_length >= 2) {
        payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      }
      if (payload_length > 0) {
        proto_item_append_text(item, "(with extra junk)");
      }
        
      break;

    case 0xFC:
      proto_item_append_text(item, "Control ");
      col_append_str(pinfo->cinfo,COL_INFO, "CTRL ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      decode_pairs(tvb, pinfo, genisys_tree, payload_offset, payload_length);
    break;

    case 0xFD:
      proto_item_append_text(item, "Recall");
      col_append_str(pinfo->cinfo,COL_INFO, "RECALL ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      if (payload_length > 0) {
        proto_item_append_text(item, "(with extra junk)");
      }
      break;

    case 0xFE:
      proto_item_append_text(item, "Control Execute");
      col_append_str(pinfo->cinfo,COL_INFO, "EXECUTE ");
      payload_length -= check_crc(tvb, pinfo, genisys_tree, offset, length - 1);
      if (payload_length > 0) {
        proto_item_append_text(item, "(with extra junk)");
      }
      break;

    default:
      /* Should not get here */
      DISSECTOR_ASSERT_NOT_REACHED();
      break;
  }
  (void) proto_tree_add_item(genisys_tree, hf_field_terminator, tvb, offset + length - 1, 1, FALSE);
}

void
decode_pairs(tvbuff_t *tvb, packet_info *pinfo, proto_tree *genisys_tree,
          guint offset, guint length)
{
  const guint8 *ptr;
  proto_item *item;
  proto_tree *subtree;

  ptr = tvb_get_ptr(tvb, offset, length);
  item = proto_tree_add_bytes(genisys_tree, hf_field_payload, tvb, offset, length, ptr);
  subtree = proto_item_add_subtree(item, ett_payload);

  while (length > 0) {
    guint8 addr, data;
    guint item_len;
    
    item_len = 1;
    --length, ++offset;
    addr = *ptr++;
    if (addr == 0xF0 && length > 0) {
      addr |= *ptr++;
      --length, ++offset;
      item_len++;
    }
    proto_tree_add_uint(subtree, hf_field_byte_addr, tvb, offset, item_len, addr);

    if (length <= 0) {
      col_append_str(pinfo->cinfo,COL_INFO, " [ODD DATA]");
      proto_item_append_text(item, " [ODD DATA]");
      return;
    }
    data = *ptr++;
    --length, ++offset;
    item_len = 1;
    if (data == 0xF0 && length > 0) {
      data |= *ptr++;
      --length, ++offset;
      item_len++;
    }
    proto_tree_add_uint(subtree, hf_field_byte_data, tvb, offset, item_len, data);
    col_append_fstr(pinfo->cinfo,COL_INFO, "%02x->%02x ", addr, data);
    /*proto_item_append_text(item, "%02x->%02x ", addr, data);*/
  }
}

/* Returns length of the (possibly-escaped) crc-16, which could be from 2-4 octets */
static guint
check_crc(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
          guint offset, guint length)
{
  guint16 provided_crc;
  guint16 calc_crc;
  proto_item *item;
  proto_tree *subtree;
  guint8 *buf;
  const guint8 *ptr;
  guint unescaped_length;
  guint i, j;
  guint crc_length = 2;

  buf = ep_alloc(length);
  ptr = tvb_get_ptr(tvb, offset, length);

  /* Back up over the (possibly escaped) little-endian crc-16, and extract it */
  provided_crc = ptr[--length];
  if (ptr[length - 1] == 0xF0) {
    provided_crc |= ptr[--length];
    ++crc_length;
  }
  provided_crc <<= 8;
  provided_crc |= ptr[--length];
  if (ptr[length - 1] == 0xF0) {
    provided_crc |= ptr[--length];
    ++crc_length;
  }

  /* 'length' now contains the length of the escaped payload, but not the CRC. 
   * Now unescape the rest of the message into 'buf'
   */

  unescaped_length = length;
  for (i = 0, j = 0; i < length; i++, j++) {
    buf[j] = ptr[i];
    if (buf[j] == 0xF0) {
      i++;
      unescaped_length--;
      if (ptr[i] > 0x0F || length == i) {
        col_set_str(pinfo->cinfo,COL_INFO, "Bad message (bogus escape)");
	return 0;
      }
      buf[j] |= ptr[i];
    }
  }

  calc_crc = crcsum(0, buf, unescaped_length);

  item = proto_tree_add_uint(tree, hf_field_crc, tvb, offset + length, crc_length, provided_crc);
  if (provided_crc == calc_crc) {
    proto_item_append_text(item, " (passed)");
  } else {
    proto_item_append_text(item, " (failed)");
  }
  subtree = proto_item_add_subtree(item, ett_crc);
  (void) proto_tree_add_uint(subtree, hf_field_crc, tvb, offset + length, crc_length, provided_crc);
  (void) proto_tree_add_uint(subtree, hf_field_crc_expected, tvb, 
                             offset + length, crc_length, calc_crc);
  return crc_length;
}

void
proto_register_genisys(void)
{
  module_t *genisys_module;

  /* Setup protocol subtree array */
  static gint *ett[] = {
    &ett_genisys,
    &ett_crc,
    &ett_payload
  };
  static const value_string header_strings[] = {
    { 0xF1, "Acknowledge master" },
    { 0xF2, "Indication data" },
    { 0xF3, "Control data checkback" },
    { 0xF9, "Common control data" },
    { 0xFA, "Acknowledge indication and poll" },
    { 0xFB, "Poll" },
    { 0xFC, "Control data" },
    { 0xFD, "Recall Header" },
    { 0xF2, "Execute controls" },
    { 0, NULL }
  };

  static hf_register_info hf[] = {
    { &hf_field_header,
      { "Header Byte",	"genisys.header", FT_UINT8, BASE_HEX, VALS(header_strings),
	0x0, "Genisys Header Byte", HFILL 
      }
    },
    { &hf_field_terminator,
      { "Terminator Byte",	"genisys.terminator", FT_UINT8, BASE_HEX, NULL,
	0x0, "Genisys Terminator Byte", HFILL 
      }
    },
    { &hf_field_slave_addr,
      { "Slave Address",	"genisys.slave_addr", FT_UINT8, BASE_HEX, NULL,
	0x0, "Genisys Slave Address", HFILL 
      }
    },
    { &hf_field_byte_addr,
      { "Byte Addr",	"genisys.byte_addr", FT_UINT8, BASE_HEX, NULL,
	0x0, "Genisys payload byte Address", HFILL 
      }
    },
    { &hf_field_byte_data,
      { "Byte Data",	"genisys.byte_data", FT_UINT8, BASE_HEX, NULL,
	0x0, "Genisys payload byte Data", HFILL 
      }
    },
    { &hf_field_payload,
      { "Payload",	"genisys.payload", FT_BYTES, BASE_HEX, NULL,
	0x0, "Genisys Payload", HFILL 
      }
    },
    { &hf_field_crc,
      { "CRC (Received)",	"genisys.crc", FT_UINT16, BASE_HEX, NULL,
	0x0, "Genisys CRC received", HFILL 
      }
    },
    { &hf_field_crc_expected,
      { "CRC (Expected)",	"genisys.crc_expected", FT_UINT16, BASE_HEX, NULL,
	0x0, "Genisys CRC Expected", HFILL 
      }
    },
    { &hf_field_junk,
      { "Junk",	"genisys.junk", FT_BYTES, BASE_HEX, NULL,
	0x0, "Genisys Inter-packet Junk", HFILL 
      }
    }
  };
  if (proto_genisys == -1) {
    proto_genisys = proto_register_protocol (
	"Genisys",		/* name */
	"Genisys",		/* short name */
	"genisys"		/* abbrev */
	);
  }
  proto_register_field_array(proto_genisys, hf, array_length(hf));
  proto_register_subtree_array(ett, array_length(ett));
  genisys_module = prefs_register_protocol(
      proto_genisys,
      proto_reg_handoff_genisys
      );
  
  prefs_register_uint_preference(
      genisys_module, 
      "tcp.port", 
      "Genisys TCP Port",
      " Genisys TCP port if other than the default",
      10, 
      &gPORT_PREF
      );

}

void
proto_reg_handoff_genisys(void)
{
  static gboolean initialized = FALSE;
  static int current_port;

  if (!initialized) {
    genisys_handle = create_dissector_handle(dissect_genisys, proto_genisys);
    initialized = TRUE;
  } else {
    dissector_delete("tcp.port", current_port, genisys_handle);
  }
  current_port = gPORT_PREF;
  dissector_add("tcp.port", current_port, genisys_handle);
}

/* CRC-16 x^16 + x^15 + x^2 + 1
 * Implementation borrowed from the Linux kernel.
 */
static const guint16 crc16_table[256] = {
  0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
  0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
  0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
  0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
  0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
  0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
  0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
  0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
  0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
  0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
  0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
  0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
  0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
  0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
  0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
  0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
  0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
  0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
  0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
  0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
  0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
  0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
  0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
  0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
  0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
  0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
  0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
  0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
  0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
  0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
  0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
  0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
};

static inline guint16 crc16_byte(guint16 crc, const guint8 data)
{
  return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff];
}

static guint16
crcsum(guint16 crc, const guint8* buffer, guint len)
{
  while (len--)
    crc = crc16_byte(crc, *buffer++);

  return crc;
}