| /* |
| * Copyright (C) 2011 Joel Armstrong <jcarmst@sandia.gov> |
| * Copyright (C) 2012 Canonical Ltd. |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License (`GPL') 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., 675 Mass Ave, Cambridge, MA 02139, USA. |
| * |
| * Based on parcel implementation from https://bitbucket.org/floren/inferno |
| * |
| */ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif |
| |
| #define _GNU_SOURCE |
| #include <stdio.h> |
| #include <stdlib.h> |
| #include <string.h> |
| #include <errno.h> |
| |
| #include <glib.h> |
| |
| #include <ofono/log.h> |
| |
| /* Parcel-handling code */ |
| #include <sys/types.h> |
| #include <string.h> |
| #include <endian.h> |
| #include <stdint.h> |
| #include <limits.h> |
| |
| #include "parcel.h" |
| |
| #define PAD_SIZE(s) (((s)+3)&~3) |
| |
| typedef uint16_t char16_t; |
| |
| void parcel_init(struct parcel *p) |
| { |
| p->data = g_malloc0(sizeof(int32_t)); |
| p->size = 0; |
| p->capacity = sizeof(int32_t); |
| p->offset = 0; |
| p->malformed = 0; |
| } |
| |
| void parcel_grow(struct parcel *p, size_t size) |
| { |
| char *new = g_realloc(p->data, p->capacity + size); |
| p->data = new; |
| p->capacity += size; |
| } |
| |
| void parcel_free(struct parcel *p) |
| { |
| g_free(p->data); |
| p->size = 0; |
| p->capacity = 0; |
| p->offset = 0; |
| } |
| |
| int32_t parcel_r_int32(struct parcel *p) |
| { |
| int32_t ret; |
| |
| if (p->malformed) |
| return 0; |
| |
| if (p->offset + sizeof(int32_t) > p->size) { |
| ofono_error("%s: parcel is too small", __func__); |
| p->malformed = 1; |
| return 0; |
| } |
| |
| ret = *((int32_t *) (void *) (p->data + p->offset)); |
| p->offset += sizeof(int32_t); |
| return ret; |
| } |
| |
| int parcel_w_int32(struct parcel *p, int32_t val) |
| { |
| for (;;) { |
| |
| if (p->offset + sizeof(int32_t) < p->capacity) { |
| /* There's enough space */ |
| *((int32_t *) (void *) (p->data + p->offset)) = val; |
| p->offset += sizeof(int32_t); |
| p->size += sizeof(int32_t); |
| break; |
| } else { |
| /* Grow data and retry */ |
| parcel_grow(p, sizeof(int32_t)); |
| } |
| } |
| return 0; |
| } |
| |
| int parcel_w_string(struct parcel *p, const char *str) |
| { |
| gunichar2 *gs16; |
| glong gs16_len; |
| size_t len; |
| size_t gs16_size; |
| |
| if (str == NULL) { |
| parcel_w_int32(p, -1); |
| return 0; |
| } |
| |
| gs16 = g_utf8_to_utf16(str, -1, NULL, &gs16_len, NULL); |
| |
| if (parcel_w_int32(p, gs16_len) == -1) |
| return -1; |
| |
| gs16_size = gs16_len * sizeof(char16_t); |
| len = gs16_size + sizeof(char16_t); |
| for (;;) { |
| size_t padded = PAD_SIZE(len); |
| |
| if (p->offset + len < p->capacity) { |
| /* There's enough space */ |
| memcpy(p->data + p->offset, gs16, gs16_size); |
| *((char16_t *) (void *) |
| (p->data + p->offset + gs16_size)) = 0; |
| p->offset += padded; |
| p->size += padded; |
| if (padded != len) { |
| |
| #if BYTE_ORDER == BIG_ENDIAN |
| static const uint32_t mask[4] = { |
| 0x00000000, 0xffffff00, |
| 0xffff0000, 0xff000000 |
| }; |
| #endif |
| #if BYTE_ORDER == LITTLE_ENDIAN |
| static const uint32_t mask[4] = { |
| 0x00000000, 0x00ffffff, |
| 0x0000ffff, 0x000000ff |
| }; |
| #endif |
| |
| *((uint32_t *) (void *) |
| (p->data + p->offset - 4)) &= |
| mask[padded - len]; |
| } |
| break; |
| |
| } else { |
| /* Grow data and retry */ |
| parcel_grow(p, padded); |
| } |
| } |
| |
| g_free(gs16); |
| return 0; |
| } |
| |
| char *parcel_r_string(struct parcel *p) |
| { |
| char *ret; |
| int len16 = parcel_r_int32(p); |
| int strbytes; |
| |
| if (p->malformed) |
| return NULL; |
| |
| /* This is how a null string is sent */ |
| if (len16 < 0) |
| return NULL; |
| |
| strbytes = PAD_SIZE((len16 + 1) * sizeof(char16_t)); |
| if (p->offset + strbytes > p->size) { |
| ofono_error("%s: parcel is too small", __func__); |
| p->malformed = 1; |
| return NULL; |
| } |
| |
| ret = g_utf16_to_utf8((gunichar2 *) (void *) (p->data + p->offset), |
| len16, NULL, NULL, NULL); |
| if (ret == NULL) { |
| ofono_error("%s: wrong UTF16 coding", __func__); |
| p->malformed = 1; |
| return NULL; |
| } |
| |
| p->offset += strbytes; |
| |
| return ret; |
| } |
| |
| void parcel_skip_string(struct parcel *p) |
| { |
| int len16 = parcel_r_int32(p); |
| int strbytes; |
| |
| if (p->malformed) |
| return; |
| |
| /* This is how a null string is sent */ |
| if (len16 < 0) |
| return; |
| |
| strbytes = PAD_SIZE((len16 + 1) * sizeof(char16_t)); |
| if (p->offset + strbytes > p->size) { |
| p->malformed = 1; |
| return; |
| } |
| |
| p->offset += strbytes; |
| } |
| |
| int parcel_w_raw(struct parcel *p, const void *data, size_t len) |
| { |
| if (data == NULL) { |
| parcel_w_int32(p, -1); |
| return 0; |
| } |
| |
| parcel_w_int32(p, len); |
| |
| for (;;) { |
| |
| if (p->offset + len < p->capacity) { |
| /* There's enough space */ |
| memcpy(p->data + p->offset, data, len); |
| p->offset += len; |
| p->size += len; |
| break; |
| } else { |
| /* Grow data and retry */ |
| parcel_grow(p, len); |
| } |
| } |
| return 0; |
| } |
| |
| void *parcel_r_raw(struct parcel *p, int *len) |
| { |
| char *ret; |
| |
| *len = parcel_r_int32(p); |
| |
| if (p->malformed || *len <= 0) |
| return NULL; |
| |
| if (p->offset + *len > p->size) { |
| ofono_error("%s: parcel is too small", __func__); |
| p->malformed = 1; |
| return NULL; |
| } |
| |
| ret = g_try_malloc0(*len); |
| if (ret == NULL) { |
| ofono_error("%s: out of memory (%d bytes)", __func__, *len); |
| return NULL; |
| } |
| |
| memcpy(ret, p->data + p->offset, *len); |
| p->offset += *len; |
| |
| return ret; |
| } |
| |
| size_t parcel_data_avail(struct parcel *p) |
| { |
| return p->size - p->offset; |
| } |
| |
| char **parcel_r_strv(struct parcel *p) |
| { |
| int i; |
| int num_str = parcel_r_int32(p); |
| char **strv; |
| |
| if (p->malformed || num_str <= 0) |
| return NULL; |
| |
| strv = g_new0(char *, num_str + 1); |
| |
| for (i = 0; i < num_str; i++) |
| strv[i] = parcel_r_string(p); |
| |
| if (p->malformed) { |
| g_strfreev(strv); |
| strv = NULL; |
| } |
| |
| return strv; |
| } |