controller_patcher/source/network/UDPServer.cpp

/****************************************************************************
 * Copyright (C) 2016,2017 Maschell
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ****************************************************************************/
#include "UDPServer.hpp"
#include <malloc.h>
#include <stdio.h>
#include <string.h>
#include <coreinit/cache.h>
#include <utils/logger.h>

#define MAX_UDP_SIZE 0x578

CThread *UDPServer::pThread    = NULL;
UDPServer *UDPServer::instance = NULL;

UDPServer::UDPServer(int32_t port) {
    int32_t ret;
    struct sockaddr_in addr;

    addr.sin_family      = AF_INET;
    addr.sin_port        = port;
    addr.sin_addr.s_addr = 0;

    this->sockfd = ret = socket(AF_INET, SOCK_DGRAM, 0);
    if (ret == -1) return;
    int32_t enable = 1;
    setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
    ret = bind(sockfd, (sockaddr *) &addr, 16);
    if (ret < 0) return;

    StartUDPThread(this);
}

UDPServer::~UDPServer() {

    CThread *pThreadPointer = UDPServer::pThread;
    if (pThreadPointer != NULL) {
        exitThread = 1;
        OSMemoryBarrier();
        if (pThreadPointer != NULL) {
            delete pThreadPointer;
            UDPServer::pThread = NULL;
            if (this->sockfd != -1) {
                close(sockfd);
            }
            this->sockfd = -1;
        }
    }

        DEBUG_FUNCTION_LINE("Thread has been closed");

}

void UDPServer::StartUDPThread(UDPServer *server) {
    int32_t priority = 28;
    if (OSGetTitleID() == 0x00050000101c9300 || //The Legend of Zelda Breath of the Wild JPN
        OSGetTitleID() == 0x00050000101c9400 || //The Legend of Zelda Breath of the Wild USA
        OSGetTitleID() == 0x00050000101c9500 || //The Legend of Zelda Breath of the Wild EUR
        OSGetTitleID() == 0x00050000101c9b00 || //The Binding of Isaac: Rebirth EUR
        OSGetTitleID() == 0x00050000101a3c00) { //The Binding of Isaac: Rebirth USA
        priority = 10;
        DEBUG_FUNCTION_LINE("This game needs higher thread priority. We set it to %d", priority);
    }
    UDPServer::pThread = CThread::create(UDPServer::DoUDPThread, (void *) server, CThread::eAttributeAffCore2, priority);
    UDPServer::pThread->resumeThread();
}

BOOL UDPServer::cpyIncrementBufferOffset(void *target, void *source, int32_t *offset, int32_t typesize, int32_t maximum) {
    if (((int) *offset + typesize) > maximum) {
        DEBUG_FUNCTION_LINE("Transfer error. Excepted %04X bytes, but only got %04X", (*offset + typesize), maximum);
        return false;
    }
    memcpy(target, (void *) ((uint32_t) source + (*offset)), typesize);
    *offset += typesize;
    return true;
}

void UDPServer::DoUDPThread(CThread *thread, void *arg) {
    UDPServer *args = (UDPServer *) arg;
    args->DoUDPThreadInternal();
}

void UDPServer::DoUDPThreadInternal() {
    uint8_t buffer[MAX_UDP_SIZE];
    int32_t n;

    my_cb_user user;
    while (1) {
        //int32_t usingVar = exitThread;
        if (exitThread) break;
        memset(buffer, 0, MAX_UDP_SIZE);
        n = recv(sockfd, buffer, MAX_UDP_SIZE, 0);
        if (n < 0) {
            int32_t errno_ = errno;
            OSSleepTicks(OSMicrosecondsToTicks(2000));
            if (errno_ != EINVAL && errno_ != ENOTCONN) {
                break;
            }
            continue;
        }
        int32_t bufferoffset = 0;
        uint8_t type;
        memcpy((void *) &type, buffer, sizeof(type));
        bufferoffset += sizeof(type);
        switch (buffer[0]) {
            case WIIU_CP_UDP_CONTROLLER_READ_DATA: {
                if (gUsedProtocolVersion >= WIIU_CP_TCP_HANDSHAKE_VERSION_1) {
                    uint8_t count_commands;
                    memcpy((void *) &count_commands, buffer + bufferoffset, sizeof(count_commands));
                    bufferoffset += sizeof(count_commands);
                    for (int32_t i = 0; i < count_commands; i++) {
                        int32_t handle;
                        uint16_t deviceSlot;
                        uint32_t hid;
                        uint8_t padslot;
                        uint8_t datasize;

                        if (!cpyIncrementBufferOffset((void *) &handle, (void *) buffer, &bufferoffset, sizeof(handle), n)) continue;
                        if (!cpyIncrementBufferOffset((void *) &deviceSlot, (void *) buffer, &bufferoffset, sizeof(deviceSlot), n)) continue;
                        hid = (1 << deviceSlot);
                        if (!cpyIncrementBufferOffset((void *) &padslot, (void *) buffer, &bufferoffset, sizeof(padslot), n)) continue;
                        if (!cpyIncrementBufferOffset((void *) &datasize, (void *) buffer, &bufferoffset, sizeof(datasize), n)) continue;
                        uint8_t *databuffer = (uint8_t *) malloc(datasize * sizeof(uint8_t));
                        if (!databuffer) {
                            DEBUG_FUNCTION_LINE("Allocating memory failed");
                            continue;
                        }

                        if (!cpyIncrementBufferOffset((void *) databuffer, (void *) buffer, &bufferoffset, datasize, n)) continue;
                        //DEBUG_FUNCTION_LINE("UDPServer::DoUDPThreadInternal(): Got handle: %d slot %04X hid %04X pad %02X datasize %02X\n",handle,deviceSlot,hid,padslot,datasize);

                        user.pad_slot            = padslot;
                        user.slotdata.deviceslot = deviceSlot;
                        user.slotdata.hidmask    = hid;

                        if (gNetworkController[deviceSlot][padslot][0] == 0) {
                            DEBUG_FUNCTION_LINE("Ehm. Pad is not connected. STOP SENDING DATA ;) \n");
                        } else {
                            ControllerPatcherHID::externHIDReadCallback(handle, databuffer, datasize, &user);
                        }
                        if (databuffer) {
                            free(databuffer);
                            databuffer = NULL;
                        }
                    }
                    break;
                }
                break;
            }
            default: {
                break;
            }
        }
    }
    if (HID_DEBUG) {
        DEBUG_FUNCTION_LINE("UDPServer Thread ended");
    }
}