/* PIPAPO - PIle PAcket POlicies
 *
 * util.c - Convenience routines
 *
 * Author: Stefano Brivio <sbrivio@redhat.com>
 * License: GPLv2
 */

#include <stdint.h>
#include <string.h>
#include <strings.h>

#include "pipapo.h"

/**
 * set_bit() - Set single bit in arbitrary-sized word
 * @data:	Word
 * @n:		Bit number
 *
 * Equivalent to set_bit() in kernel.
 */
void set_bit(uint8_t *data, int n)
{
	data[n / 8] |= 1 << (n % 8);
}

/**
 * bit_sum() - Sum bit to arbitrary-sized word with carry
 * @v:		Word
 * @n:		Bit number
 * @len:	Word length
 */
void bit_sum(uint8_t *v, int n, int len)
{
	int carry = 0, i;

	for (i = len - 1 - n / 8; i >= 0; i--) {
		if (carry)
			v[i]++;
		else
			v[i] += 1 << (n % 8);

		if (v[i])
			break;
		carry = 1;
	}
}

/**
 * bit_sub() - Subtract bit from arbitrary-sized word with carry
 * @v:		Word
 * @n:		Bit number
 * @len:	Word length
 */
void bit_sub(uint8_t *v, int n, int len)
{
	int carry = 0, i;

	for (i = len - 1 - n / 8; i >= 0; i--) {
		if (carry)
			v[i]--;
		else
			v[i] -= 1 << (n % 8);

		if (v[i] != 0xff)
			break;
		carry = 1;
	}
}

/**
 * andmem() - AND two memory regions
 * @dst:	First operand and destination
 * @src:	Second operand
 * @len:	Length of memory region
 *
 * Equivalent to bitmap_and() in kernel.
 */
void andmem(uint8_t *dst, uint8_t *src, unsigned len)
{
	while (len >= sizeof(long)) {
		*(unsigned long *)dst &= *(unsigned long *)src;
		dst += sizeof(long);
		src += sizeof(long);
		len -= sizeof(long);
	}

	while (len >= sizeof(int)) {
		*(unsigned int *)dst &= *(unsigned int *)src;
		dst += sizeof(int);
		src += sizeof(int);
		len -= sizeof(int);
	}

	while (len--) {
		*dst &= *src;
		dst++;
		src++;
	}
}

/**
 * ffs_clear() - Find first bit set in memory region and clear it
 * @data:	Start of memory region
 * @len:	Length of memory region
 * @offset:	Starting offset in byte
 *
 * Similar purpose as for_each_set_bit_from() in kernel.
 *
 * Return: position of first set bit, if any, -1 otherwise
 */
int ffs_clear(uint8_t *data, unsigned int len, int byte_offset)
{
	int tmp = 0, n, c = byte_offset;

	data += byte_offset;
	len -= byte_offset;

	while (len) {
		if (len >= sizeof(long)) {
			n = ffsl(*(unsigned long *)data);
			if (n--) {
				*((unsigned long *)data) &= ~(1UL << n);
				return n + c * 8;
			}
			len -= sizeof(long);
			data += sizeof(long);
			c += sizeof(long);
		} else if (len >= sizeof(int)) {
			n = ffs(*(unsigned int *)data);
			if (n--) {
				*((unsigned int *)data) &= ~(1U << n);
				return n + c * 8;
			}
			len -= sizeof(int);
			data += sizeof(int);
			c += sizeof(int);
		} else {
			memcpy(&tmp, data, len);
			n = ffs(tmp);
			if (n--) {
				data[n / 8] &= ~(1U << (n % 8));
				return n + c * 8;
			}
			return -1;
		}
	}

	return -1;
}

/**
 * test_bit - Test bit in memory region
 * @data:	Memory region
 * @n:		Bit to be tested
 *
 * Equivalent to test_bit() in kernel.
 *
 * Return: 1 if bit set, 0 if bit unset.
 */
int test_bit(uint8_t *data, int n)
{
	return !!(data[n / 8] & (1 << (n % 8)));
}

/**
 * fold_bits() - Shift a bitmap region, removing a given amount of bits
 * @data:	Memory region
 * @first:	First bit to be removed
 * @n:		Amount of bits to be removed
 * @len:	Size of memory region
 *
 * Similar to bitmap_shift_left() in kernel, *not* equivalent to bitmap_fold().
 * For example, with this bitmap, if start is 7 and n is 4:
 *
 * 1000 0110   1100 0001
 * |       |   |       |
 * 7       0  15       8
 *
 * we are removing these four bits:
 * 
 * x000 0110   1100 0xxx
 * |       |   |       |
 * 7       0  15       8
 *
 * resulting in:
 *
 * 0000 0110   0000 1100
 * |       |   |       |
 * 7       0  15       8
 */
void fold_bits(uint8_t *data, int first, int n, int len)
{
	uint8_t keep, carry;
	int i;

	if (first % 8)
		keep = data[first / 8] & (0xff >> (8 - first % 8));
	else
		keep = 0;

	while (n) {
		for (i = first / 8; i < len; i++) {
			if (i < len - 1)
				carry = data[i + 1] & 0x01;
			else
				carry = 0;

			data[i] >>= 1;
			data[i] |= carry << 7;
		}
		n--;
	}

	data[first / 8] &= 0xff << (first % 8);
	data[first / 8] |= keep;
}