From 04c22e5a2bc0b983e241f9d5b8362f33df3d53bf Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 18 Apr 2018 10:50:00 -0700 Subject: [PATCH] Add more detailed AvatarData packet support to Wireshark dissector plugin. --- tools/hfudt.lua | 298 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 294 insertions(+), 4 deletions(-) diff --git a/tools/hfudt.lua b/tools/hfudt.lua index 97fade9c0e..f45315a3de 100644 --- a/tools/hfudt.lua +++ b/tools/hfudt.lua @@ -19,6 +19,7 @@ local f_message_part_number = ProtoField.uint32("hfudt.message_part_number", "Me local f_type = ProtoField.uint8("hfudt.type", "Type", base.DEC) local f_version = ProtoField.uint8("hfudt.version", "Version", base.DEC) local f_type_text = ProtoField.string("hfudt.type_text", "TypeText") +local f_sender_id = ProtoField.guid("hfudt.sender_id", "Sender ID", base.DEC) -- create the fields for control packets in HFUDT local f_control_type = ProtoField.uint16("hfudt.control_type", "Control Type", base.DEC) @@ -28,11 +29,29 @@ local f_control_sub_sequence = ProtoField.uint32("hfudt.control_sub_sequence", " local f_nak_sequence_number = ProtoField.uint32("hfudt.nak_sequence_number", "NAKed Sequence Number", base.DEC) local f_nak_range_end = ProtoField.uint32("hfudt.nak_range_end", "NAK Range End", base.DEC) +-- avatar data fields +local f_avatar_data_id = ProtoField.guid("hfudt.avatar_id", "Avatar ID", base.DEC) +local f_avatar_data_has_flags = ProtoField.string("hfudt.avatar_has_flags", "Has Flags") +local f_avatar_data_position = ProtoField.string("hfudt.avatar_data_position", "Position") +local f_avatar_data_dimensions = ProtoField.string("hfudt.avatar_data_dimensions", "Dimensions") +local f_avatar_data_offset = ProtoField.string("hfudt.avatar_data_offset", "Offset") +local f_avatar_data_look_at_position = ProtoField.string("hfudt.avatar_data_look_at_position", "Look At Position") +local f_avatar_data_audio_loudness = ProtoField.string("hfudt.avatar_data_audio_loudness", "Audio Loudness") +local f_avatar_data_additional_flags = ProtoField.string("hfudt.avatar_data_additional_flags", "Additional Flags") +local f_avatar_data_parent_id = ProtoField.guid("hfudt.avatar_data_parent_id", "Parent ID") +local f_avatar_data_parent_joint_index = ProtoField.string("hfudt.avatar_data_parent_joint_index", "Parent Joint Index") +local f_avatar_data_local_position = ProtoField.string("hfudt.avatar_data_local_position", "Local Position") +local f_avatar_data_valid_rotations = ProtoField.string("hfudt.avatar_data_valid_rotations", "Valid Rotations") +local f_avatar_data_valid_translations = ProtoField.string("hfudt.avatar_data_valid_translations", "Valid Translations") +local f_avatar_data_default_rotations = ProtoField.string("hfudt.avatar_data_default_rotations", "Valid Default") +local f_avatar_data_default_translations = ProtoField.string("hfudt.avatar_data_default_translations", "Valid Default") + local SEQUENCE_NUMBER_MASK = 0x07FFFFFF p_hfudt.fields = { f_length, f_control_bit, f_reliable_bit, f_message_bit, f_sequence_number, f_type, f_type_text, f_version, + f_sender_id, f_avatar_data_id, f_avatar_data_parent_id, f_message_position, f_message_number, f_message_part_number, f_obfuscation_level, f_control_type, f_control_type_text, f_control_sub_sequence, f_ack_sequence_number, f_nak_sequence_number, f_nak_range_end, f_data @@ -261,16 +280,287 @@ function p_hfudt.dissector (buf, pinfo, root) -- read the version subtree:add(f_version, buf(payload_offset + 1, 1):le_uint()) - data_length = buf:len() - (payload_offset + 2) + -- read node GUID + local sender_id = buf(payload_offset + 2, 16) + subtree:add(f_sender_id, sender_id) - -- pull the data that is the rest of the packet - subtree:add(f_data, buf(payload_offset + 2, data_length)) + local i = payload_offset + 18 + + -- skip MD6 checksum + 16 bytes + i = i + 16 + + -- AvatarData or BulkAvatarDataPacket + if packet_type == 6 or packet_type == 11 then + + local avatar_data + if packet_type == 6 then + -- AvatarData packet + + -- avatar_id is same as sender_id + subtree:add(f_avatar_data_id, sender_id) + + -- uint16 sequence_number + local sequence_number = buf(i, 2):le_uint() + i = i + 2 + + local avatar_data_packet_len = buf:len() - i + avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len)) + i = i + avatar_data_packet_len + + add_avatar_data_subtrees(avatar_data) + + else + -- BulkAvatarData packet + while i < buf:len() do + -- avatar_id is first 16 bytes + subtree:add(f_avatar_data_id, buf(i, 16)) + i = i + 16 + + local avatar_data_packet_len = buf:len() - i + avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len)) + i = i + avatar_data_packet_len + + add_avatar_data_subtrees(avatar_data) + end + end + end end -- return the size of the header return buf:len() - end +end + +function add_avatar_data_subtrees(avatar_data) + if avatar_data["has_flags"] then + subtree:add(f_avatar_data_has_flags, avatar_data["has_flags"]) + end + if avatar_data["position"] then + subtree:add(f_avatar_data_position, avatar_data["position"]) + end + if avatar_data["dimensions"] then + subtree:add(f_avatar_data_dimensions, avatar_data["dimensions"]) + end + if avatar_data["offset"] then + subtree:add(f_avatar_data_offset, avatar_data["offset"]) + end + if avatar_data["look_at_position"] then + subtree:add(f_avatar_data_look_at_position, avatar_data["look_at_position"]) + end + if avatar_data["audio_loudness"] then + subtree:add(f_avatar_data_audio_loudness, avatar_data["audio_loudness"]) + end + if avatar_data["additional_flags"] then + subtree:add(f_avatar_data_additional_flags, avatar_data["additional_flags"]) + end + if avatar_data["parent_id"] then + subtree:add(f_avatar_data_parent_id, avatar_data["parent_id"]) + end + if avatar_data["parent_joint_index"] then + subtree:add(f_avatar_data_parent_joint_index, avatar_data["parent_joint_index"]) + end + if avatar_data["local_position"] then + subtree:add(f_avatar_data_local_position, avatar_data["local_position"]) + end + if avatar_data["valid_rotations"] then + subtree:add(f_avatar_data_valid_rotations, avatar_data["valid_rotations"]) + end + if avatar_data["valid_translations"] then + subtree:add(f_avatar_data_valid_translations, avatar_data["valid_translations"]) + end + if avatar_data["default_rotations"] then + subtree:add(f_avatar_data_default_rotations, avatar_data["default_rotations"]) + end + if avatar_data["default_translations"] then + subtree:add(f_avatar_data_default_translations, avatar_data["default_translations"]) + end +end + +function decode_vec3(buf) + local i = 0 + local x = buf(i, 4):le_float() + i = i + 4 + local y = buf(i, 4):le_float() + i = i + 4 + local z = buf(i, 4):le_float() + i = i + 4 + return {x, y, z} +end + +function decode_validity_bits(buf, num_bits) + -- first pass, decode each bit into an array of booleans + local i = 0 + local bit = 0 + local booleans = {} + for n = 1, num_bits do + local value = (bit32.band(buf(i, 1):uint(), bit32.lshift(1, bit)) ~= 0) + booleans[#booleans + 1] = value + bit = bit + 1 + if bit == 8 then + i = i + 1 + bit = 0 + end + end + + -- second pass, create a list of indices whos booleans are true + local result = {} + for n = 1, #booleans do + if booleans[n] then + result[#result + 1] = n + end + end + + return result +end + +function decode_avatar_data_packet(buf) + + local i = 0 + local result = {} + + -- uint16 has_flags + local has_flags = buf(i, 2):le_uint() + i = i + 2 + + local has_global_position = (bit32.band(has_flags, 1) ~= 0) + local has_bounding_box = (bit32.band(has_flags, 2) ~= 0) + local has_orientation = (bit32.band(has_flags, 4) ~= 0) + local has_scale = (bit32.band(has_flags, 8) ~= 0) + local has_look_at_position = (bit32.band(has_flags, 16) ~= 0) + local has_audio_loudness = (bit32.band(has_flags, 32) ~= 0) + local has_sensor_to_world_matrix = (bit32.band(has_flags, 64) ~= 0) + local has_additional_flags = (bit32.band(has_flags, 128) ~= 0) + local has_parent_info = (bit32.band(has_flags, 256) ~= 0) + local has_local_position = (bit32.band(has_flags, 512) ~= 0) + local has_face_tracker_info = (bit32.band(has_flags, 1024) ~= 0) + local has_joint_data = (bit32.band(has_flags, 2048) ~= 0) + local has_joint_default_pose_flags = (bit32.band(has_flags, 4096) ~= 0) + + result["has_flags"] = string.format("HasFlags: 0x%x", has_flags) + + if has_global_position then + local position = decode_vec3(buf(i, 12)) + result["position"] = string.format("Position: %.3f, %.3f, %.3f", position[1], position[2], position[3]) + i = i + 12 + end + + if has_bounding_box then + local dimensions = decode_vec3(buf(i, 12)) + i = i + 12 + local offset = decode_vec3(buf(i, 12)) + i = i + 12 + result["dimensions"] = string.format("Dimensions: %.3f, %.3f, %.3f", dimensions[1], dimensions[2], dimensions[3]) + result["offset"] = string.format("Offset: %.3f, %.3f, %.3f", offset[1], offset[2], offset[3]) + end + + if has_orientation then + -- TODO: orientation is hard to decode... + i = i + 6 + end + + if has_scale then + -- TODO: scale is hard to decode... + i = i + 2 + end + + if has_look_at_position then + local look_at = decode_vec3(buf(i, 12)) + i = i + 12 + result["look_at_position"] = string.format("Look At Position: %.3f, %.3f, %.3f", look_at[1], look_at[2], look_at[3]) + end + + if has_audio_loudness then + local loudness = buf(i, 1):uint() + i = i + 1 + result["audio_loudness"] = string.format("Audio Loudness: %d", loudness) + end + + if has_sensor_to_world_matrix then + -- TODO: sensor to world matrix is hard to decode + i = i + 20 + end + + if has_additional_flags then + local flags = buf(i, 1):uint() + i = i + 1 + result["additional_flags"] = string.format("Additional Flags: 0x%x", flags) + end + + if has_parent_info then + local parent_id = buf(i, 16) + i = i + 16 + local parent_joint_index = buf(i, 2):le_int() + i = i + 2 + result["parent_id"] = parent_id + result["parent_joint_index"] = string.format("Parent Joint Index: %d", parent_joint_index) + end + + if has_local_position then + local local_pos = decode_vec3(buf(i, 12)) + i = i + 12 + result["local_position"] = string.format("Local Position: %.3f, %.3f, %.3f", local_pos[1], local_pos[2], local_pos[3]) + end + + if has_face_tracker_info then + local left_eye_blink = buf(i, 4):le_float() + i = i + 4 + local right_eye_blink = buf(i, 4):le_float() + i = i + 4 + local average_loudness = buf(i, 4):le_float() + i = i + 4 + local brow_audio_lift = buf(i, 4):le_float() + i = i + 4 + local num_blendshape_coefficients = buf(i, 1):uint() + i = i + 1 + local blendshape_coefficients = {} + for n = 1, num_blendshape_coefficients do + blendshape_coefficients[n] = buf(i, 4):le_float() + i = i + 4 + end + -- TODO: insert blendshapes into result + end + + if has_joint_data then + + local num_joints = buf(i, 1):uint() + i = i + 1 + local num_validity_bytes = math.ceil(num_joints / 8) + + local indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + i = i + num_validity_bytes + result["valid_rotations"] = "Valid Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + + -- TODO: skip rotations for now + i = i + #indices * 6 + + indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + i = i + num_validity_bytes + result["valid_translations"] = "Valid Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + + -- TODO: skip translations for now + i = i + #indices * 6 + + -- TODO: skip hand controller data + i = i + 24 + + end + + if has_joint_default_pose_flags then + local num_joints = buf(i, 1):uint() + i = i + 1 + local num_validity_bytes = math.ceil(num_joints / 8) + + local indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + i = i + num_validity_bytes + result["default_rotations"] = "Default Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + + indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + i = i + num_validity_bytes + result["default_translations"] = "Default Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + end + + return result +end function p_hfudt.init() local udp_dissector_table = DissectorTable.get("udp.port")