2 files changed, 93 insertions, 23 deletions

diff --git a/src/core/hle/service/hid/controllers/touchscreen.cpp b/src/core/hle/service/hid/controllers/touchscreen.cpp
index 0df395e85..de8315ce4 100644
--- a/src/core/hle/service/hid/controllers/touchscreen.cpp
+++ b/src/core/hle/service/hid/controllers/touchscreen.cpp
@@ -40,29 +40,43 @@ void Controller_Touchscreen::OnUpdate(const Core::Timing::CoreTiming& core_timin
     cur_entry.sampling_number = last_entry.sampling_number + 1;
     cur_entry.sampling_number2 = cur_entry.sampling_number;
 
-    bool pressed = false;
-    float x, y;
-    std::tie(x, y, pressed) = touch_device->GetStatus();
-    auto& touch_entry = cur_entry.states[0];
-    touch_entry.attribute.raw = 0;
-    if (!pressed && touch_btn_device) {
-        std::tie(x, y, pressed) = touch_btn_device->GetStatus();
-    }
-    if (pressed && Settings::values.touchscreen.enabled) {
-        touch_entry.x = static_cast<u16>(x * Layout::ScreenUndocked::Width);
-        touch_entry.y = static_cast<u16>(y * Layout::ScreenUndocked::Height);
-        touch_entry.diameter_x = Settings::values.touchscreen.diameter_x;
-        touch_entry.diameter_y = Settings::values.touchscreen.diameter_y;
-        touch_entry.rotation_angle = Settings::values.touchscreen.rotation_angle;
-        const u64 tick = core_timing.GetCPUTicks();
-        touch_entry.delta_time = tick - last_touch;
-        last_touch = tick;
-        touch_entry.finger = Settings::values.touchscreen.finger;
-        cur_entry.entry_count = 1;
-    } else {
-        cur_entry.entry_count = 0;
+    updateTouchInputEvent(touch_device->GetStatus(), mouse_finger_id);
+    updateTouchInputEvent(touch_btn_device->GetStatus(), keyboar_finger_id);
+
+    std::array<Finger, 16> sorted_fingers;
+    s32_le active_fingers = 0;
+    for (Finger finger : fingers) {
+        if (finger.pressed) {
+            sorted_fingers[active_fingers++] = finger;
+        }
     }
 
+    const u64 tick = core_timing.GetCPUTicks();
+    cur_entry.entry_count = active_fingers;
+    for (size_t id = 0; id < MAX_FINGERS; id++) {
+        auto& touch_entry = cur_entry.states[id];
+        if (id < active_fingers) {
+            touch_entry.x = static_cast<u16>(sorted_fingers[id].x * Layout::ScreenUndocked::Width);
+            touch_entry.y = static_cast<u16>(sorted_fingers[id].y * Layout::ScreenUndocked::Height);
+            touch_entry.diameter_x = Settings::values.touchscreen.diameter_x;
+            touch_entry.diameter_y = Settings::values.touchscreen.diameter_y;
+            touch_entry.rotation_angle = Settings::values.touchscreen.rotation_angle;
+            touch_entry.delta_time = tick - sorted_fingers[id].last_touch;
+            sorted_fingers[id].last_touch = tick;
+            touch_entry.finger = sorted_fingers[id].id;
+            touch_entry.attribute.raw = sorted_fingers[id].attribute.raw;
+        } else {
+            // Clear touch entry
+            touch_entry.attribute.raw = 0;
+            touch_entry.x = 0;
+            touch_entry.y = 0;
+            touch_entry.diameter_x = 0;
+            touch_entry.diameter_y = 0;
+            touch_entry.rotation_angle = 0;
+            touch_entry.delta_time = 0;
+            touch_entry.finger = 0;
+        }
+    }
     std::memcpy(data + SHARED_MEMORY_OFFSET, &shared_memory, sizeof(TouchScreenSharedMemory));
 }
 
@@ -74,4 +88,45 @@ void Controller_Touchscreen::OnLoadInputDevices() {
         touch_btn_device.reset();
     }
 }
+
+void Controller_Touchscreen::updateTouchInputEvent(
+    const std::tuple<float, float, bool>& touch_input, int& finger_id) {
+    bool pressed = false;
+    float x, y;
+    std::tie(x, y, pressed) = touch_input;
+    if (pressed) {
+        if (finger_id == -1) {
+            int first_free_id = 0;
+            int found = false;
+            while (!found && first_free_id < MAX_FINGERS) {
+                if (!fingers[first_free_id].pressed) {
+                    found = true;
+                } else {
+                    first_free_id++;
+                }
+            }
+            if (found) {
+                finger_id = first_free_id;
+                fingers[finger_id].x = x;
+                fingers[finger_id].y = y;
+                fingers[finger_id].pressed = true;
+                fingers[finger_id].id = finger_id;
+                fingers[finger_id].attribute.start_touch.Assign(1);
+            }
+        } else {
+            fingers[finger_id].x = x;
+            fingers[finger_id].y = y;
+            fingers[finger_id].attribute.raw = 0;
+        }
+    } else if (finger_id != -1) {
+        if (!fingers[finger_id].attribute.end_touch) {
+            fingers[finger_id].attribute.end_touch.Assign(1);
+            fingers[finger_id].attribute.start_touch.Assign(0);
+        } else {
+            fingers[finger_id].pressed = false;
+            finger_id = -1;
+        }
+    }
+}
+
 } // namespace Service::HID
diff --git a/src/core/hle/service/hid/controllers/touchscreen.h b/src/core/hle/service/hid/controllers/touchscreen.h
index 4d9042adc..6c7620420 100644
--- a/src/core/hle/service/hid/controllers/touchscreen.h
+++ b/src/core/hle/service/hid/controllers/touchscreen.h
@@ -30,6 +30,9 @@ public:
     void OnLoadInputDevices() override;
 
 private:
+    void updateTouchInputEvent(const std::tuple<float, float, bool>& touch_input, int& finger_id);
+    static const size_t MAX_FINGERS = 16;
+
     struct Attributes {
         union {
             u32 raw{};
@@ -55,7 +58,7 @@ private:
         s64_le sampling_number;
         s64_le sampling_number2;
         s32_le entry_count;
-        std::array<TouchState, 16> states;
+        std::array<TouchState, MAX_FINGERS> states;
     };
     static_assert(sizeof(TouchScreenEntry) == 0x298, "TouchScreenEntry is an invalid size");
 
@@ -66,9 +69,21 @@ private:
     };
     static_assert(sizeof(TouchScreenSharedMemory) == 0x3000,
                   "TouchScreenSharedMemory is an invalid size");
+
+    struct Finger {
+        u64_le last_touch{};
+        float x{};
+        float y{};
+        u32_le id{};
+        bool pressed{};
+        Attributes attribute;
+    };
+
     TouchScreenSharedMemory shared_memory{};
     std::unique_ptr<Input::TouchDevice> touch_device;
     std::unique_ptr<Input::TouchDevice> touch_btn_device;
-    s64_le last_touch{};
+    int mouse_finger_id{-1};
+    int keyboar_finger_id{-1};
+    std::array<Finger, MAX_FINGERS> fingers;
 };
 } // namespace Service::HID