3 files changed, 346 insertions, 20 deletions

diff --git a/src/core/hle/service/cam/cam.cpp b/src/core/hle/service/cam/cam.cpp
index 360bcfca4..4d714037f 100644
--- a/src/core/hle/service/cam/cam.cpp
+++ b/src/core/hle/service/cam/cam.cpp
@@ -4,16 +4,287 @@
 
 #include "common/logging/log.h"
 
-#include "core/hle/service/service.h"
+#include "core/hle/kernel/event.h"
 #include "core/hle/service/cam/cam.h"
 #include "core/hle/service/cam/cam_c.h"
 #include "core/hle/service/cam/cam_q.h"
 #include "core/hle/service/cam/cam_s.h"
 #include "core/hle/service/cam/cam_u.h"
+#include "core/hle/service/service.h"
 
 namespace Service {
 namespace CAM {
 
+static const u32 TRANSFER_BYTES = 5 * 1024;
+
+static Kernel::SharedPtr<Kernel::Event> completion_event_cam1;
+static Kernel::SharedPtr<Kernel::Event> completion_event_cam2;
+static Kernel::SharedPtr<Kernel::Event> interrupt_error_event;
+static Kernel::SharedPtr<Kernel::Event> vsync_interrupt_error_event;
+
+void StartCapture(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 port = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x1, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d", port);
+}
+
+void StopCapture(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 port = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x2, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d", port);
+}
+
+void GetVsyncInterruptEvent(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 port = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x5, 1, 2);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = IPC::MoveHandleDesc();
+    cmd_buff[3] = Kernel::g_handle_table.Create(vsync_interrupt_error_event).MoveFrom();
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d", port);
+}
+
+void GetBufferErrorInterruptEvent(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 port = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x6, 1, 2);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = IPC::MoveHandleDesc();
+    cmd_buff[3] = Kernel::g_handle_table.Create(interrupt_error_event).MoveFrom();
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d", port);
+}
+
+void SetReceiving(Service::Interface* self) {
+    u32* cmd_buff  = Kernel::GetCommandBuffer();
+
+    VAddr dest     = cmd_buff[1];
+    u8 port        = cmd_buff[2] & 0xFF;
+    u32 image_size = cmd_buff[3];
+    u16 trans_unit = cmd_buff[4] & 0xFFFF;
+
+    Kernel::Event* completion_event = (Port)port == Port::Cam2 ?
+            completion_event_cam2.get() : completion_event_cam1.get();
+
+    completion_event->Signal();
+
+    cmd_buff[0] = IPC::MakeHeader(0x7, 1, 2);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = IPC::MoveHandleDesc();
+    cmd_buff[3] = Kernel::g_handle_table.Create(completion_event).MoveFrom();
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, addr=0x%X, port=%d, image_size=%d, trans_unit=%d",
+            dest, port, image_size, trans_unit);
+}
+
+void SetTransferLines(Service::Interface* self) {
+    u32* cmd_buff      = Kernel::GetCommandBuffer();
+
+    u8  port           = cmd_buff[1] & 0xFF;
+    u16 transfer_lines = cmd_buff[2] & 0xFFFF;
+    u16 width          = cmd_buff[3] & 0xFFFF;
+    u16 height         = cmd_buff[4] & 0xFFFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x9, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d, lines=%d, width=%d, height=%d",
+            port, transfer_lines, width, height);
+}
+
+void GetMaxLines(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u16 width  = cmd_buff[1] & 0xFFFF;
+    u16 height = cmd_buff[2] & 0xFFFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0xA, 2, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = TRANSFER_BYTES / (2 * width);
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, width=%d, height=%d, lines = %d",
+            width, height, cmd_buff[2]);
+}
+
+void GetTransferBytes(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 port = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0xC, 2, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = TRANSFER_BYTES;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d", port);
+}
+
+void SetTrimming(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8   port = cmd_buff[1] & 0xFF;
+    bool trim = (cmd_buff[2] & 0xFF) != 0;
+
+    cmd_buff[0] = IPC::MakeHeader(0xE, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d, trim=%d", port, trim);
+}
+
+void SetTrimmingParamsCenter(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8  port  = cmd_buff[1] & 0xFF;
+    s16 trimW = cmd_buff[2] & 0xFFFF;
+    s16 trimH = cmd_buff[3] & 0xFFFF;
+    s16 camW  = cmd_buff[4] & 0xFFFF;
+    s16 camH  = cmd_buff[5] & 0xFFFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x12, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, port=%d, trimW=%d, trimH=%d, camW=%d, camH=%d",
+            port, trimW, trimH, camW, camH);
+}
+
+void Activate(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 cam_select = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x13, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, cam_select=%d",
+            cam_select);
+}
+
+void FlipImage(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 cam_select = cmd_buff[1] & 0xFF;
+    u8 flip       = cmd_buff[2] & 0xFF;
+    u8 context    = cmd_buff[3] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x1D, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, cam_select=%d, flip=%d, context=%d",
+            cam_select, flip, context);
+}
+
+void SetSize(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 cam_select = cmd_buff[1] & 0xFF;
+    u8 size       = cmd_buff[2] & 0xFF;
+    u8 context    = cmd_buff[3] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x1F, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, cam_select=%d, size=%d, context=%d",
+            cam_select, size, context);
+}
+
+void SetFrameRate(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 cam_select = cmd_buff[1] & 0xFF;
+    u8 frame_rate = cmd_buff[2] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x20, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, cam_select=%d, frame_rate=%d",
+            cam_select, frame_rate);
+}
+
+void GetStereoCameraCalibrationData(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    // Default values taken from yuriks' 3DS. Valid data is required here or games using the
+    // calibration get stuck in an infinite CPU loop.
+    StereoCameraCalibrationData data = {};
+    data.isValidRotationXY = 0;
+    data.scale = 1.001776f;
+    data.rotationZ = 0.008322907f;
+    data.translationX = -87.70484f;
+    data.translationY = -7.640977f;
+    data.rotationX = 0.0f;
+    data.rotationY = 0.0f;
+    data.angleOfViewRight = 64.66875f;
+    data.angleOfViewLeft = 64.76067f;
+    data.distanceToChart = 250.0f;
+    data.distanceCameras = 35.0f;
+    data.imageWidth = 640;
+    data.imageHeight = 480;
+
+    cmd_buff[0] = IPC::MakeHeader(0x2B, 17, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    memcpy(&cmd_buff[2], &data, sizeof(data));
+
+    LOG_TRACE(Service_CAM, "called");
+}
+
+void GetSuitableY2rStandardCoefficient(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    cmd_buff[0] = IPC::MakeHeader(0x36, 2, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    cmd_buff[2] = 0;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called");
+}
+
+void PlayShutterSound(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    u8 sound_id = cmd_buff[1] & 0xFF;
+
+    cmd_buff[0] = IPC::MakeHeader(0x38, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called, sound_id=%d", sound_id);
+}
+
+void DriverInitialize(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    completion_event_cam1->Clear();
+    completion_event_cam2->Clear();
+    interrupt_error_event->Clear();
+    vsync_interrupt_error_event->Clear();
+
+    cmd_buff[0] = IPC::MakeHeader(0x39, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called");
+}
+
+void DriverFinalize(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    cmd_buff[0] = IPC::MakeHeader(0x3A, 1, 0);
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    LOG_WARNING(Service_CAM, "(STUBBED) called");
+}
+
 void Init() {
     using namespace Kernel;
 
@@ -21,9 +292,18 @@ void Init() {
     AddService(new CAM_Q_Interface);
     AddService(new CAM_S_Interface);
     AddService(new CAM_U_Interface);
+
+    completion_event_cam1 = Kernel::Event::Create(RESETTYPE_ONESHOT, "CAM_U::completion_event_cam1");
+    completion_event_cam2 = Kernel::Event::Create(RESETTYPE_ONESHOT, "CAM_U::completion_event_cam2");
+    interrupt_error_event = Kernel::Event::Create(RESETTYPE_ONESHOT, "CAM_U::interrupt_error_event");
+    vsync_interrupt_error_event = Kernel::Event::Create(RESETTYPE_ONESHOT, "CAM_U::vsync_interrupt_error_event");
 }
 
 void Shutdown() {
+    completion_event_cam1 = nullptr;
+    completion_event_cam2 = nullptr;
+    interrupt_error_event = nullptr;
+    vsync_interrupt_error_event = nullptr;
 }
 
 } // namespace CAM
diff --git a/src/core/hle/service/cam/cam.h b/src/core/hle/service/cam/cam.h
index 23d0c9c59..83cfe6411 100644
--- a/src/core/hle/service/cam/cam.h
+++ b/src/core/hle/service/cam/cam.h
@@ -4,7 +4,12 @@
 
 #pragma once
 
+#include "common/common_funcs.h"
 #include "common/common_types.h"
+#include "common/swap.h"
+
+#include "core/hle/kernel/kernel.h"
+#include "core/hle/service/service.h"
 
 namespace Service {
 namespace CAM {
@@ -140,6 +145,26 @@ enum class OutputFormat : u8 {
     RGB565 = 1
 };
 
+/// Stereo camera calibration data.
+struct StereoCameraCalibrationData {
+    u8 isValidRotationXY;      ///< Bool indicating whether the X and Y rotation data is valid.
+    INSERT_PADDING_BYTES(3);
+    float_le scale;            ///< Scale to match the left camera image with the right.
+    float_le rotationZ;        ///< Z axis rotation to match the left camera image with the right.
+    float_le translationX;     ///< X axis translation to match the left camera image with the right.
+    float_le translationY;     ///< Y axis translation to match the left camera image with the right.
+    float_le rotationX;        ///< X axis rotation to match the left camera image with the right.
+    float_le rotationY;        ///< Y axis rotation to match the left camera image with the right.
+    float_le angleOfViewRight; ///< Right camera angle of view.
+    float_le angleOfViewLeft;  ///< Left camera angle of view.
+    float_le distanceToChart;  ///< Distance between cameras and measurement chart.
+    float_le distanceCameras;  ///< Distance between left and right cameras.
+    s16_le imageWidth;         ///< Image width.
+    s16_le imageHeight;        ///< Image height.
+    INSERT_PADDING_BYTES(16);
+};
+static_assert(sizeof(StereoCameraCalibrationData) == 64, "StereoCameraCalibrationData structure size is wrong");
+
 struct PackageParameterCameraSelect {
     CameraSelect camera;
     s8 exposure;
@@ -165,6 +190,26 @@ struct PackageParameterCameraSelect {
 
 static_assert(sizeof(PackageParameterCameraSelect) == 28, "PackageParameterCameraSelect structure size is wrong");
 
+void StartCapture(Service::Interface* self);
+void StopCapture(Service::Interface* self);
+void GetVsyncInterruptEvent(Service::Interface* self);
+void GetBufferErrorInterruptEvent(Service::Interface* self);
+void SetReceiving(Service::Interface* self);
+void SetTransferLines(Service::Interface* self);
+void GetMaxLines(Service::Interface* self);
+void GetTransferBytes(Service::Interface* self);
+void SetTrimming(Service::Interface* self);
+void SetTrimmingParamsCenter(Service::Interface* self);
+void Activate(Service::Interface* self);
+void FlipImage(Service::Interface* self);
+void SetSize(Service::Interface* self);
+void SetFrameRate(Service::Interface* self);
+void GetStereoCameraCalibrationData(Service::Interface* self);
+void GetSuitableY2rStandardCoefficient(Service::Interface* self);
+void PlayShutterSound(Service::Interface* self);
+void DriverInitialize(Service::Interface* self);
+void DriverFinalize(Service::Interface* self);
+
 /// Initialize CAM service(s)
 void Init();
 
diff --git a/src/core/hle/service/cam/cam_u.cpp b/src/core/hle/service/cam/cam_u.cpp
index 9d59c55fb..a1070ebb2 100644
--- a/src/core/hle/service/cam/cam_u.cpp
+++ b/src/core/hle/service/cam/cam_u.cpp
@@ -2,31 +2,32 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include "core/hle/service/cam/cam.h"
 #include "core/hle/service/cam/cam_u.h"
 
 namespace Service {
 namespace CAM {
 
 const Interface::FunctionInfo FunctionTable[] = {
-    {0x00010040, nullptr,                             "StartCapture"},
-    {0x00020040, nullptr,                             "StopCapture"},
+    {0x00010040, StartCapture,                        "StartCapture"},
+    {0x00020040, StopCapture,                         "StopCapture"},
     {0x00030040, nullptr,                             "IsBusy"},
     {0x00040040, nullptr,                             "ClearBuffer"},
-    {0x00050040, nullptr,                             "GetVsyncInterruptEvent"},
-    {0x00060040, nullptr,                             "GetBufferErrorInterruptEvent"},
-    {0x00070102, nullptr,                             "SetReceiving"},
+    {0x00050040, GetVsyncInterruptEvent,              "GetVsyncInterruptEvent"},
+    {0x00060040, GetBufferErrorInterruptEvent,        "GetBufferErrorInterruptEvent"},
+    {0x00070102, SetReceiving,                        "SetReceiving"},
     {0x00080040, nullptr,                             "IsFinishedReceiving"},
-    {0x00090100, nullptr,                             "SetTransferLines"},
-    {0x000A0080, nullptr,                             "GetMaxLines"},
+    {0x00090100, SetTransferLines,                    "SetTransferLines"},
+    {0x000A0080, GetMaxLines,                         "GetMaxLines"},
     {0x000B0100, nullptr,                             "SetTransferBytes"},
-    {0x000C0040, nullptr,                             "GetTransferBytes"},
+    {0x000C0040, GetTransferBytes,                    "GetTransferBytes"},
     {0x000D0080, nullptr,                             "GetMaxBytes"},
-    {0x000E0080, nullptr,                             "SetTrimming"},
+    {0x000E0080, SetTrimming,                         "SetTrimming"},
     {0x000F0040, nullptr,                             "IsTrimming"},
     {0x00100140, nullptr,                             "SetTrimmingParams"},
     {0x00110040, nullptr,                             "GetTrimmingParams"},
-    {0x00120140, nullptr,                             "SetTrimmingParamsCenter"},
-    {0x00130040, nullptr,                             "Activate"},
+    {0x00120140, SetTrimmingParamsCenter,             "SetTrimmingParamsCenter"},
+    {0x00130040, Activate,                            "Activate"},
     {0x00140080, nullptr,                             "SwitchContext"},
     {0x00150080, nullptr,                             "SetExposure"},
     {0x00160080, nullptr,                             "SetWhiteBalance"},
@@ -36,10 +37,10 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x001A0040, nullptr,                             "IsAutoExposure"},
     {0x001B0080, nullptr,                             "SetAutoWhiteBalance"},
     {0x001C0040, nullptr,                             "IsAutoWhiteBalance"},
-    {0x001D00C0, nullptr,                             "FlipImage"},
+    {0x001D00C0, FlipImage,                           "FlipImage"},
     {0x001E0200, nullptr,                             "SetDetailSize"},
-    {0x001F00C0, nullptr,                             "SetSize"},
-    {0x00200080, nullptr,                             "SetFrameRate"},
+    {0x001F00C0, SetSize,                             "SetSize"},
+    {0x00200080, SetFrameRate,                        "SetFrameRate"},
     {0x00210080, nullptr,                             "SetPhotoMode"},
     {0x002200C0, nullptr,                             "SetEffect"},
     {0x00230080, nullptr,                             "SetContrast"},
@@ -50,7 +51,7 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x00280080, nullptr,                             "SetNoiseFilter"},
     {0x00290080, nullptr,                             "SynchronizeVsyncTiming"},
     {0x002A0080, nullptr,                             "GetLatestVsyncTiming"},
-    {0x002B0000, nullptr,                             "GetStereoCameraCalibrationData"},
+    {0x002B0000, GetStereoCameraCalibrationData,      "GetStereoCameraCalibrationData"},
     {0x002C0400, nullptr,                             "SetStereoCameraCalibrationData"},
     {0x002D00C0, nullptr,                             "WriteRegisterI2c"},
     {0x002E00C0, nullptr,                             "WriteMcuVariableI2c"},
@@ -61,11 +62,11 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x003302C0, nullptr,                             "SetPackageParameterWithoutContext"},
     {0x00340140, nullptr,                             "SetPackageParameterWithContext"},
     {0x003501C0, nullptr,                             "SetPackageParameterWithContextDetail"},
-    {0x00360000, nullptr,                             "GetSuitableY2rStandardCoefficient"},
+    {0x00360000, GetSuitableY2rStandardCoefficient,   "GetSuitableY2rStandardCoefficient"},
     {0x00370202, nullptr,                             "PlayShutterSoundWithWave"},
-    {0x00380040, nullptr,                             "PlayShutterSound"},
-    {0x00390000, nullptr,                             "DriverInitialize"},
-    {0x003A0000, nullptr,                             "DriverFinalize"},
+    {0x00380040, PlayShutterSound,                    "PlayShutterSound"},
+    {0x00390000, DriverInitialize,                    "DriverInitialize"},
+    {0x003A0000, DriverFinalize,                      "DriverFinalize"},
     {0x003B0000, nullptr,                             "GetActivatedCamera"},
     {0x003C0000, nullptr,                             "GetSleepCamera"},
     {0x003D0040, nullptr,                             "SetSleepCamera"},