Home | History | Annotate | Download | only in cortexm4 
      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 
     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 
     31 uint32_t atomicBitsetGetNumBits(const struct AtomicBitset *set)
     32 {
     33     return set->numBits;
     34 }
     35 
     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 
     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 
     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 
     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 
    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 
    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