Adding upstream version 4.2.2.upstream/4.2.2

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 420 insertions, 0 deletions

diff --git a/epan/dissectors/packet-hsfz.c b/epan/dissectors/packet-hsfz.c
new file mode 100644
index 00000000..f1bbeb96
--- /dev/null
+++ b/epan/dissectors/packet-hsfz.c
@@ -0,0 +1,420 @@
+/* packet-hsfz.c
+ * HSFZ Dissector
+ * By Dr. Lars Voelker <lars.voelker@technica-engineering.de>
+ * Copyright 2013-2019 BMW Group, Dr. Lars Voelker
+ * Copyright 2020-2023 Technica Engineering, Dr. Lars Voelker
+ * Copyright 2023-2023 BMW Group, Hermann Leinsle
+ *
+ * Wireshark - Network traffic analyzer
+ * By Gerald Combs <gerald@wireshark.org>
+ * Copyright 1998 Gerald Combs
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+
+#include <config.h>
+#include <epan/prefs.h>
+#include <epan/uat.h>
+#include "packet-tcp.h"
+#include "packet-udp.h"
+#include "packet-hsfz.h"
+
+
+#define HSFZ_HDR_LEN        6
+
+#define HSFZ_NAME           "HSFZ"
+#define HSFZ_NAME_LONG      "High Speed Fahrzeugzugang"
+#define HSFZ_NAME_FILTER    "hsfz"
+
+
+dissector_handle_t hsfz_handle_tcp;
+dissector_handle_t hsfz_handle_udp;
+dissector_handle_t uds_handle;
+
+void proto_register_hsfz(void);
+void proto_reg_handoff_hsfz(void);
+
+static int proto_hsfz = -1;
+
+/*** header fields ***/
+static int hf_hsfz_length = -1;
+static int hf_hsfz_ctrlword = -1;
+static int hf_hsfz_source_address = -1;
+static int hf_hsfz_target_address = -1;
+static int hf_hsfz_address = -1;
+static int hf_hsfz_ident_string = -1;
+static int hf_hsfz_data = -1;
+
+/*** protocol tree items ***/
+static gint ett_hsfz = -1;
+
+/* Control Words */
+#define	HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES        0x0001
+#define	HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER      0x0002
+#define HSFZ_CTRLWORD_TERMINAL15                0x0010
+#define HSFZ_CTRLWORD_VEHICLE_IDENT_DATA        0x0011
+#define HSFZ_CTRLWORD_ALIVE_CHECK               0x0012
+#define	HSFZ_CTRLWORD_STATUS_DATA_INQUIRY       0x0013
+#define	HSFZ_CTRLWORD_INCORRECT_TESTER_ADDRESS  0x0040
+#define HSFZ_CTRLWORD_INCORRECT_CONTROL_WORD    0x0041
+#define HSFZ_CTRLWORD_INCORRECT_FORMAT          0x0042
+#define HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS    0x0043
+#define HSFZ_CTRLWORD_MESSAGE_TOO_LARGE         0x0044
+#define HSFZ_CTRLWORD_DIAG_APP_NOT_READY        0x0045
+#define HSFZ_CTRLWORD_OUT_OF_MEMORY             0x00FF
+
+static const value_string hsfz_ctrlwords[] = {
+    {HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES,          "Request or Response"},
+    {HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER,        "Acknowledgment"},
+    {HSFZ_CTRLWORD_TERMINAL15,                  "Terminal 15 Control Message"},
+    {HSFZ_CTRLWORD_VEHICLE_IDENT_DATA,          "Vehicle Identification Data"},
+    {HSFZ_CTRLWORD_ALIVE_CHECK,                 "Alive check"},
+    {HSFZ_CTRLWORD_STATUS_DATA_INQUIRY,         "Status data inquiry"},
+    {HSFZ_CTRLWORD_INCORRECT_TESTER_ADDRESS,    "Incorrect tester address"},
+    {HSFZ_CTRLWORD_INCORRECT_CONTROL_WORD,      "Incorrect control word"},
+    {HSFZ_CTRLWORD_INCORRECT_FORMAT,            "Incorrect format"},
+    {HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS,      "Incorrect destination address"},
+    {HSFZ_CTRLWORD_MESSAGE_TOO_LARGE,           "Message too large"},
+    {HSFZ_CTRLWORD_DIAG_APP_NOT_READY,          "Diagnostic application not ready"},
+    {HSFZ_CTRLWORD_OUT_OF_MEMORY,               "Out of memory"},
+    {0, NULL}
+};
+
+
+/**********************************
+ ********* Configuration **********
+ **********************************/
+typedef struct _udf_one_id_string {
+    guint	id;
+    gchar*	name;
+} udf_one_id_string_t;
+
+/*** Hash Tables for lookup data ***/
+static GHashTable *ht_diag_addr = NULL;
+
+static gboolean hsfz_check_header = FALSE;
+static gboolean hsfz_show_uds_in_ack = FALSE;
+
+static udf_one_id_string_t *udf_diag_addr = NULL;
+static guint udf_diag_addr_num = 0;
+
+static void *
+udf_copy_one_id_string_cb(void* n, const void* o, size_t size _U_) {
+    udf_one_id_string_t *new_rec = (udf_one_id_string_t*)n;
+    const udf_one_id_string_t *old_rec = (const udf_one_id_string_t*)o;
+
+    if (old_rec->name) {
+        new_rec->name = g_strdup(old_rec->name);
+    } else {
+        new_rec->name = NULL;
+    }
+
+    new_rec->id = old_rec->id;
+    return new_rec;
+}
+
+static void
+udf_free_one_id_string_cb(void *r)   {
+    udf_one_id_string_t *rec = (udf_one_id_string_t*)r;
+    if (rec->name) g_free(rec->name);
+}
+
+static void
+udf_free_one_id_string_data(gpointer data _U_)  {
+    /* nothing to free here since we did not malloc data in udf_post_update_one_id_string_template_cb */
+}
+
+static bool
+udf_update_diag_addr_cb(void *r, char **err) {
+    udf_one_id_string_t *rec = (udf_one_id_string_t *)r;
+
+    if (rec->id > 0xff) {
+        *err = g_strdup_printf("HSFZ only supports 8 bit diagnostic addresses (diag_addr: %i  name: %s)", rec->id, rec->name);
+        return (*err == NULL);
+    }
+
+    if (rec->name == NULL || rec->name[0] == 0) {
+        *err = g_strdup_printf("ECU Name cannot be empty");
+        return (*err == NULL);
+    }
+
+    *err = NULL;
+    return (*err == NULL);
+}
+
+UAT_HEX_CB_DEF(udf_diag_addr, id, udf_one_id_string_t)
+UAT_CSTRING_CB_DEF(udf_diag_addr, name, udf_one_id_string_t)
+
+static void
+udf_free_key(gpointer key) {
+    wmem_free(wmem_epan_scope(), key);
+}
+
+static void
+udf_post_update_one_id_string_template_cb(udf_one_id_string_t *udf_data, guint udf_data_num, GHashTable *ht) {
+    guint i;
+    int *key = NULL;
+    int tmp;
+
+    if (udf_data_num>0) {
+        for (i = 0; i < udf_data_num; i++) {
+            key = wmem_new(wmem_epan_scope(), int);
+            tmp = udf_data[i].id;
+            *key = tmp;
+
+            g_hash_table_insert(ht, key, udf_data[i].name);
+        }
+    }
+}
+
+static void
+udf_post_update_diag_addr_cb(void) {
+    if (ht_diag_addr) {
+        g_hash_table_destroy(ht_diag_addr);
+        ht_diag_addr = NULL;
+    }
+
+    ht_diag_addr = g_hash_table_new_full(g_int_hash, g_int_equal, &udf_free_key, &udf_free_one_id_string_data);
+    udf_post_update_one_id_string_template_cb(udf_diag_addr, udf_diag_addr_num, ht_diag_addr);
+}
+
+static char*
+get_name_from_ht_diag_addr(guint identifier) {
+    guint key = identifier;
+
+    if (ht_diag_addr == NULL) {
+        return NULL;
+    }
+
+    return (char *)g_hash_table_lookup(ht_diag_addr, &key);
+}
+
+
+/**********************************
+ ****** The dissector itself ******
+ **********************************/
+
+static guint8
+dissect_hsfz_address(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *tree, gint offset, int hf_specific_address) {
+    proto_item *ti;
+    guint32 tmp;
+    char *name;
+
+    ti = proto_tree_add_item_ret_uint(tree, hf_specific_address, tvb, offset, 1, ENC_NA, &tmp);
+    name = get_name_from_ht_diag_addr((guint)tmp);
+    if (name != NULL) {
+        proto_item_append_text(ti, " (%s)", name);
+    }
+
+    ti = proto_tree_add_item(tree, hf_hsfz_address, tvb, offset, 1, ENC_BIG_ENDIAN);
+    PROTO_ITEM_SET_HIDDEN(ti);
+
+    return (guint8)tmp;
+}
+
+static int
+dissect_hsfz_message(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
+    proto_item *ti_root;
+
+    guint32 offset = 0;
+    guint32 real_length = 0;
+
+    guint8 source_addr;
+    guint8 target_addr;
+
+    if (tvb_captured_length_remaining(tvb, 0) < HSFZ_HDR_LEN) {
+        return 0;
+    }
+    col_set_str(pinfo->cinfo, COL_PROTOCOL, HSFZ_NAME);
+
+    guint32 hsfz_length = tvb_get_ntohl(tvb, 0);
+    guint16 hsfz_ctrlword = tvb_get_ntohs(tvb, 4);
+    const gchar *ctrlword_description = val_to_str(hsfz_ctrlword, hsfz_ctrlwords, "Unknown 0x%04x");
+
+    const gchar *col_string = col_get_text(pinfo->cinfo, COL_INFO);
+    if (col_string!=NULL && g_str_has_prefix(col_string, (gchar *)&"HSFZ\0")) {
+        col_append_fstr(pinfo->cinfo, COL_INFO, " / %s %s", HSFZ_NAME, ctrlword_description);
+    } else {
+        col_add_fstr(pinfo->cinfo, COL_INFO, "%s %s", HSFZ_NAME, ctrlword_description);
+    }
+
+    if (hsfz_ctrlword == HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES || (hsfz_ctrlword == HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER && hsfz_show_uds_in_ack)) {
+        real_length = HSFZ_HDR_LEN + 2;
+    } else {
+        real_length = HSFZ_HDR_LEN + hsfz_length;
+    }
+
+    ti_root = proto_tree_add_item(tree, proto_hsfz, tvb, 0, real_length, ENC_NA);
+    proto_item_append_text(ti_root, ", Length: %i, Control Word: 0x%04x (%s)", hsfz_length, hsfz_ctrlword, ctrlword_description);
+    proto_tree *hsfz_tree = proto_item_add_subtree(ti_root, ett_hsfz);
+
+    proto_tree_add_item(hsfz_tree, hf_hsfz_length, tvb, offset, 4, ENC_BIG_ENDIAN);
+    offset += 4;
+
+    proto_tree_add_item(hsfz_tree, hf_hsfz_ctrlword, tvb, offset, 2, ENC_BIG_ENDIAN);
+    offset += 2;
+
+    switch (hsfz_ctrlword) {
+    case HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES:
+    case HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER:
+        source_addr = dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_source_address);
+        offset += 1;
+
+        target_addr = dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_target_address);
+        offset += 1;
+
+        if ( (hsfz_ctrlword != HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER || hsfz_show_uds_in_ack) && uds_handle != 0) {
+            hsfz_info_t hsfz_info;
+            hsfz_info.source_address = source_addr;
+            hsfz_info.target_address = target_addr;
+
+            tvbuff_t *subtvb = tvb_new_subset_length(tvb, offset, hsfz_length - 2);
+            call_dissector_with_data(uds_handle, subtvb, pinfo, tree, &hsfz_info);
+        } else {
+            proto_tree_add_item(hsfz_tree, hf_hsfz_data, tvb, offset, hsfz_length - 2, ENC_NA);
+        }
+        break;
+
+    case HSFZ_CTRLWORD_VEHICLE_IDENT_DATA:
+        if (hsfz_length > 0) {
+            const guint8 *ident_data;
+            proto_tree_add_item_ret_string(hsfz_tree, hf_hsfz_ident_string, tvb, offset, hsfz_length, ENC_ASCII, pinfo->pool, &ident_data);
+            col_append_fstr(pinfo->cinfo, COL_INFO, " (%s)", ident_data);
+        }
+        break;
+
+    case HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS:
+    case HSFZ_CTRLWORD_OUT_OF_MEMORY:
+        if (hsfz_ctrlword == HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS || hsfz_length >= 2) {
+            dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_source_address);
+            offset += 1;
+
+            dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_target_address);
+        }
+        break;
+
+    default:
+        if (hsfz_length > 0) {
+            proto_tree_add_item(hsfz_tree, hf_hsfz_data, tvb, offset, hsfz_length, ENC_NA);
+        }
+        break;
+    }
+
+    return HSFZ_HDR_LEN + hsfz_length;
+}
+
+static guint
+get_hsfz_message_len(packet_info *pinfo _U_, tvbuff_t *tvb, int offset, void* data _U_) {
+    /* The length [uint32] does not include the header itself */
+    guint32 length = tvb_get_ntohl(tvb, offset);
+    guint16 ctrlwd = tvb_get_ntohs(tvb, offset + 4);
+
+    /* if heuristic check active: */
+    if (hsfz_check_header && (length > 0x000fffff || ctrlwd > 0x00ff )) {
+        return 1;
+    }
+
+    return HSFZ_HDR_LEN + length;
+}
+
+static int
+dissect_hsfz_tcp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
+    tcp_dissect_pdus(tvb, pinfo, tree, TRUE, HSFZ_HDR_LEN, get_hsfz_message_len, dissect_hsfz_message, NULL);
+    return tvb_captured_length(tvb);
+}
+
+static int
+dissect_hsfz_udp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
+    return udp_dissect_pdus(tvb, pinfo, tree, HSFZ_HDR_LEN, NULL, get_hsfz_message_len, dissect_hsfz_message, NULL);
+}
+
+void proto_register_hsfz(void) {
+    module_t *hsfz_module;
+    uat_t* udf_diag_addr_uat;
+
+    /* data fields */
+    static hf_register_info hf[] = {
+    { &hf_hsfz_length,
+        { "Length", "hsfz.length", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL }},
+    { &hf_hsfz_ctrlword,
+        { "Control Word", "hsfz.ctrlword", FT_UINT16, BASE_HEX, VALS(hsfz_ctrlwords), 0x0, NULL, HFILL }},
+    { &hf_hsfz_source_address,
+        { "Source Address", "hsfz.sourceaddr", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL }},
+    { &hf_hsfz_target_address,
+        { "Target Address", "hsfz.targetaddr", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL }},
+    { &hf_hsfz_address,
+        { "Address", "hsfz.address", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL } },
+    { &hf_hsfz_ident_string,
+        { "Identification String", "hsfz.identification_string", FT_STRING, BASE_NONE, NULL, 0x0, NULL, HFILL } },
+    { &hf_hsfz_data,
+        { "Data", "hsfz.data", FT_BYTES, BASE_NONE, NULL, 0x0, NULL, HFILL } },
+    };
+
+    /* entries in the protocol tree */
+    static gint *ett[] = {
+        &ett_hsfz,
+    };
+
+    /* UATs for user_data fields */
+    static uat_field_t diag_addr_uat_fields[] = {
+        UAT_FLD_HEX(udf_diag_addr, id, "Diagnostic Address", "Diagnostic Address of ECU (hex without leading 0x)"),
+        UAT_FLD_CSTRING(udf_diag_addr, name, "ECU Name", "Name of ECU (string)"),
+        UAT_END_FIELDS
+    };
+
+    proto_hsfz = proto_register_protocol(HSFZ_NAME_LONG, HSFZ_NAME, HSFZ_NAME_FILTER);
+    proto_register_field_array(proto_hsfz, hf, array_length(hf));
+    proto_register_subtree_array(ett, array_length(ett));
+
+    /* Register preferences */
+    hsfz_module = prefs_register_protocol(proto_hsfz, NULL);
+
+    prefs_register_bool_preference(hsfz_module, "header_check_heuristic", "Find start of HSFZ header by checking validity",
+        "Should the HSFZ dissector check if a HSFZ header for validity (length and control word)?", &hsfz_check_header);
+
+    prefs_register_bool_preference(hsfz_module, "show_uds_in_ack", "Show UDS in HSFZ Ack",
+        "Should the shortened UDS in the HSFZ be dissected?", &hsfz_show_uds_in_ack);
+
+    udf_diag_addr_uat = uat_new("Diagnostic Addresses",
+        sizeof(udf_one_id_string_t),            /* record size           */
+        "HSFZ_diagnostics_addresses",           /* filename              */
+        TRUE,                                   /* from_profile          */
+        (void**)&udf_diag_addr,                 /* data_ptr              */
+        &udf_diag_addr_num,                     /* numitems_ptr          */
+        UAT_AFFECTS_DISSECTION,                 /* specifies addresses   */
+        NULL,                                   /* help                  */
+        udf_copy_one_id_string_cb,              /* copy callback         */
+        udf_update_diag_addr_cb,                /* update callback       */
+        udf_free_one_id_string_cb,              /* free callback         */
+        udf_post_update_diag_addr_cb,           /* post update callback  */
+        NULL,                                   /* reset callback        */
+        diag_addr_uat_fields                    /* UAT field definitions */
+    );
+
+    prefs_register_uat_preference(hsfz_module, "_udf_diag_addr", "Diagnostic Addresses",
+        "A table to define names for diagnostic addresses", udf_diag_addr_uat);
+}
+
+void proto_reg_handoff_hsfz(void) {
+    hsfz_handle_tcp = register_dissector("hsfz_over_tcp", dissect_hsfz_tcp, proto_hsfz);
+    hsfz_handle_udp = register_dissector("hsfz_over_udp", dissect_hsfz_udp, proto_hsfz);
+
+    dissector_add_uint_range_with_preference("tcp.port", "", hsfz_handle_tcp);
+    dissector_add_uint_range_with_preference("udp.port", "", hsfz_handle_udp);
+
+    uds_handle = find_dissector("uds_over_hsfz");
+}
+
+/*
+ * Editor modelines  -  http://www.wireshark.org/tools/modelines.html
+ *
+ * Local variables:
+ * c-basic-offset: 4
+ * tab-width: 8
+ * indent-tabs-mode: nil
+ * End:
+ *
+ * vi: set shiftwidth=4 tabstop=8 expandtab:
+ * :indentSize=4:tabSize=8:noTabs=true:
+ */