xref: /aosp_15_r20/external/openthread/third_party/tcplp/lib/bitmap.c (revision cfb92d1480a9e65faed56933e9c12405f45898b4)

/*
 *  Copyright (c) 2018, Sam Kumar <[email protected]>
 *  Copyright (c) 2018, University of California, Berkeley
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of the copyright holder nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/* BITMAP */

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "bitmap.h"

void bmp_init(uint8_t* buf, size_t numbytes) {
    memset(buf, 0x00, numbytes);
}

#define _bmp_getrangeinfo(buf, start, len, first_bit_id, first_byte_ptr, last_bit_id, last_byte_ptr) \
    do { \
        first_bit_id = (start & 0x7); \
        first_byte_ptr = buf + (start >> 3); \
        last_bit_id = (len & 0x7) + first_bit_id; \
        last_byte_ptr = first_byte_ptr + (len >> 3) + (last_bit_id >> 3); \
        last_bit_id &= 0x7; \
    } while (0)

void bmp_setrange(uint8_t* buf, size_t start, size_t len) {
    uint8_t first_bit_id;
    uint8_t* first_byte_set;
    uint8_t last_bit_id;
    uint8_t* last_byte_set;
    uint8_t first_byte_mask, last_byte_mask;
    _bmp_getrangeinfo(buf, start, len, first_bit_id, first_byte_set,
                      last_bit_id, last_byte_set);

    first_byte_mask = (uint8_t) (0xFF >> first_bit_id);
    last_byte_mask = (uint8_t) (0xFF << (8 - last_bit_id));

    /* Set the bits. */
    if (first_byte_set == last_byte_set) {
        *first_byte_set |= (first_byte_mask & last_byte_mask);
    } else {
        *first_byte_set |= first_byte_mask;
        memset(first_byte_set + 1, 0xFF, (size_t) (last_byte_set - first_byte_set - 1));
        if (last_byte_mask != 0x00) {
            *last_byte_set |= last_byte_mask;
        }
    }
}

void bmp_clrrange(uint8_t* buf, size_t start, size_t len) {
    uint8_t first_bit_id;
    uint8_t* first_byte_clear;
    uint8_t last_bit_id;
    uint8_t* last_byte_clear;
    uint8_t first_byte_mask, last_byte_mask;
    _bmp_getrangeinfo(buf, start, len, first_bit_id, first_byte_clear,
                      last_bit_id, last_byte_clear);

    first_byte_mask = (uint8_t) (0xFF << (8 - first_bit_id));
    last_byte_mask = (uint8_t) (0xFF >> last_bit_id);

    /* Clear the bits. */
    if (first_byte_clear == last_byte_clear) {
        *first_byte_clear &= (first_byte_mask | last_byte_mask);
    } else {
        *first_byte_clear &= first_byte_mask;
        memset(first_byte_clear + 1, 0x00, (size_t) (last_byte_clear - first_byte_clear - 1));
        if (last_byte_mask != 0xFF) {
            *last_byte_clear &= last_byte_mask;
        }
    }
}

size_t bmp_countset(uint8_t* buf, size_t buflen, size_t start, size_t limit) {
    uint8_t first_bit_id;
    uint8_t first_byte;
    uint8_t ideal_first_byte;
    size_t numset;
    uint8_t curr_byte;
    size_t curr_index = start >> 3;
    first_bit_id = start & 0x7;
    first_byte = *(buf + curr_index);

    numset = 8 - first_bit_id; // initialize optimistically, assuming that the first byte will have all 1's in the part we care about
    ideal_first_byte = (uint8_t) (0xFF >> first_bit_id);
    first_byte &= ideal_first_byte;
    if (first_byte == ideal_first_byte) {
        // All bits in the first byte starting at first_bit_id are set
        for (curr_index = curr_index + 1; curr_index < buflen && numset < limit; curr_index++) {
            curr_byte = buf[curr_index];
            if (curr_byte == (uint8_t) 0xFF) {
                numset += 8;
            } else {
                while (curr_byte & (uint8_t) 0x80) { // we could add a numset < limit check here, but it probably isn't worth it
                    curr_byte <<= 1;
                    numset++;
                }
                break;
            }
        }
    } else {
        // The streak ends within the first byte
        do {
            first_byte >>= 1;
            ideal_first_byte >>= 1;
            numset--;
        } while (first_byte != ideal_first_byte);
    }
    return numset;
}

static inline uint8_t bmp_read_bit(uint8_t* buf, size_t i) {
    size_t byte_index = i >> 3;
    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
    return ((uint8_t) (buf[byte_index] << bit_index)) >> 7;
}

static inline void bmp_write_bit(uint8_t* buf, size_t i, uint8_t bit) {
    size_t byte_index = i >> 3;
    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
    size_t bit_shift = 7 - bit_index; // Amount to right shift to get bit in LSB
    buf[byte_index] = (buf[byte_index] & ~(1 << bit_shift)) | (bit << bit_shift);
}

static inline uint8_t bmp_read_byte(uint8_t* buf, size_t i) {
    size_t byte_index = i >> 3;
    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
    if (bit_index == 0) {
        return buf[byte_index];
    }
    return (buf[byte_index] << bit_index) | (buf[byte_index + 1] >> (8 - bit_index));
}

static inline void bmp_write_byte(uint8_t* buf, size_t i, uint8_t byte) {
    size_t byte_index = i >> 3;
    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
    if (bit_index == 0) {
        buf[byte_index] = byte;
        return;
    }
    buf[byte_index] = (buf[byte_index] & (0xFF << (8 - bit_index))) | (byte >> bit_index);
    buf[byte_index + 1] = (buf[byte_index + 1] & (0xFF >> bit_index)) | (byte << (8 - bit_index));
}

void bmp_swap(uint8_t* buf, size_t start_1, size_t start_2, size_t len) {
    while ((len & 0x7) != 0) {
        uint8_t bit_1 = bmp_read_bit(buf, start_1);
        uint8_t bit_2 = bmp_read_bit(buf, start_2);
        if (bit_1 != bit_2) {
            bmp_write_bit(buf, start_1, bit_2);
            bmp_write_bit(buf, start_2, bit_1);
        }

        start_1++;
        start_2++;
        len--;
    }

    while (len != 0) {
        uint8_t byte_1 = bmp_read_byte(buf, start_1);
        uint8_t byte_2 = bmp_read_byte(buf, start_2);
        if (byte_1 != byte_2) {
            bmp_write_byte(buf, start_1, byte_2);
            bmp_write_byte(buf, start_2, byte_1);
        }

        start_1 += 8;
        start_2 += 8;
        len -= 8;
    }
}

int bmp_isempty(uint8_t* buf, size_t buflen) {
    uint8_t* bufend = buf + buflen;
    while (buf < bufend) {
        if (*(buf++)) {
            return 0;
        }
    }
    return 1;
}

/*
 * This function is unused, but I'm leaving it in the code as a comment in case
 * it becomes useful for debugging later on.
 */
#if 0
void bmp_print(uint8_t* buf, size_t buflen) {
    size_t i;
    for (i = 0; i < buflen; i++) {
        printf("%02X", buf[i]);
    }
    printf("\n");
}
#endif