marvell/obm/Common/DFota/sdfota.c

#include "fota.h"
#include "obm2osl.h"
#include "Flash.h"
#include "malloc.h"

extern SkipBlocksInfoStruct *pSkipAddress;
extern UINT_T All_FF_flag;
extern UINT_T swd_ddr_size; //DDR size: swd_ddr_size*16MB
extern FOTA_Info FOTAInfo;

#define SQUASHFS_MAX_SIZE   0x400000

//#define SDFOTA_DEBUG
//#define SDFOTA_STAT
//#define POWEROFF_TEST

#define sdfota_info obm_printf

#ifdef SDFOTA_DEBUG
#define sdfota_dbg obm_printf
#else
#define sdfota_dbg(fm,...)
#endif

#ifdef POWEROFF_TEST
#define POWER_OFF_TEST_POINT PowerOffTestPoint();
#else
#define POWER_OFF_TEST_POINT
#endif

static int powercut_test_point = 0;

void PowerOffTestPoint(void)
{
	sdfota_info("power cut test point %d, waiting 3s for interrupt\n\r", powercut_test_point);
	Delay(3000000);
	sdfota_info("test point %d over\n\r", powercut_test_point);
	powercut_test_point++;
}

static int offtin32(uint8_t *buf)
{
	int64_t y;

	y=buf[3]&0x7F;
	y=y*256;y+=buf[2];
	y=y*256;y+=buf[1];
	y=y*256;y+=buf[0];

	if(buf[3]&0x80) y=-y;

	return y;
}

UINT_T DFota_ParseBsdiffPatch(CHAR *BsdiffPatch, UINT_T PatchLen, pBsdiffInfo pBsdiff_h)
{
	UINT_T SegNum, i;
	CHAR *PatchPtr;
	PatchPtr = BsdiffPatch;
	if(memcmp(PatchPtr, "BSDIFF50", 8) == 0) {
		pBsdiff_h->Version = 50;
	} else if (memcmp(PatchPtr, "BSDIFF40", 8) == 0) {
		pBsdiff_h->Version = 40;
	} else {
		pBsdiff_h->Version = 0;
		sdfota_dbg("Invalid Bsdiff Patch, could be image\n\r");
	}

	pBsdiff_h->Patch = BsdiffPatch;
	pBsdiff_h->PatchLen = PatchLen;

	if(pBsdiff_h->Version != 50)
		return 0;

	PatchPtr += 8;
	SegNum = offtin32(PatchPtr);
	sdfota_dbg("Segment number: %d\n\r", SegNum);;
	memset(pBsdiff_h, 0, sizeof(BsdiffInfo));
	pBsdiff_h->SegNum = SegNum;
	
	PatchPtr += 4;

	for(i = 0; i < SegNum; i++ )
	{
		pBsdiff_h->SegInfo[i].SrcLen = offtin32(PatchPtr);
		pBsdiff_h->ImageSrcLen += pBsdiff_h->SegInfo[i].SrcLen;

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].SrcComLen = offtin32(PatchPtr);
		pBsdiff_h->ImageSrcComLen += pBsdiff_h->SegInfo[i].SrcComLen;

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].DestLen = offtin32(PatchPtr);
		pBsdiff_h->ImageDestLen += pBsdiff_h->SegInfo[i].DestLen;

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].DestComLen = offtin32(PatchPtr);
		pBsdiff_h->ImageDestComLen += pBsdiff_h->SegInfo[i].DestComLen;

		if(pBsdiff_h->SegInfo[i].DestComLen > pBsdiff_h->MaxSegDecLen)
			pBsdiff_h->MaxSegDecLen = pBsdiff_h->SegInfo[i].DestComLen;

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].Compression = offtin32(PatchPtr);

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].PatchLen = offtin32(PatchPtr);

		PatchPtr += 4;
		pBsdiff_h->SegInfo[i].Bsdiff40 = PatchPtr;

		if(memcmp(pBsdiff_h->SegInfo[i].Bsdiff40, "BSDIFF40", 8) != 0)
		{
			sdfota_info("Invalid BSDIFF40 Patch Found\n\r");
			return SDFOTA_PatchParseError;
		}

		PatchPtr += pBsdiff_h->SegInfo[i].PatchLen;

		sdfota_dbg("BSDIFF40 %d: src: 0x%x/0x%x, dest: 0x%x/0x%x, compress: %d, patch: 0x%x@0x%x\n\r", 
			i, 
			pBsdiff_h->SegInfo[i].SrcLen, pBsdiff_h->SegInfo[i].SrcComLen,
			pBsdiff_h->SegInfo[i].DestLen, pBsdiff_h->SegInfo[i].DestComLen,
			pBsdiff_h->SegInfo[i].Compression,
			pBsdiff_h->SegInfo[i].PatchLen, (UINT_T)pBsdiff_h->SegInfo[i].Bsdiff40);
	}

	sdfota_dbg("MaxSegDecLen: 0x%x\n\r", pBsdiff_h->MaxSegDecLen);
	sdfota_dbg("Segment Num: 0x%x\n\r", pBsdiff_h->SegNum);
	sdfota_dbg("Image src total size: 0x%x\n\r", pBsdiff_h->ImageSrcLen);
	sdfota_dbg("Image dest total size: 0x%x\n\r", pBsdiff_h->ImageDestLen);
	sdfota_dbg("Image src comp total size: 0x%x\n\r", pBsdiff_h->ImageSrcComLen);
	sdfota_dbg("Image dest comp total size: 0x%x\n\r", pBsdiff_h->ImageDestComLen);

	return NoError;
}

INT_T Image_GetXZOP(pTIM pTIM_h, UINT32 Image_ID, UINT_T *OptNum, UINT_T *OptBuff)
{
	UINT_T Retval;
	pWTP_RESERVED_AREA_HEADER pWRAH = NULL;
	UINT_T *pXZOP;
	UINT_T XzopLen;
	UINT_T Len = 0;

	pWRAH = FindPackageInReserved(&Retval, pTIM_h, XZOP);
	if ( !( (pWRAH == NULL) || (Retval != NoError) ) ) {
		pXZOP = (UINT_T *)((UINT_T)pWRAH + 0x8);
		XzopLen = pWRAH->Size - 8;
		while(Len < XzopLen) {
			if(*pXZOP == Image_ID) {
				pXZOP ++;
				*OptNum = *pXZOP++;
				memcpy(OptBuff, pXZOP, (*OptNum) << 2);
				pXZOP += *OptNum;
				Len += 8 + ((*OptNum) << 2);

				return 0;
			} else {
				Len += 4;
				pXZOP++;
			}
		}
	}

	return -1;
}

#ifdef SDFOTA_STAT
static ota_head = 0;
static ota_end = 0;
#endif

UINT_T SDFotaState_Init(pTIM pTIM_h, PImageStruct_11 pImage_11, pBsdiffInfo pBsdiff_h, P_FOTA_Firmware pFOTA_T)
{
	UINT_T ImageType;
	UINT_T FlashBackupAddr;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	UINT_T NumBlocks = pFlashP->NumBlocks;
	UINT_T FlashSize = BlockSize * NumBlocks;
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);
	FlashBackupAddr = ALIGN_UP(pFOTA_T->FBF_Flash_Address + pFOTA_T->FBF_Size, BlockSize);
	ImageType = IMAGE_TYPE(pImage_11->commands);

	sdfota_dbg("SDFotaState FBF Address: 0x%x 0x%x 0x%x\n\r", pFOTA_T->FBF_Flash_Address, pFOTA_T->FBF_Size, FlashBackupAddr);

	#ifdef SDFOTA_STAT
	ota_head = pFOTA_T->FBF_Flash_Address;
	#endif

	if(pSDFotaState_h->CurImageID == 0) {
		pSDFotaState_h->CurImageID = pImage_11->Image_ID;
		pSDFotaState_h->ImageType = ImageType;

		switch(ImageType) {
		case DLCMD_IMG_MXZ_IMAGE_TYPE:
		case DLCMD_MULTI_XZ_IMAGE_TYPE:
			/* segmented images */
			pSDFotaState_h->SegDestBkAddr = FlashBackupAddr;
			pSDFotaState_h->PreTailAddr = ALIGN_UP(FlashBackupAddr + pBsdiff_h->MaxSegDecLen, BlockSize);
			pSDFotaState_h->NextHeadAddr = pSDFotaState_h->PreTailAddr + BlockSize;
			if((pSDFotaState_h->NextHeadAddr + BlockSize) > FlashSize)
				return DFOTA_PartitionTooSmall;
			pSDFotaState_h->NextSegWriteOffset = 0;
			pSDFotaState_h->NextSegReadOffset = 0;
			pSDFotaState_h->NextSegEraseOffset = 0;
			pSDFotaState_h->SegState = 0;
			pSDFotaState_h->SegIndex = 0;
			pSDFotaState_h->PreTailLen = 0;
			pSDFotaState_h->SegDestBkLen = 0;
			sdfota_dbg("SDFotaState Init: 0x%x 0x%x 0x%x 0x%x\n\r", 
				pSDFotaState_h->CurImageID, pSDFotaState_h->SegDestBkAddr,
				pSDFotaState_h->PreTailAddr, pSDFotaState_h->NextHeadAddr);
			break;
		default:
			/* unsegmented images */
			pSDFotaState_h->ImageState = 0;
			pSDFotaState_h->ImageBkLen = 0;
			pSDFotaState_h->ImageBkAddr = FlashBackupAddr;
			break;
		}
	}

	return NoError;
}

INT_T SDFotaState_Update(pTIM pTIM_h, INT_T SegIdx, UINT_T State)
{
	UINT_T Retval = NoError;
	P_FOTA_Firmware pFOTA_T = OTAGetConfig(pTIM_h);
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);

	if(SegIdx == -1) {/* -1  stands for the image which is not segmented*/
		pSDFotaState_h->ImageState = State;
		sdfota_dbg("SDFOTA image state: %d\n\r", State);
	#ifdef SDFOTA_STAT
		if(ota_end < pSDFotaState_h->ImageBkAddr + pSDFotaState_h->ImageBkLen)
			ota_end = pSDFotaState_h->ImageBkAddr + pSDFotaState_h->ImageBkLen;
	#endif
	} else {
		pSDFotaState_h->SegIndex = SegIdx;
		pSDFotaState_h->SegState = State;

		sdfota_dbg("SDFOTA segment state: %d %d 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n\r", 
			pSDFotaState_h->SegIndex, pSDFotaState_h->SegState,
			pSDFotaState_h->PreTailAddr, pSDFotaState_h->PreTailLen,
			pSDFotaState_h->NextHeadAddr, pSDFotaState_h->NextHeadLen,
			pSDFotaState_h->SegDestBkAddr, pSDFotaState_h->SegDestBkLen);
	#ifdef SDFOTA_STAT
		if(ota_end < pSDFotaState_h->NextHeadAddr + pSDFotaState_h->NextHeadLen)
			ota_end = pSDFotaState_h->NextHeadAddr + pSDFotaState_h->NextHeadLen;
	#endif
	}

	if(State == SDFOTA_IMG_DONE) {
	#ifdef SDFOTA_STAT
		sdfota_info("OTA range used: 0x%x - 0x%x, len: 0x%x\n\r", ota_head, ota_end, ota_end - ota_head);
	#endif
	}

	Retval = OTA_Save_Config(pTIM_h);
	if (Retval != NoError)
	{
		sdfota_info("SDFotaState_Update error: 0x%x\n\r", Retval);
		return Retval;
	}

	return NoError;
}

INT_T ReadFlashAlign(UINT_T Addr, UINT_T Buffer, UINT_T Size)
{
	UINT_T Retval;
	UINT_T AlignedAddr;
	UINT_T AlignSize;
	CHAR *TmpBuff = NULL;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	AlignedAddr = ALIGN_DN(Addr, BlockSize);

	if(AlignedAddr == Addr)
		return ReadFlash(Addr, Buffer, Size, BOOT_FLASH);

	AlignSize = Addr - AlignedAddr;
	TmpBuff = malloc(Size + AlignSize);
	if(TmpBuff == NULL) {
		return HeapExhaustedError;
	}
	Retval = ReadFlash(AlignedAddr, (UINT_T)TmpBuff, Size + AlignSize, BOOT_FLASH);
	if(Retval == NoError) {
		memcpy((CHAR *)Buffer, TmpBuff + AlignSize, Size);
	}

	free(TmpBuff);
	return Retval;
}

INT_T ReadSegment(UINT_T FlashAddr, UINT_T Buffer, UINT_T SegSize, pTIM pTIM_h, pBsdiffInfo pBsdiff_h)
{
	UINT_T Retval = NoError;
	UINT_T AlignedAddr;
	UINT_T HeadSize = 0;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	P_FOTA_Firmware pFOTA_T = OTAGetConfig(pTIM_h);
	CHAR *HeadBuff = NULL;
	CHAR *TmpBuff = NULL;
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);

	HeadSize = pSDFotaState_h->NextHeadLen;

	if(HeadSize == 0)
		return ReadFlashAlign(FlashAddr, Buffer, SegSize);

	HeadBuff = malloc(HeadSize);
	if(HeadBuff == NULL) {
		Retval = HeapExhaustedError;
		goto finish;
	}
	TmpBuff = malloc(SegSize);
	if(TmpBuff == NULL) {
		Retval = HeapExhaustedError;
		goto finish;
	}

	Retval = ReadFlashAlign(pSDFotaState_h->NextHeadAddr, (UINT_T)HeadBuff, HeadSize);
	if(Retval != NoError) goto finish;

	Retval = ReadFlashAlign(FlashAddr+pSDFotaState_h->NextSegEraseOffset, (UINT_T)TmpBuff, SegSize - HeadSize);
	if(Retval != NoError) goto finish;

	memcpy((CHAR *)Buffer, HeadBuff, HeadSize);
	memcpy((CHAR *)(Buffer + HeadSize), TmpBuff, SegSize - HeadSize);

finish:
	if(HeadBuff) free(HeadBuff);
	if(TmpBuff) free(TmpBuff);
	return Retval;
}

INT_T WriteSegment(UINT_T flash_addr, UINT_T buffer, UINT_T seg_size, pTIM pTIM_h, pBsdiffInfo pBsdiff_h)
{
	UINT_T Retval = NoError;
	UINT_T AlignedAddr;
	UINT_T PreTailSize = 0;
	UINT_T TailSize = 0, WriteSize;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	P_FOTA_Firmware pFOTA_T = OTAGetConfig(pTIM_h);
	CHAR *WriteBuff = NULL;
	CHAR *TailBuff = NULL;
	UINT_T WriteBuffLen;
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);
	AlignedAddr = ALIGN_DN(flash_addr, BlockSize);

	PreTailSize = flash_addr - AlignedAddr;
	if(PreTailSize != pSDFotaState_h->PreTailLen) {
		sdfota_info("Tail Size doesn't match\n\r");
		return SDFOTA_WriteTailMismatchError;
	}

	if(PreTailSize > 0) {
		WriteBuffLen = ALIGN_UP(PreTailSize + seg_size, BlockSize);
		WriteBuff = malloc(WriteBuffLen);
		memset(WriteBuff, 0xFF, WriteBuffLen);
		if(WriteBuff == NULL) {
			Retval = HeapExhaustedError;
			goto write_finish;
		}
		Retval = ReadFlash(pSDFotaState_h->PreTailAddr, (UINT_T)WriteBuff, PreTailSize, BOOT_FLASH);
		if(Retval != NoError) goto write_finish;
		memcpy(WriteBuff+PreTailSize, (CHAR *)buffer, seg_size);
	} else {
		WriteBuff = (CHAR *)buffer;
	}

	if ( pSDFotaState_h->SegIndex == (pBsdiff_h->SegNum - 1) ) /* last segment, no need to back up tail */
	{
		Retval = WriteFlash(AlignedAddr, (UINT_T)WriteBuff, PreTailSize + seg_size, BOOT_FLASH);
		if(Retval != NoError) goto write_finish;
		Retval = SDFotaState_Update(pTIM_h, pSDFotaState_h->SegIndex, SDFOTA_SEG_DONE);
		goto write_finish;
	}

	WriteSize = ALIGN_DN(PreTailSize + seg_size, BlockSize);
	TailSize = PreTailSize + seg_size - WriteSize;

	Retval = WriteFlash(AlignedAddr, (UINT_T)WriteBuff, WriteSize, BOOT_FLASH);
	if(Retval != NoError) goto write_finish;

	Retval = SDFotaState_Update(pTIM_h, pSDFotaState_h->SegIndex, SDFOTA_WRITE_ALIGN);
	if(Retval != NoError) goto write_finish;


	/* Here, previous tail be erased and written */
	Retval = EraseFlash(pSDFotaState_h->PreTailAddr, BlockSize, BOOT_FLASH);
	if(Retval != NoError) goto write_finish;

	TailBuff = malloc(TailSize);
	if(TailBuff == NULL) {
		Retval = HeapExhaustedError;
		goto write_finish;
	}

	memcpy(TailBuff, WriteBuff + WriteSize, TailSize);
	Retval = WriteFlash(pSDFotaState_h->PreTailAddr, (UINT_T)TailBuff, TailSize, BOOT_FLASH);
	if(Retval != NoError) goto write_finish;

	pSDFotaState_h->PreTailLen = TailSize;
	Retval = SDFotaState_Update(pTIM_h, pSDFotaState_h->SegIndex, SDFOTA_WRITE_TAIL);
	if(Retval != NoError) goto write_finish;

write_finish:
	if( WriteBuff && ((UINT_T)WriteBuff != buffer) )
		free(WriteBuff);
	if(TailBuff) free(TailBuff);
	return Retval;
}

INT_T SDfota_PowerCutResumeSegImages(pTIM pTIM_h, pBsdiffInfo pBsdiff_h, UINT_T FlashAddr)
{
	UINT_T Retval = NoError;
	UINT_T PowerOffState;
	UINT_T SegNewLen;
	UINT_T SegOldLen;
	UINT_T AddonLen;
	UINT_T AddonAddr;
	UINT_T SegEraseSize;
	UINT_T SegIdx;
	UINT_T TailSize;
	UINT_T NextSegWriteOffset;
	UINT_T NextSegReadOffset;
	UINT_T NextSegEraseOffset;
	CHAR *SegNewBuff = NULL;
	CHAR *TailBuff = NULL;
	CHAR *AddonBuff = NULL;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	P_FOTA_Firmware pFOTA_T = OTAGetConfig(pTIM_h);

	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo); 
	PowerOffState = pSDFotaState_h->SegState;
	SegIdx = pSDFotaState_h->SegIndex;

	sdfota_dbg("PowerOffState: %d\n\r", PowerOffState);

	if(PowerOffState == SDFOTA_SEG_DONE) {
		goto seg_done;
	}

	if(PowerOffState == SDFOTA_NONE) {
		return NoError;
	}

	sdfota_dbg("start to resume\n\r");
	NextSegReadOffset = pSDFotaState_h->NextSegReadOffset;
	NextSegWriteOffset = pSDFotaState_h->NextSegWriteOffset;
	NextSegEraseOffset = pSDFotaState_h->NextSegEraseOffset;
	SegOldLen = pBsdiff_h->SegInfo[SegIdx].SrcComLen;
	SegNewLen = pSDFotaState_h->SegDestBkLen;
	SegEraseSize = ALIGN_UP(NextSegWriteOffset + SegNewLen, BlockSize) - NextSegEraseOffset; //Align with Block size
	SegNewBuff = malloc(SegNewLen);
	if(SegNewBuff == NULL)	{
		Retval = HeapExhaustedError;
		goto resume_error;
	}
	sdfota_dbg("FlashAddr: 0x%x\n\r", FlashAddr);
	sdfota_dbg("NextSegWriteOffset: 0x%x\n\r", NextSegWriteOffset);
	sdfota_dbg("NextSegReadOffset: 0x%x\n\r", NextSegReadOffset);
	sdfota_dbg("NextSegEraseOffset: 0x%x\n\r", NextSegEraseOffset);
	sdfota_dbg("SegOldLen: 0x%x\n\r", SegOldLen);
	sdfota_dbg("SegNewLen: 0x%x\n\r", SegNewLen);
	sdfota_dbg("SegEraseSize: 0x%x\n\r", SegEraseSize);
	switch(PowerOffState) {
	case SDFOTA_DEST_SAVED:
		sdfota_dbg("power-cut resume from SDFOTA_DEST_SAVED\n\r");
		if(SegIdx < (pBsdiff_h->SegNum - 1) /* not last segment */) {
			/* backup head addon of next segment */
			if((NextSegEraseOffset + SegEraseSize) > (NextSegReadOffset + SegOldLen)) {
				AddonLen = NextSegEraseOffset + SegEraseSize - NextSegReadOffset - SegOldLen;
				AddonAddr = FlashAddr + NextSegReadOffset + SegOldLen;
				sdfota_dbg("Resume, head addon Address: 0x%x\n\r", AddonAddr);
				AddonBuff = malloc(AddonLen);
				if(AddonBuff == NULL) {
					Retval = HeapExhaustedError;
					goto resume_error;
				}
				Retval = ReadFlashAlign(AddonAddr, (UINT_T)AddonBuff, AddonLen);
				if(Retval != NoError) goto resume_error;
				Retval = EraseFlash(pSDFotaState_h->NextHeadAddr, AddonLen, BOOT_FLASH);
				if(Retval != NoError) goto resume_error;
				Retval = WriteFlash(pSDFotaState_h->NextHeadAddr, (UINT_T)AddonBuff, AddonLen, BOOT_FLASH);
				if(Retval != NoError) goto resume_error;
				sdfota_dbg("Backup Head addon for Seg%d: 0x%x@0x%x\n\r", SegIdx+1, AddonLen, 
							FlashAddr+NextSegReadOffset+SegOldLen);

				pSDFotaState_h->NextHeadLen = AddonLen;
				Retval = SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_HEAD_SAVED);
				if(Retval != NoError) goto resume_error;
			}
		}
		/* gothrough */
	case SDFOTA_HEAD_SAVED:
		sdfota_dbg("power-cut resume from SDFOTA_HEAD_SAVED\n\r");
		Retval = ReadFlash(pSDFotaState_h->SegDestBkAddr, (UINT_T)SegNewBuff, SegNewLen, BOOT_FLASH);
		if(Retval != NoError) goto resume_error;
		Retval = EraseFlash(FlashAddr + NextSegEraseOffset, SegEraseSize, BOOT_FLASH);
		if(Retval != NoError) goto resume_error;
		Retval = WriteSegment(FlashAddr+NextSegWriteOffset, (UINT_T)SegNewBuff, SegNewLen, pTIM_h, pBsdiff_h);
		if(Retval != NoError) goto resume_error;
		break;
	case SDFOTA_WRITE_ALIGN:
		sdfota_dbg("power-cut resume from SDFOTA_WRITE_ALIGN\n\r");
		Retval = ReadFlash(pSDFotaState_h->SegDestBkAddr, (UINT_T)SegNewBuff, SegNewLen, BOOT_FLASH);
		if(Retval != NoError) goto resume_error;
		TailSize = (FlashAddr +NextSegWriteOffset + SegNewLen) & (BlockSize - 1);
		TailBuff = malloc(TailSize); /* get an aligned buffer from heap */
		if(TailBuff == NULL) {
			Retval = HeapExhaustedError;
			goto resume_error;
		}
		memcpy(TailBuff, SegNewBuff + SegNewLen - TailSize, TailSize);
		Retval = EraseFlash(pSDFotaState_h->PreTailAddr, BlockSize, BOOT_FLASH);
		if(Retval != NoError) goto resume_error;
		Retval = WriteFlash(pSDFotaState_h->PreTailAddr, (UINT_T)TailBuff, TailSize, BOOT_FLASH);
		if(Retval != NoError) goto resume_error;
		pSDFotaState_h->PreTailLen = TailSize;
		Retval = SDFotaState_Update(pTIM_h, pSDFotaState_h->SegIndex, SDFOTA_WRITE_TAIL);
		if(Retval != NoError) goto resume_error;
		break;
	}

	NextSegWriteOffset += SegNewLen;
	NextSegReadOffset += SegOldLen;
	NextSegEraseOffset += SegEraseSize;

	pSDFotaState_h->NextSegWriteOffset = NextSegWriteOffset;
	pSDFotaState_h->NextSegReadOffset = NextSegReadOffset;
	pSDFotaState_h->NextSegEraseOffset = NextSegEraseOffset;

seg_done:
	Retval = SDFotaState_Update(pTIM_h, SegIdx + 1, SDFOTA_NONE);
	if(Retval != NoError) goto resume_error;

resume_error:
	if(SegNewBuff) free(SegNewBuff);
	if(AddonBuff) free(AddonBuff);
	if(TailBuff) free(TailBuff);
	return Retval;
}

#if TRUSTED
UINT_T SDFotaValidateImageMxz(UINT_T FlashAddr, pBsdiffInfo pBsdiff_h,
						pTIM pTIM_h, PImageStruct_11 pImage_11)
{
	CHAR *SrcComBuff = NULL;
	CHAR *SegSrcBuff = NULL;
	CHAR *DestBuff = NULL;
	CHAR *nextInBuf = NULL;
	UINT_T SegIdx = 0;
	UINT_T SegDestLen = 0;
	UINT_T SegSrcLen = 0;
	UINT_T Compressed = 0;
	UINT_T SrcPT = 0;
	UINT_T DestPT = 0;
	UINT_T Retval = NoError;
	UINT_T ImageType;

	ImageType = IMAGE_TYPE(pImage_11->commands);

	if(ImageType != DLCMD_IMG_MXZ_IMAGE_TYPE) {
		return NoError;
	}

	SrcComBuff = malloc(pBsdiff_h->ImageSrcComLen);
	if(SrcComBuff == NULL)
		return HeapExhaustedError;

	Retval = ReadFlash(FlashAddr, SrcComBuff, pBsdiff_h->ImageSrcComLen, BOOT_FLASH);
	if(Retval != NoError)
	{
		free(SrcComBuff);
		return Retval;
	}

	DestBuff = malloc(pBsdiff_h->ImageDestLen);
	if(DestBuff == NULL)
		return HeapExhaustedError;

	while (SegIdx < pBsdiff_h->SegNum) {
		if (pBsdiff_h->SegInfo[SegIdx].DestComLen == 0 ) {
			continue;
		}

		if (pBsdiff_h->SegInfo[SegIdx].SrcComLen == 0 ) {
			SegSrcLen = 0;
			SegSrcBuff = NULL;
		} else {
			SegSrcLen = pBsdiff_h->SegInfo[SegIdx].SrcLen;
			SegSrcBuff = malloc(pBsdiff_h->SegInfo[SegIdx].SrcLen);
			Retval = xz_decompress(SrcComBuff + SrcPT, 
							pBsdiff_h->SegInfo[SegIdx].SrcComLen,
							SegSrcBuff, &SegSrcLen,
							&nextInBuf, &Compressed);
			if(Compressed != pBsdiff_h->SegInfo[SegIdx].Compression) {
				Retval = XZDecodeError; goto validate_error;
			}
		}

		SegDestLen = pBsdiff_h->SegInfo[SegIdx].DestLen;
		Retval = bzpacth(SegSrcBuff, SegSrcLen, DestBuff+DestPT, &SegDestLen,
				pBsdiff_h->SegInfo[SegIdx].Bsdiff40, 
				pBsdiff_h->SegInfo[SegIdx].PatchLen);
		if(Retval != 0 || SegDestLen == 0)
		{
			Retval = DfotaDecodeError; goto validate_error;
		}
		SrcPT += pBsdiff_h->SegInfo[SegIdx].SrcComLen;
		DestPT += SegDestLen;
		if(SegSrcBuff) {
			free(SegSrcBuff);
			SegSrcBuff = NULL;
		}
		SegIdx++;
	}

	Retval = Fota_ValidateImage(DestBuff, pImage_11, pTIM_h);
validate_error:
	if(SegSrcBuff) free(SegSrcBuff);
	free(SrcComBuff);
	free(DestBuff);
	return Retval;
}
#endif

INT_T ImageMxzProcess(pTIM pTIM_h, PImageStruct_11 pImage_11, pBsdiffInfo pBsdiff_h, P_FOTA_Firmware pFOTA_T)
{
	UINT_T Retval;
	UINT_T FlashAddr;
	UINT_T SegIdx;
	CHAR *SegOldBuffBkup = NULL;
	CHAR *SegOldBuff = NULL;
	CHAR *SegOldDecBuff = NULL;
	CHAR *SegNewBuff = NULL;
	CHAR *SegNewDecBuff = NULL;
	CHAR *AddonBuff = NULL;
	CHAR *nextInBuf = NULL;
	UINT_T AddonLen = 0;
	UINT_T SegOldLen;
	UINT_T SegOldDecLen;
	UINT_T SegNewDecLen;
	UINT_T ImageOldComLen;
	UINT_T Compressed;
	UINT_T SegNewLen;
	UINT_T XzopOptNum;
	UINT_T XzopOptBuff[16];
	UINT_T SegEraseSize;
	UINT_T NextSegWriteOffset = 0;
	UINT_T NextSegReadOffset = 0;
	UINT_T NextSegEraseOffset = 0;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	UINT_T PageSize = pFlashP->PageSize;
	INT_T i;
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);

	FlashAddr = pImage_11->Flash_Start_Address;

	Retval = Image_GetXZOP(pTIM_h, pImage_11->Image_ID, &XzopOptNum, XzopOptBuff);
	if(Retval != 0) {
		return XZNoOptionError;
	}

	sdfota_dbg("process mxz, Image@0x%x SegNum %d\n\r", 
		pImage_11->Flash_Start_Address, pBsdiff_h->SegNum);

	Retval = SDfota_PowerCutResumeSegImages(pTIM_h, pBsdiff_h, FlashAddr);
	if(Retval != NoError) {
		sdfota_info("sdfota power cut resume error\n\r");
		return SDFOTA_PowerOffResumeError;
	} else {
		if (pSDFotaState_h->SegIndex == pBsdiff_h->SegNum) {
			sdfota_dbg("sdfota power-cut resume the last segment, image done\n\r");
			return NoError;
		}
	}

	NextSegWriteOffset = pSDFotaState_h->NextSegWriteOffset;
	NextSegReadOffset = pSDFotaState_h->NextSegReadOffset;
	NextSegEraseOffset = pSDFotaState_h->NextSegEraseOffset;
	SegIdx = pSDFotaState_h->SegIndex;

#if TRUSTED
	if(pTIM_h->pConsTIM->VersionBind.Trusted
		&& SegIdx == 0 /* not a resume process */
		&& pSDFotaState_h->SegState == SDFOTA_NONE /* not a resume process */
		&& pImage_11->Image_In_TIM == DTIM_PRIMARY_INC) 
	{
		Retval = SDFotaValidateImageMxz(FlashAddr, pBsdiff_h, pTIM_h, pImage_11);
		if(Retval != NoError)
			return Retval;
	}
#endif

	sdfota_dbg("NextSegWriteOffset: 0x%x\n\r", NextSegWriteOffset);
	sdfota_dbg("NextSegReadOffset:  0x%x\n\r", NextSegReadOffset);
	sdfota_dbg("NextSegEraseOffset: 0x%x\n\r", NextSegEraseOffset);
	POWER_OFF_TEST_POINT

	ImageOldComLen = pBsdiff_h->ImageSrcComLen;
	SegOldBuff = SegOldBuffBkup = malloc(ImageOldComLen - NextSegReadOffset);
	if(SegOldBuffBkup == NULL) {
		Retval = HeapExhaustedError;
		goto error;
	}
	Retval = ReadSegment(FlashAddr, (UINT_T)SegOldBuff, ImageOldComLen - NextSegReadOffset, pTIM_h, pBsdiff_h);
	if(Retval != NoError) goto error;

	sdfota_dbg("Read leftover src: 0x%x@0x%x\n\r", ImageOldComLen - NextSegReadOffset,
		FlashAddr + NextSegReadOffset);

	while(SegIdx < pBsdiff_h->SegNum)
	{
		sdfota_dbg("------------------- Segment Start %d------------------\n\r", SegIdx);
		SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_NONE);

		if( pBsdiff_h->SegInfo[SegIdx].DestComLen  == 0 ) {
			SegOldLen = pBsdiff_h->SegInfo[SegIdx].SrcComLen;
			SegNewLen = 0;
			SegEraseSize = ALIGN_UP(NextSegWriteOffset + SegNewLen, BlockSize) - NextSegEraseOffset;

			if(pSDFotaState_h->PreTailLen) {
				CHAR *LastTailBuff = NULL;
				UINT_T LastTailAddr = ALIGN_DN(NextSegWriteOffset, BlockSize);
				LastTailBuff = malloc(pSDFotaState_h->PreTailLen);
				if(LastTailBuff == NULL)
					goto comp_img_error;
				Retval = ReadFlash(pSDFotaState_h->PreTailAddr, LastTailBuff, pSDFotaState_h->PreTailLen, BOOT_FLASH);
				if(Retval != NoError) {
					free(LastTailBuff); goto comp_img_error;
				}
				Retval = WriteFlash(LastTailAddr, LastTailBuff, pSDFotaState_h->PreTailLen, BOOT_FLASH);
				if(Retval != NoError) {
					free(LastTailBuff); goto comp_img_error;
				}

			}
			goto seg_done;
		}

		SegNewLen = pBsdiff_h->SegInfo[SegIdx].DestLen;
		SegNewBuff   = malloc(SegNewLen);
		if(SegNewBuff == NULL) {
			Retval = HeapExhaustedError; goto comp_img_error;
		}
		memset(SegNewBuff, 0xFF, SegNewLen);

		if (pBsdiff_h->SegInfo[SegIdx].Compression) {
			SegOldDecLen = SegOldLen = pBsdiff_h->SegInfo[SegIdx].SrcLen; /* rough  orignal len */
			SegNewDecLen = pBsdiff_h->SegInfo[SegIdx].DestLen;
			if(SegOldLen == 0)
				goto seg_no_src;

			SegOldDecBuff = malloc(SegOldDecLen);
			if(SegOldDecBuff == NULL) {
				Retval = HeapExhaustedError; goto comp_img_error;
			}
			sdfota_dbg("start to decompress segment %d\n\r", SegIdx);
			Retval = xz_decompress(SegOldBuff, SegOldLen,
							SegOldDecBuff, &SegOldDecLen,
							&nextInBuf, &Compressed);
			if (Retval) {
				sdfota_dbg("decompress error %d %d\n\r", Retval, Compressed);
				Retval = XZDecodeError; goto comp_img_error;
			}
			SegOldLen = nextInBuf - SegOldBuff; /* compressed old Len */

			sdfota_dbg("decompress done: %d, len: 0x%x->0x%x\n\r", 
				Compressed, SegOldLen, SegOldDecLen);

			if(Compressed != pBsdiff_h->SegInfo[SegIdx].Compression) {
				Retval = XZDecodeError; goto comp_img_error;
			}

	seg_no_src:
			sdfota_dbg("start to bzpatch\n\r");
			SegNewDecBuff = malloc(SegNewDecLen);
			if(SegNewDecBuff == NULL) {
				Retval = HeapExhaustedError; goto comp_img_error;
			}
			Retval = bzpacth(SegOldDecBuff, SegOldDecLen, SegNewDecBuff, &SegNewDecLen,
				pBsdiff_h->SegInfo[SegIdx].Bsdiff40, 
				pBsdiff_h->SegInfo[SegIdx].PatchLen);
			if(Retval != 0 || SegNewDecLen == 0)
			{
				Retval = DfotaDecodeError; goto comp_img_error;
			}

			sdfota_dbg("bspatch done: len: 0x%x\n\r", SegNewDecLen);

			sdfota_dbg("start to compress\n\r");
			Retval = xz_compress(SegNewDecBuff, SegNewDecLen, SegNewBuff, 
						&SegNewLen, XzopOptNum - 1, &XzopOptBuff[1]);
			if(Retval != 0) {
				sdfota_info("%s %d compress error: %d\n\r", __FUNCTION__, __LINE__, Retval);
				Retval = XZEncodeError;
				goto comp_img_error;
			}
			if(SegOldDecBuff ) { free(SegOldDecBuff); SegOldDecBuff = NULL; }
			if(SegNewDecBuff ) { free(SegNewDecBuff); SegNewDecBuff = NULL; }
			sdfota_dbg("compress done, Len: 0x%x\n\r", SegNewLen);

		} else {
			Compressed = 0;
			sdfota_dbg("Segment %d is not compressed\n\r", SegIdx);
			sdfota_dbg("start to bzpatch\n\r");
			SegOldLen = pBsdiff_h->SegInfo[SegIdx].SrcLen; /* rough  orignal len */
			Retval = bzpacth(SegOldBuff, SegOldLen, SegNewBuff, &SegNewLen,
				pBsdiff_h->SegInfo[SegIdx].Bsdiff40, 
				pBsdiff_h->SegInfo[SegIdx].PatchLen);
			if(Retval != 0 || SegNewLen == 0)
			{
				Retval = DfotaDecodeError; goto error;
			}
			sdfota_dbg("bspatch done: len: 0x%x\n\r", SegNewLen);
		}

		SegEraseSize = ALIGN_UP(NextSegWriteOffset + SegNewLen, BlockSize) - NextSegEraseOffset; //Align with Block size

		/* backup compressed dest segment */
		Retval = EraseFlash(pSDFotaState_h->SegDestBkAddr, SegNewLen, BOOT_FLASH);
		if(Retval != NoError) goto error;

		POWER_OFF_TEST_POINT

		Retval = WriteFlash(pSDFotaState_h->SegDestBkAddr, (UINT_T)SegNewBuff, SegNewLen, BOOT_FLASH);
		if(Retval != NoError) goto error;
		pSDFotaState_h->SegDestBkLen = SegNewLen;
		Retval = SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_DEST_SAVED);
		if(Retval != NoError) goto error;

		POWER_OFF_TEST_POINT
		/* It'll cause case #89717 third issue if power-cut here for ZIMG last segment */

		sdfota_dbg("SegOldLen:          0x%x\n\r", SegOldLen);
		sdfota_dbg("SegNewLen:          0x%x\n\r", SegNewLen);
		sdfota_dbg("SegEraseSize:       0x%x\n\r", SegEraseSize);

		if(SegIdx < (pBsdiff_h->SegNum - 1) /* not last segment */) {
			/* backup head addon of next segment */
			if((NextSegEraseOffset + SegEraseSize) > (NextSegReadOffset + SegOldLen)) {
				AddonLen = NextSegEraseOffset + SegEraseSize - NextSegReadOffset - SegOldLen;
				sdfota_dbg("Next segment head addon len: 0x%x\n\r", AddonLen);
				Retval = EraseFlash(pSDFotaState_h->NextHeadAddr, AddonLen, BOOT_FLASH);
				if(Retval != NoError) goto error;
				AddonBuff = malloc(AddonLen);
				if(AddonBuff == NULL) {
					Retval = HeapExhaustedError;
					goto error;
				}
				memcpy(AddonBuff, SegOldBuff + SegOldLen, AddonLen);
				Retval = WriteFlash(pSDFotaState_h->NextHeadAddr, (UINT_T)AddonBuff, AddonLen, BOOT_FLASH);
				if(Retval != NoError) goto error;
				free(AddonBuff); AddonBuff = NULL;
				sdfota_dbg("Backup Head addon for Seg%d: 0x%x@0x%x\n\r", SegIdx+1, AddonLen, 
							FlashAddr+NextSegReadOffset+SegOldLen);

				pSDFotaState_h->NextHeadLen = AddonLen;
				Retval = SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_HEAD_SAVED);
				if(Retval != NoError) goto error;
			}
		}

		/* After Head of next seg is backed up, can erase for current segment */
		Retval = EraseFlash(FlashAddr + NextSegEraseOffset, SegEraseSize, BOOT_FLASH);
		if(Retval != NoError) goto error;
		POWER_OFF_TEST_POINT
		Retval = WriteSegment(FlashAddr+NextSegWriteOffset, (UINT_T)SegNewBuff, SegNewLen, pTIM_h, pBsdiff_h);
		if(Retval != NoError) goto error;

	seg_done:
		NextSegWriteOffset += SegNewLen;
		NextSegReadOffset += SegOldLen;
		NextSegEraseOffset += SegEraseSize;
		SegOldBuff += SegOldLen;

		pSDFotaState_h->NextSegWriteOffset = NextSegWriteOffset;
		pSDFotaState_h->NextSegReadOffset = NextSegReadOffset;
		pSDFotaState_h->NextSegEraseOffset = NextSegEraseOffset;
		Retval = SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_SEG_DONE);
		if(Retval != NoError) goto error;
		POWER_OFF_TEST_POINT

		sdfota_dbg("NextSegWriteOffset: 0x%x\n\r", NextSegWriteOffset);
		sdfota_dbg("NextSegReadOffset:  0x%x\n\r", NextSegReadOffset);
		sdfota_dbg("NextSegEraseOffset: 0x%x\n\r", NextSegEraseOffset);

		if(SegNewBuff ) { free(SegNewBuff); SegNewBuff = NULL; }

		sdfota_dbg("------------------- Seg End------------------\n\r\n\r");
		SegIdx ++;
	}

	Retval = SDFotaState_Update(pTIM_h, SegIdx, SDFOTA_IMG_DONE);
comp_img_error:
	if(SegNewDecBuff) (SegNewDecBuff);
	if(SegOldDecBuff) free(SegOldDecBuff);
error:
	if(SegOldBuffBkup) free(SegOldBuffBkup);
	if(AddonBuff) free(AddonBuff);
	if(SegNewBuff) free(SegNewBuff);
	return Retval;
}

INT_T ImageSquashProcess(pTIM pTIM_h, PImageStruct_11 pImage_11, pBsdiffInfo pBsdiff_h, P_FOTA_Firmware pFOTA_T)
{
	UINT_T Retval;
	UINT_T FlashAddr;
	UINT_T OldSizeInPatch;
	UINT_T NewSizeInPatch;
	UINT_T OldComLen;
	UINT_T NewComLen;
	UINT_T OldLen;
	UINT_T NewLen;
	UINT_T PatchLen;
	CHAR* OldComBuff = NULL;
	CHAR* NewComBuff = NULL;
	CHAR* OldBuff = NULL;
	CHAR* NewBuff = NULL;
	CHAR* patch;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	UINT_T NumBlocks = pFlashP->NumBlocks;
	UINT_T FlashSize = BlockSize * NumBlocks;
	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);

	patch = pBsdiff_h->Patch;
	PatchLen = pBsdiff_h->PatchLen;

	FlashAddr = pImage_11->Flash_Start_Address;

	if(pSDFotaState_h->CurImageID != pImage_11->Image_ID) {
		sdfota_info("SDFOTA state mismatch with current Image\n\r");
		Retval = SDFOTA_StateMismatchError;
		goto squash_error;
	}

	sdfota_dbg("start to process squash fs\n\r");

	if (pSDFotaState_h->ImageState == SDFOTA_IMG_DONE){
		sdfota_dbg("power-cut resume from SDFOTA_IMG_DONE\n\r");
		return NoError;
	}

	if((pSDFotaState_h->ImageState == SDFOTA_IMG_SAVE) && pSDFotaState_h->ImageBkLen)
	{
		NewComLen = ALIGN_UP(pSDFotaState_h->ImageBkLen, BlockSize);
		NewComBuff = malloc(NewComLen);
		memset(NewComBuff, 0xFF, NewComLen);
		NewComLen = pSDFotaState_h->ImageBkLen;
		Retval = ReadFlash(pSDFotaState_h->ImageBkAddr, (UINT_T)NewComBuff, NewComLen, BOOT_FLASH);
		if(Retval != NoError) goto squash_error;
		goto resume_write_squash;
	}

	NewComLen = OldComLen = SQUASHFS_MAX_SIZE;
	OldLen = OldSizeInPatch = get_oldsize(patch);
	NewLen = NewSizeInPatch = get_newsize(patch);

	OldComBuff = malloc(OldComLen);
	if(OldComBuff == NULL) {
		Retval = HeapExhaustedError;
		goto squash_error;
	}

	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_NONE);
	if(Retval != NoError) goto squash_error;

	sdfota_dbg("read squash, 0x%x@0x%x\n\r", OldComLen, FlashAddr);
	Retval = ReadFlash(FlashAddr, (UINT_T)OldComBuff, OldComLen, BOOT_FLASH);
	if(Retval != NoError) goto squash_error;

	POWER_OFF_TEST_POINT

	OldLen = OldComLen + OldSizeInPatch;
	OldBuff = malloc(OldLen);
	if(OldBuff == NULL) {
		Retval = HeapExhaustedError;
		goto squash_error;
	}

	sdfota_dbg("decompress squash fs\n\r");
	Retval = resquash(OldComBuff, OldComLen, OldBuff, &OldLen);
	if( Retval != 0 ) {
		Retval = SDFOTA_ResquashDecompressError;
		goto squash_error;
	}
	sdfota_dbg("decompress done 0x%x\n\r", OldLen);

	NewBuff = malloc(NewSizeInPatch);
	if(NewBuff == NULL) {
		Retval = HeapExhaustedError;
		goto squash_error;
	}

	sdfota_dbg("start to bzpacth\n\r");
	Retval = bzpacth(OldBuff, OldSizeInPatch, NewBuff, &NewLen,
				patch, PatchLen);

	if(Retval != 0 || NewLen == 0)
	{
		Retval = DfotaDecodeError; goto squash_error;
	}
	sdfota_dbg("bzpacth done, NewLen: 0x%x\n\r", NewLen);

	NewComBuff = malloc(NewComLen + 0x100000); /* extra 0x100000 is for resquash to use as temp buffer */
	if(NewComBuff == NULL) {
		Retval = HeapExhaustedError;
		goto squash_error;
	}

	sdfota_dbg("compress squash fs\n\r");
	Retval =  resquash(NewBuff, NewLen, NewComBuff, &NewComLen);
	if( Retval != 0 ) {
		Retval = SDFOTA_ResquashDecompressError;
		goto squash_error;
	}

#if TRUSTED
	if (pTIM_h->pConsTIM->VersionBind.Trusted) {
		Retval = Fota_ValidateImage(NewComBuff, pImage_11, pTIM_h);
		if(Retval != NoError)
			goto squash_error;
	}
#endif
	sdfota_dbg("compress done, 0x%x\n\r", NewComLen);

	memset(NewComBuff + NewComLen, 0xFF, BlockSize); /* clean padding */

	if((pSDFotaState_h->ImageBkAddr + NewComLen) > FlashSize) {
		Retval = DFOTA_PartitionTooSmall; goto squash_error;
	}

	Retval = EraseFlash(pSDFotaState_h->ImageBkAddr, NewComLen, BOOT_FLASH);
	if(Retval != NoError) goto squash_error;
	Retval = WriteFlash(pSDFotaState_h->ImageBkAddr, (UINT_T)NewComBuff, NewComLen, BOOT_FLASH);
	if(Retval != NoError) goto squash_error;

	POWER_OFF_TEST_POINT

	pSDFotaState_h->ImageBkLen = NewComLen;
	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_IMG_SAVE);
	if(Retval != NoError) goto squash_error;

	POWER_OFF_TEST_POINT

resume_write_squash:
	sdfota_dbg("write new squash fs\n\r");
	Retval = EraseFlash(FlashAddr, NewComLen, BOOT_FLASH);
	if(Retval != NoError) goto squash_error;
	Retval = WriteFlash(FlashAddr, (UINT_T)NewComBuff, NewComLen, BOOT_FLASH);
	if(Retval != NoError) goto squash_error;
	sdfota_dbg("write done\n\r");
	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_IMG_DONE);
	/* powercut here will cause case #89717 second issue with: "FOTA return: 0x704" */

squash_error:
	if(OldComBuff) free(OldComBuff);
	if(NewComBuff) free(NewComBuff);
	if(OldBuff) free(OldBuff);
	if(NewBuff) free(NewBuff);

	return Retval;
}

INT_T ImageRawProcess(pTIM pTIM_h, PImageStruct_11 pImage_11, pBsdiffInfo pBsdiff_h, P_FOTA_Firmware pFOTA_T)
{
	UINT_T Retval = NoError;
	UINT_T OldLen;
	UINT_T NewLen;
	UINT_T FlashAddr;
	UINT_T EraseSize;
	CHAR *OldBuff;
	CHAR *NewBuff;
	CHAR* patch;
	UINT_T PatchLen;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	UINT_T NumBlocks = pFlashP->NumBlocks;
	UINT_T FlashSize = BlockSize * NumBlocks;

	pSDfotaState pSDFotaState_h = &(pFOTA_T->SDfotaInfo);

	patch = pBsdiff_h->Patch;
	PatchLen = pBsdiff_h->PatchLen;

	FlashAddr = pImage_11->Flash_Start_Address;

	if(pSDFotaState_h->CurImageID != pImage_11->Image_ID) {
		sdfota_info("SDFOTA state mismatch with current Image\n\r");
		Retval = SDFOTA_StateMismatchError;
		goto raw_error;
	}

	if( TIM_TYPE == (pImage_11->Image_ID & 0xFFFFFF00) ) {
		NewLen = ALIGN_UP(PatchLen, BlockSize);
		NewBuff = malloc(NewLen);
		memset(NewBuff, 0xFF, NewLen);
		memcpy(NewBuff, patch, PatchLen);
		sdfota_dbg("Upgrade TIMs, write directly\n\r");
		goto resume_write_raw;
	}

	POWER_OFF_TEST_POINT

	sdfota_dbg("start to process raw image\n\r");

	if (pSDFotaState_h->ImageState == SDFOTA_IMG_DONE){
		sdfota_dbg("power-cut resume from SDFOTA_IMG_DONE\n\r");
		return NoError;
	}

	if((pSDFotaState_h->ImageState == SDFOTA_IMG_SAVE) && pSDFotaState_h->ImageBkLen)
	{
		sdfota_dbg("power-cut resume from SDFOTA_IMG_SAVE\n\r");
		NewLen = ALIGN_UP(pSDFotaState_h->ImageBkLen, BlockSize);
		NewBuff = malloc(NewLen);
		memset(NewBuff, 0xFF, NewLen);
		NewLen = pSDFotaState_h->ImageBkLen;
		Retval = ReadFlash(pSDFotaState_h->ImageBkAddr, (UINT_T)NewBuff, NewLen, BOOT_FLASH);
		if(Retval != NoError) goto raw_error;
		goto resume_write_raw;
	}

	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_NONE);
	if(Retval != NoError) goto raw_error;

	OldLen = get_oldsize(patch);
	OldLen = ALIGN_UP(OldLen, BlockSize);
	OldBuff = malloc(OldLen);
	if(OldBuff == NULL) {
		Retval = HeapExhaustedError;
		goto raw_error;
	}

	sdfota_dbg("read raw image, 0x%x@0x%x\n\r", OldLen, FlashAddr);
	Retval = ReadFlash(FlashAddr, (UINT_T)OldBuff, OldLen, BOOT_FLASH);
	if(Retval != NoError) goto raw_error;

	NewLen = get_newsize(patch);
	NewLen = ALIGN_UP(NewLen, BlockSize);
	NewBuff = malloc(NewLen);
	if(NewBuff == NULL) {
		Retval = HeapExhaustedError;
		goto raw_error;
	}
	memset(NewBuff, 0xFF, NewLen);

	sdfota_dbg("start to bzpatch\n\r");
	Retval = bzpacth(OldBuff, OldLen, NewBuff, &NewLen,
				patch, PatchLen);
	if(Retval != 0 || NewLen == 0)
	{
		Retval = DfotaDecodeError; goto raw_error;
	}

	sdfota_dbg("bzpatch done\n\r");

#if TRUSTED
	if (pTIM_h->pConsTIM->VersionBind.Trusted) {
		Retval = Fota_ValidateImage(NewBuff, pImage_11, pTIM_h);
		if(Retval != NoError)
			goto raw_error;
	}
#endif

	if((pSDFotaState_h->ImageBkAddr + NewLen) > FlashSize) {
		Retval = DFOTA_PartitionTooSmall; goto raw_error;
	}

	sdfota_dbg("start to back up\n\r");
	Retval = EraseFlash(pSDFotaState_h->ImageBkAddr, NewLen, BOOT_FLASH);
	if(Retval != NoError) goto raw_error;
	POWER_OFF_TEST_POINT
	Retval = WriteFlash(pSDFotaState_h->ImageBkAddr, (UINT_T)NewBuff, NewLen, BOOT_FLASH);
	if(Retval != NoError) goto raw_error;
	pSDFotaState_h->ImageBkLen = NewLen;
	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_IMG_SAVE);
	if(Retval != NoError) goto raw_error;
	sdfota_dbg("back up done\n\r");

	POWER_OFF_TEST_POINT

resume_write_raw:
	sdfota_dbg("write new raw image\n\r");
	EraseSize = NewLen;
#if DUAL_TIM
	if(pImage_11->Image_ID == OBMIDENTIFIER && DualTimEnabled()) {
		if(!DualTimIsEqual())
			EraseSize += MainTimSize;
	}
#endif
	EraseSize = ALIGN_UP(EraseSize, BlockSize);
	Retval = EraseFlash(FlashAddr, EraseSize, BOOT_FLASH);
	if(Retval != NoError) goto raw_error;
	POWER_OFF_TEST_POINT

#if DUAL_TIM
	/* Dual TIM needs to write the other TIM after OBM */
	if(pImage_11->Image_ID == OBMIDENTIFIER && DualTimEnabled()) {
		if(!DualTimIsEqual()) {
			UINT8_T *ObmTimBuff = NULL;
			ObmTimBuff = malloc(EraseSize);
			if(ObmTimBuff == NULL) {
				Retval = HeapExhaustedError;
				goto raw_error;
			}
			if(pImage_11->Image_In_TIM == TIMH_RECOVERY_INC) {
				memcpy(ObmTimBuff, NewBuff, NewLen);
				if(pTIM_h_Main)
					memcpy(ObmTimBuff+NewLen, pTIM_h_Main->pConsTIM, MainTimSize);
				Retval = WriteFlash(FlashAddr, ObmTimBuff, NewLen + MainTimSize, BOOT_FLASH);
			} else {
				memcpy(ObmTimBuff, NewBuff, NewLen);
				if(pTIM_h_Backup)
					memcpy(ObmTimBuff+NewLen, pTIM_h_Backup->pConsTIM, MainTimSize);
				Retval = WriteFlash(FlashAddr, ObmTimBuff, NewLen + BackupTimSize, BOOT_FLASH);
			}
			free(ObmTimBuff);
		} else {
			Retval = WriteFlash(FlashAddr, NewBuff, NewLen, BOOT_FLASH);
		}
		goto write_done;
	}
#endif

	if(pImage_11->Image_ID == TIMIDENTIFIER) { /* write TIMH */
		#if DUAL_TIM
		if(pImage_11->Image_In_TIM == TIMH_RECOVERY_INC && DualTimEnabled() ) {
			if(!DualTimIsEqual()) {
				if(pTIM_h_Backup == NULL) {
					Retval = TIMNotFound;
					goto raw_error;
				}
				Retval = WriteFlash( FlashAddr, pTIM_h_Backup->pConsTIM, BackupTimSize, BOOT_FLASH );
			}else{
				if(pTIM_h_Main == NULL) {
					Retval = TIMNotFound;
					goto raw_error;
				}
				Retval = WriteFlash( FlashAddr, pTIM_h_Main->pConsTIM, MainTimSize, BOOT_FLASH );
			}
		}
		else
		#endif
		{
			if(pTIM_h_Main == NULL) {
				Retval = TIMNotFound;
				goto raw_error;
			}
			Retval = WriteFlash( FlashAddr, pTIM_h_Main->pConsTIM, MainTimSize, BOOT_FLASH );
		}
		goto write_done;
	}

	Retval = WriteFlash(FlashAddr, (UINT_T)NewBuff, NewLen, BOOT_FLASH);

write_done:
	if(Retval != NoError) goto raw_error;
	sdfota_dbg("write done\n\r");
	Retval = SDFotaState_Update(pTIM_h, -1, SDFOTA_IMG_DONE);

raw_error:
	if(OldBuff) { free(OldBuff); OldBuff = NULL; }
	if(NewBuff) { free(NewBuff); NewBuff = NULL; }
	return Retval;
}

UINT_T SDFOTA_Upgrade(P_FOTA_Firmware pFOTA_T, pTIM pTIM_h)
{
	UINT_T Retval = NoError;
	UINT_T LoadAddr = 0;
	UINT_T ImageAddr;
	MasterBlockHeader *pMasterHeader = NULL;
	PDeviceHeader_11 pDevHeader_11=NULL;
	PImageStruct_11 pImage_11 = NULL;
	UINT_T i = 0, skip_blocks, decompressLength, n = 0;
	UINT_T imageType;
	UINT_T temp_p = NULL;
	INT_T imagenum;
	P_FlashProperties_T pFlashP = GetFlashProperties(BOOT_FLASH);
	UINT_T BlockSize = pFlashP->BlockSize;
	P_FOTA_Info pFOTAInfo = &FOTAInfo;
	pSDfotaState pSDFotaState_h = &pFOTA_T->SDfotaInfo;
	BsdiffInfo BsdiffInfo;
	pBsdiffInfo pBsdiffInfo_h = &BsdiffInfo;
#if SPI_LCD
	char str[128];
#endif

	LoadAddr = (UINT_T)malloc(ALIGN_UP(pFOTA_T->FBF_Size, BlockSize));
	if(LoadAddr == 0){
		return HeapExhaustedError;
	}

	Retval = ReadFlash(pFOTA_T->FBF_Flash_Address, LoadAddr, pFOTA_T->FBF_Size, BOOT_FLASH);
	if (Retval != NoError){
		goto error;
	}

	pMasterHeader = (MasterBlockHeader *)LoadAddr;
	temp_p = pMasterHeader->deviceHeaderOffset[0] + LoadAddr;
	pDevHeader_11 = (PDeviceHeader_11)temp_p;

	Retval = Fota_CommonPreprocess(pTIM_h, pMasterHeader, pFOTA_T, 4);
	if(Retval != NoError) {
		free((VOID *)LoadAddr);
		return Retval;
	}

	if(pFOTA_T->DFota_nOfImages==0xFFFFFFFF){
		pFOTA_T->DFota_nOfImages= pDevHeader_11->nOfImages;
		memset(&(pFOTA_T->SDfotaInfo), 0, sizeof(SDfotaState));
	}

	sdfota_dbg("sdfota start image index %d\n\r", pFOTA_T->DFota_nOfImages);

#if SPI_LCD
	for ( imagenum = pFOTA_T->DFota_nOfImages; imagenum > 0; imagenum -- )
	{
		pImage_11 = (PImageStruct_11)&pDevHeader_11->imageStruct_11[imagenum - 1];
		if ((pImage_11->Image_ID == RSAIIMAGEID) ||
				(pImage_11->Image_ID == MD5IIMAGEID) ||
				(pImage_11->Image_ID == FOTA_FBFVERSION_IMAGEID))
				n++;
	}
	n = pFOTA_T->DFota_nOfImages - n;
#endif

	for ( imagenum = pFOTA_T->DFota_nOfImages; imagenum > 0; imagenum -- )
	{
		temp_p = (UINT_T)&pDevHeader_11->imageStruct_11[imagenum - 1];
		pImage_11 = (PImageStruct_11)temp_p;
		ImageAddr = LoadAddr+(pImage_11->First_Sector<<13);

		sdfota_info("Upgrade Image: %s\n\r", ImageID2String(pImage_11->Image_ID));
		sdfota_dbg("ImageID: 0x%x, Patch/Image Address: 0x%x\n\r", pImage_11->Image_ID, ImageAddr);

		Retval = Fota_ImagePreprocess(ImageAddr, pImage_11, 4);
		if(Retval == FOTA_DummyImage) {
			Retval = NoError; /* Dummy Image, do nothing and continue */
			continue;
		} else if(Retval != NoError){
			goto error;
		}

#if SPI_LCD
		memset(str, 0, sizeof(str));
		i++;
		sprintf(str, "Update %d/%d %s.", i, n, ImageID2String(pImage_11->Image_ID));
		lcd_display_string(0, 56, str);
#endif

		Retval = DFota_ParseBsdiffPatch((CHAR *)ImageAddr, pImage_11->length, pBsdiffInfo_h);
		if(Retval != NoError) goto error;

		Retval = SDFotaState_Init(pTIM_h, pImage_11, pBsdiffInfo_h, pFOTA_T);
		if(Retval != NoError) goto error;
		imageType = IMAGE_TYPE(pImage_11->commands);
		switch(imageType) {
		case DLCMD_SQUASHFS_IMAGE_TYPE:
			sdfota_dbg("sdfota for squash fs\n\r");
			Retval = ImageSquashProcess(pTIM_h, pImage_11, pBsdiffInfo_h, pFOTA_T);
			if(Retval != NoError) goto error;
			break;
		case DLCMD_IMG_MXZ_IMAGE_TYPE:
		case DLCMD_MULTI_XZ_IMAGE_TYPE:
			sdfota_dbg("sdfota for mxz images\n\r");
			Retval = ImageMxzProcess(pTIM_h, pImage_11, pBsdiffInfo_h, pFOTA_T);
			if(Retval != NoError) goto error;
			break;
		default:
			sdfota_dbg("sdfota for raw images\n\r");
			Retval = ImageRawProcess(pTIM_h, pImage_11, pBsdiffInfo_h, pFOTA_T);
			if(Retval != NoError) goto error;
			break;
		}

		//power-cut protection
		/* clear current image state and go to next */
		pFOTA_T->DFota_nOfImages = imagenum - 1;
		memset(&(pFOTA_T->SDfotaInfo), 0, sizeof(SDfotaState));
		Retval = OTA_Save_Config(pTIM_h);
		if(Retval != NoError) goto error;
	}  //for loop

	Fota_CommonPostprocess(pTIM_h, pMasterHeader, pFOTA_T, 4);

error:
	//free buffers
	if(LoadAddr) free((void *)LoadAddr);

	return Retval;
}