//
// Created by michal on 23.4.18.
//

#include <registers/spi_reg.h>
#include <drivers/spi_master.h>
//#include <drivers/clock.h> TODO
#include <system/system_locks.h>
#include <math.h>
#include <drivers/spi.h>
#include <system/system_interrupts.h>
#include <rom/rom.h>

/**  CS pre-transmission delay
 * Unless specified in setup_time (in that case it sums up with setup_time)
 * Setting setup_time works only in half duplex mode due to hardware bug
 *
 * SPI_CLOCK_MODE_0: half a cycle before clock goes high
 * SPI_CLOCK_MODE_1: at the same time as clock goes high
 * SPI_CLOCK_MODE_2: half a cycle before clock goes low
 * SPI_CLOCK_MODE_3: at the same time as clock goes low
 */

typedef struct
{
    bool wr_bit_order;
    bool rd_bit_order;
    bool wp;
    uint8_t mosi_delay_num;
    uint8_t mosi_delay_mode;
    uint8_t miso_delay_num;
    uint8_t miso_delay_mode;
    // bool cs_hold; TODO implement?
    // uint8_t hold_time; TODO implement?
    // uint8_t cs_hold_delay; TODO implement?
    // bool cs_setup; TODO works only in half duplex - probably won't implement
    // uint8_t setup_time; TODO works only in half duplex - probably won't implement
    bool clk_equ_sysclk;
    uint8_t clkdiv_pre;
    uint8_t clkcnt_n;
    uint8_t clkcnt_h;
    uint8_t clkcnt_l;
    bool ck_out_edge;
    bool cs_keep_active;
    bool ck_idle_edge;
    bool usr_dummy_idle;
    // bool ck_dis; TODO implement?
    // uint8_t master_ck_sel; TODO implement?
} SPI_Master_Saved_Config_t;

typedef struct
{
    SPI_Handle_t * spi_handle;
    SPI_Master_Saved_Config_t devices[3];
} SPI_Master_Handle_t;

/* ------------------------------------------------------------------------------------------------------------------ */

extern SPI_Handle_t spi_handles[4];

static SPI_Master_Handle_t spi_master_handles[4] =
{
    {
        .spi_handle = &spi_handles[0]
    },
    {
        .spi_handle = &spi_handles[1]
    },
    {
        .spi_handle = &spi_handles[2]
    },
    {
        .spi_handle = &spi_handles[3]
    }
};

// will be called when switching from one CS line to another to preserve custom config for every CS line
IRAM_ATTR static void SPI_Master_Load_Config(SPI_Device_t device, SPI_Master_CS_t cs_line)
{
    // should already be in lock here

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];
    SPI_HW_t * target_device = target_handle->spi_handle->device;
    const SPI_Master_Saved_Config_t * target_config = &(target_handle->devices[cs_line]);

    target_device->ctrl.wr_bit_order = target_config->wr_bit_order;
    target_device->ctrl.rd_bit_order = target_config->rd_bit_order;
    target_device->ctrl.wp = target_config->wp;

    target_device->ctrl2.mosi_delay_num = target_config->mosi_delay_num;
    target_device->ctrl2.mosi_delay_mode = target_config->mosi_delay_mode;
    target_device->ctrl2.miso_delay_num = target_config->miso_delay_num;
    target_device->ctrl2.miso_delay_mode = target_config->miso_delay_mode;

    target_device->clock.clk_equ_sysclk = target_config->clk_equ_sysclk;
    target_device->clock.clkdiv_pre = target_config->clkdiv_pre;
    target_device->clock.clkcnt_n = target_config->clkcnt_n;
    target_device->clock.clkcnt_h = target_config->clkcnt_h;
    target_device->clock.clkcnt_l = target_config->clkcnt_l;

    target_device->user.ck_out_edge = target_config->ck_out_edge;
    target_device->pin.ck_idle_edge = target_config->ck_idle_edge;

    target_device->pin.cs_keep_active = target_config->cs_keep_active;

    target_device->user.usr_dummy_idle = target_config->usr_dummy_idle;

    switch (cs_line)
    {
        case SPI_MASTER_CS0:
            target_device->pin.cs0_dis = 0;
            target_device->pin.cs1_dis = 1;
            target_device->pin.cs2_dis = 1;
            break;
        case SPI_MASTER_CS1:
            target_device->pin.cs0_dis = 1;
            target_device->pin.cs1_dis = 0;
            target_device->pin.cs2_dis = 1;
            break;
        case SPI_MASTER_CS2:
            target_device->pin.cs0_dis = 1;
            target_device->pin.cs1_dis = 1;
            target_device->pin.cs2_dis = 0;
            break;
    }
}

/* ------------------------------------------------------------------------------------------------------------------ */

Status_t SPI_Master_Config(SPI_Device_t device, SPI_Master_CS_t cs_line, const SPI_Master_Config_t * config)
{
    assert_param(SPI_DEVICE_CHECK(device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(SPI_CLOCK_MODE_CHECK(config->clock_mode));
    assert_param(SPI_BIT_ORDER_CHECK(config->read_bit_order));
    assert_param(SPI_BIT_ORDER_CHECK(config->write_bit_order));

    uint32_t apb_frequency = 40000000; // TODO Clock_Get_APB_Frequency();

    assert_param(config->frequency <= apb_frequency);

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;
    SPI_Master_Saved_Config_t * target_config = &(target_handle->devices[cs_line]);

    target_config->cs_keep_active = config->flags & SPI_MASTER_FLAG_CS_KEEP_ENABLED;
    target_config->wp = !(config->flags & SPI_MASTER_FLAG_WP_IDLE_LOW);
    target_config->usr_dummy_idle = config->flags & SPI_MASTER_FLAG_DUMMY_CLOCK_DISABLE;

    if (config->flags & SPI_MASTER_FLAG_CS_ACTIVE_HIGH)
        target_device->pin.master_cs_pol |= 1u << cs_line;
    else
        target_device->pin.master_cs_pol &= ~(1u << cs_line);

    // set clock dividers

    unsigned target_clock_divider = (unsigned) roundf((float) apb_frequency / config->frequency);

    // TODO make duty cycle configurable?
    // setting too high value may cause SPI hardware to stop working

    if (target_clock_divider == 1)
        target_config->clk_equ_sysclk = 1;
    else if (target_clock_divider == 2)
    {
        target_config->clk_equ_sysclk = 0;
        target_config->clkdiv_pre = 0;
        target_config->clkcnt_n = target_clock_divider - 1;
        target_config->clkcnt_h = 0;
        target_config->clkcnt_l = target_config->clkcnt_n;
    }
    else if (target_clock_divider <= 64)
    {
        target_config->clk_equ_sysclk = 0;
        target_config->clkdiv_pre = 0;
        target_config->clkcnt_n = target_clock_divider - 1;
        target_config->clkcnt_h = target_config->clkcnt_n / 2 - 1;
        target_config->clkcnt_l = target_config->clkcnt_n;
    } else
    {
        for (uint8_t i = 64; i > 0; i--)
            if (target_clock_divider % i == 0)
            {
                target_config->clk_equ_sysclk = 0;
                target_config->clkdiv_pre = target_clock_divider / i - 1;
                target_config->clkcnt_n = i - 1;
                target_config->clkcnt_h = target_config->clkcnt_n / 2 - 1;
                target_config->clkcnt_l = target_config->clkcnt_n;
                break;
            }
    }

    target_config->miso_delay_mode = 0;
    target_config->miso_delay_num = 0;

    target_config->mosi_delay_mode = 0;
    target_config->mosi_delay_num = 0;

    switch (config->clock_mode)
    {
        case SPI_CLOCK_MODE_0:

            target_config->ck_idle_edge = 0;
            target_config->ck_out_edge = 0;
            break;
        case SPI_CLOCK_MODE_1:
            target_config->ck_idle_edge = 0;
            target_config->ck_out_edge = 1;
            break;
        case SPI_CLOCK_MODE_2:
            target_config->ck_idle_edge = 1;
            target_config->ck_out_edge = 1;
            break;
        case SPI_CLOCK_MODE_3:
            target_config->ck_idle_edge = 1;
            target_config->ck_out_edge = 0;
            break;
    }

    switch (config->write_bit_order)
    {
        case SPI_BIT_ORDER_LSB_FIRST:
            target_config->wr_bit_order = 1;
            break;
        case SPI_BIT_ORDER_MSB_FIRST:
            target_config->wr_bit_order = 0;
            break;
    }

    switch (config->read_bit_order)
    {
        case SPI_BIT_ORDER_LSB_FIRST:
            target_config->rd_bit_order = 1;
            break;
        case SPI_BIT_ORDER_MSB_FIRST:
            target_config->rd_bit_order = 0;
            break;
    }

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

Status_t SPI_Master_Transmit(SPI_Device_t device, SPI_Master_CS_t cs_line,
                             const SPI_Master_Transmission_t * transmission, uint8_t * mosi_buffer,
                             uint8_t * miso_buffer)
{
    assert_param(SPI_DEVICE_CHECK(device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(SPI_MASTER_MODE_CHECK(mode));
    assert_param((transmission->mosi_bits <= 8 * 32 && transmission->miso_bits <= 8 * 32) ||
                 (transmission->mosi_bits == 0 && transmission->miso_bits <= 16 * 32)     ||
                 (transmission->mosi_bits <= 16 * 32 && transmission->miso_bits == 0));
    assert_param(transmission->mosi_bits == 0 || mosi_buffer != NULL);
    assert_param(transmission->miso_bits == 0 || miso_buffer != NULL);
    assert_param(transmission->command_bits <= 16);
    assert_param(transmission->address_bits <= 64);
    assert_param(transmission->dummy_bits <= 64);
    assert_param(transmission->cs_post_delay <= 15);
    // due to hardware bug dummy phase does not work while using mosi phase
    assert_param(transmission->mosi_bits == 0 || transmission->dummy_bits == 0);

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;
    SPI_Master_Saved_Config_t * target_config = &(target_handle->devices[cs_line]);

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    // set master mode
    target_device->slave.slave_mode = 0;

    switch (transmission->mode)
    {
        case SPI_MASTER_MODE_FULL_DUPLEX:
            // disable fast read mode
            target_device->ctrl.fastrd_mode = 0;

            // disable half duplex mode
            target_device->user.sio = 0;

            // disable all fast write modes
            target_device->user.fwrite_dio = 0;
            target_device->user.fwrite_dual = 0;
            target_device->user.fwrite_qio = 0;
            target_device->user.fwrite_quad = 0;

            // enable full duplex mode
            target_device->user.doutdin = 1;
            break;
        case SPI_MASTER_MODE_HALF_DUPLEX:
            // disable fast read mode
            target_device->ctrl.fastrd_mode = 0;

            // enable half duplex mode
            target_device->user.sio = 1;

            // disable all fast write modes
            target_device->user.fwrite_dio = 0;
            target_device->user.fwrite_dual = 0;
            target_device->user.fwrite_qio = 0;
            target_device->user.fwrite_quad = 0;

            // disable full duplex mode
            target_device->user.doutdin = 0;
            break;
        case SPI_MASTER_MODE_DIO:
            // enable QIO read mode
            target_device->ctrl.fastrd_mode = 1;
            target_device->ctrl.fread_qio = 0;
            target_device->ctrl.fread_dio = 1;
            target_device->ctrl.fread_quad = 0;
            target_device->ctrl.fread_dual = 0;

            // disable half duplex mode
            target_device->user.sio = 0;

            // enable QIO write mode
            target_device->user.fwrite_dio = 1;
            target_device->user.fwrite_dual = 0;
            target_device->user.fwrite_qio = 0;
            target_device->user.fwrite_quad = 0;

            // disable full duplex mode
            target_device->user.doutdin = 0;
            break;
        case SPI_MASTER_MODE_DOUT:
            // enable QIO read mode
            target_device->ctrl.fastrd_mode = 1;
            target_device->ctrl.fread_qio = 0;
            target_device->ctrl.fread_dio = 0;
            target_device->ctrl.fread_quad = 0;
            target_device->ctrl.fread_dual = 1;

            // disable half duplex mode
            target_device->user.sio = 0;

            // enable QIO write mode
            target_device->user.fwrite_dio = 0;
            target_device->user.fwrite_dual = 1;
            target_device->user.fwrite_qio = 0;
            target_device->user.fwrite_quad = 0;

            // disable full duplex mode
            target_device->user.doutdin = 0;
            break;
        case SPI_MASTER_MODE_QIO:
            // enable QIO read mode
            target_device->ctrl.fastrd_mode = 1;
            target_device->ctrl.fread_qio = 1;
            target_device->ctrl.fread_dio = 0;
            target_device->ctrl.fread_quad = 0;
            target_device->ctrl.fread_dual = 0;

            // disable half duplex mode
            target_device->user.sio = 0;

            // enable QIO write mode
            target_device->user.fwrite_dio = 0;
            target_device->user.fwrite_dual = 0;
            target_device->user.fwrite_qio = 1;
            target_device->user.fwrite_quad = 0;

            // disable full duplex mode
            target_device->user.doutdin = 0;
            break;
        case SPI_MASTER_MODE_QOUT:
            // enable QIO read mode
            target_device->ctrl.fastrd_mode = 1;
            target_device->ctrl.fread_qio = 0;
            target_device->ctrl.fread_dio = 0;
            target_device->ctrl.fread_quad = 1;
            target_device->ctrl.fread_dual = 0;

            // disable half duplex mode
            target_device->user.sio = 0;

            // enable QIO write mode
            target_device->user.fwrite_dio = 0;
            target_device->user.fwrite_dual = 0;
            target_device->user.fwrite_qio = 0;
            target_device->user.fwrite_quad = 1;

            // disable full duplex mode
            target_device->user.doutdin = 0;
            break;
    }

    if (transmission->command_bits > 0)
    {
        if (target_config->wr_bit_order == SPI_BIT_ORDER_MSB_FIRST)
            target_device->user2.usr_command_value = (transmission->command >> 8) | (transmission->command << 8);
        else if (target_config->wr_bit_order == SPI_BIT_ORDER_LSB_FIRST)
            target_device->user2.usr_command_value = transmission->command;

        target_device->user2.usr_command_bitlen = transmission->command_bits - 1;
        target_device->user.usr_command = 1;
    } else
        target_device->user.usr_command = 0;

    if (transmission->address_bits > 0)
    {
        target_device->user.usr_addr = 1;
        target_device->user1.usr_addr_bitlen = transmission->address_bits - 1;

        if (target_config->wr_bit_order == SPI_BIT_ORDER_MSB_FIRST)
        {
            target_device->addr = transmission->address >> 32u;
            target_device->slv_wr_status = transmission->address;
        } else if (target_config->wr_bit_order == SPI_BIT_ORDER_LSB_FIRST)
        {
            union
            {
                uint32_t value;
                uint8_t bytes[4];
            } byte_converter;

            byte_converter.value = transmission->address;
            uint8_t tmp = byte_converter.bytes[0];
            byte_converter.bytes[0] = byte_converter.bytes[3];
            byte_converter.bytes[3] = tmp;

            tmp = byte_converter.bytes[1];
            byte_converter.bytes[1] = byte_converter.bytes[2];
            byte_converter.bytes[2] = tmp;

            target_device->addr = byte_converter.value;

            byte_converter.value = transmission->address >> 32u;
            tmp = byte_converter.bytes[0];
            byte_converter.bytes[0] = byte_converter.bytes[3];
            byte_converter.bytes[3] = tmp;

            tmp = byte_converter.bytes[1];
            byte_converter.bytes[1] = byte_converter.bytes[2];
            byte_converter.bytes[2] = tmp;

            target_device->slv_wr_status = byte_converter.value;
        }
    } else
        target_device->user.usr_addr = 0;

    if (transmission->dummy_bits > 0)
    {
        target_device->user1.usr_dummy_cyclelen = transmission->dummy_bits - 1;
        target_device->user.usr_dummy = 1;
    } else
        target_device->user.usr_dummy = 0;

    if (transmission->miso_bits > 0)
    {
        target_device->user.usr_miso_highpart = 0;
        target_device->miso_dlen.usr_miso_dbitlen = transmission->miso_bits - 1;
        target_device->user.usr_miso = 1;
    } else
        target_device->user.usr_miso = 0;

    if (transmission->mosi_bits > 0)
    {
        if (transmission->mosi_bits > 8 * 32)
            target_device->user.usr_mosi_highpart = 0;  // in this case data cannot be read
        else
            target_device->user.usr_mosi_highpart = 1;

        target_device->mosi_dlen.usr_mosi_dbitlen = transmission->mosi_bits - 1;
        target_device->user.usr_mosi = 1;

    } else
        target_device->user.usr_mosi = 0;

    if (transmission->cs_post_delay > 0)
    {
        target_device->ctrl2.hold_time = transmission->cs_post_delay;
        target_device->user.cs_hold = 1;
    } else
        target_device->user.cs_hold = 0;

    uint8_t fifo_pos = (transmission->mosi_bits > 8 * 32 ? 0 : 8);
    uint8_t buffer_pos = 0;
    uint32_t copied_bits = 0;

    while (copied_bits < transmission->mosi_bits)
    {
        uint32_t tmp = 0;

        for (uint8_t i = 0; i < 4; i++)
        {
            if (copied_bits >= transmission->mosi_bits)
                break;
            tmp |= mosi_buffer[buffer_pos++] << 8 * i;

            copied_bits += 8;
        }
        target_device->data_buf[fifo_pos++] = tmp;
    }

    // disable trans_done interrupt
    target_device->slave.trans_inten = 0;

    // clear trans_done interrupt
    target_device->slave.trans_done = 0;

    // enable trans_done interrupt
    target_device->slave.trans_inten = 1;

    // start user command
    target_device->cmd.usr = 1;

    while (!target_device->slave.trans_done);

    copied_bits = 0;
    fifo_pos = 0;
    buffer_pos = 0;

    while (copied_bits < transmission->miso_bits)
    {
        for (uint8_t i = 0; i < 4; i++)
        {
            if (copied_bits >= transmission->miso_bits)
                break;

            // when miso_bits is not multiple of 8 hardware will clear remaining bits itself
            miso_buffer[buffer_pos++] = (target_device->data_buf[fifo_pos] >> 8 * i) & 0xFF;

            copied_bits += 8;
        }
        fifo_pos++;
    }

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}


/* ------------------------------------------------------------------------------------------------------------------ */

Status_t SPI_Master_Flash_Read_ID(SPI_Device_t device, SPI_Master_CS_t cs_line, uint8_t * manufacturer_id,
                                  uint16_t * device_id)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(manufacturer_id != NULL);
    assert_param(device_id != NULL);

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    target_device->user.usr_miso_highpart = 0;
    target_device->user.usr_mosi_highpart = 1;

    // disable fast read mode
    target_device->ctrl.fastrd_mode = 0;

    // disable half duplex mode
    target_device->user.sio = 0;

    // disable all fast write modes
    target_device->user.fwrite_dio = 0;
    target_device->user.fwrite_dual = 0;
    target_device->user.fwrite_qio = 0;
    target_device->user.fwrite_quad = 0;

    // enable full duplex mode
    target_device->user.doutdin = 1;

    target_device->cmd.flash_rdid = 1;

    while (target_device->cmd.flash_rdid);

    *manufacturer_id = target_device->data_buf[0] & 0xFF;
    *device_id = ((target_device->data_buf[0] & 0xFF0000) >> 16u) | (target_device->data_buf[0] & 0xFF00);

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

// TODO dummy bits?
Status_t SPI_Master_Flash_Read_Status(SPI_Device_t device, SPI_Master_CS_t cs_line, uint8_t * status)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(status != NULL);

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    // disable fast read mode
    target_device->ctrl.fastrd_mode = 0;

    // disable half duplex mode
    target_device->user.sio = 0;

    // disable all fast write modes
    target_device->user.fwrite_dio = 0;
    target_device->user.fwrite_dual = 0;
    target_device->user.fwrite_qio = 0;
    target_device->user.fwrite_quad = 0;

    // enable full duplex mode
    target_device->user.doutdin = 1;

    target_device->cmd.flash_rdsr = 1;

    while (target_device->cmd.flash_rdsr);

    *status = target_device->rd_status.flash_status & 0xFF;

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

Status_t SPI_Master_Flash_Write_Status(SPI_Device_t device, SPI_Master_CS_t cs_line, uint32_t status, uint8_t status_bits)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(status_bits > 0);
    assert_param(status_bits <= 32);

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    // for some reason does not work with this bit cleared so set it
    target_device->user.doutdin = 1;

    target_device->mosi_dlen.usr_mosi_dbitlen = status_bits - 1;

    union
    {
        uint32_t value;
        uint8_t bytes[4];
    } byte_converter;

    byte_converter.value = status;
    uint8_t tmp = byte_converter.bytes[0];
    byte_converter.bytes[0] = byte_converter.bytes[3];
    byte_converter.bytes[3] = tmp;

    tmp = byte_converter.bytes[1];
    byte_converter.bytes[1] = byte_converter.bytes[2];
    byte_converter.bytes[2] = tmp;

    target_device->rd_status.flash_status = byte_converter.value;

    target_device->cmd.flash_wrsr = 1;

    while (target_device->cmd.flash_wrsr);

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

Status_t SPI_Master_Flash_Write_Enable(SPI_Device_t device, SPI_Master_CS_t cs_line)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    target_device->cmd.flash_wren = 1;

    while (target_device->cmd.flash_wren);

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

Status_t SPI_Master_Flash_Write_Disable(SPI_Device_t device, SPI_Master_CS_t cs_line)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));

    SPI_Master_Handle_t * target_handle = &spi_master_handles[device];

    if (!MUTEX_TRY_LOCK(&target_handle->spi_handle->mutex))
        return STATUS_BUSY;

    SPI_HW_t * target_device = target_handle->spi_handle->device;

    // load config for target slave
    SPI_Master_Load_Config(device, cs_line);

    target_device->cmd.flash_wrdi = 1;

    while (target_device->cmd.flash_wrdi);

    MUTEX_UNLOCK(&target_handle->spi_handle->mutex);

    return STATUS_OK;
}

Status_t SPI_Master_Flash_Read(SPI_Device_t device, SPI_Master_CS_t cs_line, SPI_Master_Mode_t mode, uint64_t address,
                               uint8_t address_bits, uint8_t * data, uint16_t data_bits)
{
    assert_param(SPI_DEVICE_CHECK(spi_device));
    assert_param(SPI_MASTER_CS_CHECK(cs_line));
    assert_param(SPI_MASTER_MODE_CHECK(mode));
    assert_param(address_bits > 0);
    assert_param(data != NULL);
    assert_param(data_bits > 0);
    assert_param(data_bits <= 16 * 32);

    // hardware does not work here so use user defined command as workaround

    SPI_Master_Transmission_t transmission;
    SPI_Bit_Order_t write_bit_order = spi_master_handles[device].devices[cs_line].wr_bit_order;

    // TODO make dummy bits configurable?

    switch (mode)
    {
        // both half and full duplex will work the same way here
        case SPI_MASTER_MODE_FULL_DUPLEX:
        case SPI_MASTER_MODE_HALF_DUPLEX:
            transmission.mode = SPI_MASTER_MODE_HALF_DUPLEX;
            transmission.command_bits = 8;
            transmission.command = (write_bit_order == SPI_BIT_ORDER_MSB_FIRST ? 0x0300 : 0x03);
            transmission.address_bits = address_bits;
            transmission.address = address;
            transmission.dummy_bits = 0;
            transmission.mosi_bits = 0;
            transmission.miso_bits = data_bits;

            break;
        case SPI_MASTER_MODE_DIO:
            // TODO M7 - M0 ?

            transmission.mode = SPI_MASTER_MODE_DIO;
            transmission.command_bits = 8;
            transmission.command = (write_bit_order == SPI_BIT_ORDER_MSB_FIRST ? 0xbb00 : 0xbb);
            transmission.address_bits = address_bits;
            transmission.address = address;
            transmission.dummy_bits = 0;
            transmission.mosi_bits = 0;
            transmission.miso_bits = data_bits;

            break;
        case SPI_MASTER_MODE_DOUT:
            transmission.mode = SPI_MASTER_MODE_DOUT;
            transmission.command_bits = 8;
            transmission.command = (write_bit_order == SPI_BIT_ORDER_MSB_FIRST ? 0x3b00 : 0x3b);
            transmission.address_bits = address_bits;
            transmission.address = address;
            transmission.dummy_bits = 8;
            transmission.mosi_bits = 0;
            transmission.miso_bits = data_bits;

            break;
        case SPI_MASTER_MODE_QIO:
            // TODO M7 - M0 ?

            transmission.mode = SPI_MASTER_MODE_QIO;
            transmission.command_bits = 8;
            transmission.command = (write_bit_order == SPI_BIT_ORDER_MSB_FIRST ? 0xeb00 : 0xeb);
            transmission.address_bits = address_bits;
            transmission.address = address;
            transmission.dummy_bits = 0;
            transmission.mosi_bits = 0;
            transmission.miso_bits = data_bits;

            break;
        case SPI_MASTER_MODE_QOUT:
            transmission.mode = SPI_MASTER_MODE_QOUT;
            transmission.command_bits = 8;
            transmission.command = (write_bit_order == SPI_BIT_ORDER_MSB_FIRST ? 0x6b00 : 0x6b);
            transmission.address_bits = address_bits;
            transmission.address = address;
            transmission.dummy_bits = 8;
            transmission.mosi_bits = 0;
            transmission.miso_bits = data_bits;

            break;
    }

    return SPI_Master_Transmit(device, cs_line, &transmission, NULL, data);
}
