suyu/src/core/hle/service/hid/hidbus/ringcon.cpp

// Copyright 2021 yuzu Emulator Project
// Licensed under GPLv2 or any later version
// Refer to the license.txt file included.

#include "core/hid/emulated_controller.h"
#include "core/hid/hid_core.h"
#include "core/hle/kernel/k_event.h"
#include "core/hle/kernel/k_readable_event.h"
#include "core/hle/service/hid/hidbus/ringcon.h"

namespace Service::HID {

RingController::RingController(Core::HID::HIDCore& hid_core_,
                               KernelHelpers::ServiceContext& service_context_)
    : HidbusBase(service_context_) {
    // Use the horizontal axis of left stick for emulating input
    // There is no point on adding a frontend implementation since Ring Fit Adventure doesn't work
    input = hid_core_.GetEmulatedController(Core::HID::NpadIdType::Player1);
}

RingController::~RingController() = default;

void RingController::OnInit() {
    return;
}

void RingController::OnRelease() {
    return;
};

void RingController::OnUpdate() {
    if (!is_activated) {
        return;
    }

    if (!device_enabled) {
        return;
    }

    if (!polling_mode_enabled || !is_transfer_memory_set) {
        return;
    }

    switch (polling_mode) {
    case JoyPollingMode::SixAxisSensorEnable: {
        enable_sixaxis_data.header.total_entries = 10;
        enable_sixaxis_data.header.result = ResultSuccess;
        const auto& last_entry =
            enable_sixaxis_data.entries[enable_sixaxis_data.header.latest_entry];

        enable_sixaxis_data.header.latest_entry =
            (enable_sixaxis_data.header.latest_entry + 1) % 10;
        auto& curr_entry = enable_sixaxis_data.entries[enable_sixaxis_data.header.latest_entry];

        curr_entry.sampling_number = last_entry.sampling_number + 1;
        curr_entry.polling_data.sampling_number = curr_entry.sampling_number;

        const RingConData ringcon_value = GetSensorValue();
        curr_entry.polling_data.out_size = sizeof(ringcon_value);
        std::memcpy(curr_entry.polling_data.data.data(), &ringcon_value, sizeof(ringcon_value));

        std::memcpy(transfer_memory, &enable_sixaxis_data, sizeof(enable_sixaxis_data));
        break;
    }
    default:
        LOG_ERROR(Service_HID, "Polling mode not supported {}", polling_mode);
        break;
    }
}

RingController::RingConData RingController::GetSensorValue() const {
    RingConData ringcon_sensor_value{
        .status = DataValid::Valid,
        .data = 0,
    };

    const f32 stick_value = static_cast<f32>(input->GetSticks().left.x) / 32767.0f;

    ringcon_sensor_value.data = static_cast<s16>(stick_value * range) + idle_value;

    return ringcon_sensor_value;
}

u8 RingController::GetDeviceId() const {
    return device_id;
}

std::vector<u8> RingController::GetReply() const {
    const RingConCommands current_command = command;

    switch (current_command) {
    case RingConCommands::GetFirmwareVersion:
        return GetFirmwareVersionReply();
    case RingConCommands::ReadId:
        return GetReadIdReply();
    case RingConCommands::c20105:
        return GetC020105Reply();
    case RingConCommands::ReadUnkCal:
        return GetReadUnkCalReply();
    case RingConCommands::ReadFactoryCal:
        return GetReadFactoryCalReply();
    case RingConCommands::ReadUserCal:
        return GetReadUserCalReply();
    case RingConCommands::ReadRepCount:
        return GetReadRepCountReply();
    case RingConCommands::ReadTotalPushCount:
        return GetReadTotalPushCountReply();
    case RingConCommands::SaveCalData:
        return GetSaveDataReply();
    default:
        return GetErrorReply();
    }
}

bool RingController::SetCommand(const std::vector<u8>& data) {
    if (data.size() < 4) {
        LOG_ERROR(Service_HID, "Command size not supported {}", data.size());
        command = RingConCommands::Error;
        return false;
    }

    // There must be a better way to do this
    const u32 command_id =
        u32{data[0]} + (u32{data[1]} << 8) + (u32{data[2]} << 16) + (u32{data[3]} << 24);
    static constexpr std::array supported_commands = {
        RingConCommands::GetFirmwareVersion,
        RingConCommands::ReadId,
        RingConCommands::c20105,
        RingConCommands::ReadUnkCal,
        RingConCommands::ReadFactoryCal,
        RingConCommands::ReadUserCal,
        RingConCommands::ReadRepCount,
        RingConCommands::ReadTotalPushCount,
        RingConCommands::SaveCalData,
    };

    for (RingConCommands cmd : supported_commands) {
        if (command_id == static_cast<u32>(cmd)) {
            return ExcecuteCommand(cmd, data);
        }
    }

    LOG_ERROR(Service_HID, "Command not implemented {}", command_id);
    command = RingConCommands::Error;
    // Signal a reply to avoid softlocking
    send_command_asyc_event->GetWritableEvent().Signal();
    return false;
}

bool RingController::ExcecuteCommand(RingConCommands cmd, const std::vector<u8>& data) {
    switch (cmd) {
    case RingConCommands::GetFirmwareVersion:
    case RingConCommands::ReadId:
    case RingConCommands::c20105:
    case RingConCommands::ReadUnkCal:
    case RingConCommands::ReadFactoryCal:
    case RingConCommands::ReadUserCal:
    case RingConCommands::ReadRepCount:
    case RingConCommands::ReadTotalPushCount:
        ASSERT_MSG(data.size() == 0x4, "data.size is not 0x4 bytes");
        command = cmd;
        send_command_asyc_event->GetWritableEvent().Signal();
        return true;
    case RingConCommands::SaveCalData: {
        ASSERT_MSG(data.size() == 0x14, "data.size is not 0x14 bytes");

        SaveCalData save_info{};
        std::memcpy(&save_info, &data, sizeof(SaveCalData));
        user_calibration = save_info.calibration;

        command = cmd;
        send_command_asyc_event->GetWritableEvent().Signal();
        return true;
    }
    default:
        LOG_ERROR(Service_HID, "Command not implemented {}", cmd);
        command = RingConCommands::Error;
        return false;
    }
}

std::vector<u8> RingController::GetFirmwareVersionReply() const {
    const FirmwareVersionReply reply{
        .status = DataValid::Valid,
        .firmware = version,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadIdReply() const {
    // The values are hardcoded from a real joycon
    const ReadIdReply reply{
        .status = DataValid::Valid,
        .id_l_x0 = 8,
        .id_l_x0_2 = 41,
        .id_l_x4 = 22294,
        .id_h_x0 = 19777,
        .id_h_x0_2 = 13621,
        .id_h_x4 = 8245,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetC020105Reply() const {
    const Cmd020105Reply reply{
        .status = DataValid::Valid,
        .data = 1,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadUnkCalReply() const {
    const ReadUnkCalReply reply{
        .status = DataValid::Valid,
        .data = 0,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadFactoryCalReply() const {
    const ReadFactoryCalReply reply{
        .status = DataValid::Valid,
        .calibration = factory_calibration,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadUserCalReply() const {
    const ReadUserCalReply reply{
        .status = DataValid::Valid,
        .calibration = user_calibration,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadRepCountReply() const {
    // The values are hardcoded from a real joycon
    const GetThreeByteReply reply{
        .status = DataValid::Valid,
        .data = {30, 0, 0},
        .crc = GetCrcValue({30, 0, 0, 0}),
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetReadTotalPushCountReply() const {
    // The values are hardcoded from a real joycon
    const GetThreeByteReply reply{
        .status = DataValid::Valid,
        .data = {30, 0, 0},
        .crc = GetCrcValue({30, 0, 0, 0}),
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetSaveDataReply() const {
    const StatusReply reply{
        .status = DataValid::Valid,
    };

    return GetDataVector(reply);
}

std::vector<u8> RingController::GetErrorReply() const {
    const ErrorReply reply{
        .status = DataValid::BadCRC,
    };

    return GetDataVector(reply);
}

u8 RingController::GetCrcValue(const std::vector<u8>& data) const {
    u8 crc = 0;
    for (std::size_t index = 0; index < data.size(); index++) {
        for (u8 i = 0x80; i > 0; i >>= 1) {
            bool bit = (crc & 0x80) != 0;
            if ((data[index] & i) != 0) {
                bit = !bit;
            }
            crc <<= 1;
            if (bit) {
                crc ^= 0x8d;
            }
        }
    }
    return crc;
}

template <typename T>
std::vector<u8> RingController::GetDataVector(const T& reply) const {
    static_assert(std::is_trivially_copyable_v<T>);
    std::vector<u8> data;
    data.resize(sizeof(reply));
    std::memcpy(data.data(), &reply, sizeof(reply));
    return data;
}

} // namespace Service::HID