1 files changed, 92 insertions, 39 deletions

diff --git a/drivers/video/fbdev/ssd1307fb.c b/drivers/video/fbdev/ssd1307fb.c
index b674948e3bb8..78ca7ffc40c2 100644
--- a/drivers/video/fbdev/ssd1307fb.c
+++ b/drivers/video/fbdev/ssd1307fb.c
@@ -28,6 +28,7 @@
 #define SSD1307FB_SET_COL_RANGE		0x21
 #define SSD1307FB_SET_PAGE_RANGE	0x22
 #define SSD1307FB_CONTRAST		0x81
+#define SSD1307FB_SET_LOOKUP_TABLE	0x91
 #define	SSD1307FB_CHARGE_PUMP		0x8d
 #define SSD1307FB_SEG_REMAP_ON		0xa1
 #define SSD1307FB_DISPLAY_OFF		0xae
@@ -36,6 +37,7 @@
 #define SSD1307FB_START_PAGE_ADDRESS	0xb0
 #define SSD1307FB_SET_DISPLAY_OFFSET	0xd3
 #define	SSD1307FB_SET_CLOCK_FREQ	0xd5
+#define	SSD1307FB_SET_AREA_COLOR_MODE	0xd8
 #define	SSD1307FB_SET_PRECHARGE_PERIOD	0xd9
 #define	SSD1307FB_SET_COM_PINS_CONFIG	0xda
 #define	SSD1307FB_SET_VCOMH		0xdb
@@ -58,10 +60,14 @@ struct ssd1307fb_deviceinfo {
 };
 
 struct ssd1307fb_par {
-	u32 com_invdir;
-	u32 com_lrremap;
+	unsigned area_color_enable : 1;
+	unsigned com_invdir : 1;
+	unsigned com_lrremap : 1;
+	unsigned com_seq : 1;
+	unsigned lookup_table_set : 1;
+	unsigned low_power : 1;
+	unsigned seg_remap : 1;
 	u32 com_offset;
-	u32 com_seq;
 	u32 contrast;
 	u32 dclk_div;
 	u32 dclk_frq;
@@ -69,6 +75,7 @@ struct ssd1307fb_par {
 	struct i2c_client *client;
 	u32 height;
 	struct fb_info *info;
+	u8 lookup_table[4];
 	u32 page_offset;
 	u32 prechargep1;
 	u32 prechargep2;
@@ -76,7 +83,6 @@ struct ssd1307fb_par {
 	u32 pwm_period;
 	struct gpio_desc *reset;
 	struct regulator *vbat_reg;
-	u32 seg_remap;
 	u32 vcomh;
 	u32 width;
 };
@@ -98,6 +104,9 @@ static const struct fb_fix_screeninfo ssd1307fb_fix = {
 
 static const struct fb_var_screeninfo ssd1307fb_var = {
 	.bits_per_pixel	= 1,
+	.red = { .length = 1 },
+	.green = { .length = 1 },
+	.blue = { .length = 1 },
 };
 
 static struct ssd1307fb_array *ssd1307fb_alloc_array(u32 len, u8 type)
@@ -149,11 +158,12 @@ static inline int ssd1307fb_write_cmd(struct i2c_client *client, u8 cmd)
 static void ssd1307fb_update_display(struct ssd1307fb_par *par)
 {
 	struct ssd1307fb_array *array;
-	u8 *vmem = par->info->screen_base;
+	u8 *vmem = par->info->screen_buffer;
+	unsigned int line_length = par->info->fix.line_length;
+	unsigned int pages = DIV_ROUND_UP(par->height, 8);
 	int i, j, k;
 
-	array = ssd1307fb_alloc_array(par->width * par->height / 8,
-				      SSD1307FB_DATA);
+	array = ssd1307fb_alloc_array(par->width * pages, SSD1307FB_DATA);
 	if (!array)
 		return;
 
@@ -186,22 +196,24 @@ static void ssd1307fb_update_display(struct ssd1307fb_par *par)
 	 *  (5) A4 B4 C4 D4 E4 F4 G4 H4
 	 */
 
-	for (i = 0; i < (par->height / 8); i++) {
+	for (i = 0; i < pages; i++) {
 		for (j = 0; j < par->width; j++) {
+			int m = 8;
 			u32 array_idx = i * par->width + j;
 			array->data[array_idx] = 0;
-			for (k = 0; k < 8; k++) {
-				u32 page_length = par->width * i;
-				u32 index = page_length + (par->width * k + j) / 8;
-				u8 byte = *(vmem + index);
-				u8 bit = byte & (1 << (j % 8));
-				bit = bit >> (j % 8);
+			/* Last page may be partial */
+			if (i + 1 == pages && par->height % 8)
+				m = par->height % 8;
+			for (k = 0; k < m; k++) {
+				u8 byte = vmem[(8 * i + k) * line_length +
+					       j / 8];
+				u8 bit = (byte >> (j % 8)) & 1;
 				array->data[array_idx] |= bit << k;
 			}
 		}
 	}
 
-	ssd1307fb_write_array(par->client, array, par->width * par->height / 8);
+	ssd1307fb_write_array(par->client, array, par->width * pages);
 	kfree(array);
 }
 
@@ -212,7 +224,7 @@ static ssize_t ssd1307fb_write(struct fb_info *info, const char __user *buf,
 	struct ssd1307fb_par *par = info->par;
 	unsigned long total_size;
 	unsigned long p = *ppos;
-	u8 __iomem *dst;
+	void *dst;
 
 	total_size = info->fix.smem_len;
 
@@ -225,7 +237,7 @@ static ssize_t ssd1307fb_write(struct fb_info *info, const char __user *buf,
 	if (!count)
 		return -EINVAL;
 
-	dst = (void __force *) (info->screen_base + p);
+	dst = info->screen_buffer + p;
 
 	if (copy_from_user(dst, buf, count))
 		return -EFAULT;
@@ -312,7 +324,7 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 
 		dev_dbg(&par->client->dev, "Using PWM%d with a %dns period.\n",
 			par->pwm->pwm, par->pwm_period);
-	};
+	}
 
 	/* Set initial contrast */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_CONTRAST);
@@ -328,10 +340,10 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 		ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SEG_REMAP_ON);
 		if (ret < 0)
 			return ret;
-	};
+	}
 
 	/* Set COM direction */
-	com_invdir = 0xc0 | (par->com_invdir & 0x1) << 3;
+	com_invdir = 0xc0 | par->com_invdir << 3;
 	ret = ssd1307fb_write_cmd(par->client,  com_invdir);
 	if (ret < 0)
 		return ret;
@@ -364,6 +376,22 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
+	/* Set Set Area Color Mode ON/OFF & Low Power Display Mode */
+	if (par->area_color_enable || par->low_power) {
+		u32 mode;
+
+		ret = ssd1307fb_write_cmd(par->client,
+					  SSD1307FB_SET_AREA_COLOR_MODE);
+		if (ret < 0)
+			return ret;
+
+		mode = (par->area_color_enable ? 0x30 : 0) |
+			(par->low_power ? 5 : 0);
+		ret = ssd1307fb_write_cmd(par->client, mode);
+		if (ret < 0)
+			return ret;
+	}
+
 	/* Set precharge period in number of ticks from the internal clock */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SET_PRECHARGE_PERIOD);
 	if (ret < 0)
@@ -379,8 +407,7 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	compins = 0x02 | !(par->com_seq & 0x1) << 4
-				   | (par->com_lrremap & 0x1) << 5;
+	compins = 0x02 | !par->com_seq << 4 | par->com_lrremap << 5;
 	ret = ssd1307fb_write_cmd(par->client, compins);
 	if (ret < 0)
 		return ret;
@@ -404,6 +431,28 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
+	/* Set lookup table */
+	if (par->lookup_table_set) {
+		int i;
+
+		ret = ssd1307fb_write_cmd(par->client,
+					  SSD1307FB_SET_LOOKUP_TABLE);
+		if (ret < 0)
+			return ret;
+
+		for (i = 0; i < ARRAY_SIZE(par->lookup_table); ++i) {
+			u8 val = par->lookup_table[i];
+
+			if (val < 31 || val > 63)
+				dev_warn(&par->client->dev,
+					 "lookup table index %d value out of range 31 <= %d <= 63\n",
+					 i, val);
+			ret = ssd1307fb_write_cmd(par->client, val);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
 	/* Switch to horizontal addressing mode */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SET_ADDRESS_MODE);
 	if (ret < 0)
@@ -432,12 +481,13 @@ static int ssd1307fb_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x0);
+	ret = ssd1307fb_write_cmd(par->client, par->page_offset);
 	if (ret < 0)
 		return ret;
 
 	ret = ssd1307fb_write_cmd(par->client,
-				  par->page_offset + (par->height / 8) - 1);
+				  par->page_offset +
+				  DIV_ROUND_UP(par->height, 8) - 1);
 	if (ret < 0)
 		return ret;
 
@@ -546,7 +596,7 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	struct fb_deferred_io *ssd1307fb_defio;
 	u32 vmem_size;
 	struct ssd1307fb_par *par;
-	u8 *vmem;
+	void *vmem;
 	int ret;
 
 	if (!node) {
@@ -603,19 +653,29 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	if (of_property_read_u32(node, "solomon,prechargep2", &par->prechargep2))
 		par->prechargep2 = 2;
 
+	if (!of_property_read_u8_array(node, "solomon,lookup-table",
+				       par->lookup_table,
+				       ARRAY_SIZE(par->lookup_table)))
+		par->lookup_table_set = 1;
+
 	par->seg_remap = !of_property_read_bool(node, "solomon,segment-no-remap");
 	par->com_seq = of_property_read_bool(node, "solomon,com-seq");
 	par->com_lrremap = of_property_read_bool(node, "solomon,com-lrremap");
 	par->com_invdir = of_property_read_bool(node, "solomon,com-invdir");
+	par->area_color_enable =
+		of_property_read_bool(node, "solomon,area-color-enable");
+	par->low_power = of_property_read_bool(node, "solomon,low-power");
 
 	par->contrast = 127;
 	par->vcomh = par->device_info->default_vcomh;
 
 	/* Setup display timing */
-	par->dclk_div = par->device_info->default_dclk_div;
-	par->dclk_frq = par->device_info->default_dclk_frq;
+	if (of_property_read_u32(node, "solomon,dclk-div", &par->dclk_div))
+		par->dclk_div = par->device_info->default_dclk_div;
+	if (of_property_read_u32(node, "solomon,dclk-frq", &par->dclk_frq))
+		par->dclk_frq = par->device_info->default_dclk_frq;
 
-	vmem_size = par->width * par->height / 8;
+	vmem_size = DIV_ROUND_UP(par->width, 8) * par->height;
 
 	vmem = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
 					get_order(vmem_size));
@@ -638,7 +698,7 @@ static int ssd1307fb_probe(struct i2c_client *client,
 
 	info->fbops = &ssd1307fb_ops;
 	info->fix = ssd1307fb_fix;
-	info->fix.line_length = par->width / 8;
+	info->fix.line_length = DIV_ROUND_UP(par->width, 8);
 	info->fbdefio = ssd1307fb_defio;
 
 	info->var = ssd1307fb_var;
@@ -647,14 +707,7 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	info->var.yres = par->height;
 	info->var.yres_virtual = par->height;
 
-	info->var.red.length = 1;
-	info->var.red.offset = 0;
-	info->var.green.length = 1;
-	info->var.green.offset = 0;
-	info->var.blue.length = 1;
-	info->var.blue.offset = 0;
-
-	info->screen_base = (u8 __force __iomem *)vmem;
+	info->screen_buffer = vmem;
 	info->fix.smem_start = __pa(vmem);
 	info->fix.smem_len = vmem_size;
 
@@ -713,7 +766,7 @@ panel_init_error:
 	if (par->device_info->need_pwm) {
 		pwm_disable(par->pwm);
 		pwm_put(par->pwm);
-	};
+	}
 regulator_enable_error:
 	if (par->vbat_reg)
 		regulator_disable(par->vbat_reg);
@@ -737,7 +790,7 @@ static int ssd1307fb_remove(struct i2c_client *client)
 	if (par->device_info->need_pwm) {
 		pwm_disable(par->pwm);
 		pwm_put(par->pwm);
-	};
+	}
 	fb_deferred_io_cleanup(info);
 	__free_pages(__va(info->fix.smem_start), get_order(info->fix.smem_len));
 	framebuffer_release(info);