rmauth_sm.self keyslot 1C test

#include <psp2kern/kernel/modulemgr.h>
#include <psp2kern/kernel/threadmgr.h>
#include <psp2kern/kernel/sysmem.h>
#include <psp2kern/kernel/cpu.h>
#include <psp2kern/io/fcntl.h>
#include <psp2kern/sblauthmgr.h>
#include "log.h"
#include "draw.h"

#define LOG(s, ...) \
    do { \
        char __buffer[256]; \
        snprintf(__buffer, sizeof(__buffer), s, ##__VA_ARGS__); \
        console_print(__buffer); \
    } while (0)

#define DBG(...) (void)0

void _start() __attribute__((weak, alias("module_start")));

void dmac5_submit(uint32_t dst_pa, uint32_t src_pa, uint32_t len, uint32_t dmac_cmd, int keyslot)
{
    SceUID reg_memuid;
    volatile uint32_t *reg_addr;
    SceKernelAllocMemBlockKernelOpt opt;

    memset(&opt, 0, sizeof(opt));
    opt.size = sizeof(opt);
    opt.attr = SCE_KERNEL_ALLOC_MEMBLOCK_ATTR_HAS_PADDR;
    opt.paddr = 0xE0410000;
    reg_memuid = ksceKernelAllocMemBlock("SceDmacmgrDmac5Reg", 0x20100206, 0x1000, &opt);

    ksceKernelGetMemBlockBase(reg_memuid, (void **)&reg_addr);
    DBG("DMAC5 reg block: 0x%08X | va: %p\n", reg_memuid, reg_addr);

    #define DMAC_COMMIT_WAIT \
        do { \
            reg_addr[10] = reg_addr[10]; \
            reg_addr[7] = 1; \
            while(reg_addr[9] & 1) \
                ; \
        } while (0)

    reg_addr[0] = (uint32_t)src_pa;
    reg_addr[1] = (uint32_t)dst_pa;
    reg_addr[2] = len;
    reg_addr[3] = dmac_cmd;
    reg_addr[4] = keyslot;
    reg_addr[5] = 0;
    // reg_addr[8] = 0;
    reg_addr[11] = 0xE070;
    reg_addr[12] = 0x700070;
    DMAC_COMMIT_WAIT;
    asm volatile ("dmb sy");
    asm volatile ("dsb sy");

    ksceKernelFreeMemBlock(reg_memuid);
}

#define SCE_KERNEL_SYSROOT_SELF_INDEX_RMAUTH_SM 1

typedef struct SceSblSmCommContext130 {
    uint32_t unk_0;
    uint32_t self_type; // 2 - user = 1 / kernel = 0
    char data0[0x90]; //hardcoded data
    char data1[0x90];
    uint32_t pathId; // 2 (2 = os0)
    uint32_t unk_12C;
} SceSblSmCommContext130;

typedef struct SceSblSmCommPair {
    uint32_t unk_0;
    uint32_t unk_4;
} SceSblSmCommPair;

typedef struct SceSblSmCommMsifData {
    unsigned int unk00;
    unsigned int unk04;
    unsigned int unk08;
    unsigned int unk0C;
    unsigned int unk10;
    unsigned int unk14;
    unsigned int unk18;
    unsigned int unk1C;
} SceSblSmCommMsifData;

extern int ksceSblSmCommStartSmFromData(int priority, const char *elf_data, int elf_size, int num1, SceSblSmCommContext130 *ctx, int *id);
extern int ksceSblSmCommStopSm(int id, SceSblSmCommPair *res);
extern int ksceSblSmCommCallFunc(int id, int command_id, int *f00d_resp, const void *data, int size);

static const unsigned char ctx_130_data[0x90] =
{
    0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x28, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00,
    0xC0, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF,
    0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x09,
    0x80, 0x03, 0x00, 0x00, 0xC3, 0x00, 0x00, 0x00, 0x80, 0x09,
    0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00
};

static int sblcomm_start_rmauth_sm(int *rmauth_sm_id)
{
    int ret;
    SceKernelSysrootSelfInfo self_info;
    SceSblSmCommContext130 smcomm_ctx;

    memset(&self_info, 0, sizeof(self_info));
    self_info.size = sizeof(self_info);

    ret = ksceSysrootGetSelfInfo(SCE_KERNEL_SYSROOT_SELF_INDEX_RMAUTH_SM, &self_info);
    if (ret < 0)
        return ret;

    memset(&smcomm_ctx, 0, sizeof(smcomm_ctx));
    memcpy(smcomm_ctx.data0, ctx_130_data, 0x90);
    smcomm_ctx.pathId = 2;
    smcomm_ctx.self_type = (smcomm_ctx.self_type & 0xFFFFFFF0) | 2;

    ret = ksceSblSmCommStartSmFromData(0, self_info.self_data,
                       self_info.self_size, 0,
                       &smcomm_ctx, rmauth_sm_id);
    if (ret < 0)
        return ret;

    return 0;
}

static int f00d_rmauth_sm_cmd_1(int *res)
{
    int ret;
    int rmauth_sm_id;
    int f00d_resp;
        SceSblSmCommPair stop_res;
        SceSblSmCommMsifData data;

    ret = sblcomm_start_rmauth_sm(&rmauth_sm_id);
    if (ret < 0)
        return ret;

    ret = ksceSblSmCommCallFunc(rmauth_sm_id, 1, &f00d_resp, &data, 0x20);
    if (ret < 0)
        return ret;

    ret = ksceSblSmCommStopSm(rmauth_sm_id, &stop_res);
    if (ret < 0)
        return ret;

    *res = data.unk10;

    return f00d_resp;
}

static int f00d_rmauth_sm_cmd_2(const uint32_t seed[8])
{
    int ret;
    int rmauth_sm_id;
    int f00d_resp;
        SceSblSmCommPair stop_res;
        uint32_t buffer[8];

    ret = sblcomm_start_rmauth_sm(&rmauth_sm_id);
    if (ret < 0)
        return ret;

    LOG("rmauth_sm id: 0x%08X\n", rmauth_sm_id);

    memcpy(buffer, seed, sizeof(buffer));
    ret = ksceSblSmCommCallFunc(rmauth_sm_id, 2, &f00d_resp, buffer, 0x20);
    if (ret < 0)
        return ret;

    ret = ksceSblSmCommStopSm(rmauth_sm_id, &stop_res);
    if (ret < 0)
        return ret;

    return f00d_resp;
}

void dmac_test()
{
    int i;
    SceUID work_memuid;
    void *work_addr;

    #define WORK_SIZE 0x3000

    work_memuid = ksceKernelAllocMemBlock("work", 0x1050D006, WORK_SIZE, NULL);
    ksceKernelGetMemBlockBase(work_memuid, &work_addr);

    DBG("Work block: 0x%08X | va: %p\n", work_memuid, work_addr);

    /* Set src (plaintext) to 0s */
    memset(work_addr, 0, WORK_SIZE);
    ksceKernelCpuDcacheAndL2WritebackRange(work_addr, WORK_SIZE);

    uint8_t *src = (uint8_t *)work_addr;
    uint8_t *dst1 = (uint8_t *)work_addr + 0x1000;
    uint8_t *dst2 = (uint8_t *)work_addr + 0x2000;

    uintptr_t src_pa, dst1_pa, dst2_pa;
    ksceKernelGetPaddr(src, &src_pa);
    ksceKernelGetPaddr(dst1, &dst1_pa);
    ksceKernelGetPaddr(dst2, &dst2_pa);

    #define DMAC_CMD    0x41    /* DES64ECB */
    #define KEY_SLOT    0x1C
    #define LEN     8   /* in bytes */

    static const uint32_t seed1[] = {
        0x11111111, 0x11111111, 0x11111111, 0x11111111,
        0x11111111, 0x11111111, 0x11111111, 0x11111111
    };

    static const uint32_t seed2[] = {
        0x22222222, 0x22222222, 0x22222222, 0x22222222,
        0x22222222, 0x22222222, 0x22222222, 0x22222222
    };

    f00d_rmauth_sm_cmd_2(seed1);
    dmac5_submit(dst1_pa, src_pa, LEN, DMAC_CMD, KEY_SLOT);

    f00d_rmauth_sm_cmd_2(seed2);
    dmac5_submit(dst2_pa, src_pa, LEN, DMAC_CMD, KEY_SLOT);

    ksceKernelCpuDcacheAndL2InvalidateRange(dst1, 0x1000);
    ksceKernelCpuDcacheAndL2InvalidateRange(dst2, 0x1000);

    LOG("\nDMAC command: 0x%08X\n", DMAC_CMD);
    LOG("DMAC key slot: 0x%02X\n", KEY_SLOT);
    LOG("DMAC job length: 0x%02X bytes\n", LEN);

    LOG("\nInput:");
    for (i = 0; i < LEN; i++)
        LOG(" %02X", src[i]);
    LOG("\n\n");

    LOG("Seed 1:");
    for (i = 0; i < LEN; i++)
        LOG(" %08lX", seed1[i]);
    LOG("\n");

    LOG("Output 1:");
    for (i = 0; i < LEN; i++)
        LOG(" %02X", dst1[i]);
    LOG("\n\n\n");

    LOG("Seed 2:");
    for (i = 0; i < LEN; i++)
        LOG(" %08lX", seed2[i]);
    LOG("\n");

    LOG("Output 2:");
    for (i = 0; i < LEN; i++)
        LOG(" %02X", dst2[i]);
    LOG("\n\n\n");

    LOG("Done!\n");

    ksceKernelFreeMemBlock(work_memuid);
}

int module_start(SceSize argc, const void *args)
{
    map_framebuffer();

    LOG("vita-keyslot-1c-test by xerpi\n\n");

    dmac_test();

    return SCE_KERNEL_START_SUCCESS;
}

int module_stop(SceSize argc, const void *args)
{
    unmap_framebuffer();

    return SCE_KERNEL_STOP_SUCCESS;
}