write_lcd_index_data(0x21, 0x0000); // RAM address set
        LCD_WriteSetInstruction(0x22);
        LCD_DataTypeSelection(LCD_DISPLAY);


#ifdef R2D_USE_LCD_DMA

  
	r2d_dma_return_path.callback_func = r2d_dma_callback;

	dma_reserve_channel (DMA_CHAN_ANY,0,DMA_QUEUE_DISABLE,
		0,r2d_dma_return_path);
 
  
	r2d_lcd_dma_channel_info.data_width = DMA_DATA_S16;
	r2d_lcd_dma_channel_info.sync = DMA_SYNC_DEVICE_HW_LCD;
	r2d_lcd_dma_channel_info.hw_priority = DMA_HW_PRIORITY_HIGH;
	r2d_lcd_dma_channel_info.flush = DMA_FLUSH_DISABLED;
	r2d_lcd_dma_channel_info.nmb_frames = 1;
	r2d_lcd_dma_channel_info.nmb_elements = 128;
	r2d_lcd_dma_channel_info.dma_end_notification = DMA_NOTIFICATION;
	r2d_lcd_dma_channel_info.secure = DMA_NOT_SECURED;
	r2d_lcd_dma_channel_info.dma_mode = DMA_MODE_SINGLE;
	r2d_lcd_dma_channel_info.transfer = DMA_MODE_TRANSFER_ENABLE;

	r2d_lcd_dma_channel_info.source_address = 0;
	r2d_lcd_dma_channel_info.source_address_mode = DMA_ADDR_MODE_POST_INC;
	r2d_lcd_dma_channel_info.source_packet = DMA_NOT_PACKED;
	r2d_lcd_dma_channel_info.source_burst = DMA_NO_BURST;

	r2d_lcd_dma_channel_info.destination_address = (UINT32)C_LCD_IF_WR_FIFO;
	r2d_lcd_dma_channel_info.destination_address_mode = DMA_ADDR_MODE_CONSTANT;
	r2d_lcd_dma_channel_info.destination_packet = DMA_NOT_PACKED;
	r2d_lcd_dma_channel_info.destination_burst = DMA_NO_BURST;

#endif /* USE_LCD_DMA */

}


//------------------------------------------------------
// LCD : Write display data
//       Wait FIFO empty before setting data type
//------------------------------------------------------
void LCD_WriteSetDisplay(UWORD16 data)
{
  WAIT_FIFO_EMPTY;
  LCD_DataTypeSelection(LCD_DISPLAY);
  LCD_Write(data);
}

//------------------------------------------------------
// LCD : Write instruction data
//       Wait FIFO empty before setting data type
//------------------------------------------------------
void LCD_WriteSetInstruction(UWORD16 instruction)
{
  WAIT_FIFO_EMPTY;
  LCD_DataTypeSelection(LCD_INSTRUCTION);
  LCD_Write(instruction);
}

//------------------------------------------------------
// LCD : Write data into FIFO
//------------------------------------------------------
void LCD_Write(UWORD16 data)
{
( *(volatile SYS_UWORD16* ) C_LCD_IF_WR_FIFO) = data;
//  SetField16(LCD_INTERFACE_WR_FIFO, DATA_TX, data);
}


//------------------------------------------------------
// LCD : Instruction/Display data type selection
//------------------------------------------------------
void LCD_DataTypeSelection(LCD_DataTypeSelection_t value)
{
if (value)
  (*(volatile SYS_UWORD16*) C_LCD_IF_LCD_CNTL_REG)|=value<<C_LCD_IF_LCD_CNTL_REG_LCD_RS_POS;  
else
  (*(volatile SYS_UWORD16*) C_LCD_IF_LCD_CNTL_REG)&=(~(1<<C_LCD_IF_LCD_CNTL_REG_LCD_RS_POS));  	
	
//  SetField16(LCD_INTERFACE_LCD_CNTL_REG, LCD_RS, value);
}



//------------------------------------------------------
// Reset LCD controller
//------------------------------------------------------
void LCD_ResetController(void)
{
  LCD_INTERFACE_LCD_CNTL_REG = (LCD_INTERFACE_LCD_CNTL_REG & 0xFFF7);
}

//------------------------------------------------------
// End LCD controller reset
//------------------------------------------------------
void LCD_EndResetController(void)
{
  LCD_INTERFACE_LCD_CNTL_REG = (LCD_INTERFACE_LCD_CNTL_REG | 0x0008);
}

//------------------------------------------------------
// Reset LCD interface
//------------------------------------------------------
void LCD_ResetInterface(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xFFFE);
  /* Wait the rest completion */
  do
  {}
  while ((LCD_INTERFACE_CNTL_REG & 0x1) == 0);
}

//------------------------------------------------------
// LCD : Clock 13MHz enable
//------------------------------------------------------
void LCD_Clock13MHzEnable(void)
{
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFFFD) | (LCD_CLOCK13_EN << LCD_INTERFACE_CNTL_REG_CLOCK13_EN_POS));
}

//------------------------------------------------------
// LCD : TX clock div
//------------------------------------------------------
void LCD_TXClockDiv(UWORD16 value)
{
//  SetField16(LCD_INTERFACE_CNTL_REG, TX_CLOCK_DIV, (value));
	 LCD_INTERFACE_CNTL_REG |= (value <<LCD_INTERFACE_CNTL_REG_TX_CLOCK_DIV_POS);
}

//------------------------------------------------------
// LCD : FIFO empty interrupt disable
//------------------------------------------------------
void LCD_FifoEmptyItDisable(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xFFBF);
}

//------------------------------------------------------
// LCD : Read event interrupt disable
//------------------------------------------------------
void LCD_ReadEventItDisable(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xFF7F);
}

//------------------------------------------------------
// LCD : DMA disable
//------------------------------------------------------
void LCD_DMADisable(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xFEFF);
}

//------------------------------------------------------
// LCD : 8086 interface type selected
//------------------------------------------------------
void LCD_8086InterfaceTypeSelected(void)
{
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFDFF) | 0x0200);
}

//------------------------------------------------------
// LCD : 6800 interface type selected
//------------------------------------------------------
void LCD_6800InterfaceTypeSelected(void)
{
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFDFF) & ~0x0200);
}


//------------------------------------------------------
// LCD : Write MSB First
//------------------------------------------------------
#if (VIRTIO==0)
void LCD_WriteMSBFirst(void)
{
//  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFBFF) | 0x0400);
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFBFF) & ~0x0400);
}

//------------------------------------------------------
// LCD : Write MSB First
//------------------------------------------------------
void LCD_WriteLSBFirst(void)
{
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFBFF) | 0x0400);
}
#else
void LCD_WriteMSBFirst(void)
{
  LCD_INTERFACE_CNTL_REG = ((LCD_INTERFACE_CNTL_REG & 0xFBFF) | 0x0400);
}
#endif

//------------------------------------------------------
// LCD : Tx never suspended
//------------------------------------------------------
void LCD_TxNotSuspended(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xF7FF);
}

//------------------------------------------------------
// LCD : 0 dummy write cycle
//------------------------------------------------------
void LCD_0DummyWriteCycle(void)
{
  LCD_INTERFACE_CNTL_REG = (LCD_INTERFACE_CNTL_REG & 0xBFFF);
}


void r2d_refresh_task_kill(void)
{
}


void r2d_dithering_init(void)
{
   R2D_MALLOC(r2d_mb_id,UINT32,sizeof(UINT32)*4,r2d_g_dithering_matrix);
   if (r2d_g_dithering_matrix!=NULL)
   {
     r2d_g_dithering_matrix[0]=0;
     r2d_g_dithering_matrix[1]=2;
     r2d_g_dithering_matrix[2]=3;
     r2d_g_dithering_matrix[3]=1;
   }
}