DSUS (DualShock Udp Server) protocol dissector for Wireshark

DSUS.lua

-- 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