diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..20d5904
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "stdbool.h": "c"
+    }
+}
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index 17bdc08..728b284 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -2,16 +2,16 @@
     // See https://go.microsoft.com/fwlink/?LinkId=733558
     // for the documentation about the tasks.json format
     "version": "2.0.0",
-    "type":"shell",
-    "command": "make",
-    "echoCommand": true,
-    "problemMatcher": {
-        "base": "gcc",
-    },
     "tasks": [
         {
             "label": "all",
-            "args": ["all"],
+            "type":"shell",
+            "command": "make all -j4",
+            "problemMatcher": {
+                "base": "gcc",
+                "owner": "gcc",
+                "fileLocation":"absolute"
+            },
             "group": {
                 "kind": "build",
                 "isDefault": true
@@ -19,7 +19,23 @@
         },
         {
             "label": "clean",
-            "args": ["clean"],
+            "type":"shell",
+            "command": "make clean -j4",
+            "problemMatcher": {
+                "base": "gcc",
+                "owner": "gcc",
+                "fileLocation":"absolute"
+            },
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            }
         }
-    ]
-}
\ No newline at end of file
+    ],
+    "presentation": {
+        "focus": true,
+        "reveal": "always",
+        "panel": "shared",
+        "clear": true,
+    }
+}
diff --git a/inc/def.h b/inc/def.h
deleted file mode 100644
index c4b2dc5..0000000
--- a/inc/def.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef __INC_DEF_H__
-#define __INC_DEF_H__
-
-#define HELLO "Hello world!"
-
-#endif // __INC_DEF_H__
\ No newline at end of file
diff --git a/inc/ftdi_dev.h b/inc/ftdi_dev.h
new file mode 100644
index 0000000..42b447f
--- /dev/null
+++ b/inc/ftdi_dev.h
@@ -0,0 +1,15 @@
+#ifndef __FTDI_DEV_H__
+#define __FTDI_DEV_H__
+
+#include <ftdi.h>
+
+struct ftdi_dev {
+    struct ftdi_context *ftdi;
+    bool is_open;
+    unsigned int vendor_id;
+    unsigned int product_id;
+    unsigned char bit_mask;     /* Mask of used bits */
+    unsigned char status_mask;  /* Mask of current status of used bits */
+};
+
+#endif
diff --git a/inc/gpio.h b/inc/gpio.h
new file mode 100644
index 0000000..915d301
--- /dev/null
+++ b/inc/gpio.h
@@ -0,0 +1,15 @@
+#ifndef __GPIO_H__
+#define __GPIO_H__
+
+struct gpio {
+    unsigned int pin;
+    struct ftdi_dev *ftdi_dev;
+};
+
+int gpio_open(const struct gpio *gpio);
+int gpio_close(const struct gpio *gpio);
+int gpio_read(const struct gpio *gpio, unsigned int *value);
+int gpio_write(const struct gpio *gpio, unsigned int value);
+int gpio_toggle(const struct gpio *gpio);
+
+#endif
diff --git a/src/gpio.c b/src/gpio.c
new file mode 100644
index 0000000..f8da101
--- /dev/null
+++ b/src/gpio.c
@@ -0,0 +1,155 @@
+#include <stdlib.h>
+#include <syslog.h>
+#include <stdbool.h>
+#include <ftdi_dev.h>
+
+#include <gpio.h>
+
+static int ftdi_open(const struct gpio *gpio)
+{
+    int res;
+
+    if((gpio->ftdi_dev->ftdi = ftdi_new()) == 0) {
+        syslog(LOG_ERR, "ftdi_new failed\n");
+        return EXIT_FAILURE;
+    }
+
+    res = ftdi_usb_open(gpio->ftdi_dev->ftdi, gpio->ftdi_dev->vendor_id, gpio->ftdi_dev->product_id);
+    if (res < 0 && res != -5) {
+        syslog(LOG_ERR, "unable to open ftdi device: %d (%s)\n", res,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        ftdi_free(gpio->ftdi_dev->ftdi);
+        return EXIT_FAILURE;
+    }
+
+    res = ftdi_read_data(gpio->ftdi_dev->ftdi, &(gpio->ftdi_dev->status_mask), 1);
+    if(res < 0) {
+        syslog(LOG_ERR, "unable to readfrom ftdi device: %d (%s)\n", res,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        return EXIT_FAILURE;
+    }
+
+    return EXIT_SUCCESS;
+}
+
+int gpio_open(const struct gpio *gpio)
+{
+    int res;
+    unsigned int init_value = 0;
+
+    if(NULL == gpio) {
+        syslog(LOG_ERR, "No valid gpio object given.\n");
+        return EXIT_FAILURE;
+    }
+
+    if(!gpio->ftdi_dev->is_open) {
+        res = ftdi_open(gpio);
+        if(res != EXIT_SUCCESS) {
+            return EXIT_FAILURE;
+        }
+    }
+
+    gpio->ftdi_dev->bit_mask |= (unsigned char)gpio->pin;
+    syslog(LOG_DEBUG, "bitmask: 0x%02x\n", gpio->ftdi_dev->bit_mask);
+    res = ftdi_set_bitmode(gpio->ftdi_dev->ftdi, gpio->ftdi_dev->bit_mask, BITMODE_BITBANG);
+    if(res < 0) {
+        syslog(LOG_ERR, "unable to set bit bang mode: %d (%s)\n", res,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        ftdi_usb_close(gpio->ftdi_dev->ftdi);
+        ftdi_free(gpio->ftdi_dev->ftdi);
+        return EXIT_FAILURE;
+    }
+
+    res = gpio_write(gpio, init_value);
+    if(res < 0) {
+        syslog(LOG_ERR, "unable to write to gpio %u (%s)\n", gpio->pin,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        ftdi_usb_close(gpio->ftdi_dev->ftdi);
+        ftdi_free(gpio->ftdi_dev->ftdi);
+        return EXIT_FAILURE;
+    }
+    gpio->ftdi_dev->is_open = true;
+
+    return EXIT_SUCCESS;
+}
+
+int gpio_close(const struct gpio *gpio)
+{
+    if(NULL == gpio) {
+        return EXIT_FAILURE;
+    }
+
+    gpio_write(gpio, 0);
+
+    gpio->ftdi_dev->bit_mask &= ~(unsigned char)(gpio->pin);
+    if(gpio->ftdi_dev->bit_mask == 0) {
+        ftdi_usb_close(gpio->ftdi_dev->ftdi);
+        ftdi_free(gpio->ftdi_dev->ftdi);
+    }
+
+    return EXIT_SUCCESS;
+}
+
+int gpio_read(const struct gpio *gpio, unsigned int *value)
+{
+    if(NULL == gpio || NULL == value) {
+        return EXIT_FAILURE;
+    }
+
+    if(gpio->ftdi_dev->status_mask & (unsigned char)(gpio->pin)) {
+        *value = 1;
+    } else {
+        *value = 0;
+    }
+
+    return EXIT_SUCCESS;
+}
+
+int gpio_write(const struct gpio *gpio, unsigned int value)
+{
+    int res;
+    unsigned char mask;
+
+    if(NULL == gpio) {
+        return EXIT_FAILURE;
+    }
+
+    if(value == 0) {
+        mask &= ~(unsigned char)(gpio->pin);
+    }
+    else {
+        mask |= (unsigned char)(gpio->pin);
+    }
+
+    res = ftdi_write_data(gpio->ftdi_dev->ftdi, &mask, 1);
+    if(res < 0) {
+        syslog(LOG_ERR, "write failed for 0x%x, error %d (%s)\n", value, res,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        return EXIT_FAILURE;
+    }
+    gpio->ftdi_dev->status_mask = mask;
+
+    return EXIT_SUCCESS;
+}
+
+int gpio_toggle(const struct gpio *gpio)
+{
+    int res;
+    unsigned char mask = gpio->ftdi_dev->status_mask;
+
+    if(NULL == gpio) {
+        return EXIT_FAILURE;
+    }
+
+    mask ^= (unsigned char)(gpio->pin);
+
+    res = ftdi_write_data(gpio->ftdi_dev->ftdi, &mask, 1);
+    if(res < 0) {
+        syslog(LOG_ERR, "toggle failed, error %d (%s)\n", res,
+               ftdi_get_error_string(gpio->ftdi_dev->ftdi));
+        return EXIT_FAILURE;
+    }
+    gpio->ftdi_dev->status_mask = mask;
+
+    return EXIT_SUCCESS;
+}
diff --git a/src/main.c b/src/main.c
index 08b36d2..d4950bc 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,58 +1,63 @@
+#include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
 #include <syslog.h>
-#include <ftdi.h>
+#include <stdbool.h>
+#include <ftdi_dev.h>
+#include <gpio.h>
 
-#include <def.h>
+/*
+#define GPIO1 0x08  // TX  (brown)
+#define GPIO2 0x01  // TX  (orange)
+#define GPIO3 0x02  // RX  (yellow)
+#define GPIO4 0x14  // RTS (green on FTDI) + DTR (on SparkFun breakout)
+*/
+
+static struct ftdi_dev ftdi_obj = {
+    .ftdi = NULL,
+    .is_open = false,
+    .vendor_id = 0x0403,
+    .product_id = 0x6001,
+    .bit_mask = 0,
+    .status_mask = 0,
+};
+
+static const struct gpio gpio_1 = {
+    .pin = 0x08, /* CTS (brown wire on FTDI cable) */
+    .ftdi_dev = &ftdi_obj,
+};
+static const struct gpio gpio_2 = {
+    .pin = 0x01, /* TX (orange wire on FTDI cable) */
+    .ftdi_dev = &ftdi_obj,
+};
 
-#define LED 0x08
 int main(int argc, char *argv[])
 {
-    struct ftdi_context *ftdi;
-    int f, retval, cnt;
-    unsigned char buf[1];
+    int res, cnt;
+    unsigned int value;
 
     openlog("ftdi_gpio", LOG_CONS | LOG_PID | LOG_NDELAY, LOG_LOCAL1);
-    setlogmask(LOG_DEBUG);
 
-    if ((ftdi = ftdi_new()) == 0) {
-        syslog(LOG_ERR, "ftdi_new failed\n");
-        return EXIT_FAILURE;
+    res = gpio_open(&gpio_1);
+    res |= gpio_open(&gpio_2);
+    if(res != EXIT_SUCCESS) {
+        syslog(LOG_ERR, "Unable to open gpio\n");
+        return res;
     }
 
-    f = ftdi_usb_open(ftdi, 0x0403, 0x6001);
-
-    if (f < 0 && f != -5) {
-        syslog(LOG_ERR, "unable to open ftdi device: %d (%s)\n", f, ftdi_get_error_string(ftdi));
-        retval = 1;
-        goto done;
-    }
-
-    f = ftdi_set_bitmode(ftdi, LED, BITMODE_BITBANG);
-    if(f < 0) {
-        syslog(LOG_ERR, "unable to set bit bang mode: %d (%s)\n", f, ftdi_get_error_string(ftdi));
-        retval = 1;
-        goto done;
-    }
-
-    buf[0] = 0;
+    gpio_write(&gpio_1, 1);
+    gpio_write(&gpio_2, 0);
     for(cnt = 0; cnt < 10; cnt++) {
-        buf[0] ^= LED,
-        f = ftdi_write_data(ftdi, buf, 1);
-        if(f < 0) {
-            syslog(LOG_ERR, "write failed for 0x%x, error %d (%s)\n",buf[0], f, ftdi_get_error_string(ftdi));
-            cnt = 10;
-        } else {
-            syslog(LOG_DEBUG, "set LED to: %u\n", (unsigned int)buf[0]);
-        }
-        
+        gpio_toggle(&gpio_1);
+        gpio_toggle(&gpio_2);
+        gpio_read(&gpio_1, &value);
+        printf("Gpio1: %u\n", value);
+        gpio_read(&gpio_2, &value);
+        printf("Gpio2: %u\n", value);
         sleep(1);
     }
+    gpio_close(&gpio_1);
+    gpio_close(&gpio_2);
 
-    ftdi_usb_close(ftdi);
-done:
-    ftdi_free(ftdi);
-    closelog();
-
-    return retval;
+    return EXIT_SUCCESS;
 }