/* * Copyright (C) 2020-2021 Tomasz Kramkowski * SPDX-License-Identifier: MIT */ #include #include #include #include #include #include "common.h" #include "pack.h" #include "trace.h" static void write_val(unsigned char *buf, size_t size, enum pack_endian e, uintmax_t val) { for (size_t i = 0; i < size; i++) buf[i] = (val >> (e == PACK_ENDIAN_LITTLE ? i : size - i - 1) * 8) & 0xff; } enum pack_status pack(void *buf_, size_t size, const char *fmt, ...) { enum pack_endian endianness = PACK_ENDIAN_BIG; unsigned char *buf = buf_; size_t offset = 0; va_list ap; tr_call("pack(%p, %" PRIuSIZE ", %s, ...)", (void *)buf, size, fmt); va_start(ap, fmt); for (int i = 0; fmt[i] != '\0'; i++) { bool sign; size_t s; union { uintmax_t unsigned_; intmax_t signed_; } v; tr_debug("i: %d, fmt[i]: %c", i, fmt[i]); sign = islower(fmt[i]); switch (fmt[i]) { case '>': endianness = PACK_ENDIAN_BIG; continue; case '<': endianness = PACK_ENDIAN_LITTLE; continue; #define T(type, sign, c_type, va_type) \ case (char)PACK_TYPE_##type: \ v.sign##_ = va_arg(ap, sign va_type); \ break; ITYPE_MACROS #undef T case 'x': v.unsigned_ = 0; sign = false; break; default: return PACK_FMTINVAL; } tr_debug("i: %d, fmt[i]: %c, sign: %ssigned", i, fmt[i], sign ? "" : "un"); s = getsize(fmt[i]); tr_debug("s: %" PRIuSIZE, s); if (s == (size_t)-1) return PACK_FMTINVAL; if (size - offset < s) return PACK_TOOSMALL; if (sign) { intmax_t n = v.signed_; tr_debug("val.s: %" PRIdMAX, v.signed_); if (v.signed_ >= 0) { v.unsigned_ = n; } else { uintmax_t offt = BITMASK(s * 8); n += 1; v.unsigned_ = offt + n; } } tr_debug("val.u: %" PRIuMAX, v.unsigned_); write_val(&buf[offset], s, endianness, v.unsigned_); offset += s; } va_end(ap); return PACK_OK; }