eg25-manager/src/at.c

/*
 * Copyright (C) 2020 Arnaud Ferraris <arnaud.ferraris@gmail.com>
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#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>
#include <string.h>
#include <termios.h>
#include <unistd.h>

#include <glib-unix.h>

static GArray *configure_commands = NULL;
static GArray *suspend_commands = NULL;
static GArray *resume_commands = NULL;
static GArray *reset_commands = NULL;

static int configure_serial(const char *tty)
{
    struct termios ttycfg;
    int fd;

    fd = open(tty, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (fd > 0) {
        tcgetattr(fd, &ttycfg);
        ttycfg.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
        ttycfg.c_oflag = 0;
        ttycfg.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
        ttycfg.c_cflag &= ~(CSIZE | PARENB);
        ttycfg.c_cflag |= CS8;
        ttycfg.c_cc[VMIN] = 1;
        ttycfg.c_cc[VTIME] = 0;

        cfsetspeed(&ttycfg, B115200);
        tcsetattr(fd, TCSANOW, &ttycfg);
    }

    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, pos = 0;

    if (at_cmd) {
    	/* Wake up the modem from soft sleep before sending an AT command */
    	gpio_sequence_wake(manager);

	/* Send AT command */
        if (at_cmd->subcmd == NULL && at_cmd->value == NULL && at_cmd->expected == NULL)
            len = snprintf(command, sizeof(command), "AT+%s\r\n", at_cmd->cmd);
        else if (at_cmd->subcmd == NULL && at_cmd->value == NULL)
            len = snprintf(command, sizeof(command), "AT+%s?\r\n", at_cmd->cmd);
        else if (at_cmd->subcmd == NULL && 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 = snprintf(command, sizeof(command), "AT+%s=\"%s\"\r\n", at_cmd->cmd, at_cmd->subcmd);
        else if (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;

        do {
            ret = write(manager->at_fd, &command[pos], len);

            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
                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) {
            manager->modem_state = EG25_STATE_POWERED;
        }
    }

    return FALSE;
}

void at_next_command(struct EG25Manager *manager)
{
    struct AtCommand *at_cmd = manager->at_cmds ? g_list_nth_data(manager->at_cmds, 0) : NULL;

    at_free_command(at_cmd, manager);
    at_send_command(manager);
}

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

    if (!at_cmd)
        return;

    at_cmd->retries++;
    if (at_cmd->retries > 3) {
        g_critical("Command %s retried %d times, aborting...", at_cmd->cmd, at_cmd->retries);
        at_next_command(manager);
    } else {
        g_timeout_add(500, G_SOURCE_FUNC(at_send_command), manager);
    }
}

void at_process_result(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;

    if (at_cmd->expected && !strstr(response, at_cmd->expected)) {
        g_free(at_cmd->value);
        at_cmd->value = at_cmd->expected;
        g_message("Got a different result than expected, changing value...");
        g_message("Expected: [%s]\nResponse: [%s]", at_cmd->expected, response);
        at_cmd->expected = NULL;
        at_send_command(manager);
    } else {
        at_next_command(manager);
    }
}

int at_append_command(struct EG25Manager *manager,
                      const char         *cmd,
                      const char         *subcmd,
                      const char         *value,
                      const char         *expected,
                      void              (*callback)
                                        (struct EG25Manager *manager,
                                         const char *response))
{
    struct AtCommand *at_cmd = calloc(1, sizeof(struct AtCommand));

    if (!at_cmd)
        return -1;

    at_cmd->cmd = g_strdup(cmd);
    if (subcmd)
        at_cmd->subcmd = g_strdup(subcmd);
    if (value)
        at_cmd->value = g_strdup(value);
    if (expected)
        at_cmd->expected = g_strdup(expected);
    if (callback)
        at_cmd->callback = callback;

    manager->at_cmds = g_list_append(manager->at_cmds, at_cmd);

    return 0;
}

#define READ_BUFFER_SIZE 256

static gboolean modem_response(gint fd,
                               GIOCondition event,
                               gpointer data)
{
    struct EG25Manager *manager = data;
    char response[READ_BUFFER_SIZE*4+1];
    char tmp[READ_BUFFER_SIZE];
    ssize_t ret, pos = 0;

    /*
     * Several reads can be necessary to get the full response, so we loop
     * until we read 0 chars with a reasonable delay between attempts
     * (remember the transfer rate is 115200 here)
     */
    do {
        ret = read(fd, tmp, sizeof(tmp));

        if (ret > 0) {
            /* If we're going to overflow truncate the data we read to fit */
            if (pos + ret >= sizeof(response)) {
                g_critical("AT response buffer full, truncating");
                ret = sizeof(response) - (pos + 1);
            }

            memcpy(&response[pos], tmp, ret);
            pos += ret;
            usleep(10000);
        }
    } 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;

        escaped = g_strescape(response, "\"");
        g_message("Response: [%s]", escaped);

        /*
         * When the modem is started, it outputs 'RDY' to indicate that
         * it is ready to receive AT commands.
         */
        if (strcmp(response, "RDY") == 0) {
            suspend_inhibit(manager, TRUE, TRUE);
            manager->modem_state = EG25_STATE_STARTED;
        }
        /*
         * Search for 'QGPSURC: "xtradataexpire"' in response to detect
         * if GNSS assistance data must be re-uploaded.
         * If that's the case, check if no AT commands are being processed
         * to avoid deadlocks and start upload.
         */
        else if (strstr(response, "QGPSURC: \"xtradataexpire\"") && manager->at_cmds == NULL)
            gnss_upload_assistance_data(manager);
        /*
         * AT command failed, retry.
         * QCFG="fast/poweroff" configuration is only available in
         * newer firmware versions, skip retrying that specific AT command.
         */
        else if (strstr(response, "ERROR") && !strstr(response, "fast/poweroff"))
            retry_at_command(manager);
        /*
         * 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") || strstr(response, "QFUPL")) {
            if (manager->at_callback != NULL)
                manager->at_callback(manager, response);
            else
                g_warning("AT command successful but no callback registered");
        }
        /* Not a recognized response, try running next command, just in case */
        else
            at_next_command(manager);
    }

    return TRUE;
}

static void parse_commands_list(toml_array_t *array, GArray **cmds)
{
    int len;

    len = toml_array_nelem(array);
    *cmds = g_array_new(FALSE, TRUE, sizeof(struct AtCommand));
    g_array_set_size(*cmds, (guint)len);
    for (int i = 0; i < len; i++) {
        struct AtCommand *cmd = &g_array_index(*cmds, struct AtCommand, i);
        toml_table_t *table = toml_table_at(array, i);
        toml_datum_t value;

        if (!table)
            continue;

        value = toml_string_in(table, "cmd");
        if (value.ok)
            cmd->cmd = value.u.s;

        value = toml_string_in(table, "subcmd");
        if (value.ok)
            cmd->subcmd = value.u.s;

        value = toml_string_in(table, "value");
        if (value.ok)
            cmd->value = value.u.s;

        value = toml_string_in(table, "expect");
        if (value.ok)
            cmd->expected = value.u.s;
    }
}

int at_init(struct EG25Manager *manager, toml_table_t *config[])
{
    toml_array_t *commands = NULL;
    gchar *uart_port = NULL;
    toml_table_t *at_config[EG25_CONFIG_COUNT];

    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);
    if (manager->at_fd < 0) {
        g_critical("Unable to configure %s", uart_port);
        g_free(uart_port);
        return 1;
    }
    g_free(uart_port);

    manager->at_source = g_unix_fd_add(manager->at_fd, G_IO_IN, modem_response, manager);

    if (!config_get_array(at_config, "configure", &commands))
        g_error("Configuration file lacks initial AT commands list");
    parse_commands_list(commands, &configure_commands);

    if (!config_get_array(at_config, "suspend", &commands))
        g_error("Configuration file lacks suspend AT commands list");
    parse_commands_list(commands, &suspend_commands);

    if (!config_get_array(at_config, "resume", &commands))
        g_error("Configuration file lacks resume AT commands list");
    parse_commands_list(commands, &resume_commands);

    if (!config_get_array(at_config, "reset", &commands))
        g_error("Configuration file lacks reset AT commands list");
    parse_commands_list(commands, &reset_commands);

    return 0;
}

void at_destroy(struct EG25Manager *manager)
{
    g_source_remove(manager->at_source);
    if (manager->at_fd > 0)
        close(manager->at_fd);

    g_array_free(configure_commands, TRUE);
    g_array_free(suspend_commands, TRUE);
    g_array_free(resume_commands, TRUE);
    g_array_free(reset_commands, TRUE);
}

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);
    }
    at_send_command(manager);
}

void at_sequence_suspend(struct EG25Manager *manager)
{
    for (guint i = 0; i < suspend_commands->len; i++) {
        struct AtCommand *cmd = &g_array_index(suspend_commands, struct AtCommand, i);
        at_append_command(manager, cmd->cmd, cmd->subcmd, cmd->value, cmd->expected, at_process_result);
    }
    at_send_command(manager);
}

void at_sequence_resume(struct EG25Manager *manager)
{
    for (guint i = 0; i < resume_commands->len; i++) {
        struct AtCommand *cmd = &g_array_index(resume_commands, struct AtCommand, i);
        at_append_command(manager, cmd->cmd, cmd->subcmd, cmd->value, cmd->expected, at_process_result);
    }
    at_send_command(manager);
}

void at_sequence_reset(struct EG25Manager *manager)
{
    for (guint i = 0; i < reset_commands->len; i++) {
        struct AtCommand *cmd = &g_array_index(reset_commands, struct AtCommand, i);
        at_append_command(manager, cmd->cmd, cmd->subcmd, cmd->value, cmd->expected, at_process_result);
    }
    at_send_command(manager);
}