static void fdctrl_write_data(FDCtrl *fdctrl, uint32_t value)

{

    FDrive *cur_drv;

    int pos;



    /* Reset mode */

    if (!(fdctrl->dor & FD_DOR_nRESET)) {

        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");

        return;

    }

    if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {

        FLOPPY_DPRINTF("error: controller not ready for writing\n");

        return;

    }

    fdctrl->dsr &= ~FD_DSR_PWRDOWN;

    /* Is it write command time ? */

    if (fdctrl->msr & FD_MSR_NONDMA) {

        /* FIFO data write */

        pos = fdctrl->data_pos++;

        pos %= FD_SECTOR_LEN;

        fdctrl->fifo[pos] = value;

        if (pos == FD_SECTOR_LEN - 1 ||

            fdctrl->data_pos == fdctrl->data_len) {

            cur_drv = get_cur_drv(fdctrl);

            if (blk_write(cur_drv->blk, fd_sector(cur_drv), fdctrl->fifo, 1)

                < 0) {

                FLOPPY_DPRINTF("error writing sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {

                FLOPPY_DPRINTF("error seeking to next sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

        }

        /* Switch from transfer mode to status mode

         * then from status mode to command mode

         */

        if (fdctrl->data_pos == fdctrl->data_len)

            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);

        return;

    }

    if (fdctrl->data_pos == 0) {

        /* Command */

        pos = command_to_handler[value & 0xff];

        FLOPPY_DPRINTF("%s command\n", handlers[pos].name);

        fdctrl->data_len = handlers[pos].parameters + 1;

        fdctrl->msr |= FD_MSR_CMDBUSY;

    }



    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);

    fdctrl->fifo[fdctrl->data_pos++] = value;

    if (fdctrl->data_pos == fdctrl->data_len) {

        /* We now have all parameters

         * and will be able to treat the command

         */

        if (fdctrl->data_state & FD_STATE_FORMAT) {

            fdctrl_format_sector(fdctrl);

            return;

        }



        pos = command_to_handler[fdctrl->fifo[0] & 0xff];

        FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);

        (*handlers[pos].handler)(fdctrl, handlers[pos].direction);

    }

}

static void fdctrl_write_data(FDCtrl *fdctrl, uint32_t value)

{

    FDrive *cur_drv;

    int pos;



    

    if (!(fdctrl->dor & FD_DOR_nRESET)) {

        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");

        return;

    }

    if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {

        FLOPPY_DPRINTF("error: controller not ready for writing\n");

        return;

    }

    fdctrl->dsr &= ~FD_DSR_PWRDOWN;

    

    if (fdctrl->msr & FD_MSR_NONDMA) {

        

        pos = fdctrl->data_pos++;

        pos %= FD_SECTOR_LEN;

        fdctrl->fifo[pos] = value;

        if (pos == FD_SECTOR_LEN - 1 ||

            fdctrl->data_pos == fdctrl->data_len) {

            cur_drv = get_cur_drv(fdctrl);

            if (blk_write(cur_drv->blk, fd_sector(cur_drv), fdctrl->fifo, 1)

                < 0) {

                FLOPPY_DPRINTF("error writing sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {

                FLOPPY_DPRINTF("error seeking to next sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

        }

        





        if (fdctrl->data_pos == fdctrl->data_len)

            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);

        return;

    }

    if (fdctrl->data_pos == 0) {

        

        pos = command_to_handler[value & 0xff];

        FLOPPY_DPRINTF("%s command\n", handlers[pos].name);

        fdctrl->data_len = handlers[pos].parameters + 1;

        fdctrl->msr |= FD_MSR_CMDBUSY;

    }



    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);

    fdctrl->fifo[fdctrl->data_pos++] = value;

    if (fdctrl->data_pos == fdctrl->data_len) {

        





        if (fdctrl->data_state & FD_STATE_FORMAT) {

            fdctrl_format_sector(fdctrl);

            return;

        }



        pos = command_to_handler[fdctrl->fifo[0] & 0xff];

        FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);

        (*handlers[pos].handler)(fdctrl, handlers[pos].direction);

    }

}

    int pos;         pos %= FD_SECTOR_LEN;     int pos;     fdctrl->fifo[fdctrl->data_pos++] = value;
static void FUNC_0(FDCtrl *VAR_0, uint32_t VAR_1)

{

    FDrive *cur_drv;

    int VAR_2;



    

    if (!(VAR_0->dor & FD_DOR_nRESET)) {

        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");

        return;

    }

    if (!(VAR_0->msr & FD_MSR_RQM) || (VAR_0->msr & FD_MSR_DIO)) {

        FLOPPY_DPRINTF("error: controller not ready for writing\n");

        return;

    }

    VAR_0->dsr &= ~FD_DSR_PWRDOWN;

    

    if (VAR_0->msr & FD_MSR_NONDMA) {

        

        VAR_2 = VAR_0->data_pos++;

        VAR_2 %= FD_SECTOR_LEN;

        VAR_0->fifo[VAR_2] = VAR_1;

        if (VAR_2 == FD_SECTOR_LEN - 1 ||

            VAR_0->data_pos == VAR_0->data_len) {

            cur_drv = get_cur_drv(VAR_0);

            if (blk_write(cur_drv->blk, fd_sector(cur_drv), VAR_0->fifo, 1)

                < 0) {

                FLOPPY_DPRINTF("error writing sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

            if (!fdctrl_seek_to_next_sect(VAR_0, cur_drv)) {

                FLOPPY_DPRINTF("error seeking to next sector %d\n",

                               fd_sector(cur_drv));

                return;

            }

        }

        





        if (VAR_0->data_pos == VAR_0->data_len)

            fdctrl_stop_transfer(VAR_0, 0x00, 0x00, 0x00);

        return;

    }

    if (VAR_0->data_pos == 0) {

        

        VAR_2 = command_to_handler[VAR_1 & 0xff];

        FLOPPY_DPRINTF("%s command\n", handlers[VAR_2].name);

        VAR_0->data_len = handlers[VAR_2].parameters + 1;

        VAR_0->msr |= FD_MSR_CMDBUSY;

    }



    FLOPPY_DPRINTF("%s: %02x\n", __func__, VAR_1);

    VAR_0->fifo[VAR_0->data_pos++] = VAR_1;

    if (VAR_0->data_pos == VAR_0->data_len) {

        





        if (VAR_0->data_state & FD_STATE_FORMAT) {

            fdctrl_format_sector(VAR_0);

            return;

        }



        VAR_2 = command_to_handler[VAR_0->fifo[0] & 0xff];

        FLOPPY_DPRINTF("treat %s command\n", handlers[VAR_2].name);

        (*handlers[VAR_2].handler)(VAR_0, handlers[VAR_2].direction);

    }

}

static void FUNC_0(FDCtrl *VAR_0, uint32_t VAR_1)
{ FDrive *cur_drv; int VAR_2; if (!(VAR_0->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); return; } if (!(VAR_0->msr & FD_MSR_RQM) || (VAR_0->msr & FD_MSR_DIO)) { FLOPPY_DPRINTF("error: controller not ready for writing\n"); return; } VAR_0->dsr &= ~FD_DSR_PWRDOWN; if (VAR_0->msr & FD_MSR_NONDMA) { VAR_2 = VAR_0->data_pos++; VAR_2 %= FD_SECTOR_LEN; VAR_0->fifo[VAR_2] = VAR_1; if (VAR_2 == FD_SECTOR_LEN - 1 ||
VAR_0->data_pos == VAR_0->data_len) { cur_drv = get_cur_drv(VAR_0); if (blk_write(cur_drv->blk, fd_sector(cur_drv), VAR_0->fifo, 1)
< 0) { FLOPPY_DPRINTF("error writing sector %d\n",
fd_sector(cur_drv)); return; } if (!fdctrl_seek_to_next_sect(VAR_0, cur_drv)) { FLOPPY_DPRINTF("error seeking to next sector %d\n",
fd_sector(cur_drv)); return; } } if (VAR_0->data_pos == VAR_0->data_len)
fdctrl_stop_transfer(VAR_0, 0x00, 0x00, 0x00); return; } if (VAR_0->data_pos == 0) { VAR_2 = command_to_handler[VAR_1 & 0xff]; FLOPPY_DPRINTF("%s command\n", handlers[VAR_2].name); VAR_0->data_len = handlers[VAR_2].parameters + 1; VAR_0->msr |= FD_MSR_CMDBUSY; } FLOPPY_DPRINTF("%s: %02x\n", __func__, VAR_1); VAR_0->fifo[VAR_0->data_pos++] = VAR_1; if (VAR_0->data_pos == VAR_0->data_len) { if (VAR_0->data_state & FD_STATE_FORMAT) { fdctrl_format_sector(VAR_0); return; } VAR_2 = command_to_handler[VAR_0->fifo[0] & 0xff]; FLOPPY_DPRINTF("treat %s command\n", handlers[VAR_2].name); (*handlers[VAR_2].handler)(VAR_0, handlers[VAR_2].direction); } }