fota_buf.c 9.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477
  1. /*
  2. * Dynamic data buffer
  3. * Copyright (c) 2007-2012, Jouni Malinen <j@w1.fi>
  4. *
  5. * This software may be distributed under the terms of the BSD license.
  6. * See README for more details.
  7. */
  8. #include "utils/includes.h"
  9. #include "utils/common.h"
  10. #include "utils/trace.h"
  11. #include "utils/fota_buf.h"
  12. #define fota_printf(...)
  13. static void *zalloc(size_t size)
  14. {
  15. void *n = malloc(size);
  16. if (n) {
  17. memset(n, 0, size);
  18. }
  19. return n;
  20. }
  21. static int hex2num(char c)
  22. {
  23. if (c >= '0' && c <= '9') {
  24. return c - '0';
  25. }
  26. if (c >= 'a' && c <= 'f') {
  27. return c - 'a' + 10;
  28. }
  29. if (c >= 'A' && c <= 'F') {
  30. return c - 'A' + 10;
  31. }
  32. return -1;
  33. }
  34. int hex2byte(const char *hex)
  35. {
  36. int a, b;
  37. a = hex2num(*hex++);
  38. if (a < 0) {
  39. return -1;
  40. }
  41. b = hex2num(*hex++);
  42. if (b < 0) {
  43. return -1;
  44. }
  45. return (a << 4) | b;
  46. }
  47. int hexstr2bin(const char *hex, u8 *buf, size_t len)
  48. {
  49. size_t i;
  50. int a;
  51. const char *ipos = hex;
  52. u8 *opos = buf;
  53. for (i = 0; i < len; i++) {
  54. a = hex2byte(ipos);
  55. if (a < 0) {
  56. return -1;
  57. }
  58. *opos++ = a;
  59. ipos += 2;
  60. }
  61. return 0;
  62. }
  63. #ifdef FOTA_TRACE
  64. #define FOTABUF_MAGIC 0x51a974e3
  65. struct fotabuf_trace {
  66. unsigned int magic;
  67. } __attribute__((aligned(8)));
  68. static struct fotabuf_trace *fotabuf_get_trace(const struct fotabuf *buf)
  69. {
  70. return (struct fotabuf_trace *)
  71. ((const u8 *) buf - sizeof(struct fotabuf_trace));
  72. }
  73. #endif /* FOTA_TRACE */
  74. static void fotabuf_overflow(const struct fotabuf *buf, size_t len)
  75. {
  76. #ifdef FOTA_TRACE
  77. struct fotabuf_trace *trace = fotabuf_get_trace(buf);
  78. if (trace->magic != FOTABUF_MAGIC) {
  79. fota_printf(MSG_ERROR, "fotabuf: invalid magic %x",
  80. trace->magic);
  81. }
  82. #endif /* FOTA_TRACE */
  83. fota_printf(MSG_ERROR, "fotabuf %p (size=%lu used=%lu) overflow len=%lu",
  84. buf, (unsigned long) buf->size, (unsigned long) buf->used,
  85. (unsigned long) len);
  86. fota_trace_show("fotabuf overflow");
  87. abort();
  88. }
  89. int fotabuf_resize(struct fotabuf **_buf, size_t add_len)
  90. {
  91. struct fotabuf *buf = *_buf;
  92. #ifdef FOTA_TRACE
  93. struct fotabuf_trace *trace;
  94. #endif /* FOTA_TRACE */
  95. if (buf == NULL) {
  96. *_buf = fotabuf_alloc(add_len);
  97. return *_buf == NULL ? -1 : 0;
  98. }
  99. #ifdef FOTA_TRACE
  100. trace = fotabuf_get_trace(buf);
  101. if (trace->magic != FOTABUF_MAGIC) {
  102. fota_printf(MSG_ERROR, "fotabuf: invalid magic %x",
  103. trace->magic);
  104. fota_trace_show("fotabuf_resize invalid magic");
  105. abort();
  106. }
  107. #endif /* FOTA_TRACE */
  108. if (buf->used + add_len > buf->size) {
  109. unsigned char *nbuf;
  110. if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
  111. nbuf = os_realloc(buf->buf, buf->used + add_len);
  112. if (nbuf == NULL) {
  113. return -1;
  114. }
  115. os_memset(nbuf + buf->used, 0, add_len);
  116. buf->buf = nbuf;
  117. } else {
  118. #ifdef FOTA_TRACE
  119. nbuf = os_realloc(trace, sizeof(struct fotabuf_trace) +
  120. sizeof(struct fotabuf) +
  121. buf->used + add_len);
  122. if (nbuf == NULL) {
  123. return -1;
  124. }
  125. trace = (struct fotabuf_trace *) nbuf;
  126. buf = (struct fotabuf *)(trace + 1);
  127. os_memset(nbuf + sizeof(struct fotabuf_trace) +
  128. sizeof(struct fotabuf) + buf->used, 0,
  129. add_len);
  130. #else /* FOTA_TRACE */
  131. nbuf = realloc(buf, sizeof(struct fotabuf) +
  132. buf->used + add_len);
  133. if (nbuf == NULL) {
  134. return -1;
  135. }
  136. buf = (struct fotabuf *) nbuf;
  137. os_memset(nbuf + sizeof(struct fotabuf) + buf->used, 0,
  138. add_len);
  139. #endif /* FOTA_TRACE */
  140. buf->buf = (u8 *)(buf + 1);
  141. *_buf = buf;
  142. }
  143. buf->size = buf->used + add_len;
  144. }
  145. return 0;
  146. }
  147. /**
  148. * fotabuf_alloc - Allocate a fotabuf of the given size
  149. * @len: Length for the allocated buffer
  150. * Returns: Buffer to the allocated fotabuf or %NULL on failure
  151. */
  152. struct fotabuf *fotabuf_alloc(size_t len)
  153. {
  154. #ifdef FOTA_TRACE
  155. struct fotabuf_trace *trace = zalloc(sizeof(struct fotabuf_trace) +
  156. sizeof(struct fotabuf) + len);
  157. struct fotabuf *buf;
  158. if (trace == NULL) {
  159. return NULL;
  160. }
  161. trace->magic = FOTABUF_MAGIC;
  162. buf = (struct fotabuf *)(trace + 1);
  163. #else /* FOTA_TRACE */
  164. struct fotabuf *buf = zalloc(sizeof(struct fotabuf) + len);
  165. if (buf == NULL) {
  166. return NULL;
  167. }
  168. #endif /* FOTA_TRACE */
  169. buf->size = len;
  170. buf->buf = (u8 *)(buf + 1);
  171. return buf;
  172. }
  173. struct fotabuf *fotabuf_alloc_ext_data(u8 *data, size_t len)
  174. {
  175. #ifdef FOTA_TRACE
  176. struct fotabuf_trace *trace = zalloc(sizeof(struct fotabuf_trace) +
  177. sizeof(struct fotabuf));
  178. struct fotabuf *buf;
  179. if (trace == NULL) {
  180. return NULL;
  181. }
  182. trace->magic = FOTABUF_MAGIC;
  183. buf = (struct fotabuf *)(trace + 1);
  184. #else /* FOTA_TRACE */
  185. struct fotabuf *buf = zalloc(sizeof(struct fotabuf));
  186. if (buf == NULL) {
  187. return NULL;
  188. }
  189. #endif /* FOTA_TRACE */
  190. buf->size = len;
  191. buf->used = len;
  192. buf->buf = data;
  193. buf->flags |= FOTABUF_FLAG_EXT_DATA;
  194. return buf;
  195. }
  196. struct fotabuf *fotabuf_alloc_copy(const void *data, size_t len)
  197. {
  198. struct fotabuf *buf = fotabuf_alloc(len);
  199. if (buf) {
  200. fotabuf_put_data(buf, data, len);
  201. }
  202. return buf;
  203. }
  204. struct fotabuf *fotabuf_dup(const struct fotabuf *src)
  205. {
  206. struct fotabuf *buf = fotabuf_alloc(fotabuf_len(src));
  207. if (buf) {
  208. fotabuf_put_data(buf, fotabuf_head(src), fotabuf_len(src));
  209. }
  210. return buf;
  211. }
  212. /**
  213. * fotabuf_free - Free a fotabuf
  214. * @buf: fotabuf buffer
  215. */
  216. void fotabuf_free(struct fotabuf *buf)
  217. {
  218. #ifdef FOTA_TRACE
  219. struct fotabuf_trace *trace;
  220. if (buf == NULL) {
  221. return;
  222. }
  223. trace = fotabuf_get_trace(buf);
  224. if (trace->magic != FOTABUF_MAGIC) {
  225. fota_printf(MSG_ERROR, "fotabuf_free: invalid magic %x",
  226. trace->magic);
  227. fota_trace_show("fotabuf_free magic mismatch");
  228. abort();
  229. }
  230. if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
  231. os_free(buf->buf);
  232. }
  233. os_free(trace);
  234. #else /* FOTA_TRACE */
  235. if (buf == NULL) {
  236. return;
  237. }
  238. if (buf->flags & FOTABUF_FLAG_EXT_DATA) {
  239. os_free(buf->buf);
  240. }
  241. os_free(buf);
  242. #endif /* FOTA_TRACE */
  243. }
  244. void fotabuf_clear_free(struct fotabuf *buf)
  245. {
  246. if (buf) {
  247. os_memset(fotabuf_mhead(buf), 0, fotabuf_len(buf));
  248. fotabuf_free(buf);
  249. }
  250. }
  251. void *fotabuf_put(struct fotabuf *buf, size_t len)
  252. {
  253. void *tmp = fotabuf_mhead_u8(buf) + fotabuf_len(buf);
  254. buf->used += len;
  255. if (buf->used > buf->size) {
  256. fotabuf_overflow(buf, len);
  257. }
  258. return tmp;
  259. }
  260. /**
  261. * fotabuf_concat - Concatenate two buffers into a newly allocated one
  262. * @a: First buffer
  263. * @b: Second buffer
  264. * Returns: fotabuf with concatenated a + b data or %NULL on failure
  265. *
  266. * Both buffers a and b will be freed regardless of the return value. Input
  267. * buffers can be %NULL which is interpreted as an empty buffer.
  268. */
  269. struct fotabuf *fotabuf_concat(struct fotabuf *a, struct fotabuf *b)
  270. {
  271. struct fotabuf *n = NULL;
  272. size_t len = 0;
  273. if (b == NULL) {
  274. return a;
  275. }
  276. if (a) {
  277. len += fotabuf_len(a);
  278. }
  279. len += fotabuf_len(b);
  280. n = fotabuf_alloc(len);
  281. if (n) {
  282. if (a) {
  283. fotabuf_put_buf(n, a);
  284. }
  285. fotabuf_put_buf(n, b);
  286. }
  287. fotabuf_free(a);
  288. fotabuf_free(b);
  289. return n;
  290. }
  291. /**
  292. * fotabuf_zeropad - Pad buffer with 0x00 octets (prefix) to specified length
  293. * @buf: Buffer to be padded
  294. * @len: Length for the padded buffer
  295. * Returns: fotabuf padded to len octets or %NULL on failure
  296. *
  297. * If buf is longer than len octets or of same size, it will be returned as-is.
  298. * Otherwise a new buffer is allocated and prefixed with 0x00 octets followed
  299. * by the source data. The source buffer will be freed on error, i.e., caller
  300. * will only be responsible on freeing the returned buffer. If buf is %NULL,
  301. * %NULL will be returned.
  302. */
  303. struct fotabuf *fotabuf_zeropad(struct fotabuf *buf, size_t len)
  304. {
  305. struct fotabuf *ret;
  306. size_t blen;
  307. if (buf == NULL) {
  308. return NULL;
  309. }
  310. blen = fotabuf_len(buf);
  311. if (blen >= len) {
  312. return buf;
  313. }
  314. ret = fotabuf_alloc(len);
  315. if (ret) {
  316. os_memset(fotabuf_put(ret, len - blen), 0, len - blen);
  317. fotabuf_put_buf(ret, buf);
  318. }
  319. fotabuf_free(buf);
  320. return ret;
  321. }
  322. void fotabuf_printf(struct fotabuf *buf, char *fmt, ...)
  323. {
  324. va_list ap;
  325. void *tmp = fotabuf_mhead_u8(buf) + fotabuf_len(buf);
  326. int res;
  327. va_start(ap, fmt);
  328. res = vsnprintf(tmp, buf->size - buf->used, fmt, ap);
  329. va_end(ap);
  330. if (res < 0 || (size_t) res >= buf->size - buf->used) {
  331. fotabuf_overflow(buf, res);
  332. }
  333. buf->used += res;
  334. }
  335. /**
  336. * fotabuf_parse_bin - Parse a null terminated string of binary data to a fotabuf
  337. * @buf: Buffer with null terminated string (hexdump) of binary data
  338. * Returns: fotabuf or %NULL on failure
  339. *
  340. * The string len must be a multiple of two and contain only hexadecimal digits.
  341. */
  342. struct fotabuf *fotabuf_parse_bin(const char *buf)
  343. {
  344. size_t len;
  345. struct fotabuf *ret;
  346. len = os_strlen(buf);
  347. if (len & 0x01) {
  348. return NULL;
  349. }
  350. len /= 2;
  351. ret = fotabuf_alloc(len);
  352. if (ret == NULL) {
  353. return NULL;
  354. }
  355. if (hexstr2bin(buf, fotabuf_put(ret, len), len)) {
  356. fotabuf_free(ret);
  357. return NULL;
  358. }
  359. return ret;
  360. }