| /* |
| * |
| * OBEX library with GLib integration |
| * |
| * Copyright (C) 2011 Intel Corporation. All rights reserved. |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License as published by |
| * the Free Software Foundation; either version 2 of the License, or |
| * (at your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU General Public License for more details. |
| * |
| * You should have received a copy of the GNU General Public License |
| * along with this program; if not, write to the Free Software |
| * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
| * |
| */ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif |
| |
| #include <string.h> |
| #include <errno.h> |
| |
| #include "gobex-defs.h" |
| #include "gobex-packet.h" |
| #include "gobex-debug.h" |
| |
| #define FINAL_BIT 0x80 |
| |
| struct _GObexPacket { |
| guint8 opcode; |
| gboolean final; |
| |
| GObexDataPolicy data_policy; |
| |
| union { |
| void *buf; /* Non-header data */ |
| const void *buf_ref; /* Reference to non-header data */ |
| } data; |
| gsize data_len; |
| |
| gsize hlen; /* Length of all encoded headers */ |
| GSList *headers; |
| |
| GObexDataProducer get_body; |
| gpointer get_body_data; |
| }; |
| |
| GObexHeader *g_obex_packet_get_header(GObexPacket *pkt, guint8 id) |
| { |
| GSList *l; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| for (l = pkt->headers; l != NULL; l = g_slist_next(l)) { |
| GObexHeader *hdr = l->data; |
| |
| if (g_obex_header_get_id(hdr) == id) |
| return hdr; |
| } |
| |
| return NULL; |
| } |
| |
| GObexHeader *g_obex_packet_get_body(GObexPacket *pkt) |
| { |
| GObexHeader *body; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| body = g_obex_packet_get_header(pkt, G_OBEX_HDR_BODY); |
| if (body != NULL) |
| return body; |
| |
| return g_obex_packet_get_header(pkt, G_OBEX_HDR_BODY_END); |
| } |
| |
| guint8 g_obex_packet_get_operation(GObexPacket *pkt, gboolean *final) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (final) |
| *final = pkt->final; |
| |
| return pkt->opcode; |
| } |
| |
| gboolean g_obex_packet_prepend_header(GObexPacket *pkt, GObexHeader *header) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| pkt->headers = g_slist_prepend(pkt->headers, header); |
| pkt->hlen += g_obex_header_get_length(header); |
| |
| return TRUE; |
| } |
| |
| gboolean g_obex_packet_add_header(GObexPacket *pkt, GObexHeader *header) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| pkt->headers = g_slist_append(pkt->headers, header); |
| pkt->hlen += g_obex_header_get_length(header); |
| |
| return TRUE; |
| } |
| |
| gboolean g_obex_packet_add_body(GObexPacket *pkt, GObexDataProducer func, |
| gpointer user_data) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (pkt->get_body != NULL) |
| return FALSE; |
| |
| pkt->get_body = func; |
| pkt->get_body_data = user_data; |
| |
| return TRUE; |
| } |
| |
| gboolean g_obex_packet_add_unicode(GObexPacket *pkt, guint8 id, |
| const char *str) |
| { |
| GObexHeader *hdr; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| hdr = g_obex_header_new_unicode(id, str); |
| if (hdr == NULL) |
| return FALSE; |
| |
| return g_obex_packet_add_header(pkt, hdr); |
| } |
| |
| gboolean g_obex_packet_add_bytes(GObexPacket *pkt, guint8 id, |
| const void *data, gsize len) |
| { |
| GObexHeader *hdr; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| hdr = g_obex_header_new_bytes(id, data, len); |
| if (hdr == NULL) |
| return FALSE; |
| |
| return g_obex_packet_add_header(pkt, hdr); |
| } |
| |
| gboolean g_obex_packet_add_uint8(GObexPacket *pkt, guint8 id, guint8 val) |
| { |
| GObexHeader *hdr; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| hdr = g_obex_header_new_uint8(id, val); |
| if (hdr == NULL) |
| return FALSE; |
| |
| return g_obex_packet_add_header(pkt, hdr); |
| } |
| |
| gboolean g_obex_packet_add_uint32(GObexPacket *pkt, guint8 id, guint32 val) |
| { |
| GObexHeader *hdr; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| hdr = g_obex_header_new_uint32(id, val); |
| if (hdr == NULL) |
| return FALSE; |
| |
| return g_obex_packet_add_header(pkt, hdr); |
| } |
| |
| const void *g_obex_packet_get_data(GObexPacket *pkt, gsize *len) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (pkt->data_len == 0) { |
| *len = 0; |
| return NULL; |
| } |
| |
| *len = pkt->data_len; |
| |
| switch (pkt->data_policy) { |
| case G_OBEX_DATA_INHERIT: |
| case G_OBEX_DATA_COPY: |
| return pkt->data.buf; |
| case G_OBEX_DATA_REF: |
| return pkt->data.buf_ref; |
| } |
| |
| g_assert_not_reached(); |
| } |
| |
| gboolean g_obex_packet_set_data(GObexPacket *pkt, const void *data, gsize len, |
| GObexDataPolicy data_policy) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (pkt->data.buf || pkt->data.buf_ref) |
| return FALSE; |
| |
| pkt->data_policy = data_policy; |
| pkt->data_len = len; |
| |
| switch (data_policy) { |
| case G_OBEX_DATA_COPY: |
| pkt->data.buf = g_memdup(data, len); |
| break; |
| case G_OBEX_DATA_REF: |
| pkt->data.buf_ref = data; |
| break; |
| case G_OBEX_DATA_INHERIT: |
| pkt->data.buf = (void *) data; |
| break; |
| } |
| |
| return TRUE; |
| } |
| |
| GObexPacket *g_obex_packet_new_valist(guint8 opcode, gboolean final, |
| guint8 first_hdr_id, va_list args) |
| { |
| GObexPacket *pkt; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", opcode); |
| |
| pkt = g_new0(GObexPacket, 1); |
| |
| pkt->opcode = opcode; |
| pkt->final = final; |
| pkt->headers = g_obex_header_create_list(first_hdr_id, args, |
| &pkt->hlen); |
| pkt->data_policy = G_OBEX_DATA_COPY; |
| |
| return pkt; |
| } |
| |
| GObexPacket *g_obex_packet_new(guint8 opcode, gboolean final, |
| guint8 first_hdr_id, ...) |
| { |
| GObexPacket *pkt; |
| va_list args; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", opcode); |
| |
| va_start(args, first_hdr_id); |
| pkt = g_obex_packet_new_valist(opcode, final, first_hdr_id, args); |
| va_end(args); |
| |
| return pkt; |
| } |
| |
| void g_obex_packet_free(GObexPacket *pkt) |
| { |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| switch (pkt->data_policy) { |
| case G_OBEX_DATA_INHERIT: |
| case G_OBEX_DATA_COPY: |
| g_free(pkt->data.buf); |
| break; |
| case G_OBEX_DATA_REF: |
| break; |
| } |
| |
| g_slist_foreach(pkt->headers, (GFunc) g_obex_header_free, NULL); |
| g_slist_free(pkt->headers); |
| g_free(pkt); |
| } |
| |
| static gboolean parse_headers(GObexPacket *pkt, const void *data, gsize len, |
| GObexDataPolicy data_policy, |
| GError **err) |
| { |
| const guint8 *buf = data; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| while (len > 0) { |
| GObexHeader *header; |
| gsize parsed; |
| |
| header = g_obex_header_decode(buf, len, data_policy, &parsed, |
| err); |
| if (header == NULL) |
| return FALSE; |
| |
| pkt->headers = g_slist_append(pkt->headers, header); |
| pkt->hlen += parsed; |
| |
| len -= parsed; |
| buf += parsed; |
| } |
| |
| return TRUE; |
| } |
| |
| static const guint8 *get_bytes(void *to, const guint8 *from, gsize count) |
| { |
| memcpy(to, from, count); |
| return (from + count); |
| } |
| |
| GObexPacket *g_obex_packet_decode(const void *data, gsize len, |
| gsize header_offset, |
| GObexDataPolicy data_policy, |
| GError **err) |
| { |
| const guint8 *buf = data; |
| guint16 packet_len; |
| guint8 opcode; |
| GObexPacket *pkt; |
| gboolean final; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, ""); |
| |
| if (data_policy == G_OBEX_DATA_INHERIT) { |
| g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_INVALID_ARGS, |
| "Invalid data policy"); |
| g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message); |
| return NULL; |
| } |
| |
| if (len < 3 + header_offset) { |
| g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_PARSE_ERROR, |
| "Not enough data to decode packet"); |
| g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message); |
| return NULL; |
| } |
| |
| buf = get_bytes(&opcode, buf, sizeof(opcode)); |
| buf = get_bytes(&packet_len, buf, sizeof(packet_len)); |
| |
| packet_len = g_ntohs(packet_len); |
| if (packet_len != len) { |
| g_set_error(err, G_OBEX_ERROR, G_OBEX_ERROR_PARSE_ERROR, |
| "Incorrect packet length (%u != %zu)", |
| packet_len, len); |
| g_obex_debug(G_OBEX_DEBUG_ERROR, "%s", (*err)->message); |
| return NULL; |
| } |
| |
| final = (opcode & FINAL_BIT) ? TRUE : FALSE; |
| opcode &= ~FINAL_BIT; |
| |
| pkt = g_obex_packet_new(opcode, final, G_OBEX_HDR_INVALID); |
| |
| if (header_offset == 0) |
| goto headers; |
| |
| g_obex_packet_set_data(pkt, buf, header_offset, data_policy); |
| buf += header_offset; |
| |
| headers: |
| if (!parse_headers(pkt, buf, len - (3 + header_offset), |
| data_policy, err)) |
| goto failed; |
| |
| return pkt; |
| |
| failed: |
| g_obex_packet_free(pkt); |
| return NULL; |
| } |
| |
| static gssize get_body(GObexPacket *pkt, guint8 *buf, gsize len) |
| { |
| guint16 u16; |
| gssize ret; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (len < 3) |
| return -ENOBUFS; |
| |
| ret = pkt->get_body(buf + 3, len - 3, pkt->get_body_data); |
| if (ret < 0) |
| return ret; |
| |
| if (ret > 0) |
| buf[0] = G_OBEX_HDR_BODY; |
| else |
| buf[0] = G_OBEX_HDR_BODY_END; |
| |
| u16 = g_htons(ret + 3); |
| memcpy(&buf[1], &u16, sizeof(u16)); |
| |
| return ret; |
| } |
| |
| gssize g_obex_packet_encode(GObexPacket *pkt, guint8 *buf, gsize len) |
| { |
| gssize ret; |
| gsize count; |
| guint16 u16; |
| GSList *l; |
| |
| g_obex_debug(G_OBEX_DEBUG_PACKET, "opcode 0x%02x", pkt->opcode); |
| |
| if (3 + pkt->data_len + pkt->hlen > len) |
| return -ENOBUFS; |
| |
| buf[0] = pkt->opcode; |
| if (pkt->final) |
| buf[0] |= FINAL_BIT; |
| |
| if (pkt->data_len > 0) { |
| if (pkt->data_policy == G_OBEX_DATA_REF) |
| memcpy(&buf[3], pkt->data.buf_ref, pkt->data_len); |
| else |
| memcpy(&buf[3], pkt->data.buf, pkt->data_len); |
| } |
| |
| count = 3 + pkt->data_len; |
| |
| for (l = pkt->headers; l != NULL; l = g_slist_next(l)) { |
| GObexHeader *hdr = l->data; |
| |
| if (count >= len) |
| return -ENOBUFS; |
| |
| ret = g_obex_header_encode(hdr, buf + count, len - count); |
| if (ret < 0) |
| return ret; |
| |
| count += ret; |
| } |
| |
| if (pkt->get_body) { |
| ret = get_body(pkt, buf + count, len - count); |
| if (ret < 0) |
| return ret; |
| if (ret == 0) { |
| if (pkt->opcode == G_OBEX_RSP_CONTINUE) |
| buf[0] = G_OBEX_RSP_SUCCESS; |
| buf[0] |= FINAL_BIT; |
| } |
| |
| count += ret + 3; |
| } |
| |
| u16 = g_htons(count); |
| memcpy(&buf[1], &u16, sizeof(u16)); |
| |
| return count; |
| } |