static void do_ramp_unlock(KHRN_FMEM_T *fmem)
{
KHRN_FMEM_TWEAK_T *h;
uint32_t i;
VG_RAMP_T *ramp;
for (h = fmem->ramp.start; h != NULL; h = h->header.next)
{
for (i = 1; i <= h->header.count; i++)
{
ramp = (VG_RAMP_T *)mem_lock(h[i].ramp.handle);
mem_unlock(ramp->data);
mem_unlock(h[i].ramp.handle);
vg_ramp_unretain(h[i].ramp.handle); /* todo: this locks internally */
vg_ramp_release(h[i].ramp.handle, h[i].ramp.i);
}
}
}
void iter_InitSequence(ITER_ID iter_id,VM_ID vm_id,INDEX start,NUM end,NUM step)
{
iter_object *iter = (iter_object*)mem_lock(iter_id);
TUPLE_ID seq = obj_CreateTuple(4);
INT_ID iend = obj_CreateInt(end);
tuple_SetItem(seq,0,iend);
INT_ID istep = obj_CreateInt(step);
tuple_SetItem(seq,1,istep);
INT_ID ipos = obj_CreateInt(start);
tuple_SetItem(seq,2,ipos);
INT_ID istart = obj_CreateInt(start);
tuple_SetItem(seq,3,istart);
iter->tag = seq;
obj_IncRefCount(seq);
iter->block_stack = 0;
iter->iter_func = &iter_Sequence;
mem_unlock(iter_id,1);
}
// 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);
}
char InitDma(void *FunctionTerminate)
{
//printf("Init DMA\n");
// Catch all signals possible - it is vital we kill the DMA engine
// on process exit!
int i;
for (i = 0; i < 64; i++) {
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = FunctionTerminate;//terminate;
sigaction(i, &sa, NULL);
}
NUM_SAMPLES = NUM_SAMPLES_MAX;
/* Use the mailbox interface to the VC to ask for physical memory */
/*unlink(MBFILE);
if (mknod(MBFILE, S_IFCHR|0600, makedev(100, 0)) < 0)
{
printf("Failed to create mailbox device\n");
return 0;
}
*/
mbox.handle = mbox_open();
if (mbox.handle < 0)
{
printf("Failed to open mailbox\n");
return(0);
}
mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES* PAGE_SIZE, PAGE_SIZE, mem_flag);
/* TODO: How do we know that succeeded? */
//printf("mem_ref %x\n", mbox.mem_ref);
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
//printf("bus_addr = %x\n", mbox.bus_addr);
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES* PAGE_SIZE);
//printf("virt_addr %p\n", mbox.virt_addr);
virtbase = (uint32_t *)mbox.virt_addr;
//printf("virtbase %p\n", virtbase);
return(1);
}
OBJECT_ID iter_Iteration(ITER_ID iter_id,VM_ID vm_id)
{
iter_object *iter = (iter_object*)mem_lock(iter_id);
TUPLE_ID it = iter->tag;
mem_unlock(iter_id,0);
OBJECT_ID next = tuple_GetNextItem(it);
if(next == 0 || obj_GetType(next) == TYPE_NONE)
{
tuple_ResetIteration(it);
if(next == 0)
{
OBJECT_ID r = obj_CreateEmpty(TYPE_NONE);
obj_IncRefCount(r);
return(r);
}
}
obj_IncRefCount(next);
return(next);
}
int gpu_fft_alloc(int mb, unsigned size, struct GPU_FFT_PTR *ptr)
{
struct GPU_FFT_HOST host;
struct GPU_FFT_BASE *base;
volatile unsigned *peri;
unsigned handle;
if (gpu_fft_get_host_info(&host))
return -5;
if (qpu_enable(mb, 1))
return -1;
// Shared memory
handle = mem_alloc(mb, size, 4096, host.mem_flg);
if (!handle)
{
qpu_enable(mb, 0);
return -3;
}
peri = (volatile unsigned *)mapmem(host.peri_addr, host.peri_size);
if (!peri)
{
mem_free(mb, handle);
qpu_enable(mb, 0);
return -4;
}
ptr->vc = mem_lock(mb, handle);
ptr->arm.vptr = mapmem(BUS_TO_PHYS(ptr->vc + host.mem_map), size);
base = (struct GPU_FFT_BASE *)ptr->arm.vptr;
base->peri = peri;
base->peri_size = host.peri_size;
base->mb = mb;
base->handle = handle;
base->size = size;
return 0;
}
static void khrn_image_to_native_buffer(KHRN_IMAGE_T *image, android_native_buffer_t *buffer, void *bits)
{
MEM_HANDLE_T mem_handle;
uint8_t *ptr;
int size;
mem_handle = image->mh_storage;
ptr = (uint8_t*)mem_lock(mem_handle);
size = mem_get_size(mem_handle);
// memcpy(bits, ptr, size);
{
/* V3D Y axis calibrarion and that of Android is opposite, so copying from top of src to bottom of dest */
/* This could be a place where we can look for optimization */
int image_height, image_stride;
int buffer_height, buffer_stride;
int copy_height, copy_stride, i;
image_height = (uint16_t)(khrn_image_is_packed_mask(image->format) ? khrn_image_get_packed_mask_height(image->height) : image->height);
image_stride = image->stride;
buffer_height = buffer->height;
buffer_stride = buffer->width * bpp;
copy_height = (image_height <= buffer_height) ? image_height : buffer_height;
copy_stride = (image_stride <= buffer_stride) ? image_stride : buffer_stride;
#ifndef BCG_FB_LAYOUT
ptr += (image->offset + size - image_stride);
#endif
for (i=0; i< copy_height; i++) {
memcpy(bits, ptr, copy_stride);
bits = (void *)((char *)bits + buffer_stride);
#ifndef BCG_FB_LAYOUT
ptr -= image_stride;
#else
ptr += image_stride;
#endif
}
}
}
void egl_disp_ready(EGL_DISP_SLOT_HANDLE_T slot_handle, bool skip)
{
#ifdef BRCM_V3D_OPT
return;
#endif
DISP_T *disp = slot_handle_get_disp(slot_handle);
uint32_t slot = slot_handle_get_slot(slot_handle);
#ifdef EGL_DISP_FPS
/* todo: this isn't safe when resizing with swap interval 0 */
MEM_HANDLE_T image_handle = disp->in_use.images[slot & (next_power_of_2(disp->in_use.n) - 1)];
KHRN_IMAGE_T *image = (KHRN_IMAGE_T *)mem_lock(image_handle);
fps_put(image, image->width - 4, 4);
mem_unlock(image_handle);
#endif
if (skip) {
disp->in_use.slots[slot].swap_interval = 0;
disp->in_use.slots[slot].skip = true;
}
khrn_barrier();
disp->in_use.slots[slot].ready = true;
notify_llat(disp);
}
void out_free_page(void *block)
{
mem_lock();
in_free_page(block);
mem_unlock();
}
static void resolve_rx_bulks(VCHIQ_STATE_T *state)
{
VCHIQ_CHANNEL_T *local = state->local;
VCHIQ_CHANNEL_T *remote = state->remote;
int retrigger = 0;
VCHIQ_TRACE("%d: rrb %x %x", state->id, local->bulk.process, local->bulk.insert);
while (local->bulk.process != local->bulk.insert) {
VCHIQ_RX_BULK_T *rx_bulk = &local->bulk.bulks[local->bulk.process & (VCHIQ_NUM_CURRENT_BULKS - 1)];
VCHIQ_SERVICE_T *service;
VCHIQ_TX_BULK_T *tx_bulk;
/* Ensure the service isn't being closed */
if (rx_bulk->dstport < VCHIQ_MAX_SERVICES)
{
service = &local->services[rx_bulk->dstport];
if (service->srvstate == VCHIQ_SRVSTATE_OPEN)
{
VCHIQ_SERVICE_T *remote_service = &remote->services[service->remoteport];
if (remote_service->process == remote_service->insert)
break; /* No tx bulk - stall */
tx_bulk = &remote_service->bulks[remote_service->process & (VCHIQ_NUM_SERVICE_BULKS - 1)];
if (rx_bulk->size == tx_bulk->size)
{
const void *tx_data;
void *rx_data;
#ifdef VCHIQ_VC_SIDE
vcos_assert(tx_bulk->handle == VCHI_MEM_HANDLE_INVALID);
tx_data = tx_bulk->data;
#else
if (tx_bulk->handle == VCHI_MEM_HANDLE_INVALID)
tx_data = tx_bulk->data;
else
{
tx_data = (const char *)mem_lock(tx_bulk->handle) + (int)tx_bulk->data;
tx_bulk->data = tx_data; /* Overwrite to avoid an abort */
}
#endif
if (rx_bulk->handle == VCHI_MEM_HANDLE_INVALID)
rx_data = rx_bulk->data;
else
{
rx_data = (char *)mem_lock(rx_bulk->handle) + (int)rx_bulk->data;
rx_bulk->data = rx_data; /* Overwrite to avoid an abort */
}
vchiq_copy_bulk_from_host(rx_data, tx_data, rx_bulk->size);
if (rx_bulk->handle != VCHI_MEM_HANDLE_INVALID)
mem_unlock(rx_bulk->handle);
#ifndef VCHIQ_VC_SIDE
if (tx_bulk->handle != VCHI_MEM_HANDLE_INVALID)
mem_unlock(tx_bulk->handle);
#endif
VCHIQ_TRACE("%d: rrb %x<->%x(%d)", state->id, local->bulk.process, remote_service->process, service->remoteport);
}
else
{
/* Abort these non-matching transfers */
rx_bulk->data = NULL;
tx_bulk->data = NULL;
VCHIQ_TRACE("%d: rrb %x<->%x(%d) ABORTED", state->id, local->bulk.process, remote_service->process, service->remoteport);
}
remote_service->process++;
remote_event_signal(&remote->trigger);
}
else
{
rx_bulk->data = NULL; /* Aborted */
VCHIQ_TRACE("%d: rrb %x ABORTED", state->id, local->bulk.process);
}
}
local->bulk.process++;
retrigger = 1;
}
if (retrigger)
remote_event_signal_local(&local->trigger);
}
static void resolve_tx_bulks(VCHIQ_STATE_T *state)
{
VCHIQ_CHANNEL_T *local = state->local;
VCHIQ_CHANNEL_T *remote = state->remote;
int retrigger = 0;
VCHIQ_TRACE("%d: rtb %x %x", state->id, remote->bulk.process, remote->bulk.insert);
while (remote->bulk.process != remote->bulk.insert) {
VCHIQ_RX_BULK_T *rx_bulk = &remote->bulk.bulks[remote->bulk.process & (VCHIQ_NUM_CURRENT_BULKS - 1)];
VCHIQ_SERVICE_T *service;
VCHIQ_TX_BULK_T *tx_bulk;
if (VCHIQ_PORT_IS_VALID(rx_bulk->dstport))
{
/* Find connected service */
service = get_connected_service(local, rx_bulk->dstport);
if (service == NULL)
{
rx_bulk->data = NULL; /* Abort */
VCHIQ_TRACE("%d: rtb %x(%d) ABORTED", state->id, remote->bulk.process, rx_bulk->dstport);
}
else
{
if (service->process == service->insert)
break; /* No matching transmit Stall */
tx_bulk = &service->bulks[service->process & (VCHIQ_NUM_SERVICE_BULKS - 1)];
if (rx_bulk->size == tx_bulk->size)
{
const void *tx_data;
vcos_assert(rx_bulk->handle == VCHI_MEM_HANDLE_INVALID);
if (tx_bulk->handle == VCHI_MEM_HANDLE_INVALID)
tx_data = tx_bulk->data;
else
{
tx_data = (const char *)mem_lock(tx_bulk->handle) + (int)tx_bulk->data;
tx_bulk->data = tx_data; /* Overwrite to avoid an abort */
}
vchiq_copy_bulk_to_host(rx_bulk->data, tx_data, rx_bulk->size);
if (tx_bulk->handle != VCHI_MEM_HANDLE_INVALID)
mem_unlock(tx_bulk->handle);
VCHIQ_TRACE("%d: rtb %x<->%x(%d)", state->id, service->process, remote->bulk.process, rx_bulk->dstport);
}
else
{
/* Abort these transfers */
rx_bulk->data = NULL;
tx_bulk->data = NULL;
VCHIQ_TRACE("%d: rtb %x<->%x(%d) ABORTED", state->id, service->process, remote->bulk.process, rx_bulk->dstport);
}
retrigger = 1;
service->process++;
}
}
remote->bulk.process++;
remote_event_signal(&remote->trigger);
}
if (retrigger)
remote_event_signal_local(&local->trigger);
}
OBJECT_ID BinaryOp(OBJECT_ID tos_id,OBJECT_ID tos1_id,unsigned char op)
{
object *tos = (object*)mem_lock(tos_id);
object *tos1 = (object*)mem_lock(tos1_id);
OBJECT_ID new_tos_id = 0;
if(tos->type == TYPE_UNICODE && tos1->type == TYPE_UNICODE && op == OPCODE_BINARY_ADD) //string add -> returns string
{
mem_unlock(tos1_id,0);
mem_unlock(tos_id,0);
return(StringAdd(tos1_id,tos_id));
}
if(tos->type == TYPE_INT && tos1->type == TYPE_UNICODE && op == OPCODE_BINARY_MULTIPLY) //unicode multiply -> returns unicode
{
//printf("string multiply\n");
mem_unlock(tos1_id,0);
mem_unlock(tos_id,0);
return(StringMultiply(tos1_id,tos_id));
}
if(tos->type == TYPE_INT && tos1->type == TYPE_TUPLE && op == OPCODE_BINARY_MULTIPLY) //tuple multiply -> returns tuple
{
NUM a =(NUM) ((int_object*)tos)->value;
NUM mnum = tuple_GetLen(tos1_id);
if(mnum == 1)
{
OBJECT_ID old = tos1_id;
tos1_id = tuple_GetItem(tos1_id,0);
mem_unlock(old,0);//TODO what if tos1_id == old .... ???????
tos1 = (object*)mem_lock(tos1_id);
}
TUPLE_ID mtr = obj_CreateTuple(a);
for(NUM i = 0; i < a; i++)
{
tuple_SetItem(mtr,i,tos1_id);
}
#ifdef USE_DEBUGGING
if((debug_level & DEBUG_VERBOSE_STEP) > 0)
obj_Dump(mtr,0,1);
#endif
mem_unlock(tos1_id,0);
mem_unlock(tos_id,0);
return(mtr);
}
if(tos->type == TYPE_BINARY_FLOAT || tos1->type == TYPE_BINARY_FLOAT) //mixed op -> returns float
{
FLOAT af = 0.0f;
FLOAT bf = 0.0f;
//printf("float ret\n");
if(tos->type == TYPE_INT)
//if (tos1->type == TYPE_INT)
{
af = (FLOAT) ((int_object*)tos)->value;
//af = (FLOAT) ((int_object*)tos1)->value;
}
if(tos1->type == TYPE_INT)
//if (tos->type == TYPE_INT)
{
bf = (FLOAT) ((int_object*)tos1)->value;
//bf = (FLOAT) ((int_object*)tos)->value;
}
if(tos->type == TYPE_BINARY_FLOAT)
//if (tos1->type == TYPE_BINARY_FLOAT)
{
af = ((float_object*)tos)->value;
//af = ((float_object*)tos1)->value;
}
if(tos1->type == TYPE_BINARY_FLOAT)
//if (tos->type == TYPE_BINARY_FLOAT)
{
bf = ((float_object*)tos1)->value;
//bf = ((float_object*)tos)->value;
}
mem_unlock(tos1_id,0);
mem_unlock(tos_id,0);
new_tos_id = obj_CreateFloat(0);
float_object *new_tos = (float_object*)mem_lock(new_tos_id);
switch(op)
{
case OPCODE_INPLACE_MULTIPLY:
case OPCODE_BINARY_MULTIPLY:
{
new_tos->value = bf * af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g * %7g = %7g\n", bf, af, bf * af);
#endif
}
break;
case OPCODE_INPLACE_OR:
case OPCODE_BINARY_OR:
{
new_tos->value = (long)bf | (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g | %7g = %7g\n", bf, af, (long)bf | (long)af);
#endif
}
break;
case OPCODE_INPLACE_XOR:
case OPCODE_BINARY_XOR:
{
new_tos->value = (long)bf ^ (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g ^ %7g = %7g\n", bf, af, (long)bf ^ (long)af);
#endif
}
break;
case OPCODE_INPLACE_AND:
case OPCODE_BINARY_AND:
{
new_tos->value = (long)bf & (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g & %7g = %7g\n", bf, af, (long)bf & (long)af);
#endif
}
break;
case OPCODE_INPLACE_LSHIFT:
case OPCODE_BINARY_LSHIFT:
{
new_tos->value = (long)bf << (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g << %7g = %7g\n", bf, af, (long)bf << (long)af);
#endif
}
break;
case OPCODE_INPLACE_RSHIFT:
case OPCODE_BINARY_RSHIFT:
{
new_tos->value = (long)bf >> (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g>> %7g = %7g\n", bf, af, (long)bf >> (long)af);
#endif
}
break;
case OPCODE_INPLACE_MODULO:
case OPCODE_BINARY_MODULO:
{
new_tos->value = (long)bf % (long)af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g %% %7g = %7g\n", bf, af, (long)bf % (long)af);
#endif
}
break;
case OPCODE_INPLACE_FLOOR_DIVIDE:
case OPCODE_BINARY_FLOOR_DIVIDE:
{
new_tos->value = floor(bf / af);
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g // %7g = %7g\n", bf, af, floor(bf / af));
#endif
}
break;
case OPCODE_INPLACE_TRUE_DIVIDE:
case OPCODE_BINARY_TRUE_DIVIDE:
{
new_tos->value = bf / af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g / %7g = %7g\n", bf, af, bf / af);
#endif
}
break;
case OPCODE_INPLACE_SUBTRACT:
case OPCODE_BINARY_SUBTRACT:
{
new_tos->value = bf-af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g - %7g = %7g\n", bf, af, bf - af);
#endif
}
break;
case OPCODE_INPLACE_POWER:
case OPCODE_BINARY_POWER:
{
new_tos->value = num_pow(bf, af); //TODO this will most likely not work correctly
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g ** %7g = %7g\n", bf, af, num_pow(bf, af));
#endif
}
break;
case OPCODE_INPLACE_ADD:
case OPCODE_BINARY_ADD:
{
new_tos->value = bf + af;
#ifdef USE_DEBUGGING
debug_printf(DEBUG_VERBOSE_STEP,"%7g + %7g = %7g\n", bf, af, bf + af);
#endif
}
break;
}
}
int
main(int argc, char **argv)
{
parseargs(argc, argv);
mbox.handle = mbox_open();
if (mbox.handle < 0)
fatal("Failed to open mailbox\n");
unsigned mbox_board_rev = get_board_revision(mbox.handle);
printf("MBox Board Revision: %#x\n", mbox_board_rev);
get_model(mbox_board_rev);
unsigned mbox_dma_channels = get_dma_channels(mbox.handle);
printf("DMA Channels Info: %#x, using DMA Channel: %d\n", mbox_dma_channels, DMA_CHAN_NUM);
printf("Using hardware: %5s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
printf("Number of channels: %5d\n", (int)num_channels);
printf("PWM frequency: %5d Hz\n", 1000000/CYCLE_TIME_US);
printf("PWM steps: %5d\n", NUM_SAMPLES);
printf("Maximum period (100 %%): %5dus\n", CYCLE_TIME_US);
printf("Minimum period (%1.3f%%): %5dus\n", 100.0*SAMPLE_US / CYCLE_TIME_US, SAMPLE_US);
printf("DMA Base: %#010x\n", DMA_BASE);
setup_sighandlers();
/* map the registers for all DMA Channels */
dma_virt_base = map_peripheral(DMA_BASE, (DMA_CHAN_SIZE * (DMA_CHAN_MAX + 1)));
/* set dma_reg to point to the DMA Channel we are using */
dma_reg = dma_virt_base + DMA_CHAN_NUM * (DMA_CHAN_SIZE / sizeof(dma_reg));
pwm_reg = map_peripheral(PWM_BASE, PWM_LEN);
pcm_reg = map_peripheral(PCM_BASE, PCM_LEN);
clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN);
/* Use the mailbox interface to the VC to ask for physical memory */
mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * PAGE_SIZE, PAGE_SIZE, mem_flag);
/* TODO: How do we know that succeeded? */
dprintf("mem_ref %u\n", mbox.mem_ref);
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
dprintf("bus_addr = %#x\n", mbox.bus_addr);
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES * PAGE_SIZE);
dprintf("virt_addr %p\n", mbox.virt_addr);
if ((unsigned long)mbox.virt_addr & (PAGE_SIZE-1))
fatal("pi-blaster: Virtual address is not page aligned\n");
/* we are done with the mbox */
mbox_close(mbox.handle);
mbox.handle = -1;
//fatal("TempFatal\n");
init_ctrl_data();
init_hardware();
init_channel_pwm();
// Init pin2gpio array with 0/false values to avoid locking all of them as PWM.
init_pin2gpio();
// Only calls update_pwm after ctrl_data calculates the pin mask to unlock all pins on start.
init_pwm();
unlink(DEVFILE);
if (mkfifo(DEVFILE, 0666) < 0)
fatal("pi-blaster: Failed to create %s: %m\n", DEVFILE);
if (chmod(DEVFILE, 0666) < 0)
fatal("pi-blaster: Failed to set permissions on %s: %m\n", DEVFILE);
printf("Initialised, ");
if (daemonize) {
if (daemon(0,1) < 0)
fatal("pi-blaster: Failed to daemonize process: %m\n");
else
printf("Daemonized, ");
}
printf("Reading %s.\n", DEVFILE);
go_go_go();
return 0;
}
/**
* Allocate and initialize memory, buffers, pages, PWM, DMA, and GPIO.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns 0 on success, -1 otherwise.
*/
int ws2811_init(ws2811_t *ws2811)
{
ws2811_device_t *device = NULL;
int chan;
// Zero mbox; non-zero values indicate action needed on cleanup
memset(&mbox, 0, sizeof(mbox));
ws2811->device = malloc(sizeof(*ws2811->device));
if (!ws2811->device)
{
return -1;
}
device = ws2811->device;
// Determine how much physical memory we need for DMA
mbox.size = PWM_BYTE_COUNT(max_channel_led_count(ws2811), ws2811->freq) +
+ sizeof(dma_cb_t);
// Round up to page size multiple
mbox.size = (mbox.size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1);
// Use the mailbox interface to request memory from the VideoCore
// We specifiy (-1) for the handle rather than calling mbox_open()
// so multiple users can share the resource.
mbox.handle = -1; // mbox_open();
mbox.mem_ref = mem_alloc(mbox.handle, mbox.size, PAGE_SIZE,
board_info_sdram_address() == 0x40000000 ? 0xC : 0x4);
if (mbox.mem_ref == (unsigned) ~0)
{
return -1;
}
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
if (mbox.bus_addr == (unsigned) ~0)
{
mem_free(mbox.handle, mbox.size);
return -1;
}
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), mbox.size);
// Initialize all pointers to NULL. Any non-NULL pointers will be freed on cleanup.
device->pwm_raw = NULL;
device->dma_cb = NULL;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811->channel[chan].leds = NULL;
}
// Allocate the LED buffers
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811_channel_t *channel = &ws2811->channel[chan];
channel->leds = malloc(sizeof(ws2811_led_t) * channel->count);
if (!channel->leds)
{
goto err;
}
memset(channel->leds, 0, sizeof(ws2811_led_t) * channel->count);
}
device->dma_cb = (dma_cb_t *)mbox.virt_addr;
device->pwm_raw = (uint8_t *)mbox.virt_addr + sizeof(dma_cb_t);
pwm_raw_init(ws2811);
memset((dma_cb_t *)device->dma_cb, 0, sizeof(dma_cb_t));
// Cache the DMA control block bus address
device->dma_cb_addr = addr_to_bus(device->dma_cb);
// Map the physical registers into userspace
if (map_registers(ws2811))
{
goto err;
}
// Initialize the GPIO pins
if (gpio_init(ws2811))
{
unmap_registers(ws2811);
goto err;
}
// Setup the PWM, clocks, and DMA
if (setup_pwm(ws2811))
{
unmap_registers(ws2811);
goto err;
}
return 0;
err:
ws2811_cleanup(ws2811);
return -1;
}
// Render a single triangle to memory.
void testTriangle() {
// Like above, we allocate/lock/map some videocore memory
// I'm just shoving everything in a single buffer because I'm lazy
// 8Mb, 4k alignment
unsigned int handle = mem_alloc(mbox, 0x800000, 0x1000, MEM_FLAG_COHERENT | MEM_FLAG_ZERO);
if (!handle) {
printf("Error: Unable to allocate memory");
return;
}
uint32_t bus_addr = mem_lock(mbox, handle);
uint8_t *list = (uint8_t*) mapmem(bus_addr, 0x800000);
uint8_t *p = list;
// Configuration stuff
// Tile Binning Configuration.
// Tile state data is 48 bytes per tile, I think it can be thrown away
// as soon as binning is finished.
addbyte(&p, 112);
addword(&p, bus_addr + 0x6200); // tile allocation memory address
addword(&p, 0x8000); // tile allocation memory size
addword(&p, bus_addr + 0x100); // Tile state data address
addbyte(&p, 30); // 1920/64
addbyte(&p, 17); // 1080/64 (16.875)
addbyte(&p, 0x04); // config
// Start tile binning.
addbyte(&p, 6);
// Primitive type
addbyte(&p, 56);
addbyte(&p, 0x32); // 16 bit triangle
// Clip Window
addbyte(&p, 102);
addshort(&p, 0);
addshort(&p, 0);
addshort(&p, 1920); // width
addshort(&p, 1080); // height
// State
addbyte(&p, 96);
addbyte(&p, 0x03); // enable both foward and back facing polygons
addbyte(&p, 0x00); // depth testing disabled
addbyte(&p, 0x02); // enable early depth write
// Viewport offset
addbyte(&p, 103);
addshort(&p, 0);
addshort(&p, 0);
// The triangle
// No Vertex Shader state (takes pre-transformed vertexes,
// so we don't have to supply a working coordinate shader to test the binner.
addbyte(&p, 65);
addword(&p, bus_addr + 0x80); // Shader Record
// primitive index list
addbyte(&p, 32);
addbyte(&p, 0x04); // 8bit index, trinagles
addword(&p, 3); // Length
addword(&p, bus_addr + 0x70); // address
addword(&p, 2); // Maximum index
// End of bin list
// Flush
addbyte(&p, 5);
// Nop
addbyte(&p, 1);
// Halt
addbyte(&p, 0);
int length = p - list;
assert(length < 0x80);
// Shader Record
p = list + 0x80;
addbyte(&p, 0x01); // flags
addbyte(&p, 6*4); // stride
addbyte(&p, 0xcc); // num uniforms (not used)
addbyte(&p, 3); // num varyings
addword(&p, bus_addr + 0xfe00); // Fragment shader code
addword(&p, bus_addr + 0xff00); // Fragment shader uniforms
addword(&p, bus_addr + 0xa0); // Vertex Data
// Vertex Data
p = list + 0xa0;
// Vertex: Top, red
addshort(&p, (1920/2) << 4); // X in 12.4 fixed point
addshort(&p, 200 << 4); // Y in 12.4 fixed point
addfloat(&p, 1.0f); // Z
addfloat(&p, 1.0f); // 1/W
addfloat(&p, 1.0f); // Varying 0 (Red)
addfloat(&p, 0.0f); // Varying 1 (Green)
addfloat(&p, 0.0f); // Varying 2 (Blue)
// Vertex: bottom left, Green
addshort(&p, 560 << 4); // X in 12.4 fixed point
addshort(&p, 800 << 4); // Y in 12.4 fixed point
addfloat(&p, 1.0f); // Z
addfloat(&p, 1.0f); // 1/W
addfloat(&p, 0.0f); // Varying 0 (Red)
addfloat(&p, 1.0f); // Varying 1 (Green)
addfloat(&p, 0.0f); // Varying 2 (Blue)
// Vertex: bottom right, Blue
addshort(&p, 1360 << 4); // X in 12.4 fixed point
addshort(&p, 800 << 4); // Y in 12.4 fixed point
addfloat(&p, 1.0f); // Z
addfloat(&p, 1.0f); // 1/W
addfloat(&p, 0.0f); // Varying 0 (Red)
addfloat(&p, 0.0f); // Varying 1 (Green)
addfloat(&p, 1.0f); // Varying 2 (Blue)
// Vertex list
p = list + 0x70;
addbyte(&p, 0); // top
addbyte(&p, 1); // bottom left
addbyte(&p, 2); // bottom right
// fragment shader
p = list + 0xfe00;
addword(&p, 0x958e0dbf);
addword(&p, 0xd1724823); /* mov r0, vary; mov r3.8d, 1.0 */
addword(&p, 0x818e7176);
addword(&p, 0x40024821); /* fadd r0, r0, r5; mov r1, vary */
addword(&p, 0x818e7376);
addword(&p, 0x10024862); /* fadd r1, r1, r5; mov r2, vary */
addword(&p, 0x819e7540);
addword(&p, 0x114248a3); /* fadd r2, r2, r5; mov r3.8a, r0 */
addword(&p, 0x809e7009);
addword(&p, 0x115049e3); /* nop; mov r3.8b, r1 */
addword(&p, 0x809e7012);
addword(&p, 0x116049e3); /* nop; mov r3.8c, r2 */
addword(&p, 0x159e76c0);
addword(&p, 0x30020ba7); /* mov tlbc, r3; nop; thrend */
addword(&p, 0x009e7000);
addword(&p, 0x100009e7); /* nop; nop; nop */
addword(&p, 0x009e7000);
addword(&p, 0x500009e7); /* nop; nop; sbdone */
// Render control list
p = list + 0xe200;
// Clear color
addbyte(&p, 114);
addword(&p, 0xff000000); // Opaque Black
addword(&p, 0xff000000); // 32 bit clear colours need to be repeated twice
addword(&p, 0);
addbyte(&p, 0);
// Tile Rendering Mode Configuration
addbyte(&p, 113);
addword(&p, bus_addr + 0x10000); // framebuffer addresss
addshort(&p, 1920); // width
addshort(&p, 1080); // height
addbyte(&p, 0x04); // framebuffer mode (linear rgba8888)
addbyte(&p, 0x00);
// Do a store of the first tile to force the tile buffer to be cleared
// Tile Coordinates
addbyte(&p, 115);
addbyte(&p, 0);
addbyte(&p, 0);
// Store Tile Buffer General
addbyte(&p, 28);
addshort(&p, 0); // Store nothing (just clear)
addword(&p, 0); // no address is needed
// Link all binned lists together
for(int x = 0; x < 30; x++) {
for(int y = 0; y < 17; y++) {
// Tile Coordinates
addbyte(&p, 115);
addbyte(&p, x);
addbyte(&p, y);
// Call Tile sublist
addbyte(&p, 17);
addword(&p, bus_addr + 0x6200 + (y * 30 + x) * 32);
// Last tile needs a special store instruction
if(x == 29 && y == 16) {
// Store resolved tile color buffer and signal end of frame
addbyte(&p, 25);
} else {
// Store resolved tile color buffer
addbyte(&p, 24);
}
}
}
int render_length = p - (list + 0xe200);
// Run our control list
printf("Binner control list constructed\n");
printf("Start Address: 0x%08x, length: 0x%x\n", bus_addr, length);
v3d[V3D_CT0CA] = bus_addr;
v3d[V3D_CT0EA] = bus_addr + length;
printf("V3D_CT0CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT0CS], v3d[V3D_CT0CA]);
// Wait for control list to execute
while(v3d[V3D_CT0CS] & 0x20);
printf("V3D_CT0CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT0CS], v3d[V3D_CT0CA]);
printf("V3D_CT1CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT1CS], v3d[V3D_CT1CA]);
v3d[V3D_CT1CA] = bus_addr + 0xe200;
v3d[V3D_CT1EA] = bus_addr + 0xe200 + render_length;
while(v3d[V3D_CT1CS] & 0x20);
printf("V3D_CT1CS: 0x%08x, Address: 0x%08x\n", v3d[V3D_CT1CS], v3d[V3D_CT1CA]);
v3d[V3D_CT1CS] = 0x20;
// just dump the frame to a file
FILE *f = fopen("frame.data", "w");
fwrite(list + 0x10000, (1920*1080*4), 1, f);
fclose(f);
printf("frame buffer memory dumpped to frame.data\n");
// Release resources
unmapmem((void *) list, 0x800000);
mem_unlock(mbox, handle);
mem_free(mbox, handle);
}
/**
* create/open a table
*
* b0 = ON for non-memory file (file is created by the user, so, it must exists in disk)
* b1 = ON for create always (truncate if it is exists); FALSE for open if exists or create if it does not exists
*/
dbt_t dbt_create(const char *fileName, int flags) {
int i, t = -1;
if (opt_usevmt) {
flags |= 1;
}
// find a free VMT
for (i = 0; i < MAX_VMT_FILES; i++) {
if (vmt[i].used == 0) {
t = i;
vmt[t].sign = (intptr_t) "VMTH";
vmt[t].ver = 1;
vmt[t].flags = flags;
vmt[t].count = 0;
vmt[t].size = 0;
break;
}
}
if (t == -1) // no vmt handle free
return -1;
// create the table
if (flags & 1) {
// VMT: use file
char fullfile[OS_PATHNAME_SIZE];
int f_handle, i_handle;
// open database
strcpy(vmt[t].base, fileName);
sprintf(fullfile, "%s.dbt", vmt[t].base);
vmt[t].f_handle = f_handle = open(fullfile, O_RDWR | O_BINARY);
if ((flags & 0x2) || vmt[t].f_handle == -1) { // create always
vmt[t].f_handle = f_handle = open(fullfile, O_RDWR | O_BINARY | O_CREAT | O_TRUNC, 0660);
// create index
sprintf(fullfile, "%s.dbi", vmt[t].base);
vmt[t].i_handle = i_handle = open(fullfile, O_RDWR | O_BINARY | O_CREAT | O_TRUNC, 0660);
} else {
// open index
sprintf(fullfile, "%s.dbi", vmt[t].base);
vmt[t].i_handle = i_handle = open(fullfile, O_RDWR | O_BINARY);
dbt_file_read_header(t);
}
vmt[t].f_handle = f_handle;
vmt[t].i_handle = i_handle;
vmt[t].used = 1;
if (flags & 4)
vmt[t].count = 0;
dbt_file_write_header(t);
} else {
// VMT: use memory
int i;
// preallocate some records
vmt[t].size = MEM_VMT_GROWSIZE;
vmt[t].m_handle = mem_alloc(sizeof(mem_t) * vmt[t].size);
// reset new data
vmt[t].m_table = (mem_t *)mem_lock(vmt[t].m_handle);
for (i = 0; i < vmt[t].size; i++) {
vmt[t].m_table[i] = 0; // make them NULL
}
//
vmt[t].used = 1;
}
return t; // return vmt-handle
}
char InitDma(void *FunctionTerminate)
{
DMA_CHANNEL=4;
if (system("rm -f linuxversion.txt") != 0) {
fprintf(stderr, "rm failed\n");
}
if (system("uname -r >> linuxversion.txt") != 0) {
fprintf(stderr, "uname failed\n");
}
char *line = NULL;
size_t size;
// int fLinux=open("Flinuxversion.txt", "r');
FILE * flinux=fopen("linuxversion.txt", "r");
if (getline(&line, &size, flinux) == -1)
{
printf("Could no get Linux version\n");
}
else
{
if(line[0]=='3')
{
printf("Wheezy\n");
DMA_CHANNEL=DMA_CHANNEL_WHEEZY;
}
if(line[0]=='4')
{
printf("Jessie\n");
DMA_CHANNEL=DMA_CHANNEL_JESSIE;
}
}
//printf("Init DMA\n");
// Catch all signals possible - it is vital we kill the DMA engine
// on process exit!
int i;
for (i = 0; i < 64; i++) {
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = FunctionTerminate;//terminate;
sigaction(i, &sa, NULL);
}
//NUM_SAMPLES = NUM_SAMPLES_MAX;
/* Use the mailbox interface to the VC to ask for physical memory */
/*
unlink(MBFILE);
if (mknod(MBFILE, S_IFCHR|0600, makedev(100, 0)) < 0)
{
printf("Failed to create mailbox device\n");
return 0;
}*/
mbox.handle = mbox_open();
if (mbox.handle < 0)
{
printf("Failed to open mailbox\n");
return(0);
}
mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES* PAGE_SIZE, PAGE_SIZE, mem_flag);
/* TODO: How do we know that succeeded? */
//printf("mem_ref %x\n", mbox.mem_ref);
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
//printf("bus_addr = %x\n", mbox.bus_addr);
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES* PAGE_SIZE);
//printf("virt_addr %p\n", mbox.virt_addr);
virtbase = (uint8_t *)((uint32_t *)mbox.virt_addr);
//printf("virtbase %p\n", virtbase);
return(1);
}
void out_add_free(void *block, oskit_size_t size)
{
mem_lock();
in_add_free(block,size);
mem_unlock();
}
/* Only available if debugging turned on. */
void out_dump(void)
{
mem_lock();
in_dump();
mem_unlock();
}
/**
* Allocate and initialize memory, buffers, pages, PWM, DMA, and GPIO.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns 0 on success, -1 otherwise.
*/
int ws2811_init(ws2811_t *ws2811)
{
ws2811_device_t *device;
const rpi_hw_t *rpi_hw;
int chan;
ws2811->rpi_hw = rpi_hw_detect();
if (!ws2811->rpi_hw)
{
return -1;
}
rpi_hw = ws2811->rpi_hw;
ws2811->device = malloc(sizeof(*ws2811->device));
if (!ws2811->device)
{
return -1;
}
device = ws2811->device;
// Determine how much physical memory we need for DMA
device->mbox.size = PWM_BYTE_COUNT(max_channel_led_count(ws2811), ws2811->freq) +
sizeof(dma_cb_t);
// Round up to page size multiple
device->mbox.size = (device->mbox.size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1);
device->mbox.handle = mbox_open();
if (device->mbox.handle == -1)
{
return -1;
}
device->mbox.mem_ref = mem_alloc(device->mbox.handle, device->mbox.size, PAGE_SIZE,
rpi_hw->videocore_base == 0x40000000 ? 0xC : 0x4);
if (device->mbox.mem_ref == 0)
{
return -1;
}
device->mbox.bus_addr = mem_lock(device->mbox.handle, device->mbox.mem_ref);
if (device->mbox.bus_addr == (uint32_t) ~0UL)
{
mem_free(device->mbox.handle, device->mbox.size);
return -1;
}
device->mbox.virt_addr = mapmem(BUS_TO_PHYS(device->mbox.bus_addr), device->mbox.size);
// Initialize all pointers to NULL. Any non-NULL pointers will be freed on cleanup.
device->pwm_raw = NULL;
device->dma_cb = NULL;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811->channel[chan].leds = NULL;
}
// Allocate the LED buffers
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811_channel_t *channel = &ws2811->channel[chan];
channel->leds = malloc(sizeof(ws2811_led_t) * channel->count);
if (!channel->leds)
{
goto err;
}
memset(channel->leds, 0, sizeof(ws2811_led_t) * channel->count);
if (!channel->strip_type)
{
channel->strip_type=WS2811_STRIP_RGB;
}
}
device->dma_cb = (dma_cb_t *)device->mbox.virt_addr;
device->pwm_raw = (uint8_t *)device->mbox.virt_addr + sizeof(dma_cb_t);
pwm_raw_init(ws2811);
memset((dma_cb_t *)device->dma_cb, 0, sizeof(dma_cb_t));
// Cache the DMA control block bus address
device->dma_cb_addr = addr_to_bus(device, device->dma_cb);
// Map the physical registers into userspace
if (map_registers(ws2811))
{
goto err;
}
// Initialize the GPIO pins
if (gpio_init(ws2811))
{
unmap_registers(ws2811);
goto err;
}
// Setup the PWM, clocks, and DMA
if (setup_pwm(ws2811))
{
unmap_registers(ws2811);
goto err;
}
return 0;
err:
ws2811_cleanup(ws2811);
return -1;
}
HI_VOID* HI_REALLOC(HI_U32 u32ModuleID, HI_VOID *pMemAddr, HI_U32 u32Size)
{
HI_VOID* pNewMemAddr = NULL;
#ifdef CMN_MMGR_SUPPORT
struct head *p;
if (NULL != pMemAddr && g_fnModuleCallback)/* when pMemAddr is NULL do malloc.*/
{
// Add memory info to MODULE MGR
if (0 == u32Size)/* when s = 0 realloc() acts like free(). */
{
HI_FREE(u32ModuleID, pMemAddr);
}
else
{
HI_S32 s32Ret = HI_FAILURE;
mem_lock(&g_MemMutex);
//lookup the module info.
s32Ret = g_fnModuleCallback(u32ModuleID, MEM_TYPE_USR, 0);
if(s32Ret != HI_SUCCESS)
{
mem_unlock(&g_MemMutex);
return NULL;
}
p = MEM_Find(pMemAddr);
if (NULL != p)
{
pNewMemAddr = realloc(pMemAddr, u32Size);
if ( NULL != pNewMemAddr)
{
g_fnModuleCallback(u32ModuleID, MEM_TYPE_USR, (-1)*(p->size));
MEM_Replace(p, pNewMemAddr, u32Size);
g_fnModuleCallback(u32ModuleID, MEM_TYPE_USR, u32Size);
mem_unlock(&g_MemMutex);
return pNewMemAddr;
}
}
mem_unlock(&g_MemMutex);
}
}
else
{
return HI_MALLOC(u32ModuleID, u32Size);
}
#else
pNewMemAddr = realloc(pMemAddr, u32Size);
#endif
return pNewMemAddr;
}
/* ----------------------------------------------------------------------
* the object was allocated from the relocatable mempool; provide a
* lightweight api call to lock the memory in place before use.
*
* returns a pointer to the locked-in-memory data.
* -------------------------------------------------------------------- */
void *
vc_pool_lock( VC_POOL_OBJECT_T *object )
{
vcos_assert( object->magic == OBJECT_MAGIC );
return mem_lock( object->mem );
}
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);
}
void out_stats(void)
{
mem_lock();
in_stats();
mem_unlock();
}
static void vc_image_to_RSO_memory(VC_IMAGE_T *image, android_native_buffer_t *buffer, void *bits)
{
KHRN_IMAGE_FORMAT_T src_format, dst_format;
KHRN_IMAGE_WRAP_T src, dst;
uint8_t* src_ptr;
int conv_width, conv_height;
dst_format = convert_color_format(buffer->format);
if (!dst_format)
return;
// Swap red and blue. Do t-format conversion if necessary.
switch (image->type) {
case VC_IMAGE_TF_RGBA32:
src_format = ABGR_8888_TF;
break;
case VC_IMAGE_TF_RGBX32:
src_format = XBGR_8888_TF;
break;
case VC_IMAGE_TF_RGB565:
src_format = RGB_565_TF;
break;
case VC_IMAGE_TF_RGBA5551:
src_format = RGBA_5551_TF;
break;
case VC_IMAGE_TF_RGBA16:
src_format = RGBA_4444_TF;
break;
case VC_IMAGE_RGBA32:
src_format = ABGR_8888_RSO;
break;
case VC_IMAGE_RGBX32:
src_format = XBGR_8888_RSO;
break;
case VC_IMAGE_RGB565:
src_format = RGB_565_RSO;
break;
default:
UNREACHABLE();
src_format = 0;
break;
}
if (!src_format)
return;
src_ptr = (image->mem_handle != MEM_INVALID_HANDLE) ? (uint8_t*)mem_lock(image->mem_handle) : (uint8_t*)image->image_data;
conv_width = (image->width <= buffer->width) ? image->width : buffer->width;
conv_height = (image->height <= buffer->height) ? image->height : buffer->height;
khrn_image_wrap(&src, src_format, conv_width, conv_height, image->pitch, src_ptr);
khrn_image_wrap(&dst, dst_format, conv_width, conv_height, buffer->width * bpp, bits);
if (src.format == dst.format)
khrn_image_copy(&dst, &src);
else
khrn_image_wrap_convert(&dst, &src, IMAGE_CONV_GL);
if (image->mem_handle != MEM_INVALID_HANDLE)
mem_unlock(image->mem_handle);
}
void out_remove_free(void *block, oskit_size_t size)
{
mem_lock();
in_remove_free(block,size);
mem_unlock();
}
int tx(uint32_t carrier_freq, char *audio_file, uint16_t pi, char *ps, char *rt, float ppm, char *control_pipe) {
// Catch all signals possible - it is vital we kill the DMA engine
// on process exit!
for (int i = 0; i < 64; i++) {
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = terminate;
sigaction(i, &sa, NULL);
}
dma_reg = map_peripheral(DMA_VIRT_BASE, DMA_LEN);
pwm_reg = map_peripheral(PWM_VIRT_BASE, PWM_LEN);
clk_reg = map_peripheral(CLK_VIRT_BASE, CLK_LEN);
gpio_reg = map_peripheral(GPIO_VIRT_BASE, GPIO_LEN);
// Use the mailbox interface to the VC to ask for physical memory.
mbox.handle = mbox_open();
if (mbox.handle < 0)
fatal("Failed to open mailbox. Check kernel support for vcio / BCM2708 mailbox.\n");
printf("Allocating physical memory: size = %d ", NUM_PAGES * 4096);
if(! (mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * 4096, 4096, MEM_FLAG))) {
fatal("Could not allocate memory.\n");
}
// TODO: How do we know that succeeded?
printf("mem_ref = %u ", mbox.mem_ref);
if(! (mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref))) {
fatal("Could not lock memory.\n");
}
printf("bus_addr = %x ", mbox.bus_addr);
if(! (mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES * 4096))) {
fatal("Could not map memory.\n");
}
printf("virt_addr = %p\n", mbox.virt_addr);
// GPIO4 needs to be ALT FUNC 0 to output the clock
gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (4 << 12);
// Program GPCLK to use MASH setting 1, so fractional dividers work
clk_reg[GPCLK_CNTL] = 0x5A << 24 | 6;
udelay(100);
clk_reg[GPCLK_CNTL] = 0x5A << 24 | 1 << 9 | 1 << 4 | 6;
ctl = (struct control_data_s *) mbox.virt_addr;
dma_cb_t *cbp = ctl->cb;
uint32_t phys_sample_dst = CM_GP0DIV;
uint32_t phys_pwm_fifo_addr = PWM_PHYS_BASE + 0x18;
// Calculate the frequency control word
// The fractional part is stored in the lower 12 bits
uint32_t freq_ctl = ((float)(PLLFREQ / carrier_freq)) * ( 1 << 12 );
for (int i = 0; i < NUM_SAMPLES; i++) {
ctl->sample[i] = 0x5a << 24 | freq_ctl; // Silence
// Write a frequency sample
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
cbp->src = mem_virt_to_phys(ctl->sample + i);
cbp->dst = phys_sample_dst;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
// Delay
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
cbp->src = mem_virt_to_phys(mbox.virt_addr);
cbp->dst = phys_pwm_fifo_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
}
cbp--;
cbp->next = mem_virt_to_phys(mbox.virt_addr);
// Here we define the rate at which we want to update the GPCLK control
// register.
//
// Set the range to 2 bits. PLLD is at 500 MHz, therefore to get 228 kHz
// we need a divisor of 500000 / 2 / 228 = 1096.491228
//
// This is 1096 + 2012*2^-12 theoretically
//
// However the fractional part may have to be adjusted to take the actual
// frequency of your Pi's oscillator into account. For example on my Pi,
// the fractional part should be 1916 instead of 2012 to get exactly
// 228 kHz. However RDS decoding is still okay even at 2012.
//
// So we use the 'ppm' parameter to compensate for the oscillator error
float divider = (500000./(2*228*(1.+ppm/1.e6)));
uint32_t idivider = (uint32_t) divider;
uint32_t fdivider = (uint32_t) ((divider - idivider)*pow(2, 12));
printf("ppm corr is %.4f, divider is %.4f (%d + %d*2^-12) [nominal 1096.4912].\n",
ppm, divider, idivider, fdivider);
pwm_reg[PWM_CTL] = 0;
udelay(10);
clk_reg[PWMCLK_CNTL] = 0x5A000006; // Source=PLLD and disable
udelay(100);
// theorically : 1096 + 2012*2^-12
clk_reg[PWMCLK_DIV] = 0x5A000000 | (idivider<<12) | fdivider;
udelay(100);
clk_reg[PWMCLK_CNTL] = 0x5A000216; // Source=PLLD and enable + MASH filter 1
udelay(100);
pwm_reg[PWM_RNG1] = 2;
udelay(10);
pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
udelay(10);
// Initialise the DMA
dma_reg[DMA_CS] = BCM2708_DMA_RESET;
udelay(10);
dma_reg[DMA_CS] = BCM2708_DMA_INT | BCM2708_DMA_END;
dma_reg[DMA_CONBLK_AD] = mem_virt_to_phys(ctl->cb);
dma_reg[DMA_DEBUG] = 7; // clear debug error flags
dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes
uint32_t last_cb = (uint32_t)ctl->cb;
// Data structures for baseband data
float data[DATA_SIZE];
int data_len = 0;
int data_index = 0;
// Initialize the baseband generator
if(fm_mpx_open(audio_file, DATA_SIZE) < 0) return 1;
// Initialize the RDS modulator
char myps[9] = {0};
set_rds_pi(pi);
set_rds_rt(rt);
uint16_t count = 0;
uint16_t count2 = 0;
int varying_ps = 0;
if(ps) {
set_rds_ps(ps);
printf("PI: %04X, PS: \"%s\".\n", pi, ps);
} else {
printf("PI: %04X, PS: <Varying>.\n", pi);
varying_ps = 1;
}
printf("RT: \"%s\"\n", rt);
// Initialize the control pipe reader
if(control_pipe) {
if(open_control_pipe(control_pipe) == 0) {
printf("Reading control commands on %s.\n", control_pipe);
} else {
printf("Failed to open control pipe: %s.\n", control_pipe);
control_pipe = NULL;
}
}
printf("Starting to transmit on %3.1f MHz.\n", carrier_freq/1e6);
for (;;) {
// Default (varying) PS
if(varying_ps) {
if(count == 512) {
snprintf(myps, 9, "%08d", count2);
set_rds_ps(myps);
count2++;
}
if(count == 1024) {
set_rds_ps("RPi-Live");
count = 0;
}
count++;
}
if(control_pipe && poll_control_pipe() == CONTROL_PIPE_PS_SET) {
varying_ps = 0;
}
usleep(5000);
uint32_t cur_cb = mem_phys_to_virt(dma_reg[DMA_CONBLK_AD]);
int last_sample = (last_cb - (uint32_t)mbox.virt_addr) / (sizeof(dma_cb_t) * 2);
int this_sample = (cur_cb - (uint32_t)mbox.virt_addr) / (sizeof(dma_cb_t) * 2);
int free_slots = this_sample - last_sample;
if (free_slots < 0)
free_slots += NUM_SAMPLES;
while (free_slots >= SUBSIZE) {
// get more baseband samples if necessary
if(data_len == 0) {
if( fm_mpx_get_samples(data) < 0 ) {
terminate(0);
}
data_len = DATA_SIZE;
data_index = 0;
}
float dval = data[data_index] * (DEVIATION / 10.);
data_index++;
data_len--;
int intval = (int)((floor)(dval));
//int frac = (int)((dval - (float)intval) * SUBSIZE);
ctl->sample[last_sample++] = (0x5A << 24 | freq_ctl) + intval; //(frac > j ? intval + 1 : intval);
if (last_sample == NUM_SAMPLES)
last_sample = 0;
free_slots -= SUBSIZE;
}
last_cb = (uint32_t)mbox.virt_addr + last_sample * sizeof(dma_cb_t) * 2;
}
return 0;
}
