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 2014/09/10.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      @(#)string.c (libstr)
   18:  *      Standard string library (for kernel link)
   19:  */
   20: 
   21: /*
   22:  *      This file is linked to the kernel only when the
   23:  *      C standard library is not used.
   24:  */
   25: 
   26: #include <basic.h>
   27: #include <tk/tkernel.h>
   28: #include <libstr.h>
   29: 
   30: /* Define weak alias symbol */
   31: #if     _Csym == 0
   32: #define weak_alias(nm, act)     __asm__(".weak "  #nm " ; "  #nm " = "  #act)
   33: #else
   34: #define weak_alias(nm, act)     __asm__(".weak _" #nm " ; _" #nm " = _" #act)
   35: #endif
   36: 
   37: #if     use_libstr_memcpy_implicit
   38:         weak_alias(memcpy, tkl_memcpy);
   39: #endif  /* use_libstr_memcpy_implicit */
   40: 
   41: #if     use_libstr_func_as_std
   42:         weak_alias(memset, tkl_memset);
   43:         weak_alias(memcmp, tkl_memcmp);
   44:         weak_alias(memmove, tkl_memmove);
   45:         weak_alias(strlen, tkl_strlen);
   46:         weak_alias(strcmp, tkl_strcmp);
   47:         weak_alias(strcpy, tkl_strcpy);
   48:         weak_alias(strncpy, tkl_strncpy);
   49:         weak_alias(strcat, tkl_strcat);
   50:         weak_alias(strncat, tkl_strncat);
   51: #endif  /* use_libstr_func_as_std */
   52: 
   53: /* memory access size */
   54: #define MASZ    (sizeof(unsigned long))
   55: #define MAMSK   (MASZ - 1)
   56: 
   57: /* memory access pointer */
   58: typedef union {
   59:         unsigned char  *cp;
   60:         unsigned long  *lp;
   61:         unsigned long  lv;
   62: } MPTR;
   63: 
   64: /*
   65:  * tkl_memset : fill memory area
   66:  */
   67: void* tkl_memset( void *s, int c, size_t n )
   68: {
   69:         MPTR           m;
   70:         size_t         cnt;
   71:         unsigned long  val;
   72: 
   73:         m.cp = (unsigned char *)s;
   74:         val = (unsigned char)c;
   75: 
   76:         cnt = m.lv & MAMSK;
   77:         if ( cnt > 0 ) {
   78:                 /* Not aligned in WASZ bytes */
   79:                 if ( n < MASZ * 2) {
   80:                         cnt = n;
   81:                 } else {
   82:                         cnt = MASZ - cnt;
   83:                 }
   84:                 /* Fill preceding bytes to align */
   85:                 n -= cnt;
   86:                 do {
   87:                         *m.cp++ = (unsigned char)val;
   88:                 } while ( --cnt > 0 );
   89:         }
   90: 
   91:         /* Fill in WASZ bytes unit */
   92:         if ( n >= MASZ ) {
   93:                 cnt = n / MASZ;
   94:                 n &= MAMSK;
   95:                 val |= val << 8;
   96:                 val |= val << 16;
   97:                 do {
   98:                         *m.lp++ = val;
   99:                 } while ( --cnt > 0 );
  100:         }
  101: 
  102:         /* Fill trailing bytes */
  103:         while ( n-- > 0 ) {
  104:                 *m.cp++ = (unsigned char)val;
  105:         }
  106:         return s;
  107: }
  108: 
  109: /*
  110:  * tkl_memcmp : perform memory comparison
  111:  */
  112: int tkl_memcmp( const void *s1, const void *s2, size_t n )
  113: {
  114:         int    result;
  115:         const unsigned char    *p1 = s1;
  116:         const unsigned char    *p2 = s2;
  117: 
  118:         while ( n-- > 0 ) {
  119:                 result = *p1++ - *p2++;
  120:                 if ( result != 0 ) return result;
  121:         }
  122:         return 0;
  123: }
  124: 
  125: /*
  126:  * tkl_memcpy : copy memory
  127:  */
  128: void* tkl_memcpy( void *dst, const void *src, size_t n )
  129: {
  130:         MPTR   s, d;
  131:         size_t cnt;
  132: 
  133:         d.cp = (unsigned char *)dst;
  134:         s.cp = (unsigned char *)src;
  135: 
  136:         if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  137:                 /* Not aligned in WASZ bytes */
  138:                 if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2) {
  139:                         /* Full copy in a byte unit */
  140:                         cnt = n;
  141:                 } else {
  142:                         /* Copy preceding bytes to align */
  143:                         cnt = MASZ - (s.lv & MAMSK);
  144:                 }
  145:                 /* Copy in a byte unit */
  146:                 n -= cnt;
  147:                 do {
  148:                         *d.cp++ = *s.cp++;
  149:                 } while ( --cnt > 0 );
  150:         }
  151: 
  152:         /* Copy in WASZ bytes unit */
  153:         if ( n >= MASZ ) {
  154:                 cnt = n / MASZ;
  155:                 n &= MAMSK;
  156:                 do {
  157:                         *d.lp++ = *s.lp++;
  158:                 } while ( --cnt > 0 );
  159:         }
  160: 
  161:         /* Copy trailing bytes */
  162:         while ( n-- > 0 ) {
  163:                 *d.cp++ = *s.cp++;
  164:         }
  165:         return dst;
  166: }
  167: 
  168: /*
  169:  * tkl_memmove : move memory
  170:  */
  171: void* tkl_memmove( void *dst, const void *src, size_t n )
  172: {
  173:         MPTR   s, d;
  174:         size_t cnt;
  175: 
  176:         d.cp = (unsigned char *)dst;
  177:         s.cp = (unsigned char *)src;
  178: 
  179:         if ( d.cp < s.cp ) {   /* Copy forward */
  180:                 if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  181:                         if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2 ) {
  182:                                 cnt = n;
  183:                         } else {
  184:                                 cnt = MASZ - (s.lv & MAMSK);
  185:                         }
  186:                         n -= cnt;
  187:                         do {
  188:                                 *d.cp++ = *s.cp++;
  189:                         } while ( --cnt > 0 );
  190:                 }
  191:                 if ( n >= MASZ ) {
  192:                         cnt = n / MASZ;
  193:                         n &= MAMSK;
  194:                         do {
  195:                                 *d.lp++ = *s.lp++;
  196:                         } while ( --cnt > 0 );
  197:                 }
  198:                 while ( n-- > 0 ) {
  199:                         *d.cp++ = *s.cp++;
  200:                 }
  201:         } else {               /* Copy backward */
  202:                 s.cp += n;
  203:                 d.cp += n;
  204:                 if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  205:                         if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2 ) {
  206:                                 cnt = n;
  207:                         } else {
  208:                                 cnt = s.lv & MAMSK;
  209:                         }
  210:                         n -= cnt;
  211:                         do {
  212:                                 *--d.cp = *--s.cp;
  213:                         } while ( --cnt > 0 );
  214:                 }
  215:                 if ( n >= MASZ ) {
  216:                         cnt = n / MASZ;
  217:                         n &= MAMSK;
  218:                         do {
  219:                                 *--d.lp = *--s.lp;
  220:                         } while ( --cnt > 0 );
  221:                 }
  222:                 while ( n-- > 0 ) {
  223:                         *--d.cp = *--s.cp;
  224:                 }
  225:         }
  226:         return dst;
  227: }
  228: 
  229: /*
  230:  * tkl_strlen : get text string length
  231:  */
  232: size_t tkl_strlen( const char *s )
  233: {
  234:         char   *cp = (char *)s;
  235: 
  236:         while ( *cp != '\0' ) cp++;
  237:         return (size_t)(cp - s);
  238: }
  239: 
  240: /*
  241:  * tkl_strcmp : perform text string comparison
  242:  */
  243: int tkl_strcmp( const char *s1, const char *s2 )
  244: {
  245:         for ( ; *s1 == *s2; s1++, s2++ ) {
  246:                 if ( *s1 == '\0' ) return 0;
  247:         }
  248:         return (unsigned char)*s1 - (unsigned char)*s2;
  249: }
  250: 
  251: #if     0
  252: /*
  253:  * tkl_strncmp : perform text string comparison of specified length
  254:  */
  255: int tkl_strncmp( const char *s1, const char *s2, size_t n )
  256: {
  257:         int    result;
  258: 
  259:         while ( n-- > 0 ) {
  260:                 result = (unsigned char)*s1 - (unsigned char)*s2++;
  261:                 if ( result != 0 ) return result;
  262:                 if ( *s1++ == '\0' ) break;
  263:         }
  264:         return 0;
  265: }
  266: #endif
  267: 
  268: /*
  269:  * tkl_strcpy : copy text string
  270:  */
  271: char* tkl_strcpy( char *dst, const char *src )
  272: {
  273:         char   *dp = dst;
  274: 
  275:         while ( (*dp++ = *src++) != '\0' );
  276:         return dst;
  277: }
  278: 
  279: /*
  280:  * tkl_strncpy : copy text string of specified length
  281:  */
  282: char* tkl_strncpy( char *dst, const char *src, size_t n )
  283: {
  284:         char   *dp = dst;
  285: 
  286:         while ( n-- > 0 ) {
  287:                 if ( (*dp++ = *src++) == '\0' ) {
  288:                         while ( n-- > 0 ) *dp++ = '\0';
  289:                         break;
  290:                 }
  291:         }
  292:         return dst;
  293: }
  294: 
  295: /*
  296:  * tkl_strcat : perform text string concatenation
  297:  */
  298: char* tkl_strcat( char *dst, const char *src )
  299: {
  300:         char   *dp = dst;
  301: 
  302:         while ( *dp != '\0' ) dp++;
  303:         while ( (*dp++ = *src++) != '\0' );
  304:         return dst;
  305: }
  306: 
  307: /*
  308:  * tkl_strncat : perform concatenation on text string of specified length
  309:  */
  310: char* tkl_strncat( char *dst, const char *src, size_t n )
  311: {
  312:         char   *dp = dst;
  313: 
  314:         while ( *dp != '\0' ) dp++;
  315:         while ( n-- > 0 ) {
  316:                 if ( (*dp++ = *src++) == '\0' ) return dst;
  317:         }
  318:         *dp = '\0';
  319:         return dst;
  320: }
  321: 
  322: /*
  323:  * tkl_strtoul : convert text string to integer value (unsigned long int)
  324:  */
  325: unsigned long int tkl_strtoul( const char *nptr, char **endptr, int base )
  326: {
  327:         unsigned long int      value = 0;
  328:         int            sign = 1;
  329:         int            i;
  330: 
  331:         while ( (*nptr == ' ' || *nptr == '\t') ) {
  332:                 ++nptr;
  333:         }
  334: 
  335:         switch ( *nptr ) {
  336:           case '-':
  337:                 sign = -1;
  338:                 /* no break */
  339:           case '+':
  340:                 ++nptr;
  341:                 /* no break */
  342:           default:
  343:                 break;
  344:         }
  345: 
  346:         if ( base == 16 ) {
  347:                 if ( *nptr == '0' ) {
  348:                         ++nptr;
  349:                         if ( *nptr == 'X' || *nptr == 'x' ) {
  350:                                 ++nptr;
  351:                         }
  352:                 }
  353:         } else if ( base == 0 ) {
  354:                 if ( *nptr == '0' ) {
  355:                         ++nptr;
  356:                         if ( *nptr == 'X' || *nptr == 'x' ) {
  357:                                 ++nptr;
  358:                                 base = 16;
  359:                         } else {
  360:                                 base = 8;
  361:                         }
  362:                 } else {
  363:                         base = 10;
  364:                 }
  365:         } else if ( base < 2 || base > 36 ) {
  366:                 base = 10;
  367:         }
  368: 
  369:         while ( *nptr != '\0' ) {
  370:                 if ( *nptr >= '0' && *nptr <= '9' ) {
  371:                         i = *nptr - '0';
  372:                 } else if ( *nptr >= 'A' && *nptr <= 'Z' ) {
  373:                         i = *nptr - 'A' + 10;
  374:                 } else if ( *nptr >= 'a' && *nptr <= 'z' ) {
  375:                         i = *nptr - 'a' + 10;
  376:                 } else {
  377:                         break;
  378:                 }
  379:                 if ( i >= base ) {
  380:                         break;
  381:                 }
  382:                 value = value * base + i;
  383:                 ++nptr;
  384:         }
  385:         if ( endptr != NULL ) {
  386:                 *endptr = (char *)nptr;
  387:         }
  388:         return value * sign;
  389: }
  390: