119 lines
2.9 KiB
C
119 lines
2.9 KiB
C
#include "stdio.h"
|
|
#include "stdint.h"
|
|
#include "soft/debug.h"
|
|
#include "stddef.h"
|
|
|
|
|
|
|
|
/* table used for ffs and ffz operation */
|
|
const uint8_t iot_bitops_ffs_tab[256] = {
|
|
0, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
8, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
|
|
5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1
|
|
};
|
|
|
|
static inline uint8_t iot_bitops_ffs(uint8_t b)
|
|
{
|
|
return iot_bitops_ffs_tab[b];
|
|
}
|
|
|
|
int gpio_find_first_int(uint8_t* bitmap, int bitmap_cnt, int start_bit) {
|
|
uint32_t i, ret = 0;
|
|
|
|
for (i = start_bit >> 3; i < bitmap_cnt; i++) {
|
|
ret = iot_bitops_ffs(bitmap[i]);
|
|
if (ret) {
|
|
bitmap[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;
|
|
}
|
|
|
|
|
|
int thread_fun(void* t)
|
|
{
|
|
uint32_t bitmap[] = { 0,0,8 };
|
|
uint32_t tmp[sizeof(bitmap) / sizeof(bitmap[0])];
|
|
int ret = 0;
|
|
for (int i = 0;i < sizeof(bitmap) / sizeof(bitmap[0]);i++) {
|
|
tmp[i] = bitmap[i];
|
|
}
|
|
ret = gpio_find_first_int((uint8_t*)tmp, sizeof(tmp), 0);
|
|
printf("ret=%d\n", ret - 1);
|
|
ret = gpio_find_first_int((uint8_t*)tmp, sizeof(tmp), ret);
|
|
printf("ret=%d\n", ret - 1);
|
|
|
|
for (int i = 0;i < sizeof(bitmap) / sizeof(bitmap[0]);i++) {
|
|
tmp[i] = bitmap[i];
|
|
}
|
|
ret = iot_bitmap_ffs_from((uint8_t*)tmp, sizeof(tmp), 0);
|
|
printf("ret=%d\n", ret - 1);
|
|
ret = iot_bitmap_ffs_from((uint8_t*)tmp, sizeof(tmp), ret);
|
|
printf("ret=%d\n", ret - 1);
|
|
}
|