Debian Codegraph @

src/libqmi-glib/test/test-port-context.c

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