Vita DMAC test

dmac_test.c

#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)

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);
	LOG("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);
}

void dmac_test()
{
	int i;
	SceUID kr_memuid;
	SceUID work_memuid;
	volatile unsigned int *kr_addr;
	void *work_addr;
	SceKernelAllocMemBlockKernelOpt opt;

	memset(&opt, 0, sizeof(opt));
	opt.size = sizeof(opt);
	opt.attr = SCE_KERNEL_ALLOC_MEMBLOCK_ATTR_HAS_PADDR;
	opt.paddr = 0xE04E0000;
	kr_memuid = ksceKernelAllocMemBlock("SceSblDMAC5DmacKRBase", 0x20100206, 0x1000, &opt);

	ksceKernelGetMemBlockBase(kr_memuid, (void **)&kr_addr);
	LOG("Keyring block: 0x08%X | va: %p\n", kr_memuid, kr_addr);

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

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

	memset(work_addr, 0, 0x2000);
	ksceKernelCpuDcacheAndL2WritebackRange(work_addr, 0x2000);

	uint8_t *src = (uint8_t *)work_addr;
	uint8_t *dst = (uint8_t *)work_addr + 0x1000;

	uintptr_t src_pa, dst_pa;
	ksceKernelGetPaddr(src, &src_pa);
	ksceKernelGetPaddr(dst, &dst_pa);

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

	static const uint8_t key[LEN] = {
		0xAA, 0xBB, 0xCC, 0xDD,
		0x11, 0x22, 0x33, 0x44
	};

	/* Set key to DMAC */
	for (i = 0; i < LEN / 4; i++)
		kr_addr[(8 * KEY_SLOT) + i] = *(uint32_t *)&key[i * 4];

	dmac5_submit(dst_pa, src_pa, LEN, DMAC_CMD, KEY_SLOT);

	ksceKernelCpuDcacheAndL2InvalidateRange(dst, 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");

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

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

	LOG("Done!\n");

	ksceKernelFreeMemBlock(kr_memuid);
	ksceKernelFreeMemBlock(work_memuid);
}

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

	LOG("DMAC 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;
}