sys/mips/mips/support.S

   1 /*      $OpenBSD: locore.S,v 1.18 1998/09/15 10:58:53 pefo Exp $        */
   2 /*-
   3  * Copyright (c) 1992, 1993
   4  *      The Regents of the University of California.  All rights reserved.
   5  *
   6  * This code is derived from software contributed to Berkeley by
   7  * Digital Equipment Corporation and Ralph Campbell.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  * 4. Neither the name of the University nor the names of its contributors
  18  *    may be used to endorse or promote products derived from this software
  19  *    without specific prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
  22  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
  25  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  26  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  27  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  28  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  30  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  31  * SUCH DAMAGE.
  32  *
  33  * Copyright (C) 1989 Digital Equipment Corporation.
  34  * Permission to use, copy, modify, and distribute this software and
  35  * its documentation for any purpose and without fee is hereby granted,
  36  * provided that the above copyright notice appears in all copies.
  37  * Digital Equipment Corporation makes no representations about the
  38  * suitability of this software for any purpose.  It is provided "as is"
  39  * without express or implied warranty.
  40  *
  41  * from: Header: /sprite/src/kernel/mach/ds3100.md/RCS/loMem.s,
  42  *      v 1.1 89/07/11 17:55:04 nelson Exp  SPRITE (DECWRL)
  43  * from: Header: /sprite/src/kernel/mach/ds3100.md/RCS/machAsm.s,
  44  *      v 9.2 90/01/29 18:00:39 shirriff Exp  SPRITE (DECWRL)
  45  * from: Header: /sprite/src/kernel/vm/ds3100.md/vmPmaxAsm.s,
  46  *      v 1.1 89/07/10 14:27:41 nelson Exp  SPRITE (DECWRL)
  47  *
  48  *      from: @(#)locore.s      8.5 (Berkeley) 1/4/94
  49  *      JNPR: support.S,v 1.5.2.2 2007/08/29 10:03:49 girish
  50  * $FreeBSD$
  51  */
  52
  53 /*
  54  * Copyright (c) 1997 Jonathan Stone (hereinafter referred to as the author)
  55  * All rights reserved.
  56  *
  57  * Redistribution and use in source and binary forms, with or without
  58  * modification, are permitted provided that the following conditions
  59  * are met:
  60  * 1. Redistributions of source code must retain the above copyright
  61  *    notice, this list of conditions and the following disclaimer.
  62  * 2. Redistributions in binary form must reproduce the above copyright
  63  *    notice, this list of conditions and the following disclaimer in the
  64  *    documentation and/or other materials provided with the distribution.
  65  * 3. All advertising materials mentioning features or use of this software
  66  *    must display the following acknowledgement:
  67  *      This product includes software developed by Jonathan R. Stone for
  68  *      the NetBSD Project.
  69  * 4. The name of the author may not be used to endorse or promote products
  70  *    derived from this software without specific prior written permission.
  71  *
  72  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
  73  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  74  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  75  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE
  76  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  77  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  78  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  79  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  80  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  81  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  82  * SUCH DAMAGE.
  83  */
  84
  85 /*
  86  *      Contains assembly language support routines.
  87  */
  88
  89 #include "opt_ddb.h"
  90 #include <sys/errno.h>
  91 #include <machine/asm.h>
  92 #include <machine/cpu.h>
  93 #include <machine/regnum.h>
  94 #include <machine/cpuregs.h>
  95 #include <machine/pcb.h>
  96
  97 #include "assym.inc"
  98
  99         .set    noreorder               # Noreorder is default style!
 100
 101 /*
 102  * Primitives
 103  */
 104
 105         .text
 106
 107 /*
 108  * Copy a null terminated string from the user address space into
 109  * the kernel address space.
 110  *
 111  *      copyinstr(fromaddr, toaddr, maxlength, &lencopied)
 112  *              caddr_t fromaddr;
 113  *              caddr_t toaddr;
 114  *              u_int maxlength;
 115  *              u_int *lencopied;
 116  */
 117 LEAF(copyinstr)
 118         PTR_LA          v0, __copyinstr_err
 119         blt             a0, zero, __copyinstr_err  # make sure address is in user space
 120         GET_CPU_PCPU(v1)
 121         PTR_L           v1, PC_CURPCB(v1)
 122         PTR_S           v0, U_PCB_ONFAULT(v1)
 123
 124         move            t0, a2
 125         beq             a2, zero, 4f
 126 1:
 127         lbu             v0, 0(a0)
 128         PTR_SUBU        a2, a2, 1
 129         beq             v0, zero, 2f
 130         sb              v0, 0(a1)               # each byte until NIL
 131         PTR_ADDU        a0, a0, 1
 132         bne             a2, zero, 1b            # less than maxlen
 133         PTR_ADDU        a1, a1, 1
 134 4:
 135         li              v0, ENAMETOOLONG        # run out of space
 136 2:
 137         beq             a3, zero, 3f            # return num. of copied bytes
 138         PTR_SUBU        a2, t0, a2              # if the 4th arg was non-NULL
 139         PTR_S           a2, 0(a3)
 140 3:
 141
 142         PTR_S           zero, U_PCB_ONFAULT(v1)
 143         j               ra
 144         nop
 145
 146 __copyinstr_err:
 147         j               ra
 148         li              v0, EFAULT
 149 END(copyinstr)
 150
 151 /*
 152  * Copy specified amount of data from user space into the kernel
 153  *      copyin(from, to, len)
 154  *              caddr_t *from;  (user source address)
 155  *              caddr_t *to;    (kernel destination address)
 156  *              unsigned len;
 157  */
 158 NESTED(copyin, CALLFRAME_SIZ, ra)
 159         PTR_SUBU        sp, sp, CALLFRAME_SIZ
 160         .mask   0x80000000, (CALLFRAME_RA - CALLFRAME_SIZ)
 161         PTR_LA  v0, copyerr
 162         blt     a0, zero, _C_LABEL(copyerr)  # make sure address is in user space
 163         REG_S   ra, CALLFRAME_RA(sp)
 164         GET_CPU_PCPU(v1)
 165         PTR_L   v1, PC_CURPCB(v1)
 166         jal     _C_LABEL(bcopy)
 167         PTR_S   v0, U_PCB_ONFAULT(v1)
 168         REG_L   ra, CALLFRAME_RA(sp)
 169         GET_CPU_PCPU(v1)
 170         PTR_L   v1, PC_CURPCB(v1)               # bcopy modified v1, so reload
 171         PTR_S   zero, U_PCB_ONFAULT(v1)
 172         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 173         j       ra
 174         move    v0, zero
 175 END(copyin)
 176
 177 /*
 178  * Copy specified amount of data from kernel to the user space
 179  *      copyout(from, to, len)
 180  *              caddr_t *from;  (kernel source address)
 181  *              caddr_t *to;    (user destination address)
 182  *              unsigned len;
 183  */
 184 NESTED(copyout, CALLFRAME_SIZ, ra)
 185         PTR_SUBU        sp, sp, CALLFRAME_SIZ
 186         .mask   0x80000000, (CALLFRAME_RA - CALLFRAME_SIZ)
 187         PTR_LA  v0, copyerr
 188         blt     a1, zero, _C_LABEL(copyerr) # make sure address is in user space
 189         REG_S   ra, CALLFRAME_RA(sp)
 190         GET_CPU_PCPU(v1)
 191         PTR_L   v1, PC_CURPCB(v1)
 192         jal     _C_LABEL(bcopy)
 193         PTR_S   v0, U_PCB_ONFAULT(v1)
 194         REG_L   ra, CALLFRAME_RA(sp)
 195         GET_CPU_PCPU(v1)
 196         PTR_L   v1, PC_CURPCB(v1)               # bcopy modified v1, so reload
 197         PTR_S   zero, U_PCB_ONFAULT(v1)
 198         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 199         j       ra
 200         move    v0, zero
 201 END(copyout)
 202
 203 LEAF(copyerr)
 204         REG_L   ra, CALLFRAME_RA(sp)
 205         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 206         j       ra
 207         li      v0, EFAULT                      # return error
 208 END(copyerr)
 209
 210 /*
 211  * {fu,su},{byte,sword,word}, fetch or store a byte, short or word to
 212  * user-space.
 213  */
 214 #ifdef __mips_n64
 215 LEAF(fueword64)
 216 XLEAF(fueword)
 217         PTR_LA  v0, fswberr
 218         blt     a0, zero, fswberr       # make sure address is in user space
 219         nop
 220         GET_CPU_PCPU(v1)
 221         PTR_L   v1, PC_CURPCB(v1)
 222         PTR_S   v0, U_PCB_ONFAULT(v1)
 223         ld      v0, 0(a0)               # fetch word
 224         PTR_S   zero, U_PCB_ONFAULT(v1)
 225         sd      v0, 0(a1)               # store word
 226         j       ra
 227         li      v0, 0
 228 END(fueword64)
 229 #endif
 230
 231 LEAF(fueword32)
 232 #ifndef __mips_n64
 233 XLEAF(fueword)
 234 #endif
 235         PTR_LA  v0, fswberr
 236         blt     a0, zero, fswberr       # make sure address is in user space
 237         nop
 238         GET_CPU_PCPU(v1)
 239         PTR_L   v1, PC_CURPCB(v1)
 240         PTR_S   v0, U_PCB_ONFAULT(v1)
 241         lw      v0, 0(a0)               # fetch word
 242         PTR_S   zero, U_PCB_ONFAULT(v1)
 243         sw      v0, 0(a1)               # store word
 244         j       ra
 245         li      v0, 0
 246 END(fueword32)
 247
 248 LEAF(fuesword)
 249         PTR_LA  v0, fswberr
 250         blt     a0, zero, fswberr       # make sure address is in user space
 251         nop
 252         GET_CPU_PCPU(v1)
 253         PTR_L   v1, PC_CURPCB(v1)
 254         PTR_S   v0, U_PCB_ONFAULT(v1)
 255         lhu     v0, 0(a0)               # fetch short
 256         PTR_S   zero, U_PCB_ONFAULT(v1)
 257         sh      v0, 0(a1)               # store short
 258         j       ra
 259         li      v0, 0
 260 END(fuesword)
 261
 262 LEAF(fubyte)
 263         PTR_LA  v0, fswberr
 264         blt     a0, zero, fswberr       # make sure address is in user space
 265         nop
 266         GET_CPU_PCPU(v1)
 267         PTR_L   v1, PC_CURPCB(v1)
 268         PTR_S   v0, U_PCB_ONFAULT(v1)
 269         lbu     v0, 0(a0)               # fetch byte
 270         j       ra
 271         PTR_S   zero, U_PCB_ONFAULT(v1)
 272 END(fubyte)
 273
 274 LEAF(suword32)
 275 #ifndef __mips_n64
 276 XLEAF(suword)
 277 #endif
 278         PTR_LA  v0, fswberr
 279         blt     a0, zero, fswberr       # make sure address is in user space
 280         nop
 281         GET_CPU_PCPU(v1)
 282         PTR_L   v1, PC_CURPCB(v1)
 283         PTR_S   v0, U_PCB_ONFAULT(v1)
 284         sw      a1, 0(a0)               # store word
 285         PTR_S   zero, U_PCB_ONFAULT(v1)
 286         j       ra
 287         move    v0, zero
 288 END(suword32)
 289
 290 #ifdef __mips_n64
 291 LEAF(suword64)
 292 XLEAF(suword)
 293         PTR_LA  v0, fswberr
 294         blt     a0, zero, fswberr       # make sure address is in user space
 295         nop
 296         GET_CPU_PCPU(v1)
 297         PTR_L   v1, PC_CURPCB(v1)
 298         PTR_S   v0, U_PCB_ONFAULT(v1)
 299         sd      a1, 0(a0)               # store word
 300         PTR_S   zero, U_PCB_ONFAULT(v1)
 301         j       ra
 302         move    v0, zero
 303 END(suword64)
 304 #endif
 305
 306 /*
 307  * casueword(9)
 308  * <v0>u_long casueword(<a0>u_long *p, <a1>u_long oldval, <a2>u_long *oldval_p,
 309  *                     <a3>u_long newval)
 310  */
 311 /*
 312  * casueword32(9)
 313  * <v0>uint32_t casueword(<a0>uint32_t *p, <a1>uint32_t oldval,
 314  *                       <a2>uint32_t newval)
 315  */
 316 LEAF(casueword32)
 317 #ifndef __mips_n64
 318 XLEAF(casueword)
 319 #endif
 320         PTR_LA  v0, fswberr
 321         blt     a0, zero, fswberr       # make sure address is in user space
 322         nop
 323         GET_CPU_PCPU(v1)
 324         PTR_L   v1, PC_CURPCB(v1)
 325         PTR_S   v0, U_PCB_ONFAULT(v1)
 326
 327         li      v0, 1
 328         move    t0, a3
 329         ll      t1, 0(a0)
 330         bne     a1, t1, 1f
 331         nop
 332         sc      t0, 0(a0)               # store word
 333         xori    v0, t0, 1
 334 1:
 335         PTR_S   zero, U_PCB_ONFAULT(v1)
 336         jr      ra
 337         sw      t1, 0(a2)               # unconditionally store old word
 338 END(casueword32)
 339
 340 #ifdef __mips_n64
 341 LEAF(casueword64)
 342 XLEAF(casueword)
 343         PTR_LA  v0, fswberr
 344         blt     a0, zero, fswberr       # make sure address is in user space
 345         nop
 346         GET_CPU_PCPU(v1)
 347         PTR_L   v1, PC_CURPCB(v1)
 348         PTR_S   v0, U_PCB_ONFAULT(v1)
 349
 350         li      v0, 1
 351         move    t0, a3
 352         lld     t1, 0(a0)
 353         bne     a1, t1, 1f
 354         nop
 355         scd     t0, 0(a0)               # store double word
 356         xori    v0, t0, 1
 357 1:
 358         PTR_S   zero, U_PCB_ONFAULT(v1)
 359         jr      ra
 360         sd      t1, 0(a2)               # unconditionally store old word
 361 END(casueword64)
 362 #endif
 363
 364 /*
 365  * Will have to flush the instruction cache if byte merging is done in hardware.
 366  */
 367 LEAF(susword)
 368         PTR_LA  v0, fswberr
 369         blt     a0, zero, fswberr       # make sure address is in user space
 370         nop
 371         GET_CPU_PCPU(v1)
 372         PTR_L   v1, PC_CURPCB(v1)
 373         PTR_S   v0, U_PCB_ONFAULT(v1)
 374         sh      a1, 0(a0)               # store short
 375         PTR_S   zero, U_PCB_ONFAULT(v1)
 376         j       ra
 377         move    v0, zero
 378 END(susword)
 379
 380 LEAF(subyte)
 381         PTR_LA  v0, fswberr
 382         blt     a0, zero, fswberr       # make sure address is in user space
 383         nop
 384         GET_CPU_PCPU(v1)
 385         PTR_L   v1, PC_CURPCB(v1)
 386         PTR_S   v0, U_PCB_ONFAULT(v1)
 387         sb      a1, 0(a0)               # store byte
 388         PTR_S   zero, U_PCB_ONFAULT(v1)
 389         j       ra
 390         move    v0, zero
 391 END(subyte)
 392
 393 LEAF(fswberr)
 394         j       ra
 395         li      v0, -1
 396 END(fswberr)
 397
 398 /*
 399  * memset(void *s1, int c, int len)
 400  * NetBSD: memset.S,v 1.3 2001/10/16 15:40:53 uch Exp
 401  */
 402 LEAF(memset)
 403         .set noreorder
 404         blt     a2, 12, memsetsmallclr  # small amount to clear?
 405         move    v0, a0                  # save s1 for result
 406
 407         sll     t1, a1, 8               # compute  c << 8 in t1
 408         or      t1, t1, a1              # compute c << 8 | c in 11
 409         sll     t2, t1, 16              # shift that left 16
 410         or      t1, t2, t1              # or together
 411
 412         PTR_SUBU        t0, zero, a0            # compute # bytes to word align address
 413         and     t0, t0, 3
 414         beq     t0, zero, 1f            # skip if word aligned
 415         PTR_SUBU        a2, a2, t0              # subtract from remaining count
 416         SWHI    t1, 0(a0)               # store 1, 2, or 3 bytes to align
 417         PTR_ADDU        a0, a0, t0
 418 1:
 419         and     v1, a2, 3               # compute number of whole words left
 420         PTR_SUBU        t0, a2, v1
 421         PTR_SUBU        a2, a2, t0
 422         PTR_ADDU        t0, t0, a0              # compute ending address
 423 2:
 424         PTR_ADDU        a0, a0, 4               # clear words
 425         bne     a0, t0, 2b              #  unrolling loop does not help
 426         sw      t1, -4(a0)              #  since we are limited by memory speed
 427
 428 memsetsmallclr:
 429         ble     a2, zero, 2f
 430         PTR_ADDU        t0, a2, a0              # compute ending address
 431 1:
 432         PTR_ADDU        a0, a0, 1               # clear bytes
 433         bne     a0, t0, 1b
 434         sb      a1, -1(a0)
 435 2:
 436         j       ra
 437         nop
 438         .set reorder
 439 END(memset)
 440
 441 /*
 442  * bzero(s1, n)
 443  */
 444 LEAF(bzero)
 445 XLEAF(blkclr)
 446         .set    noreorder
 447         blt     a1, 12, smallclr        # small amount to clear?
 448         PTR_SUBU        a3, zero, a0            # compute # bytes to word align address
 449         and     a3, a3, 3
 450         beq     a3, zero, 1f            # skip if word aligned
 451         PTR_SUBU        a1, a1, a3              # subtract from remaining count
 452         SWHI    zero, 0(a0)             # clear 1, 2, or 3 bytes to align
 453         PTR_ADDU        a0, a0, a3
 454 1:
 455         and     v0, a1, 3               # compute number of words left
 456         PTR_SUBU        a3, a1, v0
 457         move    a1, v0
 458         PTR_ADDU        a3, a3, a0              # compute ending address
 459 2:
 460         PTR_ADDU        a0, a0, 4               # clear words
 461         bne     a0, a3, 2b              #  unrolling loop does not help
 462         sw      zero, -4(a0)            #  since we are limited by memory speed
 463 smallclr:
 464         ble     a1, zero, 2f
 465         PTR_ADDU        a3, a1, a0              # compute ending address
 466 1:
 467         PTR_ADDU        a0, a0, 1               # clear bytes
 468         bne     a0, a3, 1b
 469         sb      zero, -1(a0)
 470 2:
 471         j       ra
 472         nop
 473 END(bzero)
 474
 475
 476 /*
 477  * bcmp(s1, s2, n)
 478  */
 479 LEAF(bcmp)
 480         .set    noreorder
 481         blt     a2, 16, smallcmp        # is it worth any trouble?
 482         xor     v0, a0, a1              # compare low two bits of addresses
 483         and     v0, v0, 3
 484         PTR_SUBU        a3, zero, a1            # compute # bytes to word align address
 485         bne     v0, zero, unalignedcmp  # not possible to align addresses
 486         and     a3, a3, 3
 487
 488         beq     a3, zero, 1f
 489         PTR_SUBU        a2, a2, a3              # subtract from remaining count
 490         move    v0, v1                  # init v0,v1 so unmodified bytes match
 491         LWHI    v0, 0(a0)               # read 1, 2, or 3 bytes
 492         LWHI    v1, 0(a1)
 493         PTR_ADDU        a1, a1, a3
 494         bne     v0, v1, nomatch
 495         PTR_ADDU        a0, a0, a3
 496 1:
 497         and     a3, a2, ~3              # compute number of whole words left
 498         PTR_SUBU        a2, a2, a3              #   which has to be >= (16-3) & ~3
 499         PTR_ADDU        a3, a3, a0              # compute ending address
 500 2:
 501         lw      v0, 0(a0)               # compare words
 502         lw      v1, 0(a1)
 503         PTR_ADDU        a0, a0, 4
 504         bne     v0, v1, nomatch
 505         PTR_ADDU        a1, a1, 4
 506         bne     a0, a3, 2b
 507         nop
 508         b       smallcmp                # finish remainder
 509         nop
 510 unalignedcmp:
 511         beq     a3, zero, 2f
 512         PTR_SUBU        a2, a2, a3              # subtract from remaining count
 513         PTR_ADDU        a3, a3, a0              # compute ending address
 514 1:
 515         lbu     v0, 0(a0)               # compare bytes until a1 word aligned
 516         lbu     v1, 0(a1)
 517         PTR_ADDU        a0, a0, 1
 518         bne     v0, v1, nomatch
 519         PTR_ADDU        a1, a1, 1
 520         bne     a0, a3, 1b
 521         nop
 522 2:
 523         and     a3, a2, ~3              # compute number of whole words left
 524         PTR_SUBU        a2, a2, a3              #   which has to be >= (16-3) & ~3
 525         PTR_ADDU        a3, a3, a0              # compute ending address
 526 3:
 527         LWHI    v0, 0(a0)               # compare words a0 unaligned, a1 aligned
 528         LWLO    v0, 3(a0)
 529         lw      v1, 0(a1)
 530         PTR_ADDU        a0, a0, 4
 531         bne     v0, v1, nomatch
 532         PTR_ADDU        a1, a1, 4
 533         bne     a0, a3, 3b
 534         nop
 535 smallcmp:
 536         ble     a2, zero, match
 537         PTR_ADDU        a3, a2, a0              # compute ending address
 538 1:
 539         lbu     v0, 0(a0)
 540         lbu     v1, 0(a1)
 541         PTR_ADDU        a0, a0, 1
 542         bne     v0, v1, nomatch
 543         PTR_ADDU        a1, a1, 1
 544         bne     a0, a3, 1b
 545         nop
 546 match:
 547         j       ra
 548          move   v0, zero
 549 nomatch:
 550         j       ra
 551         li      v0, 1
 552 END(bcmp)
 553
 554
 555 /*
 556  * bit = ffs(value)
 557  */
 558 LEAF(ffs)
 559         .set    noreorder
 560         beq     a0, zero, 2f
 561         move    v0, zero
 562 1:
 563         and     v1, a0, 1               # bit set?
 564         addu    v0, v0, 1
 565         beq     v1, zero, 1b            # no, continue
 566         srl     a0, a0, 1
 567 2:
 568         j       ra
 569         nop
 570 END(ffs)
 571
 572 /**
 573  * void
 574  * atomic_set_16(u_int16_t *a, u_int16_t b)
 575  * {
 576  *      *a |= b;
 577  * }
 578  */
 579 LEAF(atomic_set_16)
 580         .set    noreorder
 581         srl     a0, a0, 2       # round down address to be 32-bit aligned
 582         sll     a0, a0, 2
 583         andi    a1, a1, 0xffff
 584 1:
 585         ll      t0, 0(a0)
 586         or      t0, t0, a1
 587         sc      t0, 0(a0)
 588         beq     t0, zero, 1b
 589         nop
 590         j       ra
 591         nop
 592 END(atomic_set_16)
 593
 594 /**
 595  * void
 596  * atomic_clear_16(u_int16_t *a, u_int16_t b)
 597  * {
 598  *      *a &= ~b;
 599  * }
 600  */
 601 LEAF(atomic_clear_16)
 602         .set    noreorder
 603         srl     a0, a0, 2       # round down address to be 32-bit aligned
 604         sll     a0, a0, 2
 605         nor     a1, zero, a1
 606 1:
 607         ll      t0, 0(a0)
 608         move    t1, t0
 609         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 610         and     t1, t1, a1      # t1 has the new lower 16 bits
 611         srl     t0, t0, 16      # preserve original top 16 bits
 612         sll     t0, t0, 16
 613         or      t0, t0, t1
 614         sc      t0, 0(a0)
 615         beq     t0, zero, 1b
 616         nop
 617         j       ra
 618         nop
 619 END(atomic_clear_16)
 620
 621
 622 /**
 623  * void
 624  * atomic_subtract_16(uint16_t *a, uint16_t b)
 625  * {
 626  *      *a -= b;
 627  * }
 628  */
 629 LEAF(atomic_subtract_16)
 630         .set    noreorder
 631         srl     a0, a0, 2       # round down address to be 32-bit aligned
 632         sll     a0, a0, 2
 633 1:
 634         ll      t0, 0(a0)
 635         move    t1, t0
 636         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 637         subu    t1, t1, a1
 638         andi    t1, t1, 0xffff  # t1 has the new lower 16 bits
 639         srl     t0, t0, 16      # preserve original top 16 bits
 640         sll     t0, t0, 16
 641         or      t0, t0, t1
 642         sc      t0, 0(a0)
 643         beq     t0, zero, 1b
 644         nop
 645         j       ra
 646         nop
 647 END(atomic_subtract_16)
 648
 649 /**
 650  * void
 651  * atomic_add_16(uint16_t *a, uint16_t b)
 652  * {
 653  *      *a += b;
 654  * }
 655  */
 656 LEAF(atomic_add_16)
 657         .set    noreorder
 658         srl     a0, a0, 2       # round down address to be 32-bit aligned
 659         sll     a0, a0, 2
 660 1:
 661         ll      t0, 0(a0)
 662         move    t1, t0
 663         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 664         addu    t1, t1, a1
 665         andi    t1, t1, 0xffff  # t1 has the new lower 16 bits
 666         srl     t0, t0, 16      # preserve original top 16 bits
 667         sll     t0, t0, 16
 668         or      t0, t0, t1
 669         sc      t0, 0(a0)
 670         beq     t0, zero, 1b
 671         nop
 672         j       ra
 673         nop
 674 END(atomic_add_16)
 675
 676 /**
 677  * void
 678  * atomic_add_8(uint8_t *a, uint8_t b)
 679  * {
 680  *      *a += b;
 681  * }
 682  */
 683 LEAF(atomic_add_8)
 684         .set    noreorder
 685         srl     a0, a0, 2       # round down address to be 32-bit aligned
 686         sll     a0, a0, 2
 687 1:
 688         ll      t0, 0(a0)
 689         move    t1, t0
 690         andi    t1, t1, 0xff    # t1 has the original lower 8 bits
 691         addu    t1, t1, a1
 692         andi    t1, t1, 0xff    # t1 has the new lower 8 bits
 693         srl     t0, t0, 8       # preserve original top 24 bits
 694         sll     t0, t0, 8
 695         or      t0, t0, t1
 696         sc      t0, 0(a0)
 697         beq     t0, zero, 1b
 698         nop
 699         j       ra
 700         nop
 701 END(atomic_add_8)
 702
 703
 704 /**
 705  * void
 706  * atomic_subtract_8(uint8_t *a, uint8_t b)
 707  * {
 708  *      *a += b;
 709  * }
 710  */
 711 LEAF(atomic_subtract_8)
 712         .set    noreorder
 713         srl     a0, a0, 2       # round down address to be 32-bit aligned
 714         sll     a0, a0, 2
 715 1:
 716         ll      t0, 0(a0)
 717         move    t1, t0
 718         andi    t1, t1, 0xff    # t1 has the original lower 8 bits
 719         subu    t1, t1, a1
 720         andi    t1, t1, 0xff    # t1 has the new lower 8 bits
 721         srl     t0, t0, 8       # preserve original top 24 bits
 722         sll     t0, t0, 8
 723         or      t0, t0, t1
 724         sc      t0, 0(a0)
 725         beq     t0, zero, 1b
 726         nop
 727         j       ra
 728         nop
 729 END(atomic_subtract_8)
 730
 731         .set    noreorder               # Noreorder is default style!
 732
 733 #if defined(DDB) || defined(DEBUG)
 734
 735 LEAF(kdbpeek)
 736         PTR_LA  v1, ddberr
 737         and     v0, a0, 3                       # unaligned ?
 738         GET_CPU_PCPU(t1)
 739         PTR_L   t1, PC_CURPCB(t1)
 740         bne     v0, zero, 1f
 741         PTR_S   v1, U_PCB_ONFAULT(t1)
 742
 743         lw      v0, (a0)
 744         jr      ra
 745         PTR_S   zero, U_PCB_ONFAULT(t1)
 746
 747 1:
 748         LWHI    v0, 0(a0)
 749         LWLO    v0, 3(a0)
 750         jr      ra
 751         PTR_S   zero, U_PCB_ONFAULT(t1)
 752 END(kdbpeek)
 753
 754 LEAF(kdbpeekd)
 755         PTR_LA  v1, ddberr
 756         and     v0, a0, 3                       # unaligned ?
 757         GET_CPU_PCPU(t1)
 758         PTR_L   t1, PC_CURPCB(t1)
 759         bne     v0, zero, 1f
 760         PTR_S   v1, U_PCB_ONFAULT(t1)
 761
 762         ld      v0, (a0)
 763         jr      ra
 764         PTR_S   zero, U_PCB_ONFAULT(t1)
 765
 766 1:
 767         REG_LHI v0, 0(a0)
 768         REG_LLO v0, 7(a0)
 769         jr      ra
 770         PTR_S   zero, U_PCB_ONFAULT(t1)
 771 END(kdbpeekd)
 772
 773 ddberr:
 774         jr      ra
 775         nop
 776
 777 #if defined(DDB)
 778 LEAF(kdbpoke)
 779         PTR_LA  v1, ddberr
 780         and     v0, a0, 3                       # unaligned ?
 781         GET_CPU_PCPU(t1)
 782         PTR_L   t1, PC_CURPCB(t1)
 783         bne     v0, zero, 1f
 784         PTR_S   v1, U_PCB_ONFAULT(t1)
 785
 786         sw      a1, (a0)
 787         jr      ra
 788         PTR_S   zero, U_PCB_ONFAULT(t1)
 789
 790 1:
 791         SWHI    a1, 0(a0)
 792         SWLO    a1, 3(a0)
 793         jr      ra
 794         PTR_S   zero, U_PCB_ONFAULT(t1)
 795 END(kdbpoke)
 796
 797         .data
 798         .globl  esym
 799 esym:   .word   0
 800
 801 #endif /* DDB */
 802 #endif /* DDB || DEBUG */
 803
 804         .text
 805 LEAF(breakpoint)
 806         break   MIPS_BREAK_SOVER_VAL
 807         jr      ra
 808         nop
 809 END(breakpoint)
 810
 811 LEAF(setjmp)
 812         mfc0    v0, MIPS_COP_0_STATUS   # Later the "real" spl value!
 813         REG_S   s0, (SZREG * PCB_REG_S0)(a0)
 814         REG_S   s1, (SZREG * PCB_REG_S1)(a0)
 815         REG_S   s2, (SZREG * PCB_REG_S2)(a0)
 816         REG_S   s3, (SZREG * PCB_REG_S3)(a0)
 817         REG_S   s4, (SZREG * PCB_REG_S4)(a0)
 818         REG_S   s5, (SZREG * PCB_REG_S5)(a0)
 819         REG_S   s6, (SZREG * PCB_REG_S6)(a0)
 820         REG_S   s7, (SZREG * PCB_REG_S7)(a0)
 821         REG_S   s8, (SZREG * PCB_REG_S8)(a0)
 822         REG_S   sp, (SZREG * PCB_REG_SP)(a0)
 823         REG_S   ra, (SZREG * PCB_REG_RA)(a0)
 824         REG_S   v0, (SZREG * PCB_REG_SR)(a0)
 825         jr      ra
 826         li      v0, 0                   # setjmp return
 827 END(setjmp)
 828
 829 LEAF(longjmp)
 830         REG_L   v0, (SZREG * PCB_REG_SR)(a0)
 831         REG_L   ra, (SZREG * PCB_REG_RA)(a0)
 832         REG_L   s0, (SZREG * PCB_REG_S0)(a0)
 833         REG_L   s1, (SZREG * PCB_REG_S1)(a0)
 834         REG_L   s2, (SZREG * PCB_REG_S2)(a0)
 835         REG_L   s3, (SZREG * PCB_REG_S3)(a0)
 836         REG_L   s4, (SZREG * PCB_REG_S4)(a0)
 837         REG_L   s5, (SZREG * PCB_REG_S5)(a0)
 838         REG_L   s6, (SZREG * PCB_REG_S6)(a0)
 839         REG_L   s7, (SZREG * PCB_REG_S7)(a0)
 840         REG_L   s8, (SZREG * PCB_REG_S8)(a0)
 841         REG_L   sp, (SZREG * PCB_REG_SP)(a0)
 842         mtc0    v0, MIPS_COP_0_STATUS   # Later the "real" spl value!
 843         ITLBNOPFIX
 844         jr      ra
 845         li      v0, 1                   # longjmp return
 846 END(longjmp)