C++ console_init函数代码示例

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	static const u16 spd_addr[] = { DIMM0, 0, 0, 0, DIMM1, 0, 0, 0, };
	int needs_reset = 0;
	u32 bsp_apicid = 0;
	msr_t msr;
	struct cpuid_result cpuid1;
	struct sys_info *sysinfo = &sysinfo_car;

	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0 */
		/* Allow the HT devices to be found */
		enumerate_ht_chain();

		/* sb600_lpc_port80(); */
		sb600_pci_port80();
	}

	if (bist == 0) {
		bsp_apicid = init_cpus(cpu_init_detectedx, sysinfo);
	}

	enable_rs690_dev8(); // enable CFG access to Dev8, which is the SB P2P Bridge
	sb600_lpc_init();
#if defined(DUMP_CMOS_RAM) && (DUMP_CMOS_RAM == 0)
	check_cmos();  // rebooting in case of corrupted cmos !!!!!
#endif
	ite_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
	ite_kill_watchdog(GPIO_DEV);

	console_init();
#if defined(DUMP_CMOS_RAM) && (DUMP_CMOS_RAM == 1)
	check_cmos();  // rebooting in case of corrupted cmos !!!!!
#endif
	post_code(0x03);

	/* Halt if there was a built in self test failure */
	report_bist_failure(bist);
	__DEBUG__("bsp_apicid=0x%x\n", bsp_apicid);

	setup_sitemp_resource_map();

	setup_coherent_ht_domain();

#if CONFIG_LOGICAL_CPUS
	/* It is said that we should start core1 after all core0 launched */
	wait_all_core0_started();
	start_other_cores();
#endif
	wait_all_aps_started(bsp_apicid);

	ht_setup_chains_x(sysinfo);

	/* run _early_setup before soft-reset. */
	rs690_early_setup();
	sb600_early_setup();

	post_code(0x04);

	/* Check to see if processor is capable of changing FIDVID  */
	/* otherwise it will throw a GP# when reading FIDVID_STATUS */
	cpuid1 = cpuid(0x80000007);
	if( (cpuid1.edx & 0x6) == 0x6 ) {

		/* Read FIDVID_STATUS */
		msr=rdmsr(0xc0010042);
		__DEBUG__("begin msr fid, vid: hi=0x%x, lo=0x%x\n", msr.hi, msr.lo);

		enable_fid_change();
		enable_fid_change_on_sb(sysinfo->sbbusn, sysinfo->sbdn);
		init_fidvid_bsp(bsp_apicid);

		/* show final fid and vid */
		msr=rdmsr(0xc0010042);
		__DEBUG__("end msr fid, vid: hi=0x%x, lo=0x%x\n", msr.hi, msr.lo);

	} else {
		__DEBUG__("Changing FIDVID not supported\n");
	}

	post_code(0x05);

	needs_reset = optimize_link_coherent_ht();
	needs_reset |= optimize_link_incoherent_ht(sysinfo);
	rs690_htinit();
	__DEBUG__("needs_reset=0x%x\n", needs_reset);

	post_code(0x06);

	if (needs_reset) {
		__INFO__("ht reset -\n");
		soft_reset();
	}

	allow_all_aps_stop(bsp_apicid);

	/* It's the time to set ctrl now; */
	__DEBUG__("sysinfo->nodes: %2x  sysinfo->ctrl: %p  spd_addr: %p\n",
		     sysinfo->nodes, sysinfo->ctrl, spd_addr);
	fill_mem_ctrl(sysinfo->nodes, sysinfo->ctrl, spd_addr);
//.........这里部分代码省略.........

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	struct sys_info *sysinfo = &sysinfo_car;
	u32 bsp_apicid = 0, val;
	msr_t msr;

	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0 */
		/* Allow the HT devices to be found */
		/* mov bsp to bus 0xff when > 8 nodes */
		set_bsp_node_CHtExtNodeCfgEn();
		enumerate_ht_chain();
		bcm5785_enable_lpc();
		pc87417_enable_dev(RTC_DEV); /* Enable RTC */
	}

	post_code(0x30);

	if (bist == 0)
		bsp_apicid = init_cpus(cpu_init_detectedx, sysinfo);

	pilot_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);

	console_init();

	/* Halt if there was a built in self test failure */
	report_bist_failure(bist);

	pilot_early_init(SERIAL_DEV); //config port is being taken from SERIAL_DEV

	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x\n", val);
	printk(BIOS_DEBUG, "*sysinfo range: [%p,%p]\n",sysinfo,sysinfo+1);
	printk(BIOS_DEBUG, "bsp_apicid = %02x\n", bsp_apicid);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx\n", cpu_init_detectedx);

	/* Setup sysinfo defaults */
	set_sysinfo_in_ram(0);

#if CONFIG_UPDATE_CPU_MICROCODE
	update_microcode(val);
#endif
	post_code(0x33);

	cpuSetAMDMSR();
	post_code(0x34);

	amd_ht_init(sysinfo);
	post_code(0x35);

	/* Setup nodes PCI space and start core 0 AP init. */
	finalize_node_setup(sysinfo);

	post_code(0x36);

	/* wait for all the APs core0 started by finalize_node_setup. */
	/* FIXME: A bunch of cores are going to start output to serial at once.
	 * It would be nice to fixup prink spinlocks for ROM XIP mode.
	 * I think it could be done by putting the spinlock flag in the cache
	 * of the BSP located right after sysinfo.
	 */

	wait_all_core0_started();

#if CONFIG_LOGICAL_CPUS
	/* Core0 on each node is configured. Now setup any additional cores. */
	printk(BIOS_DEBUG, "start_other_cores()\n");
	start_other_cores();
	post_code(0x37);
	wait_all_other_cores_started(bsp_apicid);
#endif

#if CONFIG_SET_FIDVID
	msr = rdmsr(0xc0010071);
	printk(BIOS_DEBUG, "\nBegin FIDVID MSR 0xc0010071 0x%08x 0x%08x\n", msr.hi, msr.lo);

	/* FIXME: The sb fid change may survive the warm reset and only
	 * need to be done once.*/

	enable_fid_change_on_sb(sysinfo->sbbusn, sysinfo->sbdn);

	post_code(0x39);

	if (!warm_reset_detect(0)) {                    // BSP is node 0
		init_fidvid_bsp(bsp_apicid, sysinfo->nodes);
	} else {
		init_fidvid_stage2(bsp_apicid, 0);      // BSP is node 0
	}

	post_code(0x3A);

	/* show final fid and vid */
	msr=rdmsr(0xc0010071);
	printk(BIOS_DEBUG, "End FIDVIDMSR 0xc0010071 0x%08x 0x%08x\n", msr.hi, msr.lo);
#endif

	init_timer();

	/* Reset for HT, FIDVID, PLL and errata changes to take affect. */
	if (!warm_reset_detect(0)) {
//.........这里部分代码省略.........

int _main(unsigned zero, unsigned type, unsigned tags)
{    
    const char *cmdline = 0;
    int n;
    
    arm11_clock_init();

        /* must do this before board_init() so that we
        ** use the partition table in the tags if it 
        ** already exists 
        */
    if((zero == 0) && (type != 0) && tags_okay(tags)) {
        linux_type = type;
        linux_tags = tags;

        cmdline = tags_get_cmdline((void*) linux_tags);
        
        tags_import_partitions((void*) linux_tags);
        revision = tags_get_revision((void*) linux_tags);
        if(revision == 1) {
            console_set_colors(0x03E0, 0xFFFF);
        }
        if(revision == 2) {
            console_set_colors(0x49B2, 0xFFFF);
        }

            /* we're running as a second-stage, so wait for interrupt */
        boot_from_flash = 0;
    } else {
        linux_type = board_machtype();
        linux_tags = 0;
    }

    board_init();
    keypad_init();
    
    console_init();
    dprintf_set_putc(uart_putc);    

    if(linux_tags == 0) {
            /* generate atags containing partitions 
             * from the bootloader, etc 
             */
        linux_tags = ADDR_TAGS;
        create_atags(linux_tags, 0, 0, 0);
    }
    
    if (cmdline) {
        char *sn = strstr(cmdline, SERIALNO_STR);
        if (sn) {
            char *s = serialno;
            sn += SERIALNO_LEN;
            while (*sn && (*sn != ' ') && ((s - serialno) < 31)) {
                *s++ = *sn++;
            }
            *s++ = 0;
        }
    }

    cprintf("\n\nUSB FastBoot:  V%s\n", get_fastboot_version());
    cprintf("Machine ID:    %d v%d\n", linux_type, revision);
    cprintf("Build Date:    "__DATE__", "__TIME__"\n\n");

    cprintf("Serial Number: %s\n\n", serialno[0] ? serialno : "UNKNOWN");

    flash_dump_ptn();

    flash_init();

        /* scan the keyboard a bit */
    for(n = 0; n < 50; n++) {
        boot_poll();
    }

    if (boot_from_flash) {
        cprintf("\n ** BOOTING LINUX FROM FLASH **\n");
        boot_linux_from_flash();
    }

    usbloader_init();
    
    for(;;) {
        usb_poll();
    }
    return 0;
}

int main(int argc, char *argv[])
{
	kbd_event_t ev;
	coord_t coord;
	bool new_file;

	spt_t pt;

	con = console_init(stdin, stdout);
	console_clear(con);

	console_get_size(con, &scr_columns, &scr_rows);

	pane.rows = scr_rows - 1;
	pane.columns = scr_columns;
	pane.sh_row = 1;
	pane.sh_column = 1;

	/* Start with an empty sheet. */
	sheet_init(&doc.sh);

	/* Place caret at the beginning of file. */
	coord.row = coord.column = 1;
	sheet_get_cell_pt(&doc.sh, &coord, dir_before, &pt);
	sheet_place_tag(&doc.sh, &pt, &pane.caret_pos);
	pane.ideal_column = coord.column;

	if (argc == 2) {
		doc.file_name = str_dup(argv[1]);
	} else if (argc > 1) {
		printf("Invalid arguments.\n");
		return -2;
	} else {
		doc.file_name = NULL;
	}

	new_file = false;

	if (doc.file_name == NULL || file_insert(doc.file_name) != EOK)
		new_file = true;

	/* Move to beginning of file. */
	caret_move(-ED_INFTY, -ED_INFTY, dir_before);

	/* Place selection start tag. */
	tag_get_pt(&pane.caret_pos, &pt);
	sheet_place_tag(&doc.sh, &pt, &pane.sel_start);

	/* Initial display */
	cursor_visible = true;

	cursor_hide();
	console_clear(con);
	pane_text_display();
	pane_status_display();
	if (new_file && doc.file_name != NULL)
		status_display("File not found. Starting empty file.");
	pane_caret_display();
	cursor_show();

	done = false;

	while (!done) {
		console_get_kbd_event(con, &ev);
		pane.rflags = 0;

		if (ev.type == KEY_PRESS) {
			/* Handle key press. */
			if (((ev.mods & KM_ALT) == 0) &&
			    ((ev.mods & KM_SHIFT) == 0) &&
			     (ev.mods & KM_CTRL) != 0) {
				key_handle_ctrl(&ev);
			} else if (((ev.mods & KM_ALT) == 0) &&
			    ((ev.mods & KM_CTRL) == 0) &&
			     (ev.mods & KM_SHIFT) != 0) {
				key_handle_shift(&ev);
			} else if ((ev.mods & (KM_CTRL | KM_ALT | KM_SHIFT)) == 0) {
				key_handle_unmod(&ev);
			}
		}

		/* Redraw as necessary. */

		cursor_hide();

		if (pane.rflags & REDRAW_TEXT)
			pane_text_display();
		if (pane.rflags & REDRAW_ROW)
			pane_row_display();
		if (pane.rflags & REDRAW_STATUS)
			pane_status_display();
		if (pane.rflags & REDRAW_CARET)
			pane_caret_display();

		cursor_show();
	}

	console_clear(con);

	return 0;
//.........这里部分代码省略.........

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	struct sys_info *sysinfo = &sysinfo_car;
	static const u8 spd_addr[] = {RC00, DIMM0, DIMM2, 0, 0, DIMM1, DIMM3, 0, 0, };
	u32 bsp_apicid = 0, val;
	msr_t msr;

	timestamp_init(timestamp_get());
	timestamp_add_now(TS_START_ROMSTAGE);

	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0 */
		/* Allow the HT devices to be found */
		/* mov bsp to bus 0xff when > 8 nodes */
		set_bsp_node_CHtExtNodeCfgEn();
		enumerate_ht_chain();
		sb7xx_51xx_pci_port80();
	}

	post_code(0x30);

	if (bist == 0) {
		bsp_apicid = init_cpus(cpu_init_detectedx, sysinfo); /* mmconf is inited in init_cpus */
		/* All cores run this but the BSP(node0,core0) is the only core that returns. */
	}

	post_code(0x32);

	enable_rs780_dev8();
	sb7xx_51xx_lpc_init();

	ite_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
	it8718f_disable_reboot(GPIO_DEV);

	console_init();

	/* Halt if there was a built in self test failure */
	report_bist_failure(bist);

	// Load MPB
	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x\n", val);
	printk(BIOS_DEBUG, "*sysinfo range: [%p,%p]\n",sysinfo,sysinfo+1);
	printk(BIOS_DEBUG, "bsp_apicid = %02x\n", bsp_apicid);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx\n", cpu_init_detectedx);

	/* Setup sysinfo defaults */
	set_sysinfo_in_ram(0);

	update_microcode(val);

	post_code(0x33);

	cpuSetAMDMSR();
	post_code(0x34);

	amd_ht_init(sysinfo);
	post_code(0x35);

	/* Setup nodes PCI space and start core 0 AP init. */
	finalize_node_setup(sysinfo);

	/* Setup any mainboard PCI settings etc. */
	setup_mb_resource_map();
	post_code(0x36);

	/* wait for all the APs core0 started by finalize_node_setup. */
	/* FIXME: A bunch of cores are going to start output to serial at once.
	   It would be nice to fixup prink spinlocks for ROM XIP mode.
	   I think it could be done by putting the spinlock flag in the cache
	   of the BSP located right after sysinfo.
	 */
	wait_all_core0_started();

#if CONFIG_LOGICAL_CPUS
	/* Core0 on each node is configured. Now setup any additional cores. */
	printk(BIOS_DEBUG, "start_other_cores()\n");
	start_other_cores();
	post_code(0x37);
	wait_all_other_cores_started(bsp_apicid);
#endif

	post_code(0x38);

	/* run _early_setup before soft-reset. */
	rs780_early_setup();
	sb7xx_51xx_early_setup();

#if CONFIG_SET_FIDVID
	msr = rdmsr(0xc0010071);
	printk(BIOS_DEBUG, "\nBegin FIDVID MSR 0xc0010071 0x%08x 0x%08x\n", msr.hi, msr.lo);

	/* FIXME: The sb fid change may survive the warm reset and only
	   need to be done once.*/
	enable_fid_change_on_sb(sysinfo->sbbusn, sysinfo->sbdn);

	post_code(0x39);

	if (!warm_reset_detect(0)) {			// BSP is node 0
		init_fidvid_bsp(bsp_apicid, sysinfo->nodes);
//.........这里部分代码省略.........

void boardsupport_init(central_data_t *central_data) 
{
	irq_initialize_vectors();
	cpu_irq_enable();
	Disable_global_interrupt();
		
	// Initialize the sleep manager
	sleepmgr_init();
	sysclk_init();

	board_init();
	delay_init(sysclk_get_cpu_hz());
	time_keeper_init();
		
	INTC_init_interrupts();

	// Switch on the red LED
	LED_On(LED2);

	// servo_pwm_hardware_init();
	pwm_servos_init( CS_ON_SERVO_7_8 );
	
	// Init UART 0 for XBEE communication
	xbee_init(UART0);
				
	// Init UART 4 for wired communication
	//console_init(CONSOLE_UART4);
	// Init USB for wired communication
	console_init(CONSOLE_USB);
		
	// connect abstracted aliases to hardware ports
	central_data->telemetry_down_stream = xbee_get_out_stream();
	central_data->telemetry_up_stream = xbee_get_in_stream();
	central_data->debug_out_stream = console_get_out_stream();
	central_data->debug_in_stream = console_get_in_stream();
	
	// init debug output
	print_util_dbg_print_init(central_data->debug_out_stream);
	print_util_dbg_print("Debug stream initialised\r\n");

	// RC receiver initialization
	spektrum_satellite_init();

	// init imu & compass
	i2c_driver_init(I2C0);
	
	lsm330dlc_init();
	print_util_dbg_print("LSM330 initialised \r\n");
		
	hmc5883l_init_slow();
	print_util_dbg_print("HMC5883 initialised \r\n");
	
	// init radar or ultrasound (not implemented yet)
	//i2c_driver_init(I2C1);
	
	// init 6V enable
	gpio_enable_gpio_pin(AVR32_PIN_PA04);
	gpio_set_gpio_pin(AVR32_PIN_PA04);
	
	Enable_global_interrupt();

	// Init piezo speaker
	piezo_speaker_init_binary();
	
	print_util_dbg_print("Board initialised\r\n");
}

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	static const uint16_t spd_addr[] = {
		// Node 0
		DIMM0, DIMM1, DIMM2, 0,
		0, 0, 0, 0,
		// Node 1
		0, 0, 0, 0,
		0, 0, 0, 0,
	};
	unsigned bsp_apicid = 0;
	int needs_reset = 0;
	struct sys_info *sysinfo = &sysinfo_car;

	winbond_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
	console_init();
	enable_rom_decode();

	print_info("now booting... fallback\n");

	/* Is this a CPU only reset? Or is this a secondary CPU? */
	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0. */
		/* Allow the HT devices to be found. */
		enumerate_ht_chain();
	}

	print_info("now booting... real_main\n");

	if (bist == 0)
		bsp_apicid = init_cpus(cpu_init_detectedx, sysinfo);

	/* Halt if there was a built in self test failure. */
	report_bist_failure(bist);

	setup_default_resource_map();
	setup_coherent_ht_domain();
	wait_all_core0_started();

	print_info("now booting... Core0 started\n");

#if CONFIG_LOGICAL_CPUS
	/* It is said that we should start core1 after all core0 launched. */
	start_other_cores();
	wait_all_other_cores_started(bsp_apicid);
#endif
	init_timer();
	ht_setup_chains_x(sysinfo); /* Init sblnk and sbbusn, nodes, sbdn. */

	needs_reset = optimize_link_coherent_ht();
	needs_reset |= optimize_link_incoherent_ht(sysinfo);
	needs_reset |= k8t890_early_setup_ht();

	if (needs_reset) {
		print_debug("ht reset -\n");
		soft_reset();
	}

	/* the HT settings needs to be OK, because link freq change may cause HT disconnect */
	vt8237_sb_enable_fid_vid();
	enable_fid_change();
	init_fidvid_bsp(bsp_apicid);

	/* Stop the APs so we can start them later in init. */
	allow_all_aps_stop(bsp_apicid);

	/* It's the time to set ctrl now. */
	fill_mem_ctrl(sysinfo->nodes, sysinfo->ctrl, spd_addr);

	enable_smbus();

	/* this seems to be some GPIO on the SMBus--in any case, setting these
	 * two bits reduces the pullup impedance of the bus lines and is required
	 * in order to be able to read SPD info */
	smbus_write_byte(0x48, 0x07, smbus_read_byte(0x48, 0x07) | 0x80);
	smbus_write_byte(0x4a, 0x07, smbus_read_byte(0x4a, 0x07) | 0x10);

	unsigned char mask;

	mask = 0;
//	mask |= 1 /* AGP voltage 1.7 V (not verified, just vendor BIOS value) */
//	mask |= 2 /* V-Link voltage 2.6 V (not verified either) */
	smbus_write_byte(0x4a, 0x00, (smbus_read_byte(0x4a, 0x00) & ~0x0f) | (0x0f ^ (mask << 2)));
	smbus_write_byte(0x4a, 0x01, (smbus_read_byte(0x4a, 0x01) & ~0x03) | (0x03 ^ mask));

	mask = 25; /* RAM voltage in decivolts, valid range from 25 to 28 */
	mask = 3 - (mask - 25);
	smbus_write_byte(0x4a, 0x02, 0x4f | (mask << 4));
	smbus_write_byte(0x4a, 0x03, 0x04 | mask);

	sdram_initialize(sysinfo->nodes, sysinfo->ctrl, sysinfo);
	post_cache_as_ram();
}

int
main(int argc, char *argv[])
{
	int port = 2344;
	const char *config = "sys161.conf";
	const char *kernel = NULL;
	int usetcp=0;
	char *argstr = NULL;
	int j, opt;
	size_t argsize=0;
	int debugwait=0;
	int pass_signals=0;
#ifdef USE_TRACE
	int profiling=0;
#endif
	int use_second_console=0;
	const char *second_console = NULL;
	unsigned ncpus;

	/* This must come absolutely first so msg() can be used. */
	console_earlyinit();
	
	if (sizeof(u_int32_t)!=4) {
		/*
		 * Just in case.
		 */
		msg("sys161 requires sizeof(u_int32_t)==4");
		die();
	}

	while ((opt = mygetopt(argc, argv, "c:f:p:Pst:wk:"))!=-1) {
		switch (opt) {
		    case 'c': config = myoptarg; break;
		    case 'f':
#ifdef USE_TRACE
			set_tracefile(myoptarg);
#endif
			break;
		    case 'p': port = atoi(myoptarg); usetcp=1; break;
		    case 'P':
#ifdef USE_TRACE
			profiling = 1;
#endif
			break;
		    case 's': pass_signals = 1; break;
		    case 't': 
#ifdef USE_TRACE
			set_traceflags(myoptarg); 
#endif
			break;
		    case 'w': debugwait = 1; break;
		    case 'k':
		    use_second_console = 1;
		    second_console = myoptarg;
		    break;
		    default: usage(); break;
		}
	}
	if (myoptind==argc) {
		usage();
	}
	kernel = argv[myoptind++];
	
	for (j=myoptind; j<argc; j++) {
		argsize += strlen(argv[j])+1;
	}
	argstr = malloc(argsize+1);
	if (!argstr) {
		msg("malloc failed");
		die();
	}
	*argstr = 0;
	for (j=myoptind; j<argc; j++) {
		strcat(argstr, argv[j]);
		if (j<argc-1) strcat(argstr, " ");
	}

	/* This must come before bus_config in case a network card needs it */
	mkdir(".sockets", 0700);
	
	console_init(pass_signals, use_second_console, second_console);
	clock_init();
	ncpus = bus_config(config);

	initstats(ncpus);
	cpu_init(ncpus);

	if (usetcp) {
		gdb_inet_init(port);
	}
	else {
		unlink(".sockets/gdb");
		gdb_unix_init(".sockets/gdb");
	}

	unlink(".sockets/meter");
	meter_init(".sockets/meter");

	load_kernel(kernel, argstr);

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

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	u32 val;
	msr_t msr;

	/*
	 * All cores: allow caching of flash chip code and data
	 * (there are no cache-as-ram reliability concerns with family 14h)
	 */
	msr.lo = ((0x0100000000ull - CACHE_ROM_SIZE) | 5) & 0xFFFFFFFF;
	msr.hi = ((0x0100000000ull - CACHE_ROM_SIZE) | 5) >> 32;
	wrmsr (MSR_MTRR_VARIABLE_BASE6, msr);

	msr.lo = ((0x1000000000ull - CACHE_ROM_SIZE) | 0x800) & 0xFFFFFFFF;
	msr.hi = ((0x1000000000ull - CACHE_ROM_SIZE) | 0x800) >> 32;
	wrmsr (MSR_MTRR_VARIABLE_MASK6, msr);

	/* All cores: set pstate 0 (1600 MHz) early to save a few ms of boot time */
	msr.lo = 0;
	msr.hi = 0;
	wrmsr (MSR_PSTATE_CONTROL, msr);

	amd_initmmio();

	if (!cpu_init_detectedx && boot_cpu()) {
		post_code(0x30);
		sb_Poweron_Init();

		post_code(0x31);

		console_init();
	}

	/* Halt if there was a built in self test failure */
	post_code(0x34);
	report_bist_failure(bist);

	/* Load MPB */
	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x\n", val);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx\n", cpu_init_detectedx);

	post_code(0x37);
	agesawrapper_amdinitreset();

	post_code(0x39);
	agesawrapper_amdinitearly();

	int s3resume = acpi_is_wakeup_s3();
	if (!s3resume) {
		post_code(0x40);
		agesawrapper_amdinitpost();

		post_code(0x42);
		agesawrapper_amdinitenv();
		amd_initenv();

	} else { 			/* S3 detect */
		printk(BIOS_INFO, "S3 detected\n");

		post_code(0x60);
		agesawrapper_amdinitresume();

		agesawrapper_amds3laterestore();

		post_code(0x61);
		prepare_for_resume();
	}

	post_code(0x50);
	copy_and_run();
	printk(BIOS_ERR, "Error: copy_and_run() returned!\n");

	post_code(0x54);	/* Should never see this post code. */
}

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	static const uint16_t spd_addr[] = {
		DIMM0, DIMM2, 0, 0,
		DIMM1, DIMM3, 0, 0,
#if CONFIG_MAX_PHYSICAL_CPUS > 1
		DIMM4, DIMM6, 0, 0,
		DIMM5, DIMM7, 0, 0,
#endif
	};

	int needs_reset;
	unsigned nodes, bsp_apicid = 0;
	struct mem_controller ctrl[8];

	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0 */
		/* Allow the HT devices to be found */
		enumerate_ht_chain();
		sio_setup();
	}

	if (bist == 0)
		bsp_apicid = init_cpus(cpu_init_detectedx);

	it8712f_24mhz_clkin();
	it8712f_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
	console_init();

	/* Halt if there was a built in self test failure */
	report_bist_failure(bist);

#if 0
	dump_pci_device(PCI_DEV(0, 0x18, 0));
#endif

	needs_reset = setup_coherent_ht_domain();

	wait_all_core0_started();
#if CONFIG_LOGICAL_CPUS
	/* It is said that we should start core1 after all core0 launched. */
	start_other_cores();
	wait_all_other_cores_started(bsp_apicid);
#endif

	needs_reset |= ht_setup_chains_x();
	needs_reset |= ck804_early_setup_x();
	if (needs_reset) {
		print_info("ht reset -\n");
		soft_reset();
	}

	allow_all_aps_stop(bsp_apicid);

	nodes = get_nodes();
	/* It's the time to set ctrl now. */
	fill_mem_ctrl(nodes, ctrl, spd_addr);

	enable_smbus();

#if 0
	dump_spd_registers(&ctrl[0]);
	dump_smbus_registers();
#endif

	sdram_initialize(nodes, ctrl);

#if 0
	print_pci_devices();
	dump_pci_devices();
#endif

	post_cache_as_ram();
}

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{

	struct sys_info *sysinfo = (struct sys_info *)(CONFIG_DCACHE_RAM_BASE + CONFIG_DCACHE_RAM_SIZE - CONFIG_DCACHE_RAM_GLOBAL_VAR_SIZE);
	u32 bsp_apicid = 0;
	u32 val;
	msr_t msr;

	if (!cpu_init_detectedx && boot_cpu()) {
		/* Nothing special needs to be done to find bus 0 */
		/* Allow the HT devices to be found */
		/* mov bsp to bus 0xff when > 8 nodes */
		set_bsp_node_CHtExtNodeCfgEn();
		enumerate_ht_chain();

		/* Setup the rom access for 4M */
		amd8111_enable_rom();
	}

	post_code(0x30);

	if (bist == 0) {
		bsp_apicid = init_cpus(cpu_init_detectedx, sysinfo); /* mmconf is inited in init_cpus */
		/* All cores run this but the BSP(node0,core0) is the only core that returns. */
	}

	post_code(0x32);

	w83627hf_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
	uart_init();
	console_init();
	printk(BIOS_DEBUG, "\n");

//	dump_mem(CONFIG_DCACHE_RAM_BASE+CONFIG_DCACHE_RAM_SIZE-0x200, CONFIG_DCACHE_RAM_BASE+CONFIG_DCACHE_RAM_SIZE);

	/* Halt if there was a built in self test failure */
	report_bist_failure(bist);

	// Load MPB
	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x \n", val);
	printk(BIOS_DEBUG, "*sysinfo range: [%p,%p]\n",sysinfo,sysinfo+1);
	printk(BIOS_DEBUG, "bsp_apicid = %02x \n", bsp_apicid);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx \n", cpu_init_detectedx);

	/* Setup sysinfo defaults */
	set_sysinfo_in_ram(0);

	update_microcode(val);
	post_code(0x33);

	cpuSetAMDMSR();
	post_code(0x34);

	amd_ht_init(sysinfo);
	post_code(0x35);

	/* Setup nodes PCI space and start core 0 AP init. */
	finalize_node_setup(sysinfo);

	/* Setup any mainboard PCI settings etc. */
	setup_mb_resource_map();
	post_code(0x36);

	/* wait for all the APs core0 started by finalize_node_setup. */
	/* FIXME: A bunch of cores are going to start output to serial at once.
	   It would be nice to fixup prink spinlocks for ROM XIP mode.
	   I think it could be done by putting the spinlock flag in the cache
	   of the BSP located right after sysinfo.
	 */
	wait_all_core0_started();

 #if CONFIG_LOGICAL_CPUS==1
	/* Core0 on each node is configured. Now setup any additional cores. */
	printk(BIOS_DEBUG, "start_other_cores()\n");
	start_other_cores();
	post_code(0x37);
	wait_all_other_cores_started(bsp_apicid);
 #endif

	post_code(0x38);

 #if SET_FIDVID == 1
	msr = rdmsr(0xc0010071);
	printk(BIOS_DEBUG, "\nBegin FIDVID MSR 0xc0010071 0x%08x 0x%08x \n", msr.hi, msr.lo);

	/* FIXME: The sb fid change may survive the warm reset and only
	   need to be done once.*/
	enable_fid_change_on_sb(sysinfo->sbbusn, sysinfo->sbdn);

	post_code(0x39);

	if (!warm_reset_detect(0)) {			// BSP is node 0
		init_fidvid_bsp(bsp_apicid, sysinfo->nodes);
	} else {
		init_fidvid_stage2(bsp_apicid, 0);	// BSP is node 0
	}

	post_code(0x3A);

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

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	u32 val;

	/* In Hudson RRG, PMIOxD2[5:4] is "Drive strength control for
	 *  LpcClk[1:0]".  To be consistent with Parmer, setting to 4mA
	 *  even though the register is not documented in the Kabini BKDG.
	 *  Otherwise the serial output is bad code.
	 */
	outb(0xD2, 0xcd6);
	outb(0x00, 0xcd7);

	AGESAWRAPPER(amdinitmmio);

	hudson_lpc_port80();

	if (!cpu_init_detectedx && boot_cpu()) {
		post_code(0x30);

		post_code(0x31);
		console_init();
	}

	/* Halt if there was a built in self test failure */
	post_code(0x34);
	report_bist_failure(bist);

	/* Load MPB */
	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x \n", val);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx \n", cpu_init_detectedx);

	/* On Larne, after LpcClkDrvSth is set, it needs some time to be stable, because of the buffer ICS551M */
	int i;
	for(i = 0; i < 200000; i++)
		val = inb(0xcd6);

	post_code(0x37);
	AGESAWRAPPER(amdinitreset);
	post_code(0x38);
	printk(BIOS_DEBUG, "Got past yangtze_early_setup\n");

	post_code(0x39);

	AGESAWRAPPER(amdinitearly);
	int s3resume = acpi_is_wakeup_early() && acpi_s3_resume_allowed();
	if (!s3resume) {
		post_code(0x40);
		AGESAWRAPPER(amdinitpost);
		post_code(0x41);
		AGESAWRAPPER(amdinitenv);
		/* TODO: Disable cache is not ok. */
		disable_cache_as_ram();
	} else { /* S3 detect */
		printk(BIOS_INFO, "S3 detected\n");

		post_code(0x60);
		AGESAWRAPPER(amdinitresume);

		AGESAWRAPPER(amds3laterestore);

		post_code(0x61);
		prepare_for_resume();
	}

	outb(0xEA, 0xCD6);
	outb(0x1, 0xcd7);

	post_code(0x50);
	copy_and_run();

	post_code(0x54);  /* Should never see this post code. */
}

void cache_as_ram_main(unsigned long bist, unsigned long cpu_init_detectedx)
{
	u32 val;

#if CONFIG_HAVE_ACPI_RESUME
	void *resume_backup_memory;
#endif

	/*
	 * All cores: allow caching of flash chip code and data
	 * (there are no cache-as-ram reliability concerns with family 14h)
	 */
	__writemsr (0x20c, (0x0100000000ull - CONFIG_ROM_SIZE) | 5);
	__writemsr (0x20d, (0x1000000000ull - CONFIG_ROM_SIZE) | 0x800);

	/* All cores: set pstate 0 (1600 MHz) early to save a few ms of boot time */
	__writemsr (0xc0010062, 0);

	if (!cpu_init_detectedx && boot_cpu()) {
		post_code(0x30);
		sb_Poweron_Init();

		post_code(0x31);
		w83627dhg_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
		console_init();
	}

	/* Halt if there was a built in self test failure */
	post_code(0x34);
	report_bist_failure(bist);

	/* Load MPB */
	val = cpuid_eax(1);
	printk(BIOS_DEBUG, "BSP Family_Model: %08x \n", val);
	printk(BIOS_DEBUG, "cpu_init_detectedx = %08lx \n", cpu_init_detectedx);

	post_code(0x35);
	printk(BIOS_DEBUG, "agesawrapper_amdinitmmio ");
	val = agesawrapper_amdinitmmio();
	if (val)
		printk(BIOS_DEBUG, "error level: %x \n", val);
	else
		printk(BIOS_DEBUG, "passed.\n");

	post_code(0x37);
	printk(BIOS_DEBUG, "agesawrapper_amdinitreset ");
	val = agesawrapper_amdinitreset();
	if (val)
		printk(BIOS_DEBUG, "error level: %x \n", val);
	else
		printk(BIOS_DEBUG, "passed.\n");

	post_code(0x39);
	printk(BIOS_DEBUG, "agesawrapper_amdinitearly ");
	val = agesawrapper_amdinitearly ();
	if (val)
		printk(BIOS_DEBUG, "error level: %x \n", val);
	else
		printk(BIOS_DEBUG, "passed.\n");

#if CONFIG_HAVE_ACPI_RESUME
	if (!acpi_is_wakeup_early()) { /* Check for S3 resume */
#endif
		post_code(0x40);
		printk(BIOS_DEBUG, "agesawrapper_amdinitpost ");
		val = agesawrapper_amdinitpost ();
		/* Reboots with outb(3,0x92), outb(4,0xcf9) or triple-fault all
		 * hang, looks like DRAM re-init goes wrong, don't know why. */
		if (val == 7) /* fatal, amdinitenv below is going to hang */
			outb(0x06, 0x0cf9); /* reset system harder instead */
		if (val)
			printk(BIOS_DEBUG, "error level: %x \n", val);
		else
			printk(BIOS_DEBUG, "passed.\n");

		post_code(0x42);
		printk(BIOS_DEBUG, "agesawrapper_amdinitenv ");
		val = agesawrapper_amdinitenv ();
		if (val)
			printk(BIOS_DEBUG, "error level: %x \n", val);
		else
			printk(BIOS_DEBUG, "passed.\n");

#if CONFIG_HAVE_ACPI_RESUME
	} else { 			/* S3 detect */
		printk(BIOS_INFO, "S3 detected\n");

		post_code(0x60);
		printk(BIOS_DEBUG, "agesawrapper_amdinitresume ");
		val = agesawrapper_amdinitresume();
		if (val)
			printk(BIOS_DEBUG, "error level: %x \n", val);
		else
			printk(BIOS_DEBUG, "passed.\n");

		printk(BIOS_DEBUG, "agesawrapper_amds3laterestore ");
		val = agesawrapper_amds3laterestore ();
		if (val)
			printk(BIOS_DEBUG, "error level: %x \n", val);
		else
//.........这里部分代码省略.........

/* kernel entry point called at the end of the boot sequence */
void __boot() {

  if (current_cpu_id() == 0) {
    /* core 0 boots first, and does all of the initialization */

    // boot parameters are on physical page 0
    bootparams = physical_to_virtual(0x00000000);

    // initialize console early, so output works
    console_init();

    // output should now work
    printf("Welcome to my kernel!\n");
    printf("Running on a %d-way multi-core machine\n", current_cpu_exists());

    // initialize memory allocators
    mem_init();

    // prepare to handle interrupts, exceptions, etc.
    trap_init();

    // initialize keyboard late, since it isn't really used by anything else
    keyboard_init();

    // see which cores are already on
    for (int i = 0; i < 32; i++)
      printf("CPU[%d] is %s\n", i, (current_cpu_enable() & (1<<i)) ? "on" : "off");

    // turn on all other cores
    set_cpu_enable(0xFFFFFFFF);

    // see which cores got turned on
    busy_wait(0.1);
    for (int i = 0; i < 32; i++)
      printf("CPU[%d] is %s\n", i, (current_cpu_enable() & (1<<i)) ? "on" : "off");

  } else {
    /* remaining cores boot after core 0 turns them on */

    // nothing to initialize here... 
  }

  printf("Core %d of %d is alive!\n", current_cpu_id(), current_cpu_exists());

  busy_wait(current_cpu_id() * 0.1); // wait a while so messages from different cores don't get so mixed up
  int size = 64 * 1024 * 4;
  printf("about to do calloc(%d, 1)\n", size);
  unsigned int t0  = current_cpu_cycles();
  calloc(size, 1);
  unsigned int t1  = current_cpu_cycles();
  printf("DONE (%u cycles)!\n", t1 - t0);

  while (1) ;

  for (int i = 1; i < 30; i++) {
    int size = 1 << i;
    printf("about to do calloc(%d, 1)\n", size);
    calloc(size, 1);
  }



  while (1) {
    printf("Core %d is still running...\n", current_cpu_id());
    busy_wait(4.0); // wait 4 seconds
  }

  shutdown();
}

void boardsupport_init(central_data_t *central_data) 
{
	irq_initialize_vectors();
	cpu_irq_enable();
	Disable_global_interrupt();
		
	// Initialize the sleep manager
	sleepmgr_init();
	sysclk_init();

	board_init();
	delay_init(sysclk_get_cpu_hz());
	time_keeper_init();
		
	INTC_init_interrupts();

	// Switch on the red LED
	LED_On(LED2);

	// servo_pwm_hardware_init();
	pwm_servos_init( CS_ON_SERVO_7_8 );
	
	// Init UART 0 for XBEE communication
	xbee_init(UART0);
				
	// Init UART 3 for GPS communication
	gps_ublox_init(&(central_data->gps), UART3);
	
	// Init UART 4 for wired communication
	//console_init(CONSOLE_UART4);
	// Init USB for wired communication
	console_init(CONSOLE_USB);
		
	// connect abstracted aliases to hardware ports
	central_data->telemetry_down_stream = xbee_get_out_stream();
	central_data->telemetry_up_stream = xbee_get_in_stream();
	central_data->debug_out_stream = console_get_out_stream();
	central_data->debug_in_stream = console_get_in_stream();
	
	// init debug output
	print_util_dbg_print_init(central_data->debug_out_stream);
	print_util_dbg_print("Debug stream initialised\r\n");

	// Bind RC receiver with remote
	// spektrum_satellite_bind();

	// RC receiver initialization
	spektrum_satellite_init();

	// Init analog rails
	analog_monitor_conf_t analog_monitor_config = analog_monitor_default_config;
	//analog_monitor_config.conv_factor[ANALOG_RAIL_6] = 0.00023485f * 6.6f;
	//analog_monitor_config.conv_factor[ANALOG_RAIL_7] = 0.00023485f * 6.6f;
	//analog_monitor_config.conv_factor[ANALOG_RAIL_10] = -0.0002409f * 11.0f;
	//analog_monitor_config.conv_factor[ANALOG_RAIL_11] = -0.0002409f * 11.0f;
	analog_monitor_init(&central_data->analog_monitor, &analog_monitor_config);
	
	// init imu & compass
	i2c_driver_init(I2C0);
	
	lsm330dlc_init();
	print_util_dbg_print("LSM330 initialised \r\n");
		
	hmc5883l_init_slow();
	print_util_dbg_print("HMC5883 initialised \r\n");
	
	bmp085_init(&central_data->pressure);
	
	// init radar or ultrasound (not implemented yet)
	//i2c_driver_init(I2C1);
	
	// init 6V enable
	gpio_enable_gpio_pin(AVR32_PIN_PA04);
	gpio_set_gpio_pin(AVR32_PIN_PA04);
	
	Enable_global_interrupt();

	// Init piezo speaker
	piezo_speaker_init_binary();
	
	print_util_dbg_print("Board initialised\r\n");
}