int enqueue_block(volatile msg_queue_move_t* src){
if(queue_size == MOTION_QUEUE_LENGTH)
return -1;
uint32_t offset = (queue_head + queue_size) & MOTION_QUEUE_MASK;
vmemcpy(&(motion_queue[offset]), src, sizeof(msg_queue_move_t));
queue_size++;
return MOTION_QUEUE_LENGTH - queue_size;
}
int main(int argc, char **argv)
{
vme_bus_handle_t bus_handle;
vme_master_handle_t handle = NULL;
uint64_t vme_addr;
int am, dw, flags, use_memcpy, c;
void *phys_addr, *buffer, *addr;
size_t nelem, size;
int rval = 0;
/* Set up default values
*/
vme_addr = 0;
am = VME_A32SD;
dw = VME_D8;
flags = VME_CTL_PWEN;
nelem = 1;
phys_addr = NULL;
use_memcpy = 1;
rval = 0;
/* Parse the argument list
*/
while (-1 != (c = getopt(argc, argv, "a:A:d:e:f:p:")))
switch (c) {
case 'a': /* Address modifier */
if (strtoam(optarg, &am)) {
fprintf(stderr, "Invalid address modifier\n");
return -1;
}
break;
case 'A': /* VMEbus address */
vme_addr = strtoul(optarg, NULL, 0);
break;
case 'd': /* VMEbus access data width */
if (strtodw(optarg, &dw)) {
fprintf(stderr, "Invalid access data width\n");
return -1;
}
/* If a specific data width was requested, then don't
use memcpy. */
use_memcpy = 0;
break;
case 'e': /* Number of elements */
nelem = strtoul(optarg, NULL, 0);
break;
case 'f': /* Flags */
if (strtoflags(optarg, &flags)) {
fprintf(stderr, "Invalid flags\n");
return -1;
}
break;
case 'p': /* Physical address */
phys_addr = (void *) strtoul(optarg, NULL, 0);
break;
default:
fprintf(stderr,
"USAGE: vme_peek [-a addr_mod] [-A vme_addr]"
"[-d dwidth][-e num_elem][-f flags]"
"[-p phys_addr]");
return -1;
}
/* Use number of elements and data width to calculate the size.
*/
size = nelem * dw;
/* Initialize
*/
if (vme_init(&bus_handle)) {
perror("vme_init");
return -1;
}
DPRINTF("vme_addr=0x%lx\n", (unsigned long) vme_addr);
DPRINTF("am=0x%x\n", am);
DPRINTF("dw=0x%x\n", dw);
DPRINTF("size=0x%x\n", size);
DPRINTF("flags=0x%x\n", flags);
DPRINTF("phys_addr=0x%lx\n", (unsigned long) phys_addr);
/* Create a handle to the necessary window
*/
if (vme_master_window_create(bus_handle, &handle, vme_addr, am,
size, flags, phys_addr)) {
perror("vme_master_window_create");
rval = -1;
goto error_create;
}
/* This is not necessary, I just put this function here to test it and
demonstrate it's use.
*/
phys_addr = vme_master_window_phys_addr(bus_handle, handle);
if (NULL == phys_addr) {
perror("vme_master_window_phys_addr");
}
DPRINTF("Window physical address = 0x%lx\n", (unsigned long) phys_addr);
/* Map in the window
*/
addr = vme_master_window_map(bus_handle, handle, 0);
if (NULL == addr) {
perror("vme_master_window_map");
rval = -1;
goto error_map;
}
/* Create a temporary buffer to copy data into before printing it so
performance measurements won't account for local I/O.
*/
buffer = malloc(size);
if (NULL == buffer) {
perror("malloc");
rval = -1;
goto error_malloc;
}
/* Do the transfer. If data width was not given at the command line,
then use memcpy for fast transfers. Note that this may cause the
output to be byte-swapped.
*/
if (use_memcpy) {
memcpy(buffer, addr, size);
} else {
if (vmemcpy(buffer, addr, nelem, dw)) {
perror("vmemcpy");
rval = -1;
goto error_transfer;
}
}
/* Print data to stdout.
*/
if (vdump(buffer, nelem, dw)) {
perror("vdump");
rval = -1;
goto error_transfer;
}
error_transfer:
free(buffer);
error_malloc:
if (vme_master_window_unmap(bus_handle, handle)) {
perror("vme_master_window_unmap");
rval = -1;
}
error_map:
if (vme_master_window_release(bus_handle, handle)) {
perror("vme_master_window_release");
rval = -1;
}
error_create:
if (vme_term(bus_handle)) {
perror("vme_term");
return -1;
}
return rval;
}
void vntick(){
if( pvnscks != NULL ){
static fd_set rds, wts;
u32 i, mfd = 0;
struct timeval tv = { 0, 0 };
f32 curt = vcurTime();
mfd = 0;
for( i = 0; i < pvnconsSize; ++i ){
if( curt - pvntimes[ i ] > vn_timeout )
pvnremcon( i );
}
FD_ZERO( &rds );
for( i = 0; i < pvnscksSize; ++i ){
if( pvnscks[ i ] > mfd )
mfd = pvnscks[ i ];
#pragma warning( push )
#pragma warning( disable: 4127 )
FD_SET( pvnscks[ i ], &rds );
#pragma warning( pop )
}
if( mfd ){
pvncheck( "select failed!", select( mfd + 1, &rds, NULL, NULL, &tv ), NULL, NULL );
for( i = 0; i < pvnscksSize; ++i ){
if( FD_ISSET( pvnscks[ i ], &rds ) ){
SOCKET as = accept( pvnscks[ i ], NULL, NULL );
pvncheck( "accept failed", as, NULL, NULL );
pvnaddcon( &as, pvncbs[ i ] );
}
}
}
FD_ZERO( &rds );
FD_ZERO( &wts );
mfd = 0;
for( i = 0; i < pvnconsSize; ++i ){
if( pvncons[ i ] > mfd )
mfd = pvncons[ i ];
#pragma warning( push )
#pragma warning( disable: 4127 )
FD_SET( pvncons[ i ], &rds );
FD_SET( pvncons[ i ], &wts );
#pragma warning( pop )
}
if( mfd ){
pvncheck( "select failed!", select( mfd + 1, &rds, NULL, NULL, &tv ), NULL, NULL );
for( i = 0; i < pvnconsSize; ++i ){
u32 qn = ( (u32*)vmem( pvnqs ) )[ i ];
if( FD_ISSET( pvncons[ i ], &rds ) ){
static u8 buf[ vnbufsize ];
int ans = recv( pvncons[ i ], buf, vnbufsize - 1, 0 );
pvncheck( "recv failed!", ans, NULL, NULL );
if( ans == SOCKET_ERROR )
ans = 0;
if( !ans ){
pvnremcon( i );
} else{
ans = pvnccbs[ i ]( buf, ans, pvncids[ i ], vcurTime() );
if( !ans )
pvnremcon( i );
else if( vsize( ans ) ){
vappend( qn, vmem( ans ), vsize( ans ) );
}
}
}
if( FD_ISSET( pvncons[ i ], &wts ) && vsize( qn ) ){
int ans = send( pvncons[ i ], vmem( qn ), vsize( qn ), 0 );
pvncheck( "send failed!", ans, NULL, NULL );
if( ans == SOCKET_ERROR )
ans = 0;
if( (u32)ans != vsize( qn ) ){
u32 ns = vsize( qn ) - (u32)ans;
u8* tb = vsmalloc( ns );
vmemcpy( tb, ((u8*)vmem( qn )) + ans, ns );
verase( qn );
vappend( qn, tb, ns );
vsfree( tb );
}else
verase( qn );
}
}
}
}
}
/* I/O write */
static void trace_dev_write(void *opaque, target_phys_addr_t offset, uint32_t value)
{
trace_dev_state *s = (trace_dev_state *)opaque;
offset -= s->base;
switch (offset >> 2) {
case TRACE_DEV_REG_SWITCH: // context switch, switch to pid
trace_switch(value);
#ifdef DEBUG
printf("QEMU.trace: kernel, context switch %u\n", value);
#endif
break;
case TRACE_DEV_REG_TGID: // save the tgid for the following fork/clone
tgid = value;
#ifdef DEBUG
printf("QEMU.trace: kernel, tgid %u\n", value);
#endif
break;
case TRACE_DEV_REG_FORK: // fork, fork new pid
trace_fork(tgid, value);
#ifdef DEBUG
printf("QEMU.trace: kernel, fork %u\n", value);
#endif
break;
case TRACE_DEV_REG_CLONE: // fork, clone new pid (i.e. thread)
trace_clone(tgid, value);
#ifdef DEBUG
printf("QEMU.trace: kernel, clone %u\n", value);
#endif
break;
case TRACE_DEV_REG_EXECVE_VMSTART: // execve, vstart
vstart = value;
break;
case TRACE_DEV_REG_EXECVE_VMEND: // execve, vend
vend = value;
break;
case TRACE_DEV_REG_EXECVE_OFFSET: // execve, offset in EXE
eoff = value;
break;
case TRACE_DEV_REG_EXECVE_EXEPATH: // init exec, path of EXE
vstrcpy(value, path, CLIENT_PAGE_SIZE);
trace_init_exec(vstart, vend, eoff, path);
#ifdef DEBUG
printf("QEMU.trace: kernel, init exec [%lx,%lx]@%lx [%s]\n", vstart, vend, eoff, path);
#endif
path[0] = 0;
break;
case TRACE_DEV_REG_CMDLINE_LEN: // execve, process cmdline length
cmdlen = value;
break;
case TRACE_DEV_REG_CMDLINE: // execve, process cmdline
vmemcpy(value, arg, cmdlen);
trace_execve(arg, cmdlen);
#ifdef DEBUG
{
int i;
for (i = 0; i < cmdlen; i ++)
if (i != cmdlen - 1 && arg[i] == 0)
arg[i] = ' ';
printf("QEMU.trace: kernel, execve %s[%d]\n", arg, cmdlen);
}
#endif
arg[0] = 0;
break;
case TRACE_DEV_REG_EXIT: // exit, exit current process with exit code
trace_exit(value);
#ifdef DEBUG
printf("QEMU.trace: kernel, exit %x\n", value);
#endif
break;
case TRACE_DEV_REG_NAME: // record thread name
vstrcpy(value, path, CLIENT_PAGE_SIZE);
// Remove the trailing newline if it exists
int len = strlen(path);
if (path[len - 1] == '\n') {
path[len - 1] = 0;
}
trace_name(path);
#ifdef DEBUG
printf("QEMU.trace: kernel, name %s\n", path);
#endif
break;
case TRACE_DEV_REG_MMAP_EXEPATH: // mmap, path of EXE, the others are same as execve
vstrcpy(value, path, CLIENT_PAGE_SIZE);
trace_mmap(vstart, vend, eoff, path);
#ifdef DEBUG
printf("QEMU.trace: kernel, mmap [%lx,%lx]@%lx [%s]\n", vstart, vend, eoff, path);
#endif
path[0] = 0;
break;
case TRACE_DEV_REG_INIT_PID: // init, name the pid that starts before device registered
pid = value;
break;
case TRACE_DEV_REG_INIT_NAME: // init, the comm of the init pid
vstrcpy(value, path, CLIENT_PAGE_SIZE);
trace_init_name(tgid, pid, path);
#ifdef DEBUG
printf("QEMU.trace: kernel, init name %u [%s]\n", pid, path);
#endif
path[0] = 0;
break;
case TRACE_DEV_REG_DYN_SYM_ADDR: // dynamic symbol address
dsaddr = value;
break;
case TRACE_DEV_REG_DYN_SYM: // add dynamic symbol
vstrcpy(value, arg, CLIENT_PAGE_SIZE);
trace_dynamic_symbol_add(dsaddr, arg);
#ifdef DEBUG
printf("QEMU.trace: dynamic symbol %lx:%s\n", dsaddr, arg);
#endif
arg[0] = 0;
break;
case TRACE_DEV_REG_REMOVE_ADDR: // remove dynamic symbol addr
trace_dynamic_symbol_remove(value);
#ifdef DEBUG
printf("QEMU.trace: dynamic symbol remove %lx\n", dsaddr);
#endif
arg[0] = 0;
break;
case TRACE_DEV_REG_PRINT_STR: // print string
vstrcpy(value, arg, CLIENT_PAGE_SIZE);
printf("%s", arg);
arg[0] = 0;
break;
case TRACE_DEV_REG_PRINT_NUM_DEC: // print number in decimal
printf("%d", value);
break;
case TRACE_DEV_REG_PRINT_NUM_HEX: // print number in hexical
printf("%x", value);
break;
case TRACE_DEV_REG_STOP_EMU: // stop the VM execution
// To ensure that the number of instructions executed in this
// block is correct, we pretend that there was an exception.
trace_exception(0);
cpu_single_env->exception_index = EXCP_HLT;
cpu_single_env->halted = 1;
qemu_system_shutdown_request();
cpu_loop_exit();
break;
case TRACE_DEV_REG_ENABLE: // tracing enable: 0 = stop, 1 = start
if (value == 1)
start_tracing();
else if (value == 0) {
stop_tracing();
// To ensure that the number of instructions executed in this
// block is correct, we pretend that there was an exception.
trace_exception(0);
}
break;
case TRACE_DEV_REG_UNMAP_START:
unmap_start = value;
break;
case TRACE_DEV_REG_UNMAP_END:
trace_munmap(unmap_start, value);
break;
default:
cpu_abort(cpu_single_env, "trace_dev_write: Bad offset %x\n", offset);
break;
}
}
int main(int argc, char **argv)
{
vme_bus_handle_t bus_handle;
vme_slave_handle_t handle;
uint64_t vme_addr;
int as, dw, flags, use_memcpy, c, rval;
void *phys_addr, *buffer, *addr;
size_t nelem, size;
FILE *datafile;
/* Set up default values
*/
vme_addr = 0;
as = VME_A32;
dw = VME_D8;
flags = VME_CTL_PWEN | VME_CTL_PREN;
nelem = 1;
phys_addr = NULL;
use_memcpy = 1;
datafile = stdin;
rval = 0;
/* Parse the argument list
*/
while (-1 != (c = getopt(argc, argv, "a:A:d:f:F:p:")))
switch (c) {
case 'a': /* Address modifier */
if (strtoas(optarg, &as)) {
fprintf(stderr, "Invalid address space\n");
return -1;
}
break;
case 'A': /* VMEbus address */
vme_addr = strtoul(optarg, NULL, 0);
break;
case 'd': /* VMEbus access data width */
if (strtodw(optarg, &dw)) {
fprintf(stderr, "Invalid access data width\n");
return -1;
}
/* If a specific data width was requested, then don't
use memcpy. */
use_memcpy = 0;
break;
case 'f': /* Flags */
if (strtoflags(optarg, &flags)) {
fprintf(stderr, "Invalid flags\n");
return -1;
}
break;
case 'F': /* Get data from a file */
if (NULL == (datafile = fopen(optarg, "r"))) {
perror("fopen");
return -1;
}
break;
case 'p': /* Physical address */
phys_addr = (void *) strtoul(optarg, NULL, 0);
break;
default:
fprintf(stderr, "USAGE: vme_slave_poke [-a addr_mod]"
"[-A vme_addr][-d dwidth][-f flags][-F file]"
"[-p phys_addr][hexdata...]");
return -1;
}
/* Get the data to be transfered
*/
if (argc > optind) { /* Get argument from the command line */
nelem = argc - optind;
if (vconvert_hexargs(&buffer, nelem, dw, argv + optind)) {
return -1;
}
} else { /* Read data from file or stdin */
nelem = vconvert_hexvals(&buffer, dw, datafile);
if (0 == nelem) {
fprintf(stderr, "No data to transfer\n");
return -1;
}
}
/* Use number of elements and data width to calculate the size.
*/
size = nelem * dw;
/* Initialize
*/
if (vme_init(&bus_handle)) {
perror("vme_init");
rval = -1;
goto error_init;
}
DPRINTF("vme_addr=0x%lx\n", (unsigned long) vme_addr);
DPRINTF("as=0x%x\n", as);
DPRINTF("dw=0x%x\n", dw);
DPRINTF("size=0x%x\n", size);
DPRINTF("flags=0x%x\n", flags);
DPRINTF("phys_addr=0x%lx\n", (unsigned long) phys_addr);
/* Create a handle to the necessary window
*/
if (vme_slave_window_create(bus_handle, &handle, vme_addr, as, size,
flags, phys_addr)) {
perror("vme_slave_window_create");
rval = -1;
goto error_create;
}
/* This is not necessary, I just put this function here to test it and
demonstrate it's use.
*/
phys_addr = vme_slave_window_phys_addr(bus_handle, handle);
if (NULL == phys_addr) {
perror("vme_slave_window_phys_addr");
}
DPRINTF("Window physical address = 0x%lx\n", (unsigned long) phys_addr);
/* Map in the window
*/
addr = vme_slave_window_map(bus_handle, handle, 0);
if (NULL == addr) {
perror("vme_slave_window_map");
rval = -1;
goto error_map;
}
/* If no data width is specified, the use memcpy to transfer the data.
This may result in byte-swapping though.
*/
if (use_memcpy) {
memcpy(addr, buffer, size);
} else {
if (vmemcpy(addr, buffer, nelem, dw)) {
perror("vmemcpy");
rval = -1;
}
}
if (vme_slave_window_unmap(bus_handle, handle)) {
perror("vme_slave_window_unmap");
rval = -1;
}
error_map:
if (vme_slave_window_release(bus_handle, handle)) {
perror("vme_slave_window_release");
rval = -1;
}
error_create:
if (vme_term(bus_handle)) {
perror("vme_term");
rval = -1;
}
error_init:
free(buffer);
return rval;
}
s64int loadElfProgram(const char *path, struct Program *prog, struct VM *vm)
{
struct VNode node;
Elf64_Ehdr *ehdr;
Elf64_Phdr *phdr;
s64int ret, n;
u64int len, i, j, size, remain;
char *buffer;
ret = vfsOpen(path, 0, &node);
if (ret!=0)
return ret;
ehdr = (Elf64_Ehdr *)kMalloc(sizeof(Elf64_Ehdr));
if (!ehdr)
return -1;
//buffer = (char*)kMallocEx(PAGE_SIZE, 1, 0);
buffer = (char*)kMalloc(PAGE_SIZE);
n = vfsRead(&node, sizeof(Elf64_Ehdr), ehdr);
if ((n == sizeof(Elf64_Ehdr)) && isValidElfHeader(ehdr)) {
prog->entry = ehdr->e_entry;
len = ehdr->e_phentsize * ehdr->e_phnum;
phdr = (Elf64_Phdr*)kMalloc(len);
vfsSeek(&node, ehdr->e_phoff, SEEK_SET);
n = vfsRead(&node, len, phdr);
if (n==len) {
for (i=0; i<ehdr->e_phnum; i++) {
if (phdr[i].p_type == PT_LOAD) {
ret = vmAddArea(vm, phdr[i].p_vaddr, phdr[i].p_memsz,
VMA_TYPE_HEAP | VMA_OWNER_USER | VMA_STATUS_USED);
if (ret!=0) {
break;
}
size = phdr[i].p_filesz;
remain = size & 0xfff;
size -= remain;
vfsSeek(&node, phdr[i].p_offset, SEEK_SET);
for (j=0; j<size; j+=PAGE_SIZE) {
len = vfsRead(&node, PAGE_SIZE, buffer);
if (len!=PAGE_SIZE) {
ret = -1;
break;
}
//DBG("%x,%x",vm,currentTask->vm);
vmemcpy(vm, (void*)(phdr[i].p_vaddr+j), currentTask->vm, buffer, PAGE_SIZE);
}
if (remain) {
len = vfsRead(&node, remain, buffer);
if (len == remain)
vmemcpy(vm, (void*)(phdr[i].p_vaddr+size), currentTask->vm, buffer, remain);
else
ret = -1;
}
if (ret!=0)
break;
}
}
}
// setup stack
if (ret == 0) {
ret = vmAddArea(vm, USER_STACK_TOP-USER_STACK_SIZE,
USER_STACK_SIZE, VMA_TYPE_STACK | VMA_OWNER_USER | VMA_STATUS_USED);
}
kFree(phdr);
} else
ret = -1;
kFree(buffer);
kFree(ehdr);
vfsClose(&node);
return ret;
}
