lib/ultralib/src/libc/llbit.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

#include "PR/ultratypes.h"

s64 __ll_bit_extract(u64* addr, unsigned int start_bit, unsigned int length) {
    unsigned int words;
    unsigned int lbits;
    unsigned int rbits;
    u64 mask;

    words = start_bit / 64;
    lbits = start_bit & (64 - 1);
    rbits = 64 - (lbits + length);

    addr += words;
    mask = ((u64)1 << length) - 1;
    mask = mask << rbits;
    return (s64)((*addr & mask) >> rbits);
}

u64 __ull_bit_extract(u64* addr, unsigned int start_bit, unsigned int length) {
    unsigned int words;
    unsigned int lbits;
    unsigned int rbits;
    u64 mask;

    words = start_bit / 64;
    lbits = start_bit & (64 - 1);
    rbits = 64 - (lbits + length);
    addr += words;
    mask = ((u64)1 << length) - 1;
    mask = mask << rbits;
    return (u64)((*addr & mask) >> rbits);
}

u64 __ll_bit_insert(u64* addr, unsigned int start_bit, unsigned int length, u64 val) {
    unsigned int words;
    unsigned int lbits;
    unsigned int rbits;
    unsigned long long llval;
    unsigned long long mask;

    words = start_bit / 64;
    lbits = start_bit & 0x3f;
    rbits = 64 - (lbits + length);
    addr += words;
    mask = ((u64)1 << length) - 1;
    mask <<= rbits;
    llval = (val << (64 - length)) >> lbits;
    *addr = (*addr & ~mask) | llval;
    llval = llval >> rbits;
    return llval;
}