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  * int copystr(void *kfaddr, void *kdaddr, size_t maxlen, size_t *lencopied)
 109  * Copy a NIL-terminated string, at most maxlen characters long.  Return the
 110  * number of characters copied (including the NIL) in *lencopied.  If the
 111  * string is too long, return ENAMETOOLONG; else return 0.
 112  */
 113 LEAF(copystr)
 114         move            t0, a2
 115         beq             a2, zero, 4f
 116 1:
 117         lbu             v0, 0(a0)
 118         PTR_SUBU        a2, a2, 1
 119         beq             v0, zero, 2f
 120         sb              v0, 0(a1)               # each byte until NIL
 121         PTR_ADDU        a0, a0, 1
 122         bne             a2, zero, 1b            # less than maxlen
 123         PTR_ADDU        a1, a1, 1
 124 4:
 125         li              v0, ENAMETOOLONG        # run out of space
 126 2:
 127         beq             a3, zero, 3f            # return num. of copied bytes
 128         PTR_SUBU        a2, t0, a2              # if the 4th arg was non-NULL
 129         PTR_S           a2, 0(a3)
 130 3:
 131         j               ra                      # v0 is 0 or ENAMETOOLONG
 132         nop
 133 END(copystr)
 134
 135
 136 /*
 137  * Copy a null terminated string from the user address space into
 138  * the kernel address space.
 139  *
 140  *      copyinstr(fromaddr, toaddr, maxlength, &lencopied)
 141  *              caddr_t fromaddr;
 142  *              caddr_t toaddr;
 143  *              u_int maxlength;
 144  *              u_int *lencopied;
 145  */
 146 NESTED(copyinstr, CALLFRAME_SIZ, ra)
 147         PTR_SUBU        sp, sp, CALLFRAME_SIZ
 148         .mask   0x80000000, (CALLFRAME_RA - CALLFRAME_SIZ)
 149         PTR_LA  v0, copyerr
 150         blt     a0, zero, _C_LABEL(copyerr)  # make sure address is in user space
 151         REG_S   ra, CALLFRAME_RA(sp)
 152         GET_CPU_PCPU(v1)
 153         PTR_L   v1, PC_CURPCB(v1)
 154         jal     _C_LABEL(copystr)
 155         PTR_S   v0, U_PCB_ONFAULT(v1)
 156         REG_L   ra, CALLFRAME_RA(sp)
 157         GET_CPU_PCPU(v1)
 158         PTR_L   v1, PC_CURPCB(v1)
 159         PTR_S   zero, U_PCB_ONFAULT(v1)
 160         j       ra
 161         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 162 END(copyinstr)
 163
 164 /*
 165  * Copy specified amount of data from user space into the kernel
 166  *      copyin(from, to, len)
 167  *              caddr_t *from;  (user source address)
 168  *              caddr_t *to;    (kernel destination address)
 169  *              unsigned len;
 170  */
 171 NESTED(copyin, CALLFRAME_SIZ, ra)
 172         PTR_SUBU        sp, sp, CALLFRAME_SIZ
 173         .mask   0x80000000, (CALLFRAME_RA - CALLFRAME_SIZ)
 174         PTR_LA  v0, copyerr
 175         blt     a0, zero, _C_LABEL(copyerr)  # make sure address is in user space
 176         REG_S   ra, CALLFRAME_RA(sp)
 177         GET_CPU_PCPU(v1)
 178         PTR_L   v1, PC_CURPCB(v1)
 179         jal     _C_LABEL(bcopy)
 180         PTR_S   v0, U_PCB_ONFAULT(v1)
 181         REG_L   ra, CALLFRAME_RA(sp)
 182         GET_CPU_PCPU(v1)
 183         PTR_L   v1, PC_CURPCB(v1)               # bcopy modified v1, so reload
 184         PTR_S   zero, U_PCB_ONFAULT(v1)
 185         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 186         j       ra
 187         move    v0, zero
 188 END(copyin)
 189
 190 /*
 191  * Copy specified amount of data from kernel to the user space
 192  *      copyout(from, to, len)
 193  *              caddr_t *from;  (kernel source address)
 194  *              caddr_t *to;    (user destination address)
 195  *              unsigned len;
 196  */
 197 NESTED(copyout, CALLFRAME_SIZ, ra)
 198         PTR_SUBU        sp, sp, CALLFRAME_SIZ
 199         .mask   0x80000000, (CALLFRAME_RA - CALLFRAME_SIZ)
 200         PTR_LA  v0, copyerr
 201         blt     a1, zero, _C_LABEL(copyerr) # make sure address is in user space
 202         REG_S   ra, CALLFRAME_RA(sp)
 203         GET_CPU_PCPU(v1)
 204         PTR_L   v1, PC_CURPCB(v1)
 205         jal     _C_LABEL(bcopy)
 206         PTR_S   v0, U_PCB_ONFAULT(v1)
 207         REG_L   ra, CALLFRAME_RA(sp)
 208         GET_CPU_PCPU(v1)
 209         PTR_L   v1, PC_CURPCB(v1)               # bcopy modified v1, so reload
 210         PTR_S   zero, U_PCB_ONFAULT(v1)
 211         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 212         j       ra
 213         move    v0, zero
 214 END(copyout)
 215
 216 LEAF(copyerr)
 217         REG_L   ra, CALLFRAME_RA(sp)
 218         PTR_ADDU        sp, sp, CALLFRAME_SIZ
 219         j       ra
 220         li      v0, EFAULT                      # return error
 221 END(copyerr)
 222
 223 /*
 224  * {fu,su},{byte,sword,word}, fetch or store a byte, short or word to
 225  * user-space.
 226  */
 227 #ifdef __mips_n64
 228 LEAF(fueword64)
 229 XLEAF(fueword)
 230         PTR_LA  v0, fswberr
 231         blt     a0, zero, fswberr       # make sure address is in user space
 232         nop
 233         GET_CPU_PCPU(v1)
 234         PTR_L   v1, PC_CURPCB(v1)
 235         PTR_S   v0, U_PCB_ONFAULT(v1)
 236         ld      v0, 0(a0)               # fetch word
 237         PTR_S   zero, U_PCB_ONFAULT(v1)
 238         sd      v0, 0(a1)               # store word
 239         j       ra
 240         li      v0, 0
 241 END(fueword64)
 242 #endif
 243
 244 LEAF(fueword32)
 245 #ifndef __mips_n64
 246 XLEAF(fueword)
 247 #endif
 248         PTR_LA  v0, fswberr
 249         blt     a0, zero, fswberr       # make sure address is in user space
 250         nop
 251         GET_CPU_PCPU(v1)
 252         PTR_L   v1, PC_CURPCB(v1)
 253         PTR_S   v0, U_PCB_ONFAULT(v1)
 254         lw      v0, 0(a0)               # fetch word
 255         PTR_S   zero, U_PCB_ONFAULT(v1)
 256         sw      v0, 0(a1)               # store word
 257         j       ra
 258         li      v0, 0
 259 END(fueword32)
 260
 261 LEAF(fuesword)
 262         PTR_LA  v0, fswberr
 263         blt     a0, zero, fswberr       # make sure address is in user space
 264         nop
 265         GET_CPU_PCPU(v1)
 266         PTR_L   v1, PC_CURPCB(v1)
 267         PTR_S   v0, U_PCB_ONFAULT(v1)
 268         lhu     v0, 0(a0)               # fetch short
 269         PTR_S   zero, U_PCB_ONFAULT(v1)
 270         sh      v0, 0(a1)               # store short
 271         j       ra
 272         li      v0, 0
 273 END(fuesword)
 274
 275 LEAF(fubyte)
 276         PTR_LA  v0, fswberr
 277         blt     a0, zero, fswberr       # make sure address is in user space
 278         nop
 279         GET_CPU_PCPU(v1)
 280         PTR_L   v1, PC_CURPCB(v1)
 281         PTR_S   v0, U_PCB_ONFAULT(v1)
 282         lbu     v0, 0(a0)               # fetch byte
 283         j       ra
 284         PTR_S   zero, U_PCB_ONFAULT(v1)
 285 END(fubyte)
 286
 287 LEAF(suword32)
 288 #ifndef __mips_n64
 289 XLEAF(suword)
 290 #endif
 291         PTR_LA  v0, fswberr
 292         blt     a0, zero, fswberr       # make sure address is in user space
 293         nop
 294         GET_CPU_PCPU(v1)
 295         PTR_L   v1, PC_CURPCB(v1)
 296         PTR_S   v0, U_PCB_ONFAULT(v1)
 297         sw      a1, 0(a0)               # store word
 298         PTR_S   zero, U_PCB_ONFAULT(v1)
 299         j       ra
 300         move    v0, zero
 301 END(suword32)
 302
 303 #ifdef __mips_n64
 304 LEAF(suword64)
 305 XLEAF(suword)
 306         PTR_LA  v0, fswberr
 307         blt     a0, zero, fswberr       # make sure address is in user space
 308         nop
 309         GET_CPU_PCPU(v1)
 310         PTR_L   v1, PC_CURPCB(v1)
 311         PTR_S   v0, U_PCB_ONFAULT(v1)
 312         sd      a1, 0(a0)               # store word
 313         PTR_S   zero, U_PCB_ONFAULT(v1)
 314         j       ra
 315         move    v0, zero
 316 END(suword64)
 317 #endif
 318
 319 /*
 320  * casueword(9)
 321  * <v0>u_long casueword(<a0>u_long *p, <a1>u_long oldval, <a2>u_long *oldval_p,
 322  *                     <a3>u_long newval)
 323  */
 324 /*
 325  * casueword32(9)
 326  * <v0>uint32_t casueword(<a0>uint32_t *p, <a1>uint32_t oldval,
 327  *                       <a2>uint32_t newval)
 328  */
 329 LEAF(casueword32)
 330 #ifndef __mips_n64
 331 XLEAF(casueword)
 332 #endif
 333         PTR_LA  v0, fswberr
 334         blt     a0, zero, fswberr       # make sure address is in user space
 335         nop
 336         GET_CPU_PCPU(v1)
 337         PTR_L   v1, PC_CURPCB(v1)
 338         PTR_S   v0, U_PCB_ONFAULT(v1)
 339 1:
 340         move    t0, a3
 341         ll      t1, 0(a0)
 342         bne     a1, t1, 2f
 343         nop
 344         sc      t0, 0(a0)               # store word
 345         beqz    t0, 1b
 346         nop
 347         j       3f
 348         li      v0, 0
 349 2:
 350         li      v0, -1
 351 3:
 352         PTR_S   zero, U_PCB_ONFAULT(v1)
 353         jr      ra
 354         sw      t1, 0(a2)               # unconditionally store old word
 355 END(casueword32)
 356
 357 #ifdef __mips_n64
 358 LEAF(casueword64)
 359 XLEAF(casueword)
 360         PTR_LA  v0, fswberr
 361         blt     a0, zero, fswberr       # make sure address is in user space
 362         nop
 363         GET_CPU_PCPU(v1)
 364         PTR_L   v1, PC_CURPCB(v1)
 365         PTR_S   v0, U_PCB_ONFAULT(v1)
 366 1:
 367         move    t0, a3
 368         lld     t1, 0(a0)
 369         bne     a1, t1, 2f
 370         nop
 371         scd     t0, 0(a0)               # store double word
 372         beqz    t0, 1b
 373         nop
 374         j       3f
 375         li      v0, 0
 376 2:
 377         li      v0, -1
 378 3:
 379         PTR_S   zero, U_PCB_ONFAULT(v1)
 380         jr      ra
 381         sd      t1, 0(a2)               # unconditionally store old word
 382 END(casueword64)
 383 #endif
 384
 385 /*
 386  * Will have to flush the instruction cache if byte merging is done in hardware.
 387  */
 388 LEAF(susword)
 389         PTR_LA  v0, fswberr
 390         blt     a0, zero, fswberr       # make sure address is in user space
 391         nop
 392         GET_CPU_PCPU(v1)
 393         PTR_L   v1, PC_CURPCB(v1)
 394         PTR_S   v0, U_PCB_ONFAULT(v1)
 395         sh      a1, 0(a0)               # store short
 396         PTR_S   zero, U_PCB_ONFAULT(v1)
 397         j       ra
 398         move    v0, zero
 399 END(susword)
 400
 401 LEAF(subyte)
 402         PTR_LA  v0, fswberr
 403         blt     a0, zero, fswberr       # make sure address is in user space
 404         nop
 405         GET_CPU_PCPU(v1)
 406         PTR_L   v1, PC_CURPCB(v1)
 407         PTR_S   v0, U_PCB_ONFAULT(v1)
 408         sb      a1, 0(a0)               # store byte
 409         PTR_S   zero, U_PCB_ONFAULT(v1)
 410         j       ra
 411         move    v0, zero
 412 END(subyte)
 413
 414 LEAF(fswberr)
 415         j       ra
 416         li      v0, -1
 417 END(fswberr)
 418
 419 /*
 420  * memset(void *s1, int c, int len)
 421  * NetBSD: memset.S,v 1.3 2001/10/16 15:40:53 uch Exp
 422  */
 423 LEAF(memset)
 424         .set noreorder
 425         blt     a2, 12, memsetsmallclr  # small amount to clear?
 426         move    v0, a0                  # save s1 for result
 427
 428         sll     t1, a1, 8               # compute  c << 8 in t1
 429         or      t1, t1, a1              # compute c << 8 | c in 11
 430         sll     t2, t1, 16              # shift that left 16
 431         or      t1, t2, t1              # or together
 432
 433         PTR_SUBU        t0, zero, a0            # compute # bytes to word align address
 434         and     t0, t0, 3
 435         beq     t0, zero, 1f            # skip if word aligned
 436         PTR_SUBU        a2, a2, t0              # subtract from remaining count
 437         SWHI    t1, 0(a0)               # store 1, 2, or 3 bytes to align
 438         PTR_ADDU        a0, a0, t0
 439 1:
 440         and     v1, a2, 3               # compute number of whole words left
 441         PTR_SUBU        t0, a2, v1
 442         PTR_SUBU        a2, a2, t0
 443         PTR_ADDU        t0, t0, a0              # compute ending address
 444 2:
 445         PTR_ADDU        a0, a0, 4               # clear words
 446         bne     a0, t0, 2b              #  unrolling loop does not help
 447         sw      t1, -4(a0)              #  since we are limited by memory speed
 448
 449 memsetsmallclr:
 450         ble     a2, zero, 2f
 451         PTR_ADDU        t0, a2, a0              # compute ending address
 452 1:
 453         PTR_ADDU        a0, a0, 1               # clear bytes
 454         bne     a0, t0, 1b
 455         sb      a1, -1(a0)
 456 2:
 457         j       ra
 458         nop
 459         .set reorder
 460 END(memset)
 461
 462 /*
 463  * bzero(s1, n)
 464  */
 465 LEAF(bzero)
 466 XLEAF(blkclr)
 467         .set    noreorder
 468         blt     a1, 12, smallclr        # small amount to clear?
 469         PTR_SUBU        a3, zero, a0            # compute # bytes to word align address
 470         and     a3, a3, 3
 471         beq     a3, zero, 1f            # skip if word aligned
 472         PTR_SUBU        a1, a1, a3              # subtract from remaining count
 473         SWHI    zero, 0(a0)             # clear 1, 2, or 3 bytes to align
 474         PTR_ADDU        a0, a0, a3
 475 1:
 476         and     v0, a1, 3               # compute number of words left
 477         PTR_SUBU        a3, a1, v0
 478         move    a1, v0
 479         PTR_ADDU        a3, a3, a0              # compute ending address
 480 2:
 481         PTR_ADDU        a0, a0, 4               # clear words
 482         bne     a0, a3, 2b              #  unrolling loop does not help
 483         sw      zero, -4(a0)            #  since we are limited by memory speed
 484 smallclr:
 485         ble     a1, zero, 2f
 486         PTR_ADDU        a3, a1, a0              # compute ending address
 487 1:
 488         PTR_ADDU        a0, a0, 1               # clear bytes
 489         bne     a0, a3, 1b
 490         sb      zero, -1(a0)
 491 2:
 492         j       ra
 493         nop
 494 END(bzero)
 495
 496
 497 /*
 498  * bcmp(s1, s2, n)
 499  */
 500 LEAF(bcmp)
 501         .set    noreorder
 502         blt     a2, 16, smallcmp        # is it worth any trouble?
 503         xor     v0, a0, a1              # compare low two bits of addresses
 504         and     v0, v0, 3
 505         PTR_SUBU        a3, zero, a1            # compute # bytes to word align address
 506         bne     v0, zero, unalignedcmp  # not possible to align addresses
 507         and     a3, a3, 3
 508
 509         beq     a3, zero, 1f
 510         PTR_SUBU        a2, a2, a3              # subtract from remaining count
 511         move    v0, v1                  # init v0,v1 so unmodified bytes match
 512         LWHI    v0, 0(a0)               # read 1, 2, or 3 bytes
 513         LWHI    v1, 0(a1)
 514         PTR_ADDU        a1, a1, a3
 515         bne     v0, v1, nomatch
 516         PTR_ADDU        a0, a0, a3
 517 1:
 518         and     a3, a2, ~3              # compute number of whole words left
 519         PTR_SUBU        a2, a2, a3              #   which has to be >= (16-3) & ~3
 520         PTR_ADDU        a3, a3, a0              # compute ending address
 521 2:
 522         lw      v0, 0(a0)               # compare words
 523         lw      v1, 0(a1)
 524         PTR_ADDU        a0, a0, 4
 525         bne     v0, v1, nomatch
 526         PTR_ADDU        a1, a1, 4
 527         bne     a0, a3, 2b
 528         nop
 529         b       smallcmp                # finish remainder
 530         nop
 531 unalignedcmp:
 532         beq     a3, zero, 2f
 533         PTR_SUBU        a2, a2, a3              # subtract from remaining count
 534         PTR_ADDU        a3, a3, a0              # compute ending address
 535 1:
 536         lbu     v0, 0(a0)               # compare bytes until a1 word aligned
 537         lbu     v1, 0(a1)
 538         PTR_ADDU        a0, a0, 1
 539         bne     v0, v1, nomatch
 540         PTR_ADDU        a1, a1, 1
 541         bne     a0, a3, 1b
 542         nop
 543 2:
 544         and     a3, a2, ~3              # compute number of whole words left
 545         PTR_SUBU        a2, a2, a3              #   which has to be >= (16-3) & ~3
 546         PTR_ADDU        a3, a3, a0              # compute ending address
 547 3:
 548         LWHI    v0, 0(a0)               # compare words a0 unaligned, a1 aligned
 549         LWLO    v0, 3(a0)
 550         lw      v1, 0(a1)
 551         PTR_ADDU        a0, a0, 4
 552         bne     v0, v1, nomatch
 553         PTR_ADDU        a1, a1, 4
 554         bne     a0, a3, 3b
 555         nop
 556 smallcmp:
 557         ble     a2, zero, match
 558         PTR_ADDU        a3, a2, a0              # compute ending address
 559 1:
 560         lbu     v0, 0(a0)
 561         lbu     v1, 0(a1)
 562         PTR_ADDU        a0, a0, 1
 563         bne     v0, v1, nomatch
 564         PTR_ADDU        a1, a1, 1
 565         bne     a0, a3, 1b
 566         nop
 567 match:
 568         j       ra
 569          move   v0, zero
 570 nomatch:
 571         j       ra
 572         li      v0, 1
 573 END(bcmp)
 574
 575
 576 /*
 577  * bit = ffs(value)
 578  */
 579 LEAF(ffs)
 580         .set    noreorder
 581         beq     a0, zero, 2f
 582         move    v0, zero
 583 1:
 584         and     v1, a0, 1               # bit set?
 585         addu    v0, v0, 1
 586         beq     v1, zero, 1b            # no, continue
 587         srl     a0, a0, 1
 588 2:
 589         j       ra
 590         nop
 591 END(ffs)
 592
 593 /**
 594  * void
 595  * atomic_set_16(u_int16_t *a, u_int16_t b)
 596  * {
 597  *      *a |= b;
 598  * }
 599  */
 600 LEAF(atomic_set_16)
 601         .set    noreorder
 602         srl     a0, a0, 2       # round down address to be 32-bit aligned
 603         sll     a0, a0, 2
 604         andi    a1, a1, 0xffff
 605 1:
 606         ll      t0, 0(a0)
 607         or      t0, t0, a1
 608         sc      t0, 0(a0)
 609         beq     t0, zero, 1b
 610         nop
 611         j       ra
 612         nop
 613 END(atomic_set_16)
 614
 615 /**
 616  * void
 617  * atomic_clear_16(u_int16_t *a, u_int16_t b)
 618  * {
 619  *      *a &= ~b;
 620  * }
 621  */
 622 LEAF(atomic_clear_16)
 623         .set    noreorder
 624         srl     a0, a0, 2       # round down address to be 32-bit aligned
 625         sll     a0, a0, 2
 626         nor     a1, zero, a1
 627 1:
 628         ll      t0, 0(a0)
 629         move    t1, t0
 630         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 631         and     t1, t1, a1      # t1 has the new lower 16 bits
 632         srl     t0, t0, 16      # preserve original top 16 bits
 633         sll     t0, t0, 16
 634         or      t0, t0, t1
 635         sc      t0, 0(a0)
 636         beq     t0, zero, 1b
 637         nop
 638         j       ra
 639         nop
 640 END(atomic_clear_16)
 641
 642
 643 /**
 644  * void
 645  * atomic_subtract_16(uint16_t *a, uint16_t b)
 646  * {
 647  *      *a -= b;
 648  * }
 649  */
 650 LEAF(atomic_subtract_16)
 651         .set    noreorder
 652         srl     a0, a0, 2       # round down address to be 32-bit aligned
 653         sll     a0, a0, 2
 654 1:
 655         ll      t0, 0(a0)
 656         move    t1, t0
 657         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 658         subu    t1, t1, a1
 659         andi    t1, t1, 0xffff  # t1 has the new lower 16 bits
 660         srl     t0, t0, 16      # preserve original top 16 bits
 661         sll     t0, t0, 16
 662         or      t0, t0, t1
 663         sc      t0, 0(a0)
 664         beq     t0, zero, 1b
 665         nop
 666         j       ra
 667         nop
 668 END(atomic_subtract_16)
 669
 670 /**
 671  * void
 672  * atomic_add_16(uint16_t *a, uint16_t b)
 673  * {
 674  *      *a += b;
 675  * }
 676  */
 677 LEAF(atomic_add_16)
 678         .set    noreorder
 679         srl     a0, a0, 2       # round down address to be 32-bit aligned
 680         sll     a0, a0, 2
 681 1:
 682         ll      t0, 0(a0)
 683         move    t1, t0
 684         andi    t1, t1, 0xffff  # t1 has the original lower 16 bits
 685         addu    t1, t1, a1
 686         andi    t1, t1, 0xffff  # t1 has the new lower 16 bits
 687         srl     t0, t0, 16      # preserve original top 16 bits
 688         sll     t0, t0, 16
 689         or      t0, t0, t1
 690         sc      t0, 0(a0)
 691         beq     t0, zero, 1b
 692         nop
 693         j       ra
 694         nop
 695 END(atomic_add_16)
 696
 697 /**
 698  * void
 699  * atomic_add_8(uint8_t *a, uint8_t b)
 700  * {
 701  *      *a += b;
 702  * }
 703  */
 704 LEAF(atomic_add_8)
 705         .set    noreorder
 706         srl     a0, a0, 2       # round down address to be 32-bit aligned
 707         sll     a0, a0, 2
 708 1:
 709         ll      t0, 0(a0)
 710         move    t1, t0
 711         andi    t1, t1, 0xff    # t1 has the original lower 8 bits
 712         addu    t1, t1, a1
 713         andi    t1, t1, 0xff    # t1 has the new lower 8 bits
 714         srl     t0, t0, 8       # preserve original top 24 bits
 715         sll     t0, t0, 8
 716         or      t0, t0, t1
 717         sc      t0, 0(a0)
 718         beq     t0, zero, 1b
 719         nop
 720         j       ra
 721         nop
 722 END(atomic_add_8)
 723
 724
 725 /**
 726  * void
 727  * atomic_subtract_8(uint8_t *a, uint8_t b)
 728  * {
 729  *      *a += b;
 730  * }
 731  */
 732 LEAF(atomic_subtract_8)
 733         .set    noreorder
 734         srl     a0, a0, 2       # round down address to be 32-bit aligned
 735         sll     a0, a0, 2
 736 1:
 737         ll      t0, 0(a0)
 738         move    t1, t0
 739         andi    t1, t1, 0xff    # t1 has the original lower 8 bits
 740         subu    t1, t1, a1
 741         andi    t1, t1, 0xff    # t1 has the new lower 8 bits
 742         srl     t0, t0, 8       # preserve original top 24 bits
 743         sll     t0, t0, 8
 744         or      t0, t0, t1
 745         sc      t0, 0(a0)
 746         beq     t0, zero, 1b
 747         nop
 748         j       ra
 749         nop
 750 END(atomic_subtract_8)
 751
 752         .set    noreorder               # Noreorder is default style!
 753
 754 #if defined(DDB) || defined(DEBUG)
 755
 756 LEAF(kdbpeek)
 757         PTR_LA  v1, ddberr
 758         and     v0, a0, 3                       # unaligned ?
 759         GET_CPU_PCPU(t1)
 760         PTR_L   t1, PC_CURPCB(t1)
 761         bne     v0, zero, 1f
 762         PTR_S   v1, U_PCB_ONFAULT(t1)
 763
 764         lw      v0, (a0)
 765         jr      ra
 766         PTR_S   zero, U_PCB_ONFAULT(t1)
 767
 768 1:
 769         LWHI    v0, 0(a0)
 770         LWLO    v0, 3(a0)
 771         jr      ra
 772         PTR_S   zero, U_PCB_ONFAULT(t1)
 773 END(kdbpeek)
 774
 775 LEAF(kdbpeekd)
 776         PTR_LA  v1, ddberr
 777         and     v0, a0, 3                       # unaligned ?
 778         GET_CPU_PCPU(t1)
 779         PTR_L   t1, PC_CURPCB(t1)
 780         bne     v0, zero, 1f
 781         PTR_S   v1, U_PCB_ONFAULT(t1)
 782
 783         ld      v0, (a0)
 784         jr      ra
 785         PTR_S   zero, U_PCB_ONFAULT(t1)
 786
 787 1:
 788         REG_LHI v0, 0(a0)
 789         REG_LLO v0, 7(a0)
 790         jr      ra
 791         PTR_S   zero, U_PCB_ONFAULT(t1)
 792 END(kdbpeekd)
 793
 794 ddberr:
 795         jr      ra
 796         nop
 797
 798 #if defined(DDB)
 799 LEAF(kdbpoke)
 800         PTR_LA  v1, ddberr
 801         and     v0, a0, 3                       # unaligned ?
 802         GET_CPU_PCPU(t1)
 803         PTR_L   t1, PC_CURPCB(t1)
 804         bne     v0, zero, 1f
 805         PTR_S   v1, U_PCB_ONFAULT(t1)
 806
 807         sw      a1, (a0)
 808         jr      ra
 809         PTR_S   zero, U_PCB_ONFAULT(t1)
 810
 811 1:
 812         SWHI    a1, 0(a0)
 813         SWLO    a1, 3(a0)
 814         jr      ra
 815         PTR_S   zero, U_PCB_ONFAULT(t1)
 816 END(kdbpoke)
 817
 818         .data
 819         .globl  esym
 820 esym:   .word   0
 821
 822 #endif /* DDB */
 823 #endif /* DDB || DEBUG */
 824
 825         .text
 826 LEAF(breakpoint)
 827         break   MIPS_BREAK_SOVER_VAL
 828         jr      ra
 829         nop
 830 END(breakpoint)
 831
 832 LEAF(setjmp)
 833         mfc0    v0, MIPS_COP_0_STATUS   # Later the "real" spl value!
 834         REG_S   s0, (SZREG * PCB_REG_S0)(a0)
 835         REG_S   s1, (SZREG * PCB_REG_S1)(a0)
 836         REG_S   s2, (SZREG * PCB_REG_S2)(a0)
 837         REG_S   s3, (SZREG * PCB_REG_S3)(a0)
 838         REG_S   s4, (SZREG * PCB_REG_S4)(a0)
 839         REG_S   s5, (SZREG * PCB_REG_S5)(a0)
 840         REG_S   s6, (SZREG * PCB_REG_S6)(a0)
 841         REG_S   s7, (SZREG * PCB_REG_S7)(a0)
 842         REG_S   s8, (SZREG * PCB_REG_S8)(a0)
 843         REG_S   sp, (SZREG * PCB_REG_SP)(a0)
 844         REG_S   ra, (SZREG * PCB_REG_RA)(a0)
 845         REG_S   v0, (SZREG * PCB_REG_SR)(a0)
 846         jr      ra
 847         li      v0, 0                   # setjmp return
 848 END(setjmp)
 849
 850 LEAF(longjmp)
 851         REG_L   v0, (SZREG * PCB_REG_SR)(a0)
 852         REG_L   ra, (SZREG * PCB_REG_RA)(a0)
 853         REG_L   s0, (SZREG * PCB_REG_S0)(a0)
 854         REG_L   s1, (SZREG * PCB_REG_S1)(a0)
 855         REG_L   s2, (SZREG * PCB_REG_S2)(a0)
 856         REG_L   s3, (SZREG * PCB_REG_S3)(a0)
 857         REG_L   s4, (SZREG * PCB_REG_S4)(a0)
 858         REG_L   s5, (SZREG * PCB_REG_S5)(a0)
 859         REG_L   s6, (SZREG * PCB_REG_S6)(a0)
 860         REG_L   s7, (SZREG * PCB_REG_S7)(a0)
 861         REG_L   s8, (SZREG * PCB_REG_S8)(a0)
 862         REG_L   sp, (SZREG * PCB_REG_SP)(a0)
 863         mtc0    v0, MIPS_COP_0_STATUS   # Later the "real" spl value!
 864         ITLBNOPFIX
 865         jr      ra
 866         li      v0, 1                   # longjmp return
 867 END(longjmp)