/***************************************************************************
 *   Copyright (C) 2020 by Santiago González                               *
 *                                                                         *
 ***( see copyright.txt file at root folder )*******************************/
/*
 *   Based on simavr decoder
 *
 *   Copyright 2008, 2010 Michel Pollet <buserror@gmail.com>
 *
 */

#include "avrcore.h"
#include "avr_defines.h"
#include "avrsleep.h"
#include "simulator.h"
#include "datautils.h"

AvrCore::AvrCore( eMcu* mcu )
       : Mcu8bits( mcu )
{
    if( mcu->regExist("EIND") ) EIND = m_mcu->getReg( "EIND" ); 
    else EIND = nullptr; 

    if( mcu->regExist("RAMPZ") )
    {
        RAMPZ = m_mcu->getReg( "RAMPZ" ); 
        m_rampzAddr = m_mcu->getRegAddress( "RAMPZ" ); 
    }
    else RAMPZ = nullptr; 

    m_retCycles = 4;  // In AVR only used for Jump to ISR

    m_bootStart = 0;  // Bootloader start address
    m_pageSize = mcu->pgmPage(); 
    m_tmpUsed.resize( m_pageSize, 0 ); 
    m_tmpPage.resize( m_pageSize, 0 ); 

    // SPMCSR
    m_SELFPRGEN = getRegBits("SELFPRGEN", mcu ); 
    m_PGERS = getRegBits("PGERS", mcu ); 
    m_PGWRT = getRegBits("PGWRT", mcu ); 

}
AvrCore::~AvrCore() {}

void AvrCore::reset()
{
    Mcu8bits::reset(); 

    uint16_t ramEnd = m_dataMemEnd-1; 
    if( m_spl ) *m_spl = ramEnd; 
    if( m_sph ) *m_sph = ramEnd >> 8; 
}

inline void AvrCore::flags_ns( uint8_t res )
{
    bool sn = res & (1<<7); 
    write_S_Bit( S_N, sn ); 
    write_S_Bit( S_S, sn != (bool)STATUS(S_V) ); 
}
inline void AvrCore::flags_zns( uint8_t res )
{
    write_S_Bit( S_Z, (res == 0) ); 
    flags_ns( res ); 
}
inline void AvrCore::flags_Rzns( uint8_t res )
{
    if( res ) clear_S_Bit( S_Z ); 
    flags_ns( res ); 
}
inline void AvrCore::flags_sub( uint8_t res, uint8_t rd, uint8_t rr )
{
    /* carry & half carry */
    uint8_t sub_carry = (~rd & rr) | (rr & res) | (res & ~rd); 

    write_S_Bit( S_H, sub_carry & (1<<3) ); 
    write_S_Bit( S_C, sub_carry & (1<<7) ); 

    /* overflow */
    /// SREG[S_V] = ( ((rd & ~rr & ~res) | (~rd & rr & res) ) >> 7) & 1; 
    write_S_Bit( S_V, ((rd & ~rr & ~res) | (~rd & rr & res)) & (1<<7) ); 
}
inline void AvrCore::flags_sub_Rzns( uint8_t res, uint8_t rd, uint8_t rr )
{
    flags_sub( res, rd, rr ); 
    flags_Rzns( res ); 
}
inline void AvrCore::flags_add_zns( uint8_t res, uint8_t rd, uint8_t rr )
{
    /* carry & half carry */
    uint8_t add_carry = (rd & rr) | (rr & ~res) | (~res & rd); 

    write_S_Bit( S_H, add_carry & (1<<3) ); 
    write_S_Bit( S_C, add_carry & (1<<7) ); 

    /// SREG[S_V] = (((rd & rr & ~res) | (~rd & ~rr & res)) >> 7) & 1;  //overflow
    write_S_Bit( S_V, ((rd & rr & ~res) | (~rd & ~rr & res)) & (1<<7) ); 

    flags_zns( res );  //zns
}
inline void AvrCore::flags_sub_zns( uint8_t res, uint8_t rd, uint8_t rr )
{
    flags_sub( res, rd, rr ); 
    flags_zns( res );  //zns
}
inline void AvrCore::flags_znv0s( uint8_t res )
{
    clear_S_Bit( S_V ); 
    flags_zns( res ); 
}
inline void AvrCore::flags_zcnvs( uint8_t res, uint8_t vr )
{
    write_S_Bit( S_Z, (res == 0) ); 

    bool sc = (vr & 1)  != 0 ; 
    write_S_Bit( S_C, sc ); 

    bool sn = (res & (1<<7)) != 0; 
    write_S_Bit( S_N, sn ); 

    /// SREG[S_V] = SREG[S_N] ^ SREG[S_C]; 
    uint8_t sv = sn != sc; 
    write_S_Bit( S_V, sv ); 

    /// SREG[S_S] = SREG[S_N] ^ SREG[S_V]; 
    write_S_Bit( S_S, sn != sv ); 
}
inline void AvrCore::flags_zcvs( uint8_t res, uint8_t vr )
{
    write_S_Bit( S_Z, (res == 0) ); 

    bool sc = (vr & 1)  != 0 ; 
    write_S_Bit( S_C, sc ); 

    bool sn = STATUS( S_N ) != 0; 

    /// SREG[S_V] = SREG[S_N] ^ SREG[S_C]; 
    bool sv = sn != sc; 
    write_S_Bit( S_V, sv ); 

    /// SREG[S_S] = SREG[S_N] ^ SREG[S_V]; 
    write_S_Bit( S_S, sn != sv ); 
}
inline void AvrCore::flags_zns16( uint16_t res )
{
    write_S_Bit( S_Z, (res == 0) ); 

    bool sn = (res & (1<<15)) != 0 ; 
    write_S_Bit( S_N, sn ); 

    bool sv = STATUS(S_V) != 0; 
    /// SREG[S_S] = SREG[S_N] ^ SREG[S_V]; 
    write_S_Bit( S_S, sn != sv ); 
}
inline int AvrCore::is_instr_32b( uint32_t pc )
{
    uint16_t o = m_progMem[pc] & 0xfc0f; 

    return  o == 0x9200 || // STS ! Store Direct to Data Space
            o == 0x9000 || // LDS Load Direct from Data Space
            o == 0x940c || // JMP Long Jump
            o == 0x940d || // JMP Long Jump
            o == 0x940e || // CALL Long Call to sub
            o == 0x940f;    // CALL Long Call to sub
}

void AvrCore::writeFlash()
{
    if( !m_pageSize ) return; 
    if( !getRegBitsBool( m_SELFPRGEN ) ) return; 

    uint16_t z = m_dataMem[R_ZL] | (m_dataMem[R_ZH] << 8); 
    if( RAMPZ ) z |= *RAMPZ << 16; 

    //avr_cycle_timer_cancel(avr, avr_progen_clear, p); 

    if( getRegBitsBool( m_PGERS ) )
    {
        //z &= ~1; 
        z >>= 1; 
        qDebug() <<"FLASH: Erasing page"<< z/m_pageSize << "at address" << z << "words:"<< m_pageSize; 
        for( int i=0;  i<m_pageSize;  i++ )
            m_progMem[z++] = 0xff; 
    }
    else if( getRegBitsBool( m_PGWRT ) )
    {
        z &= ~(m_pageSize - 1); 
        z >>= 1; 
        qDebug() << "FLASH: Writing page "<< z/m_pageSize << "at address" << z ; 

        for( int i=0;  i<m_pageSize;  i++)
        {
            m_progMem[z++] = m_tmpPage[i]; 
            m_tmpPage[i] = 0; 
            m_tmpUsed[i] = 0; 
        }
    }
    /*else if( avr_regbit_get(avr, p->blbset) )
    {
        qDebug() <<"FLASH: Setting lock bits (ignored)"; 
    }*/
    /*else if( p->flags & AVR_SELFPROG_HAVE_RWW && avr_regbit_get(avr, p->rwwsre))
    {
        for( int i=0;  i<m_pageSize;  i++){
            m_tmpUsed[i] = 0; 
            m_tmpPage[i] = 0; 
        }
    }*/
    else {
        uint16_t r01 = m_dataMem[0] | (m_dataMem[1] << 8); 
        z >>= 1; 
        uint16_t addr = z % m_pageSize; 
        qDebug() <<"FLASH: Writing temppage at address"<< addr << "value"<< r01; 

        if( !m_tmpUsed[addr] )
        {
            m_tmpUsed[addr] = 1; 
            m_tmpPage[addr] = r01; 
        }
    }
    clearRegBits( m_SELFPRGEN ); 
}

void AvrCore::runStep()
{
    m_mcu->cyclesDone = 0; 
    uint16_t instruction = m_progMem[m_PC]; 

    uint32_t new_pc = m_PC + 1;     // future "default" pc
    //m_RET_ADDR = -1;  //new_pc; 
    int cycle = 1; 

    switch( instruction & 0xf000 )
    {
        case 0x0000:{
            switch( instruction){
                case 0x0000: {    // NOP
                }    break; 
                default:
                {
                    switch( instruction & 0xfc00) {
                        case 0x0400: {    // CPC -- Compare with carry -- 0000 01rd dddd rrrr
                            get_vd5_vr5( instruction ); 
                            uint8_t res = vd - vr - STATUS( S_C ); 
                            flags_sub_Rzns( res, vd, vr ); 
                        }    break; 
                        case 0x0c00: {    // ADD -- Add without carry -- 0000 11rd dddd rrrr
                            get_vd5_vr5( instruction ); 
                            uint8_t res = vd + vr; 
                            m_dataMem[d] = res; 
                            flags_add_zns( res, vd, vr); 
                        }    break; 
                        case 0x0800: {    // SBC -- Subtract with carry -- 0000 10rd dddd rrrr
                            get_vd5_vr5( instruction ); 
                            uint8_t res = vd - vr - STATUS( S_C ); 
                            m_dataMem[d] = res; 
                            flags_sub_Rzns( res, vd, vr); 
                        }    break; 
                        default: {
                            switch( instruction & 0xff00) {
                                case 0x0100: {    // MOVW -- Copy Register Word -- 0000 0001 dddd rrrr
                                    uint8_t d =( (instruction >> 4) & 0xf) << 1; 
                                    uint8_t r =( instruction & 0xf) << 1; 
                                    uint16_t vr = m_dataMem[r]|( m_dataMem[r+1] << 8); 
                                    SET_REG16_LH( d, vr ); 
                                }    break; 
                                case 0x0200: {    // MULS -- Multiply Signed -- 0000 0010 dddd rrrr
                                    int8_t r = 16 +( instruction & 0xf); 
                                    int8_t d = 16 +( (instruction >> 4) & 0xf); 
                                    int16_t res =( (int8_t)m_dataMem[r]) *( (int8_t)m_dataMem[d]); 
                                    SET_REG16_LH( 0, res); 
                                    /// SREG[S_C] =( res >> 15) & 1; 
                                    write_S_Bit( S_C, res & 1<<15 ); 
                                    write_S_Bit( S_Z, res == 0 ); 
                                    cycle++; 
                                }    break; 
                                case 0x0300: {    // MUL -- Multiply -- 0000 0011 fddd frrr
                                    int8_t r = 16 +( instruction & 0x7); 
                                    int8_t d = 16 +( (instruction >> 4) & 0x7); 
                                    int16_t res = 0; 
                                    uint8_t c = 0; 

                                    switch( instruction & 0x88) {
                                        case 0x00:     // MULSU -- Multiply Signed Unsigned -- 0000 0011 0ddd 0rrr
                                            res =( (uint8_t)m_dataMem[r]) *( (int8_t)m_dataMem[d]); 
                                            c =( res >> 15) & 1; 
                                            break; 
                                        case 0x08:     // FMUL -- Fractional Multiply Unsigned -- 0000 0011 0ddd 1rrr
                                            res =( (uint8_t)m_dataMem[r]) *( (uint8_t)m_dataMem[d]); 
                                            c =( res >> 15) & 1; 
                                            res <<= 1; 
                                            break; 
                                        case 0x80:     // FMULS -- Multiply Signed -- 0000 0011 1ddd 0rrr
                                            res =( (int8_t)m_dataMem[r]) *( (int8_t)m_dataMem[d]); 
                                            c =( res >> 15) & 1; 
                                            res <<= 1; 
                                            break; 
                                        case 0x88:     // FMULSU -- Multiply Signed Unsigned -- 0000 0011 1ddd 1rrr
                                            res =( (uint8_t)m_dataMem[r]) *( (int8_t)m_dataMem[d]); 
                                            c =( res >> 15) & 1; 
                                            res <<= 1; 
                                            break; 
                                    }
                                    cycle++; 
                                    SET_REG16_LH( 0, res); 
                                    write_S_Bit( S_C, c ); 
                                    write_S_Bit( S_Z, res == 0 ); 
                                }    break; 
                                default: ; //_avr_invalid_instruction(avr); 
                            }
                        }
                    }
                }
            }
        }    break; 

        case 0x1000: {
            switch( instruction & 0xfc00) {
                case 0x1800: {    // SUB -- Subtract without carry -- 0001 10rd dddd rrrr
                    get_vd5_vr5( instruction ); 
                    uint8_t res = vd - vr; 
                    m_dataMem[d] = res; 
                    flags_sub_zns( res, vd, vr); 
                }    break; 
                case 0x1000: {    // CPSE -- Compare, skip if equal -- 0001 00rd dddd rrrr
                    get_vd5_vr5( instruction ); 
                    bool res = vd == vr; 
                    if( res )
                    {
                        if( is_instr_32b( new_pc ) ) { new_pc += 2;  cycle += 2;  }
                        else                         { new_pc += 1;  cycle++;  }
                    }
                }    break; 
                case 0x1400: {    // CP -- Compare -- 0001 01rd dddd rrrr
                    get_vd5_vr5( instruction ); 
                    uint8_t res = vd - vr; 
                    flags_sub_zns( res, vd, vr); 
                }    break; 
                case 0x1c00: {    // ADC -- Add with carry -- 0001 11rd dddd rrrr
                    get_vd5_vr5( instruction ); 
                    uint8_t res = vd + vr + STATUS( S_C ); 
                    m_dataMem[d] = res; 
                    flags_add_zns( res, vd, vr ); 
                }    break; 
                default: ; //_avr_invalid_instruction(avr); 
            }
        }    break; 

        case 0x2000:
        {
            get_vd5_vr5( instruction ); 
            uint8_t res = vr; 
            bool znv = true; 
            switch( instruction & 0xfc00)
            {
                case 0x2000: {    // AND -- Logical AND -- 0010 00rd dddd rrrr
                    res &= vd; 
                }    break; 
                case 0x2400: {    // EOR -- Logical Exclusive OR -- 0010 01rd dddd rrrr
                    res ^= vd; 
                }    break; 
                case 0x2800: {    // OR -- Logical OR -- 0010 10rd dddd rrrr
                    res |= vd; 
                }    break; 
                case 0x2c00: {    // MOV -- 0010 11rd dddd rrrr
                    //res = vr; 
                    znv = false; 
                }    break; 
                default: ; //_avr_invalid_instruction(avr); 
            }
            if( znv ) flags_znv0s( res); 
            m_dataMem[d] = res; 
        }    break; 

        case 0x3000: {    // CPI -- Compare Immediate -- 0011 kkkk hhhh kkkk
            get_vh4_k8( instruction ); 
            uint8_t res = vh - k; 
            flags_sub_zns( res, vh, k); 
        }    break; 

        case 0x4000: {    // SBCI -- Subtract Immediate With Carry -- 0100 kkkk hhhh kkkk
            get_vh4_k8( instruction ); 
            uint8_t res = vh - k - STATUS( S_C ); 
            m_dataMem[h] = res; 
            flags_sub_Rzns( res, vh, k); 
        }    break; 

        case 0x5000: {    // SUBI -- Subtract Immediate -- 0101 kkkk hhhh kkkk
            get_vh4_k8( instruction ); 
            uint8_t res = vh - k; 
            m_dataMem[h] = res; 
            flags_sub_zns( res, vh, k); 
        }    break; 

        case 0x6000: {    // ORI aka SBR -- Logical OR with Immediate -- 0110 kkkk hhhh kkkk
            get_vh4_k8( instruction ); 
            uint8_t res = vh | k; 
            m_dataMem[h] = res; 
            flags_znv0s( res); 
        }    break; 

        case 0x7000: {    // ANDI    -- Logical AND with Immediate -- 0111 kkkk hhhh kkkk
            get_vh4_k8( instruction ); 
            uint8_t res = vh & k; 
            m_dataMem[h] = res; 
            flags_znv0s( res ); 
        }    break; 

        case 0xa000:
        case 0x8000: {
            /*
             * Load( LDD/STD) store instructions
             *
             * 10q0 qqsd dddd yqqq
             * s = 0 = load, 1 = store
             * y = 16 bits register index, 1 = Y, 0 = X
             * q = 6 bit displacement
             */
            uint16_t v = 0; 
            switch( instruction & 0xd008 )
            {
                case 0xa000:
                case 0x8000: {    // LD( LDD) -- Load Indirect using Z -- 10q0 qqsd dddd yqqq
                    v = m_dataMem[R_ZL] | ( m_dataMem[R_ZH] << 8); 
                }    break; 
                case 0xa008:
                case 0x8008: {    // LD( LDD) -- Load Indirect using Y -- 10q0 qqsd dddd yqqq
                    v = m_dataMem[R_YL] | ( m_dataMem[R_YH] << 8); 
                }    break; 
                //default: ; //_avr_invalid_instruction(avr); 
            }
            get_d5_q6( instruction ); 
            if( instruction & 0x0200) SET_RAM( v+q, m_dataMem[d] ); 
            else                      SET_RAM( d, GET_RAM(v+q) ); 
            cycle += 1;  // 2 cycles, 3 for tinyavr
        }    break; 

        case 0x9000: {
            // SREG set/clear instructions
            // SEH,SEI,SEN,SES,SET,SEV,SEZ;  CLH,CLI,CLN,CLS,CLT,CLV,CLZ
            if( (instruction & 0xff0f) == 0x9408 )
            {
                uint8_t bit = (instruction >> 4) & 7; 
                bool set = (instruction & 0x0080) == 0; 
                write_S_Bit( bit, set ); 
                if( bit == S_I ) m_mcu->enableInterrupts( set ); 
            }
            switch( instruction )
            {
                case 0x9588: { // SLEEP -- 1001 0101 1000 1000
                    /* Don't sleep if there are interrupts about to be serviced.
                     * Without this check, it was possible to incorrectly enter a state
                     * in which the cpu was sleeping and interrupts were disabled. For more
                     * details, see the commit message. */
                    qDebug() <<"Warning: AVR SLEEP instruction not Fully implemented"; 
////////             if( !int_pending.empty() || !SREG[S_I]) state = cpu_Sleeping; 

                    m_mcu->sleep( true ); 
                }    break; 
                case 0x9598: { // BREAK -- 1001 0101 1001 1000
                    qDebug() <<"ERROR: AVR BREAK instruction not implemented"; 
                }    break; 
                case 0x95a8: { // WDR -- Watchdog Reset -- 1001 0101 1010 1000
                    m_mcu->wdr(); 
                }    break; 
                case 0x95e8: { // SPM -- Store Program Memory -- 1001 0101 1110 1000
                    //qDebug() <<"ERROR: AVR SPM instruction not implemented"; 
                    if( m_PC >= m_bootStart ) writeFlash(); 
                    else{
                        qDebug() <<"ERROR: AVR SPM instruction from PGM address"<< m_PC; 
                        qDebug() <<"Boot start:" << m_bootStart; 
                    }
                }    break; 
                case 0x9409:   // IJMP   -- Indirect jump -- 1001 0100 0000 1001
                case 0x9419:   // EIJMP  -- Indirect jump -- 1001 0100 0001 1001   bit 4 is "Extended"
                case 0x9509:   // ICALL  -- Indirect Call to Subroutine -- 1001 0101 0000 1001
                case 0x9519: { // EICALL -- Indirect Call to Subroutine -- 1001 0101 0001 1001   bit 8 is "Call: push pc"
                    int exte = instruction & 0x10;   // Extended
                    int call = instruction & 0x100;  // Call: push pc
                    uint32_t z = m_dataMem[R_ZL] | (m_dataMem[R_ZH] << 8); 
                    if( exte ){
                        if( !EIND ){
                            qDebug() << "ERROR: AVR Invalid instruction: EICALL with no EIND"; 
                            break; 
                        }
                        z |= *EIND << 16; 
                    }
                    if( call ){
                        PUSH_STACK( new_pc ); 
                        m_RET_ADDR = new_pc; 
                        cycle += m_progAddrSize-1; 
                    }
                    new_pc = z; 
                    cycle++; 
                }    break; 
                case 0x9518:     // RETI -- Return from Interrupt -- 1001 0101 0001 1000
                    m_mcu->interrupts()->retI(); // SREG flag managed in AvrInterrupt
                case 0x9508: {   // RET -- Return -- 1001 0101 0000 1000
                    new_pc = POP_STACK(); 
                    cycle += 1 + m_progAddrSize; 
                }    break; 
                case 0x95c8: {    // LPM -- Load Program Memory R0 <-( Z) -- 1001 0101 1100 1000
                    uint16_t z = m_dataMem[R_ZL] |( m_dataMem[R_ZH] << 8); 
                    cycle += 2;  // 3 cycles
                    uint16_t prgData = m_progMem[z/2]; 
                    if( z&1 ) prgData >>= 8; 
                    m_dataMem[0] = prgData & 0xFF; 
                }    break; 
                case 0x95d8: {    // ELPM -- Load Program Memory R0 <-( Z) -- 1001 0101 1101 1000
                    if( !RAMPZ){
                        qDebug() << "ERROR: AVR Invalid instruction: ELPM with no RAMPZ"; 
                        break; 
                    }
                    uint32_t z = m_dataMem[R_ZL] |( m_dataMem[R_ZH] << 8) | (*RAMPZ << 16); 
                    uint16_t prgData = m_progMem[z/2]; 
                    if( z&1 ) prgData >>= 8; 
                    m_dataMem[0] = prgData & 0xFF; 
                    cycle += 2;  // 3 cycles
                }    break; 
                default:  {
                    switch( instruction & 0xfe0f) {
                        case 0x9000: {    // LDS -- Load Direct from Data Space, 32 bits -- 1001 0000 0000 0000
                            get_d5( instruction ); 
                            uint16_t x = m_progMem[new_pc]; 
                            new_pc += 1; 
                            m_dataMem[d] = GET_RAM(x); 
                            cycle++;  // 2 cycles
                        }    break; 
                        case 0x9005:
                        case 0x9004: {    // LPM -- Load Program Memory -- 1001 000d dddd 01oo
                            get_d5( instruction ); 
                            uint16_t z = m_dataMem[R_ZL] | (m_dataMem[R_ZH] << 8); 
                            int op = instruction & 1; 
                            uint16_t prgData = m_progMem[z/2]; 
                            if( z&1 ) prgData >>= 8; 
                            m_dataMem[d] = prgData & 0xFF; 
                            if( op) SET_REG16_HL( R_ZL, ++z ); 
                            cycle += 2;  // 3 cycles
                        }    break; 
                        case 0x9006:
                        case 0x9007: {    // ELPM -- Extended Load Program Memory -- 1001 000d dddd 01oo
                            if( !RAMPZ){
                                qDebug() << "ERROR: AVR Invalid instruction: ELPM with no RAMPZ"; 
                                break; 
                            }
                            uint16_t z = m_dataMem[R_ZL] |( m_dataMem[R_ZH] << 8) | (*RAMPZ << 16); 
                            get_d5( instruction ); 
                            int op = instruction & 1; 
                            uint16_t prgData = m_progMem[z/2]; 
                            if( z&1 ) prgData >>= 8; 
                            m_dataMem[d] = prgData & 0xFF; 
                            if( op) {
                                z++; 
                                m_dataMem[m_rampzAddr] = z >> 16; 
                                SET_REG16_HL( R_ZL, z ); 
                            }
                            cycle += 2;  // 3 cycles
                        }    break; 
                        /*
                         * Load store instructions
                         *
                         * 1001 00sr rrrr iioo
                         * s = 0 = load, 1 = store
                         * ii = 16 bits register index, 11 = X, 10 = Y, 00 = Z
                         * oo = 1) post increment, 2) pre-decrement
                         */
                        case 0x900c:
                        case 0x900d:
                        case 0x900e: {    // LD -- Load Indirect from Data using X -- 1001 000d dddd 11oo
                            int op = instruction & 3; 
                            get_d5( instruction ); 
                            uint16_t x = (m_dataMem[R_XH] << 8) | m_dataMem[R_XL]; 
                            cycle++;  // 2 cycles( 1 for tinyavr, except with inc/dec 2)
                            if( op == 2) x--; 
                            uint8_t vd = GET_RAM(x); 
                            if( op == 1) x++; 
                            SET_REG16_HL( R_XL, x); 
                            m_dataMem[d] = vd; 
                        }    break; 
                        case 0x920c:
                        case 0x920d:
                        case 0x920e: {    // ST -- Store Indirect Data Space X -- 1001 001d dddd 11oo
                            int op = instruction & 3; 
                            get_vd5( instruction ); 
                            uint16_t x =( m_dataMem[R_XH] << 8) | m_dataMem[R_XL]; 
                            cycle++;  // 2 cycles, except tinyavr
                            if( op == 2) x--; 
                            SET_RAM( x, vd ); 
                            if( op == 1) x++; 
                            SET_REG16_HL( R_XL, x); 
                        }    break; 
                        case 0x9009:
                        case 0x900a: {    // LD -- Load Indirect from Data using Y -- 1001 000d dddd 10oo
                            int op = instruction & 3; 
                            get_d5( instruction ); 
                            uint16_t y =( m_dataMem[R_YH] << 8) | m_dataMem[R_YL]; 
                            cycle++;  // 2 cycles, except tinyavr
                            if( op == 2) y--; 
                            uint8_t vd = GET_RAM(y); 
                            if( op == 1) y++; 
                            SET_REG16_HL( R_YL, y); 
                            m_dataMem[d] = vd; 
                        }    break; 
                        case 0x9209:
                        case 0x920a: {    // ST -- Store Indirect Data Space Y -- 1001 001d dddd 10oo
                            int op = instruction & 3; 
                            get_vd5( instruction ); 
                            uint16_t y =( m_dataMem[R_YH] << 8) | m_dataMem[R_YL]; 
                             cycle++; 
                            if( op == 2) y--; 
                            SET_RAM( y, vd ); 
                            if( op == 1) y++; 
                            SET_REG16_HL( R_YL, y); 
                        }    break; 
                        case 0x9200: {    // STS -- Store Direct to Data Space, 32 bits -- 1001 0010 0000 0000
                            get_vd5( instruction ); 
                            uint16_t x = m_progMem[new_pc]; 
                            new_pc += 1; 
                            cycle++; 
                            SET_RAM( x, vd ); 
                        }    break; 
                        case 0x9001:
                        case 0x9002: {    // LD -- Load Indirect from Data using Z -- 1001 000d dddd 00oo
                            int op = instruction & 3; 
                            get_d5( instruction ); 
                            uint16_t z =( m_dataMem[R_ZH] << 8) | m_dataMem[R_ZL]; 
                            cycle++; ;  // 2 cycles, except tinyavr
                            if( op == 2) z--; 
                            uint8_t vd = GET_RAM(z); 
                            if( op == 1) z++; 
                            SET_REG16_HL( R_ZL, z); 
                            m_dataMem[d] = vd; 
                        }    break; 
                        case 0x9201:
                        case 0x9202: {    // ST -- Store Indirect Data Space Z -- 1001 001d dddd 00oo
                            int op = instruction & 3; 
                            get_vd5( instruction ); 
                            uint16_t z =( m_dataMem[R_ZH] << 8) | m_dataMem[R_ZL]; 
                             cycle++;  // 2 cycles, except tinyavr
                            if( op == 2) z--; 
                            SET_RAM( z, vd ); 
                            if( op == 1 ) z++; 
                            SET_REG16_HL( R_ZL, z); 
                        }    break; 
                        case 0x900f: {    // POP -- 1001 000d dddd 1111
                            get_d5( instruction ); 
                            m_dataMem[d] = POP_STACK8(); 
                            cycle++; 
                        }    break; 
                        case 0x920f: {    // PUSH -- 1001 001d dddd 1111
                            get_vd5( instruction ); 
                            PUSH_STACK8(vd); 
                            cycle++; 
                        }    break; 
                        case 0x9400: {    // COM -- One's Complement -- 1001 010d dddd 0000
                            get_vd5( instruction ); 
                            uint8_t res = 0xff - vd; 
                            m_dataMem[d] = res; 
                            flags_znv0s( res ); 
                            set_S_Bit( S_C ); 
                        }    break; 
                        case 0x9401: {    // NEG -- Two's Complement -- 1001 010d dddd 0001
                            get_vd5( instruction ); 
                            uint8_t res = 0x00 - vd; 
                            m_dataMem[d] = res; 
                            write_S_Bit( S_H, ((res >> 3)|( vd >> 3)) & 1 ); 
                            write_S_Bit( S_V, res == 0x80 ); 
                            write_S_Bit( S_C, res != 0 ); 
                            flags_zns( res ); 
                        }    break; 
                        case 0x9402: {    // SWAP -- Swap Nibbles -- 1001 010d dddd 0010
                            get_vd5( instruction ); 
                            uint8_t res =( vd >> 4) | ( vd << 4) ; 
                            m_dataMem[d] = res; 
                        }    break; 
                        case 0x9403: {    // INC -- Increment -- 1001 010d dddd 0011
                            get_vd5( instruction ); 
                            uint8_t res = vd + 1; 
                            m_dataMem[d] = res; 
                            write_S_Bit( S_V, res == 0x80 ); 
                            flags_zns( res); 
                        }    break; 
                        case 0x9405: {    // ASR -- Arithmetic Shift Right -- 1001 010d dddd 0101
                            get_vd5( instruction ); 
                            uint8_t res = (vd >> 1) |(vd & 0x80); 
                            m_dataMem[d] = res; 
                            flags_zcnvs( res, vd ); 
                        }    break; 
                        case 0x9406: {    // LSR -- Logical Shift Right -- 1001 010d dddd 0110
                            get_vd5( instruction ); 
                            uint8_t res = vd >> 1; 
                            m_dataMem[d] = res; 
                            clear_S_Bit( S_N ); 
                            flags_zcvs( res, vd); 
                        }    break; 
                        case 0x9407: {    // ROR -- Rotate Right -- 1001 010d dddd 0111
                            get_vd5( instruction ); 
                            uint8_t res =( STATUS(S_C) ? 0x80 : 0) | vd >> 1; 
                            m_dataMem[d] = res; 
                            flags_zcnvs( res, vd); 
                        }    break; 
                        case 0x940a: {    // DEC -- Decrement -- 1001 010d dddd 1010
                            get_vd5( instruction ); 
                            uint8_t res = vd - 1; 
                            m_dataMem[d] = res; 
                            write_S_Bit( S_V, res == 0x7f ); 
                            flags_zns( res ); 
                        }    break; 
                        case 0x940c:
                        case 0x940d: {    // JMP -- Long Call to sub, 32 bits -- 1001 010a aaaa 110a
                            uint32_t a =( (instruction & 0x01f0) >> 3) |( instruction & 1); 
                            uint16_t x = m_progMem[new_pc]; 
                            a =( a << 16) | x; 
                            new_pc = a; 
                            cycle += 2; 
                        }    break; 
                        case 0x940e:
                        case 0x940f: {    // CALL -- Long Call to sub, 32 bits -- 1001 010a aaaa 111a
                            uint32_t a =( (instruction & 0x01f0) >> 3) |( instruction & 1); 
                            uint16_t x = m_progMem[new_pc]; 
                            a =( a << 16) | x; 
                            new_pc += 1; 
                            PUSH_STACK( new_pc ); 
                            m_RET_ADDR = new_pc; 
                            cycle += 1+m_progAddrSize; 
                            new_pc = a; 
                        }    break; 

                        default: {
                            switch( instruction & 0xff00) {
                                case 0x9600: {    // ADIW -- Add Immediate to Word -- 1001 0110 KKpp KKKK
                                    get_vp2_k6( instruction ); 
                                    uint16_t res = vp + k; 
                                    SET_REG16_HL( p, res ); 
                                    /// SREG[S_V] =( (~vp & res) >> 15) & 1; 
                                    write_S_Bit( S_V, (~vp & res) & (1<<15) ); 

                                    ///SREG[S_C] =( (~res & vp) >> 15) & 1; 
                                    write_S_Bit( S_C, (~res & vp) & (1<<15) ); 

                                    flags_zns16( res ); 
                                    cycle++; 
                                }    break; 
                                case 0x9700: {    // SBIW -- Subtract Immediate from Word -- 1001 0111 KKpp KKKK
                                    get_vp2_k6( instruction ); 
                                    uint16_t res = vp - k; 
                                    SET_REG16_HL( p, res ); 
                                    /// SREG[S_V] =( (vp & ~res) >> 15) & 1; 
                                    write_S_Bit( S_V, (vp & ~res) & (1<<15) ); 

                                    ///SREG[S_C] =( (res & ~vp) >> 15) & 1; 
                                    write_S_Bit( S_C, (res & ~vp) & (1<<15) ); 

                                    flags_zns16( res ); 
                                    cycle++; 
                                }    break; 
                                case 0x9800: {    // CBI -- Clear Bit in I/O Register -- 1001 1000 AAAA Abbb
                                    get_io5_b3mask( instruction ); 
                                    uint8_t res = GET_RAM( io ) & ~mask; 
                                    SET_RAM( io, res ); 
                                    cycle++; 
                                }    break; 
                                case 0x9900: {    // SBIC -- Skip if Bit in I/O Register is Cleared -- 1001 1001 AAAA Abbb
                                    get_io5_b3mask( instruction ); 
                                    uint8_t res = GET_RAM( io ) & mask; 
                                    if( !res)
                                    {
                                        if( is_instr_32b(new_pc) ) { new_pc += 2;  cycle += 2;  }
                                        else                       { new_pc += 1;  cycle++;  }
                                    }
                                }    break; 
                                case 0x9a00: {    // SBI -- Set Bit in I/O Register -- 1001 1010 AAAA Abbb
                                    get_io5_b3mask( instruction ); 
                                    uint8_t res = GET_RAM( io ) | mask; 
                                    SET_RAM( io, res ); 
                                    cycle++; 
                                }    break; 
                                case 0x9b00: {    // SBIS -- Skip if Bit in I/O Register is Set -- 1001 1011 AAAA Abbb
                                    get_io5_b3mask( instruction ); 
                                    uint8_t res = GET_RAM( io ) & mask; 
                                    if( res )
                                    {
                                        if( is_instr_32b(new_pc) ) { new_pc += 2;  cycle += 2;  }
                                        else                       { new_pc += 1;  cycle++;  }
                                    }
                                }    break; 
                                default:
                                    switch( instruction & 0xfc00) {
                                        case 0x9c00: {    // MUL -- Multiply Unsigned -- 1001 11rd dddd rrrr
                                            get_vd5_vr5( instruction ); 
                                            uint16_t res = vd * vr; 
                                            cycle++; 
                                            SET_REG16_LH( 0, res ); 
                                            write_S_Bit( S_Z, res == 0 ); 

                                            /// SREG[S_C] =( res >> 15) & 1; 
                                            write_S_Bit( S_C, res & (1<<15) ); 
                                        }    break; 
                                        default: ; //_avr_invalid_instruction(avr); 
                                    }
                            }
                        }    break; 
                    }
                }    break; 
            }
        }    break; 

        case 0xb000: {
            switch( instruction & 0xf800) {
                case 0xb800: {    // OUT A,Rr -- 1011 1AAd dddd AAAA
                    get_d5_a6( instruction ); 
                    SET_RAM( A, m_dataMem[d] ); 
                }    break; 
                case 0xb000: {    // IN Rd,A -- 1011 0AAd dddd AAAA
                    get_d5_a6( instruction ); 
                    m_dataMem[d] = GET_RAM(A); 
                }    break; 
                default: ; //_avr_invalid_instruction(avr); 
            }
        }    break; 

        case 0xc000: {    // RJMP -- 1100 kkkk kkkk kkkk
            const int16_t k = ((int16_t)((instruction << 4) & 0xFFFF)) >> 4; 
            new_pc = (new_pc + k) % m_progSize; 
            cycle++; 
        }    break; 

        case 0xd000: {    // RCALL -- 1101 kkkk kkkk kkkk
            const int16_t k = ((int16_t)((instruction << 4) & 0xFFFF)) >> 4; 
            cycle += m_progAddrSize; 
            PUSH_STACK( new_pc ); 
            m_RET_ADDR = new_pc; 
            new_pc = (new_pc + k) % m_progSize; 
        }    break; 

        case 0xe000: {    // LDI Rd, K aka SER( LDI r, 0xff) -- 1110 kkkk dddd kkkk
            get_h4_k8( instruction ); 
            m_dataMem[h] = k; 
        }    break; 

        case 0xf000: {
            switch( instruction & 0xfe00)
            {
                case 0xf000:
                case 0xf200:
                case 0xf400:
                case 0xf600: {    // BRXC/BRXS -- All the SREG branches -- 1111 0Boo oooo osss
                    int16_t o =( (int16_t)(instruction << 6)) >> 9;  // offset
                    uint8_t s = instruction & 7; 
                    bool set =( instruction & 0x0400) == 0;         // this bit means BRXC otherwise BRXS
                    int branch =( STATUS(s) && set) ||( !STATUS(s) && !set); 
                    if( branch) {
                        cycle++;  // 2 cycles if taken, 1 otherwise
                        new_pc = new_pc + o; 
                    }
                }    break; 
                case 0xf800:
                case 0xf900: {    // BLD -- Bit Store from T into a Bit in Register -- 1111 100d dddd 0bbb
                    get_vd5_s3_mask( instruction ); 
                    uint8_t v =( vd & ~mask) |( STATUS(S_T) ? mask : 0); 
                    m_dataMem[d] = v; 
                }    break; 
                case 0xfa00:
                case 0xfb00:{    // BST -- Bit Store into T from bit in Register -- 1111 101d dddd 0bbb
                    get_vd5_s3( instruction )
                    write_S_Bit( S_T, ( vd >> s) & 1 ); 
                }    break; 
                case 0xfc00:
                case 0xfe00: {    // SBRS/SBRC -- Skip if Bit in Register is Set/Clear -- 1111 11sd dddd 0bbb
                    get_vd5_s3_mask( instruction )
                    int set =( instruction & 0x0200) != 0; 
                    int branch =( (vd & mask) && set) ||( !(vd & mask) && !set); 
                    if( branch)
                    {
                        if( is_instr_32b(new_pc) ) { new_pc += 2;  cycle += 2; }
                        else                       { new_pc += 1;  cycle++;  }
                    }   break; 
                }
                default: ; //_avr_invalid_instruction(avr); 
            }
        }    break; 
        default: ; //_avr_invalid_instruction(avr); 
    }
    if( new_pc >= m_progSize ) new_pc = 0; 

    m_PC = new_pc; 
    m_mcu->cyclesDone = cycle; 
}
