// ---------------------------------------------------------------------------
// @file       avrJtag.c
// @brief      JTAG commands for programming an AVR microcontroller 
// 
// @author     Laurent Saint-Marcel (lstmarcel@yahoo.fr)
// @date       2009/09/25
// ---------------------------------------------------------------------------

#include "jtag.h"
#include "uart.h"
#include "time.h"
#include "crc.h"
#include "larvorProtocol.h"

static uint16_t addrBegin;
static uint16_t addrEnd;
static uint8_t  pageSize;

static void nop() 
{ 
    __asm__ __volatile__ ("nop");
}


#define JTAG_UART_TIMEOUT 1000 // 1000 ms = 1s
// ----------------------------------------------------------------------------
// Read 2 16 bits addresses + the 8 bits page size + 8 bits CRC for these data
// ----------------------------------------------------------------------------
static unsigned char 
        getAddrs()
{
    addrBegin = uartRead16();
    addrEnd   = uartRead16();
    pageSize  = uartRead8();
    unsigned char crc = uartRead8();
    
    // check CRC
    unsigned char crcData = 0;
    crcData = CRC16(addrBegin, crcData); 
    crcData = CRC16(addrEnd, crcData); 
    crcData = CRC8(pageSize, crcData); 
    
    if (crcData == crc) {
        uartWrite(PROG_CRC_OK);
    } else {
        uartWrite(PROG_CRC_ERROR);
    }
    return (crcData == crc)?1:0;
}

// ----------------------------------------------------------------------------
// Write 4 bits of 'instruction' in the IR register
// Initial state: TestLogicReset or RunTestIdle
// Final state:   RunTestIdle
// ----------------------------------------------------------------------------
void 
        jtagWriteInstruction(uint8_t instruction)
{
    jtagShiftIR();
    jtagWriteIR(instruction & 0x0F);
    jtagRunTestIdle();
}

// ----------------------------------------------------------------------------
// Write 'length' bits of 'cmd' in the DR register and return 'length' bits
// from DR
// Initial state: TestLogicReset or RunTestIdle
// Final state:   RunTestIdle
// ----------------------------------------------------------------------------
uint32_t
        jtagWriteData(uint32_t cmd, uint8_t length)
{
    uint8_t  shift  = 0;
    uint32_t result = 0;
    jtagShiftDR();
    while (length > 0) {
        if (length <= 8) {
            result += jtagWriteDR(((cmd>>shift)&0xFF), length, 1/*exitAtLast*/) << shift;
            break;
        } else {
            result += jtagWriteDR(((cmd>>shift)&0xFF), 8, 0/*continueAtLast*/) << shift;
        }
        length -= 8;
        shift  += 8;    
    }
    jtagRunTestIdle();
    return result;
}

// ----------------------------------------------------------------------------
// Write 15 bits of 'cmd' in the DR register and return 15 bits from DR
// Initial state: TestLogicReset or RunTestIdle
// Final state:   RunTestIdle
// ----------------------------------------------------------------------------
uint16_t
        jtagWriteProgCmd(uint8_t cmdH, uint8_t cmdL)
{
    return jtagWriteData((cmdH<<8) + cmdL, 15) & 0xFFFF;
}

// ----------------------------------------------------------------------------
// AVR jtag Entering Programming Mode
// ----------------------------------------------------------------------------
void
        jtagEnteringProgramming()
{
    jtagTestLogicReset();

    jtagWriteInstruction(JTAG_AVR_RESET);
    jtagWriteData(0x01, 1);

    jtagWriteInstruction(JTAG_PROG_ENABLE);
    jtagWriteData(0xA370, 16);
}

// ----------------------------------------------------------------------------
// AVR jtag Leaving Programming Mode
// ----------------------------------------------------------------------------
void
        jtagLeavingProgramming()
{
    jtagWriteInstruction(JTAG_PROG_COMMANDS);
    jtagWriteProgCmd(0x23, 0x00);
    jtagWriteProgCmd(0x33, 0x00);

    jtagWriteInstruction(JTAG_PROG_ENABLE);
    jtagWriteData(0x0000, 16);

    jtagWriteInstruction(JTAG_AVR_RESET);
    jtagWriteData(0x00, 1);
}

// ============================================================================
// AVR Jtag commands
// ============================================================================

// ----------------------------------------------------------------------------
// Get the 32 bits of device ID code from the IR instruction IDCODE 
// ----------------------------------------------------------------------------
uint32_t
        avrJtagGetDeviceId()
{
    uint32_t data=0, data2;
    jtagTestLogicReset();
    jtagShiftIR();
    jtagWriteIR(JTAG_IDCODE);
    jtagRunTestIdle();
    jtagShiftDR();

    data = (jtagWriteDR(0, 8, 0)) & 0xFF;
    data += (jtagWriteDR(0, 8, 0) << 8) & 0xFF00;

    data2 = (jtagWriteDR(0, 8, 0)) & 0xFF;
    data2 += (jtagWriteDR(0, 8, 1) << 8) & 0xFF00;

    data += data2 << 16;

    jtagTestLogicReset();
    return data;
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the signature bytes
// send 24 bits on the serial port
// ----------------------------------------------------------------------------
uint32_t
        avrJtagReadSignature()
{
    uint32_t data=0, data2;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 9a : Enter Signature Byte Read
    jtagWriteProgCmd(0x23, 0x08); 

    // 9b Load Addres 0x00
    jtagWriteProgCmd(0x03, 0x00); 
    // 9c Read Signature Byte
    jtagWriteProgCmd(0x32, 0x00); 
    data2 = jtagWriteProgCmd(0x33, 0x00) & 0xFF;
    data += data2 << 0;

    // 9b Load Addres 0x01
    jtagWriteProgCmd(0x03, 0x01); 
    // 9c Read Signature Byte
    jtagWriteProgCmd(0x32, 0x00);
    data2 = jtagWriteProgCmd(0x33, 0x00) & 0xFF;
    data += data2 << 8;

    // 9b Load Addres 0x02
    jtagWriteProgCmd(0x03, 0x02);
    // 9c Read Signature Byte 
    jtagWriteProgCmd(0x32, 0x00); 
    data2 = jtagWriteProgCmd(0x33, 0x00) & 0xFF;
    data += data2 << 16;

    jtagLeavingProgramming();
    return data;
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the Calibration byte
// send 8 bits on the serial port
// ----------------------------------------------------------------------------
uint8_t
        avrJtagReadCalibration()
{
    uint8_t data;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 10a : Enter Calibration Byte Read
    jtagWriteProgCmd(0x23, 0x08); 
    // 10b : Load Address
    jtagWriteProgCmd(0x03, 0x00); 
    // 10c : Read Calibration
    jtagWriteProgCmd(0x32, 0x00);

    data = (jtagWriteProgCmd(0x33, 0x00) & 0xFF);

    jtagLeavingProgramming();
    return data;
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the Fuse High byte
// send 8 bits on the serial port
// ----------------------------------------------------------------------------
uint8_t
        avrJtagReadFuseExt()
{
    uint8_t data;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 8a : Enter Fuse/Lock Bit Read
    jtagWriteProgCmd(0x23, 0x04); 
    // 8b : Read Fuse High
    jtagWriteProgCmd(0x3A, 0x00); 

    data = (jtagWriteProgCmd(0x3B, 0x00) & 0xFF);

    jtagLeavingProgramming();
    return data;
}
// ----------------------------------------------------------------------------
// AVR jtag Reading the Fuse High byte
// send 8 bits on the serial port
// ----------------------------------------------------------------------------
uint8_t 
        avrJtagReadFuseHigh()
{
    uint8_t data;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 8a : Enter Fuse/Lock Bit Read
    jtagWriteProgCmd(0x23, 0x04); 
    // 8b : Read Fuse High
    jtagWriteProgCmd(0x3E, 0x00); 

    data = (jtagWriteProgCmd(0x3F, 0x00) & 0xFF);

    jtagLeavingProgramming();
    return data;
}
// ----------------------------------------------------------------------------
// AVR jtag Reading the Fuse Low byte
// send 8 bits on the serial port
// ----------------------------------------------------------------------------
uint8_t
        avrJtagReadFuseLow()
{
    uint8_t data=0;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 8a : Enter Fuse/Lock Bit Read
    jtagWriteProgCmd(0x23, 0x04); 
    // 8c : Read Fuse Low
    jtagWriteProgCmd(0x32, 0x00); 

    data = (jtagWriteProgCmd(0x33, 0x00) & 0xFF);

    jtagLeavingProgramming();
    return data;
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the Lock byte
// send 8 bits on the serial port
// ----------------------------------------------------------------------------
uint8_t
        avrJtagReadLock()
{
    uint8_t data;
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 8a : Enter Fuse/Lock Bit Read
    jtagWriteProgCmd(0x23, 0x04); 
    // 8d : Read Lock
    jtagWriteProgCmd(0x36, 0x00); 

    data = (jtagWriteProgCmd(0x37, 0x00) & 0xFF);

    jtagLeavingProgramming();
    return data;
}

// ----------------------------------------------------------------------------
// AVR jtag Writing the Fuse Extended byte
// read 8 bits from the serial port
// ----------------------------------------------------------------------------
void 
        avrJtagWriteFuseExt(uint8_t data)
{
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 6a : Enter Write Fuse Write
    jtagWriteProgCmd(0x23, 0x40); 
    // 6b : load data
    jtagWriteProgCmd(0x13, data) ; 
    // 6c : write fuse high
    jtagWriteProgCmd(0x3B, 0x00); 
    jtagWriteProgCmd(0x39, 0x00); 
    jtagWriteProgCmd(0x3B, 0x00); 
    jtagWriteProgCmd(0x3B, 0x00); 

    // 6d : poll for write complete 
    while ((jtagWriteProgCmd(0x3B, 0x00)& 0x200) == 0); // 0x37 in thedoc. Is it a typo?

    jtagLeavingProgramming();
}

// ----------------------------------------------------------------------------
// AVR jtag Writing the Fuse High byte
// read 8 bits from the serial port
// ----------------------------------------------------------------------------
void 
        avrJtagWriteFuseHigh(uint8_t data)
{
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 6a : Enter Write Fuse Write
    jtagWriteProgCmd(0x23, 0x40); 
    // 6b : load data
    jtagWriteProgCmd(0x13, data ) ; 
    // 6c : write fuse high
    jtagWriteProgCmd(0x37, 0x00); 
    jtagWriteProgCmd(0x35, 0x00); 
    jtagWriteProgCmd(0x37, 0x00); 
    jtagWriteProgCmd(0x37, 0x00); 

    // 6d : poll for write complete 
    while ((jtagWriteProgCmd(0x37, 0x00)& 0x200) == 0);

    jtagLeavingProgramming();
}
// ----------------------------------------------------------------------------
// AVR jtag Writing the Fuse Low bytes
// read 8 bits from the serial port
// ----------------------------------------------------------------------------
void 
        avrJtagWriteFuseLow(uint8_t data)
{
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 6a : Enter Write Fuse Write
    jtagWriteProgCmd(0x23, 0x40); 
    // 6e : load data
    jtagWriteProgCmd(0x13, data ); 
    // 6f : write fuse low
    jtagWriteProgCmd(0x33, 0x00); 
    jtagWriteProgCmd(0x31, 0x00); 
    jtagWriteProgCmd(0x33, 0x00); 
    jtagWriteProgCmd(0x33, 0x00); 

    // 6g : poll for write complete 
    while ((jtagWriteProgCmd(0x33, 0x00)& 0x200) == 0);

    jtagLeavingProgramming();
}
// ----------------------------------------------------------------------------
// AVR jtag Writing the Lock bytes
// ----------------------------------------------------------------------------
void 
        avrJtagWriteLock(uint8_t data)
{
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    // 7a : Enter Write Lock Write
    jtagWriteProgCmd(0x23, 0x20); 
    // 7b : load data
    jtagWriteProgCmd(0x13, 0xC0 + (data & 0x3F)); 
    // 7c : write lock
    jtagWriteProgCmd(0x33, 0x00); 
    jtagWriteProgCmd(0x31, 0x00); 
    jtagWriteProgCmd(0x33, 0x00); 
    jtagWriteProgCmd(0x33, 0x00); 

    // 7d : poll for write complete
    while ((jtagWriteProgCmd(0x33, 0x00)& 0x200) == 0);

    jtagLeavingProgramming();
}

// ----------------------------------------------------------------------------
// AVR jtag read data from the serial port and check the CRC
// ----------------------------------------------------------------------------
static unsigned char rxBuffer[140]; // max 128 + 1 status byte + 1 CRC

static unsigned char 
        avrGetWriteBuffer(unsigned char** writeBuffer,
                          unsigned char bufferSize)
{
    // copy the data from the serial port fifo
    timeReset();
    while(1) {
        if (uartReadNBytes(rxBuffer, bufferSize + 2) != 0) // data received
            break;
        if (timeGet() > JTAG_UART_TIMEOUT) {// timeout
            uartWrite(PROG_TIMEOUT);
            return PROG_CANCEL;
        }
    }
    // check the COM status byte
    if (rxBuffer[0] == PROG_CANCEL) { // communication stopped by the PC
        uartWrite(PROG_CANCEL);
        return PROG_CANCEL;
    }

    // set the data pointer
    *writeBuffer = rxBuffer + 1;

    // compute the CRC
    unsigned char i = 0;
    unsigned char crc=0;
    while(i < bufferSize) {
        crc = CRC8((*writeBuffer)[i++], crc);
    }

    // check the CRC value to be sure that there was no error on the serial port
    uint8_t crcExpected = rxBuffer[1 + bufferSize];
    if (crc != crcExpected) {
        uartWrite(PROG_CRC_ERROR);
        return PROG_CRC_ERROR;
    } else {
        uartWrite(PROG_CRC_OK);
        return PROG_CRC_OK;
    }
}

// ----------------------------------------------------------------------------
// resynchronize the UART connection after a CRC error (probably a missing character)
// ----------------------------------------------------------------------------
static void
        avrUartResync()
{
    timeReset();
    while(uartRead(UART_DONT_WAIT_RECEIVE) != PROG_SYNC1 && timeGet() < JTAG_UART_TIMEOUT);
    while(uartRead(UART_DONT_WAIT_RECEIVE) != PROG_SYNC2 && timeGet() < JTAG_UART_TIMEOUT);
}

// ----------------------------------------------------------------------------
// 
// ----------------------------------------------------------------------------
static uint8_t
        avrJtagGetWriteBufferData(uint8_t** buffer, uint8_t size)
{
    uint8_t rcvStatus = PROG_CRC_ERROR;
    do {
        rcvStatus = avrGetWriteBuffer(buffer, size);
        if (rcvStatus == PROG_CRC_ERROR) {
            avrUartResync();
        }
    } while (rcvStatus == PROG_CRC_ERROR);
    return rcvStatus;
}

// ----------------------------------------------------------------------------
// AVR jtag Erasing the chip
// ----------------------------------------------------------------------------
void 
        avrJtagChipErase()
{
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);
    // 1a : start chip erase
    jtagWriteProgCmd(0x23, 0x80); 
    jtagWriteProgCmd(0x31, 0x80);
    jtagWriteProgCmd(0x33, 0x80);
    jtagWriteProgCmd(0x33, 0x80);

    // 1b : poll for chip erase complete 
    while ((jtagWriteProgCmd(0x33, 0x80) & 0x200) == 0) nop();

    jtagLeavingProgramming();
}

// ----------------------------------------------------------------------------
// AVR jtag Reading a eeprom block
// ----------------------------------------------------------------------------
static uint8_t 
        avrJtagEEpromRead(uint16_t begin,
                          uint16_t size,
                          uint8_t* compareBuffer,
                          uint8_t* crc)
{
    // 5a : Enter EEprom read
    jtagWriteProgCmd(0x23, 0x03);        

    uint16_t i=0, index=0;
    for(index=0; index < size; ++index) {
        uint16_t addr= begin + index;
        uint8_t addrL = ((addr) & 0xFF);
        uint8_t addrH = ((addr >> 8) & 0xFF);

        // 5b : Load Address High
        jtagWriteProgCmd(0x07, addrH);
        // 5c : Load Address Low
        jtagWriteProgCmd(0x03, addrL); 

        // 5d : Read Data Byte
        jtagWriteProgCmd(0x33, 0x00); //jtagWriteProgCmd(0x33, addrL); // TODO: is it a typo in the doc?
        jtagWriteProgCmd(0x32, 0x00);
        // read data
        uint8_t data, lastData = jtagWriteProgCmd(0x33, 0x00) & 0xFF;
        while(1) {
            jtagWriteProgCmd(0x33, 0x00);
            jtagWriteProgCmd(0x32, 0x00);
            data = jtagWriteProgCmd(0x33, 0x00) & 0xFF;
            if (data == lastData) {
                break;
            } else {
                lastData = data;
            }
        }

        if (compareBuffer != 0) {
            // compare data with the buffer
            if (compareBuffer[i] != data) {
                return PROG_ERROR;
            }
            i++;
        } else {
            // send data on the serial port
            uartWrite8(data);
            // CRC
            *crc = CRC8(data, *crc);
        }
    } 
    return PROG_OK;
}
#define avrJtagCompareEEpromBlock(addr, size, buffer)  avrJtagEEpromRead(addr, size, buffer, 0)
#define avrJtagSendEEpromBlock(addr, size, crc)        avrJtagEEpromRead(addr, size, 0, crc)

// ----------------------------------------------------------------------------
// AVR jtag Writing the EEprom page
// ----------------------------------------------------------------------------
static void 
        avrJtagWriteEEpromPage(uint16_t addr,
                               uint8_t  pageSize,
                               uint8_t* buffer)
{
    uint8_t addrH = ((addr >> 8) & 0xFF);

    // 4a : Enter EEprom Write
    jtagWriteProgCmd(0x23, 0x11);  
    // 4b : Load Address High
    jtagWriteProgCmd(0x07, addrH);

    // save data on the jtag port
    uint8_t indexInPage = 0;
    for(indexInPage=0; indexInPage < pageSize; ++indexInPage) {
        uint8_t addrL = ((addr+indexInPage) & 0xFF);
        // 4c : Load Address Low
        jtagWriteProgCmd(0x03, addrL);

        // 4d : Load Data low;
        jtagWriteProgCmd(0x13, buffer[indexInPage]);

        // 4f : Latch Data
        jtagWriteProgCmd(0x37, 0x00);
        jtagWriteProgCmd(0x77, 0x00);
        jtagWriteProgCmd(0x37, 0x00);
    }
    // 4f : Write EEProm Page
    jtagWriteProgCmd(0x33, 0x00);
    jtagWriteProgCmd(0x31, 0x00);
    jtagWriteProgCmd(0x33, 0x00);
    jtagWriteProgCmd(0x33, 0x00);

    // 4g : poll for page write complete
    while((jtagWriteProgCmd(0x33, 0x00) & 0x200) == 0) nop();
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the eeprom from addrBegin to addrEnd (addrEnd is included)
// ----------------------------------------------------------------------------
void 
        avrJtagReadEEprom()
{
    if (!getAddrs()) {
        return;
    }
    jtagEnteringProgramming();
    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    uint16_t addr;
    for (addr = addrBegin; addr <= addrEnd;) {
        uint8_t crc=0;
        // read a block, send it on the UART and compute the CRC
        avrJtagSendEEpromBlock(addr, PROG_CRC_CHECK_SIZE, &crc);
        // send the CRC
        uartWrite8(crc);
        // check the response from PC
        uint8_t answer = uartRead8();
        if (answer == PROG_CANCEL) {
            break;
        } else if (answer == PROG_CRC_OK) {
            // move to the next block
            addr = addr + PROG_CRC_CHECK_SIZE;
        } else { // PROG_CRC_ERROR
            // send the same block again
        }
    } 
    jtagLeavingProgramming();

    uartWrite(PROG_OK);
}

// ----------------------------------------------------------------------------
// AVR jtag Writing the eeprom 
// ----------------------------------------------------------------------------
void 
        avrJtagWriteEEprom()
{
    if (!getAddrs()) {
        return;
    }
    
    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);
    uint8_t status = PROG_OK;
    uint16_t addr=0;
    for(addr=addrBegin; addr <= addrEnd; addr += pageSize) {
        // get data from the serial port
        uint8_t* buffer=0;
        if (avrJtagGetWriteBufferData(&buffer, pageSize) != PROG_CRC_OK) {
            break;
        }
        uint8_t retry;
        for(retry = 0; retry < 5; retry++) {
            // write data into the flash
            avrJtagWriteEEpromPage(addr, pageSize, buffer);
            // check if data into the flash map the buffer
            if (avrJtagCompareEEpromBlock(addr, pageSize, buffer) == PROG_OK)
                break;
        }
        if (retry >= 5) {
            status = PROG_ERROR;
            break;
        }
    }

    jtagLeavingProgramming();
    uartWrite(status);
}

// ----------------------------------------------------------------------------
// AVR jtag Reading a flash block
// ----------------------------------------------------------------------------
static uint8_t 
        avrJtagFlashRead(uint16_t begin,
                         uint16_t size,
                         uint8_t* compareBuffer,
                         uint8_t* crc)
{
    // 3a : Enter Flash read
    jtagWriteProgCmd(0x23, 0x02); 
    uint16_t i=0, index=0;
    for(index=0; index < size; ++index) {
        uint16_t addr= begin + index;
        uint8_t addrH = ((addr >> 8) & 0xFF);
        uint8_t addrL = (addr & 0xFF);
        // 3b : Load Address High
        jtagWriteProgCmd(0x07, addrH);  
        // 3c : Load Address Low
        jtagWriteProgCmd(0x03, addrL); 

        // read data at least 2 times to be sure that we have read the correct value (lots of error append during the read)
        uint8_t data1, data2, lastData1, lastData2;
        // 3d : Read Data Bytes
        jtagWriteProgCmd(0x32, 0x00);
        lastData1 = jtagWriteProgCmd(0x36, 0x00) & 0xFF;
        lastData2 = jtagWriteProgCmd(0x37, 0x00) & 0xFF;
        while(1) {
            jtagWriteProgCmd(0x32, 0x00);
            data1 = jtagWriteProgCmd(0x36, 0x00) & 0xFF;
            data2 = jtagWriteProgCmd(0x37, 0x00) & 0xFF;
            if ((data1 == lastData1) && (data2 == lastData2)) {
                break;
            } else {
                lastData1 = data1;
                lastData2 = data2;
            }
        }

        if (compareBuffer != 0) {
            // compare data with the buffer
            if ((compareBuffer[i] != data1)
                || (compareBuffer[i+1] != data2))
                {
                return PROG_ERROR;
            }
            i+=2;
        } else {
            // send data on the serial port
            uartWrite8(data1);
            uartWrite8(data2);
            // CRC
            *crc = CRC8(data1, *crc);
            *crc = CRC8(data2, *crc);
        }
    } 
    return PROG_OK;
}
#define avrJtagCompareFlashBlock(addr, size, buffer)  avrJtagFlashRead(addr, size, buffer, 0)
#define avrJtagSendFlashBlock(addr, size, crc)        avrJtagFlashRead(addr, size, 0, crc)
// ----------------------------------------------------------------------------
// AVR jtag Writing the flash page
// ----------------------------------------------------------------------------
static void 
        avrJtagWriteFlashPage(uint16_t addr,
                              uint8_t  pageSize,
                              uint8_t* buffer)
{
    uint8_t addrE = 0; //((addr >> 16) & 0xFF);
    uint8_t addrH = ((addr >> 8) & 0xFF);

    // 2a : Enter Flash Write
    jtagWriteProgCmd(0x23, 0x10);
    // 2b : Load Address Extended
    jtagWriteProgCmd(0x0B, addrE);
    // 2c : Load Address High
    jtagWriteProgCmd(0x07, addrH);

    // save data on the jtag port
    uint8_t indexInPage = 0;
    for(indexInPage=0; indexInPage < pageSize; ++indexInPage) {
        uint8_t addrL = ((addr+indexInPage) & 0xFF);
        // 2d : Load Address Low
        jtagWriteProgCmd(0x03, addrL);
        uint8_t dataL = buffer[indexInPage*2];
        uint8_t dataH = buffer[indexInPage*2+1];
        // 2e : Load Data low;
        jtagWriteProgCmd(0x13, dataL);
        // 2f : Load Data high
        jtagWriteProgCmd(0x17, dataH);

        // 2e : Latch Data
        jtagWriteProgCmd(0x37, 0x00);
        jtagWriteProgCmd(0x77, 0x00);
        jtagWriteProgCmd(0x37, 0x00);
    }
    // 2f : Write Flash Page
    jtagWriteProgCmd(0x37, 0x00);
    jtagWriteProgCmd(0x35, 0x00);
    jtagWriteProgCmd(0x37, 0x00);
    jtagWriteProgCmd(0x37, 0x00);

    // 2g : poll for page write complete
    while((jtagWriteProgCmd(0x37, 0x00) & 0x200) == 0) nop();
}

// ----------------------------------------------------------------------------
// AVR jtag Reading the flash
// ----------------------------------------------------------------------------
void
        avrJtagReadFlash()
{
    if (!getAddrs()) {
        return;
    }

    jtagEnteringProgramming();
    jtagWriteInstruction(JTAG_PROG_COMMANDS);

    uint16_t addr;
    for (addr = addrBegin; addr <= addrEnd;) {
        uint8_t crc=0;
        // read a block, send it on the UART and compute the CRC
        avrJtagSendFlashBlock(addr, PROG_CRC_CHECK_SIZE, &crc);
        // send the CRC
        uartWrite8(crc);
        // check the response from PC
        uint8_t answer = uartRead8();
        if (answer == PROG_CANCEL) {
            break;
        } else if (answer == PROG_CRC_OK) {
            // move to the next block
            addr = addr + PROG_CRC_CHECK_SIZE;
        } else { // PROG_CRC_ERROR
            // send the same block again
        }
    } 

    jtagLeavingProgramming();
    uartWrite(PROG_OK);
}

// ----------------------------------------------------------------------------
// AVR jtag Writing the flash
// ----------------------------------------------------------------------------
void 
        avrJtagWriteFlash()
{
    if (!getAddrs()) {
        return;
    }

    jtagEnteringProgramming();

    jtagWriteInstruction(JTAG_PROG_COMMANDS);
    uint8_t status = PROG_OK;
    uint16_t addr=0;
    for(addr=addrBegin; addr <= addrEnd; addr += pageSize) {
        // get data from the serial port
        uint8_t* buffer=0;
        if (avrJtagGetWriteBufferData(&buffer, pageSize*2) != PROG_CRC_OK) {
            break;
        }
        uint8_t retry;
        for(retry = 0; retry < 5; retry++) {
            // write data into the flash
            avrJtagWriteFlashPage(addr, pageSize, buffer);
            // check if data into the flash map the buffer
            if (avrJtagCompareFlashBlock(addr, pageSize, buffer) == PROG_OK)
                break;
        }
        if (retry >= 5) {
            status = PROG_ERROR;
            break;
        }
    }

    jtagLeavingProgramming();
    uartWrite(status);
}