C++ enable_interrupts函数代码示例

int interrupt_init(void)
{
	enable_interrupts();

	return 0;
}

void
x86_unexpected_exception(iframe* frame)
{
	debug_exception_type type;
	uint32 signalNumber;
	int32 signalCode;
	addr_t signalAddress = 0;
	int32 signalError = B_ERROR;

	switch (frame->vector) {
		case 0:		// Divide Error Exception (#DE)
			type = B_DIVIDE_ERROR;
			signalNumber = SIGFPE;
			signalCode = FPE_INTDIV;
			signalAddress = frame->ip;
			break;

		case 4:		// Overflow Exception (#OF)
			type = B_OVERFLOW_EXCEPTION;
			signalNumber = SIGFPE;
			signalCode = FPE_INTOVF;
			signalAddress = frame->ip;
			break;

		case 5:		// BOUND Range Exceeded Exception (#BR)
			type = B_BOUNDS_CHECK_EXCEPTION;
			signalNumber = SIGTRAP;
			signalCode = SI_USER;
			break;

		case 6:		// Invalid Opcode Exception (#UD)
			type = B_INVALID_OPCODE_EXCEPTION;
			signalNumber = SIGILL;
			signalCode = ILL_ILLOPC;
			signalAddress = frame->ip;
			break;

		case 13: 	// General Protection Exception (#GP)
			type = B_GENERAL_PROTECTION_FAULT;
			signalNumber = SIGILL;
			signalCode = ILL_PRVOPC;	// or ILL_PRVREG
			signalAddress = frame->ip;
			break;

		case 16: 	// x87 FPU Floating-Point Error (#MF)
			type = B_FLOATING_POINT_EXCEPTION;
			signalNumber = SIGFPE;
			signalCode = FPE_FLTDIV;
				// TODO: Determine the correct cause via the FPU status
				// register!
			signalAddress = frame->ip;
			break;

		case 17: 	// Alignment Check Exception (#AC)
			type = B_ALIGNMENT_EXCEPTION;
			signalNumber = SIGBUS;
			signalCode = BUS_ADRALN;
			// TODO: Also get the address (from where?). Since we don't enable
			// alignment checking this exception should never happen, though.
			signalError = EFAULT;
			break;

		case 19: 	// SIMD Floating-Point Exception (#XF)
			type = B_FLOATING_POINT_EXCEPTION;
			signalNumber = SIGFPE;
			signalCode = FPE_FLTDIV;
				// TODO: Determine the correct cause via the MXCSR register!
			signalAddress = frame->ip;
			break;

		default:
			x86_invalid_exception(frame);
			return;
	}

	if (IFRAME_IS_USER(frame)) {
		struct sigaction action;
		Thread* thread = thread_get_current_thread();

		enable_interrupts();

		// If the thread has a signal handler for the signal, we simply send it
		// the signal. Otherwise we notify the user debugger first.
		if ((sigaction(signalNumber, NULL, &action) == 0
				&& action.sa_handler != SIG_DFL
				&& action.sa_handler != SIG_IGN)
			|| user_debug_exception_occurred(type, signalNumber)) {
			Signal signal(signalNumber, signalCode, signalError,
				thread->team->id);
			signal.SetAddress((void*)signalAddress);
			send_signal_to_thread(thread, signal, 0);
		}
	} else {
		char name[32];
		panic("Unexpected exception \"%s\" occurred in kernel mode! "
			"Error code: 0x%lx\n",
			exception_name(frame->vector, name, sizeof(name)),
			frame->error_code);
	}
}

int cache_post_test (int flags)
{
    void *virt = (void *)CONFIG_SYS_POST_CACHE_ADDR;
    int ints;
    int res = 0;
    int tlb = -1;		/* index to the victim TLB entry */

    /*
     * All 44x variants deal with cache management differently
     * because they have the address translation always enabled.
     * The 40x ppc's don't use address translation in U-Boot at all,
     * so we have to distinguish here between 40x and 44x.
     */
#ifdef CONFIG_440
    int word0, i;

    /*
     * Allocate a new TLB entry, since we are going to modify
     * the write-through and caching inhibited storage attributes.
     */
    program_tlb((u32)testarea, (u32)virt, CACHE_POST_SIZE,
                TLB_WORD2_I_ENABLE);

    /* Find the TLB entry */
    for (i = 0;; i++) {
        if (i >= PPC4XX_TLB_SIZE) {
            printf ("Failed to program tlb entry\n");
            return -1;
        }
        word0 = mftlb1(i);
        if (TLB_WORD0_EPN_DECODE(word0) == (u32)virt) {
            tlb = i;
            break;
        }
    }
#endif
    ints = disable_interrupts ();

    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test1 (tlb, virt, CACHE_POST_SIZE);
    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test2 (tlb, virt, CACHE_POST_SIZE);
    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test3 (tlb, virt, CACHE_POST_SIZE);
    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test4 (tlb, virt, CACHE_POST_SIZE);
    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test5 (tlb, virt, CACHE_POST_SIZE);
    WATCHDOG_RESET ();
    if (res == 0)
        res = cache_post_test6 (tlb, virt, CACHE_POST_SIZE);

    if (ints)
        enable_interrupts ();

#ifdef CONFIG_440
    remove_tlb((u32)virt, CACHE_POST_SIZE);
#endif

    return res;
}

int do_bootm (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	ulong		iflag;
	ulong		load_end = 0;
	int		ret;
	boot_os_fn	*boot_fn;
#ifdef CONFIG_NEEDS_MANUAL_RELOC
	static int relocated = 0;

	/* relocate boot function table */
	if (!relocated) {
		int i;
		for (i = 0; i < ARRAY_SIZE(boot_os); i++)
			if (boot_os[i] != NULL)
				boot_os[i] += gd->reloc_off;
		relocated = 1;
	}
#endif

	/* determine if we have a sub command */
	if (argc > 1) {
		char *endp;

		simple_strtoul(argv[1], &endp, 16);
		/* endp pointing to NULL means that argv[1] was just a
		 * valid number, pass it along to the normal bootm processing
		 *
		 * If endp is ':' or '#' assume a FIT identifier so pass
		 * along for normal processing.
		 *
		 * Right now we assume the first arg should never be '-'
		 */
		if ((*endp != 0) && (*endp != ':') && (*endp != '#'))
			return do_bootm_subcommand(cmdtp, flag, argc, argv);
	}

	if (bootm_start(cmdtp, flag, argc, argv))
		return 1;

	/*
	 * We have reached the point of no return: we are going to
	 * overwrite all exception vector code, so we cannot easily
	 * recover from any failures any more...
	 */
	iflag = disable_interrupts();

#if defined(CONFIG_CMD_USB)
	/*
	 * turn off USB to prevent the host controller from writing to the
	 * SDRAM while Linux is booting. This could happen (at least for OHCI
	 * controller), because the HCCA (Host Controller Communication Area)
	 * lies within the SDRAM and the host controller writes continously to
	 * this area (as busmaster!). The HccaFrameNumber is for example
	 * updated every 1 ms within the HCCA structure in SDRAM! For more
	 * details see the OpenHCI specification.
	 */
	usb_stop();
#endif

	ret = bootm_load_os(images.os, &load_end, 1);

	if (ret < 0) {
		if (ret == BOOTM_ERR_RESET)
			do_reset (cmdtp, flag, argc, argv);
		if (ret == BOOTM_ERR_OVERLAP) {
			if (images.legacy_hdr_valid) {
				if (image_get_type (&images.legacy_hdr_os_copy) == IH_TYPE_MULTI)
					puts ("WARNING: legacy format multi component "
						"image overwritten\n");
			} else {
				puts ("ERROR: new format image overwritten - "
					"must RESET the board to recover\n");
				show_boot_progress (-113);
				do_reset (cmdtp, flag, argc, argv);
			}
		}
		if (ret == BOOTM_ERR_UNIMPLEMENTED) {
			if (iflag)
				enable_interrupts();
			show_boot_progress (-7);
			return 1;
		}
	}

	lmb_reserve(&images.lmb, images.os.load, (load_end - images.os.load));

	if (images.os.type == IH_TYPE_STANDALONE) {
		if (iflag)
			enable_interrupts();
		/* This may return when 'autostart' is 'no' */
		bootm_start_standalone(iflag, argc, argv);
		return 0;
	}

	show_boot_progress (8);

#ifdef CONFIG_SILENT_CONSOLE
	if (images.os.os == IH_OS_LINUX)
		fixup_silent_linux();
#endif
//.........这里部分代码省略.........

/* reads data from FDC, seek commands are issued automatic */
int fdc_read_data(unsigned char *buffer, unsigned long blocks,FDC_COMMAND_STRUCT *pCMD, FD_GEO_STRUCT *pFG)
{
  /* first seek to start address */
	unsigned long len,lastblk,readblk,i,timeout,ii,offset;
	unsigned char pcn,c,retriesrw,retriescal;
	unsigned char *bufferw; /* working buffer */
	int sect_size;
	int flags;

	flags=disable_interrupts(); /* switch off all Interrupts */
	select_fdc_drive(pCMD); /* switch on drive */
	sect_size=0x080<<pFG->sect_code;
	retriesrw=0;
	retriescal=0;
	offset=0;
	if(fdc_seek(pCMD,pFG)==FALSE) {
		stop_fdc_drive(pCMD);
		enable_interrupts();
		return FALSE;
	}
	if((pCMD->result[STATUS_0]&0x20)!=0x20) {
		printf("Seek error Status: %02X\n",pCMD->result[STATUS_0]);
		stop_fdc_drive(pCMD);
		enable_interrupts();
		return FALSE;
	}
	pcn=pCMD->result[STATUS_PCN]; /* current track */
	/* now determine the next seek point */
	lastblk=pCMD->blnr + blocks;
	/*	readblk=(pFG->head*pFG->sect)-(pCMD->blnr%(pFG->head*pFG->sect)); */
	readblk=pFG->sect-(pCMD->blnr%pFG->sect);
	PRINTF("1st nr of block possible read %ld start %ld\n",readblk,pCMD->blnr);
	if(readblk>blocks) /* is end within 1st track */
		readblk=blocks; /* yes, correct it */
	PRINTF("we read %ld blocks start %ld\n",readblk,pCMD->blnr);
	bufferw = &buffer[0]; /* setup working buffer */
	do {
retryrw:
		len=sect_size * readblk;
		pCMD->cmd[COMMAND]=FDC_CMD_READ;
		if(fdc_issue_cmd(pCMD,pFG)==FALSE) {
			stop_fdc_drive(pCMD);
			enable_interrupts();
			return FALSE;
		}
		for (i=0;i<len;i++) {
			timeout=FDC_TIME_OUT;
			do {
				c=read_fdc_reg(FDC_MSR);
				if((c&0xC0)==0xC0) {
					bufferw[i]=read_fdc_reg(FDC_FIFO);
					break;
				}
				if((c&0xC0)==0x80) { /* output */
					PRINTF("Transfer error transfered: at %ld, MSR=%02X\n",i,c);
					if(i>6) {
						for(ii=0;ii<7;ii++) {
							pCMD->result[ii]=bufferw[(i-7+ii)];
						} /* for */
					}
					if(retriesrw++>FDC_RW_RETRIES) {
						if (retriescal++>FDC_CAL_RETRIES) {
							stop_fdc_drive(pCMD);
							enable_interrupts();
							return FALSE;
						}
						else {
							PRINTF(" trying to recalibrate Try %d\n",retriescal);
							if(fdc_recalibrate(pCMD,pFG)==FALSE) {
								stop_fdc_drive(pCMD);
								enable_interrupts();
								return FALSE;
							}
							retriesrw=0;
							goto retrycal;
						} /* else >FDC_CAL_RETRIES */
					}
					else {
						PRINTF("Read retry %d\n",retriesrw);
						goto retryrw;
					} /* else >FDC_RW_RETRIES */
				}/* if output */
				timeout--;
			}while(TRUE);
		} /* for len */
		/* the last sector of a track or all data has been read,
		 * we need to get the results */
		fdc_terminate(pCMD);
		offset+=(sect_size*readblk); /* set up buffer pointer */
		bufferw = &buffer[offset];
		pCMD->blnr+=readblk; /* update current block nr */
		blocks-=readblk; /* update blocks */
		if(blocks==0)
			break; /* we are finish */
		/* setup new read blocks */
		/*	readblk=pFG->head*pFG->sect; */
		readblk=pFG->sect;
		if(readblk>blocks)
			readblk=blocks;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	dataflash_print_info();
#endif

	/* initialize environment */
	if (should_load_env())
		env_relocate();
	else
		set_default_env(NULL);

#if defined(CONFIG_CMD_PCI) || defined(CONFIG_PCI)
	arm_pci_init();
#endif

	stdio_init();	/* get the devices list going. */

	jumptable_init();

#if defined(CONFIG_API)
	/* Initialize API */
	api_init();
#endif

	console_init_r();	/* fully init console as a device */

#ifdef CONFIG_DISPLAY_BOARDINFO_LATE
# ifdef CONFIG_OF_CONTROL
	/* Put this here so it appears on the LCD, now it is ready */
	display_fdt_model(gd->fdt_blob);
# else
	checkboard();
# endif
#endif

#if defined(CONFIG_ARCH_MISC_INIT)
	/* miscellaneous arch dependent initialisations */
	arch_misc_init();
#endif
#if defined(CONFIG_MISC_INIT_R)
	/* miscellaneous platform dependent initialisations */
	misc_init_r();
#endif

	 /* set up exceptions */
	interrupt_init();
	/* enable exceptions */
	enable_interrupts();

	/* Initialize from environment */
	load_addr = getenv_ulong("loadaddr", 16, load_addr);

#ifdef CONFIG_BOARD_LATE_INIT
	board_late_init();
#endif

#ifdef CONFIG_BITBANGMII
	bb_miiphy_init();
#endif
#if defined(CONFIG_CMD_NET)
	puts("Net:   ");
	eth_initialize(gd->bd);
#if defined(CONFIG_RESET_PHY_R)
	debug("Reset Ethernet PHY\n");
	reset_phy();
#endif
#endif

#ifdef CONFIG_POST
	post_run(NULL, POST_RAM | post_bootmode_get(0));
#endif

#if defined(CONFIG_PRAM) || defined(CONFIG_LOGBUFFER)
	/*
	 * Export available size of memory for Linux,
	 * taking into account the protected RAM at top of memory
	 */
	{
		ulong pram = 0;
		uchar memsz[32];

#ifdef CONFIG_PRAM
		pram = getenv_ulong("pram", 10, CONFIG_PRAM);
#endif
#ifdef CONFIG_LOGBUFFER
#ifndef CONFIG_ALT_LB_ADDR
		/* Also take the logbuffer into account (pram is in kB) */
		pram += (LOGBUFF_LEN + LOGBUFF_OVERHEAD) / 1024;
#endif
#endif
		sprintf((char *)memsz, "%ldk", (gd->ram_size / 1024) - pram);
		setenv("mem", (char *)memsz);
	}
#endif

	/* main_loop() can return to retry autoboot, if so just run it again. */
	for (;;) {
		main_loop();    /* commom/main.c */
	}

	/* NOTREACHED - no way out of command loop except booting */
}

void main()
{
  UBYTE i, j;

  disable_interrupts();
  DISPLAY_OFF;
  LCDC_REG = 0x67;
  /*
   * LCD        = Off
   * WindowBank = 0x9C00
   * Window     = On
   * BG Chr     = 0x8800
   * BG Bank    = 0x9800
   * OBJ        = 8x16
   * OBJ        = On
   * BG         = On
   */

  doorstate = CLOSED;

  /* Set palettes */
  BGP_REG = OBP0_REG = OBP1_REG = 0xE4U;

  /* Initialize the background */
  set_bkg_data(0xFC, 0x04, std_data);
  set_bkg_data(0x00, 0x2D, bkg_data);
  /*
   * Draw the background
   *
   * Width  = 0x100 = 0x20 * 8
   * Height = 0x100 = 0x20 * 8
   */
  for(i = 0; i < 32; i+=8)
    for(j = 0; j < 32; j+=8)
      set_bkg_tiles(i, j, 8, 8, bkg_tiles);
  bposx.w = 0;
  SCX_REG = 0;
  bposy.w = 0;
  SCY_REG = 0;
  bspx.w = 0xFF00;
  bspy.w = 0x0080;

  /* Initialize the window */
  set_win_data(0x80, 0x21, frame_data);
  /*
   * Draw the frame in the window
   *
   * Width  = 0x80 = 0x10 * 8
   * Height = 0x50 = 0x0A * 8
   */
  set_win_tiles(0, 0, 16, 10, frame_tiles);
  /*
   * Draw the door in the window
   *
   * Width  = 0x60 = 0x20 * 12
   * Height = 0x30 = 0x20 * 6
   */
  set_win_tiles(2, 2, 12, 6, door1_tiles);
  wposx.b.h = MAXWNDPOSX;
  wposx.b.l = 0;
  WX_REG = MAXWNDPOSX;
  wposy.b.h = MAXWNDPOSY;
  wposy.b.l = 0;
  WY_REG = MAXWNDPOSY;
  wspx.w = 0xFF80;
  wspy.w = 0xFFC0;

  /* Initialize the sprite */
  set_sprite_data(0x00, 0x1C, earth_data);
  set_sprite_prop(0, 0x00);
  set_sprite_prop(1, 0x00);
  sframe = 0;
  sposx.w  = 0x1000;
  sposy.w  = 0x1000;
  sspx.w  = 0x0040;
  sspy.w  = 0x0040;
  tile_sprite();
  place_sprite();

  DISPLAY_ON;
  enable_interrupts();

  while(1) {
    /* Skip four VBLs (slow down animation) */
    for(i = 0; i < 4; i++)
      wait_vbl_done();
    time++;
    fade();
    door();
    scroll();
    animate_sprite();
    i = joypad();
    if(i & J_B) {
      if(i & J_UP)
	bspy.w -= 0x0010;
      if(i & J_DOWN)
	bspy.w += 0x0010;
      if(i & J_LEFT)
	bspx.w -= 0x0010;
      if(i & J_RIGHT)
//.........这里部分代码省略.........

/**
 * Execute selected states of the bootm command.
 *
 * Note the arguments to this state must be the first argument, Any 'bootm'
 * or sub-command arguments must have already been taken.
 *
 * Note that if states contains more than one flag it MUST contain
 * BOOTM_STATE_START, since this handles and consumes the command line args.
 *
 * Also note that aside from boot_os_fn functions and bootm_load_os no other
 * functions we store the return value of in 'ret' may use a negative return
 * value, without special handling.
 *
 * @param cmdtp		Pointer to bootm command table entry
 * @param flag		Command flags (CMD_FLAG_...)
 * @param argc		Number of subcommand arguments (0 = no arguments)
 * @param argv		Arguments
 * @param states	Mask containing states to run (BOOTM_STATE_...)
 * @param images	Image header information
 * @param boot_progress 1 to show boot progress, 0 to not do this
 * @return 0 if ok, something else on error. Some errors will cause this
 *	function to perform a reboot! If states contains BOOTM_STATE_OS_GO
 *	then the intent is to boot an OS, so this function will not return
 *	unless the image type is standalone.
 */
int do_bootm_states(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[],
		    int states, bootm_headers_t *images, int boot_progress)
{
	boot_os_fn *boot_fn;
	ulong iflag = 0;
	int ret = 0, need_boot_fn;

	images->state |= states;

	/*
	 * Work through the states and see how far we get. We stop on
	 * any error.
	 */
	if (states & BOOTM_STATE_START)
		ret = bootm_start(cmdtp, flag, argc, argv);

	if (!ret && (states & BOOTM_STATE_FINDOS))
		ret = bootm_find_os(cmdtp, flag, argc, argv);

	if (!ret && (states & BOOTM_STATE_FINDOTHER)) {
		ret = bootm_find_other(cmdtp, flag, argc, argv);
		argc = 0;	/* consume the args */
	}

	/* Load the OS */
	if (!ret && (states & BOOTM_STATE_LOADOS)) {
		ulong load_end;

		iflag = bootm_disable_interrupts();
		ret = bootm_load_os(images, &load_end, 0);
		if (ret == 0)
			lmb_reserve(&images->lmb, images->os.load,
				    (load_end - images->os.load));
		else if (ret && ret != BOOTM_ERR_OVERLAP)
			goto err;
		else if (ret == BOOTM_ERR_OVERLAP)
			ret = 0;
#if defined(CONFIG_SILENT_CONSOLE) && !defined(CONFIG_SILENT_U_BOOT_ONLY)
		if (images->os.os == IH_OS_LINUX)
			fixup_silent_linux();
#endif
	}

	/* Relocate the ramdisk */
#ifdef CONFIG_SYS_BOOT_RAMDISK_HIGH
	if (!ret && (states & BOOTM_STATE_RAMDISK)) {
		ulong rd_len = images->rd_end - images->rd_start;

		ret = boot_ramdisk_high(&images->lmb, images->rd_start,
			rd_len, &images->initrd_start, &images->initrd_end);
		if (!ret) {
			setenv_hex("initrd_start", images->initrd_start);
			setenv_hex("initrd_end", images->initrd_end);
		}
	}
#endif
#if defined(CONFIG_OF_LIBFDT) && defined(CONFIG_LMB)
	if (!ret && (states & BOOTM_STATE_FDT)) {
		boot_fdt_add_mem_rsv_regions(&images->lmb, images->ft_addr);
		ret = boot_relocate_fdt(&images->lmb, &images->ft_addr,
					&images->ft_len);
	}
#endif

	/* From now on, we need the OS boot function */
	if (ret)
		return ret;
	boot_fn = bootm_os_get_boot_func(images->os.os);
	need_boot_fn = states & (BOOTM_STATE_OS_CMDLINE |
			BOOTM_STATE_OS_BD_T | BOOTM_STATE_OS_PREP |
			BOOTM_STATE_OS_FAKE_GO | BOOTM_STATE_OS_GO);
	if (boot_fn == NULL && need_boot_fn) {
		if (iflag)
			enable_interrupts();
		printf("ERROR: booting os '%s' (%d) is not supported\n",
//.........这里部分代码省略.........

void watchdog_reset(void)
{
	int re_enable = disable_interrupts();
	reset_4xx_watchdog();
	if (re_enable) enable_interrupts();
}

int flash_erase(flash_info_t *info, int s_first, int s_last)
{
	vu_char *caddr = (vu_char *)(info->start[0]);
	vu_char *caddr_s;
	int flag, prot, sect;
	int rc;

	if (info->flash_id == FLASH_UNKNOWN) {
		printf("Can't erase unknown flash type %08lx - aborted\n", info->flash_id);
		return (ERR_UNKNOWN_FLASH_TYPE);
	}

	if ((s_first < 0) || (s_first > s_last)) {
		printf("- No sectors to erase\n");
		return (ERR_INVAL);
	}

	prot = 0;
	for (sect = s_first; sect <= s_last; ++sect) {
		if (info->protect[sect]) {
			prot++;
		}
	}

	if (prot) {
		printf("- Warning: %d protected sectors will not be erased\n", prot);
	} else {
		printf("\n");
	}

	/* Disable interrupts which might cause a timeout here */
	flag = disable_interrupts();

	/* Start erase on unprotected sectors */
	for (sect = s_first; sect <= s_last; sect++) {
		if (info->protect[sect] == 0) {	/* not protected */
			caddr_s = (vu_char *)(info->start[sect]);

			printf("Erasing sector %2d @ %08lX... ", sect, info->start[sect]);

			caddr[0xAAA] = 0xAA;
			caddr[0x555] = 0x55;
			caddr[0xAAA] = 0x80;
			caddr[0xAAA] = 0xAA;
			caddr[0x555] = 0x55;
			caddr_s[0] = 0x30;

			reset_timer_masked();

			rc = ERR_OK;
			do {
				u8 result;

				if (get_timer_masked() > CFG_FLASH_ERASE_TOUT) {
					rc = ERR_TIMOUT;
					break;
				}

				result = caddr_s[0];

				if (result & 0x80) {
					break;
				}
				if (result & 0x20) {
					rc = ERR_PROG_ERROR;
					break;
				}
			} while (!rc);

			caddr[0xAAA] = 0x0F;

			if (rc == ERR_OK) {
				printf("OK\n");
			} else {
				printf("Failed");
				return (rc);
			}
		}
	}

	/* re-enable interrupts if necessary */
	if (flag)
		enable_interrupts();

	/* reset to read mode */
	caddr = (vu_char *)info->start[0];
	caddr[0] = 0xF0; /* reset bank */
	udelay_masked(10000);

	return (ERR_OK);
}

int main(void) {
  /* Early system initialisation */
  early_board_init();
  system_init_early();
  leds_init();

  set_busy_led(1);
  set_dirty_led(0);

  /* Due to an erratum in the LPC17xx chips anything that may change */
  /* peripheral clock scalers must come before system_init_late()    */
  uart_init();
#ifndef SPI_LATE_INIT
  spi_init(SPI_SPEED_SLOW);
#endif
  timer_init();
  i2c_init();

  /* Second part of system initialisation, switches to full speed on ARM */
  system_init_late();
  enable_interrupts();

  /* Prompt software name and version string */
  uart_puts_P(PSTR("\r\nNODISKEMU " VERSION "\r\n"));

  /* Internal-only initialisation, called here because it's faster */
  buffers_init();
  buttons_init();

  /* Anything that does something which needs the system clock */
  /* should be placed after system_init_late() */
  rtc_init();    // accesses I2C
  disk_init();   // accesses card
  read_configuration(); // restores configuration, may change device address

  filesystem_init(0);
  // FIXME: change_init();

#ifdef CONFIG_REMOTE_DISPLAY
  /* at this point all buffers should be free, */
  /* so just use the data area of the first to build the string */
  uint8_t *strbuf = buffers[0].data;
  ustrcpy_P(strbuf, versionstr);
  ustrcpy_P(strbuf+ustrlen(strbuf), longverstr);
  if (display_init(ustrlen(strbuf), strbuf)) {
    display_address(device_address);
    display_current_part(0);
  }
#endif

  set_busy_led(0);

#if defined(HAVE_SD)
  /* card switch diagnostic aid - hold down PREV button to use */
  if (menu_system_enabled && get_key_press(KEY_PREV))
    board_diagnose();
#endif

  if (menu_system_enabled)
    lcd_splashscreen();

  bus_interface_init();
  bus_init();
  read_configuration();
  late_board_init();

  for (;;) {
    if (menu_system_enabled)
      lcd_refresh();
    else {
      lcd_clear();
      lcd_printf("#%d", device_address);
    }
    /* Unit number may depend on hardware and stored settings */
    /* so present it here at last */
    printf("#%02d\r\n", device_address);
    bus_mainloop();
    bus_interface_init();
    bus_init();    // needs delay, inits device address with HW settings
  }
}

int main(void)
{
    // Disable, configure, and start the watchdog timer
    iwdg_reset();
    iwdg_set_period_ms(8000);
    iwdg_start();

    // Start and configure all hardware peripherals
    enable_interrupts();
    led_init();
    //radio_init();
    gps_init();
    eeprom_init();
    //radio_enable();

    // Set the radio shift and baud rate
    //_radio_dac_write(RADIO_COARSE, RADIO_CENTER_FREQ);
    //_radio_dac_write(RADIO_FINE, 0);
    //radio_set_shift(RADIO_SHIFT_425);
    //radio_set_baud(RADIO_BAUD_50);

    // Radio chatter
    for(uint8_t i = 0; i < 5; i++)
    {
        //radio_chatter();
        iwdg_reset();
    }
    
    int32_t lat = 0, lon = 0, alt = 0;
    uint8_t hour = 0, minute = 0, second = 0, lock = 0, sats = 0;

    while(true)
    {
        led_set(LED_GREEN, 1);

        // Get the current system tick and increment
        uint32_t tick;
        eeprom_read_dword(ticks_addr, &tick);
        tick += 1;

        // Check that we're in airborne <1g mode
        if( gps_check_nav() != 0x06 ) led_set(LED_RED, 1);

        // Get information from the GPS
        gps_check_lock(&lock, &sats);
        if( lock == 0x02 || lock == 0x03 || lock == 0x04 )
        {
            gps_get_position(&lat, &lon, &alt);
            gps_get_time(&hour, &minute, &second);
        }

        led_set(LED_GREEN, 0);

        // Format the telemetry string & transmit
        double lat_fmt = (double)lat / 10000000.0;
        double lon_fmt = (double)lon / 10000000.0;
        alt /= 1000;

        sprintf(s, "$$" CALLSIGN ",%lu,%02u:%02u:%02u,%02.7f,%03.7f,%ld,%u,%x",
            tick, hour, minute, second, lat_fmt, lon_fmt, alt,
            sats, lock);
        //radio_chatter();
        //radio_transmit_sentence(s);
        //radio_chatter();

        led_set(LED_RED, 0);
        eeprom_write_dword(ticks_addr, tick);
        iwdg_reset();
        //_delay_ms(500);
    }

    return 0;
}

//.........这里部分代码省略.........
    initializeColorSensor();
    ///////////////////////////////////////

#endif


    // configure the hardware USART device
#ifdef __USE18F26J50
    Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
            USART_CONT_RX & USART_BRGH_LOW, 0x19);
#else
#ifdef __USE18F46J50

#ifndef MOTOR_PIC
    Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
            USART_CONT_RX & USART_BRGH_LOW, 38);
#else
    Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
            USART_CONT_RX & USART_BRGH_LOW, 0x19);
#endif

#else
    OpenUSART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
            USART_CONT_RX & USART_BRGH_HIGH, 38);
    //    BAUDCONbits.BRG16 = 1;
    //    TXSTAbits.TXEN = 1;
    //    RCSTAbits.SPEN = 1;
    //    RCSTAbits.CREN = 1;
#endif
#endif

    // Peripheral interrupts can have their priority set to high or low
    // enable high-priority interrupts and low-priority interrupts
    enable_interrupts();
    LATBbits.LB7 = 0;
#ifndef MASTER_PIC
    LATBbits.LB0 = 0;
#endif
    LATBbits.LB1 = 0;
    LATBbits.LB2 = 0;
    LATBbits.LB3 = 0;
    WRITETIMER0(0x00FF);
    

    // loop forever
    // This loop is responsible for "handing off" messages to the subroutines
    // that should get them.  Although the subroutines are not threads, but
    // they can be equated with the tasks in your task diagram if you
    // structure them properly.
    while (1) {
        // Call a routine that blocks until either on the incoming
        // messages queues has a message (this may put the processor into
        // an idle mode)
        block_on_To_msgqueues();

        //We have a bunch of queues now - ToMainHigh, ToMainLow, FromMainHigh, FromMainLow,
        //FromUARTInt, and FromI2CInt
        //From queues are most important because they will be called repeatedly with busy info
        //Int queues are second because we'll often get data from either UART or I2C
        //ToMain are least

        length = FromMainHigh_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
        if (length < 0) {
            // no message, check the error code to see if it is concern
            if (length != MSGQUEUE_EMPTY) {
                // This case be handled by your code.

void board_init_r(gd_t *id, ulong dest_addr)
{

	if (id)
		gd = id;

	gd->flags |= GD_FLG_RELOC;	/* tell others: relocation done */

#ifdef CONFIG_SERIAL_MULTI
	serial_initialize();
#endif

#ifdef CONFIG_POST
	post_output_backlog();
#endif

	/* The Malloc area is at the top of simulated DRAM */
	mem_malloc_init((ulong)gd->ram_buf + gd->ram_size - TOTAL_MALLOC_LEN,
			TOTAL_MALLOC_LEN);

	/* initialize environment */
	env_relocate();

	/* IP Address */
	gd->bd->bi_ip_addr = getenv_IPaddr("ipaddr");

	stdio_init();	/* get the devices list going. */

	jumptable_init();

	console_init_r();	/* fully init console as a device */

#if defined(CONFIG_DISPLAY_BOARDINFO_LATE)
	checkboard();
#endif

#if defined(CONFIG_ARCH_MISC_INIT)
	/* miscellaneous arch dependent initialisations */
	arch_misc_init();
#endif
#if defined(CONFIG_MISC_INIT_R)
	/* miscellaneous platform dependent initialisations */
	misc_init_r();
#endif

	 /* set up exceptions */
	interrupt_init();
	/* enable exceptions */
	enable_interrupts();

#ifdef CONFIG_BOARD_LATE_INIT
	board_late_init();
#endif

#ifdef CONFIG_POST
	post_run(NULL, POST_RAM | post_bootmode_get(0));
#endif

	sandbox_main_loop_init();

	/*
	 * For now, run the main loop. Later we might let this be done
	 * in the main program.
	 */
	while (1)
		main_loop();

	/* NOTREACHED - no way out of command loop except booting */
}

static void set_expire_event(timeval_t t) {
  disable_interrupts();
  expire_event.time = t;
  schedule_insert(current_time(), &expire_event);
  enable_interrupts();
}