epan/dissectors/packet-hsfz.c

   1 /* packet-hsfz.c
   2  * HSFZ Dissector
   3  * By Dr. Lars Voelker <lars.voelker@technica-engineering.de>
   4  * Copyright 2013-2019 BMW Group, Dr. Lars Voelker
   5  * Copyright 2020-2023 Technica Engineering, Dr. Lars Voelker
   6  * Copyright 2023-2023 BMW Group, Hermann Leinsle
   7  *
   8  * Wireshark - Network traffic analyzer
   9  * By Gerald Combs <gerald@wireshark.org>
  10  * Copyright 1998 Gerald Combs
  11  *
  12  * SPDX-License-Identifier: GPL-2.0-or-later
  13  */
  14
  15
  16 #include <config.h>
  17 #include <epan/prefs.h>
  18 #include <epan/uat.h>
  19 #include "packet-tcp.h"
  20 #include "packet-udp.h"
  21 #include "packet-hsfz.h"
  22
  23
  24 #define HSFZ_HDR_LEN        6
  25
  26 #define HSFZ_NAME           "HSFZ"
  27 #define HSFZ_NAME_LONG      "High Speed Fahrzeugzugang"
  28 #define HSFZ_NAME_FILTER    "hsfz"
  29
  30
  31 dissector_handle_t hsfz_handle_tcp;
  32 dissector_handle_t hsfz_handle_udp;
  33 dissector_handle_t uds_handle;
  34
  35 void proto_register_hsfz(void);
  36 void proto_reg_handoff_hsfz(void);
  37
  38 static int proto_hsfz;
  39
  40 /*** header fields ***/
  41 static int hf_hsfz_length;
  42 static int hf_hsfz_ctrlword;
  43 static int hf_hsfz_source_address;
  44 static int hf_hsfz_target_address;
  45 static int hf_hsfz_address;
  46 static int hf_hsfz_ident_string;
  47 static int hf_hsfz_data;
  48
  49 /*** protocol tree items ***/
  50 static int ett_hsfz;
  51
  52 /* Control Words */
  53 #define HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES        0x0001
  54 #define HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER      0x0002
  55 #define HSFZ_CTRLWORD_TERMINAL15                0x0010
  56 #define HSFZ_CTRLWORD_VEHICLE_IDENT_DATA        0x0011
  57 #define HSFZ_CTRLWORD_ALIVE_CHECK               0x0012
  58 #define HSFZ_CTRLWORD_STATUS_DATA_INQUIRY       0x0013
  59 #define HSFZ_CTRLWORD_INCORRECT_TESTER_ADDRESS  0x0040
  60 #define HSFZ_CTRLWORD_INCORRECT_CONTROL_WORD    0x0041
  61 #define HSFZ_CTRLWORD_INCORRECT_FORMAT          0x0042
  62 #define HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS    0x0043
  63 #define HSFZ_CTRLWORD_MESSAGE_TOO_LARGE         0x0044
  64 #define HSFZ_CTRLWORD_DIAG_APP_NOT_READY        0x0045
  65 #define HSFZ_CTRLWORD_OUT_OF_MEMORY             0x00FF
  66
  67 static const value_string hsfz_ctrlwords[] = {
  68     {HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES,          "Request or Response"},
  69     {HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER,        "Acknowledgment"},
  70     {HSFZ_CTRLWORD_TERMINAL15,                  "Terminal 15 Control Message"},
  71     {HSFZ_CTRLWORD_VEHICLE_IDENT_DATA,          "Vehicle Identification Data"},
  72     {HSFZ_CTRLWORD_ALIVE_CHECK,                 "Alive check"},
  73     {HSFZ_CTRLWORD_STATUS_DATA_INQUIRY,         "Status data inquiry"},
  74     {HSFZ_CTRLWORD_INCORRECT_TESTER_ADDRESS,    "Incorrect tester address"},
  75     {HSFZ_CTRLWORD_INCORRECT_CONTROL_WORD,      "Incorrect control word"},
  76     {HSFZ_CTRLWORD_INCORRECT_FORMAT,            "Incorrect format"},
  77     {HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS,      "Incorrect destination address"},
  78     {HSFZ_CTRLWORD_MESSAGE_TOO_LARGE,           "Message too large"},
  79     {HSFZ_CTRLWORD_DIAG_APP_NOT_READY,          "Diagnostic application not ready"},
  80     {HSFZ_CTRLWORD_OUT_OF_MEMORY,               "Out of memory"},
  81     {0, NULL}
  82 };
  83
  84
  85 /**********************************
  86  ********* Configuration **********
  87  **********************************/
  88 typedef struct _udf_one_id_string {
  89     unsigned    id;
  90     char*       name;
  91 } udf_one_id_string_t;
  92
  93 /*** Hash Tables for lookup data ***/
  94 static GHashTable *ht_diag_addr;
  95
  96 static bool hsfz_check_header;
  97 static bool hsfz_show_uds_in_ack;
  98
  99 static udf_one_id_string_t *udf_diag_addr;
 100 static unsigned udf_diag_addr_num;
 101
 102 static void *
 103 udf_copy_one_id_string_cb(void* n, const void* o, size_t size _U_) {
 104     udf_one_id_string_t *new_rec = (udf_one_id_string_t*)n;
 105     const udf_one_id_string_t *old_rec = (const udf_one_id_string_t*)o;
 106
 107     if (old_rec->name) {
 108         new_rec->name = g_strdup(old_rec->name);
 109     } else {
 110         new_rec->name = NULL;
 111     }
 112
 113     new_rec->id = old_rec->id;
 114     return new_rec;
 115 }
 116
 117 static void
 118 udf_free_one_id_string_cb(void *r)   {
 119     udf_one_id_string_t *rec = (udf_one_id_string_t*)r;
 120     if (rec->name) g_free(rec->name);
 121 }
 122
 123 static void
 124 udf_free_one_id_string_data(void *data _U_)  {
 125     /* nothing to free here since we did not malloc data in udf_post_update_one_id_string_template_cb */
 126 }
 127
 128 static bool
 129 udf_update_diag_addr_cb(void *r, char **err) {
 130     udf_one_id_string_t *rec = (udf_one_id_string_t *)r;
 131
 132     if (rec->id > 0xff) {
 133         *err = g_strdup_printf("HSFZ only supports 8 bit diagnostic addresses (diag_addr: %i  name: %s)", rec->id, rec->name);
 134         return (*err == NULL);
 135     }
 136
 137     if (rec->name == NULL || rec->name[0] == 0) {
 138         *err = g_strdup("ECU Name cannot be empty");
 139         return (*err == NULL);
 140     }
 141
 142     *err = NULL;
 143     return (*err == NULL);
 144 }
 145
 146 UAT_HEX_CB_DEF(udf_diag_addr, id, udf_one_id_string_t)
 147 UAT_CSTRING_CB_DEF(udf_diag_addr, name, udf_one_id_string_t)
 148
 149 static void
 150 udf_free_key(void *key) {
 151     wmem_free(wmem_epan_scope(), key);
 152 }
 153
 154 static void
 155 udf_post_update_one_id_string_template_cb(udf_one_id_string_t *udf_data, unsigned udf_data_num, GHashTable *ht) {
 156     unsigned i;
 157     int *key = NULL;
 158     int tmp;
 159
 160     if (udf_data_num>0) {
 161         for (i = 0; i < udf_data_num; i++) {
 162             key = wmem_new(wmem_epan_scope(), int);
 163             tmp = udf_data[i].id;
 164             *key = tmp;
 165
 166             g_hash_table_insert(ht, key, udf_data[i].name);
 167         }
 168     }
 169 }
 170
 171 static void
 172 udf_post_update_diag_addr_cb(void) {
 173     if (ht_diag_addr) {
 174         g_hash_table_destroy(ht_diag_addr);
 175         ht_diag_addr = NULL;
 176     }
 177
 178     ht_diag_addr = g_hash_table_new_full(g_int_hash, g_int_equal, &udf_free_key, &udf_free_one_id_string_data);
 179     udf_post_update_one_id_string_template_cb(udf_diag_addr, udf_diag_addr_num, ht_diag_addr);
 180 }
 181
 182 static char*
 183 get_name_from_ht_diag_addr(unsigned identifier) {
 184     unsigned key = identifier;
 185
 186     if (ht_diag_addr == NULL) {
 187         return NULL;
 188     }
 189
 190     return (char *)g_hash_table_lookup(ht_diag_addr, &key);
 191 }
 192
 193
 194 /**********************************
 195  ****** The dissector itself ******
 196  **********************************/
 197
 198 static uint8_t
 199 dissect_hsfz_address(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *tree, int offset, int hf_specific_address) {
 200     proto_item *ti;
 201     uint32_t tmp;
 202     char *name;
 203
 204     ti = proto_tree_add_item_ret_uint(tree, hf_specific_address, tvb, offset, 1, ENC_NA, &tmp);
 205     name = get_name_from_ht_diag_addr((unsigned)tmp);
 206     if (name != NULL) {
 207         proto_item_append_text(ti, " (%s)", name);
 208     }
 209
 210     ti = proto_tree_add_item(tree, hf_hsfz_address, tvb, offset, 1, ENC_BIG_ENDIAN);
 211     PROTO_ITEM_SET_HIDDEN(ti);
 212
 213     return (uint8_t)tmp;
 214 }
 215
 216 static int
 217 dissect_hsfz_message(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
 218     proto_item *ti_root;
 219
 220     uint32_t offset = 0;
 221     uint32_t real_length = 0;
 222
 223     uint8_t source_addr;
 224     uint8_t target_addr;
 225
 226     if (tvb_captured_length_remaining(tvb, 0) < HSFZ_HDR_LEN) {
 227         return 0;
 228     }
 229     col_set_str(pinfo->cinfo, COL_PROTOCOL, HSFZ_NAME);
 230
 231     uint32_t hsfz_length = tvb_get_ntohl(tvb, 0);
 232     uint16_t hsfz_ctrlword = tvb_get_ntohs(tvb, 4);
 233     const char *ctrlword_description = val_to_str(hsfz_ctrlword, hsfz_ctrlwords, "Unknown 0x%04x");
 234
 235     const char *col_string = col_get_text(pinfo->cinfo, COL_INFO);
 236     if (col_string!=NULL && g_str_has_prefix(col_string, (char *)&"HSFZ\0")) {
 237         col_append_fstr(pinfo->cinfo, COL_INFO, " / %s %s", HSFZ_NAME, ctrlword_description);
 238     } else {
 239         col_add_fstr(pinfo->cinfo, COL_INFO, "%s %s", HSFZ_NAME, ctrlword_description);
 240     }
 241
 242     if (hsfz_ctrlword == HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES || (hsfz_ctrlword == HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER && hsfz_show_uds_in_ack)) {
 243         real_length = HSFZ_HDR_LEN + 2;
 244     } else {
 245         real_length = HSFZ_HDR_LEN + hsfz_length;
 246     }
 247
 248     ti_root = proto_tree_add_item(tree, proto_hsfz, tvb, 0, real_length, ENC_NA);
 249     proto_item_append_text(ti_root, ", Length: %i, Control Word: 0x%04x (%s)", hsfz_length, hsfz_ctrlword, ctrlword_description);
 250     proto_tree *hsfz_tree = proto_item_add_subtree(ti_root, ett_hsfz);
 251
 252     proto_tree_add_item(hsfz_tree, hf_hsfz_length, tvb, offset, 4, ENC_BIG_ENDIAN);
 253     offset += 4;
 254
 255     proto_tree_add_item(hsfz_tree, hf_hsfz_ctrlword, tvb, offset, 2, ENC_BIG_ENDIAN);
 256     offset += 2;
 257
 258     switch (hsfz_ctrlword) {
 259     case HSFZ_CTRLWORD_DIAGNOSTIC_REQ_RES:
 260     case HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER:
 261         source_addr = dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_source_address);
 262         offset += 1;
 263
 264         target_addr = dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_target_address);
 265         offset += 1;
 266
 267         if ( (hsfz_ctrlword != HSFZ_CTRLWORD_ACKNOWLEDGE_TRANSFER || hsfz_show_uds_in_ack) && uds_handle != 0) {
 268             hsfz_info_t hsfz_info;
 269             hsfz_info.source_address = source_addr;
 270             hsfz_info.target_address = target_addr;
 271
 272             tvbuff_t *subtvb = tvb_new_subset_length(tvb, offset, hsfz_length - 2);
 273             call_dissector_with_data(uds_handle, subtvb, pinfo, tree, &hsfz_info);
 274         } else {
 275             proto_tree_add_item(hsfz_tree, hf_hsfz_data, tvb, offset, hsfz_length - 2, ENC_NA);
 276         }
 277         break;
 278
 279     case HSFZ_CTRLWORD_VEHICLE_IDENT_DATA:
 280         if (hsfz_length > 0) {
 281             const uint8_t *ident_data;
 282             proto_tree_add_item_ret_string(hsfz_tree, hf_hsfz_ident_string, tvb, offset, hsfz_length, ENC_ASCII, pinfo->pool, &ident_data);
 283             col_append_fstr(pinfo->cinfo, COL_INFO, " (%s)", ident_data);
 284         }
 285         break;
 286
 287     case HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS:
 288     case HSFZ_CTRLWORD_OUT_OF_MEMORY:
 289         if (hsfz_ctrlword == HSFZ_CTRLWORD_INCORRECT_DEST_ADDRESS || hsfz_length >= 2) {
 290             dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_source_address);
 291             offset += 1;
 292
 293             dissect_hsfz_address(tvb, pinfo, hsfz_tree, offset, hf_hsfz_target_address);
 294         }
 295         break;
 296
 297     default:
 298         if (hsfz_length > 0) {
 299             proto_tree_add_item(hsfz_tree, hf_hsfz_data, tvb, offset, hsfz_length, ENC_NA);
 300         }
 301         break;
 302     }
 303
 304     return HSFZ_HDR_LEN + hsfz_length;
 305 }
 306
 307 static unsigned
 308 get_hsfz_message_len(packet_info *pinfo _U_, tvbuff_t *tvb, int offset, void* data _U_) {
 309     /* The length [uint32] does not include the header itself */
 310     uint32_t length = tvb_get_ntohl(tvb, offset);
 311     uint16_t ctrlwd = tvb_get_ntohs(tvb, offset + 4);
 312
 313     /* if heuristic check active: */
 314     if (hsfz_check_header && (length > 0x000fffff || ctrlwd > 0x00ff )) {
 315         return 1;
 316     }
 317
 318     return HSFZ_HDR_LEN + length;
 319 }
 320
 321 static int
 322 dissect_hsfz_tcp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
 323     tcp_dissect_pdus(tvb, pinfo, tree, true, HSFZ_HDR_LEN, get_hsfz_message_len, dissect_hsfz_message, NULL);
 324     return tvb_captured_length(tvb);
 325 }
 326
 327 static int
 328 dissect_hsfz_udp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
 329     return udp_dissect_pdus(tvb, pinfo, tree, HSFZ_HDR_LEN, NULL, get_hsfz_message_len, dissect_hsfz_message, NULL);
 330 }
 331
 332 void proto_register_hsfz(void) {
 333     module_t *hsfz_module;
 334     uat_t* udf_diag_addr_uat;
 335
 336     /* data fields */
 337     static hf_register_info hf[] = {
 338     { &hf_hsfz_length,
 339         { "Length", "hsfz.length", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL }},
 340     { &hf_hsfz_ctrlword,
 341         { "Control Word", "hsfz.ctrlword", FT_UINT16, BASE_HEX, VALS(hsfz_ctrlwords), 0x0, NULL, HFILL }},
 342     { &hf_hsfz_source_address,
 343         { "Source Address", "hsfz.sourceaddr", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL }},
 344     { &hf_hsfz_target_address,
 345         { "Target Address", "hsfz.targetaddr", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL }},
 346     { &hf_hsfz_address,
 347         { "Address", "hsfz.address", FT_UINT8, BASE_HEX, NULL, 0x0, NULL, HFILL } },
 348     { &hf_hsfz_ident_string,
 349         { "Identification String", "hsfz.identification_string", FT_STRING, BASE_NONE, NULL, 0x0, NULL, HFILL } },
 350     { &hf_hsfz_data,
 351         { "Data", "hsfz.data", FT_BYTES, BASE_NONE, NULL, 0x0, NULL, HFILL } },
 352     };
 353
 354     /* entries in the protocol tree */
 355     static int *ett[] = {
 356         &ett_hsfz,
 357     };
 358
 359     /* UATs for user_data fields */
 360     static uat_field_t diag_addr_uat_fields[] = {
 361         UAT_FLD_HEX(udf_diag_addr, id, "Diagnostic Address", "Diagnostic Address of ECU (hex without leading 0x)"),
 362         UAT_FLD_CSTRING(udf_diag_addr, name, "ECU Name", "Name of ECU (string)"),
 363         UAT_END_FIELDS
 364     };
 365
 366     proto_hsfz = proto_register_protocol(HSFZ_NAME_LONG, HSFZ_NAME, HSFZ_NAME_FILTER);
 367     proto_register_field_array(proto_hsfz, hf, array_length(hf));
 368     proto_register_subtree_array(ett, array_length(ett));
 369
 370     /* Register preferences */
 371     hsfz_module = prefs_register_protocol(proto_hsfz, NULL);
 372
 373     prefs_register_bool_preference(hsfz_module, "header_check_heuristic", "Find start of HSFZ header by checking validity",
 374         "Should the HSFZ dissector check if a HSFZ header for validity (length and control word)?", &hsfz_check_header);
 375
 376     prefs_register_bool_preference(hsfz_module, "show_uds_in_ack", "Show UDS in HSFZ Ack",
 377         "Should the shortened UDS in the HSFZ be dissected?", &hsfz_show_uds_in_ack);
 378
 379     udf_diag_addr_uat = uat_new("Diagnostic Addresses",
 380         sizeof(udf_one_id_string_t),            /* record size           */
 381         "HSFZ_diagnostics_addresses",           /* filename              */
 382         true,                                   /* from_profile          */
 383         (void**)&udf_diag_addr,                 /* data_ptr              */
 384         &udf_diag_addr_num,                     /* numitems_ptr          */
 385         UAT_AFFECTS_DISSECTION,                 /* specifies addresses   */
 386         NULL,                                   /* help                  */
 387         udf_copy_one_id_string_cb,              /* copy callback         */
 388         udf_update_diag_addr_cb,                /* update callback       */
 389         udf_free_one_id_string_cb,              /* free callback         */
 390         udf_post_update_diag_addr_cb,           /* post update callback  */
 391         NULL,                                   /* reset callback        */
 392         diag_addr_uat_fields                    /* UAT field definitions */
 393     );
 394
 395     prefs_register_uat_preference(hsfz_module, "_udf_diag_addr", "Diagnostic Addresses",
 396         "A table to define names for diagnostic addresses", udf_diag_addr_uat);
 397 }
 398
 399 void proto_reg_handoff_hsfz(void) {
 400     hsfz_handle_tcp = register_dissector("hsfz_over_tcp", dissect_hsfz_tcp, proto_hsfz);
 401     hsfz_handle_udp = register_dissector("hsfz_over_udp", dissect_hsfz_udp, proto_hsfz);
 402
 403     dissector_add_uint_range_with_preference("tcp.port", "", hsfz_handle_tcp);
 404     dissector_add_uint_range_with_preference("udp.port", "", hsfz_handle_udp);
 405
 406     uds_handle = find_dissector("uds_over_hsfz");
 407 }
 408
 409 /*
 410  * Editor modelines  -  http://www.wireshark.org/tools/modelines.html
 411  *
 412  * Local variables:
 413  * c-basic-offset: 4
 414  * tab-width: 8
 415  * indent-tabs-mode: nil
 416  * End:
 417  *
 418  * vi: set shiftwidth=4 tabstop=8 expandtab:
 419  * :indentSize=4:tabSize=8:noTabs=true:
 420  */