Optimzation, anyone?

Hi,
So I'm working on my T6963/RA6963 GLCD "driver" and it's slowly going forward. Before moving futher I'm in a bit of optimization mode and one of the biggest bottlenecks I've found this far is my SetPixel routine which is used to calculate the RAM byte and bit adress for a specific pixel and set it. The routine looks like this:

Code:

---

SetPixel:

    GLCD_Data = GFX_HOME + (GLCD_Y * GFX_AREA)
    GLCD_Data = GLCD_Data + (GLCD_X / 6)        ' RAM Adress of byte
    GLCD_Pixel = 5 - GLCD_X // 6                        ' Bit in above byte
     
    ' First set adress pointer to the correct location (byte in VRAM)
    ' We've assigned the data to write above.
    GLCD_Cmd = ADRESS_POINTER_SET
    GOSUB Write_TwoBytes
   
    ' Then set the pixel in the byte pointed at.
    GLCD_CMD = BIT_SET_RESET + SET + GLCD_Pixel
    GOSUB Write_CMD

    RETURN

---

The complete routine, including execution of the subroutines being called executes in roughly 950 cycles. Out of these 950 cycles, ~650 are spent in the first three lines of the above code. Concidering this routine has to executed for each and every pixel drawn on the screen this is where an increase in performance is going to make the most difference.

Background:
GLCD_X and GLCD_Y are both WORD variables specifying the X- and Y-coordinate of the pixel to be set.
GLCD_Data is WORD variable
GLCD_Pixel is BYTE variable
GFX_HOME is a constant specifying the start adress of the graphics section in RAM
GFX_AREA is a constant specifying the number of byte per line (number of columns)

Each pixel line on the display concists of 40bytes, 40bytes*8bits=320bits but the display is only 240bits wide. This means that 2bits in each BYTE is discarded so the first pixel of a line is bit 5 of that particular byte and then it goes on like, 5,4,3,2,1,0 - 5,4,3,2,1,0 - 5,4,3,2,1,0 and so on. (There are other modes too but lets not get into that right now).

So lets assume for a moment that graphic RAM starts at 0 in memory, (GFX_HOME=0) and that we're using 40 columns as described above (GFX_AREA=40). Simple example:

Code:

---

GLCD_X = 0  ' Set pixel in top left corner of display
GLCD_Y = 0
GOSUB SetPixel

---

What's supposed to happen is that GLCD_Data is set to 0 (because we're supposed to set a bit in the very first byte in RAM) and GLCD_Pixel is set to 5 - and that IS what is happening but it takes a freakin amount of time ;-)

Any pointers as to how to increase performance on this would be most welcome. Every cycle counts.

/Henrik.