void drv_init_concerto(void)
{
dm_v2_init_para_t dm_para = {0};
u32 dm_app_addr = 0;
spiflash_cfg( );
//init data manager....
dm_para.flash_base_addr = get_flash_addr();
dm_para.max_blk_num = DM_MAX_BLOCK_NUM;
dm_para.task_prio = MDL_DM_MONITOR_TASK_PRIORITY;
dm_para.task_stack_size = MDL_DM_MONITOR_TASK_STKSIZE;
dm_para.open_monitor = TRUE;
dm_para.para_size = sizeof(dm_v2_init_para_t);
dm_para.use_mutex = TRUE;
dm_para.mutex_prio = 1;
dm_para.test_mode = FALSE;
dm_init_v2(&dm_para);
// g_dm_head_start = find_dm_head_start();
dm_set_header(class_get_handle_by_id(DM_CLASS_ID), DM_BOOTER_START_ADDR);
dm_set_header(class_get_handle_by_id(DM_CLASS_ID), DM_HDR_START_ADDR_BAK);
dm_app_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), DM_MAINCODE_BLOCK_ID)
- get_flash_addr();
dm_set_header(class_get_handle_by_id(DM_CLASS_ID),dm_app_addr);
}
void ota_dm_api_init(void)
{
ota_dm_config_t p_cfg = {0};
dm_dmh_info_t *p_all_dmh_info = NULL;
#if 0
p_cfg.ota_dm_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID),
STATIC_SAVE_DATA_BLOCK_ID) - get_flash_addr();
p_cfg.ota_dm_backup_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID),
STATIC_SAVE_DATA_RESERVE_BLOCK_ID) - get_flash_addr();
OS_PRINTF("#### debug dm addr:0x%x,dm backup addr:0x%x\n",p_cfg.ota_dm_addr,p_cfg.ota_dm_backup_addr);
#endif
p_cfg.is_use_mutex = TRUE;
p_cfg.mutex_prio = 1;
p_cfg.is_protect = TRUE;
p_cfg.disable_backup_block = FALSE;
p_cfg.debug_level = OTA_DM_DEBUG_ALL;
p_cfg.ota_dm_api_printf = NULL;
p_cfg.align_size = PICE_MAX_ALIGN_SIZE;
p_cfg.flash_size = CHARSTO_SIZE;
p_cfg.pp_dm_info = &p_all_dmh_info;
p_cfg.ota_dm_block_id = STATIC_SAVE_DATA_BLOCK_ID;
p_cfg.ota_dm_backup_block_id =STATIC_SAVE_DATA_RESERVE_BLOCK_ID;
OS_PRINTF("#####debug ui ota dm api init\n");
mul_ota_dm_api_init(&p_cfg);
/****test use ,please don't use in project,but factory upg*****/
//mul_ota_dm_api_reset_data_block();
}
static BOOL mem_addr_legal(u32 addr, u32 size)
{
u32 flash_b = 0;
u32 flash_e = 0;
u32 cache_mem_b = 0;
u32 cache_mem_e = 0;
u32 nc_mem_b = 0; // none cache memory begin
u32 nc_mem_e = 0; // none cache memory end
u32 reg_b = 0;
u32 reg_e = 0;
flash_b = get_flash_addr();
flash_e = flash_b + CHARSTO_SIZE;
cache_mem_b = 0x80000000;
cache_mem_e = WHOLE_MEM_END & 0x8FFFFFFF;
nc_mem_b = 0xa0000000;
nc_mem_e = WHOLE_MEM_END;
// FIXME:
reg_b = 0xbf000000;
reg_e = 0xbff00000;
if (is_in_range((u32)addr, size, flash_b, flash_e) ||
is_in_range((u32)addr, size, cache_mem_b, cache_mem_e) ||
is_in_range((u32)addr, size, nc_mem_b, nc_mem_e) ||
is_in_range((u32)addr, size, reg_b, reg_e))
{
return TRUE;
}
return FALSE;
}
u32 get_dm_block_real_file_addr(u8 block_id)
{
dmh_block_info_t real_dm_info = {0};
u32 block_addr = 0;
u32 dm_addr = 0;
BOOL ret = 0;
dmh_block_info_t block_dm_info = {0};
if(dm_get_block_header(class_get_handle_by_id(DM_CLASS_ID),block_id,&block_dm_info) == DM_FAIL)
{
return 0;
}
block_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), block_id);
block_addr = block_addr - get_flash_addr();
dm_addr = block_addr - block_dm_info.base_addr;
if(block_dm_info.type != BLOCK_TYPE_RO)
{
return block_addr;
}
ret = get_dm_block_real_file_info(block_id,&real_dm_info);
if((ret == 0) && (block_addr == (real_dm_info.base_addr + dm_addr)))
{
return block_addr;
}
return 0;
}
static RET_CODE nvram_read(u32 offset, u8 *p_buf, u32 *size)
{
charsto_device_t *p_charsto_dev = dev_find_identifier(NULL, DEV_IDT_TYPE,
SYS_DEV_TYPE_CHARSTO);
RET_CODE ret = SUCCESS;
handle_t dm_handle = class_get_handle_by_id(DM_CLASS_ID);
u32 base_addr = dm_get_block_addr(dm_handle, CADATA_BLOCK_ID) - get_flash_addr();
MT_ASSERT(dm_handle != NULL);
OS_PRINTF("CA nvram_read offset= 0x%x, *size=0x%x, base_addr=0x%x \n",offset, *size, base_addr);
#if WRITE_DATA_TO_MEM
memcpy(p_buf, (u8 *)&p_ca_buffer[offset], *size);
OS_PRINTF("CA nvram_read end\n");
#else
mtos_task_lock();
ret = charsto_read(p_charsto_dev, base_addr + offset, p_buf, *size);
if (ret != SUCCESS)
{
OS_PRINTF("read error\n");
mtos_task_unlock();
return ERR_FAILURE;
}
mtos_task_unlock();
OS_PRINTF("nvram_read success!\n");
#endif
return SUCCESS;
}
static s32 load_logo(void)
{
OS_PRINTK("\n###fast logo logo buf :0x%x flash addr 0x%x\n",
dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), START_LOGO_BLOCK_ID)),get_flash_addr();
g_p_logo_buf = (u8 *)(dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), START_LOGO_BLOCK_ID));
g_logo_size = get_dm_block_real_file_size(START_LOGO_BLOCK_ID);//dm_get_block_size(class_get_handle_by_id(DM_CLASS_ID),START_LOGO_BLOCK_ID);
OS_PRINTK("\n###fast logo logo buf :0x%x,size:0x%x\n",*((u32 *)g_p_logo_buf),g_logo_size);
return SUCCESS;
}
void on_cas_init(void)
{
u32 cas_id = 0;
cas_adapter_cfg_t cas_cfg = {0};
cas_module_cfg_t cas_module_cfg = {0};
p_ca_buffer = mtos_malloc(CA_DATA_SIZE);
MT_ASSERT(NULL != p_ca_buffer);
memset(p_ca_buffer, 0, CA_DATA_SIZE);
//config cas adapter
cas_cfg.p_smc_drv[0] = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_SMC);
cas_cfg.slot_num = 1;
cas_cfg.p_task_stack = mtos_malloc(4096);
MT_ASSERT(NULL != cas_cfg.p_task_stack);
cas_cfg.stack_size = 4096;
cas_cfg.task_prio = DRV_CAS_ADAPTER_TASK_PRIORITY;
cas_cfg.p_data_task_stack = mtos_malloc(4096);
MT_ASSERT(NULL != cas_cfg.p_data_task_stack);
cas_cfg.data_stack_size = 4096;
//cas_cfg.data_task_prio = MDL_CAS_TASK_PRIO_BEGIN+1;
#ifndef WIN32
cas_init(&cas_cfg);
#endif
cas_module_cfg.cas_lib_type = CAS_LIB_TEST;
cas_module_cfg.p_dmx_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_PTI);
cas_module_cfg.max_mail_num = 30;
cas_module_cfg.mail_policy = POLICY_BY_ID;
cas_module_cfg.flash_start_adr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), CADATA_BLOCK_ID)
- get_flash_addr();
OS_PRINTF("set ca addr is 0x%x\n", cas_module_cfg.flash_start_adr);
cas_module_cfg.flash_size = CA_DATA_SIZE;
cas_module_cfg.level = 0;
cas_module_cfg.task_prio = MDL_CAS_TASK_PRIO_BEGIN;
cas_module_cfg.end_task_prio = MDL_CAS_TASK_PRIO_END;
cas_module_cfg.stack_size = 8 * KBYTES;
cas_module_cfg.p_task_stack = mtos_malloc(8 * KBYTES);
cas_module_cfg.filter_mode = ECM_FILTER_CONTINUOUS_DISABLED;
cas_module_cfg.nvram_read = nvram_read;
cas_module_cfg.nvram_write = nvram_write;
cas_module_cfg.queue_task_prio = LOWEST_TASK_PRIORITY -2; //LOWEST_TASK_PRIORITY -1; //panhui
MT_ASSERT(cas_module_cfg.p_task_stack != NULL);
#ifndef WIN32
cas_tr_attach(&cas_module_cfg, &cas_id);
cas_module_init(CAS_ID_TR);
#endif
register_monitor_table();
}
static int drv_dm_init(void)
{
dm_v2_init_para_t dm_para = {0};
u32 dm_app_addr = 0;
RET_CODE ret;
ret = ATTACH_DRIVER(CHARSTO, concerto, default, default);
MT_ASSERT(ret == SUCCESS);
spiflash_cfg( );
//init data manager....
dm_para.flash_base_addr = get_flash_addr();
dm_para.max_blk_num = DM_MAX_BLOCK_NUM;
dm_para.task_prio = MDL_DM_MONITOR_TASK_PRIORITY;
OS_PRINTF("%s %d dm_para.task_prio = %d\n",__FUNCTION__,__LINE__, dm_para.task_prio);
dm_para.task_stack_size = MDL_DM_MONITOR_TASK_STKSIZE;
dm_para.open_monitor = TRUE;
dm_para.para_size = sizeof(dm_v2_init_para_t);
dm_para.use_mutex = TRUE;
dm_para.mutex_prio = 1;
dm_para.test_mode = FALSE;
OS_PRINTK("dm_para.flash_base_addr :0x%x \n",dm_para.flash_base_addr );
dm_init_v2(&dm_para);
OS_PRINTF("set header [0x%08x]\n",DM_BOOTER_START_ADDR);
dm_set_header(class_get_handle_by_id(DM_CLASS_ID), DM_BOOTER_START_ADDR);
OS_PRINTF("set header [0x%08x]\n",DM_HDR_START_ADDR);
dm_set_header(class_get_handle_by_id(DM_CLASS_ID), DM_HDR_START_ADDR);
dm_app_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), DM_APP_BLOCK_ID)
- get_flash_addr();
OS_PRINTF("set header [0x%08x]\n",dm_app_addr);
dm_set_header(class_get_handle_by_id(DM_CLASS_ID),dm_app_addr);
//load customer hw config
//dm_load_customer_hw_cfg();
return SUCCESS;
}
void on_cas_init(void)
{
u32 cas_id = 0;
cas_adapter_cfg_t cas_cfg = {0};
cas_module_cfg_t cas_module_cfg = {0};
p_ca_buffer = mtos_malloc(CA_DATA_SIZE);
MT_ASSERT(NULL != p_ca_buffer);
memset(p_ca_buffer, 0, CA_DATA_SIZE);
//config cas adapter
cas_cfg.p_smc_drv[0] = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_SMC);
cas_cfg.slot_num = 1;
#if 1
cas_cfg.p_task_stack = mtos_malloc(4096);
cas_cfg.stack_size = 4096;
cas_cfg.task_prio = DRV_CAS_ADAPTER_TASK_PRIORITY;
#ifndef WIN32
cas_init(&cas_cfg);
cas_module_cfg.p_dmx_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_PTI);
cas_module_cfg.nvram_read = nvram_read;
cas_module_cfg.nvram_write = nvram_write;
cas_module_cfg.flash_start_adr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), CADATA_BLOCK_ID)
- get_flash_addr();
OS_PRINTF("set ca addr is 0x%x\n", cas_module_cfg.flash_start_adr);
cas_module_cfg.flash_size = CA_DATA_SIZE;
cas_module_cfg.task_prio = DRV_CAS_MODUL_TASK_PRIORITY_START;
cas_module_cfg.end_task_prio = DRV_CAS_MODUL_TASK_PRIORITY_END;
cas_module_cfg.queue_task_prio = CUSTOMER_TASK_PRIORITY;
OS_PRINTF("####debug cas_tr_attach 0\n");
cas_tr_attach(&cas_module_cfg, &cas_id);
OS_PRINTF("####debug cas_tr_attach\n");
cas_module_init(AP_CAS_ID);
OS_PRINTF("####debug cas_module_init\n");
#endif
#endif
register_monitor_table();
}
static RET_CODE nvram_read(u32 offset, u8 *p_buf, u32 *size)
{
charsto_device_t *p_charsto_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_CHARSTO);
RET_CODE ret = SUCCESS;
handle_t dm_handle = class_get_handle_by_id(DM_CLASS_ID);
u32 base_addr = dm_get_block_addr(dm_handle, CADATA_BLOCK_ID) - get_flash_addr();
mtos_task_lock();
MT_ASSERT(dm_handle != NULL);
ret = charsto_read(p_charsto_dev, base_addr + offset, p_buf, *size);
if (ret != SUCCESS)
{
mtos_task_unlock();
OS_PRINTF("read error\n");
return ERR_FAILURE;
}
OS_PRINTF("nvram_read success!\n");
mtos_task_unlock();
return SUCCESS;
}
BOOL get_dm_block_real_file_info(u8 block_id,dmh_block_info_t *block_info)
{
void *p_dev = NULL;
u32 block_addr = 0;
u32 block_size = 0;
u32 wr_addr = 0;
dmh_block_info_t real_dm_info = {0};
RET_CODE ret = ERR_NOFEATURE;
dmh_block_info_t block_dm_info = {0};
if(dm_get_block_header(class_get_handle_by_id(DM_CLASS_ID),block_id,&block_dm_info) == DM_FAIL)
{
return -1;
}
if(block_dm_info.type != BLOCK_TYPE_RO)
{
memcpy(block_info,&block_dm_info,sizeof(dmh_block_info_t));
return 0;
}
p_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_CHARSTO);
MT_ASSERT(NULL != p_dev);
block_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), block_id);
if(block_addr == 0)
{
return -1;
}
block_addr = block_addr - get_flash_addr();
block_size = dm_get_block_size(class_get_handle_by_id(DM_CLASS_ID),block_id);
wr_addr = block_addr + block_size - sizeof(dmh_block_info_t);
ret = charsto_read(p_dev,wr_addr,(u8 *)&real_dm_info,sizeof(dmh_block_info_t));
if((ret == SUCCESS) && (block_id == real_dm_info.id))
{
memcpy(block_info,&real_dm_info,sizeof(dmh_block_info_t));
return 0;
}
return -1;
}
void on_cas_init(void)
{
u32 cas_id = 0;
cas_adapter_cfg_t cas_cfg = {0};
cas_module_cfg_t cas_module_cfg = {0};
p_ca_buffer = mtos_malloc(CA_DATA_SIZE);
MT_ASSERT(NULL != p_ca_buffer);
memset(p_ca_buffer, 0, CA_DATA_SIZE);
//config cas adapter
cas_cfg.p_smc_drv[0] = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_SMC);
cas_cfg.slot_num = 1;
cas_cfg.p_task_stack = mtos_malloc(4096);
cas_cfg.stack_size = 4096;
cas_cfg.task_prio = DRV_CAS_ADAPTER_TASK_PRIORITY;
#ifndef WIN32
cas_init(&cas_cfg);
#endif
cas_module_cfg.p_dmx_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_PTI);
cas_module_cfg.nvram_read = NULL;
cas_module_cfg.nvram_write = NULL;
cas_module_cfg.flash_start_adr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), CADATA_BLOCK_ID)
- get_flash_addr();
OS_PRINTF("set ca addr is 0x%x\n", cas_module_cfg.flash_start_adr);
cas_module_cfg.flash_size = CA_DATA_SIZE;
cas_module_cfg.task_prio = MDL_CAS_TASK_PRIO_BEGIN; //need fixed
cas_module_cfg.end_task_prio = MDL_CAS_TASK_PRIO_END; //need fixed
cas_module_cfg.machine_serial_get = NULL;
cas_module_cfg.query_check = force_channel_pg_check;
#ifndef WIN32
cas_ds_attach_v5(&cas_module_cfg, &cas_id);
cas_module_init(CAS_ID_DS);
#endif
register_monitor_table();
}
RET_CODE check_is_msg_serial_num(u8 *p_uni_str)
{
charsto_device_t *p_charsto_dev = dev_find_identifier(NULL, DEV_IDT_TYPE, SYS_DEV_TYPE_CHARSTO);
//dm_v2_init_para_t dm_para = { 0 };
u32 indentity_block_addr;
u32 rw_addr;
u8 *p_buffer = mtos_malloc(CHARSTO_SECTOR_SIZE);
charsto_prot_status_t st_set = {0};
charsto_prot_status_t st_old = {0};
memset(p_buffer, 0, CHARSTO_SECTOR_SIZE);
//unprotect
dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_GET_STATUS, (u32)&st_old);
st_set.prt_t = PRT_UNPROT_ALL;
dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_SET_STATUS, (u32)&st_set);
//if(charsto_unprotect_all(p_charsto_dev) == SUCCESS)
{
indentity_block_addr = dm_get_block_addr(class_get_handle_by_id(DM_CLASS_ID), IDENTITY_BLOCK_ID) - get_flash_addr();
rw_addr = indentity_block_addr / CHARSTO_SECTOR_SIZE * CHARSTO_SECTOR_SIZE;
if(charsto_read(p_charsto_dev, rw_addr, p_buffer, CHARSTO_SECTOR_SIZE) == SUCCESS)
{
memcpy(p_buffer+ (indentity_block_addr - rw_addr), p_uni_str, 64);
if(charsto_erase(p_charsto_dev, rw_addr, 1) == SUCCESS)
{
charsto_writeonly(p_charsto_dev, rw_addr, p_buffer, CHARSTO_SECTOR_SIZE);
}
else
{
OS_PRINTF("charsto erase fail\n");
return ERR_FAILURE;
}
}
else
{
OS_PRINTF("charsto_read fail\n");
return ERR_FAILURE;
}
}
//restore
dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_SET_STATUS, (u32)&st_old);
mtos_free(p_buffer);
return SUCCESS;
}
static RET_CODE nvram_write(u32 offset, u8 *p_buf, u32 size)
{
charsto_device_t *p_charsto_dev = dev_find_identifier(NULL, DEV_IDT_TYPE,
SYS_DEV_TYPE_CHARSTO);
RET_CODE ret = SUCCESS;
handle_t dm_handle = class_get_handle_by_id(DM_CLASS_ID);
u32 base_addr = dm_get_block_addr(dm_handle, CADATA_BLOCK_ID) - get_flash_addr();
u32 start_addr = 0;
u32 padding = 0;
u32 wrlen = 0;
mtos_task_lock();
MT_ASSERT(dm_handle != NULL);
OS_PRINTF("ca write addr[0x%x], size[0x%x]\n", offset, size);
start_addr = ((base_addr + offset) / CHARSTO_SECTOR_SIZE) * CHARSTO_SECTOR_SIZE;
padding = (base_addr + offset) % CHARSTO_SECTOR_SIZE;
while(size)
{
if (size > (CHARSTO_SECTOR_SIZE - padding))
{
wrlen = CHARSTO_SECTOR_SIZE - padding;
}
else
{
wrlen = size;
}
//read sector data
ret = charsto_read(p_charsto_dev, start_addr, p_ca_buffer, CHARSTO_SECTOR_SIZE);
if (ret != SUCCESS)
{
mtos_task_unlock();
OS_PRINTF("write/read error1\n");
return ERR_FAILURE;
}
memcpy(p_ca_buffer + padding, p_buf, wrlen);
//charsto_unprotect_all(p_charsto_dev);
ret = charsto_erase(p_charsto_dev, start_addr, 1);
//charsto_protect_all(p_charsto_dev);
if (ret != SUCCESS)
{
mtos_task_unlock();
OS_PRINTF("write/erase error2\n");
return ERR_FAILURE;
}
//charsto_unprotect_all(p_charsto_dev);
charsto_writeonly(p_charsto_dev, start_addr, p_ca_buffer, CHARSTO_SECTOR_SIZE);
//charsto_protect_all(p_charsto_dev);
if (ret != SUCCESS)
{
mtos_task_unlock();
OS_PRINTF("write error3\n");
return ERR_FAILURE;
}
start_addr += CHARSTO_SECTOR_SIZE;
padding = 0;
p_buf += wrlen;
size -= wrlen;
}
OS_PRINTF("nvram_write success!\n");
mtos_task_unlock();
return SUCCESS;
}
static RET_CODE nvram_ad_logo_write(ST_DSAD_AV_SHOW_INFO * av_show_info)
{
RET_CODE ret = SUCCESS;
handle_t dm_handle = class_get_handle_by_id(DM_CLASS_ID);
charsto_device_t *p_charsto_dev = dev_find_identifier(NULL, DEV_IDT_TYPE,SYS_DEV_TYPE_CHARSTO);
u32 base_addr = dm_get_block_addr(dm_handle, START_LOGO_BLOCK_ID) - get_flash_addr();
u32 block_size = 0;
u32 wr_addr = 0;
u8 *p_data = NULL;
u32 size = 0;
//lint -save -e64
charsto_prot_status_t st_old={0};
charsto_prot_status_t st_set={0};
dmh_block_info_t real_dm_info={0};
//lint -restore
u8 logo_head[4] = {0}; // logo data size; without head size.
u32 head_size = sizeof(logo_head);
block_size = dm_get_block_size(dm_handle, START_LOGO_BLOCK_ID);
size = av_show_info->uiDataLen;
p_data = av_show_info->pucAvData;
if(base_addr == 0 || block_size == 0 || (size + head_size) > block_size)
{
OS_PRINTF("Error!,blocksize:%d,size:%d\n",block_size, size);
return ERR_FAILURE;
}
wr_addr = base_addr + block_size - sizeof(dmh_block_info_t);
ret = charsto_read(p_charsto_dev,wr_addr,(u8 *)&real_dm_info,sizeof(dmh_block_info_t));
if(ret == SUCCESS)
{
real_dm_info.size = size;
}
else
{
OS_PRINTF("Error!\n");
return ERR_FAILURE;
}
dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_GET_STATUS, (u32)&st_old);
//unprotect
st_set.prt_t = PRT_UNPROT_ALL;
dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_SET_STATUS, (u32)&st_set);
ret = charsto_erase(p_charsto_dev, base_addr, block_size/CHARSTO_SECTOR_SIZE);
MT_ASSERT(ret == SUCCESS);
logo_head[0] = SYS_GET_BYTE3(size);
logo_head[1] = SYS_GET_BYTE2(size);
logo_head[2] = SYS_GET_BYTE1(size);
logo_head[3] = SYS_GET_BYTE0(size);
ret = charsto_writeonly(p_charsto_dev, base_addr, logo_head, head_size);
ret = charsto_writeonly(p_charsto_dev, base_addr+head_size, p_data, size);
MT_ASSERT(ret == SUCCESS);
//write dmh_block_info_t infomation for OTA upgrade, the end of block.
ret = charsto_writeonly(p_charsto_dev, wr_addr, (u8 *)&real_dm_info, sizeof(dmh_block_info_t));
OS_PRINTF("!!!!!!!!!!wr_addr=0x%x,sizeof(dmh_block_info_t)=0x%x\n",wr_addr,sizeof(dmh_block_info_t));
MT_ASSERT(ret == SUCCESS);
//restore
ret = dev_io_ctrl(p_charsto_dev, CHARSTO_IOCTRL_SET_STATUS, (u32)&st_old);
MT_ASSERT(ret == SUCCESS);
OS_PRINTF("AD nvram_write success !! \n");
return SUCCESS;
//lint -save -e550
}
/***********************************************************************
This is the main controller loop.
sequences of operations:
- reads from CAN bus or serial port 1.
- reads encoders (or ADC).
- computes the control value (a PID in this version).
- checks limits and other errors.
- sets PWM
- does extra functions (e.g. communicate with neighboring cards).
***********************************************************************/
void main(void)
{
Int32 PWMoutput [JN];
Int32 PWMoutput_old [JN];
byte i=0;
byte wi=0;
byte k=0;
UInt16 *value=0;
Int32 t1val=0;
Int32 PID_R= 2;
Int32 kpp=1;
Int16 current_turn=0;
Int16 print_number=0;
Int16 real_pos=0;
byte first_step=0;
#if (VERSION == 0x0351)
#define winSizeMax 32
#define initialWindowSize 4
#else
#define winSizeMax 32
#define initialWindowSize 30
#endif
byte divJntPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divJntVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte headJntPos[JN]=INIT_ARRAY(0); //current joint pos
byte tailJntPos[JN]=INIT_ARRAY(0);
byte headJntVel[JN]=INIT_ARRAY(0); //current joint vel
byte tailJntVel[JN]=INIT_ARRAY(0);
byte headMotPos[JN]=INIT_ARRAY(0); //current motor pos
byte tailMotPos[JN]=INIT_ARRAY(0);
byte headMotVel[JN]=INIT_ARRAY(0); //current motor vel
byte tailMotVel[JN]=INIT_ARRAY(0);
Int32 jntPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 jntVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int16 _safeband[JN]; //it is a value for reducing the JOINT limit of 2*_safeband [tick encoder]
#ifdef TEMPERATURE_SENSOR
byte TempSensCount1 = 0;
UInt32 TempSensCount2 = 0;
byte temp_sens_status=0;
overtemp[0]=0;
overtemp[1]=0;
errortemp[0]=0;
errortemp[1]=0;
#endif
/* gets the address of flash memory from the linker */
_flash_addr = get_flash_addr();
/* enable interrupts */
setReg(SYS_CNTL, 0);
// IPL channels from 0 to 6 enabled
// external interrupts IRQA and IRQB disabled
setRegBits(IPR, 0xFE00);
// enable FAULT
__ENIGROUP (61, 3);
#if (VERSION == 0x0254)
#else
__ENIGROUP (60, 3);
#endif
// enable SCI
__ENIGROUP (52, 4);
__ENIGROUP (53, 4);
__ENIGROUP (50, 4);
__ENIGROUP (51, 4);
// enable data flash
__ENIGROUP (13, 4);
// enable CAN
__ENIGROUP (14, 6);
__ENIGROUP (15, 6);
__ENIGROUP (16, 6);
__ENIGROUP (17, 6);
// enable ADCA/ADCB
__ENIGROUP (55, 6);
__ENIGROUP (54, 6);
//enable PWM reload
__ENIGROUP (59, 7); // PMWA
#if (VERSION == 0x0254)
#else
__ENIGROUP (58, 7); // PWMB
#endif
// enable timers
// TIMER_A
__ENIGROUP (45, 7); //Timer for the encoder commutation if used
__ENIGROUP (44, 7); //
__ENIGROUP (43, 7); //
__ENIGROUP (42, 4); //TI1 1ms delay main loop
// TIMER_B
__ENIGROUP (41, 7); //
__ENIGROUP (40, 7); //
__ENIGROUP (39, 7); //
__ENIGROUP (38, 7);
// TIMER_C
__ENIGROUP (37, 1);
__ENIGROUP (36, 1);
__ENIGROUP (35, 1);
__ENIGROUP (34, 1);
// TIMER_D
__ENIGROUP (33, 7); //1ms delay duty cycle
__ENIGROUP (32, 1);
__ENIGROUP (31, 1);
__ENIGROUP (30, 1);
__EI();
flash_interface_init (JN);
readFromFlash (_flash_addr);
if (_version==_flash_version)
{
}
else
{
writeToFlash(_flash_addr);
}
__DI();
#warning "debug"// ;
__EI();
init_leds ();
#if (VERSION == 0x0254)
Init_Brushless_Comm (1,HALL);
#else
Init_Brushless_Comm (JN,HALL);
#endif
can_interface_init (JN);
init_strain ();
init_position_abs_ssi ();
#if VERSION ==0x0257
init_relative_position_abs_ssi();
#endif
init_faults (true,true,true);
init_position_encoder ();
TI1_init ();
//variable init
mainLoopOVF=0;
_count=0;
for(i=0;i<JN;i++)
{
_received_pid[i].rec_pid=0;
}
BUS_OFF=false;
#warning "debug"// ;
//__EI();
// print_version ();
/* initialization */
for (i=0; i<JN; i++) _calibrated[i] = false;
/* reset trajectory generation */
for (i=0; i<JN; i++) abort_trajectory (i, 0);
///////////////////////////////////////
// reset of the ABS_SSI
// this is needed because the AS5045 gives the first value wrong !!!
for (i=0; i<JN; i++) _position[i]=(Int32) Filter_Bit(get_position_abs_ssi(i));
for (i=0; i<JN; i++) _max_real_position[i]=Filter_Bit(4095);
//////////////////////////////////////
/* initialize speed and acceleration to zero (useful later on) */
for (i=0; i<JN; i++) _position_old[i] = 0;
for (i=0; i<JN; i++) _speed[i] = 0;
for (i=0; i<JN; i++) _accel[i] = 0;
for (i=0; i<JN; i++) _safeband[i] =5; //5 ticks => 1 grado di AEA.
for (i=0; i<JN; i++) PWMoutput [i] = PWMoutput_old[i] = 0;
/* reset the recursive windows for storage of position and velocity data */
/* (for velocity and position estimates) */
for(i=0;i<JN;i++)
{
for(wi=0;wi<winSizeMax;wi++)
{
jntPosWindow[wi][i]=_position[i];
jntVelWindow[wi][i]=0;
motPosWindow[wi][i]=0;
motVelWindow[wi][i]=0;
}
}
//set_relative_position_abs_ssi(1,get_absolute_real_position_abs_ssi(1));
/* main control loop */
for(_counter = 0;; _counter ++)
{
if (_counter >= CAN_SYNCHRO_STEPS) _counter = 0;
led3_on
while (_wait);
_count=0;
led3_off
// BUS_OFF check
if (getCanBusOffstatus() )
{
#ifdef DEBUG_CAN_MSG
can_printf("DISABLE BUS OFF");
#endif
for (i=0; i<JN; i++) put_motor_in_fault(i);
led1_off
}
else
led1_on
// READING CAN MESSAGES
can_interface();
for (i=0; i<JN; i++)
if (_pad_enabled[i]==false && _control_mode[i]!=MODE_HW_FAULT) _control_mode[i]=MODE_IDLE;
//Position calculation
// This is used to have a shift of the zero-cross out of the
// joint workspace
//
// max_real_position is the limit of the joint starting from
// 4095 and going to decrease this number without zero-cross
// untill the joint limit is reached
#if VERSION == 0x0257
_position_old[0]=_position[0];
if(get_error_abs_ssi(0)==ERR_OK)
_position[0]=Filter_Bit (get_position_abs_ssi(0));
_position_old[1]=_position[1];
if(get_error_abs_ssi(1)==ERR_OK)
_position[1]=Filter_Bit (get_position_abs_ssi(1));
#else
for (i=0; i<JN; i++)
{
_position_old[i]=_position[i];
if(get_error_abs_ssi(i)==ERR_OK)
_position[i]=Filter_Bit (get_position_abs_ssi(i));
}
#endif
// get_commutations() is used to read the incremental encoder of the motors.
// the variable _motor_position is then used to estimate the rotor speed and
// compensate the back-EMF of the motor.
for (i=0; i<JN; i++) _motor_position[i]=get_position_encoder(i);//get_commutations(i);
///////////////////////////////////////////DEBUG////////////
#if (VERSION !=0x0254)
for (i=0; i<JN; i++)
{
if (get_error_abs_ssi(i)==ERR_ABS_SSI)
{
put_motor_in_fault(i);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",i);
#endif
}
}
#endif
#if (VERSION ==0x0254)
if (get_error_abs_ssi(0)==ERR_ABS_SSI)
{
put_motor_in_fault(0);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",0);
#endif
}
#endif
//DO NOTHING
// decoupling the position
decouple_positions();
/* velocity and acceleration estimators */
{
for (i=0; i<JN; i++)
{
//joint velocity estimator
tailJntPos[i]=headJntPos[i]+(winSizeMax-divJntPos[i]); if(tailJntPos[i]>=winSizeMax) tailJntPos[i]=tailJntPos[i]%winSizeMax;
_speed_old[i] = _speed[i];
jntPosWindow[headJntPos[i]][i]=_position[i];
_speed[i] = (Int32) (((jntPosWindow[headJntPos[i]][i] - jntPosWindow[tailJntPos[i]][i] ))<<_jntVel_est_shift[i]);
// _speed[i] <<= _jntVel_est_shift[i];
_speed[i] = (Int32)(_speed[i]) / divJntPos[i];
headJntPos[i]=headJntPos[i]+1; if(headJntPos[i]>=winSizeMax) headJntPos[i]=0;
/*
//joint acceleration estimator
tailJntVel[i]=headJntVel[i]+(winSizeMax-divJntVel[i]); if(tailJntVel[i]>=winSizeMax) tailJntVel[i]=tailJntVel[i]%winSizeMax;
_accel_old[i] = _accel[i];
jntVelWindow[headJntVel[i]][i]=_speed[i];
_accel[i] = ((jntVelWindow[headJntVel[i]][i] - jntVelWindow[tailJntVel[i]][i] ));
_accel[i] << _jntAcc_est_shift[i];
_accel[i] = (Int32)(_accel[i]) / divJntVel[i];
headJntVel[i]=headJntVel[i]+1; if(headJntVel[i]>=winSizeMax) headJntVel[i]=0;
*/
//motor velocity estimator
tailMotPos[i]=headMotPos[i]+(winSizeMax-divMotPos[i]); if(tailMotPos[i]>=winSizeMax) tailMotPos[i]=tailMotPos[i]%winSizeMax;
_motor_speed_old[i] = _motor_speed[i];
motPosWindow[headMotPos[i]][i]=_motor_position[i];
_motor_speed[i] = ((motPosWindow[headMotPos[i]][i] - motPosWindow[tailMotPos[i]][i] ));
_motor_speed[i] <<= _motVel_est_shift[i];
_motor_speed[i] = (_motor_speed[i]) / divMotPos[i];
headMotPos[i]=headMotPos[i]+1; if(headMotPos[i]>=winSizeMax) headMotPos[i]=0;
}
}
/* in position? */
#if (VERSION != 0x0254)
for (i=0; i<JN; i++) _in_position[i] = check_in_position(i);
#else
_in_position[0] = check_in_position(0);
#endif
/* in reference configuration for calibration? */
//for (i=0; i<JN; i++) check_in_position_calib(i);
//******************************************* POSITION LIMIT CHECK ***************************/
for (i=0; i<JN; i++) check_range(i, _safeband[i], PWMoutput);
//******************************************* COMPUTES CONTROLS *****************************/
//FT sensor watchdog update
for (i=0; i<STRAIN_MAX; i++)
if (_strain_wtd[i]>0) _strain_wtd[i]--;
for (i=0; i<JN; i++)
{
//computing the PWM value (PID)
PWMoutput[i] = compute_pwm(i);
// PWM filtering in torque control if there is no bemf compensation
#if (VERSION != 0x0351)
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
if (_useFilter[i] == 3) PWMoutput[i] = lpf_ord1_3hz (PWMoutput[i], i);
}
// saving the PWM value before the decoupling
_bfc_PWMoutput[i] = PWMoutput_old[i] = PWMoutput[i];
// applying the saturation to the PWM
if (_bfc_PWMoutput[i] < -MAX_DUTY) _bfc_PWMoutput[i]=-MAX_DUTY;
else if (_bfc_PWMoutput[i] > MAX_DUTY) _bfc_PWMoutput[i]= MAX_DUTY;
#endif //(VERSION != 0x0351)
}
//decouple PWM
decouple_dutycycle(PWMoutput);
//******************************************* SATURATES CONTROLS ***************************/
/* back emf compensation + controls saturation (if necessary) */
for (i=0; i<JN; i++)
{
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
#if (VERSION != 0x0351)
// Back emf compensation
//PWMoutput[i]+=compensate_bemf(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_bemf(i, _speed[i]); //use the joint speed
//add the coulomb friction compensation term
if (_kstp_torque[i] != 0 ||
_kstn_torque[i] != 0)
//PWMoutput[i]+=compensate_friction(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_friction(i, _speed[i]); //use the joint speed
// Protection for joints out of the admissible range during force control
check_range_torque(i, _safeband[i], PWMoutput);
// PWM saturation
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit_torque[i] );
#else //(VERSION != 0x0351)
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
#endif //(VERSION != 0x0351)
}
else
{
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
}
if (_pid[i] < -MAX_DUTY) _pid[i]=-MAX_DUTY;
else if (_pid[i] > MAX_DUTY) _pid[i]= MAX_DUTY;
}
/* generate PWM */
for (i=0; i<JN; i++)
{
if (!mode_is_idle(i)) {PWM_generate(i,_pid[i]);}
}
/* Check Current done in T1 */
/* do extra functions, communicate, etc. */
//send broadcast data
can_send_broadcast();
//send additional debug information
//can_send_broadcast_debug(1,1);
/***********************************************************************
// Check Current is made here
/***********************************************************************/
#if (VERSION != 0x0254)
for (i=0; i<JN; i++)
#else
for (i=0; i<1; i++)
#endif
{
if ((get_current(i)>=25000) || (get_current(i)<=-25000))
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d curr %f",i,get_current(i));
#endif
}
check_current(i, (_pid[i] > 0));
compute_i2t(i);
if (_filt_current[i] > MAX_I2T_CURRENT)
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d filtcurr %f",i,_filt_current[i]);
#endif
}
}
// Check for the MAIN LOOP duration
// t1val= (UInt16) TI1_getCounter();
if ( _count>0)
{
mainLoopOVF=1;
_count=0;
}
/* tells that the control cycle is completed */
_wait = true;
} /* end for(;;) */
