xref: /src/sys/arch/evbppc/nintendo/dev/wiifb.c

/* $NetBSD: wiifb.c,v 1.1 2026/01/09 22:54:30 jmcneill Exp $ */

/*-
 * Copyright (c) 2024-2025 Jared McNeill <jmcneill@invisible.ca>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <sys/cdefs.h>
__KERNEL_RCSID(0, "$NetBSD: wiifb.c,v 1.1 2026/01/09 22:54:30 jmcneill Exp $");

#include <sys/param.h>
#include <sys/bus.h>
#include <sys/device.h>
#include <sys/systm.h>

#include <machine/wii.h>
#include <machine/wiiu.h>
#include <powerpc/spr.h>
#include <powerpc/oea/spr.h>
#include <powerpc/oea/hid.h>

#include <dev/videomode/videomode.h>
#include <dev/wsfb/genfbvar.h>

#include "mainbus.h"
#include "vireg.h"
#include "gxreg.h"

#define WIIFB_RGB_WIDTH			640
#define WIIFB_RGB_HEIGHT		480
#define WIIFB_RGB_BPP			32

#define WIIFB_ERROR_BLINK_INTERVAL	1000000

#define WIIFB_TOP_BOTTOM_BORDER		16
#define WIIFB_EFFECTIVE_START(p, w)	\
	((uintptr_t)(p) + WIIFB_TOP_BOTTOM_BORDER * (w) * 2)
#define WIIFB_EFFECTIVE_HEIGHT(h)	\
	((h) - WIIFB_TOP_BOTTOM_BORDER * 2)

#define WIIFB_FIFO_SIZE			(256 * 1024)

#define IBM750CL_SPR_HID2		920
#define  IBM750CL_SPR_HID2_WPE		0x40000000	/* Write pipe enable */
#define IBM750CL_SPR_WPAR		921

struct wiifb_mode {
	const char *		name;
	u_int			width;
	u_int			height;
	u_int			lines;
};

static uint32_t wiifb_devcmap[16] = {
	0x00800080,	/* Black */
	0x1dff1d6b,	/* Blue */
	0x4b554b4a,	/* Green */
	0x80808080,	/* Cyan */
	0x4c544cff,	/* Red */
	0x3aaa34b5,	/* Magenta */
	0x7140718a,	/* Brown */
	0xff80ff80,	/* White */
	0x80808080,	/* Gray */
	0xc399c36a,	/* Bright Blue */
	0xd076d074,	/* Bright Green */
	0x80808080,	/* Bright Cyan */
	0x4c544cff,	/* Bright Red */
	0x3aaa34b5,	/* Bright Magenta */
	0xe100e194,	/* Bright Yellow */
	0xff80ff80	/* Bright White */
};

#define WIIFB_MODE_INDEX(fmt, interlaced)	((fmt << 1) | interlaced)

static const struct wiifb_mode wiifb_modes[] = {
	[WIIFB_MODE_INDEX(VI_DCR_FMT_NTSC, 0)] = {
		.name = "NTSC 480p",
		.width = 640,
		.height = 480,
		.lines = 525,
	},
	[WIIFB_MODE_INDEX(VI_DCR_FMT_NTSC, 1)] = {
		.name = "NTSC 480i",
		.width = 640,
		.height = 480,
		.lines = 525,
	},
	[WIIFB_MODE_INDEX(VI_DCR_FMT_PAL, 1)] = {
		.name = "PAL 576i",
		.width = 640,
		.height = 574,
		.lines = 625,
	},

};
#define WIIFB_NMODES	__arraycount(wiifb_modes)

struct wiifb_dma {
	bus_dmamap_t		dma_map;
	bus_dma_tag_t		dma_tag;
	bus_size_t		dma_size;
	bus_dma_segment_t	dma_segs[1];
	int			dma_nsegs;
	void			*dma_addr;
};

struct wiifb_softc {
	struct genfb_softc	sc_gen;

	bus_space_tag_t		sc_bst;
	bus_space_handle_t	sc_bsh;
	bus_dma_tag_t		sc_dmat;

	void			*sc_bits;

	uint8_t			sc_format;
	bool			sc_interlaced;

	const struct wiifb_mode	*sc_curmode;

	u_int			sc_wsmode;

	volatile uint32_t	*sc_efb;
	volatile uint16_t	*sc_cp;
	volatile uint16_t	*sc_pe;
	volatile uint32_t	*sc_pi;
	gx_wgpipe_t		*sc_wgpipe;

	struct wiifb_dma	sc_rgb;
	struct wiifb_dma	sc_fifo;

	uint16_t		sc_token;
};

#define	RD2(sc, reg)		\
	bus_space_read_2((sc)->sc_bst, (sc)->sc_bsh, (reg))
#define	RD4(sc, reg)		\
	bus_space_read_4((sc)->sc_bst, (sc)->sc_bsh, (reg))
#define	WR2(sc, reg, val)	\
	bus_space_write_2((sc)->sc_bst, (sc)->sc_bsh, (reg), (val))
#define	WR4(sc, reg, val)	\
	bus_space_write_4((sc)->sc_bst, (sc)->sc_bsh, (reg), (val))

#define CP_WRITE(sc, off, val)	(sc)->sc_cp[(off)] = (val)
#define CP_READ(sc, off)	(sc)->sc_cp[(off)]

#define PE_WRITE(sc, off, val)	(sc)->sc_pe[(off)] = (val)
#define PE_READ(sc, off)	(sc)->sc_pe[(off)]

#define PI_WRITE(sc, off, val)	(sc)->sc_pi[(off)] = (val)
#define PI_READ(sc, off)	(sc)->sc_pi[(off)]

static int	wiifb_match(device_t, cfdata_t, void *);
static void	wiifb_attach(device_t, device_t, void *);

static void	wiifb_accel_init(struct wiifb_softc *);
static int	wiifb_vi_intr(void *);
static void	wiifb_vi_refresh(void *);

static void	wiifb_init(struct wiifb_softc *);
static void	wiifb_set_mode(struct wiifb_softc *, uint8_t, bool);
static void	wiifb_set_fb(struct wiifb_softc *);
static void	wiifb_clear_xfb(struct wiifb_softc *);

static int	wiifb_ioctl(void *, void *, u_long, void *, int, lwp_t *);
static paddr_t	wiifb_mmap(void *, void *, off_t, int);

static struct genfb_ops wiifb_ops = {
	.genfb_ioctl = wiifb_ioctl,
	.genfb_mmap = wiifb_mmap,
};

CFATTACH_DECL_NEW(wiifb, sizeof(struct wiifb_softc),
	wiifb_match, wiifb_attach, NULL, NULL);

static int
wiifb_match(device_t parent, cfdata_t cf, void *aux)
{
	struct mainbus_attach_args *maa = aux;

	return !wiiu_native && strcmp(maa->maa_name, "genfb") == 0;
}

static void
wiifb_attach(device_t parent, device_t self, void *aux)
{
	struct wiifb_softc *sc = device_private(self);
	prop_dictionary_t dict = device_properties(self);
	struct mainbus_attach_args *maa = aux;
	int error;

	sc->sc_gen.sc_dev = self;
	sc->sc_bst = maa->maa_bst;
	error = bus_space_map(sc->sc_bst, VI_BASE, VI_SIZE, 0, &sc->sc_bsh);
	if (error != 0) {
		panic("couldn't map registers");
	}
	sc->sc_bits = mapiodev(XFB_START, XFB_SIZE, true);
	sc->sc_wsmode = WSDISPLAYIO_MODE_EMUL;
	sc->sc_dmat = maa->maa_dmat;

	wiifb_clear_xfb(sc);

	wiifb_init(sc);
	wiifb_set_mode(sc, sc->sc_format, sc->sc_interlaced);

	prop_dictionary_set_uint32(dict, "width", sc->sc_curmode->width);
	prop_dictionary_set_uint32(dict, "height",
	    WIIFB_EFFECTIVE_HEIGHT(sc->sc_curmode->height));
	prop_dictionary_set_uint8(dict, "depth", 16);
	prop_dictionary_set_uint32(dict, "address", XFB_START);
	prop_dictionary_set_uint32(dict, "virtual_address",
	    WIIFB_EFFECTIVE_START(sc->sc_bits, sc->sc_curmode->width));
	prop_dictionary_set_uint64(dict, "devcmap", (uintptr_t)wiifb_devcmap);

	genfb_init(&sc->sc_gen);

	aprint_naive("\n");
	aprint_normal(": %s\n", sc->sc_curmode->name);

	genfb_cnattach();
	prop_dictionary_set_bool(dict, "is_console", true);
	genfb_attach(&sc->sc_gen, &wiifb_ops);

	wiifb_accel_init(sc);
}

static void
wiifb_clear_xfb(struct wiifb_softc *sc)
{
	u_int offset;
	uint32_t *p;

	/*
	 * Paint the entire XFB black. Use 4-byte accesses as the Wii will
	 * ignore 1- and 2- byte writes to uncached memory.
	 */
	for (p = sc->sc_bits, offset = 0;
	     offset < XFB_SIZE;
	     offset += 4, p++) {
		*p = wiifb_devcmap[0];
	}
}

static int
wiifb_vi_init(struct wiifb_softc *sc)
{
	device_t dev = sc->sc_gen.sc_dev;
	void *ih;

	WR4(sc, VI_DI0, VI_DI_ENB |
			__SHIFTIN(1, VI_DI_VCT) |
			__SHIFTIN(1, VI_DI_HCT));
	WR4(sc, VI_DI1, 0);
	WR4(sc, VI_DI2, 0);
	WR4(sc, VI_DI3, 0);

	ih = intr_establish_xname(PI_IRQ_VI, IST_LEVEL, IPL_TTY,
	    wiifb_vi_intr, sc, device_xname(dev));
	if (ih == NULL) {
		aprint_error_dev(dev, "failed to install VI intr handler\n");
		return EIO;
	}

	return 0;
}

static int
wiifb_dma_alloc(struct wiifb_softc *sc, bus_size_t size, bus_size_t align,
    int flags, struct wiifb_dma *dma)
{
	bus_dma_tag_t dmat = sc->sc_dmat; // &wii_mem1_bus_dma_tag;
	int error;

	error = bus_dmamem_alloc(dmat, size, align, 0,
	    dma->dma_segs, 1, &dma->dma_nsegs, BUS_DMA_WAITOK);
	if (error)
		return error;

	error = bus_dmamem_map(dmat, dma->dma_segs, dma->dma_nsegs,
	    size, &dma->dma_addr, BUS_DMA_WAITOK | flags);
	if (error)
		goto free;

	error = bus_dmamap_create(dmat, size, dma->dma_nsegs,
	    size, 0, BUS_DMA_WAITOK, &dma->dma_map);
	if (error)
		goto unmap;

	error = bus_dmamap_load(dmat, dma->dma_map, dma->dma_addr,
	    size, NULL, BUS_DMA_WAITOK);
	if (error)
	    goto destroy;

	dma->dma_size = size;
	dma->dma_tag = dmat;

	memset(dma->dma_addr, 0, dma->dma_size);

	return 0;

destroy:
	bus_dmamap_destroy(dmat, dma->dma_map);
unmap:
	bus_dmamem_unmap(dmat, dma->dma_addr, dma->dma_size);
free:
	bus_dmamem_free(dmat, dma->dma_segs, dma->dma_nsegs);

	return error;
}

static void *
wiifb_mapreg(struct wiifb_softc *sc, bus_addr_t base, bus_size_t size)
{
	bus_space_handle_t bsh;
	int error;

	error = bus_space_map(sc->sc_bst, base, size,
	    BUS_SPACE_MAP_LINEAR, &bsh);
	if (error != 0) {
		panic("couldn't map 0x%x", base);
	}
	return bus_space_vaddr(sc->sc_bst, bsh);
}

static void
wiifb_set_wgpipe(bus_addr_t base)
{
	uint32_t value;

	if (base) {
		mtspr(IBM750CL_SPR_WPAR, base);
	}
	value = mfspr(IBM750CL_SPR_HID2);
	if (base) {
		value |= IBM750CL_SPR_HID2_WPE;
	} else {
		value &= ~IBM750CL_SPR_HID2_WPE;
	}
	mtspr(IBM750CL_SPR_HID2, value);
}

static void
wiifb_ppcsync(void)
{
	uint32_t value;

	value = mfspr(SPR_HID0);
	mtspr(SPR_HID0, value | 0x8);
	asm volatile("isync" ::: "memory");
	asm volatile("sync" ::: "memory");
	mtspr(SPR_HID0, value);
}

static void
wiifb_gx_bp_load(struct wiifb_softc *sc, uint32_t data)
{
	GX_STRICT_ORDER(sc->sc_wgpipe->u8 = 0x61);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = data);
}

static void
wiifb_gx_cp_load(struct wiifb_softc *sc, uint8_t addr, uint32_t data)
{
	GX_STRICT_ORDER(sc->sc_wgpipe->u8 = 0x08);
	GX_STRICT_ORDER(sc->sc_wgpipe->u8 = addr);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = data);
}

static void
wiifb_gx_xf_load(struct wiifb_softc *sc, uint16_t addr, uint32_t data)
{
	GX_STRICT_ORDER(sc->sc_wgpipe->u8 = 0x10);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = addr);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = data);
}

static void
wiifb_gx_xf_load_multi(struct wiifb_softc *sc, uint16_t addr,
    uint16_t count, uint32_t *data)
{
	uint16_t n;

	GX_STRICT_ORDER(sc->sc_wgpipe->u8 = 0x10);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 =
			(((uint32_t)count - 1) << 16) | addr);
	for (n = 0; n < count; n++) {
		GX_STRICT_ORDER(sc->sc_wgpipe->u32 = data[n]);
	}
}

static void
wiifb_gx_set_viewport(struct wiifb_softc *sc, u_int w, u_int h)
{
	uint32_t data[6];

	KASSERT(w == 640);
	KASSERT(h == 480);

	data[0] = 0x00000140;
	data[1] = 0xffffff10;
	data[2] = 0x00ffffff;
	data[3] = 0x00000296;
	data[4] = 0x00000246;
	data[5] = 0x00ffffff;

	wiifb_gx_xf_load_multi(sc, GX_XF_VIEWPORT_X0,
	    __arraycount(data), data);
}

static void
wiifb_gx_set_scissor(struct wiifb_softc *sc, u_int x, u_int y,
    u_int w, u_int h)
{
	uint32_t xo = x + 0x156;
	uint32_t yo = y + 0x156;
	uint32_t wo = xo + w - 1;
	uint32_t ho = yo + h - 1;

	wiifb_gx_bp_load(sc, 0x20000000 | yo | (xo << 12));
	wiifb_gx_bp_load(sc, 0x21000000 | ho | (wo << 12));
	wiifb_gx_bp_load(sc, 0x59000000 | GX_XY(xo >> 1, yo >> 1));
}

static void
wiifb_gx_init(struct wiifb_softc *sc)
{
	const uint32_t fifo_start = sc->sc_fifo.dma_segs[0].ds_addr;
	const uint32_t fifo_end = fifo_start + sc->sc_fifo.dma_size - 4;
	const uint32_t fifo_hiwat = GX_FIFO_HIWAT(sc->sc_fifo.dma_size);
	const uint32_t fifo_lowat = GX_FIFO_LOWAT(sc->sc_fifo.dma_size);

	/* Disable WGPIPE and unlink CP FIFO before changing settings. */
	wiifb_set_wgpipe(0);
	CP_WRITE(sc, CP_CR, 0);
	CP_WRITE(sc, CP_CLEAR, CP_CLEAR_UNDERFLOW | CP_CLEAR_OVERFLOW);

	/* Setup GP FIFO */
	CP_WRITE(sc, CP_FIFO_BASE_LO, LOWER_16_BITS(fifo_start));
	CP_WRITE(sc, CP_FIFO_BASE_HI, UPPER_16_BITS(fifo_start));
	CP_WRITE(sc, CP_FIFO_END_LO, LOWER_16_BITS(fifo_end));
	CP_WRITE(sc, CP_FIFO_END_HI, UPPER_16_BITS(fifo_end));
	CP_WRITE(sc, CP_FIFO_HIWAT_LO, LOWER_16_BITS(fifo_hiwat));
	CP_WRITE(sc, CP_FIFO_HIWAT_HI, UPPER_16_BITS(fifo_hiwat));
	CP_WRITE(sc, CP_FIFO_LOWAT_LO, LOWER_16_BITS(fifo_lowat));
	CP_WRITE(sc, CP_FIFO_LOWAT_HI, UPPER_16_BITS(fifo_lowat));
	CP_WRITE(sc, CP_FIFO_RW_DIST_LO, 0);
	CP_WRITE(sc, CP_FIFO_RW_DIST_HI, 0);
	CP_WRITE(sc, CP_FIFO_WRITE_PTR_LO, LOWER_16_BITS(fifo_start));
	CP_WRITE(sc, CP_FIFO_WRITE_PTR_HI, UPPER_16_BITS(fifo_start));
	CP_WRITE(sc, CP_FIFO_READ_PTR_LO, LOWER_16_BITS(fifo_start));
	CP_WRITE(sc, CP_FIFO_READ_PTR_HI, UPPER_16_BITS(fifo_start));
	wiifb_ppcsync();

	/* Setup CPU FIFO */
	PI_WRITE(sc, PI_FIFO_BASE_START, fifo_start);
	PI_WRITE(sc, PI_FIFO_BASE_END, fifo_end);
	PI_WRITE(sc, PI_FIFO_WRITE_PTR, fifo_start);
	wiifb_ppcsync();

	/* Link CP/PE FIFO and enable GP FIFO */
	CP_WRITE(sc, CP_CR, CP_CR_GP_LINK_ENABLE);
	CP_WRITE(sc, CP_CR, CP_READ(sc, CP_CR) | CP_CR_READ_ENABLE);

	/* Init pixel engine */
	PE_WRITE(sc, PE_ZCONF,
	    PE_ZCONF_UPD_ENABLE |
	    PE_ZCONF_FUNC_ALWAYS |
	    PE_ZCONF_COMP_ENABLE);
	PE_WRITE(sc, PE_ALPHA_CONF,
	    PE_ALPHA_CONF_OP_SET |
	    PE_ALPHA_CONF_SRC_1 |
	    PE_ALPHA_CONF_DST_0 |
	    PE_ALPHA_CONF_UPD_A |
	    PE_ALPHA_CONF_UPD_C);
	PE_WRITE(sc, PE_ALPHA_DEST, 0);
	PE_WRITE(sc, PE_ALPHA_MODE, PE_ALPHA_MODE_ALWAYS);
	PE_WRITE(sc, PE_ALPHA_READ,
	    PE_ALPHA_READ_UNK | PE_ALPHA_READ_FF);

	/* Enable WG pipe */
	wiifb_set_wgpipe(WGPIPE_BASE);

	/* Sanitize command processor registers */
	for (int n = 0; n < 8; n++) {
		wiifb_gx_cp_load(sc, 0x80 + n, 0x80000000);
	}
        wiifb_gx_cp_load(sc, 0x20, 0);

	/* Sanitize transform unit registers */
        wiifb_gx_xf_load(sc, 0x1000, 0x3f);
        wiifb_gx_xf_load(sc, 0x1005, 0x01);
        wiifb_gx_xf_load(sc, 0x1012, 0x01);
        wiifb_gx_xf_load(sc, 0x1006, 0);

	/* Initialize blitting processor */
        wiifb_gx_bp_load(sc, 0x00000001);
        wiifb_gx_bp_load(sc, 0x01666666);
        wiifb_gx_bp_load(sc, 0x02666666);
        wiifb_gx_bp_load(sc, 0x03666666);
        wiifb_gx_bp_load(sc, 0x04666666);
        wiifb_gx_bp_load(sc, 0x22000606);
        wiifb_gx_bp_load(sc, 0x23000000);
        wiifb_gx_bp_load(sc, 0x24000000);
        wiifb_gx_bp_load(sc, 0x42000000);
        wiifb_gx_bp_load(sc, 0x44000003);
        wiifb_gx_bp_load(sc, 0x43000000);
        wiifb_gx_bp_load(sc, 0x53595000);
        wiifb_gx_bp_load(sc, 0x54000015);
        wiifb_gx_bp_load(sc, 0x550003ff);
        wiifb_gx_bp_load(sc, 0x560003ff);
        wiifb_gx_bp_load(sc, 0x5800000f);
        wiifb_gx_bp_load(sc, 0x67000000);

	/* Set viewport and scissor parameters */
	wiifb_gx_set_viewport(sc, WIIFB_RGB_WIDTH, WIIFB_RGB_HEIGHT);
	wiifb_gx_set_scissor(sc, 0, 0, WIIFB_RGB_WIDTH, WIIFB_RGB_HEIGHT);

	wiifb_gx_bp_load(sc, 0x4000001f);

	if (sc->sc_curmode->height == 574) {
		/* Scale 480 lines to PAL display height */
		wiifb_gx_bp_load(sc, 0x4e000127);
	}

	/* Copy mode parameters */
	wiifb_gx_bp_load(sc, 0x410004bc);

	/* Copy source */
	wiifb_gx_bp_load(sc, 0x49000000 | GX_XY(0, 0));
	wiifb_gx_bp_load(sc, 0x4a000000 |
	    GX_XY(WIIFB_RGB_WIDTH - 1, WIIFB_RGB_HEIGHT - 1));

	/* Copy destination */
	wiifb_gx_bp_load(sc, 0x4d000000 | (WIIFB_RGB_WIDTH >> 4));

	/* XFB address */
	wiifb_gx_bp_load(sc, 0x4b000000 | (XFB_START >> 5));

	/* Copy clear settings */
	wiifb_gx_bp_load(sc, 0x4f000000);
	wiifb_gx_bp_load(sc, 0x50000000);
	wiifb_gx_bp_load(sc, 0x5100ffff);
}

static void
wiifb_gx_flush(struct wiifb_softc *sc)
{
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	GX_STRICT_ORDER(sc->sc_wgpipe->u32 = 0);
	wiifb_ppcsync();
}

static void
wiifb_accel_init(struct wiifb_softc *sc)
{
	bus_size_t rgb_size;
	bus_size_t fifo_size;

	if (wiifb_vi_init(sc) != 0) {
		panic("couldn't init VI");
	}

	rgb_size = WIIFB_RGB_WIDTH * WIIFB_RGB_HEIGHT;
	rgb_size = rgb_size * WIIFB_RGB_BPP / NBBY;
	rgb_size = roundup(rgb_size, PAGE_SIZE);
	if (wiifb_dma_alloc(sc, rgb_size, PAGE_SIZE, BUS_DMA_PREFETCHABLE,
			    &sc->sc_rgb) != 0) {
		panic("couldn't alloc rgb fb");
	}

	fifo_size = WIIFB_FIFO_SIZE;
	if (wiifb_dma_alloc(sc, fifo_size, GX_FIFO_ALIGN, BUS_DMA_NOCACHE,
	    		    &sc->sc_fifo) != 0) {
		panic("couldn't alloc gx fifo");
	}

	sc->sc_efb = wiifb_mapreg(sc, EFB_BASE, EFB_SIZE);
	sc->sc_cp = wiifb_mapreg(sc, CP_BASE, CP_SIZE);
	sc->sc_pe = wiifb_mapreg(sc, PE_BASE, PE_SIZE);
	sc->sc_pi = wiifb_mapreg(sc, PI_BASE, PI_SIZE);
	sc->sc_wgpipe = wiifb_mapreg(sc, WGPIPE_BASE, WGPIPE_SIZE);

	wiifb_gx_init(sc);
};

static void
wiifb_rgb_to_efb(struct wiifb_softc *sc)
{
	u_int y;
	uint32_t *src = sc->sc_rgb.dma_addr;
	uint32_t *dst = __UNVOLATILE(sc->sc_efb);
	register_t hid0 = mfspr(SPR_HID0);

	KASSERT(src != NULL);
	KASSERT(dst != NULL);

	/* Disable store gathering while writing to EFB. */
	mtspr(SPR_HID0, hid0 & ~HID0_SGE);

	for (y = 0; y < WIIFB_RGB_HEIGHT; y++) {
		memcpy(dst, src, WIIFB_RGB_WIDTH * 4);
		src += WIIFB_RGB_WIDTH;
		dst += 1024;
	}

	/* Re-enable store gathering. */
	mtspr(SPR_HID0, hid0);
}

static void
wiifb_efb_to_xfb(struct wiifb_softc *sc)
{
	const uint32_t copy_mask = sc->sc_curmode->height == 574 ? 0x400 : 0x0;

	/* Execute copy to XFB */
	wiifb_gx_bp_load(sc, 0x52004803 | copy_mask);
}

static void
wiifb_gx_draw_done(struct wiifb_softc *sc, uint16_t token)
{
	/* Draw done */
	wiifb_gx_bp_load(sc, 0x45000002);
	/* Write tokens */
        wiifb_gx_bp_load(sc, 0x48000000 | token);
        wiifb_gx_bp_load(sc, 0x47000000 | token);
	/* Flush WG pipe */
	wiifb_gx_flush(sc);
}

static void
wiifb_vi_refresh(void *priv)
{
	struct wiifb_softc *sc = priv;

	wiifb_rgb_to_efb(sc);
	wiifb_efb_to_xfb(sc);
	wiifb_gx_draw_done(sc, sc->sc_token++);
}

static int
wiifb_vi_intr(void *priv)
{
	struct wiifb_softc *sc = priv;
	uint32_t di0;
	int ret = 0;

	di0 = RD4(sc, VI_DI0);

	WR4(sc, VI_DI0, RD4(sc, VI_DI0) & ~VI_DI_INT);
	WR4(sc, VI_DI1, RD4(sc, VI_DI1) & ~VI_DI_INT);
	WR4(sc, VI_DI2, RD4(sc, VI_DI2) & ~VI_DI_INT);
	WR4(sc, VI_DI3, RD4(sc, VI_DI3) & ~VI_DI_INT);

	if ((di0 & VI_DI_INT) != 0 &&
	    sc->sc_wsmode != WSDISPLAYIO_MODE_EMUL) {
		wiifb_vi_refresh(sc);
		ret = 1;
	}

	return ret;
}

static void
wiifb_init(struct wiifb_softc *sc)
{
	uint16_t dcr;
	uint16_t visel;

	/* Read current display format and interlaced settings. */
	dcr = RD2(sc, VI_DCR);
	if ((dcr & VI_DCR_ENB) != 0) {
		sc->sc_format = __SHIFTOUT(dcr, VI_DCR_FMT);
		sc->sc_interlaced = (dcr & VI_DCR_NIN) == 0;
	} else {
		visel = RD2(sc, VI_VISEL);
		sc->sc_format = VI_DCR_FMT_NTSC;
		sc->sc_interlaced = (visel & VI_VISEL_COMPONENT_CABLE) == 0;
	}

	/* Reset video interface. */
	WR2(sc, VI_DCR, VI_DCR_RST);
	delay(1000);

	/* Initialize video format and interlace selector. */
	dcr = __SHIFTIN(sc->sc_format, VI_DCR_FMT) |
	      (sc->sc_interlaced ? 0 : VI_DCR_NIN);
	WR2(sc, VI_DCR, dcr);
}

static void
wiifb_set_mode(struct wiifb_softc *sc, uint8_t format, bool interlaced)
{
	u_int modeidx;
	u_int strides, reads;

	modeidx = WIIFB_MODE_INDEX(format, interlaced);
	if (modeidx == WIIFB_MODE_INDEX(VI_DCR_FMT_NTSC, 1)) {
		/* NTSC 480i Magic numbers from YAGCD. */
		WR2(sc, VI_VTR, 0x0f06);
		WR4(sc, VI_HTR0, 0x476901AD);
		WR4(sc, VI_HTR1, 0x02EA5140);
		WR4(sc, VI_VTO, 0x00030018);
		WR4(sc, VI_VTE, 0x00020019);
		WR4(sc, VI_BBOI, 0x410C410C);
		WR4(sc, VI_BBEI, 0x40ED40ED);
		WR2(sc, VI_DPV, 0x0000);
		WR2(sc, VI_DPH, 0x0000);
	} else if (modeidx == WIIFB_MODE_INDEX(VI_DCR_FMT_NTSC, 0)) {
		/* NTSC 480p */
		WR2(sc, VI_VTR, 0x1e0c);
		WR4(sc, VI_HTR0, 0x476901ad);
		WR4(sc, VI_HTR1, 0x030a4940);
		WR4(sc, VI_VTO, 0x00060030);
		WR4(sc, VI_VTE, 0x00060030);
		WR4(sc, VI_BBOI, 0x81d881d8);
		WR4(sc, VI_BBEI, 0x81d881d8);
		WR2(sc, VI_DPV, 0x0000);
		WR2(sc, VI_DPH, 0x0000);
	} else if (modeidx == WIIFB_MODE_INDEX(VI_DCR_FMT_PAL, 1)) {
		/* PAL 576i */
		WR2(sc, VI_VTR, 0x11f5);
		WR4(sc, VI_HTR0, 0x4b6a01b0);
		WR4(sc, VI_HTR1, 0x02f85640);
		WR4(sc, VI_VTO, 0x00010023);
		WR4(sc, VI_VTE, 0x00000024);
		WR4(sc, VI_BBOI, 0x4d2b4d6d);
		WR4(sc, VI_BBEI, 0x4d8a4d4c);
		WR2(sc, VI_DPV, 0x013c);
		WR2(sc, VI_DPH, 0x0144);
	} else {
		/*
		 * Display mode is not supported. Blink the slot LED to
		 * indicate failure.
		 */
		wii_slot_led_blink(WIIFB_ERROR_BLINK_INTERVAL);
	}

	if (modeidx >= WIIFB_NMODES || wiifb_modes[modeidx].name == NULL) {
		panic("Unsupported format (0x%x) / interlaced (%d) settings",
		    sc->sc_format, sc->sc_interlaced);
	}
	sc->sc_curmode = &wiifb_modes[modeidx];

	/* Filter coefficient table, values from YAGCD. */
	WR4(sc, VI_FCT0, 0x1ae771f0);
	WR4(sc, VI_FCT1, 0x0db4a574);
	WR4(sc, VI_FCT2, 0x00c1188e);
	WR4(sc, VI_FCT3, 0xc4c0cbe2);
	WR4(sc, VI_FCT4, 0xfcecdecf);
	WR4(sc, VI_FCT5, 0x13130f08);
	WR4(sc, VI_FCT6, 0x00080C0f);

	/* Unknown registers. */
	WR4(sc, VI_UNKNOWN_68H, 0x00ff0000);
	WR2(sc, VI_UNKNOWN_76H, 0x00ff);
	WR4(sc, VI_UNKNOWN_78H, 0x00ff00ff);
	WR4(sc, VI_UNKNOWN_7CH, 0x00ff00ff);

	/* Picture configuration */
	strides = (sc->sc_curmode->width * 2) / (interlaced ? 16 : 32);
	reads = (sc->sc_curmode->width * 2) / 32;
	WR2(sc, VI_PICCONF,
	    __SHIFTIN(strides, VI_PICCONF_STRIDES) |
	    __SHIFTIN(reads, VI_PICCONF_READS));

	/* Horizontal scaler configuration */
	if (interlaced) {
		WR2(sc, VI_HSR, __SHIFTIN(256, VI_HSR_STP));
	} else {
		WR2(sc, VI_HSR, __SHIFTIN(244, VI_HSR_STP) | VI_HSR_HS_EN);
	}

	/* Video clock configuration */
	WR2(sc, VI_VICLK,
	    interlaced ? VI_VICLK_SEL_27MHZ : VI_VICLK_SEL_54MHZ);

	/* Horizontal scaling width */
	WR2(sc, VI_HSCALINGW, sc->sc_curmode->width);

	/* Set framebuffer address */
	wiifb_set_fb(sc);

	/* Finally, enable the framebuffer */
	WR2(sc, VI_DCR, RD2(sc, VI_DCR) | VI_DCR_ENB);
}

static void
wiifb_set_fb(struct wiifb_softc *sc)
{
	uint32_t taddr = XFB_START;
	uint32_t baddr = taddr + (sc->sc_interlaced ?
				  sc->sc_curmode->width * 2 : 0);

	WR4(sc, VI_TFBL,
	    VI_TFBL_PGOFF |
	    __SHIFTIN((taddr >> 5), VI_TFBL_FBB) |
	    __SHIFTIN((taddr / 2) & 0xf, VI_TFBL_XOF));
	WR4(sc, VI_TFBR, 0);

	WR4(sc, VI_BFBL,
	    VI_BFBL_PGOFF |
	    __SHIFTIN((baddr >> 5), VI_BFBL_FBB) |
	    __SHIFTIN((baddr / 2) & 0xf, VI_BFBL_XOF));
	WR4(sc, VI_BFBR, 0);
}

static int
wiifb_ioctl(void *v, void *vs, u_long cmd, void *data, int flag, lwp_t *l)
{
	struct wiifb_softc *sc = v;
	struct wsdisplayio_bus_id *busid;
	struct wsdisplayio_fbinfo *fbi;
	u_int video;
	u_int wsmode;

	switch (cmd) {
	case WSDISPLAYIO_GTYPE:
		*(u_int *)data = WSDISPLAY_TYPE_GENFB;
		return 0;
	case WSDISPLAYIO_GET_BUSID:
		busid = data;
		busid->bus_type = WSDISPLAYIO_BUS_SOC;
		return 0;
	case WSDISPLAYIO_GET_FBINFO:
		fbi = data;
		/*
		 * rasops info does not match the pixel encoding due to our
		 * devcmap, so fill out fbinfo manually instead of relying
		 * on wsdisplayio_get_fbinfo.
		 */
		fbi->fbi_fboffset = 0;
		fbi->fbi_width = WIIFB_RGB_WIDTH;
		fbi->fbi_height = WIIFB_RGB_HEIGHT;
		fbi->fbi_bitsperpixel = WIIFB_RGB_BPP;
		fbi->fbi_stride = fbi->fbi_width * fbi->fbi_bitsperpixel / 8;
		fbi->fbi_fbsize = fbi->fbi_height * fbi->fbi_stride;
		fbi->fbi_pixeltype = WSFB_RGB;
		fbi->fbi_flags = WSFB_VRAM_IS_RAM;
		return 0;

	case WSDISPLAYIO_SVIDEO:
		video = *(u_int *)data;
		switch (video) {
		case WSDISPLAYIO_VIDEO_OFF:
			out32(HW_VIDIM, __SHIFTIN(7, VIDIM_Y) |
					__SHIFTIN(7, VIDIM_C) |
					VIDIM_E);
			return 0;
		case WSDISPLAYIO_VIDEO_ON:
			out32(HW_VIDIM, 0);
			return 0;
		default:
			return EINVAL;
		}

	case WSDISPLAYIO_SMODE:
		wsmode = *(u_int *)data;
		if (wsmode != WSDISPLAYIO_MODE_EMUL) {
			/* Blank the RGB FB when leaving text mode */
			memset(sc->sc_rgb.dma_addr, 0, sc->sc_rgb.dma_size);
		}
		if (sc->sc_wsmode != wsmode) {
			sc->sc_wsmode = wsmode;

			if (wsmode == WSDISPLAYIO_MODE_EMUL) {
				wiifb_clear_xfb(sc);
			}
		}
		return EPASSTHROUGH;
	}

	return EPASSTHROUGH;
}

static paddr_t
wiifb_mmap(void *v, void *vs, off_t off, int prot)
{
	struct wiifb_softc *sc = v;

	if (sc->sc_wsmode == WSDISPLAYIO_MODE_EMUL || sc->sc_efb == NULL) {
		return -1;
	}

	if (off < 0 || off >= sc->sc_rgb.dma_size) {
		return -1;
	}

	return bus_dmamem_mmap(sc->sc_rgb.dma_tag,
	    sc->sc_rgb.dma_segs, sc->sc_rgb.dma_nsegs,
	    off, prot, BUS_DMA_PREFETCHABLE);
}