bool CFtdiItf::AtmelCmd(DWORD CmdCode,void* Res)
{
    DWORD Cmd;
    Cmd=((CmdCode & 0xFF000000)>>24) | ((CmdCode & 0x00FF0000)>>8) | ((CmdCode & 0x0000FF00)<<8);
    if (!Res) {
        Cmd|=((CmdCode & 0x000000FF)<<24);
        return Send(&Cmd,4);
    };
    if (!Send(&Cmd,3)) return false;
    return RecvByte(Res,CmdCode);
}

bool CFtdiItf::WritePage(const BYTE* Buf,unsigned Addr)
{
    DWORD Cmd;
    unsigned i;

    for(i=0;i<(PageSize>>1);i++) {
        Cmd=0x40000000 | (i<<8) | *Buf++;
        AtmelCmd(Cmd,0);
        Cmd=0x48000000 | (i<<8) | *Buf++;
        AtmelCmd(Cmd,0);
    }
    Addr&=~(PageSize-1);
    AtmelCmd(0x4C000000 | (Addr<<7),0);
    DoSync();
    Sleep(20);
    return true;
}

/* Write memory parameters */
#pragma pack(ALIGN_INT8)
typedef struct __ISP2_WRITE_MEM_PARAM
{
UINT8 cmd_id; /* Command ID */
UINT16 num_bytes; /* Total number of bytes to program, LSB first */
UINT8 mode; /* Programming mode */
UINT8 delay; /* Delay, used for different types of programming termination,
according to mode byte */
UINT8 cmd_WrMem; /* Command 1 - Load Page, Write Programm Memory */
UINT8 cmd_WrPgm; /* Command 2 - Write Programm Memory Page */
UINT8 cmd_RdMem; /* Command 3 - Read Programm Memory */
UINT8 poll_1; /* Poll value 1 */
UINT8 poll_2; /* Poll value 2 - not using for flash programming */
UINT8 data[1]; /* Data array */

} ISP2_WRITE_MEM_PARAM, *P_ISP2_WRITE_MEM_PARAM;
#pragma pack()

/*******************************************************************************
*******************************************************************************/
static INT CMD_WriteMem(P_ISP2_WRITE_MEM_PARAM param, INT msg_len, UINT8 mem_type)
{
UINT8 byte, cmd[4], data[4];
UINT8 val_poll, rdy_poll;
INT i;

/* Check parameters */
if(msg_len < sizeof(ISP2_WRITE_MEM_PARAM) || /* Wrong message */
g_ISP2.state != ISP2_IN_PROG_STATE) /* Illegal programmer state */
return ISP2_STATUS_CMD_FAILED;

/* Get parameters */
param->num_bytes = SWAP16(param->num_bytes); /* Correct number of bytes read */

/* Make polling flags */
if(ISP2_WRMEM_MODE_PAGE & param->mode) /* Page mode */
{
val_poll = ISP2_WRMEM_MODE_PAGE_VALUE_POLL;
rdy_poll = ISP2_WRMEM_MODE_PAGE_READY_POLL;
}
else /* Word mode for flash, byte mode for eeprom */
{
val_poll = ISP2_WRMEM_MODE_WORD_VALUE_POLL;
rdy_poll = ISP2_WRMEM_MODE_WORD_READY_POLL;
}

/* Check delay value */
if(!param->delay)
param->delay = ISP2_CFG_DEF_WRITE_DELAY;

/* Check extended address for FLASH programming */
if(ISP2_MEM_TYPE_FLASH == mem_type && g_ISP2.prog_addr & 0x80000000)
{
cmd[0] = 0x4d; /* Load Extended Address command */
cmd[1] = 0x00;
cmd[2] = LOBYTE(HIWORD(g_ISP2.prog_addr));
cmd[3] = 0x00;
ISP_Tx(cmd, 32); /* Send extended address byte */
}

/* Write cycle */
for(i = 0; i < param->num_bytes; i++)
{
/* Load address and data byte */
cmd[1] = LOWORD(HIBYTE(g_ISP2.prog_addr));
cmd[2] = LOWORD(LOBYTE(g_ISP2.prog_addr));
cmd[3] = param->data[i];
byte = cmd[3];

/* Check memory type and make command byte */
if(ISP2_MEM_TYPE_FLASH == mem_type)
{
/* Make command byte (Load Page, Write Program Memory) &
increment address if hi byte writing */
if(i & 0x01)
{
cmd[0] = param->cmd_WrMem | 0x08; /* Make command for hight byte */
g_ISP2.prog_addr++; /* Increment address */
}
else
cmd[0] = param->cmd_WrMem & ~0x08; /* Make command for low byte */
}
else /* EEPROM */
{
cmd[0] = param->cmd_WrMem;
g_ISP2.prog_addr++; /* Increment address */
}

ISP_Tx(cmd, 32); /* Send command */

/* In page mode after send last byte need send command WRITE PAGE */
if(ISP2_WRMEM_MODE_PAGE & param->mode) /* Page mode */
{
if(i < param->num_bytes - 1 || !(ISP2_WRMEM_MODE_PAGE_WRITE_ENABLE & param->mode))
continue; /* It's not last byte or write page bit not set */

cmd[0] = param->cmd_WrPgm; /* Make Write Program Page command */
ISP_Tx(cmd, 32); /* Send PAGE WRITE command */
}

/* Check write success */
if(param->mode & val_poll) /* Value polling */
{
if((byte == param->poll_1) || /* Does not use polling if value is some for polling values */
(ISP2_MEM_TYPE_EEPROM == mem_type && byte == param->poll_2)) /* poll #2 use only EEPROM */
BOARD_DelayMS(param->delay);
else
{
/* Make READ command */
if(ISP2_MEM_TYPE_FLASH == mem_type)
{
if(i & 0x01) /* Make Read Program Memory command */
cmd[0] = param->cmd_RdMem | 0x08; /* Read hight byte */
else
cmd[0] = param->cmd_RdMem & ~0x08; /* Read low byte */
}
else
cmd[0] = param->cmd_RdMem;

/* Wait, when value was write complete */
BOARD_SetTimeout(param->delay);
do
{
ISP_Ex(cmd, data, 32); /* Send command */
if(BOARD_IsTimeout())
return ISP2_STATUS_CMD_TOUT;
}
while(data[3] != byte);
}
}
else
{
if(param->mode & rdy_poll) /* RDY/BSY polling */
{
if(FALSE == WaitReady(param->delay))
return ISP2_STATUS_CMD_TOUT;
}
else /* Timed delay */
BOARD_DelayMS(param->delay);
}
}
return ISP2_STATUS_CMD_OK;
}