1  #include <stdlib.h>
  2  #include <sys/lock.h>
  3  #include "bsp.h"
  4  #include "global_config.h"
  5  #include "printf.h"
  6  
  7  #define LOCK_MAX_NUM (CONFIG_SDK_LOCK_NUM)
  8  
  9  typedef long _lock_t;
 10  
 11  typedef struct
 12  {
 13      _lock_t *lock;
 14      long counter;
 15      unsigned long core;
 16  } reculock_t;
 17  
 18  reculock_t reculock[LOCK_MAX_NUM];
 19  
 20  void show_error(void)
 21  {
 22      register unsigned long a7 asm("a7") = 93;
 23      register unsigned long a0 asm("a0") = 0;
 24      register unsigned long a1 asm("a1") = 0;
 25      register unsigned long a2 asm("a2") = 0;
 26  
 27      asm volatile("scall"
 28                   : "+r"(a0)
 29                   : "r"(a1), "r"(a2), "r"(a7));
 30  }
 31  
 32  static inline long lock_trylock(_lock_t *lock)
 33  {
 34      long res = atomic_swap(lock, -1);
 35      /* Use memory barrier to keep coherency */
 36      mb();
 37      return res;
 38  }
 39  
 40  static inline void lock_lock(_lock_t *lock)
 41  {
 42      while(lock_trylock(lock))
 43          ;
 44  }
 45  
 46  static inline void lock_unlock(_lock_t *lock)
 47  {
 48      /* Use memory barrier to keep coherency */
 49      mb();
 50      atomic_swap(lock, 0);
 51      asm volatile("nop");
 52  }
 53  
 54  static reculock_t *get_reculock(_lock_t *lock)
 55  {
 56      for(uint32_t i = 0; i < LOCK_MAX_NUM; i++)
 57      {
 58          if(reculock[i].lock == lock)
 59              return &reculock[i];
 60      }
 61      return NULL;
 62  }
 63  
 64  static reculock_t *get_free_reculock(void)
 65  {
 66      for(uint32_t i = 0; i < LOCK_MAX_NUM; i++)
 67      {
 68          if(reculock[i].lock == NULL)
 69          {
 70              return &reculock[i];
 71          }
 72      }
 73      return NULL;
 74  }
 75  
 76  static reculock_t *reculock_init(_lock_t *lock)
 77  {
 78      reculock_t *v_reculock = get_free_reculock();
 79      if(v_reculock == NULL)
 80      {
 81          return NULL;
 82      }
 83      *v_reculock = (reculock_t){
 84          .lock = lock,
 85          .counter = 0,
 86          .core = 0,
 87      };
 88      return v_reculock;
 89  }
 90  
 91  static void reculock_deinit(_lock_t *lock)
 92  {
 93      lock_lock(lock);
 94      reculock_t *v_reculock = get_reculock(lock);
 95      if(v_reculock)
 96      {
 97          *v_reculock = (reculock_t){
 98              .lock = NULL,
 99              .counter = 0,
100              .core = 0,
101          };
102      }
103      lock_unlock(lock);
104  }
105  
106  static inline int reculock_trylock(_lock_t *lock)
107  {
108      int res = 0;
109      unsigned long core;
110  
111      asm volatile("csrr %0, mhartid;"
112                   : "=r"(core));
113      if(lock_trylock(lock))
114      {
115          return -1;
116      }
117  
118      reculock_t *v_reculock = get_reculock(lock);
119      if(v_reculock == NULL)
120      {
121          v_reculock = reculock_init(lock);
122          if(v_reculock == NULL)
123          {
124              lock_unlock(lock);
125              show_error();
126          }
127      }
128  
129      if(v_reculock->counter == 0)
130      {
131          /* First time get lock */
132          v_reculock->counter++;
133          v_reculock->core = core;
134          res = 0;
135      } else if(v_reculock->core == core)
136      {
137          /* Same core get lock */
138          v_reculock->counter++;
139          res = 0;
140      } else
141      {
142          /* Different core get lock */
143          res = -1;
144      }
145      lock_unlock(lock);
146  
147      return res;
148  }
149  
150  static inline void reculock_lock(_lock_t *lock)
151  {
152      unsigned long core;
153  
154      asm volatile("csrr %0, mhartid;"
155                   : "=r"(core));
156      lock_lock(lock);
157  
158      reculock_t *v_reculock = get_reculock(lock);
159      if(v_reculock == NULL)
160      {
161          v_reculock = reculock_init(lock);
162          if(v_reculock == NULL)
163          {
164              lock_unlock(lock);
165              show_error();
166          }
167      }
168  
169      if(v_reculock->counter == 0)
170      {
171          /* First time get lock */
172          v_reculock->counter++;
173          v_reculock->core = core;
174      } else if(v_reculock->core == core)
175      {
176          /* Same core get lock */
177          v_reculock->counter++;
178      } else
179      {
180          /* Different core get lock */
181          lock_unlock(lock);
182          do
183          {
184              while(atomic_read(&reculock->counter))
185                  ;
186          } while(reculock_trylock(lock));
187          return;
188      }
189      lock_unlock(lock);
190  }
191  
192  static inline void reculock_unlock(_lock_t *lock)
193  {
194      unsigned long core;
195  
196      asm volatile("csrr %0, mhartid;"
197                   : "=r"(core));
198      lock_lock(lock);
199  
200      reculock_t *v_reculock = get_reculock(lock);
201      if(v_reculock == NULL)
202      {
203          lock_unlock(lock);
204          show_error();
205      }
206  
207      if(v_reculock->core == core)
208      {
209          /* Same core release lock */
210          v_reculock->counter--;
211          if(v_reculock->counter <= 0)
212          {
213              v_reculock->core = 0;
214              v_reculock->counter = 0;
215              v_reculock->lock = NULL;
216          }
217      } else
218      {
219          /* Different core release lock */
220          lock_unlock(lock);
221          show_error();
222      }
223      lock_unlock(lock);
224  }
225  
226  void _lock_init(_lock_t *lock)
227  {
228      *lock = 0;
229  }
230  
231  void _lock_init_recursive(_lock_t *lock)
232  {
233      reculock_init(lock);
234  }
235  
236  void _lock_close(_lock_t *lock)
237  {
238      lock_unlock(lock);
239  }
240  
241  void _lock_close_recursive(_lock_t *lock)
242  {
243      reculock_deinit(lock);
244  }
245  
246  void _lock_acquire(_lock_t *lock)
247  {
248      lock_lock(lock);
249  }
250  
251  void _lock_acquire_recursive(_lock_t *lock)
252  {
253      reculock_lock(lock);
254  }
255  
256  int _lock_try_acquire(_lock_t *lock)
257  {
258      return lock_trylock(lock);
259  }
260  
261  int _lock_try_acquire_recursive(_lock_t *lock)
262  {
263      return reculock_trylock(lock);
264  }
265  
266  void _lock_release(_lock_t *lock)
267  {
268      lock_unlock(lock);
269  }
270  
271  void _lock_release_recursive(_lock_t *lock)
272  {
273      reculock_unlock(lock);
274  }
275