[qemu.git] / hw / misc / milkymist-pfpu.c

/*
 *  QEMU model of the Milkymist programmable FPU.
 *
 *  Copyright (c) 2010 Michael Walle <[email protected]>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Specification available at:
 *   http://www.milkymist.org/socdoc/pfpu.pdf
 *
 */

#include "hw/hw.h"
#include "hw/sysbus.h"
#include "trace.h"
#include "qemu/log.h"
#include "qemu/error-report.h"
#include <math.h>

/* #define TRACE_EXEC */

#ifdef TRACE_EXEC
#    define D_EXEC(x) x
#else
#    define D_EXEC(x)
#endif

enum {
    R_CTL = 0,
    R_MESHBASE,
    R_HMESHLAST,
    R_VMESHLAST,
    R_CODEPAGE,
    R_VERTICES,
    R_COLLISIONS,
    R_STRAYWRITES,
    R_LASTDMA,
    R_PC,
    R_DREGBASE,
    R_CODEBASE,
    R_MAX
};

enum {
    CTL_START_BUSY = (1<<0),
};

enum {
    OP_NOP = 0,
    OP_FADD,
    OP_FSUB,
    OP_FMUL,
    OP_FABS,
    OP_F2I,
    OP_I2F,
    OP_VECTOUT,
    OP_SIN,
    OP_COS,
    OP_ABOVE,
    OP_EQUAL,
    OP_COPY,
    OP_IF,
    OP_TSIGN,
    OP_QUAKE,
};

enum {
    GPR_X = 0,
    GPR_Y = 1,
    GPR_FLAGS = 2,
};

enum {
    LATENCY_FADD = 5,
    LATENCY_FSUB = 5,
    LATENCY_FMUL = 7,
    LATENCY_FABS = 2,
    LATENCY_F2I = 2,
    LATENCY_I2F = 3,
    LATENCY_VECTOUT = 0,
    LATENCY_SIN = 4,
    LATENCY_COS = 4,
    LATENCY_ABOVE = 2,
    LATENCY_EQUAL = 2,
    LATENCY_COPY = 2,
    LATENCY_IF = 2,
    LATENCY_TSIGN = 2,
    LATENCY_QUAKE = 2,
    MAX_LATENCY = 7
};

#define GPR_BEGIN       0x100
#define GPR_END         0x17f
#define MICROCODE_BEGIN 0x200
#define MICROCODE_END   0x3ff
#define MICROCODE_WORDS 2048

#define REINTERPRET_CAST(type, val) (*((type *)&(val)))

#ifdef TRACE_EXEC
static const char *opcode_to_str[] = {
    "NOP", "FADD", "FSUB", "FMUL", "FABS", "F2I", "I2F", "VECTOUT",
    "SIN", "COS", "ABOVE", "EQUAL", "COPY", "IF", "TSIGN", "QUAKE",
};
#endif

#define TYPE_MILKYMIST_PFPU "milkymist-pfpu"
#define MILKYMIST_PFPU(obj) \
    OBJECT_CHECK(MilkymistPFPUState, (obj), TYPE_MILKYMIST_PFPU)

struct MilkymistPFPUState {
    SysBusDevice parent_obj;

    MemoryRegion regs_region;
    CharDriverState *chr;
    qemu_irq irq;

    uint32_t regs[R_MAX];
    uint32_t gp_regs[128];
    uint32_t microcode[MICROCODE_WORDS];

    int output_queue_pos;
    uint32_t output_queue[MAX_LATENCY];
};
typedef struct MilkymistPFPUState MilkymistPFPUState;

static inline hwaddr
get_dma_address(uint32_t base, uint32_t x, uint32_t y)
{
    return base + 8 * (128 * y + x);
}

static inline void
output_queue_insert(MilkymistPFPUState *s, uint32_t val, int pos)
{
    s->output_queue[(s->output_queue_pos + pos) % MAX_LATENCY] = val;
}

static inline uint32_t
output_queue_remove(MilkymistPFPUState *s)
{
    return s->output_queue[s->output_queue_pos];
}

static inline void
output_queue_advance(MilkymistPFPUState *s)
{
    s->output_queue[s->output_queue_pos] = 0;
    s->output_queue_pos = (s->output_queue_pos + 1) % MAX_LATENCY;
}

static int pfpu_decode_insn(MilkymistPFPUState *s)
{
    uint32_t pc = s->regs[R_PC];
    uint32_t insn = s->microcode[pc];
    uint32_t reg_a = (insn >> 18) & 0x7f;
    uint32_t reg_b = (insn >> 11) & 0x7f;
    uint32_t op = (insn >> 7) & 0xf;
    uint32_t reg_d = insn & 0x7f;
    uint32_t r = 0;
    int latency = 0;

    switch (op) {
    case OP_NOP:
        break;
    case OP_FADD:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = a + b;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_FADD;
        D_EXEC(qemu_log("ADD a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_FSUB:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = a - b;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_FSUB;
        D_EXEC(qemu_log("SUB a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_FMUL:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = a * b;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_FMUL;
        D_EXEC(qemu_log("MUL a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_FABS:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float t = fabsf(a);
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_FABS;
        D_EXEC(qemu_log("ABS a=%f t=%f, r=%08x\n", a, t, r));
    } break;
    case OP_F2I:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        int32_t t = a;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_F2I;
        D_EXEC(qemu_log("F2I a=%f t=%d, r=%08x\n", a, t, r));
    } break;
    case OP_I2F:
    {
        int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
        float t = a;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_I2F;
        D_EXEC(qemu_log("I2F a=%08x t=%f, r=%08x\n", a, t, r));
    } break;
    case OP_VECTOUT:
    {
        uint32_t a = cpu_to_be32(s->gp_regs[reg_a]);
        uint32_t b = cpu_to_be32(s->gp_regs[reg_b]);
        hwaddr dma_ptr =
            get_dma_address(s->regs[R_MESHBASE],
                    s->gp_regs[GPR_X], s->gp_regs[GPR_Y]);
        cpu_physical_memory_write(dma_ptr, &a, 4);
        cpu_physical_memory_write(dma_ptr + 4, &b, 4);
        s->regs[R_LASTDMA] = dma_ptr + 4;
        D_EXEC(qemu_log("VECTOUT a=%08x b=%08x dma=%08x\n", a, b, dma_ptr));
        trace_milkymist_pfpu_vectout(a, b, dma_ptr);
    } break;
    case OP_SIN:
    {
        int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
        float t = sinf(a * (1.0f / (M_PI * 4096.0f)));
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_SIN;
        D_EXEC(qemu_log("SIN a=%d t=%f, r=%08x\n", a, t, r));
    } break;
    case OP_COS:
    {
        int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
        float t = cosf(a * (1.0f / (M_PI * 4096.0f)));
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_COS;
        D_EXEC(qemu_log("COS a=%d t=%f, r=%08x\n", a, t, r));
    } break;
    case OP_ABOVE:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = (a > b) ? 1.0f : 0.0f;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_ABOVE;
        D_EXEC(qemu_log("ABOVE a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_EQUAL:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = (a == b) ? 1.0f : 0.0f;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_EQUAL;
        D_EXEC(qemu_log("EQUAL a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_COPY:
    {
        r = s->gp_regs[reg_a];
        latency = LATENCY_COPY;
        D_EXEC(qemu_log("COPY"));
    } break;
    case OP_IF:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        uint32_t f = s->gp_regs[GPR_FLAGS];
        float t = (f != 0) ? a : b;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_IF;
        D_EXEC(qemu_log("IF f=%u a=%f b=%f t=%f, r=%08x\n", f, a, b, t, r));
    } break;
    case OP_TSIGN:
    {
        float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
        float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
        float t = (b < 0) ? -a : a;
        r = REINTERPRET_CAST(uint32_t, t);
        latency = LATENCY_TSIGN;
        D_EXEC(qemu_log("TSIGN a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
    } break;
    case OP_QUAKE:
    {
        uint32_t a = s->gp_regs[reg_a];
        r = 0x5f3759df - (a >> 1);
        latency = LATENCY_QUAKE;
        D_EXEC(qemu_log("QUAKE a=%d r=%08x\n", a, r));
    } break;

    default:
        error_report("milkymist_pfpu: unknown opcode %d", op);
        break;
    }

    if (!reg_d) {
        D_EXEC(qemu_log("%04d %8s R%03d, R%03d <L=%d, E=%04d>\n",
                    s->regs[R_PC], opcode_to_str[op], reg_a, reg_b, latency,
                    s->regs[R_PC] + latency));
    } else {
        D_EXEC(qemu_log("%04d %8s R%03d, R%03d <L=%d, E=%04d> -> R%03d\n",
                    s->regs[R_PC], opcode_to_str[op], reg_a, reg_b, latency,
                    s->regs[R_PC] + latency, reg_d));
    }

    if (op == OP_VECTOUT) {
        return 0;
    }

    /* store output for this cycle */
    if (reg_d) {
        uint32_t val = output_queue_remove(s);
        D_EXEC(qemu_log("R%03d <- 0x%08x\n", reg_d, val));
        s->gp_regs[reg_d] = val;
    }

    output_queue_advance(s);

    /* store op output */
    if (op != OP_NOP) {
        output_queue_insert(s, r, latency-1);
    }

    /* advance PC */
    s->regs[R_PC]++;

    return 1;
};

static void pfpu_start(MilkymistPFPUState *s)
{
    int x, y;
    int i;

    for (y = 0; y <= s->regs[R_VMESHLAST]; y++) {
        for (x = 0; x <= s->regs[R_HMESHLAST]; x++) {
            D_EXEC(qemu_log("\nprocessing x=%d y=%d\n", x, y));

            /* set current position */
            s->gp_regs[GPR_X] = x;
            s->gp_regs[GPR_Y] = y;

            /* run microcode on this position */
            i = 0;
            while (pfpu_decode_insn(s)) {
                /* decode at most MICROCODE_WORDS instructions */
                if (++i >= MICROCODE_WORDS) {
                    error_report("milkymist_pfpu: too many instructions "
                            "executed in microcode. No VECTOUT?");
                    break;
                }
            }

            /* reset pc for next run */
            s->regs[R_PC] = 0;
        }
    }

    s->regs[R_VERTICES] = x * y;

    trace_milkymist_pfpu_pulse_irq();
    qemu_irq_pulse(s->irq);
}

static inline int get_microcode_address(MilkymistPFPUState *s, uint32_t addr)
{
    return (512 * s->regs[R_CODEPAGE]) + addr - MICROCODE_BEGIN;
}

static uint64_t pfpu_read(void *opaque, hwaddr addr,
                          unsigned size)
{
    MilkymistPFPUState *s = opaque;
    uint32_t r = 0;

    addr >>= 2;
    switch (addr) {
    case R_CTL:
    case R_MESHBASE:
    case R_HMESHLAST:
    case R_VMESHLAST:
    case R_CODEPAGE:
    case R_VERTICES:
    case R_COLLISIONS:
    case R_STRAYWRITES:
    case R_LASTDMA:
    case R_PC:
    case R_DREGBASE:
    case R_CODEBASE:
        r = s->regs[addr];
        break;
    case GPR_BEGIN ... GPR_END:
        r = s->gp_regs[addr - GPR_BEGIN];
        break;
    case MICROCODE_BEGIN ...  MICROCODE_END:
        r = s->microcode[get_microcode_address(s, addr)];
        break;

    default:
        error_report("milkymist_pfpu: read access to unknown register 0x"
                TARGET_FMT_plx, addr << 2);
        break;
    }

    trace_milkymist_pfpu_memory_read(addr << 2, r);

    return r;
}

static void pfpu_write(void *opaque, hwaddr addr, uint64_t value,
                       unsigned size)
{
    MilkymistPFPUState *s = opaque;

    trace_milkymist_pfpu_memory_write(addr, value);

    addr >>= 2;
    switch (addr) {
    case R_CTL:
        if (value & CTL_START_BUSY) {
            pfpu_start(s);
        }
        break;
    case R_MESHBASE:
    case R_HMESHLAST:
    case R_VMESHLAST:
    case R_CODEPAGE:
    case R_VERTICES:
    case R_COLLISIONS:
    case R_STRAYWRITES:
    case R_LASTDMA:
    case R_PC:
    case R_DREGBASE:
    case R_CODEBASE:
        s->regs[addr] = value;
        break;
    case GPR_BEGIN ...  GPR_END:
        s->gp_regs[addr - GPR_BEGIN] = value;
        break;
    case MICROCODE_BEGIN ...  MICROCODE_END:
        s->microcode[get_microcode_address(s, addr)] = value;
        break;

    default:
        error_report("milkymist_pfpu: write access to unknown register 0x"
                TARGET_FMT_plx, addr << 2);
        break;
    }
}

static const MemoryRegionOps pfpu_mmio_ops = {
    .read = pfpu_read,
    .write = pfpu_write,
    .valid = {
        .min_access_size = 4,
        .max_access_size = 4,
    },
    .endianness = DEVICE_NATIVE_ENDIAN,
};

static void milkymist_pfpu_reset(DeviceState *d)
{
    MilkymistPFPUState *s = MILKYMIST_PFPU(d);
    int i;

    for (i = 0; i < R_MAX; i++) {
        s->regs[i] = 0;
    }
    for (i = 0; i < 128; i++) {
        s->gp_regs[i] = 0;
    }
    for (i = 0; i < MICROCODE_WORDS; i++) {
        s->microcode[i] = 0;
    }
    s->output_queue_pos = 0;
    for (i = 0; i < MAX_LATENCY; i++) {
        s->output_queue[i] = 0;
    }
}

static int milkymist_pfpu_init(SysBusDevice *dev)
{
    MilkymistPFPUState *s = MILKYMIST_PFPU(dev);

    sysbus_init_irq(dev, &s->irq);

    memory_region_init_io(&s->regs_region, OBJECT(dev), &pfpu_mmio_ops, s,
            "milkymist-pfpu", MICROCODE_END * 4);
    sysbus_init_mmio(dev, &s->regs_region);

    return 0;
}

static const VMStateDescription vmstate_milkymist_pfpu = {
    .name = "milkymist-pfpu",
    .version_id = 1,
    .minimum_version_id = 1,
    .fields = (VMStateField[]) {
        VMSTATE_UINT32_ARRAY(regs, MilkymistPFPUState, R_MAX),
        VMSTATE_UINT32_ARRAY(gp_regs, MilkymistPFPUState, 128),
        VMSTATE_UINT32_ARRAY(microcode, MilkymistPFPUState, MICROCODE_WORDS),
        VMSTATE_INT32(output_queue_pos, MilkymistPFPUState),
        VMSTATE_UINT32_ARRAY(output_queue, MilkymistPFPUState, MAX_LATENCY),
        VMSTATE_END_OF_LIST()
    }
};

static void milkymist_pfpu_class_init(ObjectClass *klass, void *data)
{
    DeviceClass *dc = DEVICE_CLASS(klass);
    SysBusDeviceClass *k = SYS_BUS_DEVICE_CLASS(klass);

    k->init = milkymist_pfpu_init;
    dc->reset = milkymist_pfpu_reset;
    dc->vmsd = &vmstate_milkymist_pfpu;
}

static const TypeInfo milkymist_pfpu_info = {
    .name          = TYPE_MILKYMIST_PFPU,
    .parent        = TYPE_SYS_BUS_DEVICE,
    .instance_size = sizeof(MilkymistPFPUState),
    .class_init    = milkymist_pfpu_class_init,
};

static void milkymist_pfpu_register_types(void)
{
    type_register_static(&milkymist_pfpu_info);
}

type_init(milkymist_pfpu_register_types)
Commit	Line	Data
5ee18b9c MW	1	/*
	2	* QEMU model of the Milkymist programmable FPU.
	3	*
	4	* Copyright (c) 2010 Michael Walle <[email protected]>
	5	*
	6	* This library is free software; you can redistribute it and/or
	7	* modify it under the terms of the GNU Lesser General Public
	8	* License as published by the Free Software Foundation; either
	9	* version 2 of the License, or (at your option) any later version.
	10	*
	11	* This library is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	14	* Lesser General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU Lesser General Public
	17	* License along with this library; if not, see <http://www.gnu.org/licenses/>.
	18	*
	19	*
	20	* Specification available at:
	21	* http://www.milkymist.org/socdoc/pfpu.pdf
	22	*
	23	*/
	24
83c9f4ca PB	25	#include "hw/hw.h"
83c9f4ca PB	26	#include "hw/sysbus.h"
5ee18b9c	27	#include "trace.h"
1de7afc9 PB	28	#include "qemu/log.h"
1de7afc9 PB	29	#include "qemu/error-report.h"
5ee18b9c MW	30	#include <math.h>
	31
	32	/* #define TRACE_EXEC */
	33
	34	#ifdef TRACE_EXEC
	35	# define D_EXEC(x) x
	36	#else
	37	# define D_EXEC(x)
	38	#endif
	39
	40	enum {
	41	R_CTL = 0,
	42	R_MESHBASE,
	43	R_HMESHLAST,
	44	R_VMESHLAST,
	45	R_CODEPAGE,
	46	R_VERTICES,
	47	R_COLLISIONS,
	48	R_STRAYWRITES,
	49	R_LASTDMA,
	50	R_PC,
	51	R_DREGBASE,
	52	R_CODEBASE,
	53	R_MAX
	54	};
	55
	56	enum {
	57	CTL_START_BUSY = (1<<0),
	58	};
	59
	60	enum {
	61	OP_NOP = 0,
	62	OP_FADD,
	63	OP_FSUB,
	64	OP_FMUL,
	65	OP_FABS,
	66	OP_F2I,
	67	OP_I2F,
	68	OP_VECTOUT,
	69	OP_SIN,
	70	OP_COS,
	71	OP_ABOVE,
	72	OP_EQUAL,
	73	OP_COPY,
	74	OP_IF,
	75	OP_TSIGN,
	76	OP_QUAKE,
	77	};
	78
	79	enum {
	80	GPR_X = 0,
	81	GPR_Y = 1,
	82	GPR_FLAGS = 2,
	83	};
	84
	85	enum {
	86	LATENCY_FADD = 5,
	87	LATENCY_FSUB = 5,
	88	LATENCY_FMUL = 7,
	89	LATENCY_FABS = 2,
	90	LATENCY_F2I = 2,
	91	LATENCY_I2F = 3,
	92	LATENCY_VECTOUT = 0,
	93	LATENCY_SIN = 4,
94	LATENCY_COS = 4,
95	LATENCY_ABOVE = 2,
96	LATENCY_EQUAL = 2,
97	LATENCY_COPY = 2,
98	LATENCY_IF = 2,
99	LATENCY_TSIGN = 2,
100	LATENCY_QUAKE = 2,
101	MAX_LATENCY = 7
102	};
103
104	#define GPR_BEGIN 0x100
105	#define GPR_END 0x17f
106	#define MICROCODE_BEGIN 0x200
107	#define MICROCODE_END 0x3ff
108	#define MICROCODE_WORDS 2048
109
110	#define REINTERPRET_CAST(type, val) (((type )&(val)))
111
112	#ifdef TRACE_EXEC
113	static const char *opcode_to_str[] = {
114	"NOP", "FADD", "FSUB", "FMUL", "FABS", "F2I", "I2F", "VECTOUT",
115	"SIN", "COS", "ABOVE", "EQUAL", "COPY", "IF", "TSIGN", "QUAKE",
116	};
117	#endif
118
aee31f7b AF	119	#define TYPE_MILKYMIST_PFPU "milkymist-pfpu"
	120	#define MILKYMIST_PFPU(obj) \
	121	OBJECT_CHECK(MilkymistPFPUState, (obj), TYPE_MILKYMIST_PFPU)
	122
5ee18b9c	123	struct MilkymistPFPUState {
aee31f7b AF	124	SysBusDevice parent_obj;
aee31f7b AF	125
d46ccfce	126	MemoryRegion regs_region;
5ee18b9c MW	127	CharDriverState *chr;
	128	qemu_irq irq;
	129
	130	uint32_t regs[R_MAX];
	131	uint32_t gp_regs[128];
	132	uint32_t microcode[MICROCODE_WORDS];
	133
	134	int output_queue_pos;
	135	uint32_t output_queue[MAX_LATENCY];
	136	};
	137	typedef struct MilkymistPFPUState MilkymistPFPUState;
	138
a8170e5e	139	static inline hwaddr
5ee18b9c MW	140	get_dma_address(uint32_t base, uint32_t x, uint32_t y)
	141	{
	142	return base + 8 * (128 * y + x);
	143	}
	144
	145	static inline void
	146	output_queue_insert(MilkymistPFPUState *s, uint32_t val, int pos)
	147	{
	148	s->output_queue[(s->output_queue_pos + pos) % MAX_LATENCY] = val;
	149	}
	150
	151	static inline uint32_t
	152	output_queue_remove(MilkymistPFPUState *s)
	153	{
	154	return s->output_queue[s->output_queue_pos];
	155	}
	156
	157	static inline void
	158	output_queue_advance(MilkymistPFPUState *s)
	159	{
	160	s->output_queue[s->output_queue_pos] = 0;
	161	s->output_queue_pos = (s->output_queue_pos + 1) % MAX_LATENCY;
	162	}
	163
	164	static int pfpu_decode_insn(MilkymistPFPUState *s)
	165	{
	166	uint32_t pc = s->regs[R_PC];
	167	uint32_t insn = s->microcode[pc];
	168	uint32_t reg_a = (insn >> 18) & 0x7f;
	169	uint32_t reg_b = (insn >> 11) & 0x7f;
	170	uint32_t op = (insn >> 7) & 0xf;
	171	uint32_t reg_d = insn & 0x7f;
7f7454ec	172	uint32_t r = 0;
5ee18b9c MW	173	int latency = 0;
	174
	175	switch (op) {
	176	case OP_NOP:
	177	break;
	178	case OP_FADD:
	179	{
	180	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	181	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	182	float t = a + b;
	183	r = REINTERPRET_CAST(uint32_t, t);
	184	latency = LATENCY_FADD;
	185	D_EXEC(qemu_log("ADD a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	186	} break;
	187	case OP_FSUB:
	188	{
	189	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	190	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	191	float t = a - b;
	192	r = REINTERPRET_CAST(uint32_t, t);
	193	latency = LATENCY_FSUB;
	194	D_EXEC(qemu_log("SUB a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	195	} break;
	196	case OP_FMUL:
	197	{
	198	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	199	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	200	float t = a * b;
	201	r = REINTERPRET_CAST(uint32_t, t);
	202	latency = LATENCY_FMUL;
	203	D_EXEC(qemu_log("MUL a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	204	} break;
	205	case OP_FABS:
	206	{
	207	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	208	float t = fabsf(a);
	209	r = REINTERPRET_CAST(uint32_t, t);
	210	latency = LATENCY_FABS;
	211	D_EXEC(qemu_log("ABS a=%f t=%f, r=%08x\n", a, t, r));
	212	} break;
	213	case OP_F2I:
	214	{
	215	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	216	int32_t t = a;
	217	r = REINTERPRET_CAST(uint32_t, t);
	218	latency = LATENCY_F2I;
	219	D_EXEC(qemu_log("F2I a=%f t=%d, r=%08x\n", a, t, r));
	220	} break;
	221	case OP_I2F:
	222	{
	223	int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
	224	float t = a;
	225	r = REINTERPRET_CAST(uint32_t, t);
	226	latency = LATENCY_I2F;
	227	D_EXEC(qemu_log("I2F a=%08x t=%f, r=%08x\n", a, t, r));
	228	} break;
	229	case OP_VECTOUT:
	230	{
	231	uint32_t a = cpu_to_be32(s->gp_regs[reg_a]);
	232	uint32_t b = cpu_to_be32(s->gp_regs[reg_b]);
a8170e5e	233	hwaddr dma_ptr =
5ee18b9c MW	234	get_dma_address(s->regs[R_MESHBASE],
5ee18b9c MW	235	s->gp_regs[GPR_X], s->gp_regs[GPR_Y]);
e1fe50dc SW	236	cpu_physical_memory_write(dma_ptr, &a, 4);
e1fe50dc SW	237	cpu_physical_memory_write(dma_ptr + 4, &b, 4);
5ee18b9c MW	238	s->regs[R_LASTDMA] = dma_ptr + 4;
	239	D_EXEC(qemu_log("VECTOUT a=%08x b=%08x dma=%08x\n", a, b, dma_ptr));
	240	trace_milkymist_pfpu_vectout(a, b, dma_ptr);
	241	} break;
	242	case OP_SIN:
	243	{
	244	int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
	245	float t = sinf(a * (1.0f / (M_PI * 4096.0f)));
	246	r = REINTERPRET_CAST(uint32_t, t);
	247	latency = LATENCY_SIN;
	248	D_EXEC(qemu_log("SIN a=%d t=%f, r=%08x\n", a, t, r));
	249	} break;
	250	case OP_COS:
	251	{
	252	int32_t a = REINTERPRET_CAST(int32_t, s->gp_regs[reg_a]);
	253	float t = cosf(a * (1.0f / (M_PI * 4096.0f)));
	254	r = REINTERPRET_CAST(uint32_t, t);
	255	latency = LATENCY_COS;
	256	D_EXEC(qemu_log("COS a=%d t=%f, r=%08x\n", a, t, r));
	257	} break;
	258	case OP_ABOVE:
	259	{
	260	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	261	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	262	float t = (a > b) ? 1.0f : 0.0f;
	263	r = REINTERPRET_CAST(uint32_t, t);
	264	latency = LATENCY_ABOVE;
	265	D_EXEC(qemu_log("ABOVE a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	266	} break;
	267	case OP_EQUAL:
	268	{
	269	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	270	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	271	float t = (a == b) ? 1.0f : 0.0f;
	272	r = REINTERPRET_CAST(uint32_t, t);
	273	latency = LATENCY_EQUAL;
	274	D_EXEC(qemu_log("EQUAL a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	275	} break;
	276	case OP_COPY:
	277	{
	278	r = s->gp_regs[reg_a];
	279	latency = LATENCY_COPY;
	280	D_EXEC(qemu_log("COPY"));
	281	} break;
	282	case OP_IF:
	283	{
	284	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	285	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	286	uint32_t f = s->gp_regs[GPR_FLAGS];
	287	float t = (f != 0) ? a : b;
	288	r = REINTERPRET_CAST(uint32_t, t);
	289	latency = LATENCY_IF;
	290	D_EXEC(qemu_log("IF f=%u a=%f b=%f t=%f, r=%08x\n", f, a, b, t, r));
	291	} break;
	292	case OP_TSIGN:
	293	{
	294	float a = REINTERPRET_CAST(float, s->gp_regs[reg_a]);
	295	float b = REINTERPRET_CAST(float, s->gp_regs[reg_b]);
	296	float t = (b < 0) ? -a : a;
	297	r = REINTERPRET_CAST(uint32_t, t);
	298	latency = LATENCY_TSIGN;
	299	D_EXEC(qemu_log("TSIGN a=%f b=%f t=%f, r=%08x\n", a, b, t, r));
	300	} break;
	301	case OP_QUAKE:
302	{
303	uint32_t a = s->gp_regs[reg_a];
304	r = 0x5f3759df - (a >> 1);
305	latency = LATENCY_QUAKE;
306	D_EXEC(qemu_log("QUAKE a=%d r=%08x\n", a, r));
307	} break;
308
309	default:
6daf194d	310	error_report("milkymist_pfpu: unknown opcode %d", op);
5ee18b9c MW	311	break;
	312	}
	313
	314	if (!reg_d) {
	315	D_EXEC(qemu_log("%04d %8s R%03d, R%03d <L=%d, E=%04d>\n",
	316	s->regs[R_PC], opcode_to_str[op], reg_a, reg_b, latency,
	317	s->regs[R_PC] + latency));
	318	} else {
	319	D_EXEC(qemu_log("%04d %8s R%03d, R%03d <L=%d, E=%04d> -> R%03d\n",
	320	s->regs[R_PC], opcode_to_str[op], reg_a, reg_b, latency,
	321	s->regs[R_PC] + latency, reg_d));
	322	}
	323
	324	if (op == OP_VECTOUT) {
	325	return 0;
	326	}
	327
	328	/* store output for this cycle */
	329	if (reg_d) {
	330	uint32_t val = output_queue_remove(s);
	331	D_EXEC(qemu_log("R%03d <- 0x%08x\n", reg_d, val));
	332	s->gp_regs[reg_d] = val;
	333	}
	334
	335	output_queue_advance(s);
	336
	337	/* store op output */
	338	if (op != OP_NOP) {
	339	output_queue_insert(s, r, latency-1);
	340	}
	341
	342	/* advance PC */
	343	s->regs[R_PC]++;
	344
	345	return 1;
	346	};
	347
	348	static void pfpu_start(MilkymistPFPUState *s)
	349	{
	350	int x, y;
	351	int i;
	352
	353	for (y = 0; y <= s->regs[R_VMESHLAST]; y++) {
	354	for (x = 0; x <= s->regs[R_HMESHLAST]; x++) {
	355	D_EXEC(qemu_log("\nprocessing x=%d y=%d\n", x, y));
	356
	357	/* set current position */
	358	s->gp_regs[GPR_X] = x;
	359	s->gp_regs[GPR_Y] = y;
	360
	361	/* run microcode on this position */
	362	i = 0;
	363	while (pfpu_decode_insn(s)) {
	364	/* decode at most MICROCODE_WORDS instructions */
c6dc3dd7	365	if (++i >= MICROCODE_WORDS) {
5ee18b9c	366	error_report("milkymist_pfpu: too many instructions "
6daf194d	367	"executed in microcode. No VECTOUT?");
5ee18b9c MW	368	break;
	369	}
	370	}
	371
	372	/* reset pc for next run */
	373	s->regs[R_PC] = 0;
	374	}
	375	}
	376
	377	s->regs[R_VERTICES] = x * y;
	378
	379	trace_milkymist_pfpu_pulse_irq();
	380	qemu_irq_pulse(s->irq);
	381	}
	382
	383	static inline int get_microcode_address(MilkymistPFPUState *s, uint32_t addr)
	384	{
	385	return (512 * s->regs[R_CODEPAGE]) + addr - MICROCODE_BEGIN;
	386	}
	387
a8170e5e	388	static uint64_t pfpu_read(void *opaque, hwaddr addr,
d46ccfce	389	unsigned size)
5ee18b9c MW	390	{
	391	MilkymistPFPUState *s = opaque;
	392	uint32_t r = 0;
	393
	394	addr >>= 2;
	395	switch (addr) {
	396	case R_CTL:
	397	case R_MESHBASE:
	398	case R_HMESHLAST:
	399	case R_VMESHLAST:
	400	case R_CODEPAGE:
	401	case R_VERTICES:
	402	case R_COLLISIONS:
	403	case R_STRAYWRITES:
	404	case R_LASTDMA:
	405	case R_PC:
	406	case R_DREGBASE:
	407	case R_CODEBASE:
	408	r = s->regs[addr];
	409	break;
	410	case GPR_BEGIN ... GPR_END:
	411	r = s->gp_regs[addr - GPR_BEGIN];
	412	break;
	413	case MICROCODE_BEGIN ... MICROCODE_END:
	414	r = s->microcode[get_microcode_address(s, addr)];
	415	break;
	416
	417	default:
	418	error_report("milkymist_pfpu: read access to unknown register 0x"
	419	TARGET_FMT_plx, addr << 2);
	420	break;
	421	}
	422
	423	trace_milkymist_pfpu_memory_read(addr << 2, r);
	424
	425	return r;
	426	}
	427
a8170e5e	428	static void pfpu_write(void *opaque, hwaddr addr, uint64_t value,
d46ccfce	429	unsigned size)
5ee18b9c MW	430	{
	431	MilkymistPFPUState *s = opaque;
	432
	433	trace_milkymist_pfpu_memory_write(addr, value);
	434
	435	addr >>= 2;
	436	switch (addr) {
	437	case R_CTL:
	438	if (value & CTL_START_BUSY) {
	439	pfpu_start(s);
	440	}
	441	break;
	442	case R_MESHBASE:
	443	case R_HMESHLAST:
	444	case R_VMESHLAST:
	445	case R_CODEPAGE:
	446	case R_VERTICES:
	447	case R_COLLISIONS:
	448	case R_STRAYWRITES:
	449	case R_LASTDMA:
	450	case R_PC:
	451	case R_DREGBASE:
	452	case R_CODEBASE:
	453	s->regs[addr] = value;
	454	break;
	455	case GPR_BEGIN ... GPR_END:
	456	s->gp_regs[addr - GPR_BEGIN] = value;
	457	break;
	458	case MICROCODE_BEGIN ... MICROCODE_END:
	459	s->microcode[get_microcode_address(s, addr)] = value;
	460	break;
	461
	462	default:
	463	error_report("milkymist_pfpu: write access to unknown register 0x"
	464	TARGET_FMT_plx, addr << 2);
	465	break;
	466	}
	467	}
	468
d46ccfce MW	469	static const MemoryRegionOps pfpu_mmio_ops = {
	470	.read = pfpu_read,
	471	.write = pfpu_write,
	472	.valid = {
	473	.min_access_size = 4,
	474	.max_access_size = 4,
	475	},
	476	.endianness = DEVICE_NATIVE_ENDIAN,
5ee18b9c MW	477	};
	478
	479	static void milkymist_pfpu_reset(DeviceState *d)
	480	{
aee31f7b	481	MilkymistPFPUState *s = MILKYMIST_PFPU(d);
5ee18b9c MW	482	int i;
	483
	484	for (i = 0; i < R_MAX; i++) {
	485	s->regs[i] = 0;
	486	}
	487	for (i = 0; i < 128; i++) {
	488	s->gp_regs[i] = 0;
	489	}
	490	for (i = 0; i < MICROCODE_WORDS; i++) {
	491	s->microcode[i] = 0;
	492	}
	493	s->output_queue_pos = 0;
	494	for (i = 0; i < MAX_LATENCY; i++) {
	495	s->output_queue[i] = 0;
	496	}
	497	}
	498
	499	static int milkymist_pfpu_init(SysBusDevice *dev)
	500	{
aee31f7b	501	MilkymistPFPUState *s = MILKYMIST_PFPU(dev);
5ee18b9c MW	502
	503	sysbus_init_irq(dev, &s->irq);
	504
3c161542	505	memory_region_init_io(&s->regs_region, OBJECT(dev), &pfpu_mmio_ops, s,
d46ccfce	506	"milkymist-pfpu", MICROCODE_END * 4);
750ecd44	507	sysbus_init_mmio(dev, &s->regs_region);
5ee18b9c MW	508
	509	return 0;
	510	}
	511
	512	static const VMStateDescription vmstate_milkymist_pfpu = {
	513	.name = "milkymist-pfpu",
	514	.version_id = 1,
	515	.minimum_version_id = 1,
35d08458	516	.fields = (VMStateField[]) {
5ee18b9c MW	517	VMSTATE_UINT32_ARRAY(regs, MilkymistPFPUState, R_MAX),
	518	VMSTATE_UINT32_ARRAY(gp_regs, MilkymistPFPUState, 128),
	519	VMSTATE_UINT32_ARRAY(microcode, MilkymistPFPUState, MICROCODE_WORDS),
	520	VMSTATE_INT32(output_queue_pos, MilkymistPFPUState),
	521	VMSTATE_UINT32_ARRAY(output_queue, MilkymistPFPUState, MAX_LATENCY),
	522	VMSTATE_END_OF_LIST()
	523	}
	524	};
	525
999e12bb AL	526	static void milkymist_pfpu_class_init(ObjectClass klass, void data)
999e12bb AL	527	{
39bffca2	528	DeviceClass *dc = DEVICE_CLASS(klass);
999e12bb AL	529	SysBusDeviceClass *k = SYS_BUS_DEVICE_CLASS(klass);
	530
	531	k->init = milkymist_pfpu_init;
39bffca2 AL	532	dc->reset = milkymist_pfpu_reset;
39bffca2 AL	533	dc->vmsd = &vmstate_milkymist_pfpu;
999e12bb AL	534	}
999e12bb AL	535
8c43a6f0	536	static const TypeInfo milkymist_pfpu_info = {
aee31f7b	537	.name = TYPE_MILKYMIST_PFPU,
39bffca2 AL	538	.parent = TYPE_SYS_BUS_DEVICE,
	539	.instance_size = sizeof(MilkymistPFPUState),
	540	.class_init = milkymist_pfpu_class_init,
5ee18b9c MW	541	};
5ee18b9c MW	542
83f7d43a	543	static void milkymist_pfpu_register_types(void)
5ee18b9c	544	{
39bffca2	545	type_register_static(&milkymist_pfpu_info);
5ee18b9c MW	546	}
5ee18b9c MW	547
83f7d43a	548	type_init(milkymist_pfpu_register_types)