/*	$OpenBSD: udl.c,v 1.60 2010/07/29 12:37:16 maja Exp $ */

/*
 * Copyright (c) 2009 Marcus Glocker <mglocker@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * Driver for the ``DisplayLink DL-120 / DL-160'' graphic chips based
 * on the reversed engineered specifications of Florian Echtler
 * <floe@butterbrot.org>:
 *
 * 	http://floe.butterbrot.org/displaylink/doku.php
 *
 * This driver has been inspired by the cfxga(4) driver because we have
 * to deal with similar challenges, like no direct access to the video
 * memory.
 */

#include <sys/param.h>
#include <sys/device.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/proc.h>
#include <uvm/uvm.h>

#include <machine/bus.h>

#include <dev/usb/usb.h>
#include <dev/usb/usbdi.h>
#include <dev/usb/usbdi_util.h>
#include <dev/usb/usbdevs.h>

#include <dev/wscons/wsconsio.h>
#include <dev/wscons/wsdisplayvar.h>
#include <dev/rasops/rasops.h>

#include <dev/videomode/videomode.h>
#include <dev/videomode/edidvar.h>

#include <dev/usb/udl.h>
#include <dev/usb/udlio.h>

/*
 * Defines.
 */
#if 0
#define UDL_DEBUG
#endif
#ifdef UDL_DEBUG
int udl_debug = 1;
#define DPRINTF(l, x...) do { if ((l) <= udl_debug) printf(x); } while (0)
#else
#define DPRINTF(l, x...)
#endif

#define DN(sc)		((sc)->sc_dev.dv_xname)
#define FUNC		__func__

/*
 * Prototypes.
 */
int		udl_match(struct device *, void *, void *);
void		udl_attach(struct device *, struct device *, void *);
void		udl_attach_hook(void *);
int		udl_detach(struct device *, int);
int		udl_activate(struct device *, int);

int		udl_ioctl(void *, u_long, caddr_t, int, struct proc *);
paddr_t		udl_mmap(void *, off_t, int);
int		udl_alloc_screen(void *, const struct wsscreen_descr *,
		    void **, int *, int *, long *);
void		udl_free_screen(void *, void *);
int		udl_show_screen(void *, void *, int,
		    void (*)(void *, int, int), void *);
void		udl_burner(void *, u_int, u_int);

int		udl_copycols(void *, int, int, int, int);
int		udl_copyrows(void *, int, int, int);
int		udl_erasecols(void *, int, int, int, long);
int		udl_eraserows(void *, int, int, long);
int		udl_putchar(void *, int, int, u_int, long);
int		udl_do_cursor(struct rasops_info *);
int		udl_draw_char(struct udl_softc *, uint16_t, uint16_t, u_int,
		    uint32_t, uint32_t);
int		udl_damage(struct udl_softc *, uint8_t *,
		    uint32_t, uint32_t, uint32_t, uint32_t);
int		udl_draw_image(struct udl_softc *, uint8_t *,
		    uint32_t, uint32_t, uint32_t, uint32_t);

usbd_status	udl_ctrl_msg(struct udl_softc *, uint8_t, uint8_t,
		    uint16_t, uint16_t, uint8_t *, size_t);
usbd_status	udl_poll(struct udl_softc *, uint32_t *);
usbd_status	udl_read_1(struct udl_softc *, uint16_t, uint8_t *);
usbd_status	udl_write_1(struct udl_softc *, uint16_t, uint8_t);
usbd_status	udl_read_edid(struct udl_softc *, uint8_t *);
uint8_t		udl_lookup_mode(uint16_t, uint16_t, uint8_t, uint16_t,
		    uint32_t);
int		udl_select_chip(struct udl_softc *);
usbd_status	udl_set_enc_key(struct udl_softc *, uint8_t *, uint8_t);
usbd_status	udl_set_decomp_table(struct udl_softc *, uint8_t *, uint16_t);

int		udl_load_huffman(struct udl_softc *);
void		udl_free_huffman(struct udl_softc *);
int		udl_fbmem_alloc(struct udl_softc *);
void		udl_fbmem_free(struct udl_softc *);
usbd_status	udl_cmd_alloc_xfer(struct udl_softc *);
void		udl_cmd_free_xfer(struct udl_softc *);
int		udl_cmd_alloc_buf(struct udl_softc *);
void		udl_cmd_free_buf(struct udl_softc *);
void		udl_cmd_insert_int_1(struct udl_softc *, uint8_t);
void		udl_cmd_insert_int_2(struct udl_softc *, uint16_t);
void		udl_cmd_insert_int_3(struct udl_softc *, uint32_t);
void		udl_cmd_insert_int_4(struct udl_softc *, uint32_t);
void		udl_cmd_insert_buf(struct udl_softc *, uint8_t *, uint32_t);
int		udl_cmd_insert_buf_comp(struct udl_softc *, uint8_t *,
		    uint32_t);
int		udl_cmd_insert_head_comp(struct udl_softc *, uint32_t);
int		udl_cmd_insert_check(struct udl_softc *, int);
void		udl_cmd_set_xfer_type(struct udl_softc *, int);
void		udl_cmd_save_offset(struct udl_softc *);
void		udl_cmd_restore_offset(struct udl_softc *);
void		udl_cmd_write_reg_1(struct udl_softc *, uint8_t, uint8_t);
void		udl_cmd_write_reg_3(struct udl_softc *, uint8_t, uint32_t);
usbd_status	udl_cmd_send(struct udl_softc *);
usbd_status	udl_cmd_send_async(struct udl_softc *);
void		udl_cmd_send_async_cb(usbd_xfer_handle, usbd_private_handle,
		    usbd_status);

usbd_status	udl_init_chip(struct udl_softc *);
void		udl_init_fb_offsets(struct udl_softc *, uint32_t, uint32_t,
		    uint32_t, uint32_t);
usbd_status	udl_init_resolution(struct udl_softc *);
usbd_status	udl_clear_screen(struct udl_softc *);
void		udl_select_mode(struct udl_softc *);
int		udl_fb_buf_write(struct udl_softc *, uint8_t *, uint32_t,
		    uint32_t, uint16_t);
int		udl_fb_block_write(struct udl_softc *, uint16_t, uint32_t,
		    uint32_t, uint32_t, uint32_t);
int		udl_fb_line_write(struct udl_softc *, uint16_t, uint32_t,
		    uint32_t, uint32_t);
int		udl_fb_off_write(struct udl_softc *, uint16_t, uint32_t,
		    uint16_t);
int		udl_fb_block_copy(struct udl_softc *, uint32_t, uint32_t,
		    uint32_t, uint32_t, uint32_t, uint32_t);
int		udl_fb_line_copy(struct udl_softc *, uint32_t, uint32_t,
		    uint32_t, uint32_t, uint32_t);
int		udl_fb_off_copy(struct udl_softc *, uint32_t, uint32_t,
		    uint16_t);
int		udl_fb_buf_write_comp(struct udl_softc *, uint8_t *, uint32_t,
		    uint32_t, uint16_t);
int		udl_fb_block_write_comp(struct udl_softc *, uint16_t, uint32_t,
		    uint32_t, uint32_t, uint32_t);
int		udl_fb_line_write_comp(struct udl_softc *, uint16_t, uint32_t,
		    uint32_t, uint32_t);
int		udl_fb_off_write_comp(struct udl_softc *, uint16_t, uint32_t,
		    uint16_t);
int		udl_fb_block_copy_comp(struct udl_softc *, uint32_t, uint32_t,
		    uint32_t, uint32_t, uint32_t, uint32_t);
int		udl_fb_line_copy_comp(struct udl_softc *, uint32_t, uint32_t,
		    uint32_t, uint32_t, uint32_t);
int		udl_fb_off_copy_comp(struct udl_softc *, uint32_t, uint32_t,
		    uint16_t);
#ifdef UDL_DEBUG
void		udl_hexdump(void *, int, int);
usbd_status	udl_init_test(struct udl_softc *);
#endif

/*
 * Driver glue.
 */
struct cfdriver udl_cd = {
	NULL, "udl", DV_DULL
};

const struct cfattach udl_ca = {
	sizeof(struct udl_softc),
	udl_match,
	udl_attach,
	udl_detach,
	udl_activate
};

/*
 * wsdisplay glue.
 */
struct wsscreen_descr udl_stdscreen = {
	"std",			/* name */
	0, 0,			/* ncols, nrows */
	NULL,			/* textops */
	0, 0,			/* fontwidth, fontheight */
	WSSCREEN_WSCOLORS	/* capabilities */
};

const struct wsscreen_descr *udl_scrlist[] = {
	&udl_stdscreen
};

struct wsscreen_list udl_screenlist = {
	sizeof(udl_scrlist) / sizeof(struct wsscreen_descr *), udl_scrlist
};

struct wsdisplay_accessops udl_accessops = {
	udl_ioctl,
	udl_mmap,
	udl_alloc_screen,
	udl_free_screen,
	udl_show_screen,
	NULL,
	NULL,
	NULL,
	udl_burner
};

/*
 * Matching devices.
 */
struct udl_type {
	struct usb_devno	udl_dev;
	uint16_t		udl_chip;
};

static const struct udl_type udl_devs[] = {
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_LCD4300U },	DL120 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_LCD8000U },	DL120 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_GUC2020 },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_LD220 },	DL165 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_VCUD60 },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_DLDVI },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_VGA10 },	DL120 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_WSDVI },	DLUNK },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_EC008 },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_HPDOCK },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_NL571 },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_M01061 },	DL195 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_NBDOCK },	DL165 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_SWDVI },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_UM7X0 },	DL120 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_CONV },	DL160 },
	{ { USB_VENDOR_DISPLAYLINK, USB_PRODUCT_DISPLAYLINK_LUM70 },	DL125 }
};
#define udl_lookup(v, p) ((struct udl_type *)usb_lookup(udl_devs, v, p))

int
udl_match(struct device *parent, void *match, void *aux)
{
	struct usb_attach_arg *uaa = aux;

	if (uaa->iface != NULL)
		return (UMATCH_NONE);

	if (udl_lookup(uaa->vendor, uaa->product) != NULL)
		return (UMATCH_VENDOR_PRODUCT);

	return (UMATCH_NONE);
}

void
udl_attach(struct device *parent, struct device *self, void *aux)
{
	struct udl_softc *sc = (struct udl_softc *)self;
	struct usb_attach_arg *uaa = aux;
	struct wsemuldisplaydev_attach_args aa;
	usbd_status error;
	int err, i;

	sc->sc_udev = uaa->device;
	sc->sc_chip = udl_lookup(uaa->vendor, uaa->product)->udl_chip;
	sc->sc_width = 0;
	sc->sc_height = 0;
	sc->sc_depth = 16;
	sc->sc_cur_mode = MAX_DL_MODES;

	/*
	 * Override chip if requested.
	 */
	if ((sc->sc_dev.dv_cfdata->cf_flags & 0xff00) > 0) {
		i = ((sc->sc_dev.dv_cfdata->cf_flags & 0xff00) >> 8) - 1;
		if (i <= DLMAX) {
			sc->sc_chip = i;
			printf("%s: %s: cf_flags (0x%04x) forced chip to %d\n",
			    DN(sc), FUNC,
			    sc->sc_dev.dv_cfdata->cf_flags, i);
		}
	}

	/*
	 * The product might have more than one chip
	 */
	if (sc->sc_chip == DLUNK)
		if (udl_select_chip(sc))
			return;

	/*
	 * Set device configuration descriptor number.
	 */
	error = usbd_set_config_no(sc->sc_udev, 1, 0);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Create device handle to interface descriptor.
	 */
	error = usbd_device2interface_handle(sc->sc_udev, 0, &sc->sc_iface);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Allocate bulk command xfer.
	 */
	error = udl_cmd_alloc_xfer(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Allocate command buffer.
	 */
	err = udl_cmd_alloc_buf(sc);
	if (err != 0)
		return;

	/*
	 * Open bulk TX pipe.
	 */
	error = usbd_open_pipe(sc->sc_iface, 0x01, USBD_EXCLUSIVE_USE,
	    &sc->sc_tx_pipeh);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Device initialization is done per synchronous xfers.
	 */
	udl_cmd_set_xfer_type(sc, UDL_CMD_XFER_SYNC);

	/*
	 * Initialize chip.
	 */
	error = udl_init_chip(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Select edid mode.
	 */
	udl_select_mode(sc);

	/*
	 * Override mode if requested.
	 */
	if ((sc->sc_dev.dv_cfdata->cf_flags & 0xff) > 0) {
		i = (sc->sc_dev.dv_cfdata->cf_flags & 0xff) - 1;

		if (i < MAX_DL_MODES) {
			if (udl_modes[i].chip <= sc->sc_chip) {
				sc->sc_width = udl_modes[i].hdisplay;
				sc->sc_height = udl_modes[i].vdisplay;
				printf("%s: %s: cf_flags (0x%04x) ",
				    DN(sc), FUNC,
				    sc->sc_dev.dv_cfdata->cf_flags);
				printf("forced mode to %d\n", i);
				sc->sc_cur_mode = i;
			}
		}
	}

	error = udl_init_resolution(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return;

	/*
	 * Attach wsdisplay.
	 */
	aa.console = 0;
	aa.scrdata = &udl_screenlist;
	aa.accessops = &udl_accessops;
	aa.accesscookie = sc;
	aa.defaultscreens = 0;

	sc->sc_wsdisplay = config_found(self, &aa, wsemuldisplaydevprint);

	usbd_add_drv_event(USB_EVENT_DRIVER_ATTACH, sc->sc_udev, &sc->sc_dev);

	/*
	 * Load Huffman table.
	 */
	if (rootvp == NULL)
		mountroothook_establish(udl_attach_hook, sc);
	else
		udl_attach_hook(sc);
}

void
udl_attach_hook(void *arg)
{
	struct udl_softc *sc = arg;

	if (udl_load_huffman(sc) != 0) {
		/* compression not possible */
		printf("%s: run in uncompressed mode\n", DN(sc));
		sc->udl_fb_buf_write = udl_fb_buf_write;
		sc->udl_fb_block_write = udl_fb_block_write;
		sc->udl_fb_line_write = udl_fb_line_write;
		sc->udl_fb_off_write = udl_fb_off_write;
		sc->udl_fb_block_copy = udl_fb_block_copy;
		sc->udl_fb_line_copy = udl_fb_line_copy;
		sc->udl_fb_off_copy = udl_fb_off_copy;
	} else {
		/* compression possible */
		sc->udl_fb_buf_write = udl_fb_buf_write_comp;
		sc->udl_fb_block_write = udl_fb_block_write_comp;
		sc->udl_fb_line_write = udl_fb_line_write_comp;
		sc->udl_fb_off_write = udl_fb_off_write_comp;
		sc->udl_fb_block_copy = udl_fb_block_copy_comp;
		sc->udl_fb_line_copy = udl_fb_line_copy_comp;
		sc->udl_fb_off_copy = udl_fb_off_copy_comp;
	}
#ifdef UDL_DEBUG
	if (udl_debug >= 4)
		udl_init_test(sc);
#endif
	/*
	 * From this point on we do asynchronous xfers.
	 */
	udl_cmd_set_xfer_type(sc, UDL_CMD_XFER_ASYNC);

	/*
	 * Set initial wsdisplay emulation mode.
	 */
	sc->sc_mode = WSDISPLAYIO_MODE_EMUL;
}

int
udl_detach(struct device *self, int flags)
{
	struct udl_softc *sc = (struct udl_softc *)self;

	/*
	 * Close bulk TX pipe.
	 */
	if (sc->sc_tx_pipeh != NULL) {
		usbd_abort_pipe(sc->sc_tx_pipeh);
		usbd_close_pipe(sc->sc_tx_pipeh);
	}

	/*
	 * Free command buffer.
	 */
	udl_cmd_free_buf(sc);

	/*
	 * Free command xfer.
	 */
	udl_cmd_free_xfer(sc);

	/*
	 * Free Huffman table.
	 */
	udl_free_huffman(sc);

	/*
	 * Free framebuffer memory.
	 */
	udl_fbmem_free(sc);

	/*
	 * Detach wsdisplay.
	 */
	if (sc->sc_wsdisplay != NULL)
		config_detach(sc->sc_wsdisplay, DETACH_FORCE);

	usbd_add_drv_event(USB_EVENT_DRIVER_DETACH, sc->sc_udev, &sc->sc_dev);

	return (0);
}

int
udl_activate(struct device *self, int act)
{
	switch (act) {
	case DVACT_ACTIVATE:
		break;
	case DVACT_DEACTIVATE:
		break;
	}

	return (0);
}

/* ---------- */

int
udl_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
{
	struct udl_softc *sc;
	struct wsdisplay_fbinfo *wdf;
	struct udl_ioctl_damage *d;
	int r, error, mode;

	sc = v;

	DPRINTF(1, "%s: %s: ('%c', %d, %d)\n",
	    DN(sc), FUNC, IOCGROUP(cmd), cmd & 0xff, IOCPARM_LEN(cmd));

	switch (cmd) {
	case WSDISPLAYIO_GTYPE:
		*(u_int *)data = WSDISPLAY_TYPE_DL;
		break;
	case WSDISPLAYIO_GINFO:
		wdf = (struct wsdisplay_fbinfo *)data;
		wdf->height = sc->sc_height;
		wdf->width = sc->sc_width;
		wdf->depth = sc->sc_depth;
		wdf->cmsize = 0;	/* XXX fill up colormap size */
		break;
	case WSDISPLAYIO_SMODE:
		mode = *(u_int *)data;
		if (mode == sc->sc_mode)
			break;
		switch (mode) {
		case WSDISPLAYIO_MODE_EMUL:
			/* clear screen */
			(void)udl_clear_screen(sc);
			break;
		case WSDISPLAYIO_MODE_DUMBFB:
			/* TODO */
			break;
		default:
			return (EINVAL);
		}
		sc->sc_mode = mode;
		break;
	case WSDISPLAYIO_LINEBYTES:
		*(u_int *)data = sc->sc_width * (sc->sc_depth / 8);
		break;
	case WSDISPLAYIO_SVIDEO:
	case WSDISPLAYIO_GVIDEO:
		/* handled for us by wscons */
		break;
	case UDLIO_DAMAGE:
		d = (struct udl_ioctl_damage *)data;
		d->status = UDLIO_STATUS_OK;
		r = udl_damage(sc, sc->sc_fbmem, d->x1, d->x2, d->y1, d->y2);
		if (r != 0) {
			error = tsleep(sc, 0, "udlio", hz / 100);
			if (error) {
				d->status = UDLIO_STATUS_FAILED;
			} else {
				r = udl_damage(sc, sc->sc_fbmem, d->x1, d->x2,
				    d->y1, d->y2);
				if (r != 0)
					d->status = UDLIO_STATUS_FAILED;
			}
		}
		break;
	default:
		return (-1);
	}

	return (0);
}

paddr_t
udl_mmap(void *v, off_t off, int prot)
{
	struct udl_softc *sc;
	caddr_t p;
	paddr_t pa;

	sc = v;

	DPRINTF(1, "%s: %s\n", DN(sc), FUNC);

	/* allocate framebuffer memory */
	if (udl_fbmem_alloc(sc) == -1)
		return (-1);

	/* return memory address to userland process */
	p = sc->sc_fbmem + off;
	if (pmap_extract(pmap_kernel(), (vaddr_t)p, &pa) == FALSE) {
		printf("udl_mmap: invalid page\n");
		udl_fbmem_free(sc);
		return (-1);
	}
#if defined(__powerpc__) || defined(__sparc64__)
	return (pa);
#else
	return (atop(pa));
#endif
}

int
udl_alloc_screen(void *v, const struct wsscreen_descr *type,
    void **cookiep, int *curxp, int *curyp, long *attrp)
{
	struct udl_softc *sc = v;
	struct wsdisplay_font *font;

	DPRINTF(1, "%s: %s\n", DN(sc), FUNC);

	if (sc->sc_nscreens > 0)
		return (ENOMEM);

	/*
	 * Initialize rasops.
	 */
	sc->sc_ri.ri_depth = sc->sc_depth;
	sc->sc_ri.ri_bits = NULL;
	sc->sc_ri.ri_width = sc->sc_width;
	sc->sc_ri.ri_height = sc->sc_height;
	sc->sc_ri.ri_stride = sc->sc_width * sc->sc_height / 8;
	sc->sc_ri.ri_hw = (void *)sc;
	sc->sc_ri.ri_flg = 0;

	/* swap B and R at 16 bpp */
	if (sc->sc_depth == 16) {
		sc->sc_ri.ri_rnum = 5;
		sc->sc_ri.ri_rpos = 11;
		sc->sc_ri.ri_gnum = 6;
		sc->sc_ri.ri_gpos = 5;
		sc->sc_ri.ri_bnum = 5;
		sc->sc_ri.ri_bpos = 0;
	}

	rasops_init(&sc->sc_ri, 100, 200);

	sc->sc_ri.ri_ops.copycols = udl_copycols;
	sc->sc_ri.ri_ops.copyrows = udl_copyrows;
	sc->sc_ri.ri_ops.erasecols = udl_erasecols;
	sc->sc_ri.ri_ops.eraserows = udl_eraserows;
	sc->sc_ri.ri_ops.putchar = udl_putchar;
	sc->sc_ri.ri_do_cursor = udl_do_cursor;

	sc->sc_ri.ri_ops.alloc_attr(&sc->sc_ri, 0, 0, 0, attrp);

	udl_stdscreen.nrows = sc->sc_ri.ri_rows;
	udl_stdscreen.ncols = sc->sc_ri.ri_cols;
	udl_stdscreen.textops = &sc->sc_ri.ri_ops;
	udl_stdscreen.fontwidth = sc->sc_ri.ri_font->fontwidth;
	udl_stdscreen.fontheight = sc->sc_ri.ri_font->fontheight;
	udl_stdscreen.capabilities = sc->sc_ri.ri_caps;

	*cookiep = &sc->sc_ri;
	*curxp = 0;
	*curyp = 0;

	/* allocate character backing store */
	sc->sc_cbs = malloc((sc->sc_ri.ri_rows * sc->sc_ri.ri_cols) *
	    sizeof(*sc->sc_cbs), M_DEVBUF, M_ZERO);
	if (sc->sc_cbs == NULL) {
		printf("%s: can't allocate mem for character backing store!\n",
		    DN(sc));
		return (ENOMEM);
	}

	sc->sc_nscreens++;

	font = sc->sc_ri.ri_font;
	DPRINTF(1, "%s: %s: using font %s (%dx%d)\n",
	    DN(sc), FUNC, font->name, sc->sc_ri.ri_cols, sc->sc_ri.ri_rows);

	return (0);
}

void
udl_free_screen(void *v, void *cookie)
{
	struct udl_softc *sc;

	sc = v;

	DPRINTF(1, "%s: %s\n", DN(sc), FUNC);

	/* free character backing store */
	if (sc->sc_cbs != NULL)
		free(sc->sc_cbs, M_DEVBUF);

	sc->sc_nscreens--;
}

int
udl_show_screen(void *v, void *cookie, int waitok,
    void (*cb)(void *, int, int), void *cbarg)
{
	struct udl_softc *sc;

	sc = v;

	DPRINTF(1, "%s: %s\n", DN(sc), FUNC);

	return (0);
}

void
udl_burner(void *v, u_int on, u_int flags)
{
	struct udl_softc *sc;

	sc = v;

	DPRINTF(1, "%s: %s: screen %s\n", DN(sc), FUNC, on ? "ON" : "OFF");

	if (on)
		udl_cmd_write_reg_1(sc, UDL_REG_SCREEN, UDL_REG_SCREEN_ON);
	else
		udl_cmd_write_reg_1(sc, UDL_REG_SCREEN, UDL_REG_SCREEN_OFF);

	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);

	(void)udl_cmd_send_async(sc);
}

/* ---------- */

int
udl_copycols(void *cookie, int row, int src, int dst, int num)
{
	struct rasops_info *ri = cookie;
	struct udl_softc *sc;
	int sx, sy, dx, dy, cx, cy, r;
	usbd_status error;

	sc = ri->ri_hw;

	DPRINTF(2, "%s: %s: row=%d, src=%d, dst=%d, num=%d\n",
	    DN(sc), FUNC, row, src, dst, num);

	udl_cmd_save_offset(sc);

	sx = src * ri->ri_font->fontwidth;
	sy = row * ri->ri_font->fontheight;
	dx = dst * ri->ri_font->fontwidth;
	dy = row * ri->ri_font->fontheight;
	cx = num * ri->ri_font->fontwidth;
	cy = ri->ri_font->fontheight;

	/* copy row block to off-screen first to fix overlay-copy problem */
	r = (sc->udl_fb_block_copy)
	    (sc, sx, sy, 0, sc->sc_ri.ri_emuheight, cx, cy);
	if (r != 0)
		goto fail;

	/* copy row block back from off-screen now */
	r = (sc->udl_fb_block_copy)
	    (sc, 0, sc->sc_ri.ri_emuheight, dx, dy, cx, cy);
	if (r != 0)
		goto fail;

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		return (EAGAIN);
	}

	/* update character backing store */
	bcopy(sc->sc_cbs + ((row * sc->sc_ri.ri_cols) + src),
	    sc->sc_cbs + ((row * sc->sc_ri.ri_cols) + dst),
	    num * sizeof(*sc->sc_cbs));

	return (0);
}

int
udl_copyrows(void *cookie, int src, int dst, int num)
{
	struct rasops_info *ri = cookie;
	struct udl_softc *sc;
	int sy, dy, cx, cy, r;
	usbd_status error;

	sc = ri->ri_hw;

	DPRINTF(2, "%s: %s: src=%d, dst=%d, num=%d\n",
	    DN(sc), FUNC, src, dst, num);

	udl_cmd_save_offset(sc);

	sy = src * sc->sc_ri.ri_font->fontheight;
	dy = dst * sc->sc_ri.ri_font->fontheight;
	cx = sc->sc_ri.ri_emuwidth;
	cy = num * sc->sc_ri.ri_font->fontheight;

	/* copy row block to off-screen first to fix overlay-copy problem */
	r = (sc->udl_fb_block_copy)
	    (sc, 0, sy, 0, sc->sc_ri.ri_emuheight, cx, cy);
	if (r != 0)
		goto fail;

	/* copy row block back from off-screen now */
	r = (sc->udl_fb_block_copy)
	    (sc, 0, sc->sc_ri.ri_emuheight, 0, dy, cx, cy);
	if (r != 0)
		goto fail;

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		return (EAGAIN);
	}

	/* update character backing store */
	bcopy(sc->sc_cbs + (src * sc->sc_ri.ri_cols),
	    sc->sc_cbs + (dst * sc->sc_ri.ri_cols),
	    (num * sc->sc_ri.ri_cols) * sizeof(*sc->sc_cbs));

	return (0);
}

int
udl_erasecols(void *cookie, int row, int col, int num, long attr)
{
	struct rasops_info *ri = cookie;
	struct udl_softc *sc = ri->ri_hw;
	uint16_t bgc;
	int fg, bg;
	int x, y, cx, cy, r;
	usbd_status error;

	sc = ri->ri_hw;

	DPRINTF(2, "%s: %s: row=%d, col=%d, num=%d\n",
	    DN(sc), FUNC, row, col, num);

	udl_cmd_save_offset(sc);

	sc->sc_ri.ri_ops.unpack_attr(cookie, attr, &fg, &bg, NULL);
	bgc = (uint16_t)sc->sc_ri.ri_devcmap[bg];

	x = col * sc->sc_ri.ri_font->fontwidth;
	y = row * sc->sc_ri.ri_font->fontheight;
	cx = num * sc->sc_ri.ri_font->fontwidth;
	cy = sc->sc_ri.ri_font->fontheight;

	r = (sc->udl_fb_block_write)(sc, bgc, x, y, cx, cy);
	if (r != 0)
		goto fail;

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		return (EAGAIN);
	}

	/* update character backing store */
	bzero(sc->sc_cbs + ((row * sc->sc_ri.ri_cols) + col),
	    num * sizeof(*sc->sc_cbs));

	return (0);
}

int
udl_eraserows(void *cookie, int row, int num, long attr)
{
	struct rasops_info *ri = cookie;
	struct udl_softc *sc;
	uint16_t bgc;
	int fg, bg;
	int x, y, cx, cy, r;
	usbd_status error;

	sc = ri->ri_hw;

	DPRINTF(2, "%s: %s: row=%d, num=%d\n", DN(sc), FUNC, row, num);

	udl_cmd_save_offset(sc);

	sc->sc_ri.ri_ops.unpack_attr(cookie, attr, &fg, &bg, NULL);
	bgc = (uint16_t)sc->sc_ri.ri_devcmap[bg];

	x = 0;
	y = row * sc->sc_ri.ri_font->fontheight;
	cx = sc->sc_ri.ri_emuwidth;
	cy = num * sc->sc_ri.ri_font->fontheight;

	r = (sc->udl_fb_block_write)(sc, bgc, x, y, cx, cy);
	if (r != 0)
		goto fail;

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		return (EAGAIN);
	}

	/* update character backing store */
	bzero(sc->sc_cbs + (row * sc->sc_ri.ri_cols),
	    (num * sc->sc_ri.ri_cols) * sizeof(*sc->sc_cbs));

	return (0);
}

int
udl_putchar(void *cookie, int row, int col, u_int uc, long attr)
{
	struct rasops_info *ri = cookie;
	struct udl_softc *sc = ri->ri_hw;
	int r;
	uint16_t fgc, bgc;
	uint32_t x, y, fg, bg;

	DPRINTF(4, "%s: %s\n", DN(sc), FUNC);

	udl_cmd_save_offset(sc);

	sc->sc_ri.ri_ops.unpack_attr(cookie, attr, &fg, &bg, NULL);
	fgc = (uint16_t)sc->sc_ri.ri_devcmap[fg];
	bgc = (uint16_t)sc->sc_ri.ri_devcmap[bg];

	x = col * ri->ri_font->fontwidth;
	y = row * ri->ri_font->fontheight;

	if (uc == ' ') {
		/*
		 * Writting a block for the space character instead rendering
		 * it from font bits is more slim.
		 */
		r = (sc->udl_fb_block_write)(sc, bgc, x, y,
		    ri->ri_font->fontwidth, ri->ri_font->fontheight);
		if (r != 0)
			goto fail;
	} else {
		/* render a character from font bits */
		r = udl_draw_char(sc, fgc, bgc, uc, x, y);
		if (r != 0)
			goto fail;
	}

	/*
	 * We don't call udl_cmd_send_async() here, since sending each
	 * character by itself gets the performance down bad.  Instead the
	 * character will be buffered until another rasops function flush
	 * the buffer.
	 */

	/* update character backing store */
	sc->sc_cbs[(row * sc->sc_ri.ri_cols) + col] = uc;

	return (0);

fail:
	udl_cmd_restore_offset(sc);
	return (EAGAIN);
}

int
udl_do_cursor(struct rasops_info *ri)
{
	struct udl_softc *sc = ri->ri_hw;
	int r, pos;
	uint32_t x, y;
	uint8_t save_cursor;
	usbd_status error;

	DPRINTF(2, "%s: %s: ccol=%d, crow=%d\n",
	    DN(sc), FUNC, ri->ri_ccol, ri->ri_crow);

	udl_cmd_save_offset(sc);
	save_cursor = sc->sc_cursor_on;

	x = ri->ri_ccol * ri->ri_font->fontwidth;
	y = ri->ri_crow * ri->ri_font->fontheight;

	if (sc->sc_cursor_on == 0) {
		/* save the last character block to off-screen */
		r = (sc->udl_fb_block_copy)(sc, x, y, 0, sc->sc_ri.ri_emuheight,
		    ri->ri_font->fontwidth, ri->ri_font->fontheight);
		if (r != 0)
			goto fail;

		/* draw cursor */
		pos = (ri->ri_crow * sc->sc_ri.ri_cols) + ri->ri_ccol;
		if (sc->sc_cbs[pos] == 0 || sc->sc_cbs[pos] == ' ') {
			r = (sc->udl_fb_block_write)(sc, 0xffff, x, y,
			    ri->ri_font->fontwidth, ri->ri_font->fontheight);
		} else {
			r = udl_draw_char(sc, 0x0000, 0xffff, sc->sc_cbs[pos],
			    x, y);
		}
		if (r != 0)
			goto fail;

		sc->sc_cursor_on = 1;
	} else {
		/* restore the last saved character from off-screen */
		r = (sc->udl_fb_block_copy)(sc, 0, sc->sc_ri.ri_emuheight, x, y,
		    ri->ri_font->fontwidth, ri->ri_font->fontheight);
		if (r != 0)
			goto fail;

		sc->sc_cursor_on = 0;
	}

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		sc->sc_cursor_on = save_cursor;
		return (EAGAIN);
	}

	return (0);
}

int
udl_draw_char(struct udl_softc *sc, uint16_t fg, uint16_t bg, u_int uc,
    uint32_t x, uint32_t y)
{
	int i, j, ly, r;
	uint8_t *fontchar;
	uint8_t buf[UDL_CMD_MAX_DATA_SIZE];
	uint16_t *line, lrgb16, fontbits, luc;
	struct wsdisplay_font *font = sc->sc_ri.ri_font;

	fontchar = (uint8_t *)(font->data + (uc - font->firstchar) *
	    sc->sc_ri.ri_fontscale);

	ly = y;
	for (i = 0; i < font->fontheight; i++) {
		if (font->fontwidth > 8) {
			fontbits = betoh16(*(uint16_t *)fontchar);
		} else {
			fontbits = *fontchar;
			fontbits = fontbits << 8;
		}
		line = (uint16_t *)buf;

		for (j = 15; j > (15 - font->fontwidth); j--) {
			luc = 1 << j;
			if (fontbits & luc)
				lrgb16 = htobe16(fg);
			else
				lrgb16 = htobe16(bg);
			bcopy(&lrgb16, line, 2);
			line++;
		}
		r = (sc->udl_fb_buf_write)(sc, buf, x, ly, font->fontwidth);
		if (r != 0)
			return (r);
		ly++;

		fontchar += font->stride;
	}

	return (0);
}

int
udl_damage(struct udl_softc *sc, uint8_t *image,
    uint32_t x1, uint32_t x2, uint32_t y1, uint32_t y2)
{
	int r;
	int x, y, width, height;
	usbd_status error;

	udl_cmd_save_offset(sc);

	x = x1;
	y = y1;
	width = x2 - x1;
	height = y2 - y1;

	r = udl_draw_image(sc, image, x, y, width, height);
	if (r != 0)
		goto fail;

	error = udl_cmd_send_async(sc);
	if (error != USBD_NORMAL_COMPLETION) {
fail:
		udl_cmd_restore_offset(sc);
		return (EAGAIN);
	}

	return (0);
}

int
udl_draw_image(struct udl_softc *sc, uint8_t *image,
    uint32_t x, uint32_t y, uint32_t width, uint32_t height)
{
	int i, j, r;
	int width_cur, x_cur;
	uint8_t buf[UDL_CMD_MAX_DATA_SIZE];
	uint16_t *image16, lrgb16;
	uint32_t off, block;

	for (i = 0; i < height; i++) {
		off = ((y * sc->sc_width) + x) * 2;
		x_cur = x;
		width_cur = width;

		while (width_cur) {
			if (width_cur > UDL_CMD_MAX_PIXEL_COUNT)
				block = UDL_CMD_MAX_PIXEL_COUNT;
			else
				block = width_cur;

			/* fix RGB ordering */
			image16 = (uint16_t *)(image + off);
			for (j = 0; j < (block * 2); j += 2) {
				lrgb16 = htobe16(*image16);
				bcopy(&lrgb16, buf + j, 2);
				image16++;
			}

			r = (sc->udl_fb_buf_write)(sc, buf, x_cur, y, block);
			if (r != 0)
				return (r);

			off += block * 2;
			x_cur += block;
			width_cur -= block;
		}
		y++;
	}

	return (0);
}

/* ---------- */

usbd_status
udl_ctrl_msg(struct udl_softc *sc, uint8_t rt, uint8_t r,
    uint16_t index, uint16_t value, uint8_t *buf, size_t len)
{
	usb_device_request_t req;
	usbd_status error;

	req.bmRequestType = rt;
	req.bRequest = r;
	USETW(req.wIndex, index);
	USETW(req.wValue, value);
	USETW(req.wLength, len);

	error = usbd_do_request(sc->sc_udev, &req, buf);
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		return (error);
	}

	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_poll(struct udl_softc *sc, uint32_t *buf)
{
	uint8_t lbuf[4];
	usbd_status error;

	error = udl_ctrl_msg(sc, UT_READ_VENDOR_DEVICE,
	    UDL_CTRL_CMD_POLL, 0x0000, 0x0000, lbuf, 4);
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		return (error);
	}
	*buf = *(uint32_t *)lbuf;

	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_read_1(struct udl_softc *sc, uint16_t addr, uint8_t *buf)
{
	uint8_t lbuf[1];
	usbd_status error;

	error = udl_ctrl_msg(sc, UT_READ_VENDOR_DEVICE,
	    UDL_CTRL_CMD_READ_1, addr, 0x0000, lbuf, 1);
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		return (error);
	}
	*buf = *(uint8_t *)lbuf;

	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_write_1(struct udl_softc *sc, uint16_t addr, uint8_t buf)
{
	usbd_status error;

	error = udl_ctrl_msg(sc, UT_WRITE_VENDOR_DEVICE,
	    UDL_CTRL_CMD_WRITE_1, addr, 0x0000, &buf, 1);
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		return (error);
	}

	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_read_edid(struct udl_softc *sc, uint8_t *buf)
{
	uint8_t lbuf[64];
	uint16_t offset;
	usbd_status error;

	offset = 0;

	error = udl_ctrl_msg(sc, UT_READ_VENDOR_DEVICE,
	    UDL_CTRL_CMD_READ_EDID, 0x00a1, (offset << 8), lbuf, 64);
	if (error != USBD_NORMAL_COMPLETION)
		goto fail;
	bcopy(lbuf + 1, buf + offset, 63);
	offset += 63;

	error = udl_ctrl_msg(sc, UT_READ_VENDOR_DEVICE,
	    UDL_CTRL_CMD_READ_EDID, 0x00a1, (offset << 8), lbuf, 64);
	if (error != USBD_NORMAL_COMPLETION)
		goto fail;
	bcopy(lbuf + 1, buf + offset, 63);
	offset += 63;

	error = udl_ctrl_msg(sc, UT_READ_VENDOR_DEVICE,
	    UDL_CTRL_CMD_READ_EDID, 0x00a1, (offset << 8), lbuf, 3);
	if (error != USBD_NORMAL_COMPLETION)
		goto fail;
	bcopy(lbuf + 1, buf + offset, 2);

	return (USBD_NORMAL_COMPLETION);
fail:
	printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
	return (error);
}

uint8_t
udl_lookup_mode(uint16_t hdisplay, uint16_t vdisplay, uint8_t hz,
    uint16_t chip, uint32_t clock)
{
	uint8_t	idx = 0;

	/*
	 * Check first if we have a matching mode with pixelclock
	 */
	while (idx < MAX_DL_MODES) {
		if ((udl_modes[idx].hdisplay == hdisplay) &&
		    (udl_modes[idx].vdisplay == vdisplay) &&
		    (udl_modes[idx].clock == clock) &&
		    (udl_modes[idx].chip <= chip)) {
			return(idx);
		}
		idx++;
	}

	/*
	 * If not, check for matching mode with update frequency
	 */
	idx = 0;
	while (idx < MAX_DL_MODES) {
		if ((udl_modes[idx].hdisplay == hdisplay) &&
		    (udl_modes[idx].vdisplay == vdisplay) &&
		    (udl_modes[idx].hz == hz) &&
		    (udl_modes[idx].chip <= chip)) {
			return(idx);
		}
		idx++;
	}

	return(idx);
}

int
udl_select_chip(struct udl_softc *sc)
{
	char serialnum[USB_MAX_STRING_LEN];
	usb_device_descriptor_t *dd;
	usb_string_descriptor_t us;
	usbd_status error;
	int len, i, n;
	char *s;
	uint16_t c;

	sc->sc_chip = DL120;

	dd = usbd_get_device_descriptor(sc->sc_udev);

	bzero(serialnum, sizeof serialnum);
	error = usbd_get_string_desc(sc->sc_udev, dd->iSerialNumber,
	    0, &us, &len);
	if (error != USBD_NORMAL_COMPLETION)
		return (1);
		
	s = &serialnum[0];
	n = len / 2 - 1;
	for (i = 0; i < n && i < USB_MAX_STRING_LEN; i++) {
		c = UGETW(us.bString[i]);
		/* Convert from Unicode, handle buggy strings. */
		if ((c & 0xff00) == 0)
			*s++ = c;
		else if ((c & 0x00ff) == 0)
			*s++ = c >> 8;
		else
			*s++ = '?';
	}
	*s++ = 0;

	if (strlen(serialnum) > 7)
		if (strncmp(serialnum, "0198-13", 7) == 0)
			sc->sc_chip = DL160;

	DPRINTF(1, "%s: %s: iSerialNumber (%s) used to select chip (%d)\n",
	     DN(sc), FUNC, serialnum, sc->sc_chip);

	return (0);
}

usbd_status
udl_set_enc_key(struct udl_softc *sc, uint8_t *buf, uint8_t len)
{
	usbd_status error;

	error = udl_ctrl_msg(sc, UT_WRITE_VENDOR_DEVICE,
	    UDL_CTRL_CMD_SET_KEY, 0x0000, 0x0000, buf, len);
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		return (error);
	}
	
	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_set_decomp_table(struct udl_softc *sc, uint8_t *buf, uint16_t len)
{
	int err;

	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_DECOMP);
	udl_cmd_insert_int_4(sc, 0x263871cd);	/* magic number */
	udl_cmd_insert_int_4(sc, 0x00000200);	/* 512 byte chunks */
	udl_cmd_insert_buf(sc, buf, len);

	err = udl_cmd_send(sc);
	if (err != 0)
		return (USBD_INVAL);

	return (USBD_NORMAL_COMPLETION);
}

/* ---------- */

int
udl_load_huffman(struct udl_softc *sc)
{
	const char *name = "udl_huffman";
	int error;

	if (sc->sc_huffman == NULL) {
		error = loadfirmware(name, &sc->sc_huffman,
		    &sc->sc_huffman_size);
		if (error != 0) {
			printf("%s: error %d, could not read huffman table "
			    "%s!\n", DN(sc), error, name);
			return (EIO);
		}
	}

	DPRINTF(1, "%s: huffman table %s allocated\n", DN(sc), name);

	return (0);
}

void
udl_free_huffman(struct udl_softc *sc)
{
	if (sc->sc_huffman != NULL) {
		free(sc->sc_huffman, M_DEVBUF);
		sc->sc_huffman = NULL;
		sc->sc_huffman_size = 0;
		DPRINTF(1, "%s: huffman table freed\n", DN(sc));
	}
}

int
udl_fbmem_alloc(struct udl_softc *sc)
{
	int size;

	size = (sc->sc_width * sc->sc_height) * (sc->sc_depth / 8);
	size = round_page(size);

	if (sc->sc_fbmem == NULL) {
		sc->sc_fbmem = malloc(size, M_DEVBUF, M_ZERO);
		if (sc->sc_fbmem == NULL)
			return (-1);
	}

	return (0);
}

void
udl_fbmem_free(struct udl_softc *sc)
{
	if (sc->sc_fbmem != NULL) {
		free(sc->sc_fbmem, M_DEVBUF);
		sc->sc_fbmem = NULL;
	}
}

usbd_status
udl_cmd_alloc_xfer(struct udl_softc *sc)
{
	int i;

	for (i = 0; i < UDL_CMD_XFER_COUNT; i++) {
		struct udl_cmd_xfer *cx = &sc->sc_cmd_xfer[i];

		cx->sc = sc;

		cx->xfer = usbd_alloc_xfer(sc->sc_udev);
		if (cx->xfer == NULL) {
			printf("%s: %s: can't allocate xfer handle!\n",
			    DN(sc), FUNC);
			return (USBD_NOMEM);
		}

		cx->buf = usbd_alloc_buffer(cx->xfer, UDL_CMD_MAX_XFER_SIZE);
		if (cx->buf == NULL) {
			printf("%s: %s: can't allocate xfer buffer!\n",
			    DN(sc), FUNC);
			return (USBD_NOMEM);
		}
	}

	return (USBD_NORMAL_COMPLETION);
}

void
udl_cmd_free_xfer(struct udl_softc *sc)
{
	int i;

	for (i = 0; i < UDL_CMD_XFER_COUNT; i++) {
		struct udl_cmd_xfer *cx = &sc->sc_cmd_xfer[i];

		if (cx->xfer != NULL) {
			usbd_free_xfer(cx->xfer);
			cx->xfer = NULL;
		}
	}
}

int
udl_cmd_alloc_buf(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	cb->buf = malloc(UDL_CMD_MAX_XFER_SIZE, M_DEVBUF, 0);
	if (cb->buf == NULL) {
		printf("%s: %s: can't allocate buffer!\n",
		    DN(sc), FUNC);
		return (ENOMEM);
	}
	cb->off = 0;
	cb->compblock = 0;

	return (0);
}

void
udl_cmd_free_buf(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	if (cb->buf != NULL) {
		free(cb->buf, M_DEVBUF);
		cb->buf = NULL;
	}
	cb->off = 0;
}

void
udl_cmd_insert_int_1(struct udl_softc *sc, uint8_t value)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	cb->buf[cb->off] = value;

	cb->off += 1;
}

void
udl_cmd_insert_int_2(struct udl_softc *sc, uint16_t value)
{
	uint16_t lvalue;
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	lvalue = htobe16(value);
	bcopy(&lvalue, cb->buf + cb->off, 2);

	cb->off += 2;
}

void
udl_cmd_insert_int_3(struct udl_softc *sc, uint32_t value)
{
	uint32_t lvalue;
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
#if BYTE_ORDER == BIG_ENDIAN
	lvalue = htobe32(value) << 8;
#else
	lvalue = htobe32(value) >> 8;
#endif
	bcopy(&lvalue, cb->buf + cb->off, 3);

	cb->off += 3;
}

void
udl_cmd_insert_int_4(struct udl_softc *sc, uint32_t value)
{
	uint32_t lvalue;
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	lvalue = htobe32(value);
	bcopy(&lvalue, cb->buf + cb->off, 4);

	cb->off += 4;
}

void
udl_cmd_insert_buf(struct udl_softc *sc, uint8_t *buf, uint32_t len)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	bcopy(buf, cb->buf + cb->off, len);

	cb->off += len;
}

int
udl_cmd_insert_buf_comp(struct udl_softc *sc, uint8_t *buf, uint32_t len)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	struct udl_huffman *h;
	uint8_t bit_pos;
	uint16_t *pixels, prev;
	int16_t diff;
	uint32_t bit_count, bit_pattern, bit_cur;
	int i, j, bytes, eob, padding, next;

	pixels = (uint16_t *)buf;
	bit_pos = bytes = eob = padding = 0;

	/*
	 * If the header doesn't fit into the 512 byte main-block anymore,
	 * skip the header and finish up the main-block.  We return zero
	 * to signal our caller that the header has been skipped.
	 */
	if (cb->compblock >= UDL_CB_RESTART_SIZE) {
		cb->off -= UDL_CMD_WRITE_HEAD_SIZE;
		cb->compblock -= UDL_CMD_WRITE_HEAD_SIZE;
		eob = 1;
	}

	/*
	 * Generate a sub-block with maximal 256 pixels compressed data.
	 */
	for (i = 0; i < len / 2 && eob == 0; i++) {
		/* get difference between current and previous pixel */
		if (i > 0)
			prev = betoh16(pixels[i - 1]);
		else
			prev = 0;

		/* get the huffman difference bit sequence */
		diff = betoh16(pixels[i]) - prev;
		h = (struct udl_huffman *)(sc->sc_huffman + UDL_HUFFMAN_BASE);
		h += diff;
		bit_count = h->bit_count;
		bit_pattern = betoh32(h->bit_pattern);


		/* we are near the end of the main-block, so quit loop */
		if (bit_count % 8 == 0)
			next = bit_count / 8;
		else
			next = (bit_count / 8) + 1;

		if (cb->compblock + next >= UDL_CB_BODY_SIZE) {
			eob = 1;
			break;
		}

		/* generate one pixel compressed data */
		for (j = 0; j < bit_count; j++) {
			if (bit_pos == 0)
				cb->buf[cb->off] = 0;
			bit_cur = (bit_pattern >> j) & 1;
			cb->buf[cb->off] |= (bit_cur << bit_pos);
			bit_pos++;

			if (bit_pos == 8) {
				bit_pos = 0;
				cb->off++;
				cb->compblock++;
			}
		}
		bytes += 2;
	}

	/*
	 * If we have bits left in our last byte, round up to the next
	 * byte, so we don't overwrite them.
	 */
	if (bit_pos != 0) {
		cb->off++;
		cb->compblock++;
	}

	/*
	 * Finish up a 512 byte main-block.  The leftover space gets
	 * padded to zero.  Finally terminate the block by writting the
	 * 0xff-into-UDL_REG_SYNC-register sequence.
	 */
	if (eob == 1) {
		padding = (UDL_CB_BODY_SIZE - cb->compblock);
		for (i = 0; i < padding; i++) {
			cb->buf[cb->off] = 0;
			cb->off++;
			cb->compblock++;
		}
		udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);
		cb->compblock = 0;
	}

	/* return how many bytes we have compressed */
	return (bytes);
}

int
udl_cmd_insert_head_comp(struct udl_softc *sc, uint32_t len)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	int i, padding;

	if (cb->compblock > UDL_CB_BODY_SIZE) {
		cb->off -= UDL_CMD_COPY_HEAD_SIZE;
		cb->compblock -= UDL_CMD_COPY_HEAD_SIZE;

		padding = (UDL_CB_BODY_SIZE - cb->compblock);
		for (i = 0; i < padding; i++) {
			cb->buf[cb->off] = 0;
			cb->off++;
			cb->compblock++;
		}
		udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);
		cb->compblock = 0;
		return (0);
	}

	return (len);
}

int
udl_cmd_insert_check(struct udl_softc *sc, int len)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	int total;
	usbd_status error;

	total = cb->off + len;

	if (total > UDL_CMD_MAX_XFER_SIZE) {
		/* command buffer is almost full, try to flush it */
		if (cb->xfer_type == UDL_CMD_XFER_ASYNC)
			error = udl_cmd_send_async(sc);
		else
			error = udl_cmd_send(sc);
		if (error != USBD_NORMAL_COMPLETION) {
			DPRINTF(1, "%s: %s: can't flush full command buffer\n",
			    DN(sc), FUNC);
			return (EAGAIN);
		}
	}

	return (0);
}

void
udl_cmd_set_xfer_type(struct udl_softc *sc, int xfer_type)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	cb->xfer_type = xfer_type;
}

void
udl_cmd_save_offset(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	cb->off_save = cb->off;
	cb->compblock_save = cb->compblock;
}

void
udl_cmd_restore_offset(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;

	cb->off = cb->off_save;
	cb->compblock = cb->compblock_save;
}

void
udl_cmd_write_reg_1(struct udl_softc *sc, uint8_t reg, uint8_t val)
{
	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_REG_WRITE_1);
	udl_cmd_insert_int_1(sc, reg);
	udl_cmd_insert_int_1(sc, val);
}

void
udl_cmd_write_reg_3(struct udl_softc *sc, uint8_t reg, uint32_t val)
{
	udl_cmd_write_reg_1(sc, reg + 0, (val >> 16) & 0xff);
	udl_cmd_write_reg_1(sc, reg + 1, (val >> 8) & 0xff);
	udl_cmd_write_reg_1(sc, reg + 2, (val >> 0) & 0xff);
}

usbd_status
udl_cmd_send(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	struct udl_cmd_xfer *cx = &sc->sc_cmd_xfer[0];
	int len;
	usbd_status error;

	/* mark end of command stack */
	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_EOC);

	bcopy(cb->buf, cx->buf, cb->off);

	len = cb->off;
	error = usbd_bulk_transfer(cx->xfer, sc->sc_tx_pipeh,
	    USBD_NO_COPY | USBD_SHORT_XFER_OK, 1000, cx->buf, &len,
	    "udl_bulk_xmit");
	if (error != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		/* we clear our buffer now to avoid growing out of bounds */
		goto fail;
	}
	DPRINTF(1, "%s: %s: sent %d of %d bytes\n",
	    DN(sc), FUNC, len, cb->off);
fail:
	cb->off = 0;
	cb->compblock = 0;

	return (error);
}

usbd_status
udl_cmd_send_async(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	struct udl_cmd_xfer *cx;
	usbd_status error;
	int i, s;

	/* check if command xfer queue is full */
	if (sc->sc_cmd_xfer_cnt == UDL_CMD_XFER_COUNT)
		return (USBD_IN_USE);

	s = splusb();	/* no callbacks please until accounting is done */

	/* find a free command xfer buffer */
	for (i = 0; i < UDL_CMD_XFER_COUNT; i++) {
		if (sc->sc_cmd_xfer[i].busy == 0)
			break;
	}
	if (i == UDL_CMD_XFER_COUNT) {
		/* this shouldn't happen */
		return (USBD_IN_USE);
	}
	cx = &sc->sc_cmd_xfer[i];

	/* mark end of command stack */
	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_EOC);

	/* copy command buffer to xfer buffer */
	bcopy(cb->buf, cx->buf, cb->off);

	/* do xfer */
	usbd_setup_xfer(cx->xfer, sc->sc_tx_pipeh, cx, cx->buf, cb->off,
	     USBD_NO_COPY, 1000, udl_cmd_send_async_cb);
	error = usbd_transfer(cx->xfer);
	if (error != 0 && error != USBD_IN_PROGRESS) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(error));
		splx(s);
		return (error);
	}
	DPRINTF(2, "%s: %s: sending %d bytes from buffer no. %d\n",
	    DN(sc), FUNC, cb->off, i);

	/* free command buffer, lock xfer buffer */
	cb->off = 0;
	cb->compblock = 0;
	cx->busy = 1;
	sc->sc_cmd_xfer_cnt++;

	splx(s);

	return (USBD_NORMAL_COMPLETION);
}

void
udl_cmd_send_async_cb(usbd_xfer_handle xfer, usbd_private_handle priv,
    usbd_status status)
{
	struct udl_cmd_xfer *cx = priv;
	struct udl_softc *sc = cx->sc;
	int len;

	if (status != USBD_NORMAL_COMPLETION) {
		printf("%s: %s: %s!\n", DN(sc), FUNC, usbd_errstr(status));

		if (status == USBD_NOT_STARTED || status == USBD_CANCELLED)
			return;
		if (status == USBD_STALLED)
			usbd_clear_endpoint_stall_async(sc->sc_tx_pipeh);
		goto skip;
	}
	usbd_get_xfer_status(xfer, NULL, NULL, &len, NULL);

	DPRINTF(3, "%s: %s: sent %d bytes\n", DN(sc), FUNC, len);
skip:
	/* free xfer buffer */
	cx->busy = 0;
	sc->sc_cmd_xfer_cnt--;

	/* wakeup UDLIO_DAMAGE if it sleeps for a free xfer buffer */
	wakeup(sc);
}

/* ---------- */

usbd_status
udl_init_chip(struct udl_softc *sc)
{
	uint8_t ui8;
	uint32_t ui32;
	usbd_status error;

	error = udl_poll(sc, &ui32);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: poll=0x%08x\n", DN(sc), FUNC, ui32);

	error = udl_read_1(sc, 0xc484, &ui8);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: read 0x%02x from 0xc484\n", DN(sc), FUNC, ui8);

	error = udl_write_1(sc, 0xc41f, 0x01);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: write 0x01 to 0xc41f\n", DN(sc), FUNC);

	error = udl_read_edid(sc, sc->sc_edid);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: read EDID\n", DN(sc), FUNC);

	error = udl_set_enc_key(sc, udl_null_key_1, sizeof(udl_null_key_1));
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: set encryption key\n", DN(sc), FUNC);

	error = udl_write_1(sc, 0xc40b, 0x00);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: write 0x00 to 0xc40b\n", DN(sc), FUNC, ui8);

	error = udl_set_decomp_table(sc, udl_decomp_table,
	    sizeof(udl_decomp_table));
	if (error != USBD_NORMAL_COMPLETION)
		return (error);
	DPRINTF(1, "%s: %s: set decompression table\n", DN(sc), FUNC);

	return (USBD_NORMAL_COMPLETION);
}

void
udl_init_fb_offsets(struct udl_softc *sc, uint32_t start16, uint32_t stride16,
    uint32_t start8, uint32_t stride8)
{
	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0x00);
	udl_cmd_write_reg_3(sc, UDL_REG_ADDR_START16, start16);
	udl_cmd_write_reg_3(sc, UDL_REG_ADDR_STRIDE16, stride16);
	udl_cmd_write_reg_3(sc, UDL_REG_ADDR_START8, start8);
	udl_cmd_write_reg_3(sc, UDL_REG_ADDR_STRIDE8, stride8);
	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);
}

usbd_status
udl_init_resolution(struct udl_softc *sc)
{
	int i;
	usbd_status error;
	uint8_t *buf = udl_modes[sc->sc_cur_mode].mode;

	/* write resolution values and set video memory offsets */
	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0x00);
	for (i = 0; i < UDL_MODE_SIZE; i++)
		udl_cmd_write_reg_1(sc, i, buf[i]);
	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);

	udl_init_fb_offsets(sc, 0x000000, 0x000a00, 0x555555, 0x000500);
	error = udl_cmd_send(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);

	/* clear screen */
	error = udl_clear_screen(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);

	/* show framebuffer content */
	udl_cmd_write_reg_1(sc, UDL_REG_SCREEN, UDL_REG_SCREEN_ON);
	udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);
	error = udl_cmd_send(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);

	return (USBD_NORMAL_COMPLETION);
}

usbd_status
udl_clear_screen(struct udl_softc *sc)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	usbd_status error;

	/* clear screen */
	udl_fb_block_write(sc, 0x0000, 0, 0, sc->sc_width, sc->sc_height);
	if (cb->xfer_type == UDL_CMD_XFER_ASYNC)
		error = udl_cmd_send_async(sc);
	else
		error = udl_cmd_send(sc);
	if (error != USBD_NORMAL_COMPLETION)
		return (error);

	return (USBD_NORMAL_COMPLETION);
}

void
udl_select_mode(struct udl_softc *sc)
{
	struct udl_mode mode;
	int index = MAX_DL_MODES, i;

	/* try to get the preferred mode from EDID */
	edid_parse(sc->sc_edid, &sc->sc_edid_info);
#ifdef UDL_DEBUG
	edid_print(&sc->sc_edid_info);
#endif
	if (sc->sc_edid_info.edid_preferred_mode != NULL) {
		mode.hz = 
		    (sc->sc_edid_info.edid_preferred_mode->dot_clock * 1000) /
		    (sc->sc_edid_info.edid_preferred_mode->htotal *
		     sc->sc_edid_info.edid_preferred_mode->vtotal);
		mode.clock = 
		    sc->sc_edid_info.edid_preferred_mode->dot_clock / 10;
		mode.hdisplay =
		    sc->sc_edid_info.edid_preferred_mode->hdisplay;
		mode.vdisplay =
		    sc->sc_edid_info.edid_preferred_mode->vdisplay;
		index = udl_lookup_mode(mode.hdisplay, mode.vdisplay, mode.hz,
		    sc->sc_chip, mode.clock);
		sc->sc_cur_mode = index;
	} else {
		DPRINTF(1, "%s: %s: no preferred mode found!\n", DN(sc), FUNC);
	}

	if (index == MAX_DL_MODES) {
		DPRINTF(1, "%s: %s: no mode line found for %dx%d @ %dHz!\n",
		    DN(sc), FUNC, mode.hdisplay, mode.vdisplay, mode.hz);

		i = 0;
		while (i < sc->sc_edid_info.edid_nmodes) {
			mode.hz = 
			    (sc->sc_edid_info.edid_modes[i].dot_clock * 1000) /
			    (sc->sc_edid_info.edid_modes[i].htotal *
			     sc->sc_edid_info.edid_modes[i].vtotal);
			mode.clock = 
			    sc->sc_edid_info.edid_modes[i].dot_clock / 10;
			mode.hdisplay =
			    sc->sc_edid_info.edid_modes[i].hdisplay;
			mode.vdisplay =
			    sc->sc_edid_info.edid_modes[i].vdisplay;
			index = udl_lookup_mode(mode.hdisplay, mode.vdisplay,
			    mode.hz, sc->sc_chip, mode.clock);
			if (index < MAX_DL_MODES)
				if ((sc->sc_cur_mode == MAX_DL_MODES) ||
				    (index > sc->sc_cur_mode))
					sc->sc_cur_mode = index;
			i++;
		}
	}

	/*
	 * If no mode found use default.
	 */
	if (sc->sc_cur_mode == MAX_DL_MODES)
		sc->sc_cur_mode = udl_lookup_mode(800, 600, 60, sc->sc_chip, 0);

	mode = udl_modes[sc->sc_cur_mode];
	sc->sc_width = mode.hdisplay;
	sc->sc_height = mode.vdisplay;

	/*
	 * We always use 16bit color depth for now.
	 */
	sc->sc_depth = 16;

	DPRINTF(1, "%s: %s: %dx%d @ %dHz\n",
	    DN(sc), FUNC, mode.hdisplay, mode.vdisplay, mode.hz);
}

int
udl_fb_buf_write(struct udl_softc *sc, uint8_t *buf, uint32_t x,
    uint32_t y, uint16_t width)
{
	uint16_t lwidth;
	uint32_t off;
	int r;

	r = udl_cmd_insert_check(sc, UDL_CMD_WRITE_MAX_SIZE);
	if (r != 0)
		return (r);

	off = ((y * sc->sc_width) + x) * 2;
	lwidth = width * 2;

	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_FB_WRITE | UDL_BULK_CMD_FB_WORD);
	udl_cmd_insert_int_3(sc, off);
	udl_cmd_insert_int_1(sc, width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);

	udl_cmd_insert_buf(sc, buf, lwidth);

	return (0);
}

int
udl_fb_block_write(struct udl_softc *sc, uint16_t rgb16, uint32_t x,
    uint32_t y, uint32_t width, uint32_t height)
{
	uint32_t i;
	int r;

	for (i = 0; i < height; i++) {
		r = udl_fb_line_write(sc, rgb16, x, y + i, width);
		if (r != 0)
			return (r);
	}

	return (0);
}

int
udl_fb_line_write(struct udl_softc *sc, uint16_t rgb16, uint32_t x,
    uint32_t y, uint32_t width)
{
	uint32_t off, block;
	int r;

	off = (y * sc->sc_width) + x;

	while (width) {
		if (width > UDL_CMD_MAX_PIXEL_COUNT)	
			block = UDL_CMD_MAX_PIXEL_COUNT;
		else
			block = width;

		r = udl_fb_off_write(sc, rgb16, off, block);
		if (r != 0)
			return (r);

		off += block;
		width -= block;
	}

	return (0);
}

int
udl_fb_off_write(struct udl_softc *sc, uint16_t rgb16, uint32_t off,
    uint16_t width)
{
	uint8_t buf[UDL_CMD_MAX_DATA_SIZE];
	uint16_t lwidth, lrgb16;
	uint32_t loff;
	int i, r;

	r = udl_cmd_insert_check(sc, UDL_CMD_WRITE_MAX_SIZE);
	if (r != 0)
		return (r);

	loff = off * 2;
	lwidth = width * 2;

	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_FB_WRITE | UDL_BULK_CMD_FB_WORD);
	udl_cmd_insert_int_3(sc, loff);
	udl_cmd_insert_int_1(sc, width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);

	for (i = 0; i < lwidth; i += 2) {
		lrgb16 = htobe16(rgb16);
		bcopy(&lrgb16, buf + i, 2);
	}

	udl_cmd_insert_buf(sc, buf, lwidth);

	return (0);
}

int
udl_fb_block_copy(struct udl_softc *sc, uint32_t src_x, uint32_t src_y,
    uint32_t dst_x, uint32_t dst_y, uint32_t width, uint32_t height)
{
	int i, r;

	for (i = 0; i < height; i++) {
		r = udl_fb_line_copy(sc, src_x, src_y + i, dst_x, dst_y + i,
		    width);
		if (r != 0)
			return (r);
	}

	return (0);
}


int
udl_fb_line_copy(struct udl_softc *sc, uint32_t src_x, uint32_t src_y,
    uint32_t dst_x, uint32_t dst_y, uint32_t width)
{
	uint32_t src_off, dst_off, block;
	int r;

	src_off = (src_y * sc->sc_width) + src_x;
	dst_off = (dst_y * sc->sc_width) + dst_x;

	while (width) {
		if (width > UDL_CMD_MAX_PIXEL_COUNT)
			block = UDL_CMD_MAX_PIXEL_COUNT;
		else
			block = width;

		r = udl_fb_off_copy(sc, src_off, dst_off, block);
		if (r != 0)
			return (r);

		src_off += block;
		dst_off += block;
		width -= block;
	}

	return (0);
}

int
udl_fb_off_copy(struct udl_softc *sc, uint32_t src_off, uint32_t dst_off,
    uint16_t width)
{
	uint32_t ldst_off, lsrc_off;
	int r;

	r = udl_cmd_insert_check(sc, UDL_CMD_COPY_MAX_SIZE);
	if (r != 0)
		return (r);

	ldst_off = dst_off * 2;
	lsrc_off = src_off * 2;

	udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
	udl_cmd_insert_int_1(sc, UDL_BULK_CMD_FB_COPY | UDL_BULK_CMD_FB_WORD);
	udl_cmd_insert_int_3(sc, ldst_off);
	udl_cmd_insert_int_1(sc, width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);
	udl_cmd_insert_int_3(sc, lsrc_off);

	return (0);
}

int
udl_fb_buf_write_comp(struct udl_softc *sc, uint8_t *buf, uint32_t x,
    uint32_t y, uint16_t width)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	uint8_t *count;
	uint16_t lwidth;
	uint32_t off;
	int r, sent;

	r = udl_cmd_insert_check(sc, UDL_CMD_WRITE_MAX_SIZE);
	if (r != 0)
		return (r);

	off = ((y * sc->sc_width) + x) * 2;
	lwidth = width * 2;

	/*
	 * A new compressed stream needs the 0xff-into-UDL_REG_SYNC-register
	 * sequence always as first command.
	 */
	if (cb->off == 0)
		udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);

	r = sent = 0;
	while (sent < lwidth) {
		udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
		udl_cmd_insert_int_1(sc,
		    UDL_BULK_CMD_FB_WRITE |
		    UDL_BULK_CMD_FB_WORD |
		    UDL_BULK_CMD_FB_COMP);
		udl_cmd_insert_int_3(sc, off + sent);
		udl_cmd_insert_int_1(sc,
		    width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);
		cb->compblock += UDL_CMD_WRITE_HEAD_SIZE;

		count = &cb->buf[cb->off - 1];
		r = udl_cmd_insert_buf_comp(sc, buf + sent, lwidth - sent);
		if (r > 0 && r != (lwidth - sent)) {
			*count = r / 2;
			width -= r / 2;
		}
		sent += r;
	}

	return (0);
}

int
udl_fb_block_write_comp(struct udl_softc *sc, uint16_t rgb16, uint32_t x,
    uint32_t y, uint32_t width, uint32_t height)
{
	uint32_t i;
	int r;

	for (i = 0; i < height; i++) {
		r = udl_fb_line_write_comp(sc, rgb16, x, y + i, width);
		if (r != 0)
			return (r);
	}

	return (0);
}

int
udl_fb_line_write_comp(struct udl_softc *sc, uint16_t rgb16, uint32_t x,
    uint32_t y, uint32_t width)
{
	uint32_t off, block;
	int r;

	off = (y * sc->sc_width) + x;

	while (width) {
		if (width > UDL_CMD_MAX_PIXEL_COUNT)	
			block = UDL_CMD_MAX_PIXEL_COUNT;
		else
			block = width;

		r = udl_fb_off_write_comp(sc, rgb16, off, block);
		if (r != 0)
			return (r);

		off += block;
		width -= block;
	}

	return (0);
}

int
udl_fb_off_write_comp(struct udl_softc *sc, uint16_t rgb16, uint32_t off,
    uint16_t width)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	uint8_t buf[UDL_CMD_MAX_DATA_SIZE];
	uint8_t *count;
	uint16_t lwidth, lrgb16;
	uint32_t loff;
	int i, r, sent;

	r = udl_cmd_insert_check(sc, UDL_CMD_WRITE_MAX_SIZE);
	if (r != 0)
		return (r);

	loff = off * 2;
	lwidth = width * 2;

	for (i = 0; i < lwidth; i += 2) {
		lrgb16 = htobe16(rgb16);
		bcopy(&lrgb16, buf + i, 2);
	}

	/*
	 * A new compressed stream needs the 0xff-into-UDL_REG_SYNC-register
	 * sequence always as first command.
	 */
	if (cb->off == 0)
		udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);

	r = sent = 0;
	while (sent < lwidth) {
		udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
		udl_cmd_insert_int_1(sc,
		    UDL_BULK_CMD_FB_WRITE |
		    UDL_BULK_CMD_FB_WORD |
		    UDL_BULK_CMD_FB_COMP);
		udl_cmd_insert_int_3(sc, loff + sent);
		udl_cmd_insert_int_1(sc,
		    width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);
		cb->compblock += UDL_CMD_WRITE_HEAD_SIZE;

		count = &cb->buf[cb->off - 1];
		r = udl_cmd_insert_buf_comp(sc, buf + sent, lwidth - sent);
		if (r > 0 && r != (lwidth - sent)) {
			*count = r / 2;
			width -= r / 2;
		}
		sent += r;
	}

	return (0);
}

int
udl_fb_block_copy_comp(struct udl_softc *sc, uint32_t src_x, uint32_t src_y,
    uint32_t dst_x, uint32_t dst_y, uint32_t width, uint32_t height)
{
	int i, r;

	for (i = 0; i < height; i++) {
		r = udl_fb_line_copy_comp(sc, src_x, src_y + i,
		    dst_x, dst_y + i, width);
		if (r != 0)
			return (r);
	}

	return (0);
}

int
udl_fb_line_copy_comp(struct udl_softc *sc, uint32_t src_x, uint32_t src_y,
    uint32_t dst_x, uint32_t dst_y, uint32_t width)
{
	uint32_t src_off, dst_off, block;
	int r;

	src_off = (src_y * sc->sc_width) + src_x;
	dst_off = (dst_y * sc->sc_width) + dst_x;

	while (width) {
		if (width > UDL_CMD_MAX_PIXEL_COUNT)
			block = UDL_CMD_MAX_PIXEL_COUNT;
		else
			block = width;

		r = udl_fb_off_copy_comp(sc, src_off, dst_off, block);
		if (r != 0)
			return (r);

		src_off += block;
		dst_off += block;
		width -= block;
	}

	return (0);
}

int
udl_fb_off_copy_comp(struct udl_softc *sc, uint32_t src_off, uint32_t dst_off,
    uint16_t width)
{
	struct udl_cmd_buf *cb = &sc->sc_cmd_buf;
	uint32_t ldst_off, lsrc_off;
	int r;

	r = udl_cmd_insert_check(sc, UDL_CMD_COPY_MAX_SIZE);
	if (r != 0)
		return (r);

	ldst_off = dst_off * 2;
	lsrc_off = src_off * 2;

	/*
	 * A new compressed stream needs the 0xff-into-UDL_REG_SYNC-register
	 * sequence always as first command.
	 */
	if (cb->off == 0)
		udl_cmd_write_reg_1(sc, UDL_REG_SYNC, 0xff);

	r = 0;
	while (r < 1) {
		udl_cmd_insert_int_1(sc, UDL_BULK_SOC);
		udl_cmd_insert_int_1(sc,
		    UDL_BULK_CMD_FB_COPY | UDL_BULK_CMD_FB_WORD);
		udl_cmd_insert_int_3(sc, ldst_off);
		udl_cmd_insert_int_1(sc,
		    width >= UDL_CMD_MAX_PIXEL_COUNT ? 0 : width);
		udl_cmd_insert_int_3(sc, lsrc_off);
		cb->compblock += UDL_CMD_COPY_HEAD_SIZE;

		r = udl_cmd_insert_head_comp(sc, UDL_CMD_COPY_HEAD_SIZE);
	}

	return (0);
}

/* ---------- */
#ifdef UDL_DEBUG
void
udl_hexdump(void *buf, int len, int quiet)
{
	int i;

	for (i = 0; i < len; i++) {
		if (quiet == 0) {
			if (i % 16 == 0)
				printf("%s%5i:", i ? "\n" : "", i);
			if (i % 4 == 0)
				printf(" ");
		}
		printf("%02x", (int)*((u_char *)buf + i));
	}
	printf("\n");
}

usbd_status
udl_init_test(struct udl_softc *sc)
{
	int i, j, parts, loops;
	uint16_t color;
	uint16_t rgb24[3] = { 0xf800, 0x07e0, 0x001f };

	loops = (sc->sc_width * sc->sc_height) / UDL_CMD_MAX_PIXEL_COUNT;
	parts = loops / 3;
	color = rgb24[0];

	j = 1;
	for (i = 0; i < loops; i++) {
		if (i == parts) {
			color = rgb24[j];
			parts += parts;
			j++;
		}
		(sc->udl_fb_off_write)(sc, color, i * UDL_CMD_MAX_PIXEL_COUNT,
		    UDL_CMD_MAX_PIXEL_COUNT);
	}
	(void)udl_cmd_send(sc);

	return (USBD_NORMAL_COMPLETION);
}
#endif