Exemple #1
0
//-----------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);
}
Exemple #2
0
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");
	}
}
Exemple #3
0
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);
}
Exemple #4
0
//----------- 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();
	}
}
Exemple #5
0
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);
}
Exemple #6
0
/*
 * 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);
}
Exemple #7
0
/*
 * 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);
}
Exemple #8
0
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;
}
Exemple #9
0
/* 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, &regnum) && 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*)&regnum) && *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;
}
Exemple #11
0
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;
}
Exemple #12
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, &regs);
	
	
#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, &regs);

	/* 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;
}
Exemple #14
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;
        }
    }
}
Exemple #15
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;
}
Exemple #16
0
/* 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;
}
Exemple #17
0
/*
 * 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);
}
Exemple #18
0
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);
}
Exemple #19
0
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);
	}
}
Exemple #20
0
// ####### 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]);
		}
	}
}
Exemple #21
0
//-------------------------------------------------------------------
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);
}
Exemple #22
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);
}
Exemple #23
0
int iostream::write( void* from, int tpsize, int dtsize )
{
	return ( srct == 0 ) ? write_file( from, tpsize, dtsize ) : write_mem( from, tpsize, dtsize );
}
Exemple #24
0
//================================================
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;
	}
}
Exemple #25
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);
}