}
	return TRUE;

BadBlockReturn:
	//NandDbgPutString ("Bad blk: ");
	//NandDbgPutHex (dwBlockIndex);
	//NandDbgPutString ("\r\n");
	FMD_SetBlockStatus(dwBlockIndex, BLOCK_STATUS_BAD);
	return FALSE;
}

BOOL FMD_SetBlockStatus(DWORD blockID, DWORD dwStatus)
{
	BOOL bRet = TRUE;
	SectorInfo SI;
	DWORD Sector = BOOT_BLOCK_TO_SECTOR (blockID);

	if (!FMDReadSectorInfo (Sector, &SI))
	{
		bRet = FALSE;
		goto FMD_SetBlockStatus_Exit;
	}

	if (dwStatus & BLOCK_STATUS_BAD)
	{
		SI.bBadBlock = 0;
	}

	if (dwStatus & BLOCK_STATUS_READONLY)
	{
		SI.bOEMReserved &= ~OEM_BLOCK_READONLY;
	}

	if (dwStatus & BLOCK_STATUS_RESERVED)
	{
		SI.bOEMReserved &= ~OEM_BLOCK_RESERVED;
	}

	if (!FMDWriteSectorInfo (Sector, &SI))
	{
		bRet = FALSE;
	}

FMD_SetBlockStatus_Exit:
//	if (!bRet)
//	{
//		NandDbgPutString("\r\nSetBlockStatus err: ");
//		NandDbgPutHex(blockID);
//	}
	return bRet;
}

#ifndef BUILDING_NBOOT_ICE
/*
    @func   DWORD | FMD_GetBlockStatus | Returns the status of the specified block.
    @rdesc  Block status (see fmd.h).
    @comm    
    @xref   
*/
DWORD FMD_GetBlockStatus(DWORD blockID)
{
    SectorInfo SI;
	SECTOR_ADDR Sector = BOOT_BLOCK_TO_SECTOR (blockID);
	DWORD dwResult = 0;

	if (!FMDReadSectorInfo(Sector, &SI)) 
		return BLOCK_STATUS_UNKNOWN;

	if (!(SI.bOEMReserved & OEM_BLOCK_READONLY))  
		dwResult |= BLOCK_STATUS_READONLY;

	if (!(SI.bOEMReserved & OEM_BLOCK_RESERVED))  
		dwResult |= BLOCK_STATUS_RESERVED;

	if(SI.bBadBlock != 0xFF)
		dwResult |= BLOCK_STATUS_BAD;

	return(dwResult);
}

BOOL NandWriteCurImg()
{
	BOOL bRet = TRUE;

	if(g_nCurImg == ATLAS_IMAGE_WINDOWS_CE_NBOOT)
	{
		if(!WriteImage(NBOOT_BLOCK, 1, (PBYTE)SDRAM_IMG_CACHE_START/*, &g_dwNBootBlock, NULL*/))
		{
			bRet = FALSE;
		}
	}
	else if(g_nCurImg == ATLAS_IMAGE_WINDOWS_CE_EBOOT)
	{
		if(!WriteImage(g_dwEBootBlock, FILE_TO_BLOCK_SIZE(g_iCurImgLength), (PBYTE)SDRAM_IMG_CACHE_START/*, &g_dwEBootBlock, NULL*/))
		{
			bRet = FALSE;
		}
	}
	else if(g_nCurImg == ATLAS_IMAGE_WINDOWS_CE_DM)
	{
		if(!WriteImage(g_dwDMBlock, FILE_TO_BLOCK_SIZE(g_iCurImgLength), (PBYTE)SDRAM_IMG_CACHE_START/*, &g_dwDMBlock, NULL*/))
		{
			bRet = FALSE;
		}
	}

	if (bRet)
		NandDbgPutString("\r\nSucceed");

	return bRet;
}

BOOL NandWriteToc (void)
{
	IMAGE_DESCRIPTOR*	pid;

	if (!IS_VALID_TOC (g_pTOC))
	{
		memset ((void *)g_pTOC, 0, sizeof(TOC));
		g_pTOC->dwSignature			= TOC_SIGNATURE;
		g_pTOC->dwVersion			= NBOOT_VERSION;
		g_pTOC->dwMaxBadBlocks		= MAX_BAD_BLOCKS;
		g_pTOC->blockNo				= g_dwTocBlock;

		g_pTOC->BootCfg.ImageIndex	= TOC_NK_ENTRY;
		g_pTOC->BootCfg.ConfigFlags	= BOOT_TYPE_MULTISTAGE | TARGET_TYPE_NAND | CONFIG_FLAGS_DHCP | CONFIG_FLAGS_DEBUGGER;
		g_pTOC->BootCfg.BootDelay	= CONFIG_BOOTDELAY_DEFAULT;

		// TOC entry for eboot.nb0
		pid = &g_pTOC->id[TOC_EBOOT_ENTRY];
		pid->dwVersion				= 0x0100;
		pid->dwSignature			= IMAGE_EBOOT_SIG;
		pid->dwImageType			= IMAGE_TYPE_RAMIMAGE;
		pid->dwLoadAddress			= g_dwEbootLoadAddr;
		pid->dwJumpAddress			= g_dwEbootLaunchAddr;
		pid->dwStartBlock			= g_dwEBootBlock;
		pid->sgList[0].dwSector		= BOOT_BLOCK_TO_SECTOR (g_dwEBootBlock);
		pid->sgList[0].dwLength		= BOOT_BLOCK_TO_SECTOR (g_dwEbootBlockSize);
		pid->dwTtlSectors			= BOOT_BLOCK_TO_SECTOR (g_dwEbootBlockSize);
		strncpy ((char *)pid->ucString, "eboot.nb0", IMAGE_STRING_LEN - 1);

		//  TOC entry for nk.nb0
		pid							= &g_pTOC->id[TOC_NK_ENTRY];
		pid->dwVersion				= 0x0001;
		pid->dwSignature			= IMAGE_RAM_SIG;
		pid->dwImageType			= IMAGE_TYPE_RAMIMAGE | IMAGE_TYPE_BINFS | IMAGE_TYPE_MXIP;
		pid->dwStartBlock			= g_dwNKBlock;
		pid->sgList[0].dwSector		= BOOT_BLOCK_TO_SECTOR (g_dwNKBlock);
		strncpy ((char *)pid->ucString, "PPC2K3.NB0", IMAGE_STRING_LEN - 1);

		//  TOC entry for DM
		if (g_dwDMBlockSize > 0)
		{
			pid						= &g_pTOC->id[TOC_DM_ENTRY];
			pid->dwVersion			= 0x0001;
			pid->dwSignature		= IMAGE_DM_SIG;
			pid->dwImageType		= IMAGE_TYPE_RAMIMAGE;
			pid->dwLoadAddress		= g_dwDMLoadAddr;
			pid->dwJumpAddress		= g_dwDMLaunchAddr;
			pid->dwStartBlock		= g_dwDMBlock;
			pid->sgList[0].dwSector = BOOT_BLOCK_TO_SECTOR (g_dwDMBlock);
			pid->sgList[0].dwLength = BOOT_BLOCK_TO_SECTOR (g_dwDMBlockSize);
			pid->dwTtlSectors		= BOOT_BLOCK_TO_SECTOR (g_dwDMBlockSize);
			strncpy ((char *)pid->ucString, "DM", IMAGE_STRING_LEN - 1);
		}

        g_pTOC->secureBlock.dwStartBlock = g_dwSecureBlock;
        g_pTOC->secureBlock.dwNumOfPages = BOOT_BLOCK_TO_SECTOR( g_dwSecureBlockSize );
	}

	if (!FMD_EraseBlock (g_dwTocBlock))
	{
		return FALSE;
	}

	if (!FMD_WriteSector (BOOT_BLOCK_TO_SECTOR (g_dwTocBlock), (PBYTE)g_pTOC, &g_SI, 1)) 
	{
//		NandDbgPutString ("\r\nTOC failed");
		return FALSE;
	}

	NandDbgPutString ("\r\nTOC updated");
	return TRUE;
}

BOOL NandLocateBlocks (void)
{
	//TOC should be the searched from physical block 1. The first block that is not really bad is TOC
	DWORD dwBlock = 1, dwStart = 1;

	for (; dwBlock < SEARCH_BLOCKS + dwStart; ++dwBlock)
	{
		if (NandIsOEMReservedBlock (dwBlock))
		{
			break;
		}
	}

	if (dwBlock >= SEARCH_BLOCKS + dwStart)
		return FALSE;

	g_dwTocBlock = dwBlock;

	if (!FMD_ReadSector (BOOT_BLOCK_TO_SECTOR (g_dwTocBlock), (LPBYTE)g_pTOC, NULL, 1))
	{
		return FALSE;
	}

	g_dwEBootBlock		= g_pTOC->id[TOC_EBOOT_ENTRY].dwStartBlock;
	g_dwNKBlock			= g_pTOC->id[TOC_NK_ENTRY].dwStartBlock;
	g_dwDMBlock			= g_pTOC->id[TOC_DM_ENTRY].dwStartBlock;	//leon01 add for DM entry

	return TRUE;
}

//Erase all but the NBoot block
BOOL NandEraseAll()
{
    BOOL bRet = TRUE;
    DWORD dwBlockNo;

    for (dwBlockNo = 1; dwBlockNo < g_FlashExtInfo.fi.dwNumBlocks; ++dwBlockNo)
    {
        if (!FMD_EraseBlock (dwBlockNo))
            bRet = FALSE;
	}

    return bRet;
}

#if 0
void TOC_Entry_Print()
{
	NandDbgPutString("\r\nCurrent Entry in TOC: ");
	NandDbgPutString("\r\n Load Address: ");
	NandDbgPutHex(g_pTOC->id[g_dwEntry].dwLoadAddress);
	NandDbgPutString("\r\n Jump Address: ");
	NandDbgPutHex(g_pTOC->id[g_dwEntry].dwJumpAddress);
	NandDbgPutString("\r\n Ttl Sectors: ");
	NandDbgPutHex(g_pTOC->id[g_dwEntry].dwTtlSectors);
	NandDbgPutString("\r\n Ttl Sectors: ");
	NandDbgPutHex(g_pTOC->id[g_dwEntry].dwTtlSectors);
	NandDbgPutString("\r\n Chain in TOC: ");
	NandDbgPutString("\r\n Chain Load Addr: ");
	NandDbgPutHex(g_pTOC->chainInfo.dwLoadAddress);
}
#endif

BOOL NandReadRegion(DWORD dwStartSec, DWORD dwNumSectors, DWORD dwRAM)
{
	DWORD dwSector = dwStartSec;
	
	// read each sg segment
    while (dwNumSectors) 
	{
		//nk image use BINFS, which may has bad blocks
		if(dwSector % g_FlashExtInfo.fi.wSectorsPerBlock == 0)
		{
			if((g_dwEntry == TOC_NK_ENTRY) && (g_pTOC->id[TOC_NK_ENTRY].dwImageType & IMAGE_TYPE_MXIP))
			{
				if(IS_BLOCK_UNUSABLE(BOOT_SECTOR_TO_BLOCK(dwSector)))
				{
					dwSector += g_FlashExtInfo.fi.wSectorsPerBlock;
					if(dwSector >= BOOT_BLOCK_TO_SECTOR (g_FlashExtInfo.fi.dwNumBlocks))
						return FALSE;
					continue;
				}
			}
			else
			{
				if( !NandIsOEMReservedBlock(BOOT_SECTOR_TO_BLOCK(dwSector)))
				{
					dwSector += g_FlashExtInfo.fi.wSectorsPerBlock;
//					dwNumSectors -= g_FlashExtInfo.fi.wSectorsPerBlock;
					if(dwSector >= BOOT_BLOCK_TO_SECTOR (g_FlashExtInfo.fi.dwNumBlocks))
						return FALSE;
					continue;
				}
			}
		}
		
        if ( !FMD_ReadSector(dwSector,
                            (LPBYTE)dwRAM,
                            NULL, 1) )
        {
        	return FALSE;
        }

    	dwSector++;
    	dwNumSectors--;
        dwRAM += g_FlashExtInfo.fi.wDataBytesPerSector;
    }
	return TRUE;
}

// -----------------------------------------------------------------------------
//  NandReadImage:
//      Reads nk.nb0 off NAND
//      Returns ERR_Xxx
// -----------------------------------------------------------------------------
DWORD
NandReadImage()
{
    DWORD dwSectorsNeeded;
    DWORD dwSector, dwLength;         // Start Sector & Length
    DWORD dwRAM, i;

	//g_TOC has been read out in NandLocateTocAndEBoot. Let's verify if it's ok
    if ( !IS_VALID_TOC(g_pTOC) ) {
        return ERR_INVALID_TOC;
    }

//	TOC_Entry_Print();

    if ( !(g_pTOC->id[g_dwEntry].dwImageType & IMAGE_TYPE_RAMIMAGE) ) {
//        NandDbgPutString("ERR_INVALID_FILE_TYPE: ");
//        NandDbgPutHex(g_pTOC->id[g_dwEntry].dwImageType);
        return ERR_INVALID_FILE_TYPE;
    }

	dwSectorsNeeded = g_pTOC->id[g_dwEntry].dwTtlSectors;

	dwRAM    = VIRTUAL_TO_PHYSICAL(g_pTOC->id[g_dwEntry].dwLoadAddress);

	g_dwJumpAddr = g_pTOC->id[g_dwEntry].dwJumpAddress ? VIRTUAL_TO_PHYSICAL(g_pTOC->id[g_dwEntry].dwJumpAddress) :
											   VIRTUAL_TO_PHYSICAL(g_pTOC->id[g_dwEntry].dwLoadAddress);

    //
    // Load the disk image directly into RAM
    // BUGBUG: recover from read failures
    //
    i = 0;
	while (dwSectorsNeeded && i < MAX_SG_SECTORS)
	{
        dwSector = g_pTOC->id[g_dwEntry].sgList[i].dwSector;
        dwLength = g_pTOC->id[g_dwEntry].sgList[i].dwLength;
        
		if(!NandReadRegion(dwSector, dwLength, dwRAM))
		{
//            NandDbgPutString("ERR_DISK_OP_FAIL2!");
            return ERR_DISK_OP_FAIL2;
		}
		dwRAM += (dwLength << g_FlashExtInfo.ucSectorSizeBits);
        dwSectorsNeeded -= g_pTOC->id[g_dwEntry].sgList[i].dwLength;
        i++;
    }
    
    if (g_pTOC->chainInfo.dwLoadAddress == 0 || g_pTOC->id[g_dwEntry].dwImageType == IMAGE_TYPE_RAMIMAGE) 
        return ERR_SUCCESS;

    // Load the Chain.bin stored on NAND to the SDRAM
    dwRAM = VIRTUAL_TO_PHYSICAL(g_pTOC->chainInfo.dwLoadAddress);

    dwSectorsNeeded = g_pTOC->chainInfo.dwLength;
    dwSector = g_pTOC->chainInfo.dwFlashAddress;
    
//    NandDbgPutString("Reading Chain from NAND\r\n");
//    NandDbgPutString("LoadAddr: ");
//    NandDbgPutHex(dwRAM);
//    NandDbgPutString("NAND SectorAddr: ");
//    NandDbgPutHex(dwSector);
//    NandDbgPutString("Length: ");
//    NandDbgPutHex(dwSectorsNeeded);

	if(!NandReadRegion(dwSector, dwSectorsNeeded, dwRAM))
	{
        return ERR_DISK_OP_FAIL2;
	}

    return ERR_SUCCESS;
}

BOOL TOC_ReserveBlocks (
    DWORD *pdwStartBlock,
    DWORD *pdwFirstAvaiableBlock,
    DWORD dwNumBlocks)
{
    DWORD dwBlockNo = *pdwStartBlock;
    *pdwFirstAvaiableBlock = 0;

    while (dwNumBlocks > 0 && dwBlockNo < g_FlashExtInfo.fi.dwNumBlocks)
    {
        if (NandCheckAndReserveBlock (dwBlockNo))
        {
            --dwNumBlocks;
            if (0 == *pdwFirstAvaiableBlock)
            {
                *pdwFirstAvaiableBlock = dwBlockNo;
            }
        }
        ++dwBlockNo;
    }

    *pdwStartBlock = dwBlockNo;
    return dwBlockNo < g_FlashExtInfo.fi.dwNumBlocks;
}

int TOC_Reset (unsigned int wParam)
{
    NandEraseAll ();

    OEMGetEBootInfo (&g_dwEbootBlockSize, &g_dwEbootLoadAddr, &g_dwEbootLaunchAddr);
    OEMGetDMInfo (&g_dwDMBlockSize, &g_dwDMLoadAddr, &g_dwDMLaunchAddr);

    OEMGetSecureBlockInfo (&g_dwSecureBlockSize);
    g_dwSecureBlockSize += RESERVED_SECURE_BLOCKS;

    memset ((void *)g_pTOC, 0, sizeof (TOC));

    //  NAND flash layout
    //  | NBoot (block 0) | TOC | EBOOT | DM | NK | ... |
    {
        DWORD dwNextSearchBlock = 1;
    	if (TOC_ReserveBlocks (&dwNextSearchBlock, &g_dwTocBlock, g_dwTOCBlockSize) &&
            TOC_ReserveBlocks (&dwNextSearchBlock, &g_dwEBootBlock, g_dwEbootBlockSize) &&
            TOC_ReserveBlocks (&dwNextSearchBlock, &g_dwDMBlock, g_dwDMBlockSize) &&
            TOC_ReserveBlocks (&dwNextSearchBlock, &g_dwSecureBlock, g_dwSecureBlockSize))
        {
            g_dwNKBlock = dwNextSearchBlock;
            NandWriteToc ();
            return TRUE;
        }
    }

    return FALSE;
}
#endif

BOOL  FMDVerifyWrite(DWORD cs)
{
#if 0
    DWORD  dwState=0xff;

    NF_INIT_FIFO();
    NF_IO_RD_MODE();
	NF_ADDR_CYCLE_NUM(0);
	NF_SET_ADDR_HI (0);
    NF_SET_ADDR(0);
    NF_LEN(4);
	NF_FIFO_THRD(4);

	NF_CLR_INT (0xff);
	NF_CMD(SMDF_CMD_READ_STATUS);
	NF_START_FIFO();

    NF_WAIT_INT (SMDF_FIFO_INT);

    NF_RD_FIFO(dwState);
    
    NF_STOP_FIFO();

    if( dwState & 0x01 )
    {
        return FALSE;
    }
#endif
    return TRUE;
}
/*
void FMDDirectReadMode(void)
{
    RESET_DECLARE(RESET_SR_SM_RST);
    RESET_CLEAR(RESET_SR_SM_RST);

    v_pSMDFRegs->SmWait         = 0x1878;

    v_pSMDFRegs->SmDirectRead   |= 1;   // set to direct read mode
    v_pSMDFRegs->SmDmaIoCtrl    = 3;    // set to IO read status
    v_pSMDFRegs->SmDmaIoLen     = 0;
    v_pSMDFRegs->SmFIFOOp       = 1;    // must start FIFO

}*/