/* packet-uavcan.c * Routines for dissection of UAVCAN v1 * * Peter van der Perk * NXP Semiconductors B.V. * Copyright 2020 * * Wireshark - Network traffic analyzer * By Gerald Combs * Copyright 1998 Gerald Combs * * SPDX-License-Identifier: GPL-2.0-or-later */ #include "config.h" #include #include #include #include #include #include #include #include #include "packet-socketcan.h" #define ANONYMOUS_FLAG 0x8000 #define BROADCAST_FLAG 0x4000 #define ADDR_MASK 0XFF #define START_OF_TRANSFER 0x80 #define END_OF_TRANSFER 0x40 #define TOGGLE 0x20 #define TRANSFER_ID 0x1F #define UAVCAN_SUBJECT_ID(can_id) ((can_id & 0x001FFF00) >> 8) #define UAVCAN_SERVICE_ID(can_id) ((can_id & 0x007FC000) >> 14) #define UAVCAN_DESTINATION_ID(can_id) ((can_id & 0x00003F80) >> 7) #define UAVCAN_SOURCE_ID(can_id) ((can_id & 0x0000007F)) #define UAVCAN_IS_SERVICE(can_id) ((can_id & 0x2000000) != 0) #define UAVCAN_IS_MESSAGE(can_id) ((can_id & 0x2000000) == 0) #define UAVCAN_IS_REQUEST(can_id) ((can_id & 0x01000000) != 0) #define UAVCAN_IS_RESPONSE(can_id) ((can_id & 0x01000000) == 0) #define UAVCAN_IS_ANONYMOUS(can_id) ((can_id & 0x01000000) != 0) typedef union { struct { guint8 serv_not_msg : 1; guint8 req_not_rsp : 1; guint16 service_id : 9; guint8 dest_id : 7; guint8 source_id : 7; guint8 transfer_id : 7; }uavcan_service; guint32 seq_service_id; }uavcan_service_lookup_id; typedef union { struct { guint8 serv_not_msg : 1; guint16 subject_id : 10; guint8 source_id : 7; guint8 transfer_id : 7; guint32 padding : 7; }uavcan_message; guint32 seq_message_id; }uavcan_message_lookup_id; struct uavcan_proto_data { guint32 seq_id; gboolean toggle_error; }; void proto_register_uavcan(void); void proto_reg_handoff_uavcan(void); static int proto_uavcan = -1; static int hf_uavcan_can_id = -1; static int hf_uavcan_priority = -1; static int hf_uavcan_anonymous = -1; static int hf_uavcan_req_not_rsp = -1; static int hf_uavcan_serv_not_msg = -1; static int hf_uavcan_subject_id = -1; static int hf_uavcan_service_id = -1; static int hf_uavcan_dst_addr = -1; static int hf_uavcan_src_addr = -1; static int hf_uavcan_data = -1; static int hf_uavcan_start_of_transfer = -1; static int hf_uavcan_end_of_transfer = -1; static int hf_uavcan_toggle = -1; static int hf_uavcan_transfer_id = -1; static int hf_heartbeat_uptime = -1; static int hf_heartbeat_health = -1; static int hf_heartbeat_mode = -1; static int hf_heartbeat_status_code = -1; static int hf_list_index = -1; static int hf_register_name = -1; static int hf_register_access_mutable = -1; static int hf_register_access_persistent = -1; static int hf_register_value_tag = -1; static int hf_register_value_size = -1; static int hf_node_id = -1; static int hf_pnp_unique_id = -1; static int hf_pnp_unique_id_hash = -1; static int hf_pnp_alloc = -1; static int hf_uavcan_primitive_Empty = -1; static int hf_uavcan_primitive_String = -1; static int hf_uavcan_primitive_Unstructured = -1; static int hf_uavcan_primitive_array_Bit = -1; static int hf_uavcan_primitive_array_Integer64 = -1; static int hf_uavcan_primitive_array_Integer32 = -1; static int hf_uavcan_primitive_array_Integer16 = -1; static int hf_uavcan_primitive_array_Integer8 = -1; static int hf_uavcan_primitive_array_Natural64 = -1; static int hf_uavcan_primitive_array_Natural32 = -1; static int hf_uavcan_primitive_array_Natural16 = -1; static int hf_uavcan_primitive_array_Natural8 = -1; static int hf_uavcan_primitive_array_Real64 = -1; static int hf_uavcan_primitive_array_Real32 = -1; static int hf_uavcan_primitive_array_Real16 = -1; static int hf_uavcan_time_syncronizedtimestamp = -1; static int uavcan_address_type = -1; static wmem_tree_t *fragment_info_table = NULL; static reassembly_table uavcan_reassembly_table; static int hf_uavcan_packet_fragment = -1; static int hf_uavcan_packet_complete = -1; static int hf_uavcan_packet_unknown_fragment = -1; static gint ett_uavcan = -1; static gint ett_uavcan_can = -1; static gint ett_uavcan_message = -1; static expert_field ei_uavcan_toggle_bit_error = EI_INIT; static gint ett_uavcan_fragment = -1; static gint ett_uavcan_fragments = -1; static int hf_uavcan_fragments = -1; static int hf_uavcan_fragment = -1; static int hf_uavcan_fragment_overlap = -1; static int hf_uavcan_fragment_overlap_conflicts = -1; static int hf_uavcan_fragment_multiple_tails = -1; static int hf_uavcan_fragment_too_long_fragment = -1; static int hf_uavcan_fragment_error = -1; static int hf_uavcan_fragment_count = -1; static int hf_uavcan_reassembled_in = -1; static int hf_uavcan_reassembled_length = -1; typedef struct _fragment_info_t { gint toggle; gint fragment_id; guint32 seq_id; } fragment_info_t; guint32 uavcan_seq_id = 0; static const fragment_items uavcan_frag_items = { /* Fragment subtrees */ &ett_uavcan_fragment, &ett_uavcan_fragments, /* Fragment fields */ &hf_uavcan_fragments, &hf_uavcan_fragment, &hf_uavcan_fragment_overlap, &hf_uavcan_fragment_overlap_conflicts, &hf_uavcan_fragment_multiple_tails, &hf_uavcan_fragment_too_long_fragment, &hf_uavcan_fragment_error, &hf_uavcan_fragment_count, /* Reassembled in field */ &hf_uavcan_reassembled_in, /* Reassembled length field */ &hf_uavcan_reassembled_length, /* Reassembled data field */ NULL, /* Tag */ "Message fragments" }; static const value_string uavcan_subject_id_vals[] = { { 7168,"Synchronization.1.0"}, { 7509,"Heartbeat.1.0"}, { 7510,"List.0.1"}, { 8165,"NodeIDAllocationData.2.0"}, { 8166,"NodeIDAllocationData.1.0"}, { 8184,"Record.1.X"}, { 0, NULL } }; static const value_string uavcan_service_id_vals[] = { { 384,"Access.1.0"}, { 385,"List.1.0"}, { 430,"GetInfo.1.0"}, { 434,"GetTransportStatistics.1.0"}, { 435,"ExecuteCommand.1.X"}, { 0, NULL } }; value_string_ext uavcan_subject_id_vals_ext = VALUE_STRING_EXT_INIT(uavcan_subject_id_vals); value_string_ext uavcan_service_id_vals_ext = VALUE_STRING_EXT_INIT(uavcan_service_id_vals); static void uavcan_subject_id(gchar *result, guint32 addr ) { if ((addr > 7167) && (addr < 8192)) g_snprintf(result, ITEM_LABEL_LENGTH, "%d (%s)", addr, val_to_str_ext_const(addr, &uavcan_subject_id_vals_ext, "Reserved")); else if ((addr > 0) && (addr < 6144)) g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Unregulated identifier)", addr); else g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Non-standard fixed regulated identifier)", addr); } static void uavcan_service_id(gchar *result, guint32 addr ) { if ((addr > 383) && (addr < 512)) g_snprintf(result, ITEM_LABEL_LENGTH, "%d (%s)", addr, val_to_str_ext_const(addr, &uavcan_service_id_vals_ext, "Reserved")); else if ((addr > 0) && (addr < 256)) g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Unregulated identifier)", addr); else g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Non-standard fixed regulated identifier)", addr); } static void uavcan_priority(gchar *result, guint32 addr ) { switch(addr){ case 0: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Exceptional)", addr); break; case 1: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Immediate)", addr); break; case 2: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Fast)", addr); break; case 3: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (High)", addr); break; case 4: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Nominal)", addr); break; case 5: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Low)", addr); break; case 6: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Slow)", addr); break; case 7: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Optional)", addr); break; } } static void uavcan_heartbeat_mode(gchar *result, guint32 addr ) { switch(addr){ case 0: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Operational)", addr); break; case 1: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Initialization)", addr); break; case 2: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Maintenance)", addr); break; case 3: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Software update)", addr); break; } } static void uavcan_heartbeat_health(gchar *result, guint32 addr ) { switch(addr){ case 0: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Nominal)", addr); break; case 1: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Advisory)", addr); break; case 2: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Caution)", addr); break; case 3: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Warning)", addr); break; } } static void uavcan_value_tag(gchar *result, guint32 addr ) { switch(addr){ case 0: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Empty)", addr); break; case 1: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (String)", addr); break; case 2: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Unstructured)", addr); break; case 3: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Bit array)", addr); break; case 4: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Integer 64 Array)", addr); break; case 5: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Integer 32 Array)", addr); break; case 6: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Integer 16 Array)", addr); break; case 7: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Integer 8 Array)", addr); break; case 8: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Natural 64 Array)", addr); break; case 9: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Natural 32 Array)", addr); break; case 10: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Natural 16 Array)", addr); break; case 11: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Natural 8 Array)", addr); break; case 12: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Real 64 Array)", addr); break; case 13: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Real 32 Array)", addr); break; case 14: g_snprintf(result, ITEM_LABEL_LENGTH, "%d (Real 16 Array)", addr); break; } } static void uavcan_nodeid_alloc(gchar *result, guint32 addr ) { if(addr == 0) { g_snprintf(result, ITEM_LABEL_LENGTH, "request message"); } else { g_snprintf(result, ITEM_LABEL_LENGTH, "response message"); } } static void dissect_list_service_data(tvbuff_t *tvb, int tvb_offset, proto_tree *tree, struct can_info can_info) { proto_tree *msg_tree; proto_item *ti; if(UAVCAN_IS_REQUEST(can_info.id)){ proto_item_append_text(tree, " (%s)", "List1.0"); proto_tree_add_item(tree, hf_list_index, tvb, tvb_offset, 2, ENC_LITTLE_ENDIAN); } else { msg_tree = proto_tree_add_subtree(tree, tvb, tvb_offset, tvb_reported_length(tvb), ett_uavcan_message, NULL, "Service response"); proto_item_append_text(msg_tree, " (%s)", "List.1.0"); guint8 str_len = tvb_get_guint8(tvb, tvb_offset); ti = proto_tree_add_item(msg_tree, hf_register_name, tvb, tvb_offset+1, str_len, ENC_NA); proto_item_set_generated(ti); } } static void dissect_access_service_data(tvbuff_t *tvb, int tvb_offset, proto_tree *tree, struct can_info can_info) { guint8 str_len; proto_item *ti; proto_tree *msg_tree; gint offset; msg_tree = proto_tree_add_subtree(tree, tvb, tvb_offset, tvb_reported_length(tvb), ett_uavcan_message, NULL, "Service"); offset = tvb_offset; if(UAVCAN_IS_REQUEST(can_info.id)) { proto_item_append_text(msg_tree, " %s", "request"); str_len = tvb_get_guint8(tvb, offset); offset += 1; ti = proto_tree_add_item(msg_tree, hf_register_name, tvb, offset, str_len, ENC_NA); proto_item_set_generated(ti); offset += str_len; } else { proto_item_append_text(msg_tree, " %s", "response"); ti = proto_tree_add_item(msg_tree, hf_uavcan_time_syncronizedtimestamp, tvb, offset, 7, ENC_LITTLE_ENDIAN); proto_item_set_generated(ti); offset += 7; ti = proto_tree_add_item(msg_tree, hf_register_access_mutable, tvb, offset, 1, ENC_NA); proto_item_set_generated(ti); ti = proto_tree_add_item(msg_tree, hf_register_access_persistent, tvb, offset, 1, ENC_NA); proto_item_set_generated(ti); offset += 1; } proto_item_append_text(msg_tree, " (%s)", "Access.1.0"); guint8 tag = tvb_get_guint8(tvb, offset); ti = proto_tree_add_item(msg_tree, hf_register_value_tag, tvb, offset, 1, ENC_NA); proto_item_set_generated(ti); offset += 1; if(tag == 1) { /* String */ ti = proto_tree_add_item(msg_tree, hf_register_value_size, tvb, offset, 1, ENC_NA); proto_item_set_generated(ti); str_len = tvb_get_guint8(tvb, offset); offset += 1; ti = proto_tree_add_item(msg_tree, hf_register_name, tvb, offset, str_len, ENC_NA); proto_item_set_generated(ti); } else if(tag == 2 || tag == 3) { ti = proto_tree_add_item(msg_tree, hf_uavcan_data, tvb, 0, tvb_captured_length(tvb), ENC_NA); proto_item_set_generated(ti); } else { guint8 array_len = tvb_get_guint8(tvb, offset); if( array_len == 0 || tag == 0) { ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_Empty, tvb, 0, 0, ENC_LITTLE_ENDIAN); } else { ti = proto_tree_add_item(msg_tree, hf_register_value_size, tvb, offset, 1, ENC_NA); proto_item_set_generated(ti); offset += 1; for (guint8 i = 0; i < array_len; i++) { switch(tag) { case 4: /*Integer64*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Integer64, tvb, offset, 8, ENC_LITTLE_ENDIAN); offset += 8; break; case 5: /*Integer32*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Integer32, tvb, offset, 4, ENC_LITTLE_ENDIAN); offset += 4; break; case 6: /*Integer16*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Integer16, tvb, offset, 2, ENC_LITTLE_ENDIAN); offset += 2; break; case 7: /*Integer8*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Integer8, tvb, offset, 1, ENC_LITTLE_ENDIAN); offset += 1; break; case 8: /*Natural64*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Natural64, tvb, offset, 2, ENC_LITTLE_ENDIAN); offset += 8; break; case 9: /*Natural32*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Natural32, tvb, offset, 2, ENC_LITTLE_ENDIAN); offset += 4; break; case 10: /*Natural16*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Natural16, tvb, offset, 2, ENC_LITTLE_ENDIAN); offset += 2; break; case 11: /*Natural8*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Natural8, tvb, offset, 2, ENC_LITTLE_ENDIAN); offset += 1; break; case 12: /*Real64*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Real64, tvb, offset, 8, ENC_LITTLE_ENDIAN); offset += 8; break; case 13: /*Real16*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Real32, tvb, offset, 8, ENC_LITTLE_ENDIAN); offset += 8; break; case 14: /*Real8*/ ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_array_Real16, tvb, offset, 8, ENC_LITTLE_ENDIAN); offset += 8; break; default: ti = proto_tree_add_item(msg_tree, hf_uavcan_primitive_Empty, tvb, 0, 0, ENC_LITTLE_ENDIAN); } proto_item_set_generated(ti); } } } } static int dissect_uavcan(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void* data) { proto_item *ti, *can_id_item, *toggle; proto_tree *uavcan_tree, *can_tree, *msg_tree; gint offset = 0; struct can_info can_info; guint16 *src_addr, *dest_addr; guint8 tail_byte; DISSECTOR_ASSERT(data); can_info = *((struct can_info*)data); tail_byte = tvb_get_guint8(tvb, tvb_captured_length(tvb)-1); if ((can_info.id & CAN_ERR_FLAG) || !(can_info.id & CAN_EFF_FLAG)) { /* Error frames and frames with standards ids are not for us */ return 0; } col_set_str(pinfo->cinfo, COL_PROTOCOL, "UAVCAN"); col_clear(pinfo->cinfo, COL_INFO); ti = proto_tree_add_item(tree, proto_uavcan, tvb, offset, tvb_reported_length(tvb), ENC_NA); uavcan_tree = proto_item_add_subtree(ti, ett_uavcan); can_tree = proto_tree_add_subtree_format(uavcan_tree, tvb, 0, 0, ett_uavcan_can, NULL, "CAN ID field: 0x%08x", can_info.id); can_id_item = proto_tree_add_uint(can_tree, hf_uavcan_can_id, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(can_id_item); /* Dissect UAVCAN v1 Message frame */ if (UAVCAN_IS_MESSAGE(can_info.id)) { ti = proto_tree_add_uint(can_tree, hf_uavcan_priority, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_serv_not_msg, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_anonymous, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_subject_id, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_src_addr, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); /* Set source address */ src_addr = (guint16*)wmem_alloc(pinfo->pool, 2); *src_addr = (guint16)UAVCAN_SOURCE_ID(can_info.id); if(UAVCAN_IS_ANONYMOUS(can_info.id)){ *src_addr |= ANONYMOUS_FLAG; } set_address(&pinfo->src, uavcan_address_type, 2, (const void*)src_addr); /* Fill in "destination" address even if its "broadcast" */ dest_addr = (guint16*)wmem_alloc(pinfo->pool, 2); *dest_addr = BROADCAST_FLAG; set_address(&pinfo->dst, uavcan_address_type, 2, (const void*)dest_addr); gchar col_info_string[50]; uavcan_subject_id(&col_info_string[0], UAVCAN_SUBJECT_ID(can_info.id) ); col_add_fstr(pinfo->cinfo, COL_INFO, "Message: %s", col_info_string); } else { /* UAVCAN v1 Service frame */ ti = proto_tree_add_uint(can_tree, hf_uavcan_priority, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_serv_not_msg, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_req_not_rsp, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_service_id, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_dst_addr, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); ti = proto_tree_add_uint(can_tree, hf_uavcan_src_addr, tvb, -8, sizeof(can_info.id), can_info.id); proto_item_set_generated(ti); /* Set source address */ src_addr = (guint16*)wmem_alloc(pinfo->pool, 2); *src_addr = (guint16)UAVCAN_SOURCE_ID(can_info.id); set_address(&pinfo->src, uavcan_address_type, 2, (const void*)src_addr); dest_addr = (guint16*)wmem_alloc(pinfo->pool, 2); *dest_addr = (guint16)UAVCAN_DESTINATION_ID(can_info.id); set_address(&pinfo->dst, uavcan_address_type, 2, (const void*)dest_addr); gchar col_info_string[50]; uavcan_service_id(&col_info_string[0], UAVCAN_SERVICE_ID(can_info.id) ); if((can_info.id & 0x01000000) == 0 ) { col_add_fstr(pinfo->cinfo, COL_INFO, "Service response: %s", col_info_string); } else { col_add_fstr(pinfo->cinfo, COL_INFO, "Service request: %s", col_info_string); } } msg_tree = proto_tree_add_subtree(uavcan_tree, tvb, 0, tvb_reported_length(tvb), ett_uavcan_message, NULL, "Message"); proto_tree_add_item(msg_tree, hf_uavcan_start_of_transfer, tvb, tvb_captured_length(tvb)-1, 1, ENC_NA); proto_tree_add_item(msg_tree, hf_uavcan_end_of_transfer, tvb, tvb_captured_length(tvb)-1, 1, ENC_NA); toggle = proto_tree_add_item(msg_tree, hf_uavcan_toggle, tvb, tvb_captured_length(tvb)-1, 1, ENC_NA); proto_tree_add_item(msg_tree, hf_uavcan_transfer_id, tvb, tvb_captured_length(tvb)-1, 1, ENC_NA); if((tail_byte & (START_OF_TRANSFER | END_OF_TRANSFER)) == (START_OF_TRANSFER | END_OF_TRANSFER)) { /* Single frame */ if (UAVCAN_IS_MESSAGE(can_info.id) && UAVCAN_SUBJECT_ID(can_info.id) == 7509 ) { /* Dissect UAVCAN v1 Heartbeat 1.0 frame */ proto_item_append_text(msg_tree, " (%s)", "Heartbeat1.0"); proto_tree_add_item(msg_tree, hf_heartbeat_uptime, tvb, 0, 4, ENC_LITTLE_ENDIAN); proto_tree_add_item(msg_tree, hf_heartbeat_health, tvb, 4, 1, ENC_NA); proto_tree_add_item(msg_tree, hf_heartbeat_mode, tvb, 5, 1, ENC_NA); proto_tree_add_item(msg_tree, hf_heartbeat_status_code, tvb, 6, 1, ENC_NA); } else if (UAVCAN_IS_SERVICE(can_info.id) && UAVCAN_SERVICE_ID(can_info.id) == 384) { /* Dissect UAVCAN v1 Access 1.0 frame */ dissect_access_service_data(tvb, 0, msg_tree, can_info); } else if (UAVCAN_IS_SERVICE(can_info.id) && UAVCAN_SERVICE_ID(can_info.id) == 385) { /* Dissect UAVCAN v1 List 1.0 frame */ dissect_list_service_data(tvb, 0, msg_tree, can_info); } else if (UAVCAN_IS_MESSAGE(can_info.id) && UAVCAN_SUBJECT_ID(can_info.id) == 8166) { /* Dissect UAVCAN v1 NodeIDAllocationData1.0 allocation request */ proto_item_append_text(msg_tree, " (%s)", "NodeIDAllocationData.1.0"); proto_tree_add_item(msg_tree, hf_pnp_unique_id_hash, tvb, 0, 6, ENC_NA); proto_tree_add_item(msg_tree, hf_pnp_alloc, tvb, 6, 1, ENC_NA); } else if (UAVCAN_IS_MESSAGE(can_info.id) && UAVCAN_SUBJECT_ID(can_info.id) == 8165) { /* Dissect UAVCAN v1 NodeIDAllocationData2.0 allocation request/response */ proto_item_append_text(msg_tree, " (%s)", "NodeIDAllocationData.2.0"); proto_tree_add_item(msg_tree, hf_node_id, tvb, 0, 2, ENC_LITTLE_ENDIAN); proto_tree_add_item(msg_tree, hf_pnp_unique_id, tvb, 2, 16, ENC_NA); proto_tree_add_uint(msg_tree, hf_pnp_alloc, tvb, -8, sizeof(can_info.id), !UAVCAN_IS_ANONYMOUS(can_info.id)); } else { /* UAVCAN v1 frame payload */ proto_tree_add_item(msg_tree, hf_uavcan_data, tvb, 0, tvb_captured_length(tvb)-1, ENC_NA); } } else { /* UAVCAN v1 frame payload */ proto_tree_add_item(msg_tree, hf_uavcan_data, tvb, 0, tvb_captured_length(tvb)-1, ENC_NA); } /* Re-assembly attempt */ if((tail_byte & (START_OF_TRANSFER | END_OF_TRANSFER)) != (START_OF_TRANSFER | END_OF_TRANSFER)) { /* Multi-frame */ struct uavcan_proto_data *uavcan_frame_data; if (!pinfo->fd->visited) { /* Not visited */ guint32 lookup_id = 0; /* Semi-unique id for lookup table note transfer-ID rolls-over every 32 times */ fragment_info_t *fragment_info ; if (UAVCAN_IS_MESSAGE(can_info.id)) { /* Message */ uavcan_message_lookup_id *id = (uavcan_message_lookup_id*)&lookup_id; id->uavcan_message.serv_not_msg = 0; id->uavcan_message.subject_id = UAVCAN_SUBJECT_ID(can_info.id); id->uavcan_message.source_id = UAVCAN_SOURCE_ID(can_info.id); id->uavcan_message.transfer_id = (tail_byte & TRANSFER_ID); id->uavcan_message.padding = 0; } else { /* Service */ uavcan_service_lookup_id *id = (uavcan_service_lookup_id*)&lookup_id; id->uavcan_service.serv_not_msg = 1; id->uavcan_service.req_not_rsp = ((can_info.id & 0x01000000) >> 24); id->uavcan_service.service_id = UAVCAN_SERVICE_ID(can_info.id); id->uavcan_service.dest_id = UAVCAN_DESTINATION_ID(can_info.id); id->uavcan_service.source_id = UAVCAN_SOURCE_ID(can_info.id); id->uavcan_service.transfer_id = (tail_byte & TRANSFER_ID); } fragment_info = (fragment_info_t *) wmem_tree_lookup32(fragment_info_table, lookup_id); /* Check lookup table */ if (fragment_info == NULL) { /* Doesn't exist, allocate lookup_id */ fragment_info = (fragment_info_t *) wmem_new(wmem_file_scope(), fragment_info_t); fragment_info->fragment_id = 0; fragment_info->toggle = tail_byte & TOGGLE; wmem_tree_insert32(fragment_info_table, lookup_id, fragment_info); } /* Store sequence number and status in pinfo so we can revisit later */ uavcan_frame_data = (struct uavcan_proto_data *)wmem_alloc0(wmem_file_scope(), sizeof(struct uavcan_proto_data)); p_add_proto_data(wmem_file_scope(), pinfo, proto_uavcan, 0, uavcan_frame_data); if((tail_byte & START_OF_TRANSFER) != 0){ /* Start of transfer */ uavcan_frame_data->toggle_error = 0; fragment_info->fragment_id = 0; fragment_info->seq_id = uavcan_seq_id; uavcan_seq_id += 1; } else { /* Update transfer */ fragment_info->fragment_id += 1; uavcan_frame_data->toggle_error = ((tail_byte & TOGGLE) == fragment_info->toggle) ? TRUE : FALSE; } uavcan_frame_data->seq_id = fragment_info->seq_id; fragment_info->toggle = tail_byte & TOGGLE; pinfo->fragmented = TRUE; fragment_add_seq_check(&uavcan_reassembly_table, tvb, offset, pinfo, fragment_info->seq_id, NULL, /* ID for fragments belonging together */ fragment_info->fragment_id, /* fragment sequence number */ tvb_captured_length_remaining(tvb, offset)-1, /* fragment length - minus tail byte */ ((tail_byte & END_OF_TRANSFER) == 0) ? TRUE : FALSE); /* More fragments? */ } else { /* Visited get reassembled data */ fragment_head *reassembled = NULL; uavcan_frame_data = (struct uavcan_proto_data*)p_get_proto_data(wmem_file_scope(), pinfo, proto_uavcan, 0); reassembled = fragment_get_reassembled_id(&uavcan_reassembly_table, pinfo, uavcan_frame_data->seq_id); if(uavcan_frame_data->toggle_error == 1){ expert_add_info_format(pinfo, toggle, &ei_uavcan_toggle_bit_error, "Expected Toggle %u got %u.", !((tail_byte & TOGGLE) != 0) , ((tail_byte & TOGGLE) != 0)); } col_append_str(pinfo->cinfo, COL_INFO, " (Multi-frame)"); process_reassembled_data(tvb, offset, pinfo, "Reassembled Message", reassembled, &uavcan_frag_items, NULL, uavcan_tree); /* Parsing reassembled data */ if((tail_byte & END_OF_TRANSFER) != 0){ /* List.1.0 Service response */ if (UAVCAN_IS_SERVICE(can_info.id) && UAVCAN_IS_RESPONSE(can_info.id) && UAVCAN_SERVICE_ID(can_info.id) == 385 ) { tvbuff_t *reassembled_tvb = tvb_new_chain(tvb, reassembled->tvb_data); /* Reassembled tvb chain */ dissect_list_service_data(reassembled_tvb, 0, uavcan_tree, can_info); } else if (UAVCAN_IS_MESSAGE(can_info.id) && UAVCAN_SUBJECT_ID(can_info.id) == 8166 ) { /* NodeIDAllocationData.1.0 Service */ tvbuff_t *reassembled_tvb = tvb_new_chain(tvb, reassembled->tvb_data); /* Reassembled tvb chain */ msg_tree = proto_tree_add_subtree(uavcan_tree, reassembled_tvb, 0, tvb_reported_length(reassembled_tvb), ett_uavcan_message, NULL, "Message"); proto_item_append_text(msg_tree, " (%s)", "NodeIDAllocationData.1.0"); proto_tree_add_item(msg_tree, hf_pnp_unique_id_hash, reassembled_tvb, 0, 6, ENC_NA); proto_tree_add_item(msg_tree, hf_pnp_alloc, reassembled_tvb, 6, 1, ENC_NA); proto_tree_add_item(msg_tree, hf_node_id, reassembled_tvb, 7, 2, ENC_LITTLE_ENDIAN); } else if (UAVCAN_IS_SERVICE(can_info.id) && UAVCAN_SERVICE_ID(can_info.id) == 384 ) { /* Access.1.0 Service */ tvbuff_t *reassembled_tvb = tvb_new_chain(tvb, reassembled->tvb_data); /* Reassembled tvb chain */ dissect_access_service_data(reassembled_tvb, 0, uavcan_tree, can_info); } } } } return tvb_captured_length(tvb); } static int UAVCAN_addr_to_str(const address* addr, gchar *buf, int buf_len) { const guint16 *addrdata = (const guint16 *)addr->data; if((*addrdata & ANONYMOUS_FLAG) != 0) { return (int)g_snprintf(buf, buf_len, "Anonymous"); } else if((*addrdata & BROADCAST_FLAG) != 0) { return (int)g_snprintf(buf, buf_len, "Broadcast"); } else { guint8 real_addr = (guint8)(*addrdata & ADDR_MASK); guint32_to_str_buf(real_addr, buf, buf_len); return (int)strlen(buf); } } static int UAVCAN_addr_str_len(const address* addr _U_) { return 12; /* Leaves required space (10 bytes) for uint_to_str_back() */ } static const char* UAVCAN_col_filter_str(const address* addr _U_, gboolean is_src) { if (is_src) return "uavcan.src_addr"; return "uavcan.dst_addr"; } static int UAVCAN_addr_len(void) { return 2; } void proto_register_uavcan(void) { static hf_register_info hf[] = { { &hf_uavcan_can_id, {"CAN Identifier", "uavcan.can_id", FT_UINT32, BASE_HEX, NULL, CAN_EFF_MASK, NULL, HFILL } }, { &hf_uavcan_priority, {"Priority", "uavcan.priority", FT_UINT32, BASE_CUSTOM, CF_FUNC(uavcan_priority), 0x1C000000, NULL, HFILL } }, { &hf_uavcan_serv_not_msg, {"Service, not message", "uavcan.serv_not_msg", FT_UINT32, BASE_DEC, NULL, 0x02000000, NULL, HFILL } }, { &hf_uavcan_anonymous, {"Anonymous", "uavcan.anonymous", FT_UINT32, BASE_DEC, NULL, 0x01000000, NULL, HFILL } }, { &hf_uavcan_req_not_rsp, {"Request, not response", "uavcan.req_not_rsp", FT_UINT32, BASE_DEC, NULL, 0x01000000, NULL, HFILL } }, { &hf_uavcan_subject_id, {"Subject ID", "uavcan.subject_id", FT_UINT32, BASE_CUSTOM, CF_FUNC(uavcan_subject_id), 0x001FFF00, NULL, HFILL } }, { &hf_uavcan_service_id, {"Service ID", "uavcan.service_id", FT_UINT32, BASE_CUSTOM, CF_FUNC(uavcan_service_id), 0x007FC000, NULL, HFILL } }, { &hf_uavcan_dst_addr, {"Destination node-ID", "uavcan.dst_addr", FT_UINT32, BASE_DEC, NULL, 0x00003F80, NULL, HFILL } }, { &hf_uavcan_src_addr, {"Source node-ID", "uavcan.src_addr", FT_UINT32, BASE_DEC, NULL, 0x0000007F, NULL, HFILL } }, { &hf_uavcan_data, {"Payload", "uavcan.payload", FT_BYTES, BASE_NONE|BASE_ALLOW_ZERO, NULL, 0x0, NULL, HFILL } }, { &hf_uavcan_start_of_transfer, {"Start of transfer", "uavcan.start_of_transfer", FT_UINT8, BASE_DEC, NULL, START_OF_TRANSFER, NULL, HFILL} }, { &hf_uavcan_end_of_transfer, {"End of transfer", "uavcan.end_of_transfer", FT_UINT8, BASE_DEC, NULL, END_OF_TRANSFER, NULL, HFILL} }, { &hf_uavcan_toggle, {"Toggle", "uavcan.toggle", FT_UINT8, BASE_DEC, NULL, TOGGLE, NULL, HFILL} }, { &hf_uavcan_transfer_id, {"Transfer-ID", "uavcan.transfer_id", FT_UINT8, BASE_DEC, NULL, TRANSFER_ID, NULL, HFILL} }, // PNP { &hf_node_id, {"Node ID", "uavcan.node.id", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_pnp_unique_id, {"Unique ID", "uavcan.pnp.unique_id", FT_BYTES, BASE_NONE|BASE_ALLOW_ZERO, NULL, 0x0, NULL, HFILL} }, { &hf_pnp_unique_id_hash, {"Unique ID hash", "uavcan.pnp.unique_id_hash", FT_BYTES, BASE_NONE|BASE_ALLOW_ZERO, NULL, 0x0, NULL, HFILL} }, { &hf_pnp_alloc, {"allocation type", "uavcan.pnp.allocation", FT_UINT8, BASE_CUSTOM, CF_FUNC(uavcan_nodeid_alloc), 0x0, NULL, HFILL} }, // Heartbeat 1.0 { &hf_heartbeat_uptime, {"Uptime", "uavcan.Heartbeat.uptime", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_heartbeat_health, {"Health", "uavcan.Heartbeat.health", FT_UINT8, BASE_CUSTOM, CF_FUNC(uavcan_heartbeat_health), 0x0, NULL, HFILL} }, { &hf_heartbeat_mode, {"Mode", "uavcan.Heartbeat.mode", FT_UINT8, BASE_CUSTOM, CF_FUNC(uavcan_heartbeat_mode), 0x0, NULL, HFILL} }, { &hf_heartbeat_status_code, {"Vendor specific status code", "uavcan.Heartbeat.vendor_specific_status_code", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_time_syncronizedtimestamp, {"Timestamp (usec)", "uavcan.time.SynchronizedTimestamp", FT_UINT56, BASE_DEC, NULL, 0x0, NULL, HFILL} }, // List1.0 Request { &hf_list_index, {"Index", "uavcan.register.List.index", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_register_name, {"Name", "uavcan.register.Name", FT_STRING, BASE_NONE, NULL, 0x0, NULL, HFILL} }, // Access1.0 Value1.0 { &hf_register_access_mutable, {"Mutable", "uavcan.register.Access.mutable", FT_UINT8, BASE_DEC, NULL, 0x1, NULL, HFILL } }, { &hf_register_access_persistent, {"Persistent", "uavcan.register.Access.persistent", FT_UINT8, BASE_DEC, NULL, 0x2, NULL, HFILL } }, { &hf_register_value_tag, {"Tag", "uavcan.register.Value.tag", FT_UINT8, BASE_CUSTOM, CF_FUNC(uavcan_value_tag), 0x0, NULL, HFILL} }, { &hf_register_value_size, {"Array size", "uavcan.primitive.array.size", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_Empty, {"Empty", "uavcan.primitive.Empty", FT_NONE, BASE_NONE, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_String, {"String", "uavcan.primitive.String", FT_STRING, BASE_NONE, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_Unstructured, {"Unstructured", "uavcan.primitive.array.Unstructured", FT_BYTES, BASE_NONE|BASE_ALLOW_ZERO, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Bit, {"Bit", "uavcan.primitive.array.Bit", FT_BYTES, BASE_NONE|BASE_ALLOW_ZERO, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Integer64, {"Integer64", "uavcan.primitive.array.Integer64", FT_INT64, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Integer32, {"Integer32", "uavcan.primitive.array.Integer32", FT_INT32, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Integer16, {"Integer16", "uavcan.primitive.array.Integer16", FT_INT16, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Integer8, {"Integer8", "uavcan.primitive.array.Integer8", FT_INT8, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Natural64, {"Natural64", "uavcan.primitive.array.Natural64", FT_UINT64, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Natural32, {"Natural32", "uavcan.primitive.array.Natural32", FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Natural16, {"Natural16", "uavcan.primitive.array.Natural16", FT_UINT16, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Natural8, {"Natural8", "uavcan.primitive.array.Natural8", FT_UINT8, BASE_DEC, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Real64, {"Real64", "uavcan.primitive.array.Real64", FT_DOUBLE, BASE_NONE, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Real32, {"Real32", "uavcan.primitive.array.Real32", FT_FLOAT, BASE_NONE, NULL, 0x0, NULL, HFILL} }, { &hf_uavcan_primitive_array_Real16, {"Real16", "uavcan.primitive.array.Real16", FT_IEEE_11073_SFLOAT, BASE_NONE, NULL, 0x0, NULL, HFILL} /* TODO not sure check */ }, { &hf_uavcan_fragments, { "Message fragments", "uavcan.fragments", FT_NONE, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment, { "Message fragment", "uavcan.fragment", FT_FRAMENUM, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_overlap, { "Message fragment overlap", "uavcan.fragment.overlap", FT_BOOLEAN, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_overlap_conflicts, { "Message fragment overlapping with conflicting data", "uavcan.fragment.overlap.conflicts", FT_BOOLEAN, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_multiple_tails, { "Message has multiple tail fragments", "uavcan.fragment.multiple_tails", FT_BOOLEAN, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_too_long_fragment, { "Message fragment too long", "uavcan.fragment.too_long_fragment", FT_BOOLEAN, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_error, { "Message defragmentation error", "uavcan.fragment.error", FT_FRAMENUM, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_fragment_count, { "Message fragment count", "uavcan.fragment.count", FT_UINT32, BASE_DEC, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_reassembled_in, { "Reassembled in", "uavcan.reassembled.in", FT_FRAMENUM, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_reassembled_length, { "Reassembled payload length", "uavcan.reassembled.length", FT_UINT32, BASE_DEC, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_packet_fragment, { "Packet Fragment", "uavcan.fragment", FT_NONE, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_packet_complete, { "Packet Complete", "uavcan.complete", FT_NONE, BASE_NONE, NULL, 0x00, NULL, HFILL } }, { &hf_uavcan_packet_unknown_fragment, { "Unknown Packet Fragment", "hci_usb.packet.unknown_fragment", FT_NONE, BASE_NONE, NULL, 0x00, NULL, HFILL } }, }; static gint *ett[] = { &ett_uavcan, &ett_uavcan_can, &ett_uavcan_message, &ett_uavcan_fragment, &ett_uavcan_fragments }; reassembly_table_register(&uavcan_reassembly_table, &addresses_reassembly_table_functions); fragment_info_table = wmem_tree_new_autoreset(wmem_epan_scope(), wmem_file_scope()); expert_module_t* expert_uavcan; proto_uavcan = proto_register_protocol("UAVCAN v1", "UAVCAN", "uavcan"); static ei_register_info ei[] = { { &ei_uavcan_toggle_bit_error, { "uavcan.toggle_bit.error", PI_MALFORMED, PI_ERROR, "Toggle bit error", EXPFILL }} }; proto_register_field_array(proto_uavcan, hf, array_length(hf)); proto_register_subtree_array(ett, array_length(ett)); expert_uavcan = expert_register_protocol(proto_uavcan); expert_register_field_array(expert_uavcan, ei, array_length(ei)); uavcan_address_type = address_type_dissector_register("AT_UAVCAN", "UAVCAN Address", UAVCAN_addr_to_str, UAVCAN_addr_str_len, NULL, UAVCAN_col_filter_str, UAVCAN_addr_len, NULL, NULL); } void proto_reg_handoff_uavcan(void) { dissector_handle_t uavcan_handle; uavcan_handle = create_dissector_handle( dissect_uavcan, proto_uavcan ); dissector_add_for_decode_as("can.subdissector", uavcan_handle ); } /* * Editor modelines - https://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: */