123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477 |
- /*
- * Dynamic data buffer
- * Copyright (c) 2007-2012, Jouni Malinen <j@w1.fi>
- *
- * This software may be distributed under the terms of the BSD license.
- * See README for more details.
- */
- #include "utils/includes.h"
- #include "utils/common.h"
- #include "utils/trace.h"
- #include "utils/fota_buf.h"
- #define fota_printf(...)
- static void *zalloc(size_t size)
- {
- void *n = malloc(size);
- if (n) {
- memset(n, 0, size);
- }
- return n;
- }
- static int hex2num(char c)
- {
- if (c >= '0' && c <= '9') {
- return c - '0';
- }
- if (c >= 'a' && c <= 'f') {
- return c - 'a' + 10;
- }
- if (c >= 'A' && c <= 'F') {
- return c - 'A' + 10;
- }
- return -1;
- }
- int hex2byte(const char *hex)
- {
- int a, b;
- a = hex2num(*hex++);
- if (a < 0) {
- return -1;
- }
- b = hex2num(*hex++);
- if (b < 0) {
- return -1;
- }
- return (a << 4) | b;
- }
- int hexstr2bin(const char *hex, u8 *buf, size_t len)
- {
- size_t i;
- int a;
- const char *ipos = hex;
- u8 *opos = buf;
- for (i = 0; i < len; i++) {
- a = hex2byte(ipos);
- if (a < 0) {
- return -1;
- }
- *opos++ = a;
- ipos += 2;
- }
- return 0;
- }
- #ifdef FOTA_TRACE
- #define FOTABUF_MAGIC 0x51a974e3
- struct fotabuf_trace {
- unsigned int magic;
- } __attribute__((aligned(8)));
- static struct fotabuf_trace *fotabuf_get_trace(const struct fotabuf *buf)
- {
- return (struct fotabuf_trace *)
- ((const u8 *) buf - sizeof(struct fotabuf_trace));
- }
- #endif /* FOTA_TRACE */
- static void fotabuf_overflow(const struct fotabuf *buf, size_t len)
- {
- #ifdef FOTA_TRACE
- struct fotabuf_trace *trace = fotabuf_get_trace(buf);
- if (trace->magic != FOTABUF_MAGIC) {
- fota_printf(MSG_ERROR, "fotabuf: invalid magic %x",
- trace->magic);
- }
- #endif /* FOTA_TRACE */
- fota_printf(MSG_ERROR, "fotabuf %p (size=%lu used=%lu) overflow len=%lu",
- buf, (unsigned long) buf->size, (unsigned long) buf->used,
- (unsigned long) len);
- fota_trace_show("fotabuf overflow");
- abort();
- }
- int fotabuf_resize(struct fotabuf **_buf, size_t add_len)
- {
- struct fotabuf *buf = *_buf;
- #ifdef FOTA_TRACE
- struct fotabuf_trace *trace;
- #endif /* FOTA_TRACE */
- if (buf == NULL) {
- *_buf = fotabuf_alloc(add_len);
- return *_buf == NULL ? -1 : 0;
- }
- #ifdef FOTA_TRACE
- trace = fotabuf_get_trace(buf);
- if (trace->magic != FOTABUF_MAGIC) {
- fota_printf(MSG_ERROR, "fotabuf: invalid magic %x",
- trace->magic);
- fota_trace_show("fotabuf_resize invalid magic");
- abort();
- }
- #endif /* FOTA_TRACE */
- if (buf->used + add_len > buf->size) {
- unsigned char *nbuf;
- if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
- nbuf = os_realloc(buf->buf, buf->used + add_len);
- if (nbuf == NULL) {
- return -1;
- }
- os_memset(nbuf + buf->used, 0, add_len);
- buf->buf = nbuf;
- } else {
- #ifdef FOTA_TRACE
- nbuf = os_realloc(trace, sizeof(struct fotabuf_trace) +
- sizeof(struct fotabuf) +
- buf->used + add_len);
- if (nbuf == NULL) {
- return -1;
- }
- trace = (struct fotabuf_trace *) nbuf;
- buf = (struct fotabuf *)(trace + 1);
- os_memset(nbuf + sizeof(struct fotabuf_trace) +
- sizeof(struct fotabuf) + buf->used, 0,
- add_len);
- #else /* FOTA_TRACE */
- nbuf = realloc(buf, sizeof(struct fotabuf) +
- buf->used + add_len);
- if (nbuf == NULL) {
- return -1;
- }
- buf = (struct fotabuf *) nbuf;
- os_memset(nbuf + sizeof(struct fotabuf) + buf->used, 0,
- add_len);
- #endif /* FOTA_TRACE */
- buf->buf = (u8 *)(buf + 1);
- *_buf = buf;
- }
- buf->size = buf->used + add_len;
- }
- return 0;
- }
- /**
- * fotabuf_alloc - Allocate a fotabuf of the given size
- * @len: Length for the allocated buffer
- * Returns: Buffer to the allocated fotabuf or %NULL on failure
- */
- struct fotabuf *fotabuf_alloc(size_t len)
- {
- #ifdef FOTA_TRACE
- struct fotabuf_trace *trace = zalloc(sizeof(struct fotabuf_trace) +
- sizeof(struct fotabuf) + len);
- struct fotabuf *buf;
- if (trace == NULL) {
- return NULL;
- }
- trace->magic = FOTABUF_MAGIC;
- buf = (struct fotabuf *)(trace + 1);
- #else /* FOTA_TRACE */
- struct fotabuf *buf = zalloc(sizeof(struct fotabuf) + len);
- if (buf == NULL) {
- return NULL;
- }
- #endif /* FOTA_TRACE */
- buf->size = len;
- buf->buf = (u8 *)(buf + 1);
- return buf;
- }
- struct fotabuf *fotabuf_alloc_ext_data(u8 *data, size_t len)
- {
- #ifdef FOTA_TRACE
- struct fotabuf_trace *trace = zalloc(sizeof(struct fotabuf_trace) +
- sizeof(struct fotabuf));
- struct fotabuf *buf;
- if (trace == NULL) {
- return NULL;
- }
- trace->magic = FOTABUF_MAGIC;
- buf = (struct fotabuf *)(trace + 1);
- #else /* FOTA_TRACE */
- struct fotabuf *buf = zalloc(sizeof(struct fotabuf));
- if (buf == NULL) {
- return NULL;
- }
- #endif /* FOTA_TRACE */
- buf->size = len;
- buf->used = len;
- buf->buf = data;
- buf->flags |= FOTABUF_FLAG_EXT_DATA;
- return buf;
- }
- struct fotabuf *fotabuf_alloc_copy(const void *data, size_t len)
- {
- struct fotabuf *buf = fotabuf_alloc(len);
- if (buf) {
- fotabuf_put_data(buf, data, len);
- }
- return buf;
- }
- struct fotabuf *fotabuf_dup(const struct fotabuf *src)
- {
- struct fotabuf *buf = fotabuf_alloc(fotabuf_len(src));
- if (buf) {
- fotabuf_put_data(buf, fotabuf_head(src), fotabuf_len(src));
- }
- return buf;
- }
- /**
- * fotabuf_free - Free a fotabuf
- * @buf: fotabuf buffer
- */
- void fotabuf_free(struct fotabuf *buf)
- {
- #ifdef FOTA_TRACE
- struct fotabuf_trace *trace;
- if (buf == NULL) {
- return;
- }
- trace = fotabuf_get_trace(buf);
- if (trace->magic != FOTABUF_MAGIC) {
- fota_printf(MSG_ERROR, "fotabuf_free: invalid magic %x",
- trace->magic);
- fota_trace_show("fotabuf_free magic mismatch");
- abort();
- }
- if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
- os_free(buf->buf);
- }
- os_free(trace);
- #else /* FOTA_TRACE */
- if (buf == NULL) {
- return;
- }
- if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
- os_free(buf->buf);
- }
- os_free(buf);
- #endif /* FOTA_TRACE */
- }
- void fotabuf_clear_free(struct fotabuf *buf)
- {
- if (buf) {
- os_memset(fotabuf_mhead(buf), 0, fotabuf_len(buf));
- fotabuf_free(buf);
- }
- }
- void *fotabuf_put(struct fotabuf *buf, size_t len)
- {
- void *tmp = fotabuf_mhead_u8(buf) + fotabuf_len(buf);
- buf->used += len;
- if (buf->used > buf->size) {
- fotabuf_overflow(buf, len);
- }
- return tmp;
- }
- /**
- * fotabuf_concat - Concatenate two buffers into a newly allocated one
- * @a: First buffer
- * @b: Second buffer
- * Returns: fotabuf with concatenated a + b data or %NULL on failure
- *
- * Both buffers a and b will be freed regardless of the return value. Input
- * buffers can be %NULL which is interpreted as an empty buffer.
- */
- struct fotabuf *fotabuf_concat(struct fotabuf *a, struct fotabuf *b)
- {
- struct fotabuf *n = NULL;
- size_t len = 0;
- if (b == NULL) {
- return a;
- }
- if (a) {
- len += fotabuf_len(a);
- }
- len += fotabuf_len(b);
- n = fotabuf_alloc(len);
- if (n) {
- if (a) {
- fotabuf_put_buf(n, a);
- }
- fotabuf_put_buf(n, b);
- }
- fotabuf_free(a);
- fotabuf_free(b);
- return n;
- }
- /**
- * fotabuf_zeropad - Pad buffer with 0x00 octets (prefix) to specified length
- * @buf: Buffer to be padded
- * @len: Length for the padded buffer
- * Returns: fotabuf padded to len octets or %NULL on failure
- *
- * If buf is longer than len octets or of same size, it will be returned as-is.
- * Otherwise a new buffer is allocated and prefixed with 0x00 octets followed
- * by the source data. The source buffer will be freed on error, i.e., caller
- * will only be responsible on freeing the returned buffer. If buf is %NULL,
- * %NULL will be returned.
- */
- struct fotabuf *fotabuf_zeropad(struct fotabuf *buf, size_t len)
- {
- struct fotabuf *ret;
- size_t blen;
- if (buf == NULL) {
- return NULL;
- }
- blen = fotabuf_len(buf);
- if (blen >= len) {
- return buf;
- }
- ret = fotabuf_alloc(len);
- if (ret) {
- os_memset(fotabuf_put(ret, len - blen), 0, len - blen);
- fotabuf_put_buf(ret, buf);
- }
- fotabuf_free(buf);
- return ret;
- }
- void fotabuf_printf(struct fotabuf *buf, char *fmt, ...)
- {
- va_list ap;
- void *tmp = fotabuf_mhead_u8(buf) + fotabuf_len(buf);
- int res;
- va_start(ap, fmt);
- res = vsnprintf(tmp, buf->size - buf->used, fmt, ap);
- va_end(ap);
- if (res < 0 || (size_t) res >= buf->size - buf->used) {
- fotabuf_overflow(buf, res);
- }
- buf->used += res;
- }
- /**
- * fotabuf_parse_bin - Parse a null terminated string of binary data to a fotabuf
- * @buf: Buffer with null terminated string (hexdump) of binary data
- * Returns: fotabuf or %NULL on failure
- *
- * The string len must be a multiple of two and contain only hexadecimal digits.
- */
- struct fotabuf *fotabuf_parse_bin(const char *buf)
- {
- size_t len;
- struct fotabuf *ret;
- len = os_strlen(buf);
- if (len & 0x01) {
- return NULL;
- }
- len /= 2;
- ret = fotabuf_alloc(len);
- if (ret == NULL) {
- return NULL;
- }
- if (hexstr2bin(buf, fotabuf_put(ret, len), len)) {
- fotabuf_free(ret);
- return NULL;
- }
- return ret;
- }
|