if (pixels&(3<<4))
    SETPIXEL(x+1, y, *(pTrans+(3&(pixels>>4))));
  if (!--xsize)
    return;
WriteTBit2:
  if (pixels&(3<<2))
    SETPIXEL(x+2, y, *(pTrans+(3&(pixels>>2))));
  if (!--xsize)
    return;
WriteTBit3:
  if (pixels&(3<<0))
    SETPIXEL(x+3, y, *(pTrans+(3&(pixels))));
  if (!--xsize)
    return;
  pixels = *(++p);
  x+=4;
  goto WriteTBit0;
/*
        Write without transparency, xor
*/
WriteXBit0:
  XORPIXEL_DATA(x+0, y, *(pTrans+(pixels>>6)));
  if (!--xsize)
    return;
WriteXBit1:
  XORPIXEL_DATA(x+1, y, *(pTrans+(3&(pixels>>4))));
  if (!--xsize)
    return;
WriteXBit2:
  XORPIXEL_DATA(x+2, y, *(pTrans+(3&(pixels>>2))));
  if (!--xsize)
    return;
WriteXBit3:
  XORPIXEL_DATA(x+3, y, *(pTrans+(3&(pixels))));
  if (!--xsize)
    return;
  pixels = *(++p);
  x+=4;
  goto WriteXBit0;
/*
        Write with transparency, xor
*/
WriteTXBit0:
  if (pixels&(3<<6))
    XORPIXEL_DATA(x+0, y, *(pTrans+(pixels>>6)));
  if (!--xsize)
    return;
WriteTXBit1:
  if (pixels&(3<<4))
    XORPIXEL_DATA(x+1, y, *(pTrans+(3&(pixels>>4))));
  if (!--xsize)
    return;
WriteTXBit2:
  if (pixels&(3<<2))
    XORPIXEL_DATA(x+2, y, *(pTrans+(3&(pixels>>2))));
  if (!--xsize)
    return;
WriteTXBit3:
  if (pixels&(3<<0))
    XORPIXEL_DATA(x+3, y, *(pTrans+(3&(pixels))));
  if (!--xsize)
    return;
  pixels = *(++p);
  x+=4;
  goto WriteTXBit0;
}

#endif

#endif

/*
        *********************************************************
        *                                                       *
        *  Draw Bitmap 4 BPP                                    *
        *                                                       *
        *********************************************************
*/

#if (LCD_MAX_LOG_COLORS > 4)

static void  DrawBitLine4BPP(int x, int y, U8 const *p, int Diff, int xsize, const LCD_PIXELINDEX*pTrans) {
  PIXELCOLOR pixels;
/*
// Jump to right entry point
*/
  pixels = *p;
  if (GUI_Context.DrawMode & LCD_DRAWMODE_TRANS) {
    if ((Diff&1) ==0)
      goto WriteTBit0;
    goto WriteTBit1;
  } else {
    if ((Diff&1) ==0)
      goto WriteBit0;
    goto WriteBit1;
  }
/*
        Write without transparency
*/
WriteBit0:
  SETPIXEL(x+0, y, *(pTrans+(pixels>>4)));
  if (!--xsize)
    return;
WriteBit1:
  SETPIXEL(x+1, y, *(pTrans+(pixels&0xf)));
  if (!--xsize)
    return;
  x+=2;
  pixels = *(++p);
  goto WriteBit0;
/*
        Write with transparency
*/
WriteTBit0:
  if (pixels>>4)
    SETPIXEL(x+0, y, *(pTrans+(pixels>>4)));
  if (!--xsize)
    return;
WriteTBit1:
  if (pixels&0xf)
    SETPIXEL(x+1, y, *(pTrans+(pixels&0xf)));
  if (!--xsize)
    return;
  x+=2;
  pixels = *(++p);
  goto WriteTBit0;
}

#endif

/*
        *********************************************************
        *                                                       *
        *  Draw Bitmap 8 BPP  (256 colors)                      *
        *                                                       *
        *      Default (no optimization)                        *
        *                                                       *
        *********************************************************
*/

#if (LCD_MAX_LOG_COLORS > 16)
static void  DrawBitLine8BPP(int x, int y, U8 const *p, int xsize, const LCD_PIXELINDEX*pTrans) {
  LCD_PIXELINDEX pixel;
  if (pTrans) {
    if ((GUI_Context.DrawMode & LCD_DRAWMODE_TRANS)==0) {
      while (xsize > 0) {
        pixel = *p;
        SETPIXEL(x+0, y, *(pTrans+pixel));
        xsize--; x++; p++;
      }
    } else {   /* Handle transparent bitmap */
      while (xsize > 0) {
        pixel = *p;
        if (pixel)
          SETPIXEL(x+0, y, *(pTrans+pixel));
        xsize--; x++; p++;
      }
    }
  } else {
    if ((GUI_Context.DrawMode & LCD_DRAWMODE_TRANS)==0) {
      while (xsize > 0) {
        pixel = *p;
        SETPIXEL(x+0, y, pixel);
        xsize--; x++; p++;
      }
    } else {   /* Handle transparent bitmap */
      while (xsize > 0) {
        pixel = *p;
        if (pixel)
          SETPIXEL(x+0, y, pixel);
        xsize--; x++; p++;
      }
    }
  }
}

#endif

/*
        *********************************************************
        *                                                       *
        *         Universal draw Bitmap routine                 *
        *                                                       *
        *********************************************************
*/

void LCD_L0_DrawBitmap(int x0, int y0,
                       int xsize, int ysize,
                       int BitsPerPixel,
                       int BytesPerLine,
                       const U8* pData, int Diff,
                       const LCD_PIXELINDEX* pTrans)
{
  int i;
  /* Use aID for bitmaps without palette */
  if (!pTrans) {
    pTrans = aID;
  }
  /*
     Use DrawBitLineXBPP
  */
  switch (BitsPerPixel) {
  case 1:
    #if (LCD_OPTIMIZE)               \
        && (LCD_BITSPERPIXEL == 2)   \
        && (LCD_SWAP_XY ==1)         \
        && (LCD_MIRROR_X==1)         \
        && (LCD_MIRROR_Y==0)         \
        && (!defined (LCD_LUT_SEG))
      SET_X_INCREMENT();
      xsize += Diff & 7;
      for (i = Diff; i < xsize; i++) {
        DrawBitLine1BPP_Swap(
          x0+i, y0,
          pData + (i>>3),
          ysize,
          pTrans,
          BytesPerLine,
          i
        );
      }
      SET_Y_INCREMENT();
      break;
    #else
      for (i=0; i<ysize; i++) {
        DrawBitLine1BPP(x0, i+y0, pData, Diff, xsize, pTrans);
        pData += BytesPerLine;
      }
      break;
    #endif
    #if (LCD_MAX_LOG_COLORS > 2)
    case 2:
      #if (LCD_OPTIMIZE)               \
          && (LCD_BITSPERPIXEL == 2)   \
          && (LCD_SWAP_XY ==1)         \
          && (LCD_MIRROR_X==1)         \
          && (LCD_MIRROR_Y==0)         \
          && (!defined (LCD_LUT_SEG))
        SET_X_INCREMENT();
        xsize += Diff & 3;
        for (i = Diff; i < xsize; i++) {
          DrawBitLine2BPP_Swap(
            x0+i, y0,
            pData + (i>>2),
            ysize,
            pTrans,
            BytesPerLine,
            i
          );
        }
        SET_Y_INCREMENT();
        break;
      #else
        for (i=0; i<ysize; i++) {
          DrawBitLine2BPP(x0, i+y0, pData, Diff, xsize, pTrans);
          pData += BytesPerLine;
        }
        break;
      #endif
    #endif
    #if (LCD_MAX_LOG_COLORS > 4)
      case 4:
        for (i=0; i<ysize; i++) {
          DrawBitLine4BPP(x0, i+y0, pData, Diff, xsize, pTrans);
          pData += BytesPerLine;
        }
        break;
    #endif
    #if (LCD_MAX_LOG_COLORS > 16)
      case 8:
        for (i=0; i<ysize; i++) {
          DrawBitLine8BPP(x0, i+y0, pData, xsize, pTrans);
          pData += BytesPerLine;
        }
        break;
    #endif
  }
}

/*
        *********************************************************
        *                                                       *
        *       LCD_L0_SetOrg                                   *
        *                                                       *
        *********************************************************

Purpose:        Sets the original position of the virtual display.
                Has no function at this point with the PC-driver.
*/

int OrgX, OrgY;
void LCD_L0_SetOrg(int x, int y) {
  OrgX = x;
  OrgY = y;
}

/*
        *********************************************************
        *                                                       *
        *       LCD_On                                          *
        *       LCD_Off                                         *
        *                                                       *
        *********************************************************
*/

void LCD_Off          (void) {
  aState[0].ConReg0 = (LCD_INIT_R00) & ~(1<<6);
  LCD_WriteReg0( 0, aState[0].ConReg0);
}

void LCD_On           (void) {
  aState[0].ConReg0 = (LCD_INIT_R00) |  (1<<6);
  LCD_WriteReg0( 0, aState[0].ConReg0);
}

/*
        *********************************************************
        *                                                       *
        *       LCD_L0_ReInit : Re-Init the display                *
        *                                                       *
        *********************************************************

ReInit contains all of the code that can be re-executed at any point without
changing or even disturbing what can be seen on the LCD.
Note that it is also used as a subroutine by LCD_L0_Init().

*/

void  LCD_L0_ReInit(void) {
  LCD_INIT_CONTROLLER();
  aState[0].ConReg0 = (LCD_INIT_R00) | 0x80;
  aState[0].ConReg1 = (LCD_INIT_R01) & (~2);
/* Write control register 0, 1 */
  LCD_WriteReg0( 0, aState[0].ConReg0); /* Control register 1 */ \
  LCD_WriteReg0( 1, aState[0].ConReg1); /* Control register 2 */ \

}

/*
        *********************************************************
        *                                                       *
        *       LCD_Init : Init the display                     *
        *                                                       *
        *********************************************************
*/

int  LCD_L0_Init(void) {
  U8 Data;
  GUI_DEBUG_LOG("\nLCD_L0_Init()");
/* Invalidate register cache */
  { int i;
    for (i=0; i<LCD_NUM_CONTROLLERS; i++) {
     aState[i].x = aState[i].y = 255;
    }
  }
  LCD_L0_ReInit();
/* Check memory */
  SET_X0(1);
  SET_Y0(2);
  WRITE_VMEM0(0x12);
  SET_X0(3);
  SET_Y0(4);
  WRITE_VMEM0(0x34);
  SET_X0(1);
  SET_Y0(2);
  READ_VMEM0(Data);
  if (Data != 0x12) {
    GUI_DEBUG_ERROROUT("\nLCD6642X: video memory problem");
    return LCD_ERR_MEMORY;
  }
  SET_X0(3);
  SET_Y0(4);
  READ_VMEM0(Data);
  if (Data != 0x34) {
    GUI_DEBUG_ERROROUT("\nLCD6642X: video memory problem");
    return LCD_ERR_MEMORY;
  }
/* Fill LCD memory with gray scale bar */
  #if 1
  {
    int x, y;
    int MaxX, MaxY;
    int IncX = (aState[0].ConReg1 & 1<<1) ? 1 : 0;
    if (IncX) {
      MaxX = MAX_COMS;
      MaxY = MAX_SEGS/4;
    } else {
      MaxY = MAX_COMS;
      MaxX = MAX_SEGS/4;
    }
    for (x=0; x < MaxX; x++) {
      if (IncX) {
        SET_X0(0);
        SET_Y0(x);
      } else {
        SET_X0(x);
        SET_Y0(0);
      }
      for (y=0; y<MaxY; y++) {
        WRITE_VMEM0(0x00);
      }
    }
  }
  #endif
  return 0;    /* Init successfull ! */
}

/*
        *********************************************************
        *                                                       *
        *       LCD_L0_CheckInit                                   *
        *                                                       *
        *  Check if display controller(s) is/are still          *
        *  properly initialized                                 *
        *                                                       *
        *********************************************************

Return value:	0 => No error, init is O.K.

*/

#if LCD_SUPPORT_CHECKINIT

char LCD_L0_CheckInit(void) {
  return 0;
}
#endif

#else

void LCD6642X(void) { } /* avoid empty object files */

#endif  /* Right LCD_CONTROLLER */