/* SCE CONFIDENTIAL
PLAYSTATION(R)3 Programmer Tool Runtime Library 154.001
* Copyright (C) 2004,2006 Sony Computer Entertainment Inc. 
* All Rights Reserved.
*/
_FUNC_DEF(char *,strncat,(char *_Restrict s1, const char *_Restrict s2, size_t n))
{
  vec_uchar16 mask;

  // !!!TREVOR substituting this for the ones=spu_orc(ones,ones) makes no difference
  // making this "const" or not slows the unaligned case by 5 cycles, speeds up the aligned by 3
  
  const vec_uchar16 ones = VEC_UCHAR16(
      0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
      0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff);
  const vec_uchar16 zeros = VEC_UCHAR16(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
  vec_uint4 gather;
  vec_uchar16 result;
  vec_uchar16 tsrc;
  
  // Some of the loops below expect n to be at least 1

  if(n <= 0) return(s1);

  // Convert incoming char pointers to (possiblly unaligned) vector chars

  vec_uchar16       *vdestination = (vec_uchar16 *)s1;
  const vec_uchar16 *vsource      = (const vec_uchar16 *)s2;

  // Find the zero terminator in the dest string

  vec_uchar16 pattern = __Shuffles[((unsigned)vdestination & 15)];
  do
  {
    // Do an unaligned fetch of the next quadword
    tsrc = spu_shuffle(*vdestination, *(vdestination+1), pattern);
    vdestination++;
    // result bytes are all ones if a source byte is zero
    result = spu_cmpeq(tsrc, zeros);
    // gather the lsb from each byte into preferred slot of "gather"
    gather = spu_gather(result);
    // keep looping till we find a zero
  } while (si_to_uint((qword)gather) == 0);

  // Back up destination so it points to the zero terminator
  
  unsigned int zero_pos = si_to_uint((qword)spu_cntlz(gather));
  vdestination = (vec_uchar16 *)(((unsigned char *)vdestination) - (16 - (zero_pos-16)));
  
  // Now we use the strncat algorithm (without the zero padding feature)
  // Get the byte offset in the quadword for source and dest
  
  unsigned srcshift  = ((unsigned)vsource) & 0xf;
  unsigned destshift = ((unsigned)vdestination) & 0xf;

  // Fast path for quadword aligned copies

  n++;
  if(srcshift == 0 && destshift == 0)
  {
    // Copy aligned quadwords till we hit a zero or 'n' runs out
    while(1)
    {
      tsrc = *(vsource++);
      // Check for a zero in the source
      result = spu_cmpeq(tsrc, zeros);
      gather = spu_gather(result);
      // keep copying till we hit a zero terminator or get to the last quadword, then break 
      if ((si_to_uint((qword)gather) == 0) && (n > 16))
      {
        *(vdestination++) = tsrc;
      }
      else
      {
        break;
      }
      n -= 16;
    }
    // Find position of the zero terminator
    unsigned int term_pos = si_to_uint((qword)spu_cntlz(gather))-15;
    // N could still be big, so we have to check it
    if(n < term_pos)
    {
      // This mask is inclusive of the nth byte
      vec_uchar16 mask_n    = spu_rlmaskqwbyte(ones, -n);
      // This mask is everything up to the nth byte
      vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -(n-1));
      // Zero the from the nth byte on inclusive
      tsrc = spu_sel(tsrc, zeros, mask_zero);
      // Move everything up to and including the nth byte
      *vdestination = spu_sel(tsrc, *vdestination, mask_n);
    }
    else
    {
      // Make a mask that will include the zero terminator
      vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -term_pos);
      *vdestination = spu_sel(tsrc, *vdestination, mask_zero);
    }
    return(s1);
  }

  // This is the path for unaligned quadwords

  unsigned remainder = 16-destshift;     // this is how many bytes we could copy during the first pass
  mask = spu_slqwbyte(ones, remainder);  // mask puts ones where we want to save the leading dest data
  unsigned endshift = destshift+n;       // if < 16 we will be copying just one quadword
  
  // If we don't have enough source data to fill the trailing part of the quadword, we have to put ones
  // in the mask where we want to save the trailing destination data too.

  if(endshift < 16)
  {
    mask = spu_or(mask,spu_rlmaskqwbyte(ones, -(endshift)));
  }

  // Align the source with the destination and put it in tsrc

  int maskshift = (int)srcshift - (int)destshift;
  
  if(maskshift <=0)
  {
    // In this case the source has enough bytes to fill the dest, so we don't need to shuffle
    // Right shift the source to line it up with the destination
    tsrc = spu_rlmaskqwbyte(*vsource,maskshift);
  }
  else
  {
    // In this case we have to get bytes from source & source+1 to get enough bytes to fill the dest,
    // so we do a shuffle that gets the bytes and shifts them left at the same time
    vec_uchar16 pattern1 = __Shuffles[maskshift];
    tsrc = spu_shuffle(*vsource, *(vsource+1), pattern1);
  }
  
  // Check for a zero terminator in the source
  result = spu_cmpeq(tsrc, zeros);
  // Ignore matches in bytes we're not going to copy
  result = spu_andc(result, mask);
  gather = spu_gather(result);
  // Generate a mask that would exclude things after a found zero
  unsigned int term_pos          = si_to_uint((qword)spu_cntlz(gather))-15;
  vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -term_pos);
  // This will shorten our mask to stop on the zero terminator
  mask = spu_or(mask,mask_zero);

  if(endshift < 16)
  {
    // Tnen we need to force the last byte to be zero
    vec_uchar16 mask_n = spu_rlmaskqwbyte(ones,-(endshift-1));
    tsrc = spu_sel(tsrc,zeros,mask_n);
  }

  // combine the source and destination and save it
  *vdestination = spu_sel(tsrc, *vdestination, mask);

  // exit now if this was the last quadword
  
  if(endshift < 16)
  {
     return(s1);
  }
  
  // Advance everything by the number of bytes we just copied
  // This will align the destination address but may NOT align the source
  // This is intentional and used to fetch the right shuffle pattern

  n -= remainder;
  vsource = (const vec_uchar16 *)(((const unsigned char *)vsource) + remainder);
  vdestination = (vec_uchar16 *)(((unsigned char *)vdestination) + remainder);

  // Load the shuffle pattern we need for the rest of the copy

  pattern = __Shuffles[((unsigned)vsource & 15)];

  // Copy however many full words there are left

  while (1) 
  {
    // Get source word
    tsrc = spu_shuffle(*vsource, *(vsource+1), pattern);
    // Check for a zero terminator
    result = spu_cmpeq(tsrc, zeros);
    gather = spu_gather(result);
    if ((si_to_uint((qword)gather) == 0) && (n > 16))
    {
      *(vdestination++) = tsrc;
    }
    else
    {
      break;
    }
    vsource++;
    n -= 16;
  }
  
  // Find position of the zero terminator
  term_pos = si_to_uint((qword)spu_cntlz(gather))-15;
  // N could still be big, so we have to check it
  if(n < term_pos)
  {
    // This mask is inclusive of the nth byte
    vec_uchar16 mask_n    = spu_rlmaskqwbyte(ones, -n);
    // This mask is everything up to the nth byte
    vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -(n-1));
    // Zero the from the nth byte on inclusive
    tsrc = spu_sel(tsrc, zeros, mask_zero);
    // Move everything up to and including the nth byte
    *vdestination = spu_sel(tsrc, *vdestination, mask_n);
  }
  else
  {
    // Make a mask that will include the zero terminator
    vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -term_pos);
    *vdestination = spu_sel(tsrc, *vdestination, mask_zero);
  }

return(s1);

  // Which endgame do we do?
  if(n <= 16)
  {
    // In this case, N has run out so the value in tsrc will be the last one we touch
    // Make a mask so we only access the last "n" bytes
    vec_uchar16 mask_n    = spu_rlmaskqwbyte(ones, -n);
    // Find position of zero terminator (if any) and make a mask to zero everything after it
    unsigned int term_pos          = si_to_uint((qword)spu_cntlz(gather))-16;
    vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -term_pos);
    // Zero out everything after the zero terminator
    tsrc = spu_sel(tsrc, zeros, mask_zero);
    // Copy into the last "n" bytes
    *vdestination = spu_sel(tsrc, *vdestination, mask_n);
    return(s1);
  }
  else
  {
    // Here we hit a zero terminator before the N ran out, so we need to zero fill the
    // remaining quadwords right up to the end
    // Find position of zero terminator make a mask and zero everything after it
    unsigned int term_pos = si_to_uint((qword)spu_cntlz(gather))-16;
    vec_uchar16 mask_zero = spu_rlmaskqwbyte(ones, -term_pos);
    *(vdestination++) = spu_sel(tsrc, zeros, mask_zero);
    n -= 16;
    while(1)
    {
      if(n > 16)
      {
        *(vdestination++) = zeros;
      }
      else
      {
        // Only zero the last 'n' bytes
        vec_uchar16 mask_n = spu_rlmaskqwbyte(ones, -n);
        *vdestination = spu_sel(zeros, *vdestination, mask_n);
        return(s1);
      }
      n-=16;
    }
  }
  return(s1);
}
