marvell/uboot/board/Marvell/common/asr_common.c

#include <common.h>
#include <malloc.h>
#include "tim.h"
#include "asr_common.h"
#include "obm2osl.h"
#include <asm/io.h>
#include <asm/arch/cpu.h>
#ifdef CONFIG_LZMA
#include <lzma/LzmaTypes.h>
#include <lzma/LzmaDec.h>
#include <lzma/LzmaTools.h>
#endif /* CONFIG_LZMA */

DECLARE_GLOBAL_DATA_PTR;

/* set none-zero data to put it into data sesction */
static struct asr_mflag *asr_mflag_g = 0xFFFFFFFF;

u32 get_mem_size_bytes(void)
{
	int i;
	u32 ram_size = 0;

	for (i = 0; i < CONFIG_NR_DRAM_BANKS; i++) {
		ram_size += gd->bd->bi_dram[i].size;
	}

	return ram_size;
}

static pTIM pdtim_primary = NULL;

#ifdef CONFIG_NSAID_MEM_IOSLATE
static u32 nsaid_ddr_range_idx = 0;

void nsaid_config_one_range(struct nsaid_range_desc *desc)
{
	u32 value;

        value = readl(DDR_RANGE_ACCESS_CTRL);
	value &= ~(0x1 << 9);
	writel(value, DDR_RANGE_ACCESS_CTRL);

	writel(/*(0x1 << 3) | (0x0 << 1) | (0x0 << 4) | (0x1 << 6)
			| */(desc->buff_start & 0xfffff000),
			(DDR_RANGE0_LOW_CFG + (8 * nsaid_ddr_range_idx)));
	writel(((desc->buff_end) & 0xfffff000),
			(DDR_RANGE0_TOP_LOW_CFG + (8 * nsaid_ddr_range_idx)));

	value = readl(DDR_RANGE0_LOW_CFG + (8 * nsaid_ddr_range_idx));
	value |= 0x1;
	printf("R%d-reg0x%x: 0x%x  ", nsaid_ddr_range_idx, (DDR_RANGE0_LOW_CFG + (8 * nsaid_ddr_range_idx)), value);
	writel(value, (DDR_RANGE0_LOW_CFG + (8 * nsaid_ddr_range_idx)));
	printf("buf[0x%x 0x%x] ", desc->buff_start, desc->buff_end);

	value = desc->perm_desc.value;
	writel(value, (DDR_RANGE0_MASK_CFG + (nsaid_ddr_range_idx * 4)));
	printf("perm: 0x%x\n", value);

	nsaid_ddr_range_idx++;
}

void nsaid_basic_init(void)
{
	int i;
	u32 val;

	/*allow r/w on all ranges/masters */
	for (i = 0; i < 0x10; i++)
		writel(0x0, (DDR_RANGE0_MASK_CFG + (i * 4)));

	for (i = 0; i < 0x20; i++)
		writel(0x0, (DDR_RANGE0_LOW_CFG + (i * 4)));

	for (i = 0; i < 0x20; i++)
		writel(0x0, (DDR_RANGE0_TOP_LOW_CFG + (i * 4)));

	/* set nsaid for master ports, CIU + 0x150
	* [0 - 3]:  CR7
	* [4 - 7]:  DTC
	* [8 -11]:  BX2U1
	* [12-15]:  BX2U0
	* [16-19]:  BX2C
	* [20-23]:  MSA
	* [24-27]:  4GM
	* [28-31]:  5GM
	*/
	val = (0x1 << 0)
		| (0x2 << 4)
		| (0x3 << 8)
		| (0x4 << 12)
		| (0x5 << 16)
		| (0x6 << 20)
		| (0x7 << 24)
		| (0x8 << 28);
	asr_ciu_write(CIU_NSAID_SETTING0, val);

	/* set nsaid for master ports, CIU + 0x150
	* [0 - 3]:  CA7
	* [4 - 7]:  TOE
	* [8 -11]:  PCIE0
	* [12-15]:  PCIE1
	* [16-19]:  HSDMA
	* [20-23]:  AP FAB
	* [24-27]:  MFC
	* [31]:     AP_FAB_L2_EN
	*/
	val = (0x0 << 0)
		| (0x9 << 4)
		| (0xA << 8)
		| (0xB << 12)
		| (0xC << 16)
		| (0xD << 20)
		| (0xE << 24);
	asr_ciu_write(CIU_NSAID_SETTING1, val);

	printf("adc_err_info: 0x%x 0x%x 0x%x 0x%x\n",
		readl(DDR_ADC_ERR_INFO + 0),
		readl(DDR_ADC_ERR_INFO + 4),
		readl(DDR_ADC_ERR_INFO + 8),
		readl(DDR_ADC_ERR_INFO + 0xc));

	/* clear error */
	writel(readl(DDR_ADC_ERR_INFO + 0) | (0x1 << 24),
		(DDR_ADC_ERR_INFO + 0));
	printf("nsaid begin\n");
}
#endif

char *image_id_to_string(unsigned int id)
{
	static char str[5];
	memset(str, 0, sizeof(str));
	str[0] = (id >> 24) & 0xff;
	str[1] = (id >> 16) & 0xff;
	str[2] = (id >> 8) & 0xff;
	str[3] = id & 0xff;
	return str;
}

void asr_free_dtim_primary(void)
{
	if (pdtim_primary) {
		free(pdtim_primary);
		pdtim_primary = NULL;
	}
}

pTIM asr_get_dtim_primary(void)
{
	pTIM ptim = NULL;
	pTIM pdtim = NULL;
	int ret = 0;
	struct OBM2OSL *params = NULL;
	unsigned char *tim_addr;
	unsigned int flash_addr = DTIM_PRI_FLASH_OFFSET;

	if(pdtim_primary)
		return pdtim_primary;

	pdtim = malloc(sizeof(struct TIM));
	if(pdtim == NULL)
	{
		return 0;
	}

	tim_addr = malloc(DTIM_PRI_FLASH_LEN);
	if(tim_addr == NULL)
	{
		free(pdtim);
		return 0;
	}

#ifdef CONFIG_AB_SYSTEM
	flash_addr = ab_get_image_offset(DTIM_PRIMARY);
#else
	ptim = asr_read_tim();
	if (ptim)
		flash_addr = get_image_offset_in_tim(ptim, DTIM_PRIMARY, PrimaryImage);
#endif
	flash_read(tim_addr, flash_addr, DTIM_PRI_FLASH_LEN);

	/* dtim itself must be validated in OBM */
	ret = set_tim_pointers(tim_addr, pdtim);

	if (ptim) {
		ret = verify_dtim(pdtim, ptim);
	}

	if(ret == 0){
		pdtim_primary = pdtim;
		return pdtim;
	}

	/* If no DTIM.Primary found, free the buffers */
	free(pdtim);
	free(tim_addr);
	return 0;
}

/*
* In current design, kernel/CP/MSA/RF image is included in DTIM.Primary or not.
* If it's included in DTIM.Primay and it's load address is not 0xFFFFFFFF, it's 
* loaded by OBM, then U-boot don't need to load it any more.
*/
int image_load_by_obm(unsigned int image_id)
{
	unsigned int tim_addr;
	struct TIM* p_dtim = NULL;
	struct IMAGE_INFO_3_4_0 * pimg;

	p_dtim = asr_get_dtim_primary();
	if(p_dtim == NULL)
		return 0;

	pimg = find_image_intim(p_dtim, DTIM_TIM_VER_3_4_0, image_id);
	if(pimg == NULL)
		return 0;

#ifdef CONFIG_ASR_SDTIM
	if(image_is_stdim_included(pimg)) {
		/* to simplify the design, uboot loads all the sdtim included
		* images except itself.
		*/
		return 0;
	}
#endif

	if(pimg && pimg->load_addr != 0xffffffff)
		return 1;

	return 0;
}
#define LZMA_HEADER     0x5D
int asr_load_dtim_image(unsigned int load_addr, struct TIM* p_dtim, 
	unsigned int image_id, int validate)
{
	int ret = 0;
	struct IMAGE_INFO_3_4_0 * pimg = NULL;
	unsigned int offset = 0;
#ifdef CONFIG_ASR_SDTIM
	struct IMAGE_INFO_3_4_0 * psdtim_img = NULL;
	struct IMAGE_INFO_3_4_0 * pimg_in_sdtim = NULL;
	struct TIM* p_sdtim = NULL;
	char *sdtim_buf = NULL;
	unsigned int sdtim_space_size = SDTIM_MAX_SIZE;
	unsigned int sec_cfg = 0;
#endif
	char *rf_h;
	u32 dict_size;
#ifdef CONFIG_ASR_RF_LZMA
	int size;
	u32 pBuf, tmp_buf;
#endif
	if(load_addr == 0 && validate == 0)
		return 0; /* need to do nothing */

	pimg = find_image_intim(p_dtim, DTIM_TIM_VER_3_4_0, image_id);
	if(pimg == NULL) {
		printf("%s is not in DTIM_Primary\n", image_id_to_string(image_id));
		return 0;
	}

#ifdef CONFIG_AB_SYSTEM
	offset = ab_get_image_offset(image_id);
#else
	offset = pimg->flash_entryaddr;
#endif

#ifdef CONFIG_ASR_SDTIM
	if ( image_is_stdim_included(pimg) ) { /* sdtim enabled */
		p_sdtim = malloc(sizeof(struct TIM));
		if(p_sdtim == NULL)
			return -1;

		sdtim_buf = malloc(sdtim_space_size);
		if(sdtim_buf == NULL) {
			free(p_sdtim);
			return -1;
		}

		flash_read(sdtim_buf, offset, sdtim_space_size);
		ret = set_tim_pointers(sdtim_buf, p_sdtim);
		if(ret) {
			free(p_sdtim);
			return -1;
		}

		psdtim_img = find_image_intim(p_sdtim, DTIM_TIM_VER_3_4_0, SDTIM);
		if(psdtim_img == NULL)
			goto sdtim_img_err;

		sdtim_space_size = get_sdtim_space_size(psdtim_img);

		pimg_in_sdtim = find_image_intim(p_sdtim, DTIM_TIM_VER_3_4_0, image_id);
		if(pimg_in_sdtim == NULL)
			goto sdtim_img_err;

		offset += sdtim_space_size;

		if (load_addr) {
			if(image_id == RFBIID) { /* for multi rfbin, decompress */
				flash_read(load_addr, offset, 0x1000);
				rf_h = (char *)load_addr;
				dict_size = (*(rf_h+4)<<24) + (*(rf_h+3)<<16) + (*(rf_h+2)<<8) + (*(rf_h+1));
				if((*rf_h == LZMA_HEADER) && !(dict_size&(dict_size-1))) { /* NAND use LZMA rfbin */
#ifdef CONFIG_ASR_RF_LZMA
					size = CONFIG_ASR_RF_LZMA_DDR_SIZE;
					tmp_buf = (u32)malloc(CONFIG_ASR_RF_LZMA_DDR_SIZE + 0x20000);	//malloc more memory for lzma
					pBuf = roundup(tmp_buf, 0x100);
					flash_read((u32)pBuf, (u32)offset, (u32)CONFIG_ASR_RF_LZMA_FLASH_SIZE);
					lzmaBuffToBuffDecompress((unsigned char *)load_addr, (SizeT *)&size,
							(u8 *)pBuf, (SizeT )CONFIG_ASR_RF_LZMA_FLASH_SIZE);
					free(tmp_buf);
#endif
				} else {            /* NAND use RAW rfbin */
					flash_read(load_addr, offset, pimg_in_sdtim->image_size_to_hash);
				}
			} else {
				flash_read(load_addr, offset, pimg_in_sdtim->image_size_to_hash);
			}
		}

#ifdef CONFIG_AB_SYSTEM
		sec_cfg = 1;	/* AB always validate image */
#else
		sec_cfg = get_sdtim_secure_cfg(psdtim_img);
#endif

#ifdef TRUSTED_BOOT
		if(validate && sec_cfg) {
			if (load_addr)
				ret = validate_image(load_addr, image_id, p_sdtim);
			else /* just to verify */
				ret = validate_image_segment(offset, image_id, p_sdtim, flash_read);
			printf("%s verified, %s, offset: 0x%x, size: 0x%x\n", image_id_to_string(image_id),
				(ret == 0)?"PASS":"FAIL", offset, pimg_in_sdtim->image_size_to_hash);
		}
#endif

sdtim_img_err:
		if(p_sdtim) free(p_sdtim);
		if(sdtim_buf) free(sdtim_buf);
		return ret;
	}
#endif
	if (load_addr) {
		if(image_id == RFBIID) { /* for multi rfbin, decompress */
			flash_read(load_addr, offset, 0x1000);
			rf_h = (char *)load_addr;
			dict_size = (*(rf_h+4)<<24) + (*(rf_h+3)<<16) + (*(rf_h+2)<<8) + (*(rf_h+1));
			if((*rf_h == LZMA_HEADER) && !(dict_size&(dict_size-1))) { /* NAND use LZMA rfbin */
#ifdef CONFIG_ASR_RF_LZMA
				size = CONFIG_ASR_RF_LZMA_DDR_SIZE;
				tmp_buf = (u32)malloc(CONFIG_ASR_RF_LZMA_DDR_SIZE + 0x20000);	//malloc more memory for lzma
				pBuf = roundup(tmp_buf, 0x100);
				flash_read((u32)pBuf, (u32)offset, (u32)CONFIG_ASR_RF_LZMA_FLASH_SIZE);
				lzmaBuffToBuffDecompress((unsigned char *)load_addr, (SizeT *)&size,
						(u8 *)pBuf, (SizeT )CONFIG_ASR_RF_LZMA_FLASH_SIZE);
				free(tmp_buf);
#endif
			} else {
				flash_read(load_addr, offset, pimg->image_size_to_hash);
			}
		} else {
			flash_read(load_addr, offset, pimg->image_size_to_hash);
		}
	}

#if TRUSTED_BOOT
	if(validate)  {
		if (load_addr)
			ret = validate_image(load_addr, image_id, p_dtim);
		else /* just to verify */
			ret = validate_image_segment(offset, image_id, p_dtim, flash_read);
		printf("%s verified, %s, offset: 0x%x, size: 0x%x\n", image_id_to_string(image_id),
			(ret == 0)?"PASS":"FAIL", offset, pimg->image_size_to_hash);
	}
#endif

	return ret;
}

int asr_load_verify_kernel(unsigned char load_status, int ramdump_boot)
{
	int ret = 0;
	struct TIM* p_dtim = NULL;
	int validate = 1;

	/* kernel is already loaded and verified by OBM */
	if(!load_status)
		return 0;

	p_dtim = asr_get_dtim_primary();
	if(p_dtim == NULL) {
		printf("Get DTIM.Primary error\n\r");
		return -1;
	}

#if 0
	if(ramdump_boot)
		validate = 0;
#endif

	if (find_image_intim(p_dtim, DTIM_TIM_VER_3_4_0, ZIMG)) {
		ret = asr_load_dtim_image(CONFIG_LOADADDR, p_dtim, ZIMG, validate);

		if(ret) {
#ifndef CONFIG_AB_SYSTEM
			asr_flag_trust_boot_update(TB_ZIMG_ERROR);
#endif
			return ret;
		}
	} else {
		flash_read(CONFIG_LOADADDR, KERNEL_FLASH_OFFSET, KERNEL_SIZE);
	}

	return 0;
}

int asr_load_verify_ap_images(struct ap_img_info * ap_imgs, int num)
{
	int i = 0;
	int ret = 0;
	int validate = 0;
	struct TIM* p_dtim = NULL;
	unsigned int load_addr, image_id;

#ifdef TRUSTED_BOOT
	validate = 1;
#endif

	p_dtim = asr_get_dtim_primary();
	if(p_dtim == NULL) {
		printf("Get DTIM.Primary error\n\r");
		return -1;
	}

	for(i = 0; i < num; i++)
	{
		image_id = ap_imgs[i].image_id;
		load_addr = ap_imgs[i].load_addr;
		ret = asr_load_dtim_image(load_addr, p_dtim, image_id, validate);
		if(ret) {
			printf("%s load/verify error\n", image_id_to_string(image_id));
#ifndef CONFIG_AB_SYSTEM
			asr_flag_trust_boot_image_error(image_id);
#endif
			return ret;
		}
	}

	return ret;
}

int asr_load_verify_cp_images(u32 cpbt_mode, struct cp_img_flash_layout *lwg_layout)
{
	int ret;
	unsigned int arbel_flash_offset;
	unsigned int msa_flash_offset;
	unsigned int rf_flash_offset;
	unsigned int bx2_flash_offset;

	if (lwg_layout == NULL) //ltg_layout could be NULL but lwg_layout couldn't
	{
		printf("main cp image layout is NULL\n");
		return -1;
	}

	if (lwg_layout) 
	{
		ret = cp_load_table_init(lwg_layout->arbel_offset);
		if (ret) {
#ifndef CONFIG_AB_SYSTEM
			asr_flag_trust_boot_update(TB_CPHD_ERROR);
			set_nocp_mode(1);
#endif
			goto tb_error;
		} else {
			arbel_flash_offset = lwg_layout->arbel_offset;
			msa_flash_offset = lwg_layout->msa_offset;
			rf_flash_offset = lwg_layout->rf_offset;
			bx2_flash_offset = lwg_layout->bx2_offset;
			goto load_modem;
		}
	}

load_modem:
	if( load_arbel_image(arbel_flash_offset) || 
		load_msa_image(msa_flash_offset) || 
		load_rf_image(rf_flash_offset) )
		goto tb_error;

	if (cpu_is_asr1901() || cpu_is_asr1903() || cpu_is_asr1906()) {
		if(bx2_flash_offset && load_bx2_image(bx2_flash_offset))
			goto tb_error;
	}

#ifdef CONFIG_AB_SYSTEM
#ifndef CONFIG_ASR_EMMC_BOOT
#ifdef OEM_DATA_AB_ENABLED
		mtd_part_add(OEM_DATA_ADDR, OEM_DATA_SIZE, OEM_NVM_UBI_VOLUME);
		mtd_part_add(NVM_MTD_PART_ADDR, NVM_MTD_PART_SIZE, NVM_MTD_PART);
		// for print the info
		run_command("mtdparts", 0);
#endif
#endif
#endif

	asr_platform_update_cp_param(NULL);
	load_cp_reliable_data(lwg_layout->cp_rd_offset, lwg_layout->cp_rd_bk_offset);
	load_ap_reliable_data(lwg_layout->ap_rd_offset, lwg_layout->ap_rd_bk_offset);
	if(lwg_layout->nvm_tbl && lwg_layout->nvm_tbl_size)
		load_ubifs_nvm_for_cp(lwg_layout->nvm_tbl, lwg_layout->nvm_tbl_size);
	cp_keysection_data_init();
	if (!cpbt_mode)
		asr_cp_release();
	return 0;

tb_error:
	return -1;
}

int asr_get_sim_lock_fuse(void)
{
	unsigned aes_clk_res_ctrl;
	unsigned value = 0;

#ifdef CONFIG_PXA182X
	aes_clk_res_ctrl = readl(APMU_AES_CLK_RES_CTRL);
	writel(0x9, APMU_AES_CLK_RES_CTRL); //turn on clock
	value = readl(GEU_FUSE_VAL_APCFG1);
	value = (value >> 16) & 1;
	writel(aes_clk_res_ctrl, APMU_AES_CLK_RES_CTRL);
#elif defined(CONFIG_ASR1802S)
	aes_clk_res_ctrl = readl(APMU_AES_CLK_RES_CTRL);
	writel(0x9, APMU_AES_CLK_RES_CTRL); //turn on clock
	value = readl(GEU_FUSE_VAL_APCFG1);
	value &= 1;
	writel(aes_clk_res_ctrl, APMU_AES_CLK_RES_CTRL);
#else
	printf("warning: sim lock fuse read\n"); /* TODO, 1901 */
#endif

	return value;
}

#ifdef CONFIG_TEE_OS
unsigned int asr_get_tos_load_address(void)
{
	struct OBM2OSL *params = NULL;
	struct TIM* ptim = NULL;
	struct IMAGE_INFO_3_4_0 * pimg;
	unsigned int addr = 0x2000000;

	ptim = asr_get_dtim_primary();

	if(ptim == NULL)
		return addr;

	pimg = find_image_intim(ptim, DTIM_TIM_VER_3_4_0, TZSI);
	if(pimg == NULL) {
		return addr;
	}

#ifdef CONFIG_ASR_SDTIM
	if ( image_is_stdim_included(pimg) ) { /* sdtim enabled */
		int ret = 0;
		struct TIM* p_sdtim = NULL;
		uint8_t *sdtim_buf = NULL;
		uint32_t offset;
		p_sdtim = malloc(sizeof(struct TIM));
		if(p_sdtim == NULL)
			return addr;

		sdtim_buf = malloc(SDTIM_MAX_SIZE);
		if(sdtim_buf == NULL) {
			free(p_sdtim);
			return addr;
		}

#ifdef CONFIG_AB_SYSTEM
		offset = ab_get_image_offset(pimg->imageid);
#else
		offset = pimg->flash_entryaddr;
#endif

		flash_read(sdtim_buf, offset, SDTIM_MAX_SIZE);
		ret = set_tim_pointers(sdtim_buf, p_sdtim);
		if(ret)
		{
			free(p_sdtim);
			free(sdtim_buf);
			return addr;
		}

		pimg = find_image_intim(p_sdtim, DTIM_TIM_VER_3_4_0, TZSI);
		if(pimg != NULL)
			addr = pimg->load_addr;
		free(p_sdtim);
		free(sdtim_buf);
		return addr;
	}
#endif

	addr = pimg->load_addr;
	return addr;
}
#endif


int get_ramdump_flag_f_apcp(void)
{
	unsigned int *ramdump_indicator = (unsigned int *)CONFIG_CRASHKERNEL_INDICATOR;
	/* returns 1 for ap/cp assert */
	if ((*ramdump_indicator == 0x454d4d44))
		return 1;
	else
		return 0;
}


int get_ramdump_flag_f(void)
{
	unsigned int *ramdump_indicator = (unsigned int *)CONFIG_CRASHKERNEL_INDICATOR;
	/* returns 0 for cpassert dump to use long malloc buffer */
	if ((*ramdump_indicator == 0x454d4d44) && (!ramdump_is_cpssert()))
		return 1;
	else
		return 0;
}

struct asr_mflag *get_asr_mflag(void)
{
	asr_mflag_g = (struct asr_mflag *)(CONFIG_CRASHKERNEL_INDICATOR + ASR_MFLAG_OFFSET_FROM_CRASHKERNEL);
	return asr_mflag_g;
}

#ifdef CONFIG_CMD_FASTBOOT
int get_fastboot_flag_f(void)
{
	struct asr_mflag *asr_flag = get_asr_mflag();
	if (asr_flag->fastboot_flag == 0x46415354)
		return 1;
	return 0;
}

int reset_fastboot_flag_f(void)
{
	struct asr_mflag *asr_flag = get_asr_mflag();
	asr_flag->fastboot_flag = 0;
	flush_cache((unsigned long)&(asr_flag->fastboot_flag),
		  (unsigned long)sizeof(asr_flag->fastboot_flag));
}

void run_fastboot_cmd(void)
{
	if (get_fastboot_flag_f())
	{
		printf("Detect FASTBOOT signature!!\n");
		reset_fastboot_flag_f();
		setenv("fbenv", "nand0");
		run_command("fb", 0);	
	}	
}
#endif

int prepare_ap_diag_buff(u32 buff_st, u32 buff_end)
{
	u32 buff_len;
	struct asr_mflag *mflag = get_asr_mflag();

	printf("diagbf magic: %08x, len: %x, buff[0x%08x, 0x%08x]\n",
		mflag->diag_buff_magic, mflag->diag_buff_len, buff_st, buff_end);
	if ((mflag->diag_buff_magic != AP_DIAG_BUF_SET_MAGIC && mflag->diag_buff_magic != AP_DIAG_BUF_SET_MAGIC2)
		|| (!mflag->diag_buff_len)) {
		return -1;
	}

	if (buff_st & 0x3fffff) {
		buff_st += 0x3fffff;
		buff_st &= 0xffc00000;
	}

	buff_len = buff_end - buff_st;

#ifdef CONFIG_MAX_DIAG_BUFF_LEN
	if (buff_len > CONFIG_MAX_DIAG_BUFF_LEN)
		buff_len = CONFIG_MAX_DIAG_BUFF_LEN;
#endif

	if (mflag->diag_buff_len > buff_len) {
		mflag->diag_buff_len = buff_len;
		if (mflag->diag_buff_len >= (4 * 1024 * 1024))
			mflag->diag_buff_len = (4 * 1024 * 1024);
		else if (mflag->diag_buff_len >= (2 * 1024 * 1024))
			mflag->diag_buff_len = (2 * 1024 * 1024);
		else
			mflag->diag_buff_len = (1 * 1024 * 1024);		
	}

	mflag->diag_buff_addr = buff_st;
	printf("final diag buf: %08x len: %x\n", mflag->diag_buff_addr, mflag->diag_buff_len);
	memset(mflag->diag_buff_addr, 0x0, mflag->diag_buff_len);
	return 0;
}