1 files changed, 30 insertions, 11 deletions

diff --git a/drivers/staging/fbtft/fb_ssd1306.c b/drivers/staging/fbtft/fb_ssd1306.c
index 80fc57029fee..96c58de85288 100644
--- a/drivers/staging/fbtft/fb_ssd1306.c
+++ b/drivers/staging/fbtft/fb_ssd1306.c
@@ -62,6 +62,8 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0xA8);
 	if (par->info->var.yres == 64)
 		write_reg(par, 0x3F);
+	else if (par->info->var.yres == 48)
+		write_reg(par, 0x2F);
 	else
 		write_reg(par, 0x1F);
 
@@ -82,7 +84,7 @@ static int init_display(struct fbtft_par *par)
 	/* Vertical addressing mode  */
 	write_reg(par, 0x01);
 
-	/*Set Segment Re-map */
+	/* Set Segment Re-map */
 	/* column address 127 is mapped to SEG0 */
 	write_reg(par, 0xA0 | 0x1);
 
@@ -95,6 +97,9 @@ static int init_display(struct fbtft_par *par)
 	if (par->info->var.yres == 64)
 		/* A[4]=1b, Alternative COM pin configuration */
 		write_reg(par, 0x12);
+	else if (par->info->var.yres == 48)
+		/* A[4]=1b, Alternative COM pin configuration */
+		write_reg(par, 0x12);
 	else
 		/* A[4]=0b, Sequential COM pin configuration */
 		write_reg(par, 0x02);
@@ -124,6 +129,19 @@ static int init_display(struct fbtft_par *par)
 	return 0;
 }
 
+static void set_addr_win_64x48(struct fbtft_par *par)
+{
+	/* Set Column Address */
+	write_reg(par, 0x21);
+	write_reg(par, 0x20);
+	write_reg(par, 0x5F);
+
+	/* Set Page Address */
+	write_reg(par, 0x22);
+	write_reg(par, 0x0);
+	write_reg(par, 0x5);
+}
+
 static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 {
 	/* Set Lower Column Start Address for Page Addressing Mode */
@@ -132,12 +150,15 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	write_reg(par, 0x10 | 0x0);
 	/* Set Display Start Line */
 	write_reg(par, 0x40 | 0x0);
+
+	if (par->info->var.xres == 64 && par->info->var.yres == 48)
+		set_addr_win_64x48(par);
 }
 
 static int blank(struct fbtft_par *par, bool on)
 {
 	fbtft_par_dbg(DEBUG_BLANK, par, "%s(blank=%s)\n",
-		__func__, on ? "true" : "false");
+		      __func__, on ? "true" : "false");
 
 	if (on)
 		write_reg(par, 0xAE);
@@ -147,7 +168,7 @@ static int blank(struct fbtft_par *par, bool on)
 }
 
 /* Gamma is used to control Contrast */
-static int set_gamma(struct fbtft_par *par, unsigned long *curves)
+static int set_gamma(struct fbtft_par *par, u32 *curves)
 {
 	/* apply mask */
 	curves[0] &= 0xFF;
@@ -162,26 +183,24 @@ static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 {
 	u16 *vmem16 = (u16 *)par->info->screen_buffer;
+	u32 xres = par->info->var.xres;
+	u32 yres = par->info->var.yres;
 	u8 *buf = par->txbuf.buf;
 	int x, y, i;
 	int ret = 0;
 
-	for (x = 0; x < par->info->var.xres; x++) {
-		for (y = 0; y < par->info->var.yres/8; y++) {
+	for (x = 0; x < xres; x++) {
+		for (y = 0; y < yres / 8; y++) {
 			*buf = 0x00;
 			for (i = 0; i < 8; i++)
-				*buf |= (vmem16[(y * 8 + i) *
-						par->info->var.xres + x] ?
-					 1 : 0) << i;
+				*buf |= (vmem16[(y * 8 + i) * xres + x] ? 1 : 0) << i;
 			buf++;
 		}
 	}
 
 	/* Write data */
 	gpio_set_value(par->gpio.dc, 1);
-	ret = par->fbtftops.write(par, par->txbuf.buf,
-				  par->info->var.xres * par->info->var.yres /
-				  8);
+	ret = par->fbtftops.write(par, par->txbuf.buf, xres * yres / 8);
 	if (ret < 0)
 		dev_err(par->info->device, "write failed and returned: %d\n",
 			ret);