path: root/src/lib/fixedpoint.c

/* -*- 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