1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <stdint.h>
18 #include <string.h>
19 #include <atomicBitset.h>
20 #include <atomic.h>
21
22
atomicBitsetInit(struct AtomicBitset * set,uint32_t numBits)23 void atomicBitsetInit(struct AtomicBitset *set, uint32_t numBits)
24 {
25 set->numBits = numBits;
26 memset(set->words, 0, sizeof(uint32_t) * ATOMIC_BITSET_NUM_WORDS(numBits));
27 if (numBits & 31) //mark all high bits so that atomicBitsetFindClearAndSet() is simpler
28 set->words[numBits / 32] = ((uint32_t)((int32_t)-1LL)) << (numBits & 31);
29 }
30
atomicBitsetGetNumBits(const struct AtomicBitset * set)31 uint32_t atomicBitsetGetNumBits(const struct AtomicBitset *set)
32 {
33 return set->numBits;
34 }
35
atomicBitsetGetBit(const struct AtomicBitset * set,uint32_t num)36 bool atomicBitsetGetBit(const struct AtomicBitset *set, uint32_t num)
37 {
38 if (num >= set->numBits) /* any value is as good as the next */
39 return false;
40
41 return !!((set->words[num / 32]) & (1UL << (num & 31)));
42 }
43
atomicBitsetClearBit(struct AtomicBitset * set,uint32_t num)44 void atomicBitsetClearBit(struct AtomicBitset *set, uint32_t num)
45 {
46 uint32_t idx = num / 32, mask = 1UL << (num & 31), status, tmp;
47 uint32_t *wordPtr = set->words + idx;
48
49 if (num >= set->numBits)
50 return;
51
52 do {
53 asm volatile(
54 " ldrex %0, [%2] \n"
55 " bics %0, %3 \n"
56 " strex %1, %0, [%2] \n"
57 :"=r"(tmp), "=r"(status), "=r"(wordPtr), "=r"(mask)
58 :"2"(wordPtr), "3"(mask)
59 :"cc","memory"
60 );
61 } while (status);
62 }
63
atomicBitsetSetBit(struct AtomicBitset * set,uint32_t num)64 void atomicBitsetSetBit(struct AtomicBitset *set, uint32_t num)
65 {
66 uint32_t idx = num / 32, mask = 1UL << (num & 31), status, tmp;
67 uint32_t *wordPtr = set->words + idx;
68
69 if (num >= set->numBits)
70 return;
71
72 do {
73 asm volatile(
74 " ldrex %0, [%2] \n"
75 " orrs %0, %3 \n"
76 " strex %1, %0, [%2] \n"
77 :"=r"(tmp), "=r"(status), "=r"(wordPtr), "=r"(mask)
78 :"2"(wordPtr), "3"(mask)
79 :"cc","memory"
80 );
81 } while (status);
82 }
83
atomicBitsetFindClearAndSet(struct AtomicBitset * set)84 int32_t atomicBitsetFindClearAndSet(struct AtomicBitset *set)
85 {
86 uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(set->numBits);
87 uint32_t scratch1, scratch2, scratch3, bit = 32;
88 uint32_t *wordPtr = set->words;
89
90 for (idx = 0; idx < numWords; idx++, wordPtr++) {
91 asm volatile(
92 "1: \n"
93 " ldrex %0, [%4] \n"
94 " mvns %3, %0 \n"
95 " beq 1f \n"
96 " clz %1, %3 \n"
97 " rsb %1, #31 \n"
98 " lsl %3, %2, %1 \n"
99 " orrs %0, %3 \n"
100 " strex %3, %0, [%4] \n"
101 " cbz %3, 1f \n"
102 " movs %1, #32 \n"
103 " b 1b \n"
104 "1: \n"
105 :"=r"(scratch1), "=r"(bit), "=r"(scratch2), "=l"(scratch3), "=r"(wordPtr)
106 :"1"(32), "2"(1), "4"(wordPtr)
107 :"cc", "memory"
108 );
109
110 if (bit != 32)
111 return (idx * 32) + bit;
112 }
113
114 return -1;
115 }
116
atomicBitsetXchg(struct AtomicBitset * atomicallyAccessedSet,struct AtomicBitset * otherSet)117 bool atomicBitsetXchg(struct AtomicBitset *atomicallyAccessedSet, struct AtomicBitset *otherSet)
118 {
119 uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(atomicallyAccessedSet->numBits);
120
121 if (atomicallyAccessedSet->numBits != otherSet->numBits)
122 return false;
123
124 for (idx = 0; idx < numWords; idx++)
125 otherSet->words[idx] = atomicXchg32bits(&atomicallyAccessedSet->words[idx], otherSet->words[idx]);
126
127 return true;
128 }
129
atomicBitsetBulkRead(struct AtomicBitset * set,uint32_t * dest,uint32_t numBits)130 bool atomicBitsetBulkRead(struct AtomicBitset *set, uint32_t *dest, uint32_t numBits)
131 {
132 uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(set->numBits);
133
134 if (set->numBits != numBits)
135 return false;
136
137 for (idx = 0; idx < numWords; idx++)
138 dest[idx] = atomicRead32bits(&set->words[idx]);
139
140 return true;
141 }
142