gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    micro T-Kernel 3.00.00
    4:  *
    5:  *    Copyright (C) 2006-2019 by Ken Sakamura.
    6:  *    This software is distributed under the T-License 2.1.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by TRON Forum(http://www.tron.org) at 2019/12/11.
   10:  *
   11:  *----------------------------------------------------------------------
   12:  */
   13: 
   14: /*
   15:  *      bitop.c
   16:  *      T-Kernel standard library
   17:  */
   18: 
   19: #include <tk/tkernel.h>
   20: 
   21: /*** macros ***/
   22: /* bit operation macro */
   23: #if BIGENDIAN
   24: #define _BIT_SET_N(n) ( (UB)0x80 >> ((n) & 7) )
   25: #define _BIT_SHIFT(n) ( (UB)n >> 1 )
   26: #else
   27: #define _BIT_SET_N(n) ( (UB)0x01 << ((n) & 7) )
   28: #define _BIT_SHIFT(n) ( (UB)n << 1 )
   29: #endif
   30: 
   31: 
   32: /*** bit operation ***/
   33: #ifdef USE_FUNC_TSTDLIB_BITCLR
   34: /* tstdlib_bitclr : clear specified bit */
   35: void
   36: knl_bitclr( void *base, W offset )
   37: {
   38:         register UB *cp, mask;
   39: 
   40:         if (offset < 0) {
   41:                 return;
   42:         }
   43: 
   44:         cp = (UB*)base;
   45:         cp += offset / 8;
   46: 
   47:         mask = _BIT_SET_N(offset);
   48: 
   49:         *cp &= ~mask;
   50: }
   51: #endif /* USE_FUNC_TSTDLIB_BITCLR */
   52: 
   53: #ifdef USE_FUNC_TSTDLIB_BITSET
   54: /* tstdlib_bitset : set specified bit */
   55: void
   56: knl_bitset( void *base, W offset )
   57: {
   58:         register UB *cp, mask;
   59: 
   60:         if (offset < 0) {
   61:                 return;
   62:         }
   63: 
   64:         cp = (UB*)base;
   65:         cp += offset / 8;
   66: 
   67:         mask = _BIT_SET_N(offset);
   68: 
   69:         *cp |= mask;
   70: }
   71: #endif /* USE_FUNC_TSTDLIB_BITSET */
   72: 
   73: #ifdef USE_FUNC_TSTDLIB_BITSEARCH1
   74: /* tstdlib_bitsearch1 : perform 1 search on bit string */
   75: W
   76: knl_bitsearch1( void *base, W offset, W width )
   77: {
   78:         register UB *cp, mask;
   79:         register W position;
   80: 
   81:         if ((offset < 0) || (width < 0)) {
   82:                 return -1;
   83:         }
   84: 
   85:         cp = (UB*)base;
   86:         cp += offset / 8;
   87: 
   88:         position = 0;
   89:         mask = _BIT_SET_N(offset);
   90: 
   91:         while (position < width) {
   92:                 if (*cp) {            /* includes 1 --> search bit of 1 */
   93:                         while (1) {
   94:                                 if (*cp & mask) {
   95:                                         if (position < width) {
   96:                                                 return position;
   97:                                         } else {
   98:                                                 return -1;
   99:                                         }
  100:                                 }
  101:                                 mask = _BIT_SHIFT(mask);
  102:                                 ++position;
  103:                         }
  104:                 } else {              /* all bits are 0 --> 1 Byte skip */
  105:                         if (position) {
  106:                                 position += 8;
  107:                         } else {
  108:                                 position = 8 - (offset & 7);
  109:                                 mask = _BIT_SET_N(0);
  110:                         }
  111:                         cp++;
  112:                 }
  113:         }
  114: 
  115:         return -1;
  116: }
  117: #endif /* USE_FUNC_TSTDLIB_BITSEARCH1 */