2 files changed, 312 insertions, 95 deletions

diff --git a/drivers/staging/udlfb/udlfb.c b/drivers/staging/udlfb/udlfb.c
index c9ac687..6977b71 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 7c46477..d28e783 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)