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();
}

/*
*	@fn		nm_drv_init
*	@brief	Initialize NMC1000 driver
*	@return	M2M_SUCCESS in case of success and Negative error code in case of failure
*   @param [in]	arg
*				Generic argument
*	@author	M. Abdelmawla
*	@date	15 July 2012
*	@version	1.0
*/
sint8 nm_drv_init(void * arg)
{
	tstrM2mRev strtmp;
	sint8 ret = M2M_SUCCESS;
	uint8 u8Mode = M2M_WIFI_MODE_NORMAL;
	
	if(NULL != arg) {
		if(M2M_WIFI_MODE_CONFIG == *((uint8 *)arg)) {
			u8Mode = M2M_WIFI_MODE_CONFIG;
		} else {
			/*continue running*/
		}
	} else {
		/*continue running*/
	}
	
	ret = nm_bus_iface_init(NULL);
	if (M2M_SUCCESS != ret) {
		M2M_ERR("[nmi start]: fail init bus\n");
		goto ERR1;
	}

#ifdef BUS_ONLY
	return;
#endif
	
	
#ifdef NO_HW_CHIP_EN
	ret = chip_wake();
	nm_bsp_sleep(10);
	if (M2M_SUCCESS != ret) {
		M2M_ERR("[nmi start]: fail chip_wakeup\n");
		goto ERR2;
	}
	/**
	Go...
	**/
	ret = chip_reset();
	if (M2M_SUCCESS != ret) {
		goto ERR2;
	}
#endif
	M2M_INFO("Chip ID %lx\n", nmi_get_chipid());
#ifdef CONF_WINC_USE_SPI
	/* Must do this after global reset to set SPI data packet size. */
	nm_spi_init();
#endif
#ifdef NO_HW_CHIP_EN
	/*return power save to default value*/
	chip_idle();

	ret = cpu_start();
	if (M2M_SUCCESS != ret) {
		goto ERR2;
	}
#endif
	ret = wait_for_bootrom(u8Mode);
	if (M2M_SUCCESS != ret) {
		goto ERR2;
	}
		
	ret = wait_for_firmware_start(u8Mode);
	if (M2M_SUCCESS != ret) {
		goto ERR2;
	}
	
	if(M2M_WIFI_MODE_CONFIG == u8Mode) {
		goto ERR1;
	} else {
		/*continue running*/
	}
	
	ret = enable_interrupts();
	if (M2M_SUCCESS != ret) {
		M2M_ERR("failed to enable interrupts..\n");
		goto ERR2;
	}
	
	ret = nm_get_firmware_info(&strtmp);

	M2M_INFO("Firmware ver   : %u.%u.%u\n", strtmp.u8FirmwareMajor, strtmp.u8FirmwareMinor, strtmp.u8FirmwarePatch);
	M2M_INFO("Min driver ver : %u.%u.%u\n", strtmp.u8DriverMajor, strtmp.u8DriverMinor, strtmp.u8DriverPatch);
	M2M_INFO("Curr driver ver: %u.%u.%u\n", M2M_DRIVER_VERSION_MAJOR_NO, M2M_DRIVER_VERSION_MINOR_NO, M2M_DRIVER_VERSION_PATCH_NO);

	if(strtmp.u8FirmwareMajor != M2M_DRIVER_VERSION_MAJOR_NO
			|| strtmp.u8FirmwareMinor != M2M_DRIVER_VERSION_MINOR_NO)
	{
		ret = M2M_ERR_FW_VER_MISMATCH;
		M2M_ERR("Firmware version mismatch!\n");
	}
//.........这里部分代码省略.........

void main () 
{
   int R_B,L_B, R_VU, L_VU, avg_R=0, avg_L=0;
   
   coldstart();                        // Grundinitialisierung
      enable_interrupts(GLOBAL);
      
   set_adc_channel(0);
   delay_ms (1);
   avg_L = read_adc();
   
   set_adc_channel(1);
   delay_ms (1);
   avg_R = read_adc();


   while (TRUE)                         // Beginn Hauptprogramm
   {
      
        //      VU-Meter
      set_adc_channel(0);
      delay_ms (1);
      L_VU = read_adc();
      
      set_adc_channel(1);
      delay_ms (1);
      R_VU = read_adc();
      
      avg_L = (avg_L + L_VU) /2;
      avg_R = (avg_R + R_VU) /2;
      
      if((avg_L + avg_R) >= 3){
 
         //      VU-Meter - Portout
         L_B = Makevu (L_VU);
         R_B = Makevu (R_VU);
         Portout (L_B,R_B);
         
         //      Bass, mid, treble
           
      
         set_adc_channel(2);
         delay_ms (1);
         if (read_adc() > 560) output_low (Ledb_p);
         else output_high (Ledb_p);
         
         set_adc_channel(3);
         delay_ms (1);
         if (read_adc() > 570) output_low (Ledm_p);
         else output_high (Ledm_p);
         
         set_adc_channel(4);
         delay_ms (1);
         if (read_adc() > 550) output_low (Ledt_p);
         else output_high (Ledt_p);
      }
      else {
         LEDTest8();
         checkKeys();
         LEDTest6();
         checkKeys();
         LEDTest3();
         checkKeys();
         LEDTest4();
         checkKeys();
         LEDTest2();
         checkKeys();
         LEDTest5();
         checkKeys();
         LEDTest7();
         checkKeys();
         LEDTest1();
      }
      checkKeys();
   } 
}

static int
at91_attach(device_t dev)
{
	struct at91_softc *sc = device_get_softc(dev);
	int i;

	arm_post_filter = at91_eoi;

	at91_softc = sc;
	sc->sc_st = &at91_bs_tag;
	sc->sc_sh = AT91_BASE;
	sc->sc_aic_sh = AT91_BASE + AT91_SYS_BASE;
	sc->dev = dev;

	sc->sc_irq_rman.rm_type = RMAN_ARRAY;
	sc->sc_irq_rman.rm_descr = "AT91 IRQs";
	if (rman_init(&sc->sc_irq_rman) != 0 ||
	    rman_manage_region(&sc->sc_irq_rman, 1, 31) != 0)
		panic("at91_attach: failed to set up IRQ rman");

	sc->sc_mem_rman.rm_type = RMAN_ARRAY;
	sc->sc_mem_rman.rm_descr = "AT91 Memory";
	if (rman_init(&sc->sc_mem_rman) != 0)
		panic("at91_attach: failed to set up memory rman");
	/*
	 * Manage the physical space, defined as being everything that isn't
	 * DRAM.
	 */
	if (rman_manage_region(&sc->sc_mem_rman, 0, PHYSADDR - 1) != 0)
		panic("at91_attach: failed to set up memory rman");
	if (rman_manage_region(&sc->sc_mem_rman, PHYSADDR + (256 << 20),
	    0xfffffffful) != 0)
		panic("at91_attach: failed to set up memory rman");

	/*
	 * Setup the interrupt table.
	 */
	if (soc_info.soc_data == NULL || soc_info.soc_data->soc_irq_prio == NULL)
		panic("Interrupt priority table missing\n");
	for (i = 0; i < 32; i++) {
		bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_SVR +
		    i * 4, i);
		/* Priority. */
		bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_SMR + i * 4,
		    soc_info.soc_data->soc_irq_prio[i]);
		if (i < 8)
			bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_EOICR,
			    1);
	}

	bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_SPU, 32);
	/* No debug. */
	bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_DCR, 0);
	/* Disable and clear all interrupts. */
	bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_IDCR, 0xffffffff);
	bus_space_write_4(sc->sc_st, sc->sc_aic_sh, IC_ICCR, 0xffffffff);

        /*
         * Add this device's children...
         */
	at91_cpu_add_builtin_children(dev, soc_info.soc_data->soc_children);
	soc_info.soc_data->soc_clock_init();

	bus_generic_probe(dev);
	bus_generic_attach(dev);
	enable_interrupts(I32_bit | F32_bit);
	return (0);
}

void
undefinedinstruction(struct trapframe *frame)
{
	struct thread *td;
	u_int fault_pc;
	int fault_instruction;
	int fault_code;
	int coprocessor;
	struct undefined_handler *uh;
	int error;
#ifdef VERBOSE_ARM32
	int s;
#endif
	ksiginfo_t ksi;

	/* Enable interrupts if they were enabled before the exception. */
	if (__predict_true(frame->tf_spsr & PSR_I) == 0)
		enable_interrupts(PSR_I);
	if (__predict_true(frame->tf_spsr & PSR_F) == 0)
		enable_interrupts(PSR_F);

	VM_CNT_INC(v_trap);

	fault_pc = frame->tf_pc;

	/*
	 * Get the current thread/proc structure or thread0/proc0 if there is
	 * none.
	 */
	td = curthread == NULL ? &thread0 : curthread;

	coprocessor = 0;
	if ((frame->tf_spsr & PSR_T) == 0) {
		/*
		 * Make sure the program counter is correctly aligned so we
		 * don't take an alignment fault trying to read the opcode.
		 */
		if (__predict_false((fault_pc & 3) != 0)) {
			ksiginfo_init_trap(&ksi);
			ksi.ksi_signo = SIGILL;
			ksi.ksi_code = ILL_ILLADR;
			ksi.ksi_addr = (u_int32_t *)(intptr_t) fault_pc;
			trapsignal(td, &ksi);
			userret(td, frame);
			return;
		}

		/*
		 * Should use fuword() here .. but in the interests of
		 * squeezing every bit of speed we will just use ReadWord().
		 * We know the instruction can be read as was just executed
		 * so this will never fail unless the kernel is screwed up
		 * in which case it does not really matter does it ?
		 */

		fault_instruction = *(u_int32_t *)fault_pc;

		/* Check for coprocessor instruction */

		/*
		 * According to the datasheets you only need to look at bit
		 * 27 of the instruction to tell the difference between and
		 * undefined instruction and a coprocessor instruction
		 * following an undefined instruction trap.
		 */

		if (ARM_COPROC_INSN(fault_instruction))
			coprocessor = ARM_COPROC(fault_instruction);
		else {          /* check for special instructions */
			if (ARM_VFP_INSN(fault_instruction))
				coprocessor = COPROC_VFP; /* vfp / simd */
		}
	} else {
#if __ARM_ARCH >= 7
		fault_instruction = *(uint16_t *)fault_pc;
		if (THUMB_32BIT_INSN(fault_instruction)) {
			fault_instruction <<= 16;
			fault_instruction |= *(uint16_t *)(fault_pc + 2);

			/*
			 * Is it a Coprocessor, Advanced SIMD, or
			 * Floating-point instruction.
			 */
			if (THUMB_COPROC_INSN(fault_instruction)) {
				if (THUMB_COPROC_UNDEFINED(fault_instruction)) {
					/* undefined insn */
				} else if (THUMB_VFP_INSN(fault_instruction))
					coprocessor = COPROC_VFP;
				else
					coprocessor =
					    THUMB_COPROC(fault_instruction);
			}
		}
#else
		/*
		 * No support for Thumb-2 on this cpu
		 */
		ksiginfo_init_trap(&ksi);
		ksi.ksi_signo = SIGILL;
		ksi.ksi_code = ILL_ILLADR;
//.........这里部分代码省略.........