Created
October 2, 2020 14:52
-
-
Save ratijas/b38abbb6a38e3059231280d6ba97ce15 to your computer and use it in GitHub Desktop.
DSUS (DualShock Udp Server) protocol dissector for Wireshark
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| -- cache globals to local for speed. | |
| local format = string.format | |
| local tostring = tostring | |
| local tonumber = tonumber | |
| local sqrt = math.sqrt | |
| local pairs = pairs | |
| -- declare DSUS protocol | |
| local dsus_proto = Proto.new("DSUS", "DualShock UDP Server/Client for Cemuhook") | |
| -- setup protocol fields. | |
| local fds = {} | |
| fds.header = ProtoField.new("Header", "dsus.header", ftypes.NONE) | |
| fds.magic = ProtoField.string("dsus.magic", "Magic number", base.ASCII) | |
| fds.version = ProtoField.uint16("dsus.version", "Version", base.DEC) | |
| fds.version32 = ProtoField.uint32("dsus.version32", "Version", base.DEC) | |
| fds.size = ProtoField.uint16("dsus.size", "Payload size", base.DEC) | |
| fds.crc = ProtoField.uint32("dsus.crc", "Checksum", base.HEX) | |
| fds.sender_id = ProtoField.uint32("dsus.id", "Sender ID", base.HEX) | |
| local message_types = { | |
| [0x100000] = "Version", | |
| [0x100001] = "List ports / Port info", | |
| [0x100002] = "Pad request / Pad data", | |
| } | |
| fds.payload = ProtoField.new("Payload", "dsus.payload", ftypes.NONE) | |
| fds.type = ProtoField.uint32("dsus.type", "Message type", base.HEX, message_types, nil, "Requests and responses share same message type id") | |
| -- list ports | |
| fds.num_of_pad_requests = ProtoField.uint32("dsus.num_of_pad_requests", "Number of pads requested", base.DEC) | |
| fds.pads = ProtoField.new("Pads indices", "dsus.pads", ftypes.STRING) | |
| -- port info / pad meta | |
| fds.pad = ProtoField.new("Pad Meta", "dsus.pad", ftypes.NONE) | |
| fds.pad_id = ProtoField.uint8("dsus.pad.id", "Pad ID", base.DEC) | |
| local pad_states = { | |
| [0x00] = "Disconnected", | |
| [0x01] = "Reserved", | |
| [0x02] = "Connected", | |
| } | |
| fds.pad_state = ProtoField.uint8("dsus.pad.state", "Pad State", base.DEC, pad_states) | |
| local pad_models = { | |
| [0] = "None", | |
| [1] = "DS3", | |
| [2] = "DS4", | |
| [3] = "Generic", | |
| } | |
| fds.model = ProtoField.uint8("dsus.pad.model", "Pad Model", base.DEC, pad_models) | |
| local connection_types = { | |
| [0x00] = "None", | |
| [0x01] = "USB", | |
| [0x02] = "Bluetooth", | |
| } | |
| fds.connection_type = ProtoField.uint8("dsus.pad.connection_type", "Connection Type", base.DEC, connection_types) | |
| fds.mac = ProtoField.bytes("dsus.pad.mac", "Pad MAC Address", base.COLON) | |
| local battery_statuses = { | |
| [0x00] = "None", | |
| [0x01] = "Dying", | |
| [0x02] = "Low", | |
| [0x03] = "Medium", | |
| [0x04] = "High", | |
| [0x05] = "Full", | |
| [0xEE] = "Charging", | |
| [0xEF] = "Charged", | |
| } | |
| fds.battery_status = ProtoField.uint8("dsus.pad.battery", "Battery Status", base.DEC, battery_statuses) | |
| fds.is_active = ProtoField.bool("dsus.pad.active", "Is Active", base.NONE, { "Yes", "No" }) | |
| -- pad data request | |
| fds.flags = ProtoField.uint8("dsus.flags", "Flags", base.HEX) | |
| fds.flags_byID = ProtoField.bool("dsus.flags.by_id", "By ID", 8, nil, 0x01, "reference a pad using its ID") | |
| fds.flags_byMAC = ProtoField.bool("dsus.flags.by_mac", "By MAC", 8, nil, 0x02, "reference a pad using its MAC") | |
| -- pad data response | |
| fds.packet_counter = ProtoField.uint32("dsus.packet_counter", "Packet Counter", base.DEC) | |
| fds.button_flags = ProtoField.uint32("dsus.button", "Button Flags", base.HEX, nil, nil, "One bit per button") | |
| fds.button_dpad_left = ProtoField.bool("dsus.button.dpad.left", "D-pad left", 32, nil, 0x80000000, "D-pad left (bitmask version)") | |
| fds.button_dpad_down = ProtoField.bool("dsus.button.dpad.down", "D-pad down", 32, nil, 0x40000000, "D-pad down (bitmask version)") | |
| fds.button_dpad_right = ProtoField.bool("dsus.button.dpad.right", "D-pad right", 32, nil, 0x20000000, "D-pad right (bitmask version)") | |
| fds.button_dpad_up = ProtoField.bool("dsus.button.dpad.up", "D-pad up", 32, nil, 0x10000000, "D-pad up (bitmask version)") | |
| fds.button_options = ProtoField.bool("dsus.button.options", "Options", 32, nil, 0x08000000, "Options button") | |
| fds.button_R3 = ProtoField.bool("dsus.button.R3", "R3", 32, nil, 0x04000000, "R3 button") | |
| fds.button_L3 = ProtoField.bool("dsus.button.L3", "L3", 32, nil, 0x02000000, "L3 button") | |
| fds.button_share = ProtoField.bool("dsus.button.share", "Share", 32, nil, 0x01000000, "Share button") | |
| fds.button_square = ProtoField.bool("dsus.button.square", "Square", 32, nil, 0x00800000, "Square button (bitmask version)") | |
| fds.button_cross = ProtoField.bool("dsus.button.cross", "Cross", 32, nil, 0x00400000, "Cross button (bitmask version)") | |
| fds.button_circle = ProtoField.bool("dsus.button.circle", "Circle", 32, nil, 0x00200000, "Circle button (bitmask version)") | |
| fds.button_triangle = ProtoField.bool("dsus.button.triangle", "Triangle", 32, nil, 0x00100000, "Triangle button (bitmask version)") | |
| fds.button_r1 = ProtoField.bool("dsus.button.R1", "R1", 32, nil, 0x00080000, "R1 button (bitmask version)") | |
| fds.button_l1 = ProtoField.bool("dsus.button.L1", "L1", 32, nil, 0x00040000, "L1 button (bitmask version)") | |
| fds.button_r2 = ProtoField.bool("dsus.button.R2", "R2", 32, nil, 0x00020000, "R2 button (bitmask version)") | |
| fds.button_l2 = ProtoField.bool("dsus.button.L2", "L2", 32, nil, 0x00010000, "L2 button (bitmask version)") | |
| fds.button_ps = ProtoField.bool("dsus.button.ps", "PS", 32, nil, 0x0000FF00, "PS button") | |
| fds.button_touch = ProtoField.bool("dsus.button.touch", "Touch", 32, nil, 0x000000FF, "Touch button") | |
| fds.stick = ProtoField.new("Control Sticks", "dsus.stick", ftypes.NONE) | |
| fds.stick_left_x = ProtoField.uint8("dsus.stick.left.x", "Left Stick X", base.DEC) | |
| fds.stick_left_y = ProtoField.uint8("dsus.stick.left.y", "Left Stick Y", base.DEC) | |
| fds.stick_right_x = ProtoField.uint8("dsus.stick.right.x", "Right Stick X", base.DEC) | |
| fds.stick_right_y = ProtoField.uint8("dsus.stick.right.y", "Right Stick Y", base.DEC) | |
| fds.button_switches = ProtoField.new("Button Switches", "dsus.switch", ftypes.NONE, nil, nil, nil, "Whole byte per button") | |
| fds.button_byte_dpad_left = ProtoField.bool("dsus.switch.dpad.left", "D-pad left", 8, nil, 0xFF, "D-pad left (byte version)") | |
| fds.button_byte_dpad_down = ProtoField.bool("dsus.switch.dpad.down", "D-pad down", 8, nil, 0xFF, "D-pad down (byte version)") | |
| fds.button_byte_dpad_right = ProtoField.bool("dsus.switch.dpad.right", "D-pad right", 8, nil, 0xFF, "D-pad right (byte version)") | |
| fds.button_byte_dpad_up = ProtoField.bool("dsus.switch.dpad.up", "D-pad up", 8, nil, 0xFF, "D-pad up (byte version)") | |
| fds.button_byte_square = ProtoField.bool("dsus.switch.square", "Square", 8, nil, 0xFF, "Square (byte version)") | |
| fds.button_byte_cross = ProtoField.bool("dsus.switch.cross", "Cross", 8, nil, 0xFF, "Cross (byte version)") | |
| fds.button_byte_circle = ProtoField.bool("dsus.switch.circle", "Circle", 8, nil, 0xFF, "Circle (byte version)") | |
| fds.button_byte_triangle = ProtoField.bool("dsus.switch.triangle", "Triangle", 8, nil, 0xFF, "Triangle (byte version)") | |
| fds.button_byte_r1 = ProtoField.bool("dsus.switch.R1", "R1", 8, nil, 0xFF, "R1 (byte version)") | |
| fds.button_byte_l1 = ProtoField.bool("dsus.switch.L1", "L1", 8, nil, 0xFF, "L1 (byte version)") | |
| fds.trigger = ProtoField.new("Triggers", "dsus.trigger", ftypes.NONE) | |
| fds.trigger_r2 = ProtoField.uint8("dsus.trigger.R2", "R2 Trigger", base.DEC) | |
| fds.trigger_l2 = ProtoField.uint8("dsus.trigger.L2", "L2 Trigger", base.DEC) | |
| fds.trackpad = ProtoField.new("Trackpads", "dsus.trackpad", ftypes.NONE) | |
| fds.trackpad_active = ProtoField.bool("dsus.trackpad.active", "Active", 8, nil, 0xFF) | |
| fds.trackpad_id = ProtoField.uint8("dsus.trackpad.id", "ID", base.HEX) | |
| fds.trackpad_x = ProtoField.uint8("dsus.trackpad.x", "X", base.DEC) | |
| fds.trackpad_y = ProtoField.uint8("dsus.trackpad.y", "Y", base.DEC) | |
| fds.motion = ProtoField.new("Motion Sensors", "dsus.motion", ftypes.NONE) | |
| fds.timestamp = ProtoField.double("dsus.time", "Timestamp", base.DEC) | |
| fds.acceleration_x = ProtoField.float("dsus.accel.x", "Acceleration X") | |
| fds.acceleration_y = ProtoField.float("dsus.accel.y", "Acceleration Y") | |
| fds.acceleration_z = ProtoField.float("dsus.accel.z", "Acceleration Z") | |
| fds.gyro_x = ProtoField.float("dsus.gyro.x", "Gyro X") | |
| fds.gyro_y = ProtoField.float("dsus.gyro.y", "Gyro Y") | |
| fds.gyro_z = ProtoField.float("dsus.gyro.z", "Gyro Z") | |
| -- register proto fields | |
| dsus_proto.fields = fds | |
| -- setup expert info fields | |
| local ef_too_short = ProtoExpert.new("dsus.too_short.expert", "Packet is too short", | |
| expert.group.MALFORMED, expert.severity.ERROR) | |
| local ef_too_much = ProtoExpert.new("dsus.too_much.expert", "Too much pad requests", | |
| expert.group.REQUEST_CODE, expert.severity.ERROR) | |
| local ef_length = ProtoExpert.new("dsus.length.expert", "Unexpected payload length", | |
| expert.group.MALFORMED, expert.severity.ERROR) | |
| local ef_magic = ProtoExpert.new("dsus.magic.expert", "Magic Number mismatch", | |
| expert.group.MALFORMED, expert.severity.ERROR) | |
| local ef_type = ProtoExpert.new("dsus.type.expert", "DSUS message type", | |
| expert.group.REQUEST_CODE, expert.severity.ERROR) | |
| -- register expert info fields | |
| dsus_proto.experts = { ef_too_short, ef_too_much, ef_length, ef_magic, ef_type } | |
| local magic_field = Field.new("dsus.magic") | |
| local function is_request() return magic_field()() == "DSUC" end | |
| local function is_response() return magic_field()() == "DSUS" end | |
| -- register DSUS to handle UDP port range | |
| local function register_udp_port_range(start_port, end_port) | |
| local udp_port_table = DissectorTable.get("udp.port") | |
| for port = start_port, end_port do | |
| udp_port_table:add(port, dsus_proto) | |
| end | |
| end | |
| -- Handle preferences changes. | |
| function dsus_proto.init() | |
| register_udp_port_range(26760, 26760) | |
| end | |
| -- Generic unique DSUS module error. | |
| -- Not a string, so error() does not mess it up with metadata. | |
| local ERR_DSUS = {"DSUS error"} | |
| -- Call `f` in __protected mode__. | |
| -- Catch only errors which equal to given `err`, | |
| -- re-throw any others. | |
| local function catch(err, f, ...) | |
| local ok, msg = pcall(f, ...) | |
| -- if it's not our error, re-raise it | |
| if not ok and msg ~= err then | |
| error(msg) | |
| end | |
| end | |
| local function assert_len(expected, actual, tree) | |
| if expected ~= actual then | |
| local msg = string.format("Unexpected payload length. Expected: %d, Actual: %d", expected, actual) | |
| tree:add_proto_expert_info(ef_length, msg) | |
| error(ERR_DSUS) | |
| end | |
| end | |
| local function pad_meta(tvb, tree) | |
| assert_len(12, tvb:len(), tree) | |
| local meta = tree:add(fds.pad, tvb()) | |
| meta:add(fds.pad_id, tvb(0, 1)) | |
| meta:add(fds.pad_state, tvb(1, 1)) | |
| meta:add(fds.model, tvb(2, 1)) | |
| meta:add(fds.connection_type, tvb(3, 1)) | |
| meta:add(fds.mac, tvb(4, 6)) | |
| meta:add(fds.battery_status, tvb(10, 1)) | |
| meta:add(fds.is_active, tvb(11, 1)) | |
| end | |
| local function pad_data(tvb, tree) | |
| assert_len(68, tvb:len(), tree) | |
| tree:add_le(fds.packet_counter, tvb(0, 4)) | |
| local flags_range = tvb(4, 4) | |
| local flags_tree = tree:add_le(fds.button_flags, tvb(4, 4)) | |
| flags_tree:add(fds.button_dpad_left, flags_range) | |
| flags_tree:add(fds.button_dpad_down, flags_range) | |
| flags_tree:add(fds.button_dpad_right, flags_range) | |
| flags_tree:add(fds.button_dpad_up, flags_range) | |
| flags_tree:add(fds.button_options, flags_range) | |
| flags_tree:add(fds.button_R3, flags_range) | |
| flags_tree:add(fds.button_L3, flags_range) | |
| flags_tree:add(fds.button_share, flags_range) | |
| flags_tree:add(fds.button_square, flags_range) | |
| flags_tree:add(fds.button_cross, flags_range) | |
| flags_tree:add(fds.button_circle, flags_range) | |
| flags_tree:add(fds.button_triangle, flags_range) | |
| flags_tree:add(fds.button_r1, flags_range) | |
| flags_tree:add(fds.button_l1, flags_range) | |
| flags_tree:add(fds.button_r2, flags_range) | |
| flags_tree:add(fds.button_l2, flags_range) | |
| flags_tree:add(fds.button_ps, flags_range) | |
| flags_tree:add(fds.button_touch, flags_range) | |
| local stick_range = tvb(8, 4) | |
| local stick_tree = tree:add(fds.stick, stick_range) | |
| stick_tree:add(fds.stick_left_x, stick_range(0, 1)) | |
| stick_tree:add(fds.stick_left_y, stick_range(1, 1)) | |
| stick_tree:add(fds.stick_right_x, stick_range(2, 1)) | |
| stick_tree:add(fds.stick_right_y, stick_range(3, 1)) | |
| local switches_range = tvb(12, 10) | |
| local switches_tree = tree:add(fds.button_switches, switches_range) | |
| switches_tree:add(fds.button_byte_dpad_left, switches_range(0, 1)) | |
| switches_tree:add(fds.button_byte_dpad_down, switches_range(1, 1)) | |
| switches_tree:add(fds.button_byte_dpad_right, switches_range(2, 1)) | |
| switches_tree:add(fds.button_byte_dpad_up, switches_range(3, 1)) | |
| switches_tree:add(fds.button_byte_square, switches_range(4, 1)) | |
| switches_tree:add(fds.button_byte_cross, switches_range(5, 1)) | |
| switches_tree:add(fds.button_byte_circle, switches_range(6, 1)) | |
| switches_tree:add(fds.button_byte_triangle, switches_range(7, 1)) | |
| switches_tree:add(fds.button_byte_r1, switches_range(8, 1)) | |
| switches_tree:add(fds.button_byte_l1, switches_range(9, 1)) | |
| local trigger_tree = tree:add(fds.trigger, tvb(22, 2)) | |
| trigger_tree:add(fds.trigger_r2, tvb(22, 1)) | |
| trigger_tree:add(fds.trigger_l2, tvb(23, 1)) | |
| local trackpad_range = tvb(24, 12) | |
| local trackpad_tree = tree:add(fds.trackpad, trackpad_range) | |
| -- first trackpad | |
| trackpad_tree:add(fds.trackpad_active, trackpad_range( 0, 1)) | |
| trackpad_tree:add(fds.trackpad_id, trackpad_range( 1, 1)) | |
| trackpad_tree:add(fds.trackpad_x, trackpad_range( 2, 2)) | |
| trackpad_tree:add(fds.trackpad_y, trackpad_range( 4, 2)) | |
| -- 2nd trackpad | |
| trackpad_tree:add(fds.trackpad_active, trackpad_range( 6, 1)) | |
| trackpad_tree:add(fds.trackpad_id, trackpad_range( 7, 1)) | |
| trackpad_tree:add(fds.trackpad_x, trackpad_range( 8, 2)) | |
| trackpad_tree:add(fds.trackpad_y, trackpad_range(10, 2)) | |
| local motion_tree = tree:add(fds.motion, tvb(36, 32)) | |
| local timestamp = tvb(36, 8):le_int64():tonumber() / 1000000.0 | |
| motion_tree:add_le(fds.timestamp, tvb(36, 8), timestamp):append_text(" seconds") | |
| motion_tree:add_le(fds.acceleration_x, tvb(44, 4)) | |
| motion_tree:add_le(fds.acceleration_y, tvb(48, 4)) | |
| motion_tree:add_le(fds.acceleration_z, tvb(52, 4)) | |
| motion_tree:add_le(fds.gyro_x, tvb(56, 4)) | |
| motion_tree:add_le(fds.gyro_y, tvb(60, 4)) | |
| motion_tree:add_le(fds.gyro_z, tvb(64, 4)) | |
| end | |
| local function list_ports_request(tvb, tree) | |
| assert_len(12, tvb:len(), tree) | |
| local pads_range = tvb(4, 4) | |
| local pads = pads_range:le_uint() | |
| tree:add_le(fds.num_of_pad_requests, pads_range) | |
| if pads > 4 then | |
| tree:add_proto_expert_info(ef_too_much, string.format("Too much pads requested: %d", pads)) | |
| error(ERR_DSUS) | |
| else | |
| local request_indices = {} | |
| for i = 1, pads do | |
| local index = tvb(7 + i, 1):le_uint() | |
| table.insert(request_indices, index) | |
| end | |
| tree:add(fds.pads, tvb(8, pads), table.concat(request_indices, ', ')) | |
| end | |
| end | |
| local function ports_info_response(tvb, tree) | |
| assert_len(16, tvb:len(), tree) | |
| pad_meta(tvb(4):tvb(), tree) | |
| end | |
| local function pad_data_request(tvb, tree) | |
| assert_len(12, tvb:len(), tree) | |
| local flag_range = tvb(4, 1) | |
| local flags_tree = tree:add(fds.flags, flag_range) | |
| flags_tree:add(fds.flags_byID, flag_range) | |
| flags_tree:add(fds.flags_byMAC, flag_range) | |
| tree:add(fds.pad_id, tvb(5, 1)) | |
| tree:add(fds.mac, tvb(6, 6)) | |
| end | |
| local function pad_data_response(tvb, tree) | |
| assert_len(84, tvb:len(), tree) | |
| pad_meta(tvb(4, 12):tvb(), tree) | |
| pad_data(tvb(16):tvb(), tree) | |
| end | |
| -- Inner packet dissector function, which does not catch errors. | |
| local function dissector(tvb, pinfo, tree) | |
| local dsus = tree:add(dsus_proto, tvb()) | |
| if tvb:len() < 16 then | |
| dsus:add_proto_expert_info(ef_too_short) | |
| end | |
| local header_range = tvb(0, 16) | |
| local header_tree = dsus:add(fds.header, header_range) | |
| local magic = header_range(0, 4) | |
| header_tree:add(fds.magic, magic) | |
| if not is_request() and not is_response() then | |
| dsus:add_proto_expert_info(ef_magic) | |
| end | |
| header_tree:add_le(fds.version, header_range(4, 2)) | |
| header_tree:add_le(fds.size, header_range(6, 2)) | |
| header_tree:add_le(fds.crc, header_range(8, 4)):append_text(" [unverified]") | |
| header_tree:add_le(fds.sender_id, header_range(12, 4)) | |
| local payload_range = tvb(16) | |
| local payload_tvb = payload_range:tvb() | |
| local payload_tree = dsus:add(fds.payload, payload_range) | |
| local type_range = payload_range(0, 4) | |
| local type_int = type_range:le_int() | |
| pinfo.cols.protocol = magic:string() | |
| local function set_type(type_desc) | |
| pinfo.cols.info = string.format("%s=%s", magic:string(), type_desc) | |
| dsus:set_text(format("%s, Type: %s", magic:string(), type_desc)) | |
| payload_tree:add_le(fds.type, type_range, type_int, string.format("Type: %s", type_desc)) | |
| end | |
| if type_int == 0x100000 then | |
| -- Version info request / Version info response | |
| if is_request() then | |
| set_type("Version request") | |
| -- do nothing | |
| elseif is_response() then | |
| set_type("Version response") | |
| assert_len(8, payload_tvb:len(), payload_tree) | |
| payload_tree:add_le(fds.version32, payload_range(4, 4)) | |
| end | |
| elseif type_int == 0x100001 then | |
| -- List Ports request / Port info response | |
| if is_request() then | |
| set_type("List ports request") | |
| list_ports_request(payload_tvb, payload_tree) | |
| elseif is_response() then | |
| set_type("Port info response") | |
| ports_info_response(payload_tvb, payload_tree) | |
| end | |
| elseif type_int == 0x100002 then | |
| -- Pad request/response | |
| if is_request() then | |
| set_type("Pad data request") | |
| pad_data_request(payload_tvb, payload_tree) | |
| elseif is_response() then | |
| set_type("Pad data response") | |
| pad_data_response(payload_tvb, payload_tree) | |
| end | |
| else | |
| dsus:add_proto_expert_info(ef_type) | |
| end | |
| end | |
| -- packet dissector | |
| function dsus_proto.dissector(tvb, pinfo, tree) | |
| -- catch our errors from inner function | |
| catch(ERR_DSUS, dissector, tvb, pinfo, tree) | |
| end |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment