/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details:
 *
 * Copyright (C) 2014 Aleksander Morgado <aleksander@aleksander.es>
 *
 * Higly based on the test-port-context setup in ModemManager.
 */

#include <config.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>

#include <gio/gio.h>
#include <gio/gunixsocketaddress.h>
#include <libqmi-glib.h>

#include "test-port-context.h"

#define BUFFER_SIZE 1024

struct _TestPortContext {
    gchar *name;
    GThread *thread;
    gboolean ready;
    GCond ready_cond;
    GMutex ready_mutex;
    GMainLoop *loop;
    GSocketService *socket_service;
    GList *clients;
    GMutex command_mutex;
    GByteArray *command;
    GByteArray *response;
};

/*****************************************************************************/
/* Helpers */

static gchar *
str_hex (gconstpointer mem,
         gsize size,
         gchar delimiter)
{
    const guint8 *data = mem;
	gsize i;
	gsize j;
	gsize new_str_length;
	gchar *new_str;

	/* Get new string length. If input string has N bytes, we need:
	 * - 1 byte for last NUL char
	 * - 2N bytes for hexadecimal char representation of each byte...
	 * - N-1 bytes for the separator ':'
	 * So... a total of (1+2N+N-1) = 3N bytes are needed... */
	new_str_length =  3 * size;

	/* Allocate memory for new array and initialize contents to NUL */
	new_str = g_malloc0 (new_str_length);

	/* Print hexadecimal representation of each byte... */
	for (i = 0, j = 0; i < size; i++, j += 3) {
		/* Print character in output string... */
		snprintf (&new_str[j], 3, "%02X", data[i]);
		/* And if needed, add separator */
		if (i != (size - 1) )
			new_str[j + 2] = delimiter;
	}

	/* Set output string */
	return new_str;
}

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

void
test_port_context_set_command (TestPortContext *ctx,
                               const guint8    *command,
                               gsize            command_size,
                               const guint8    *response,
                               gsize            response_size,
                               guint16          transaction_id)
{
    g_mutex_lock (&ctx->command_mutex);
    {
        g_assert (!ctx->command);
        ctx->command = g_byte_array_append (g_byte_array_sized_new (command_size), command, command_size);
        qmi_message_set_transaction_id ((QmiMessage *)ctx->command, transaction_id);

        g_assert (!ctx->response);
        ctx->response = g_byte_array_append (g_byte_array_sized_new (response_size), response, response_size);
        qmi_message_set_transaction_id ((QmiMessage *)ctx->response, transaction_id);
    }
    g_mutex_unlock (&ctx->command_mutex);
}

static GByteArray *
process_next_command (TestPortContext *ctx,
                      GByteArray      *buffer)
{
    QmiMessage   *message;
    GError       *error = NULL;
    const guint8 *message_raw;
    gsize         message_raw_length;
    gchar        *expected;
    gchar        *received;
    GByteArray   *response;

    /* Every message received must start with the QMUX marker.
     * If it doesn't, we broke framing :-/
     * If we broke framing, an error should be reported and the device
     * should get closed */
    if (buffer->len > 0 && buffer->data[0] != QMI_MESSAGE_QMUX_MARKER)
        g_assert_not_reached ();

    message = qmi_message_new_from_raw (buffer, &error);
    if (!message) {
        if (!error)
            /* More data we need */
            return NULL;
        /* Fail */
        g_assert_no_error (error);
    }

    /* Process received message */
    message_raw = qmi_message_get_raw (message, &message_raw_length, &error);
    g_assert_no_error (error);
    g_assert (message_raw);

    /* Get printables to compare (we'll just get a nicer error if they are
     * different), compared to a simple memcmp(). */
    g_mutex_lock (&ctx->command_mutex);
    {
        g_assert (ctx->command);
        expected = str_hex (ctx->command->data, ctx->command->len, ':');
    }
    g_mutex_unlock (&ctx->command_mutex);

    received = str_hex (message_raw, message_raw_length, ':');
    g_assert_cmpstr (expected, ==, received);
    g_free (expected);
    g_free (received);
    qmi_message_unref (message);
    g_byte_array_unref (ctx->command);
    ctx->command = NULL;

    /* Command Expected == Received, so now return the Response */
    g_mutex_lock (&ctx->command_mutex);
    {
        response = ctx->response;
        ctx->response = NULL;
    }
    g_mutex_unlock (&ctx->command_mutex);

    return response;
}

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

typedef struct {
    TestPortContext *ctx;
    GSocketConnection *connection;
    GSource *connection_readable_source;
    GByteArray *buffer;
} Client;

static void
client_free (Client *client)
{
    g_source_destroy (client->connection_readable_source);
    g_source_unref (client->connection_readable_source);
    g_output_stream_close (g_io_stream_get_output_stream (G_IO_STREAM (client->connection)), NULL, NULL);
    if (client->buffer)
        g_byte_array_unref (client->buffer);
    g_object_unref (client->connection);
    g_slice_free (Client, client);
}

static void
connection_close (Client *client)
{
    client->ctx->clients = g_list_remove (client->ctx->clients, client);
    client_free (client);
}

static void
client_parse_request (Client *client)
{
    GByteArray *response;

    do {
        response = process_next_command (client->ctx, client->buffer);
        if (response) {
            GError *error = NULL;

            if (!g_output_stream_write_all (g_io_stream_get_output_stream (G_IO_STREAM (client->connection)),
                                            response->data,
                                            response->len,
                                            NULL, /* bytes_written */
                                            NULL, /* cancellable */
                                            &error)) {
                g_warning ("Cannot send response to client: %s", error->message);
                g_error_free (error);
            }
            g_byte_array_unref (response);
        }
    } while (response);
}

static gboolean
connection_readable_cb (GSocket *socket,
                        GIOCondition condition,
                        Client *client)
{
    guint8 buffer[BUFFER_SIZE];
    GError *error = NULL;
    gssize r;

    if (condition & G_IO_HUP || condition & G_IO_ERR) {
        g_debug ("client connection closed");
        connection_close (client);
        return FALSE;
    }

    if (!(condition & G_IO_IN || condition & G_IO_PRI))
        return TRUE;

    r = g_input_stream_read (g_io_stream_get_input_stream (G_IO_STREAM (client->connection)),
                             buffer,
                             BUFFER_SIZE,
                             NULL,
                             &error);

    if (r < 0) {
        g_warning ("Error reading from istream: %s", error ? error->message : "unknown");
        if (error)
            g_error_free (error);
        /* Close the device */
        connection_close (client);
        return FALSE;
    }

    if (r == 0)
        return TRUE;

    /* else, r > 0 */
    if (!G_UNLIKELY (client->buffer))
        client->buffer = g_byte_array_sized_new (r);
    g_byte_array_append (client->buffer, buffer, r);

    /* Try to parse input messages */
    client_parse_request (client);

    return TRUE;
}

static Client *
client_new (TestPortContext *ctx,
            GSocketConnection *connection)
{
    Client *client;

    client = g_slice_new0 (Client);
    client->ctx = ctx;
    client->connection = g_object_ref (connection);
    client->connection_readable_source = g_socket_create_source (g_socket_connection_get_socket (client->connection),
                                                                 G_IO_IN | G_IO_PRI | G_IO_ERR | G_IO_HUP,
                                                                 NULL);
    g_source_set_callback (client->connection_readable_source,
                           (GSourceFunc)connection_readable_cb,
                           client,
                           NULL);
    g_source_attach (client->connection_readable_source, g_main_context_get_thread_default ());

    return client;
}

/* /\*****************************************************************************\/ */

static void
incoming_cb (GSocketService *service,
             GSocketConnection *connection,
             GObject *unused,
             TestPortContext *ctx)
{
    Client *client;

    client = client_new (ctx, connection);
    ctx->clients = g_list_append (ctx->clients, client);
}

static void
create_socket_service (TestPortContext *ctx)
{
    GError *error = NULL;
    GSocketService *service;
    GSocketAddress *address;
    GSocket *socket;

    g_assert (ctx->socket_service == NULL);

    /* Create socket */
    socket = g_socket_new (G_SOCKET_FAMILY_UNIX,
                           G_SOCKET_TYPE_STREAM,
                           G_SOCKET_PROTOCOL_DEFAULT,
                           &error);
    if (!socket)
        g_error ("Cannot create socket: %s", error->message);

    /* Bind to address */
    address = (g_unix_socket_address_new_with_type (
                   ctx->name,
                   -1,
                   G_UNIX_SOCKET_ADDRESS_ABSTRACT));
    if (!g_socket_bind (socket, address, TRUE, &error))
        g_error ("Cannot bind socket '%s': %s", ctx->name, error->message);

    /* Listen */
    if (!g_socket_listen (socket, &error))
        g_error ("Cannot listen in socket: %s", error->message);

    /* Create socket service */
    service = g_socket_service_new ();
    g_signal_connect (service, "incoming", G_CALLBACK (incoming_cb), ctx);
    if (!g_socket_listener_add_socket (G_SOCKET_LISTENER (service),
                                       socket,
                                       NULL, /* don't pass an object, will take a reference */
                                       &error))
        g_error ("Cannot add listener to socket: %s", error->message);

    /* Start it */
    g_socket_service_start (service);

    /* And store it */
    ctx->socket_service = service;

    /* Signal that the thread is ready */
    g_mutex_lock (&ctx->ready_mutex);
    ctx->ready = TRUE;
    g_cond_signal (&ctx->ready_cond);
    g_mutex_unlock (&ctx->ready_mutex);

    if (socket)
        g_object_unref (socket);
    if (address)
        g_object_unref (address);
}

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

void
test_port_context_stop (TestPortContext *ctx)
{
    g_assert (ctx->thread != NULL);
    g_assert (ctx->loop != NULL);

    g_main_loop_quit (ctx->loop);

    g_thread_join (ctx->thread);
    ctx->thread = NULL;
}

static gpointer
port_context_thread_func (TestPortContext *ctx)
{
    GMainContext *thread_context;

    thread_context = g_main_context_new ();
    g_main_context_push_thread_default (thread_context);

    create_socket_service (ctx);

    g_assert (ctx->loop == NULL);
    ctx->loop = g_main_loop_new (g_main_context_get_thread_default (), FALSE);
    g_main_loop_run (ctx->loop);
    g_main_loop_unref (ctx->loop);
    ctx->loop = NULL;
    return NULL;
}

void
test_port_context_start (TestPortContext *ctx)
{
    g_assert (ctx->thread == NULL);
    ctx->thread = g_thread_new (ctx->name,
                                (GThreadFunc)port_context_thread_func,
                                ctx);

    /* Now wait until the thread has finished its initialization and is
     * ready to serve connections */
    g_mutex_lock (&ctx->ready_mutex);
    while (!ctx->ready)
        g_cond_wait (&ctx->ready_cond, &ctx->ready_mutex);
    g_mutex_unlock (&ctx->ready_mutex);
}

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

void
test_port_context_free (TestPortContext *ctx)
{
    g_assert (ctx->thread == NULL);
    g_assert (ctx->loop == NULL);

    g_cond_clear (&ctx->ready_cond);
    g_mutex_clear (&ctx->ready_mutex);
    g_mutex_clear (&ctx->command_mutex);

    g_list_free_full (ctx->clients, (GDestroyNotify)client_free);
    if (ctx->socket_service) {
        if (g_socket_service_is_active (ctx->socket_service))
            g_socket_service_stop (ctx->socket_service);
        g_object_unref (ctx->socket_service);
    }
    g_free (ctx->name);
    if (ctx->command)
        g_byte_array_unref (ctx->command);
    if (ctx->response)
        g_byte_array_unref (ctx->response);
    g_slice_free (TestPortContext, ctx);
}

TestPortContext *
test_port_context_new (const gchar *name)
{
    TestPortContext *ctx;

    ctx = g_slice_new0 (TestPortContext);
    ctx->name = g_strdup (name);
    g_cond_init (&ctx->ready_cond);
    g_mutex_init (&ctx->ready_mutex);
    g_mutex_init (&ctx->command_mutex);
    return ctx;
}