Staging: udlfb: add dynamic modeset support

/*****************************************************************************

 *                          DLFB Kernel Driver                               *

 *                            Version 0.2 (udlfb)                            *

 *             (C) 2009 Roberto De Ioris <roberto@unbit.it>                  *

 *                                                                           *

 *     This file is licensed under the GPLv2. See COPYING in the package.    *

 * Based on the amazing work of Florian Echtler and libdlo 0.1               *

 *                                                                           *

 *                                                                           *

 * 10.06.09 release 0.2.3 (edid ioctl, fallback for unsupported modes)       *

 * 05.06.09 release 0.2.2 (real screen blanking, rle compression, double buffer) *

 * 31.05.09 release 0.2                                                      *

 * 22.05.09 First public (ugly) release                                      *

 *****************************************************************************/

/*

 * udlfb.c -- Framebuffer driver for DisplayLink USB controller

 *

 * Copyright (C) 2009 Roberto De Ioris <roberto@unbit.it>

 * Copyright (C) 2009 Jaya Kumar <jayakumar.lkml@gmail.com>

 *

 * This file is subject to the terms and conditions of the GNU General Public

 * License v2. See the file COPYING in the main directory of this archive for

 * more details.

 *

 * Layout is based on skeletonfb by James Simmons and Geert Uytterhoeven,

 * usb-skeleton by GregKH.

 *

 * Device-specific portions based on information from Displaylink, with work

 * from Florian Echtler, Henrik Bjerregaard Pedersen, and others.

 */

#include <linux/module.h>

#include <linux/kernel.h>

#include "udlfb.h"

#define DRIVER_VERSION "DLFB 0.2"

#define DRIVER_VERSION "DisplayLink Framebuffer Driver 0.4.1"

/* memory functions taken from vfb */

static struct fb_fix_screeninfo dlfb_fix = {

       .id =           "displaylinkfb",

       .type =         FB_TYPE_PACKED_PIXELS,

       .visual =       FB_VISUAL_TRUECOLOR,

       .xpanstep =     0,

       .ypanstep =     0,

       .ywrapstep =    0,

       .accel =        FB_ACCEL_NONE,

};

static void *rvmalloc(unsigned long size)

#define NR_USB_REQUEST_I2C_SUB_IO 0x02

#define NR_USB_REQUEST_CHANNEL 0x12

/*

 * Inserts a specific DisplayLink controller command into the provided

 * buffer.

 */

static char *insert_command(char *buf, u8 reg, u8 val)

{

	void *mem;

	unsigned long adr;

       *buf++ = 0xAF;

       *buf++ = 0x20;

       *buf++ = reg;

       *buf++ = val;

       return buf;

}

	size = PAGE_ALIGN(size);

	mem = vmalloc_32(size);

	if (!mem)

		return NULL;

static char *insert_vidreg_lock(char *buf)

{

       return insert_command(buf, 0xFF, 0x00);

}

	memset(mem, 0, size);	/* Clear the ram out, no junk to the user */

	adr = (unsigned long)mem;

	while (size > 0) {

		SetPageReserved(vmalloc_to_page((void *)adr));

		adr += PAGE_SIZE;

		size -= PAGE_SIZE;

static char *insert_vidreg_unlock(char *buf)

{

       return insert_command(buf, 0xFF, 0xFF);

}

	return mem;

/*

 * Once you send this command, the DisplayLink framebuffer gets driven to the

 * display.

 */

static char *insert_enable_hvsync(char *buf)

{

       return insert_command(buf, 0x1F, 0x00);

}

static void rvfree(void *mem, unsigned long size)

static char *insert_set_color_depth(char *buf, u8 selection)

{

	unsigned long adr;

       return insert_command(buf, 0x00, selection);

}

	if (!mem)

		return;

static char *insert_set_base16bpp(char *wrptr, u32 base)

{

       /* the base pointer is 16 bits wide, 0x20 is hi byte. */

       wrptr = insert_command(wrptr, 0x20, base >> 16);

       wrptr = insert_command(wrptr, 0x21, base >> 8);

       return insert_command(wrptr, 0x22, base);

}

	adr = (unsigned long)mem;

	while ((long)size > 0) {

		ClearPageReserved(vmalloc_to_page((void *)adr));

		adr += PAGE_SIZE;

		size -= PAGE_SIZE;

static char *insert_set_base8bpp(char *wrptr, u32 base)

{

       wrptr = insert_command(wrptr, 0x26, base >> 16);

       wrptr = insert_command(wrptr, 0x27, base >> 8);

       return insert_command(wrptr, 0x28, base);

}

static char *insert_command_16(char *wrptr, u8 reg, u16 value)

{

       wrptr = insert_command(wrptr, reg, value >> 8);

       return insert_command(wrptr, reg+1, value);

}

/*

 * This is kind of weird because the controller takes some

 * register values in a different byte order than other registers.

 */

static char *insert_command_16be(char *wrptr, u8 reg, u16 value)

{

       wrptr = insert_command(wrptr, reg, value);

       return insert_command(wrptr, reg+1, value >> 8);

}

/*

 * LFSR is linear feedback shift register. The reason we have this is

 * because the display controller needs to minimize the clock depth of

 * various counters used in the display path. So this code reverses the

 * provided value into the lfsr16 value by counting backwards to get

 * the value that needs to be set in the hardware comparator to get the

 * same actual count. This makes sense once you read above a couple of

 * times and think about it from a hardware perspective.

 */

static u16 lfsr16(u16 actual_count)

{

	u32 lv = 0xFFFF; /* This is the lfsr value that the hw starts with */

	while (actual_count--) {

		lv =	 ((lv << 1) |

			(((lv >> 15) ^ (lv >> 4) ^ (lv >> 2) ^ (lv >> 1)) & 1))

			& 0xFFFF;

	}

	return (u16) lv;

}

/*

 * This does LFSR conversion on the value that is to be written.

 * See LFSR explanation above for more detail.

 */

static char *insert_command_lfsr16(char *wrptr, u8 reg, u16 value)

{

	return insert_command_16(wrptr, reg, lfsr16(value));

}

	vfree(mem);

/*

 * This takes a standard fbdev screeninfo struct and all of its monitor mode

 * details and converts them into the DisplayLink equivalent register commands.

 */

static char *insert_set_vid_cmds(char *wrptr, struct fb_var_screeninfo *var)

{

	u16 xds, yds;

	u16 xde, yde;

	u16 yec;

	/* x display start */

	xds = var->left_margin + var->hsync_len;

	wrptr = insert_command_lfsr16(wrptr, 0x01, xds);

	/* x display end */

	xde = xds + var->xres;

	wrptr = insert_command_lfsr16(wrptr, 0x03, xde);

	/* y display start */

	yds = var->upper_margin + var->vsync_len;

	wrptr = insert_command_lfsr16(wrptr, 0x05, yds);

	/* y display end */

	yde = yds + var->yres;

	wrptr = insert_command_lfsr16(wrptr, 0x07, yde);

	/* x end count is active + blanking - 1 */

	wrptr = insert_command_lfsr16(wrptr, 0x09, xde + var->right_margin - 1);

	/* libdlo hardcodes hsync start to 1 */

	wrptr = insert_command_lfsr16(wrptr, 0x0B, 1);

	/* hsync end is width of sync pulse + 1 */

	wrptr = insert_command_lfsr16(wrptr, 0x0D, var->hsync_len + 1);

	/* hpixels is active pixels */

	wrptr = insert_command_16(wrptr, 0x0F, var->xres);

	/* yendcount is vertical active + vertical blanking */

	yec = var->yres + var->upper_margin + var->lower_margin +

			var->vsync_len;

	wrptr = insert_command_lfsr16(wrptr, 0x11, yec);

	/* libdlo hardcodes vsync start to 0 */

	wrptr = insert_command_lfsr16(wrptr, 0x13, 0);

	/* vsync end is width of vsync pulse */

	wrptr = insert_command_lfsr16(wrptr, 0x15, var->vsync_len);

	/* vpixels is active pixels */

	wrptr = insert_command_16(wrptr, 0x17, var->yres);

	/* convert picoseconds to 5kHz multiple for pclk5k = x * 1E12/5k */

	wrptr = insert_command_16be(wrptr, 0x1B, 200*1000*1000/var->pixclock);

	return wrptr;

}

/*

 * This takes a standard fbdev screeninfo struct that was fetched or prepared

 * and then generates the appropriate command sequence that then drives the

 * display controller.

 */

static int dlfb_set_video_mode(struct dlfb_data *dev,

				struct fb_var_screeninfo *var)

{

	char *buf;

	char *wrptr;

	int retval = 0;

	int writesize;

	buf = dev->buf;

	/*

	* This first section has to do with setting the base address on the

	* controller * associated with the display. There are 2 base

	* pointers, currently, we only * use the 16 bpp segment.

	*/

	wrptr = insert_vidreg_lock(buf);

	wrptr = insert_set_color_depth(wrptr, 0x00);

	/* set base for 16bpp segment to 0 */

	wrptr = insert_set_base16bpp(wrptr, 0);

	/* set base for 8bpp segment to end of fb */

	wrptr = insert_set_base8bpp(wrptr, dev->info->fix.smem_len);

	wrptr = insert_set_vid_cmds(wrptr, var);

	wrptr = insert_enable_hvsync(wrptr);

	wrptr = insert_vidreg_unlock(wrptr);

	writesize = wrptr - buf;

	mutex_lock(&dev->bulk_mutex);

	if (!dev->interface) {		/* disconnect() was called */

		mutex_unlock(&dev->bulk_mutex);

		retval = -ENODEV;

		goto error;

	}

	retval = dlfb_bulk_msg(dev, writesize);

	mutex_unlock(&dev->bulk_mutex);

	if (retval) {

		dev_err(&dev->udev->dev, "Problem %d with submit write bulk.\n",

					retval);

		goto error;

	}

	return 0;

error:

	return retval;

}

/*

 * This is necessary before we can communicate with the display controller.

 */

static int dlfb_select_std_channel(struct dlfb_data *dev)

{

	int ret;

	u8 set_def_chn[] = {	   0x57, 0xCD, 0xDC, 0xA7,

				0x1C, 0x88, 0x5E, 0x15,

				0x60, 0xFE, 0xC6, 0x97,

				0x16, 0x3D, 0x47, 0xF2  };

	ret = usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0),

			NR_USB_REQUEST_CHANNEL,

			(USB_DIR_OUT | USB_TYPE_VENDOR), 0, 0,

			set_def_chn, sizeof(set_def_chn), USB_CTRL_SET_TIMEOUT);

	return ret;

}

/*

 * Query EDID from the handware, then hand it off to fbdev's edid parse

 * routine which should give us back a filled in screeninfo structure.

 */

static int dlfb_get_var_from_edid(struct dlfb_data *dev,

					struct fb_var_screeninfo *var)

{

	int ret;

	dlfb_edid(dev);

	ret = fb_parse_edid(dev->edid, var);

	return ret;

}

static int dlfb_mmap(struct fb_info *info, struct vm_area_struct *vma)

};

/*

static struct usb_device_id id_table [] = {

	{ USB_DEVICE(0x17e9, 0x023d) },

	{ }

};

 * There are many DisplayLink-based products, all with unique PIDs. We are able

 * to support all volume ones (circa 2009) with a single driver, so we match

 * globally on VID. TODO: Probe() needs to detect when we might be running

 * "future" chips, and bail on those, so a compatible driver can match.

 */

static struct usb_device_id id_table[] = {

	{.idVendor = 0x17e9, .match_flags = USB_DEVICE_ID_MATCH_VENDOR,},

	{},

			if (thistime > 255)

				thistime = 255;

			// find position of first pixel that has changed

			if (dev_info->backing_buffer) {

				/* find first pixel that has changed */

				firstdiff = -1;

				for (j = 0; j < thistime * 2; j++) {

					if (dev_info->backing_buffer

				    [base - dev_info->base16 + j] != data[j]) {

					    [base - dev_info->base16 + j]

					    != data[j]) {

						firstdiff = j / 2;

						break;

					}

				}

			} else {

				firstdiff = 0;

			}

			if (firstdiff >= 0) {

				char *end_of_rle;

			rem -= thistime;

		}

		memcpy(dev_info->backing_buffer + (base - dev_info->base16) -

		if (dev_info->backing_buffer)

			memcpy(dev_info->backing_buffer +

			       (base - dev_info->base16) -

			       (width * 2), data - (width * 2), width * 2);

		base += (dev_info->info->var.xres * 2) - (width * 2);

	for (i = y; i < y + height; i++) {

		if (dev_info->backing_buffer) {

			for (j = 0; j < width * 2; j += 2) {

			dev_info->backing_buffer[base - dev_info->base16 + j] =

				dev_info->backing_buffer

					[base - dev_info->base16 + j] =

					(char)(col >> 8);

			dev_info->backing_buffer[base - dev_info->base16 + j +

						 1] = (char)(col);

				dev_info->backing_buffer

					[base - dev_info->base16 + j + 1] =

					(char)(col);

			}

		}

		if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {

			ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);

			bufptr = dev_info->buf;

	.fb_blank = dlfb_blank,

};

static int

dlfb_probe(struct usb_interface *interface, const struct usb_device_id *id)

static int dlfb_probe(struct usb_interface *interface,

			const struct usb_device_id *id)

{

	struct dlfb_data *dev_info;

	struct device *mydev;

	struct usb_device *usbdev;

	struct dlfb_data *dev;

	struct fb_info *info;

	int videomemorysize;

	unsigned char *videomemory;

	int retval = -ENOMEM;

	struct fb_var_screeninfo *var;

	struct fb_bitfield red = { 11, 5, 0 };

	struct fb_bitfield green = { 5, 6, 0 };

	struct fb_bitfield blue = { 0, 5, 0 };

	int ret;

	char rbuf[4];

	usbdev = usb_get_dev(interface_to_usbdev(interface));

	mydev = &usbdev->dev;

	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);

	if (dev_info == NULL) {

		printk("cannot allocate dev_info structure.\n");

		return -ENOMEM;

	dev = kzalloc(sizeof(*dev), GFP_KERNEL);

	if (dev == NULL) {

		dev_err(mydev, "failed alloc of dev struct\n");

		goto err_devalloc;

	}

	mutex_init(&dev_info->bulk_mutex);

	dev_info->udev = usb_get_dev(interface_to_usbdev(interface));

	dev_info->interface = interface;

	printk("DisplayLink device attached\n");

	mutex_init(&dev->bulk_mutex);

	dev->udev = usbdev;

	dev->interface = interface;

	usb_set_intfdata(interface, dev);

	/* add framebuffer info to usb interface */

	usb_set_intfdata(interface, dev_info);

	dev_info(mydev, "dlfb_probe: setting up DisplayLink device\n");

	dev_info->buf = kmalloc(BUF_SIZE, GFP_KERNEL);

	/* usb_buffer_alloc(dev_info->udev, BUF_SIZE , GFP_KERNEL, &dev_info->tx_urb->transfer_dma); */

	if (dev_info->buf == NULL) {

		printk("unable to allocate memory for dlfb commands\n");

		goto out;

	/*

	 * TODO: replace single 64K buffer with buffer list

	 * and async dispatch

	 */

	dev->buf = kmalloc(BUF_SIZE, GFP_KERNEL);

	if (dev->buf == NULL) {

		dev_err(mydev, "unable to allocate memory for dlfb commands\n");

		goto err_usballoc;

	}

	dev_info->bufend = dev_info->buf + BUF_SIZE;

	dev_info->tx_urb = usb_alloc_urb(0, GFP_KERNEL);

	usb_fill_bulk_urb(dev_info->tx_urb, dev_info->udev,

			  usb_sndbulkpipe(dev_info->udev, 1), dev_info->buf, 0,

			  dlfb_bulk_callback, dev_info);

	dev->bufend = dev->buf + BUF_SIZE;

	ret =

	    usb_control_msg(dev_info->udev, usb_rcvctrlpipe(dev_info->udev, 0),

			    (0x06), (0x80 | (0x02 << 5)), 0, 0, rbuf, 4, 0);

	printk("ret control msg 0: %d %x%x%x%x\n", ret, rbuf[0], rbuf[1],

	       rbuf[2], rbuf[3]);

	dev->tx_urb = usb_alloc_urb(0, GFP_KERNEL);

	usb_fill_bulk_urb(dev->tx_urb, dev->udev,

			  usb_sndbulkpipe(dev->udev, 1), dev->buf, 0,

			  dlfb_bulk_callback, dev);

	dlfb_edid(dev_info);

	/* allocates framebuffer driver structure, not framebuffer memory */

	info = framebuffer_alloc(0, mydev);

	if (!info)

		goto err_fballoc;

	info = framebuffer_alloc(sizeof(u32) * 256, &dev_info->udev->dev);

	dev->info = info;

	info->par = dev;

	info->pseudo_palette = dev->pseudo_palette;

	if (!info) {

		printk("non posso allocare il framebuffer displaylink");

		goto out;

	var = &info->var;

	retval = dlfb_get_var_from_edid(dev, var);

	if (retval) {

		/* had a problem getting edid. so fallback to 640x480 */

		dev_err(mydev, "Problem %d with EDID.\n", retval);

		var->xres = 640;

		var->yres = 480;

	}

	fb_parse_edid(dev_info->edid, &info->var);

	printk("EDID XRES %d YRES %d\n", info->var.xres, info->var.yres);

	if (dlfb_set_video_mode(dev_info, info->var.xres, info->var.yres) != 0) {

		info->var.xres = 1280;

		info->var.yres = 1024;

		if (dlfb_set_video_mode

		    (dev_info, info->var.xres, info->var.yres) != 0) {

			goto out;

		}

	}

	printk("found valid mode...%d\n", info->var.pixclock);

	info->pseudo_palette = info->par;

	info->par = dev_info;

	dev_info->info = info;

	/*

	 * ok, now that we've got the size info, we can alloc our framebuffer.

	 * We are using 16bpp.

	 */

	info->var.bits_per_pixel = 16;

	info->fix = dlfb_fix;

	info->fix.line_length = var->xres * (var->bits_per_pixel / 8);

	videomemorysize = info->fix.line_length * var->yres;

	/*

	 * The big chunk of system memory we use as a virtual framebuffer.

	 * Pages don't need to be set RESERVED (non-swap) immediately on 2.6

	 * remap_pfn_page() syscall in our mmap and/or defio will handle.

	 */

	videomemory = vmalloc(videomemorysize);

	if (!videomemory)

		goto err_vidmem;

	memset(videomemory, 0, videomemorysize);

	info->screen_base = videomemory;

	info->fix.smem_len = PAGE_ALIGN(videomemorysize);

	info->fix.smem_start = (unsigned long) videomemory;

	info->flags =

	    FBINFO_DEFAULT | FBINFO_READS_FAST | FBINFO_HWACCEL_IMAGEBLIT |

	    FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT;

	info->fbops = &dlfb_ops;

	info->screen_base = rvmalloc(dev_info->screen_size);

	if (info->screen_base == NULL) {

		printk

		    ("cannot allocate framebuffer virtual memory of %d bytes\n",

		     dev_info->screen_size);

		goto out0;

	}

	printk("screen base allocated !!!\n");

	dev_info->backing_buffer = kzalloc(dev_info->screen_size, GFP_KERNEL);

	if (!dev_info->backing_buffer)

		printk("non posso allocare il backing buffer\n");

	/* info->var = dev_info->si; */

	/*

	 * Second framebuffer copy, mirroring the state of the framebuffer

	 * on the physical USB device. We can function without this.

	 * But with imperfect damage info we may end up sending pixels over USB

	 * that were, in fact, unchanged -- wasting limited USB bandwidth

	 */

	dev->backing_buffer = vmalloc(dev->screen_size);

	if (!dev->backing_buffer)

		dev_info(mydev, "No backing buffer allocated!\n");

	info->var.bits_per_pixel = 16;

	info->var.activate = FB_ACTIVATE_TEST;

	info->var.vmode = FB_VMODE_NONINTERLACED;

	info->fbops = &dlfb_ops;

	info->var.red.offset = 11;

	info->var.red.length = 5;

	info->var.red.msb_right = 0;

	var->vmode = FB_VMODE_NONINTERLACED;

	var->red = red;

	var->green = green;

	var->blue = blue;

	info->var.green.offset = 5;

	info->var.green.length = 6;

	info->var.green.msb_right = 0;

	/*

	 * TODO: Enable FB_CONFIG_DEFIO support

	info->var.blue.offset = 0;

	info->var.blue.length = 5;

	info->var.blue.msb_right = 0;

	 info->fbdefio = &dlfb_defio;

	 fb_deferred_io_init(info);

	/* info->var.pixclock =  (10000000 / FB_W * 1000 / FB_H)/2 ; */

	 */

	info->fix.smem_start = (unsigned long)info->screen_base;

	info->fix.smem_len = PAGE_ALIGN(dev_info->screen_size);

	if (strlen(dev_info->udev->product) > 15) {

		memcpy(info->fix.id, dev_info->udev->product, 15);

	} else {

		memcpy(info->fix.id, dev_info->udev->product,

		       strlen(dev_info->udev->product));

	retval = fb_alloc_cmap(&info->cmap, 256, 0);

	if (retval < 0) {

		dev_err(mydev, "Failed to allocate colormap\n");

		goto err_cmap;

	}

	info->fix.type = FB_TYPE_PACKED_PIXELS;

	info->fix.visual = FB_VISUAL_TRUECOLOR;

	info->fix.accel = info->flags;

	info->fix.line_length = dev_info->line_length;

	if (fb_alloc_cmap(&info->cmap, 256, 0) < 0)

		goto out1;

	dlfb_select_std_channel(dev);

	dlfb_set_video_mode(dev, var);

	/* TODO: dlfb_dpy_update(dev); */

	printk("colormap allocated\n");

	if (register_framebuffer(info) < 0)

		goto out2;

	retval = register_framebuffer(info);

	if (retval < 0)

		goto err_regfb;

	draw_rect(dev_info, 0, 0, dev_info->info->var.xres,

		  dev_info->info->var.yres, 0x30, 0xff, 0x30);

	/* paint "successful" green screen */

	draw_rect(dev, 0, 0, dev->info->var.xres,

		  dev->info->var.yres, 0x30, 0xff, 0x30);

	dev_info(mydev, "DisplayLink USB device %d now attached, "

			"using %dK of memory\n", info->node,

		 ((dev->backing_buffer) ?

		  videomemorysize * 2 : videomemorysize) >> 10);

	return 0;