lael320256_demo.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h>
#include <ctype.h>
#include <time.h>
#include <wiringPi.h>
#include <math.h>
#include <bcm_host.h>
#include <pthread.h>

// Pin 9  - WPi 9
// Pin 11 - WPi 7
// Pin 13 - WPi 15
// Pin 15 - WPi 16

#define PIN_VS   (9)
#define PIN_HS   (7)
#define PIN_VCLK (15)
#define PIN_VID  (16)

#define SCR_WIDTH (320)
#define SCR_HEIGHT (256)
#define SCR_STRIDE (SCR_WIDTH / 8)

unsigned char framebufA[SCR_STRIDE * SCR_HEIGHT];
unsigned char framebufB[SCR_STRIDE * SCR_HEIGHT];
unsigned char *framebuffers[] = {framebufA, framebufB};
unsigned char *frontbuffer;

sem_t update_wait_for_image;
sem_t image_wait_for_update;
pthread_mutex_t fps_lock;

int fps_counter;

static void delay_ns(unsigned int ns)
{
    struct timespec sleeper, dummy;
    sleeper.tv_sec  = 0;
    sleeper.tv_nsec = ns ;
    nanosleep (&sleeper, &dummy) ;
}

void frame(void) {
    uint8_t *rdptr = frontbuffer;
    digitalWrite(PIN_VCLK, 1);
    for (int y = 0; y < SCR_HEIGHT + 1; y++) {
        digitalWrite(PIN_HS, 0);
        for (int x = 0; x < 4; x++) {
            digitalWrite(PIN_VCLK, 0);
            digitalWrite(PIN_VCLK, 0);
            digitalWrite(PIN_VCLK, 1);
            digitalWrite(PIN_VCLK, 1);
        }
        digitalWrite(PIN_HS, 1);
        for (int x = 0; x < SCR_STRIDE; x++) {
            uint8_t d = *rdptr++;
            digitalWrite(PIN_VS, (y == 0) ? (((x > 10) && (x < 20)) ? 0 : 1) : 1);
            for (int b = 0; b < 8; b++) {
                digitalWrite(PIN_VID, d & 0x01);
                d >>= 1;
                digitalWrite(PIN_VCLK, 0);
                digitalWrite(PIN_VCLK, 0);
                digitalWrite(PIN_VCLK, 1);
            }
        }    
    }

    digitalWrite(PIN_VS, 0);
    digitalWrite(PIN_HS, 0);
    digitalWrite(PIN_VCLK, 0);
}

void putpixel(unsigned char *buf, int x, int y, int c) {
    if (c)
        buf[SCR_STRIDE * y + x / 8] |= 1 << (x % 8);
    else
        buf[SCR_STRIDE * y + x / 8] &= ~(1 << (x % 8));
}

static int add_clamp(int a, int b) {
    int sum = a + b;
    if (sum < 0) return 0;
    if (sum > 255) return 255;
    return sum;
}

static int add_clamp_pixel(uint8_t *buf, int x, int y, int v) {
    buf[y * SCR_WIDTH + x] = add_clamp(buf[y * SCR_WIDTH + x], v);
}

void *image_thread_entrance(void *args) {
    DISPMANX_DISPLAY_HANDLE_T display;
    DISPMANX_MODEINFO_T display_info;
    DISPMANX_RESOURCE_HANDLE_T screen_resource;
    VC_IMAGE_TRANSFORM_T transform;
    uint32_t image_prt;
    VC_RECT_T rect1;
    int ret;
    char *fbp = malloc(SCR_WIDTH * SCR_HEIGHT * 2);
    char *fbg = malloc(SCR_WIDTH * SCR_HEIGHT);

    bcm_host_init();

    display = vc_dispmanx_display_open(0);
    if (!display) {
        fprintf(stderr, "Unable to open primary display");
        return NULL;
    }
    ret = vc_dispmanx_display_get_info(display, &display_info);
    if (ret) {
        fprintf(stderr, "Unable to get primary display information");
        return NULL;
    }
    fprintf(stdout, "Primary display is %d x %d", display_info.width, display_info.height);

    screen_resource = vc_dispmanx_resource_create(VC_IMAGE_RGB565, SCR_WIDTH, SCR_HEIGHT, &image_prt);
    if (!screen_resource) {
        fprintf(stderr, "Unable to create screen buffer");
        vc_dispmanx_display_close(display);
        return NULL;
    }

    vc_dispmanx_rect_set(&rect1, 0, 0, SCR_WIDTH, SCR_HEIGHT);

    int backbufID = 0;

    while (1) {
        clock_t t = clock();

        ret = vc_dispmanx_snapshot(display, screen_resource, 0);
        vc_dispmanx_resource_read_data(screen_resource, &rect1, fbp, SCR_WIDTH * 2);
        uint16_t *rdptr = (uint16_t *)fbp;
        uint8_t *wrptr = (uint8_t *)fbg;

        sem_wait(&image_wait_for_update);

        for (int y = 0; y < SCR_HEIGHT; y++) {
            for (int x = 0; x < SCR_WIDTH; x++) {
                uint16_t rgb = *rdptr++;
                //putpixel(framebuffers[backbufID], x, y, (rgb > 0x8000));
                uint8_t red = ((rgb & 0xF800) >> 8);
                uint8_t green = ((rgb & 0x07E0) >> 3);
                uint8_t blue = ((rgb & 0x001F) << 3);
                uint8_t grayscale = (0.2126 * red) + (0.7152 * green) + (0.0722 * blue);
                *wrptr++ = grayscale;
            }
        }

        for (int y = 0; y < SCR_HEIGHT; y++) {
            for (int x = 0; x < SCR_WIDTH; x++) {
                uint8_t old = fbg[y * SCR_WIDTH + x];
                uint8_t new = (old & 0x80) ? 0xff : 0x00;
                int quantError = (int)old - (int)new;

                fbg[y * SCR_WIDTH + x] = new;
                add_clamp_pixel(fbg, x + 1, y    , quantError * 7 / 16);
                if (x != 0)
                    add_clamp_pixel(fbg, x - 1, y + 1, quantError * 3 / 16);
                if (y != (SCR_HEIGHT - 1)) {
                    add_clamp_pixel(fbg, x    , y + 1, quantError * 5 / 16);
                    add_clamp_pixel(fbg, x + 1, y + 1, quantError * 1 / 16);
                }
            }
        }

        for (int y = 0; y < SCR_HEIGHT; y++) {
            for (int x = 0; x < SCR_WIDTH; x++) {
                putpixel(framebuffers[backbufID], x, y, fbg[y * SCR_WIDTH + x]);
            }
        }

        frontbuffer = framebuffers[backbufID];
        backbufID = !backbufID;

        sem_post(&update_wait_for_image);

        pthread_mutex_lock(&fps_lock);
        fps_counter++;
        pthread_mutex_unlock(&fps_lock);

        // FPS limit
        t = clock() - t;
        int time_to_sleep = CLOCKS_PER_SEC / 61 - t;
        if (time_to_sleep > 0) {
            int ns_to_sleep = time_to_sleep * (1000000000 / CLOCKS_PER_SEC);
            delay_ns(ns_to_sleep);
        }
    }

    ret = vc_dispmanx_resource_delete(screen_resource);
    vc_dispmanx_display_close(display);

    free(fbp);
    free(fbg);

    return NULL;
}

void *update_thread_entrance(void *args) {
    while (1) {
        sem_wait(&update_wait_for_image);
        frame();
        sem_post(&image_wait_for_update);
    }

    return NULL;
}

int main(int argc, char *argv[]) {
    pthread_t update_thread;
    pthread_t image_thread;

    fps_counter = 0;

    wiringPiSetup();

    pinMode(PIN_VS, OUTPUT);
    pinMode(PIN_HS, OUTPUT);
    pinMode(PIN_VCLK, OUTPUT);
    pinMode(PIN_VID, OUTPUT);

    digitalWrite(PIN_VS, 0);
    digitalWrite(PIN_HS, 0);
    digitalWrite(PIN_VCLK, 0);
    digitalWrite(PIN_VID, 0);

    sem_init(&update_wait_for_image, 0, 0);
    sem_init(&image_wait_for_update, 0, 1);
    pthread_mutex_init(&fps_lock, NULL);

    pthread_create(&update_thread, NULL, update_thread_entrance, NULL);
    pthread_create(&image_thread, NULL, image_thread_entrance, NULL);

    printf("CLOCKS_PER_SEC = %d\n", CLOCKS_PER_SEC);

    while(1) {
        sleep(1);
        pthread_mutex_lock(&fps_lock);
        printf("FPS: %d\n", fps_counter);
        fps_counter = 0;
        pthread_mutex_unlock(&fps_lock);
    }

    pthread_join(update_thread, NULL);
    pthread_join(image_thread, NULL);

    sem_destroy(&update_wait_for_image);
    sem_destroy(&image_wait_for_update);

    return 0;
}