diff options
Diffstat (limited to 'util.c')
-rw-r--r-- | util.c | 217 |
1 files changed, 217 insertions, 0 deletions
@@ -0,0 +1,217 @@ +/* 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; +} |