Baremetal Arduino: Chip8 Emulator with Mega

// ./src/chip8/chip8.cpp
#include "chip8.h"

#include "logger.h"
#include "../tavr/time.h"
#include "../tavr/rand.h"

TChip8::TChip8(TLogger* logger, TTime* time, TRandom* rand, TDisplay* disp, TKeyboard* keyb, TSound* sound)
{
    m_logger = logger;
    m_time = time;
    m_rand = rand;
    m_emulatorRunning = true;
    m_key_pressed = false;
    m_display = disp;
    m_keyboard = keyb;
    m_sound = sound;
}
TChip8::~TChip8() {}

void TChip8::init(uint8_t rom)
{
    // Clear stack
    for(auto i = 0; i < STACK_SIZE; i++)
        m_stack[i] = 0;

    // Clear RAM
    for(auto i = 0; i < TOTAL_RAM; i++)
        m_ram[i] = 0;

    // Load font set into memory
    for(auto i = 0; i < FONTSET_SIZE; i++)
        m_ram[i] = FONTSET[i];

    // Reset timers
    m_delay_timer = 0;
    m_sound_timer = 0;

    // Start keyboard state as all unpressed
    for (auto i = 0; i < NUM_KEYS; i++)
        m_keys[i] = 0;
    m_key_pressed = false;
    
    // Initialize cpu registers
    m_pcreg = PC_START;
    m_current_op = 0;
    m_sp_reg = 0;
    m_ireg = 0;
    m_instruction = 0;

    // Clear registers
    for(auto i = 0; i < NUM_V_REGISTERS; i++)
        m_reg[i] = 0;
    
    // Load ROM to the ram
    TRom_Loader::loadRom(rom, m_ram + PROGRAM_START_ADDR, m_logger);
    
    // Initialize and clear display
    m_display->init();
    m_display->clear();
    m_display->update();

    // Init sound and keyboard
    m_sound->init();
    m_keyboard->init();
}

void TChip8::run()
{
    if (!m_emulatorRunning)
        return;

    uint64_t stime = m_time->millis();

    // CPU Cycle
    fetch();
    decode();
    execute();

    // Update button states
    m_keyboard->update(m_keys);

    // ~60Hz
    if (m_tickCounter == 2)
    {
        if (m_delay_timer > 0)
            m_delay_timer--;
        if (m_sound_timer > 0)
        {
            m_sound_timer--;
            m_sound->play();
        }
        else m_sound->pause();
    }

    // ~30Hz (if you have an SPI display, use the 60Hz)
    if (m_tickCounter >= 3)
    {
        m_display->update(); // upddate display
        m_tickCounter = 0; // reset counter
    }

    m_tickCounter++;
    uint64_t etime = m_time->millis();
    int64_t sleepTime = 10 - (etime - stime);

    if (sleepTime > 0)
        m_time->waitms(sleepTime);
}

void TChip8::deinit()
{
    m_display->deinit();
    m_keyboard->deinit();
    m_sound->deinit();
}

void TChip8::fetch()
{
    m_current_op = ((uint16_t)m_ram[m_pcreg] << 8) | m_ram[m_pcreg+1];
    m_pcreg += 2;
}

void TChip8::decode() { m_instruction = m_current_op >> 12; }

void TChip8::execute()
{
    switch(m_instruction)
    {
        case 0x0: decode_0_instruction(); break;
        case 0x1: jump_to(); break;
        case 0x2: call_subroutine(); break;
        case 0x3: skip_next_instruction_eq(); break;
        case 0x4: skip_next_instruction_ne(); break;
        case 0x5: skip_next_instruction_vx_vy(); break;
        case 0x6: register_set(); break;
        case 0x7: add_reg_imm(); break;
        case 0x8: decode_8_instruction(); break;
        case 0x9: skip_next_instruction_vx_vy_ne(); break;
        case 0xA: set_index_register(); break;
        case 0xB: jump_with_v0(); break;
        case 0xC: generate_random_number(); break;
        case 0xD: draw_sprite(); break;
        case 0xE: decode_E_instruction(); break;
        case 0xF: decode_F_instruction(); break;
        default:
            if (m_logger->beginLog(ELogLevel::ERROR))
            {
                m_logger->beLog("Impossible instruction: ");
                m_logger->beLog(m_instruction);
                m_logger->endLog();
            }
            break;
    }
}


// CPU functions
/*****************************
 * 0ZZZ
 *****************************/
void TChip8::decode_0_instruction()
{
    switch(m_current_op & 0xFF)
    {
        case 0xE0: clear_screen(); break;
        case 0xEE: return_from_subroutine(); break;
        default: 
            if (m_logger->beginLog(ELogLevel::ERROR))
            {
                m_logger->beLog("Impossible instruction 0: ");
                m_logger->beLog(m_instruction);
                m_logger->endLog();
            }
            break;
    }
}

/*****************************
 * 0ZE0
 *****************************/
void TChip8::clear_screen()
{
    m_display->clear();
}

/*****************************
 * 0ZEE
 *****************************/
void TChip8::return_from_subroutine()
{
    m_sp_reg--;
    m_pcreg = m_stack[m_sp_reg];
}

/*****************************
 * 1NNN
 *****************************/
void TChip8::jump_to()
{
    m_pcreg = m_current_op & 0x0FFF;
}

/*****************************
 * 2NNN
 *****************************/
void TChip8::call_subroutine()
{
    uint16_t nnn = m_current_op & 0x0FFF;

    m_stack[m_sp_reg] = m_pcreg;
    m_sp_reg++;
    m_pcreg = nnn;
}

/*****************************
 * 3XNN
 *****************************/
void TChip8::skip_next_instruction_eq()
{
    uint8_t value = m_current_op & 0xFF;
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    if(m_reg[reg] == value)
        m_pcreg += 2;
}

/*****************************
 * 4XNN
 *****************************/
void TChip8::skip_next_instruction_ne()
{
    uint8_t value = m_current_op & 0xFF;
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    if(m_reg[reg] != value)
        m_pcreg += 2;
}

/*****************************
 * 5XYZ
 *****************************/
void TChip8::skip_next_instruction_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    if(m_reg[reg_x] == m_reg[reg_y])
        m_pcreg += 2;
}

/*****************************
 * 6XNN
 *****************************/
void TChip8::register_set()
{
    uint8_t value = m_current_op & 0xFF;
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    m_reg[reg] = value;
}

/*****************************
 * 7XNN
 *****************************/
void TChip8::add_reg_imm()
{
    uint8_t value = m_current_op & 0xFF;
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    m_reg[reg] += value;
}

/*****************************
 * 8XYZ
 *****************************/
void TChip8::decode_8_instruction()
{
    switch(m_current_op & 0xF)
    {
        case 0x0: move_vy_to_vx(); break;
        case 0x1: or_vx_vy(); break;
        case 0x2: and_vx_vy(); break;
        case 0x3: xor_vx_vy(); break;
        case 0x4: add_vx_vy(); break;
        case 0x5: sub_vx_vy(); break;
        case 0x6: shift_right_reg(); break;
        case 0x7: subn_vx_vy(); break;
        case 0xE: shift_left_reg(); break;
        default: 
            if (m_logger->beginLog(ELogLevel::ERROR))
            {
                m_logger->beLog("Impossible instruction 8: ");
                m_logger->beLog(m_current_op & 0xF);
                m_logger->endLog();
            }
            break;
    }
}

/*****************************
 * 8XY0
 *****************************/
void TChip8::move_vy_to_vx()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    m_reg[reg_x] = m_reg[reg_y];
}

/*****************************
 * 8XY1
 *****************************/
void TChip8::or_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    m_reg[reg_x] |= m_reg[reg_y];
}

/*****************************
 * 8XY2
 *****************************/
void TChip8::and_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    m_reg[reg_x] &= m_reg[reg_y];
}

/*****************************
 * 8XY3
 *****************************/
void TChip8::xor_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    m_reg[reg_x] ^= m_reg[reg_y];
}

/*****************************
 * 8XY4
 *****************************/
void TChip8::add_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;
    uint16_t add = m_reg[reg_x] + m_reg[reg_y];

    if(add > 0xFF)
        m_reg[0xF] = 1;
    else
        m_reg[0xF] = 0;

    m_reg[reg_x] = add & 0xFF;
}

/*****************************
 * 8XY5
 *****************************/
void TChip8::sub_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    if(m_reg[reg_x] > m_reg[reg_y])
        m_reg[0xF] = 1;
    else
        m_reg[0xF] = 0;

    m_reg[reg_x] -= m_reg[reg_y];
}

/*****************************
 * 8XY6
 *****************************/
void TChip8::shift_right_reg()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    if(m_reg[reg] % 2 == 1 )
        m_reg[0xF] = 1;
    else
        m_reg[0xF] = 0;

    m_reg[reg] >>= 1;
}

/*****************************
 * 8XY7
 *****************************/
void TChip8::subn_vx_vy()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    if(m_reg[reg_y] > m_reg[reg_x])
        m_reg[0xF] = 1;
    else
        m_reg[0xF] = 0;

    m_reg[reg_x] = m_reg[reg_y] - m_reg[reg_x];
}

/*****************************
 * 8XYE
 *****************************/
void TChip8::shift_left_reg()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    if((m_reg[reg] & 0x80) == 1)
        m_reg[0xF] = 1;
    else
        m_reg[0xF] = 0;

    m_reg[reg] <<= 1;
}

/*****************************
 * 9XYZ
 *****************************/
void TChip8::skip_next_instruction_vx_vy_ne()
{
    uint8_t reg_x = (m_current_op >> 8) & 0x0F;
    uint8_t reg_y = (m_current_op >> 4) & 0x0F;

    if(m_reg[reg_x] != m_reg[reg_y])
        m_pcreg += 2;
}

/*****************************
 * ANNN
 *****************************/
void TChip8::set_index_register()
{
    m_ireg = m_current_op & 0x0FFF;
}

/*****************************
 * BNNN
 *****************************/
void TChip8::jump_with_v0()
{
    uint16_t nnn = m_current_op & 0x0FFF;

    m_pcreg = nnn + m_reg[0];
}

/*****************************
 * CXKK
 *****************************/
void TChip8::generate_random_number()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    uint8_t kk = m_current_op & 0xFF;
    uint8_t randNum = (uint8_t)(m_rand->next() % 256);

    m_reg[reg] = randNum & kk;
}

/*****************************
 * DXYN
 *****************************/
void TChip8::draw_sprite()
{
    uint8_t v_reg_x = (m_current_op & 0x0F00) >> 8;
    uint8_t v_reg_y = (m_current_op & 0x00F0) >> 4;   
    uint8_t sprite_height = m_current_op & 0x000F;
    uint8_t x_location = m_reg[v_reg_x];
    uint8_t y_location = m_reg[v_reg_y];

    // Reset colision register
    m_reg[0xF] = 0;
    for(int y_coordinate = 0; y_coordinate < sprite_height; y_coordinate++)
    {
        uint8_t pixel = m_ram[m_ireg + y_coordinate];
        for(int x_coordinate = 0; x_coordinate < 8; x_coordinate++)
        {
            if((pixel & (0x80 >> x_coordinate)) != 0)
            { 
                uint8_t y_lc = y_location + y_coordinate;
                uint8_t x_lc = x_location + x_coordinate;
                if(m_display->read(x_lc, y_lc))
                {
                    m_reg[0xF] = 1;
                    m_display->draw(x_lc, y_lc, 0);
                }
                else m_display->draw(x_lc, y_lc, 1);
            }
        }
    }
}

/*****************************
 * EZZZ
 *****************************/
void TChip8::decode_E_instruction()
{
    switch(m_current_op & 0xFF)
    {
        case 0x009E: skip_next_inst_if_key_pressed(); break;
        case 0x00A1: skip_next_inst_if_not_pressed(); break;
        default:
            if (m_logger->beginLog(ELogLevel::ERROR))
            {
                m_logger->beLog("Impossible instruction E: ");
                m_logger->beLog(m_current_op & 0xFF);
                m_logger->endLog();
            }
            break;
    }
}

/*****************************
 * EX9E
 *****************************/
void TChip8::skip_next_inst_if_key_pressed()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    uint8_t val = m_reg[reg];

    if(m_keys[val] != 0)
        m_pcreg += 2;
}

/*****************************
 * EXA1
 *****************************/
void TChip8::skip_next_inst_if_not_pressed()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    uint8_t val = m_reg[reg];

    if(m_keys[val] == 0)
        m_pcreg += 2;
}

/*****************************
 * FZZZ
 *****************************/
void TChip8::decode_F_instruction()
{
    switch(m_current_op & 0xFF)
    {
        case 0x0007: load_reg_with_delay_timer(); break;
        case 0x000A: wait_key_press(); break;
        case 0x0015: load_delay_timer_with_reg(); break;
        case 0x0018: load_sound_timer_with_reg(); break;
        case 0x001E: add_ireg_with_reg(); break;
        case 0x0029: load_font_from_vx(); break;
        case 0x0033: store_binary_code_decimal_representation(); break;
        case 0x0055: load_memory_from_regs(); break;
        case 0x0065: load_regs_from_memory(); break;
        default:
            if (m_logger->beginLog(ELogLevel::ERROR))
            {
                m_logger->beLog("Impossible instruction F: ");
                m_logger->beLog(m_current_op & 0xFF);
                m_logger->endLog();
            }
            break;
    }
}

/*****************************
 * FX07
 *****************************/
void TChip8::load_reg_with_delay_timer()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    m_reg[reg] = m_delay_timer;
}

/*****************************
 * FX0A
 *****************************/
void TChip8::wait_key_press()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    bool key_pressed = false;

    for(int i=0; i<NUM_KEYS; i++)
    {
        if(m_keys[i] != 0)
        {
            m_reg[reg] = i;
            key_pressed = true;
        }
    }

    if(!key_pressed)
        m_pcreg -= 2;
}

/*****************************
 * FX15
 *****************************/
void TChip8::load_delay_timer_with_reg()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    m_delay_timer = m_reg[reg];
}

/*****************************
 * FX18
 *****************************/
void TChip8::load_sound_timer_with_reg()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    m_sound_timer = m_reg[reg];
}

/*****************************
 * FX1E
 *****************************/
void TChip8::add_ireg_with_reg()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    m_ireg += m_reg[reg];
}

/*****************************
 * FX29
 *****************************/
void TChip8::load_font_from_vx()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;
    m_ireg = m_reg[reg] * 0x5;
}

/*****************************
 * FX33
 *****************************/
void TChip8::store_binary_code_decimal_representation()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    m_ram[m_ireg] = m_reg[reg] / 100;
    m_ram[m_ireg+1] = (m_reg[reg] / 10) % 10;
    m_ram[m_ireg+1] = (m_reg[reg] % 100) % 10;
}

/*****************************
 * FX55
 *****************************/
void TChip8::load_memory_from_regs()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    for(int i=0; i<=reg; i++)
        m_ram[m_ireg + i] = m_reg[i];

    m_ireg += (reg + 1);
}

/*****************************
 * FX65
 *****************************/
void TChip8::load_regs_from_memory()
{
    uint8_t reg = (m_current_op >> 8) & 0x0F;

    for(int i=0; i<=reg; i++)
        m_reg[i] = m_ram[m_ireg + i];

    m_ireg += (reg + 1);
}