#include "ibm.h"
#include "ide.h"
#include "ide_atapi.h"
#include "scsi.h"
#include "timer.h"

#define IDE_TIME (100 * TIMER_USEC)

#define ATAPI_STATE_IDLE 0
#define ATAPI_STATE_COMMAND 1
#define ATAPI_STATE_GOT_COMMAND 2
#define ATAPI_STATE_NEXT_PHASE 3
#define ATAPI_STATE_READ_STATUS 4
#define ATAPI_STATE_READ_MESSAGE 5
#define ATAPI_STATE_END_PHASE 6
#define ATAPI_STATE_READ_DATA 7
#define ATAPI_STATE_READ_DATA_WAIT 8
#define ATAPI_STATE_WRITE_DATA 9
#define ATAPI_STATE_WRITE_DATA_WAIT 10
#define ATAPI_STATE_RETRY_READ_DMA 11
#define ATAPI_STATE_RETRY_WRITE_DMA 12

#define FEATURES_DMA 0x01
#define FEATURES_OVERLAP 0x02

ATAPI *atapi;

void atapi_data_write(atapi_device_t *atapi_dev, uint16_t val) {
        switch (atapi_dev->state) {
        case ATAPI_STATE_COMMAND:
                atapi_dev->command[atapi_dev->command_pos++] = val & 0xff;
                atapi_dev->command[atapi_dev->command_pos++] = val >> 8;
                if (atapi_dev->command_pos >= 12) {
                        atapi_dev->state = ATAPI_STATE_GOT_COMMAND;
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                        *atapi_dev->atastat = BUSY_STAT | (*atapi_dev->atastat & ERR_STAT);
                }
                break;

        case ATAPI_STATE_WRITE_DATA_WAIT:
                atapi_dev->data[atapi_dev->data_write_pos++] = val & 0xff;
                atapi_dev->data[atapi_dev->data_write_pos++] = val >> 8;
                //                pclog("Write ATAPI data %i %i %i\n", atapi_dev->data_write_pos, atapi_dev->data_read_pos,
                //                idecallback[atapi_dev->board]);
                if (atapi_dev->data_write_pos >= atapi_dev->data_read_pos) {
                        atapi_dev->bus_state = 0;
                        atapi_dev->state = ATAPI_STATE_WRITE_DATA;
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                        *atapi_dev->atastat = BUSY_STAT | (*atapi_dev->atastat & ERR_STAT);
                }
                break;
        }
}

uint16_t atapi_data_read(atapi_device_t *atapi_dev) {
        uint16_t temp = 0xffff;

        switch (atapi_dev->state) {
        case ATAPI_STATE_READ_DATA_WAIT:
                if (atapi_dev->data_read_pos >= atapi_dev->data_write_pos) {
                        //                        pclog("ATAPI_STATE_READ_DATA_WAIT - out of data!\n");
                        break;
                }

                temp = atapi_dev->data[atapi_dev->data_read_pos++];
                if (atapi_dev->data_read_pos < atapi_dev->data_write_pos)
                        temp |= (atapi_dev->data[atapi_dev->data_read_pos++] << 8);
                //                pclog("Read data %04x\n", temp);
                if (atapi_dev->data_read_pos >= atapi_dev->data_write_pos) {
                        atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                        *atapi_dev->atastat = BUSY_STAT | (*atapi_dev->atastat & ERR_STAT);
                }
                break;
        }

        return temp;
}

static int wait_for_bus(scsi_bus_t *bus, int state, int req_needed) {
        int c;

        for (c = 0; c < 20; c++) {
                int bus_state = scsi_bus_read(bus);

                if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) == state && (bus_state & BUS_BSY)) {
                        if (!req_needed || (bus_state & BUS_REQ))
                                return 1;
                }
        }

        return 0;
}

void atapi_command_start(atapi_device_t *atapi, uint8_t features) {
        scsi_bus_t *bus = &atapi->bus;

        atapi->use_dma = features & 1;

        scsi_bus_update(bus, BUS_SEL | BUS_SETDATA(1 << 0));
        if (!(scsi_bus_read(bus) & BUS_BSY))
                fatal("STATE_SCSI_SELECT failed to select target\n");

        scsi_bus_update(bus, 0);
        if (!(scsi_bus_read(bus) & BUS_BSY))
                fatal("STATE_SCSI_SELECT failed to select target 2\n");

        /*Device should now be selected*/
        if (!wait_for_bus(bus, BUS_CD, 1))
                fatal("Device failed to request command\n");

        atapi->state = ATAPI_STATE_COMMAND;
        atapi->command_pos = 0;
}

uint8_t atapi_read_iir(atapi_device_t *atapi_dev) {
        uint8_t val = 0;
        int bus_status = atapi_dev->bus_state; // scsi_bus_read(&atapi_dev->bus);

        if (bus_status & BUS_CD)
                val |= 1;
        if (bus_status & BUS_IO)
                val |= 2;

        //        pclog("Read IIR %02X %02x\n", val, bus_status);

        return val;
}

uint8_t atapi_read_drq(atapi_device_t *atapi_dev) { return atapi_dev->bus_state & BUS_REQ; }

void atapi_set_transfer_granularity(atapi_device_t *atapi_dev, int size) {
        if (atapi_dev->max_transfer_len > size)
                atapi_dev->max_transfer_len -= (atapi_dev->max_transfer_len % size);
}

void atapi_process_packet(atapi_device_t *atapi_dev) {
        //        pclog("atapi_process_packet: state=%i\n", atapi_dev->state);
        switch (atapi_dev->state) {
        case ATAPI_STATE_COMMAND:
                *atapi_dev->atastat = READY_STAT | (*atapi_dev->atastat & ERR_STAT);
                atapi_dev->max_transfer_len = (*atapi_dev->cylinder) & ~1;
                if (!atapi_dev->max_transfer_len)
                        atapi_dev->max_transfer_len = 0xfffe;
                atapi_dev->bus_state = BUS_CD | BUS_REQ;
                break;

        case ATAPI_STATE_GOT_COMMAND: {
                int pos = 0;
                int bus_state;
                int c;

                for (c = 0; c < 20; c++) {
                        bus_state = scsi_bus_read(&atapi_dev->bus);

                        if (!(bus_state & BUS_BSY))
                                fatal("SEND_COMMAND - dropped BSY\n");
                        if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != BUS_CD)
                                fatal("SEND_COMMAND - bus phase incorrect\n");
                        if (bus_state & BUS_REQ)
                                break;
                }
                if (c == 20)
                        fatal("SEND_COMMAND timed out\n");

                while (1) {
                        int bus_out;

                        for (c = 0; c < 20; c++) {
                                int bus_state = scsi_bus_read(&atapi_dev->bus);

                                if (!(bus_state & BUS_BSY))
                                        fatal("SEND_COMMAND - dropped BSY\n");
                                if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != BUS_CD)
                                        break;
                                if (bus_state & BUS_REQ)
                                        break;
                        }
                        if (c == 20) {
                                //                                        pclog("SEND_COMMAND timed out %x\n",
                                //                                        scsi_bus_read(&atapi_dev->bus));
                                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                                break;
                        }

                        bus_state = scsi_bus_read(&atapi_dev->bus);

                        if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG | BUS_REQ)) != (BUS_CD | BUS_REQ)) {
                                //                                        pclog("SEND_COMMAND - bus state changed %x\n",
                                //                                        bus_state);
                                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                                break;
                        }

                        if (pos >= atapi_dev->command_pos)
                                bus_out = BUS_SETDATA(0); /*Pad out with zeroes*/
                        else
                                bus_out = BUS_SETDATA(atapi_dev->command[pos++]);
                        scsi_bus_update(&atapi_dev->bus, bus_out | BUS_ACK);
                        scsi_bus_update(&atapi_dev->bus, bus_out & ~BUS_ACK);

                        if (!(bus_state & BUS_BSY))
                                fatal("SEND_COMMAND - dropped BSY\n");

                        //                                if (pos >= atapi_dev->command_pos)
                        //                                        fatal("Out of data!\n");
                }
        } break;

        case ATAPI_STATE_NEXT_PHASE: {
                int c;

                for (c = 0; c < 20; c++) {
                        int bus_state = scsi_bus_read(&atapi_dev->bus);

                        //                                if (!c)
                        //                                        pclog("NEXT_PHASE - %x\n", bus_state);
                        if (!(bus_state & BUS_BSY))
                                fatal("NEXT_PHASE - dropped BSY waiting\n");

                        if (bus_state & BUS_REQ) {
                                scsi_device_t *scsi_dev = atapi_dev->bus.devices[0];
                                void *scsi_data = atapi_dev->bus.device_data[0];

                                switch (bus_state & (BUS_IO | BUS_CD | BUS_MSG)) {
                                case 0:
                                        atapi_dev->data_read_pos = scsi_dev->get_bytes_required(scsi_data);

                                        atapi_dev->data_write_pos = 0;

                                        if (atapi_dev->use_dma) {
                                                if (ide_bus_master_write_data) {
                                                        if (ide_bus_master_write_data(atapi_dev->board, atapi_dev->data,
                                                                                      atapi_dev->data_read_pos,
                                                                                      ide_bus_master_p)) {
                                                                atapi_dev->state = ATAPI_STATE_RETRY_WRITE_DMA;
                                                                timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                                                        } else {
                                                                atapi_dev->data_write_pos = atapi_dev->data_read_pos;
                                                                atapi_dev->bus_state = 0;
                                                                atapi_dev->state = ATAPI_STATE_WRITE_DATA;
                                                                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                                                        }
                                                } else {
                                                        atapi_dev->state = ATAPI_STATE_RETRY_WRITE_DMA;
                                                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                                                }
                                        } else {
                                                if (atapi_dev->data_read_pos > atapi_dev->max_transfer_len)
                                                        atapi_dev->data_read_pos = atapi_dev->max_transfer_len;

                                                atapi_dev->state = ATAPI_STATE_WRITE_DATA_WAIT;

                                                *atapi_dev->cylinder = atapi_dev->data_read_pos;

                                                //                                                        pclog("WRITE_DATA:
                                                //                                                        bytes=%i\n",
                                                //                                                        *atapi_dev->cylinder);
                                                *atapi_dev->atastat = READY_STAT | DRQ_STAT | (*atapi_dev->atastat & ERR_STAT);
                                                atapi_dev->bus_state = BUS_REQ;
                                                ide_irq_raise(atapi_dev->ide);
                                        }
                                        break;

                                case BUS_IO:
                                        atapi_dev->state = ATAPI_STATE_READ_DATA;
                                        atapi_dev->data_read_pos = atapi_dev->data_write_pos = 0;
                                        break;

                                case (BUS_IO | BUS_CD):
                                        atapi_dev->state = ATAPI_STATE_READ_STATUS;
                                        break;

                                case (BUS_CD | BUS_IO | BUS_MSG):
                                        atapi_dev->state = ATAPI_STATE_READ_MESSAGE;
                                        break;

                                default:
                                        fatal("NEXT_PHASE - bus state changed %x\n", bus_state);
                                }
                                break;
                        }
                }
                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
        } break;

        case ATAPI_STATE_READ_STATUS: {
                int c;

                for (c = 0; c < 20; c++) {
                        int bus_state = scsi_bus_read(&atapi_dev->bus);

                        if (!(bus_state & BUS_BSY))
                                fatal("READ_STATUS - dropped BSY waiting\n");

                        if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != (BUS_CD | BUS_IO))
                                fatal("READ_STATUS - changed phase\n");

                        if (bus_state & BUS_REQ) {
                                //                                        uint8_t status = BUS_GETDATA(bus_state);
                                int bus_out = 0;

                                //                                        pclog("Read status %02x\n", status);

                                scsi_bus_update(&atapi_dev->bus, bus_out | BUS_ACK);
                                scsi_bus_update(&atapi_dev->bus, bus_out & ~BUS_ACK);

                                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                break;
                        }
                }
                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
        } break;

        case ATAPI_STATE_READ_MESSAGE: {
                int c;

                for (c = 0; c < 20; c++) {
                        int bus_state = scsi_bus_read(&atapi_dev->bus);

                        if (!(bus_state & BUS_BSY))
                                fatal("READ_MESSAGE - dropped BSY waiting\n");

                        if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != (BUS_CD | BUS_IO | BUS_MSG)) {
                                //                                        pclog("READ_MESSAGE - changed phase\n");
                                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                break;
                        }

                        if (bus_state & BUS_REQ) {
                                uint8_t msg = BUS_GETDATA(bus_state);
                                int bus_out = 0;

                                //                                        pclog("Read message %02x\n", msg);
                                scsi_bus_update(&atapi_dev->bus, bus_out | BUS_ACK);
                                scsi_bus_update(&atapi_dev->bus, bus_out & ~BUS_ACK);

                                switch (msg) {
                                case MSG_COMMAND_COMPLETE:
                                        atapi_dev->state = ATAPI_STATE_END_PHASE;
                                        break;

                                default:
                                        fatal("READ_MESSAGE - unknown message %02x\n", msg);
                                }
                                break;
                        }
                }
                timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
        } break;

        case ATAPI_STATE_READ_DATA: {
                while (atapi_dev->data_write_pos < atapi_dev->max_transfer_len) {
                        int c;

                        for (c = 0; c < 20; c++) {
                                int bus_state = scsi_bus_read(&atapi_dev->bus);

                                if (!(bus_state & BUS_BSY))
                                        fatal("READ_DATA - dropped BSY waiting\n");

                                if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != BUS_IO) {
                                        //                                                pclog("READ_DATA - changed phase\n");
                                        break;
                                }

                                if (bus_state & BUS_REQ) {
                                        uint8_t data = BUS_GETDATA(bus_state);
                                        int bus_out = 0;

                                        atapi_dev->data[atapi_dev->data_write_pos++] = data;

                                        //                                                pclog("Read data %02x %i\n", data,
                                        //                                                atapi_dev->data_write_pos);
                                        scsi_bus_update(&atapi_dev->bus, bus_out | BUS_ACK);
                                        scsi_bus_update(&atapi_dev->bus, bus_out & ~BUS_ACK);
                                        break;
                                }
                        }
                        if ((scsi_bus_read(&atapi_dev->bus) & (BUS_IO | BUS_CD | BUS_MSG)) != BUS_IO)
                                break;
                }
                if (atapi_dev->use_dma) {
                        if (ide_bus_master_read_data) {
                                if (ide_bus_master_read_data(atapi_dev->board, atapi_dev->data, atapi_dev->data_write_pos,
                                                             ide_bus_master_p)) {
                                        atapi_dev->state = ATAPI_STATE_RETRY_READ_DMA;
                                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                                } else {
                                        atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                                }
                        } else {
                                atapi_dev->state = ATAPI_STATE_RETRY_READ_DMA;
                                timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                        }
                } else {
                        atapi_dev->state = ATAPI_STATE_READ_DATA_WAIT;
                        *atapi_dev->atastat = READY_STAT | DRQ_STAT | (*atapi_dev->atastat & ERR_STAT);
                        *atapi_dev->cylinder = atapi_dev->data_write_pos;
                        //                                pclog("READ_DATA: bytes=%i\n", *atapi_dev->cylinder);
                        atapi_dev->bus_state = BUS_IO | BUS_REQ;
                        ide_irq_raise(atapi_dev->ide);
                }
        } break;

        case ATAPI_STATE_WRITE_DATA: {
                atapi_dev->data_read_pos = 0;
                //                        pclog("ATAPI_STATE_WRITE_DATA - %i\n", atapi_dev->data_write_pos);
                while (atapi_dev->data_read_pos < atapi_dev->data_write_pos) {
                        int bus_state = scsi_bus_read(&atapi_dev->bus);

                        if (!(bus_state & BUS_BSY))
                                fatal("WRITE_DATA - dropped BSY waiting\n");

                        if ((bus_state & (BUS_IO | BUS_CD | BUS_MSG)) != 0) {
                                //                                        pclog("WRITE_DATA - changed phase\n");
                                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                                break;
                        }

                        if (bus_state & BUS_REQ) {
                                uint8_t data = atapi_dev->data[atapi_dev->data_read_pos++];
                                int bus_out;

                                //                                        pclog("Write data %02x %i\n", data,
                                //                                        atapi_dev->data_read_pos-1);
                                bus_out = BUS_SETDATA(data);
                                scsi_bus_update(&atapi_dev->bus, bus_out | BUS_ACK);
                                scsi_bus_update(&atapi_dev->bus, bus_out & ~BUS_ACK);
                        }
                }

                atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
        } break;

        case ATAPI_STATE_END_PHASE: {
                int c;
                /*Wait for SCSI command to move to next phase*/
                for (c = 0; c < 20; c++) {
                        int bus_state = scsi_bus_read(&atapi_dev->bus);

                        if (!(bus_state & BUS_BSY)) {
                                //                                        pclog("END_PHASE - dropped BSY waiting\n");
                                atapi_dev->state = ATAPI_STATE_IDLE;
                                break;
                        }

                        if (bus_state & BUS_REQ)
                                fatal("END_PHASE - unexpected REQ\n");
                }
                if (atapi_dev->state == ATAPI_STATE_IDLE) {
                        scsi_device_t *scsi_dev = atapi_dev->bus.devices[0];
                        void *scsi_data = atapi_dev->bus.device_data[0];

                        *atapi_dev->atastat = READY_STAT;
                        atapi_dev->bus_state = BUS_IO | BUS_CD;
                        if (scsi_dev->get_status(scsi_data) != STATUS_GOOD) {
                                *atapi_dev->error = (scsi_dev->get_sense_key(scsi_data) << 4) | ABRT_ERR;
                                if (scsi_dev->get_sense_key(scsi_data) == KEY_UNIT_ATTENTION)
                                        *atapi_dev->error |= MCR_ERR;
                                *atapi_dev->atastat |= ERR_STAT;
                        }
                        ide_irq_raise(atapi_dev->ide);
                } else
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
        } break;

        case ATAPI_STATE_RETRY_READ_DMA: {
                if (ide_bus_master_read_data(atapi_dev->board, atapi_dev->data, atapi_dev->data_write_pos, ide_bus_master_p)) {
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                } else {
                        atapi_dev->state = ATAPI_STATE_NEXT_PHASE;
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                }
        } break;

        case ATAPI_STATE_RETRY_WRITE_DMA: {
                if (ide_bus_master_write_data(atapi_dev->board, atapi_dev->data, atapi_dev->data_read_pos, ide_bus_master_p)) {
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 1 * IDE_TIME);
                } else {
                        atapi_dev->bus_state = 0;
                        atapi_dev->state = ATAPI_STATE_WRITE_DATA;
                        timer_set_delay_u64(&ide_timer[atapi_dev->board], 6 * IDE_TIME);
                }
        } break;
        }
}

void atapi_reset(atapi_device_t *atapi_dev) {
        pclog("atapi_reset\n");
        atapi_dev->state = ATAPI_STATE_IDLE;
        atapi_dev->command_pos = 0;
        atapi_dev->data_read_pos = 0;
        atapi_dev->data_write_pos = 0;
        scsi_bus_reset(&atapi_dev->bus);
}
