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:  *    Added by T-Engine Forum at 2013/03/11.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      string.c
   18:  *      Standard string / memory operations used for t-monitor
   19:  */
   20: 
   21: #include <tmonitor.h>
   22: 
   23: /* memory access size */
   24: #define MASZ    (sizeof(unsigned long))
   25: #define MAMSK   (MASZ - 1)
   26: 
   27: /* memory access pointer */
   28: typedef union {
   29:         unsigned char  *cp;
   30:         unsigned long  *lp;
   31:         unsigned long  lv;
   32: } MPTR;
   33: 
   34: /*
   35:  * memset : fill memory area
   36:  */
   37: void* memset( void *s, int c, size_t n )
   38: {
   39:         MPTR           m;
   40:         size_t         cnt;
   41:         unsigned long  val;
   42: 
   43:         m.cp = (unsigned char *)s;
   44:         val = (unsigned char)c;
   45: 
   46:         cnt = m.lv & MAMSK;
   47:         if ( cnt > 0 ) {
   48:                 /* Not aligned in WASZ bytes */
   49:                 if ( n < MASZ * 2) {
   50:                         cnt = n;
   51:                 } else {
   52:                         cnt = MASZ - cnt;
   53:                 }
   54:                 /* Fill preceding bytes to align */
   55:                 n -= cnt;
   56:                 do {
   57:                         *m.cp++ = (unsigned char)val;
   58:                 } while ( --cnt > 0 );
   59:         }
   60: 
   61:         /* Fill in WASZ bytes unit */
   62:         if ( n >= MASZ ) {
   63:                 cnt = n / MASZ;
   64:                 n &= MAMSK;
   65:                 val |= val << 8;
   66:                 val |= val << 16;
   67:                 do {
   68:                         *m.lp++ = val;
   69:                 } while ( --cnt > 0 );
   70:         }
   71: 
   72:         /* Fill trailing bytes */
   73:         while ( n-- > 0 ) {
   74:                 *m.cp++ = (unsigned char)val;
   75:         }
   76:         return s;
   77: }
   78: 
   79: /*
   80:  * memcmp : perform memory comparison
   81:  */
   82: int memcmp( const void *s1, const void *s2, size_t n )
   83: {
   84:         int    result;
   85:         const unsigned char    *p1 = s1;
   86:         const unsigned char    *p2 = s2;
   87: 
   88:         while ( n-- > 0 ) {
   89:                 result = *p1++ - *p2++;
   90:                 if ( result != 0 ) return result;
   91:         }
   92:         return 0;
   93: }
   94: 
   95: /*
   96:  * memcpy : copy memory
   97:  */
   98: void* memcpy( void *dst, const void *src, size_t n )
   99: {
  100:         MPTR   s, d;
  101:         size_t cnt;
  102: 
  103:         d.cp = (unsigned char *)dst;
  104:         s.cp = (unsigned char *)src;
  105: 
  106:         if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  107:                 /* Not aligned in WASZ bytes */
  108:                 if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2) {
  109:                         /* Full copy in a byte unit */
  110:                         cnt = n;
  111:                 } else {
  112:                         /* Copy preceding bytes to align */
  113:                         cnt = MASZ - (s.lv & MAMSK);
  114:                 }
  115:                 /* Copy in a byte unit */
  116:                 n -= cnt;
  117:                 do {
  118:                         *d.cp++ = *s.cp++;
  119:                 } while ( --cnt > 0 );
  120:         }
  121: 
  122:         /* Copy in WASZ bytes unit */
  123:         if ( n >= MASZ ) {
  124:                 cnt = n / MASZ;
  125:                 n &= MAMSK;
  126:                 do {
  127:                         *d.lp++ = *s.lp++;
  128:                 } while ( --cnt > 0 );
  129:         }
  130: 
  131:         /* Copy trailing bytes */
  132:         while ( n-- > 0 ) {
  133:                 *d.cp++ = *s.cp++;
  134:         }
  135:         return dst;
  136: }
  137: 
  138: /*
  139:  * memmove : move memory
  140:  */
  141: void* memmove( void *dst, const void *src, size_t n )
  142: {
  143:         MPTR   s, d;
  144:         size_t cnt;
  145: 
  146:         d.cp = (unsigned char *)dst;
  147:         s.cp = (unsigned char *)src;
  148: 
  149:         if ( d.cp < s.cp ) {   /* Copy forward */
  150:                 if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  151:                         if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2 ) {
  152:                                 cnt = n;
  153:                         } else {
  154:                                 cnt = MASZ - (s.lv & MAMSK);
  155:                         }
  156:                         n -= cnt;
  157:                         do {
  158:                                 *d.cp++ = *s.cp++;
  159:                         } while ( --cnt > 0 );
  160:                 }
  161:                 if ( n >= MASZ ) {
  162:                         cnt = n / MASZ;
  163:                         n &= MAMSK;
  164:                         do {
  165:                                 *d.lp++ = *s.lp++;
  166:                         } while ( --cnt > 0 );
  167:                 }
  168:                 while ( n-- > 0 ) {
  169:                         *d.cp++ = *s.cp++;
  170:                 }
  171:         } else {               /* Copy backward */
  172:                 s.cp += n;
  173:                 d.cp += n;
  174:                 if ( ( (s.lv | d.lv) & MAMSK ) != 0 ) {
  175:                         if ( ( (s.lv ^ d.lv) & MAMSK ) != 0 || n < MASZ * 2 ) {
  176:                                 cnt = n;
  177:                         } else {
  178:                                 cnt = s.lv & MAMSK;
  179:                         }
  180:                         n -= cnt;
  181:                         do {
  182:                                 *--d.cp = *--s.cp;
  183:                         } while ( --cnt > 0 );
  184:                 }
  185:                 if ( n >= MASZ ) {
  186:                         cnt = n / MASZ;
  187:                         n &= MAMSK;
  188:                         do {
  189:                                 *--d.lp = *--s.lp;
  190:                         } while ( --cnt > 0 );
  191:                 }
  192:                 while ( n-- > 0 ) {
  193:                         *--d.cp = *--s.cp;
  194:                 }
  195:         }
  196:         return dst;
  197: }
  198: 
  199: /*
  200:  * strlen : get text string length
  201:  */
  202: size_t strlen( const char *s )
  203: {
  204:         char   *cp = (char *)s;
  205: 
  206:         while ( *cp != '\0' ) cp++;
  207:         return (size_t)(cp - s);
  208: }
  209: 
  210: /*
  211:  * strcmp : perform text string comparison
  212:  */
  213: int strcmp( const char *s1, const char *s2 )
  214: {
  215:         for ( ; *s1 == *s2; s1++, s2++ ) {
  216:                 if ( *s1 == '\0' ) return 0;
  217:         }
  218:         return (unsigned char)*s1 - (unsigned char)*s2;
  219: }
  220: 
  221: /*
  222:  * strncmp : perform text string comparison of specified length
  223:  */
  224: int strncmp( const char *s1, const char *s2, size_t n )
  225: {
  226:         int    result;
  227: 
  228:         while ( n-- > 0 ) {
  229:                 result = (unsigned char)*s1 - (unsigned char)*s2++;
  230:                 if ( result != 0 ) return result;
  231:                 if ( *s1++ == '\0' ) break;
  232:         }
  233:         return 0;
  234: }
  235: 
  236: /*
  237:  * strcpy : copy text string
  238:  */
  239: char* strcpy( char *dst, const char *src )
  240: {
  241:         char   *dp = dst;
  242: 
  243:         while ( (*dp++ = *src++) != '\0' );
  244:         return dst;
  245: }
  246: 
  247: /*
  248:  * strncpy : copy text string of specified length
  249:  */
  250: char* strncpy( char *dst, const char *src, size_t n )
  251: {
  252:         char   *dp = dst;
  253: 
  254:         while ( n-- > 0 ) {
  255:                 if ( (*dp++ = *src++) == '\0' ) {
  256:                         while ( n-- > 0 ) *dp++ = '\0';
  257:                         break;
  258:                 }
  259:         }
  260:         return dst;
  261: }
  262: