switch-pico/switch2_commands.cpp

#include "switch2_commands.h"

#include <cstring>

// Captured command vectors and acknowledgements:
// https://github.com/ndeadly/switch2_controller_research/blob/d1c5a7f7ba298f83017fae84952a4e6d2ef8fc92/commands.md
namespace {

constexpr uint8_t kSelectReportResponse[] = {
    0x03, 0x01, 0x00, 0x0A, 0x00, 0xF8, 0x00, 0x00,
};
constexpr uint8_t kInitializeUsbResponse[] = {
    0x03, 0x01, 0x00, 0x0D, 0x00, 0xF8, 0x00, 0x00,
    0x01, 0x00, 0x00, 0x00,
};

}  // namespace

Switch2VendorCommand switch2_classify_vendor_request(
    const uint8_t* data,
    std::size_t length) {
    if (data == nullptr || length < 8) {
        return Switch2VendorCommand::Unsupported;
    }
    if (data[0] != 0x03 || data[1] != 0x91 || data[2] != 0x00 ||
        data[4] != 0x00 || data[6] != 0x00 || data[7] != 0x00) {
        return Switch2VendorCommand::Unsupported;
    }
    if (length != static_cast<std::size_t>(8 + data[5])) {
        return Switch2VendorCommand::Unsupported;
    }

    switch (data[3]) {
        case 0x0A:
            if (data[5] != 4 || data[9] != 0 || data[10] != 0 || data[11] != 0) {
                return Switch2VendorCommand::Unsupported;
            }
            if (data[8] == 0x05) return Switch2VendorCommand::SelectReport05;
            if (data[8] == 0x09) return Switch2VendorCommand::SelectReport09;
            return Switch2VendorCommand::Unsupported;

        case 0x0D:
            if (data[5] == 8 && data[8] == 0x01) {
                return Switch2VendorCommand::InitializeUsb;
            }
            return Switch2VendorCommand::Unsupported;

        default:
            return Switch2VendorCommand::Unsupported;
    }
}

std::size_t switch2_build_vendor_response(
    Switch2VendorCommand command,
    uint8_t* output,
    std::size_t capacity) {
    const uint8_t* response = nullptr;
    std::size_t response_length = 0;

    switch (command) {
        case Switch2VendorCommand::Unsupported:
            return 0;
        case Switch2VendorCommand::SelectReport05:
        case Switch2VendorCommand::SelectReport09:
            response = kSelectReportResponse;
            response_length = sizeof(kSelectReportResponse);
            break;
        case Switch2VendorCommand::InitializeUsb:
            response = kInitializeUsbResponse;
            response_length = sizeof(kInitializeUsbResponse);
            break;
    }

    if (output == nullptr || capacity < response_length) {
        return 0;
    }
    std::memcpy(output, response, response_length);
    return response_length;
}