diff --git a/data/eg25-manager.service.in b/data/eg25-manager.service.in
new file mode 100644
index 0000000..fe050c1
--- /dev/null
+++ b/data/eg25-manager.service.in
@@ -0,0 +1,21 @@
+[Unit]
+Description=Quectel EG25 modem
+Before=ModemManager.service
+
+[Service]
+Type=simple
+ExecStart=@bindir@/eg25manager
+Restart=on-failure
+ProtectControlGroups=true
+ProtectHome=true
+ProtectSystem=strict
+RestrictSUIDSGID=true
+PrivateTmp=true
+MemoryDenyWriteExecute=true
+PrivateMounts=true
+NoNewPrivileges=true
+CapabilityBoundingSet=
+LockPersonality=true
+
+[Install]
+WantedBy=multi-user.target
diff --git a/data/meson.build b/data/meson.build
index 30a1c92..56bc6e3 100644
--- a/data/meson.build
+++ b/data/meson.build
@@ -11,3 +11,13 @@ conf_files = [
 ]
 
 install_data(conf_files)
+
+serviceconf = configuration_data()
+serviceconf.set('bindir', bindir)
+configure_file(
+  input: 'eg25-manager.service.in',
+  output: 'eg25-manager.service',
+  install_dir: systemdsystemdir,
+  configuration: serviceconf,
+  install: true
+)
diff --git a/meson.build b/meson.build
index fbbbf64..d8d2202 100644
--- a/meson.build
+++ b/meson.build
@@ -8,7 +8,7 @@
 project (
   'eg25-manager',
   'c',
-  version : '0.4.0',
+  version : '0.4.1',
   license : 'GPLv3+',
   meson_version : '>= 0.50.0',
   default_options :
@@ -28,6 +28,8 @@ datadir = get_option('datadir')
 sysconfdir = get_option('sysconfdir')
 bindir = join_paths(prefix, get_option('bindir'))
 udevrulesdir = join_paths(prefix, 'lib/udev/rules.d')
+systemddir = join_paths(prefix, 'lib/systemd')
+systemdsystemdir = join_paths(systemddir, 'system')
 
 if datadir.startswith('/')
   full_datadir = datadir
@@ -46,6 +48,7 @@ eg25_datadir = join_paths(full_datadir, meson.project_name())
 
 add_global_arguments('-D@0@="@1@"'.format('EG25_CONFDIR', eg25_confdir), language : 'c')
 add_global_arguments('-D@0@="@1@"'.format('EG25_DATADIR', eg25_datadir), language : 'c')
+add_global_arguments('-D@0@="@1@"'.format('EG25_VERSION', meson.project_version()), language : 'c')
 
 mmglib_dep = dependency('mm-glib', required : false)
 if mmglib_dep.found()
diff --git a/src/at.c b/src/at.c
index 228ded6..ee99ff4 100644
--- a/src/at.c
+++ b/src/at.c
@@ -5,10 +5,12 @@
  */
 
 #include "at.h"
+#include "config.h"
 #include "suspend.h"
 #include "gpio.h"
 #include "gnss.h"
 
+#include <errno.h>
 #include <fcntl.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -46,11 +48,29 @@ static int configure_serial(const char *tty)
     return fd;
 }
 
+static void at_free_command(gpointer cmd, gpointer data)
+{
+    struct AtCommand *at_cmd = cmd;
+    struct EG25Manager *manager = data;
+
+    if (!at_cmd)
+        return;
+
+    g_free(at_cmd->cmd);
+    g_free(at_cmd->subcmd);
+    g_free(at_cmd->value);
+    g_free(at_cmd->expected);
+    g_free(at_cmd);
+
+    if (manager && manager->at_cmds)
+        manager->at_cmds = g_list_remove(manager->at_cmds, at_cmd);
+}
+
 gboolean at_send_command(struct EG25Manager *manager)
 {
     char command[256];
     struct AtCommand *at_cmd = manager->at_cmds ? g_list_nth_data(manager->at_cmds, 0) : NULL;
-    int ret, len = 0;
+    int ret, len = 0, pos = 0;
 
     if (at_cmd) {
     	/* Wake up the modem from soft sleep before sending an AT command */
@@ -58,39 +78,72 @@ gboolean at_send_command(struct EG25Manager *manager)
 
 	/* Send AT command */
         if (at_cmd->subcmd == NULL && at_cmd->value == NULL && at_cmd->expected == NULL)
-            len = sprintf(command, "AT+%s\r\n", at_cmd->cmd);
+            len = snprintf(command, sizeof(command), "AT+%s\r\n", at_cmd->cmd);
         else if (at_cmd->subcmd == NULL && at_cmd->value == NULL)
-            len = sprintf(command, "AT+%s?\r\n", at_cmd->cmd);
+            len = snprintf(command, sizeof(command), "AT+%s?\r\n", at_cmd->cmd);
         else if (at_cmd->subcmd == NULL && at_cmd->value)
-            len = sprintf(command, "AT+%s=%s\r\n", at_cmd->cmd, at_cmd->value);
+            len = snprintf(command,  sizeof(command),"AT+%s=%s\r\n", at_cmd->cmd, at_cmd->value);
         else if (at_cmd->subcmd && at_cmd->value == NULL)
-            len = sprintf(command, "AT+%s=\"%s\"\r\n", at_cmd->cmd, at_cmd->subcmd);
+            len = snprintf(command, sizeof(command), "AT+%s=\"%s\"\r\n", at_cmd->cmd, at_cmd->subcmd);
         else if (at_cmd->subcmd && at_cmd->value)
-            len = sprintf(command, "AT+%s=\"%s\",%s\r\n", at_cmd->cmd, at_cmd->subcmd, at_cmd->value);
+            len = snprintf(command, sizeof(command), "AT+%s=\"%s\",%s\r\n", at_cmd->cmd, at_cmd->subcmd, at_cmd->value);
+
+        if (len < 0) {
+            g_warning("snprintf(3) failed");
+            at_next_command(manager);
+            return FALSE;
+        }
+        else if (len >= sizeof(command)) {
+            g_warning("AT command does not fit into buffer "
+                "(%d bytes required, %zu available)", len, sizeof(command));
+            at_next_command(manager);
+            return FALSE;
+        }
+
         manager->at_callback = at_cmd->callback;
 
-        ret = write(manager->at_fd, command, len);
-        if (ret < len)
-            g_warning("Couldn't write full AT command: wrote %d/%d bytes", ret, len);
+        do {
+            ret = write(manager->at_fd, &command[pos], len);
 
-        g_message("Sending command: %s", g_strstrip(command));
+            if (ret < 0) {
+                switch (errno) {
+                case EAGAIN:
+                case EINTR:
+                    /* Try again. */
+                    break;
+
+                default:
+                    g_warning("error sending AT command: %s", strerror(errno));
+                    at_next_command(manager);
+                    return FALSE;
+                }
+            }
+            else {
+                len -= ret;
+                pos += ret;
+            }
+        } while (len > 0);
+
+        g_message("Successfully sent command: %s", g_strstrip(command));
     } else {
     	/* Allow the modem to enter soft sleep again when we sent the AT command*/
     	gpio_sequence_sleep(manager);
-	
-	if (manager->modem_state < EG25_STATE_CONFIGURED) {
-	    if (manager->modem_iface == MODEM_IFACE_MODEMMANAGER) {
-#ifdef HAVE_MMGLIB
-                MMModemState modem_state = mm_modem_get_state(manager->mm_modem);
 
-		if (manager->mm_modem && modem_state >= MM_MODEM_STATE_REGISTERED)
-                    modem_update_state(manager, modem_state);
-		else
-                    manager->modem_state = EG25_STATE_CONFIGURED;
+        if (manager->modem_state < EG25_STATE_CONFIGURED) {
+            if (manager->modem_iface == MODEM_IFACE_MODEMMANAGER) {
+#ifdef HAVE_MMGLIB
+                if (manager->modem_state == EG25_STATE_ACQUIRED) {
+                    MMModemState modem_state = mm_modem_get_state(manager->mm_modem);
+
+                    if (manager->mm_modem && modem_state >= MM_MODEM_STATE_REGISTERED)
+                        modem_update_state(manager, modem_state);
+                    else
+                        manager->modem_state = EG25_STATE_CONFIGURED;
+                }
 #endif
             } else {
                 manager->modem_state = EG25_STATE_CONFIGURED;
-	    }
+            }
         } else if (manager->modem_state == EG25_STATE_SUSPENDING) {
             modem_suspend_post(manager);
         } else if (manager->modem_state == EG25_STATE_RESETTING) {
@@ -105,16 +158,7 @@ void at_next_command(struct EG25Manager *manager)
 {
     struct AtCommand *at_cmd = manager->at_cmds ? g_list_nth_data(manager->at_cmds, 0) : NULL;
 
-    if (!at_cmd)
-        return;
-
-    g_free(at_cmd->cmd);
-    g_free(at_cmd->subcmd);
-    g_free(at_cmd->value);
-    g_free(at_cmd->expected);
-    g_free(at_cmd);
-    manager->at_cmds = g_list_remove(manager->at_cmds, at_cmd);
-
+    at_free_command(at_cmd, manager);
     at_send_command(manager);
 }
 
@@ -209,12 +253,15 @@ static gboolean modem_response(gint fd,
     } while (ret > 0 && pos < (sizeof(response) - 1));
 
     if (pos > 0) {
+        g_autofree gchar *escaped = NULL;
+
         response[pos] = 0;
         g_strstrip(response);
         if (strlen(response) == 0)
             return TRUE;
 
-        g_message("Response: [%s]", response);
+        escaped = g_strescape(response, "\"");
+        g_message("Response: [%s]", escaped);
 
         /*
          * When the modem is started, it outputs 'RDY' to indicate that
@@ -240,14 +287,15 @@ static gboolean modem_response(gint fd,
         else if (strstr(response, "ERROR") && !strstr(response, "fast/poweroff"))
             retry_at_command(manager);
         /*
-         * Successfull AT responses contain 'OK', except for AT+QFUPL which also
+         * Successful AT responses contain 'OK', except for AT+QFUPL which also
          * returns 'CONNECT' when the modem is ready to receive data over serial
+         * and '+QFUPL:...' when data upload is complete
          */
-        else if (strstr(response, "OK") || strstr(response, "CONNECT")) {
+        else if (strstr(response, "OK") || strstr(response, "CONNECT") || strstr(response, "QFUPL")) {
             if (manager->at_callback != NULL)
                 manager->at_callback(manager, response);
             else
-                g_warning("AT command succesfull but no callback registered");
+                g_warning("AT command successful but no callback registered");
         }
         /* Not a recognized response, try running next command, just in case */
         else
@@ -273,67 +321,61 @@ static void parse_commands_list(toml_array_t *array, GArray **cmds)
             continue;
 
         value = toml_string_in(table, "cmd");
-        if (value.ok) {
-            cmd->cmd = g_strdup(value.u.s);
-            free(value.u.s);
-        }
+        if (value.ok)
+            cmd->cmd = value.u.s;
 
         value = toml_string_in(table, "subcmd");
-        if (value.ok) {
-            cmd->subcmd = g_strdup(value.u.s);
-            free(value.u.s);
-        }
+        if (value.ok)
+            cmd->subcmd = value.u.s;
 
         value = toml_string_in(table, "value");
-        if (value.ok) {
-            cmd->value = g_strdup(value.u.s);
-            free(value.u.s);
-        }
+        if (value.ok)
+            cmd->value = value.u.s;
 
         value = toml_string_in(table, "expect");
-        if (value.ok) {
-            cmd->expected = g_strdup(value.u.s);
-            free(value.u.s);
-        }
+        if (value.ok)
+            cmd->expected = value.u.s;
     }
 }
 
-int at_init(struct EG25Manager *manager, toml_table_t *config)
+int at_init(struct EG25Manager *manager, toml_table_t *config[])
 {
-    toml_array_t *commands;
-    toml_datum_t uart_port;
+    toml_array_t *commands = NULL;
+    gchar *uart_port = NULL;
+    toml_table_t *at_config[EG25_CONFIG_COUNT];
 
-    uart_port = toml_string_in(config, "uart");
-    if (!uart_port.ok)
+    for (int i = 0; i < EG25_CONFIG_COUNT; i++)
+        at_config[i] = config[i] ? toml_table_in(config[i], "at") : NULL;
+
+    if (!at_config[EG25_CONFIG_SYS])
+        g_error("Default config file lacks the 'at' section!");
+
+    if (!config_get_string(at_config, "uart", &uart_port))
         g_error("Configuration file lacks UART port definition");
 
-    manager->at_fd = configure_serial(uart_port.u.s);
+    manager->at_fd = configure_serial(uart_port);
     if (manager->at_fd < 0) {
-        g_critical("Unable to configure %s", uart_port.u.s);
-        free(uart_port.u.s);
+        g_critical("Unable to configure %s", uart_port);
+        g_free(uart_port);
         return 1;
     }
-    free(uart_port.u.s);
+    g_free(uart_port);
 
     manager->at_source = g_unix_fd_add(manager->at_fd, G_IO_IN, modem_response, manager);
 
-    commands = toml_array_in(config, "configure");
-    if (!commands)
+    if (!config_get_array(at_config, "configure", &commands))
         g_error("Configuration file lacks initial AT commands list");
     parse_commands_list(commands, &configure_commands);
 
-    commands = toml_array_in(config, "suspend");
-    if (!commands)
+    if (!config_get_array(at_config, "suspend", &commands))
         g_error("Configuration file lacks suspend AT commands list");
     parse_commands_list(commands, &suspend_commands);
 
-    commands = toml_array_in(config, "resume");
-    if (!commands)
+    if (!config_get_array(at_config, "resume", &commands))
         g_error("Configuration file lacks resume AT commands list");
     parse_commands_list(commands, &resume_commands);
 
-    commands = toml_array_in(config, "reset");
-    if (!commands)
+    if (!config_get_array(at_config, "reset", &commands))
         g_error("Configuration file lacks reset AT commands list");
     parse_commands_list(commands, &reset_commands);
 
@@ -354,6 +396,13 @@ void at_destroy(struct EG25Manager *manager)
 
 void at_sequence_configure(struct EG25Manager *manager)
 {
+    /*
+     * When configuring a new modem we should avoid processing an old
+     * command queue, so let's first clear the whole list
+     */
+    if (manager->at_cmds)
+        g_list_foreach(manager->at_cmds, at_free_command, manager);
+
     for (guint i = 0; i < configure_commands->len; i++) {
         struct AtCommand *cmd = &g_array_index(configure_commands, struct AtCommand, i);
         at_append_command(manager, cmd->cmd, cmd->subcmd, cmd->value, cmd->expected, at_process_result);
diff --git a/src/at.h b/src/at.h
index 0364e2c..8e6c5d1 100644
--- a/src/at.h
+++ b/src/at.h
@@ -17,7 +17,7 @@ typedef struct AtCommand {
     int retries;
 } AtCommand;
 
-int at_init(struct EG25Manager *manager, toml_table_t *config);
+int at_init(struct EG25Manager *manager, toml_table_t *config[]);
 void at_destroy(struct EG25Manager *manager);
 
 void at_process_result(struct EG25Manager *manager,
diff --git a/src/config.c b/src/config.c
new file mode 100644
index 0000000..d4ce151
--- /dev/null
+++ b/src/config.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2020 Arnaud Ferraris <arnaud.ferraris@gmail.com>
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#include "config.h"
+#include "toml.h"
+
+gboolean config_get_bool(toml_table_t **config, const gchar *key, gboolean *result)
+{
+    toml_datum_t value = { .ok = 0 };
+
+    if (config[EG25_CONFIG_USER])
+        value = toml_bool_in(config[EG25_CONFIG_USER], key);
+    if (!value.ok)
+        value = toml_bool_in(config[EG25_CONFIG_SYS], key);
+    if (value.ok && result)
+        *result = value.u.b;
+
+    return !!value.ok;
+}
+
+gboolean config_get_int(toml_table_t **config, const gchar *key, gint *result)
+{
+    toml_datum_t value = { .ok = 0 };
+
+    if (config[EG25_CONFIG_USER])
+        value = toml_int_in(config[EG25_CONFIG_USER], key);
+    if (!value.ok)
+        value = toml_int_in(config[EG25_CONFIG_SYS], key);
+    if (value.ok && result)
+        *result = value.u.i;
+
+    return !!value.ok;
+}
+
+gboolean config_get_uint(toml_table_t **config, const gchar *key, guint *result)
+{
+    gint value;
+    gboolean found;
+
+    found = config_get_int(config, key, &value);
+    if (found) {
+        if (value <= 0 || value >= G_MAXUINT) {
+            g_message("Value out of range for [%s], discarding", key);
+            found = FALSE;
+        }
+    }
+
+    if (found && result)
+        *result = (guint) value;
+
+    return found;
+}
+
+gboolean config_get_string(toml_table_t **config, const gchar *key, gchar **result)
+{
+    toml_datum_t value = { .ok = 0 };
+
+    if (config[EG25_CONFIG_USER])
+        value = toml_string_in(config[EG25_CONFIG_USER], key);
+    if (!value.ok)
+        value = toml_string_in(config[EG25_CONFIG_SYS], key);
+    if (value.ok && result)
+        *result = value.u.s;
+
+    return !!value.ok;
+}
+
+gboolean config_get_array(toml_table_t **config, const gchar *key, toml_array_t **result)
+{
+    toml_array_t *array = NULL;
+
+    if (config[EG25_CONFIG_USER])
+        array = toml_array_in(config[EG25_CONFIG_USER], key);
+    if (!array)
+        array = toml_array_in(config[EG25_CONFIG_SYS], key);
+    if (array && result)
+        *result = array;
+
+    return !!array;
+}
diff --git a/src/config.h b/src/config.h
new file mode 100644
index 0000000..d1a214f
--- /dev/null
+++ b/src/config.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2020 Arnaud Ferraris <arnaud.ferraris@gmail.com>
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#pragma once
+
+#include <glib.h>
+
+#include "manager.h"
+#include "toml.h"
+
+/*
+ * Helper functions for parsing config files: each function retrieves the
+ * value for key `key`, with the user config file having priority over the
+ * default config file. The values are stored in `result`.
+ *
+ * They all return TRUE if the value was found, FALSE otherwise.
+ */
+
+gboolean config_get_bool(toml_table_t **config, const gchar *key, gboolean *result);
+gboolean config_get_int(toml_table_t **config, const gchar *key, gint *result);
+gboolean config_get_uint(toml_table_t **config, const gchar *key, guint *result);
+gboolean config_get_string(toml_table_t **config, const gchar *key, gchar **result);
+gboolean config_get_array(toml_table_t **config, const gchar *key, toml_array_t **result);
diff --git a/src/gnss.c b/src/gnss.c
index aeb73af..6503943 100644
--- a/src/gnss.c
+++ b/src/gnss.c
@@ -4,12 +4,18 @@
  * SPDX-License-Identifier: GPL-3.0-or-later
  */
 
+#include "config.h"
 #include "gnss.h"
 #include "manager.h"
 #include "at.h"
 
+#include <sys/sendfile.h>
+#include <sys/stat.h>
+#include <errno.h>
+
 #define BUFFER_SIZE 256
-#define UPLOAD_DELAY 100000
+#define UPLOAD_DELAY_US 25000
+#define UPLOAD_TIMEOUT_S 10
 #define RESCHEDULE_IN_SECS 30
 
 static void gnss_step(struct EG25Manager *manager);
@@ -27,51 +33,60 @@ gboolean gnss_upload_assistance_data(struct EG25Manager *manager)
         return FALSE;
     }
 
+    /* data upload isn't necessary to bring the modem onine, so we should wait
+     * until we've finished the rest of our configuration */
+    if (!manager->modem_iface ||
+         manager->modem_state < EG25_STATE_CONFIGURED ||
+         manager->modem_state > EG25_STATE_CONNECTED) {
+        g_message ("Rescheduling upload since modem isn't online yet, in %ds",
+                   RESCHEDULE_IN_SECS);
+        manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
+        return TRUE;
+    }
+
+#ifdef HAVE_MMGLIB
     /* ModemManager's Location is only available after unlocking */
-    if(!manager->mm_location) {
+    if(manager->modem_iface == MODEM_IFACE_MODEMMANAGER && !manager->mm_location) {
         g_message ("Rescheduling upload since Location interface is not available, in %ds",
                    RESCHEDULE_IN_SECS);
         manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
         return TRUE;
     }
+#endif
 
     manager->gnss_assistance_step = EG25_GNSS_STEP_FIRST;
     gnss_step(manager);
     return FALSE;
 }
 
-void gnss_init(struct EG25Manager *manager, toml_table_t *config)
+void gnss_init(struct EG25Manager *manager, toml_table_t *config[])
 {
-    toml_datum_t enabled;
-    toml_datum_t url;
-    toml_datum_t file;
+    toml_table_t *gnss_config[EG25_CONFIG_COUNT];
     g_autoptr (GError) error = NULL;
 
+    for (int i = 0; i < EG25_CONFIG_COUNT; i++)
+        gnss_config[i] = config[i] ? toml_table_in(config[i], "gnss") : NULL;
+
+    if (!gnss_config[EG25_CONFIG_SYS])
+        g_error("Default config file lacks the 'gnss' section!");
+
     /*
      * GNSS assistance is an optional feature, you can disable it
      * if you want in the configuration file.
      * In case the configuration is missing, we assume GNSS assistance
      * to be disabled.
      */
-    enabled = toml_bool_in(config, "enabled");
-    manager->gnss_assistance_enabled = FALSE;
-    if (enabled.ok)
-        manager->gnss_assistance_enabled = enabled.u.b;
+    config_get_bool(gnss_config, "enabled", &manager->gnss_assistance_enabled);
 
     if (!manager->gnss_assistance_enabled) {
         g_message("GNSS assistance is disabled!");
         return;
     }
 
-    url = toml_string_in(config, "url");
-    if (url.ok)
-        manager->gnss_assistance_url = url.u.s;
-    else
+    if (!config_get_string(gnss_config, "url", &manager->gnss_assistance_url))
         g_error("GNSS assistance server URL is missing from config file");
-    file = toml_string_in(config, "file");
-    if (file.ok)
-        manager->gnss_assistance_file = file.u.s;
-    else
+
+    if (!config_get_string(gnss_config, "file", &manager->gnss_assistance_file))
         g_error("GNSS assistance file name is missing from config file");
 
     /* Create temporary file to store assistance data */
@@ -87,6 +102,8 @@ void gnss_init(struct EG25Manager *manager, toml_table_t *config)
 
 void gnss_destroy(struct EG25Manager *manager)
 {
+    g_free(manager->gnss_assistance_url);
+    g_free(manager->gnss_assistance_file);
     close(manager->gnss_assistance_fd);
 }
 
@@ -182,69 +199,71 @@ static void state_at_gnss(struct EG25Manager *manager)
 
 static void fetch_assistance_data(struct EG25Manager *manager)
 {
-    CURL *curl;
     CURLcode response;
-    long status_code;
-    gchar *url = NULL;
+    curl_off_t downloaded;
+    CURL *curl = NULL;
+    g_autofree gchar *url = NULL;
     FILE *tmp_file = NULL;
-    long int size;
+    gchar errbuf[CURL_ERROR_SIZE];
+    errbuf[0] = 0;
 
     /* Fetch assistance data with curl */
-    tmp_file = fdopen(manager->gnss_assistance_fd, "wb");
+    tmp_file = fdopen(manager->gnss_assistance_fd, "wb+");
+    if (tmp_file == NULL) {
+        g_critical("Unable to open file to save assistance data: %s",
+                   g_strerror(errno));
+        goto bail;
+    }
+
+    lseek(manager->gnss_assistance_fd, 0, SEEK_SET);
+    if (ftruncate(manager->gnss_assistance_fd, 0) < 0)
+        g_warning("Unable to truncate file, assistance data might be invalid!");
     url = g_strconcat(manager->gnss_assistance_url, "/",
                       manager->gnss_assistance_file, NULL);
+
     curl = curl_easy_init();
-    if (!curl)
-        g_error ("Unable to initialize curl");
+    if (!curl) {
+        g_critical("Unable to initialize curl");
+        goto bail;
+    }
+
     curl_easy_setopt(curl, CURLOPT_URL, url);
     curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, NULL);
     curl_easy_setopt(curl, CURLOPT_WRITEDATA, tmp_file);
+    curl_easy_setopt(curl, CURLOPT_ERRORBUFFER, errbuf);
     curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);
     curl_easy_setopt(curl, CURLOPT_FAILONERROR, 1L);
+
     response = curl_easy_perform(curl);
-    if (response == CURLE_HTTP_RETURNED_ERROR) {
-        curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &status_code);
-        curl_easy_cleanup(curl);
-        g_warning ("Unable to fetch GNSS assistance data from %s (HTTP %ld)",
-                   url, status_code);
-
-        /* Restart upload on HTTP error status code */
-        g_message ("Rescheduling upload because of failure in %ds",
-                   RESCHEDULE_IN_SECS);
-        manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
-        g_timeout_add_seconds(RESCHEDULE_IN_SECS,
-                              G_SOURCE_FUNC(gnss_upload_assistance_data),
-                              manager);
-        return;
+    if (response != CURLE_OK) {
+        g_warning("Unable to fetch GNSS assistance data from %s: %s",
+                  url, strlen(errbuf) ? errbuf : curl_easy_strerror(response));
+        goto bail;
     }
 
-    /* Get file size in bytes */
-    size = (long int)lseek(manager->gnss_assistance_fd, 0, SEEK_END);
-    lseek(manager->gnss_assistance_fd, 0, SEEK_SET);
-
-    if (size <= 0) {
-        g_warning ("GNSS assistance data contains 0 bytes,"
-                   "check network connection.");
-        /*
-         * Restart upload when file does not contain any data,
-         * mostly because of no network connection.
-         */
-        g_message ("Rescheduling upload because of failure in %ds",
-                   RESCHEDULE_IN_SECS);
-        manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
-        g_timeout_add_seconds(RESCHEDULE_IN_SECS,
-                              G_SOURCE_FUNC(gnss_upload_assistance_data),
-                              manager);
-        return;
+    response = curl_easy_getinfo(curl, CURLINFO_SIZE_DOWNLOAD_T, &downloaded);
+    if (response) {
+        g_critical("Unable to get number of downloaded bytes from curl");
+        goto bail;
+    } else if (downloaded <= 0) {
+        g_warning("Downloaded empty assistance data file");
+        goto bail;
     }
 
-    g_message("Fetching GNSS assistance data from %s was successfull", url);
+    g_message("Fetching GNSS assistance data from %s was successful", url);
+
+    fflush(tmp_file);
     curl_easy_cleanup(curl);
-    g_free(url);
 
     /* Go to the next step */
     manager->gnss_assistance_step++;
     gnss_step(manager);
+    return;
+
+bail:
+    if (curl != NULL)
+        curl_easy_cleanup(curl);
+    manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
 }
 
 /******************************************************************************/
@@ -255,6 +274,12 @@ static void init_assistance_data_upload_ready(struct EG25Manager *manager,
     /* Search for 'CONNECT' in response to start upload */
     if (strstr(response, "CONNECT")) {
         g_message("Modem ready for GNSS assistance data upload");
+        manager->gnss_assistance_step++;
+        gnss_step(manager);
+    } else if (strstr(response, "QFUPL")) {
+        /* Clear QFUPL AT command and process next */
+        at_next_command(manager);
+
         manager->gnss_assistance_step++;
         gnss_step(manager);
     }
@@ -264,18 +289,24 @@ static void init_assistance_data_upload_start(struct EG25Manager *manager,
                                               const char *response)
 {
     gchar value[BUFFER_SIZE];
-    long int size;
+    off_t size;
 
     /* Process AT response */
     at_process_result(manager, response);
 
     /* Get file size in bytes */
-    size = (long int)lseek(manager->gnss_assistance_fd, 0, SEEK_END);
+    size = lseek(manager->gnss_assistance_fd, 0, SEEK_END);
+    if (size == -1) {
+        g_critical("gnss: unable to read size of xtra data file: %s", g_strerror(errno));
+
+        manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
+        return;
+    }
     lseek(manager->gnss_assistance_fd, 0, SEEK_SET);
 
     /* Start upload */
-    g_snprintf(value, BUFFER_SIZE, "\"RAM:%s\",%ld\r\n",
-               manager->gnss_assistance_file, size);
+    g_snprintf(value, BUFFER_SIZE, "\"RAM:%s\",%ld,%d",
+               manager->gnss_assistance_file, size, UPLOAD_TIMEOUT_S);
     g_message("Initiate GNSS assistance data upload: %s", value);
     at_append_command(manager, "QFUPL", NULL, value, NULL,
                       init_assistance_data_upload_ready);
@@ -295,39 +326,34 @@ static void init_assistance_data_upload(struct EG25Manager *manager)
 
 static void upload_assistance_data(struct EG25Manager *manager)
 {
-    char buffer[2*BUFFER_SIZE];
-    gint len;
-    gboolean success = TRUE;
+    gint error;
+    glong written_total = 0;
+    gint ret;
+    struct stat sb;
 
-    /* Copy downloaded XTRA assistance data to the modem over serial */
-    while((len = read(manager->gnss_assistance_fd, buffer, 2*BUFFER_SIZE)) > 0)
-    {
-        len = write(manager->at_fd, buffer, len);
-        if (len < 0) {
-            success = FALSE;
-            g_error("Writing GNSS assistance data failed: %d", len);
-            break;
-        }
-        usleep(UPLOAD_DELAY);
-        g_message("Uploaded %d bytes", len);
+    if (fstat(manager->gnss_assistance_fd, &sb) != 0) {
+        g_critical("gnss: unable to stat xtra data file: %s", g_strerror(errno));
+
+        /* Make sure the upload times out and the modem goes back to AT command mode */
+        sleep(UPLOAD_TIMEOUT_S + 1);
+        manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
+        return;
     }
 
-    /* Clear QFUPL AT command and process next */
-    at_next_command(manager);
+    do {
+        errno = 0;
+        /* Copy downloaded XTRA assistance data to the modem over serial */
+        ret = sendfile(manager->at_fd, manager->gnss_assistance_fd, &written_total, BUFFER_SIZE);
+        error = errno;
+        usleep(UPLOAD_DELAY_US);
+    } while ((!error && written_total < sb.st_size) || (ret == -1 && error == EAGAIN));
 
     /* Go to the next step if successful */
-    if (success) {
-        manager->gnss_assistance_step++;
-        gnss_step(manager);
-    }
-    /* Restart upload */
-    else {
-        g_message ("Rescheduling upload because of failure in %ds",
-                   RESCHEDULE_IN_SECS);
+    if (!error) {
+        g_message("Successfully uploaded %ld bytes to the modem", written_total);
+    } else {
+        g_critical("Unable to upload xtra data: %s", g_strerror(error));
         manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
-        g_timeout_add_seconds(RESCHEDULE_IN_SECS,
-                              G_SOURCE_FUNC(gnss_upload_assistance_data),
-                              manager);
     }
 }
 
@@ -351,13 +377,13 @@ static void finish_assistance_data_upload(struct EG25Manager *manager)
 
     /* Configure GNSS assistance clock to current system time (UTC) */
     datetime = g_date_time_new_now_utc();
-    timestring = g_date_time_format(datetime, "0,\"%Y/%m/%d,%H:%M:%S\"\r\n");
+    timestring = g_date_time_format(datetime, "0,\"%Y/%m/%d,%H:%M:%S\"");
     g_message("Setting GNSS assistance UTC clock to: %s", timestring);
     at_append_command(manager, "QGPSXTRATIME", NULL, timestring, NULL,
                          at_process_result);
 
     /* Configure GNSS engine to use uploaded GNSS assistance data */
-    g_snprintf(value, BUFFER_SIZE, "\"RAM:%s\"\r\n",
+    g_snprintf(value, BUFFER_SIZE, "\"RAM:%s\"",
                manager->gnss_assistance_file);
     g_message("Setting GNSS assistance file to: %s", value);
     at_append_command(manager, "QGPSXTRADATA", NULL, value, NULL,
@@ -370,9 +396,9 @@ static void finish_assistance_data_upload(struct EG25Manager *manager)
 #ifdef HAVE_MMGLIB
 static void enable_mm_gnss(struct EG25Manager *manager)
 {
-    MMModemLocationSource sources;
-    gboolean signal_location;
     g_autoptr (GError) error = NULL;
+    MMModemLocationSource sources = mm_modem_location_get_enabled(manager->mm_location);
+    gboolean signal_location = mm_modem_location_signals_location(manager->mm_location);
 
     if (manager->gnss_sources & EG25_GNSS_SOURCE_UNMANAGED)
         sources |= MM_MODEM_LOCATION_SOURCE_GPS_UNMANAGED;
@@ -381,8 +407,6 @@ static void enable_mm_gnss(struct EG25Manager *manager)
     if (manager->gnss_sources & EG25_GNSS_SOURCE_RAW)
         sources |= MM_MODEM_LOCATION_SOURCE_GPS_RAW;
 
-    sources = mm_modem_location_get_enabled(manager->mm_location);
-    signal_location = mm_modem_location_signals_location(manager->mm_location);
     mm_modem_location_setup_sync(manager->mm_location, sources,
                                  signal_location, NULL, &error);
     if (error != NULL)
@@ -419,12 +443,21 @@ void gnss_step(struct EG25Manager *manager)
         g_message("GNSS assistance upload started...");
         /* fall-through */
 
+    case EG25_GNSS_STEP_FETCH_ASSISTANCE_DATA:
+        g_message("GNSS assistance upload step (%d/%d): "
+                  "fetching assistance data",
+                  manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
+        fetch_assistance_data(manager);
+        break;
+
 #ifdef HAVE_MMGLIB
     case EG25_GNSS_STEP_MM_GNSS_DISABLE:
-        g_message("GNSS assistance upload step (%d/%d): "
-                  "disabling GNSS engine through ModemManager",
-                  manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
-        disable_mm_gnss(manager);
+        if (manager->modem_iface == MODEM_IFACE_MODEMMANAGER) {
+            g_message("GNSS assistance upload step (%d/%d): "
+                      "disabling GNSS engine through ModemManager",
+                      manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
+            disable_mm_gnss(manager);
+        }
         manager->gnss_assistance_step++;
         /* fall-through */
 #endif
@@ -436,13 +469,6 @@ void gnss_step(struct EG25Manager *manager)
         state_at_gnss(manager);
         break;
 
-    case EG25_GNSS_STEP_FETCH_ASSISTANCE_DATA:
-        g_message("GNSS assistance upload step (%d/%d): "
-                  "fetching assistance data",
-                  manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
-        fetch_assistance_data(manager);
-        break;
-
     case EG25_GNSS_STEP_INIT_UPLOAD:
         g_message("GNSS assistance upload step (%d/%d): initiating upload",
                   manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
@@ -464,10 +490,12 @@ void gnss_step(struct EG25Manager *manager)
 
 #ifdef HAVE_MMGLIB
     case EG25_GNSS_STEP_MM_GNSS_ENABLE:
-        g_message("GNSS assistance upload step (%d/%d): "
-                  "re-enabling GNSS through ModemManager",
-                  manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
-        enable_mm_gnss(manager);
+        if (manager->modem_iface == MODEM_IFACE_MODEMMANAGER) {
+            g_message("GNSS assistance upload step (%d/%d): "
+                      "re-enabling GNSS through ModemManager",
+                      manager->gnss_assistance_step, EG25_GNSS_STEP_LAST);
+            enable_mm_gnss(manager);
+        }
         manager->gnss_assistance_step++;
         /* fall-through */
 #endif
@@ -485,4 +513,3 @@ void gnss_step(struct EG25Manager *manager)
         break;
     }
 }
-
diff --git a/src/gnss.h b/src/gnss.h
index 1b49403..78bdc18 100644
--- a/src/gnss.h
+++ b/src/gnss.h
@@ -7,10 +7,11 @@
 #pragma once
 
 #include <time.h>
+#include <unistd.h>
 #include <curl/curl.h>
 
 #include "manager.h"
 
-void gnss_init(struct EG25Manager *manager, toml_table_t *config);
+void gnss_init(struct EG25Manager *manager, toml_table_t *config[]);
 void gnss_destroy(struct EG25Manager *manager);
 gboolean gnss_upload_assistance_data(struct EG25Manager *manager);
diff --git a/src/gpio.c b/src/gpio.c
index aae9b94..c7cfc53 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: GPL-3.0-or-later
  */
 
+#include "config.h"
 #include "gpio.h"
 
 #include <unistd.h>
@@ -13,8 +14,6 @@
 
 #define MAX_GPIOCHIP_LINES 352
 
-#define GPIO_IDX_INVAL 0xffff
-
 enum {
     GPIO_OUT_DTR = 0,
     GPIO_OUT_PWRKEY,
@@ -24,12 +23,23 @@ enum {
     GPIO_OUT_COUNT
 };
 
-
 enum {
     GPIO_IN_STATUS = 0,
     GPIO_IN_COUNT
 };
 
+static char *gpio_out_names[] = {
+    "dtr",
+    "pwrkey",
+    "reset",
+    "apready",
+    "disable",
+};
+
+static char *gpio_in_names[] = {
+    "status",
+};
+
 int gpio_sequence_poweron(struct EG25Manager *manager)
 {
     gpiod_line_set_value(manager->gpio_out[GPIO_OUT_PWRKEY], 1);
@@ -71,12 +81,14 @@ int gpio_sequence_resume(struct EG25Manager *manager)
 
 int gpio_sequence_wake(struct EG25Manager *manager)
 {
-    gpiod_line_set_value(manager->gpio_out[GPIO_OUT_DTR], 0);
+    if (gpiod_line_get_value(manager->gpio_out[GPIO_OUT_DTR])) {
+        gpiod_line_set_value(manager->gpio_out[GPIO_OUT_DTR], 0);
 
-    /* Give the modem 200ms to wake from soft sleep */
-    usleep(200000);
+        /* Give the modem 200ms to wake from soft sleep */
+        usleep(200000);
 
-    g_message("Executed soft wake sequence");
+        g_message("Executed soft wake sequence");
+    }
 
     return 0;
 }
@@ -89,24 +101,17 @@ int gpio_sequence_sleep(struct EG25Manager *manager)
     return 0;
 }
 
-static guint get_config_gpio(toml_table_t *config, const char *id)
-{
-    toml_datum_t value = toml_int_in(config, id);
-    guint gpio;
-
-    if (!value.ok)
-        return GPIO_IDX_INVAL;
-
-    gpio = (guint)value.u.i;
-
-    return gpio;
-}
-
-int gpio_init(struct EG25Manager *manager, toml_table_t *config)
+int gpio_init(struct EG25Manager *manager, toml_table_t *config[])
 {
     int i, ret;
-    guint gpio_out_idx[GPIO_OUT_COUNT];
-    guint gpio_in_idx[GPIO_IN_COUNT];
+    guint offset, chipidx, gpio_idx;
+    toml_table_t *gpio_config[EG25_CONFIG_COUNT];
+
+    for (i = 0; i < EG25_CONFIG_COUNT; i++)
+        gpio_config[i] = config[i] ? toml_table_in(config[i], "gpio") : NULL;
+
+    if (!gpio_config[EG25_CONFIG_SYS])
+        g_error("Default config file lacks the 'gpio' section!");
 
     manager->gpiochip[0] = gpiod_chip_open_by_label(GPIO_CHIP1_LABEL);
     if (!manager->gpiochip[0]) {
@@ -120,21 +125,15 @@ int gpio_init(struct EG25Manager *manager, toml_table_t *config)
         return 1;
     }
 
-    gpio_out_idx[GPIO_OUT_DTR] = get_config_gpio(config, "dtr");
-    gpio_out_idx[GPIO_OUT_PWRKEY] = get_config_gpio(config, "pwrkey");
-    gpio_out_idx[GPIO_OUT_RESET] = get_config_gpio(config, "reset");
-    gpio_out_idx[GPIO_OUT_APREADY] = get_config_gpio(config, "apready");
-    gpio_out_idx[GPIO_OUT_DISABLE] = get_config_gpio(config, "disable");
-    gpio_in_idx[GPIO_IN_STATUS] = get_config_gpio(config, "status");
-
     for (i = 0; i < GPIO_OUT_COUNT; i++) {
-        guint offset, chipidx;
+        if (!config_get_uint(gpio_config, gpio_out_names[i], &gpio_idx))
+            g_error("Unable to get config for output GPIO '%s'", gpio_out_names[i]);
 
-        if (gpio_out_idx[i] < MAX_GPIOCHIP_LINES) {
-            offset = gpio_out_idx[i];
+        if (gpio_idx < MAX_GPIOCHIP_LINES) {
+            offset = gpio_idx;
             chipidx = 0;
         } else {
-            offset = gpio_out_idx[i] - MAX_GPIOCHIP_LINES;
+            offset = gpio_idx - MAX_GPIOCHIP_LINES;
             chipidx = 1;
         }
 
@@ -152,16 +151,14 @@ int gpio_init(struct EG25Manager *manager, toml_table_t *config)
     }
 
     for (i = 0; i < GPIO_IN_COUNT; i++) {
-        guint offset, chipidx;
-
-        if (gpio_in_idx[i] == GPIO_IDX_INVAL)
+        if (!config_get_uint(gpio_config, gpio_in_names[i], &gpio_idx))
             continue;
 
-        if (gpio_in_idx[i] < MAX_GPIOCHIP_LINES) {
-            offset = gpio_in_idx[i];
+        if (gpio_idx < MAX_GPIOCHIP_LINES) {
+            offset = gpio_idx;
             chipidx = 0;
         } else {
-            offset = gpio_in_idx[i] - MAX_GPIOCHIP_LINES;
+            offset = gpio_idx - MAX_GPIOCHIP_LINES;
             chipidx = 1;
         }
 
diff --git a/src/gpio.h b/src/gpio.h
index a041bdc..50dcdba 100644
--- a/src/gpio.h
+++ b/src/gpio.h
@@ -8,7 +8,7 @@
 
 #include "manager.h"
 
-int gpio_init(struct EG25Manager *state, toml_table_t *config);
+int gpio_init(struct EG25Manager *state, toml_table_t *config[]);
 void gpio_destroy(struct EG25Manager *state);
 
 int gpio_sequence_poweron(struct EG25Manager *state);
diff --git a/src/manager.c b/src/manager.c
index d13a2a0..cba4e72 100644
--- a/src/manager.c
+++ b/src/manager.c
@@ -5,6 +5,7 @@
  */
 
 #include "at.h"
+#include "config.h"
 #include "gpio.h"
 #include "manager.h"
 
@@ -33,6 +34,12 @@
 #define EG25_DATADIR "/usr/share/eg25-manager"
 #endif
 
+#ifndef EG25_VERSION
+#define EG25_VERSION "0.0.0"
+#endif
+
+#define POWERON_DELAY_US 100000UL
+
 static gboolean quit_app(struct EG25Manager *manager)
 {
     int i;
@@ -138,20 +145,24 @@ static gboolean modem_reset_done(struct EG25Manager* manager)
     return FALSE;
 }
 
-void modem_reset(struct EG25Manager *manager)
+gboolean modem_reset(struct EG25Manager *manager)
 {
     int fd, ret, len;
 
-    if (manager->reset_timer)
-        return;
+    if (manager->reset_timer) {
+        g_message("modem_reset: timer already setup, skipping...");
+        return G_SOURCE_REMOVE;
+    }
 
     /*
      * If we are managing the modem through lets say ofono, we should not
      * reset the modem based on the availability of USB ID
      * TODO: Improve ofono plugin and add support for fetching USB ID
      */
-    if (manager->modem_iface != MODEM_IFACE_MODEMMANAGER)
-        return;        
+    if (manager->modem_iface != MODEM_IFACE_MODEMMANAGER) {
+        g_message("modem_reset: not using ModemManager, bail out!");
+        return G_SOURCE_REMOVE;
+    }
 
     if (manager->modem_recovery_timer) {
         g_source_remove(manager->modem_recovery_timer);
@@ -159,37 +170,49 @@ void modem_reset(struct EG25Manager *manager)
     }
 
     if (!manager->modem_usb_id) {
-        g_warning("Unknown modem USB ID");
+        g_warning("Empty modem USB ID");
         goto error;
     }
 
+    g_message("Trying to reset modem with USB ID '%s'", manager->modem_usb_id);
+
     len = strlen(manager->modem_usb_id);
 
     manager->modem_state = EG25_STATE_RESETTING;
 
     fd = open("/sys/bus/usb/drivers/usb/unbind", O_WRONLY);
-    if (fd < 0)
+    if (fd < 0) {
+        g_warning("Unable to open /sys/bus/usb/drivers/usb/unbind");
         goto error;
+    }
     ret = write(fd, manager->modem_usb_id, len);
-    if (ret < len)
+    if (ret < len) {
         g_warning("Couldn't unbind modem: wrote %d/%d bytes", ret, len);
+        goto error;
+    }
     close(fd);
 
     fd = open("/sys/bus/usb/drivers/usb/bind", O_WRONLY);
-    if (fd < 0)
+    if (fd < 0) {
+        g_warning("Unable to open /sys/bus/usb/drivers/usb/unbind");
         goto error;
+    }
     ret = write(fd, manager->modem_usb_id, len);
-    if (ret < len)
+    if (ret < len) {
         g_warning("Couldn't bind modem: wrote %d/%d bytes", ret, len);
+        goto error;
+    }
     close(fd);
 
+    g_message("Successfully reset modem's USB connection");
+
     /*
      * 3s is long enough to make sure the modem has been bound back and
      * short enough to ensure it hasn't been acquired by ModemManager
      */
     manager->reset_timer = g_timeout_add_seconds(3, G_SOURCE_FUNC(modem_reset_done), manager);
 
-    return;
+    return G_SOURCE_REMOVE;
 
 error:
     // Release blocking sleep inhibitor
@@ -199,8 +222,15 @@ error:
         g_source_remove(manager->modem_boot_timer);
         manager->modem_boot_timer = 0;
     }
+
     // Everything else failed, reboot the modem
+    g_message("USB reset failed, falling back to AT command");
     at_sequence_reset(manager);
+
+    // Setup timer for making sure we don't queue other reset commands
+    manager->reset_timer = g_timeout_add_seconds(30, G_SOURCE_FUNC(modem_reset_done), manager);
+
+    return G_SOURCE_REMOVE;
 }
 
 void modem_suspend_pre(struct EG25Manager *manager)
@@ -225,7 +255,7 @@ void modem_resume_post(struct EG25Manager *manager)
     at_sequence_resume(manager);
 }
 
-static toml_table_t *parse_config_file(char *config_file)
+static toml_table_t *parse_config_file(char *config_file, gboolean force_default)
 {
     toml_table_t *toml_config;
     gchar *compatible;
@@ -249,28 +279,26 @@ static toml_table_t *parse_config_file(char *config_file)
         } while (pos < len);
 
         for (pos = 0; pos < compat->len; pos++) {
-            g_autofree gchar *filename = g_strdup_printf(EG25_CONFDIR "/%s.toml", (gchar *)g_ptr_array_index(compat, pos));
+            g_autofree gchar *filename = NULL;
+            if (force_default)
+                filename = g_strdup_printf(EG25_DATADIR "/%s.toml", (gchar *)g_ptr_array_index(compat, pos));
+            else
+                filename = g_strdup_printf(EG25_CONFDIR "/%s.toml", (gchar *)g_ptr_array_index(compat, pos));
+
             if (access(filename, F_OK) == 0) {
                 g_message("Opening config file: %s", filename);
                 f = fopen(filename, "r");
                 break;
             }
         }
-
-        if (!f) {
-            for (pos = 0; pos < compat->len; pos++) {
-                g_autofree gchar *filename = g_strdup_printf(EG25_DATADIR "/%s.toml", (gchar *)g_ptr_array_index(compat, pos));
-                if (access(filename, F_OK) == 0) {
-                    g_message("Opening config file: %s", filename);
-                    f = fopen(filename, "r");
-                    break;
-                }
-            }
-        }
     }
 
-    if (!f)
-        g_error("unable to find a suitable config file!");
+    if (!f) {
+        if (force_default)
+            g_error("unable to find a suitable config file!");
+        else
+            return NULL;
+    }
 
     toml_config = toml_parse_file(f, error, sizeof(error));
     if (!toml_config)
@@ -285,16 +313,18 @@ int main(int argc, char *argv[])
     g_autoptr(GError) err = NULL;
     struct EG25Manager manager;
     gchar *config_file = NULL;
-    toml_table_t *toml_config;
-    toml_table_t *toml_manager;
-    toml_datum_t toml_value;
+    gboolean show_version = FALSE;
+    toml_table_t *toml_config[EG25_CONFIG_COUNT];
+    toml_table_t *manager_config[EG25_CONFIG_COUNT];
     const GOptionEntry options[] = {
         { "config", 'c', 0, G_OPTION_ARG_STRING, &config_file, "Config file to use.", NULL },
+        { "version", 'v', 0, G_OPTION_ARG_NONE, &show_version, "Display version information and exit.", NULL },
         { NULL, 0, 0, G_OPTION_ARG_NONE, NULL, NULL, NULL }
     };
 
     memset(&manager, 0, sizeof(manager));
     manager.at_fd = -1;
+    manager.poweron_delay = POWERON_DELAY_US;
     manager.suspend_delay_fd = -1;
     manager.suspend_block_fd = -1;
 
@@ -305,45 +335,53 @@ int main(int argc, char *argv[])
         return 1;
     }
 
-    manager.loop = g_main_loop_new(NULL, FALSE);
-
-    toml_config = parse_config_file(config_file);
-
-    toml_manager = toml_table_in(toml_config, "manager");
-    if (toml_manager) {
-        toml_value = toml_bool_in(toml_manager, "need_libusb");
-        if (toml_value.ok)
-            manager.use_libusb = toml_value.u.b;
-
-        toml_value = toml_int_in(toml_manager, "usb_vid");
-        if (toml_value.ok)
-            manager.usb_vid = toml_value.u.i;
-
-        toml_value = toml_int_in(toml_manager, "usb_pid");
-        if (toml_value.ok)
-            manager.usb_pid = toml_value.u.i;
-
-        toml_value = toml_int_in(toml_manager, "poweron_delay");
-        if (toml_value.ok) {
-            if (toml_value.u.i >= 0 && toml_value.u.i <= G_MAXULONG) {
-                // Safe to cast into gulong
-                manager.poweron_delay = (gulong) toml_value.u.i;
-            } else {
-                // Changed from initialized default value but not in range
-                g_message("Configured poweron_delay out of range, using default");
-            }
-        }
+    if (show_version) {
+        printf("eg25-manager version %s\n", EG25_VERSION);
+        return 0;
     }
 
-    at_init(&manager, toml_table_in(toml_config, "at"));
-    gpio_init(&manager, toml_table_in(toml_config, "gpio"));
+    manager.loop = g_main_loop_new(NULL, FALSE);
+
+    toml_config[EG25_CONFIG_SYS] = parse_config_file(NULL, TRUE);
+    toml_config[EG25_CONFIG_USER] = parse_config_file(config_file, FALSE);
+
+    /*
+     * We need at least one valid config file, and assuming it's
+     * EG25_CONFIG_SYS will make the rest easier to implement
+     */
+    if (!toml_config[EG25_CONFIG_SYS] && toml_config[EG25_CONFIG_USER]) {
+        toml_config[EG25_CONFIG_SYS] = toml_config[EG25_CONFIG_USER];
+        toml_config[EG25_CONFIG_USER] = NULL;
+    }
+
+    if (!toml_config[EG25_CONFIG_SYS])
+        g_error("Unable to parse config file!");
+
+    for (int i = 0; i < EG25_CONFIG_COUNT; i++)
+        manager_config[i] = toml_config[i] ? toml_table_in(toml_config[i], "manager") : NULL;
+
+    if (!manager_config[EG25_CONFIG_SYS])
+        g_error("Default config file lacks the 'manager' section!");
+
+    config_get_bool(manager_config, "need_libusb", &manager.use_libusb);
+    config_get_uint(manager_config, "usb_vid", &manager.usb_vid);
+    config_get_uint(manager_config, "usb_pid", &manager.usb_pid);
+    config_get_uint(manager_config, "poweron_delay", &manager.poweron_delay);
+
+    at_init(&manager, toml_config);
+    gpio_init(&manager, toml_config);
 #ifdef HAVE_MMGLIB
-    mm_iface_init(&manager, toml_table_in(toml_config, "mm-iface"));
+    mm_iface_init(&manager, toml_config);
 #endif
-    ofono_iface_init(&manager);
-    suspend_init(&manager, toml_table_in(toml_config, "suspend"));
-    udev_init(&manager, toml_table_in(toml_config, "udev"));
-    gnss_init(&manager, toml_table_in(toml_config, "gnss"));
+    ofono_iface_init(&manager, toml_config);
+    suspend_init(&manager, toml_config);
+    udev_init(&manager, toml_config);
+    gnss_init(&manager, toml_config);
+
+    for (int i = 0; i < EG25_CONFIG_COUNT; i++) {
+        if (toml_config[i])
+            toml_free(toml_config[i]);
+    }
 
     g_idle_add(G_SOURCE_FUNC(modem_start), &manager);
 
diff --git a/src/manager.h b/src/manager.h
index 3e097b3..2c36348 100644
--- a/src/manager.h
+++ b/src/manager.h
@@ -18,11 +18,11 @@
 
 typedef enum {
     EG25_GNSS_STEP_FIRST = 0,
+    EG25_GNSS_STEP_FETCH_ASSISTANCE_DATA,
 #ifdef HAVE_MMGLIB
     EG25_GNSS_STEP_MM_GNSS_DISABLE,
 #endif
     EG25_GNSS_STEP_AT_GNSS_DISABLE,
-    EG25_GNSS_STEP_FETCH_ASSISTANCE_DATA,
     EG25_GNSS_STEP_INIT_UPLOAD,
     EG25_GNSS_STEP_UPLOAD,
     EG25_GNSS_STEP_FINISH_UPLOAD,
@@ -48,10 +48,10 @@ enum EG25State {
     EG25_STATE_STARTED, // Modem has been started and declared itdata ready
     EG25_STATE_ACQUIRED, // Modem has been probed by ModemManager
     EG25_STATE_CONFIGURED, // Modem has been configured through AT commands
-    EG25_STATE_SUSPENDING, // System is going into suspend
-    EG25_STATE_RESUMING, // System is being resumed, waiting for modem to come back
     EG25_STATE_REGISTERED, // Modem is unlocked and registered to a network provider
     EG25_STATE_CONNECTED, // Modem is connected (data connection active)
+    EG25_STATE_SUSPENDING, // System is going into suspend
+    EG25_STATE_RESUMING, // System is being resumed, waiting for modem to come back
     EG25_STATE_RESETTING, // Something went wrong, we're restarting the modem
     EG25_STATE_FINISHING
 };
@@ -62,13 +62,19 @@ enum ModemIface {
     MODEM_IFACE_OFONO
 };
 
+enum EG25Config {
+    EG25_CONFIG_SYS = 0,
+    EG25_CONFIG_USER,
+    EG25_CONFIG_COUNT
+};
+
 struct EG25Manager {
     GMainLoop *loop;
     guint reset_timer;
     gboolean use_libusb;
     guint usb_vid;
     guint usb_pid;
-    gulong poweron_delay;
+    guint poweron_delay;
 
     int at_fd;
     guint at_source;
@@ -113,7 +119,7 @@ struct EG25Manager {
 };
 
 void modem_configure(struct EG25Manager *data);
-void modem_reset(struct EG25Manager *data);
+gboolean modem_reset(struct EG25Manager *data);
 void modem_suspend_pre(struct EG25Manager *data);
 void modem_suspend_post(struct EG25Manager *data);
 void modem_resume_pre(struct EG25Manager *data);
diff --git a/src/meson.build b/src/meson.build
index 69a7e7e..cc13d2f 100644
--- a/src/meson.build
+++ b/src/meson.build
@@ -9,6 +9,7 @@ subdir('libgdbofono')
 
 src = [
         'at.c', 'at.h',
+        'config.c', 'config.h',
         'gpio.c', 'gpio.h',
         'manager.c', 'manager.h',
         'ofono-iface.c', 'ofono-iface.h',
diff --git a/src/mm-iface.c b/src/mm-iface.c
index 577a718..a55e335 100644
--- a/src/mm-iface.c
+++ b/src/mm-iface.c
@@ -209,7 +209,7 @@ static void mm_vanished_cb(GDBusConnection    *connection,
     mm_iface_clean(manager);
 }
 
-void mm_iface_init(struct EG25Manager *manager, toml_table_t *config)
+void mm_iface_init(struct EG25Manager *manager, toml_table_t *config[])
 {
     manager->mm_watch = g_bus_watch_name(G_BUS_TYPE_SYSTEM, MM_DBUS_SERVICE,
                                          G_BUS_NAME_WATCHER_FLAGS_AUTO_START,
diff --git a/src/mm-iface.h b/src/mm-iface.h
index 1bc8164..d30dd12 100644
--- a/src/mm-iface.h
+++ b/src/mm-iface.h
@@ -8,5 +8,5 @@
 
 #include "manager.h"
 
-void mm_iface_init(struct EG25Manager *data, toml_table_t *config);
+void mm_iface_init(struct EG25Manager *data, toml_table_t *config[]);
 void mm_iface_destroy(struct EG25Manager *data);
diff --git a/src/ofono-iface.c b/src/ofono-iface.c
index bc37f38..6c67396 100644
--- a/src/ofono-iface.c
+++ b/src/ofono-iface.c
@@ -128,7 +128,7 @@ static void ofono_vanished_cb(GDBusConnection    *connection,
     }
 }
 
-void ofono_iface_init(struct EG25Manager *manager)
+void ofono_iface_init(struct EG25Manager *manager, toml_table_t *config[])
 {
     manager->ofono_watch = g_bus_watch_name(G_BUS_TYPE_SYSTEM, OFONO_SERVICE,
                                             G_BUS_NAME_WATCHER_FLAGS_AUTO_START,
diff --git a/src/ofono-iface.h b/src/ofono-iface.h
index fd3766e..a7258e7 100644
--- a/src/ofono-iface.h
+++ b/src/ofono-iface.h
@@ -8,5 +8,5 @@
 
 #include "manager.h"
 
-void ofono_iface_init(struct EG25Manager *data);
+void ofono_iface_init(struct EG25Manager *data, toml_table_t *config[]);
 void ofono_iface_destroy(struct EG25Manager *data);
diff --git a/src/suspend.c b/src/suspend.c
index f9bedf9..4bace47 100644
--- a/src/suspend.c
+++ b/src/suspend.c
@@ -9,6 +9,7 @@
  * SPDX-License-Identifier: GPL-3.0-or-later
  */
 
+#include "config.h"
 #include "manager.h"
 
 #include <gio/gunixfdlist.h>
@@ -172,7 +173,7 @@ static void signal_cb(GDBusProxy *proxy,
         modem_resume_pre(manager);
         if (
 #ifdef HAVE_MMGLIB
-            manager->mm_modem || 
+            manager->mm_modem ||
 #endif
             manager->modem_iface == MODEM_IFACE_OFONO) {
             /*
@@ -238,18 +239,26 @@ static void on_proxy_acquired(GObject *object,
     }
 }
 
-void suspend_init(struct EG25Manager *manager, toml_table_t *config)
+void suspend_init(struct EG25Manager *manager, toml_table_t *config[])
 {
-    toml_datum_t timeout_value;
+    toml_table_t *suspend_config[EG25_CONFIG_COUNT];
 
-    if (config) {
-        timeout_value = toml_int_in(config, "boot_timeout");
-        if (timeout_value.ok)
-            manager->modem_boot_timeout = (guint)timeout_value.u.i;
+    for (int i = 0; i < EG25_CONFIG_COUNT; i++)
+        suspend_config[i] = config[i] ? toml_table_in(config[i], "suspend") : NULL;
 
-        timeout_value = toml_int_in(config, "recovery_timeout");
-        if (timeout_value.ok)
-            manager->modem_recovery_timeout = (guint)timeout_value.u.i;
+    /* 
+     * The `suspend` section is optional in both the user and system config files,
+     * so let's make sure suspend_config[EG25_CONFIG_SYS] is valid if one of the
+     * files has it.
+     */
+    if (suspend_config[EG25_CONFIG_USER] && !suspend_config[EG25_CONFIG_SYS]) {
+        suspend_config[EG25_CONFIG_SYS] = suspend_config[EG25_CONFIG_USER];
+        suspend_config[EG25_CONFIG_USER] = NULL;
+    }
+
+    if (suspend_config[EG25_CONFIG_SYS]) {
+        config_get_uint(suspend_config, "boot_timeout", &manager->modem_boot_timeout);
+        config_get_uint(suspend_config, "recovery_timeout", &manager->modem_recovery_timeout);
     }
 
     if (manager->modem_boot_timeout == 0)
diff --git a/src/suspend.h b/src/suspend.h
index 6b92015..fc8648a 100644
--- a/src/suspend.h
+++ b/src/suspend.h
@@ -8,7 +8,7 @@
 
 #include "manager.h"
 
-void suspend_init (struct EG25Manager *data, toml_table_t *config);
+void suspend_init (struct EG25Manager *data, toml_table_t *config[]);
 void suspend_destroy (struct EG25Manager *data);
 
 void suspend_inhibit (struct EG25Manager *data, gboolean inhibit, gboolean block);
diff --git a/src/udev.c b/src/udev.c
index 10302cd..9790c15 100644
--- a/src/udev.c
+++ b/src/udev.c
@@ -16,16 +16,16 @@ static void udev_event_cb(GUdevClient *client, gchar *action, GUdevDevice *devic
         manager->modem_state == EG25_STATE_RESETTING ||
         !manager->modem_usb_id) {
         return;
-    }
+        }
 
-    if (strncmp(g_udev_device_get_name(device), manager->modem_usb_id, strlen(manager->modem_usb_id)) == 0 &&
+    if (strcmp(g_udev_device_get_name(device), manager->modem_usb_id) == 0 &&
         manager->reset_timer == 0) {
         g_message("Lost modem, resetting...");
-        modem_reset(manager);
+        g_timeout_add_seconds(2, G_SOURCE_FUNC(modem_reset), manager);
     }
 }
 
-void udev_init (struct EG25Manager *manager, toml_table_t *config)
+void udev_init (struct EG25Manager *manager, toml_table_t *config[])
 {
     const char * const subsystems[] = { "usb", NULL };
 
diff --git a/src/udev.h b/src/udev.h
index 6b17323..1d399d7 100644
--- a/src/udev.h
+++ b/src/udev.h
@@ -8,5 +8,5 @@
 
 #include "manager.h"
 
-void udev_init (struct EG25Manager *data, toml_table_t *config);
+void udev_init (struct EG25Manager *data, toml_table_t *config[]);
 void udev_destroy (struct EG25Manager *data);