stun.c 7.33 KB
Newer Older
Olivier Crête's avatar
import  
Olivier Crête committed
1

Dafydd Harries's avatar
Dafydd Harries committed
2
#include <string.h>
Olivier Crête's avatar
import  
Olivier Crête committed
3
4

#include <arpa/inet.h>
Dafydd Harries's avatar
Dafydd Harries committed
5
6

#include "stun.h"
Olivier Crête's avatar
import  
Olivier Crête committed
7

8
9
10
11
12
13
14
15
16
17
18
/* round up to multiple of 4 */
G_GNUC_CONST
static guint
ceil4 (guint n)
{
  if (n % 4 == 0)
    return n;
  else
    return n + 4 - (n % 4);
}

19
G_GNUC_WARN_UNUSED_RESULT
Olivier Crête's avatar
import  
Olivier Crête committed
20
static StunAttribute *
21
stun_attribute_new (guint type)
Olivier Crête's avatar
import  
Olivier Crête committed
22
{
23
  StunAttribute *attr = g_slice_new0 (StunAttribute);
Olivier Crête's avatar
import  
Olivier Crête committed
24
25
26
27
28
29

  attr->type = type;
  return attr;
}

StunAttribute *
30
stun_attribute_mapped_address_new (guint32 ip, guint16 port)
Olivier Crête's avatar
import  
Olivier Crête committed
31
{
32
  StunAttribute *attr = stun_attribute_new (STUN_ATTRIBUTE_MAPPED_ADDRESS);
Olivier Crête's avatar
import  
Olivier Crête committed
33
34
35
36
37
38
39
40
41

  attr->length = 8;
  attr->address.padding = 0;
  attr->address.af = 1;
  attr->address.ip = ip;
  attr->address.port = port;
  return attr;
}

Dafydd Harries's avatar
Dafydd Harries committed
42
43
44
45
46
47
48
49
50
51
52
53
StunAttribute *
stun_attribute_username_new (const gchar *username)
{
  StunAttribute *attr;

  attr = stun_attribute_new (STUN_ATTRIBUTE_USERNAME);
  g_assert (strlen (username) < sizeof (attr->username));
  attr->length = strlen (username);
  strcpy (attr->username, username);
  return attr;
}

Olivier Crête's avatar
import  
Olivier Crête committed
54
void
55
stun_attribute_free (StunAttribute *attr)
Olivier Crête's avatar
import  
Olivier Crête committed
56
{
57
  g_slice_free (StunAttribute, attr);
Olivier Crête's avatar
import  
Olivier Crête committed
58
59
}

60
G_GNUC_WARN_UNUSED_RESULT
61
static gboolean
62
_stun_attribute_unpack (StunAttribute *attr, guint length, const gchar *s)
Olivier Crête's avatar
import  
Olivier Crête committed
63
{
64
  guint type;
Olivier Crête's avatar
import  
Olivier Crête committed
65

66
67
68
69
70
71
72
  if (length < 4)
    /* must start with 16 bit type, 16 bit length */
    return FALSE;

  type = ntohs (*(guint16 *) s);

  switch (type)
73
74
    {
      case STUN_ATTRIBUTE_MAPPED_ADDRESS:
75
76
77
        if (length != 12)
          return FALSE;

78
79
80
81
82
        attr->address.af = (guint8) s[5];
        g_assert (attr->address.af == 1);
        attr->address.port = ntohs (*(guint16 *)(s + 6));
        attr->address.ip = ntohl (*(guint32 *)(s + 8));
        break;
83

84
85
86
87
88
89
90
91
92
93
94
95
96
      case STUN_ATTRIBUTE_USERNAME:
      case STUN_ATTRIBUTE_PASSWORD:
        if (length - 4 > sizeof (attr->username) / sizeof (gchar))
          return FALSE;

        attr->length = length - 4;

        if (type == STUN_ATTRIBUTE_USERNAME)
          memcpy (attr->username, s + 4, attr->length);
        else
          memcpy (attr->password, s + 4, attr->length);
        break;

97
      default:
Dafydd Harries's avatar
Dafydd Harries committed
98
        /* unknown attribute; we can only unpack the type */
99
100
        break;
    }
101

102
  attr->type = type;
103
  return TRUE;
104
105
106
107
108
109
110
111
}

StunAttribute *
stun_attribute_unpack (guint length, const gchar *s)
{
  StunAttribute *attr;

  attr = stun_attribute_new (0);
Olivier Crête's avatar
import  
Olivier Crête committed
112

113
114
  if (_stun_attribute_unpack (attr, length, s))
    return attr;
115
116
117

  stun_attribute_free (attr);
  return NULL;
Olivier Crête's avatar
import  
Olivier Crête committed
118
119
120
}

guint
121
stun_attribute_pack (StunAttribute *attr, gchar **packed)
Olivier Crête's avatar
import  
Olivier Crête committed
122
{
123
124
125
126
  switch (attr->type)
    {
      case STUN_ATTRIBUTE_MAPPED_ADDRESS:
        {
127
128
129
130
131
132
133
134
135
136
137
138
          if (packed != NULL)
            {
              StunAttribute *ret = g_malloc0 (sizeof (StunAttribute));

              ret->type = htons (attr->type);
              ret->length = htons (8);
              ret->address.af = attr->address.af;
              ret->address.port = htons (attr->address.port);
              ret->address.ip = htonl (attr->address.ip);
              *packed = (gchar *) ret;
            }

139
140
          return 12;
        }
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156

      case STUN_ATTRIBUTE_USERNAME:
        {
          if (packed != NULL)
            {
              StunAttribute *ret = g_malloc0 (sizeof (StunAttribute));

              ret->type = htons (attr->type);
              ret->length = htons (attr->length);
              memcpy (ret->username, attr->username, attr->length);
              *packed = (gchar *) ret;
            }

          return ceil4 (4 + attr->length);
        }

157
158
      default:
        return 0;
Olivier Crête's avatar
import  
Olivier Crête committed
159
160
161
162
  }
}

gchar *
163
stun_attribute_dump (StunAttribute *attr)
Olivier Crête's avatar
import  
Olivier Crête committed
164
{
165
166
167
168
169
170
171
172
173
174
  switch (attr->type)
    {
      case STUN_ATTRIBUTE_MAPPED_ADDRESS:
        return g_strdup_printf (
          "MAPPED-ADDRESS %d.%d.%d.%d:%d",
            (attr->address.ip & 0xff000000) >> 24,
            (attr->address.ip & 0x00ff0000) >> 16,
            (attr->address.ip & 0x0000ff00) >>  8,
            (attr->address.ip & 0x000000ff) >>  0,
            attr->address.port);
175
176
177
      case STUN_ATTRIBUTE_USERNAME:
        return g_strdup_printf (
          "USERNAME \"%*s\"", attr->length, attr->username);
178
179
180
      default:
        return g_strdup_printf ("UNKNOWN (%d)", attr->type);
    }
Olivier Crête's avatar
import  
Olivier Crête committed
181
182
}

183
void
184
stun_message_init (StunMessage *msg, guint type, gchar *id)
185
186
{
  msg->type = type;
187
188
189

  if (id != NULL)
    memcpy (msg->transaction_id, id, 16);
190
191
}

192
StunMessage *
193
stun_message_new (guint type, gchar *id, guint n_attributes)
Olivier Crête's avatar
import  
Olivier Crête committed
194
{
195
  StunMessage *msg = g_slice_new0 (StunMessage);
Olivier Crête's avatar
import  
Olivier Crête committed
196

197
  stun_message_init (msg, type, id);
198
199
200
201
202

  if (n_attributes != 0)
    msg->attributes = g_malloc0 (
        (n_attributes + 1) * sizeof (StunAttribute *));

Olivier Crête's avatar
import  
Olivier Crête committed
203
204
205
206
  return msg;
}

void
207
stun_message_free (StunMessage *msg)
Olivier Crête's avatar
import  
Olivier Crête committed
208
209
210
{
  StunAttribute **attr;

211
212
213
214
  if (msg->attributes)
    {
      for (attr = msg->attributes; *attr; attr++)
        stun_attribute_free (*attr);
Olivier Crête's avatar
import  
Olivier Crête committed
215

216
217
      g_free (msg->attributes);
    }
Olivier Crête's avatar
import  
Olivier Crête committed
218

219
  g_slice_free (StunMessage, msg);
Olivier Crête's avatar
import  
Olivier Crête committed
220
221
222
}

StunMessage *
223
stun_message_unpack (guint length, gchar *s)
Olivier Crête's avatar
import  
Olivier Crête committed
224
225
226
227
{
  guint attr_length;
  guint n_attributes = 0;
  guint i;
228
  guint offset;
Olivier Crête's avatar
import  
Olivier Crête committed
229
  StunAttribute *attr;
230
  StunMessage *msg;
Olivier Crête's avatar
import  
Olivier Crête committed
231
232
233

  /* message header is 20 bytes */

234
  g_assert (length >= 20);
Olivier Crête's avatar
import  
Olivier Crête committed
235
236
237

  /* count the number of attributes */

238
  for (offset = 20; offset < length; offset += attr_length)
239
    {
240
      attr_length = ceil4 (4 + ntohs (*(guint16 *)(s + offset + 2)));
241
242
      n_attributes++;
    }
Olivier Crête's avatar
import  
Olivier Crête committed
243

244
  /* create message structure */
Olivier Crête's avatar
import  
Olivier Crête committed
245

246
  msg = stun_message_new (ntohs (*(guint16 *) s), s + 4, n_attributes);
Olivier Crête's avatar
import  
Olivier Crête committed
247
248
249

  /* unpack attributes */

250
  for (i = 0, offset = 20; i < n_attributes; i++, offset += attr_length)
251
    {
252
      attr_length = 4 + ntohs (*(guint16 *)(s + offset + 2));
253
254
      attr = msg->attributes[i] = stun_attribute_unpack (attr_length,
          s + offset);
255
      attr_length = ceil4 (attr_length);
256
    }
Olivier Crête's avatar
import  
Olivier Crête committed
257
258
259
260
261

  return msg;
}

guint
262
stun_message_pack (StunMessage *msg, gchar **packed)
Olivier Crête's avatar
import  
Olivier Crête committed
263
{
264
  gchar *tmp;
265
  guint length = 0;
Olivier Crête's avatar
import  
Olivier Crête committed
266

267
268
269
  if (msg->attributes)
    {
      StunAttribute **attr;
Olivier Crête's avatar
import  
Olivier Crête committed
270

271
      for (attr = msg->attributes; *attr; attr++)
272
        length += stun_attribute_pack (*attr, NULL);
273
    }
Olivier Crête's avatar
import  
Olivier Crête committed
274

275
  g_assert (length % 4 == 0);
276
277
278
279
  tmp = g_malloc0 (length + 20);
  *(guint16 *) (tmp + 0) = htons (msg->type);
  *(guint16 *) (tmp + 2) = htons (length);
  memcpy (tmp + 4, msg->transaction_id, 16);
Olivier Crête's avatar
import  
Olivier Crête committed
280

281
282
283
  if (msg->attributes)
    {
      StunAttribute **attr;
284
      gchar *pos = tmp + 20;
285
286
287
288
289

      for (attr = msg->attributes; *attr; attr++)
        {
          gchar *attr_packed;
          guint attr_length = stun_attribute_pack (*attr, &attr_packed);
290
          memcpy (pos, attr_packed, attr_length);
291
          g_free (attr_packed);
292
          pos += attr_length;
293
        }
Olivier Crête's avatar
import  
Olivier Crête committed
294
295
    }

296
  *packed = tmp;
297
  return length + 20;
Olivier Crête's avatar
import  
Olivier Crête committed
298
299
300
301
302
303
}

gchar *
stun_message_dump (StunMessage *msg)
{
  StunAttribute **attr;
304
  GString *tmp = g_string_new ("");
Olivier Crête's avatar
import  
Olivier Crête committed
305
306
307
308
309
310
  const gchar *name;

  switch (msg->type) {
    case STUN_MESSAGE_BINDING_REQUEST:
      name = "BINDING-REQUEST";
      break;
311
312
313
    case STUN_MESSAGE_BINDING_RESPONSE:
      name = "BINDING-RESPONSE";
      break;
Olivier Crête's avatar
import  
Olivier Crête committed
314
    default:
315
      name = "(UNKNOWN)";
Olivier Crête's avatar
import  
Olivier Crête committed
316
317
  }

318
  g_string_printf (tmp,
319
    "%s %08x:%08x:%08x:%08x\n",
Olivier Crête's avatar
import  
Olivier Crête committed
320
      name,
321
322
323
324
      *(guint32 *)(msg->transaction_id),
      *(guint32 *)(msg->transaction_id + 4),
      *(guint32 *)(msg->transaction_id + 8),
      *(guint32 *)(msg->transaction_id + 12));
Olivier Crête's avatar
import  
Olivier Crête committed
325

326
327
328
329
  if (msg->attributes)
    for (attr = msg->attributes; *attr; attr++)
      {
          gchar *dump = stun_attribute_dump (*attr);
330
          g_string_append_printf (tmp, "  %s\n", dump);
331
332
          g_free (dump);
      }
Olivier Crête's avatar
import  
Olivier Crête committed
333

334
  return g_string_free (tmp, FALSE);
Olivier Crête's avatar
import  
Olivier Crête committed
335
336
}