libgpiod 2 added for GPIO store and show in IOTR/MTR3

1 files changed, 215 insertions, 1 deletions

diff --git a/src/Device/Device.cpp b/src/Device/Device.cpp
index feb763d..9a9e36c 100644
--- a/src/Device/Device.cpp
+++ b/src/Device/Device.cpp
@@ -15,8 +15,10 @@
  *
  ***********************************************************************/
 
+#include <gpiod.h>
 #include "Device.h"
 
+const std::string cmdname ("mts-io-sysfs");
 const std::vector<std::string> Device::apIdentifiers = {
     "ap1", "ap2", "lora", "lora-2", "slot1", "slot2"};
 const std::regex
@@ -494,8 +496,196 @@ void Device::printUsage(std::string program) {
     exitHandler(0);
 }
 
+/*
+ * Print an error message with errno string
+ * and exit with internal identifying number.
+ * parm1:   Message to be printed
+ * parm2:   Saved errno value
+ * parm3:   An integer exit value (-1 means don't exit)
+ * Returns: Void
+ */
+const int NOEXIT = -1;
+void Device::simpleError(std::string msg, int error, int exitval) {
+    std::cerr << cmdname;
+    std::cerr << ": ";
+    std::cerr << msg;
+    std::cerr << ": ";
+    const int len = 512;
+    char buf[len];
+    std::cerr << strerror_r(error,buf,len);
+    std::cerr << "\n";
+    if (exitval > NOEXIT)
+        exitHandler(exitval);
+}
+
+/*
+ * Given a GPIO name, find the chip and line number
+ * parm1:   Name of GPIO
+ * parm2:   pointer to GPIO offset where the name
+ *          was found on return
+ * Returns: chip where GPIO name was found
+ *
+ * Caller should free the chip pointer with
+ * gpiod_chip_close.
+ *
+ * It is assumed the gpio devices are numbered
+ * starting at 0, and are all sequential.
+ *
+ * This function was in the old libgpiod and
+ * was removed from the new library. This
+ * functionality should not have been removed.
+ */
+struct gpiod_chip * Device::gpio_request::gpiod_ctxless_find_line(Device *device, std::string name, unsigned int *r_offset)
+{
+    const std::string gpioDevPrefix = "/dev/gpiochip";
+    int chipnum;
+    char *gpiodevname = NULL;
+    struct gpiod_chip *found_chip;
+    struct gpiod_chip_info *chip_info;
+    struct gpiod_line_info *info;
+    int num_lines = 0;
+    int offset;
+    const char *nm;
+    Device d;
+
+    for(chipnum=0;1;chipnum++) {
+	std::string gpioDevName = gpioDevPrefix;
+	gpioDevName += std::to_string(chipnum);
+        found_chip = gpiod_chip_open(gpioDevName.c_str());
+        if (found_chip == NULL) {
+            if(chipnum == 0) {
+                int error = errno;
+                std::string msg = "Could not open ";
+                msg.append(gpiodevname);
+                device->simpleError(msg,error,NOEXIT);
+            }
+            free(gpiodevname);
+            gpiodevname = NULL;
+            break;
+        }
+            
+        chip_info = gpiod_chip_get_info(found_chip);
+        if (!chip_info) {
+            int error = errno;
+            std::string msg = "Can't read info from GPIO ";
+            msg.append(gpiodevname);
+            device->simpleError(msg,error,100);
+        }
+        num_lines = gpiod_chip_info_get_num_lines(chip_info);
+        gpiod_chip_info_free(chip_info);
+        free(gpiodevname);
+        for (offset = 0; offset < num_lines; offset++) {
+            info = gpiod_chip_get_line_info(found_chip, offset);
+            nm = gpiod_line_info_get_name(info);
+            if (nm && (name.compare(nm) == 0)) {
+                *r_offset = offset;
+                gpiod_line_info_free(info);
+                return found_chip;  // Caller must close chip
+            }
+            gpiod_line_info_free(info);
+            
+        }
+        gpiod_chip_close(found_chip);
+    }
+    *r_offset = 0xffffffff;
+    return NULL;
+}
+
+Device::gpio_request:: gpio_request(Device *device, const std::string name)
+{
+    line_request = NULL;
+    settings = NULL;
+    line_config = NULL;
+    request_config = NULL;
+
+    if(name.empty())
+        device->simpleError("Name not provided\n",EINVAL,101);
+    chip = gpiod_ctxless_find_line(device, name, &rq_offset);
+    if(chip == NULL)
+        return;
+    request_config = gpiod_request_config_new();
+    if (request_config == NULL)
+        device->simpleError("Could not allocate gpiod request_config", errno, 102);
+    gpiod_request_config_set_consumer(request_config,"mts-io-sysfs");
+    line_config = gpiod_line_config_new();
+    if (line_config == NULL)
+        device->simpleError("Could not allocate gpiod line_config", errno, 103);
+    settings = gpiod_line_settings_new();
+    if (settings == NULL)
+        device->simpleError("Could not allocate gpiod line_settings", errno, 104);
+    int result = -1;
+    result = gpiod_line_settings_set_direction(settings,GPIOD_LINE_DIRECTION_AS_IS);
+    if (result == -1)
+        device->simpleError("Could not set direction gpiod line_settings_set_direction GPIOD_LINE_DIRECTION_AS_IS", errno, 105);
+
+    result = gpiod_line_settings_set_bias(settings,GPIOD_LINE_BIAS_AS_IS);
+    if (result == -1)
+        device->simpleError("Could not set bias gpiod line_settings_set_bias GPIOD_LINE_BIAS_AS_IS", errno, 106);
+
+    unsigned int offsets[1];
+    size_t num_offsets = 1;
+    offsets[0] = rq_offset; // Exactly one GPIO is being queried
+
+    result = gpiod_line_config_add_line_settings(line_config,offsets,num_offsets,settings);
+    if (result == -1)
+        device->simpleError("Could not add line settings gpiod line_config_add_line_settings", errno, 107);
+
+    line_request = gpiod_chip_request_lines(chip,request_config,line_config);  // acquire line(s) for exclusive usage.
+    if (line_request == NULL)
+        device->simpleError("Could not request lines gpiod chip_request_lines", errno, 108);
+}
+Device::gpio_request:: ~gpio_request()
+{    
+    if(line_request)
+        gpiod_line_request_release(line_request);
+    if(settings)
+        gpiod_line_settings_free(settings);
+    if(line_config)
+        gpiod_line_config_free(line_config);
+    if(request_config)
+        gpiod_request_config_free(request_config);
+    if(chip)
+        gpiod_chip_close(chip);
+}
+
+struct gpiod_line_request * Device::gpio_request::request()
+{
+    /* This value can be NULL if the gpio name was
+     * not found.
+     */
+    return this->line_request;
+};
+
+unsigned int Device::gpio_request::offset()
+{
+    return this->rq_offset;
+};
+
+
+/* If the name starts in upper case, assume
+ * the name is a GPIO line name from gpio-hog
+ * in device tree.  If it starts in lower case
+ * assume the name is an attribute of the mts-io
+ * driver. GPIOs can only be zero
+ * or one, never a text string.
+ */
 void Device::show(std::string name) {
     std::string fileData;
+    if(isupper(name[0])) {
+        gpio_request gr(this, name);
+        if (gr.request()) {
+            enum gpiod_line_value lv = gpiod_line_request_get_value(gr.request(),gr.offset());
+            if(lv != GPIOD_LINE_VALUE_ERROR) {
+                if(lv == GPIOD_LINE_VALUE_INACTIVE)
+                    std::cout << "0\n";
+                else
+                    std::cout << "1\n";
+                exitHandler(0);
+            } else
+                simpleError("Invalid GPIO line value", errno, 109);
+        }
+    } // Upper case name
+
     int32_t code = MTS::System::readFile(SYSFS_PLATFORM + name, fileData);
     if (code == 0) {
         printf("%s", fileData.c_str());
@@ -504,7 +694,7 @@ void Device::show(std::string name) {
     printError("cat: can't open %s%s: %s",
         SYSFS_PLATFORM, name.c_str(),strerror(errno));
     exitHandler(99);
-}
+}  // End of show
 
 void Device::showTrigger(std::string name) {
     std::string fileData;
@@ -519,12 +709,36 @@ void Device::showTrigger(std::string name) {
     exitHandler(99);
 }
 
+// Prefer gpiod if name starts with upper case and is value 0 or 1
 void Device::store(std::string name, std::string value) {
     if (!isRoot) {
         printError("Must be root to store to %s", name.c_str());
         exitHandler(99);
     }
+
     printDebug("Setting %s to %s", name.c_str(), value.c_str());
+
+    if(isupper(name[0])) {
+        enum gpiod_line_value lv = GPIOD_LINE_VALUE_ERROR;
+
+        if (value.compare("0") == 0)
+            lv = GPIOD_LINE_VALUE_INACTIVE;
+        if (value.compare("1") == 0)
+            lv = GPIOD_LINE_VALUE_ACTIVE;
+        if (lv != GPIOD_LINE_VALUE_ERROR) {
+            gpio_request gr(this, name);
+            int retval;
+            if (gr.request()) {
+                retval = gpiod_line_request_set_value(gr.request(),gr.offset(),lv);
+
+                if(retval != -1)
+                    exitHandler(0);
+                else
+                    simpleError("Could not set GPIO value",errno,111);
+            }
+        }
+    }
+
     std::ofstream fileToWrite(SYSFS_PLATFORM + name);
     if (fileToWrite.is_open()) {
         fileToWrite << value;