/* packet-hsfz.c * HSFZ Dissector * By Dr. Lars Voelker * 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 * Copyright 1998 Gerald Combs * * SPDX-License-Identifier: GPL-2.0-or-later */ #include #include #include #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; /*** header fields ***/ static int hf_hsfz_length; static int hf_hsfz_ctrlword; static int hf_hsfz_source_address; static int hf_hsfz_target_address; static int hf_hsfz_address; static int hf_hsfz_ident_string; static int hf_hsfz_data; /*** protocol tree items ***/ static gint ett_hsfz; /* 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: */