SameBoy/Core/memory.c

#include <stdio.h>
#include <stdbool.h>
#include "gb.h"

typedef uint8_t read_function_t(GB_gameboy_t *gb, uint16_t addr);
typedef void write_function_t(GB_gameboy_t *gb, uint16_t addr, uint8_t value);

typedef enum {
    GB_BUS_MAIN, /* In DMG: Cart and RAM. In CGB: Cart only */
    GB_BUS_RAM, /* In CGB only. */
    GB_BUS_VRAM,
    GB_BUS_INTERNAL, /* Anything in highram. Might not be the most correct name. */
} bus_t;

static bus_t bus_for_addr(GB_gameboy_t *gb, uint16_t addr)
{
    if (addr < 0x8000) {
        return GB_BUS_MAIN;
    }
    if (addr < 0xA000) {
        return GB_BUS_VRAM;
    }
    if (addr < 0xC000) {
        return GB_BUS_MAIN;
    }
    if (addr < 0xFE00) {
        return GB_is_cgb(gb)? GB_BUS_RAM : GB_BUS_MAIN;
    }
    return GB_BUS_INTERNAL;
}

static uint16_t bitwise_glitch(uint16_t a, uint16_t b, uint16_t c)
{
    return ((a ^ c) & (b ^ c)) ^ c;
}

static uint16_t bitwise_glitch_read(uint16_t a, uint16_t b, uint16_t c)
{
    return b | (a & c);
}

static uint16_t bitwise_glitch_read_secondary(uint16_t a, uint16_t b, uint16_t c, uint16_t d)
{
    return (b & (a | c | d)) | (a & c & d);
}

/*
 Used on the MGB in some scenarios
static uint16_t bitwise_glitch_mgb(uint16_t a, uint16_t b, uint16_t c, uint16_t d, uint16_t e, bool variant)
{
    return (c & (e | d | b | (variant? 0 : a))) | (b & d & (a | e));
}
*/

static uint16_t bitwise_glitch_tertiary_read_1(uint16_t a, uint16_t b, uint16_t c, uint16_t d, uint16_t e)
{
    return c | (a & b & d & e);
}

static uint16_t bitwise_glitch_tertiary_read_2(uint16_t a, uint16_t b, uint16_t c, uint16_t d, uint16_t e)
{
    return (c & (a | b | d | e)) | (a & b & d & e);
}

static uint16_t bitwise_glitch_tertiary_read_3(uint16_t a, uint16_t b, uint16_t c, uint16_t d, uint16_t e)
{
    return (c & (a | b | d | e)) | (b & d & e);
}

static uint16_t bitwise_glitch_quaternary_read_dmg(uint16_t a, uint16_t b, uint16_t c, uint16_t d,
                                                   uint16_t e, uint16_t f, uint16_t g, uint16_t h)
{
    /* On my DMG, some cases are non-deterministic, while on some other DMGs they yield constant zeros.
       The non deterministic cases are affected by (on the row 40 case) 34, 36, 3e and 28, and potentially
       others. For my own sanity I'm going to emulate the DMGs that output zeros. */
    (void)a;
    return (e & (h | g | (~d & f) | c | b)) | (c & g & h);
}

/*
 
// Oh my.
static uint16_t bitwise_glitch_quaternary_read_mgb(uint16_t a, uint16_t b, uint16_t c, uint16_t d,
                                                   uint16_t e, uint16_t f, uint16_t g, uint16_t h)
{
    return (e & (h | g | c | (a & b))) | ((c & h) & (g & (~f | b | a | ~d) | (a & b & f)));
}
*/

static uint16_t bitwise_glitch_quaternary_read_sgb2(uint16_t a, uint16_t b, uint16_t c, uint16_t d,
                                                    uint16_t e, uint16_t f, uint16_t g, uint16_t h)
{
    return (e & (h | g | c | (a & b))) | ((c & g & h) & (b | a | ~f));
}

void GB_trigger_oam_bug(GB_gameboy_t *gb, uint16_t address)
{
    if (GB_is_cgb(gb)) return;
    
    if (address >= 0xFE00 && address < 0xFF00) {
        if (gb->accessed_oam_row != 0xff && gb->accessed_oam_row >= 8) {
            uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);
            base[0] = bitwise_glitch(base[0],
                                     base[-4],
                                     base[-2]);
            for (unsigned i = 2; i < 8; i++) {
                gb->oam[gb->accessed_oam_row + i] = gb->oam[gb->accessed_oam_row - 8 + i];
            }
        }
    }
}

static void oam_bug_secondary_read_corruption(GB_gameboy_t *gb)
{
    if (gb->accessed_oam_row < 0x98) {
        uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);
        base[-4] = bitwise_glitch_read_secondary(base[-8],
                                                 base[-4],
                                                 base[0],
                                                 base[-2]);
        for (unsigned i = 0; i < 8; i++) {
            gb->oam[gb->accessed_oam_row - 0x10 + i] = gb->oam[gb->accessed_oam_row - 0x08 + i];
        }
    }
}

/*
static void oam_bug_tertiary_read_corruption_mgb(GB_gameboy_t *gb)
{
    if (gb->accessed_oam_row < 0x98) {
        uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);
        uint16_t temp = bitwise_glitch_mgb(
                                           base[0],
                                           base[-2],
                                           base[-4],
                                           base[-8],
                                           base[-16],
                                           true);
        
        base[-4] = bitwise_glitch_mgb(
                                      base[0],
                                      base[-2],
                                      base[-4],
                                      base[-8],
                                      base[-16],
                                      false);
        for (unsigned i = 0; i < 8; i++) {
            gb->oam[gb->accessed_oam_row - 0x10 + i] = gb->oam[gb->accessed_oam_row - 0x20 + i] = gb->oam[gb->accessed_oam_row - 0x08 + i];
        }
        
        base[-8] = temp;
        base[-16] = temp;
    }
}
*/

static void oam_bug_quaternary_read_corruption(GB_gameboy_t *gb, typeof(bitwise_glitch_quaternary_read_dmg) *bitwise_op)
{
    if (gb->accessed_oam_row < 0x98) {
        uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);

        base[-4] = bitwise_op(*(uint16_t *)gb->oam,
                              base[0],
                              base[-2],
                              base[-3],
                              base[-4],
                              base[-7],
                              base[-8],
                              base[-16]);
        for (unsigned i = 0; i < 8; i++) {
            gb->oam[gb->accessed_oam_row - 0x10 + i] = gb->oam[gb->accessed_oam_row - 0x20 + i] = gb->oam[gb->accessed_oam_row - 0x08 + i];
        }
    }
}

static void oam_bug_tertiary_read_corruption(GB_gameboy_t *gb, typeof(bitwise_glitch_tertiary_read_1) *bitwise_op)
{
    if (gb->accessed_oam_row < 0x98) {
        uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);
        
        /* On some instances, the corruption row is copied to the first row for some accessed row. On my DMG it happens
         for row 80, and on my MGB it happens on row 60. Some instances only copy odd or even bytes. Additionally,
         for some instances on accessed rows that do not affect the first row, the last two bytes of the preceeding
         row are also corrupted in a non-deterministic probability. */
        
        base[-4] = bitwise_op(
                              base[0],
                              base[-2],
                              base[-4],
                              base[-8],
                              base[-16]);
        for (unsigned i = 0; i < 8; i++) {
            gb->oam[gb->accessed_oam_row - 0x10 + i] = gb->oam[gb->accessed_oam_row - 0x20 + i] = gb->oam[gb->accessed_oam_row - 0x08 + i];
        }
    }
}

void GB_trigger_oam_bug_read(GB_gameboy_t *gb, uint16_t address)
{
    if (GB_is_cgb(gb)) return;
    
    if (address >= 0xFE00 && address < 0xFF00) {
        if (gb->accessed_oam_row != 0xff && gb->accessed_oam_row >= 8) {
            if ((gb->accessed_oam_row & 0x18) == 0x10) {
                oam_bug_secondary_read_corruption(gb);
            }
            else if ((gb->accessed_oam_row & 0x18) == 0x00) {
                /* Everything in this specific case is *extremely* revision and instance specific. */
                if (gb->model == GB_MODEL_MGB) {
                    /* TODO: This is rather simplified, research further */
                    oam_bug_tertiary_read_corruption(gb, bitwise_glitch_tertiary_read_3);
                }
                else if (gb->accessed_oam_row == 0x40) {
                     oam_bug_quaternary_read_corruption(gb,
                                                        ((gb->model & ~GB_MODEL_NO_SFC_BIT) == GB_MODEL_SGB2)?
                                                        bitwise_glitch_quaternary_read_sgb2:
                                                        bitwise_glitch_quaternary_read_dmg);
                }
                else if ((gb->model & ~GB_MODEL_NO_SFC_BIT) != GB_MODEL_SGB2) {
                    if (gb->accessed_oam_row == 0x20) {
                        oam_bug_tertiary_read_corruption(gb, bitwise_glitch_tertiary_read_2);
                    }
                    else if (gb->accessed_oam_row == 0x60) {
                        oam_bug_tertiary_read_corruption(gb, bitwise_glitch_tertiary_read_3);
                    }
                    else {
                        oam_bug_tertiary_read_corruption(gb, bitwise_glitch_tertiary_read_1);
                    }
                }
                else {
                    oam_bug_tertiary_read_corruption(gb, bitwise_glitch_tertiary_read_2);
                }
            }
            else {
                uint16_t *base = (uint16_t *)(gb->oam + gb->accessed_oam_row);
                base[-4] =
                base[0]     = bitwise_glitch_read(base[0],
                                                  base[-4],
                                                  base[-2]);
            }
            for (unsigned i = 0; i < 8; i++) {
                gb->oam[gb->accessed_oam_row + i] = gb->oam[gb->accessed_oam_row - 8 + i];
            }
            if (gb->accessed_oam_row == 0x80) {
                memcpy(gb->oam, gb->oam + gb->accessed_oam_row, 8);
            }
            else if (gb->model == GB_MODEL_MGB && gb->accessed_oam_row == 0x40) {
                memcpy(gb->oam, gb->oam + gb->accessed_oam_row, 8);
            }
        }
    }
}


static bool is_addr_in_dma_use(GB_gameboy_t *gb, uint16_t addr)
{
    if (!gb->dma_steps_left || (gb->dma_cycles < 0 && !gb->is_dma_restarting) || addr >= 0xFE00) return false;
    return bus_for_addr(gb, addr) == bus_for_addr(gb, gb->dma_current_src);
}

static uint8_t read_rom(GB_gameboy_t *gb, uint16_t addr)
{
    if (addr < 0x100 && !gb->boot_rom_finished) {
        return gb->boot_rom[addr];
    }

    if (addr >= 0x200 && addr < 0x900 && GB_is_cgb(gb) && !gb->boot_rom_finished) {
        return gb->boot_rom[addr];
    }

    if (!gb->rom_size) {
        return 0xFF;
    }
    unsigned effective_address = (addr & 0x3FFF) + gb->mbc_rom0_bank * 0x4000;
    return gb->rom[effective_address & (gb->rom_size - 1)];
}

static uint8_t read_mbc_rom(GB_gameboy_t *gb, uint16_t addr)
{
    unsigned effective_address = (addr & 0x3FFF) + gb->mbc_rom_bank * 0x4000;
    return gb->rom[effective_address & (gb->rom_size - 1)];
}

static uint8_t read_vram(GB_gameboy_t *gb, uint16_t addr)
{
    if (gb->vram_read_blocked) {
        return 0xFF;
    }
    if (gb->display_state == 22 && GB_is_cgb(gb) && !gb->cgb_double_speed) {
        if (addr & 0x1000) {
            addr = gb->last_tile_index_address;
        }
        else if (gb->last_tile_data_address & 0x1000) {
            /* TODO: This is case is more complicated then the rest and differ between revisions
               It's probably affected by how VRAM is layed out, might be easier after a decap is done*/
        }
        else {
            addr = gb->last_tile_data_address;
        }
    }
    return gb->vram[(addr & 0x1FFF) + (gb->cgb_vram_bank? 0x2000 : 0)];
}

static uint8_t read_mbc7_ram(GB_gameboy_t *gb, uint16_t addr)
{
    if (!gb->mbc_ram_enable || !gb->mbc7.secondary_ram_enable) return 0xFF;
    if (addr >= 0xB000) return 0xFF;
    switch ((addr >> 4) & 0xF) {
        case 2: return gb->mbc7.x_latch;
        case 3: return gb->mbc7.x_latch >> 8;
        case 4: return gb->mbc7.y_latch;
        case 5: return gb->mbc7.y_latch >> 8;
        case 6: return 0;
        case 8: return  gb->mbc7.eeprom_do | (gb->mbc7.eeprom_di << 1) |
                       (gb->mbc7.eeprom_clk << 6) | (gb->mbc7.eeprom_cs << 7);
    }
    return 0xFF;
}

static uint8_t read_mbc_ram(GB_gameboy_t *gb, uint16_t addr)
{
    if (gb->cartridge_type->mbc_type == GB_MBC7) {
        return read_mbc7_ram(gb, addr);
    }

    if (gb->cartridge_type->mbc_type == GB_HUC3) {
        switch (gb->huc3.mode) {
            case 0xC: // RTC read
                if (gb->huc3.access_flags == 0x2) {
                    return 1;
                }
                return gb->huc3.read;
            case 0xD: // RTC status
                return 1;
            case 0xE: // IR mode
                return gb->effective_ir_input; // TODO: What are the other bits?
            default:
                GB_log(gb, "Unsupported HuC-3 mode %x read: %04x\n", gb->huc3.mode, addr);
                return 1; // TODO: What happens in this case?
            case 0: // TODO: R/O RAM? (or is it disabled?)
            case 0xA: // RAM
                break;
        }
    }
    
    if (gb->cartridge_type->mbc_type == GB_TPP1) {
        switch (gb->tpp1.mode) {
            case 0:
                switch (addr & 3) {
                    case 0: return gb->tpp1.rom_bank;
                    case 1: return gb->tpp1.rom_bank >> 8;
                    case 2: return gb->tpp1.ram_bank;
                    case 3: return gb->rumble_strength | gb->tpp1_mr4;
                }
            case 2:
            case 3:
                break; // Read RAM
            case 5:
                return gb->rtc_latched.data[(addr & 3) ^ 3];
            default:
                return 0xFF;
        }
    }
    else if ((!gb->mbc_ram_enable) &&
        gb->cartridge_type->mbc_subtype != GB_CAMERA &&
        gb->cartridge_type->mbc_type != GB_HUC1 &&
        gb->cartridge_type->mbc_type != GB_HUC3) {
        return 0xFF;
    }
    
    if (gb->cartridge_type->mbc_type == GB_HUC1 && gb->huc1.ir_mode) {
        return 0xc0 | gb->effective_ir_input;
    }
    
    if (gb->cartridge_type->has_rtc && gb->cartridge_type->mbc_type != GB_HUC3 &&
        gb->mbc3.rtc_mapped) {
        /* RTC read */
        if (gb->mbc_ram_bank <= 4) {
            gb->rtc_latched.seconds &= 0x3F;
            gb->rtc_latched.minutes &= 0x3F;
            gb->rtc_latched.hours &= 0x1F;
            gb->rtc_latched.high &= 0xC1;
            return gb->rtc_latched.data[gb->mbc_ram_bank];
        }
        return 0xFF;
    }

    if (gb->camera_registers_mapped) {
        return GB_camera_read_register(gb, addr);
    }

    if (!gb->mbc_ram || !gb->mbc_ram_size) {
        return 0xFF;
    }

    if (gb->cartridge_type->mbc_subtype == GB_CAMERA && gb->mbc_ram_bank == 0 && addr >= 0xa100 && addr < 0xaf00) {
        return GB_camera_read_image(gb, addr - 0xa100);
    }

    uint8_t effective_bank = gb->mbc_ram_bank;
    if (gb->cartridge_type->mbc_type == GB_MBC3 && !gb->is_mbc30) {
        if (gb->cartridge_type->has_rtc) {
            if (effective_bank > 3) return 0xFF;
        }
        effective_bank &= 0x3;
    }
    uint8_t ret = gb->mbc_ram[((addr & 0x1FFF) + effective_bank * 0x2000) & (gb->mbc_ram_size - 1)];
    if (gb->cartridge_type->mbc_type == GB_MBC2) {
        ret |= 0xF0;
    }
    return ret;
}

static uint8_t read_ram(GB_gameboy_t *gb, uint16_t addr)
{
    return gb->ram[addr & 0x0FFF];
}

static uint8_t read_banked_ram(GB_gameboy_t *gb, uint16_t addr)
{
    return gb->ram[(addr & 0x0FFF) + gb->cgb_ram_bank * 0x1000];
}

static uint8_t read_high_memory(GB_gameboy_t *gb, uint16_t addr)
{

    if (gb->hdma_on) {
        return gb->last_opcode_read;
    }
    
    if (addr < 0xFE00) {
        return read_banked_ram(gb, addr);
    }

    if (addr < 0xFF00) {
        if (gb->oam_write_blocked && !GB_is_cgb(gb)) {
            GB_trigger_oam_bug_read(gb, addr);
            return 0xff;
        }
        
        if ((gb->dma_steps_left && (gb->dma_cycles > 0 || gb->is_dma_restarting))) {
            /* Todo: Does reading from OAM during DMA causes the OAM bug? */
            return 0xff;
        }
        
        if (gb->oam_read_blocked) {
            if (!GB_is_cgb(gb)) {
                if (addr < 0xFEA0) {
                    uint16_t *oam = (uint16_t *)gb->oam;
                    if (gb->accessed_oam_row == 0) {
                        oam[(addr & 0xf8) >> 1] =
                        oam[0] = bitwise_glitch_read(oam[0],
                                                     oam[(addr & 0xf8) >> 1],
                                                     oam[(addr & 0xff) >> 1]);

                        for (unsigned i = 2; i < 8; i++) {
                            gb->oam[i] = gb->oam[(addr & 0xf8) + i];
                        }
                    }
                    else if (gb->accessed_oam_row == 0xa0) {
                        uint8_t target = (addr & 7) | 0x98;
                        uint16_t a = oam[0x9c >> 1],
                                 b = oam[target >> 1],
                                 c = oam[(addr & 0xf8) >> 1];
                        switch (addr & 7) {
                            case 0:
                            case 1:
                                /* Probably instance specific */
                                if ((gb->model & GB_MODEL_FAMILY_MASK) == GB_MODEL_DMG_FAMILY) {
                                    oam[target >> 1] = (a & b) | (a & c) | (b & c);
                                }
                                else {
                                    oam[target >> 1] = bitwise_glitch_read(a, b, c);
                                }
                                break;
                            case 2:
                            case 3: {
                                /* Probably instance specific */
                                c = oam[(addr & 0xfe) >> 1];
                                
                                // MGB only: oam[target >> 1] = bitwise_glitch_read(a, b, c);
                                oam[target >> 1] = (a & b) | (a & c) | (b & c);
                                break;
                            }
                            case 4:
                            case 5:
                                break; // No additional corruption
                            case 6:
                            case 7:
                                oam[target >> 1] = bitwise_glitch_read(a, b, c);
                                break;
                                
                            default:
                                __builtin_unreachable();
                        }
                        
                        for (unsigned i = 0; i < 8; i++) {
                            gb->oam[(addr & 0xf8) + i] = gb->oam[0x98 + i];
                        }
                    }
                }
            }
            return 0xff;
        }
        
        if (addr < 0xFEA0) {
            return gb->oam[addr & 0xFF];
        }
        
        if (gb->oam_read_blocked) {
            return 0xFF;
        }
        
        switch (gb->model) {
            case GB_MODEL_CGB_E:
            case GB_MODEL_AGB:
                return (addr & 0xF0) | ((addr >> 4) & 0xF);

            case GB_MODEL_CGB_D:
                if (addr > 0xfec0) {
                    addr |= 0xf0;
                }
                return gb->extra_oam[addr - 0xfea0];
                
            case GB_MODEL_CGB_C:
            case GB_MODEL_CGB_B:
            // case GB_MODEL_CGB_A:
             case GB_MODEL_CGB_0:
                addr &= ~0x18;
                return gb->extra_oam[addr - 0xfea0];
                
            case GB_MODEL_DMG_B:
            case GB_MODEL_MGB:
            case GB_MODEL_SGB_NTSC:
            case GB_MODEL_SGB_PAL:
            case GB_MODEL_SGB_NTSC_NO_SFC:
            case GB_MODEL_SGB_PAL_NO_SFC:
            case GB_MODEL_SGB2:
            case GB_MODEL_SGB2_NO_SFC:
                break;
        }
    }

    if (addr < 0xFF00) {
        return 0;
    }

    if (addr < 0xFF80) {
        switch (addr & 0xFF) {
            case GB_IO_IF:
                return gb->io_registers[GB_IO_IF] | 0xE0;
            case GB_IO_TAC:
                return gb->io_registers[GB_IO_TAC] | 0xF8;
            case GB_IO_STAT:
                return gb->io_registers[GB_IO_STAT] | 0x80;
            case GB_IO_OPRI:
                if (!GB_is_cgb(gb)) {
                    return 0xFF;
                }
                return gb->io_registers[GB_IO_OPRI] | 0xFE;

            case GB_IO_PCM12:
                if (!GB_is_cgb(gb)) return 0xFF;
                return ((gb->apu.is_active[GB_SQUARE_2] ? (gb->apu.samples[GB_SQUARE_2] << 4) : 0) |
                        (gb->apu.is_active[GB_SQUARE_1] ? (gb->apu.samples[GB_SQUARE_1]) : 0)) & (gb->model <= GB_MODEL_CGB_C? gb->apu.pcm_mask[0] : 0xFF);
            case GB_IO_PCM34:
                if (!GB_is_cgb(gb)) return 0xFF;
                return ((gb->apu.is_active[GB_NOISE] ? (gb->apu.samples[GB_NOISE] << 4) : 0) |
                        (gb->apu.is_active[GB_WAVE] ? (gb->apu.samples[GB_WAVE]) : 0))  & (gb->model <= GB_MODEL_CGB_C? gb->apu.pcm_mask[1] : 0xFF);
            case GB_IO_JOYP:
                GB_timing_sync(gb);
            case GB_IO_TMA:
            case GB_IO_LCDC:
            case GB_IO_SCY:
            case GB_IO_SCX:
            case GB_IO_LY:
            case GB_IO_LYC:
            case GB_IO_BGP:
            case GB_IO_OBP0:
            case GB_IO_OBP1:
            case GB_IO_WY:
            case GB_IO_WX:
            case GB_IO_SC:
            case GB_IO_SB:
            case GB_IO_DMA:
                return gb->io_registers[addr & 0xFF];
            case GB_IO_TIMA:
                if (gb->tima_reload_state == GB_TIMA_RELOADING) {
                    return 0;
                }
                return gb->io_registers[GB_IO_TIMA];
            case GB_IO_DIV:
                return gb->div_counter >> 8;
            case GB_IO_HDMA5:
                if (!gb->cgb_mode) return 0xFF;
                return ((gb->hdma_on || gb->hdma_on_hblank)? 0 : 0x80) | ((gb->hdma_steps_left - 1) & 0x7F);
            case GB_IO_SVBK:
                if (!gb->cgb_mode) {
                    return 0xFF;
                }
                return gb->cgb_ram_bank | ~0x7;
            case GB_IO_VBK:
                if (!GB_is_cgb(gb)) {
                    return 0xFF;
                }
                return gb->cgb_vram_bank | ~0x1;

            /* Todo: It seems that a CGB in DMG mode can access BGPI and OBPI, but not BGPD and OBPD? */
            case GB_IO_BGPI:
            case GB_IO_OBPI:
                if (!GB_is_cgb(gb)) {
                    return 0xFF;
                }
                return gb->io_registers[addr & 0xFF] | 0x40;

            case GB_IO_BGPD:
            case GB_IO_OBPD:
            {
                if (!gb->cgb_mode && gb->boot_rom_finished) {
                    return 0xFF;
                }
                if (gb->cgb_palettes_blocked) {
                    return 0xFF;
                }
                uint8_t index_reg = (addr & 0xFF) - 1;
                return ((addr & 0xFF) == GB_IO_BGPD?
                       gb->background_palettes_data :
                       gb->sprite_palettes_data)[gb->io_registers[index_reg] & 0x3F];
            }

            case GB_IO_KEY1:
                if (!gb->cgb_mode) {
                    return 0xFF;
                }
                return (gb->io_registers[GB_IO_KEY1] & 0x7F) | (gb->cgb_double_speed? 0xFE : 0x7E);

            case GB_IO_RP: {
                if (!gb->cgb_mode) return 0xFF;
                /* You will read your own IR LED if it's on. */
                uint8_t ret = (gb->io_registers[GB_IO_RP] & 0xC1) | 0x2E;
                if (gb->model != GB_MODEL_CGB_E) {
                    ret |= 0x10;
                }
                if (((gb->io_registers[GB_IO_RP] & 0xC0) == 0xC0 && gb->effective_ir_input) && gb->model != GB_MODEL_AGB) {
                    ret &= ~2;
                }
                return ret;
            }
            case GB_IO_PSWX:
            case GB_IO_PSWY:
                return GB_is_cgb(gb)? gb->io_registers[addr & 0xFF] : 0xFF;
            case GB_IO_PSW:
                return gb->cgb_mode? gb->io_registers[addr & 0xFF] : 0xFF;
            case GB_IO_UNKNOWN5:
                return GB_is_cgb(gb)? gb->io_registers[addr & 0xFF] | 0x8F : 0xFF;
            default:
                if ((addr & 0xFF) >= GB_IO_NR10 && (addr & 0xFF) <= GB_IO_WAV_END) {
                    return GB_apu_read(gb, addr & 0xFF);
                }
                return 0xFF;
        }
        __builtin_unreachable();
    }

    if (addr == 0xFFFF) {
        /* Interrupt Mask */
        return gb->interrupt_enable;
    }

    /* HRAM */
    return gb->hram[addr - 0xFF80];
}

static read_function_t *const read_map[] =
{
    read_rom,         read_rom,         read_rom, read_rom,         /* 0XXX, 1XXX, 2XXX, 3XXX */
    read_mbc_rom,     read_mbc_rom,     read_mbc_rom, read_mbc_rom, /* 4XXX, 5XXX, 6XXX, 7XXX */
    read_vram,        read_vram,                                    /* 8XXX, 9XXX */
    read_mbc_ram,     read_mbc_ram,                                 /* AXXX, BXXX */
    read_ram,         read_banked_ram,                              /* CXXX, DXXX */
    read_ram,         read_high_memory,                             /* EXXX FXXX */
};

void GB_set_read_memory_callback(GB_gameboy_t *gb, GB_read_memory_callback_t callback)
{
    gb->read_memory_callback = callback;
}

uint8_t GB_read_memory(GB_gameboy_t *gb, uint16_t addr)
{
    if (gb->n_watchpoints) {
        GB_debugger_test_read_watchpoint(gb, addr);
    }
    if (is_addr_in_dma_use(gb, addr)) {
        addr = gb->dma_current_src;
    }
    uint8_t data = read_map[addr >> 12](gb, addr);
    GB_apply_cheat(gb, addr, &data);
    if (gb->read_memory_callback) {
        data = gb->read_memory_callback(gb, addr, data);
    }
    return data;
}

static void write_mbc(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    switch (gb->cartridge_type->mbc_type) {
        case GB_NO_MBC: return;
        case GB_MBC1:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000: gb->mbc_ram_enable = (value & 0xF) == 0xA; break;
                case 0x2000: case 0x3000: gb->mbc1.bank_low  = value; break;
                case 0x4000: case 0x5000: gb->mbc1.bank_high = value; break;
                case 0x6000: case 0x7000: gb->mbc1.mode      = value; break;
            }
            break;
        case GB_MBC2:
            switch (addr & 0x4100) {
                case 0x0000: gb->mbc_ram_enable = (value & 0xF) == 0xA; break;
                case 0x0100: gb->mbc2.rom_bank  = value; break;
            }
            break;
        case GB_MBC3:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000: gb->mbc_ram_enable = (value & 0xF) == 0xA; break;
                case 0x2000: case 0x3000: gb->mbc3.rom_bank  = value; break;
                case 0x4000: case 0x5000:
                    gb->mbc3.ram_bank  = value;
                    gb->mbc3.rtc_mapped = value & 8;
                    break;
                case 0x6000: case 0x7000:
                    memcpy(&gb->rtc_latched, &gb->rtc_real, sizeof(gb->rtc_real));
                    break;
            }
            break;
        case GB_MBC5:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000: gb->mbc_ram_enable      = value == 0x0A; break;
                case 0x2000:              gb->mbc5.rom_bank_low   = value; break;
                case 0x3000:              gb->mbc5.rom_bank_high  = value; break;
                case 0x4000: case 0x5000:
                    if (gb->cartridge_type->has_rumble) {
                        if (!!(value & 8) != !!gb->rumble_strength) {
                            gb->rumble_strength = gb->rumble_strength? 0 : 3;
                        }
                        value &= 7;
                    }
                    gb->mbc5.ram_bank = value;
                    gb->camera_registers_mapped = (value & 0x10) && gb->cartridge_type->mbc_subtype == GB_CAMERA;
                    break;
            }
            break;
        case GB_MBC7:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000: gb->mbc_ram_enable             = value == 0x0A; break;
                case 0x2000: case 0x3000: gb->mbc7.rom_bank              = value; break;
                case 0x4000: case 0x5000: gb->mbc7.secondary_ram_enable  = value == 0x40; break;
            }
            break;
        case GB_HUC1:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000: gb->huc1.ir_mode = (value & 0xF) == 0xE; break;
                case 0x2000: case 0x3000: gb->huc1.bank_low  = value; break;
                case 0x4000: case 0x5000: gb->huc1.bank_high = value; break;
                case 0x6000: case 0x7000: gb->huc1.mode      = value; break;
            }
            break;
        case GB_HUC3:
            switch (addr & 0xF000) {
                case 0x0000: case 0x1000:
                    gb->huc3.mode = value & 0xF;
                    gb->mbc_ram_enable = gb->huc3.mode == 0xA;
                    break;
                case 0x2000: case 0x3000: gb->huc3.rom_bank  = value; break;
                case 0x4000: case 0x5000: gb->huc3.ram_bank  = value; break;
            }
            break;
        case GB_TPP1:
            switch (addr & 3) {
                case 0:
                    gb->tpp1.rom_bank &= 0xFF00;
                    gb->tpp1.rom_bank |= value;
                    break;
                case 1:
                    gb->tpp1.rom_bank &= 0xFF;
                    gb->tpp1.rom_bank |= value << 8;
                    break;
                case 2:
                    gb->tpp1.ram_bank = value;
                    break;
                case 3:
                    switch (value) {
                        case 0:
                        case 2:
                        case 3:
                        case 5:
                            gb->tpp1.mode = value;
                            break;
                        case 0x10:
                            memcpy(&gb->rtc_latched, &gb->rtc_real, sizeof(gb->rtc_real));
                            break;
                        case 0x11: {
                            memcpy(&gb->rtc_real, &gb->rtc_latched, sizeof(gb->rtc_real));
                            break;
                        }
                        case 0x14:
                            gb->tpp1_mr4 &= ~0x8;
                            break;
                        case 0x18:
                            gb->tpp1_mr4 &= ~0x4;
                            break;
                        case 0x19:
                            gb->tpp1_mr4 |= 0x4;
                            break;
                            
                        case 0x20:
                        case 0x21:
                        case 0x22:
                        case 0x23:
                            gb->rumble_strength = value & 3;
                            break;
                    }
            }
            break;
    }
    GB_update_mbc_mappings(gb);
}

static void write_vram(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    if (gb->vram_write_blocked) {
        //GB_log(gb, "Wrote %02x to %04x (VRAM) during mode 3\n", value, addr);
        return;
    }
    /* TODO: not verified */
    if (gb->display_state == 22 && GB_is_cgb(gb) && !gb->cgb_double_speed) {
        if (addr & 0x1000) {
            addr = gb->last_tile_index_address;
        }
        else if (gb->last_tile_data_address & 0x1000) {
            /* TODO: This is case is more complicated then the rest and differ between revisions
             It's probably affected by how VRAM is layed out, might be easier after a decap is done */
        }
        else {
            addr = gb->last_tile_data_address;
        }
    }
    gb->vram[(addr & 0x1FFF) + (gb->cgb_vram_bank? 0x2000 : 0)] = value;
}

static bool huc3_write(GB_gameboy_t *gb, uint8_t value)
{
    switch (gb->huc3.mode) {
        case 0xB: // RTC Write
            switch (value >> 4) {
                case 1:
                    if (gb->huc3.access_index < 3) {
                        gb->huc3.read = (gb->huc3.minutes >> (gb->huc3.access_index  * 4)) & 0xF;
                    }
                    else if (gb->huc3.access_index < 7) {
                        gb->huc3.read = (gb->huc3.days >> ((gb->huc3.access_index - 3) * 4)) & 0xF;
                    }
                    else {
                        // GB_log(gb, "Attempting to read from unsupported HuC-3 register: %03x\n", gb->huc3.access_index);
                    }
                    gb->huc3.access_index++;
                    break;
                case 2:
                case 3:
                    if (gb->huc3.access_index < 3) {
                        gb->huc3.minutes &= ~(0xF << (gb->huc3.access_index * 4));
                        gb->huc3.minutes |= ((value & 0xF) << (gb->huc3.access_index * 4));
                    }
                    else if (gb->huc3.access_index < 7)  {
                        gb->huc3.days &= ~(0xF << ((gb->huc3.access_index - 3) * 4));
                        gb->huc3.days |= ((value & 0xF) << ((gb->huc3.access_index - 3) * 4));
                    }
                    else if (gb->huc3.access_index >= 0x58 && gb->huc3.access_index <= 0x5a) {
                        gb->huc3.alarm_minutes &= ~(0xF << ((gb->huc3.access_index - 0x58) * 4));
                        gb->huc3.alarm_minutes |= ((value & 0xF) << ((gb->huc3.access_index - 0x58) * 4));
                    }
                    else if (gb->huc3.access_index >= 0x5b && gb->huc3.access_index <= 0x5e) {
                        gb->huc3.alarm_days &= ~(0xF << ((gb->huc3.access_index - 0x5b) * 4));
                        gb->huc3.alarm_days |= ((value & 0xF) << ((gb->huc3.access_index - 0x5b) * 4));
                    }
                    else if (gb->huc3.access_index == 0x5f) {
                        gb->huc3.alarm_enabled = value & 1;
                    }
                    else {
                        // GB_log(gb, "Attempting to write %x to unsupported HuC-3 register: %03x\n", value & 0xF, gb->huc3.access_index);
                    }
                    if ((value >> 4) == 3) {
                        gb->huc3.access_index++;
                    }
                    break;
                case 4:
                    gb->huc3.access_index &= 0xF0;
                    gb->huc3.access_index |= value & 0xF;
                    break;
                case 5:
                    gb->huc3.access_index &= 0x0F;
                    gb->huc3.access_index |= (value & 0xF) << 4;
                    break;
                case 6:
                    gb->huc3.access_flags = (value & 0xF);
                    break;
                    
                default:
                    break;
            }
            
            return true;
        case 0xD: // RTC status
            // Not sure what writes here mean, they're always 0xFE
            return true;
        case 0xE: { // IR mode
            if (gb->cart_ir != (value & 1)) {
                gb->cart_ir = value & 1;
                if (gb->infrared_callback) {
                    gb->infrared_callback(gb, value & 1);
                }
            }
            return true;
        }
        case 0xC:
            return true;
        default:
            return false;
        case 0: // Disabled
        case 0xA: // RAM
            return false;
    }
}

static void write_mbc7_ram(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    if (!gb->mbc_ram_enable || !gb->mbc7.secondary_ram_enable) return;
    if (addr >= 0xB000) return;
    switch ((addr >> 4) & 0xF) {
        case 0: {
            if (value == 0x55) {
                gb->mbc7.latch_ready = true;
                gb->mbc7.x_latch = gb->mbc7.y_latch = 0x8000;
            }
        }
        case 1: {
            if (value == 0xAA) {
                gb->mbc7.latch_ready = false;
                gb->mbc7.x_latch = 0x81D0 + 0x70 * gb->accelerometer_x;
                gb->mbc7.y_latch = 0x81D0 + 0x70 * gb->accelerometer_y;
            }
        }
        case 8: {
            gb->mbc7.eeprom_cs = value & 0x80;
            gb->mbc7.eeprom_di = value & 2;
            if (gb->mbc7.eeprom_cs) {
                if (!gb->mbc7.eeprom_clk && (value & 0x40)) { // Clocked
                    gb->mbc7.eeprom_do = gb->mbc7.read_bits >> 15;
                    gb->mbc7.read_bits <<= 1;
                    gb->mbc7.read_bits |= 1;
                    if (gb->mbc7.bits_countdown == 0) {
                        /* Not transferring extra bits for a command*/
                        gb->mbc7.eeprom_command <<= 1;
                        gb->mbc7.eeprom_command |= gb->mbc7.eeprom_di;
                        if (gb->mbc7.eeprom_command & 0x400) {
                            // Got full command
                            switch ((gb->mbc7.eeprom_command >> 6) & 0xF) {
                                case 0x8:
                                case 0x9:
                                case 0xA:
                                case 0xB:
                                    // READ
                                    gb->mbc7.read_bits = LE16(((uint16_t *)gb->mbc_ram)[gb->mbc7.eeprom_command & 0x7F]);
                                    gb->mbc7.eeprom_command = 0;
                                    break;
                                case 0x3: // EWEN (Eeprom Write ENable)
                                    gb->mbc7.eeprom_write_enabled = true;
                                    gb->mbc7.eeprom_command = 0;
                                    break;
                                case 0x0: // EWDS (Eeprom Write DiSable)
                                    gb->mbc7.eeprom_write_enabled = false;
                                    gb->mbc7.eeprom_command = 0;
                                    break;
                                case 0x4:
                                case 0x5:
                                case 0x6:
                                case 0x7:
                                    // WRITE
                                    if (gb->mbc7.eeprom_write_enabled) {
                                        ((uint16_t *)gb->mbc_ram)[gb->mbc7.eeprom_command & 0x7F] = 0;
                                    }
                                    gb->mbc7.bits_countdown = 16;
                                    // We still need to process this command, don't erase eeprom_command
                                    break;
                                case 0xC:
                                case 0xD:
                                case 0xE:
                                case 0xF:
                                    // ERASE
                                    if (gb->mbc7.eeprom_write_enabled) {
                                        ((uint16_t *)gb->mbc_ram)[gb->mbc7.eeprom_command & 0x7F] = 0xFFFF;
                                        gb->mbc7.read_bits = 0x3FFF; // Emulate some time to settle
                                    }
                                    gb->mbc7.eeprom_command = 0;
                                    break;
                                case 0x2:
                                    // ERAL (ERase ALl)
                                    if (gb->mbc7.eeprom_write_enabled) {
                                        memset(gb->mbc_ram, 0xFF, gb->mbc_ram_size);
                                        ((uint16_t *)gb->mbc_ram)[gb->mbc7.eeprom_command & 0x7F] = 0xFFFF;
                                        gb->mbc7.read_bits = 0xFF; // Emulate some time to settle
                                    }
                                    gb->mbc7.eeprom_command = 0;
                                    break;
                                case 0x1:
                                    // WRAL (WRite ALl)
                                    if (gb->mbc7.eeprom_write_enabled) {
                                        memset(gb->mbc_ram, 0, gb->mbc_ram_size);
                                    }
                                    gb->mbc7.bits_countdown = 16;
                                    // We still need to process this command, don't erase eeprom_command
                                    break;
                            }
                        }
                    }
                    else {
                        // We're shifting in extra bits for a WRITE/WRAL command
                        gb->mbc7.bits_countdown--;
                        gb->mbc7.eeprom_do = true;
                        if (gb->mbc7.eeprom_di) {
                            uint16_t bit = LE16(1 << gb->mbc7.bits_countdown);
                            if (gb->mbc7.eeprom_command & 0x100) {
                                // WRITE
                                ((uint16_t *)gb->mbc_ram)[gb->mbc7.eeprom_command & 0x7F] |= bit;
                            }
                            else {
                                // WRAL
                                for (unsigned i = 0; i < 0x7F; i++) {
                                    ((uint16_t *)gb->mbc_ram)[i] |= bit;
                                }
                            }
                        }
                        if (gb->mbc7.bits_countdown == 0) { // We're done
                            gb->mbc7.eeprom_command = 0;
                            gb->mbc7.read_bits = (gb->mbc7.eeprom_command & 0x100)? 0xFF : 0x3FFF; // Emulate some time to settle
                        }
                    }
                }
            }
            gb->mbc7.eeprom_clk = value & 0x40;
        }
    }
}

static void write_mbc_ram(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    if (gb->cartridge_type->mbc_type == GB_MBC7) {
        write_mbc7_ram(gb, addr, value);
        return;
    }
    
    if (gb->cartridge_type->mbc_type == GB_HUC3) {
        if (huc3_write(gb, value)) return;
    }
    
    if (gb->camera_registers_mapped) {
        GB_camera_write_register(gb, addr, value);
        return;
    }
    
    if (gb->cartridge_type->mbc_type == GB_TPP1) {
        switch (gb->tpp1.mode) {
            case 3:
                break;
            case 5:
                gb->rtc_latched.data[(addr & 3) ^ 3] = value;
                return;
            default:
                return;
        }
    }
    
    if ((!gb->mbc_ram_enable)
       && gb->cartridge_type->mbc_type != GB_HUC1) return;
    
    if (gb->cartridge_type->mbc_type == GB_HUC1 && gb->huc1.ir_mode) {
        if (gb->cart_ir != (value & 1)) {
            gb->cart_ir = value & 1;
            if (gb->infrared_callback) {
                gb->infrared_callback(gb, value & 1);
            }
        }
        return;
    }

    if (gb->cartridge_type->has_rtc && gb->mbc3.rtc_mapped) {
        if (gb->mbc_ram_bank <= 4) {
            if (gb->mbc_ram_bank == 0) {
                gb->rtc_cycles = 0;
            }
            gb->rtc_real.data[gb->mbc_ram_bank] = value;
        }
        return;
    }

    if (!gb->mbc_ram || !gb->mbc_ram_size) {
        return;
    }
    
    uint8_t effective_bank = gb->mbc_ram_bank;
    if (gb->cartridge_type->mbc_type == GB_MBC3 && !gb->is_mbc30) {
        if (gb->cartridge_type->has_rtc) {
            if (effective_bank > 3) return;
        }
        effective_bank &= 0x3;
    }

    gb->mbc_ram[((addr & 0x1FFF) + effective_bank * 0x2000) & (gb->mbc_ram_size - 1)] = value;
}

static void write_ram(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    gb->ram[addr & 0x0FFF] = value;
}

static void write_banked_ram(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    gb->ram[(addr & 0x0FFF) + gb->cgb_ram_bank * 0x1000] = value;
}

static void write_high_memory(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    if (addr < 0xFE00) {
        GB_log(gb, "Wrote %02x to %04x (RAM Mirror)\n", value, addr);
        write_banked_ram(gb, addr, value);
        return;
    }

    if (addr < 0xFF00) {
        if (gb->oam_write_blocked) {
            GB_trigger_oam_bug(gb, addr);
            return;
        }
        
        if ((gb->dma_steps_left && (gb->dma_cycles > 0 || gb->is_dma_restarting))) {
            /* Todo: Does writing to OAM during DMA causes the OAM bug? */
            return;
        }
        
        if (GB_is_cgb(gb)) {
            if (addr < 0xFEA0) {
                gb->oam[addr & 0xFF] = value;
            }
            switch (gb->model) {
                case GB_MODEL_CGB_D:
                    if (addr > 0xfec0) {
                        addr |= 0xf0;
                    }
                    gb->extra_oam[addr - 0xfea0] = value;
                    break;
                case GB_MODEL_CGB_C:
                case GB_MODEL_CGB_B:
                // case GB_MODEL_CGB_A:
                case GB_MODEL_CGB_0: 
                    addr &= ~0x18;
                    gb->extra_oam[addr - 0xfea0] = value;
                    break;
                case GB_MODEL_DMG_B:
                case GB_MODEL_MGB:
                case GB_MODEL_SGB_NTSC:
                case GB_MODEL_SGB_PAL:
                case GB_MODEL_SGB_NTSC_NO_SFC:
                case GB_MODEL_SGB_PAL_NO_SFC:
                case GB_MODEL_SGB2:
                case GB_MODEL_SGB2_NO_SFC:
                case GB_MODEL_CGB_E:
                case GB_MODEL_AGB:
                    break;
            }
            return;
        }
        
        if (addr < 0xFEA0) {
            if (gb->accessed_oam_row == 0xa0) {
                for (unsigned i = 0; i < 8; i++) {
                    if ((i & 6)  != (addr & 6)) {
                        gb->oam[(addr & 0xf8) + i] = gb->oam[0x98 + i];
                    }
                    else {
                        gb->oam[(addr & 0xf8) + i] = bitwise_glitch(gb->oam[(addr & 0xf8) + i], gb->oam[0x9c], gb->oam[0x98 + i]);
                    }
                }
            }
            
            gb->oam[addr & 0xFF] = value;
            
            if (gb->accessed_oam_row == 0) {
                gb->oam[0] = bitwise_glitch(gb->oam[0],
                                            gb->oam[(addr & 0xf8)],
                                            gb->oam[(addr & 0xfe)]);
                gb->oam[1] = bitwise_glitch(gb->oam[1],
                                            gb->oam[(addr & 0xf8) + 1],
                                            gb->oam[(addr & 0xfe) | 1]);
                for (unsigned i = 2; i < 8; i++) {
                    gb->oam[i] = gb->oam[(addr & 0xf8) + i];
                }
            }
        }
        else if (gb->accessed_oam_row == 0) {
            gb->oam[addr & 0x7] = value;
        }
        return;
    }

    /* Todo: Clean this code up: use a function table and move relevant code to display.c and timing.c
       (APU read and writes are already at apu.c) */
    if (addr < 0xFF80) {
        /* Hardware registers */
        switch (addr & 0xFF) {
            case GB_IO_WY:
                if (value == gb->current_line) {
                    gb->wy_triggered = true;
                }
            case GB_IO_WX:
            case GB_IO_IF:
            case GB_IO_SCX:
            case GB_IO_SCY:
            case GB_IO_BGP:
            case GB_IO_OBP0:
            case GB_IO_OBP1:
            case GB_IO_SB:
            case GB_IO_PSWX:
            case GB_IO_PSWY:
            case GB_IO_PSW:
            case GB_IO_UNKNOWN5:
                gb->io_registers[addr & 0xFF] = value;
                return;
            case GB_IO_OPRI:
                if ((!gb->boot_rom_finished || (gb->io_registers[GB_IO_KEY0] & 8)) && GB_is_cgb(gb)) {
                    gb->io_registers[addr & 0xFF] = value;
                    gb->object_priority = (value & 1) ? GB_OBJECT_PRIORITY_X : GB_OBJECT_PRIORITY_INDEX;
                }
                else if (gb->cgb_mode) {
                    gb->io_registers[addr & 0xFF] = value;

                }
                return;
            case GB_IO_LYC:
                
                /* TODO: Probably completely wrong in double speed mode */
                
                /* TODO: This hack is disgusting */
                if (gb->display_state == 29 && GB_is_cgb(gb)) {
                    gb->ly_for_comparison = 153;
                    GB_STAT_update(gb);
                    gb->ly_for_comparison = 0;
                }
                
                gb->io_registers[addr & 0xFF] = value;
                
                /* These are the states when LY changes, let the display routine call GB_STAT_update for use
                   so it correctly handles T-cycle accurate LYC writes */
                if (!GB_is_cgb(gb)  || (
                    gb->display_state != 35 &&
                    gb->display_state != 26 &&
                    gb->display_state != 15 &&
                    gb->display_state != 16)) {
                    
                    /* More hacks to make LYC write conflicts work */
                    if (gb->display_state == 14 && GB_is_cgb(gb)) {
                        gb->ly_for_comparison = 153;
                        GB_STAT_update(gb);
                        gb->ly_for_comparison = -1;
                    }
                    else {
                        GB_STAT_update(gb);
                    }
                }
                return;
                
            case GB_IO_TIMA:
                if (gb->tima_reload_state != GB_TIMA_RELOADED) {
                    gb->io_registers[GB_IO_TIMA] = value;
                }
                return;

            case GB_IO_TMA:
                gb->io_registers[GB_IO_TMA] = value;
                if (gb->tima_reload_state != GB_TIMA_RUNNING) {
                    gb->io_registers[GB_IO_TIMA] = value;
                }
                return;

            case GB_IO_TAC:
                GB_emulate_timer_glitch(gb, gb->io_registers[GB_IO_TAC], value);
                gb->io_registers[GB_IO_TAC] = value;
                return;


            case GB_IO_LCDC:
                if ((value & 0x80) && !(gb->io_registers[GB_IO_LCDC] & 0x80)) {
                    gb->display_cycles = 0;
                    gb->display_state = 0;
                    gb->double_speed_alignment = 0;
                    if (GB_is_sgb(gb)) {
                        gb->frame_skip_state = GB_FRAMESKIP_SECOND_FRAME_RENDERED;
                    }
                    else if (gb->frame_skip_state == GB_FRAMESKIP_SECOND_FRAME_RENDERED) {
                        gb->frame_skip_state = GB_FRAMESKIP_LCD_TURNED_ON;
                    }
                    GB_timing_sync(gb);
                }
                else if (!(value & 0x80) && (gb->io_registers[GB_IO_LCDC] & 0x80)) {
                    /* Sync after turning off LCD */
                    gb->double_speed_alignment = 0;
                    GB_timing_sync(gb);
                    GB_lcd_off(gb);
                }
                /* Handle disabling objects while already fetching an object */
                if ((gb->io_registers[GB_IO_LCDC] & 2) && !(value & 2)) {
                    if (gb->during_object_fetch) {
                        gb->cycles_for_line += gb->display_cycles;
                        gb->display_cycles = 0;
                        gb->object_fetch_aborted = true;
                    }
                }
                gb->io_registers[GB_IO_LCDC] = value;
                if (!(value & 0x20)) {
                    gb->wx_triggered = false;
                    gb->wx166_glitch = false;
                }
                return;

            case GB_IO_STAT:
                /* Delete previous R/W bits */
                gb->io_registers[GB_IO_STAT] &= 7;
                /* Set them by value */
                gb->io_registers[GB_IO_STAT] |= value & ~7;
                /* Set unused bit to 1 */
                gb->io_registers[GB_IO_STAT] |= 0x80;
                
                GB_STAT_update(gb);
                return;

            case GB_IO_DIV:
                GB_set_internal_div_counter(gb, 0);
                /* Reset the div state machine */
                gb->div_state = 0;
                gb->div_cycles = 0;
                return;

            case GB_IO_JOYP:
                if (gb->joyp_write_callback) {
                    gb->joyp_write_callback(gb, value);
                    GB_update_joyp(gb);
                }
                else if ((gb->io_registers[GB_IO_JOYP] & 0x30) != (value & 0x30)) {
                    GB_sgb_write(gb, value);
                    gb->io_registers[GB_IO_JOYP] = (value & 0xF0) | (gb->io_registers[GB_IO_JOYP] & 0x0F);
                    GB_update_joyp(gb);
                }
                return;

            case GB_IO_BANK:
                gb->boot_rom_finished = true;
                return;

            case GB_IO_KEY0:
                if (GB_is_cgb(gb) && !gb->boot_rom_finished) {
                    gb->cgb_mode = !(value & 0xC); /* The real "contents" of this register aren't quite known yet. */
                    gb->io_registers[GB_IO_KEY0] = value;
                }
                return;

            case GB_IO_DMA:
                if (gb->dma_steps_left) {
                    /* This is not correct emulation, since we're not really delaying the second DMA.
                       One write that should have happened in the first DMA will not happen. However,
                       since that byte will be overwritten by the second DMA before it can actually be
                       read, it doesn't actually matter. */
                    gb->is_dma_restarting = true;
                }
                gb->dma_cycles = -7;
                gb->dma_current_dest = 0;
                gb->dma_current_src = value << 8;
                gb->dma_steps_left = 0xa0;
                gb->io_registers[GB_IO_DMA] = value;
                return;
            case GB_IO_SVBK:
                if (gb->cgb_mode || (GB_is_cgb(gb) && !gb->boot_rom_finished)) {
                    gb->cgb_ram_bank = value & 0x7;
                    if (!gb->cgb_ram_bank) {
                        gb->cgb_ram_bank++;
                    }
                }
                return;
            case GB_IO_VBK:
                if (!gb->cgb_mode) {
                    return;
                }
                gb->cgb_vram_bank = value & 0x1;
                return;

            case GB_IO_BGPI:
            case GB_IO_OBPI:
                if (!GB_is_cgb(gb)) {
                    return;
                }
                gb->io_registers[addr & 0xFF] = value;
                return;
            case GB_IO_BGPD:
            case GB_IO_OBPD:
                if (!gb->cgb_mode && gb->boot_rom_finished) {
                    /* Todo: Due to the behavior of a broken Game & Watch Gallery 2 ROM on a real CGB. A proper test ROM
                       is required. */
                    return;
                }
                
                uint8_t index_reg = (addr & 0xFF) - 1;
                if (gb->cgb_palettes_blocked) {
                    if (gb->io_registers[index_reg] & 0x80) {
                        gb->io_registers[index_reg]++;
                        gb->io_registers[index_reg] |= 0x80;
                    }
                    return;
                }
                ((addr & 0xFF) == GB_IO_BGPD?
                 gb->background_palettes_data :
                 gb->sprite_palettes_data)[gb->io_registers[index_reg] & 0x3F] = value;
                GB_palette_changed(gb, (addr & 0xFF) == GB_IO_BGPD, gb->io_registers[index_reg] & 0x3F);
                if (gb->io_registers[index_reg] & 0x80) {
                    gb->io_registers[index_reg]++;
                    gb->io_registers[index_reg] |= 0x80;
                }
                return;
            case GB_IO_KEY1:
                if (!gb->cgb_mode) {
                    return;
                }
                gb->io_registers[GB_IO_KEY1] = value;
                return;
            case GB_IO_HDMA1:
                if (gb->cgb_mode) {
                    gb->hdma_current_src &= 0xF0;
                    gb->hdma_current_src |= value << 8;
                }
                return;
            case GB_IO_HDMA2:
                if (gb->cgb_mode) {
                    gb->hdma_current_src &= 0xFF00;
                    gb->hdma_current_src |= value & 0xF0;
                }
                return;
            case GB_IO_HDMA3:
                if (gb->cgb_mode) {
                    gb->hdma_current_dest &= 0xF0;
                    gb->hdma_current_dest |= value << 8;
                }
                return;
            case GB_IO_HDMA4:
                if (gb->cgb_mode) {
                    gb->hdma_current_dest &= 0x1F00;
                    gb->hdma_current_dest |= value & 0xF0;
                }
                return;
            case GB_IO_HDMA5:
                if (!gb->cgb_mode) return;
                if ((value & 0x80) == 0 && gb->hdma_on_hblank) {
                    gb->hdma_on_hblank = false;
                    return;
                }
                gb->hdma_on = (value & 0x80) == 0;
                gb->hdma_on_hblank = (value & 0x80) != 0;
                if (gb->hdma_on_hblank && (gb->io_registers[GB_IO_STAT] & 3) == 0) {
                    gb->hdma_on = true;
                }
                gb->io_registers[GB_IO_HDMA5] = value;
                gb->hdma_steps_left = (gb->io_registers[GB_IO_HDMA5] & 0x7F) + 1;
                /* Todo: Verify this. Gambatte's DMA tests require this. */
                if (gb->hdma_current_dest + (gb->hdma_steps_left << 4) > 0xFFFF) {
                    gb->hdma_steps_left = (0x10000 - gb->hdma_current_dest) >> 4;
                }
                gb->hdma_cycles = -12;
                return;

            /*  Todo: what happens when starting a transfer during a transfer?
                What happens when starting a transfer during external clock? 
            */
            case GB_IO_SC:
                if (!gb->cgb_mode) {
                    value |= 2;
                }
                gb->io_registers[GB_IO_SC] = value | (~0x83);
                if ((value & 0x80) && (value & 0x1) ) {
                    gb->serial_length = gb->cgb_mode && (value & 2)? 16 : 512;
                    gb->serial_count = 0;
                    /* Todo: This is probably incorrect for CGB's faster clock mode. */
                    gb->serial_cycles &= 0xFF;
                    if (gb->serial_transfer_bit_start_callback) {
                        gb->serial_transfer_bit_start_callback(gb, gb->io_registers[GB_IO_SB] & 0x80);
                    }
                }
                else {
                    gb->serial_length = 0;
                }
                return;

            case GB_IO_RP: {
                if (!GB_is_cgb(gb)) {
                    return;
                }
                if ((gb->io_registers[GB_IO_RP] ^ value) & 1) {
                    if (gb->infrared_callback) {
                        gb->infrared_callback(gb, value & 1);
                    }
                }
                gb->io_registers[GB_IO_RP] = value;

                return;
            }

            default:
                if ((addr & 0xFF) >= GB_IO_NR10 && (addr & 0xFF) <= GB_IO_WAV_END) {
                    GB_apu_write(gb, addr & 0xFF, value);
                    return;
                }
                GB_log(gb, "Wrote %02x to %04x (HW Register)\n", value, addr);
                return;
        }
    }

    if (addr == 0xFFFF) {
        /* Interrupt mask */
        gb->interrupt_enable = value;
        return;
    }
    
    /* HRAM */
    gb->hram[addr - 0xFF80] = value;
}



static write_function_t *const write_map[] =
{
    write_mbc,         write_mbc,        write_mbc, write_mbc, /* 0XXX, 1XXX, 2XXX, 3XXX */
    write_mbc,         write_mbc,        write_mbc, write_mbc, /* 4XXX, 5XXX, 6XXX, 7XXX */
    write_vram,        write_vram,                             /* 8XXX, 9XXX */
    write_mbc_ram,     write_mbc_ram,                          /* AXXX, BXXX */
    write_ram,         write_banked_ram,                       /* CXXX, DXXX */
    write_ram,         write_high_memory,                      /* EXXX FXXX */
};

void GB_write_memory(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
{
    if (gb->n_watchpoints) {
        GB_debugger_test_write_watchpoint(gb, addr, value);
    }
    if (is_addr_in_dma_use(gb, addr)) {
        /* Todo: What should happen? Will this affect DMA? Will data be written? What and where? */
        return;
    }
    write_map[addr >> 12](gb, addr, value);
}

void GB_dma_run(GB_gameboy_t *gb)
{
    while (gb->dma_cycles >= 4 && gb->dma_steps_left) {
        /* Todo: measure this value */
        gb->dma_cycles -= 4;
        gb->dma_steps_left--;
        
        if (gb->dma_current_src < 0xe000) {
            gb->oam[gb->dma_current_dest++] = GB_read_memory(gb, gb->dma_current_src);
        }
        else {
            /* Todo: Not correct on the CGB */
            gb->oam[gb->dma_current_dest++] = GB_read_memory(gb, gb->dma_current_src & ~0x2000);
        }
        
        /* dma_current_src must be the correct value during GB_read_memory */
        gb->dma_current_src++;
        if (!gb->dma_steps_left) {
            gb->is_dma_restarting = false;
        }
    }
}

void GB_hdma_run(GB_gameboy_t *gb)
{
    if (!gb->hdma_on) return;

    while (gb->hdma_cycles >= 0x4) {
        gb->hdma_cycles -= 0x4;

        GB_write_memory(gb, 0x8000 | (gb->hdma_current_dest++ & 0x1FFF), GB_read_memory(gb, (gb->hdma_current_src++)));
        
        if ((gb->hdma_current_dest & 0xf) == 0) {
            if (--gb->hdma_steps_left == 0) {
                gb->hdma_on = false;
                gb->hdma_on_hblank = false;
                gb->hdma_starting = false;
                gb->io_registers[GB_IO_HDMA5] &= 0x7F;
                break;
            }
            if (gb->hdma_on_hblank) {
                gb->hdma_on = false;
                break;
            }
        }
    }
}