/*****************************************************************************/
/* Copyright (c) 2008 NXP B.V.                                               */
/*                                                                           */
/* This program is free software; you can redistribute it and/or modify      */
/* it under the terms of the GNU General Public License as published by      */
/* the Free Software Foundation, using version 2 of the License.             */
/*                                                                           */
/* This program is distributed in the hope that it will be useful,           */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of            */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the              */
/* GNU General Public License for more details.                              */
/*                                                                           */
/* You should have received a copy of the GNU General Public License         */
/* along with this program; if not, write to the Free Software               */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307       */
/* USA.                                                                      */
/*                                                                           */
/*****************************************************************************/
/*
                                  IPSTB Project
                         ---------------------------------

Filename: phStbLCD.c


Rev Date       Author      Comments
--------------------------------------------------------------------------------
  1 20061130   neill       Original
  2 20061206   neill       Use a combination of tmdl and direct access
  3 20070112   neill       Use kernel thread and scheduling
  4 20070118   neill       Fix for kernel version 2.6.19.1
  5 20070118   laird       Fix for kernel version 2.6.19.*
  6 20070308   neill       Changes to support multiple frame buffers in DirectFB
  7 20071023   neill       Fix compiler warnings
  8 20080205   batelaan    Update file copyright/license text
  9 20080829   batelaan    Fix compilation error
 10 20090121   batelaan    Update author/license.
*/

/** @file
    phStbLCD.c   - Frontpanel LCD device driver
*/

#define __PHSTBLCD_C__

#include <asm/io.h>
#include <asm/mach-pnx833x/irq.h>
#include <linux/module.h>
#include <linux/sched.h>
#include <linux/kernel.h>
#include <linux/delay.h>
#include <linux/errno.h>
#include <linux/mm.h>
#include <linux/ioport.h>
#include <linux/interrupt.h>
#include <linux/fb.h>
#include <linux/platform_device.h>

#include <tmosal.h>
#include "tmdlPio.h"

#include "phStbLCD.h"

/******************************************************************************
* LOCAL MACROS                                                                *
*******************************************************************************/

/**** Module Setup ****/
MODULE_DESCRIPTION(PHSTBLCD_MODULE_DESCRIPTION);
MODULE_AUTHOR("NXP B.V.");
MODULE_LICENSE("GPL v2");

//  ----------------------- PHSTBLCD MODULE Debug Level   ------------------------
static int debug = 0;
module_param(debug, int, 0);
MODULE_PARM_DESC(debug, "Debug level (0-1)");


//  ----------------------- PHSTBLCD Kernel Debug --------------------------------
#ifdef __PHSTBLCD_C__
#undef  dprintk
#define dprintk(num, format, args...)                                           \
    do {                                                                        \
        if (debug >= num) printk(format, ##args);                               \
    } while (0)
#endif

/******************************************************************************
* LOCAL TYPEDEFS                                                              *
*******************************************************************************/
#define LCD_X_RES    132
#define LCD_Y_RES    132

#define DISON       0xaf
#define DISOFF      0xae
#define DISNOR      0xa6
#define DISINV      0xa7
#define COMSCN      0xbb
#define DISCTL      0xca
#define SLPIN       0x95
#define SLPOUT      0x94
#define PASET       0x75
#define CASET       0x15
#define DATCTL      0xbc
#define RGBSET8     0xce
#define RAMWR       0x5c
#define PTLIN       0xa8
#define PTLOUT      0xa9
#define RMWIN       0xe0
#define RMWOUT      0xee
#define ASCSET      0xaa
#define SCSTART     0xab
#define OSCON       0xd1
#define OSCOFF      0xd2
#define PWRCTR      0x20
#define VOLCTR      0x81
#define VOLUP       0xd6
#define VOLDOWN     0xd7
#define TMPGRD      0x82
#define EPCTIN      0xcd
#define EPCOUT      0xcc
#define EPMWR       0xfc
#define EPMRD       0xfd
#define EPSRRD1     0x7c
#define EPSRRD2     0x7d
#define NOP         0x25
#define IDMOFF      0x38
#define IDMON       0x39

#define LCD_CMD     0
#define LCD_DATA    1

#define RGB332_RED_MASK      0xe0
#define RGB332_GREEN_MASK    0x1c
#define RGB332_BLUE_MASK     0x03

#define ARGB8888_RED_SHIFT   0
#define ARGB8888_GREEN_SHIFT 3
#define ARGB8888_BLUE_SHIFT  6

#define RGB444_TOP_MASK      0xF0

typedef enum
{
    rgb332,
    rgb444,
    argb8888,
    rgb_invalid
} phStbLCD_displayFormat;

/******************************************************************************
* STATIC FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

int __init phStbLCD_probe(struct device *device);
static int phStbLCD_remove(struct device *device);
int __init phStbLCD_init(void);

/******************************************************************************
* EXPORTED DATA                                                               *
*******************************************************************************/

/******************************************************************************
* LOCAL DATA                                                                  *
*******************************************************************************/

static int update_frequency   = 10;
module_param(update_frequency, int, 0);
MODULE_PARM_DESC(update_frequency, "Automatic update frequency (Hz)");

static int update_latency   = 1;
module_param(update_latency, int, 0);
MODULE_PARM_DESC(update_latency, "Automatic update latency (1-10)");

static char* input_mode = "ARGB8888";
module_param(input_mode, charp, 0);
MODULE_PARM_DESC(input_mode, "Display input mode (RGB332 | ARGB8888)");

static char* output_mode = "RGB332";
module_param(output_mode, charp, 0);
MODULE_PARM_DESC(output_mode, "Display output mode (RGB332 | RGB444)");

static phStbLCD_displayFormat gphStbLCD_inputFormat;
static phStbLCD_displayFormat gphStbLCD_outputFormat;

struct fb_info *pfb_info = NULL;
static char* gphStbLCD_screenBuffer = NULL;
static volatile int update_exit = 0;

static struct fb_var_screeninfo phStbLCD_var __initdata = {
    .xres         = LCD_X_RES,
    .yres         = LCD_Y_RES,
    .xres_virtual = LCD_X_RES,
    .yres_virtual = LCD_Y_RES*2,
    .xoffset      = 0,
    .yoffset      = 0,
    .left_margin  = 0,
    .right_margin = 0,
    .upper_margin = 0,
    .lower_margin = 0,
    .vmode        = FB_VMODE_NONINTERLACED,
};

static struct fb_fix_screeninfo phStbLCD_fix __initdata = {
    .id         = "phStbLCD FB",
    .type       = FB_TYPE_PACKED_PIXELS,
    .visual     = FB_VISUAL_TRUECOLOR,
    .xpanstep   = 0,
	.ypanstep   = 1,
    .ywrapstep  = 1,
    .accel =    FB_ACCEL_NONE,
};

static struct device_driver phStbLCD_driver = {
    .name   = "phStbLCD_fb",
    .bus    = &platform_bus_type,
    .probe  = phStbLCD_probe,
    .remove = __devexit_p(phStbLCD_remove),
};

static void phStbLCD_platform_release(struct device *device)
{
}

static struct platform_device phStbLCD_device = {
    .name = "phStbLCD_fb",
    .dev  = {
        .release = phStbLCD_platform_release,
    }
};

static unsigned int crc_table[256] = {
	0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b,
    0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61,
    0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, 0x4c11db70, 0x48d0c6c7,
    0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
    0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3,
    0x709f7b7a, 0x745e66cd, 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
    0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, 0xbe2b5b58, 0xbaea46ef,
    0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
    0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb,
    0xceb42022, 0xca753d95, 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1,
    0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, 0x34867077, 0x30476dc0,
    0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
    0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4,
    0x0808d07d, 0x0cc9cdca, 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde,
    0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, 0x5e9f46bf, 0x5a5e5b08,
    0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
    0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc,
    0xb6238b25, 0xb2e29692, 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6,
    0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, 0xe0b41de7, 0xe4750050,
    0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
    0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34,
    0xdc3abded, 0xd8fba05a, 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637,
    0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, 0x4f040d56, 0x4bc510e1,
    0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
    0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5,
    0x3f9b762c, 0x3b5a6b9b, 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
    0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, 0xf12f560e, 0xf5ee4bb9,
    0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
    0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd,
    0xcda1f604, 0xc960ebb3, 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7,
    0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, 0x9b3660c6, 0x9ff77d71,
    0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
    0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2,
    0x470cdd2b, 0x43cdc09c, 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8,
    0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, 0x119b4be9, 0x155a565e,
    0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
    0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a,
    0x2d15ebe3, 0x29d4f654, 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0,
    0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, 0xe3a1cbc1, 0xe760d676,
    0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
    0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662,
    0x933eb0bb, 0x97ffad0c, 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668,
	0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
};

/******************************************************************************
* FUNCTION IMPLEMENTATION                                                     *
*******************************************************************************/

/* GPIO related functions */
#define USE_TMDLPIO
#if defined(USE_TMDLPIO)

#undef USE_GPIO

#define CLK_PIN   (13)
#define CLK_MASK  ((tmdlPioLines_t)1<<CLK_PIN)
#define CLK_ONE   CLK_MASK
#define CLK_ZERO  0
#define DATA_PIN  (12)
#define DATA_MASK ((tmdlPioLines_t)1<<DATA_PIN)
#define DATA_ONE  DATA_MASK
#define DATA_ZERO 0

static tmInstance_t  phStbLCD_pioInstance = (tmInstance_t)Null;

static void phStbLCD_gpioGet(void);

static void phStbLCD_gpioInit(void)
{
    tmdlPioInstanceSetup_t pioSetup;
    tmdlPioIcfgCmd_t       pioCfgCmd;
    tmdlPioSetModeMask_t   pioCfgData;
    tmErrorCode_t error;

    error = tmdlPioOpen( &phStbLCD_pioInstance );
    if ( error == TM_OK )
    {
        pioSetup.lineSelect = (CLK_MASK|DATA_MASK);

        error = tmdlPioInstanceSetup( phStbLCD_pioInstance, &pioSetup );
        if ( error == TM_OK )
        {
            pioCfgCmd = tmdlPioCmdSetMode;
            pioCfgData.lineSelInput  = 0;
#if defined(USE_GPIO)
            pioCfgData.lineSelOutput = (CLK_MASK|DATA_MASK);
            pioCfgData.lineSelPeriph = 0;
#else
            pioCfgData.lineSelOutput = 0;
            pioCfgData.lineSelPeriph = (CLK_MASK|DATA_MASK);
#endif
            error = tmdlPioInstanceConfig( phStbLCD_pioInstance, pioCfgCmd, (pVoid)&pioCfgData  );

#if !defined(USE_GPIO)
            phStbLCD_gpioGet();
#endif
            if (error != TM_OK)
            {
                printk("phStbLCD : Error returned from tmdlPioInstanceConfig %x\n", error);
            }
        }
        else
        {
            printk("phStbLCD : Error returned from tmdlPioInstanceSetup %x\n", error);
        }
    }
    else
    {
        printk("phStbLCD : Error returned from tmdlPioOpen %x\n", error);
    }
}

static void phStbLCD_gpioTerm(void)
{
    tmdlPioClose(phStbLCD_pioInstance);
}

#if defined(USE_GPIO)

#define phStbLCD_gpioSet(which, value) \
{ \
    tmErrorCode_t error; \
    error = tmdlPioWrite(phStbLCD_pioInstance, which, value); \
    if (error != TM_OK) \
    { \
        printk("phStbLCD : Error returned from tmdlPioWrite %x\n", error); \
    } \
}

#else

/* Use the frequency generator interface */
#undef CLK_MASK
#undef CLK_ONE
#undef CLK_ZERO
#undef DATA_MASK
#undef DATA_ONE
#undef DATA_ZERO

#define GPIO_DATA (unsigned int volatile*) 0xb7e0f0a4
#define CLK_MASK  0x30
#define CLK_ONE   0x10
#define CLK_ZERO  0x20
#define DATA_MASK 0x0C
#define DATA_ONE  0x04
#define DATA_ZERO 0x08

static unsigned int currentValue;

#define phStbLCD_gpioSet(which, value) \
{ \
    *(GPIO_DATA) = currentValue = (currentValue & ~(which)) | (value); \
}

static void phStbLCD_gpioGet(void)
{
    currentValue = *(GPIO_DATA);
}

#endif
#else
#define CLK_PIN          (13)
#define CLK_MASK         ((unsigned int)1<<CLK_PIN)
#define CLK_ONE          CLK_MASK
#define CLK_ZERO         0
#define DATA_PIN         (12)
#define DATA_MASK        ((unsigned int)1<<DATA_PIN)
#define DATA_ONE         DATA_MASK
#define DATA_ZERO        0
#define GPIO_BASE        0xb7e0f000
#define DIRECTION_OFFSET 0x008
#define OUTPUT_OFFSET    0x004

static void phStbLCD_gpioInit(void)
{
    unsigned int volatile * pOutput = (unsigned int*) (GPIO_BASE+DIRECTION_OFFSET);
    *pOutput &= ~(CLK_MASK|DATA_MASK);
    *pOutput |= (CLK_MASK|DATA_MASK);
}

static void phStbLCD_gpioTerm(void)
{
}

#define phStbLCD_gpioSet(which, value) \
{ \
    unsigned int volatile * pOutput = (unsigned int*) (GPIO_BASE+OUTPUT_OFFSET); \
    *pOutput = (*pOutput & ~(which)) | (value); \
}
#endif

static phStbLCD_displayFormat phStbLCD_getDisplayFormat(char* formatString)
{
    if (strstr(formatString, "RGB332"))
    {
        return rgb332;
    }
    if (strstr(formatString, "RGB444"))
    {
        return rgb444;
    }
    if (strstr(formatString, "ARGB8888"))
    {
        return argb8888;
    }
    return rgb_invalid;
}

static char* phStbLCD_modeToString(phStbLCD_displayFormat format)
{
    switch (format)
    {
        case(rgb332)   : return "RGB332";
        case(rgb444)   : return "RGB444";
        case(argb8888) : return "ARGB8888";
        default        : return "Invalid";
    }
    return "Invalid";
}

static int phStbLCD_bytesPerPixel(phStbLCD_displayFormat format)
{
    switch (format)
    {
        case(rgb332)   : return 1;
        case(rgb444)   : return 2;
        case(argb8888) : return 4;
        default        : return 0;
    }
    return 0;
}

/**
 * sysfs entries
 */
static ssize_t phStbLCD_show_memsize(struct device *dev, struct device_attribute *attr, char *buf)
{
    return snprintf(buf, PAGE_SIZE, "%u\n", LCD_X_RES * LCD_Y_RES * phStbLCD_bytesPerPixel(gphStbLCD_inputFormat));
}

static DEVICE_ATTR(size, S_IRUGO, phStbLCD_show_memsize, NULL);

static ssize_t phStbLCD_show_rev(struct device *dev, struct device_attribute *attr, char *buf)
{
    return snprintf(buf, PAGE_SIZE, "%d\n", 0);
}

static DEVICE_ATTR(revision, S_IRUGO, phStbLCD_show_rev, NULL);


static ssize_t phStbLCD_show_vendor(struct device *dev, struct device_attribute *attr, char *buf)
{
    return snprintf(buf, PAGE_SIZE, "0x%04x", 0xDEAF);
}

static DEVICE_ATTR(vendor, S_IRUGO, phStbLCD_show_vendor, NULL);

static ssize_t phStbLCD_show_device(struct device *dev, struct device_attribute *attr, char *buf)
{
    return snprintf(buf, PAGE_SIZE, "0x%04x", 0xABBA);
}

static DEVICE_ATTR(device, S_IRUGO, phStbLCD_show_device, NULL);

static void __devexit phStbLCD_remove_sysfs(struct device *dev)
{
    device_remove_file(dev, &dev_attr_size);
    device_remove_file(dev, &dev_attr_revision);
    device_remove_file(dev, &dev_attr_device);
    device_remove_file(dev, &dev_attr_vendor);
}

static void phStbLCD_create_sysfs(struct device *dev)
{
    int error;
    error = device_create_file(dev, &dev_attr_size);
    if (error!=0) 
    {
        error = device_create_file(dev, &dev_attr_revision);
        if (error!=0) 
        {
            error = device_create_file(dev, &dev_attr_device);
            if (error!=0) 
            {
                error = device_create_file(dev, &dev_attr_vendor);
            }
        }
    }
}

/* SPI command handling functions */

static void phStbLCD_spiCommand(unsigned char data)
{
    int i = 8;

    phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ZERO);
    phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ZERO);

    for(i=0; i<8; i++)
    {
        if ((data & 0x80))
        {
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ONE);
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ONE);
        }
        else
        {
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ZERO);
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ZERO);
        }
        data <<= 1;
    }
}

static void phStbLCD_spiWriteData(unsigned char data)
{
    int i;

    phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ONE);
    phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ONE);
    for(i=0; i<8; i++)
    {
        if ((data & 0x80))
        {
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ONE);
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ONE);
        }
        else
        {
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ZERO|DATA_ZERO);
            phStbLCD_gpioSet(CLK_MASK|DATA_MASK, CLK_ONE|DATA_ZERO);
        }
        data <<= 1;
    }
}

static void phStbLCD_startup ( void )
{
     phStbLCD_gpioInit();

     phStbLCD_spiCommand(OSCON);  // oscon

     phStbLCD_spiCommand(DISCTL);  // display control
     phStbLCD_spiWriteData(3);
     phStbLCD_spiWriteData(32);
     phStbLCD_spiWriteData(12);
     phStbLCD_spiWriteData(0);

     phStbLCD_spiCommand(COMSCN);  // comscn
     phStbLCD_spiWriteData(0x01);

     phStbLCD_spiCommand(SLPOUT);  // sleep out

     phStbLCD_spiCommand(VOLCTR);  // electronic volume, this is kinda contrast/brightness
     phStbLCD_spiWriteData(35);       //  this might be different for individual LCDs
     phStbLCD_spiWriteData(3);       //

     phStbLCD_spiCommand(PTLOUT);
     phStbLCD_spiCommand(PWRCTR);  // power ctrl
     phStbLCD_spiWriteData(0x0f);       //everything on, no external reference resistors
     msleep(100);

     phStbLCD_spiCommand(DISINV);  // display mode

     phStbLCD_spiCommand(DATCTL);  // datctl
     phStbLCD_spiWriteData(0x05);        // Orientation
     phStbLCD_spiWriteData(0);
     if (gphStbLCD_outputFormat == rgb332)
     {
         phStbLCD_spiWriteData(0x01);
     }
     else
     {
         phStbLCD_spiWriteData(0x02);
     }


     phStbLCD_spiCommand(RGBSET8);   // setup color lookup table
     // color table
     //RED
     phStbLCD_spiWriteData(0);
     phStbLCD_spiWriteData(2);
     phStbLCD_spiWriteData(4);
     phStbLCD_spiWriteData(6);
     phStbLCD_spiWriteData(8);
     phStbLCD_spiWriteData(10);
     phStbLCD_spiWriteData(12);
     phStbLCD_spiWriteData(15);
     // GREEN
     phStbLCD_spiWriteData(0);
     phStbLCD_spiWriteData(2);
     phStbLCD_spiWriteData(4);
     phStbLCD_spiWriteData(6);
     phStbLCD_spiWriteData(8);
     phStbLCD_spiWriteData(10);
     phStbLCD_spiWriteData(12);
     phStbLCD_spiWriteData(15);
     //BLUE
     phStbLCD_spiWriteData(0);
     phStbLCD_spiWriteData(4);
     phStbLCD_spiWriteData(9);
     phStbLCD_spiWriteData(15);

     phStbLCD_spiCommand(NOP);  // nop

     phStbLCD_spiCommand(DISON);   // display on
     msleep(200);
}

/* Screen update functions */

static inline unsigned char phStbLCD_8888To332(unsigned char * data)
{
    return ((data[3]>>ARGB8888_BLUE_SHIFT)&RGB332_BLUE_MASK) |
           ((data[2]>>ARGB8888_GREEN_SHIFT)&RGB332_GREEN_MASK) |
           ((data[1]>>ARGB8888_RED_SHIFT)&RGB332_RED_MASK);
}

static inline void phStbLCD_332To444(unsigned char *data, unsigned char *outputByte)
{
    outputByte[0] = (data[0]&RGB332_RED_MASK)        | ((data[0]&RGB332_GREEN_MASK)>>1);
    outputByte[1] = ((data[0]&RGB332_BLUE_MASK)<<6)  | ((data[LCD_X_RES+1]&RGB332_RED_MASK)>>4);
    outputByte[2] = ((data[LCD_X_RES+1]&RGB332_GREEN_MASK)<<3) | ((data[LCD_X_RES+0]&RGB332_BLUE_MASK)<<2);
}

static inline void phStbLCD_8888To444(unsigned char *data, unsigned char *outputByte, int dataSize)
{
    outputByte[0] = (data[1]&RGB444_TOP_MASK) | (data[2]>>4);
    outputByte[1] = (data[3]&RGB444_TOP_MASK) | (data[(LCD_X_RES*dataSize)+1]>>4);
    outputByte[2] = (data[(LCD_X_RES*dataSize)+2]&RGB444_TOP_MASK) | (data[(LCD_X_RES*dataSize)+3]>>4);
}

static void phStbLCD_refreshScreen(int yStart, int yEnd)
{
    int x;
    int y;
    char *data;
    int dataSize;
    dataSize = phStbLCD_bytesPerPixel(gphStbLCD_inputFormat);

    data=&gphStbLCD_screenBuffer[(yStart+pfb_info->var.yoffset)*LCD_X_RES*dataSize];

    dprintk(1, "phStbLCD : Screen refresh %d - %d\n", yStart, yEnd);

    phStbLCD_spiCommand(PASET);
    phStbLCD_spiWriteData(0);
    phStbLCD_spiWriteData(LCD_X_RES-1);

    phStbLCD_spiCommand(CASET);
    phStbLCD_spiWriteData(yStart);
    phStbLCD_spiWriteData(LCD_Y_RES-1);

    phStbLCD_spiCommand(RAMWR);
    if (gphStbLCD_outputFormat == rgb332)
    {
        switch(gphStbLCD_inputFormat)
        {
            case(rgb332) :
                for (y = 0; y < (1+yEnd-yStart); y++)
                {
                    for (x = 0; x < LCD_X_RES; x++)
                    {
                        phStbLCD_spiWriteData(*data);
                        data += dataSize;
                    }
                    /* Be friendly - allow other applications some CPU time! */
                    schedule();
                }
                break;
            case(argb8888) :
                for (y = 0; y < (1+yEnd-yStart); y++)
                {
                    for (x = 0; x < LCD_X_RES; x++)
                    {
                        phStbLCD_spiWriteData(phStbLCD_8888To332(data));
                        data += dataSize;
                    }
                    /* Be friendly - allow other applications some CPU time! */
                    schedule();
                }
                break;
            default:
                break;
        }
    }
    else
    {
        switch(gphStbLCD_inputFormat)
        {
            case(rgb332) :
                for (y = 0; y < (1+yEnd-yStart)/2; y++)
                {
                    for (x = 0; x < LCD_X_RES; x++)
                    {
                        unsigned char outputByte[3];
                        phStbLCD_332To444(data, outputByte);
                        phStbLCD_spiWriteData(outputByte[0]);
                        phStbLCD_spiWriteData(outputByte[1]);
                        phStbLCD_spiWriteData(outputByte[2]);
                        data += dataSize;
                    }
                    /* Be friendly - allow other applications some CPU time! */
                    schedule();
                    data+=(LCD_X_RES*dataSize);
                }
                break;
            case(argb8888) :
                for (y = 0; y < (1+yEnd-yStart)/2; y++)
                {
                    for (x = 0; x < LCD_X_RES; x++)
                    {
                        unsigned char outputByte[3];
                        phStbLCD_8888To444(data, outputByte, dataSize);
                        phStbLCD_spiWriteData(outputByte[0]);
                        phStbLCD_spiWriteData(outputByte[1]);
                        phStbLCD_spiWriteData(outputByte[2]);
                        data += dataSize;
                    }
                    /* Be friendly - allow other applications some CPU time! */
                    schedule();
                    data+=(LCD_X_RES*dataSize);
                }
                break;
            default:
                break;
        }
    }
}

static unsigned int phStbLCD_crc32(const unsigned char *pData, int length)
{
    int i;
    unsigned int crc = 0xffffffff;

    for (i = 0; i < length; i++)
    {
        crc = (crc << 8) ^ crc_table[((crc >> 24) ^ *pData++) & 0xff];
    }

    return crc;
}

static int phStbLCD_updateScreen (void * data)
{
    static int same = 0;
    static unsigned int line_csum[LCD_Y_RES];
    static int yStartSaved = LCD_Y_RES;
    static int yEndSaved = 0;
    int dataSize;
    dataSize = phStbLCD_bytesPerPixel(gphStbLCD_inputFormat);

    while(!update_exit)
    {
        int yStart = LCD_Y_RES;
        int yEnd = 0;
        int i;
        int changed =0;

        dprintk(1, "phStbLCD : Checking for 'dirty' display\n");
        /* Work out which lines of the display have changed ... the 'dirty' area */
        for( i=0; i<LCD_Y_RES; i++)
        {
            unsigned int csum;
            csum=phStbLCD_crc32(&gphStbLCD_screenBuffer[(i+pfb_info->var.yoffset)*LCD_X_RES*dataSize],LCD_X_RES*dataSize);
            /* Be friendly - allow other applications some CPU time! */
            schedule();
            if (csum!=line_csum[i])
            {
                 changed = 1;
                 line_csum[i] = csum;
                 if (i<yStart)
                 {
                     yStart=i;
                 }
                 if (i>yEnd)
                 {
                     yEnd=i;
                 }
             }
             i++;
         }
         if (changed)
         {
             dprintk(1, "phStbLCD : 'dirty' display lines %d - %d\n", yStart, yEnd);
         }
         else
         {
             dprintk(1, "phStbLCD : No new 'dirty' display lines\n");
         }

         if (changed==0)
         {
             if (same<update_latency)
             {
                 same++;
                 dprintk(1, "phStbLCD : Updating latency info to %d\n", same);
             }
         }
         else
         {
             same = 0;
             if (yStart<yStartSaved)
             {
                 yStartSaved=yStart;
             }
             if (yEnd>yEndSaved)
             {
                 yEndSaved=yEnd;
             }
             dprintk(1, "phStbLCD : Updating refresh info to %d - %d\n", yStartSaved, yEndSaved);
         }

         /* Check display latency to see if the display should be updated */
         if (same == update_latency-1)
         {
             /* Expand the selected area slightly ... 4096 colour issue */
             if (yStartSaved)
             {
                 yStartSaved--;
             }
             yStartSaved = (yStartSaved/2)*2;
             yEndSaved = yEndSaved+2;
             if (yEndSaved>=LCD_Y_RES)
             {
                 yEndSaved = LCD_Y_RES-1;
             }

             /* Re-draw the display */
             phStbLCD_refreshScreen(yStartSaved, yEndSaved);

             /* Reset the 'dirty' area */
             yStartSaved = LCD_Y_RES;
             yEndSaved = 0;
         }
         msleep(1000/update_frequency);
    }

    dprintk(1, "Exit update thread\n");
    update_exit = 0;

    return 0;
}

static int phStbLCD_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
{
    int changed = 0;
    changed = ((info->var.xres != var->xres) ||
               (info->var.yres != var->yres) ||
               (info->var.xres_virtual != var->xres_virtual) ||
               (info->var.yres_virtual != var->yres_virtual));

    /*If something has changed look at values*/
    if(changed)
    {
        info->var.xres = var->xres;
        if (info->var.xres > 132) 
        {
            info->var.xres = 132;
        }
        info->var.yres = var->yres;
        info->var.xres_virtual = var->xres_virtual;
        info->var.yres_virtual = var->yres_virtual;
    }
    return 0;
}

static int phStbLCD_set_par(struct fb_info *info)
{
    return 0;
}

static int phStbLCD_setcolreg(unsigned regno, unsigned red, unsigned green,
               unsigned blue, unsigned transp,
               struct fb_info *info)
{
    return 0;
}

static int phStbLCD_pan_display(struct fb_var_screeninfo *var,
                               struct fb_info *info)
{
     if (((var->xoffset + var->xres) > var->xres_virtual)||
        ((var->yoffset + var->yres) > var->yres_virtual))
    {
        return -EINVAL;
    }

    /*Update local settings*/
    info->var.xoffset = var->xoffset;
    info->var.yoffset = var->yoffset;

    return 0;
}

static int phStbLCD_blank(int blank_mode, struct fb_info *info)
{
    return 0;
}

int phStbLCD_ioctl(struct fb_info *info,
                  unsigned int cmd,
                  unsigned long arg)
{
    return 0;
}

static struct fb_ops phStbLCD_ops = {
    .fb_check_var   = phStbLCD_check_var,
    .fb_set_par     = phStbLCD_set_par,
    .fb_setcolreg   = phStbLCD_setcolreg,
    .fb_blank       = phStbLCD_blank,
    .fb_pan_display = phStbLCD_pan_display,
    .fb_ioctl       = phStbLCD_ioctl
};

int __init phStbLCD_probe(struct device *device)
{
    struct platform_device *dev = to_platform_device(device);
    struct fb_info *info;
    int retval = -ENOMEM;
    unsigned long page = 0;

    gphStbLCD_inputFormat = phStbLCD_getDisplayFormat(input_mode);
    if ((gphStbLCD_inputFormat != rgb332) && (gphStbLCD_inputFormat != argb8888))
    {
        printk("phStbLCD : Invalid input mode - selecting default ARGB8888\n");
        gphStbLCD_outputFormat = argb8888;
    }
    gphStbLCD_outputFormat = phStbLCD_getDisplayFormat(output_mode);
    if ((gphStbLCD_outputFormat != rgb332) && (gphStbLCD_outputFormat != rgb444))
    {
        printk("phStbLCD : Invalid output mode - selecting default RGB332\n");
        gphStbLCD_outputFormat = rgb332;
    }

    phStbLCD_startup();

    /* Do we need to probe ?*/
    pfb_info = info = framebuffer_alloc(0, &dev->dev);
    if (!info)
        goto err;

    info->node = -1;
    info->flags = FBINFO_FLAG_DEFAULT;
    info->fbops = &phStbLCD_ops;
    info->fix = phStbLCD_fix;
    info->var = phStbLCD_var;

    switch(gphStbLCD_inputFormat)
    {
        case(rgb332) :
            info->var.bits_per_pixel = 8;
            info->var.red.offset     = 5;
            info->var.red.length     = 3;
            info->var.green.offset   = 2;
            info->var.green.length   = 3;
            info->var.blue.offset    = 0;
            info->var.blue.length    = 2;
            break;
        case(argb8888) :
            info->var.bits_per_pixel = 32;
            info->var.red.offset     = 16;
            info->var.red.length     = 8;
            info->var.green.offset   = 8;
            info->var.green.length   = 8;
            info->var.blue.offset    = 0;
            info->var.blue.length    = 8;
            break;
        default :
            break;
    }

    info->screen_size = info->var.xres_virtual * info->var.yres_virtual * phStbLCD_bytesPerPixel(gphStbLCD_inputFormat);

    gphStbLCD_screenBuffer = info->screen_base = (void *)__get_free_pages(GFP_KERNEL, get_order(info->screen_size-1));
    for (page = (int)info->screen_base;
         page < PAGE_ALIGN((unsigned long)info->screen_base + info->screen_size);
         page += PAGE_SIZE)
    {
        SetPageReserved(virt_to_page((char*)page));
    }
    memset(info->screen_base, 0, info->screen_size);

    info->fix.smem_start=virt_to_phys(info->screen_base);
    info->fix.smem_len=info->screen_size;
    info->fix.line_length=info->screen_size/info->var.yres_virtual;

    info->par = NULL;
    info->pseudo_palette = info->par;

    retval = fb_alloc_cmap(&info->cmap, 256, 0);
    if (retval < 0)
        goto err1;

    retval = register_framebuffer(info);
    if (retval < 0)
        goto err2;

    dev_set_drvdata(&dev->dev, info);

    phStbLCD_create_sysfs(&dev->dev);

    /* Start the update thread */
    retval = kernel_thread (phStbLCD_updateScreen, 0, 0);

    return 0;

err2:
    fb_dealloc_cmap(&info->cmap);
err1:
    framebuffer_release(info);
    free_pages((unsigned int)gphStbLCD_screenBuffer, get_order(info->screen_size - 1));
err:
    return retval;
}

static int __devexit phStbLCD_remove(struct device *device)
{
    struct fb_info *info = dev_get_drvdata(device);

    /* First we should powerdown the LCD (switch off backlight etc) */
    phStbLCD_spiCommand(SLPIN);  // sleep in

    /* Free up video memory and unregister device */
    if (info)
    {
        unregister_framebuffer(info);
        phStbLCD_remove_sysfs(device);
        fb_dealloc_cmap(&info->cmap);
        framebuffer_release(info);
        free_pages((unsigned int)gphStbLCD_screenBuffer, get_order(info->screen_size - 1));
    }

    phStbLCD_gpioTerm();

    return 0;
}

int __init phStbLCD_init(void)
{

    int ret = 0;

    ret = driver_register(&phStbLCD_driver);
    if (!ret)
    {
        ret = platform_device_register(&phStbLCD_device);
        if (ret)
        {
            driver_unregister(&phStbLCD_driver);
        }
    }

    printk ("%s (%s-%s)\n", PHSTBLCD_MODULE_DESCRIPTION, __DATE__, __TIME__);
    dprintk(1, "phStbLCD : Input %s Output %s Update Freq %d Hz Update Latency %d frame(s)\n",
            phStbLCD_modeToString(gphStbLCD_inputFormat),
            phStbLCD_modeToString(gphStbLCD_outputFormat),
            update_frequency,
            update_latency);

    return ret;

}

void __exit phStbLCD_exit(void)
{
    /* Wait for the update thread to exit */
    update_exit = 1;
    while(update_exit)
    {
        msleep(1);
    }
    platform_device_unregister(&phStbLCD_device);
    driver_unregister(&phStbLCD_driver);
}

module_init(phStbLCD_init);
module_exit(phStbLCD_exit);
