2 * UDP Gateway utility packet assembly and disassembly layer
3 * Copyright (C) 2004 Jan Kratochvil <project-udpgate@jankratochvil.net>
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; exactly version 2 of June 1991 is required
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 #include <glib/gmessages.h>
23 #include <glib/gstring.h>
24 #include <glib/ghash.h>
25 #include <netinet/in.h>
27 #include <glib/gslist.h>
30 #include "main.h" /* for optarg_verbose */
33 static void packet_assembly_guint32(GString *gstring,guint32 value_guint32)
35 guint32 value_guint32_htonl;
37 g_return_if_fail(gstring!=NULL);
39 value_guint32_htonl=htonl(value_guint32);
40 g_assert(4==sizeof(value_guint32_htonl));
41 g_string_append_len(gstring,(const gchar *)&value_guint32_htonl,sizeof(value_guint32_htonl));
44 static void packet_assembly_guint16(GString *gstring,guint16 value_guint16)
46 guint16 value_guint16_htonl;
48 g_return_if_fail(gstring!=NULL);
50 value_guint16_htonl=htons(value_guint16);
51 g_assert(2==sizeof(value_guint16_htonl));
52 g_string_append_len(gstring,(const gchar *)&value_guint16_htonl,sizeof(value_guint16_htonl));
55 static void packet_assembly_data_guint32(GString *gstring,guint32 value_guint32)
57 guint32 value_guint32_htonl=htonl(value_guint32);
59 g_return_if_fail(gstring!=NULL);
61 g_assert(4==sizeof(value_guint32_htonl));
62 packet_assembly_guint32(gstring,sizeof(value_guint32_htonl));
63 packet_assembly_guint32(gstring,value_guint32_htonl);
66 static void packet_assembly_data_guint16(GString *gstring,guint16 value_guint16)
68 guint16 value_guint16_htonl=htons(value_guint16);
70 g_return_if_fail(gstring!=NULL);
72 g_assert(2==sizeof(value_guint16_htonl));
73 packet_assembly_guint32(gstring,sizeof(value_guint16_htonl));
74 packet_assembly_guint16(gstring,value_guint16_htonl);
77 static void packet_assembly_data_string(GString *gstring,const gchar *string)
80 #ifndef G_DISABLE_ASSERT
81 size_t gstring_length_orig;
82 #endif /* !G_DISABLE_ASSERT */
84 g_return_if_fail(gstring!=NULL);
85 g_return_if_fail(string!=NULL);
87 string_length=strlen(string);
88 packet_assembly_guint32(gstring,string_length);
89 #ifndef G_DISABLE_ASSERT
90 gstring_length_orig=gstring->len;
91 #endif /* !G_DISABLE_ASSERT */
92 g_string_append(gstring,string);
93 #ifndef G_DISABLE_ASSERT
94 g_assert(gstring_length_orig+string_length==gstring->len);
95 #endif /* !G_DISABLE_ASSERT */
98 static void packet_assembly_foreach(gpointer elem_type_gpointer,gpointer value,GString *gstring /* user_data */) /* GHFunc */
100 guint32 elem_type_uint32;
102 g_return_if_fail(gstring!=NULL);
104 elem_type_uint32=GPOINTER_TO_UINT(elem_type_gpointer);
105 packet_assembly_guint32(gstring,elem_type_uint32);
107 switch (elem_type_uint32) {
108 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
109 packet_assembly_data_string(gstring,value);
111 g_message(_("Assembled PROGRAM_VERSION: %s"),(const char *)value);
113 case PACKET_ELEM_TYPE_CLIENT_INADDR:
114 packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
116 g_message(_("Assembled CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
118 case PACKET_ELEM_TYPE_DATA_GUINT32:
119 packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
121 g_message(_("Assembled DATA_GUINT32: 0x%08x"),(unsigned)GPOINTER_TO_UINT(value));
123 case PACKET_ELEM_TYPE_CLIENT_PORT:
124 packet_assembly_data_guint16(gstring,GPOINTER_TO_UINT(value));
126 g_message(_("Assembled CLIENT_PORT: %d"),(int)GPOINTER_TO_UINT(value));
129 g_assert_not_reached();
133 void *packet_assembly(size_t *packet_length_pointer,GHashTable *values)
137 g_return_val_if_fail(packet_length_pointer!=NULL,NULL);
138 g_return_val_if_fail(values!=NULL,NULL);
141 g_message(_("Assembling packet..."));
143 gstring=g_string_new(NULL);
145 g_string_append(gstring,PACKET_HEADER);
147 packet_assembly_foreach(GINT_TO_POINTER(PACKET_ELEM_TYPE_PROGRAM_VERSION),VERSION,gstring);
148 g_hash_table_foreach(
149 values, /* hash_table */
150 (GHFunc)packet_assembly_foreach, /* func */
151 gstring); /* user_data */
154 g_message(_("Packet assembly done."));
156 *packet_length_pointer=gstring->len;
157 return g_string_free(gstring,
158 FALSE); /* free_segment */
161 static gboolean packet_disassembly_guint32
162 (guint32 *elem_type_guint32_pointer,gconstpointer *packet_pointer,gconstpointer packet_end)
164 g_return_val_if_fail(elem_type_guint32_pointer!=NULL,FALSE);
165 g_return_val_if_fail(packet_pointer!=NULL,FALSE);
166 g_return_val_if_fail(packet_end!=NULL,FALSE);
167 g_return_val_if_fail(*packet_pointer<=packet_end,FALSE);
169 if ((*packet_pointer)+sizeof(*elem_type_guint32_pointer)>packet_end)
172 *elem_type_guint32_pointer=ntohl(*((guint32 *)*packet_pointer));
173 (*packet_pointer)+=sizeof(*elem_type_guint32_pointer);
177 static void packet_disassembly_value_destroy_func(gpointer data) /* GDestroyNotify */
182 /* map: (GHashTable *) -> (GSList *) */
183 static GHashTable *packet_disassembly_destroy_registry;
185 static void packet_disassembly_destroy_key_destroy_func(GHashTable *hash) /* GDestroyNotify */
187 g_return_if_fail(hash!=NULL);
189 g_hash_table_destroy(hash);
192 static void packet_disassembly_destroy_value_destroy_func(GSList *items) /* GDestroyNotify */
194 /* 'items' may be NULL */
198 (GFunc)g_free, /* func */
199 NULL); /* user_data */
202 static void packet_disassembly_destroy_registry_init(void)
204 if (!packet_disassembly_destroy_registry) {
205 packet_disassembly_destroy_registry=g_hash_table_new_full(
206 g_direct_hash, /* hash_func */
207 g_direct_equal, /* key_equal_func */
208 (GDestroyNotify)packet_disassembly_destroy_key_destroy_func, /* key_destroy_func */
209 (GDestroyNotify)packet_disassembly_destroy_value_destroy_func); /* value_destroy_func */
213 void packet_disassembly_destroy(GHashTable *hash)
215 gboolean errgboolean;
217 g_return_if_fail(hash!=NULL);
219 packet_disassembly_destroy_registry_init();
221 errgboolean=g_hash_table_remove(packet_disassembly_destroy_registry,hash);
222 g_assert(errgboolean==TRUE);
225 static gboolean packet_disassembly_data_string
226 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
230 g_return_val_if_fail(value_pointer!=NULL,FALSE);
231 g_return_val_if_fail(elem_data!=NULL,FALSE);
232 g_return_val_if_fail(items_pointer!=NULL,FALSE);
234 udpgate_newn(string,elem_data_length_guint32+1);
235 *items_pointer=g_slist_prepend(*items_pointer,string);
236 memcpy(string,elem_data,elem_data_length_guint32);
237 string[elem_data_length_guint32]='\0';
238 *value_pointer=string;
243 static gboolean packet_disassembly_data_guint32
244 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
246 guint32 value_guint32;
248 g_return_val_if_fail(value_pointer!=NULL,FALSE);
249 g_return_val_if_fail(elem_data!=NULL,FALSE);
250 g_return_val_if_fail(items_pointer!=NULL,FALSE);
252 g_assert(4==sizeof(value_guint32));
253 if (elem_data_length_guint32!=sizeof(value_guint32))
256 value_guint32=ntohl(*(guint32 *)elem_data);
257 *value_pointer=GUINT_TO_POINTER(value_guint32);
262 static gboolean packet_disassembly_data_guint16
263 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
265 guint16 value_guint16;
267 g_return_val_if_fail(value_pointer!=NULL,FALSE);
268 g_return_val_if_fail(elem_data!=NULL,FALSE);
269 g_return_val_if_fail(items_pointer!=NULL,FALSE);
271 g_assert(2==sizeof(value_guint16));
272 if (elem_data_length_guint32!=sizeof(value_guint16))
275 value_guint16=ntohs(*(guint16 *)elem_data);
276 *value_pointer=GUINT_TO_POINTER((guint)value_guint16);
281 GHashTable *packet_disassembly(gconstpointer packet,size_t packet_length)
284 gconstpointer packet_end;
287 g_return_val_if_fail(packet!=NULL,NULL);
290 g_message(_("Decoding packet..."));
292 r=g_hash_table_new_full(
293 g_direct_hash, /* hash_func */
294 g_direct_equal, /* key_equal_func */
295 NULL, /* key_destroy_func */
296 packet_disassembly_value_destroy_func); /* value_destroy_func */
297 packet_end=packet+packet_length;
299 if (packet+strlen(PACKET_HEADER)>packet_end) {
300 err_g_hash_table_destroy_r:
301 g_hash_table_destroy(r);
304 if (memcmp(packet,PACKET_HEADER,strlen(PACKET_HEADER)))
305 goto err_g_hash_table_destroy_r;
306 packet+=strlen(PACKET_HEADER);
309 while (packet<packet_end) {
310 guint32 elem_type_guint32;
311 guint32 elem_data_length_guint32;
312 gconstpointer elem_data;
315 if (!packet_disassembly_guint32(&elem_type_guint32,&packet,packet_end)) {
316 err_packet_disassembly_destroy_value_destroy_func_items:
317 /* 'items' are not yet registered in 'packet_disassembly_destroy_registry' */
318 packet_disassembly_destroy_value_destroy_func(items);
319 goto err_g_hash_table_destroy_r;
321 if (!packet_disassembly_guint32(&elem_data_length_guint32,&packet,packet_end))
322 goto err_packet_disassembly_destroy_value_destroy_func_items;
324 packet+=elem_data_length_guint32;
326 switch (elem_type_guint32) {
327 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
328 if (!packet_disassembly_data_string(&value,elem_data,elem_data_length_guint32,&items))
329 goto err_packet_disassembly_destroy_value_destroy_func_items;
331 g_message(_("Decoded PROGRAM_VERSION: %s"),(const char *)value);
333 case PACKET_ELEM_TYPE_CLIENT_INADDR:
334 if (!packet_disassembly_data_guint32(&value,elem_data,elem_data_length_guint32,&items))
335 goto err_packet_disassembly_destroy_value_destroy_func_items;
337 g_message(_("Decoded CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
339 case PACKET_ELEM_TYPE_DATA_GUINT32:
340 if (!packet_disassembly_data_guint32(&value,elem_data,elem_data_length_guint32,&items))
341 goto err_packet_disassembly_destroy_value_destroy_func_items;
343 g_message(_("Decoded DATA_GUINT32: 0x%08x"),(unsigned)GPOINTER_TO_UINT(value));
345 case PACKET_ELEM_TYPE_CLIENT_PORT:
346 if (!packet_disassembly_data_guint16(&value,elem_data,elem_data_length_guint32,&items))
347 goto err_packet_disassembly_destroy_value_destroy_func_items;
349 g_message(_("Decoded CLIENT_PORT: %d"),GPOINTER_TO_UINT(value));
352 if (elem_type_guint32&PACKET_ELEM_ATTR_MANDATORY) {
354 g_message(_("Decoding failed on ATTR_MANDATORY ELEM code 0x%08x"),(unsigned)elem_type_guint32);
355 goto err_packet_disassembly_destroy_value_destroy_func_items;
359 g_message(_("Decoding ignores ATTR_OPTIONAL ELEM code 0x%08x"),(unsigned)elem_type_guint32);
363 g_hash_table_insert(r,GUINT_TO_POINTER(elem_type_guint32),value);
365 g_assert(packet==packet_end);
367 packet_disassembly_destroy_registry_init();
368 g_hash_table_insert(packet_disassembly_destroy_registry,r,items);
371 g_message(_("Decoding done."));