#include "progbob-avrisp.h"
#include "serialport/serialport.hpp"
#include "avrisp_stk500v2.h"
#include "dude.h"

#include <iostream>
#include <iomanip>
#include <string.h>
#include <boost/crc.hpp>      // for boost::crc_basic, boost::crc_optimal
//#include <boost/cstdint.hpp>  // for boost::uint16_t

#define PROGBOB_PAGESIZE 2048
#define PROGBOB_CHUNKSIZE 64

#define MSB(x) ((uint8_t)((((uint16_t)(x))&0xff00)>>8))
#define LSB(x) ((uint8_t)((((uint16_t)(x))&0x00ff)>>0))
#define MAKE_WORD(hi, lo) (((uint16_t)(lo))|(((uint16_t)(hi))<<8))

// helper (avrlibc function _crc_ccitt_update())
// Polynomial: x^16 + x^12 + x^5 + 1 (0x8408)
// Initial value: 0xffff
static uint16_t crc_ccitt_update (uint16_t crc, uint8_t data) {
    data ^= (crc & 0xff);
    data ^= data << 4;
    return ((((uint16_t)data << 8) | ((crc>>8)&0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
}

static std::string uint2hex(unsigned int num, unsigned int hexdigits) {
    std::stringstream ss;
    ss << std::setw(hexdigits) << std::setfill('0') << std::hex << num;
    return ss.str();
}

static std::string uint2str(unsigned int num) {
    std::stringstream ss;
    ss << num;
    return ss.str();
}


class ProgBob_AVRISP_priv {
private:
    static const size_t buffer_size = 512;
    uint8_t buffer[buffer_size];
    uint8_t sequence;
    
protected:
    SerialPortController & spc;
    SerialPort * serialport;
    AvrProgrammerConfig cfg;
    ProgrammerBase::progress_fn * progress_callback;
    
    unsigned int serial_receive(uint8_t * buffer, unsigned int size, unsigned int timeout_ms);
    unsigned int exchange(unsigned int txsize, unsigned int timeout_ms);
    uint8_t * getBuffer() {return buffer+5;}
    
    ProgBob_AVRISP_priv();
    void drain_serial(unsigned int timeout_ms);

    // Commands:
    std::string cmd_signOn();
    void cmd_setParameter(uint8_t parameterID, uint8_t value);
    uint8_t cmd_getParameter(uint8_t parameterID);
    void cmd_osccal();
    void cmd_loadAddress(uint32_t address);
    void cmd_firmwareUpgrade();
    
    void cmd_enterProgmodeIsp();
    void cmd_leaveProgmodeIsp();
    void cmd_chipEraseIsp();
    void cmd_programFlashIsp(uint16_t numBytes, uint8_t * data);
    void cmd_readFlashIsp(uint16_t numBytes, uint8_t * data);
    void cmd_programEepromIsp(uint16_t numBytes, uint8_t * data);
    void cmd_readEepromIsp(uint16_t numBytes, uint8_t * data);
    void cmd_programFuseIsp(uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4);
    uint8_t cmd_readFuseIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4);
    void cmd_programLockIsp(uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4);
    uint8_t cmd_readLockIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4);
    uint8_t cmd_readSignatureIsp(uint8_t addr);
    uint8_t cmd_readOsccalIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4);
    uint8_t cmd_spiMulti(uint8_t numTx, uint8_t numRx, uint8_t rxStartAddr, uint8_t * data);

    // other
    void doProgress(unsigned int progress, unsigned int progressFull, const std::string & info, bool replace) {progress_callback(progress, progressFull, info, replace);}
    void doProgress(unsigned int progress, unsigned int progressFull) {progress_callback(progress, progressFull, "", false);}
    void doProgress(const std::string & info, bool replace) {progress_callback(0, 0, info, replace);}
    void updateBlockProgress(const std::string & msg1, const std::string & msg2, uint32_t address, unsigned int page, unsigned int pagecnt, bool replace);

    friend class ProgBob_AVRISP;
};


//////////////////////// private OBJECT


ProgBob_AVRISP_priv::ProgBob_AVRISP_priv() : spc(SerialPortController::singleton()) {
    
}

unsigned int ProgBob_AVRISP_priv::serial_receive(uint8_t * buffer, unsigned int size, unsigned int timeout_ms) {
    unsigned int result = 0;
    while (size>0) {
        int len = serialport->read((char *)buffer, size, timeout_ms);
        if (len==0) break;
        if (len<0) {
            //std::cout << "serial error: " << len << std::endl;
            throw std::runtime_error("Serial error");
        }
        size -= len;
        buffer += len;
        result += len;
    }
    return result;
}


unsigned int ProgBob_AVRISP_priv::exchange(unsigned int txsize, unsigned int timeout_ms) {
    try {
        // transmit:
        uint8_t chksum = 0x00;
        buffer[0] = AVRISP_MESSAGE_START;
        buffer[1] = ++sequence;
        buffer[2] = MSB(txsize);
        buffer[3] = LSB(txsize);
        buffer[4] = AVRISP_TOKEN;
        if (txsize>275) throw std::runtime_error("Protocol tx error: size error");
        for (unsigned int i=0; i<txsize+5; i++) chksum ^= buffer[i];
        buffer[5+txsize] = chksum;
        serialport->write((char *)buffer, txsize+6);
        
        // receive:
        unsigned int rxsize = 0;
        int len = serial_receive(buffer, 6, timeout_ms);
        if (len!=6) throw std::runtime_error("Protocol rx error: no head");
        if (buffer[0] != AVRISP_MESSAGE_START) throw std::runtime_error("Protocol rx error: start [0x1b] expected");
        if (buffer[1] != sequence) throw std::runtime_error("Protocol rx error: wrong sequence");
        if (buffer[4] != AVRISP_TOKEN) throw std::runtime_error("Protocol rx error: token [0x0e] expected");
        rxsize = MAKE_WORD(buffer[2], buffer[3]);
        //std::cout << "head received waiting for " << rxsize << " bytes..." << std::endl;
        if (rxsize>275) throw std::runtime_error("Protocol rx error: size error");
        len = serial_receive(buffer+6, rxsize, timeout_ms);
        //std::cout << "data received: " << len << " bytes..." << std::endl;
        if (len!=rxsize) throw std::runtime_error("Protocol rx error: missing data");
        chksum = 0;
        for (unsigned int i=0; i<rxsize+5; i++) chksum ^= buffer[i];
        if (buffer[5+rxsize] != chksum) throw std::runtime_error("Protocol rx error: wrong checksum");
        return rxsize;
    } catch (...) {
        //std::cout << "EXCEPTION" << std::endl;
        drain_serial(10);
        throw;
    }
}


void ProgBob_AVRISP_priv::drain_serial(unsigned int timeout_ms) {
    try {
        while (serial_receive(buffer, 64, timeout_ms));
    } catch (...) {
    }
}


std::string ProgBob_AVRISP_priv::cmd_signOn() {
    //std::cout<<"ProgBob_AVRISP_priv::cmd_signOn"<<std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_SIGN_ON;
    unsigned int len = exchange(1, 1000);
    if (len<3) throw std::runtime_error("error: cmd_signOn #1");
    if (buf[0]!=CMD_SIGN_ON) throw std::runtime_error("error: cmd_signOn #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_signOn #3");
    if (buf[2]!=(len-3)) throw std::runtime_error("error: cmd_signOn #4");
    return std::string((char*)buf+3, (size_t)len-3);
}


uint8_t ProgBob_AVRISP_priv::cmd_getParameter(uint8_t parameterID) {
    //std::cout << "ProgBob_AVRISP::cmd_getParameter()" << std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_GET_PARAMETER;
    buf[1] = parameterID;
    unsigned int len = exchange(2, 100);
    if (len!=3) throw std::runtime_error("error: cmd_getParameter #1");
    if (buf[0]!=CMD_GET_PARAMETER) throw std::runtime_error("error: cmd_getParameter #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_getParameter #3");
    return buf[2];
}


void ProgBob_AVRISP_priv::cmd_setParameter(uint8_t parameterID, uint8_t value) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_SET_PARAMETER;
    buf[1] = parameterID;
    buf[2] = value;
    unsigned int len = exchange(3, 100);
    if (len!=2) throw std::runtime_error("error: cmd_setParameter #1");
    if (buf[0]!=CMD_SET_PARAMETER) throw std::runtime_error("error: cmd_setParameter #2");
    if (buf[1]==STATUS_CMD_FAILED) throw std::runtime_error("error: cmd_setParameter failed");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_setParameter #3");
}


void ProgBob_AVRISP_priv::cmd_osccal() {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_OSCCAL;
    unsigned int len = exchange(1, 100);
    if (len!=2) throw std::runtime_error("error: cmd_oscal #1");
    if (buf[0]!=CMD_OSCCAL) throw std::runtime_error("error: cmd_oscal #2");
    if (buf[1]==STATUS_CMD_FAILED) throw std::runtime_error("error: cmd_oscal failed");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_oscal #2");
}


void ProgBob_AVRISP_priv::cmd_loadAddress(uint32_t address) {
    //std::cout << "ProgBob_AVRISP::cmd_loadAddress()" << std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_LOAD_ADDRESS;
    buf[1] = (address>>24) & 0xff;
    buf[2] = (address>>16) & 0xff;
    buf[3] = (address>>8) & 0xff;
    buf[4] = (address>>0) & 0xff;
    unsigned int len = exchange(5, 100);
    if (len!=2) throw std::runtime_error("error: cmd_loadAddress #1");
    if (buf[0]!=CMD_LOAD_ADDRESS) throw std::runtime_error("error: cmd_loadAddress #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_loadAddress #3");
}


void ProgBob_AVRISP_priv::cmd_firmwareUpgrade() {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_FIRMWARE_UPGRADE;
    memmove (buf+1, "fwupgrade", 10);
    unsigned int len = exchange(1, 100);
    if (len!=2) throw std::runtime_error("error: cmd_firmwareUpgrade #1");
    if (buf[0]!=CMD_FIRMWARE_UPGRADE) throw std::runtime_error("error: cmd_firmwareUpgrade #2");
    if (buf[1]==STATUS_CMD_FAILED) throw std::runtime_error("error: cmd_firmwareUpgrade failed");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_firmwareUpgrade #2");
}


void ProgBob_AVRISP_priv::cmd_enterProgmodeIsp() {
    //std::cout<<"ProgBob_AVRISP_priv::cmd_enterProgmodeIsp"<<std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_ENTER_PROGMODE_ISP;
    buf[1] = cfg.isp_enter_timeout;
    buf[2] = cfg.isp_enter_stabDelay;
    buf[3] = cfg.isp_enter_cmdexeDelay;
    buf[4] = cfg.isp_enter_synchLoops;
    buf[5] = cfg.isp_enter_byteDelay;
    buf[6] = cfg.isp_enter_pollValue;
    buf[7] = cfg.isp_enter_pollIndex;
    buf[8] = cfg.isp_enter_cmd1;
    buf[9] = cfg.isp_enter_cmd2;
    buf[10] = cfg.isp_enter_cmd3;
    buf[11] = cfg.isp_enter_cmd4;
    unsigned int len = exchange(12, 2000);
    if (len!=2) throw std::runtime_error("error: cmd_enterProgmodeIsp #1");
    if (buf[0]!=CMD_ENTER_PROGMODE_ISP) throw std::runtime_error("error: cmd_enterProgmodeIsp #2");
    if (buf[1]==STATUS_CMD_TOUT) throw std::runtime_error("error: cmd_enterProgmodeIsp timeout");
    if (buf[1]==STATUS_CMD_FAILED) throw std::runtime_error("error: cmd_enterProgmodeIsp failed");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_enterProgmodeIsp #3");
    //std::cout << "ProgBob_AVRISP::cmd_enterProgmodeIsp() finished" << std::endl;
}

void ProgBob_AVRISP_priv::cmd_leaveProgmodeIsp() {
    //std::cout << "ProgBob_AVRISP::cmd_leaveProgmodeIsp()" << std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_LEAVE_PROGMODE_ISP;
    buf[1] = cfg.isp_leave_preDelay;
    buf[2] = cfg.isp_leave_postDelay;
    unsigned int len = exchange(3, 2000);
    if (len!=2) throw std::runtime_error("error: cmd_leaveProgmodeIsp #1");
    if (buf[0]!=CMD_LEAVE_PROGMODE_ISP) throw std::runtime_error("error: cmd_leaveProgmodeIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_leaveProgmodeIsp #3");
    //std::cout << "ProgBob_AVRISP::cmd_leaveProgmodeIsp() finished" << std::endl;
}

void ProgBob_AVRISP_priv::cmd_chipEraseIsp() {
    //std::cout << "ProgBob_AVRISP::cmd_chipEraseIsp()" << std::endl;
    uint8_t * buf = getBuffer();
    buf[0] = CMD_CHIP_ERASE_ISP;
    buf[1] = cfg.isp_erase_eraseDelay;
    buf[2] = cfg.isp_erase_pollMethod;
    buf[3] = cfg.isp_erase_cmd1;
    buf[4] = cfg.isp_erase_cmd2;
    buf[5] = cfg.isp_erase_cmd3;
    buf[6] = cfg.isp_erase_cmd4;
    unsigned int len = exchange(7, 5000);
    if (len!=2) throw std::runtime_error("error: cmd_chipEraseIsp #1");
    if (buf[0]!=CMD_CHIP_ERASE_ISP) throw std::runtime_error("error: cmd_chipEraseIsp #2");
    if (buf[1]==STATUS_CMD_TOUT) throw std::runtime_error("error: cmd_chipEraseIsp timeout");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_chipEraseIsp #3");
}

void ProgBob_AVRISP_priv::cmd_programFlashIsp(uint16_t numBytes, uint8_t * data) {
    //std::cout << "ProgBob_AVRISP::cmd_programFlashIsp()" << std::endl;
    if (numBytes>256) throw std::runtime_error("error: cmd_programFlashIsp size to large");
    uint8_t * buf = getBuffer();
    buf[0] = CMD_PROGRAM_FLASH_ISP;
    buf[1] = (numBytes>>8) & 0xff;
    buf[2] = (numBytes>>0) & 0xff;
    buf[3] = cfg.isp_program_flash_mode;
    buf[4] = cfg.isp_program_flash_delay;
    buf[5] = cfg.isp_program_flash_cmd1;
    buf[6] = cfg.isp_program_flash_cmd2;
    buf[7] = cfg.isp_program_flash_cmd3;
    buf[8] = cfg.isp_program_flash_poll1;
    buf[9] = cfg.isp_program_flash_poll2;
    memmove(buf+10, data, numBytes);
    unsigned int len = exchange(10+numBytes, 1000);
    if (len!=2) throw std::runtime_error("error: cmd_programFlashIsp #1");
    if (buf[0]!=CMD_PROGRAM_FLASH_ISP) throw std::runtime_error("error: cmd_programFlashIsp #2");
    if (buf[1]==STATUS_CMD_TOUT) throw std::runtime_error("error: cmd_programFlashIsp timeout");
    if (buf[1]==STATUS_RDY_BSY_TOUT) throw std::runtime_error("error: cmd_programFlashIsp busy");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programFlashIsp #3");
}


void ProgBob_AVRISP_priv::cmd_readFlashIsp(uint16_t numBytes, uint8_t * data) {
    if (numBytes>256) throw std::runtime_error("error: cmd_readFlashIsp size to large");
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_FLASH_ISP;
    buf[1] = (numBytes>>8) & 0xff;
    buf[2] = (numBytes>>0) & 0xff;
    buf[3] = cfg.isp_read_flash_cmd1;
    unsigned int len = exchange(4, 1000);
    if (len!=3+numBytes) throw std::runtime_error("error: cmd_readFlashIsp #1");
    if (buf[0]!=CMD_READ_FLASH_ISP) throw std::runtime_error("error: cmd_readFlashIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readFlashIsp #3");
    if (buf[numBytes+2]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readFlashIsp #4");
    memmove(data, buf+2, numBytes);
}

void ProgBob_AVRISP_priv::cmd_programEepromIsp(uint16_t numBytes, uint8_t * data) {
    //std::cout << "ProgBob_AVRISP::cmd_programEepromIsp()" << std::endl;
    if (numBytes>256) throw std::runtime_error("error: cmd_programEepromIsp size to large");
    uint8_t * buf = getBuffer();
    buf[0] = CMD_PROGRAM_EEPROM_ISP;
    buf[1] = (numBytes>>8) & 0xff;
    buf[2] = (numBytes>>0) & 0xff;
    buf[3] = cfg.isp_program_eeprom_mode;
    buf[4] = cfg.isp_program_eeprom_delay;
    buf[5] = cfg.isp_program_eeprom_cmd1;
    buf[6] = cfg.isp_program_eeprom_cmd2;
    buf[7] = cfg.isp_program_eeprom_cmd3;
    buf[8] = cfg.isp_program_eeprom_poll1;
    buf[9] = cfg.isp_program_eeprom_poll2;
    memmove(buf+10, data, numBytes);
    unsigned int len = exchange(10+numBytes, 1000);
    if (len!=2) throw std::runtime_error("error: cmd_programEepromIsp #1");
    if (buf[0]!=CMD_PROGRAM_FLASH_ISP) throw std::runtime_error("error: cmd_programEepromIsp #2");
    if (buf[1]==STATUS_CMD_TOUT) throw std::runtime_error("error: cmd_programEepromIsp timeout");
    if (buf[1]==STATUS_RDY_BSY_TOUT) throw std::runtime_error("error: cmd_programEepromIsp busy");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programEepromIsp #3");
}


void ProgBob_AVRISP_priv::cmd_readEepromIsp(uint16_t numBytes, uint8_t * data) {
    if (numBytes>256) throw std::runtime_error("error: cmd_readEepromIsp size to large");
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_EEPROM_ISP;
    buf[1] = (numBytes>>8) & 0xff;
    buf[2] = (numBytes>>0) & 0xff;
    buf[3] = cfg.isp_read_eeprom_cmd1;
    unsigned int len = exchange(4, 1000);
    if (len!=3+numBytes) throw std::runtime_error("error: cmd_readEepromIsp #1");
    if (buf[0]!=CMD_READ_FLASH_ISP) throw std::runtime_error("error: cmd_readEepromIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readEepromIsp #3");
    if (buf[numBytes+2]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readEepromIsp #4");
    memmove(data, buf+2, numBytes);
}


void ProgBob_AVRISP_priv::cmd_programFuseIsp(uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_PROGRAM_FUSE_ISP;
    buf[1] = cmd1;
    buf[2] = cmd2;
    buf[3] = cmd3;
    buf[4] = cmd4;
    unsigned int len = exchange(5, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_programFuseIsp #1");
    if (buf[0]!=CMD_PROGRAM_FUSE_ISP) throw std::runtime_error("error: cmd_programFuseIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programFuseIsp #3");
    if (buf[2]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programFuseIsp #4");
}


uint8_t ProgBob_AVRISP_priv::cmd_readFuseIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_FUSE_ISP;
    buf[1] = retAddr;
    buf[2] = cmd1;
    buf[3] = cmd2;
    buf[4] = cmd3;
    buf[5] = cmd4;
    unsigned int len = exchange(6, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_readFuseIsp #1");
    if (buf[0]!=CMD_READ_FUSE_ISP) throw std::runtime_error("error: cmd_readFuseIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readFuseIsp #3");
    if (buf[3]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readFuseIsp #4");
    return buf[2];
}


void ProgBob_AVRISP_priv::cmd_programLockIsp(uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_PROGRAM_LOCK_ISP;
    buf[1] = cmd1;
    buf[2] = cmd2;
    buf[3] = cmd3;
    buf[4] = cmd4;
    unsigned int len = exchange(5, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_programLockIsp #1");
    if (buf[0]!=CMD_PROGRAM_LOCK_ISP) throw std::runtime_error("error: cmd_programLockIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programLockIsp #3");
    if (buf[2]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_programLockIsp #4");
}


uint8_t ProgBob_AVRISP_priv::cmd_readLockIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_LOCK_ISP;
    buf[1] = retAddr;
    buf[2] = cmd1;
    buf[3] = cmd2;
    buf[4] = cmd3;
    buf[5] = cmd4;
    unsigned int len = exchange(6, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_readLockIsp #1");
    if (buf[0]!=CMD_READ_LOCK_ISP) throw std::runtime_error("error: cmd_readLockIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readLockIsp #3");
    if (buf[3]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readLockIsp #4");
    return buf[2];
}


uint8_t ProgBob_AVRISP_priv::cmd_readSignatureIsp(uint8_t addr) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_SIGNATURE_ISP;
    buf[1] = cfg.isp_read_signature_RetAddr;
    buf[2] = cfg.isp_read_signature_cmd1;
    buf[3] = cfg.isp_read_signature_cmd2;
    buf[4] = cfg.isp_read_signature_cmd3 | addr;
    buf[5] = cfg.isp_read_signature_cmd4;
    unsigned int len = exchange(6, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_readSignatureIsp #1");
    if (buf[0]!=CMD_READ_SIGNATURE_ISP) throw std::runtime_error("error: cmd_readSignatureIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readSignatureIsp #3");
    if (buf[3]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readSignatureIsp #4");
    return buf[2];
}


uint8_t ProgBob_AVRISP_priv::cmd_readOsccalIsp(uint8_t retAddr, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_READ_OSCCAL_ISP;
    buf[1] = retAddr;
    buf[2] = cmd1;
    buf[3] = cmd2;
    buf[4] = cmd3;
    buf[5] = cmd4;
    unsigned int len = exchange(6, 1000);
    if (len!=4) throw std::runtime_error("error: cmd_readOsccalIsp #1");
    if (buf[0]!=CMD_READ_OSCCAL_ISP) throw std::runtime_error("error: cmd_readOsccalIsp #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readOsccalIsp #3");
    if (buf[3]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_readOsccalIsp #4");
    return buf[2];
}


uint8_t ProgBob_AVRISP_priv::cmd_spiMulti(uint8_t numTx, uint8_t numRx, uint8_t rxStartAddr, uint8_t * data) {
    uint8_t * buf = getBuffer();
    buf[0] = CMD_SPI_MULTI;
    buf[1] = numTx;
    buf[2] = numRx;
    buf[3] = rxStartAddr;
    memmove(buf+4, data, numTx);
    unsigned int len = exchange(4+numTx, 1000);
    if (len!=3+numRx) throw std::runtime_error("error: cmd_spiMulti #1");
    if (buf[0]!=CMD_SPI_MULTI) throw std::runtime_error("error: cmd_spiMulti #2");
    if (buf[1]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_spiMulti #3");
    if (buf[2+numRx]!=STATUS_CMD_OK) throw std::runtime_error("error: cmd_spiMulti #4");
    memmove(data, buf+2, numRx);
}



void ProgBob_AVRISP_priv::updateBlockProgress(const std::string & msg1, const std::string & msg2, uint32_t address, unsigned int page, unsigned int pagecnt, bool replace) {
    std::string txt = msg1 + " 0x"+uint2hex(address, 4)+" ("+uint2str(page)+"/"+uint2str(pagecnt)+")";
    if (msg2!="") txt+= " "+msg2;
    //doMessage(0, txt, replace);
    //doAvrdudeProgress((100*page)/pagecnt, 0);
    progress_callback(page, pagecnt, txt, replace);
}


//////////////////////// public OBJECT


ProgBob_AVRISP::ProgBob_AVRISP(progress_fn callback) : ProgrammerBase(), priv(new ProgBob_AVRISP_priv()) { 
    priv->serialport = 0;
    priv->progress_callback = callback;
    //std::cout << "serport count = " << priv->spc.getNoPorts() << std::endl;
}


ProgBob_AVRISP::~ProgBob_AVRISP() {
    delete priv->serialport;
    delete priv;
}


void ProgBob_AVRISP::connect(const std::string & port) {
    if (port=="") {
        if (priv->serialport) {
            // 1.) reset baudrate, 2.) close
            priv->serialport->setBaudrate(19200);
            delete priv->serialport;
            priv->serialport = 0;
        }
    } else {
        int index = priv->spc.findPort(port.c_str());
        //std::cout << "serport index for '" << port << "' = " << index << std::endl;
        priv->serialport = priv->spc.open(port.c_str(), 115200);
        if (!priv->serialport) throw std::runtime_error("Unable to open serial port!");

        priv->drain_serial(10);
        std::string name = priv->cmd_signOn();
        if (name!="AVRISP_2") throw std::runtime_error("Unknown programmer type '"+name+"'!");
        priv->cmd_getParameter(0x90);
        priv->cmd_getParameter(0x91);
        priv->cmd_getParameter(0x92);
        priv->cmd_getParameter(0x93);
        priv->cmd_getParameter(0x94);
    }
}

void ProgBob_AVRISP::configure(const std::string & part) {
    dude_loadAvrProgrammerConfig(priv->cfg, part);
}

void ProgBob_AVRISP::signalError() {
    //std::cout << "ProgBob_AVRISP::signalError()" << std::endl;
    //priv->signalError();
}

std::string ProgBob_AVRISP::getStatus() {
    //std::cout << "ProgBob_AVRISP::getStatus()" << std::endl;
    return "";
}


void ProgBob_AVRISP::reset() {
    std::cout << "ProgBob_AVRISP::reset()" << std::endl;
}


void ProgBob_AVRISP::program(const std::string & segment, unsigned int startAddress, const std::vector<uint8_t> & _memory) {
    priv->doProgress(0, 100);
    if (segment=="flash") {
        unsigned int pagesize = priv->cfg.part_flash_pagesize;
        unsigned int pagecount = priv->cfg.part_flash_pagecount;
        uint32_t pagemask = ~((uint32_t)pagesize-1);
        std::vector<uint8_t> memory(_memory);
        int progpages = ((memory.size()-1)/pagesize) + 1;
        memory.resize(progpages*pagesize, 0xff);
        unsigned int endAddress = startAddress+memory.size();
        unsigned int pageAddress = startAddress&pagemask;
        if (pageAddress!=startAddress) throw std::runtime_error("Flash programmming must be aligned to page boundaries!");
        unsigned int pageAddressLast = (endAddress-1)&pagemask;
        priv->updateBlockProgress("programming flash block", "", pageAddress, 0, progpages, false);
        for (unsigned int i=0; i<progpages; i++) {
            priv->updateBlockProgress("programming flash block", "", pageAddress, i+1, progpages, true);
            priv->cmd_loadAddress(pageAddress/2); // 16 bit address
            priv->cmd_programFlashIsp(pagesize, memory.data()+pageAddress);
            pageAddress += pagesize;
        }
        priv->updateBlockProgress("programming flash block", "finished", pageAddress, progpages, progpages, true);
    }
    priv->doProgress(100, 100);
}


void ProgBob_AVRISP::verify(const std::string & segment, unsigned int startAddress, const std::vector<uint8_t> & _memory) {
    priv->doProgress(0, 100);
    if (segment=="flash") {
        unsigned int pagesize = priv->cfg.part_flash_pagesize;
        unsigned int pagecount = priv->cfg.part_flash_pagecount;
        uint32_t pagemask = ~((uint32_t)pagesize-1);
        std::vector<uint8_t> memory(_memory);
        int progpages = ((memory.size()-1)/pagesize) + 1;
        memory.resize(progpages*pagesize, 0xff);
        std::vector<uint8_t> pagemem(0);
        pagemem.resize(pagesize, 0xff);
        unsigned int endAddress = startAddress+memory.size();
        unsigned int pageAddress = startAddress&pagemask;
        if (pageAddress!=startAddress) throw std::runtime_error("Flash programmming must be aligned to page boundaries!");
        unsigned int pageAddressLast = (endAddress-1)&pagemask;
        priv->updateBlockProgress("validating flash block", "", pageAddress, 0, progpages, false);
        for (unsigned int i=0; i<progpages; i++) {
            priv->updateBlockProgress("validating flash block", "", pageAddress, i+1, progpages, true);
            priv->cmd_loadAddress(pageAddress/2); // 16 bit address
            priv->cmd_readFlashIsp(pagesize, pagemem.data());
            for (uint32_t i=0; i<pagesize; i++) {
                if (pagemem[i]!=memory[i+pageAddress]) {
                    throw std::runtime_error("Verify error!");
                }
            }
            pageAddress += pagesize;
        }
        priv->updateBlockProgress("validating flash block", "finished", pageAddress, progpages, progpages, true);
    }
    priv->doProgress(100, 100);
}

void ProgBob_AVRISP::startProgramMode(const std::string & mode, bool erase) {
    priv->doProgress("enter programming mode", false);
    priv->cmd_enterProgmodeIsp();
}

void ProgBob_AVRISP::erase() {
    priv->doProgress("erasing chip", false);
    priv->cmd_chipEraseIsp();
    priv->cmd_enterProgmodeIsp();
}


void ProgBob_AVRISP::leaveProgramMode() {
    priv->doProgress("leaving programming mode", false);
    priv->cmd_leaveProgmodeIsp();
}

void ProgBob_AVRISP::checkSignature() {
    uint32_t sig = 0;
    sig |= priv->cmd_readSignatureIsp(0);
    sig <<= 8;
    sig |= priv->cmd_readSignatureIsp(1);
    sig <<= 8;
    sig |= priv->cmd_readSignatureIsp(2);
    
    if (sig!=priv->cfg.part_signature) {
        throw std::runtime_error("Wrong chip signature: "+uint2hex(sig, 6));
    }
    
    priv->doProgress("chip signature " + uint2hex(sig, 6) + " -> " + priv->cfg.name, false);
}
