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
  31 static void packet_assembly_guint32(GString *gstring,guint32 value_guint32)
  32 {
  33 guint32 value_guint32_htonl;
  34
  35         g_return_if_fail(gstring!=NULL);
  36
  37         value_guint32_htonl=htonl(value_guint32);
  38         g_assert(4==sizeof(value_guint32_htonl));
  39         g_string_append_len(gstring,(const gchar *)&value_guint32_htonl,sizeof(value_guint32_htonl));
  40 }
  41
  42 static void packet_assembly_guint16(GString *gstring,guint16 value_guint16)
  43 {
  44 guint16 value_guint16_htonl;
  45
  46         g_return_if_fail(gstring!=NULL);
  47
  48         value_guint16_htonl=htons(value_guint16);
  49         g_assert(2==sizeof(value_guint16_htonl));
  50         g_string_append_len(gstring,(const gchar *)&value_guint16_htonl,sizeof(value_guint16_htonl));
  51 }
  52
  53 static void packet_assembly_data_guint32(GString *gstring,guint32 value_guint32)
  54 {
  55 guint32 value_guint32_htonl=htonl(value_guint32);
  56
  57         g_return_if_fail(gstring!=NULL);
  58
  59         g_assert(4==sizeof(value_guint32_htonl));
  60         packet_assembly_guint32(gstring,sizeof(value_guint32_htonl));
  61         packet_assembly_guint32(gstring,value_guint32_htonl);
  62 }
  63
  64 static void packet_assembly_data_guint16(GString *gstring,guint16 value_guint16)
  65 {
  66 guint16 value_guint16_htonl=htons(value_guint16);
  67
  68         g_return_if_fail(gstring!=NULL);
  69
  70         g_assert(2==sizeof(value_guint16_htonl));
  71         packet_assembly_guint32(gstring,sizeof(value_guint16_htonl));
  72         packet_assembly_guint16(gstring,value_guint16_htonl);
  73 }
  74
  75 static void packet_assembly_data_string(GString *gstring,const gchar *string)
  76 {
  77 size_t string_length;
  78 #ifndef G_DISABLE_ASSERT
  79 size_t gstring_length_orig;
  80 #endif  /* !G_DISABLE_ASSERT */
  81
  82         g_return_if_fail(gstring!=NULL);
  83         g_return_if_fail(string!=NULL);
  84
  85         string_length=strlen(string);
  86         packet_assembly_guint32(gstring,string_length);
  87 #ifndef G_DISABLE_ASSERT
  88         gstring_length_orig=gstring->len;
  89 #endif  /* !G_DISABLE_ASSERT */
  90         g_string_append(gstring,string);
  91 #ifndef G_DISABLE_ASSERT
  92         g_assert(gstring_length_orig+string_length==gstring->len);
  93 #endif  /* !G_DISABLE_ASSERT */
  94 }
  95
  96 static void packet_assembly_foreach(gpointer elem_type_gpointer,gpointer value,GString *gstring /* user_data */)        /* GHFunc */
  97 {
  98 guint32 elem_type_uint32;
  99
 100         g_return_if_fail(gstring!=NULL);
 101
 102         elem_type_uint32=GPOINTER_TO_UINT(elem_type_gpointer);
 103         packet_assembly_guint32(gstring,elem_type_uint32);
 104
 105         switch (elem_type_uint32) {
 106                 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
 107                         packet_assembly_data_string(gstring,value);
 108                         break;
 109                 case PACKET_ELEM_TYPE_CLIENT_INADDR:
 110                 case PACKET_ELEM_TYPE_DATA_GUINT32:
 111                         packet_assembly_data_guint32(gstring,GPOINTER_TO_UINT(value));
 112                         break;
 113                 case PACKET_ELEM_TYPE_CLIENT_PORT:
 114                         packet_assembly_data_guint16(gstring,GPOINTER_TO_UINT(value));
 115                         break;
 116                 default:
 117                         g_assert_not_reached();
 118                 }
 119 }
 120
 121 void *packet_assembly(size_t *packet_length_pointer,GHashTable *values)
 122 {
 123 GString *gstring;
 124
 125         g_return_val_if_fail(packet_length_pointer!=NULL,NULL);
 126         g_return_val_if_fail(values!=NULL,NULL);
 127
 128         gstring=g_string_new(NULL);
 129
 130         g_string_append(gstring,PACKET_HEADER);
 131
 132         packet_assembly_foreach(GINT_TO_POINTER(PACKET_ELEM_TYPE_PROGRAM_VERSION),VERSION,gstring);
 133         g_hash_table_foreach(
 134                         values, /* hash_table */
 135                         (GHFunc)packet_assembly_foreach,        /* func */
 136                         gstring);       /* user_data */
 137
 138         *packet_length_pointer=gstring->len;
 139         return g_string_free(gstring,
 140                         FALSE); /* free_segment */
 141 }
 142
 143 static gboolean packet_disassembly_guint32
 144                 (guint32 *elem_type_guint32_pointer,gconstpointer *packet_pointer,gconstpointer packet_end)
 145 {
 146         g_return_val_if_fail(elem_type_guint32_pointer!=NULL,FALSE);
 147         g_return_val_if_fail(packet_pointer!=NULL,FALSE);
 148         g_return_val_if_fail(packet_end!=NULL,FALSE);
 149         g_return_val_if_fail(*packet_pointer<=packet_end,FALSE);
 150
 151         if ((*packet_pointer)+sizeof(*elem_type_guint32_pointer)>packet_end)
 152                 return FALSE;
 153
 154         *elem_type_guint32_pointer=ntohl(*((guint32 *)*packet_pointer));
 155         (*packet_pointer)+=sizeof(*elem_type_guint32_pointer);
 156         return TRUE;
 157 }
 158
 159 static void packet_disassembly_value_destroy_func(gpointer data)        /* GDestroyNotify */
 160 {
 161
 162 }
 163
 164 /* map: (GHashTable *) -> (GSList *) */
 165 static GHashTable *packet_disassembly_destroy_registry;
 166
 167 static void packet_disassembly_destroy_key_destroy_func(GHashTable *hash) /* GDestroyNotify */
 168 {
 169         g_return_if_fail(hash!=NULL);
 170
 171         g_hash_table_destroy(hash);
 172 }
 173
 174 static void packet_disassembly_destroy_value_destroy_func(GSList *items)        /* GDestroyNotify */
 175 {
 176         /* 'items' may be NULL */
 177
 178         g_slist_foreach(
 179                         items,  /* list */
 180                         (GFunc)g_free,  /* func */
 181                         NULL);  /* user_data */
 182 }
 183
 184 static void packet_disassembly_destroy_registry_init(void)
 185 {
 186         if (!packet_disassembly_destroy_registry) {
 187                 packet_disassembly_destroy_registry=g_hash_table_new_full(
 188                         g_direct_hash,  /* hash_func */
 189                         g_direct_equal, /* key_equal_func */
 190                         (GDestroyNotify)packet_disassembly_destroy_key_destroy_func,    /* key_destroy_func */
 191                         (GDestroyNotify)packet_disassembly_destroy_value_destroy_func); /* value_destroy_func */
 192                 }
 193 }
 194
 195 void packet_disassembly_destroy(GHashTable *hash)
 196 {
 197 gboolean errgboolean;
 198
 199         g_return_if_fail(hash!=NULL);
 200
 201         packet_disassembly_destroy_registry_init();
 202
 203         errgboolean=g_hash_table_remove(packet_disassembly_destroy_registry,hash);
 204         g_assert(errgboolean==TRUE);
 205 }
 206
 207 static gboolean packet_disassembly_data_string
 208                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 209 {
 210 gchar *string;
 211
 212         g_return_val_if_fail(value_pointer!=NULL,FALSE);
 213         g_return_val_if_fail(elem_data!=NULL,FALSE);
 214         g_return_val_if_fail(items_pointer!=NULL,FALSE);
 215
 216         udpgate_newn(string,elem_data_length_guint32+1);
 217         *items_pointer=g_slist_prepend(*items_pointer,string);
 218         memcpy(string,elem_data,elem_data_length_guint32);
 219         string[elem_data_length_guint32]='\0';
 220         *value_pointer=string;
 221
 222         return TRUE;
 223 }
 224
 225 static gboolean packet_disassembly_data_guint32
 226                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 227 {
 228 guint32 value_guint32;
 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         g_assert(4==sizeof(value_guint32));
 235         if (elem_data_length_guint32!=sizeof(value_guint32))
 236                 return FALSE;
 237
 238         value_guint32=ntohl(*(guint32 *)elem_data);
 239         *value_pointer=GUINT_TO_POINTER(value_guint32);
 240
 241         return TRUE;
 242 }
 243
 244 static gboolean packet_disassembly_data_guint16
 245                 (gpointer *value_pointer,gconstpointer elem_data,guint32 elem_data_length_guint32,GSList **items_pointer)
 246 {
 247 guint16 value_guint16;
 248
 249         g_return_val_if_fail(value_pointer!=NULL,FALSE);
 250         g_return_val_if_fail(elem_data!=NULL,FALSE);
 251         g_return_val_if_fail(items_pointer!=NULL,FALSE);
 252
 253         g_assert(2==sizeof(value_guint16));
 254         if (elem_data_length_guint32!=sizeof(value_guint16))
 255                 return FALSE;
 256
 257         value_guint16=ntohs(*(guint16 *)elem_data);
 258         *value_pointer=GUINT_TO_POINTER((guint)value_guint16);
 259
 260         return TRUE;
 261 }
 262
 263 GHashTable *packet_disassembly(gconstpointer packet,size_t packet_length)
 264 {
 265 GHashTable *r;
 266 gconstpointer packet_end;
 267 GSList *items;
 268
 269         g_return_val_if_fail(packet!=NULL,NULL);
 270
 271         r=g_hash_table_new_full(
 272                         g_direct_hash,  /* hash_func */
 273                         g_direct_equal, /* key_equal_func */
 274                         NULL,   /* key_destroy_func */
 275                         packet_disassembly_value_destroy_func); /* value_destroy_func */
 276         packet_end=packet+packet_length;
 277
 278         if (packet+strlen(PACKET_HEADER)>packet_end) {
 279 err_g_hash_table_destroy_r:
 280                 g_hash_table_destroy(r);
 281                 return NULL;
 282                 }
 283         if (memcmp(packet,PACKET_HEADER,strlen(PACKET_HEADER)))
 284                 goto err_g_hash_table_destroy_r;
 285         packet+=strlen(PACKET_HEADER);
 286
 287         items=NULL;
 288         while (packet<packet_end) {
 289 guint32 elem_type_guint32;
 290 guint32 elem_data_length_guint32;
 291 gconstpointer elem_data;
 292 gpointer value;
 293
 294                 if (!packet_disassembly_guint32(&elem_type_guint32,&packet,packet_end)) {
 295 err_packet_disassembly_destroy_value_destroy_func_items:
 296                         /* 'items' are not yet registered in 'packet_disassembly_destroy_registry' */
 297                         packet_disassembly_destroy_value_destroy_func(items);
 298                         goto err_g_hash_table_destroy_r;
 299                         }
 300                 if (!packet_disassembly_guint32(&elem_data_length_guint32,&packet,packet_end))
 301                         goto err_packet_disassembly_destroy_value_destroy_func_items;
 302                 elem_data=packet;
 303                 packet+=elem_data_length_guint32;
 304
 305                 switch (elem_type_guint32) {
 306                 case PACKET_ELEM_TYPE_PROGRAM_VERSION:
 307                         if (!packet_disassembly_data_string(&value,elem_data,elem_data_length_guint32,&items))
 308                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 309                         break;
 310                 case PACKET_ELEM_TYPE_CLIENT_INADDR:
 311                 case PACKET_ELEM_TYPE_DATA_GUINT32:
 312                         if (!packet_disassembly_data_guint32(&value,elem_data,elem_data_length_guint32,&items))
 313                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 314                         break;
 315                 case PACKET_ELEM_TYPE_CLIENT_PORT:
 316                         if (!packet_disassembly_data_guint16(&value,elem_data,elem_data_length_guint32,&items))
 317                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 318                         break;
 319                 default:
 320                         if (elem_type_guint32&PACKET_ELEM_ATTR_MANDATORY)
 321                                 goto err_packet_disassembly_destroy_value_destroy_func_items;
 322                         else /* ignore the PACKET_ELEM_ATTR_OPTIONAL packet */;
 323                         continue;
 324                         }
 325                 g_hash_table_insert(r,GUINT_TO_POINTER(elem_type_guint32),value);
 326                 }
 327         g_assert(packet==packet_end);
 328
 329         packet_disassembly_destroy_registry_init();
 330         g_hash_table_insert(packet_disassembly_destroy_registry,r,items);
 331
 332         return r;
 333 }