diff --git a/C/Server/Makefile b/C/Server/Makefile
new file mode 100644
index 0000000..4f0b384
--- /dev/null
+++ b/C/Server/Makefile
@@ -0,0 +1,15 @@
+SDIR	:=	./
+IDIR 	:=	./
+CC	:=	gcc
+
+CFILES	:=	$(foreach dir,$(SDIR),$(wildcard $(dir)/*.c))
+HFILES	:=	$(foreach dir,$(HDIR),$(wildcard $(dir)/*.h))
+
+build:
+	@echo
+	@echo Compiling and linking files
+	$(CC) $(CFILES) -I $(IDIR) -Os -o pixelvloed -Wall
+	@echo Done
+	@echo
+	
+	
diff --git a/C/Server/main.c b/C/Server/main.c
new file mode 100644
index 0000000..a240af3
--- /dev/null
+++ b/C/Server/main.c
@@ -0,0 +1,172 @@
+#include <unistd.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <netinet/in.h>
+#include <netdb.h>
+#include <arpa/inet.h>
+#include <sys/socket.h>
+
+#include "screenbuffer.h"
+
+#define UDP_PORT 5005    // the port users will be connecting to
+#define DISCOVER_PORT 5006    // the port on which the server will broadcast its details
+
+static void die(char *msg)
+{
+	fprintf(stderr, "%s\n", msg);
+	exit(EXIT_FAILURE);
+}
+
+static int get_udp_socket(int port)
+{
+	int sock_fd;
+	struct sockaddr_in6 addr;
+
+	memset(&addr, 0, sizeof(addr));
+
+	if ( ( sock_fd = socket(PF_INET6, SOCK_DGRAM,0) ) < 0 )
+		die("could not create socket");
+
+	addr.sin6_port = htons(port);
+	addr.sin6_family = AF_INET6;
+	addr.sin6_addr = in6addr_any;
+
+	if ( bind(sock_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0 )
+		die("could not bind to UDP");
+
+	return sock_fd;
+}
+
+
+static ssize_t get_udp_packet(int sock_fd, char *packet, size_t size)
+{
+	ssize_t n_read;
+	while ((n_read = recv(sock_fd, packet, size, 0)) < 0 );
+	return n_read;
+}
+
+// PixelVloed C version
+
+// application entry point
+int main(int argc, char* argv[])
+{
+  uint8_t packet[1120 + 32];
+  ssize_t packet_size;
+  uint16_t x, y;
+  uint8_t r, g, b, a;
+  uint8_t pixellength = 0, protocol, version;
+  uint16_t i;
+
+  int sock_fd = get_udp_socket(UDP_PORT);
+  
+  // Init screen buffer
+  if (!init_frame_buffer()) {
+    die("Failed to init screen buffer.\n");
+  }
+
+  // draw...
+  while( (packet_size = get_udp_packet(sock_fd, (char*)&packet, sizeof(packet))) >= 0 ) {
+    
+    
+    
+    // Get protocol from the packet
+    version = packet[1];    // 
+    protocol = packet[0];   // Alpha in old
+    
+    //printf("V:%d, P:%d\n", version, protocol);
+    
+    switch (protocol) {
+      // Protocol 0: xyrgb 16:16:8:8:8 specified for each pixel
+      case 0:
+        pixellength = 7;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | packet[i+1]<<8;
+	        y = packet[i+2] | packet[i+3]<<8;
+	        r = packet[i+4];
+	        g = packet[i+5];
+	        b = packet[i+6];
+	        a = 0xFF;
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Protocol 1: xyrgba 16:16:8:8:8:8 specified for each pixel
+      case 1:
+        pixellength = 8;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | packet[i+1]<<8;
+	        y = packet[i+2] | packet[i+3]<<8;
+	        r = packet[i+4];
+	        g = packet[i+5];
+	        b = packet[i+6];
+	        a = packet[i+7];
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Protocol 2: xyrgb 12:12:8:8:8 specified for each pixel
+      case 2:
+        pixellength = 6;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | (packet[i+1]&0xF0)<<4;
+	        y = (packet[i+1]&0x0F) | packet[i+2]<<4;
+	        r = packet[i+3];
+	        g = packet[i+4];
+	        b = packet[i+5];
+	        a = 0xFF;
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Protocol 3: xyrgba 12:12:8:8:8:8 specified for each pixel
+      case 3:
+        pixellength = 7;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | (packet[i+1]&0xF0)<<4;
+	        y = (packet[i+1]&0x0F) | packet[i+2]<<4;
+	        r = packet[i+3];
+	        g = packet[i+4];
+	        b = packet[i+5];
+	        a = packet[i+6];
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Protocol 4: xyrgb 12:12:3:3:2 specified for each pixel
+      case 4:
+        pixellength = 4;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | (packet[i+1]&0xF0)<<4;
+	        y = (packet[i+1]&0x0F) | packet[i+2]<<4;
+	        r = packet[i+3] & 0xE0;
+	        g = packet[i+3]<<3 & 0xE0;
+	        b = packet[i+3]<<6 & 0xC0;
+	        a = 0xFF;
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Protocol 5: xyrgba 12:12:2:2:2:2 specified for each pixel
+      case 5:
+        pixellength = 4;
+        for (i=2; i<packet_size; i+=pixellength) {
+          x = packet[i  ] | (packet[i+1]&0xF0)<<4;
+	        y = (packet[i+1]&0x0F) | packet[i+2]<<4;
+	        r = packet[i+3] & 0xC0;
+	        g = packet[i+3]<<2 & 0xC0;
+	        b = packet[i+3]<<4 & 0xC0;
+	        a = packet[i+3]<<6 & 0xC0;
+	        write_pixel_to_screen(x, y, r, g, b, a);
+	      }
+	      break;
+	    // Error no protocol defined
+	    default:
+	      break;
+	  }
+    
+    
+  }
+  
+  // De init screen buffer
+  deinit_frame_buffer();
+  
+  return 0; 
+}
diff --git a/C/Server/pixelvloed b/C/Server/pixelvloed
new file mode 100755
index 0000000..fff0eaf
Binary files /dev/null and b/C/Server/pixelvloed differ
diff --git a/C/Server/screenbuffer.c b/C/Server/screenbuffer.c
new file mode 100644
index 0000000..3e60b45
--- /dev/null
+++ b/C/Server/screenbuffer.c
@@ -0,0 +1,145 @@
+/***************************************************
+ * The following frame buffer modes are supported:
+ * - 24bpp - rgba 8/16,8/8,8/0,0/0
+ * - 32bpp - rgba 8/24,8/16,8/8,0,0
+ * 
+ * Use "fbset -s" to get the currently used mode
+ **************************************************/
+#include "screenbuffer.h"
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <fcntl.h>
+#include <linux/fb.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+int fbfd = 0;
+struct fb_var_screeninfo vinfo;
+struct fb_fix_screeninfo finfo;
+long int screensize = 0;
+char *fbp = 0;
+  
+
+int8_t init_frame_buffer(void) {
+  // Open the file for reading and writing
+  fbfd = open("/dev/fb0", O_RDWR);
+  if (!fbfd) {
+    printf("Error: cannot open framebuffer device.\n");
+    return 0;
+  }
+  printf("The framebuffer device was opened successfully.\n");
+
+  // Get variable screen information
+  if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo)) {
+    printf("Error reading variable information.\n");
+    return 0;
+  }
+  printf("Screen info %dx%d, %dbpp\n", vinfo.xres, vinfo.yres, vinfo.bits_per_pixel );
+
+  // Get fixed screen information
+  if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {
+    printf("Error reading fixed information.\n");
+    return 0;
+  }
+  
+  // Check if the framebuffer mode is supported
+  if ((vinfo.bits_per_pixel == 24 || vinfo.bits_per_pixel == 32) &&
+       vinfo.red.length == 8      && vinfo.red.offset == 16 &&
+       vinfo.green.length == 8    && vinfo.green.offset == 8 &&
+       vinfo.blue.length == 8     && vinfo.blue.offset == 0 &&
+       vinfo.transp.length == 0   && vinfo.transp.offset == 0) {
+    printf("Screen buffer mode is supported.\n");
+  } else {
+    printf("ERROR: Screen buffer mode is not supported.\n");
+    return 0;
+  }
+
+  // Map fb to user mem 
+  screensize = finfo.smem_len;
+  fbp = (char*)mmap(0, 
+                    screensize, 
+                    PROT_READ | PROT_WRITE, 
+                    MAP_SHARED, 
+                    fbfd, 
+                    0);
+
+  if ((long)fbp == -1) {
+    printf("Error: Failed to mmap.\n");
+    return 0;
+  }
+  return 1;
+}
+
+
+void deinit_frame_buffer(void)
+{
+  // cleanup
+  munmap((void *)fbp, screensize);
+  close(fbfd);
+}
+
+void write_pixel_to_screen(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a){
+  uint32_t pix_offset;
+  uint16_t temp_r, temp_g, temp_b;
+  
+  // If the pixel is out of the screen there is not need to draw it
+  if (x >= vinfo.xres || y >= vinfo.yres) {
+    return;
+  }
+
+  //pix_offset = byte_pp * x + y * finfo.line_length;
+  
+  // If alpha is used calculate it, otherwise just paint the colors
+  if (a != 0xFF) {
+  
+    // Get the old pixel
+    if (vinfo.bits_per_pixel == 24) {
+      // 24bpp format
+      pix_offset = 3 * x + y * finfo.line_length;
+      temp_r = fbp[pix_offset+0];
+      temp_g = fbp[pix_offset+1];
+      temp_b = fbp[pix_offset+2];
+    } else if (vinfo.bits_per_pixel == 32) {
+      // 32bpp format
+      pix_offset = 4 * x + y * finfo.line_length;
+      temp_r = fbp[pix_offset+0];
+      temp_g = fbp[pix_offset+1];
+      temp_b = fbp[pix_offset+2];
+    } else {
+      temp_r = 0;
+      temp_g = 0;
+      temp_b = 0;
+    }
+    //printf("%08x : %04x %04x %04x\n", pixel.pixel32, temp_r, temp_g, temp_b);
+    // Aply alpha
+    temp_r = (temp_r * (256-a))>>8;
+    temp_r = temp_r + ((r * (a))>>8);
+    temp_g = (temp_g * (256-a))>>8;
+    temp_g = temp_g + ((g * (a))>>8);
+    temp_b = (temp_b * (256-a))>>8;
+    temp_b = temp_b + ((b * (a))>>8);
+  } else {
+    temp_r = r;
+    temp_g = g;
+    temp_b = b;
+  }
+  
+  // Write new pixels to buffer
+  if (vinfo.bits_per_pixel == 24) {
+    // 24bpp format
+    pix_offset = 3 * x + y * finfo.line_length;
+    fbp[pix_offset+2] = temp_r;
+    fbp[pix_offset+1] = temp_g;
+    fbp[pix_offset+0] = temp_b;
+  }
+  if (vinfo.bits_per_pixel == 32) {
+    // 32bpp format
+    pix_offset = 4 * x + y * finfo.line_length;
+    fbp[pix_offset+2] = temp_r;
+    fbp[pix_offset+1] = temp_g;
+    fbp[pix_offset+0] = temp_b;
+  }
+}
diff --git a/C/Server/screenbuffer.h b/C/Server/screenbuffer.h
new file mode 100644
index 0000000..cf449c0
--- /dev/null
+++ b/C/Server/screenbuffer.h
@@ -0,0 +1,11 @@
+#ifndef _SCREENBUFFER_H
+#define _SCREENBUFFER_H
+#include <stdint.h>
+
+
+
+int8_t init_frame_buffer(void);
+void deinit_frame_buffer(void);
+void write_pixel_to_screen(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a);
+
+#endif
diff --git a/vloed.c b/vloed.c
deleted file mode 100644
index 48259fe..0000000
--- a/vloed.c
+++ /dev/null
@@ -1,152 +0,0 @@
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <fcntl.h>
-#include <linux/fb.h>
-#include <sys/mman.h>
-#include <sys/ioctl.h>
-
-#include <netinet/in.h>
-#include <netdb.h>
-#include <arpa/inet.h>
-#include <sys/socket.h>
-
-#define UDP_PORT 5005    // the port users will be connecting to
-#define DISCOVER_PORT 5006    // the port on which the server will broadcast its details
-
-static void die(char *msg)
-{
-	fprintf(stderr, "%s\n", msg);
-	exit(EXIT_FAILURE);
-}
-
-static int get_udp_socket(int port)
-{
-	int sock_fd;
-	struct sockaddr_in6 addr;
-
-	memset(&addr, 0, sizeof(addr));
-
-	if ( ( sock_fd = socket(PF_INET6, SOCK_DGRAM,0) ) < 0 )
-		die("could not create socket");
-
-	addr.sin6_port = htons(port);
-	addr.sin6_family = AF_INET6;
-	addr.sin6_addr = in6addr_any;
-
-	if ( bind(sock_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0 )
-		die("could not bind to UDP");
-
-	return sock_fd;
-}
-
-
-static ssize_t get_udp_packet(int sock_fd, char *packet, size_t size)
-{
-	ssize_t n_read;
-	while ((n_read = recv(sock_fd, packet, size, 0)) < 0 );
-	return n_read;
-}
-
-// PixelVloed C version
-
-// application entry point
-int main(int argc, char* argv[])
-{
-  int fbfd = 0;
-  struct fb_var_screeninfo orig_vinfo;
-  struct fb_var_screeninfo vinfo;
-  struct fb_fix_screeninfo finfo;
-  long int screensize = 0;
-  char *fbp = 0;
-
-  char packet[1120];
-  ssize_t packet_size;
-
-  int sock_fd = get_udp_socket(UDP_PORT);
-
-  // Open the file for reading and writing
-  fbfd = open("/dev/fb0", O_RDWR);
-  if (!fbfd) {
-    die("Error: cannot open framebuffer device.\n");
-  }
-  printf("The framebuffer device was opened successfully.\n");
-
-  // Get variable screen information
-  if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo)) {
-    printf("Error reading variable information.\n");
-  }
-  printf("Original %dx%d, %dbpp\n", vinfo.xres, vinfo.yres, 
-         vinfo.bits_per_pixel );
-
-  // Store for reset (copy vinfo to vinfo_orig)
-  memcpy(&orig_vinfo, &vinfo, sizeof(struct fb_var_screeninfo));
-
-  // Change variable info
-  vinfo.bits_per_pixel = 24;
-  if (ioctl(fbfd, FBIOPUT_VSCREENINFO, &vinfo)) {
-    printf("Error setting variable information.\n");
-  }
-  
-  // Get fixed screen information
-  if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {
-    printf("Error reading fixed information.\n");
-  }
-
-  // map fb to user mem 
-  screensize = finfo.smem_len;
-  fbp = (char*)mmap(0, 
-                    screensize, 
-                    PROT_READ | PROT_WRITE, 
-                    MAP_SHARED, 
-                    fbfd, 
-                    0);
-
-  if ((long)fbp == -1) {
-    die("Failed to mmap.\n");
-  }
-  else {
-    // draw...
-    int x, y, pixelcount;
-    unsigned int color = 0;
-    int pixeloffset = 2;
-    int pixellength = 0;
-    unsigned int pix_offset;
-    unsigned bpp = vinfo.bits_per_pixel / 8;
-    
-    while( (packet_size = get_udp_packet(sock_fd, packet, sizeof(packet))) >= 0 ) {
-      if((uint8_t)packet[0] == 1){
-        pixellength = 8;
-      } else {
-        pixellength = 7;
-      }
-
-      // how many pixels
-      pixelcount = (packet_size-1)/pixellength;
-      //printf("%d pixels in packet of size: %d using a pixel size of: %d \n", pixelcount, packet_size, pixellength);
-      // fetch a pixel from the udp socket
-      int i = 0;
-      for(i=pixeloffset;  // skip pixeloffset
-          i<packet_size-(pixellength-1); // walk trough all bytes untill we get to the end minus one package length
-          i+=pixellength){
-		    x = (uint8_t)packet[i  ] + (((uint8_t)packet[i+1])<<8);
-		    y = (uint8_t)packet[i+2] + (((uint8_t)packet[i+3])<<8);
-        //printf("%d %d %d %d %d %d\n", x, y, packet[i  ], packet[i+1], packet[i+2], packet[i+3]);
-        if(x < vinfo.xres && y < vinfo.yres){
-            pix_offset = bpp * x + y * finfo.line_length;
-            fbp[pix_offset++] = packet[i+6];
-            fbp[pix_offset++] = packet[i+5];
-            fbp[pix_offset] = packet[i+4];
-        }
-      }
-    }
-  }
-  // cleanup
-  munmap((void *)fbp, screensize);
-  if (ioctl(fbfd, FBIOPUT_VSCREENINFO, &orig_vinfo)) {
-    printf("Error re-setting variable information.\n");
-  }
-  close(fbfd);
-  return 0; 
-}