staging: udlfb: enhance EDID and mode handling support

Enhance EDID and mode table handling

* Adds support for lower-end DL chips on higher-end monitors
* Adds support for using fbdev standard EDID parsing and handling
* Adds support for more dynamic reallocation of framebuffer, based on new EDID

Queries EDID from hardware with several retries then
queries DisplayLink chip for max pixel area via vendor descriptor.
Uses existing fbdev mode handling service funcations to build mode list
of common modes between adapter and monitor.
This enables lower-end chips (e.g. DL-125) to successfully pick a compatible
mode on higher end monitors (e.g. whose default mode is over 1280x1024).
Reallocate framebuffer if necessary in response to new max mode.

Signed-off-by: Bernie Thompson <bernie@plugable.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

2 files changed, 312 insertions, 95 deletions

diff --git a/drivers/staging/udlfb/udlfb.c b/drivers/staging/udlfb/udlfb.c
index c9ac6879e933..6977b71e8c3c 100644
--- a/drivers/staging/udlfb/udlfb.c
+++ b/drivers/staging/udlfb/udlfb.c
@@ -696,19 +696,32 @@ static void dlfb_ops_fillrect(struct fb_info *info,
 
 }
 
-static void dlfb_get_edid(struct dlfb_data *dev)
+static int dlfb_get_edid(struct dlfb_data *dev, char *edid, int len)
 {
 	int i;
 	int ret;
-	char rbuf[2];
+	char *rbuf;
 
-	for (i = 0; i < sizeof(dev->edid); i++) {
+	rbuf = kmalloc(2, GFP_KERNEL);
+	if (!rbuf)
+		return 0;
+
+	for (i = 0; i < len; i++) {
 		ret = usb_control_msg(dev->udev,
 				    usb_rcvctrlpipe(dev->udev, 0), (0x02),
 				    (0x80 | (0x02 << 5)), i << 8, 0xA1, rbuf, 2,
-				    0);
-		dev->edid[i] = rbuf[1];
+				    HZ);
+		if (ret < 1) {
+			dl_err("Read EDID byte %d failed err %x\n", i, ret);
+			i--;
+			break;
+		}
+		edid[i] = rbuf[1];
 	}
+
+	kfree(rbuf);
+
+	return i;
 }
 
 static int dlfb_ops_ioctl(struct fb_info *info, unsigned int cmd,
@@ -724,8 +737,7 @@ static int dlfb_ops_ioctl(struct fb_info *info, unsigned int cmd,
 	/* TODO: Update X server to get this from sysfs instead */
 	if (cmd == DLFB_IOCTL_RETURN_EDID) {
 		char *edid = (char *)arg;
-		dlfb_get_edid(dev);
-		if (copy_to_user(edid, dev->edid, sizeof(dev->edid)))
+		if (copy_to_user(edid, dev->edid, dev->edid_size))
 			return -EFAULT;
 		return 0;
 	}
@@ -910,8 +922,13 @@ static int dlfb_is_valid_mode(struct fb_videomode *mode,
 {
 	struct dlfb_data *dev = info->par;
 
-	if (mode->xres * mode->yres > dev->sku_pixel_limit)
+	if (mode->xres * mode->yres > dev->sku_pixel_limit) {
+		dl_warn("%dx%d beyond chip capabilities\n",
+		       mode->xres, mode->yres);
 		return 0;
+	}
+
+	dl_info("%dx%d valid mode\n", mode->xres, mode->yres);
 
 	return 1;
 }
@@ -951,10 +968,27 @@ static int dlfb_ops_check_var(struct fb_var_screeninfo *var,
 static int dlfb_ops_set_par(struct fb_info *info)
 {
 	struct dlfb_data *dev = info->par;
+	int result;
+	u16 *pix_framebuffer;
+	int i;
 
 	dl_notice("set_par mode %dx%d\n", info->var.xres, info->var.yres);
 
-	return dlfb_set_video_mode(dev, &info->var);
+	result = dlfb_set_video_mode(dev, &info->var);
+
+	if ((result == 0) && (dev->fb_count == 0)) {
+
+		/* paint greenscreen */
+
+		pix_framebuffer = (u16 *) info->screen_base;
+		for (i = 0; i < info->fix.smem_len / 2; i++)
+			pix_framebuffer[i] = 0x37e6;
+
+		dlfb_handle_damage(dev, 0, 0, info->var.xres, info->var.yres,
+				   info->screen_base);
+	}
+
+	return result;
 }
 
 /*
@@ -1003,46 +1037,166 @@ static struct fb_ops dlfb_ops = {
 	.fb_set_par = dlfb_ops_set_par,
 };
 
+
+/*
+ * Assumes &info->lock held by caller
+ * Assumes no active clients have framebuffer open
+ */
+static int dlfb_realloc_framebuffer(struct dlfb_data *dev, struct fb_info *info)
+{
+	int retval = -ENOMEM;
+	int old_len = info->fix.smem_len;
+	int new_len;
+	unsigned char *old_fb = info->screen_base;
+	unsigned char *new_fb;
+	unsigned char *new_back;
+
+	dl_warn("Reallocating framebuffer. Addresses will change!\n");
+
+	new_len = info->fix.line_length * info->var.yres;
+
+	if (PAGE_ALIGN(new_len) > old_len) {
+		/*
+		 * Alloc system memory for virtual framebuffer
+		 */
+		new_fb = vmalloc(new_len);
+		if (!new_fb) {
+			dl_err("Virtual framebuffer alloc failed\n");
+			goto error;
+		}
+
+		if (info->screen_base) {
+			memcpy(new_fb, old_fb, old_len);
+			vfree(info->screen_base);
+		}
+
+		info->screen_base = new_fb;
+		info->fix.smem_len = PAGE_ALIGN(new_len);
+		info->fix.smem_start = (unsigned long) new_fb;
+		info->flags = udlfb_info_flags;
+
+		/*
+		 * Second framebuffer copy to mirror the framebuffer state
+		 * on the physical USB device. We can function without this.
+		 * But with imperfect damage info we may send pixels over USB
+		 * that were, in fact, unchanged - wasting limited USB bandwidth
+		 */
+		new_back = vmalloc(new_len);
+		if (!new_back)
+			dl_info("No shadow/backing buffer allcoated\n");
+		else {
+			if (dev->backing_buffer)
+				vfree(dev->backing_buffer);
+			dev->backing_buffer = new_back;
+			memset(dev->backing_buffer, 0, new_len);
+		}
+	}
+
+	retval = 0;
+
+error:
+	return retval;
+}
+
 /*
- * Calls dlfb_get_edid() to query the EDID of attached monitor via usb cmds
- * Then parses EDID into three places used by various parts of fbdev:
+ * 1) Get EDID from hw, or use sw default
+ * 2) Parse into various fb_info structs
+ * 3) Allocate virtual framebuffer memory to back highest res mode
+ *
+ * Parses EDID into three places used by various parts of fbdev:
  * fb_var_screeninfo contains the timing of the monitor's preferred mode
  * fb_info.monspecs is full parsed EDID info, including monspecs.modedb
  * fb_info.modelist is a linked list of all monitor & VESA modes which work
  *
  * If EDID is not readable/valid, then modelist is all VESA modes,
  * monspecs is NULL, and fb_var_screeninfo is set to safe VESA mode
- * Returns 0 if EDID parses successfully
+ * Returns 0 if successful
  */
-static int dlfb_parse_edid(struct dlfb_data *dev,
-			    struct fb_var_screeninfo *var,
-			    struct fb_info *info)
+static int dlfb_setup_modes(struct dlfb_data *dev,
+			   struct fb_info *info,
+			   char *default_edid, size_t default_edid_size)
 {
 	int i;
 	const struct fb_videomode *default_vmode = NULL;
 	int result = 0;
+	char *edid;
+	int tries = 3;
+
+	if (info->dev) /* only use mutex if info has been registered */
+		mutex_lock(&info->lock);
+
+	edid = kmalloc(MAX_EDID_SIZE, GFP_KERNEL);
+	if (!edid) {
+		result = -ENOMEM;
+		goto error;
+	}
 
 	fb_destroy_modelist(&info->modelist);
 	memset(&info->monspecs, 0, sizeof(info->monspecs));
 
-	dlfb_get_edid(dev);
-	fb_edid_to_monspecs(dev->edid, &info->monspecs);
+	/*
+	 * Try to (re)read EDID from hardware first
+	 * EDID data may return, but not parse as valid
+	 * Try again a few times, in case of e.g. analog cable noise
+	 */
+	while (tries--) {
+
+		i = dlfb_get_edid(dev, edid, MAX_EDID_SIZE);
+
+		if (i >= MIN_EDID_SIZE)
+			fb_edid_to_monspecs(edid, &info->monspecs);
+
+		if (info->monspecs.modedb_len > 0) {
+			dev->edid = edid;
+			dev->edid_size = i;
+			break;
+		}
+	}
 
+	/* If that fails, use a previously returned EDID if available */
+	if (info->monspecs.modedb_len == 0) {
+
+		dl_err("Unable to get valid EDID from device/display\n");
+
+		if (dev->edid) {
+			fb_edid_to_monspecs(dev->edid, &info->monspecs);
+			if (info->monspecs.modedb_len > 0)
+				dl_err("Using previously queried EDID\n");
+		}
+	}
+
+	/* If that fails, use the default EDID we were handed */
+	if (info->monspecs.modedb_len == 0) {
+		if (default_edid_size >= MIN_EDID_SIZE) {
+			fb_edid_to_monspecs(default_edid, &info->monspecs);
+			if (info->monspecs.modedb_len > 0) {
+				memcpy(edid, default_edid, default_edid_size);
+				dev->edid = edid;
+				dev->edid_size = default_edid_size;
+				dl_err("Using default/backup EDID\n");
+			}
+		}
+	}
+
+	/* If we've got modes, let's pick a best default mode */
 	if (info->monspecs.modedb_len > 0) {
 
 		for (i = 0; i < info->monspecs.modedb_len; i++) {
 			if (dlfb_is_valid_mode(&info->monspecs.modedb[i], info))
 				fb_add_videomode(&info->monspecs.modedb[i],
 					&info->modelist);
+			else /* if we've removed top/best mode */
+				info->monspecs.misc &= ~FB_MISC_1ST_DETAIL;
 		}
 
 		default_vmode = fb_find_best_display(&info->monspecs,
 						     &info->modelist);
-	} else {
-		struct fb_videomode fb_vmode = {0};
+	}
 
-		dl_err("Unable to get valid EDID from device/display\n");
-		result = 1;
+	/* If everything else has failed, fall back to safe default mode */
+	if (default_vmode == NULL) {
+
+		struct fb_videomode fb_vmode = {0};
 
 		/*
 		 * Add the standard VESA modes to our modelist
@@ -1068,8 +1222,30 @@ static int dlfb_parse_edid(struct dlfb_data *dev,
 						     &info->modelist);
 	}
 
-	fb_videomode_to_var(var, default_vmode);
-	dlfb_var_color_format(var);
+	/* If we have good mode and no active clients*/
+	if ((default_vmode != NULL) && (dev->fb_count == 0)) {
+
+		fb_videomode_to_var(&info->var, default_vmode);
+		dlfb_var_color_format(&info->var);
+
+		/*
+		 * with mode size info, we can now alloc our framebuffer.
+		 */
+		memcpy(&info->fix, &dlfb_fix, sizeof(dlfb_fix));
+		info->fix.line_length = info->var.xres *
+			(info->var.bits_per_pixel / 8);
+
+		result = dlfb_realloc_framebuffer(dev, info);
+
+	} else
+		result = -EINVAL;
+
+error:
+	if (edid && (dev->edid != edid))
+		kfree(edid);
+
+	if (info->dev)
+		mutex_unlock(&info->lock);
 
 	return result;
 }
@@ -1106,24 +1282,27 @@ static ssize_t metrics_cpu_kcycles_used_show(struct device *fbdev,
 			atomic_read(&dev->cpu_kcycles_used));
 }
 
-static ssize_t edid_show(struct file *filp, struct kobject *kobj,
-			 struct bin_attribute *a,
+static ssize_t edid_show(
+			struct file *filp,
+			struct kobject *kobj, struct bin_attribute *a,
 			 char *buf, loff_t off, size_t count) {
 	struct device *fbdev = container_of(kobj, struct device, kobj);
 	struct fb_info *fb_info = dev_get_drvdata(fbdev);
 	struct dlfb_data *dev = fb_info->par;
-	char *edid = &dev->edid[0];
-	const size_t size = sizeof(dev->edid);
 
-	if (dlfb_parse_edid(dev, &fb_info->var, fb_info))
+	if (dev->edid == NULL)
 		return 0;
 
-	if (off >= size)
+	if ((off >= dev->edid_size) || (count > dev->edid_size))
 		return 0;
 
-	if (off + count > size)
-		count = size - off;
-	memcpy(buf, edid + off, count);
+	if (off + count > dev->edid_size)
+		count = dev->edid_size - off;
+
+	dl_info("sysfs edid copy %p to %p, %d bytes\n",
+		dev->edid, buf, (int) count);
+
+	memcpy(buf, dev->edid, count);
 
 	return count;
 }
@@ -1261,19 +1440,82 @@ static int dlfb_select_std_channel(struct dlfb_data *dev)
 	return ret;
 }
 
+static int dlfb_parse_vendor_descriptor(struct dlfb_data *dev,
+					struct usb_device *usbdev)
+{
+	char *desc;
+	char *buf;
+	char *desc_end;
+
+	u8 total_len = 0;
+
+	buf = kzalloc(MAX_VENDOR_DESCRIPTOR_SIZE, GFP_KERNEL);
+	if (!buf)
+		return false;
+	desc = buf;
+
+	total_len = usb_get_descriptor(usbdev, 0x5f, /* vendor specific */
+				    0, desc, MAX_VENDOR_DESCRIPTOR_SIZE);
+	if (total_len > 5) {
+		dl_info("vendor descriptor length:%x data:%02x %02x %02x %02x" \
+			"%02x %02x %02x %02x %02x %02x %02x\n",
+			total_len, desc[0],
+			desc[1], desc[2], desc[3], desc[4], desc[5], desc[6],
+			desc[7], desc[8], desc[9], desc[10]);
+
+		if ((desc[0] != total_len) || /* descriptor length */
+		    (desc[1] != 0x5f) ||   /* vendor descriptor type */
+		    (desc[2] != 0x01) ||   /* version (2 bytes) */
+		    (desc[3] != 0x00) ||
+		    (desc[4] != total_len - 2)) /* length after type */
+			goto unrecognized;
+
+		desc_end = desc + total_len;
+		desc += 5; /* the fixed header we've already parsed */
+
+		while (desc < desc_end) {
+			u8 length;
+			u16 key;
+
+			key = *((u16 *) desc);
+			desc += sizeof(u16);
+			length = *desc;
+			desc++;
+
+			switch (key) {
+			case 0x0200: { /* max_area */
+				u32 max_area;
+				max_area = le32_to_cpu(*((u32 *)desc));
+				dl_warn("DL chip limited to %d pixel modes\n",
+					max_area);
+				dev->sku_pixel_limit = max_area;
+				break;
+			}
+			default:
+				break;
+			}
+			desc += length;
+		}
+	}
+
+	goto success;
 
+unrecognized:
+	/* allow udlfb to load for now even if firmware unrecognized */
+	dl_err("Unrecognized vendor firmware descriptor\n");
+
+success:
+	kfree(buf);
+	return true;
+}
 static int dlfb_usb_probe(struct usb_interface *interface,
 			const struct usb_device_id *id)
 {
 	struct usb_device *usbdev;
-	struct dlfb_data *dev;
+	struct dlfb_data *dev = 0;
 	struct fb_info *info = 0;
-	int videomemorysize;
 	int i;
-	unsigned char *videomemory;
 	int retval = -ENOMEM;
-	struct fb_var_screeninfo *var;
-	u16 *pix_framebuffer;
 
 	/* usb initialization */
 
@@ -1287,12 +1529,25 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 
 	/* we need to wait for both usb and fbdev to spin down on disconnect */
 	kref_init(&dev->kref); /* matching kref_put in usb .disconnect fn */
-	kref_get(&dev->kref); /* matching kref_put in .fb_destroy function*/
+	kref_get(&dev->kref); /* matching kref_put in free_framebuffer_work */
 
 	dev->udev = usbdev;
 	dev->gdev = &usbdev->dev; /* our generic struct device * */
 	usb_set_intfdata(interface, dev);
 
+	dl_info("%s %s - serial #%s\n",
+		usbdev->manufacturer, usbdev->product, usbdev->serial);
+	dl_info("vid_%04x&pid_%04x&rev_%04x driver's dlfb_data struct at %p\n",
+		usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
+		usbdev->descriptor.bcdDevice, dev);
+
+	dev->sku_pixel_limit = 2048 * 1152; /* default to maximum */
+
+	if (!dlfb_parse_vendor_descriptor(dev, usbdev)) {
+		dl_err("firmware not recognized. Assume incompatible device\n");
+		goto error;
+	}
+
 	if (!dlfb_alloc_urb_list(dev, WRITES_IN_FLIGHT, MAX_TRANSFER)) {
 		retval = -ENOMEM;
 		dl_err("dlfb_alloc_urb_list failed\n");
@@ -1314,50 +1569,6 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 	info->pseudo_palette = dev->pseudo_palette;
 	info->fbops = &dlfb_ops;
 
-	var = &info->var;
-
-	/* TODO set limit based on actual SKU detection */
-	dev->sku_pixel_limit = 2048 * 1152;
-
-	INIT_LIST_HEAD(&info->modelist);
-	dlfb_parse_edid(dev, var, info);
-
-	/*
-	 * ok, now that we've got the size info, we can alloc our framebuffer.
-	 */
-	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.
-	 * TODO: Handle fbcon cursor code calling blit in interrupt context
-	 */
-	videomemory = vmalloc(videomemorysize);
-	if (!videomemory) {
-		retval = -ENOMEM;
-		dl_err("Virtual framebuffer alloc failed\n");
-		goto error;
-	}
-
-	info->screen_base = videomemory;
-	info->fix.smem_len = PAGE_ALIGN(videomemorysize);
-	info->fix.smem_start = (unsigned long) videomemory;
-	info->flags = udlfb_info_flags;
-
-
-	/*
-	 * 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(videomemorysize);
-	if (!dev->backing_buffer)
-		dl_warn("No shadow/backing buffer allcoated\n");
-	else
-		memset(dev->backing_buffer, 0, videomemorysize);
-
 	retval = fb_alloc_cmap(&info->cmap, 256, 0);
 	if (retval < 0) {
 		dl_err("fb_alloc_cmap failed %x\n", retval);
@@ -1367,6 +1578,14 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 	INIT_DELAYED_WORK(&dev->free_framebuffer_work,
 			  dlfb_free_framebuffer_work);
 
+	INIT_LIST_HEAD(&info->modelist);
+
+	retval = dlfb_setup_modes(dev, info, NULL, 0);
+	if (retval != 0) {
+		dl_err("unable to find common mode for display and adapter\n");
+		goto error;
+	}
+
 	/* ready to begin using device */
 
 #ifdef CONFIG_FB_DEFERRED_IO
@@ -1375,17 +1594,9 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 	atomic_set(&dev->usb_active, 1);
 	dlfb_select_std_channel(dev);
 
-	dlfb_ops_check_var(var, info);
+	dlfb_ops_check_var(&info->var, info);
 	dlfb_ops_set_par(info);
 
-	/* paint greenscreen */
-	pix_framebuffer = (u16 *) videomemory;
-	for (i = 0; i < videomemorysize / 2; i++)
-		pix_framebuffer[i] = 0x37e6;
-
-	dlfb_handle_damage(dev, 0, 0, info->var.xres, info->var.yres,
-				videomemory);
-
 	retval = register_framebuffer(info);
 	if (retval < 0) {
 		dl_err("register_framebuffer failed %d\n", retval);
@@ -1397,11 +1608,11 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 
 	device_create_bin_file(info->dev, &edid_attr);
 
-	dl_err("DisplayLink USB device /dev/fb%d attached. %dx%d resolution."
+	dl_info("DisplayLink USB device /dev/fb%d attached. %dx%d resolution."
 			" Using %dK framebuffer memory\n", info->node,
-			var->xres, var->yres,
+			info->var.xres, info->var.yres,
 			((dev->backing_buffer) ?
-			videomemorysize * 2 : videomemorysize) >> 10);
+			info->fix.smem_len * 2 : info->fix.smem_len) >> 10);
 	return 0;
 
 error:
diff --git a/drivers/staging/udlfb/udlfb.h b/drivers/staging/udlfb/udlfb.h
index 7c46477af2a2..d28e783482de 100644
--- a/drivers/staging/udlfb/udlfb.h
+++ b/drivers/staging/udlfb/udlfb.h
@@ -44,7 +44,8 @@ struct dlfb_data {
 	atomic_t usb_active; /* 0 = update virtual buffer, but no usb traffic */
 	atomic_t lost_pixels; /* 1 = a render op failed. Need screen refresh */
 	atomic_t use_defio; /* 0 = rely on ioctls and blit/copy/fill rects */
-	char edid[128];
+	char *edid; /* null until we read edid from hw or get from sysfs */
+	size_t edid_size;
 	int sku_pixel_limit;
 	int base16;
 	int base8;
@@ -64,6 +65,11 @@ struct dlfb_data {
 #define MAX_TRANSFER (PAGE_SIZE*16 - BULK_SIZE)
 #define WRITES_IN_FLIGHT (4)
 
+#define MIN_EDID_SIZE 128
+#define MAX_EDID_SIZE 128
+
+#define MAX_VENDOR_DESCRIPTOR_SIZE 256
+
 #define GET_URB_TIMEOUT	HZ
 #define FREE_URB_TIMEOUT (HZ*2)