int sflash_platform_init( int peripheral_id, void** platform_peripheral_out )
{
uint8_t temp1, temp2;
uint32_t j;
spi_flash_init();
SPI_FLASH_CS_LOW;
temp1 = spi_wr(0x90);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp2 = spi_wr(0x00);
SPI_FLASH_CS_HIGH;
for(j=0; j<5000; j++);
printf("Flash ID1 %x, %x, %x\r\n", temp1, temp2, j);
SPI_FLASH_CS_LOW;
temp1 = spi_wr(0x90);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp1 = spi_wr(0x00);
temp2 = spi_wr(0x00);
SPI_FLASH_CS_HIGH;
for(j=0; j<5000; j++);
printf("Flash ID2 %x, %x. %x\r\n", temp1, temp2, j);
return 0;
}
/**@brief Function for application main entry.
*/
int main(void)
{
uint32_t err_code;
bool erase_bonds;
// Initialize.
app_trace_init();
timers_init();
buttons_leds_init(&erase_bonds);
ble_stack_init();
device_manager_init(erase_bonds);
gap_params_init();
advertising_init();
services_init();
sensor_simulator_init();
conn_params_init();
#if defined(__RING_SUPPORT__)
btn_init();
spi_flash_init();
sensor_init();
lcd_init();
#endif
// Start execution.
application_timers_start();
err_code = ble_advertising_start(BLE_ADV_MODE_FAST);
APP_ERROR_CHECK(err_code);
// Enter main loop.
for (;;)
{
power_manage();
}
}
/**
****************************************************************************************
* @brief Initialize the spi flash
****************************************************************************************
**/
void spi_flash_peripheral_init(void)
{
GPIO_ConfigurePin( SPI_GPIO_PORT, SPI_CS_PIN, OUTPUT, PID_SPI_EN, true );
GPIO_ConfigurePin( SPI_GPIO_PORT, SPI_CLK_PIN, OUTPUT, PID_SPI_CLK, false );
GPIO_ConfigurePin( SPI_GPIO_PORT, SPI_DO_PIN, OUTPUT, PID_SPI_DO, false );
GPIO_ConfigurePin( SPI_GPIO_PORT, SPI_DI_PIN, INPUT, PID_SPI_DI, false );
spi_FLASH_CS_Pad.pin = SPI_CS_PIN;
spi_FLASH_CS_Pad.port = SPI_GPIO_PORT;
// Enable SPI & SPI FLASH
spi_flash_init(SPI_FLASH_DEFAULT_SIZE, SPI_FLASH_DEFAULT_PAGE);
spi_init(&spi_FLASH_CS_Pad, SPI_MODE_8BIT, SPI_ROLE_MASTER, SPI_CLK_IDLE_POL_LOW,SPI_PHA_MODE_0,SPI_MINT_DISABLE,SPI_XTAL_DIV_2);
}
/**
* Read from flash on RTG4
*/
static int read_program_from_flash(uint8_t *read_buf)
{
uint16_t status;
int flash_address = 0;
int count = 0;
spi_flash_status_t result;
struct device_Info DevInfo;
spi_flash_init();
spi_flash_control_hw( SPI_FLASH_RESET, 0, &status );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* First fetch status register. First byte in low 8 bits, second byte in
* upper 8 bits.
*/
result = spi_flash_control_hw( SPI_FLASH_GET_STATUS, 0, &status );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* Write something to all 32768 blocks of 256 bytes in the 8MB FLASH.
*/
for( count = 0; count != ((32*1024)/256); ++count )
{
/*----------------------------------------------------------------------
* Write our values to the FLASH, read them back and compare.
* Placing a breakpoint on the while statement below will allow
* you break on any failures.
*/
spi_flash_read ( flash_address, read_buf, FLASH_SEGMENT_SIZE );
read_buf += FLASH_SEGMENT_SIZE;
flash_address += FLASH_SEGMENT_SIZE; /* Step to the next 256 byte chunk */
}
UART_polled_tx_string( &g_uart, " Flash read success\n\r" );
return(0);
}
int _spi_rdc(unsigned int dram_addr,unsigned int flash_addr, unsigned int len)
{
UBOOT_TRACE("IN\n");
UBOOT_DEBUG("dram_addr=0x%x\n",dram_addr);
UBOOT_DEBUG("flash_addr=0x%x\n",flash_addr);
UBOOT_DEBUG("len=0x%x\n",len);
/* check alignment and show warning*/
if (flash_addr % SERFLASH_BLOCK_SIZE)
{
//printf("WARNING: it is better to set flash start addr aligned to %d !!!\n", SERFLASH_BLOCK_SIZE);
}
if (dram_addr % SERFLASH_BLOCK_SIZE)
{
//printf("WARNING: it is better to set dram start addr aligned to %d !!!\n", SERFLASH_BLOCK_SIZE);
}
if (len % SERFLASH_BLOCK_SIZE)
{
//printf("WARNING: it is better to set total length aligned to %d !!!\n", SERFLASH_BLOCK_SIZE);
}
/* init SPI flash first */
if (0 == spiInitFlag)
{
spi_flash_init();
}
/* read from flash to dram */
if (0 == MDrv_SERFLASH_Read(flash_addr, len, (MS_U8 *)dram_addr))
{
UBOOT_ERROR("ERROR: SPI DMA fail !!!\n");
return 1;
}
else
{
UBOOT_TRACE("OK\n");
return 0;
}
}
DSTATUS disk_initialize (
BYTE drv /* Physical drive nmuber (0..) */
)
{
int ret;
//switch(drv)
//{
//case MMC:
// bus_in_sdio = 1;
// ret = check_sdcard();
// break;
//case SPI_FLASH:
ret = spi_flash_init();
//}
if( ret == 0 )
return RES_OK;
return RES_ERROR;
}
/*
* Minimal set of commands to read WPSR from SPI.
* Returns 0 on success, < 0 on failure.
*/
int early_spi_read_wpsr(u8 *sr)
{
uint8_t rdsr;
int ret = 0;
spi_flash_init();
/* sending NULL for spiflash struct parameter since we are not
* calling HWSEQ read_status() call via Probe.
*/
ret = pch_hwseq_read_status(NULL, &rdsr);
if (ret) {
printk(BIOS_ERR, "SPI rdsr failed\n");
return ret;
}
*sr = rdsr & WPSR_MASK_SRP0_BIT;
return 0;
}
static void da14583_spi_flash_init(void)
{
SPI_Pad_t cs_pad_param;
// configure internal flash GPIOS
GPIO_ConfigurePin( DA14583_SPI_FLASH_EN_GPIO_PORT, DA14583_SPI_FLASH_EN_GPIO_PIN, OUTPUT, PID_SPI_EN, true );
GPIO_ConfigurePin( DA14583_SPI_FLASH_CLK_GPIO_PORT, DA14583_SPI_FLASH_CLK_GPIO_PIN, OUTPUT, PID_SPI_CLK, false );
GPIO_ConfigurePin( DA14583_SPI_FLASH_DO_GPIO_PORT, DA14583_SPI_FLASH_DO_GPIO_PIN, OUTPUT, PID_SPI_DO, false );
GPIO_ConfigurePin( DA14583_SPI_FLASH_DI_GPIO_PORT, DA14583_SPI_FLASH_DI_GPIO_PIN, INPUT, PID_SPI_DI, false );
cs_pad_param.port = DA14583_SPI_FLASH_EN_GPIO_PORT;
cs_pad_param.pin = DA14583_SPI_FLASH_EN_GPIO_PIN;
// Initialize SPI driver
spi_init(&cs_pad_param, SPI_MODE_8BIT, SPI_ROLE_MASTER, SPI_CLK_IDLE_POL_LOW, SPI_PHA_MODE_0, SPI_MINT_DISABLE, SPI_XTAL_DIV_8);
// Initialize SPI Flash driver
spi_flash_init(DA14583_SPI_FLASH_SIZE, DA14583_SPI_FLASH_PAGE_SIZE);
// Power up flash
spi_flash_release_from_power_down();
}
/**
****************************************************************************************
* @brief Initialize the flash size
****************************************************************************************
*/
void spi_flash_size_init(void)
{
uint32_t flash_size = 0;//SPI_FLASH_DEFAULT_SIZE; // Default memory size in bytes
uint32_t flash_page = 0;//SPI_FLASH_DEFAULT_PAGE; // Default memory page size in bytes
uint16_t man_dev_id = 0;
man_dev_id=spi_read_flash_memory_man_and_dev_id();
switch(man_dev_id)
{
case W25X10CL_MANF_DEV_ID:
flash_size = W25X10CL_SIZE;
flash_page = W25X10CL_PAGE;
break;
case W25X20CL_MANF_DEV_ID:
flash_size = W25X20CL_SIZE;
flash_page = W25X20CL_PAGE;
break;
default:
flash_size = SPI_FLASH_DEFAULT_SIZE;
flash_page = SPI_FLASH_DEFAULT_PAGE;
break;
}
spi_flash_init(flash_size, flash_page);
}
/**
* Starts the initialization, the tasks and then the scheduler.
*/
int main(void)
{
/* disable ETM at very first */
PINSEL10 &= ~((unsigned int)(1 << 3));
/* Initialize BMC hardware */
global_data.bmc_resetcause = 0x00;
/* IPMI message sequence counter */
global_data.seq_counter = 0x00;
/* site number used for entity instance */
global_data.bmc_siteno = 0x00;
/* reset sdr list */
sdr_list.sdr_count = 0;
/* init PLL */
init_clock();
/* init interrupts */
init_irq();
/* get reset cause */
init_resetcause();
/* init port pins */
init_portpins();
/* CUSTOM BOARD INIT CODE LOCATION 1 */
custom_init_1();
/* get HW and IPMB address */
custom_get_hw_ipmb_address();
uptime_init();
/* init RTC */
rtc_init();
/* create the FreeRTOS queues */
create_queues();
uart_init(UART_DEBUG_CONNECTOR, 115200);
uart_printf("Firmware started...\n");
/* init SPI/SSP controllers */
spi_init();
/* init SPI FLASH */
spi_flash_init_devices();
spi_flash_init(&spi_flash);
#ifdef CFG_MMC_I2C_MASTER
/* init BMC master I2C bus */
master_i2c_init(MASTER_BUS_I2C);
#endif
/* init EEPROM file system */
//spi_eeprom_init(&spi_eeprom);
#ifdef CFG_FS
if (fs_init(&efs, &spi_eeprom) != 0)
{
uart_printf("\nEEPROM not accesable!\n");
/* reboot bmc */
lpc_watchdog_start(CFG_BL_WATCHDOG_TIMEOUT, WD_RESET_MODE);
while (1);
}
#endif
/* init leds */
//led_init();
#ifndef CFG_ONCHIP_FLASH_GLOBAL_CONF
/* check EEPROM areas */
//eeprom_integrity_check_areas();
#endif
/* init CLI (and debug console) */
cli_uart_init(115200);
/* handle reset type warm/cold? */
//fru_handle_reset_type();
#ifdef CFG_CM
cm_fru_get_last_known_state();
#endif
#ifdef CFG_EXT_INT
/* init external interrupts */
external_interrupt_init();
#endif
#ifdef CFG_HELPER
/* init the helper task */
helper_init();
#endif
#ifdef CFG_BIOS_FLASH
/* init BIOS FLASH */
spi_flash_init(&bios_flash);
/* init FPGA BIOS flash selection */
bios_restore_active_flash_from_eeprom();
#endif
/* CUSTOM BOARD INIT CODE LOCATION 2 */
//custom_init_2();
/* get global configuration from EEPROM */
global_config();
/* CUSTOM BOARD INIT CODE LOCATION 3 */
//custom_init_3();
/* parse FRU */
fru_init(0);
#if defined (CFG_CM) || defined (CFG_MMC) || defined (CFG_IRTM) || defined(CFG_MCMC)
/* init the IPMB-L interface(s) */
ipmbl_init();
#endif
#ifdef CFG_LAN
/* read and set ncsi mac address from fru */
custom_set_ncsi_mac();
#endif
/* create message pool for IPMI messages */
message_pool_init();
/* init board task */
board_init();
/* init the BMC task */
bmc_init();
#ifdef CFG_PI_SERIAL_BASIC
/* init the payload interface */
pi_uart_b_init(CFG_PI_PORT_RATE);
#endif /* CFG_PI_SERIAL_BASIC */
#ifdef CFG_PI_SERIAL_TERMINAL
/* init the payload interface */
pi_uart_t_init(CFG_PI_PORT_RATE);
#endif /* CFG_PI_SERIAL_TERMINAL */
#ifdef CFG_PI_KCS
/* initialise kcs interface */
kcs_init();
#endif /* CFG_PI_KCS */
#ifdef CFG_ATCA
if (global_conf.operation_mode == OPERATION_MODE_STANDALONE)
{
/* configure IPMB-A and IPMB-B as master only in stanalone mode */
master_i2c_init(IPMB0A_I2C);
master_i2c_init(IPMB0B_I2C);
/* enable IPMB-0 pull ups and buffer */
ipmb0_bus_ctrl(IPMB0_A, IPMB_ENABLE);
ipmb0_bus_ctrl(IPMB0_B, IPMB_ENABLE);
}
else
{
/* init the IPMB-0 interface */
ipmb0_init();
}
#endif
#ifdef CFG_LAN_PLUS
sol_init();
#endif
#ifdef CFG_LAN
/* init ethernet hardware and Task */
eth_start();
#endif
/* init the SDR task */
/* PORT_NOTE: Needs to be started AFTER BMC task because of Semaphore dependency */
sdr_init();
/* parse all PICMG Records in FRU data and store relevant information */
//fru_parse_picmg();
/* init the IPMI message hub */
msg_hub_init();
/* init the event task */
event_init();
#ifdef CFG_CM
/* init the CM task */
init_cm();
#endif
#ifdef CFG_COOLING_MANAGER
cool_init();
#endif
/* do all post tests */
//post_test();
/* needs to be done after sdr initialization */
//hpm_check_bl_flags();
#ifdef CFG_BIOS_FLASH
/* collect BIOS/NVRAM version info (if payload power is off) */
if (!(signal_read(&sig_payload_power_enable)))
{
bios_redundancy_get_versions();
}
#endif
#ifdef CFG_WATCHDOG
/* start the FW watchdog */
lpc_watchdog_start(CFG_FW_WATCHDOG_TIMEOUT, WD_RESET_MODE);
#endif
/* set desired debug output mode before starting scheduler */
global_debug_uart_enabled = CFG_GLOBAL_DEBUG_UART;
/* all tasks have been initialized, start the scheduler */
vTaskStartScheduler();
/* Should never reach here! */
while (1);
}
void dfu_spi_flash_init(void){
spi_flash.slave = SPI_SE_3;
spi_flash_init(&spi_flash);
}
/*
* Write to flash on RTG4
*/
static int write_program_to_flash(uint8_t *write_buf)
{
uint8_t write_buffer[FLASH_SEGMENT_SIZE];
uint8_t read_buffer[FLASH_SEGMENT_SIZE];
uint16_t status;
int flash_address = 0;
int count = 0;
spi_flash_status_t result;
struct device_Info DevInfo;
spi_flash_init();
spi_flash_control_hw( SPI_FLASH_RESET, 0, &status );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* First fetch status register. First byte in low 8 bits, second byte in
* upper 8 bits.
*/
result = spi_flash_control_hw( SPI_FLASH_GET_STATUS, 0, &status );
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* Fetch protection register value for each of the 128 sectors.
* After power up these should all read as 0xFF
*/
for( count = 0; count != 128; ++count )
{
result = spi_flash_control_hw( SPI_FLASH_GET_PROTECT,
count * FLASH_SECTOR_SIZE,
&read_buffer[count] );
}
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* Show sector protection in action by:
* - unprotecting the first sector
* - erasing the sector
* - writing some data to the first 256 bytes
* - protecting the first sector
* - erasing the first sector
* - reading back the first 256 bytes of the first sector
* - unprotecting the first sector
* - erasing the sector
* - reading back the first 256 bytes of the first sector
*
* The first read should still show the written data in place as the erase
* will fail. the second read should show all 0xFFs. Step through the code
* in debug mode and examine the read buffer after the read operations to
* see this.
*/
result = spi_flash_control_hw( SPI_FLASH_SECTOR_UNPROTECT, flash_address, NULL );
//device D works
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_4KBLOCK_ERASE, flash_address , NULL );
//device D-- now working
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
memset( write_buffer, count, FLASH_SEGMENT_SIZE );
strcpy( (char *)write_buffer, "Microsemi FLASH test" );
spi_flash_write( flash_address, write_buffer, FLASH_SEGMENT_SIZE );
//device D --
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_SECTOR_PROTECT, flash_address, NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_4KBLOCK_ERASE, flash_address , NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_GET_STATUS, 0, &status );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
spi_flash_read ( flash_address, read_buffer, FLASH_SEGMENT_SIZE);
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_SECTOR_UNPROTECT, flash_address, NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_4KBLOCK_ERASE, flash_address , NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_GET_STATUS, 0, &status );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_SECTOR_UNPROTECT, flash_address, NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
result = spi_flash_control_hw( SPI_FLASH_4KBLOCK_ERASE, flash_address , NULL );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
spi_flash_read ( flash_address, read_buffer, FLASH_SEGMENT_SIZE );
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* Read the protection registers again so you can see that the first sector
* is unprotected now.
*/
for( count = 0; count != 128; ++count )
{
spi_flash_control_hw( SPI_FLASH_GET_PROTECT, count * FLASH_SECTOR_SIZE,
&write_buffer[count] );
}
//device D
result = spi_flash_control_hw( SPI_FLASH_READ_DEVICE_ID,
count * FLASH_SECTOR_SIZE,
&DevInfo );
/*--------------------------------------------------------------------------
* Write something to all 32768 blocks of 256 bytes in the 8MB FLASH.
*/
for( count = 0; count != ((32*1024)/256) /*32768*/; ++count )
{
/*----------------------------------------------------------------------
* Vary the fill for each chunk of 256 bytes
*/
memset( write_buffer, count, FLASH_SEGMENT_SIZE );
strcpy( (char *)write_buffer, "Microsemi FLASH test" );
/*----------------------------------------------------------------------
* at the start of each sector we need to make sure it is unprotected
* so we can erase blocks within it. The spi_flash_write() function
* unprotects the sector as well but we need to start erasing before the
* first write takes place.
*/
if(0 == (flash_address % FLASH_SECTOR_SIZE))
{
result = spi_flash_control_hw( SPI_FLASH_SECTOR_UNPROTECT, flash_address, NULL );
}
/*----------------------------------------------------------------------
* At the start of each 4K block we issue an erase so that we are then
* free to write anything we want to the block. If we don't do this the
* write may fail as we can only effectively turn 1s to 0s when we
* write. For example if we have an erased location with 0xFF in it and
* we write 0xAA to it first and then later on write 0x55, the resulting
* value is 0x00...
*/
if(0 == (flash_address % FLASH_BLOCK_SIZE))
{
result = spi_flash_control_hw( SPI_FLASH_4KBLOCK_ERASE, flash_address , NULL );
}
/*----------------------------------------------------------------------
* Write our values to the FLASH, read them back and compare.
* Placing a breakpoint on the while statement below will allow
* you break on any failures.
*/
spi_flash_write( flash_address, write_buf, FLASH_SEGMENT_SIZE );
spi_flash_read ( flash_address, read_buffer, FLASH_SEGMENT_SIZE );
if( memcmp( write_buf, read_buffer, FLASH_SEGMENT_SIZE ) )
{
while(1) // Breakpoint here will trap write faults
{
}
}
write_buf += FLASH_SEGMENT_SIZE;
flash_address += FLASH_SEGMENT_SIZE; /* Step to the next 256 byte chunk */
}
/*--------------------------------------------------------------------------
* One last look at the protection registers which should all be 0 now
*/
for( count = 0; count != 128; ++count )
{
spi_flash_control_hw( SPI_FLASH_GET_PROTECT, count * FLASH_SECTOR_SIZE,
&write_buffer[count] );
}
UART_polled_tx_string( &g_uart, " Flash write success\n\r" );
return(0);
}
int _spi_wrc(unsigned int dram_addr,unsigned int flash_addr, unsigned int len)
{
MS_U8 *dram_addr_for_verify=NULL;
UBOOT_TRACE("IN\n");
UBOOT_DEBUG("dram_addr=0x%x\n",dram_addr);
UBOOT_DEBUG("flash_addr=0x%x\n",flash_addr);
UBOOT_DEBUG("len=0x%x\n",len);
/* init SPI flash first */
if (0 == spiInitFlag)
{
spi_flash_init();
}
/* SPI erase */
UBOOT_INFO("Erasing...\n");
MDrv_SERFLASH_WriteProtect(FALSE);
if(len>=0x10000) // a bank size is 64KBytes
{
UBOOT_INFO("block erase\n");
if (0==MDrv_SERFLASH_AddressErase(flash_addr, len, TRUE))
{
UBOOT_ERROR("FAIL !!!\n");
return 1;
}
}
else
{
if (len % 0x1000 != 0) // Check 4K alignment
{
printf("Wrong Alignment Length, FAIL !!!\n");
return 1;
}
UBOOT_INFO("sector erase\n");
if (0==MDrv_SERFLASH_SectorErase(flash_addr, flash_addr+len-1))
{
UBOOT_ERROR("FAIL !!!\n");
return 1;
}
}
/* SPI write */
UBOOT_INFO("Writing...\n");
MDrv_SERFLASH_Write(flash_addr, len, (MS_U8 *)dram_addr);
MDrv_SERFLASH_WriteProtect(TRUE);
/* SPI verify */
unsigned int u32VerifySize = 0;
unsigned int u32VerifyOffset = 0;
UBOOT_INFO("Verifying...\n");
dram_addr_for_verify = malloc(SERFLASH_BLOCK_SIZE);//((dram_addr+len-1+SERFLASH_BLOCK_SIZE) / SERFLASH_BLOCK_SIZE) * SERFLASH_BLOCK_SIZE;/* fix expression of dram_end_address for readability */
if(dram_addr_for_verify==NULL)
{
UBOOT_ERROR("malloc fail\n");
return -1;
}
while(len > 0)
{
if(len >= SERFLASH_BLOCK_SIZE)
{
u32VerifySize = SERFLASH_BLOCK_SIZE;
}
else
{
u32VerifySize=len;
}
if(0 != MDrv_SERFLASH_Read(flash_addr+u32VerifyOffset, u32VerifySize, dram_addr_for_verify))
{
if(memcmp((void*)(dram_addr+u32VerifyOffset),dram_addr_for_verify,u32VerifySize)!=0)
{
free(dram_addr_for_verify);
UBOOT_ERROR("FAIL !!!\n");
return -1;
}
}
len-=u32VerifySize;
u32VerifyOffset+=SERFLASH_BLOCK_SIZE;
}
free(dram_addr_for_verify);
UBOOT_TRACE("OK\n");
return 0;
}
int do_spi_init ( cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
return spi_flash_init();
}
void rt_init_thread_entry(void* parameter)
{
rt_components_init();
LED_init();
Motor_Init();
rt_kprintf("start device init\n");
//rt_hw_i2c1_init();
i2cInit();
rt_hw_spi2_init();
rt_hw_spi3_init();
rt_event_init(&ahrs_event, "ahrs", RT_IPC_FLAG_FIFO);
dmp_init();
sonar_init();
HMC5983_Init();
adns3080_Init();
//config_bt();
rt_thread_init(&led_thread,
"led",
led_thread_entry,
RT_NULL,
led_stack,
256, 16, 1);
rt_thread_startup(&led_thread);
spi_flash_init();
// bmp085_init("i2c1");
rt_kprintf("device init succeed\n");
if (dfs_mount("flash0", "/", "elm", 0, 0) == 0)
{
rt_kprintf("flash0 mount to /.\n");
}
else
{
rt_kprintf("flash0 mount to / failed.\n");
}
//default settings
PID_Init(&p_rate_pid, 0, 0, 0);
PID_Init(&r_rate_pid, 0, 0, 0);
PID_Init(&y_rate_pid, 0, 0, 0);
PID_Init(&p_angle_pid, 0, 0, 0);
PID_Init(&r_angle_pid, 0, 0, 0);
PID_Init(&y_angle_pid, 0, 0, 0);
PID_Init(&x_v_pid, 0, 0, 0);
PID_Init(&y_v_pid, 0, 0, 0);
PID_Init(&x_d_pid, 0, 0, 0);
PID_Init(&y_d_pid, 0, 0, 0);
PID_Init(&h_pid, 0, 0, 0);
load_settings(&settings, "/setting", &p_angle_pid, &p_rate_pid
, &r_angle_pid, &r_rate_pid
, &y_angle_pid, &y_rate_pid
, &x_d_pid, &x_v_pid
, &y_d_pid, &y_v_pid
, &h_pid);
settings.roll_min = settings.pitch_min = settings.yaw_min = 1000;
settings.th_min = 1000;
settings.roll_max = settings.pitch_max = settings.yaw_max = 2000;
settings.th_max = 2000;
// if(settings.pwm_init_mode)
// {
// Motor_Set(1000,1000,1000,1000);
//
// rt_thread_delay(RT_TICK_PER_SECOND*5);
//
// Motor_Set(0,0,0,0);
//
// settings.pwm_init_mode=0;
// save_settings(&settings,"/setting");
//
// rt_kprintf("pwm init finished!\n");
// }
get_pid();
PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 75.0, 20.0);
PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&h_pid, 1.0 / 60.0, 20.0);
rt_thread_init(&control_thread,
"control",
control_thread_entry,
RT_NULL,
control_stack,
1024, 3, 5);
rt_thread_startup(&control_thread);
rt_thread_init(&correct_thread,
"correct",
correct_thread_entry,
RT_NULL,
correct_stack,
1024, 12, 1);
rt_thread_startup(&correct_thread);
LED1(5);
}
