//-----------Begin of function UnitGroup::move--------------//
//
// This function designate a troop to move to the given destination.
// PathReuse is called by the troop to find the path to the destination
//
// <int> destXLoc, destYLoc - the destination to move to.
// <bool> forceMoveFlag - force move flag (default: false)
// <char> remoteAction - remote action
//
void UnitGroup::move(int destXLoc, int destYLoc, bool forceMoveFlag, char remoteAction)
{
if( !remoteAction && remote.is_enable() )
{
// packet structure : <xLoc> <yLoc> <forceMoveFlag> <write_mem...>
short *shortPtr = (short *)remote.new_send_queue_msg(MSG_UNIT_MOVE, sizeof(short)*3 + get_write_len() );
shortPtr[0] = destXLoc;
shortPtr[1] = destYLoc;
shortPtr[2] = forceMoveFlag ? 1 : 0;
write_mem(shortPtr+3);
return;
}
group_by_mobile_type();
if(unit_group_land.size())
unit_group_land.exe_move(destXLoc, destYLoc, forceMoveFlag);
if(unit_group_air.size())
unit_group_air.exe_move(destXLoc, destYLoc, forceMoveFlag);
}
void stm1_inst(int inst, ARMProc *proc) {
int i;
LSMAddrResult *result;
LSMAddrResult *(*func)(int, ARMProc *);
int register_list;
int address;
int start_address;
int end_address;
int value;
int **reg;
if(!check_condition(proc, inst))
return;
register_list = getbits(inst, 0, 16);
lsm_addr_modes = lsm_addressing_dict();
reg = &proc->r0;
for(i = 0; i < LSM_ADDRESSING_NUMBER; i++) {
if(test_inst(lsm_addr_modes[i], inst)) {
func = lsm_addr_modes[i]->execute;
result = (*func)(inst, proc);
break;
}
}
start_address = result->start_address;
end_address = result->end_address;
address = start_address;
for(i = 0; i < 16; i++) {
if(getbit(register_list, i)) {
write_mem(proc, address, 4, **(reg + i));
address += 4;
}
}
if(end_address != address - 4) {
fprintf(stderr, "Load memory error");
}
}
static void return_write_mem(char *str)
{
unsigned long address;
unsigned long value;
char *end;
address = simple_strtoul(str, &end, 0);
if (*end != '-') {
printk("Bad address in %s\n", str);
return;
}
value = simple_strtoul(end + 1, &end, 0);
if (*end != '\0') {
printk("Bad value in %s\n", str);
return;
}
dump_address = (unsigned long *)address;
write_mem(value);
}
//----------- Begin of function UnitGroup::return_camp ---------------//
//
void UnitGroup::return_camp(char remoteAction)
{
if( !remoteAction && remote.is_enable() )
{
// packet structure : <write_mem...>
short *shortPtr = (short *)remote.new_send_queue_msg(MSG_UNITS_RETURN_CAMP, get_write_len() );
write_mem(shortPtr);
return;
}
Unit* unitPtr;
for( int i=1 ; i<=size() ; i++ )
{
unitPtr = get_unit(i);
if( unitPtr->home_camp_firm_recno )
unitPtr->return_camp();
}
}
static void
pciproxy_write_mem(ulong addr, ulong data, int len, void *usr)
{
pciproxy_device_t *pdev = (pciproxy_device_t *) usr;
char *lvaddr;
int ind;
DPRINT("write mem @ 0x%lx: 0x%lx", addr, data);
/* find the bar */
ind = pciproxy_find_bar_for_mem_access(pdev, addr, len);
if (ind == -1) {
PPLOG("could not satisfy memory write request");
return;
}
lvaddr = (char *)pdev->bars[ind].lvbase + (addr - pdev->bars[ind].mmum.mbase);
write_mem(lvaddr, data, len);
}
/*
* push_gdt
*
* Allocates and populates a page in the guest phys memory space to hold
* the boot-time GDT. Since vmd(8) is acting as the bootloader, we need to
* create the same GDT that a real bootloader would have created.
* This is loaded into the guest phys RAM space at address GDT_PAGE.
*/
static void
push_gdt(void)
{
uint8_t gdtpage[PAGE_SIZE];
struct mem_segment_descriptor *sd;
memset(&gdtpage, 0, sizeof(gdtpage));
sd = (struct mem_segment_descriptor *)&gdtpage;
/*
* Create three segment descriptors:
*
* GDT[0] : null desriptor. "Created" via memset above.
* GDT[1] (selector @ 0x8): Executable segment, for CS
* GDT[2] (selector @ 0x10): RW Data segment, for DS/ES/SS
*/
setsegment(&sd[1], 0, 0xffffffff, SDT_MEMERA, SEL_KPL, 1, 1);
setsegment(&sd[2], 0, 0xffffffff, SDT_MEMRWA, SEL_KPL, 1, 1);
write_mem(GDT_PAGE, gdtpage, PAGE_SIZE);
}
/*
* push_stack
*
* Creates the boot stack page in the guest address space. When using a real
* bootloader, the stack will be prepared using the following format before
* transitioning to kernel start, so vmd(8) needs to mimic the same stack
* layout. The stack content is pushed to the guest phys RAM at address
* STACK_PAGE. The bootloader operates in 32 bit mode; each stack entry is
* 4 bytes.
*
* Stack Layout: (TOS == Top Of Stack)
* TOS location of boot arguments page
* TOS - 0x4 size of the content in the boot arguments page
* TOS - 0x8 size of low memory (biosbasemem: kernel uses BIOS map only if 0)
* TOS - 0xc size of high memory (biosextmem, not used by kernel at all)
* TOS - 0x10 kernel 'end' symbol value
* TOS - 0x14 version of bootarg API
*
* Parameters:
* bootargsz: size of boot arguments
* end: kernel 'end' symbol value
*
* Return values:
* size of the stack
*/
static size_t
push_stack(uint32_t bootargsz, uint32_t end)
{
uint32_t stack[1024];
uint16_t loc;
memset(&stack, 0, sizeof(stack));
loc = 1024;
stack[--loc] = BOOTARGS_PAGE;
stack[--loc] = bootargsz;
stack[--loc] = 0; /* biosbasemem */
stack[--loc] = 0; /* biosextmem */
stack[--loc] = end;
stack[--loc] = 0x0e;
stack[--loc] = MAKEBOOTDEV(0x4, 0, 0, 0, 0); /* bootdev: sd0a */
stack[--loc] = 0x0;
write_mem(STACK_PAGE, &stack, PAGE_SIZE);
return (1024 - (loc - 1)) * sizeof(uint32_t);
}
void test_dma_writes(ssize_t f) {
int PAGE_SIZE = 4*1024;
int BUF_SIZE = PAGE_SIZE;
char *data = 0;
char *ret_data = 0;
posix_memalign ((void*)&data, PAGE_SIZE, BUF_SIZE);
posix_memalign ((void*)&ret_data, PAGE_SIZE, BUF_SIZE);
memset (data, 1234, BUF_SIZE/4);
sprintf (data, "Hello from DE4!\n");
data[15] = '\0';
fprintf (stderr, "First small part of what we're writing: %s\n", data);
// fprintf (stderr, "Writing %d bytes to %p from %p\n", BUF_SIZE, 0, data);
write_mem (f, 0, (void*)(0), data, BUF_SIZE);
while (!dma_is_idle(f))
dma_update(f);
// fprintf (stderr, "Reading back %d bytes from %p to %p\n", BUF_SIZE, 0, ret_data);
read_mem (f, 0, (void*)(0), ret_data, BUF_SIZE);
while (!dma_is_idle(f))
dma_update(f);
fprintf (stderr, "First small part of data we got back: %s\n", ret_data);
assert (memcmp(data, ret_data, BUF_SIZE) == 0);
fprintf (stderr, "DMA write passed!\n");
return;
}
/* return value: 0 if the simulator is to be killed,
* 1 if the simulator is to be continued.
*/
static int process_gdb_loop(void)
{
int addr;
int length;
int cpu_id;
int step_cpu, other_cpu = 0;
char *ptr;
char type;
int regnum;
uint32_t val;
regnum = regnum;
step_cpu = other_cpu = 0;
/* if the hardware is running, we dropped here because the user has
* hit break in gdb, so we send a signal to GDB indicating that */
if (hardware->running == 1) {
remcomOutBuffer[0] = 'S';
remcomOutBuffer[1] = '0';
remcomOutBuffer[2] = '5';
remcomOutBuffer[3] = 0;
putpacket((unsigned char *)remcomOutBuffer);
}
while (1)
{
remcomOutBuffer[0] = 0;
ptr = (char*)getpacket();
if (ptr == NULL) {
/* we didn't receive a valid packet, assume that
the connection has been terminated */
gdb_interface_close();
return 1;
}
if (debug_packets) printf("from gdb:%s\n", ptr);
switch (*ptr++) {
case '?': /* `?' -- last signal */
remcomOutBuffer[0] = 'S';
remcomOutBuffer[1] = '0';
remcomOutBuffer[2] = '1';
remcomOutBuffer[3] = 0;
break;
case 'c': /* cAA..AA Continue at address AA..AA(optional) */
if (hexToInt(&ptr, &addr))
set_pc(step_cpu, addr);
hardware->running = 1;
return 1;
break;
case 'd': /* `d' -- toggle debug *(deprecated)* */
debug_packets = (debug_packets + 1) % 2;
break;
case 'g': /* return the value of the CPU registers */
read_registers(other_cpu, remcomOutBuffer);
break;
case 'G': /* set the value of the CPU registers - return OK */
write_registers(other_cpu, ptr);
strcpy(remcomOutBuffer,"OK");
break;
case 'H': /* `H'CT... -- set thread */
type = *ptr++;
if (hexToInt(&ptr, &cpu_id)) {
if (cpu_id == -1 || cpu_id == 0) /* XXX all threads */
cpu_id = 1;
if (type == 'c') {
step_cpu = cpu_id - 1; /* minus one because
gdb threats start from 1
and yams cpu's from 0. */
strcpy(remcomOutBuffer, "OK");
} else if (type == 'g') {
other_cpu = cpu_id - 1; /* same here */
strcpy(remcomOutBuffer, "OK");
} else
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E01");
break;
case 'k' : /* kill the program */
return 0;
break;
case 'm': /* mAA..AA,LLLL Read LLLL bytes at address AA..AA */
if (hexToInt(&ptr,&addr) &&*ptr++==','&& hexToInt(&ptr,&length)) {
if (read_mem(addr, length, remcomOutBuffer))
strcpy(remcomOutBuffer, "E03");
} else
strcpy(remcomOutBuffer, "E01");
break;
case 'M': /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA */
if (hexToInt(&ptr, &addr) && *ptr++ == ','
&& hexToInt(&ptr, &length) && *ptr++ == ':') {
if (!write_mem(addr, length, ptr))
strcpy(remcomOutBuffer, "OK");
else
strcpy(remcomOutBuffer, "E03");
}
else
strcpy(remcomOutBuffer, "E02");
break;
case 'p': /* `p'HEX NUMBER OF REGISTER -- read register packet */
if (hexToInt(&ptr, ®num) && regnum <= 73)
sprintf(remcomOutBuffer, "%08x",
read_register(other_cpu, regnum));
else
sprintf(remcomOutBuffer, "E01");
break;
case 'P': /* `P'N...`='R... -- write register */
if (hexToInt(&ptr, (int*)®num) && *ptr++=='=' && hexToInt(&ptr, (int*)&val)) {
write_register(other_cpu, regnum, val);
sprintf(remcomOutBuffer, "OK");
} else
sprintf(remcomOutBuffer, "E01");
case 'q': /* `q'QUERY -- general query */
if (!strcmp(ptr, "fThreadInfo")) {
int i;
char *ptr = remcomOutBuffer;
ptr += sprintf(ptr, "m01");
if (hardware->num_cpus > 1)
for (i = 1; i < hardware->num_cpus; i++)
ptr += sprintf(ptr, ",%02x", i + 1);
sprintf(ptr, "l");
}
break;
case 's': /* `s'ADDR -- step */
command_step(1);
sprintf(remcomOutBuffer, "S01");
break;
case 'T': /* `T'XX -- thread alive */
if (hexToInt(&ptr, &cpu_id) && --cpu_id < hardware->num_cpus)
strcpy(remcomOutBuffer, "OK");
else
strcpy(remcomOutBuffer, "E01");
break;
case 'z': /* remove breakpoint: `Z'TYPE`,'ADDR`,'LENGTH */
type = *ptr++;
if (*ptr++== ',' && hexToInt(&ptr, &addr)
&& *ptr++ == ',' && hexToInt(&ptr, &length)) {
if (type == '1') { /* hardware breakpoint */
command_breakpoint(0xFFFFFFFF);
strcpy(remcomOutBuffer, "OK");
} else /* all others are unsupported */
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E02");
break;
case 'Z': /* insert breakpoint: `Z'TYPE`,'ADDR`,'LENGTH */
type = *ptr++;
if (*ptr++== ',' && hexToInt(&ptr, &addr)
&& *ptr++ == ',' && hexToInt(&ptr, &length)) {
if (type == '1') { /* hardware breakpoint */
command_breakpoint(addr);
strcpy(remcomOutBuffer, "OK");
} else /* all others are unsupported */
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E02");
break;
default:
break;
} /* switch */
/* reply to the request */
putpacket((unsigned char *)remcomOutBuffer);
if (debug_packets) printf("to gdb: %s\n", remcomOutBuffer);
}
}
int main(int argc, char *argv[])
{
FILE *fp;
int i;
format_srm();
memset(&files, 0, sizeof(files));
if (argc < 2) {
Usage:
printf("Usage:\n");
printf(" To create a srm file: %s [file.eep] [file.mpk] [file.sra] [file.fla]\n", argv[0]);
printf(" To extract a srm file: %s <file.srm>\n\n", argv[0]);
printf("Output files will be placed in the same directory of the input files.\n");
exit(EXIT_FAILURE);
}
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-h") || !strcmp(argv[i], "--help") || !strcmp(argv[i], "/?"))
goto Usage;
if (ext_matches("srm", argv[i]))
strncpy(files.srm, argv[i], 512);
if (ext_matches("eep", argv[i]))
strncpy(files.eep, argv[i], 512);
if (ext_matches("mpk", argv[i]))
strncpy(files.mpk, argv[i], 512);
if (ext_matches("sra", argv[i]))
strncpy(files.sra, argv[i], 512);
if (ext_matches("fla", argv[i]))
strncpy(files.fla, argv[i], 512);
}
if (*files.srm) {
puts("Running in srm extraction mode");
read_mem(files.srm, &srm, sizeof(srm));
if (!is_empty_mem(srm.eeprom, sizeof(srm.eeprom)))
change_extension(files.eep, files.srm, ".eep");
if (!is_empty_mem((uint8_t*)srm.mempack, sizeof(srm.mempack)))
change_extension(files.mpk, files.srm, ".mpk");
if (!is_empty_mem(srm.sram, sizeof(srm.sram)))
change_extension(files.sra, files.srm, ".sra");
if (!is_empty_mem(srm.flashram, sizeof(srm.flashram)))
change_extension(files.fla, files.srm, ".fla");
int over = ((*files.eep && access(files.eep, 0) != -1) || (*files.mpk && access(files.mpk, 0) != -1) ||
(*files.sra && access(files.sra, 0) != -1) || (*files.fla && access(files.fla, 0) != -1));
if (over && !overwrite_confirm("existing saves in the source directory")) {
puts("existing saves unmodified.");
exit(EXIT_SUCCESS);
}
if (*files.eep)
write_mem(files.eep, srm.eeprom, sizeof(srm.eeprom));
if (*files.mpk)
write_mem(files.mpk, srm.mempack, sizeof(srm.mempack));
if (*files.sra)
write_mem(files.sra, srm.sram, sizeof(srm.sram));
if (*files.fla)
write_mem(files.fla, srm.flashram, sizeof(srm.flashram));
} else {
puts("srm file unspecified, running in srm creation mode");
if (!files.eep && !files.mpk && !files.sra && !files.fla)
die("no input files.");
/* pick the first filename */
if (!*files.srm) {
if (*files.eep)
change_extension(files.srm, files.eep, ".srm");
else if (*files.mpk)
change_extension(files.srm, files.mpk, ".srm");
else if (*files.sra)
change_extension(files.srm, files.sra, ".srm");
else if (*files.fla)
change_extension(files.srm, files.fla, ".srm");
if (strlen(files.srm) + 3 > 512)
die("path too long");
}
if (*files.eep)
read_mem(files.eep, srm.eeprom, sizeof(srm.eeprom));
if (*files.mpk)
read_mem(files.mpk, srm.mempack, sizeof(srm.mempack));
if (*files.sra)
read_mem(files.sra, srm.sram, sizeof(srm.sram));
if (*files.fla)
read_mem(files.fla, srm.flashram, sizeof(srm.flashram));
int over = access(files.srm, 0 /*F_OK*/) != -1;
if (over && !overwrite_confirm(files.srm)) {
printf("%s unmodified.\n", files.srm);
exit(EXIT_SUCCESS);
}
printf("Writting srm data to %s...\n", files.srm);
fp = fopen(files.srm, "wb");
if (!fp) {
perror(files.srm);
exit(EXIT_FAILURE);
}
fwrite(srm.eeprom, sizeof(srm.eeprom), 1, fp);
fwrite(srm.mempack, sizeof(srm.mempack), 1, fp);
fwrite(srm.sram, sizeof(srm.sram), 1, fp);
fwrite(srm.flashram, sizeof(srm.flashram), 1, fp);
fclose(fp);
}
return EXIT_SUCCESS;
}
int __start() {
printf("-- boot complete -- \r\n");
refresh = 0;
old_img_addr = SRAM_BASEADDR;
new_img_addr = SRAM_BASEADDR + 0x80000;
clr_screen(old_img_addr);
clr_screen(new_img_addr);
draw_gun(0, 30);
/* blinker */
set_pixel(old_img_addr, 20, 10, 1);
set_pixel(old_img_addr, 20, 11, 1);
set_pixel(old_img_addr, 20, 12, 1);
/* another blinker */
set_pixel(old_img_addr, 30, 10, 1);
set_pixel(old_img_addr, 31, 10, 1);
set_pixel(old_img_addr, 32, 10, 1);
/* pixel alone, will die after the first iteration */
set_pixel(old_img_addr, 32, 30, 1);
/* start vga */
write_mem(VGA_BASEADDR + VGA_CFG_OFFSET, old_img_addr);
/* program timer: 0x01000000 / (50MHz) = 0.33554432 seconds */
write_mem(TIMER_BASEADDR + TIMER_0_TLR_OFFSET, 0x04000000);
write_mem(TIMER_BASEADDR + TIMER_0_CSR_OFFSET, TIMER_RELOAD);
write_mem(TIMER_BASEADDR + TIMER_0_CSR_OFFSET, TIMER_START);
/* program gpio to input */
write_mem(GPIO_BASEADDR + GPIO_TRI_OFFSET, GPIO_INPUT);
while (1) {
/* glider */
set_pixel(old_img_addr, 11, 10, 1);
set_pixel(old_img_addr, 12, 11, 1);
set_pixel(old_img_addr, 10, 12, 1);
set_pixel(old_img_addr, 11, 12, 1);
set_pixel(old_img_addr, 12, 12, 1);
/* another glider */
set_pixel(old_img_addr, 51, 10, 1);
set_pixel(old_img_addr, 50, 11, 1);
set_pixel(old_img_addr, 52, 12, 1);
set_pixel(old_img_addr, 51, 12, 1);
set_pixel(old_img_addr, 50, 12, 1);
while (1) {
uint32_t d = read_mem(GPIO_BASEADDR + GPIO_DATA_OFFSET);
if (TEST_BIT(d, GPIO_BTN0)) {
break;
}
// put cpu in a lower consumption state while waiting for an int
wait_for_irq();
//cpu_relax();
}
}
return 0;
}
/*
* XXX this function needs a cleanup block, lots of free(blah); return (0)
* in various cases, ds should be set to VIRTIO_BLK_S_IOERR, if we can
* XXX cant trust ring data from VM, be extra cautious.
*/
int
vioblk_notifyq(struct vioblk_dev *dev)
{
uint64_t q_gpa;
uint32_t vr_sz;
uint16_t idx, cmd_desc_idx, secdata_desc_idx, ds_desc_idx;
uint8_t ds;
int ret;
off_t secbias;
char *vr, *secdata;
struct vring_desc *desc, *cmd_desc, *secdata_desc, *ds_desc;
struct vring_avail *avail;
struct vring_used *used;
struct virtio_blk_req_hdr cmd;
ret = 0;
/* Invalid queue? */
if (dev->cfg.queue_notify > 0)
return (0);
vr_sz = vring_size(VIOBLK_QUEUE_SIZE);
q_gpa = dev->vq[dev->cfg.queue_notify].qa;
q_gpa = q_gpa * VIRTIO_PAGE_SIZE;
vr = malloc(vr_sz);
if (vr == NULL) {
log_warn("malloc error getting vioblk ring");
return (0);
}
memset(vr, 0, vr_sz);
if (read_mem(q_gpa, vr, vr_sz)) {
log_warnx("error reading gpa 0x%llx", q_gpa);
free(vr);
return (0);
}
/* Compute offsets in ring of descriptors, avail ring, and used ring */
desc = (struct vring_desc *)(vr);
avail = (struct vring_avail *)(vr +
dev->vq[dev->cfg.queue_notify].vq_availoffset);
used = (struct vring_used *)(vr +
dev->vq[dev->cfg.queue_notify].vq_usedoffset);
idx = dev->vq[dev->cfg.queue_notify].last_avail & VIOBLK_QUEUE_MASK;
if ((avail->idx & VIOBLK_QUEUE_MASK) == idx) {
log_warnx("vioblk queue notify - nothing to do?");
free(vr);
return (0);
}
cmd_desc_idx = avail->ring[idx] & VIOBLK_QUEUE_MASK;
cmd_desc = &desc[cmd_desc_idx];
if ((cmd_desc->flags & VRING_DESC_F_NEXT) == 0) {
log_warnx("unchained vioblk cmd descriptor received "
"(idx %d)", cmd_desc_idx);
free(vr);
return (0);
}
/* Read command from descriptor ring */
if (read_mem(cmd_desc->addr, &cmd, cmd_desc->len)) {
log_warnx("vioblk: command read_mem error @ 0x%llx",
cmd_desc->addr);
free(vr);
return (0);
}
switch (cmd.type) {
case VIRTIO_BLK_T_IN:
/* first descriptor */
secdata_desc_idx = cmd_desc->next & VIOBLK_QUEUE_MASK;
secdata_desc = &desc[secdata_desc_idx];
if ((secdata_desc->flags & VRING_DESC_F_NEXT) == 0) {
log_warnx("unchained vioblk data descriptor "
"received (idx %d)", cmd_desc_idx);
free(vr);
return (0);
}
secbias = 0;
do {
/* read the data (use current data descriptor) */
/*
* XXX waste to malloc secdata in vioblk_do_read
* and free it here over and over
*/
secdata = vioblk_do_read(dev, cmd.sector + secbias,
(ssize_t)secdata_desc->len);
if (secdata == NULL) {
log_warnx("vioblk: block read error, "
"sector %lld", cmd.sector);
free(vr);
return (0);
}
if (write_mem(secdata_desc->addr, secdata,
secdata_desc->len)) {
log_warnx("can't write sector "
"data to gpa @ 0x%llx",
secdata_desc->addr);
dump_descriptor_chain(desc, cmd_desc_idx);
free(vr);
free(secdata);
return (0);
}
free(secdata);
secbias += (secdata_desc->len / VIRTIO_BLK_SECTOR_SIZE);
secdata_desc_idx = secdata_desc->next &
VIOBLK_QUEUE_MASK;
secdata_desc = &desc[secdata_desc_idx];
} while (secdata_desc->flags & VRING_DESC_F_NEXT);
ds_desc_idx = secdata_desc_idx;
ds_desc = secdata_desc;
ds = VIRTIO_BLK_S_OK;
if (write_mem(ds_desc->addr, &ds, ds_desc->len)) {
log_warnx("can't write device status data @ "
"0x%llx", ds_desc->addr);
dump_descriptor_chain(desc, cmd_desc_idx);
free(vr);
return (0);
}
ret = 1;
dev->cfg.isr_status = 1;
used->ring[used->idx & VIOBLK_QUEUE_MASK].id = cmd_desc_idx;
used->ring[used->idx & VIOBLK_QUEUE_MASK].len = cmd_desc->len;
used->idx++;
dev->vq[dev->cfg.queue_notify].last_avail = avail->idx &
VIOBLK_QUEUE_MASK;
if (write_mem(q_gpa, vr, vr_sz)) {
log_warnx("vioblk: error writing vio ring");
}
break;
case VIRTIO_BLK_T_OUT:
secdata_desc_idx = cmd_desc->next & VIOBLK_QUEUE_MASK;
secdata_desc = &desc[secdata_desc_idx];
if ((secdata_desc->flags & VRING_DESC_F_NEXT) == 0) {
log_warnx("wr vioblk: unchained vioblk data "
"descriptor received (idx %d)", cmd_desc_idx);
free(vr);
return (0);
}
secdata = malloc(MAXPHYS);
if (secdata == NULL) {
log_warn("wr vioblk: malloc error, len %d",
secdata_desc->len);
free(vr);
return (0);
}
secbias = 0;
do {
if (read_mem(secdata_desc->addr, secdata,
secdata_desc->len)) {
log_warnx("wr vioblk: can't read "
"sector data @ 0x%llx",
secdata_desc->addr);
dump_descriptor_chain(desc, cmd_desc_idx);
free(vr);
free(secdata);
return (0);
}
if (vioblk_do_write(dev, cmd.sector + secbias,
secdata, (ssize_t)secdata_desc->len)) {
log_warnx("wr vioblk: disk write error");
free(vr);
free(secdata);
return (0);
}
secbias += secdata_desc->len / VIRTIO_BLK_SECTOR_SIZE;
secdata_desc_idx = secdata_desc->next &
VIOBLK_QUEUE_MASK;
secdata_desc = &desc[secdata_desc_idx];
} while (secdata_desc->flags & VRING_DESC_F_NEXT);
free(secdata);
ds_desc_idx = secdata_desc_idx;
ds_desc = secdata_desc;
ds = VIRTIO_BLK_S_OK;
if (write_mem(ds_desc->addr, &ds, ds_desc->len)) {
log_warnx("wr vioblk: can't write device status "
"data @ 0x%llx", ds_desc->addr);
dump_descriptor_chain(desc, cmd_desc_idx);
free(vr);
return (0);
}
ret = 1;
dev->cfg.isr_status = 1;
used->ring[used->idx & VIOBLK_QUEUE_MASK].id = cmd_desc_idx;
used->ring[used->idx & VIOBLK_QUEUE_MASK].len = cmd_desc->len;
used->idx++;
dev->vq[dev->cfg.queue_notify].last_avail = avail->idx &
VIOBLK_QUEUE_MASK;
if (write_mem(q_gpa, vr, vr_sz))
log_warnx("wr vioblk: error writing vio ring");
break;
case VIRTIO_BLK_T_FLUSH:
case VIRTIO_BLK_T_FLUSH_OUT:
ds_desc_idx = cmd_desc->next & VIOBLK_QUEUE_MASK;
ds_desc = &desc[ds_desc_idx];
ds = VIRTIO_BLK_S_OK;
if (write_mem(ds_desc->addr, &ds, ds_desc->len)) {
log_warnx("fl vioblk: can't write device status "
"data @ 0x%llx", ds_desc->addr);
dump_descriptor_chain(desc, cmd_desc_idx);
free(vr);
return (0);
}
ret = 1;
dev->cfg.isr_status = 1;
used->ring[used->idx & VIOBLK_QUEUE_MASK].id = cmd_desc_idx;
used->ring[used->idx & VIOBLK_QUEUE_MASK].len = cmd_desc->len;
used->idx++;
dev->vq[dev->cfg.queue_notify].last_avail = avail->idx &
VIOBLK_QUEUE_MASK;
if (write_mem(q_gpa, vr, vr_sz)) {
log_warnx("fl vioblk: error writing vio ring");
}
break;
}
free(vr);
return (ret);
}
int main(int argc, char *argv[])
{
pid_t pid = 0;
struct pt_regs2 regs;
unsigned long dlopenaddr, mprotectaddr, codeaddr, libaddr;
unsigned long *p;
int fd = 0;
int n = 0;
char buf[32];
char *arg;
int opt;
char *dumpFolder = NULL;
int libFd = -1;
void *mmapAddr = NULL;
int libLength = 0;
char *needle = ".................____________.......................";
void *startOfNeedle = NULL;
int result;
// dbg for rwx protection:
/* printf("---\n"); */
/* result = mprotect(0xbefdf000, 0x20000, PROT_READ|PROT_WRITE|PROT_EXEC); */
/* printf("mprotect %d\n", result); */
/* printf("\t\t%s\n", strerror(*(int*)__errno()) ); */
/* 1 - parse cmdline */
while ((opt = getopt(argc, argv, "p:l:f:d")) != -1) {
switch (opt) {
case 'p':
pid = strtol(optarg, NULL, 0);
break;
case 'l':
n = strlen(optarg)+1;
n = n/4 + (n%4 ? 1 : 0);
arg = malloc(n*sizeof(unsigned long));
memcpy(arg, optarg, n*4);
/* arg = strdup(optarg); */
/* n = strlen(arg) */
/* printf("%s\n", arg); */
break;
case 'f':
dumpFolder = strdup(optarg);
break;
case 'd':
debug = 1;
break;
default:
#ifdef DEBUG
fprintf(stderr, "error usage: %s -p PID -l LIBNAME -f DUMP_FOLDER -d (debug on)\n", argv[0]);
#endif
exit(0);
break;
}
}
if (pid == 0 || n == 0 || strlen(dumpFolder) == 0) {
#ifdef DEBUG
printf("pid %d\n", pid);
fprintf(stderr, "usage: %s -p PID -l LIBNAME -f DUMP_FOLDER -d (debug on)\n", argv[0]);
#endif
exit(0);
}
if (0 > find_name(pid, "mprotect", &mprotectaddr)) {
#ifdef DEBUG
printf("can't find address of mprotect(), error!\n");
#endif
exit(1);
}
#ifdef DEBUG
printf("mprotect: 0x%x\n", mprotectaddr);
#endif
/* 2 - patch */
#ifdef DEBUG
printf("[*] Patching %s to dump into folder %s\n", arg, dumpFolder);
#endif
libFd = open(arg, O_RDWR);
if (libFd == -1) {
#ifdef DEBUG
printf("[E] Could not open %s %s\n", arg, strerror(*(int*)__errno()));
#endif
exit(1);
}
libLength = lseek(libFd,0,SEEK_END);
mmapAddr = mmap(NULL, libLength, PROT_READ|PROT_WRITE, MAP_SHARED, libFd, 0 );
if( mmapAddr == MAP_FAILED ) {
#ifdef DEBUG
printf("[E] Map failed %s\n", arg);
#endif
exit(1);
}
#ifdef DEBUG
printf("[*] searching %s from %p to %p\n", needle, mmapAddr, mmapAddr + libLength);
#endif
startOfNeedle = memmem(mmapAddr, libLength, needle, strlen(needle));
if( startOfNeedle == 0) {
#ifdef DEBUG
printf("\tneedle not found, the library might be already patched\n");
#endif
;
}
else {
#ifdef DEBUG
printf("\t found at %p, patching..\n", startOfNeedle);
#endif
memcpy(startOfNeedle, dumpFolder, strlen(dumpFolder)+1);
}
needle = memmem(mmapAddr, libLength, dumpFolder, strlen(dumpFolder));
#ifdef DEBUG
printf("\t verify the patch: %s @ %p\n", needle, needle );
#endif
result = munmap(mmapAddr, libLength);
#ifdef DEBUG
printf("[*] unmap %d\n", result);
#endif
close(libFd);
/* 3 - inject */
void *ldl = dlopen("libdl.so", RTLD_LAZY);
if (ldl) {
dlopenaddr = dlsym(ldl, "dlopen");
dlclose(ldl);
}
unsigned long int lkaddr;
unsigned long int lkaddr2;
find_linker(getpid(), &lkaddr);
//printf("own linker: 0x%x\n", lkaddr);
//printf("offset %x\n", dlopenaddr - lkaddr);
find_linker(pid, &lkaddr2);
//printf("tgt linker: %x\n", lkaddr2);
//printf("tgt dlopen : %x\n", lkaddr2 + (dlopenaddr - lkaddr));
dlopenaddr = lkaddr2 + (dlopenaddr - lkaddr);
#ifdef DEBUG
printf("dlopen: 0x%x\n", dlopenaddr);
#endif
// Attach
if (0 > ptrace(PTRACE_ATTACH, pid, 0, 0)) {
#ifdef DEBUG
printf("cannot attach to %d, error!\n", pid);
#endif
exit(1);
}
waitpid(pid, NULL, 0);
sprintf(buf, "/proc/%d/mem", pid);
fd = open(buf, O_WRONLY);
if (0 > fd) {
#ifdef DEBUG
printf("cannot open %s, error!\n", buf);
#endif
exit(1);
}
result = ptrace(PTRACE_GETREGS, pid, 0, ®s);
#ifdef DEBUG
printf("ptrace getregs %d\n", result);
#endif
sc[11] = regs.ARM_r0;
sc[12] = regs.ARM_r1;
sc[13] = regs.ARM_r2;
sc[14] = regs.ARM_r3;
sc[15] = regs.ARM_lr;
sc[16] = regs.ARM_pc;
sc[17] = regs.ARM_sp;
sc[19] = dlopenaddr;
#ifdef DEBUG
printf("pc=%x lr=%x sp=%x fp=%x\n", regs.ARM_pc, regs.ARM_lr, regs.ARM_sp, regs.ARM_fp);
printf("r0=%x r1=%x\n", regs.ARM_r0, regs.ARM_r1);
printf("r2=%x r3=%x\n", regs.ARM_r2, regs.ARM_r3);
#endif
// push library name to stack
libaddr = regs.ARM_sp - n*4 - sizeof(sc);
sc[18] = libaddr;
//printf("libaddr: %x\n", libaddr);
if (stack_start == 0) {
stack_start = (unsigned long int) strtol(argv[3], NULL, 16);
stack_start = stack_start << 12;
stack_end = stack_start + strtol(argv[4], NULL, 0);
}
#ifdef DEBUG
printf("stack: 0x%x-0x%x leng = %d\n", stack_start, stack_end, stack_end-stack_start);
#endif
// write library name to stack
if (0 > write_mem(pid, (unsigned long*)arg, n, libaddr)) {
#ifdef DEBUG
printf("cannot write library name (%s) to stack, error!\n", arg);
#endif
exit(1);
}
// write code to stack
codeaddr = regs.ARM_sp - sizeof(sc);
if (0 > write_mem(pid, (unsigned long*)&sc, sizeof(sc)/sizeof(long), codeaddr)) {
#ifdef DEBUG
printf("cannot write code, error!\n");
#endif
exit(1);
}
#ifdef DEBUG
printf("executing injection code at 0x%x\n", codeaddr);
#endif
// calc stack pointer
regs.ARM_sp = regs.ARM_sp - n*4 - sizeof(sc);
// call mprotect() to make stack executable
regs.ARM_r0 = stack_start; // want to make stack executable
//printf("r0 %x\n", regs.ARM_r0);
regs.ARM_r1 = stack_end - stack_start; // stack size
//printf("mprotect(%x, %d, ALL)\n", regs.ARM_r0, regs.ARM_r1);
regs.ARM_r2 = PROT_READ|PROT_WRITE|PROT_EXEC; // protections
regs.ARM_lr = codeaddr; // points to loading and fixing code
regs.ARM_pc = mprotectaddr; // execute mprotect()
// detach and continue
result = ptrace(PTRACE_SETREGS, pid, 0, ®s);
/* printf("%d\n", result); */
/* return 0; */
#ifdef DEBUG
printf("first ptrace %d\n", result);
if( result == -1 )
printf("\t\t%s\n", strerror(*(int*)__errno()) );
#endif
result = ptrace(PTRACE_DETACH, pid, 0, 0);
#ifdef DEBUG
printf("second ptrace %d\n", result);
if( result == -1 )
printf("\t\t%s\n", strerror(*(int*)__errno()) );
#endif
#ifdef DEBUG
printf("library injection completed!\n");
#endif
return 0;
}
void run()
{
uint8_t key[2];
uint8_t key_itr = 0;
while(key_itr < 2)
key[++key_itr] = 0;
key_itr = 0;
moveflags = 0;
recordIter = 0;
speed = 127;
le.clear();
re.clear();
recordTime.stop();
recordTime.clear();
uint32_t nextPlay = 0;
uint32_t nextPlayBase = 0;
state = 0;
encoder_play_l.stop();
encoder_play_r.stop();
startTime = 0;
/*rs232.send("YuniRC program has started!\r\n"
"Controls: W,A,S,D - movement, Space - read sensor values,");
rs232.wait();
rs232.send(" R - reset encoders, Q - On/Off engine correction, 1 2 3 - speed \r\n");
rs232.wait();
rs232.send("Engine correction is disabled.\r\n"); */
char ch;
while(true)
{
if(state & STATE_ERASING)
continue;
// Move correction
if((state & STATE_CORRECTION) && (moveflags == MOVE_FORWARD || moveflags == MOVE_BACKWARD))
MovementCorrection();
if((state & STATE_PLAY) && (lastAdress == 0 || EventHappened(&lastRec, &nextPlayBase, &nextPlay)))
{
encoder_play_l.stop();
encoder_play_r.stop();
read_mem(&lastRec, lastAdress);
lastAdress += REC_SIZE;
if((lastRec.key[0] == 0 && lastRec.key[1] == 0 && lastRec.getBigNum() == 0) ||
lastAdress > 512)
{
state &= ~(STATE_PLAY);
rs232.send("Playback finished\r\n");
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
continue;
}
SetMovement(lastRec.key);
nextPlay = 0;
nextPlayBase = 0;
if(lastRec.end_event == EVENT_TIME)
{
nextPlayBase = getTickCount();
nextPlay = (uint32_t(lastRec.getBigNum())*10000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}
//Uncomment to set messure delay
/*else if(lastRec.end_event == EVENT_RANGE_MIDDLE_HIGHER || lastRec.end_event == EVENT_RANGE_MIDDLE_LOWER)
{
nextPlayBase = getTickCount();
nextPlay = (50000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}*/
else if(lastRec.end_event == EVENT_DISTANCE || lastRec.end_event == EVENT_DISTANCE_LEFT || lastRec.end_event == EVENT_DISTANCE_RIGHT)
{
encoder_play_r.clear();
encoder_play_l.clear();
encoder_play_l.start();
encoder_play_r.start();
}
++recordIter;
}
//Read command
if(!rs232.peek(ch))
continue;
key[key_itr] = uint8_t(ch);
++key_itr;
//key recieved
if(key_itr >= 2)
{
key_itr = 0;
// FIXME: ignore two or more keys at once
if((state & STATE_RECORD) && char(lastRec.key[1]) == 'd' && char(key[1]) != 'u' && char(key[0]) != 'C')
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
continue;
}
bool down_only = SetMovement(key);
if(char(key[0]) == 'O' || char(key[0]) == 'P')
continue;
else if((state & STATE_RECORD) && char(key[0]) != 'C' &&
(!down_only || (down_only && char(key[1]) == 'd'))) // do not record down only keys
{
if(!recordTime.isRunning())
{
recordTime.clear();
recordTime.start();
}
if(recordIter > 0)
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
lastAdress+=REC_SIZE;
}
if(recordIter < MEM_SIZE-1)
{
while(key_itr < 2)
{
lastRec.key[key_itr] = key[key_itr];
++key_itr;
}
key_itr = 0;
recordTime.clear();
++recordIter;
}
else
{
key[0] = uint8_t('C');
key[1] = uint8_t('d');
rs232.send("Memory full\r\n");
continue;
}
}
}
// EEPROM Flash mode
else if(ch == 0x1C)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
erase_eeprom();
rs232.sendCharacter(0x1D);
for(lastAdress = 0; true; )
{
if(!rs232.peek(ch))
continue;
if(ch == 0x1E && lastAdress%5 == 0)
break;
write_byte(lastAdress, uint8_t(ch));
++lastAdress;
rs232.sendCharacter(0x1F);
}
lastAdress = 0;
}
// EEPROM read mode
else if(ch == 0x16)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
rs232.sendCharacter(0x17);
for(lastAdress = 0; lastAdress < 512; ++lastAdress)
{
rs232.wait();
rs232.sendCharacter(read_byte(lastAdress));
}
rs232.sendCharacter(0x18);
lastAdress = 0;
}
}
}
bool SetMovement(uint8_t key[])
{
// Set Movement Flags
bool down = (key[1] == uint8_t('d'));
bool down_only = false;
// only down keys
if(down)
{
switch(char(key[0]))
{
//speed (1 2 3 on keyboard Oo)
case 'a': speed = 50; break;
case 'b': speed = 100; break;
case 'c': speed = 127; break;
case 'R': // reset encoders
re.clear();
le.clear();
break;
case 'Q': // on/off correction
rs232.send("Engine correction is ");
if(state & STATE_CORRECTION)
{
state &= ~(STATE_CORRECTION);
rs232.send("disabled \r\n");
}
else
{
state |= STATE_CORRECTION;
rs232.send("enabled \r\n");
}
break;
case 'C':
rs232.wait();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
if(!(state & STATE_RECORD))
{
state |= STATE_RECORD;
recordIter = 0;
rs232.send("Erasing EEPROM...");
state |= STATE_ERASING;
erase_eeprom();
state &= ~(STATE_ERASING);
rs232.send("done\r\n");
lastAdress = 0;
}
else
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
recordTime.stop();
recordTime.clear();
recordIter = 0;
state &= ~(STATE_RECORD);
}
rs232.send("Trace recording is ");
if(state & STATE_RECORD){ rs232.send("enabled \r\n");}
else {rs232.send("disabled \r\n");}
break;
case 'P':
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
setMotorPower(0, 0);
if(!(state & STATE_PLAY))
{
recordTime.stop();
recordTime.clear();
rs232.send("Playing..\r\n");
recordIter = 0;
lastAdress = 0;
state |= STATE_PLAY;
state &= ~(STATE_RECORD);
}
else
{
rs232.send("Playback stopped\r\n");
state &= ~(STATE_PLAY);
}
break;
case 'O':
if(state & STATE_RECORD)
break;
rs232.send("Playback ");
if(state & STATE_PLAY)
{
rs232.send("unpaused\r\n");
state &= ~(STATE_PLAY);
}
else
{
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
rs232.send("paused\r\n");
state |= STATE_PLAY;
}
break;
}
}
// Movement
switch(char(key[0]))
{
case 'W':
if(!(moveflags & MOVE_BACKWARD))
{
if(down) moveflags |= MOVE_FORWARD;
else moveflags &= ~(MOVE_FORWARD);
}
break;
case 'S':
if(!(moveflags & MOVE_FORWARD))
{
if(down) moveflags |= MOVE_BACKWARD;
else moveflags &= ~(MOVE_BACKWARD);
}
break;
case 'A':
if(!(moveflags & MOVE_RIGHT))
{
if(down) moveflags |= MOVE_LEFT;
else moveflags &= ~(MOVE_LEFT);
}
break;
case 'D':
if(!(moveflags & MOVE_LEFT))
{
if(down) moveflags |= MOVE_RIGHT;
else moveflags &= ~(MOVE_RIGHT);
}
break;
default:
down_only = true;
break;
}
// Sensors
if(char(key[0]) == ' ' && down) // Space
{
rs232.wait();
rs232.send("\r\nSensors: ");
rs232.dumpNumber(getSensorValue(6));
rs232.sendNumber(getSensorValue(7)); // proud
/*rs232.send("\r\nSensors: \r\n");
rs232.send(" ");
rs232.sendNumber(getSensorValue(5));
rs232.send(" ");
rs232.sendNumber(getSensorValue(1));
rs232.wait();
rs232.send("\r\n");
rs232.sendNumber(getSensorValue(2));
rs232.send(" ");
rs232.sendNumber(getSensorValue(3));
rs232.wait(); */
rs232.send("\r\nEncoders: \r\n L: ");
rs232.sendNumber(le.get());
rs232.send(" R: ");
rs232.sendNumber(re.get());
rs232.send("\r\nRange \r\nL: ");
rs232.wait();
rs232.sendNumber(ReadRange(FINDER_LEFT));
rs232.send("cm M: ");
rs232.sendNumber(ReadRange(FINDER_MIDDLE));
rs232.send("cm R: ");
rs232.sendNumber(ReadRange(FINDER_RIGHT));
rs232.send("cm\r\n");
}
//Set motors
if(moveflags & MOVE_FORWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(speed-TURN_VALUE, speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(speed, speed-TURN_VALUE);
else
{
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(speed, speed);
state &= ~(STATE_CORRECTION2);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_BACKWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(-(speed-TURN_VALUE), -speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(-speed, -(speed-TURN_VALUE));
else
{
state &= ~(STATE_CORRECTION2);
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(-speed, -speed);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_LEFT)
{
setMotorPower(-speed, speed);
startTime = getTickCount();
}
else if(moveflags & MOVE_RIGHT)
{
setMotorPower(speed, -speed);
startTime = getTickCount();
}
else
{
startTime = getTickCount();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
state &= ~(STATE_CORRECTION2);
}
return down_only;
}
/* Test reading and writing long chunks of data */
void test_large_read_write (ssize_t f) {
int j, num_write_runs = 5;
size_t buf_size = 80 * 1024 * 1024;
char *buf1 = NULL;
char *buf2 = NULL;
const size_t ALIGNMENT = 4096;
posix_memalign ((void**)&buf1, ALIGNMENT, buf_size);
posix_memalign ((void**)&buf2, ALIGNMENT, buf_size);
if (buf1 == NULL || buf2 == NULL) {
printf ("Couldn't allocate memory! FAILED\n");
}
/* Some "real" data to fill memory with. */
FILE *data_file = fopen ("tests/ulysses.txt", "r");
if (data_file == NULL) {
printf ("Couldn't open data file! FAILED\n");
}
/* read data file in chunks of 4 KB */
size_t read_step = 1024 * 4;
size_t num_read = 0;
size_t incr = 0, file_size = 0;
while ( (num_read = fread (buf1 + incr, sizeof(char), read_step, data_file)) ) {
incr += num_read;
if (num_read < read_step) {
/* Done reading */
break;
}
}
/* Copy the file content until fill the buffer */
file_size = incr;
while (incr < (buf_size - file_size)) {
memcpy (buf1 + incr, buf1, file_size);
incr += file_size;
}
assert (incr < buf_size);
incr = incr - (incr % ALIGNMENT);
printf ("Useful data size for large read/write test is %zu bytes\n", incr);
clock_t start = clock();
clock_t e1, e2;
/* Write to different locations on each run just to avoid
* any kind of caching on PCIe block. */
for (j = 0; j < num_write_runs; j++) {
write_mem (f, 0, (void*)(incr * j), buf1, incr);
while (!dma_is_idle(f))
dma_update(f);
}
e1 = clock();
read_mem (f, 0, 0, buf2, incr);
while (!dma_is_idle(f))
dma_update(f);
e2 = clock();
double t1 = ((double)e1 - start) / CLOCKS_PER_SEC;
double t2 = ((double)e2 - e1) / CLOCKS_PER_SEC;
int mb = 1024 * 1024;
printf ("Writing %zu bytes took %.3f seconds (%.3f MB / sec)\n",
incr, t1, incr / mb / t1 * num_write_runs );
printf ("Reading %zu bytes took %.3f seconds (%.3f MB / sec)\n",
incr, t2, incr / mb / t2 );
/* Make sure what we read back is the same as what we wrote */
assert (memcmp (buf1, buf2, incr) == 0);
printf ("test_large_read_write PASSED\n");
free (buf1);
free (buf2);
return;
}
/*
* XXX cant trust ring data from VM, be extra cautious.
* XXX advertise link status to guest
*/
int
vionet_notifyq(struct vionet_dev *dev)
{
uint64_t q_gpa;
uint32_t vr_sz;
uint16_t idx, pkt_desc_idx, hdr_desc_idx, dxx;
size_t pktsz;
int ret, num_enq, ofs;
char *vr, *pkt;
struct vring_desc *desc, *pkt_desc, *hdr_desc;
struct vring_avail *avail;
struct vring_used *used;
vr = pkt = NULL;
ret = 0;
/* Invalid queue? */
if (dev->cfg.queue_notify != 1) {
vionet_notify_rx(dev);
goto out;
}
vr_sz = vring_size(VIONET_QUEUE_SIZE);
q_gpa = dev->vq[dev->cfg.queue_notify].qa;
q_gpa = q_gpa * VIRTIO_PAGE_SIZE;
vr = malloc(vr_sz);
if (vr == NULL) {
log_warn("malloc error getting vionet ring");
goto out;
}
memset(vr, 0, vr_sz);
if (read_mem(q_gpa, vr, vr_sz)) {
log_warnx("error reading gpa 0x%llx", q_gpa);
goto out;
}
/* Compute offsets in ring of descriptors, avail ring, and used ring */
desc = (struct vring_desc *)(vr);
avail = (struct vring_avail *)(vr +
dev->vq[dev->cfg.queue_notify].vq_availoffset);
used = (struct vring_used *)(vr +
dev->vq[dev->cfg.queue_notify].vq_usedoffset);
num_enq = 0;
idx = dev->vq[dev->cfg.queue_notify].last_avail & VIONET_QUEUE_MASK;
if ((avail->idx & VIONET_QUEUE_MASK) == idx) {
log_warnx("vionet tx queue notify - nothing to do?");
goto out;
}
while ((avail->idx & VIONET_QUEUE_MASK) != idx) {
hdr_desc_idx = avail->ring[idx] & VIONET_QUEUE_MASK;
hdr_desc = &desc[hdr_desc_idx];
pktsz = 0;
dxx = hdr_desc_idx;
do {
pktsz += desc[dxx].len;
dxx = desc[dxx].next;
} while (desc[dxx].flags & VRING_DESC_F_NEXT);
pktsz += desc[dxx].len;
/* Remove virtio header descriptor len */
pktsz -= hdr_desc->len;
/*
* XXX check sanity pktsz
* XXX too long and > PAGE_SIZE checks
* (PAGE_SIZE can be relaxed to 16384 later)
*/
pkt = malloc(pktsz);
if (pkt == NULL) {
log_warn("malloc error alloc packet buf");
goto out;
}
ofs = 0;
pkt_desc_idx = hdr_desc->next & VIONET_QUEUE_MASK;
pkt_desc = &desc[pkt_desc_idx];
while (pkt_desc->flags & VRING_DESC_F_NEXT) {
/* must be not writable */
if (pkt_desc->flags & VRING_DESC_F_WRITE) {
log_warnx("unexpected writable tx desc "
"%d", pkt_desc_idx);
goto out;
}
/* Read packet from descriptor ring */
if (read_mem(pkt_desc->addr, pkt + ofs,
pkt_desc->len)) {
log_warnx("vionet: packet read_mem error "
"@ 0x%llx", pkt_desc->addr);
goto out;
}
ofs += pkt_desc->len;
pkt_desc_idx = pkt_desc->next & VIONET_QUEUE_MASK;
pkt_desc = &desc[pkt_desc_idx];
}
/* Now handle tail descriptor - must be not writable */
if (pkt_desc->flags & VRING_DESC_F_WRITE) {
log_warnx("unexpected writable tx descriptor %d",
pkt_desc_idx);
goto out;
}
/* Read packet from descriptor ring */
if (read_mem(pkt_desc->addr, pkt + ofs,
pkt_desc->len)) {
log_warnx("vionet: packet read_mem error @ "
"0x%llx", pkt_desc->addr);
goto out;
}
/* XXX signed vs unsigned here, funky cast */
if (write(dev->fd, pkt, pktsz) != (int)pktsz) {
log_warnx("vionet: tx failed writing to tap: "
"%d", errno);
goto out;
}
ret = 1;
dev->cfg.isr_status = 1;
used->ring[used->idx & VIONET_QUEUE_MASK].id = hdr_desc_idx;
used->ring[used->idx & VIONET_QUEUE_MASK].len = hdr_desc->len;
used->idx++;
dev->vq[dev->cfg.queue_notify].last_avail =
(dev->vq[dev->cfg.queue_notify].last_avail + 1);
num_enq++;
idx = dev->vq[dev->cfg.queue_notify].last_avail &
VIONET_QUEUE_MASK;
}
if (write_mem(q_gpa, vr, vr_sz)) {
log_warnx("vionet: tx error writing vio ring");
}
out:
free(vr);
free(pkt);
return (ret);
}
int
vionet_enq_rx(struct vionet_dev *dev, char *pkt, ssize_t sz, int *spc)
{
uint64_t q_gpa;
uint32_t vr_sz;
uint16_t idx, pkt_desc_idx, hdr_desc_idx;
ptrdiff_t off;
int ret;
char *vr;
struct vring_desc *desc, *pkt_desc, *hdr_desc;
struct vring_avail *avail;
struct vring_used *used;
struct vring_used_elem *ue;
ret = 0;
vr_sz = vring_size(VIONET_QUEUE_SIZE);
q_gpa = dev->vq[0].qa;
q_gpa = q_gpa * VIRTIO_PAGE_SIZE;
vr = malloc(vr_sz);
if (vr == NULL) {
log_warn("rx enq: malloc error getting vionet ring");
return (0);
}
memset(vr, 0, vr_sz);
if (read_mem(q_gpa, vr, vr_sz)) {
log_warnx("rx enq: error reading gpa 0x%llx", q_gpa);
free(vr);
return (0);
}
/* Compute offsets in ring of descriptors, avail ring, and used ring */
desc = (struct vring_desc *)(vr);
avail = (struct vring_avail *)(vr +
dev->vq[0].vq_availoffset);
used = (struct vring_used *)(vr +
dev->vq[0].vq_usedoffset);
idx = dev->vq[0].last_avail & VIONET_QUEUE_MASK;
if ((dev->vq[0].notified_avail & VIONET_QUEUE_MASK) == idx) {
log_warnx("vionet queue notify - no space, dropping packet");
free(vr);
return (0);
}
hdr_desc_idx = avail->ring[idx] & VIONET_QUEUE_MASK;
hdr_desc = &desc[hdr_desc_idx];
pkt_desc_idx = hdr_desc->next & VIONET_QUEUE_MASK;
pkt_desc = &desc[pkt_desc_idx];
/* must be not readable */
if ((pkt_desc->flags & VRING_DESC_F_WRITE) == 0) {
log_warnx("unexpected readable rx descriptor %d",
pkt_desc_idx);
free(vr);
return (0);
}
/* Write packet to descriptor ring */
if (write_mem(pkt_desc->addr, pkt, sz)) {
log_warnx("vionet: rx enq packet write_mem error @ "
"0x%llx", pkt_desc->addr);
free(vr);
return (0);
}
ret = 1;
dev->cfg.isr_status = 1;
ue = &used->ring[used->idx & VIONET_QUEUE_MASK];
ue->id = hdr_desc_idx;
ue->len = hdr_desc->len + sz;
used->idx++;
dev->vq[0].last_avail = (dev->vq[0].last_avail + 1);
*spc = dev->vq[0].notified_avail - dev->vq[0].last_avail;
off = (char *)ue - vr;
if (write_mem(q_gpa + off, ue, sizeof *ue))
log_warnx("vionet: error writing vio ring");
else {
off = (char *)&used->idx - vr;
if (write_mem(q_gpa + off, &used->idx, sizeof used->idx))
log_warnx("vionet: error writing vio ring");
}
free(vr);
return (ret);
}
int main(void)
{
uint8_t flags = MCUSR;
MCUSR = 0;
setWDT(WATCHDOG_OFF);
if (flags & RESET_FLAGS) START_PROGRAM;
SETUP_UART;
setWDT(WATCHDOG_125MS);
#ifdef OTA_ENABLED
if(OTA)
{
SETUP_UART_OTA;
setWDT(WATCHDOG_2S);
}
#endif
register uint8_t cmd;
register address_t address = 0;
register length_t length;
while(1)
{
cmd = read();
switch(cmd)
{
case SIGNATURE:
{
validate();
write(SIGNATURE_0);
write(SIGNATURE_1);
write(SIGNATURE_2);
}
break;
case ADDRESS:
{
GETADDRESS(address);
validate();
}
break;
case READ:
{
GETLENGTH(length);
validate();
read_mem(address, length);
}
break;
case WRITE:
{
GETLENGTH(length);
validate();
write_buffer(length);
write_mem(ram_bfr, address, length);
}
break;
case EXIT:
{
validate();
reset();
}
break;
default:
{
#ifdef OTA_ENABLED
if(OTA == 0)
#endif
write(FAIL);
}
break;
}
#ifdef OTA_ENABLED
if(OTA == 0)
#endif
write(OK);
}
}
// ####### begin Gilbert 25/1 ########//
// -------- begin of function UnitGroup::transform_mfort --------//
//
// convert to grokken units to FirmMonsterFortress
//
void UnitGroup::transform_mfort(int destXLoc, int destYLoc, char remoteAction)
{
if( size() == 0 )
return;
if( !remoteAction && remote.is_enable() )
{
// packet structure : <xLoc> <yLoc> <write_mem...>
short *shortPtr = (short *)remote.new_send_queue_msg(MSG_UNITS_GO_TRANSFORM_MFORT, sizeof(short)*2 + get_write_len() );
shortPtr[0] = destXLoc;
shortPtr[1] = destYLoc;
write_mem(shortPtr+2);
return;
}
short builderUnit[MAX_EXTRA_BUILDER]; // = (short*) mem_add_clear(MAX_EXTRA_BUILDER*sizeof(short));
int checkIndex = 0;
Unit *cmdUnit = get_unit(1);
for(int i = 1; i <= size() && checkIndex < MAX_EXTRA_BUILDER; i++)
{
Unit *unitPtr = get_unit(i);
if(unitPtr->unit_id == cmdUnit->unit_id && unitPtr->nation_recno == cmdUnit->nation_recno
&& unitPtr->is_visible() )
{
builderUnit[checkIndex] = unitPtr->sprite_recno;
checkIndex++;
}
}
if(checkIndex == MAX_EXTRA_BUILDER)
{
long dist2[MAX_EXTRA_BUILDER][MAX_EXTRA_BUILDER]; // distance between each builder and each destination
// dist[ builder i][ place j ]
int j;
int destX[MAX_EXTRA_BUILDER], destY[MAX_EXTRA_BUILDER];
err_when( MAX_EXTRA_BUILDER != 4 );
destX[0] = destXLoc; // top left
destY[0] = destYLoc;
destX[1] = destXLoc + 2; // top right
destY[1] = destYLoc;
destX[2] = destXLoc; // bottom left
destY[2] = destYLoc + 2;
destX[3] = destXLoc + 2; // bottom right
destY[3] = destYLoc + 2;
// find distance
for( i = 0; i < checkIndex; i++ )
{
Unit *unitPtr = unit_array[builderUnit[i]];
long srcX = unitPtr->next_x_loc();
long srcY = unitPtr->next_y_loc();
for( j = 0; j < checkIndex; j++ )
{ // don't use points distance
dist2[i][j] = (srcX-destX[j])*(srcX-destX[j]) + (srcY-destY[j])*(srcY-destY[j]);
}
}
// find a set to minimize sum of dist2
int bestSet[MAX_EXTRA_BUILDER] = { 0, 1, 2, 3 }; // builderUnit i to place i
int minSumDist2 = dist2[bestSet[0]][0] + dist2[bestSet[1]][1] + dist2[bestSet[2]][2] + dist2[bestSet[3]][3];
for( int i0 = 0; i0 < MAX_EXTRA_BUILDER; i0++ )
{
for( int i1 = 0; i1 < MAX_EXTRA_BUILDER; i1++ )
{
if( i1 == i0 )
continue;
for( int i2 = 0; i2 < MAX_EXTRA_BUILDER; i2++ )
{
if( i2 == i0 || i2 == i1 )
continue;
for( int i3 = 0; i3 < MAX_EXTRA_BUILDER; i3++ )
{
if( i3 == i0 || i3 == i1 || i3 == i2 )
continue;
int sumDist2 = dist2[i0][0] + dist2[i1][1] + dist2[i2][2] + dist2[i3][3];
if( sumDist2 < minSumDist2 )
{
minSumDist2 = sumDist2;
bestSet[0] = i0;
bestSet[1] = i1;
bestSet[2] = i2;
bestSet[3] = i3;
}
}
}
}
}
for(j = 0; j < MAX_EXTRA_BUILDER; j++)
{
Unit *unitPtr = unit_array[builderUnit[bestSet[j]]];
unitPtr->move_to(destX[j], destY[j]);
unitPtr->cur_order.set(UNIT_TRANSFORM_FORTRESS, 0, destX[j], destY[j]);
}
}
}
//-------------------------------------------------------------------
int sim_one ( void )
{
unsigned short inst;
unsigned short op;
unsigned short imm,simm;
unsigned short addr;
unsigned short data;
unsigned short adata,bdata,cdata;
unsigned short brdest;
unsigned char ra,rb,rc;
inst=fetch_mem(pc);
pc=pc+1;
op=(inst>>13)&7;
ra=(inst>>10)&7;
rb=(inst>> 7)&7;
rc=(inst>> 0)&7;
imm=inst&0x3FF;
simm=inst&0x7F;
if(simm&0x40) simm|=(~0)<<7;
switch(op)
{
case 0: //ADD
{
if(inst&0x0078)
{
printf("undefined instruction [0x%04X] 0x%04X\n",pc-1,inst);
return(1);
}
if(show_diss) printf("[0x%04X] 0x%04X add r%u,r%u,r%u\n",pc-1,inst,ra,rb,rc);
bdata=read_reg(rb);
cdata=read_reg(rc);
write_reg(ra,bdata+cdata);
break;
}
case 1: //ADDI
{
if(show_diss) printf("[0x%04X] 0x%04X addi r%u,r%u,0x%04X (%d)\n",pc-1,inst,ra,rb,simm,(short)simm);
bdata=read_reg(rb);
write_reg(ra,bdata+simm);
break;
}
case 2: //NAND
{
if(inst&0x0078)
{
printf("undefined instruction [0x%04X] 0x%04X\n",pc-1,inst);
return(1);
}
if(show_diss) printf("[0x%04X] 0x%04X nand r%u,r%u,r%u\n",pc-1,inst,ra,rb,rc);
bdata=read_reg(rb);
cdata=read_reg(rc);
write_reg(ra,~(bdata&cdata));
break;
}
case 3: //LUI
{
if(show_diss) printf("[0x%04X] 0x%04X lui r%u,0x%03X (0x%04X)\n",pc-1,inst,ra,imm,imm<<6);
write_reg(ra,imm<<6);
break;
}
case 4: //SW
{
data=reg[ra];
addr=reg[rb]+simm;
if(show_diss) printf("[0x%04X] 0x%04X sw r%u,[r%u%+d] ([0x%04X]<=0x%04X) \n",pc-1,inst,ra,rb,((short)simm),addr,data);
data=read_reg(ra);
addr=read_reg(rb)+simm;
write_mem(addr,data);
break;
}
case 5: //LW
{
addr=reg[rb]+simm;
data=mem[addr];
if(show_diss) printf("[0x%04X] 0x%04X lw r%u,[r%u%+d] ([0x%04X]=>0x%04X) \n",pc-1,inst,ra,rb,((short)simm),addr,data);
addr=read_reg(rb)+simm;
data=read_mem(addr);
write_reg(ra,data);
break;
}
case 6: //BEQ
{
adata=reg[ra];
bdata=reg[rb];
brdest=pc+simm;
if(show_diss) printf("[0x%04X] 0x%04X beq r%u,r%u,0x%04X (0x%04X 0x%04X)\n",pc-1,inst,ra,rb,brdest,adata,bdata);
adata=read_reg(ra);
bdata=read_reg(rb);
if(adata==bdata) pc=brdest;
break;
}
case 7: //JALR
{
if(inst==0xFFFF)
{
if(show_diss) printf("[0x%04X] 0x%04X halt\n",pc-1,inst);
return(1);
}
if(inst&0x007F)
{
printf("undefined instruction [0x%04X] 0x%04X\n",pc-1,inst);
return(1);
}
brdest=reg[rb];
if(show_diss) printf("[0x%04X] 0x%04X jalr r%u,r%u (0x%04X)\n",pc-1,inst,ra,rb,brdest);
write_reg(ra,pc);
brdest=read_reg(rb);
pc=brdest;
break;
}
}
return(0);
}
/*
* mbcopy
*
* copies 'sz' bytes from buffer 'src' to guest paddr 'dst'.
*
* Parameters:
* src: source buffer to copy from
* dst: destination guest paddr_t to copy to
* sz: number of bytes to copy
*
* Return values:
* nothing
*/
static void
mbcopy(void *src, paddr_t dst, int sz)
{
write_mem(dst, src, sz);
}
int iostream::write( void* from, int tpsize, int dtsize )
{
return ( srct == 0 ) ? write_file( from, tpsize, dtsize ) : write_mem( from, tpsize, dtsize );
}
//================================================
void updateFW_control(void)
{
u8 cmd = 0;
u8 crc = 0;
u32* flagAddr;
u32 flagValue = 0;
FLASH_Status status = FLASH_COMPLETE;
while(1)
{
// Reload IWDG counter
IWDG_ReloadCounter();
cmd = USART_GetC();
switch(cmd)
{
case CMD_WRITE:
crc = USART_GetC();
if(crc == (CMD_WRITE^0xFF))
{
// Ack to host
USART_PutC(ACK);
write_mem();
}
else
USART_PutC(NACK);
break;
case CMD_REBOOT:
crc = USART_GetC();
if(crc == (CMD_REBOOT^0xFF))
{
// Ack to host
USART_PutC(ACK);
// Reboot uC
NVIC_SystemReset();
}
else
USART_PutC(NACK);
break;
case CMD_UPDATE_COMPLETE:
if(fUpdating)
{
// Write memmory
FLASH_UnlockBank1();
status = FLASH_ErasePage(FLAG_ADDR);
if(status == FLASH_COMPLETE)
status = FLASH_ProgramWord(FLAG_ADDR,(u32)FLAG_UPDATED);
fUpdating = false;
FLASH_LockBank1();
if(status == FLASH_COMPLETE)
USART_PutC(ACK);
else
USART_PutC(NACK);
}
else
USART_PutC(NACK);
break;
case CMD_RUN_APP:
// Read out flag value
flagAddr = (u32*)FLAG_ADDR;
flagValue = (u32)(*flagAddr);
if(flagValue == FLAG_UPDATED)
{
USART_PutC(ACK);
// Disable systick
SysTick->CTRL &=~( SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_TICKINT_Msk |
SysTick_CTRL_ENABLE_Msk);
// Jump to user application
JumpAddress = *(__IO u32*) (MAIN_APP_ADDR+ 4);
Jump_To_Application = (pFunction) JumpAddress;
// Initialize user application's Stack Pointer
__set_MSP(*(__IO u32*) MAIN_APP_ADDR);
Jump_To_Application();
}
else
{
USART_PutC(NACK);
}
break;
default:
break;
}
timingCount = 0;
}
}
int
viornd_notifyq(void)
{
uint64_t q_gpa;
uint32_t vr_sz;
size_t sz;
int ret;
char *buf, *rnd_data;
struct vring_desc *desc;
struct vring_avail *avail;
struct vring_used *used;
ret = 0;
/* Invalid queue? */
if (viornd.cfg.queue_notify > 0)
return (0);
vr_sz = vring_size(VIORND_QUEUE_SIZE);
q_gpa = viornd.vq[viornd.cfg.queue_notify].qa;
q_gpa = q_gpa * VIRTIO_PAGE_SIZE;
buf = malloc(vr_sz);
if (buf == NULL) {
log_warn("malloc error getting viornd ring");
return (0);
}
memset(buf, 0, vr_sz);
if (read_mem(q_gpa, buf, vr_sz)) {
free(buf);
return (0);
}
desc = (struct vring_desc *)(buf);
avail = (struct vring_avail *)(buf +
viornd.vq[viornd.cfg.queue_notify].vq_availoffset);
used = (struct vring_used *)(buf +
viornd.vq[viornd.cfg.queue_notify].vq_usedoffset);
sz = desc[avail->ring[avail->idx]].len;
if (sz > MAXPHYS)
fatal("viornd descriptor size too large (%zu)", sz);
rnd_data = malloc(sz);
if (rnd_data != NULL) {
arc4random_buf(rnd_data, desc[avail->ring[avail->idx]].len);
if (write_mem(desc[avail->ring[avail->idx]].addr,
rnd_data, desc[avail->ring[avail->idx]].len)) {
log_warnx("viornd: can't write random data @ "
"0x%llx",
desc[avail->ring[avail->idx]].addr);
} else {
/* ret == 1 -> interrupt needed */
/* XXX check VIRTIO_F_NO_INTR */
ret = 1;
viornd.cfg.isr_status = 1;
used->ring[used->idx].id = avail->ring[avail->idx];
used->ring[used->idx].len =
desc[avail->ring[avail->idx]].len;
used->idx++;
if (write_mem(q_gpa, buf, vr_sz)) {
log_warnx("viornd: error writing vio ring");
}
}
free(rnd_data);
} else
fatal("memory allocation error for viornd data");
free(buf);
return (ret);
}