eg25-manager/src/gnss.c

/*
 * Copyright (C) 2021 Dylan Van Assche <me@dylanvanassche.be>
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include "gnss.h"
#include "at.h"
#include "config.h"
#include "manager.h"

#include <errno.h>
#include <sys/sendfile.h>
#include <sys/stat.h>

#define BUFFER_SIZE        256
#define UPLOAD_DELAY_US    25000
#define UPLOAD_TIMEOUT_S   10
#define RESCHEDULE_IN_SECS 30

static void gnss_step(struct EG25Manager *manager);

gboolean gnss_upload_assistance_data(struct EG25Manager *manager)
{
    if (!manager->gnss_assistance_enabled) {
        g_message("GNSS assistance is disabled!");
        return FALSE;
    }

    if (manager->gnss_assistance_step < EG25_GNSS_STEP_LAST) {
        g_warning("GNSS assistance data upload already in process (%d/%d)",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        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->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[])
{
    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.
     */
    config_get_bool(gnss_config, "enabled", &manager->gnss_assistance_enabled);

    if (!manager->gnss_assistance_enabled) {
        g_message("GNSS assistance is disabled!");
        return;
    }

    if (!config_get_string(gnss_config, "url", &manager->gnss_assistance_url))
        g_error("GNSS assistance server URL is missing from config file");

    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 */
    manager->gnss_assistance_fd = g_file_open_tmp(NULL, NULL, &error);
    if (error != NULL)
        g_error("Unable to create temporary file: %s", error->message);

    /* Initialize state and schedule upload */
    manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
    g_timeout_add_seconds(RESCHEDULE_IN_SECS, G_SOURCE_FUNC(gnss_upload_assistance_data), manager);
}

void gnss_destroy(struct EG25Manager *manager)
{
    g_free(manager->gnss_assistance_url);
    g_free(manager->gnss_assistance_file);
    close(manager->gnss_assistance_fd);
}

/******************************************************************************/

#ifdef HAVE_MMGLIB
static void disable_mm_gnss(struct EG25Manager *manager)
{
    MMModemLocationSource sources;
    gboolean              signals_location;
    g_autoptr(GError) error = NULL;

    sources = mm_modem_location_get_enabled(manager->mm_location);
    signals_location = mm_modem_location_signals_location(manager->mm_location);
    manager->gnss_sources = EG25_GNSS_SOURCE_NONE;

    /* Save GNSS engine state */
    if (sources & MM_MODEM_LOCATION_SOURCE_GPS_NMEA)
        manager->gnss_sources |= EG25_GNSS_SOURCE_NMEA;
    else
        manager->gnss_sources &= ~EG25_GNSS_SOURCE_NMEA;

    if (sources & MM_MODEM_LOCATION_SOURCE_GPS_RAW)
        manager->gnss_sources |= EG25_GNSS_SOURCE_RAW;
    else
        manager->gnss_sources &= ~EG25_GNSS_SOURCE_RAW;

    if (sources & MM_MODEM_LOCATION_SOURCE_GPS_UNMANAGED)
        manager->gnss_sources |= EG25_GNSS_SOURCE_UNMANAGED;
    else
        manager->gnss_sources &= ~EG25_GNSS_SOURCE_UNMANAGED;

    /* Disable GNSS engine */
    sources &= ~MM_MODEM_LOCATION_SOURCE_GPS_RAW;
    sources &= ~MM_MODEM_LOCATION_SOURCE_GPS_NMEA;
    sources &= ~MM_MODEM_LOCATION_SOURCE_GPS_UNMANAGED;
    mm_modem_location_setup_sync(manager->mm_location, sources, signals_location, NULL, &error);
    if (error != NULL) {
        g_warning("Unable to disable GNSS engine through ModemManager: %s", error->message);
    }
}
#endif

static void disable_at_gnss_cb(struct EG25Manager *manager, const char *response)
{
    /* Clear QGPSEND AT command and process next */
    at_next_command(manager);

    /* Go to the next step */
    manager->gnss_assistance_step++;
    gnss_step(manager);
}

static void state_at_gnss_cb(struct EG25Manager *manager, const char *response)
{
    struct AtCommand *at_cmd = manager->at_cmds ? g_list_nth_data(manager->at_cmds, 0) : NULL;

    if (!at_cmd)
        return;

    /* Parse GNSS engine status and disable it if needed */
    if (strstr(response, "QGPS: 1")) {
        manager->gnss_sources |= EG25_GNSS_SOURCE_QGPS;
        g_free(at_cmd->value);
        g_free(at_cmd->cmd);
        at_cmd->expected = NULL;
        at_cmd->subcmd = NULL;
        at_cmd->value = NULL;
        at_cmd->cmd = g_strdup("QGPSEND");
        at_cmd->callback = disable_at_gnss_cb;
        at_send_command(manager);
    }
    /* QGPS is already disabled, move on to next step */
    else {
        at_next_command(manager);
        manager->gnss_sources &= ~EG25_GNSS_SOURCE_QGPS;
        manager->gnss_assistance_step++;
        gnss_step(manager);
    }
}

static void state_at_gnss(struct EG25Manager *manager)
{
    /* Asynchronously send AT command to query status of GNSS engine */
    at_append_command(manager, "QGPS?", NULL, NULL, NULL, state_at_gnss_cb);
    at_send_command(manager);
}

/******************************************************************************/

static void fetch_assistance_data(struct EG25Manager *manager)
{
    CURLcode          response;
    curl_off_t        downloaded;
    CURL             *curl = NULL;
    g_autofree gchar *url = NULL;
    FILE             *tmp_file = NULL;
    gchar             errbuf[CURL_ERROR_SIZE];
    errbuf[0] = 0;

    /* Fetch assistance data with curl */
    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_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_OK) {
        g_warning("Unable to fetch GNSS assistance data from %s: %s",
                  url,
                  strlen(errbuf) ? errbuf : curl_easy_strerror(response));
        goto bail;
    }

    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 successful", url);

    /* Go to the next step */
    manager->gnss_assistance_step++;
    gnss_step(manager);

    goto cleanup;

bail:
    manager->gnss_assistance_step = EG25_GNSS_STEP_LAST;
cleanup:
    fflush(tmp_file);
    fclose(tmp_file);
    if (curl != NULL)
        curl_easy_cleanup(curl);
}

/******************************************************************************/

static void init_assistance_data_upload_ready(struct EG25Manager *manager, const char *response)
{
    /* 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);
    }
}

static void init_assistance_data_upload_start(struct EG25Manager *manager, const char *response)
{
    gchar value[BUFFER_SIZE];
    off_t size;

    /* Process AT response */
    at_process_result(manager, response);

    /* Get file size in bytes */
    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,%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);
    at_send_command(manager);
}

static void init_assistance_data_upload(struct EG25Manager *manager)
{
    /*
     * Delete all previous GNSS assistance data files in RAM
     * and start uploading the latest one to RAM.
     */
    at_append_command(manager, "QFDEL", NULL, "\"RAM:*\"\r\n", NULL, init_assistance_data_upload_start);
    at_send_command(manager);
}

static void upload_assistance_data(struct EG25Manager *manager)
{
    gint        error;
    off_t       written_total = 0;
    gint        ret;
    struct stat sb;

    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;
    }

    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 (!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;
    }
}

static void finish_assistance_data_upload_cb(struct EG25Manager *manager, const char *response)
{
    /* Process response */
    at_process_result(manager, response);
    g_message("GNSS assistance data upload finished");

    /* Go to the next step */
    manager->gnss_assistance_step++;
    gnss_step(manager);
}

static void finish_assistance_data_upload(struct EG25Manager *manager)
{
    gchar      value[BUFFER_SIZE];
    GDateTime *datetime;
    gchar     *timestring;

    /* 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\"");
    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\"", manager->gnss_assistance_file);
    g_message("Setting GNSS assistance file to: %s", value);
    at_append_command(manager, "QGPSXTRADATA", NULL, value, NULL, finish_assistance_data_upload_cb);
    at_send_command(manager);
}

/******************************************************************************/

#ifdef HAVE_MMGLIB
static void enable_mm_gnss(struct EG25Manager *manager)
{
    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;
    if (manager->gnss_sources & EG25_GNSS_SOURCE_NMEA)
        sources |= MM_MODEM_LOCATION_SOURCE_GPS_NMEA;
    if (manager->gnss_sources & EG25_GNSS_SOURCE_RAW)
        sources |= MM_MODEM_LOCATION_SOURCE_GPS_RAW;

    mm_modem_location_setup_sync(manager->mm_location, sources, signal_location, NULL, &error);
    if (error != NULL)
        g_warning("Unable to enable GNSS engine through ModemManager: %s", error->message);
}
#endif

static void enable_at_gnss_cb(struct EG25Manager *manager, const char *response)
{
    manager->gnss_assistance_step++;
    gnss_step(manager);
}

static void enable_at_gnss(struct EG25Manager *manager)
{
    if (manager->gnss_sources & EG25_GNSS_SOURCE_QGPS) {
        at_append_command(manager, "QGPS", NULL, "1", NULL, enable_at_gnss_cb);
        at_send_command(manager);
        return;
    }
    manager->gnss_assistance_step++;
    gnss_step(manager);
}

/******************************************************************************/

void gnss_step(struct EG25Manager *manager)
{
    switch (manager->gnss_assistance_step) {
    case EG25_GNSS_STEP_FIRST:
        manager->gnss_assistance_step++;
        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:
        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

    case EG25_GNSS_STEP_AT_GNSS_DISABLE:
        g_message("GNSS assistance upload step (%d/%d): "
                  "disabling GNSS engine through AT+QGPS",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        state_at_gnss(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);
        init_assistance_data_upload(manager);
        break;

    case EG25_GNSS_STEP_UPLOAD:
        g_message("GNSS assistance upload step (%d/%d): "
                  "uploading assistance data",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        upload_assistance_data(manager);
        break;

    case EG25_GNSS_STEP_FINISH_UPLOAD:
        g_message("GNSS assistance upload step (%d/%d): finishing upload",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        finish_assistance_data_upload(manager);
        break;

#ifdef HAVE_MMGLIB
    case EG25_GNSS_STEP_MM_GNSS_ENABLE:
        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

    case EG25_GNSS_STEP_AT_QGPS_ENABLE:
        g_message("GNSS assistance upload step (%d/%d): "
                  "re-enabling GNSS through AT+QGPS",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        enable_at_gnss(manager);
        break;

    case EG25_GNSS_STEP_LAST:
        g_message("GNSS assistance upload step (%d/%d): finished",
                  manager->gnss_assistance_step,
                  EG25_GNSS_STEP_LAST);
        break;
    }
}