sys/arm/include/atomic-v6.h

   1 /* $NetBSD: atomic.h,v 1.1 2002/10/19 12:22:34 bsh Exp $ */
   2
   3 /*-
   4  * Copyright (C) 2003-2004 Olivier Houchard
   5  * Copyright (C) 1994-1997 Mark Brinicombe
   6  * Copyright (C) 1994 Brini
   7  * All rights reserved.
   8  *
   9  * This code is derived from software written for Brini by Mark Brinicombe
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  * 1. Redistributions of source code must retain the above copyright
  15  *    notice, this list of conditions and the following disclaimer.
  16  * 2. Redistributions in binary form must reproduce the above copyright
  17  *    notice, this list of conditions and the following disclaimer in the
  18  *    documentation and/or other materials provided with the distribution.
  19  * 3. All advertising materials mentioning features or use of this software
  20  *    must display the following acknowledgement:
  21  *      This product includes software developed by Brini.
  22  * 4. The name of Brini may not be used to endorse or promote products
  23  *    derived from this software without specific prior written permission.
  24  *
  25  * THIS SOFTWARE IS PROVIDED BY BRINI ``AS IS'' AND ANY EXPRESS OR
  26  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  27  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  28  * IN NO EVENT SHALL BRINI BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  29  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  30  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  31  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  32  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  33  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
  34  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  35  *
  36  * $FreeBSD$
  37  */
  38
  39 #ifndef _MACHINE_ATOMIC_V6_H_
  40 #define _MACHINE_ATOMIC_V6_H_
  41
  42 #ifndef _MACHINE_ATOMIC_H_
  43 #error Do not include this file directly, use <machine/atomic.h>
  44 #endif
  45
  46 #if __ARM_ARCH >= 7
  47 #define isb()  __asm __volatile("isb" : : : "memory")
  48 #define dsb()  __asm __volatile("dsb" : : : "memory")
  49 #define dmb()  __asm __volatile("dmb" : : : "memory")
  50 #elif __ARM_ARCH >= 6
  51 #define isb()  __asm __volatile("mcr p15, 0, %0, c7, c5, 4" : : "r" (0) : "memory")
  52 #define dsb()  __asm __volatile("mcr p15, 0, %0, c7, c10, 4" : : "r" (0) : "memory")
  53 #define dmb()  __asm __volatile("mcr p15, 0, %0, c7, c10, 5" : : "r" (0) : "memory")
  54 #else
  55 #error Only use this file with ARMv6 and later
  56 #endif
  57
  58 #define mb()   dmb()
  59 #define wmb()  dmb()
  60 #define rmb()  dmb()
  61
  62 #define ARM_HAVE_ATOMIC64
  63
  64 #define ATOMIC_ACQ_REL_LONG(NAME)                                       \
  65 static __inline void                                                    \
  66 atomic_##NAME##_acq_long(__volatile u_long *p, u_long v)                \
  67 {                                                                       \
  68         atomic_##NAME##_long(p, v);                                     \
  69         dmb();                                                          \
  70 }                                                                       \
  71                                                                         \
  72 static __inline  void                                                   \
  73 atomic_##NAME##_rel_long(__volatile u_long *p, u_long v)                \
  74 {                                                                       \
  75         dmb();                                                          \
  76         atomic_##NAME##_long(p, v);                                     \
  77 }
  78
  79 #define ATOMIC_ACQ_REL(NAME, WIDTH)                                     \
  80 static __inline  void                                                   \
  81 atomic_##NAME##_acq_##WIDTH(__volatile uint##WIDTH##_t *p, uint##WIDTH##_t v)\
  82 {                                                                       \
  83         atomic_##NAME##_##WIDTH(p, v);                                  \
  84         dmb();                                                          \
  85 }                                                                       \
  86                                                                         \
  87 static __inline  void                                                   \
  88 atomic_##NAME##_rel_##WIDTH(__volatile uint##WIDTH##_t *p, uint##WIDTH##_t v)\
  89 {                                                                       \
  90         dmb();                                                          \
  91         atomic_##NAME##_##WIDTH(p, v);                                  \
  92 }
  93
  94 static __inline void
  95 atomic_add_32(volatile uint32_t *p, uint32_t val)
  96 {
  97         uint32_t tmp = 0, tmp2 = 0;
  98
  99         __asm __volatile(
 100             "1: ldrex   %0, [%2]        \n"
 101             "   add     %0, %0, %3      \n"
 102             "   strex   %1, %0, [%2]    \n"
 103             "   cmp     %1, #0          \n"
 104             "   it      ne              \n"
 105             "   bne     1b              \n"
 106             : "=&r" (tmp), "+r" (tmp2)
 107             ,"+r" (p), "+r" (val) : : "cc", "memory");
 108 }
 109
 110 static __inline void
 111 atomic_add_64(volatile uint64_t *p, uint64_t val)
 112 {
 113         uint64_t tmp;
 114         uint32_t exflag;
 115
 116         __asm __volatile(
 117             "1:                                                 \n"
 118             "   ldrexd  %Q[tmp], %R[tmp], [%[ptr]]              \n"
 119             "   adds    %Q[tmp], %Q[val]                        \n"
 120             "   adc     %R[tmp], %R[tmp], %R[val]               \n"
 121             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 122             "   teq     %[exf], #0                              \n"
 123             "   it      ne                                      \n"
 124             "   bne     1b                                      \n"
 125             : [exf] "=&r" (exflag),
 126               [tmp] "=&r" (tmp)
 127             : [ptr] "r"   (p),
 128               [val] "r"   (val)
 129             : "cc", "memory");
 130 }
 131
 132 static __inline void
 133 atomic_add_long(volatile u_long *p, u_long val)
 134 {
 135
 136         atomic_add_32((volatile uint32_t *)p, val);
 137 }
 138
 139 ATOMIC_ACQ_REL(add, 32)
 140 ATOMIC_ACQ_REL(add, 64)
 141 ATOMIC_ACQ_REL_LONG(add)
 142
 143 static __inline void
 144 atomic_clear_32(volatile uint32_t *address, uint32_t setmask)
 145 {
 146         uint32_t tmp = 0, tmp2 = 0;
 147
 148         __asm __volatile(
 149             "1: ldrex   %0, [%2]        \n"
 150             "   bic     %0, %0, %3      \n"
 151             "   strex   %1, %0, [%2]    \n"
 152             "   cmp     %1, #0          \n"
 153             "   it      ne              \n"
 154             "   bne     1b              \n"
 155             : "=&r" (tmp), "+r" (tmp2), "+r" (address), "+r" (setmask)
 156             : : "cc", "memory");
 157 }
 158
 159 static __inline void
 160 atomic_clear_64(volatile uint64_t *p, uint64_t val)
 161 {
 162         uint64_t tmp;
 163         uint32_t exflag;
 164
 165         __asm __volatile(
 166             "1:                                                 \n"
 167             "   ldrexd  %Q[tmp], %R[tmp], [%[ptr]]              \n"
 168             "   bic     %Q[tmp], %Q[val]                        \n"
 169             "   bic     %R[tmp], %R[val]                        \n"
 170             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 171             "   teq     %[exf], #0                              \n"
 172             "   it      ne                                      \n"
 173             "   bne     1b                                      \n"
 174             : [exf] "=&r" (exflag),
 175               [tmp] "=&r" (tmp)
 176             : [ptr] "r"   (p),
 177               [val] "r"   (val)
 178             : "cc", "memory");
 179 }
 180
 181 static __inline void
 182 atomic_clear_long(volatile u_long *address, u_long setmask)
 183 {
 184
 185         atomic_clear_32((volatile uint32_t *)address, setmask);
 186 }
 187
 188 ATOMIC_ACQ_REL(clear, 32)
 189 ATOMIC_ACQ_REL(clear, 64)
 190 ATOMIC_ACQ_REL_LONG(clear)
 191
 192 #define ATOMIC_FCMPSET_CODE(RET, TYPE, SUF)                   \
 193     {                                                         \
 194         TYPE tmp;                                             \
 195                                                               \
 196         __asm __volatile(                                     \
 197             "1: ldrex" SUF "   %[tmp], [%[ptr]]          \n"  \
 198             "   ldr" SUF "     %[ret], [%[oldv]]         \n"  \
 199             "   teq            %[tmp], %[ret]            \n"  \
 200             "   ittee          ne                        \n"  \
 201             "   str" SUF "ne   %[tmp], [%[oldv]]         \n"  \
 202             "   movne          %[ret], #0                \n"  \
 203             "   strex" SUF "eq %[ret], %[newv], [%[ptr]] \n"  \
 204             "   eorseq         %[ret], #1                \n"  \
 205             "   beq            1b                        \n"  \
 206             : [ret] "=&r" (RET),                              \
 207               [tmp] "=&r" (tmp)                               \
 208             : [ptr] "r"   (_ptr),                             \
 209               [oldv] "r"  (_old),                             \
 210               [newv] "r"  (_new)                              \
 211             : "cc", "memory");                                \
 212     }
 213
 214 #define ATOMIC_FCMPSET_CODE64(RET)                                 \
 215     {                                                              \
 216         uint64_t cmp, tmp;                                         \
 217                                                                    \
 218         __asm __volatile(                                          \
 219             "1: ldrexd   %Q[tmp], %R[tmp], [%[ptr]]           \n"  \
 220             "   ldrd     %Q[cmp], %R[cmp], [%[oldv]]          \n"  \
 221             "   teq      %Q[tmp], %Q[cmp]                     \n"  \
 222             "   it       eq                                   \n"  \
 223             "   teqeq    %R[tmp], %R[cmp]                     \n"  \
 224             "   ittee    ne                                   \n"  \
 225             "   movne    %[ret], #0                           \n"  \
 226             "   strdne   %[cmp], [%[oldv]]                    \n"  \
 227             "   strexdeq %[ret], %Q[newv], %R[newv], [%[ptr]] \n"  \
 228             "   eorseq   %[ret], #1                           \n"  \
 229             "   beq      1b                                   \n"  \
 230             : [ret] "=&r" (RET),                                   \
 231               [cmp] "=&r" (cmp),                                   \
 232               [tmp] "=&r" (tmp)                                    \
 233             : [ptr] "r"   (_ptr),                                  \
 234               [oldv] "r"  (_old),                                  \
 235               [newv] "r"  (_new)                                   \
 236             : "cc", "memory");                                     \
 237     }
 238
 239 static __inline int
 240 atomic_fcmpset_8(volatile uint8_t *_ptr, uint8_t *_old, uint8_t _new)
 241 {
 242         int ret;
 243
 244         ATOMIC_FCMPSET_CODE(ret, uint8_t, "b");
 245         return (ret);
 246 }
 247 #define atomic_fcmpset_8        atomic_fcmpset_8
 248
 249 static __inline int
 250 atomic_fcmpset_acq_8(volatile uint8_t *_ptr, uint8_t *_old, uint8_t _new)
 251 {
 252         int ret;
 253
 254         ATOMIC_FCMPSET_CODE(ret, uint8_t, "b");
 255         dmb();
 256         return (ret);
 257 }
 258
 259 static __inline int
 260 atomic_fcmpset_rel_8(volatile uint8_t *_ptr, uint8_t *_old, uint8_t _new)
 261 {
 262         int ret;
 263
 264         dmb();
 265         ATOMIC_FCMPSET_CODE(ret, uint8_t, "b");
 266         return (ret);
 267 }
 268
 269 static __inline int
 270 atomic_fcmpset_16(volatile uint16_t *_ptr, uint16_t *_old, uint16_t _new)
 271 {
 272         int ret;
 273
 274         ATOMIC_FCMPSET_CODE(ret, uint16_t, "h");
 275         return (ret);
 276 }
 277 #define atomic_fcmpset_16       atomic_fcmpset_16
 278
 279 static __inline int
 280 atomic_fcmpset_acq_16(volatile uint16_t *_ptr, uint16_t *_old, uint16_t _new)
 281 {
 282         int ret;
 283
 284         ATOMIC_FCMPSET_CODE(ret, uint16_t, "h");
 285         dmb();
 286         return (ret);
 287 }
 288
 289 static __inline int
 290 atomic_fcmpset_rel_16(volatile uint16_t *_ptr, uint16_t *_old, uint16_t _new)
 291 {
 292         int ret;
 293
 294         dmb();
 295         ATOMIC_FCMPSET_CODE(ret, uint16_t, "h");
 296         return (ret);
 297 }
 298
 299 static __inline int
 300 atomic_fcmpset_32(volatile uint32_t *_ptr, uint32_t *_old, uint32_t _new)
 301 {
 302         int ret;
 303
 304         ATOMIC_FCMPSET_CODE(ret, uint32_t, "");
 305         return (ret);
 306 }
 307
 308 static __inline int
 309 atomic_fcmpset_acq_32(volatile uint32_t *_ptr, uint32_t *_old, uint32_t _new)
 310 {
 311         int ret;
 312
 313         ATOMIC_FCMPSET_CODE(ret, uint32_t, "");
 314         dmb();
 315         return (ret);
 316 }
 317
 318 static __inline int
 319 atomic_fcmpset_rel_32(volatile uint32_t *_ptr, uint32_t *_old, uint32_t _new)
 320 {
 321         int ret;
 322
 323         dmb();
 324         ATOMIC_FCMPSET_CODE(ret, uint32_t, "");
 325         return (ret);
 326 }
 327
 328 static __inline int
 329 atomic_fcmpset_long(volatile u_long *_ptr, u_long *_old, u_long _new)
 330 {
 331         int ret;
 332
 333         ATOMIC_FCMPSET_CODE(ret, u_long, "");
 334         return (ret);
 335 }
 336
 337 static __inline int
 338 atomic_fcmpset_acq_long(volatile u_long *_ptr, u_long *_old, u_long _new)
 339 {
 340         int ret;
 341
 342         ATOMIC_FCMPSET_CODE(ret, u_long, "");
 343         dmb();
 344         return (ret);
 345 }
 346
 347 static __inline int
 348 atomic_fcmpset_rel_long(volatile u_long *_ptr, u_long *_old, u_long _new)
 349 {
 350         int ret;
 351
 352         dmb();
 353         ATOMIC_FCMPSET_CODE(ret, u_long, "");
 354         return (ret);
 355 }
 356
 357 static __inline int
 358 atomic_fcmpset_64(volatile uint64_t *_ptr, uint64_t *_old, uint64_t _new)
 359 {
 360         int ret;
 361
 362         ATOMIC_FCMPSET_CODE64(ret);
 363         return (ret);
 364 }
 365
 366 static __inline int
 367 atomic_fcmpset_acq_64(volatile uint64_t *_ptr, uint64_t *_old, uint64_t _new)
 368 {
 369         int ret;
 370
 371         ATOMIC_FCMPSET_CODE64(ret);
 372         dmb();
 373         return (ret);
 374 }
 375
 376 static __inline int
 377 atomic_fcmpset_rel_64(volatile uint64_t *_ptr, uint64_t *_old, uint64_t _new)
 378 {
 379         int ret;
 380
 381         dmb();
 382         ATOMIC_FCMPSET_CODE64(ret);
 383         return (ret);
 384 }
 385
 386 #define ATOMIC_CMPSET_CODE(RET, SUF)                         \
 387     {                                                        \
 388         __asm __volatile(                                    \
 389             "1: ldrex" SUF "   %[ret], [%[ptr]]          \n" \
 390             "   teq            %[ret], %[oldv]           \n" \
 391             "   itee           ne                        \n" \
 392             "   movne          %[ret], #0                \n" \
 393             "   strex" SUF "eq %[ret], %[newv], [%[ptr]] \n" \
 394             "   eorseq         %[ret], #1                \n" \
 395             "   beq            1b                        \n" \
 396             : [ret] "=&r" (RET)                              \
 397             : [ptr] "r"   (_ptr),                            \
 398               [oldv] "r"  (_old),                            \
 399               [newv] "r"  (_new)                             \
 400             : "cc", "memory");                               \
 401     }
 402
 403 #define ATOMIC_CMPSET_CODE64(RET)                                 \
 404     {                                                             \
 405         uint64_t tmp;                                             \
 406                                                                   \
 407         __asm __volatile(                                         \
 408             "1: ldrexd   %Q[tmp], %R[tmp], [%[ptr]]           \n" \
 409             "   teq      %Q[tmp], %Q[oldv]                    \n" \
 410             "   it       eq                                   \n" \
 411             "   teqeq    %R[tmp], %R[oldv]                    \n" \
 412             "   itee     ne                                   \n" \
 413             "   movne    %[ret], #0                           \n" \
 414             "   strexdeq %[ret], %Q[newv], %R[newv], [%[ptr]] \n" \
 415             "   eorseq   %[ret], #1                           \n" \
 416             "   beq      1b                                   \n" \
 417             : [ret] "=&r" (RET),                                  \
 418               [tmp] "=&r" (tmp)                                   \
 419             : [ptr] "r"   (_ptr),                                 \
 420               [oldv] "r"  (_old),                                 \
 421               [newv] "r"  (_new)                                  \
 422             : "cc", "memory");                                    \
 423     }
 424
 425 static __inline int
 426 atomic_cmpset_8(volatile uint8_t *_ptr, uint8_t _old, uint8_t _new)
 427 {
 428         int ret;
 429
 430         ATOMIC_CMPSET_CODE(ret, "b");
 431         return (ret);
 432 }
 433 #define atomic_cmpset_8         atomic_cmpset_8
 434
 435 static __inline int
 436 atomic_cmpset_acq_8(volatile uint8_t *_ptr, uint8_t _old, uint8_t _new)
 437 {
 438         int ret;
 439
 440         ATOMIC_CMPSET_CODE(ret, "b");
 441         dmb();
 442         return (ret);
 443 }
 444
 445 static __inline int
 446 atomic_cmpset_rel_8(volatile uint8_t *_ptr, uint8_t _old, uint8_t _new)
 447 {
 448         int ret;
 449
 450         dmb();
 451         ATOMIC_CMPSET_CODE(ret, "b");
 452         return (ret);
 453 }
 454
 455 static __inline int
 456 atomic_cmpset_16(volatile uint16_t *_ptr, uint16_t _old, uint16_t _new)
 457 {
 458         int ret;
 459
 460         ATOMIC_CMPSET_CODE(ret, "h");
 461         return (ret);
 462 }
 463 #define atomic_cmpset_16        atomic_cmpset_16
 464
 465 static __inline int
 466 atomic_cmpset_acq_16(volatile uint16_t *_ptr, uint16_t _old, uint16_t _new)
 467 {
 468         int ret;
 469
 470         ATOMIC_CMPSET_CODE(ret, "h");
 471         dmb();
 472         return (ret);
 473 }
 474
 475 static __inline int
 476 atomic_cmpset_rel_16(volatile uint16_t *_ptr, uint16_t _old, uint16_t _new)
 477 {
 478         int ret;
 479
 480         dmb();
 481         ATOMIC_CMPSET_CODE(ret, "h");
 482         return (ret);
 483 }
 484
 485 static __inline int
 486 atomic_cmpset_32(volatile uint32_t *_ptr, uint32_t _old, uint32_t _new)
 487 {
 488         int ret;
 489
 490         ATOMIC_CMPSET_CODE(ret, "");
 491         return (ret);
 492 }
 493
 494 static __inline int
 495 atomic_cmpset_acq_32(volatile uint32_t *_ptr, uint32_t _old, uint32_t _new)
 496 {
 497         int ret;
 498
 499         ATOMIC_CMPSET_CODE(ret, "");
 500         dmb();
 501         return (ret);
 502 }
 503
 504 static __inline int
 505 atomic_cmpset_rel_32(volatile uint32_t *_ptr, uint32_t _old, uint32_t _new)
 506 {
 507         int ret;
 508
 509         dmb();
 510         ATOMIC_CMPSET_CODE(ret, "");
 511         return (ret);
 512 }
 513
 514 static __inline int
 515 atomic_cmpset_long(volatile u_long *_ptr, u_long _old, u_long _new)
 516 {
 517         int ret;
 518
 519         ATOMIC_CMPSET_CODE(ret, "");
 520         return (ret);
 521 }
 522
 523 static __inline int
 524 atomic_cmpset_acq_long(volatile u_long *_ptr, u_long _old, u_long _new)
 525 {
 526         int ret;
 527
 528         ATOMIC_CMPSET_CODE(ret, "");
 529         dmb();
 530         return (ret);
 531 }
 532
 533 static __inline int
 534 atomic_cmpset_rel_long(volatile u_long *_ptr, u_long _old, u_long _new)
 535 {
 536         int ret;
 537
 538         dmb();
 539         ATOMIC_CMPSET_CODE(ret, "");
 540         return (ret);
 541 }
 542
 543 static __inline int
 544 atomic_cmpset_64(volatile uint64_t *_ptr, uint64_t _old, uint64_t _new)
 545 {
 546         int ret;
 547
 548         ATOMIC_CMPSET_CODE64(ret);
 549         return (ret);
 550 }
 551
 552 static __inline int
 553 atomic_cmpset_acq_64(volatile uint64_t *_ptr, uint64_t _old, uint64_t _new)
 554 {
 555         int ret;
 556
 557         ATOMIC_CMPSET_CODE64(ret);
 558         dmb();
 559         return (ret);
 560 }
 561
 562 static __inline int
 563 atomic_cmpset_rel_64(volatile uint64_t *_ptr, uint64_t _old, uint64_t _new)
 564 {
 565         int ret;
 566
 567         dmb();
 568         ATOMIC_CMPSET_CODE64(ret);
 569         return (ret);
 570 }
 571
 572 static __inline uint32_t
 573 atomic_fetchadd_32(volatile uint32_t *p, uint32_t val)
 574 {
 575         uint32_t tmp = 0, tmp2 = 0, ret = 0;
 576
 577         __asm __volatile(
 578             "1: ldrex   %0, [%3]        \n"
 579             "   add     %1, %0, %4      \n"
 580             "   strex   %2, %1, [%3]    \n"
 581             "   cmp     %2, #0          \n"
 582             "   it      ne              \n"
 583             "   bne     1b              \n"
 584             : "+r" (ret), "=&r" (tmp), "+r" (tmp2), "+r" (p), "+r" (val)
 585             : : "cc", "memory");
 586         return (ret);
 587 }
 588
 589 static __inline uint64_t
 590 atomic_fetchadd_64(volatile uint64_t *p, uint64_t val)
 591 {
 592         uint64_t ret, tmp;
 593         uint32_t exflag;
 594
 595         __asm __volatile(
 596             "1:                                                 \n"
 597             "   ldrexd  %Q[ret], %R[ret], [%[ptr]]              \n"
 598             "   adds    %Q[tmp], %Q[ret], %Q[val]               \n"
 599             "   adc     %R[tmp], %R[ret], %R[val]               \n"
 600             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 601             "   teq     %[exf], #0                              \n"
 602             "   it      ne                                      \n"
 603             "   bne     1b                                      \n"
 604             : [ret] "=&r" (ret),
 605               [exf] "=&r" (exflag),
 606               [tmp] "=&r" (tmp)
 607             : [ptr] "r"   (p),
 608               [val] "r"   (val)
 609             : "cc", "memory");
 610         return (ret);
 611 }
 612
 613 static __inline u_long
 614 atomic_fetchadd_long(volatile u_long *p, u_long val)
 615 {
 616
 617         return (atomic_fetchadd_32((volatile uint32_t *)p, val));
 618 }
 619
 620 static __inline uint32_t
 621 atomic_load_acq_32(volatile uint32_t *p)
 622 {
 623         uint32_t v;
 624
 625         v = *p;
 626         dmb();
 627         return (v);
 628 }
 629
 630 static __inline uint64_t
 631 atomic_load_64(volatile uint64_t *p)
 632 {
 633         uint64_t ret;
 634
 635         /*
 636          * The only way to atomically load 64 bits is with LDREXD which puts the
 637          * exclusive monitor into the exclusive state, so reset it to open state
 638          * with CLREX because we don't actually need to store anything.
 639          */
 640         __asm __volatile(
 641             "ldrexd     %Q[ret], %R[ret], [%[ptr]]      \n"
 642             "clrex                                      \n"
 643             : [ret] "=&r" (ret)
 644             : [ptr] "r"   (p)
 645             : "cc", "memory");
 646         return (ret);
 647 }
 648
 649 static __inline uint64_t
 650 atomic_load_acq_64(volatile uint64_t *p)
 651 {
 652         uint64_t ret;
 653
 654         ret = atomic_load_64(p);
 655         dmb();
 656         return (ret);
 657 }
 658
 659 static __inline u_long
 660 atomic_load_acq_long(volatile u_long *p)
 661 {
 662         u_long v;
 663
 664         v = *p;
 665         dmb();
 666         return (v);
 667 }
 668
 669 static __inline uint32_t
 670 atomic_readandclear_32(volatile uint32_t *p)
 671 {
 672         uint32_t ret, tmp = 0, tmp2 = 0;
 673
 674         __asm __volatile(
 675             "1: ldrex   %0, [%3]        \n"
 676             "   mov     %1, #0          \n"
 677             "   strex   %2, %1, [%3]    \n"
 678             "   cmp     %2, #0          \n"
 679             "   it      ne              \n"
 680             "   bne     1b              \n"
 681             : "=r" (ret), "=&r" (tmp), "+r" (tmp2), "+r" (p)
 682             : : "cc", "memory");
 683         return (ret);
 684 }
 685
 686 static __inline uint64_t
 687 atomic_readandclear_64(volatile uint64_t *p)
 688 {
 689         uint64_t ret, tmp;
 690         uint32_t exflag;
 691
 692         __asm __volatile(
 693             "1:                                                 \n"
 694             "   ldrexd  %Q[ret], %R[ret], [%[ptr]]              \n"
 695             "   mov     %Q[tmp], #0                             \n"
 696             "   mov     %R[tmp], #0                             \n"
 697             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 698             "   teq     %[exf], #0                              \n"
 699             "   it      ne                                      \n"
 700             "   bne     1b                                      \n"
 701             : [ret] "=&r" (ret),
 702               [exf] "=&r" (exflag),
 703               [tmp] "=&r" (tmp)
 704             : [ptr] "r"   (p)
 705             : "cc", "memory");
 706         return (ret);
 707 }
 708
 709 static __inline u_long
 710 atomic_readandclear_long(volatile u_long *p)
 711 {
 712
 713         return (atomic_readandclear_32((volatile uint32_t *)p));
 714 }
 715
 716 static __inline void
 717 atomic_set_32(volatile uint32_t *address, uint32_t setmask)
 718 {
 719         uint32_t tmp = 0, tmp2 = 0;
 720
 721         __asm __volatile(
 722             "1: ldrex   %0, [%2]        \n"
 723             "   orr     %0, %0, %3      \n"
 724             "   strex   %1, %0, [%2]    \n"
 725             "   cmp     %1, #0          \n"
 726             "   it      ne              \n"
 727             "   bne     1b              \n"
 728             : "=&r" (tmp), "+r" (tmp2), "+r" (address), "+r" (setmask)
 729             : : "cc", "memory");
 730 }
 731
 732 static __inline void
 733 atomic_set_64(volatile uint64_t *p, uint64_t val)
 734 {
 735         uint64_t tmp;
 736         uint32_t exflag;
 737
 738         __asm __volatile(
 739             "1:                                                 \n"
 740             "   ldrexd  %Q[tmp], %R[tmp], [%[ptr]]              \n"
 741             "   orr     %Q[tmp], %Q[val]                        \n"
 742             "   orr     %R[tmp], %R[val]                        \n"
 743             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 744             "   teq     %[exf], #0                              \n"
 745             "   it      ne                                      \n"
 746             "   bne     1b                                      \n"
 747             : [exf] "=&r" (exflag),
 748               [tmp] "=&r" (tmp)
 749             : [ptr] "r"   (p),
 750               [val] "r"   (val)
 751             : "cc", "memory");
 752 }
 753
 754 static __inline void
 755 atomic_set_long(volatile u_long *address, u_long setmask)
 756 {
 757
 758         atomic_set_32((volatile uint32_t *)address, setmask);
 759 }
 760
 761 ATOMIC_ACQ_REL(set, 32)
 762 ATOMIC_ACQ_REL(set, 64)
 763 ATOMIC_ACQ_REL_LONG(set)
 764
 765 static __inline void
 766 atomic_subtract_32(volatile uint32_t *p, uint32_t val)
 767 {
 768         uint32_t tmp = 0, tmp2 = 0;
 769
 770         __asm __volatile(
 771             "1: ldrex   %0, [%2]        \n"
 772             "   sub     %0, %0, %3      \n"
 773             "   strex   %1, %0, [%2]    \n"
 774             "   cmp     %1, #0          \n"
 775             "   it      ne              \n"
 776             "   bne     1b              \n"
 777             : "=&r" (tmp), "+r" (tmp2), "+r" (p), "+r" (val)
 778             : : "cc", "memory");
 779 }
 780
 781 static __inline void
 782 atomic_subtract_64(volatile uint64_t *p, uint64_t val)
 783 {
 784         uint64_t tmp;
 785         uint32_t exflag;
 786
 787         __asm __volatile(
 788             "1:                                                 \n"
 789             "   ldrexd  %Q[tmp], %R[tmp], [%[ptr]]              \n"
 790             "   subs    %Q[tmp], %Q[val]                        \n"
 791             "   sbc     %R[tmp], %R[tmp], %R[val]               \n"
 792             "   strexd  %[exf], %Q[tmp], %R[tmp], [%[ptr]]      \n"
 793             "   teq     %[exf], #0                              \n"
 794             "   it      ne                                      \n"
 795             "   bne     1b                                      \n"
 796             : [exf] "=&r" (exflag),
 797               [tmp] "=&r" (tmp)
 798             : [ptr] "r"   (p),
 799               [val] "r"   (val)
 800             : "cc", "memory");
 801 }
 802
 803 static __inline void
 804 atomic_subtract_long(volatile u_long *p, u_long val)
 805 {
 806
 807         atomic_subtract_32((volatile uint32_t *)p, val);
 808 }
 809
 810 ATOMIC_ACQ_REL(subtract, 32)
 811 ATOMIC_ACQ_REL(subtract, 64)
 812 ATOMIC_ACQ_REL_LONG(subtract)
 813
 814 static __inline void
 815 atomic_store_64(volatile uint64_t *p, uint64_t val)
 816 {
 817         uint64_t tmp;
 818         uint32_t exflag;
 819
 820         /*
 821          * The only way to atomically store 64 bits is with STREXD, which will
 822          * succeed only if paired up with a preceeding LDREXD using the same
 823          * address, so we read and discard the existing value before storing.
 824          */
 825         __asm __volatile(
 826             "1:                                                 \n"
 827             "   ldrexd  %Q[tmp], %R[tmp], [%[ptr]]              \n"
 828             "   strexd  %[exf], %Q[val], %R[val], [%[ptr]]      \n"
 829             "   teq     %[exf], #0                              \n"
 830             "   it      ne                                      \n"
 831             "   bne     1b                                      \n"
 832             : [tmp] "=&r" (tmp),
 833               [exf] "=&r" (exflag)
 834             : [ptr] "r"   (p),
 835               [val] "r"   (val)
 836             : "cc", "memory");
 837 }
 838
 839 static __inline void
 840 atomic_store_rel_32(volatile uint32_t *p, uint32_t v)
 841 {
 842
 843         dmb();
 844         *p = v;
 845 }
 846
 847 static __inline void
 848 atomic_store_rel_64(volatile uint64_t *p, uint64_t val)
 849 {
 850
 851         dmb();
 852         atomic_store_64(p, val);
 853 }
 854
 855 static __inline void
 856 atomic_store_rel_long(volatile u_long *p, u_long v)
 857 {
 858
 859         dmb();
 860         *p = v;
 861 }
 862
 863 static __inline int
 864 atomic_testandclear_32(volatile uint32_t *ptr, u_int bit)
 865 {
 866         int newv, oldv, result;
 867
 868         __asm __volatile(
 869             "   mov     ip, #1                                  \n"
 870             "   lsl     ip, ip, %[bit]                          \n"
 871             /*  Done with %[bit] as input, reuse below as output. */
 872             "1:                                                 \n"
 873             "   ldrex   %[oldv], [%[ptr]]                       \n"
 874             "   bic     %[newv], %[oldv], ip                    \n"
 875             "   strex   %[bit], %[newv], [%[ptr]]               \n"
 876             "   teq     %[bit], #0                              \n"
 877             "   it      ne                                      \n"
 878             "   bne     1b                                      \n"
 879             "   ands    %[bit], %[oldv], ip                     \n"
 880             "   it      ne                                      \n"
 881             "   movne   %[bit], #1                              \n"
 882             : [bit]  "=&r"   (result),
 883               [oldv] "=&r"   (oldv),
 884               [newv] "=&r"   (newv)
 885             : [ptr]  "r"     (ptr),
 886                      "[bit]" (bit)
 887             : "cc", "ip", "memory");
 888
 889         return (result);
 890 }
 891
 892 static __inline int
 893 atomic_testandclear_int(volatile u_int *p, u_int v)
 894 {
 895
 896         return (atomic_testandclear_32((volatile uint32_t *)p, v));
 897 }
 898
 899 static __inline int
 900 atomic_testandclear_long(volatile u_long *p, u_int v)
 901 {
 902
 903         return (atomic_testandclear_32((volatile uint32_t *)p, v));
 904 }
 905 #define atomic_testandclear_long        atomic_testandclear_long
 906
 907 static __inline int
 908 atomic_testandset_32(volatile uint32_t *ptr, u_int bit)
 909 {
 910         int newv, oldv, result;
 911
 912         __asm __volatile(
 913             "   mov     ip, #1                                  \n"
 914             "   lsl     ip, ip, %[bit]                          \n"
 915             /*  Done with %[bit] as input, reuse below as output. */
 916             "1:                                                 \n"
 917             "   ldrex   %[oldv], [%[ptr]]                       \n"
 918             "   orr     %[newv], %[oldv], ip                    \n"
 919             "   strex   %[bit], %[newv], [%[ptr]]               \n"
 920             "   teq     %[bit], #0                              \n"
 921             "   it      ne                                      \n"
 922             "   bne     1b                                      \n"
 923             "   ands    %[bit], %[oldv], ip                     \n"
 924             "   it      ne                                      \n"
 925             "   movne   %[bit], #1                              \n"
 926             : [bit]  "=&r"   (result),
 927               [oldv] "=&r"   (oldv),
 928               [newv] "=&r"   (newv)
 929             : [ptr]  "r"     (ptr),
 930                      "[bit]" (bit)
 931             : "cc", "ip", "memory");
 932
 933         return (result);
 934 }
 935
 936 static __inline int
 937 atomic_testandset_int(volatile u_int *p, u_int v)
 938 {
 939
 940         return (atomic_testandset_32((volatile uint32_t *)p, v));
 941 }
 942
 943 static __inline int
 944 atomic_testandset_long(volatile u_long *p, u_int v)
 945 {
 946
 947         return (atomic_testandset_32((volatile uint32_t *)p, v));
 948 }
 949 #define atomic_testandset_long  atomic_testandset_long
 950
 951 static __inline int
 952 atomic_testandset_64(volatile uint64_t *p, u_int v)
 953 {
 954         volatile uint32_t *p32;
 955
 956         p32 = (volatile uint32_t *)p;
 957         /* Assume little-endian */
 958         if (v >= 32) {
 959                 v &= 0x1f;
 960                 p32++;
 961         }
 962         return (atomic_testandset_32(p32, v));
 963 }
 964
 965 static __inline uint32_t
 966 atomic_swap_32(volatile uint32_t *p, uint32_t v)
 967 {
 968         uint32_t ret, exflag;
 969
 970         __asm __volatile(
 971             "1: ldrex   %[ret], [%[ptr]]                \n"
 972             "   strex   %[exf], %[val], [%[ptr]]        \n"
 973             "   teq     %[exf], #0                      \n"
 974             "   it      ne                              \n"
 975             "   bne     1b                              \n"
 976             : [ret] "=&r"  (ret),
 977               [exf] "=&r" (exflag)
 978             : [val] "r"  (v),
 979               [ptr] "r"  (p)
 980             : "cc", "memory");
 981         return (ret);
 982 }
 983
 984 static __inline uint64_t
 985 atomic_swap_64(volatile uint64_t *p, uint64_t v)
 986 {
 987         uint64_t ret;
 988         uint32_t exflag;
 989
 990         __asm __volatile(
 991             "1: ldrexd  %Q[ret], %R[ret], [%[ptr]]              \n"
 992             "   strexd  %[exf], %Q[val], %R[val], [%[ptr]]      \n"
 993             "   teq     %[exf], #0                              \n"
 994             "   it      ne                                      \n"
 995             "   bne     1b                                      \n"
 996             : [ret] "=&r" (ret),
 997               [exf] "=&r" (exflag)
 998             : [val] "r"   (v),
 999               [ptr] "r"   (p)
1000             : "cc", "memory");
1001         return (ret);
1002 }
1003
1004 #undef ATOMIC_ACQ_REL
1005 #undef ATOMIC_ACQ_REL_LONG
1006
1007 static __inline void
1008 atomic_thread_fence_acq(void)
1009 {
1010
1011         dmb();
1012 }
1013
1014 static __inline void
1015 atomic_thread_fence_rel(void)
1016 {
1017
1018         dmb();
1019 }
1020
1021 static __inline void
1022 atomic_thread_fence_acq_rel(void)
1023 {
1024
1025         dmb();
1026 }
1027
1028 static __inline void
1029 atomic_thread_fence_seq_cst(void)
1030 {
1031
1032         dmb();
1033 }
1034
1035 #endif /* _MACHINE_ATOMIC_V6_H_ */