From a7ead42f52d3a06baf72af082b942a1106be5748 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Mon, 14 Jul 2014 09:20:47 +1000
Subject: [PATCH] HAL_Linux: added initial gpio library

---
 libraries/AP_HAL_Linux/GPIO.cpp            | 116 +++++++++++++++++++--
 libraries/AP_HAL_Linux/GPIO.h              |  10 ++
 libraries/AP_HAL_Linux/HAL_Linux_Class.cpp |   2 +-
 3 files changed, 121 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_Linux/GPIO.cpp b/libraries/AP_HAL_Linux/GPIO.cpp
index c39f0ce5d5..65223cbb3a 100644
--- a/libraries/AP_HAL_Linux/GPIO.cpp
+++ b/libraries/AP_HAL_Linux/GPIO.cpp
@@ -3,6 +3,13 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_ERLE
 
 #include "GPIO.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
 
 using namespace Linux;
 
@@ -10,26 +17,123 @@ LinuxGPIO::LinuxGPIO()
 {}
 
 void LinuxGPIO::init()
-{}
+{
+    int fd, len;
+    char buf[64];
+
+    fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", LED_AMBER);
+    ::write(fd, buf, len);
+    close(fd);
+    
+      fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", LED_BLUE);
+    ::write(fd, buf, len);
+    close(fd);
+
+    fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", LED_SAFETY);
+    ::write(fd, buf, len);
+    close(fd);
+
+    fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", SAFETY_SWITCH);
+    ::write(fd, buf, len);
+    close(fd);
+}
 
 void LinuxGPIO::pinMode(uint8_t pin, uint8_t output)
-{}
+{
+    int fd;
+    char buf[64];
+
+    snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR  "/gpio%d/direction", pin);
+
+    fd = ::open(buf, O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::direction");
+    }
+
+    if (output == GPIO_OUTPUT)
+        ::write(fd, "out", 4);
+    else
+        ::write(fd, "in", 3);
+
+    close(fd);
+}
 
 int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin)
 {
-	return -1;
+    return -1;
 }
 
 
 uint8_t LinuxGPIO::read(uint8_t pin) {
-    return 0;
+    int fd;
+    char buf[64];
+    char ch;
+
+    snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", pin);
+
+    fd = open(buf, O_RDONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::read");
+    }
+
+    ::read(fd, &ch, 1);
+    
+    close(fd);
+
+    if (ch == '0') {
+        return 0;
+    } 
+    else if (ch == '1'){
+        return 1;
+    }
+
+    return -1;
 }
 
 void LinuxGPIO::write(uint8_t pin, uint8_t value)
-{}
+{
+    int fd;
+    char buf[64];
+
+    snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", pin);
+
+    fd = open(buf, O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::write");
+    }
+
+    if (value==LOW)
+       ::write(fd, "0", 2);
+    else
+       ::write(fd, "1", 2);
+
+    close(fd);
+}
 
 void LinuxGPIO::toggle(uint8_t pin)
-{}
+{
+    write(pin, !read(pin));
+}
 
 /* Alternative interface: */
 AP_HAL::DigitalSource* LinuxGPIO::channel(uint16_t n) {
diff --git a/libraries/AP_HAL_Linux/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h
index 82023d8c50..02cf399283 100644
--- a/libraries/AP_HAL_Linux/GPIO.h
+++ b/libraries/AP_HAL_Linux/GPIO.h
@@ -4,6 +4,16 @@
 
 #include <AP_HAL_Linux.h>
 
+#define SYSFS_GPIO_DIR "/sys/class/gpio"
+#define LED_AMBER       117
+#define LED_BLUE        48
+#define LED_SAFETY      61
+#define SAFETY_SWITCH   116
+#define LOW             0
+#define HIGH            1
+
+
+
 class Linux::LinuxGPIO : public AP_HAL::GPIO {
 public:
     LinuxGPIO();
diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
index dee60bcf6c..ba3e75123f 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
+++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
@@ -73,7 +73,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const
     }
 
     scheduler->init(NULL);
-
+    gpio->init();
     rcout->init(NULL);
     uartA->begin(115200);
     i2c->begin();