bcm68620_release/release/host_driver/utils/bcmolt_bit_utils.c

/*
<:copyright-BRCM:2016:DUAL/GPL:standard

   Broadcom Proprietary and Confidential.(c) 2016 Broadcom
   All Rights Reserved

Unless you and Broadcom execute a separate written software license
agreement governing use of this software, this software is licensed
to you under the terms of the GNU General Public License version 2
(the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php,
with the following added to such license:

   As a special exception, the copyright holders of this software give
   you permission to link this software with independent modules, and
   to copy and distribute the resulting executable under terms of your
   choice, provided that you also meet, for each linked independent
   module, the terms and conditions of the license of that module.
   An independent module is a module which is not derived from this
   software.  The special exception does not apply to any modifications
   of the software.

Not withstanding the above, under no circumstances may you combine
this software in any way with any other Broadcom software provided
under a license other than the GPL, without Broadcom's express prior
written consent.

:>
 */


#include "bcmos_system.h"
#include "math.h"
#include "bcmolt_bit_utils.h"


/** dynamically allocate space for an array of `nbits' bits and initalize
 *  the bits to all be zero.
 *
 * \param[out]  bv      pointer to a bit_vector struct.
 * \param[in]   nbits   number of bits to allocate.
 * \return      FALSE if space was not available, otherwise TRUE.
 */
bcmos_bool bcmolt_bv_new(bit_vector *bv, const uint32_t nbits)
{
    uint32_t nwords = BCMOS_DIVIDE_ROUND_UP(nbits, (BITS_SZ));

    bv->nbits  = nbits;
    bv->vector = (bv_bits *)calloc(nwords, sizeof(bv_bits));
    return (bv->vector != NULL);
}


/** return the value of the `offset'th bit element of the bit vector.
 *
 * \param[in]   bv      pointer to a bit_vector struct.
 * \param[in]   offset  offset of bit to test.
 * \return      FALSE if the bit offset is out of range, otherwise TRUE.
 */
bcmos_bool bcmolt_bv_get(const bit_vector *bv, const uint32_t offset)
{
    bcmos_bool    rv = BCMOS_FALSE;

    if (offset <= bv->nbits)
    {
        rv = test_bits_set(bv->vector[(offset / BITS_SZ)],
                           (1 << (offset % BITS_SZ)));
    }
    else
    {
        BCMOS_TRACE_ERR("out of range %u\n", offset);
    }
    return rv;
}


/** set or clear the bit in position `offset' of the bit vector.
 *  bv->vector[bit_pos] is to be set (assigned to 1) if value is TRUE,
 *  otherwise it is to be cleared (assigned to 0).
 *
 * \param[in]   bv      pointer to a bit_vector struct.
 * \param[in]   offset  offset of bit to set or clear.
 * \param[in]   value   boolean value. TRUE for set, BCMOS_FALSE for clear.
 * \return      FALSE if the bit offset is out of range, otherwise TRUE.
 */
void bcmolt_bv_assign(bit_vector       *bv,
                      const uint32_t    offset,
                      const bcmos_bool  value)
{
    if (offset <= bv->nbits)
    {
        if (value)
        {
            bv->vector[offset / BITS_SZ] |= (1 << (offset % BITS_SZ));
        }
        else
        {
            bv->vector[offset / BITS_SZ] &= ~(1 << (offset % BITS_SZ));
        }
    }
    else
    {
        BCMOS_TRACE_ERR("out of range %u\n", offset);
    }
}


/** toggle the bit in position `offset' of the bit vector.
 *  i.e. if it was 1 it is 0; if it was 0 it is 1.
 *
 * \param[in]   bv      pointer to a bit_vector struct.
 * \param[in]   offset  offset of bit to toggle.
 * \return      FALSE if the bit offset is out of range, otherwise TRUE.
 */
void bcmolt_bv_toggle(bit_vector *bv, const uint32_t offset)
{
    if (offset <= bv->nbits)
    {
        bv->vector[offset / BITS_SZ] ^= (1 << (offset % BITS_SZ));
    }
    else
    {
        BCMOS_TRACE_ERR("out of range %u\n", offset);
    }
}


/** copy bit vector from 'src' to 'dst'.
 *
 * \param[out]  dst     pointer to a bit_vector struct to copy to.
 * \param[in]   src     pointer to a bit_vector struct to copy from.
 * \param[in]   nbits   number of bits to copy.
 * \return      none.
 */
void bcmolt_bv_copy(bit_vector        *dst, 
                    const bit_vector  *src, 
                    const uint32_t     nbits)
{
    uint32_t  i;
    uint32_t  nwords    = nbits / BITS_SZ;
    bv_bits   bit_remainder = nbits % BITS_SZ;

    if ((nbits <= dst->nbits) && (nbits <= src->nbits))
    {
        for (i = 0; i < nwords; i++)
        {
            dst->vector[i] = src->vector[i];
        }

        if (0 != bit_remainder)
        {
            dst->vector[nwords] = (dst->vector[nwords] & ~((2^bit_remainder) - 1)) |
                (src->vector[nwords] & ((2^bit_remainder) - 1));
        }
    }
    else
    {
        BCMOS_TRACE_ERR("out of range %u\n", nbits);
    }
}


/** Print bit pattern of word FORMATTED to string.
 *
 * \param[in]   value   value to transform to bit string.
 * \param[in]   bitcnt  count of bits to be shown.
 * \param[out]  outstr  pointer to a output buffer to store the string.
 * \return      none
 * \warning     this fn doesn't check the size of 'outstr'.
 *              the caller should ensure 'outstr' has enough room for the
 *              bit string, space characters and null terminator.
 */
void bcmolt_bit_string(const bv_bits value, const uint32_t bitcnt, char *outstr)
{
    uint32_t  offset;

    if (bitcnt <= BITS_SZ)
    {
        for (offset = 0; offset < bitcnt; offset++)
        {
            *outstr++ = ((value >> offset) & 1) + '0';
            if ((((offset + 1) % 4) == 0))
            {
                *outstr++ = ' ';
            }
        }
    }

    *outstr = '\0';
}


/** Print all bits in the bit vector.
 *
 * \param[in]   bv      pointer to a bit_vector struct.
 * \return      none.
 */
void bcmolt_bv_dump(const bit_vector *bv)
{
    uint32_t  idx;                        /* word idx */
    char      outstr[BITS_SZ + 8 + 1];    /* 8 spaces + null */

    for (idx = 0; idx < (bv->nbits / BITS_SZ); idx++)
    {
        bcmolt_bit_string(bv->vector[idx], BITS_SZ, outstr);
        bcmos_printf("%s\n", outstr);
    }

    if (0 != (bv->nbits % BITS_SZ))
    {
        bcmolt_bit_string(bv->vector[idx], (bv->nbits % BITS_SZ), outstr);
        bcmos_printf("%s\n", outstr);
    }
}


/** Count the number of bits set in a long integer.
 *
 * \param[in]   num     integer value.
 * \return      number of bits set.
 */
uint32_t bcmolt_bit_count(uint32_t num)
{
    num = ((num & 0xAAAAAAAAL) >>  1) + (num & 0x55555555L);
    num = ((num & 0xCCCCCCCCL) >>  2) + (num & 0x33333333L);
    num = ((num & 0xF0F0F0F0L) >>  4) + (num & 0x0F0F0F0FL);
    num = ((num & 0xFF00FF00L) >>  8) + (num & 0x00FF00FFL);
    num = ((num & 0xFFFF0000L) >> 16) + (num & 0x0000FFFFL);
    return num;
}


/** Count the number of bits set in the whole bit vector.
 *
 * \param[in]   bv      pointer to the bit vector struct.
 * \return      number of bits set.
 */
uint32_t bcmolt_bv_bit_count(const bit_vector *bv)
{
    uint32_t  nwords = BCMOS_DIVIDE_ROUND_UP(bv->nbits, (BITS_SZ));
    uint32_t  cnt = 0;

    while (0 != nwords--)
    {
        cnt += bcmolt_bit_count(bv->vector[nwords]);
    }

    return cnt;
}



/** Copy bit range from a 32-bit word array to an arbitrary bit position of
 *  the destination 32-bit word array.
 *
 * \param[in]   dst             destination buffer
 * \param[in]   dst_bytes       destination buffer size in bytes
 * \param[in]   dst_bit_pos     least bit position to dest
 * \param[in]   src             source buffer
 * \param[in]   n_bits          how many bits to copy from source
 * \note        src is in little endian and dst is in big endian.
 */
void bcmos_bit_range_set(uint32_t *dst, uint32_t dst_bytes, uint16_t dst_bit_pos,
                         uint32_t *src, uint16_t n_bits)
{
    uint16_t    bp = dst_bit_pos;
    uint16_t    len;
    uint32_t    wp;
    uint32_t    mask;
    uint32_t    src_idx;
    uint32_t    dst_idx;

    wp = bp / 32;
    bp = bp & (32 - 1);
    src_idx = 0;

    for (len = n_bits; len > 0; len -= 32)
    {
        if (bp != 0)
        {
            mask = (len < 32) ? (1 << len) - 1 : 0xFFFFFFFF;
            dst_idx = wp;
            dst[dst_idx] &= ~(mask << bp);
            dst[dst_idx] |= src[src_idx] << bp;
            wp++;
            if (len > (32 - bp))
            {
                dst_idx = wp;
                dst[dst_idx] &= ~(mask >> (32 - bp));
                dst[dst_idx] |= src[src_idx] >> (32 - bp) & ((1 << bp) - 1);
            }
        }
        else
        {
            dst_idx = wp;
            if (len < 32)
            {
                mask = (1 << len) - 1;
                dst[dst_idx] &= ~mask;
                dst[dst_idx] |= src[src_idx] << bp;
            }
            else
            {
                dst[dst_idx] = src[src_idx];
            }
            wp++;
        }
        src_idx++;
    }
}


/** Get bit range at an arbitrary bit position of a 32-bit word array
 * 
 * \param[in]   src             source buffer (e.g. dataport)
 * \param[in]   src_bytes       source buffer size in bytes
 * \param[in]   src_bit_pos     least bit position of the source
 * \param[in]   dst             destination buffer to store the bit value
 * \param[in]   n_bits          how many bits to copy from srouce
 * \note        src is in big endian and dst is in little endian.
 */
void bcmos_bit_range_get(const uint32_t *src, uint32_t src_bytes,
                         uint16_t src_bit_pos, uint32_t *dst, uint16_t n_bits)
{
    uint16_t    bp = src_bit_pos;   /* for readability */
    uint16_t    len = n_bits;
    uint32_t    wp;
    uint32_t    src_idx;
    uint32_t    dst_idx;

    if (n_bits == 1)
    {
        wp = bp / 32;
        bp = bp & (32 - 1);
        src_idx = BCMOS_DIVIDE_ROUND_UP(src_bytes, 4) - 1 - wp;
        dst[0] = ((src[src_idx] & (1 << bp)) != 0) ? 1: 0;
        return;
    }

    wp = bp / 32;
    bp = bp & (32 - 1);
    dst_idx = 0;

    for (; len > 0; len -= 32)
    {
        if (bp != 0)
        {
            src_idx = BCMOS_DIVIDE_ROUND_UP(src_bytes, 4) - 1 - wp;
            dst[dst_idx] = src[src_idx] >> bp & ((1 << (32 - bp)) - 1);
            wp++;
            if (len > (32 - bp))
            {
                src_idx = BCMOS_DIVIDE_ROUND_UP(src_bytes, 4) - 1 - wp;
                dst[dst_idx] |= src[src_idx] << (32 - bp);
            }
        }
        else
        {
            src_idx = BCMOS_DIVIDE_ROUND_UP(src_bytes, 4) - 1 - wp;
            dst[dst_idx] = src[src_idx];
            wp++;
        }

        if (len < 32)
        {
            dst[dst_idx] &= ((1 << len) - 1);
        }
        dst_idx++;
    }
}