Partial implementation of network config, calibration

sw/src/userspace/remotectl.cpp

@@ -14,6 +14,7 @@
 #include <stdio.h>
 #include <algorithm>
 #include <chrono>
+#include <fstream>
 #include <functional>
 #include <istream>
 #include <map>
@@ -34,6 +35,8 @@ using namespace puzzlefw;
 namespace asio = boost::asio;
 
 
+/* ********  Utility functions  ******** */
+
 /** Return true if the string ends with the suffix. */
 bool str_ends_with(const std::string& value, const std::string& suffix)
 {
@@ -102,6 +105,254 @@ bool parse_float(const std::string& s, double& v)
 }
 
 
+/* ********  Network configuration  ******** */
+
+enum NetworkConfigMode { NETCFG_INVALID = 0, NETCFG_DHCP, NETCFG_STATIC };
+
+struct NetworkConfig {
+    NetworkConfigMode mode;
+    asio::ip::address_v4 ipaddr;
+    asio::ip::address_v4 netmask;
+    asio::ip::address_v4 gateway;
+    NetworkConfig() : mode(NETCFG_INVALID) { }
+};
+
+
+/** Read network configuration from file. */
+bool read_network_config(const char *filename, NetworkConfig& ipcfg)
+{
+    ipcfg = NetworkConfig();
+
+    std::ifstream is(filename);
+    if (!is) {
+        log(LOG_ERROR, "Can not read %s", filename);
+        return false;
+    }
+
+    std::string line;
+
+    while (std::getline(is, line)) {
+        boost::system::error_code ec{};
+        size_t p = line.find_last_not_of(" \t\n\v\f\r");
+        if (p != line.npos) {
+            line.erase(p + 1);
+        }
+        if (line == "MODE=dhcp") {
+            ipcfg.mode = NETCFG_DHCP;
+        }
+        if (line == "MODE=static") {
+            ipcfg.mode = NETCFG_STATIC;
+        }
+        if (line.compare(0, 7, "IPADDR=") == 0) {
+            ipcfg.ipaddr = asio::ip::make_address_v4(line.substr(7), ec);
+        }
+        if (line.compare(0, 8, "NETMASK=") == 0) {
+            ipcfg.netmask = asio::ip::make_address_v4(line.substr(8), ec);
+        }
+        if (line.compare(0, 8, "GATEWAY=") == 0 && line.size() > 8) {
+            ipcfg.gateway = asio::ip::make_address_v4(line.substr(8), ec);
+        }
+        if (ec) {
+            return false;
+        }
+    }
+
+    return ((ipcfg.mode == NETCFG_DHCP)
+            || (ipcfg.mode == NETCFG_STATIC
+                && (! ipcfg.ipaddr.is_unspecified())
+                && (! ipcfg.netmask.is_unspecified())));
+}
+
+
+/** Parse network configuration arguments. */
+bool parse_network_config(const std::vector<std::string>& args,
+                          NetworkConfig& ipcfg,
+                          std::string& errmsg)
+{
+    ipcfg = NetworkConfig{};
+    errmsg = std::string();
+
+    if (args.size() == 1 && str_to_lower(args[0]) == "dhcp") {
+        ipcfg.mode = NETCFG_DHCP;
+        return true;
+    }
+
+    if ((args.size() == 3 || args.size() == 4)
+            && str_to_lower(args[0]) == "static") {
+        ipcfg.mode = NETCFG_STATIC;
+
+        boost::system::error_code ec{};
+        ipcfg.ipaddr = asio::ip::make_address_v4(args[1], ec);
+        if (ec || ipcfg.ipaddr.is_unspecified()) {
+            errmsg = "Invalid IP address";
+            return false;
+        }
+
+        ipcfg.netmask = asio::ip::make_address_v4(args[2], ec);
+        if (ec || ipcfg.netmask.is_unspecified()) {
+            errmsg = "Invalid netmask";
+            return false;
+        }
+
+        // Check that netmask describes a valid prefix.
+        unsigned int mask = ipcfg.netmask.to_uint();
+        if (mask & ((~mask) >> 1)) {
+            errmsg = "Invalid netmask";
+            return false;
+        }
+
+        // Optional gateway address.
+        if (args.size() == 4) {
+            ipcfg.gateway = asio::ip::make_address_v4(args[3], ec);
+            if (ec) {
+                errmsg = "Invalid gateway";
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    errmsg = "Invalid address mode";
+    return false;
+}
+
+
+/* ********  Calibration  ******** */
+
+enum RangeSpec {
+    RANGE_NONE = 0,
+    RANGE_LO = 1,
+    RANGE_HI = 2
+};
+
+struct ChannelCalibration {
+    RangeSpec range_spec;
+    double offset_lo;
+    double offset_hi;
+    double gain_lo;
+    double gain_hi;
+};
+
+struct Calibration {
+    ChannelCalibration channel_cal[4];
+};
+
+
+/** Read calibration from file. */
+void read_calibration_file(const char *filename, Calibration& cal)
+{
+    // Set defaults in case calibration is missing or incomplete.
+    for (unsigned int channel = 0; channel < 4; channel++) {
+        cal.channel_cal[channel].range_spec = RANGE_LO;
+        cal.channel_cal[channel].offset_lo = 8192;
+        cal.channel_cal[channel].offset_hi = 8192;
+        cal.channel_cal[channel].gain_lo = -8191;
+        cal.channel_cal[channel].gain_hi = -409;
+    }
+
+    std::ifstream is(filename);
+    if (!is) {
+        log(LOG_ERROR, "Can not read calibration file");
+        return;
+    }
+
+    std::string line;
+    while (std::getline(is, line)) {
+
+        size_t p = line.find_last_not_of(" \t\n\v\f\r");
+        if (p != line.npos) {
+            line.erase(p + 1);
+        }
+
+        p = line.find('=');
+        if (p == line.npos) {
+            continue;
+        }
+
+        std::string label = line.substr(0, p);
+        std::string value = line.substr(p + 1);
+
+        if (label.size() < 4
+                || label[0] != 'C'
+                || label[1] != 'H'
+                || label[2] < '1'
+                || label[2] > '4'
+                || label[3] != '_') {
+            continue;
+        }
+
+        unsigned int channel = label[2] - '1';
+        label.erase(0, 4);
+
+        if (label == "RANGE") {
+            if (value == "LO") {
+                cal.channel_cal[channel].range_spec = RANGE_LO;
+            }
+            if (value == "HI") {
+                cal.channel_cal[channel].range_spec = RANGE_HI;
+            }
+        }
+        if (label == "OFFSET_LO") {
+            parse_float(value, cal.channel_cal[channel].offset_lo);
+        }
+        if (label == "OFFSET_HI") {
+            parse_float(value, cal.channel_cal[channel].offset_hi);
+        }
+        if (label == "GAIN_LO") {
+            parse_float(value, cal.channel_cal[channel].gain_lo);
+        }
+        if (label == "GAIN_HI") {
+            parse_float(value, cal.channel_cal[channel].gain_hi);
+        }
+    }
+}
+
+
+/** Write calibration to file. */
+bool write_calibration_file(const char *filename, const Calibration& cal)
+{
+    std::ofstream os(filename);
+    if (!os) {
+        log(LOG_ERROR, "Can not write calibration file");
+        return false;
+    }
+
+    for (unsigned int channel = 0; channel < 4; channel++) {
+        std::string line;
+        line = str_format("CH%u_RANGE=%s\n", channel + 1,
+            (cal.channel_cal[channel].range_spec == RANGE_HI) ? "HI" : "LO");
+        os << line;
+
+        line = str_format("CH%u_OFFSET_LO=%.6f\n", channel + 1,
+            cal.channel_cal[channel].offset_lo);
+        os << line;
+
+        line = str_format("CH%u_OFFSET_HI=%.6f\n", channel + 1,
+            cal.channel_cal[channel].offset_hi);
+        os << line;
+
+        line = str_format("CH%u_GAIN_LO=%.6f\n", channel + 1,
+            cal.channel_cal[channel].gain_lo);
+        os << line;
+
+        line = str_format("CH%u_GAIN_HI=%.6f\n", channel + 1,
+            cal.channel_cal[channel].gain_hi);
+        os << line;
+    }
+
+    if (!os) {
+        log(LOG_ERROR, "Can not write calibration file");
+        return false;
+    }
+
+    return true;
+}
+
+
+
+/* ********  class CommandHandler  ******** */
+
 // Forward declaration.
 class ControlServer;
 
@@ -119,8 +370,10 @@ public:
     // Configuration files.
     static constexpr const char * CFG_FILE_CALIBRATION =
         "/var/lib/puzzlefw/cfg/calibration.conf";
-    static constexpr const char * CFG_FILE_NETWORK =
+    static constexpr const char * CFG_FILE_NETWORK_SAVED =
         "/var/lib/puzzlefw/cfg/network.conf";
+    static constexpr const char * CFG_FILE_NETWORK_ACTIVE =
+        "/var/lib/puzzlefw/cfg/network_active.conf";
 
     enum ExitStatus {
         EXIT_ERROR = 1,
@@ -128,24 +381,6 @@ public:
         EXIT_REBOOT = 11
     };
 
-    enum RangeSpec {
-        RANGE_NONE = 0,
-        RANGE_LO = 1,
-        RANGE_HI = 2
-    };
-
-    struct ChannelCalibration {
-        RangeSpec range_spec;
-        double offset_lo;
-        double offset_hi;
-        double gain_lo;
-        double gain_hi;
-    };
-
-    struct Calibration {
-        ChannelCalibration channel_cal[4];
-    };
-
     struct CommandEnvironment {
         int channel;
         RangeSpec range_spec;
@@ -206,7 +441,7 @@ public:
      */
     void reset()
     {
-        read_calibration();
+        read_calibration_file(CFG_FILE_CALIBRATION, m_calibration);
         m_device.set_adc_simulation_enabled(false);
         m_device.set_digital_simulation_enabled(false);
         m_device.set_trigger_mode(TRIG_NONE);
@@ -260,10 +495,10 @@ public:
                 && action.compare(0, 6, "ain:ch") == 0
                 && action[7] == ':') {
             char channel_digit = action[6];
-            if (channel_digit < '0' || channel_digit > '3') {
+            if (channel_digit < '1' || channel_digit > '4') {
                 return err_unknown_command();
             }
-            env.channel = channel_digit - '0';
+            env.channel = channel_digit - '1';
             action[6] = 'N';  // mark channel index
         }
 
@@ -323,14 +558,14 @@ public:
 
         // Handle command IPCFG.
         if (action == "ipcfg" || action == "ipcfg:saved") {
-            if (tokens.size() < 3) {
+            if (tokens.size() < 2) {
                 return err_missing_argument();
             }
-            if (tokens.size() > 4) {
+            if (tokens.size() > 5) {
                 return err_unexpected_argument();
             }
-            const std::string& gw = (tokens.size() > 3) ? tokens[3] : "";
+            tokens.erase(tokens.begin());
-            return cmd_ipcfg(env, tokens[1], tokens[2], gw);
+            return cmd_ipcfg(env, tokens);
         }
 
         return err_unknown_command();
@@ -338,27 +573,13 @@ public:
 
 private:
     /** Asynchronously stop control and/or data servers. */
-    void stop_server(bool stop_control, std::function<void()> handler);
+    void stop_server(std::function<void()> handler);
     void stop_data_servers(unsigned int idx, std::function<void()> handler);
 
     /** Asynchronously start control and/or data servers. */
-    void start_server(bool start_control);
+    void start_server();
     void start_data_servers(unsigned int idx);
 
-    /** Read calibration from file. */
-    void read_calibration()
-    {
-        for (int c = 0; c < 4; c++) {
-            m_calibration.channel_cal[c].range_spec = RANGE_LO;
-            m_calibration.channel_cal[c].offset_lo = 8192;
-            m_calibration.channel_cal[c].offset_hi = 8192;
-            m_calibration.channel_cal[c].gain_lo = -8191;
-            m_calibration.channel_cal[c].gain_hi = -409;
-        }
-
-        // TODO - read from file
-    }
-
     /** Convert a raw ADC sample to Volt. */
     double convert_sample_to_volt(unsigned int channel, unsigned int sample)
     {
@@ -456,7 +677,7 @@ private:
     }
 
     /** Handle command AIN:CHn:OFFS[:range]? */
-    std::string qry_channel_offs(CommandEnvironment env)
+    std::string qry_channel_offset(CommandEnvironment env)
     {
         ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
         RangeSpec range_spec = env.range_spec;
@@ -608,8 +829,25 @@ private:
     /** Handle command IPCFG[:SAVED]? */
     std::string qry_ipcfg(CommandEnvironment env)
     {
-        // TODO - read network config from file
+        const char *filename = env.saved_flag ? CFG_FILE_NETWORK_SAVED
-        return "ERROR";
+                                              : CFG_FILE_NETWORK_ACTIVE;
+        NetworkConfig ipcfg;
+        if (! read_network_config(filename, ipcfg)) {
+            return "ERROR Unconfigured";
+        }
+
+        std::string result;
+        if (ipcfg.mode == NETCFG_DHCP) {
+            return "DHCP";
+        }
+        if (ipcfg.mode == NETCFG_STATIC) {
+            return "STATIC "
+                    + ipcfg.ipaddr.to_string()
+                    + " " + ipcfg.netmask.to_string()
+                    + " " + ipcfg.gateway.to_string();
+        }
+
+        return "ERROR Unconfigured";
     }
 
     /** Handle command RESET */
@@ -706,8 +944,8 @@ private:
     }
 
     /** Handle command AIN:CHn:OFFS[:range] */
-    std::string cmd_channel_offs(CommandEnvironment env,
+    std::string cmd_channel_offset(CommandEnvironment env,
-                                 const std::string& arg)
+                                   const std::string& arg)
     {
         double offs;
         if ((! parse_float(arg, offs))
@@ -962,18 +1200,24 @@ private:
     /** Handle command AIN:MINMAX:CLEAR */
     std::string cmd_cal_save(CommandEnvironment env)
     {
-        // TODO - write calibration to file
+        if (! write_calibration_file(CFG_FILE_CALIBRATION, m_calibration)) {
+            return "ERROR Can not write calibration";
+        }
+
         // TODO - invoke script to save to SD card
+
         return "ERROR";
     }
 
     /** Handle command IPCFG */
     std::string cmd_ipcfg(CommandEnvironment env,
-                          const std::string& ipaddr,
+                          const std::vector<std::string>& args)
-                          const std::string& netmask,
-                          const std::string& gateway)
     {
-        // TODO -- parse and check address
+        NetworkConfig ipcfg;
+        std::string errmsg;
+        if (! parse_network_config(args, ipcfg, errmsg)) {
+            return "ERROR " + errmsg;
+        }
         // TODO -- activate or save address, restart networking if needed
         return "ERROR";
     }
@@ -987,8 +1231,8 @@ private:
         { "ain:channels:count?",    &CommandHandler::qry_channels_count },
         { "ain:channels:active?",   &CommandHandler::qry_channels_active },
         { "ain:chN:range?",         &CommandHandler::qry_channel_range },
-        { "ain:chN:offs?",          &CommandHandler::qry_channel_offs },
+        { "ain:chN:offset?",        &CommandHandler::qry_channel_offset },
-        { "ain:chN:offs:RR?",       &CommandHandler::qry_channel_offs },
+        { "ain:chN:offset:RR?",     &CommandHandler::qry_channel_offset },
         { "ain:chN:gain?",          &CommandHandler::qry_channel_gain },
         { "ain:chN:gain:RR?",       &CommandHandler::qry_channel_gain },
         { "ain:chN:sample?",        &CommandHandler::qry_channel_sample },
@@ -1025,8 +1269,8 @@ private:
             command_table_one_arg = {
         { "ain:channels:active",    &CommandHandler::cmd_channels_active },
         { "ain:chN:range",          &CommandHandler::cmd_channel_range },
-        { "ain:chN:offs",           &CommandHandler::cmd_channel_offs },
+        { "ain:chN:offset",         &CommandHandler::cmd_channel_offset },
-        { "ain:chN:offs:RR",        &CommandHandler::cmd_channel_offs },
+        { "ain:chN:offset:RR",      &CommandHandler::cmd_channel_offset },
         { "ain:chN:gain",           &CommandHandler::cmd_channel_gain },
         { "ain:chN:gain:RR",        &CommandHandler::cmd_channel_gain },
         { "ain:srate",              &CommandHandler::cmd_srate },
@@ -1052,6 +1296,8 @@ private:
 };
 
 
+/* ********  class ControlServer  ******** */
+
 /**
  * Manage TCP connections for remote control.
  */
@@ -1374,21 +1620,16 @@ private:
 
 /* ******** Methods for class CommandHandler ******** */
 
-void CommandHandler::stop_server(bool stop_control,
+void CommandHandler::stop_server(std::function<void()> handler)
-                                 std::function<void()> handler)
 {
-    if (stop_control) {
+    asio::post(m_control_server->get_executor(),
-        asio::post(m_control_server->get_executor(),
+        [this,handler]() {
-            [this,handler]() {
+            m_control_server->stop_server();
-                m_control_server->stop_server();
+            asio::post(m_strand,
-                asio::post(m_strand,
+                [this,handler]() {
-                    [this,handler]() {
+                    stop_data_servers(0, handler);
-                        stop_data_servers(0, handler);
+                });
-                    });
+        });
-            });
-    } else {
-        stop_data_servers(0, handler);
-    }
 }
 
 void CommandHandler::stop_data_servers(unsigned int idx,
@@ -1408,20 +1649,16 @@ void CommandHandler::stop_data_servers(unsigned int idx,
     }
 }
 
-void CommandHandler::start_server(bool start_control)
+void CommandHandler::start_server()
 {
-    if (start_control) {
+    asio::post(m_control_server->get_executor(),
-        asio::post(m_control_server->get_executor(),
+        [this]() {
-            [this]() {
+            m_control_server->start_server();
-                m_control_server->start_server();
+            asio::post(m_strand,
-                asio::post(m_strand,
+                [this]() {
-                    [this]() {
+                    start_data_servers(0);
-                        start_data_servers(0);
+                });
-                    });
+        });
-            });
-    } else {
-        start_data_servers(0);
-    }
 }
 
 void CommandHandler::start_data_servers(unsigned int idx)
@@ -1522,9 +1759,8 @@ int run_remote_control_server(
     io.run();
 
     // TODO -- multi-threading
-    // TODO -- get exit status from command handler
 
-    return 0;
+    return command_handler.exit_status();
 }