/* -*- mode: c; c-file-style: "openbsd" -*- */ /* * Copyright (c) 2013 Vincent Bernat * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above * copyright notice and this permission notice appear in all copies. * * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ #include #include #include #include #include "fixedpoint.h" /* This is not a general purpose fixed point library. First, there is no * arithmetic. Second, some functions assume that the total precision does not * exceed 64 bits. */ #ifdef ENABLE_LLDPMED # ifndef ntohll # define ntohll(x) \ (((u_int64_t)(ntohl((int)(((x) << 32) >> 32))) << 32) | \ (unsigned int)ntohl(((int)((x) >> 32)))) # endif /** * Convert a string to fixed point number. * * @param repr String to convert. * @param end If not NULL, will contain a pointer to the character after the * last character used in the conversion. * @param intbits Number of bits to represent the integer part. * @param fltbits Number of bits to represent the float part. * @return A fixed point number. * * If there is an overflow, there will be a truncation. Moreover, the fraction * part will be rounded to the nearest possible power of two representation. The * point will depend on the number of decimal provided with the fraction * part. */ struct fp_number fp_strtofp(const char *repr, char **end, unsigned intbits, unsigned fltbits) { char *endptr = NULL, *e2; struct fp_number result = { .integer = { 0, intbits }, .fraction = { 0, fltbits, 0 } }; result.integer.value = strtoll(repr, &endptr, 10); if (result.integer.value >= (1LL << (intbits - 1))) result.integer.value = (1LL << (intbits - 1)) - 1; else if (result.integer.value < ~(1LL << (intbits - 1)) + 1) result.integer.value = ~(1LL << (intbits - 1)) + 1; if (*endptr == '.') { long long precision = 1; e2 = endptr + 1; result.fraction.value = strtoll(e2, &endptr, 10); /* Convert to a representation in power of two. Get the * precision from the number of digits provided. This is NOT the * value of the higher bits in the binary representation: we * consider that if the user inputs, 0.9375, it means to * represent anything between 0 and 0.9999 with the same * precision. Therefore, we don't have only 4 bits of precision * but 14. */ while (e2++ != endptr) precision *= 10; result.fraction.value <<= fltbits; result.fraction.value /= precision; result.fraction.precision = (precision == 1) ? 1 : (sizeof(precision) * 8 - __builtin_clzll(precision - 1)); if (result.fraction.precision > fltbits) result.fraction.precision = fltbits; } if (end) *end = endptr; return result; } /** * Get a string representation of a fixed point number. * * @param fp Fixed point number. * @param suffix If not NULL, use the first character when positive and the * second one when negative instead of prefixing by `-`. * @return the string representation * * Since we convert from binary to decimal, we are as precise as the binary * representation. */ char * fp_fptostr(struct fp_number fp, const char *suffix) { char *result = NULL; char *frac = NULL; int negative = (fp.integer.value < 0); if (fp.fraction.value == 0) frac = strdup(""); else { long long decimal = fp.fraction.value; long long precision = 1; int len = 0; while ((1LL << fp.fraction.precision) > precision) { precision *= 10; len += 1; } /* We did round-up, when converting from decimal. We round-down * to have some coherency. */ precision /= 10; len -= 1; if (precision == 0) precision = 1; decimal *= precision; decimal >>= fp.fraction.bits; if (asprintf(&frac, ".%0*llu", len, decimal) == -1) return NULL; } if (asprintf(&result, "%s%llu%s%c", (suffix == NULL && negative) ? "-" : "", (negative) ? (-fp.integer.value) : fp.integer.value, frac, (suffix && !negative) ? suffix[0] : (suffix && negative) ? suffix[1] : ' ') == -1) { free(frac); return NULL; } free(frac); if (!suffix) result[strlen(result) - 1] = '\0'; return result; } /** * Turn a fixed point number into its representation in a buffer. * * @param fp Fixed point number. * @param buf Output buffer. * @param shift Number of bits to skip at the beginning of the buffer. * * The representation of a fixed point number is the precision (always 6 bits * because we assume that int part + frac part does not exceed 64 bits), the * integer part and the fractional part. */ void fp_fptobuf(struct fp_number fp, unsigned char *buf, unsigned shift) { unsigned long long value = (fp.integer.value >= 0) ? ((fp.integer.value << fp.fraction.bits) + fp.fraction.value) : (~(((unsigned long long)(-fp.integer.value) << fp.fraction.bits) + fp.fraction.value) + 1); unsigned long long ints[] = { fp.integer.bits + fp.fraction.precision, value }; unsigned int bits[] = { 6, fp.integer.bits + fp.fraction.bits }; unsigned i, obit, o; for (i = 0, obit = 8 - (shift % 8), o = shift / 8; i < 2;) { if (obit > bits[i]) { /* We need to clear bits that will be overwritten but do not * touch other bits */ if (bits[i] != 0) { buf[o] = buf[o] & (~((1 << obit) - 1) | ((1 << (obit - bits[i])) - 1)); buf[o] = buf[o] | ((ints[i] & ((1 << bits[i]) - 1)) << (obit - bits[i])); obit -= bits[i]; } i++; } else { /* As in the other branch... */ buf[o] = buf[o] & (~((1 << obit) - 1)); buf[o] = buf[o] | ((ints[i] >> (bits[i] - obit)) & ((1 << obit) - 1)); bits[i] -= obit; obit = 8; o++; } } } /** * Parse a fixed point number from a buffer. * * @param buf Input buffer * @param intbits Number of bits used for integer part. * @param fltbits Number of bits used for fractional part. * @param shift Number of bits to skip at the beginning of the buffer. * * @return the parsed fixed point number. * * The representation is the same as for @c fp_fptobuf(). */ struct fp_number fp_buftofp(const unsigned char *buf, unsigned intbits, unsigned fltbits, unsigned shift) { unsigned long long value = 0, precision = 0; unsigned long long *ints[] = { &precision, &value }; unsigned int bits[] = { 6, intbits + fltbits }; unsigned o, ibit, i; for (o = 0, ibit = 8 - (shift % 8), i = shift / 8; o < 2;) { if (ibit > bits[o]) { if (bits[o] > 0) { *ints[o] = *ints[o] | ((buf[i] >> (ibit - bits[o])) & ((1ULL << bits[o]) - 1)); ibit -= bits[o]; } o++; } else { *ints[o] = *ints[o] | ((buf[i] & ((1ULL << ibit) - 1)) << (bits[o] - ibit)); bits[o] -= ibit; ibit = 8; i++; } } /* Don't handle too low precision */ if (precision > intbits) precision -= intbits; else precision = intbits; int negative = !!(value & (1ULL << (intbits + fltbits - 1))); if (negative) value = (~value + 1) & ((1ULL << (intbits + fltbits - 1)) - 1); struct fp_number result = { .integer = { value >> fltbits, intbits }, .fraction = { value & ((1ULL << fltbits) - 1), fltbits, precision } }; if (negative) result.integer.value = -result.integer.value; return result; } /** * Negate a fixed point number. */ struct fp_number fp_negate(struct fp_number fp) { unsigned intbits = fp.integer.bits; struct fp_number result = fp; result.integer.value = -result.integer.value; if (result.integer.value >= (1LL << (intbits - 1))) result.integer.value = (1LL << (intbits - 1)) - 1; else if (result.integer.value < ~(1LL << (intbits - 1)) + 1) result.integer.value = ~(1LL << (intbits - 1)) + 1; return result; } #endif