/******************************************************************************/
/*
 *
 * @file fip_urv.c
 *
 * Hydra software
 *
 ******************************************************************************/

#include <stdint.h>
#include "common.h"

#define HAVE_MONIMOD    1   /* System board monimod */
#define HAVE_FANTRAY    0
#define HAVE_RATOPUS    0

/* I2C open-core registers */
#define OC_I2C_PRES_LO  0
#define OC_I2C_PRES_HI  1
#define OC_I2C_CTRL     2
#define OC_I2C_RXTX     3
#define OC_I2C_CR       4
#define OC_I2C_CFG      5

/* Produced variable length (the reply) */
#define VAR_LENGTH      16

#define CONS_VAR_LENGTH 124

/* Magic number for applications.  */
#define APP_MAGIC 0xb1507d38

/* System Board Monimod I2C Address */
#define MONIMOD_I2C_ADDR    (0x19 << 1)
#define MONIMOD_I2C_BUS_ID  0
#define MONIMOD_I2C_BYTES   16

/* Ratopus AD7291 I2C Address */
#define RAD7291_I2C_ADDR    (0x2B << 1)
#define RAD7291_I2C_BUS_ID  1
#define RAD7291_I2C_BYTES   10

/* Fantray Monimod I2C Address */
#define FANTRAY_I2C_ADDR    (0x12 << 1)
#define FANTRAY_I2C_BUS_ID  2
#define FANTRAY_I2C_BYTES   14

/* System board buffer size */
#define SB_TOTAL_BYTES  (MONIMOD_I2C_BYTES * HAVE_MONIMOD \
                         + RAD7291_I2C_BYTES * HAVE_RATOPUS \
                         + FANTRAY_I2C_BYTES * HAVE_FANTRAY)

/* Align system buffer size */
#define SB_BUFFER_BYTES     ((SB_TOTAL_BYTES + 3) & ~3)

#if HAVE_RATOPUS
/* i2c commands to initialize ratopus */
static const unsigned char initialize_ratopus_cmd[] = {
    I2C_CMD_START, RAD7291_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (3 << 4), 0x00, 0x0F, 0xA0, /* Init reg 0x0 */

    I2C_CMD_IDLE
};
#endif

/* i2c commands that are executed to get values from monimod */
static const unsigned char i2c_loop_cmds[] = {
#if HAVE_MONIMOD
    /*****************     SYSTEM BOARD MONIMOD SEQUENCE     ******************/
    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x00,  /* page 0 */

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8c, /* iout */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x01,  /* page 1 */

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x02,  /* page 2 */

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x03,  /* page 3 */

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8d, /* temp-1 */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8e, /* temp-2 */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, MONIMOD_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8f, /* temp-3 */
    I2C_CMD_START, MONIMOD_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),
#endif

#if HAVE_FANTRAY
    /*********************     FANTRAY MONIMOD SEQUENCE     *******************/
    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x00,  /* page 0 */

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8c, /* iout */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE_STOP | (2 << 4), 0x00, 0x01,  /* page 1 */

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8b, /* vout */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x8d, /* temp-1 */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x90, /* Fan Speed 1 */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x91, /* Fan Speed 2 */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    I2C_CMD_START, FANTRAY_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x92, /* Fan Speed 3 */
    I2C_CMD_START, FANTRAY_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),

    /***********************     RATOPUS SEQUENCE     ************************/
    I2C_CMD_START, RAD7291_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x01, /* Voltage Conversion Ratopus */
    I2C_CMD_START, RAD7291_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (8 << 4),   /* Read 8 bytes */

    I2C_CMD_START, RAD7291_I2C_ADDR,
    I2C_CMD_WRITE | (1 << 4), 0x02, /* Temperature */
    I2C_CMD_START, RAD7291_I2C_ADDR | 1,
    I2C_CMD_READ_STOP | (2 << 4),   /* Read 2 bytes */
#endif

    I2C_CMD_IDLE

};

/**************************** Type Definitions ********************************/
struct i2c_cmd {
    unsigned char state;
    unsigned char len;
    unsigned char coff;
    unsigned char boff;
    unsigned char buf[SB_BUFFER_BYTES];
    const unsigned char *cmds;
} __attribute__((aligned(4)));

/************************** Variable Definitions ******************************/
static unsigned char system_buf[SB_BUFFER_BYTES + 2];
static unsigned char system_cycle;

/* Use of i2c for monimod scan.
   0: never (allowed for user)
   1: scan (forbidden to user)
   2: scanning.
   3: in reset */
static unsigned char i2c_bus_status;

/* Define Array with Stadistics */
static unsigned stat_regs[STAT_NBR];

/******************************* Functions ************************************/
/* Save regs to memory to detect any divergence */
extern void refresh_regs(void);

static unsigned extract_word (volatile uint32_t *buf)
{
    unsigned res;

    /* Little endian */
    res = (buf[0] & 0xff) << 0;
    res |= (buf[1] & 0xff) << 8;
    res |= (buf[2] & 0xff) << 16;
    res |= (buf[3] & 0xff) << 24;

    return res;
}

static void insert_word(unsigned char *buf, unsigned val)
{
    buf[0] = (val >> 0) & 0xff;
    buf[1] = (val >> 8) & 0xff;
    buf[2] = (val >> 16) & 0xff;
    buf[3] = (val >> 24) & 0xff;
}

static inline void do_i2c_write (struct i2c_cmd *i2c)
{
    /* Set the stop bit only on the last byte and if requested */
    unsigned char stop =
        i2c->len == 1 && i2c->state == I2C_CMD_WRITE_STOP;
    REGS->i2c[OC_I2C_RXTX] = i2c->cmds[i2c->coff];
    REGS->i2c[OC_I2C_CR] = 0x10 | (stop ? 0x40 : 0);  /* WRITE */
    i2c->coff++;
}

static inline void do_i2c_read (struct i2c_cmd *i2c)
{
    /* Set the stop bit only on the last byte and if requested */
    unsigned char stop = i2c->len == 1 && i2c->state == I2C_CMD_READ_STOP;
    REGS->i2c[OC_I2C_CR] = 0x20 | (stop ? 0x48 : 0);  /* READ */
}

static void do_i2c_done (struct i2c_cmd *i2c, unsigned char stat)
{
    /* According to the current command (state) check if it has been
       executed, and the status */
    if (i2c->state == I2C_CMD_START) {
        if (stat == 0x41) {
            /* Busy + Interrupt, ok */
            i2c->state = I2C_CMD_RUN;
            return;
        }
    } else if (i2c->state == I2C_CMD_WRITE
             || i2c->state == I2C_CMD_WRITE_STOP) {
        if (stat == 0x41 || (stat == 0x01 && i2c->len == 1)) {
            /* Next byte */
            i2c->len--;
            if (i2c->len == 0) {
                i2c->state = I2C_CMD_RUN;
            } else {
                do_i2c_write(i2c);
            }
            return;
        }
    } else if (i2c->state == I2C_CMD_READ
             || i2c->state == I2C_CMD_READ_STOP) {
        /* We are the master.  No need to check STAT, as the ACK/BUSY
           bits are in fact set by us */
        /* Read the byte received */
        i2c->buf[i2c->boff] = REGS->i2c[OC_I2C_RXTX];
        i2c->boff++;
        i2c->len--;
        if (i2c->len == 0) {
            /* All the bytes have been read, next i2c command */
            i2c->state = I2C_CMD_RUN;
        } else {
            /* Prepare to read next byte */
            do_i2c_read (i2c);
        }
        return;
    } else if (i2c->state == I2C_CMD_READLEN) {
        if (stat == 0x41) {
            i2c->len = REGS->i2c[OC_I2C_RXTX];
            i2c->buf[i2c->boff] = i2c->len;
            i2c->boff++;
            i2c->state = I2C_CMD_RUN;
            return;
        }
    }

    i2c->state = I2C_CMD_ERR;
    REGS->i2c[OC_I2C_CR] = 0x40; /* Stop */

    if (i2c_bus_status == 2) {
        /* This is an error during monimod scan, shutdown monimod */
        if (REGS->i2c[OC_I2C_CFG] == MONIMOD_I2C_BUS_ID) {
            REGS->monimod_shdn = 1;
            stat_regs[STAT_MONIMOD_RST]++;
        } else if (REGS->i2c[OC_I2C_CFG] == RAD7291_I2C_BUS_ID) {
            stat_regs[STAT_RATOPUS_ERR]++;
        } else if (REGS->i2c[OC_I2C_CFG] == FANTRAY_I2C_BUS_ID) {
            REGS->fan_ctrl = REGS->fan_ctrl | FIP_URV_REGS_FAN_CTRL_RSTN;
            stat_regs[STAT_FANTRAY_RST]++;
        }
        i2c_bus_status = 3;
    }
}

static void do_i2c_run (struct i2c_cmd *i2c)
{
    /* Read the command to be executed */
    unsigned char cmd = i2c->cmds[i2c->coff];

    /* Default state: the command */
    i2c->state = cmd & 0x0f;

    if (cmd & 0xf0) {
        i2c->len = cmd >> 4;
    }

    if ((cmd & 0x0f) == I2C_CMD_IDLE) {
        /* Last command */
        if (i2c_bus_status == 2) {
            /* Was for monitor scanning */
            unsigned i;

            system_buf[0] = sizeof(system_buf);
            system_buf[1] = (HAVE_MONIMOD << I2C_SLV_MON)
                    | (HAVE_FANTRAY << I2C_SLV_FT)
                    | (HAVE_RATOPUS << I2C_SLV_RTP);

            /* Copy result, so that a consistent view can be extracted */
            for (i = 0; i < sizeof(i2c->buf); i++) {
                system_buf[2 + i] = i2c->buf[i];
            }
            /* Leave that mode */
            i2c_bus_status = 1;
            /* Change cycle */
            system_cycle++;
        }
    }
    else if (cmd == I2C_CMD_START) {
        if (i2c_bus_status != 0) {
            if (i2c->cmds[i2c->coff + 1] == MONIMOD_I2C_ADDR) {
                if (REGS->i2c[OC_I2C_CFG] != MONIMOD_I2C_BUS_ID) {
                    REGS->i2c[OC_I2C_CFG] = MONIMOD_I2C_BUS_ID;
                }
            }
            else if (i2c->cmds[i2c->coff + 1] == RAD7291_I2C_ADDR) {
                if (REGS->i2c[OC_I2C_CFG] != RAD7291_I2C_BUS_ID) {
                    REGS->i2c[OC_I2C_CFG] = RAD7291_I2C_BUS_ID;
                }
            }
            else if (i2c->cmds[i2c->coff + 1] == FANTRAY_I2C_ADDR) {
                if (REGS->i2c[OC_I2C_CFG] != FANTRAY_I2C_BUS_ID) {
                    REGS->i2c[OC_I2C_CFG] = FANTRAY_I2C_BUS_ID;
                }
            }
        }

        REGS->i2c[OC_I2C_RXTX] = i2c->cmds[i2c->coff + 1];
        REGS->i2c[OC_I2C_CR] = 0x90;  /* START + WRITE */
        i2c->coff += 2;
    } else if ((cmd & 0x0f) == I2C_CMD_WRITE
             || (cmd & 0x0f) == I2C_CMD_WRITE_STOP) {
        i2c->coff++;
        do_i2c_write(i2c);
    } else if ((cmd & 0x0f) == I2C_CMD_READ
             || (cmd & 0x0f) == I2C_CMD_READ_STOP
             || (cmd & 0x0f) == I2C_CMD_READLEN) {
        i2c->coff++;
        do_i2c_read(i2c);
    } else {
        /* Bad command */
        i2c->state = I2C_CMD_ERR;
    }
}

static void
copy_fip_to_app(unsigned cmd)
{
    unsigned lmb = REGS->app[0].mailboxes.mboxout; /* written by mgmt */
    unsigned rmb = REGS->app[0].mailboxes.mboxin;  /* written by app */
    unsigned wslot = (lmb >> 8) & 0xff;
    unsigned rslot = rmb & 0xff;
    unsigned nslot = (wslot + 1) & 3;
    volatile uint32_t *dest;
    volatile uint32_t *src;
    unsigned i;
    unsigned d;

    if (nslot == rslot) {
        /* No free slot, discard. */
        stat_regs[STAT_DATA_WDIS]++;
        return;
    }

    /* Pack and copy.  */
    dest = &REGS->sh_mem[wslot << 5];
    d = cmd & 0x0f;
    src = &REGS->fip_reg[0x2];

    for (i=0; i < CONS_VAR_LENGTH; i+=4) {
        if(i > 0)
        {
            src += 4;
            d = src[0];
        }

        d |= src[1] << 8;
        d |= src[2] << 16;
        d |= src[3] << 24;
        *dest++ = d;
    }

    /* Update slot.  */
    REGS->app[0].mailboxes.mboxout = (lmb + (1 << 8)) & 0xffff03ff;
    stat_regs[STAT_DATA_WR]++;
}

static int
copy_app_to_fip(void)
{
    unsigned rmb = REGS->app[0].mailboxes.mboxin;  /* written by app */
    unsigned lmb = REGS->app[0].mailboxes.mboxout; /* written by mgmt */
    unsigned wslot = (rmb >> 8) & 0xff;
    unsigned rslot = lmb & 0xff;
    volatile uint32_t *src;
    unsigned i;
    unsigned d;

    /* Maybe nothing to write. */
    if (rslot == wslot)
        return 0;

    src = &REGS->sh_mem[128 + (rslot << 5)];

    d = *src++;
    d &= 0xffffff0f;
    d |= CMD_APP_FLAG;

    /* Produce variable */
    REGS->fip_var3 = 1;

    /* Unpack and copy */
    i = 0;
    while (1) {
        REGS->fip_reg[0x102 + i] = d;
        i++;
        if (i == VAR_LENGTH)
            break;
        if ((i & 3) == 0)
            d = *src++;
        else
            d >>= 8;
    }

    REGS->fip_var3 = 0;

    /* Update slot.  */
    REGS->app[0].mailboxes.mboxout = (lmb + 1) & 0xffffff03;

    return 1;
}

static void
load_app_cpu(void)
{
    uint32_t *envm = (uint32_t *)REGS->envm;
    unsigned off;
    unsigned i;
    unsigned len;

    /* Try to find the magic number.  */
    off = 0x4000 >> 2;
    if (envm[off + 1] != APP_MAGIC) {
        off = 0x6000 >> 2;
        if (envm[off + 1] != APP_MAGIC) {
            return;
        }
    }

    /* Start address.  */
    stat_regs[STAT_APP0_START] = off << 2;

    /* Copy.
       TODO: protection against errors ? */
    len = REGS->config_ram & FIP_URV_REGS_CONFIG_RAM_APP0_IRAM_MASK;
    len = 1 << (len + 8 - 2);
    for (i = 0; i < len; i++) {
        REGS->app0_mem[i] = envm[off + i];
        SUPERVISOR->wd_key = WD_KEY;
    }

    /* Start */
    REGS->app[0].cr = 0;
}

/********************************** MAIN **************************************/
int main (void)
{
    unsigned timeout_led;
    unsigned char reply[VAR_LENGTH];
    unsigned char rdy;
    unsigned char leds;
    unsigned char rlen;
#if HAVE_RATOPUS
    unsigned char ratopus_init_state;
#endif

    static struct i2c_cmd i2c;
    static unsigned timeout_mon;
    static unsigned char user_i2c_cmds[16];

#if VAR_LENGTH == 8
  /* That's the default */
#elif VAR_LENGTH == 16
  REGS->fip_var = 2;
  SUPERVISOR->wd_period = 5000;
#elif VAR_LENGTH == 64
  REGS->fip_var = 4;
  SUPERVISOR->wd_period = 5000;
#elif VAR_LENGTH == 124
  REGS->fip_var = 5;
  SUPERVISOR->wd_period = 8500;
#else
# error "Incorrect VAR_LENGTH"
#endif

    timeout_led = 0;
    rdy = 0;

    /* Var1 has been consumed so it's released in case of timeout */
    REGS->fip_var1 = 0;

    /* Clear recovery bit */
    SUPERVISOR->cpu = 0;

    if (SUPERVISOR->reset_cause == 0) {
        /* POWER UP */
        leds = 1;
        i2c.state = I2C_CMD_IDLE;
        i2c.coff = 0;
        i2c.boff = 0;
        i2c.len = 0;
        i2c.cmds = user_i2c_cmds;

        timeout_mon = 0;
        system_cycle = 0;
        i2c_bus_status = 1;
        stat_regs[STAT_MIN_WD_CNT] = ~0;

        /* Power-on monimod */
        REGS->monimod_shdn = 0;

        /* Power on fantray */
        REGS->fan_ctrl &= ~FIP_URV_REGS_FAN_CTRL_RSTN;

        /* Enable scrubber */
        SUPERVISOR->scrub_cfg = HYDRA_SUPERVISOR_REGS_SCRUB_CFG_IRAM_EN
                            | HYDRA_SUPERVISOR_REGS_SCRUB_CFG_DRAM_EN;

        /* Init i2c: prescaler (400khz) */
        REGS->i2c[OC_I2C_PRES_LO] = 25;
        REGS->i2c[OC_I2C_PRES_HI] = 0;
        REGS->i2c[OC_I2C_CTRL] = 0x80;  /* Enable */

        i2c.cmds = I2C_CMD_IDLE;
        i2c.coff = 0;
        i2c.boff = 0;
        i2c.state = I2C_CMD_IDLE;

        /* Build date stat */
        stat_regs[STAT_BUILD_DATE] = C_BUILD_DATE;
        stat_regs[STAT_GIT_HASH] = C_GIT_HASH;

        /* Load application from eNVM. */
        load_app_cpu();

#if HAVE_RATOPUS
        /* Flag ratopus, system starting */
        ratopus_init_state = 0;
#endif
    } else {
        leds = 3;
        stat_regs[STAT_RESET]++;
    }

    /*************************     Infinite loop     **************************/
    while (1) {
        unsigned cpu;

        /* Check for FIP message */
        unsigned int fip_status;

        /* Get minimum watchdog count */
        unsigned count = SUPERVISOR->wd_count;
        if (count < stat_regs[STAT_MIN_WD_CNT]) {
            stat_regs[STAT_MIN_WD_CNT] = count;
        }

        SUPERVISOR->wd_key = WD_KEY;

        refresh_regs();

        stat_regs[STAT_CYCLE_CNT]++;

        /* Force recovery in case of divergence */
        cpu = SUPERVISOR->cpu;
        if (cpu != 0) {
            stat_regs[STAT_RECOVERY]++;
            stat_regs[STAT_LAST_RST] = cpu;
            asm volatile ("");
            SUPERVISOR->cpu = HYDRA_SUPERVISOR_REGS_CPU_RECOVERY;
        }

        fip_status = REGS->fip_status;

        /* Control leds */
        if (leds & 0x80) {
            REGS->leds = leds;
        } else {
            /* Leds 3 & 2 are for liveness */
            REGS->leds = leds;
            timeout_led++;
            if (timeout_led == 100000000){ //KB: it was 100000, change because we want led to stay longer ON
                leds = (leds & 1) ^ 1;
                timeout_led = 0;
            }
        }

        if (fip_status & 1) {
            /* Variable var1 received (from FIP) */
            unsigned char cmd;
            unsigned char id;

            /* Consume variable */
            REGS->fip_var1 = 1;
            cmd = REGS->fip_reg[0x02];

            if (cmd & CMD_APP_FLAG) {
                /* Just data.  */
                copy_fip_to_app (cmd);
            }
            else {
                id = REGS->fip_reg[0x03];

                /* Default reply: command + id */
                reply[0] = cmd;
                reply[1] = id;
                reply[3] = REGS->fip_reg[0x02 + CONS_VAR_LENGTH]; /* MPS */
                /* MPS: bit 0: refreshment, bit 2: significance */

                /* A reply is ready. */
                rdy = 1;

                /* Default reply length */
                rlen = 8;

                stat_regs[STAT_FIP_CMD]++;

                /* No switch (smaller)  */
                if (cmd == CMD_SET_LEDS) {
                    leds = REGS->fip_reg[0x06];
                }
                else if (cmd == CMD_READ_WORD) {
                    volatile unsigned *addr;
                    addr = (unsigned *)extract_word(&REGS->fip_reg[0x6]);
                    insert_word(&reply[4], *addr);
                }
                else if (cmd == CMD_WRITE_WORD) {
                    volatile unsigned *addr;
                    unsigned val;
                    unsigned k;
                    unsigned l;

                    stat_regs[STAT_FIP_WRITES]++;

                    l = REGS->fip_reg[0x02 + 2];
                    addr = (unsigned *)extract_word(&REGS->fip_reg[0x02 + 4]);

                    stat_regs[STAT_WRITE_ADDR] = (unsigned)addr;

                    for (k = 4; k < l; k += 4) {
                        val = extract_word(&REGS->fip_reg[0x02 + 0x04 + k]);
                        addr[(k - 4) >> 2] = val;
                    }
                }
                else if (cmd == CMD_WRITE_ECC) {
                    volatile unsigned *addr;
                    unsigned val;
                    unsigned ecc;

                    stat_regs[STAT_FIP_WRITES]++;
                    addr = (unsigned *)extract_word(&REGS->fip_reg[0x6]);
                    stat_regs[STAT_WRITE_ADDR] = (unsigned)addr | 1;

                    val = extract_word(&REGS->fip_reg[0x6 + 4]);
                    ecc = REGS->fip_reg[0x6 + 8];
                    SUPERVISOR->dram_ecc_mask = ecc;
                    *addr = val;
                    SUPERVISOR->dram_ecc_mask = 0;
                }
                else if (cmd == CMD_I2C_MON) {
                    unsigned val = REGS->fip_reg[0x06];
                    if ((i2c_bus_status == 0 && val == 1)
                        || (i2c_bus_status == 1 && val == 0)) {
                        /* Can only disable scanning, when idle. */
                        i2c_bus_status = val;
                    }
                    insert_word(&reply[4], i2c_bus_status);
                }
                else if (cmd == CMD_I2C_TFR) {
                    if (i2c_bus_status != 0
                        || (i2c.state & 0x0f) != I2C_CMD_IDLE) {
                        /* Busy */
                        reply[4] = 1;
                    } else {
                        /* Extract i2c commands from fip */
                        unsigned k;
                        unsigned clen = REGS->fip_reg[0x6];
                        for (k = 0; k < clen; k++) {
                            user_i2c_cmds[k] = REGS->fip_reg[0x7 + k];
                        }
                        user_i2c_cmds[clen] = I2C_CMD_IDLE;
                        i2c.coff = 0;
                        i2c.boff = 0;
                        i2c.cmds = user_i2c_cmds;
                        /* Has to execute i2c commands */
                        i2c.state = I2C_CMD_RUN;
                        /* fip reply */
                        reply[4] = 0;
                    }
                }
                else if (cmd == CMD_I2C_STAT) {
                    reply[4] = i2c.state;
                    reply[5] = i2c.len;
                    reply[6] = i2c.coff;
                    reply[7] = i2c.boff;
                }
                else if (cmd == CMD_I2C_BUF) {
                    unsigned off = REGS->fip_reg[0x6];
                    reply[4] = i2c.buf[off + 0];
                    reply[5] = i2c.buf[off + 1];
                    reply[6] = i2c.buf[off + 2];
                    reply[7] = i2c.buf[off + 3];
                }
                else if (cmd == CMD_MON_BUF) {
                    unsigned off = REGS->fip_reg[0x6];
                    off <<= 1;
                    reply[4] = system_buf[off + 0];
                    reply[5] = system_buf[off + 1];
                    reply[6] = 0;
                    reply[7] = system_cycle;
                }
                else if (cmd == CMD_STAT_BUF) {
                    unsigned off = REGS->fip_reg[0x6];
                    insert_word(&reply[4], stat_regs[off]);
                }
                else if (cmd == CMD_CPU_FLT) {
                    unsigned cpu = REGS->fip_reg[0x6];
                    if (SUPERVISOR->force_divergence != cpu) {
                        reply[4] = 1;
                    }
                }
                else if (cmd == CMD_FIP_PING) {
                    unsigned i;
                    rlen = VAR_LENGTH;
                    reply[2] = rlen;
                    for (i = 4; i < rlen; i++) {
                        reply[i] = ~REGS->fip_reg[0x02 + i];
                    }
                }
                else if (cmd == CMD_WAIT) {
                    unsigned val;
                    unsigned param;

                    val = extract_word(&REGS->fip_reg[0x6]);
                    param = extract_word(&REGS->fip_reg[0x6 + 4]);
                    SUPERVISOR->wd_key = WD_KEY;

                    while (val-- != 0) {
                        if (param & 1) {
                            SUPERVISOR->wd_key = WD_KEY;
                        }
                    }
                } else if (cmd == CMD_FAN_OFF) {
                    REGS->fan_ctrl = REGS->fan_ctrl | FIP_URV_REGS_FAN_CTRL_RSTN;
                    reply[4] = REGS->fan_ctrl;
                    reply[5] = 0;
                    reply[6] = 0;
                    reply[7] = 0;
                } else if (cmd == CMD_FAN_ON) {
                    REGS->fan_ctrl = REGS->fan_ctrl & ~(FIP_URV_REGS_FAN_CTRL_RSTN);
                    reply[4] = REGS->fan_ctrl;
                    reply[5] = 0;
                    reply[6] = 0;
                    reply[7] = 0;
                } else {
                    /* No reply for pad (or error).  */
                    reply[0] = CMD_ERROR;
                    rdy = 0;
                }
            }

            /* Variable has been consumed */
            REGS->fip_var1 = 0;
        }

        if (fip_status & 2) {
            /* var3 can be written. */

            /* High priority: answer packet from management. */
            if (rdy) {
                int i;

                /* Produce variable */
                REGS->fip_var3 = 1;

                /* Write a reply. */
                for (i = 0; i < rlen; i++) {
                    REGS->fip_reg[0x102 + i] = reply[i];
                }

                REGS->fip_var3 = 0;

                rdy = 0;
            }
            else if (copy_app_to_fip()) {
                /* done */
            }
            else {
                /* Nothing to do; do not send data, clear var3_rdy.  */
                REGS->fip_var3 = 2;
            }
        }

        if ((i2c.state & 0x0f) != I2C_CMD_IDLE) {
            if (i2c.state != I2C_CMD_RUN) {
                /* For end of transfer, check TIP (transfer in progess)  */
                unsigned char stat = REGS->i2c[OC_I2C_CR];
                if (!(stat & 2)) {
                    do_i2c_done (&i2c, stat);
                }
            }

            /* Execute the command.
               As do_i2c_done can change the state, the state needs to
               be checked again */
            if (i2c.state == I2C_CMD_RUN) {
                do_i2c_run (&i2c);
            }
        }
        else if (i2c_bus_status == 1 && timeout_mon++ == 200000) {
            /* Was waiting, start a new cycle. */
            timeout_mon = 0;

            /* Start loop commands */
            i2c.cmds = i2c_loop_cmds;

#if HAVE_RATOPUS
            if (ratopus_init_state == 0) {
                ratopus_init_state = 1;
                /* Initialize Ratopus */
                i2c.cmds = initialize_ratopus_cmd;
            }
#endif

            i2c.coff = 0;
            i2c.boff = 0;
            i2c.state = I2C_CMD_RUN;

            /* Scanning */
            i2c_bus_status = 2;
        }
        else if (i2c_bus_status == 3 && timeout_mon++ == 100000) {
            /* Was in reset (power-off), re-enable monimod */
            if (0) {
            }
#if HAVE_MONIMOD
            else if (REGS->i2c[OC_I2C_CFG] != MONIMOD_I2C_BUS_ID) {
                REGS->monimod_shdn = 0;
            }
#endif
#if HAVE_FANTRAY
            else if (REGS->i2c[OC_I2C_CFG] != FANTRAY_I2C_BUS_ID) {
                REGS->fan_ctrl &= ~FIP_URV_REGS_FAN_CTRL_RSTN;
            }
#endif
            i2c_bus_status = 1;
            timeout_mon = 0;
        }
    }
}
