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         g_return_if_fail(gstring!=NULL);
  58
  59         g_assert(4==sizeof(value_guint32));
  60         packet_assembly_guint32(gstring,sizeof(value_guint32));
  61         packet_assembly_guint32(gstring,value_guint32);
  62 }
  63
  64 static void packet_assembly_data_guint16(GString *gstring,guint16 value_guint16)
  65 {
  66         g_return_if_fail(gstring!=NULL);
  67
  68         g_assert(2==sizeof(value_guint16));
  69         packet_assembly_guint32(gstring,sizeof(value_guint16));
  70         packet_assembly_guint16(gstring,value_guint16);
  71 }
  72
  73 static void packet_assembly_data_string(GString *gstring,const gchar *string)
  74 {
  75 size_t string_length;
  76 #ifndef G_DISABLE_ASSERT
  77 size_t gstring_length_orig;
  78 #endif  /* !G_DISABLE_ASSERT */
  79
  80         g_return_if_fail(gstring!=NULL);
  81         g_return_if_fail(string!=NULL);
  82
  83         string_length=strlen(string);
  84         packet_assembly_guint32(gstring,string_length);
  85 #ifndef G_DISABLE_ASSERT
  86         gstring_length_orig=gstring->len;
  87 #endif  /* !G_DISABLE_ASSERT */
  88         g_string_append(gstring,string);
  89 #ifndef G_DISABLE_ASSERT
  90         g_assert(gstring_length_orig+string_length==gstring->len);
  91 #endif  /* !G_DISABLE_ASSERT */
  92 }
  93
  94 static void packet_assembly_foreach(gpointer elem_type_gpointer,gpointer value,GString *gstring /* user_data */)        /* GHFunc */
  95 {
  96 guint32 elem_type_uint32;
  97
  98         g_return_if_fail(gstring!=NULL);
  99
 100         elem_type_uint32=GPOINTER_TO_UINT(elem_type_gpointer);
 101         packet_assembly_guint32(gstring,elem_type_uint32);
 102
 103         switch (elem_type_uint32) {
 104                 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
 105                         packet_assembly_data_string(gstring,value);
 106                         if (optarg_verbose)
 107                                 g_message(_("Assembled PROGRAM_VERSION: %s"),(const char *)value);
 108                         break;
 109                 case PACKET_ELEM_TYPE_CLIENT_INADDR:
 110                         packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
 111                         if (optarg_verbose)
 112                                 g_message(_("Assembled CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
 113                         break;
 114                 case PACKET_ELEM_TYPE_DATA_GUINT32:
 115                         packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
 116                         if (optarg_verbose)
 117                                 g_message(_("Assembled DATA_GUINT32: 0x%08X"),(unsigned)GPOINTER_TO_UINT(value));
 118                         break;
 119                 case PACKET_ELEM_TYPE_CLIENT_PORT:
 120                         packet_assembly_data_guint16(gstring,GPOINTER_TO_UINT(value));
 121                         if (optarg_verbose)
 122                                 g_message(_("Assembled CLIENT_PORT: %d"),(int)GPOINTER_TO_UINT(value));
 123                         break;
 124                 default:
 125                         g_assert_not_reached();
 126                 }
 127 }
 128
 129 void *packet_assembly(size_t *packet_length_pointer,GHashTable *values)
 130 {
 131 GString *gstring;
 132
 133         g_return_val_if_fail(packet_length_pointer!=NULL,NULL);
 134         g_return_val_if_fail(values!=NULL,NULL);
 135
 136         if (optarg_verbose)
 137                 g_message(_("Assembling packet..."));
 138
 139         gstring=g_string_new(NULL);
 140
 141         g_string_append(gstring,PACKET_HEADER);
 142
 143         packet_assembly_foreach(GINT_TO_POINTER(PACKET_ELEM_TYPE_PROGRAM_VERSION),VERSION,gstring);
 144         g_hash_table_foreach(
 145                         values, /* hash_table */
 146                         (GHFunc)packet_assembly_foreach,        /* func */
 147                         gstring);       /* user_data */
 148
 149         if (optarg_verbose)
 150                 g_message(_("Packet assembly done."));
 151
 152         *packet_length_pointer=gstring->len;
 153         return g_string_free(gstring,
 154                         FALSE); /* free_segment */
 155 }
 156
 157 static gboolean packet_disassembly_guint32
 158                 (guint32 *elem_type_guint32_pointer,gconstpointer *packet_pointer,gconstpointer packet_end)
 159 {
 160         g_return_val_if_fail(elem_type_guint32_pointer!=NULL,FALSE);
 161         g_return_val_if_fail(packet_pointer!=NULL,FALSE);
 162         g_return_val_if_fail(packet_end!=NULL,FALSE);
 163         g_return_val_if_fail(*packet_pointer<=packet_end,FALSE);
 164
 165         if ((*packet_pointer)+sizeof(*elem_type_guint32_pointer)>packet_end)
 166                 return FALSE;
 167
 168         *elem_type_guint32_pointer=ntohl(*((guint32 *)*packet_pointer));
 169         (*packet_pointer)+=sizeof(*elem_type_guint32_pointer);
 170         return TRUE;
 171 }
 172
 173 static void packet_disassembly_value_destroy_func(gpointer data)        /* GDestroyNotify */
 174 {
 175
 176 }
 177
 178 /* map: (GHashTable *) -> (GSList *) */
 179 static GHashTable *packet_disassembly_destroy_registry;
 180
 181 static void packet_disassembly_destroy_key_destroy_func(GHashTable *hash) /* GDestroyNotify */
 182 {
 183         g_return_if_fail(hash!=NULL);
 184
 185         g_hash_table_destroy(hash);
 186 }
 187
 188 static void packet_disassembly_destroy_value_destroy_func(GSList *items)        /* GDestroyNotify */
 189 {
 190         /* 'items' may be NULL */
 191
 192         g_slist_foreach(
 193                         items,  /* list */
 194                         (GFunc)g_free,  /* func */
 195                         NULL);  /* user_data */
 196 }
 197
 198 static void packet_disassembly_destroy_registry_init(void)
 199 {
 200         if (!packet_disassembly_destroy_registry) {
 201                 packet_disassembly_destroy_registry=g_hash_table_new_full(
 202                         g_direct_hash,  /* hash_func */
 203                         g_direct_equal, /* key_equal_func */
 204                         (GDestroyNotify)packet_disassembly_destroy_key_destroy_func,    /* key_destroy_func */
 205                         (GDestroyNotify)packet_disassembly_destroy_value_destroy_func); /* value_destroy_func */
 206                 }
 207 }
 208
 209 void packet_disassembly_destroy(GHashTable *hash)
 210 {
 211 gboolean errgboolean;
 212
 213         g_return_if_fail(hash!=NULL);
 214
 215         packet_disassembly_destroy_registry_init();
 216
 217         errgboolean=g_hash_table_remove(packet_disassembly_destroy_registry,hash);
 218         g_assert(errgboolean==TRUE);
 219 }
 220
 221 static gboolean packet_disassembly_data_string
 222                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 223 {
 224 gchar *string;
 225
 226         g_return_val_if_fail(value_pointer!=NULL,FALSE);
 227         g_return_val_if_fail(elem_data!=NULL,FALSE);
 228         g_return_val_if_fail(items_pointer!=NULL,FALSE);
 229
 230         udpgate_newn(string,elem_data_length_guint32+1);
 231         *items_pointer=g_slist_prepend(*items_pointer,string);
 232         memcpy(string,elem_data,elem_data_length_guint32);
 233         string[elem_data_length_guint32]='\0';
 234         *value_pointer=string;
 235
 236         return TRUE;
 237 }
 238
 239 static gboolean packet_disassembly_data_guint32
 240                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 241 {
 242 guint32 value_guint32;
 243
 244         g_return_val_if_fail(value_pointer!=NULL,FALSE);
 245         g_return_val_if_fail(elem_data!=NULL,FALSE);
 246         g_return_val_if_fail(items_pointer!=NULL,FALSE);
 247
 248         g_assert(4==sizeof(value_guint32));
 249         if (elem_data_length_guint32!=sizeof(value_guint32))
 250                 return FALSE;
 251
 252         value_guint32=ntohl(*(guint32 *)elem_data);
 253         *value_pointer=GUINT_TO_POINTER(value_guint32);
 254
 255         return TRUE;
 256 }
 257
 258 static gboolean packet_disassembly_data_guint16
 259                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 260 {
 261 guint16 value_guint16;
 262
 263         g_return_val_if_fail(value_pointer!=NULL,FALSE);
 264         g_return_val_if_fail(elem_data!=NULL,FALSE);
 265         g_return_val_if_fail(items_pointer!=NULL,FALSE);
 266
 267         g_assert(2==sizeof(value_guint16));
 268         if (elem_data_length_guint32!=sizeof(value_guint16))
 269                 return FALSE;
 270
 271         value_guint16=ntohs(*(guint16 *)elem_data);
 272         *value_pointer=GUINT_TO_POINTER((guint)value_guint16);
 273
 274         return TRUE;
 275 }
 276
 277 gboolean packet_recognized(gconstpointer packet,size_t packet_length)
 278 {
 279         if (strlen(PACKET_HEADER)>packet_length)
 280                 return FALSE;
 281         if (memcmp(packet,PACKET_HEADER,strlen(PACKET_HEADER)))
 282                 return FALSE;
 283         return TRUE;
 284 }
 285
 286 GHashTable *packet_disassembly(gconstpointer packet,size_t packet_length)
 287 {
 288 GHashTable *r;
 289 gconstpointer packet_end;
 290 GSList *items;
 291
 292         g_return_val_if_fail(packet!=NULL,NULL);
 293
 294         if (optarg_verbose)
 295                 g_message(_("Decoding packet..."));
 296
 297         r=g_hash_table_new_full(
 298                         g_direct_hash,  /* hash_func */
 299                         g_direct_equal, /* key_equal_func */
 300                         NULL,   /* key_destroy_func */
 301                         packet_disassembly_value_destroy_func); /* value_destroy_func */
 302         packet_end=packet+packet_length;
 303
 304         if (packet+strlen(PACKET_HEADER)>packet_end) {
 305 err_g_hash_table_destroy_r:
 306                 g_hash_table_destroy(r);
 307                 return NULL;
 308                 }
 309         if (memcmp(packet,PACKET_HEADER,strlen(PACKET_HEADER)))
 310                 goto err_g_hash_table_destroy_r;
 311         packet+=strlen(PACKET_HEADER);
 312
 313         items=NULL;
 314         while (packet<packet_end) {
 315 guint32 elem_type_guint32;
 316 guint32 elem_data_length_guint32;
 317 gconstpointer elem_data;
 318 gpointer value;
 319
 320                 if (!packet_disassembly_guint32(&elem_type_guint32,&packet,packet_end)) {
 321 err_packet_disassembly_destroy_value_destroy_func_items:
 322                         /* 'items' are not yet registered in 'packet_disassembly_destroy_registry' */
 323                         packet_disassembly_destroy_value_destroy_func(items);
 324                         goto err_g_hash_table_destroy_r;
 325                         }
 326                 if (!packet_disassembly_guint32(&elem_data_length_guint32,&packet,packet_end))
 327                         goto err_packet_disassembly_destroy_value_destroy_func_items;
 328                 elem_data=packet;
 329                 packet+=elem_data_length_guint32;
 330
 331                 switch (elem_type_guint32) {
 332                 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
 333                         if (!packet_disassembly_data_string(&value,elem_data,elem_data_length_guint32,&items))
 334                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 335                         if (optarg_verbose)
 336                                 g_message(_("Decoded PROGRAM_VERSION: %s"),(const char *)value);
 337                         break;
 338                 case PACKET_ELEM_TYPE_CLIENT_INADDR:
 339                         if (!packet_disassembly_data_guint32(&value,elem_data,elem_data_length_guint32,&items))
 340                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 341                         if (optarg_verbose)
 342                                 g_message(_("Decoded CLIENT_INADDR: %s"),HOSTIP_GUINT32_TO_STRING(GPOINTER_TO_UINT(value)));
 343                         break;
 344                 case PACKET_ELEM_TYPE_DATA_GUINT32:
 345                         if (!packet_disassembly_data_guint32(&value,elem_data,elem_data_length_guint32,&items))
 346                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 347                         if (optarg_verbose)
 348                                 g_message(_("Decoded DATA_GUINT32: 0x%08X"),(unsigned)GPOINTER_TO_UINT(value));
 349                         break;
 350                 case PACKET_ELEM_TYPE_CLIENT_PORT:
 351                         if (!packet_disassembly_data_guint16(&value,elem_data,elem_data_length_guint32,&items))
 352                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 353                         if (optarg_verbose)
 354                                 g_message(_("Decoded CLIENT_PORT: %d"),GPOINTER_TO_UINT(value));
 355                         break;
 356                 default:
 357                         if (elem_type_guint32&PACKET_ELEM_ATTR_MANDATORY) {
 358                                 if (optarg_verbose)
 359                                         g_message(_("Decoding failed on ATTR_MANDATORY ELEM code 0x%08X"),(unsigned)elem_type_guint32);
 360                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 361                                 }
 362                         else {
 363                                 if (optarg_verbose)
 364                                         g_message(_("Decoding ignores ATTR_OPTIONAL ELEM code 0x%08X"),(unsigned)elem_type_guint32);
 365                                 }
 366                         continue;
 367                         }
 368                 g_hash_table_insert(r,GUINT_TO_POINTER(elem_type_guint32),value);
 369                 }
 370         g_assert(packet==packet_end);
 371
 372         packet_disassembly_destroy_registry_init();
 373         g_hash_table_insert(packet_disassembly_destroy_registry,r,items);
 374
 375         if (optarg_verbose)
 376                 g_message(_("Decoding done."));
 377
 378         return r;
 379 }