/****************************************************************************

Copyright(c) 2019 by Aerospace C.Power (Chongqing) Microelectronics. ALL RIGHTS RESERVED.

This Information is proprietary to Aerospace C.Power (Chongqing) Microelectronics and MAY NOT
be copied by any method or incorporated into another program without
the express written consent of Aerospace C.Power. This Information or any portion
thereof remains the property of Aerospace C.Power. The Information contained herein
is believed to be accurate and Aerospace C.Power assumes no responsibility or
liability for its use in any way and conveys no license or title under
any patent or copyright and makes no representation or warranty that this
Information is free from patent or copyright infringement.

****************************************************************************/


/* os shim includes */
#include "os_types.h"
#include "os_mem.h"

/* common includes */
#include "iot_bitops.h"

uint32_t iot_bitmap_ffs(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret)
            break;
    }

    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_fls(uint8_t *bm, uint16_t size)
{
    int32_t i;
    uint32_t ret = 0;

    for (i = size - 1; i >= 0; i--) {
        ret = iot_bitops_fls(bm[i]);
        if (ret)
            break;
    }

    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_ffs_and_c(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret) {
            /* clear the bit to 0 */
            bm[i] &= ~(1 << (ret - 1));
            break;
        }
    }

    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_ffs_from(uint8_t *bm, uint16_t size, uint32_t sp)
{
    uint32_t i, sp_byte, sp_bit, ret = 0;

    sp--;
    sp_byte = sp >> 3;
    sp_bit = sp & 0x7;
    if (sp_byte >= size) {
        sp_byte = 0;
        sp_bit = 0;
    }
    if (sp_bit) {
        for (i = sp_bit; i < 8; i++) {
            if (bm[sp_byte] & (1 << i)) {
                ret = i + 1;
                i = sp_byte;
                goto done;
            }
        }
        sp_byte++;
        sp_bit = 0;
    }

    for (i = sp_byte; i < size; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret) {
            goto done;
        }
    }

    /* wrap around and start from 0 again */
    for (i = 0; i < sp_byte; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret) {
            goto done;
        }
    }

done:

    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_ffz(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret = iot_bitops_ffz(bm[i]);
        if (ret)
            break;
    }

    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_ffz_from(uint8_t *bm, uint16_t size, uint32_t sp)
{
    uint32_t i, sp_byte, sp_bit, ret = 0;

    sp--;
    sp_byte = sp >> 3;
    sp_bit = sp & 0x7;
    if (sp_byte >= size) {
        sp_byte = 0;
        sp_bit = 0;
    }
    if (sp_bit) {
        for (i = sp_bit; i < 8; i++) {
            if (!(bm[sp_byte] & (1 << i))) {
                ret = i + 1;
                i = sp_byte;
                goto done;
            }
        }
        sp_byte++;
        sp_bit = 0;
    }

    for (i = sp_byte; i < size; i++) {
        ret = iot_bitops_ffz(bm[i]);
        if (ret) {
            goto done;
        }
    }

    /* wrap around and start from 0 again */
    for (i = 0; i < sp_byte; i++) {
        ret = iot_bitops_ffz(bm[i]);
        if (ret) {
            goto done;
        }
    }

done:
    if (ret)
        ret += i << 3;

    return ret;
}

uint32_t iot_bitmap_ffz_and_s(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret = iot_bitops_ffz(bm[i]);
        if (ret) {
            /* set the bit to 1 */
            bm[i] |= (1 << (ret - 1));
            break;
        }
    }

    if (ret)
        ret += i << 3;

    return ret;
}

void iot_bitmap_set(uint8_t *bm, uint16_t size, uint32_t bit)
{
    uint32_t idx;

    bit--;
    idx = bit >> 3;
    if (idx < size) {
        bm[idx] |= (1 << (bit & 0x7));
    }
}

bool_t iot_bitmap_is_set(uint8_t *bm, uint16_t size, uint32_t bit)
{
    uint32_t idx;

    bit--;
    idx = bit >> 3;
    if (idx < size) {
        return !!(bm[idx] & (1 << (bit & 0x7)));
    } else {
        return false;
    }
}

void iot_bitmap_clear(uint8_t *bm, uint16_t size, uint32_t bit)
{
    uint32_t idx;

    bit--;
    idx = bit >> 3;
    if (idx < size) {
        bm[idx] &= ~(1 << (bit & 0x7));
    }
}

void iot_bitmap_reverse(uint8_t *bm, uint16_t size)
{
    uint32_t i;

    for (i = 0; i < size; i++) {
        bm[i] = ~bm[i];
    }
}

uint32_t iot_bitmap_cbs(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret += iot_bitops_cbs(bm[i]);
    }

    return ret;
}

uint32_t iot_bitmap_cbz(uint8_t *bm, uint16_t size)
{
    uint32_t i, ret = 0;

    for (i = 0; i < size; i++) {
        ret += iot_bitops_cbz(bm[i]);
    }

    return ret;
}