Custom 3DS service tst:srv

custom_service.cpp

#include <3ds.h>
#include <cinttypes>
#include <stdio.h>
#include <string.h>

#define STACKSIZE (4 * 1024)
#define HELLO_COMMAND_ID 0x1
#define SHUTDOWN_COMMAND_ID 0x2

Handle server_port;
Thread handler_thread;

#define CONCAT2(x, y) DO_CONCAT2(x, y)
#define DO_CONCAT2(x, y) x ## y

#define variable_name(var) CONCAT2(var, __LINE__)

#define printf(...) { u8* variable_name(savedbuff) = my_printf_guard(); printf(__VA_ARGS__); my_printf_restore(variable_name(savedbuff)); }

u8* my_printf_guard() {
    // Saves the contents of the TLS command buffer and returns them in a variable
    // The caller is responsible for the management of the returned memory.
    // my_printf_restore deletes this memory automatically.
    // This is needed because printf corrupts the TLS+0x80 area.
    u8* tls = new u8[0x200];
    memcpy(tls, getThreadCommandBuffer(), 0x200);

    return tls;
}

void my_printf_restore(u8* tls) {
    memcpy(getThreadCommandBuffer(), tls, 0x200);
    delete[] tls;
}

void Pause() {
    while (aptMainLoop()) {
        hidScanInput();
        if (hidKeysDown() & KEY_A) break;

        gfxFlushBuffers();
        gfxSwapBuffers();

        gspWaitForVBlank();
    }
}

void ServiceHandlerThread(void* arg) {
    printf("Handler thread created\n");
    Handle handles[] = { server_port, 0 };
    bool shutdown = false;
    s32 index = 0;
    int num_handles = 1;
    
    volatile u32* cmd_buff = getThreadCommandBuffer();
    cmd_buff[0] = 0xFFFF0000;

    do {
        Result result = svcReplyAndReceive(&index, handles, shutdown ? 0 : num_handles, handles[1]);
        u64 now = svcGetSystemTick();
        //printf("ReplyAndReceive returned index %08X shutting down: %s\n", index, shutdown ? "yes" : "no");

        if (shutdown)
            return;

        if (result != 0) {
            printf("svcReplyAndReceive error: %08X\n", (u32)result);
            return;
        }

        if (index >= num_handles) {
            printf("Index error %08X\n", index);
            return;
        }

        switch (index) {
            case 0: // The ServerPort was signaled
                // Accept the new ServerSession
                result = svcAcceptSession(&handles[1], server_port);
                if (result != 0) {
                    //printf("svcAcceptSession error: %08X\n", (u32)result);
                    return;
                }
                //printf("Accepted a new session\n");
                num_handles++;
                break;
            case 1: { // The ServerSession was signaled
                // Handle the command
                u32 command = (cmd_buff[0] >> 16) & 0xFFFF;
                switch (command) {
                    case HELLO_COMMAND_ID: {
                        //printf("Hello, received %08X\n", cmd_buff[1]);
                        cmd_buff[0] = IPC_MakeHeader(HELLO_COMMAND_ID, 3, 0);
                        cmd_buff[1] = 0x0000BABE;

                        memcpy((void*)&cmd_buff[2], &now, sizeof(u64));
                        break;
                    }
                    case SHUTDOWN_COMMAND_ID:
                        printf("Shutting down the server.\n");
                        cmd_buff[0] = IPC_MakeHeader(SHUTDOWN_COMMAND_ID, 0, 0);
                        shutdown = true;
                        break;
                    default:
                        printf("Unknown command %08X\n", command);
                        break;
                }
                break;
            }
            default:
                printf("Unknown index signaled: %08X\n", index);
                break;
        }
    } while (true);

    printf("Exiting thread function\n");
}

int main() {
    gfxInitDefault();
    consoleInit(GFX_TOP, NULL);
    
    volatile u32* cmd_buff = getThreadCommandBuffer();
    Handle client_session;
    s32 prio = 0;

    u64 resp_time = 0, now = 0, after = 0;

    printf("Registering service\n");

    // Register a dummy service
    Result result = srvRegisterService(&server_port, "tst:srv", 1);
    if (result != 0) {
        printf("Could not register the service: %08X\n", (u32)result);
        goto exit;
    }

    printf("Service registered, retrieving thread priority\n");

    svcGetThreadPriority(&prio, CUR_THREAD_HANDLE);

    printf("Thread priority %08X, creating thread\n", prio);

    // Create a thread to handle our service's requests
    handler_thread = threadCreate(ServiceHandlerThread, nullptr, STACKSIZE, prio + 1, -2, false);

    printf("Thread created, retrieving service handle\n");

    // Get a ClientSession handle to our service
    result = srvGetServiceHandleDirect(&client_session, "tst:srv");
    if (result != 0) {
        printf("Could not retrieve the service handle: %08X\n", (u32)result);
        goto cleanup;
    }

    printf("Sending HELLO request to server\n");

    // Send a command to our service
    cmd_buff[0] = IPC_MakeHeader(HELLO_COMMAND_ID, 3, 0);
    cmd_buff[1] = 0x0000CAFE;
    now = svcGetSystemTick();
    memcpy((void*)&cmd_buff[2], &now, sizeof(u64));

    result = svcSendSyncRequest(client_session);
    after = svcGetSystemTick();
    if (result != 0) {
        printf("Sync Request error: %08X\n", (u32)result);
        goto cleanup;
    }

    memcpy(&resp_time, (void*)&cmd_buff[2], sizeof(u64));

    // Check the response
    printf("Service answered with %08X Sent: %016" PRIx64 " After: %016" PRIx64 " Resp: %016" PRIx64 "\n", cmd_buff[1], now, after, resp_time);

    printf("Sending SHUTDOWN request to server\n");

    // Tell our service to shutdown
    cmd_buff[0] = IPC_MakeHeader(SHUTDOWN_COMMAND_ID, 0, 0);

    result = svcSendSyncRequest(client_session);
    if (result != 0) {
        printf("Sync Request error: %08X\n", (u32)result);
        goto cleanup;
    }

    printf("Exiting main thread\n");

cleanup:
    //threadJoin(handler_thread, U64_MAX);
    threadFree(handler_thread);

exit:
    printf("Press A to exit\n");
    Pause();

    srvUnregisterService("tst:srv");

    gfxExit();
    return 0;
}