/**
 *
 * Function to print heading for bumpers, cliff, and light detection
 */
void printHeader()
{
	// print the headers
	serial_puts("| Bump L | Bump R | Cliff L  | Cliff LF | Cliff RF | Cliff R | Cliff SL | Cliff SLF | Cliff SRF | Cliff SR |\n\r");
}

/**
 * @brief 
 */
static void uart_dev_close(void)
{
    serial_puts("SW: UART cannot be closed.\n");
    return;
}

/**
 * @brief 
 */
static void uart_dev_ioctl(void)
{
    serial_puts("SW: UART ioctl\n");
    return;
}

/*
 * Use serial_puts() instead of printf() to avoid printf buffer overflow
 * for long help messages
 */
int do_help (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
{
	int i;
	int rcode = 0;

	if (argc == 1) {	/*show list of commands */
		/* pointer arith! */
		int cmd_items = ((int)__diag_cmd_end - (int)__diag_cmd_start) / sizeof(cmd_tbl_t);
		const int cmd_num =10;					/* there is a bug,  cmd_num shoud be equal to cmd_items*/
		cmd_tbl_t *cmd_array[cmd_num];	/* Used to be: cmd_tbl_t *cmd_array[cmd_items] */
		int i, j, swaps;

		/* Make array of commands from .diag_cmd section */
		cmdtp = __diag_cmd_start;
		for (i = 0; i < cmd_items; i++) {
			cmd_array[i] = cmdtp++;
		}

		/* Sort command list (trivial bubble sort) */
		for (i = cmd_items - 1; i > 0; --i) {
			swaps = 0;
			for (j = 0; j < i; ++j) {
				if (strcmp (cmd_array[j]->name,
					    cmd_array[j + 1]->name) > 0) {
					cmd_tbl_t *tmp;
					tmp = cmd_array[j];
					cmd_array[j] = cmd_array[j + 1];
					cmd_array[j + 1] = tmp;
					++swaps;
				}
			}
			if (!swaps)
				break;
		}

		/* print short help (usage) */
		for (i = 0; i < cmd_items; i++) {
			const char *usage = cmd_array[i]->usage;

			/* allow user abort */
			if (ctrlc ())
				return 1;
			if (usage == NULL)
				continue;
			serial_puts (usage);
		}
		return 0;
	}
	/*
	 * command help (long version)
	 */
	for (i = 1; i < argc; ++i) {
		if ((cmdtp = find_cmd (argv[i])) != NULL) {
#ifdef	CFG_LONGHELP
			/* found - print (long) help info */
			serial_puts (cmdtp->name);
			serial_putc (' ');
			if (cmdtp->help) {
				serial_puts (cmdtp->help);
			} else {
				serial_puts ("- No help available.\r\n");
				rcode = 1;
			}
			serial_putc ('\n');
#else	/* no long help available */
			if (cmdtp->usage)
				serial_puts (cmdtp->usage);
#endif	/* CFG_LONGHELP */
		} else {
			printf ("Unknown command '%s' - try 'help' without arguments"
							"for list of all known commands\r\n", argv[i]
					);
			rcode = 1;
		}
	}
	return rcode;
}

void
diag_puts(char *str)
{
  serial_puts(str);
}

void cooling(void)
{
	int i;
	writel(0,P_WATCHDOG_TC);//disable Watchdog
	//GPIOX_53 reset chip power ctrl

	clrbits_le32(P_PREG_FGPIO_O, 1<<21);
	clrbits_le32(P_PREG_FGPIO_EN_N, 1<<21);
	for(i=0; i<800; i++)
	{
		__udelay(1000);
	}
	//vcc_12v/24v power down GPIOX_70
	clrbits_le32(P_PREG_GGPIO_O, 1<<6);
	clrbits_le32(P_PREG_GGPIO_EN_N, 1<<6);

	setbits_le32(P_PERIPHS_PIN_MUX_2,((1<<29)|(1<<30)));
	writel(0x18003033, P_UART1_CONTROL);

	serial_puts("\nstandby...\n");

	writel(0x209861f1, P_HHI_GCLK_MPEG0);
	writel(0x208b8028, P_HHI_GCLK_MPEG1);
	writel(0xfffffc07, P_HHI_GCLK_MPEG2);
	writel(0xffc40021, P_HHI_GCLK_OTHER);

	//analog off
	WRITE_CBUS_REG(SAR_ADC_REG3, 0x3008510a);
	//WRITE_CBUS_REG(VGHL_PWM_REG0, 0x0); //the same with off
	//WRITE_CBUS_REG(WIFI_ADC_SAMPLING, 0x0); //the same with off
	WRITE_APB_REG(ADC_EN_ADC, 0x0); //the same with off
	//WRITE_AHB_REG(WIFI_ADC_DAC, 0x0); //the same with off
	//WRITE_AHB_REG(ADC_EN_CMLGEN_RES, 0x0); //the same with off
	//WRITE_AHB_REG(WIFI_SARADC, 0x0); //the same with off
	//usb off
	WRITE_CBUS_REG(PREI_USB_PHY_REG, 0x8040012b);

	//clock off
	//WRITE_CBUS_REG(HHI_DEMOD_CLK_CNTL, 0x400); //the same with off
	//WRITE_CBUS_REG(HHI_SATA_CLK_CNTL, 0x0); //the same with off
	//WRITE_CBUS_REG(HHI_ETH_CLK_CNTL, 0x207); //the same with off
	//WRITE_CBUS_REG(HHI_WIFI_CLK_CNTL, 0x0); //the same with off
	WRITE_CBUS_REG(HHI_VID_CLK_CNTL, 0x840e);
	WRITE_CBUS_REG(HHI_AUD_CLK_CNTL, 0x800018);
	WRITE_CBUS_REG(HHI_MALI_CLK_CNTL, 0x202);
	WRITE_CBUS_REG(HHI_HDMI_CLK_CNTL, 0x203);
	WRITE_CBUS_REG(HHI_MPEG_CLK_CNTL, 0x1083);

	//pll off
	WRITE_CBUS_REG(HHI_DEMOD_PLL_CNTL, 0x8232);
	WRITE_CBUS_REG(HHI_VID_PLL_CNTL, 0x8641);
	WRITE_CBUS_REG(HHI_AUD_PLL_CNTL, 0xca80);
	WRITE_CBUS_REG(HHI_OTHER_PLL_CNTL, 0x887d);

#ifdef SYSTEM_16K
	if (READ_CBUS_REG(HHI_MPEG_CLK_CNTL)&(1<<8))
		CLEAR_CBUS_REG_MASK(HHI_MPEG_CLK_CNTL, (1<<8)); // clk81 = xtal
	SET_CBUS_REG_MASK(HHI_MPEG_CLK_CNTL, (1<<9));		// xtal_rtc = rtc
	WRITE_CBUS_REG_BITS(HHI_MPEG_CLK_CNTL, 0x1, 0, 6);	// devider = 2
	WRITE_CBUS_REG_BITS(HHI_MPEG_CLK_CNTL, 0, 12, 2);	// clk81 src -> xtal_rtc
	SET_CBUS_REG_MASK(HHI_MPEG_CLK_CNTL, (1<<8));		// clk81 = xtal_rtc / devider
#else
	CLEAR_CBUS_REG_MASK(HHI_MPEG_CLK_CNTL, (1<<8)); // clk81 = xtal
	WRITE_CBUS_REG_BITS(HHI_MPEG_CLK_CNTL, 0x1e, 0, 6); // devider = 30
	WRITE_CBUS_REG_BITS(HHI_MPEG_CLK_CNTL, 0, 12, 2);	// clk81 src -> xtal_rtc
	SET_CBUS_REG_MASK(HHI_MPEG_CLK_CNTL, (1<<8));		// clk81 = xtal_rtc / devider
#endif
	CLEAR_CBUS_REG_MASK(HHI_A9_CLK_CNTL, (1<<7));		// clka9 = xtal_rtc / 2
#ifdef SYSTEM_16K
	SET_CBUS_REG_MASK(PREG_CTLREG0_ADDR, 1);
#endif
	WRITE_CBUS_REG(HHI_A9_AUTO_CLK0,
		(2 << 24)	 |	 // sleep select 1000uS timebase
		(0x20 << 16)	  |   // Set the delay wakeup time (32mS)
		(0 << 5)		|	// don't clear the FIQ global mask
		(0 << 4)		|	// don't clear the IRQ global mask
		(2 << 2));				  // Set interrupt wakeup only
	WRITE_CBUS_REG(HHI_A9_AUTO_CLK1,
		(0 << 20)				|	// start delay timebase
		(1 << 12)	 |	 // 1uS enable delay
		(1 << 8)   |   // 1uS gate delay
		(1 << 0));		   // 1us start delay
	SET_CBUS_REG_MASK(HHI_A9_AUTO_CLK0, 1 << 0);
	SET_CBUS_REG_MASK(HHI_SYS_PLL_CNTL, (1<<15));		// turn off sys pll
	
	while(1)
	{
		if(serial_tstc())	break;
	}
	//vcc_12v/24v power on
	setbits_le32(P_PREG_GGPIO_EN_N, 1<<6);
	for(i=0; i<800; i++)
	{
		__udelay(1000);
	}
	//GPIOX_53 reset chip power ctrl
	setbits_le32(P_PREG_FGPIO_O, 1<<21);

	memory_pll_init(0,NULL);

//.........这里部分代码省略.........

void board_init_f(ulong bootflag)
{
	bd_t *bd;
	init_fnc_t **init_fnc_ptr;
	int j;

#ifndef CONFIG_SYS_NO_FLASH
	ulong flash_size;
#endif

	gd = (gd_t *) (CONFIG_SYS_GBL_DATA_OFFSET);

	/* Clear initial global data */
	memset((void *)gd, 0, sizeof(gd_t));

	gd->bd = (bd_t *) (gd + 1);	/* At end of global data */
	gd->baudrate = CONFIG_BAUDRATE;
	gd->cpu_clk = CONFIG_SYS_CLK_FREQ;

	bd = gd->bd;
	bd->bi_memstart = CONFIG_SYS_RAM_BASE;
	bd->bi_memsize = CONFIG_SYS_RAM_SIZE;
	bd->bi_flashstart = CONFIG_SYS_FLASH_BASE;
#if	defined(CONFIG_SYS_SRAM_BASE) && defined(CONFIG_SYS_SRAM_SIZE)
	bd->bi_sramstart = CONFIG_SYS_SRAM_BASE;
	bd->bi_sramsize = CONFIG_SYS_SRAM_SIZE;
#endif
	bd->bi_baudrate = CONFIG_BAUDRATE;
	bd->bi_bootflags = bootflag;	/* boot / reboot flag (for LynxOS)    */

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

	for (init_fnc_ptr = init_sequence, j = 0; *init_fnc_ptr;
	     ++init_fnc_ptr, j++) {
#ifdef DEBUG_INIT_SEQUENCE
		if (j > 9)
			str_init_seq[9] = '0' + (j / 10);
		str_init_seq[10] = '0' + (j - (j / 10) * 10);
		serial_puts(str_init_seq);
#endif
		if ((*init_fnc_ptr + gd->reloc_off) () != 0) {
			hang();
		}
	}
#ifdef DEBUG_INIT_SEQUENCE
	serial_puts(str_init_seq_done);
#endif

	/*
	 * Now that we have DRAM mapped and working, we can
	 * relocate the code and continue running from DRAM.
	 *
	 * Reserve memory at end of RAM for (top down in that order):
	 *  - kernel log buffer
	 *  - protected RAM
	 *  - LCD framebuffer
	 *  - monitor code
	 *  - board info struct
	 */
#ifdef DEBUG_MEM_LAYOUT
	printf("CONFIG_SYS_MONITOR_BASE:       0x%lx\n", CONFIG_SYS_MONITOR_BASE);
	printf("CONFIG_ENV_ADDR:           0x%lx\n", CONFIG_ENV_ADDR);
	printf("CONFIG_SYS_RELOC_MONITOR_BASE: 0x%lx (%d)\n", CONFIG_SYS_RELOC_MONITOR_BASE,
	       CONFIG_SYS_MONITOR_LEN);
	printf("CONFIG_SYS_MALLOC_BASE:        0x%lx (%d)\n", CONFIG_SYS_MALLOC_BASE,
	       CONFIG_SYS_MALLOC_LEN);
	printf("CONFIG_SYS_INIT_SP_OFFSET:     0x%lx (%d)\n", CONFIG_SYS_INIT_SP_OFFSET,
	       CONFIG_SYS_STACK_SIZE);
	printf("CONFIG_SYS_PROM_OFFSET:        0x%lx (%d)\n", CONFIG_SYS_PROM_OFFSET,
	       CONFIG_SYS_PROM_SIZE);
	printf("CONFIG_SYS_GBL_DATA_OFFSET:    0x%lx (%d)\n", CONFIG_SYS_GBL_DATA_OFFSET,
	       GENERATED_GBL_DATA_SIZE);
#endif

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

#if defined(CONFIG_NEEDS_MANUAL_RELOC)
	/*
	 * We have to relocate the command table manually
	 */
	fixup_cmdtable(ll_entry_start(cmd_tbl_t, cmd),
			ll_entry_count(cmd_tbl_t, cmd));
#endif /* defined(CONFIG_NEEDS_MANUAL_RELOC) */

#if defined(CONFIG_CMD_AMBAPP) && defined(CONFIG_SYS_AMBAPP_PRINT_ON_STARTUP)
	puts("AMBA:\n");
	do_ambapp_print(NULL, 0, 0, NULL);
#endif

	/* initialize higher level parts of CPU like time base and timers */
	cpu_init_r();

	/* start timer */
	timer_interrupt_init();

	/*
//.........这里部分代码省略.........

/*==============================================================================
 * - T_telnet_server_start()
 *
 * - fake telnet server task
 */
void T_telnet_server_start ()
{
    int                 socket_id;
    struct sockaddr_in  serv_addr;
    struct sockaddr_in  clnt_addr;
    char                recv_buff[256];
    int                 recv_len;
    socklen_t           addr_len = sizeof(struct sockaddr_in);
    int                 lwip_ret;
    char               *start_cmd;

    socket_id = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (socket_id < 0) {
        serial_printf("telnet: socket() failed!\n");
        return;
    }

    memset(&serv_addr, 0, sizeof(serv_addr));
    serv_addr.sin_len    = sizeof(struct sockaddr_in);
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port   = htons(TELNET_PORT);
    serv_addr.sin_addr.s_addr = INADDR_ANY;

    lwip_ret = bind(socket_id, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
    if (lwip_ret < 0) {
        serial_printf("telnet: bind() failed!\n");
        goto over;
    }

    FOREVER {
        recv_len = recvfrom (socket_id, recv_buff, sizeof(recv_buff), 0, (struct sockaddr *)&clnt_addr, &addr_len);
        if (recv_len <= 0) {
            if (errno != ETIMEDOUT && errno != EWOULDBLOCK) {
                serial_printf("telnet: recvfrom () failed!\n");
                goto over;
            }
            continue;
        }

        _G_telnet_open = 1;       /* when serial_putc() stor in _G_telnet_buffer */
        _G_telnet_buf_context = 0;                  /* clear output buffer */

        /*
         * delete user input head space char
         */
        recv_buff[recv_len] = '\0';                 /* terminal user data */
        start_cmd = recv_buff;
        while (isspace(*start_cmd)) {
        	start_cmd++;
        }

        if (strlen(start_cmd) == 0) { /* cmd_line have no char except blank */
            serial_puts("-> ");
        } else {                      /* deal with cmd_line */

            if ((strncmp(start_cmd, "vi", 2)) == 0 && 
                    ((strlen(start_cmd) == 2) || (isspace(start_cmd[2])))) {           /* is "vi" command */
                serial_puts("\nDon't use VI!");
            } else {
                cmd_do(start_cmd);
            }
            serial_puts("\n-> ");
        }

        sendto (socket_id, _G_telnet_buffer, _G_telnet_buf_context, 0, (const struct sockaddr *)&clnt_addr, addr_len);
        _G_telnet_open = 0;       /* when serial_putc() not stor int _G_telnet_buffer */
    }

over:
    _G_telnet_open = 0;
    close(socket_id);
}

main()
{
	uint8_t state = 0;

	serial_baud_9600();
	serial_mode_8e1();
	serial_transmitter_enable();

	sleep_mode_idle();

	pin13_mode_output();
	pin13_low();

	/* setup timer2 to trigger interrupt a
	 * once every millisecond
	 * 128 * (124 + 1) / 16MHz = 1ms */
	timer2_mode_ctc();
	timer2_clock_d128();
	timer2_compare_a_set(124);
	timer2_interrupt_a_enable();

	adc_reference_internal_5v();
	adc_pin_select(5);
	adc_clock_d128();
	adc_trigger_freerunning();
	adc_trigger_enable();
	adc_interrupt_enable();
	adc_enable();

	sei();
	while (1) {
		uint16_t value;

		cli();
		if (!new_value) {
			sleep_enable();
			sei();
			sleep_cpu();
			sleep_disable();
			continue;
		}
		sei();

		value = adc_data();
		new_value = 0;

		if (state && value < BOUND_LOW) {
			uint16_t now = time;
			char *p;

			timer2_clock_reset();
			time = 0;

			pin13_low();

			p = sprint_uint16_b10(buf, now);
			*p++ = '\n';
			*p = '\0';
			serial_puts(buf);

			state = 0;
			continue;
		}

		if (value > BOUND_HIGH) {
			pin13_high();
			state = 1;
		}
	}
}

SPL_STATIC_FUNC void pll_init(struct pll_clk_settings * plls)
{

	//Enable PLLs pins
	//*P_AM_ANALOG_TOP_REG1 |= 0x1; // Enable DDR_PLL enable pin	
	//#define AM_ANALOG_TOP_REG1    0x206F  ->  0xC11081BC 	
	Wr(AM_ANALOG_TOP_REG1, Rd(AM_ANALOG_TOP_REG1)|1);

	//*P_HHI_MPLL_CNTL5   |= 0x1; // Enable Both MPLL and SYS_PLL enable pin
	//move to following SYS PLL init

	Wr(HHI_MPLL_CNTL, 0x4000067d );
	
	//switch a9 clock to  oscillator in the first.  This is sync mux.
#if 0	
    Wr( HHI_A9_CLK_CNTL, 0);
#else
	Wr( HHI_A9_CLK_CNTL, Rd(HHI_A9_CLK_CNTL) & (~(1<<7)));
	__udelay(10);
	Wr( HHI_A9_CLK_CNTL, 0);
	__udelay(10);
	Wr(HHI_MPEG_CLK_CNTL, Rd(HHI_MPEG_CLK_CNTL) & (~(1<<8)) );	
#endif

	serial_init(52|UART_CNTL_MASK_TX_EN|UART_CNTL_MASK_RX_EN);	//clk81 switch to 24M, init serial to print info
	__udelay(100);

	do{
		//BANDGAP reset for SYS_PLL,AUD_PLL,MPLL lock fail
		//Note: once SYS PLL is up, there is no need to
		//          use AM_ANALOG_TOP_REG1 for AUD, MPLL
		//          lock fail
		Wr_reg_bits(HHI_MPLL_CNTL5,0,0,1);
		__udelay(10);
		Wr_reg_bits(HHI_MPLL_CNTL5,1,0,1);
		__udelay(1000); //1ms for bandgap bootup

		M6TV_PLL_RESET(HHI_SYS_PLL_CNTL);
		Wr(HHI_SYS_PLL_CNTL2,M6TV_SYS_PLL_CNTL_2);
		Wr(HHI_SYS_PLL_CNTL3,M6TV_SYS_PLL_CNTL_3);
		Wr(HHI_SYS_PLL_CNTL4,M6TV_SYS_PLL_CNTL_4);
		Wr(HHI_SYS_PLL_CNTL, plls->sys_pll_cntl);
		//M6TV_PLL_WAIT_FOR_LOCK(HHI_SYS_PLL_CNTL);

		__udelay(500); //wait 100us for PLL lock
		
#ifdef CONFIG_ENABLE_WATCHDOG
		pll_times++;
		if(pll_times > 1){
			serial_puts("\npll_times1:");
			serial_put_dword(pll_times);
			if(pll_times>PLL_TIMES){
				serial_puts(__FILE__);
				serial_puts(__FUNCTION__);
				serial_put_dword(__LINE__);
				AML_WATCH_DOG_START();
			}
		}
#endif
	}while((Rd(HHI_SYS_PLL_CNTL)&0x80000000)==0);

	//A9 clock setting
	Wr(HHI_A9_CLK_CNTL,(plls->sys_clk_cntl & (~(1<<7))));
	__udelay(1);
	//enable A9 clock
	Wr(HHI_A9_CLK_CNTL,(plls->sys_clk_cntl | (1<<7)));

	/*
	//AUDIO PLL
	M6TV_PLL_RESET(HHI_AUDCLK_PLL_CNTL);
	Wr(HHI_AUDCLK_PLL_CNTL2, M6TV_AUD_PLL_CNTL_2 );
	Wr(HHI_AUDCLK_PLL_CNTL3, M6TV_AUD_PLL_CNTL_3 );
	Wr(HHI_AUDCLK_PLL_CNTL4, M6TV_AUD_PLL_CNTL_4 );
	Wr(HHI_AUDCLK_PLL_CNTL5, M6TV_AUD_PLL_CNTL_5 );
	Wr(HHI_AUDCLK_PLL_CNTL6, M6TV_AUD_PLL_CNTL_6 );
	Wr(HHI_AUDCLK_PLL_CNTL,  0x20242 );
	M6TV_PLL_WAIT_FOR_LOCK(HHI_AUDCLK_PLL_CNTL);
	*/
	
	//FIXED PLL/Multi-phase PLL, fixed to 2GHz
	M6TV_PLL_RESET(HHI_MPLL_CNTL);
	Wr(HHI_MPLL_CNTL2, M6TV_MPLL_CNTL_2 );
	Wr(HHI_MPLL_CNTL3, M6TV_MPLL_CNTL_3 );
	Wr(HHI_MPLL_CNTL4, M6TV_MPLL_CNTL_4 );
	Wr(HHI_MPLL_CNTL5, M6TV_MPLL_CNTL_5 );
	Wr(HHI_MPLL_CNTL6, M6TV_MPLL_CNTL_6 );
	Wr(HHI_MPLL_CNTL7, M6TV_MPLL_CNTL_7 );
	Wr(HHI_MPLL_CNTL8, M6TV_MPLL_CNTL_8 );
	Wr(HHI_MPLL_CNTL9, M6TV_MPLL_CNTL_9 );
	Wr(HHI_MPLL_CNTL10,M6TV_MPLL_CNTL_10);
	Wr(HHI_MPLL_CNTL, 0x4000067d );
	M6TV_PLL_WAIT_FOR_LOCK(HHI_MPLL_CNTL);

	//clk81=fclk_div5 /2=400/2=200M
	Wr(HHI_MPEG_CLK_CNTL, plls->mpeg_clk_cntl );
	serial_init(plls->uart);	//clk81 switch to MPLL, init serial to print info

#ifdef CONFIG_ENABLE_WATCHDOG
		pll_times=0;
#endif
//.........这里部分代码省略.........

void serial_buffered_puts (const char *s)
{
	serial_puts (s);
}

ssize_t
SerialConsole::WriteAt(void *cookie, off_t /*pos*/, const void *buffer, size_t bufferSize)
{
	serial_puts((const char *)buffer, bufferSize);
	return bufferSize;
}

/**
*
* Function to loop through the sensor array and decide what to print
* Prints the total number of sensors activated as well as which specific sensors are activated
*/
void printSensorStatus(int arr[])
{
	// print the total number of sensors activated
	int numberSensors = 0;
	char totalString[40];
	for(int i = 0; i < 10; i++) {
		if(sensorArray[i] != 0) {
			numberSensors++;
		}
	}
	sprintf(totalString, "NUMBER OF SENSORS ACTIVATED: %d", numberSensors);
	serial_puts(totalString);
	serial_putc('\n');
	serial_putc('\r');
	//print individually each of the total issues
	if(sensorArray[LEFT_BUMPER] == 1)
	{
		serial_puts("  LEFT BUMPER HAS BEEN HIT\n\r");
	}
	if(sensorArray[RIGHT_BUMPER] == 1)
	{
		serial_puts("  RIGHT BUMPER HAS BEEN HIT\n\r");
	}
	if(sensorArray[CLIFF_LEFT] == 1)
	{
		serial_puts("  OVER LEFT CLIFF\n\r");
	}
	if(sensorArray[CLIFF_FRONT_LEFT] == 1)
	{
		serial_puts("  OVER FRONT LEFT CLIFF\n\r");
	}
	if(sensorArray[CLIFF_FRONT_RIGHT] == 1)
	{
		serial_puts("  OVER FRONT RIGHT CLIFF\n\r");
	}
	if(sensorArray[CLIFF_RIGHT] == 1)
	{
		serial_puts("  OVER RIGHT CLIFF\n\r");
	}
	if(sensorArray[CLIFF_LEFT_SIGNAL] != 0)
	{
		if (sensorArray[CLIFF_LEFT_SIGNAL] == 1) {
			serial_puts("  LEFT OVER WHITE TAPE\n\r");
		}
		else {
			serial_puts("  LEFT OVER BLACK TAPE\n\r");
		}
	}
	if(sensorArray[CLIFF_FRONT_LEFT_SIGNAL] != 0)
	{
		if (sensorArray[CLIFF_FRONT_LEFT_SIGNAL] == 1) {
			serial_puts("  LEFT FRONT OVER WHITE TAPE\n\r");
		}
		else {
			serial_puts("LEFT FRONT OVER BLACK TAPE\n\r");
		}
	}
	if(sensorArray[CLIFF_FRONT_RIGHT_SIGNAL] != 0)
	{
		if (sensorArray[CLIFF_FRONT_RIGHT_SIGNAL] == 1)
		{
			serial_puts("  RIGHT FRONT OVER WHITE TAPE\n\r");
		}
		else
		{
			serial_puts("  RIGHT FRONT OVER BLACK TAPE\n\r");
		}
	}
	if(sensorArray[CLIFF_RIGHT_SIGNAL] != 0)
	{
		if (sensorArray[CLIFF_RIGHT_SIGNAL] == 1)
		{
			serial_puts("  RIGHT OVER WHITE TAPE\n\r");
		}
		else
		{
			serial_puts("  RIGHT OVER BLACK TAPE\n\r");
		}
	}
}

/**
 *
 * Function to control the iRobot depending on the character pressed, call it like keyboardInput(serial_getc());
 * @param c the character that would determine if the iRobot moves, uses serial_getc()
 */
void keyboardInput(char c)
{
	// toggle precision mode, if activated, move is 5 cm and 5 degrees
	if(c == 'T')
	{
		if(precision == 0){
			serial_puts("PRECISION ACTIVATED\n\r\n\r");
			precision = 1;
		}
		else{
			serial_puts("PRECISION DEACTIVATED\n\r\n\r");
			precision = 0;
		}
	}
	
	// move the iRobot forward, 10 cm
	else if(c == 'W')
	{
		if(precision)
		{
			serial_puts("MOVING FORWARD 5 CM\n\r\n\r");
			moveFowardUpdate(sensor_data, 5);
		}
		else
		{
			serial_puts("MOVING FORWARD 10 CM\n\r\n\r");
			moveFowardUpdate(sensor_data, 10);
		}
		wait_ms(100);
	}
	
	// move the iRobot backwards, 10 cm
	else if(c == 'S')
	{
		if(precision)
		{
			serial_puts("MOVING BACKWARD 5 CM\n\r\n\r");
			moveBackward(sensor_data, 5);
		}
		else
		{
			serial_puts("MOVING BACKWARD 10 CM\n\r\n\r");
			moveBackward(sensor_data, 10);
		}
		wait_ms(100);
	}

	// rotate the iRobot counter clockwise, 15 degrees
	else if(c == 'A')
	{
		if(precision)
		{
			serial_puts("TURNING COUNTER CLOCKWISE 5 DEGREES\n\r\n\r");
			turn_counter_clockwise(sensor_data, 5); // TODO
		}
		else
		{
			serial_puts("TURNING COUNTER CLOCKWISE 15 DEGREES\n\r\n\r");
			turn_counter_clockwise(sensor_data, 15); // TODO
		}
		wait_ms(100);
	}

	// rotate the iRobot clockwise, 15 degrees
	else if(c == 'D')
	{
		if(precision)
		{
			serial_puts("TURNING CLOCKWISE 5 DEGREEES\n\r\n\r");
			turn_clockwise(sensor_data, 5); // TODO
		}
		else
		{
			serial_puts("TURNING CLOCKWISE 15 DEGREEES\n\r\n\r");
			turn_clockwise(sensor_data, 15); // TODO
		}
		wait_ms(100);
	}

	// start sweeping for ir and sonar data
	else if(c == ' ')
	{
		oi_play_song(0);
		serial_puts("SWEEPING FOR OBJECTS\n\r");
		smallestObjectSweep();
		wait_ms(100);
	}
	
	// clear screen
	else if(c == '-')
	{
		clearScreen();
		wait_ms(100);
	}
	
//.........这里部分代码省略.........

STATIC_PREFIX
datum * 
memTestDevice(volatile datum * baseAddress, unsigned long nBytes)	
{
    unsigned long offset;
    unsigned long nWords = nBytes / sizeof(datum);
    datum pattern;
    datum antipattern;
    serial_puts("Total Size");serial_put_dword(nBytes);

    /*
     * Fill memory with a known pattern.
     */
    for (pattern = 1, offset = 0; offset < nWords; pattern++, offset++)
    {
        baseAddress[offset] = pattern;
#ifdef AML_DEBUG_ROM
        if(((offset+1)&0x3ffff)==0)
        {
        	serial_putc('\r');serial_put_hex((offset+1)<<2,32);
        	writel(0,P_WATCHDOG_RESET);
        }
#endif

    }
    serial_puts(" Stage 1 finish\n");
//    serial_putc('\n');

    /*
     * Check each location and invert it for the second pass.
     */
    for (pattern = 1, offset = 0; offset < nWords; pattern++, offset++)
    {
        if (baseAddress[offset] != pattern)
        {
                
            return ((datum *) &baseAddress[offset]);
        }

        antipattern = ~pattern;
        baseAddress[offset] = antipattern;
#ifdef AML_DEBUG_ROM
        if(((offset+1)&0x3ffff)==0)
        {
            writel(0,P_WATCHDOG_RESET);
        	serial_putc('\r');serial_put_hex((offset+1)<<2,32);
        }
#endif

    }
    serial_puts(" Stage 2 finish\n");
    
    /*
     * Check each location for the inverted pattern and zero it.
     */
    for (pattern = 1, offset = 0; offset < nWords; pattern++, offset++)
    {
        antipattern = ~pattern;
        if (baseAddress[offset] != antipattern)
        {
            
            return ((datum *) &baseAddress[offset]);
        }
#ifdef AML_DEBUG_ROM
        if(((offset+1)&0x3ffff)==0)
        {
            writel(0,P_WATCHDOG_RESET);
        	serial_putc('\r');serial_put_hex((offset+1)<<2,32);
        }
#endif

    }
#undef AML_DEBUG_ROM
    serial_puts(" Stage 3 finish\n");
    return (NULL);

}   /* memTestDevice() */

void hang(void)
{
	serial_puts("ERROR: please reset the target\n");
	for (;;)
		;
}

void panel_power_off(void)
{
	gpio_direction_output(MIPI_PWR, 0); /* 2.8v en*/
	serial_puts("byd_9177aa panel display off\n");
}

int main(void)
{
	unsigned cmd;
	char c;
	int i = 0,j;
	timer_init();
#ifdef POWER_OFF_VDDIO	
	f_serial_puts("sleep ... off\n");
#else
	f_serial_puts("sleep7 .......\n");
#endif
		
	while(1){
		
		cmd = readl(P_AO_RTI_STATUS_REG0);
		if(cmd == 0)
		{
			delay_ms(10);
			continue;
		}
		c = (char)cmd;
		if(c == 't')
		{
#if (defined(POWER_OFF_VDDIO) || defined(POWER_OFF_HDMI_VCC) || defined(POWER_OFF_AVDD33) || defined(POWER_OFF_AVDD25))
			init_I2C();
#endif
			copy_reboot_code();
			enter_power_down();
			//test_arc_core();
			break;
		}
		else if(c == 'q')
		{
				serial_puts(" - quit command loop\n");
				writel(0,P_AO_RTI_STATUS_REG0);
			  break;
		}
		else
		{
				serial_puts(" - cmd no support (ARC)\n");
		}
		//command executed
		writel(0,P_AO_RTI_STATUS_REG0);
	}
	
	while(1){
	    udelay(6000);
	    cmd = readl(P_AO_RTI_STATUS_REG1);
	    c = (char)cmd;
	    if(c == 0)
	    {
	        udelay(6000);
	        cmd = readl(P_AO_RTI_STATUS_REG1);
	        c = (char)cmd;
	        if((c == 0)||(c!='r'))
	        {
	            #ifdef _UART_DEBUG_COMMUNICATION_
	            serial_put_hex(cmd,32);
	            f_serial_puts(" arm boot fail\n\n");
	            wait_uart_empty();
	            #endif
	            #if 0 //power down 
	            cmd = readl(P_AO_GPIO_O_EN_N);
	            cmd &= ~(1<<6);
	            cmd &= ~(1<<22);
	            writel(cmd,P_AO_GPIO_O_EN_N);
	            #endif
	        }
	    }
	    else if(c=='r')
	    {
	        writel(0,0xc8100030);
	        #ifdef _UART_DEBUG_COMMUNICATION_
	        //f_serial_puts("arm boot succ\n");
	        //wait_uart_empty();
	        #endif
	    }
	    else
	    {
	        #ifdef _UART_DEBUG_COMMUNICATION_
	        serial_put_hex(cmd,32);
	        f_serial_puts(" arm unkonw state\n");
	        wait_uart_empty();
	        #endif
	    }
	    //cmd='f';
	    //writel(cmd,P_AO_RTI_STATUS_REG1);
	    
		asm(".long 0x003f236f"); //add sync instruction.
		//asm("SLEEP");
		asm("FLAG 1");//halt mode
	}
	return 0;
}

/*
 * spl_boot:
 *
 * All supported booting types of all supported SoCs are listed here.
 * Generic readback APIs are provided for each supported booting type
 * eg. nand_read_skip_bad
 */
u32 spl_boot(void)
{
	void (*image)(void);

#ifdef CONFIG_SPEAR_USBTTY
	plat_late_init();
	return 1;
#endif

	/*
	 * All the supported booting devices are listed here. Each of
	 * the booting type supported by the platform would define the
	 * macro xxx_BOOT_SUPPORTED to TRUE.
	 */

	if (SNOR_BOOT_SUPPORTED && snor_boot_selected()) {
		/* SNOR-SMI initialization */
		snor_init();

		serial_puts("Booting via SNOR\n");
		/* Serial NOR booting */
		if (1 == snor_image_load((u8 *)CONFIG_SYS_UBOOT_BASE,
					    &image, loader_name)) {
			/* Platform related late initialasations */
			plat_late_init();

			/* Jump to boot image */
			serial_puts("Jumping to U-Boot\n");
			boot_image(image);
			return 1;
		}
	}

	if (NAND_BOOT_SUPPORTED && nand_boot_selected()) {
		/* NAND booting */
		/* Not ported from XLoader to SPL yet */
		return 0;
	}

	if (PNOR_BOOT_SUPPORTED && pnor_boot_selected()) {
		/* PNOR booting */
		/* Not ported from XLoader to SPL yet */
		return 0;
	}

	if (MMC_BOOT_SUPPORTED && mmc_boot_selected()) {
		/* MMC booting */
		/* Not ported from XLoader to SPL yet */
		return 0;
	}

	if (SPI_BOOT_SUPPORTED && spi_boot_selected()) {
		/* SPI booting */
		/* Not supported for any platform as of now */
		return 0;
	}

	if (I2C_BOOT_SUPPORTED && i2c_boot_selected()) {
		/* I2C booting */
		/* Not supported for any platform as of now */
		return 0;
	}

	/*
	 * All booting types without memory are listed as below
	 * Control has to be returned to BootROM in case of all
	 * the following booting scenarios
	 */

	if (USB_BOOT_SUPPORTED && usb_boot_selected()) {
		plat_late_init();
		return 1;
	}

	if (TFTP_BOOT_SUPPORTED && tftp_boot_selected()) {
		plat_late_init();
		return 1;
	}

	if (UART_BOOT_SUPPORTED && uart_boot_selected()) {
		plat_late_init();
		return 1;
	}

	/* Ideally, the control should not reach here. */
	hang();
}

static int mmc_block_readm(u32 src, u32 num, u8 *dst)
{
	u8 *resp;
	u32 stat, timeout, data, cnt, nob, sorm;

	resp = mmc_cmd(16, 0x200, 0x401, MSC_CMDAT_RESPONSE_R1);
	REG_MSC_BLKLEN = 0x200;
	REG_MSC_NOB = num / 512;
	nob  = num / 512;

	if (nob == 1) {
		if (highcap)
			resp = mmc_cmd(17, src, 0x409, MSC_CMDAT_RESPONSE_R1);
		else
			resp = mmc_cmd(17, src * 512, 0x409, MSC_CMDAT_RESPONSE_R1);
			
		sorm = 0;
	} else {
		if (highcap)
			resp = mmc_cmd(18, src, 0x409, MSC_CMDAT_RESPONSE_R1);
		else
			resp = mmc_cmd(18, src * 512, 0x409, MSC_CMDAT_RESPONSE_R1);

		sorm = 1;
	}

	for (nob; nob >= 1; nob--) {
		timeout = 0x3ffffff;

		while (timeout) {
			timeout--;
			stat = REG_MSC_STAT;
			if (stat & MSC_STAT_TIME_OUT_READ) {
				serial_puts("\n MSC_STAT_TIME_OUT_READ\n\n");
				return -1;
			}
			else if (stat & MSC_STAT_CRC_READ_ERROR) {
				serial_puts("\n MSC_STAT_CRC_READ_ERROR\n\n");
				return -1;
			}
			else if (!(stat & MSC_STAT_DATA_FIFO_EMPTY)) {
				/* Ready to read data */
				break;
			}
			udelay(1);
		}
		if (!timeout) {
			serial_puts("\n mmc/sd read timeout\n");
			return -1;
		}

		/* Read data from RXFIFO. It could be FULL or PARTIAL FULL */
		cnt = 128;
		while (cnt) {
			while (cnt && (REG_MSC_STAT & MSC_STAT_DATA_FIFO_EMPTY))
				;
			cnt --;

			data = REG_MSC_RXFIFO;
			{
				*dst++ = (u8)(data >> 0);
				*dst++ = (u8)(data >> 8);
				*dst++ = (u8)(data >> 16);
				*dst++ = (u8)(data >> 24);
			}
		}
	}

	if (sorm)
		resp = mmc_cmd(12, 0, 0x41, MSC_CMDAT_RESPONSE_R1);

	while (!(REG_MSC_STAT & MSC_STAT_DATA_TRAN_DONE))
		;
	
	REG_MSC_IREG |= MSC_IREG_DATA_TRAN_DONE;	

	jz_mmc_stop_clock();
	return 0;
}