gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T-Kernel 2.0 Software Package
    4:  *
    5:  *    Copyright 2011 by Ken Sakamura.
    6:  *    This software is distributed under the latest version of T-License 2.x.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by T-Engine Forum(http://www.t-engine.org/) at 2011/05/17.
   10:  *    Modified by T-Engine Forum at 2013/02/12.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      bitsearch1.c (libsys)
   18:  */
   19: 
   20: #include <machine.h>
   21: #include <sys/bitop.h>
   22: 
   23: /*
   24:  * Retrieve bit 1 in word-aligned array.
   25:  *      A faster version of BitSearch1 available in case where base is word-aligned.
   26:  *      This implementation uses binary search algorithm for efficiency.
   27:  */
   28: EXPORT W BitSearch1_w( const UW *base, W offset, W width )
   29: {
   30:         UW     *cp, v;
   31:         W      position;
   32: 
   33:         cp = (UW*)base;
   34:         cp += offset / 32;
   35:         if ( offset & 31 ) {
   36: #if BIGENDIAN
   37:                 v = *cp & (((UW)1U << (32 - (offset & 31))) - 1);
   38: #else
   39:                 v = *cp & ~(((UW)1U << (offset & 31)) - 1);
   40: #endif
   41:         } else {
   42:                 v = *cp;
   43:         }
   44: 
   45:         position = 0;
   46:         while ( position < width ) {
   47:                 if ( v ) {            /* includes 1 --> search bit of 1 */
   48:                         if ( !position ) position -= (offset & 31);
   49: #if BIGENDIAN
   50:                         if ( !(v & 0xffff0000U) ) {
   51:                                 v <<= 16;
   52:                                 position += 16;
   53:                         }
   54:                         if ( !(v & 0xff000000U) ) {
   55:                                 v <<= 8;
   56:                                 position += 8;
   57:                         }
   58:                         if ( !(v & 0xf0000000U) ) {
   59:                                 v <<= 4;
   60:                                 position += 4;
   61:                         }
   62:                         if ( !(v & 0xc0000000U) ) {
   63:                                 v <<= 2;
   64:                                 position += 2;
   65:                         }
   66:                         if ( !(v & 0x80000000U) ) {
   67:                                 ++position;
   68:                         }
   69: #else
   70:                         if ( !(v & 0xffffU) ) {
   71:                                 v >>= 16;
   72:                                 position += 16;
   73:                         }
   74:                         if ( !(v & 0xffU) ) {
   75:                                 v >>= 8;
   76:                                 position += 8;
   77:                         }
   78:                         if ( !(v & 0xfU) ) {
   79:                                 v >>= 4;
   80:                                 position += 4;
   81:                         }
   82:                         if ( !(v & 0x3U) ) {
   83:                                 v >>= 2;
   84:                                 position += 2;
   85:                         }
   86:                         if ( !(v & 0x1U) ) {
   87:                                 ++position;
   88:                         }
   89: #endif
   90:                         if ( position < width ) {
   91:                                 return position;
   92:                         } else {
   93:                                 return -1;
   94:                         }
   95:                 } else {              /* all bits are 0 --> 1 Word skip */
   96:                         if ( position ) {
   97:                                 position += 32;
   98:                         } else {
   99:                                 position = 32 - (offset & 31);
  100:                         }
  101:                         v = *++cp;
  102:                 }
  103:         }
  104: 
  105:         return -1;
  106: }
  107: 
  108: /*
  109:  * Retrieve bit 1.
  110:  *      Retrieve the width bits from the position of specified bit and return the position.
  111:  *      If not found, return -1.
  112:  *      The found position is returned in relative position from the retrieval start position.
  113:  *      When found, the return value is between 0 and width-1.
  114:  */
  115: EXPORT W BitSearch1( const void *base, W offset, W width )
  116: {
  117: #if ALLOW_MISALIGN
  118:         return BitSearch1_w((const UW*)base, offset, width);
  119: #else
  120:         /* Align base to the left word boundary, then use BitSearch1_w */
  121:         UINT   wofs = ((UINT)base) & 3;
  122:         return BitSearch1_w((const UW*)((const UB*)base - wofs), offset + wofs * 8, width);
  123: #endif
  124: }