1 files changed, 255 insertions, 0 deletions
diff --git a/src/lib/fixedpoint.c b/src/lib/fixedpoint.c
new file mode 100644
index 0000000..4df0b50
--- /dev/null
+++ b/src/lib/fixedpoint.c
@@ -0,0 +1,255 @@
+/* -*- mode: c; c-file-style: "openbsd" -*- */
+/*
+ * Copyright (c) 2013 Vincent Bernat <bernat@luffy.cx>
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <arpa/inet.h>
+#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