/* bits.c - bitfield extraction code

 *

 * This file is Copyright 2010 by the GPSD project

 * SPDX-License-Identifier: BSD-2-clause

 *

 * Bitfield extraction functions.  In each, start is a bit index  - not

 * a byte index - and width is a bit width.  The width is bounded above by

 * 64 bits.

 *

 * The sbits() function assumes twos-complement arithmetic. ubits()

 * and sbits() assume no padding in integers.

 */

#include "../include/gpsd_config.h"  // must be before all includes

#include <assert.h>

#include <limits.h>

#include <stdbool.h>

#include <stdint.h>

#include <string.h>

#include "../include/bits.h"

/* extract a (zero-origin) bitfield from a buffer) as an

 * unsigned uint64_t

 * Note: max width 56!

 *

 * Parameters: buf -- the buffer

 *             start -- starting bit of desired bitfield

 *             width -- width of desired bitfield (0 to 56)

 *             le -- little endian input (swap bytes)

 *

 * Returns: bitfield as uint64_t

 *          zero on errors (56 < width)

 */

uint64_t ubits(const unsigned char buf[], unsigned int start,

               unsigned int width, bool le)

{

    uint64_t fld = 0;

    unsigned int i;

    unsigned end;

    assert(width <= sizeof(uint64_t) * CHAR_BIT);

    if (0 == width ||

        56 < width) {

        return 0;

    }

    for (i = start / CHAR_BIT;

         i < (start + width + CHAR_BIT - 1) / CHAR_BIT; i++) {

        fld <<= CHAR_BIT;

        fld |= (uint64_t)buf[i];

    }

    end = (start + width) % CHAR_BIT;

    if (0 != end) {

        fld >>= (CHAR_BIT - end);

    }

    fld &= ~(~0ULL << width);

    if (le) {

        // extraction as a little-endian requested

        uint64_t reversed = 0;

        for (i = width; i; --i) {

            reversed <<= 1;

            if (1 == (1 & fld)) {

                reversed |= 1;

            }

            fld >>= 1;

        }

        fld = reversed;

    }

    return fld;

}

// extract a bitfield from the buffer as a signed big-endian long

int64_t sbits(const unsigned char buf[], unsigned int start, unsigned int width,

              bool le)

{

    uint64_t fld = ubits(buf, start, width, le);

    /* ensure width > 0 as the result of

       1ULL << (width - 1)

       is undefined for width <= 0 */

    assert(width > 0);

    if (fld & (1ULL << (width - 1))) {

        fld |= (~0ULL << (width - 1));

    }

    return (int64_t)fld;

}

union int_float {

    int32_t i;

    float f;

};

union long_double {

    int64_t l;

    double d;

};

float getlef32(const char *buf, int off)

{

    union int_float i_f;

    i_f.i = getles32(buf, off);

    return i_f.f;

}

double getled64(const char *buf, int off)

{

    union long_double l_d;

    l_d.l = getles64(buf, off);

    return l_d.d;

}

float getbef32(const char *buf, int off)

{

    union int_float i_f;

    i_f.i = getbes32(buf, off);

    return i_f.f;

}

double getbed64(const char *buf, int off)

{

    union long_double l_d;

    l_d.l = getbes64(buf, off);

    return l_d.d;

}

void putbef32(char *buf, int off, float val)

{

    union int_float i_f;

    i_f.f = val;

    putbe32(buf, off, i_f.i);

}

void shiftleft(unsigned char *data, int size, unsigned short left)

{

    unsigned char *byte;

    if (CHAR_BIT <= left) {

        size -= left / CHAR_BIT;

        memmove(data, data + left / CHAR_BIT,

                (size + CHAR_BIT - 1) / CHAR_BIT);

        left %= CHAR_BIT;

    }

    for (byte = data; size--; ++byte ) {

        unsigned char bits;

        if (size) {

            bits = byte[1] >> (CHAR_BIT - left);

        } else {

            bits = 0;

        }

        *byte <<= left;

        // Yes, the mask should not be needed, but id avoids a compiler

        // bug in gcc-amd64 11.2.1

        *byte |= 0x0ff & bits;

    }

}

#ifdef __UNUSED__

void putbed64(char *buf, int off, double val)

{

    union long_double l_d;

    l_d.d = val;

    putbe32(buf, (off), (l_d.l) >> 32);

    putbe32(buf, (off)+4, (l_d.l));

}

// byte-swap a 16-bit unsigned int

u_int16_t swap_u16(u_int16_t i)

{

    u_int8_t c1, c2;

    c1 = i & 255;

    c2 = (i >> 8) & 255;

    return (c1 << 8) + c2;

}

// byte-swap a 32-bit unsigned int

u_int32_t swap_u32(u_int32_t i)

{

    u_int8_t c1, c2, c3, c4;

    c1 = i & 255;

    c2 = (i >> 8) & 255;

    c3 = (i >> 16) & 255;

    c4 = (i >> 24) & 255;

    return ((u_int32_t)c1 << 24) +

            ((u_int32_t)c2 << 16) +

            ((u_int32_t)c3 << 8) + c4;

}

// byte-swap a 64-bit unsigned int

u_int64_t swap_u64(u_int64_t i)

{

    u_int8_t c1, c2, c3, c4, c5, c6, c7, c8;

    c1 = i & 255;

    c2 = (i >> 8) & 255;

    c3 = (i >> 16) & 255;

    c4 = (i >> 24) & 255;

    c5 = (i >> 32) & 255;

    c6 = (i >> 40) & 255;

    c7 = (i >> 48) & 255;

    c8 = (i >> 56) & 255;

    return ((u_int64_t)c1 << 56) +

            ((u_int64_t)c2 << 48) +

            ((u_int64_t)c3 << 40) +

            ((u_int64_t)c4 << 32) +

            ((u_int64_t)c5 << 24) +

            ((u_int64_t)c6 << 16) +

            ((u_int64_t)c7 << 8) +

            c8;

}

#endif  // __UNUSED__

// vim: set expandtab shiftwidth=4