src/packet.c

   1 /* $Id$
   2  * UDP Gateway utility packet assembly and disassembly layer
   3  * Copyright (C) 2004 Jan Kratochvil <project-udpgate@jankratochvil.net>
   4  *
   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
   8  *
   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.
  13  *
  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
  17  */
  18
  19
  20 #include "config.h"
  21
  22 #include <glib/gmessages.h>
  23 #include <glib/gstring.h>
  24 #include <glib/ghash.h>
  25 #include <netinet/in.h>
  26 #include <string.h>
  27 #include <glib/gslist.h>
  28
  29 #include "packet.h"
  30 #include "main.h"       /* for optarg_verbose */
  31
  32
  33 static void packet_assembly_guint32(GString *gstring,guint32 value_guint32)
  34 {
  35 guint32 value_guint32_htonl;
  36
  37         g_return_if_fail(gstring!=NULL);
  38
  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));
  42 }
  43
  44 static void packet_assembly_guint16(GString *gstring,guint16 value_guint16)
  45 {
  46 guint16 value_guint16_htonl;
  47
  48         g_return_if_fail(gstring!=NULL);
  49
  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));
  53 }
  54
  55 static void packet_assembly_data_guint32(GString *gstring,guint32 value_guint32)
  56 {
  57 guint32 value_guint32_htonl=htonl(value_guint32);
  58
  59         g_return_if_fail(gstring!=NULL);
  60
  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);
  64 }
  65
  66 static void packet_assembly_data_guint16(GString *gstring,guint16 value_guint16)
  67 {
  68 guint16 value_guint16_htonl=htons(value_guint16);
  69
  70         g_return_if_fail(gstring!=NULL);
  71
  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);
  75 }
  76
  77 static void packet_assembly_data_string(GString *gstring,const gchar *string)
  78 {
  79 size_t string_length;
  80 #ifndef G_DISABLE_ASSERT
  81 size_t gstring_length_orig;
  82 #endif  /* !G_DISABLE_ASSERT */
  83
  84         g_return_if_fail(gstring!=NULL);
  85         g_return_if_fail(string!=NULL);
  86
  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 */
  96 }
  97
  98 static void packet_assembly_foreach(gpointer elem_type_gpointer,gpointer value,GString *gstring /* user_data */)        /* GHFunc */
  99 {
 100 guint32 elem_type_uint32;
 101
 102         g_return_if_fail(gstring!=NULL);
 103
 104         elem_type_uint32=GPOINTER_TO_UINT(elem_type_gpointer);
 105         packet_assembly_guint32(gstring,elem_type_uint32);
 106
 107         switch (elem_type_uint32) {
 108                 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
 109                         packet_assembly_data_string(gstring,value);
 110                         if (optarg_verbose)
 111                                 g_message(_("Assembled PROGRAM_VERSION: %s"),(const char *)value);
 112                         break;
 113                 case PACKET_ELEM_TYPE_CLIENT_INADDR:
 114                         packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
 115                         if (optarg_verbose)
 116                                 g_message(_("Assembled CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
 117                         break;
 118                 case PACKET_ELEM_TYPE_DATA_GUINT32:
 119                         packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
 120                         if (optarg_verbose)
 121                                 g_message(_("Assembled DATA_GUINT32: 0x%08x"),(unsigned)GPOINTER_TO_UINT(value));
 122                         break;
 123                 case PACKET_ELEM_TYPE_CLIENT_PORT:
 124                         packet_assembly_data_guint16(gstring,GPOINTER_TO_UINT(value));
 125                         if (optarg_verbose)
 126                                 g_message(_("Assembled CLIENT_PORT: %d"),(int)GPOINTER_TO_UINT(value));
 127                         break;
 128                 default:
 129                         g_assert_not_reached();
 130                 }
 131 }
 132
 133 void *packet_assembly(size_t *packet_length_pointer,GHashTable *values)
 134 {
 135 GString *gstring;
 136
 137         g_return_val_if_fail(packet_length_pointer!=NULL,NULL);
 138         g_return_val_if_fail(values!=NULL,NULL);
 139
 140         if (optarg_verbose)
 141                 g_message(_("Assembling packet..."));
 142
 143         gstring=g_string_new(NULL);
 144
 145         g_string_append(gstring,PACKET_HEADER);
 146
 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 */
 152
 153         if (optarg_verbose)
 154                 g_message(_("Packet assembly done."));
 155
 156         *packet_length_pointer=gstring->len;
 157         return g_string_free(gstring,
 158                         FALSE); /* free_segment */
 159 }
 160
 161 static gboolean packet_disassembly_guint32
 162                 (guint32 *elem_type_guint32_pointer,gconstpointer *packet_pointer,gconstpointer packet_end)
 163 {
 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);
 168
 169         if ((*packet_pointer)+sizeof(*elem_type_guint32_pointer)>packet_end)
 170                 return FALSE;
 171
 172         *elem_type_guint32_pointer=ntohl(*((guint32 *)*packet_pointer));
 173         (*packet_pointer)+=sizeof(*elem_type_guint32_pointer);
 174         return TRUE;
 175 }
 176
 177 static void packet_disassembly_value_destroy_func(gpointer data)        /* GDestroyNotify */
 178 {
 179
 180 }
 181
 182 /* map: (GHashTable *) -> (GSList *) */
 183 static GHashTable *packet_disassembly_destroy_registry;
 184
 185 static void packet_disassembly_destroy_key_destroy_func(GHashTable *hash) /* GDestroyNotify */
 186 {
 187         g_return_if_fail(hash!=NULL);
 188
 189         g_hash_table_destroy(hash);
 190 }
 191
 192 static void packet_disassembly_destroy_value_destroy_func(GSList *items)        /* GDestroyNotify */
 193 {
 194         /* 'items' may be NULL */
 195
 196         g_slist_foreach(
 197                         items,  /* list */
 198                         (GFunc)g_free,  /* func */
 199                         NULL);  /* user_data */
 200 }
 201
 202 static void packet_disassembly_destroy_registry_init(void)
 203 {
 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 */
 210                 }
 211 }
 212
 213 void packet_disassembly_destroy(GHashTable *hash)
 214 {
 215 gboolean errgboolean;
 216
 217         g_return_if_fail(hash!=NULL);
 218
 219         packet_disassembly_destroy_registry_init();
 220
 221         errgboolean=g_hash_table_remove(packet_disassembly_destroy_registry,hash);
 222         g_assert(errgboolean==TRUE);
 223 }
 224
 225 static gboolean packet_disassembly_data_string
 226                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 227 {
 228 gchar *string;
 229
 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);
 233
 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;
 239
 240         return TRUE;
 241 }
 242
 243 static gboolean packet_disassembly_data_guint32
 244                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 245 {
 246 guint32 value_guint32;
 247
 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);
 251
 252         g_assert(4==sizeof(value_guint32));
 253         if (elem_data_length_guint32!=sizeof(value_guint32))
 254                 return FALSE;
 255
 256         value_guint32=ntohl(*(guint32 *)elem_data);
 257         *value_pointer=GUINT_TO_POINTER(value_guint32);
 258
 259         return TRUE;
 260 }
 261
 262 static gboolean packet_disassembly_data_guint16
 263                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 264 {
 265 guint16 value_guint16;
 266
 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);
 270
 271         g_assert(2==sizeof(value_guint16));
 272         if (elem_data_length_guint32!=sizeof(value_guint16))
 273                 return FALSE;
 274
 275         value_guint16=ntohs(*(guint16 *)elem_data);
 276         *value_pointer=GUINT_TO_POINTER((guint)value_guint16);
 277
 278         return TRUE;
 279 }
 280
 281 GHashTable *packet_disassembly(gconstpointer packet,size_t packet_length)
 282 {
 283 GHashTable *r;
 284 gconstpointer packet_end;
 285 GSList *items;
 286
 287         g_return_val_if_fail(packet!=NULL,NULL);
 288
 289         if (optarg_verbose)
 290                 g_message(_("Decoding packet..."));
 291
 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;
 298
 299         if (packet+strlen(PACKET_HEADER)>packet_end) {
 300 err_g_hash_table_destroy_r:
 301                 g_hash_table_destroy(r);
 302                 return NULL;
 303                 }
 304         if (memcmp(packet,PACKET_HEADER,strlen(PACKET_HEADER)))
 305                 goto err_g_hash_table_destroy_r;
 306         packet+=strlen(PACKET_HEADER);
 307
 308         items=NULL;
 309         while (packet<packet_end) {
 310 guint32 elem_type_guint32;
 311 guint32 elem_data_length_guint32;
 312 gconstpointer elem_data;
 313 gpointer value;
 314
 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;
 320                         }
 321                 if (!packet_disassembly_guint32(&elem_data_length_guint32,&packet,packet_end))
 322                         goto err_packet_disassembly_destroy_value_destroy_func_items;
 323                 elem_data=packet;
 324                 packet+=elem_data_length_guint32;
 325
 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;
 330                         if (optarg_verbose)
 331                                 g_message(_("Decoded PROGRAM_VERSION: %s"),(const char *)value);
 332                         break;
 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;
 336                         if (optarg_verbose)
 337                                 g_message(_("Decoded CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
 338                         break;
 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;
 342                         if (optarg_verbose)
 343                                 g_message(_("Decoded DATA_GUINT32: 0x%08x"),(unsigned)GPOINTER_TO_UINT(value));
 344                         break;
 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;
 348                         if (optarg_verbose)
 349                                 g_message(_("Decoded CLIENT_PORT: %d"),GPOINTER_TO_UINT(value));
 350                         break;
 351                 default:
 352                         if (elem_type_guint32&PACKET_ELEM_ATTR_MANDATORY) {
 353                                 if (optarg_verbose)
 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;
 356                                 }
 357                         else {
 358                                 if (optarg_verbose)
 359                                         g_message(_("Decoding ignores ATTR_OPTIONAL ELEM code 0x%08x"),(unsigned)elem_type_guint32);
 360                                 }
 361                         continue;
 362                         }
 363                 g_hash_table_insert(r,GUINT_TO_POINTER(elem_type_guint32),value);
 364                 }
 365         g_assert(packet==packet_end);
 366
 367         packet_disassembly_destroy_registry_init();
 368         g_hash_table_insert(packet_disassembly_destroy_registry,r,items);
 369
 370         if (optarg_verbose)
 371                 g_message(_("Decoding done."));
 372
 373         return r;
 374 }