| 1 |
/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ |
| 2 |
/* |
| 3 |
* This program is free software; you can redistribute it and/or modify |
| 4 |
* it under the terms of the GNU General Public License as published by |
| 5 |
* the Free Software Foundation; either version 2 of the License, or |
| 6 |
* (at your option) any later version. |
| 7 |
* |
| 8 |
* This program is distributed in the hope that it will be useful, |
| 9 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 10 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 11 |
* GNU General Public License for more details: |
| 12 |
* |
| 13 |
* Copyright (C) 2014 Aleksander Morgado <[email protected]> |
| 14 |
* |
| 15 |
* Higly based on the test-port-context setup in ModemManager. |
| 16 |
*/ |
| 17 |
|
| 18 |
#include <config.h> |
| 19 |
#include <stdio.h> |
| 20 |
#include <string.h> |
| 21 |
#include <stdint.h> |
| 22 |
|
| 23 |
#include <gio/gio.h> |
| 24 |
#include <gio/gunixsocketaddress.h> |
| 25 |
#include <libqmi-glib.h> |
| 26 |
|
| 27 |
#include "test-port-context.h" |
| 28 |
|
| 29 |
#define BUFFER_SIZE 1024 |
| 30 |
|
| 31 |
struct _TestPortContext { |
| 32 |
gchar *name; |
| 33 |
GThread *thread; |
| 34 |
gboolean ready; |
| 35 |
GCond ready_cond; |
| 36 |
GMutex ready_mutex; |
| 37 |
GMainLoop *loop; |
| 38 |
GSocketService *socket_service; |
| 39 |
GList *clients; |
| 40 |
GMutex command_mutex; |
| 41 |
GByteArray *command; |
| 42 |
GByteArray *response; |
| 43 |
}; |
| 44 |
|
| 45 |
/*****************************************************************************/ |
| 46 |
/* Helpers */ |
| 47 |
|
| 48 |
static gchar * |
| 49 |
str_hex (gconstpointer mem, |
| 50 |
gsize size, |
| 51 |
gchar delimiter) |
| 52 |
{ |
| 53 |
const guint8 *data = mem; |
| 54 |
gsize i; |
| 55 |
gsize j; |
| 56 |
gsize new_str_length; |
| 57 |
gchar *new_str; |
| 58 |
|
| 59 |
/* Get new string length. If input string has N bytes, we need: |
| 60 |
* - 1 byte for last NUL char |
| 61 |
* - 2N bytes for hexadecimal char representation of each byte... |
| 62 |
* - N-1 bytes for the separator ':' |
| 63 |
* So... a total of (1+2N+N-1) = 3N bytes are needed... */ |
| 64 |
new_str_length = 3 * size; |
| 65 |
|
| 66 |
/* Allocate memory for new array and initialize contents to NUL */ |
| 67 |
new_str = g_malloc0 (new_str_length); |
| 68 |
|
| 69 |
/* Print hexadecimal representation of each byte... */ |
| 70 |
for (i = 0, j = 0; i < size; i++, j += 3) { |
| 71 |
/* Print character in output string... */ |
| 72 |
snprintf (&new_str[j], 3, "%02X", data[i]); |
| 73 |
/* And if needed, add separator */ |
| 74 |
if (i != (size - 1) ) |
| 75 |
new_str[j + 2] = delimiter; |
| 76 |
} |
| 77 |
|
| 78 |
/* Set output string */ |
| 79 |
return new_str; |
| 80 |
} |
| 81 |
|
| 82 |
/*****************************************************************************/ |
| 83 |
|
| 84 |
void |
| 85 |
test_port_context_set_command (TestPortContext *ctx, |
| 86 |
const guint8 *command, |
| 87 |
gsize command_size, |
| 88 |
const guint8 *response, |
| 89 |
gsize response_size, |
| 90 |
guint16 transaction_id) |
| 91 |
{ |
| 92 |
g_mutex_lock (&ctx->command_mutex); |
| 93 |
{ |
| 94 |
g_assert (!ctx->command); |
| 95 |
ctx->command = g_byte_array_append (g_byte_array_sized_new (command_size), command, command_size); |
| 96 |
qmi_message_set_transaction_id ((QmiMessage *)ctx->command, transaction_id); |
| 97 |
|
| 98 |
g_assert (!ctx->response); |
| 99 |
ctx->response = g_byte_array_append (g_byte_array_sized_new (response_size), response, response_size); |
| 100 |
qmi_message_set_transaction_id ((QmiMessage *)ctx->response, transaction_id); |
| 101 |
} |
| 102 |
g_mutex_unlock (&ctx->command_mutex); |
| 103 |
} |
| 104 |
|
| 105 |
static GByteArray * |
| 106 |
process_next_command (TestPortContext *ctx, |
| 107 |
GByteArray *buffer) |
| 108 |
{ |
| 109 |
QmiMessage *message; |
| 110 |
GError *error = NULL; |
| 111 |
const guint8 *message_raw; |
| 112 |
gsize message_raw_length; |
| 113 |
gchar *expected; |
| 114 |
gchar *received; |
| 115 |
GByteArray *response; |
| 116 |
|
| 117 |
/* Every message received must start with the QMUX or QRTR marker. |
| 118 |
* If it doesn't, we broke framing :-/ |
| 119 |
* If we broke framing, an error should be reported and the device |
| 120 |
* should get closed */ |
| 121 |
if (buffer->len > 0 && |
| 122 |
buffer->data[0] != QMI_MESSAGE_QMUX_MARKER && |
| 123 |
buffer->data[0] != QMI_MESSAGE_QRTR_MARKER) |
| 124 |
g_assert_not_reached (); |
| 125 |
|
| 126 |
message = qmi_message_new_from_raw (buffer, &error); |
| 127 |
if (!message) { |
| 128 |
if (!error) |
| 129 |
/* More data we need */ |
| 130 |
return NULL; |
| 131 |
/* Fail */ |
| 132 |
g_assert_no_error (error); |
| 133 |
} |
| 134 |
|
| 135 |
/* Process received message */ |
| 136 |
message_raw = qmi_message_get_raw (message, &message_raw_length, &error); |
| 137 |
g_assert_no_error (error); |
| 138 |
g_assert (message_raw); |
| 139 |
|
| 140 |
/* Get printables to compare (we'll just get a nicer error if they are |
| 141 |
* different), compared to a simple memcmp(). */ |
| 142 |
g_mutex_lock (&ctx->command_mutex); |
| 143 |
{ |
| 144 |
g_assert (ctx->command); |
| 145 |
expected = str_hex (ctx->command->data, ctx->command->len, ':'); |
| 146 |
} |
| 147 |
g_mutex_unlock (&ctx->command_mutex); |
| 148 |
|
| 149 |
received = str_hex (message_raw, message_raw_length, ':'); |
| 150 |
g_assert_cmpstr (expected, ==, received); |
| 151 |
g_free (expected); |
| 152 |
g_free (received); |
| 153 |
qmi_message_unref (message); |
| 154 |
g_byte_array_unref (ctx->command); |
| 155 |
ctx->command = NULL; |
| 156 |
|
| 157 |
/* Command Expected == Received, so now return the Response */ |
| 158 |
g_mutex_lock (&ctx->command_mutex); |
| 159 |
{ |
| 160 |
response = ctx->response; |
| 161 |
ctx->response = NULL; |
| 162 |
} |
| 163 |
g_mutex_unlock (&ctx->command_mutex); |
| 164 |
|
| 165 |
return response; |
| 166 |
} |
| 167 |
|
| 168 |
/*****************************************************************************/ |
| 169 |
|
| 170 |
typedef struct { |
| 171 |
TestPortContext *ctx; |
| 172 |
GSocketConnection *connection; |
| 173 |
GSource *connection_readable_source; |
| 174 |
GByteArray *buffer; |
| 175 |
} Client; |
| 176 |
|
| 177 |
static void |
| 178 |
client_free (Client *client) |
| 179 |
{ |
| 180 |
g_source_destroy (client->connection_readable_source); |
| 181 |
g_source_unref (client->connection_readable_source); |
| 182 |
g_output_stream_close (g_io_stream_get_output_stream (G_IO_STREAM (client->connection)), NULL, NULL); |
| 183 |
if (client->buffer) |
| 184 |
g_byte_array_unref (client->buffer); |
| 185 |
g_object_unref (client->connection); |
| 186 |
g_slice_free (Client, client); |
| 187 |
} |
| 188 |
|
| 189 |
static void |
| 190 |
connection_close (Client *client) |
| 191 |
{ |
| 192 |
client->ctx->clients = g_list_remove (client->ctx->clients, client); |
| 193 |
client_free (client); |
| 194 |
} |
| 195 |
|
| 196 |
static void |
| 197 |
client_parse_request (Client *client) |
| 198 |
{ |
| 199 |
GByteArray *response; |
| 200 |
|
| 201 |
do { |
| 202 |
response = process_next_command (client->ctx, client->buffer); |
| 203 |
if (response) { |
| 204 |
GError *error = NULL; |
| 205 |
|
| 206 |
if (!g_output_stream_write_all (g_io_stream_get_output_stream (G_IO_STREAM (client->connection)), |
| 207 |
response->data, |
| 208 |
response->len, |
| 209 |
NULL, /* bytes_written */ |
| 210 |
NULL, /* cancellable */ |
| 211 |
&error)) { |
| 212 |
g_warning ("Cannot send response to client: %s", error->message); |
| 213 |
g_error_free (error); |
| 214 |
} |
| 215 |
g_byte_array_unref (response); |
| 216 |
} |
| 217 |
} while (response); |
| 218 |
} |
| 219 |
|
| 220 |
static gboolean |
| 221 |
connection_readable_cb (GSocket *socket, |
| 222 |
GIOCondition condition, |
| 223 |
Client *client) |
| 224 |
{ |
| 225 |
guint8 buffer[BUFFER_SIZE]; |
| 226 |
GError *error = NULL; |
| 227 |
gssize r; |
| 228 |
|
| 229 |
if (condition & G_IO_HUP || condition & G_IO_ERR) { |
| 230 |
g_debug ("client connection closed"); |
| 231 |
connection_close (client); |
| 232 |
return FALSE; |
| 233 |
} |
| 234 |
|
| 235 |
if (!(condition & G_IO_IN || condition & G_IO_PRI)) |
| 236 |
return TRUE; |
| 237 |
|
| 238 |
r = g_input_stream_read (g_io_stream_get_input_stream (G_IO_STREAM (client->connection)), |
| 239 |
buffer, |
| 240 |
BUFFER_SIZE, |
| 241 |
NULL, |
| 242 |
&error); |
| 243 |
|
| 244 |
if (r < 0) { |
| 245 |
g_warning ("Error reading from istream: %s", error ? error->message : "unknown"); |
| 246 |
if (error) |
| 247 |
g_error_free (error); |
| 248 |
/* Close the device */ |
| 249 |
connection_close (client); |
| 250 |
return FALSE; |
| 251 |
} |
| 252 |
|
| 253 |
if (r == 0) |
| 254 |
return TRUE; |
| 255 |
|
| 256 |
/* else, r > 0 */ |
| 257 |
if (!G_UNLIKELY (client->buffer)) |
| 258 |
client->buffer = g_byte_array_sized_new (r); |
| 259 |
g_byte_array_append (client->buffer, buffer, r); |
| 260 |
|
| 261 |
/* Try to parse input messages */ |
| 262 |
client_parse_request (client); |
| 263 |
|
| 264 |
return TRUE; |
| 265 |
} |
| 266 |
|
| 267 |
static Client * |
| 268 |
client_new (TestPortContext *ctx, |
| 269 |
GSocketConnection *connection) |
| 270 |
{ |
| 271 |
Client *client; |
| 272 |
|
| 273 |
client = g_slice_new0 (Client); |
| 274 |
client->ctx = ctx; |
| 275 |
client->connection = g_object_ref (connection); |
| 276 |
client->connection_readable_source = g_socket_create_source (g_socket_connection_get_socket (client->connection), |
| 277 |
G_IO_IN | G_IO_PRI | G_IO_ERR | G_IO_HUP, |
| 278 |
NULL); |
| 279 |
g_source_set_callback (client->connection_readable_source, |
| 280 |
(GSourceFunc)connection_readable_cb, |
| 281 |
client, |
| 282 |
NULL); |
| 283 |
g_source_attach (client->connection_readable_source, g_main_context_get_thread_default ()); |
| 284 |
|
| 285 |
return client; |
| 286 |
} |
| 287 |
|
| 288 |
/* /\*****************************************************************************\/ */ |
| 289 |
|
| 290 |
static void |
| 291 |
incoming_cb (GSocketService *service, |
| 292 |
GSocketConnection *connection, |
| 293 |
GObject *unused, |
| 294 |
TestPortContext *ctx) |
| 295 |
{ |
| 296 |
Client *client; |
| 297 |
|
| 298 |
client = client_new (ctx, connection); |
| 299 |
ctx->clients = g_list_append (ctx->clients, client); |
| 300 |
} |
| 301 |
|
| 302 |
static void |
| 303 |
create_socket_service (TestPortContext *ctx) |
| 304 |
{ |
| 305 |
GError *error = NULL; |
| 306 |
GSocketService *service; |
| 307 |
GSocketAddress *address; |
| 308 |
GSocket *socket; |
| 309 |
|
| 310 |
g_assert (ctx->socket_service == NULL); |
| 311 |
|
| 312 |
/* Create socket */ |
| 313 |
socket = g_socket_new (G_SOCKET_FAMILY_UNIX, |
| 314 |
G_SOCKET_TYPE_STREAM, |
| 315 |
G_SOCKET_PROTOCOL_DEFAULT, |
| 316 |
&error); |
| 317 |
if (!socket) |
| 318 |
g_error ("Cannot create socket: %s", error->message); |
| 319 |
|
| 320 |
/* Bind to address */ |
| 321 |
address = (g_unix_socket_address_new_with_type ( |
| 322 |
ctx->name, |
| 323 |
-1, |
| 324 |
G_UNIX_SOCKET_ADDRESS_ABSTRACT)); |
| 325 |
if (!g_socket_bind (socket, address, TRUE, &error)) |
| 326 |
g_error ("Cannot bind socket '%s': %s", ctx->name, error->message); |
| 327 |
|
| 328 |
/* Listen */ |
| 329 |
if (!g_socket_listen (socket, &error)) |
| 330 |
g_error ("Cannot listen in socket: %s", error->message); |
| 331 |
|
| 332 |
/* Create socket service */ |
| 333 |
service = g_socket_service_new (); |
| 334 |
g_signal_connect (service, "incoming", G_CALLBACK (incoming_cb), ctx); |
| 335 |
if (!g_socket_listener_add_socket (G_SOCKET_LISTENER (service), |
| 336 |
socket, |
| 337 |
NULL, /* don't pass an object, will take a reference */ |
| 338 |
&error)) |
| 339 |
g_error ("Cannot add listener to socket: %s", error->message); |
| 340 |
|
| 341 |
/* Start it */ |
| 342 |
g_socket_service_start (service); |
| 343 |
|
| 344 |
/* And store it */ |
| 345 |
ctx->socket_service = service; |
| 346 |
|
| 347 |
/* Signal that the thread is ready */ |
| 348 |
g_mutex_lock (&ctx->ready_mutex); |
| 349 |
ctx->ready = TRUE; |
| 350 |
g_cond_signal (&ctx->ready_cond); |
| 351 |
g_mutex_unlock (&ctx->ready_mutex); |
| 352 |
|
| 353 |
if (socket) |
| 354 |
g_object_unref (socket); |
| 355 |
if (address) |
| 356 |
g_object_unref (address); |
| 357 |
} |
| 358 |
|
| 359 |
/*****************************************************************************/ |
| 360 |
|
| 361 |
void |
| 362 |
test_port_context_stop (TestPortContext *ctx) |
| 363 |
{ |
| 364 |
g_assert (ctx->thread != NULL); |
| 365 |
g_assert (ctx->loop != NULL); |
| 366 |
|
| 367 |
g_main_loop_quit (ctx->loop); |
| 368 |
|
| 369 |
g_thread_join (ctx->thread); |
| 370 |
ctx->thread = NULL; |
| 371 |
} |
| 372 |
|
| 373 |
static gpointer |
| 374 |
port_context_thread_func (TestPortContext *ctx) |
| 375 |
{ |
| 376 |
GMainContext *thread_context; |
| 377 |
|
| 378 |
thread_context = g_main_context_new (); |
| 379 |
g_main_context_push_thread_default (thread_context); |
| 380 |
|
| 381 |
create_socket_service (ctx); |
| 382 |
|
| 383 |
g_assert (ctx->loop == NULL); |
| 384 |
ctx->loop = g_main_loop_new (g_main_context_get_thread_default (), FALSE); |
| 385 |
g_main_loop_run (ctx->loop); |
| 386 |
g_main_loop_unref (ctx->loop); |
| 387 |
ctx->loop = NULL; |
| 388 |
return NULL; |
| 389 |
} |
| 390 |
|
| 391 |
void |
| 392 |
test_port_context_start (TestPortContext *ctx) |
| 393 |
{ |
| 394 |
g_assert (ctx->thread == NULL); |
| 395 |
ctx->thread = g_thread_new (ctx->name, |
| 396 |
(GThreadFunc)port_context_thread_func, |
| 397 |
ctx); |
| 398 |
|
| 399 |
/* Now wait until the thread has finished its initialization and is |
| 400 |
* ready to serve connections */ |
| 401 |
g_mutex_lock (&ctx->ready_mutex); |
| 402 |
while (!ctx->ready) |
| 403 |
g_cond_wait (&ctx->ready_cond, &ctx->ready_mutex); |
| 404 |
g_mutex_unlock (&ctx->ready_mutex); |
| 405 |
} |
| 406 |
|
| 407 |
/*****************************************************************************/ |
| 408 |
|
| 409 |
void |
| 410 |
test_port_context_free (TestPortContext *ctx) |
| 411 |
{ |
| 412 |
g_assert (ctx->thread == NULL); |
| 413 |
g_assert (ctx->loop == NULL); |
| 414 |
|
| 415 |
g_cond_clear (&ctx->ready_cond); |
| 416 |
g_mutex_clear (&ctx->ready_mutex); |
| 417 |
g_mutex_clear (&ctx->command_mutex); |
| 418 |
|
| 419 |
g_list_free_full (ctx->clients, (GDestroyNotify)client_free); |
| 420 |
if (ctx->socket_service) { |
| 421 |
if (g_socket_service_is_active (ctx->socket_service)) |
| 422 |
g_socket_service_stop (ctx->socket_service); |
| 423 |
g_object_unref (ctx->socket_service); |
| 424 |
} |
| 425 |
g_free (ctx->name); |
| 426 |
if (ctx->command) |
| 427 |
g_byte_array_unref (ctx->command); |
| 428 |
if (ctx->response) |
| 429 |
g_byte_array_unref (ctx->response); |
| 430 |
g_slice_free (TestPortContext, ctx); |
| 431 |
} |
| 432 |
|
| 433 |
TestPortContext * |
| 434 |
test_port_context_new (const gchar *name) |
| 435 |
{ |
| 436 |
TestPortContext *ctx; |
| 437 |
|
| 438 |
ctx = g_slice_new0 (TestPortContext); |
| 439 |
ctx->name = g_strdup (name); |
| 440 |
g_cond_init (&ctx->ready_cond); |
| 441 |
g_mutex_init (&ctx->ready_mutex); |
| 442 |
g_mutex_init (&ctx->command_mutex); |
| 443 |
return ctx; |
| 444 |
} |