1 files changed, 72 insertions, 10 deletions

diff --git a/drivers/video/fbsysfs.c b/drivers/video/fbsysfs.c
index ddc9443254d9..63b505cce4ec 100644
--- a/drivers/video/fbsysfs.c
+++ b/drivers/video/fbsysfs.c
@@ -242,10 +242,68 @@ static ssize_t show_virtual(struct class_device *class_device, char *buf)
                         fb_info->var.yres_virtual);
 }
 
-static ssize_t store_cmap(struct class_device *class_device, const char * buf,
+/* Format for cmap is "%02x%c%4x%4x%4x\n" */
+/* %02x entry %c transp %4x red %4x blue %4x green \n */
+/* 255 rows at 16 chars equals 4096 */
+/* PAGE_SIZE can be 4096 or larger */
+static ssize_t store_cmap(struct class_device *class_device, const char *buf,
                           size_t count)
 {
-//      struct fb_info *fb_info = (struct fb_info *)class_get_devdata(class_device);
+        struct fb_info *fb_info = (struct fb_info *)class_get_devdata(class_device);
+        int rc, i, start, length, transp = 0;
+
+        if ((count > 4096) || ((count % 16) != 0) || (PAGE_SIZE < 4096))
+                return -EINVAL;
+
+        if (!fb_info->fbops->fb_setcolreg && !fb_info->fbops->fb_setcmap)
+                return -EINVAL;
+
+        sscanf(buf, "%02x", &start);
+        length = count / 16;
+
+        for (i = 0; i < length; i++)
+                if (buf[i * 16 + 2] != ' ')
+                        transp = 1;
+
+        /* If we can batch, do it */
+        if (fb_info->fbops->fb_setcmap && length > 1) {
+                struct fb_cmap umap;
+
+                memset(&umap, 0, sizeof(umap));
+                if ((rc = fb_alloc_cmap(&umap, length, transp)))
+                        return rc;
+
+                umap.start = start;
+                for (i = 0; i < length; i++) {
+                        sscanf(&buf[i * 16 +  3], "%4hx", &umap.red[i]);
+                        sscanf(&buf[i * 16 +  7], "%4hx", &umap.blue[i]);
+                        sscanf(&buf[i * 16 + 11], "%4hx", &umap.green[i]);
+                        if (transp)
+                                umap.transp[i] = (buf[i * 16 +  2] != ' ');
+                }
+                rc = fb_info->fbops->fb_setcmap(&umap, fb_info);
+                fb_copy_cmap(&umap, &fb_info->cmap);
+                fb_dealloc_cmap(&umap);
+
+                return rc;
+        }
+        for (i = 0; i < length; i++) {
+                u16 red, blue, green, tsp;
+
+                sscanf(&buf[i * 16 +  3], "%4hx", &red);
+                sscanf(&buf[i * 16 +  7], "%4hx", &blue);
+                sscanf(&buf[i * 16 + 11], "%4hx", &green);
+                tsp = (buf[i * 16 +  2] != ' ');
+                if ((rc = fb_info->fbops->fb_setcolreg(start++,
+                                      red, green, blue, tsp, fb_info)))
+                        return rc;
+
+                fb_info->cmap.red[i] = red;
+                fb_info->cmap.blue[i] = blue;
+                fb_info->cmap.green[i] = green;
+                if (transp)
+                        fb_info->cmap.transp[i] = tsp;
+        }
         return 0;
 }
 
@@ -253,20 +311,24 @@ static ssize_t show_cmap(struct class_device *class_device, char *buf)
 {
         struct fb_info *fb_info =
                 (struct fb_info *)class_get_devdata(class_device);
-        unsigned int offset = 0, i;
+        unsigned int i;
 
         if (!fb_info->cmap.red || !fb_info->cmap.blue ||
-            !fb_info->cmap.green || !fb_info->cmap.transp)
+           !fb_info->cmap.green)
+                return -EINVAL;
+
+        if (PAGE_SIZE < 4096)
                 return -EINVAL;
 
+        /* don't mess with the format, the buffer is PAGE_SIZE */
+        /* 255 entries at 16 chars per line equals 4096 = PAGE_SIZE */
         for (i = 0; i < fb_info->cmap.len; i++) {
-                offset += snprintf(buf, PAGE_SIZE - offset,
-                                   "%d,%d,%d,%d,%d\n", i + fb_info->cmap.start,
-                                   fb_info->cmap.red[i], fb_info->cmap.blue[i],
-                                   fb_info->cmap.green[i],
-                                   fb_info->cmap.transp[i]);
+                sprintf(&buf[ i * 16], "%02x%c%4x%4x%4x\n", i + fb_info->cmap.start,
+                        ((fb_info->cmap.transp && fb_info->cmap.transp[i]) ? '*' : ' '),
+                        fb_info->cmap.red[i], fb_info->cmap.blue[i],
+                        fb_info->cmap.green[i]);
         }
-        return offset;
+        return 4096;
 }
 
 static ssize_t store_blank(struct class_device *class_device, const char * buf,