void gpu_fft_base_release(struct GPU_FFT_BASE *base)
{
int mb = base->mb;
unsigned handle = base->handle, size = base->size;
unmapmem((void*)base->peri, base->peri_size);
unmapmem((void*)base, size);
mem_unlock(mb, handle);
mem_free(mb, handle);
qpu_enable(mb, 0);
}
// Close this library and deallocate everything
int bcm2835_close(void)
{
int ok = 1; // Success.
if (debug) return ok;
unmapmem((void**) &bcm2835_gpio, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_pwm, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_clk, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_spi0, BCM2835_BLOCK_SIZE);
if (memfd >= 0)
{
close(memfd);
memfd = -1;
}
return ok;
}
u32
prepare_for_sleep (u32 firmware_waking_vector)
{
u8 *p;
int wakeup_entry_len;
/* Get the suspend-lock to make other processors stopping or staying
in the guest mode. */
get_suspend_lock ();
/* Now the VMM is executed by the current processor only.
Call suspend functions. */
call_initfunc ("suspend");
/* Initialize variables used by wakeup functions */
wakeup_cpucount = 0;
spinlock_init (&wakeup_cpucount_lock);
waking_vector = firmware_waking_vector;
wakeup_prepare ();
/* Copy the wakeup_entry code. */
wakeup_entry_len = wakeup_entry_end - wakeup_entry_start;
p = mapmem_hphys (wakeup_entry_addr, wakeup_entry_len, MAPMEM_WRITE);
memcpy (p, wakeup_entry_start, wakeup_entry_len);
unmapmem (p, wakeup_entry_len);
return wakeup_entry_addr;
}
/**
* Cleanup previously allocated device memory and buffers.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns None
*/
void ws2811_cleanup(ws2811_t *ws2811)
{
int chan;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
if (ws2811->channel[chan].leds)
{
free(ws2811->channel[chan].leds);
}
ws2811->channel[chan].leds = NULL;
}
if (mbox.virt_addr != NULL)
{
unmapmem(mbox.virt_addr, mbox.size);
mem_unlock(mbox.handle, mbox.mem_ref);
mem_free(mbox.handle, mbox.mem_ref);
if (mbox.handle >= 0)
mbox_close(mbox.handle);
memset(&mbox, 0, sizeof(mbox));
}
ws2811_device_t *device = ws2811->device;
if (device) {
free(device);
}
ws2811->device = NULL;
}
static void
terminate(int num)
{
// Stop outputting and generating the clock.
if (clk_reg && gpio_reg && mbox.virt_addr) {
// Set GPIO4 to be an output (instead of ALT FUNC 0, which is the clock).
gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (1 << 12);
// Disable the clock generator.
clk_reg[GPCLK_CNTL] = 0x5A;
}
if (dma_reg && mbox.virt_addr) {
dma_reg[DMA_CS] = BCM2708_DMA_RESET;
udelay(10);
}
fm_mpx_close();
close_control_pipe();
if (mbox.virt_addr != NULL) {
unmapmem(mbox.virt_addr, NUM_PAGES * 4096);
mem_unlock(mbox.handle, mbox.mem_ref);
mem_free(mbox.handle, mbox.mem_ref);
}
printf("Terminating: cleanly deactivated the DMA engine and killed the carrier.\n");
exit(num);
}
/**
* Cleanup previously allocated device memory and buffers.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns None
*/
void ws2811_cleanup(ws2811_t *ws2811)
{
ws2811_device_t *device = ws2811->device;
int chan;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
if (ws2811->channel[chan].leds)
{
free(ws2811->channel[chan].leds);
}
ws2811->channel[chan].leds = NULL;
}
if (device->mbox.handle != -1)
{
videocore_mbox_t *mbox = &device->mbox;
unmapmem(mbox->virt_addr, mbox->size);
mem_unlock(mbox->handle, mbox->mem_ref);
mem_free(mbox->handle, mbox->mem_ref);
mbox_close(mbox->handle);
mbox->handle = -1;
}
if (device) {
free(device);
}
ws2811->device = NULL;
}
static void
terminate(int dummy)
{
int i;
dprintf("Resetting DMA...\n");
if (dma_reg && mbox.virt_addr) {
for (i = 0; i < num_channels; i++)
channel_pwm[i] = 0;
update_pwm();
udelay(CYCLE_TIME_US);
dma_reg[DMA_CS] = DMA_RESET;
udelay(10);
}
dprintf("Freeing mbox memory...\n");
if (mbox.virt_addr != NULL) {
unmapmem(mbox.virt_addr, NUM_PAGES * PAGE_SIZE);
if (mbox.handle <= 2) {
/* we need to reopen mbox file */
mbox.handle = mbox_open();
}
mem_unlock(mbox.handle, mbox.mem_ref);
mem_free(mbox.handle, mbox.mem_ref);
mbox_close(mbox.handle);
}
dprintf("Unlink %s...\n", DEVFILE);
unlink(DEVFILE);
dprintf("Unlink %s...\n", DEVFILE_MBOX);
unlink(DEVFILE_MBOX);
printf("pi-blaster stopped.\n");
exit(1);
}
static void
loadcfg (void)
{
struct loadcfg_data *d;
u8 *pass, *data;
ulong rbx;
struct config_data *tmpbuf;
if (!enable)
return;
current->vmctl.read_general_reg (GENERAL_REG_RBX, &rbx);
rbx &= 0xFFFFFFFF;
d = mapmem_hphys (rbx, sizeof *d, 0);
ASSERT (d);
if (d->len != sizeof *d)
panic ("size mismatch: %d, %d\n", d->len,
(int)sizeof *d);
pass = mapmem_hphys (d->pass, d->passlen, 0);
ASSERT (pass);
data = mapmem_hphys (d->data, d->datalen, 0);
ASSERT (data);
tmpbuf = alloc (d->datalen);
ASSERT (tmpbuf);
#ifdef CRYPTO_VPN
decryptcfg (pass, d->passlen, data, d->datalen, tmpbuf);
#else
panic ("cannot decrypt");
#endif
unmapmem (pass, d->passlen);
unmapmem (data, d->datalen);
unmapmem (d, sizeof *d);
config.len = 0;
if ((tmpbuf->len + 15) / 16 == d->datalen / 16) {
if (tmpbuf->len != sizeof config)
panic ("config size mismatch: %d, %d\n", tmpbuf->len,
(int)sizeof config);
data = mapmem_hphys (d->data, sizeof config, MAPMEM_WRITE);
ASSERT (data);
memcpy (data, tmpbuf, sizeof config);
unmapmem (data, d->datalen);
current->vmctl.write_general_reg (GENERAL_REG_RAX, 1);
} else {
current->vmctl.write_general_reg (GENERAL_REG_RAX, 0);
}
free (tmpbuf);
}
// Close this library and deallocate everything
int bcm2835_close(void)
{
if (debug) return 1; // Success
unmapmem((void**) &bcm2835_gpio, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_pwm, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_clk, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_spi0, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_bsc0, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_bsc1, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_st, BCM2835_BLOCK_SIZE);
unmapmem((void**) &bcm2835_pads, BCM2835_BLOCK_SIZE);
return 1; // Success
}
static void
copy_input_buffer (void *input, u16 length)
{
void *p;
if (!length)
return;
p = mapmem_hphys (INPUT_PARAM_BLK_ADDR, length, MAPMEM_WRITE);
memcpy (p, input, length);
unmapmem (p, length);
}
static void
copy_output_buffer (void *output, u16 length)
{
void *p;
if (!length)
return;
p = mapmem_hphys (OUTPUT_PARAM_BLK_ADDR, length, 0);
memcpy (output, p, length);
unmapmem (p, length);
}
void
tcg_measure (void *virt, u32 len)
{
struct TCG_HashLogExtendEvent_input_param_blk input;
struct {
struct TCG_HashLogExtendEvent_output_param_blk output;
char buf[32];
} s;
u32 *log;
u32 log_phys, phys;
u32 ret, feat, event, edi;
u8 major, minor;
void *tmp;
if (!len)
return;
if (!int1a_TCG_StatusCheck (&ret, &major, &minor, &feat, &event,
&edi))
return;
phys = 0x100000;
log_phys = phys + len;
log = mapmem_hphys (log_phys, 32, MAPMEM_WRITE);
tmp = mapmem_hphys (phys, len, MAPMEM_WRITE);
memcpy (tmp, virt, len);
unmapmem (tmp, len);
memset (&input, 0, sizeof input);
input.u.format_2.IPBLength = sizeof input.u.format_2;
input.u.format_2.HashDataPtr = phys;
input.u.format_2.HashDataLen = len;
input.u.format_2.PCRIndex = 4;
input.u.format_2.LogDataPtr = log_phys;
input.u.format_2.LogDataLen = 32;
memset (log, 0, 32);
log[0] = input.u.format_2.PCRIndex;
log[1] = 13;
if (!int1a_TCG_HashLogExtendEvent (&input, &s.output, sizeof s, &ret))
printf ("TCG_HashLogExtendEvent error\n");
else
printf ("EventNumber = %u\n", s.output.EventNumber);
unmapmem (log, 32);
}
static void *
acpi_mapmem (u64 addr, int len)
{
static void *oldmap;
static int oldlen = 0;
if (oldlen)
unmapmem (oldmap, oldlen);
oldlen = len;
oldmap = mapmem (MAPMEM_HPHYS | MAPMEM_WRITE, addr, len);
ASSERT (oldmap);
return oldmap;
}
static void
copy_output_param_blk (void *output, u16 size)
{
void *p;
u16 *length;
p = mapmem_hphys (OUTPUT_PARAM_BLK_ADDR, 0x10000, 0);
length = p;
if (size > *length)
size = *length;
if (size)
memcpy (output, p, size);
unmapmem (p, 0x10000);
}
static void
mmio_gphys_access (phys_t gphysaddr, bool wr, void *buf, uint len, u32 flags)
{
void *p;
if (!len)
return;
p = mapmem_gphys (gphysaddr, len, (wr ? MAPMEM_WRITE : 0) | flags);
ASSERT (p);
if (wr)
memcpy (p, buf, len);
else
memcpy (buf, p, len);
unmapmem (p, len);
}
static void
copy_bios_area (void *save, void *load)
{
void *p;
if (save) {
p = mapmem_hphys (0, BIOS_AREA_SIZE, 0);
if (p) {
memcpy (save, p, BIOS_AREA_SIZE);
unmapmem (p, BIOS_AREA_SIZE);
} else {
printf ("map error\n");
}
}
if (load) {
p = mapmem_hphys (0, BIOS_AREA_SIZE, MAPMEM_WRITE);
if (p) {
memcpy (p, load, BIOS_AREA_SIZE);
unmapmem (p, BIOS_AREA_SIZE);
} else {
printf ("map error\n");
}
}
}
/**
* Unmap all devices from virtual memory.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns None
*/
static void unmap_registers(ws2811_t *ws2811)
{
ws2811_device_t *device = ws2811->device;
if (device->dma)
{
unmapmem((void *)device->dma, sizeof(dma_t));
}
if (device->pwm)
{
unmapmem((void *)device->pwm, sizeof(pwm_t));
}
if (device->cm_pwm)
{
unmapmem((void *)device->cm_pwm, sizeof(cm_pwm_t));
}
if (device->gpio)
{
unmapmem((void *)device->gpio, sizeof(gpio_t));
}
}
static void stop_dma(void)
{
#ifdef WITH_MEMORY_BUFFER
//pthread_cancel(th1);
EndOfApp=1;
pthread_join(th1, NULL);
#endif
if (FileInHandle != -1) {
close(FileInHandle);
FileInHandle = -1;
}
if (dma_reg) {
//Stop Main DMA
dma_reg[DMA_CS+DMA_CHANNEL*0x40] = BCM2708_DMA_INT | BCM2708_DMA_END;
udelay(100);
dma_reg[DMA_CS+DMA_CHANNEL*0x40] = BCM2708_DMA_RESET;
udelay(100);
//Stop DMA PWMFrequency
dma_reg[DMA_CS+DMA_CHANNEL_PWMFREQUENCY*0x40] = BCM2708_DMA_INT | BCM2708_DMA_END;
udelay(100);
dma_reg[DMA_CS+DMA_CHANNEL_PWMFREQUENCY*0x40] = BCM2708_DMA_RESET;
udelay(100);
//printf("Reset DMA Done\n");
clk_reg[GPCLK_CNTL] = 0x5A << 24 | 0 << 9 | 1 << 4 | 6; //NO MASH !!!
udelay(500);
gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (0 << 12); //DISABLE CLOCK - In case used by digilite
clk_reg[PWMCLK_CNTL] = 0x5A000006 | (0 << 9) ;
udelay(500);
clk_reg[PCMCLK_CNTL] = 0x5A000006;
udelay(500);
//printf("Resetpcm Done\n");
pwm_reg[PWM_DMAC] = 0;
udelay(100);
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
udelay(100);
//printf("Reset pwm Done\n");
}
if (mbox.virt_addr != NULL) {
unmapmem(mbox.virt_addr, NUM_PAGES * PAGE_SIZE);
//printf("Unmapmem Done\n");
mem_unlock(mbox.handle, mbox.mem_ref);
//printf("Unmaplock Done\n");
mem_free(mbox.handle, mbox.mem_ref);
//printf("Unmapfree Done\n");
}
}
void dispmanxtest() {
DISPMANX_DISPLAY_HANDLE_T display;
int ret;
DISPMANX_MODEINFO_T displayinfo;
DISPMANX_RESOURCE_HANDLE_T res;
int width = 1024;
int height = 576;
uint32_t vc_image_ptr;
DISPMANX_UPDATE_HANDLE_T update;
VC_RECT_T dst_rect,src_rect;
DISPMANX_ELEMENT_HANDLE_T element;
bcm_host_init();
display = vc_dispmanx_display_open(0);
ret = vc_dispmanx_display_get_info( display, &displayinfo);
assert(ret==0);
printf("display is %dx%d\n",displayinfo.width,displayinfo.height);
res = vc_dispmanx_resource_create(VC_IMAGE_YUV420,width,height,&vc_image_ptr);
vc_image_ptr = vc_dispmanx_resource_get_image_handle(res);
printf("vc_image_ptr %x\n",vc_image_ptr);
assert(res);
update = vc_dispmanx_update_start(10);
assert(update);
vc_dispmanx_rect_set(&src_rect,0,0,width<<16,height<<16);
vc_dispmanx_rect_set(&dst_rect,0,(displayinfo.height - height)/2,width-32,height);
element = vc_dispmanx_element_add(update,display,2000,&dst_rect,res,&src_rect,DISPMANX_PROTECTION_NONE,NULL,NULL,DISPMANX_NO_ROTATE);
ret = vc_dispmanx_update_submit_sync(update);
assert(ret==0);
uint8_t *rawfb = (uint8_t*)mapmem(vc_image_ptr,0x1000);
for (int i=0; i<0x100; i++) {
printf("%02x ",rawfb[i]);
}
printf("\n");
unmapmem(rawfb,0x1000);
puts("sleeping");
sleep(10);
update = vc_dispmanx_update_start(10);
assert(update);
ret = vc_dispmanx_element_remove(update,element);
assert(ret==0);
ret = vc_dispmanx_update_submit_sync(update);
assert(ret==0);
ret = vc_dispmanx_resource_delete(res);
assert(ret==0);
ret = vc_dispmanx_display_close(display);
assert(ret==0);
}
/* Close this library and deallocate everything */
int bcm2835_close(void)
{
if (debug) return 1; /* Success */
unmapmem((void**) &bcm2835_peripherals, bcm2835_peripherals_size);
bcm2835_peripherals = MAP_FAILED;
bcm2835_gpio = MAP_FAILED;
bcm2835_pwm = MAP_FAILED;
bcm2835_clk = MAP_FAILED;
bcm2835_pads = MAP_FAILED;
bcm2835_spi0 = MAP_FAILED;
bcm2835_bsc0 = MAP_FAILED;
bcm2835_bsc1 = MAP_FAILED;
bcm2835_st = MAP_FAILED;
return 1; /* Success */
}
// Execute a nop control list to prove that we have contol.
void testControlLists() {
// First we allocate and map some videocore memory to build our control list in.
// ask the blob to allocate us 256 bytes with 4k alignment, zero it.
unsigned int handle = mem_alloc(mbox, 0x100, 0x1000, MEM_FLAG_COHERENT | MEM_FLAG_ZERO);
if (!handle) {
printf("Error: Unable to allocate memory");
return;
}
// ask the blob to lock our memory at a single location at give is that address.
uint32_t bus_addr = mem_lock(mbox, handle);
// map that address into our local address space.
uint8_t *list = (uint8_t*) mapmem(bus_addr, 0x100);
// Now we construct our control list.
// 255 nops, with a halt somewhere in the middle
for(int i = 0; i < 0xff; i++) {
list[i] = 1; // NOP
}
list[0xbb] = 0; // Halt.
// And then we setup the v3d pipeline to execute the control list.
printf("V3D_CT0CS: 0x%08x\n", v3d[V3D_CT0CS]);
printf("Start Address: 0x%08x\n", bus_addr);
// Current Address = start of our list (bus address)
v3d[V3D_CT0CA] = bus_addr;
// End Address = just after the end of our list (bus address)
// This also starts execution.
v3d[V3D_CT0EA] = bus_addr + 0x100;
// print status while running
printf("V3D_CT0CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT0CS], v3d[V3D_CT0CA]);
// Wait a second to be sure the contorl list execution has finished
while(v3d[V3D_CT0CS] & 0x20);
// print the status again.
printf("V3D_CT0CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT0CS], v3d[V3D_CT0CA]);
// Release memory;
unmapmem((void *) list, 0x100);
mem_unlock(mbox, handle);
mem_free(mbox, handle);
}
static void
wakeup_ap (void)
{
u8 buf[5];
u8 *p;
/* Put a "ljmpw" instruction to the physical address 0 to
avoid the alignment restriction of the SIPI. */
p = mapmem_hphys (0, 5, MAPMEM_WRITE);
memcpy (buf, p, 5);
p[0] = 0xEA; /* ljmpw */
p[1] = wakeup_entry_addr & 0xF;
p[2] = 0;
p[3] = wakeup_entry_addr >> 4;
p[4] = wakeup_entry_addr >> 12;
ap_start_addr (0, wakeup_ap_loopcond, NULL);
memcpy (p, buf, 5);
unmapmem (p, 5);
}
static void
handle_identify_command (struct nvme_host *host,
struct nvme_comp *h_admin_comp,
struct nvme_request *req)
{
struct nvme_cmd *cmd = &req->cmd.std;
u8 tx_type = NVME_CMD_GET_TX_TYPE (cmd);
if (tx_type != NVME_CMD_TX_TYPE_PRP ||
!NVME_CMD_PRP_PTR1 (cmd))
return;
u16 controller_id = cmd->cmd_flags[0] >> 16;
u8 cns = cmd->cmd_flags[0] & 0xFF;
identify_default_filter (cmd->nsid,
controller_id,
cns,
req->h_buf);
if (host->io_interceptor &&
host->io_interceptor->filter_identify_data) {
struct nvme_io_interceptor *io_interceptor;
void *interceptor;
io_interceptor = host->io_interceptor;
interceptor = io_interceptor->interceptor;
io_interceptor->filter_identify_data (interceptor,
cmd->nsid,
controller_id,
cns,
req->h_buf);
}
void *g_buf = mapmem_gphys (req->g_data_ptr.prp_entry.ptr1,
host->page_nbytes,
MAPMEM_WRITE);
memcpy (g_buf, req->h_buf, host->page_nbytes);
unmapmem (g_buf, host->page_nbytes);
}
static void
boot_guest (void)
{
struct config_data *d;
ulong rbx;
if (!enable)
return;
if (currentcpu->cpunum != 0)
panic ("boot from AP");
current->vmctl.read_general_reg (GENERAL_REG_RBX, &rbx);
rbx &= 0xFFFFFFFF;
d = mapmem_hphys (rbx, sizeof *d, 0);
ASSERT (d);
if (d->len != sizeof *d)
panic ("config size mismatch: %d, %d\n", d->len,
(int)sizeof *d);
memcpy (&config, d, sizeof *d);
unmapmem (d, sizeof *d);
do_boot_guest ();
}
static void
wakeup_ap (void)
{
u8 buf[5];
u8 *p;
/* Do nothing if no APs were started before suspend. It is
* true when there is only one logical processor for real or
* the guest OS uses BSP only on UEFI systems. */
if (num_of_processors + 1 == 1)
return;
/* Put a "ljmpw" instruction to the physical address 0 to
avoid the alignment restriction of the SIPI. */
p = mapmem_hphys (0, 5, MAPMEM_WRITE);
memcpy (buf, p, 5);
p[0] = 0xEA; /* ljmpw */
p[1] = wakeup_entry_addr & 0xF;
p[2] = 0;
p[3] = wakeup_entry_addr >> 4;
p[4] = wakeup_entry_addr >> 12;
ap_start_addr (0, wakeup_ap_loopcond, NULL);
memcpy (p, buf, 5);
unmapmem (p, 5);
}
static void
install_int0x15_hook (void)
{
u64 int0x15_code, int0x15_data, int0x15_base;
u64 int0x15_vector_phys = 0x15 * 4;
int count, len1, len2, i;
struct e820_data *q;
u64 b1, l1, b2, l2;
u32 n, nn1, nn2;
u32 t1, t2;
void *p;
len1 = guest_int0x15_hook_end - guest_int0x15_hook;
int0x15_code = alloc_realmodemem (len1);
count = 0;
for (n = 0, nn1 = 1; nn1; n = nn1) {
nn1 = getfakesysmemmap (n, &b1, &l1, &t1);
nn2 = getsysmemmap (n, &b2, &l2, &t2);
if (nn1 == nn2 && b1 == b2 && l1 == l2 && t1 == t2)
continue;
count++;
}
len2 = count * sizeof (struct e820_data);
int0x15_data = alloc_realmodemem (len2);
if (int0x15_data > int0x15_code)
int0x15_base = int0x15_code;
else
int0x15_base = int0x15_data;
int0x15_base &= 0xFFFF0;
/* save old interrupt vector */
read_hphys_l (int0x15_vector_phys, &guest_int0x15_orig, 0);
/* write parameters properly */
guest_int0x15_e801_fake_ax = e801_fake_ax;
guest_int0x15_e801_fake_bx = e801_fake_bx;
guest_int0x15_e820_data_minus0x18 = int0x15_data - int0x15_base - 0x18;
guest_int0x15_e820_end = int0x15_data + len2 - int0x15_base;
/* copy the program code */
p = mapmem_hphys (int0x15_code, len1, MAPMEM_WRITE);
memcpy (p, guest_int0x15_hook, len1);
unmapmem (p, len1);
/* create e820_data */
q = mapmem_hphys (int0x15_data, len2, MAPMEM_WRITE);
i = 0;
for (n = 0, nn1 = 1; nn1; n = nn1) {
nn1 = getfakesysmemmap (n, &b1, &l1, &t1);
nn2 = getsysmemmap (n, &b2, &l2, &t2);
if (nn1 == nn2 && b1 == b2 && l1 == l2 && t1 == t2)
continue;
ASSERT (i < count);
q[i].n = n;
q[i].nn = nn1;
q[i].base = b1;
q[i].len = l1;
q[i].type = t1;
i++;
}
unmapmem (q, len2);
/* set interrupt vector */
write_hphys_l (int0x15_vector_phys, (int0x15_code - int0x15_base) |
(int0x15_base << 12), 0);
}
static int
usb_match_buffers(const struct usb_hook_pattern *data,
struct usb_buffer_list *buffers)
{
struct usb_buffer_list *be;
size_t len;
virt_t vadr;
u64 target;
int i;
while (data) {
/* look for a buffer chunk */
for (be = buffers; be; be = be->next)
if ((data->pid == be->pid) &&
(data->offset <= be->offset))
break;
if (!be)
return -1;
/* extract the target data */
len = be->offset + be->len - data->offset;
if (len < sizeof(u64)) {
u8 c[sizeof(u64)];
/* the target may be placed accoss buffer boundary */
/* former */
len = be->offset + be->len - data->offset;
vadr = (virt_t)mapmem_gphys(be->padr, be->len, 0);
ASSERT(vadr);
for (i=len; i > 0; i--)
c[i-1] = *(u8 *)(vadr + data->offset -
be->offset + i);
unmapmem((void *)vadr, be->len);
/* latter */
be = be->next;
if (!be || (be->pid != data->pid))
return -1;
vadr = (virt_t)mapmem_gphys(be->padr, be->len, 0);
ASSERT(vadr);
for (i = len; i < sizeof(u64); i++)
c[i] = *(u8 *)(vadr + data->offset -
be->offset + i);
unmapmem((void *)vadr, be->len);
target = *(u64 *)c;
} else {
/* */
vadr = (virt_t)mapmem_gphys(be->padr, be->len, 0);
ASSERT(vadr);
target = *(u64 *)(vadr + data->offset);
unmapmem((void *)vadr, be->len);
}
/* match the pattern */
target &= data->mask;
if (target != data->pattern)
return -1;
data = data->next;
}
/* exactly matched */
return 0;
}
// ページの解放
void pro100_free_page(void *v, phys_t ptr)
{
unmapmem(v, PAGESIZE);
free_page_phys(ptr);
}
int main(int argc, char *argv[]) {
int mb = mbox_open();
/* Enable QPU for execution */
int qpu_enabled = !qpu_enable(mb, 1);
if (!qpu_enabled) {
printf("Unable to enable QPU. Check your firmware is latest.");
goto cleanup;
}
/* Allocate GPU memory and map it into ARM address space */
unsigned size = 4096;
unsigned align = 4096;
unsigned handle = mem_alloc(mb, size, align, GPU_MEM_FLG);
if (!handle) {
printf("Failed to allocate GPU memory.");
goto cleanup;
}
void *gpu_pointer = (void *)mem_lock(mb, handle);
void *arm_pointer = mapmem((unsigned)gpu_pointer+GPU_MEM_MAP, size);
unsigned *p = (unsigned *)arm_pointer;
/*
+---------------+ <----+
| QPU Code | |
| ... | |
+---------------+ <--+ |
| Uniforms | | |
+---------------+ | |
| QPU0 Uniform -----+ |
| QPU0 Start -------+
| ... |
| QPUn Uniform |
| QPUn PC -------+
+---------------+
*/
/* Copy QPU program into GPU memory */
unsigned *qpu_code = p;
memcpy(p, program, sizeof program);
p += (sizeof program)/(sizeof program[0]);
/* Build Uniforms */
unsigned *qpu_uniform = p;
*p++ = 1;
/* Build QPU Launch messages */
unsigned *qpu_msg = p;
*p++ = as_gpu_address(qpu_uniform);
*p++ = as_gpu_address(qpu_code);
int i;
for (i=0; i<16+1+2; i++) {
printf("%08x: %08x\n", gpu_pointer+i*4, ((unsigned *)arm_pointer)[i]);
}
printf("qpu exec %08x\n", gpu_pointer + ((void *)qpu_msg - arm_pointer));
/* Launch QPU program and block till its done */
unsigned r = execute_qpu(mb, GPU_QPUS, as_gpu_address(qpu_msg), 1, 10000);
printf("%d\n", r);
cleanup:
/* Release GPU memory */
if (arm_pointer) {
unmapmem(arm_pointer, size);
}
if (handle) {
mem_unlock(mb, handle);
mem_free(mb, handle);
}
/* Release QPU */
if (qpu_enabled) {
qpu_enable(mb, 0);
}
/* Release mailbox */
mbox_close(mb);
}
static int
memdump_msghandler (int m, int c, struct msgbuf *buf, int bufcnt)
{
u8 *q, *tmp;
struct memdump_data *d;
void *recvbuf, *sendbuf;
int recvlen, sendlen, errlen;
char *errbuf;
int num = -1;
struct memdump_gphys_data gphys_data;
struct memdump_gvirt_data gvirt_data;
struct memdump_hvirt_data hvirt_data;
if (m != 1)
return -1;
if (bufcnt < 3)
return -1;
recvbuf = buf[0].base;
recvlen = buf[0].len;
sendbuf = buf[1].base;
sendlen = buf[1].len;
errbuf = buf[2].base;
errlen = buf[2].len;
if (recvlen < sizeof (struct memdump_data))
return -1;
if (errlen > 0)
errbuf[0] = '\0';
d = (struct memdump_data *)recvbuf;
q = sendbuf;
switch ((enum memdump_type)c) {
case MEMDUMP_GPHYS:
gphys_data.physaddr = d->physaddr;
gphys_data.q = q;
gphys_data.sendlen = sendlen;
num = callfunc_and_getint (memdump_gphys, &gphys_data);
break;
case MEMDUMP_HVIRT:
hvirt_data.p = (u8 *)d->virtaddr;
hvirt_data.q = q;
hvirt_data.sendlen = sendlen;
num = callfunc_and_getint (memdump_hvirt, &hvirt_data);
break;
case MEMDUMP_HPHYS:
tmp = mapmem_hphys (d->physaddr, sendlen, 0);
if (tmp) {
hvirt_data.p = tmp;
hvirt_data.q = q;
hvirt_data.sendlen = sendlen;
num = callfunc_and_getint (memdump_hvirt, &hvirt_data);
unmapmem (tmp, sendlen);
} else {
snprintf (errbuf, errlen,
"mapmem_hphys failed (phys=0x%llX)",
d->physaddr);
}
break;
case MEMDUMP_GVIRT:
gvirt_data.d = d;
gvirt_data.q = q;
gvirt_data.sendlen = sendlen;
gvirt_data.errbuf = errbuf;
gvirt_data.errlen = errlen;
num = callfunc_and_getint (memdump_gvirt, &gvirt_data);
break;
default:
return -1;
}
if (num != -1)
snprintf (errbuf, errlen, "exception %d", num);
return 0;
}
