gobex/gobex-packet.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *
   4  *  OBEX library with GLib integration
   5  *
   6  *  Copyright (C) 2011  Intel Corporation. All rights reserved.
   7  *
   8  */
   9
  10 #ifdef HAVE_CONFIG_H
  11 #include <config.h>
  12 #endif
  13
  14 #include <string.h>
  15 #include <errno.h>
  16
  17 #include "gobex-defs.h"
  18 #include "gobex-packet.h"
  19 #include "gobex-debug.h"
  20
  21 #define FINAL_BIT 0x80
  22
  23 struct _GObexPacket {
  24         guint8 opcode;
  25         gboolean final;
  26
  27         GObexDataPolicy data_policy;
  28
  29         union {
  30                 void *buf;              /* Non-header data */
  31                 const void *buf_ref;    /* Reference to non-header data */
  32         } data;
  33         gsize data_len;
  34
  35         gsize hlen;             /* Length of all encoded headers */
  36         GSList *headers;
  37
  38         GObexDataProducer get_body;
  39         gpointer get_body_data;
  40 };
  41
  42 GObexHeader *g_obex_packet_get_header(GObexPacket *pkt, guint8 id)
  43 {
  44         GSList *l;
  45
  46         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
  47
  48         for (l = pkt->headers; l != NULL; l = g_slist_next(l)) {
  49                 GObexHeader *hdr = l->data;
  50
  51                 if (g_obex_header_get_id(hdr) == id)
  52                         return hdr;
  53         }
  54
  55         return NULL;
  56 }
  57
  58 GObexHeader *g_obex_packet_get_body(GObexPacket *pkt)
  59 {
  60         GObexHeader *body;
  61
  62         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
  63
  64         body = g_obex_packet_get_header(pkt, G_OBEX_HDR_BODY);
  65         if (body != NULL)
  66                 return body;
  67
  68         return g_obex_packet_get_header(pkt, G_OBEX_HDR_BODY_END);
  69 }
  70
  71 guint8 g_obex_packet_get_operation(GObexPacket *pkt, gboolean *final)
  72 {
  73         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
  74
  75         if (final)
  76                 *final = pkt->final;
  77
  78         return pkt->opcode;
  79 }
  80
  81 gboolean g_obex_packet_prepend_header(GObexPacket *pkt, GObexHeader *header)
  82 {
  83         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
  84
  85         pkt->headers = g_slist_prepend(pkt->headers, header);
  86         pkt->hlen += g_obex_header_get_length(header);
  87
  88         return TRUE;
  89 }
  90
  91 gboolean g_obex_packet_add_header(GObexPacket *pkt, GObexHeader *header)
  92 {
  93         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
  94
  95         pkt->headers = g_slist_append(pkt->headers, header);
  96         pkt->hlen += g_obex_header_get_length(header);
  97
  98         return TRUE;
  99 }
 100
 101 gboolean g_obex_packet_add_body(GObexPacket *pkt, GObexDataProducer func,
 102                                                         gpointer user_data)
 103 {
 104         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 105
 106         if (pkt->get_body != NULL)
 107                 return FALSE;
 108
 109         pkt->get_body = func;
 110         pkt->get_body_data = user_data;
 111
 112         return TRUE;
 113 }
 114
 115 gboolean g_obex_packet_add_unicode(GObexPacket *pkt, guint8 id,
 116                                                         const char *str)
 117 {
 118         GObexHeader *hdr;
 119
 120         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 121
 122         hdr = g_obex_header_new_unicode(id, str);
 123         if (hdr == NULL)
 124                 return FALSE;
 125
 126         return g_obex_packet_add_header(pkt, hdr);
 127 }
 128
 129 gboolean g_obex_packet_add_bytes(GObexPacket *pkt, guint8 id,
 130                                                 const void *data, gsize len)
 131 {
 132         GObexHeader *hdr;
 133
 134         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 135
 136         hdr = g_obex_header_new_bytes(id, data, len);
 137         if (hdr == NULL)
 138                 return FALSE;
 139
 140         return g_obex_packet_add_header(pkt, hdr);
 141 }
 142
 143 gboolean g_obex_packet_add_uint8(GObexPacket *pkt, guint8 id, guint8 val)
 144 {
 145         GObexHeader *hdr;
 146
 147         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 148
 149         hdr = g_obex_header_new_uint8(id, val);
 150         if (hdr == NULL)
 151                 return FALSE;
 152
 153         return g_obex_packet_add_header(pkt, hdr);
 154 }
 155
 156 gboolean g_obex_packet_add_uint32(GObexPacket *pkt, guint8 id, guint32 val)
 157 {
 158         GObexHeader *hdr;
 159
 160         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 161
 162         hdr = g_obex_header_new_uint32(id, val);
 163         if (hdr == NULL)
 164                 return FALSE;
 165
 166         return g_obex_packet_add_header(pkt, hdr);
 167 }
 168
 169 const void *g_obex_packet_get_data(GObexPacket *pkt, gsize *len)
 170 {
 171         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 172
 173         if (pkt->data_len == 0) {
 174                 *len = 0;
 175                 return NULL;
 176         }
 177
 178         *len = pkt->data_len;
 179
 180         switch (pkt->data_policy) {
 181         case G_OBEX_DATA_INHERIT:
 182         case G_OBEX_DATA_COPY:
 183                 return pkt->data.buf;
 184         case G_OBEX_DATA_REF:
 185                 return pkt->data.buf_ref;
 186         }
 187
 188         g_assert_not_reached();
 189 }
 190
 191 gboolean g_obex_packet_set_data(GObexPacket *pkt, const void *data, gsize len,
 192                                                 GObexDataPolicy data_policy)
 193 {
 194         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 195
 196         if (pkt->data.buf || pkt->data.buf_ref)
 197                 return FALSE;
 198
 199         pkt->data_policy = data_policy;
 200         pkt->data_len = len;
 201
 202         switch (data_policy) {
 203         case G_OBEX_DATA_COPY:
 204                 pkt->data.buf = g_memdup(data, len);
 205                 break;
 206         case G_OBEX_DATA_REF:
 207                 pkt->data.buf_ref = data;
 208                 break;
 209         case G_OBEX_DATA_INHERIT:
 210                 pkt->data.buf = (void *) data;
 211                 break;
 212         }
 213
 214         return TRUE;
 215 }
 216
 217 GObexPacket *g_obex_packet_new_valist(guint8 opcode, gboolean final,
 218                                         guint first_hdr_id, va_list args)
 219 {
 220         GObexPacket *pkt;
 221
 222         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", opcode);
 223
 224         pkt = g_new0(GObexPacket, 1);
 225
 226         pkt->opcode = opcode;
 227         pkt->final = final;
 228         pkt->headers = g_obex_header_create_list(first_hdr_id, args,
 229                                                                 &pkt->hlen);
 230         pkt->data_policy = G_OBEX_DATA_COPY;
 231
 232         return pkt;
 233 }
 234
 235 GObexPacket *g_obex_packet_new(guint8 opcode, gboolean final,
 236                                                 guint first_hdr_id, ...)
 237 {
 238         GObexPacket *pkt;
 239         va_list args;
 240
 241         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", opcode);
 242
 243         va_start(args, first_hdr_id);
 244         pkt = g_obex_packet_new_valist(opcode, final, first_hdr_id, args);
 245         va_end(args);
 246
 247         return pkt;
 248 }
 249
 250 static void header_free(void *data, void *user_data)
 251 {
 252         g_obex_header_free(data);
 253 }
 254
 255 void g_obex_packet_free(GObexPacket *pkt)
 256 {
 257         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 258
 259         switch (pkt->data_policy) {
 260         case G_OBEX_DATA_INHERIT:
 261         case G_OBEX_DATA_COPY:
 262                 g_free(pkt->data.buf);
 263                 break;
 264         case G_OBEX_DATA_REF:
 265                 break;
 266         }
 267
 268         g_slist_foreach(pkt->headers, header_free, NULL);
 269         g_slist_free(pkt->headers);
 270         g_free(pkt);
 271 }
 272
 273 static gboolean parse_headers(GObexPacket *pkt, const void *data, gsize len,
 274                                                 GObexDataPolicy data_policy,
 275                                                 GError **err)
 276 {
 277         const guint8 *buf = data;
 278
 279         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 280
 281         while (len > 0) {
 282                 GObexHeader *header;
 283                 gsize parsed;
 284
 285                 header = g_obex_header_decode(buf, len, data_policy, &parsed,
 286                                                                         err);
 287                 if (header == NULL)
 288                         return FALSE;
 289
 290                 pkt->headers = g_slist_append(pkt->headers, header);
 291                 pkt->hlen += parsed;
 292
 293                 len -= parsed;
 294                 buf += parsed;
 295         }
 296
 297         return TRUE;
 298 }
 299
 300 static const guint8 *get_bytes(void *to, const guint8 *from, gsize count)
 301 {
 302         memcpy(to, from, count);
 303         return (from + count);
 304 }
 305
 306 GObexPacket *g_obex_packet_decode(const void *data, gsize len,
 307                                                 gsize header_offset,
 308                                                 GObexDataPolicy data_policy,
 309                                                 GError **err)
 310 {
 311         const guint8 *buf = data;
 312         guint16 packet_len;
 313         guint8 opcode;
 314         GObexPacket *pkt;
 315         gboolean final;
 316
 317         g_obex_debug(G_OBEX_DEBUG_PACKET, "");
 318
 319         if (data_policy == G_OBEX_DATA_INHERIT) {
 320                 if (!err)
 321                         return NULL;
 322                 g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_INVALID_ARGS,
 323                                                         "Invalid data policy");
 324                 g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message);
 325                 return NULL;
 326         }
 327
 328         if (len < 3 + header_offset) {
 329                 if (!err)
 330                         return NULL;
 331                 g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_PARSE_ERROR,
 332                                         "Not enough data to decode packet");
 333                 g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message);
 334                 return NULL;
 335         }
 336
 337         buf = get_bytes(&opcode, buf, sizeof(opcode));
 338         buf = get_bytes(&packet_len, buf, sizeof(packet_len));
 339
 340         packet_len = g_ntohs(packet_len);
 341         if (packet_len != len) {
 342                 if (!err)
 343                         return NULL;
 344                 g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_PARSE_ERROR,
 345                                 "Incorrect packet length (%u != %zu)",
 346                                 packet_len, len);
 347                 g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message);
 348                 return NULL;
 349         }
 350
 351         final = (opcode & FINAL_BIT) ? TRUE : FALSE;
 352         opcode &= ~FINAL_BIT;
 353
 354         pkt = g_obex_packet_new(opcode, final, G_OBEX_HDR_INVALID);
 355
 356         if (header_offset == 0)
 357                 goto headers;
 358
 359         g_obex_packet_set_data(pkt, buf, header_offset, data_policy);
 360         buf += header_offset;
 361
 362 headers:
 363         if (!parse_headers(pkt, buf, len - (3 + header_offset),
 364                                                         data_policy, err))
 365                 goto failed;
 366
 367         return pkt;
 368
 369 failed:
 370         g_obex_packet_free(pkt);
 371         return NULL;
 372 }
 373
 374 static gssize get_body(GObexPacket *pkt, guint8 *buf, gsize len)
 375 {
 376         guint16 u16;
 377         gssize ret;
 378
 379         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 380
 381         if (len < 3)
 382                 return -ENOBUFS;
 383
 384         ret = pkt->get_body(buf + 3, len - 3, pkt->get_body_data);
 385         if (ret < 0)
 386                 return ret;
 387
 388         if (ret > 0)
 389                 buf[0] = G_OBEX_HDR_BODY;
 390         else
 391                 buf[0] = G_OBEX_HDR_BODY_END;
 392
 393         u16 = g_htons(ret + 3);
 394         memcpy(&buf[1], &u16, sizeof(u16));
 395
 396         return ret;
 397 }
 398
 399 gssize g_obex_packet_encode(GObexPacket *pkt, guint8 *buf, gsize len)
 400 {
 401         gssize ret;
 402         gsize count;
 403         guint16 u16;
 404         GSList *l;
 405
 406         g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode);
 407
 408         if (3 + pkt->data_len + pkt->hlen > len)
 409                 return -ENOBUFS;
 410
 411         buf[0] = pkt->opcode;
 412         if (pkt->final)
 413                 buf[0] |= FINAL_BIT;
 414
 415         if (pkt->data_len > 0) {
 416                 if (pkt->data_policy == G_OBEX_DATA_REF)
 417                         memcpy(&buf[3], pkt->data.buf_ref, pkt->data_len);
 418                 else
 419                         memcpy(&buf[3], pkt->data.buf, pkt->data_len);
 420         }
 421
 422         count = 3 + pkt->data_len;
 423
 424         for (l = pkt->headers; l != NULL; l = g_slist_next(l)) {
 425                 GObexHeader *hdr = l->data;
 426
 427                 if (count >= len)
 428                         return -ENOBUFS;
 429
 430                 ret = g_obex_header_encode(hdr, buf + count, len - count);
 431                 if (ret < 0)
 432                         return ret;
 433
 434                 count += ret;
 435         }
 436
 437         if (pkt->get_body) {
 438                 ret = get_body(pkt, buf + count, len - count);
 439                 if (ret < 0)
 440                         return ret;
 441                 if (ret == 0) {
 442                         if (pkt->opcode == G_OBEX_RSP_CONTINUE)
 443                                 buf[0] = G_OBEX_RSP_SUCCESS;
 444                         buf[0] |= FINAL_BIT;
 445                 }
 446
 447                 count += ret + 3;
 448         }
 449
 450         u16 = g_htons(count);
 451         memcpy(&buf[1], &u16, sizeof(u16));
 452
 453         return count;
 454 }