/*
 * M7memset.S: SPARC M7 optimized memset.
 *
 * Copyright (c) 2016, Oracle and/or its affiliates.  All rights reserved.
 */

/*
 * M7memset.S: M7 optimized memset.
 *
 * char *memset(sp, c, n)
 *
 * Set an array of n chars starting at sp to the character c.
 * Return sp.
 *
 * Fast assembler language version of the following C-program for memset
 * which represents the `standard' for the C-library.
 *
 *      void *
 *      memset(void *sp1, int c, size_t n)
 *      {
 *          if (n != 0) {
 *              char *sp = sp1;
 *              do {
 *                  *sp++ = (char)c;
 *              } while (--n != 0);
 *          }
 *          return (sp1);
 *      }
 *
 * The algorithm is as follows :
 *
 *      For small 6 or fewer bytes stores, bytes will be stored.
 *
 *      For less than 32 bytes stores, align the address on 4 byte boundary.
 *      Then store as many 4-byte chunks, followed by trailing bytes.
 *
 *      For sizes greater than 32 bytes, align the address on 8 byte boundary.
 *      if (count >= 64) {
 *              store 8-bytes chunks to align the address on 64 byte boundary
 *              if (value to be set is zero && count >= MIN_ZERO) {
 *                      Using BIS stores, set the first long word of each
 *                      64-byte cache line to zero which will also clear the
 *                      other seven long words of the cache line.
 *              }
 *              else if (count >= MIN_LOOP) {
 *                      Using BIS stores, set the first long word of each of
 *                      ST_CHUNK cache lines (64 bytes each) before the main
 *                      loop is entered.
 *                      In the main loop, continue pre-setting the first long
 *                      word of each cache line ST_CHUNK lines in advance while
 *                      setting the other seven long words (56 bytes) of each
 *                      cache line until fewer than ST_CHUNK*64 bytes remain.
 *                      Then set the remaining seven long words of each cache
 *                      line that has already had its first long word set.
 *              }
 *              store remaining data in 64-byte chunks until less than
 *              64 bytes remain.
 *       }
 *       Store as many 8-byte chunks, followed by trailing bytes.
 *
 * BIS = Block Init Store
 *   Doing the advance store of the first element of the cache line
 *   initiates the displacement of a cache line while only using a single
 *   instruction in the pipeline. That avoids various pipeline delays,
 *   such as filling the miss buffer. The performance effect is
 *   similar to prefetching for normal stores.
 *   The special case for zero fills runs faster and uses fewer instruction
 *   cycles than the normal memset loop.
 *
 * We only use BIS for memset of greater than MIN_LOOP bytes because a sequence
 * BIS stores must be followed by a membar #StoreStore. The benefit of
 * the BIS store must be balanced against the cost of the membar operation.
 */

/*
 * ASI_STBI_P marks the cache line as "least recently used"
 * which means if many threads are active, it has a high chance
 * of being pushed out of the cache between the first initializing
 * store and the final stores.
 * Thus, we use ASI_STBIMRU_P which marks the cache line as
 * "most recently used" for all but the last store to the cache line.
 */

#include <asm/asi.h>
#include <asm/page.h>

#define ASI_STBI_P      ASI_BLK_INIT_QUAD_LDD_P
#define ASI_STBIMRU_P   ASI_ST_BLKINIT_MRU_P


#define ST_CHUNK        24   /* multiple of 4 due to loop unrolling */
#define MIN_LOOP        16320
#define MIN_ZERO        512

        .section        ".text"
        .align          32

/*
 * Define clear_page(dest) as memset(dest, 0, PAGE_SIZE)
 * (can create a more optimized version later.)
 */
        .globl          M7clear_page
        .globl          M7clear_user_page
M7clear_page:           /* clear_page(dest) */
M7clear_user_page:
        set     PAGE_SIZE, %o1
        /* fall through into bzero code */

        .size           M7clear_page,.-M7clear_page
        .size           M7clear_user_page,.-M7clear_user_page

/*
 * Define bzero(dest, n) as memset(dest, 0, n)
 * (can create a more optimized version later.)
 */
        .globl          M7bzero
M7bzero:                /* bzero(dest, size) */
        mov     %o1, %o2
        mov     0, %o1
        /* fall through into memset code */

        .size           M7bzero,.-M7bzero

        .global         M7memset
        .type           M7memset, #function
        .register       %g3, #scratch
M7memset:
        mov     %o0, %o5                ! copy sp1 before using it
        cmp     %o2, 7                  ! if small counts, just write bytes
        bleu,pn %xcc, .wrchar
         and     %o1, 0xff, %o1          ! o1 is (char)c

        sll     %o1, 8, %o3
        or      %o1, %o3, %o1           ! now o1 has 2 bytes of c
        sll     %o1, 16, %o3
        cmp     %o2, 32
        blu,pn  %xcc, .wdalign
         or      %o1, %o3, %o1           ! now o1 has 4 bytes of c

        sllx    %o1, 32, %o3
        or      %o1, %o3, %o1           ! now o1 has 8 bytes of c

.dbalign:
        andcc   %o5, 7, %o3             ! is sp1 aligned on a 8 byte bound?
        bz,pt   %xcc, .blkalign         ! already long word aligned
         sub     %o3, 8, %o3             ! -(bytes till long word aligned)

        add     %o2, %o3, %o2           ! update o2 with new count
        ! Set -(%o3) bytes till sp1 long word aligned
1:      stb     %o1, [%o5]              ! there is at least 1 byte to set
        inccc   %o3                     ! byte clearing loop
        bl,pt   %xcc, 1b
         inc     %o5

        ! Now sp1 is long word aligned (sp1 is found in %o5)
.blkalign:
        cmp     %o2, 64                 ! check if there are 64 bytes to set
        blu,pn  %xcc, .wrshort
         mov     %o2, %o3

        andcc   %o5, 63, %o3            ! is sp1 block aligned?
        bz,pt   %xcc, .blkwr            ! now block aligned
         sub     %o3, 64, %o3            ! o3 is -(bytes till block aligned)
        add     %o2, %o3, %o2           ! o2 is the remainder

        ! Store -(%o3) bytes till dst is block (64 byte) aligned.
        ! Use long word stores.
        ! Recall that dst is already long word aligned
1:
        addcc   %o3, 8, %o3
        stx     %o1, [%o5]
        bl,pt   %xcc, 1b
         add     %o5, 8, %o5

        ! Now sp1 is block aligned
.blkwr:
        andn    %o2, 63, %o4            ! calculate size of blocks in bytes
        brz,pn  %o1, .wrzero            ! special case if c == 0
         and     %o2, 63, %o3            ! %o3 = bytes left after blk stores.

        set     MIN_LOOP, %g1
        cmp     %o4, %g1                ! check there are enough bytes to set
        blu,pn  %xcc, .short_set        ! to justify cost of membar
                                        ! must be > pre-cleared lines
         nop

        ! initial cache-clearing stores
        ! get store pipeline moving
        rd      %asi, %g3               ! save %asi to be restored later
        wr     %g0, ASI_STBIMRU_P, %asi

        ! Primary memset loop for large memsets
.wr_loop:
        sub     %o5, 8, %o5             ! adjust %o5 for ASI store alignment
        mov     ST_CHUNK, %g1
.wr_loop_start:
        stxa    %o1, [%o5+8]%asi
        subcc   %g1, 4, %g1
        stxa    %o1, [%o5+8+64]%asi
        add     %o5, 256, %o5
        stxa    %o1, [%o5+8-128]%asi
        bgu     %xcc, .wr_loop_start
         stxa    %o1, [%o5+8-64]%asi

        sub     %o5, ST_CHUNK*64, %o5   ! reset %o5
        mov     ST_CHUNK, %g1

.wr_loop_rest:
        stxa    %o1, [%o5+8+8]%asi
        sub     %o4, 64, %o4
        stxa    %o1, [%o5+16+8]%asi
        subcc   %g1, 1, %g1
        stxa    %o1, [%o5+24+8]%asi
        stxa    %o1, [%o5+32+8]%asi
        stxa    %o1, [%o5+40+8]%asi
        add     %o5, 64, %o5
        stxa    %o1, [%o5-8]%asi
        bgu     %xcc, .wr_loop_rest
         stxa    %o1, [%o5]ASI_STBI_P

        ! If more than ST_CHUNK*64 bytes remain to set, continue
        ! setting the first long word of each cache line in advance
        ! to keep the store pipeline moving.

        cmp     %o4, ST_CHUNK*64
        bge,pt  %xcc, .wr_loop_start
         mov     ST_CHUNK, %g1

        brz,a,pn %o4, .asi_done
         add     %o5, 8, %o5             ! restore %o5 offset

.wr_loop_small:
        stxa    %o1, [%o5+8]%asi
        stxa    %o1, [%o5+8+8]%asi
        stxa    %o1, [%o5+16+8]%asi
        stxa    %o1, [%o5+24+8]%asi
        stxa    %o1, [%o5+32+8]%asi
        subcc   %o4, 64, %o4
        stxa    %o1, [%o5+40+8]%asi
        add     %o5, 64, %o5
        stxa    %o1, [%o5-8]%asi
        bgu,pt  %xcc, .wr_loop_small
         stxa    %o1, [%o5]ASI_STBI_P

        ba      .asi_done
         add     %o5, 8, %o5             ! restore %o5 offset

        ! Special case loop for zero fill memsets
        ! For each 64 byte cache line, single STBI to first element
        ! clears line
.wrzero:
        cmp     %o4, MIN_ZERO           ! check if enough bytes to set
                                        ! to pay %asi + membar cost
        blu     %xcc, .short_set
         nop
        sub     %o4, 256, %o4

.wrzero_loop:
        mov     64, %g3
        stxa    %o1, [%o5]ASI_STBI_P
        subcc   %o4, 256, %o4
        stxa    %o1, [%o5+%g3]ASI_STBI_P
        add     %o5, 256, %o5
        sub     %g3, 192, %g3
        stxa    %o1, [%o5+%g3]ASI_STBI_P
        add %g3, 64, %g3
        bge,pt  %xcc, .wrzero_loop
         stxa    %o1, [%o5+%g3]ASI_STBI_P
        add     %o4, 256, %o4

        brz,pn  %o4, .bsi_done
         nop

.wrzero_small:
        stxa    %o1, [%o5]ASI_STBI_P
        subcc   %o4, 64, %o4
        bgu,pt  %xcc, .wrzero_small
         add     %o5, 64, %o5
        ba,a    .bsi_done

.asi_done:
        wr      %g3, 0x0, %asi          ! restored saved %asi
.bsi_done:
        membar  #StoreStore             ! required by use of Block Store Init

.short_set:
        cmp     %o4, 64                 ! check if 64 bytes to set
        blu     %xcc, 5f
         nop
4:                                      ! set final blocks of 64 bytes
        stx     %o1, [%o5]
        stx     %o1, [%o5+8]
        stx     %o1, [%o5+16]
        stx     %o1, [%o5+24]
        subcc   %o4, 64, %o4
        stx     %o1, [%o5+32]
        stx     %o1, [%o5+40]
        add     %o5, 64, %o5
        stx     %o1, [%o5-16]
        bgu,pt  %xcc, 4b
         stx     %o1, [%o5-8]

5:
        ! Set the remaining long words
.wrshort:
        subcc   %o3, 8, %o3             ! Can we store any long words?
        blu,pn  %xcc, .wrchars
         and     %o2, 7, %o2             ! calc bytes left after long words
6:
        subcc   %o3, 8, %o3
        stx     %o1, [%o5]              ! store the long words
        bgeu,pt %xcc, 6b
         add     %o5, 8, %o5

.wrchars:                               ! check for extra chars
        brnz    %o2, .wrfin
         nop
        retl
         nop

.wdalign:
        andcc   %o5, 3, %o3             ! is sp1 aligned on a word boundary
        bz,pn   %xcc, .wrword
         andn    %o2, 3, %o3             ! create word sized count in %o3

        dec     %o2                     ! decrement count
        stb     %o1, [%o5]              ! clear a byte
        b       .wdalign
         inc     %o5                     ! next byte

.wrword:
        subcc   %o3, 4, %o3
        st      %o1, [%o5]              ! 4-byte writing loop
        bnz,pt  %xcc, .wrword
         add     %o5, 4, %o5

        and     %o2, 3, %o2             ! leftover count, if any

.wrchar:
        ! Set the remaining bytes, if any
        brz     %o2, .exit
         nop
.wrfin:
        deccc   %o2
        stb     %o1, [%o5]
        bgu,pt  %xcc, .wrfin
         inc     %o5
.exit:
        retl                            ! %o0 was preserved
         nop

        .size           M7memset,.-M7memset