W odpowiednim pliku implementacyjnym NokiaLCD.cpp definiujemy wszystkie funkcje jakie są zawarte w powyższej definicji klasy. Pierwsze i najważniejsze funkcje realizują programowy interfejs SPI umożliwiający wysyłanie komend oraz danych do kontrolera wyświetlacza:

/* ***************************** */
/* *** SPI-related functions *** */
/* ***************************** */

void NokiaLCD::spi_write(unsigned char data) {
    CLK = 1;
    for(int i = 0; i < 8; ++i){
        CLK = 0;
        if(data&0x80)
            DATA = 1;
        else
            DATA = 0;
        CLK = 1;
        data <<= 1;
    }
}

void NokiaLCD::spi_writeCommand(unsigned char cmd) {
    CS = 0;
    CLK = 0;
    DATA = 0;           // For commands the first bit is 0
    spi_write(cmd);
    CS = 1;
}

void NokiaLCD::spi_writeData(unsigned char data) {
    CS = 0;
    CLK = 0;
    DATA = 1;           // For data the first bit is 1
    spi_write(data);
    CS = 1;
}

Równie istotny jest konstruktor klasy NokiaLCD, który przyjmuje cztery argumenty typu PinName (nazwy linii GPIO) i wykorzystuje je przy inicjalizacji zmiennych typu DigitalOut (przeprowadzanej na liście inicjalizacyjnej). Wewnątrz konstruktora wykonywana jest jeszcze inicjalizacja wyświetlacza, przez od razu po utworzeniu obiektu klasy NokiaLCD można rysować dowolne elementy na ekranie:

NokiaLCD::NokiaLCD(PinName cs, PinName clk, PinName data, PinName res)
    : CLK(clk), CS(cs), DATA(data), RES(res) {
    CS = 1;
    CLK = 1;
    DATA = 1;
    
    // Hardware reset
    RES = 0;
    wait_ms(100);
    RES = 1;
    wait_ms(100);
    
    // Sleep out
    spi_writeCommand(SLEEPOUT);
    
    // Inversion on
    spi_writeCommand(INVOFF);

    // Color Interface Pixel Format
    spi_writeCommand(COLMOD);
    spi_writeData(0x05);        // 0x05 = 16 bits-per-pixel
    
    // Memory access controler
    spi_writeCommand(MADCTL);
    spi_writeData(0x10);
    
    wait_ms(10);
    
    // Display On
    spi_writeCommand(DISPON);
}

Rys. 5. Domyślna orientacja osi X i Y

Użyta w powyższym kodzie komenda MADCTL pozwala ustawić orientację osi X i Y (rysunki 5 i 6), natomiast komenda COLMOD wybiera format w jakim będą przesyłane wartości kolorów i w danym przykładzie do tego celu są potrzebne 2 bajty (rysunek 7).

Rys. 6. Orientacja osi X i Y w zależności od wartości bitów danych komendy MADCTL

Rys. 7. Ramki wartości kolorów w formacie RGB565

Funkcja czyszcząca ekran zadanym kolorem za pomocą komend CASET oraz PASET ustawia zakres wartości współrzędnych w jakim będzie operować, a następnie w pętli zapisuje wartości poszczególnych pikseli. W przypadku ustawiania kontrastu wystarczy za pomocą komendy SETCON wysłać wartość z przedziału od -64 do 63:

/* ************************* */
/* *** Control functions *** */
/* ************************* */

void NokiaLCD::clear(unsigned int color) {
    // Set column address range
    spi_writeCommand(CASET);
    spi_writeData(0);
    spi_writeData(131);
    
    // Set page(row) address range
    spi_writeCommand(PASET);
    spi_writeData(0);
    spi_writeData(131);
    
    // Write memory
    spi_writeCommand(RAMWR);
    for(int i = 0; i < (132 * 132); i++) { // Write R[4:0] and G[5:3] spi_writeData( (color >> 8)&0xFF );
        // Write G[2:0] and B[4:0]
        spi_writeData( color&0xFF );
    }
}

void NokiaLCD::setContrast(char contrast) {
    // Contrast value should be in range 
    spi_writeCommand(SETCON);
    spi_writeData(contrast);
}

Funkcje do rysowania pojedynczych pikseli oraz wypełniania wskazanego obszaru pod względem implementacji wyglądają podobnie jak funkcja czyszczenia ekranu:

/* ********************************** */
/* *** Graphics-related functions *** */
/* ********************************** */

void NokiaLCD::drawPixel(int x, int y, unsigned int color) {
    // Set column address range
    spi_writeCommand(CASET);
    spi_writeData(x);
    spi_writeData(131);
    
    // Set page(row) address range
    spi_writeCommand(PASET);
    spi_writeData(y);
    spi_writeData(131);
    
    // Write memory
    spi_writeCommand(RAMWR);
    // Write R[4:0] and G[5:3]
    spi_writeData( (color >> 8)&0xFF );
    // Write G[2:0] and B[4:0]
    spi_writeData( color&0xFF );
}

void NokiaLCD::fillRect(int x, int y, int width, int height, unsigned int color) {
    // Set column address range
    spi_writeCommand(CASET);
    spi_writeData(x);
    spi_writeData(x + width - 1);
    
    // Set page(row) address range
    spi_writeCommand(PASET);
    spi_writeData(y);
    spi_writeData(y + height - 1);
    
    // Write memory
    spi_writeCommand(RAMWR);
    for(int i = 0; i < (width * height); i++){ // Write R[4:0] and G[5:3] spi_writeData( (color >> 8)&0xFF );
        // Write G[2:0] and B[4:0]
        spi_writeData( color&0xFF );
    }
}