/*
   Support.c, Copyright  (c) by Christian Conrad,
   Swiss Federal Institute of Technology,
   Computer Engineering and Networks Laboratory.
 
   TOPSY -- A Teachable Operating System.
            Implementation of a tiny and simple
            micro kernel for teaching purposes.
 
   For further information, please visit http://www.tik.ee.ethz.ch/~topsy
 
   This software is provided under the terms of the GNU General Public Licence.
 
   A full copy of the GNU GPL is provided in the file COPYING found in the 
   development root of Topsy.
 */
/*
	
	File:                  $Source: /home/ruf/cvs_Topsy/cvs/Topsy/Topsy/Support.c,v $
	Affiliation:           ETH Zuerich, TIK
	Version:               $Revision: 2936 $
	Last Date of Change:   $Date: 2007-03-21 15:23:42 +0100 (Mit, 21 Mär 2007) $  by: $Author: baderma $
	
*/

#include "Support.h"
#include "Lock.h"
#ifdef CONFIGURE_ARCH_SA  /* lpr 010724 */
#include "lib1funcs.h"
#endif


void byteCopy( Address targetAddress, Address sourceAddress, unsigned long int nbBytes)
{
  register unsigned char* t = (unsigned char*)targetAddress;
  register unsigned char* s = (unsigned char*)sourceAddress;
  register long  len = nbBytes;

  /* somewhen this needs to be replaced with efficent assembler functions */
  if ( len < 0 )
  {
    len = (1UL << (sizeof(long)*8 - 1) ) - 1;
    byteCopy(targetAddress+len, sourceAddress+len, nbBytes-len);
    /* I don't think that somebody wants to copy more then 2147483647 bytes
     * but hey do it the right way or forget it */
  }
  /*
   * Unroll the loop to make overhead from branches small.
   */

  while( (len -= 32 ) >= 0)
  {
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
  }

  len += 32;
  while( (len -= 4) >= 0)
  {
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
    *t++ = *s++;
  }
  len += 4;
  if ( len == 0 )
    return;

  while ( (len -= 1) >= 0 )
  {
    *t++ = *s++;
  }
#if 0
  /* XXX does not work if targetAddress or sourceAddress starts or end at a non
     word boundary */
  long* tar = (long*)targetAddress;
  long* src = (long*)sourceAddress;
  int rem = 0;

  rem = nbBytes % (sizeof(long)/sizeof(char));
  nbBytes /= (sizeof(long)/sizeof(char));

  /* long copy */
  for (; nbBytes > 0; nbBytes--)
  {
    *tar++ = *src++;
  }
  /* byte copy the remaining bytes */
  if(rem)
  {
    char* source = (char *)(src-1);
    char* target = (char *)(tar-1);
    target++;
    source++;
    for( ; rem > 0; rem--)
      *target++ = *source++;
  }
#endif
}

void zeroOut( Address targetAddress, unsigned long int nbBytes)
{
  long* tar = (long*)targetAddress;
  int rem = 0;

  rem = nbBytes % (sizeof(long)/sizeof(char));
  nbBytes /= (sizeof(long)/sizeof(char));

  /* long zeroout */
  for (; nbBytes > 0; nbBytes--)
  {
    *tar++ = 0;
  }
  /* byte copy the remaining bytes */
  if(rem)
  {
    char* target = (char *)(tar-1);
    target++;
    for( ; rem > 0; rem--)
      *target++ = 0;
  }
}

void mem_set_uint32(Address targetAddress,
                    unsigned long pattern, unsigned long int nbBytes)
{
  long* tar = (long*)targetAddress;

  nbBytes >>= 2;

  /* long zeroout */
  for (; nbBytes > 0; nbBytes--)
  {
    *tar++ = pattern;
  }
}

int mem_cmp(register char *s, register char *t, unsigned long int len)
{
  register char x;

  for (;;)
  {
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    ++s;
    ++t;
  }
  return ((int)(unsigned int)(unsigned char) x)
         - ((int)(unsigned int)(unsigned char) *t);
}

/* string functions (like in libc but with a _ after str */
/* NOTE: we do not care if you point to a valid pointer, that's your problem */
void str_cpy( char* target, char* source)
{
  while ((*target++ = *source++) != '\0')
    ;
}

/*	The str_ncpy() function copies up to size - 1 characters from the NULL-
		terminated string source to target, NUL-terminating the result.
*//* NOTE:	under libc strncpy does not terminate the target 
						if strlen(source) >= size. This is a major bug and cause for
						many security holes. Topsy DOES always terminate the target.
*/
void str_ncpy(char* target, char* source, unsigned long int size)
{
  --size; /* the NULL termination at the end of the string */
  while (((*target++ = *source++) != '\0') && ((--size) > 0))
    ;
  *target = '\0';
}

int str_neql(char* s1, char* s2, unsigned long int len)
{
  return !str_ncmp(s1, s2, len);
}

int str_eql(char* s1, char* s2)
{
  return !str_cmp(s1, s2);
}

/* str_ncmp(char *p1, char *p2, int pl);
   return values:
   p1 == p2 : 0
   p1 <  p2 : < 0
   p2 >  p2 : > 0
 */
int str_ncmp(register char *s, register char *t, unsigned long int len)
{
  register char x;

  for (;;)
  {
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    if (!len--)
      return 0;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
  }
  return ((int)(unsigned int)(unsigned char) x)
         - ((int)(unsigned int)(unsigned char) *t);
}


int str_cmp(register char *s, register char *t)
{
  register char x;

  for (;;)
  {
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
    x = *s;
    if (x != *t)
      break;
    if (!x)
      break;
    ++s;
    ++t;
  }
  return ((int)(unsigned int)(unsigned char) x)
         - ((int)(unsigned int)(unsigned char) *t);
}


unsigned long int str_len(register char *s)
{
  register char *t;

  t = s;
  for (;;)
  {
    if (!*t)
      return t - s;
    ++t;
    if (!*t)
      return t - s;
    ++t;
    if (!*t)
      return t - s;
    ++t;
    if (!*t)
      return t - s;
    ++t;
  }
}

int isprintable(char s)
{
  if ( s > 31 && 127 > s )
    return 1;
  return 0;
}

void dumpMemory(unsigned char *at, unsigned int pre, unsigned int post)
/* dump memory around address at with pre and post additional context */
{
#if 1
  unsigned char* start = NULL;
  unsigned char* end = NULL;
  unsigned char* t = NULL;
  int i = 0;

  at = (unsigned char*)((unsigned long)at & ~0x3); /* round down to word */
  pre = (pre + 0x3) & ~0x3; /* round up to word */
  post= (post + 0x3) & ~0x3;

  start = at - pre;
  end = at + post;

  printf("Memory dump form %p to %p", start, end);
  while( start < end )
  {
    printf("\n%08lx:", (unsigned long)start);
    for (i = 0, t = start; t < end && i < 16; ++i )
    {
      printf("%s %.2x", (i&0x7)?"":" ", *t++);
    }
    while ( i < 16 )
    {
      printf("%s   ", (i&0x7)?"":" ");
      ++i;
    }
    printf("  |");
    for (i = 0; start < end && i < 16; ++i )
    {
      printf("%c", isprintable(*start)?*start:'.');
      start++;
    }
    while ( i++ < 16 )
      printf(" ");
    printf("|");
  }
  printf("\n");
#endif
}

/* The rest of the file is for printf and vsnprintf */

/* Macros for setting flags and vice versa */
#define SET_FLAG(f)		(flags |= f)
#define IS_FLAG(f)		(flags & f)
#define UNSET_FLAG(f)	(flags &= !f)

/* flags */
#define	ALT_FORM	0x01
#define L_ADJUST	0x02
#define ZERO_PAD	0x04
#define SIGNED		0x08
#define LONG_INT	0x10
#define SHORT_INT	0x20
#define VOID_PTR	0x40


#define SIGN_MASK   (1UL << (sizeof(long)*8-1))

#define FMT_LONG 44 /* enough space to hold 2^128 - 1 in decimal or octal plus \0 */

static void format_long(char *buf, unsigned long val, char type, int prec, char* sign, int flags)
{
  int base = 0;
  char tmp[FMT_LONG];
  int pos = 0, i = 0;
  char *digit = "0123456789ABCDEF";

  if( IS_FLAG(SIGNED) && (val & SIGN_MASK))
  {
    *buf++ = '-';
    val = ~val + 1; /* other vay to say val*=-1 :-) */
  }
  else if ( IS_FLAG(SIGNED) && sign )
  {
    *buf++ = *sign;
  }

  switch(type)
  {
  case 'd':
    base = 10;
    break;
  case 'o':
    base = 8;
    break;
  case 'x':
    digit = "0123456789abcdef";
  case 'X':
    base = 16;
    break;
  default:
    str_cpy(buf, "bug in vsprintf: bad base");
    return;
  }
  if(val==0)
  {
    tmp[0]=digit[0];
    pos++;
  }
  else
    while(val)
    {
      tmp[pos] = digit[val%base];
      val = val / base;
      pos++;
    }
  /* in tmp ist the unsigned number; pos is lenght */
  if(prec > 0 && pos < prec )
  {
    if ( prec >= FMT_LONG )
      prec = FMT_LONG-1; /* overflow protection */
    for(i=pos; i<prec; i++)
      *buf++ = '0'; /* pad with 0 if wanted precision is larger */
  }
  for(pos--; pos>=0; pos--)
  {
    *buf++ = tmp[pos];
  }
  *buf = '\0';
}


/* this is far from complete and standard...  */
/* kernel must not use floats anyway... (gfa) */
/* but acts like a real posix vsnprintf. (cj) */
int vsnprintf(char *buffer, unsigned long len, char *fmt, va_list ap)
{
  /* first some macros */
#define MWRITE(c) \
		if ( len ) { \
			*buffer++ = c; \
			len--; \
		} \
		written++ /* count even if buffer full (posix) */
  /* Macros for converting digits to letters and vice versa */
#define	TO_DIGIT(c)	((c) - '0')
#define IS_DIGIT(c)	((unsigned)TO_DIGIT(c) <= 9)
#define	TO_CHAR(n)	((n) + '0')
#define GET_WIDTH(ui, s) \
		ui = 0; \
		while ( IS_DIGIT(*s) ) { \
			ui = 10 * ui + TO_DIGIT(*s++); \
		} \
		--s /* while goes one to far */
  unsigned long written = 0, tlen;
  char *sign = 0;
  char numbuf[FMT_LONG];
  int flags = 0, done = 0, field = 0, pfield = 0, i = 0;
  char base = 0; /* d for decimal, x for hex, X for HEX, o for octal */
  /* vars for va_arg */
  char c = 0, *s = NULL;
  long l = 0; /* shorts will be stored in longs */
  unsigned long ul = 0;

  buffer[--len] = '\0'; /* buffer has to be 0-Terminated */
  while ( *fmt )
  {
    if (*fmt == '%')
    {
      done = 1;
      flags = 0;
      pfield = 0;
      field = 0;
      sign = 0;
      do
      {
        switch(*++fmt)
        {
        case '#':
          done = 0;
          SET_FLAG(ALT_FORM);
          break;
        case '-':
          done = 0;
          SET_FLAG(L_ADJUST);
          break;
        case '+':
          done = 0;
          sign = "+";
          break;
        case ' ':
          done = 0;
          if (! sign)
            sign = " ";
          break;
        case '0':
          done = 0;
          SET_FLAG(ZERO_PAD);
          break;
        case '.':
          done = 0;
          ++fmt; /* start with next char */
          GET_WIDTH(pfield, fmt);
          break;
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9':
          done = 0;
          GET_WIDTH(field, fmt);
          break;
        case 'l':
          done = 0;
          SET_FLAG(LONG_INT);
          break;
        case 'h':
          done = 0;
          SET_FLAG(SHORT_INT);
          break;
        case 'd':
        case 'i':
          done = 1;
          base = 'd';
          SET_FLAG(SIGNED);
          goto number;
        case 'o':
          done = 1;
          base = 'o';
          goto number;
        case 'u':
          done = 1;
          base = 'd';
          goto number;
        case 'X':
          done = 1;
          base = 'X';
          goto number;
        case 'x':
          done = 1;
          base = 'x';
          goto number;
        case 'c':
          done = 1;
          c = (unsigned char)va_arg(ap, int);
          MWRITE(c);
          break;
        case 's':
          done = 1;
          s = va_arg(ap, char*);
          if ( s == 0 )
            s = "(null char)";
          if (! pfield > 0 )
          {
            pfield = str_len(s);
          }
          i = field - pfield;
          if ( i > 0 && !IS_FLAG(L_ADJUST) )
          { /* right adjusted */
            while ( i-- )
            {
              if ( IS_FLAG(ZERO_PAD) )
              {
                MWRITE('0');
              }
              else
              {
                MWRITE(' ');
              }
            }
          }
          while( pfield-- )
          {
            if ( (c = *s++) == '\0' )
              break;
            MWRITE(c);
          }
          if ( i > 0 && IS_FLAG(L_ADJUST) )
          {
            while ( i-- )
            { /* left adjusted */
              MWRITE(' ');
            }
          }
          break;
        case 'p':
          done = 1;
          SET_FLAG(ALT_FORM|VOID_PTR);
          pfield = 8;
          base = 'x';
          goto number;
        case 'D':
          done = 1;
          SET_FLAG(LONG_INT|SIGNED);
          base = 'd';
          goto number;
        case 'O':
          done = 1;
          SET_FLAG(LONG_INT);
          base = 'o';
          goto number;
        case 'U':
          done = 1;
          SET_FLAG(LONG_INT);
          base = 'd';
number:
          /* this is the correct way to extract va_args */
          if ( IS_FLAG(SIGNED) )
          {
            l = IS_FLAG(LONG_INT) ? va_arg(ap, long) :
                IS_FLAG(SHORT_INT) ? (long)(short)va_arg(ap, int) :
                (long)va_arg(ap, int);
            format_long(numbuf, (unsigned long)l, base, pfield, sign, flags);
          }
          else
          {
            ul= IS_FLAG(LONG_INT) ? va_arg(ap, unsigned long) :
                IS_FLAG(SHORT_INT) ? (unsigned long)(unsigned short)va_arg(ap, int):
                IS_FLAG(VOID_PTR) ? (unsigned long)va_arg(ap, void*):
                (unsigned long)va_arg(ap, unsigned int);
            format_long(numbuf, ul, base, pfield, sign, flags);
          }
          /* now we have the padded, signed value in numbuf */
          /* now take care of field, alternate form, left adjust, zero pad */
          i = 0;
          if ( IS_FLAG(ALT_FORM) &&
               !( numbuf[0] == '0' && numbuf[1] == '\0' ) )
          {
            /* prepend only if != 0 */
            switch (base)
            {
            case 'x':
            case 'X':
              i = 2;
              break;
            case 'o':
              i = 1;
              break;
            }
          }
          tlen = str_len(numbuf);
          pfield = field - tlen - i;
          if ( pfield > 0 && !IS_FLAG(L_ADJUST) )
          { /* right adjusted */
            while ( pfield-- )
            {
              if ( IS_FLAG(ZERO_PAD) )
              {
                MWRITE('0');
              }
              else
              {
                MWRITE(' ');
              }
            }
          }
          if ( i != 0 )
          { /* print alternate form */
            MWRITE('0');
            if ( i == 2 )
            {
              MWRITE(base);
            }
          }
          for ( i = 0; i < (int) tlen; i++ )
          {
            MWRITE(numbuf[i]);
          }
          if ( pfield > 0 && IS_FLAG(L_ADJUST) )
          {
            while ( pfield-- )
            { /* left adjusted only with spaces */
              MWRITE(' ');
            }
          }
          /* finaly all is written, move to next */
          break;
        default:
          done = 1;
          MWRITE(*fmt);
          if ( *fmt == '\0')
            return written-1; /* don't count '\0' */
          break;
        }
      }
      while ( !done );
      fmt++;
    }
    else
    {
      MWRITE(*fmt);
      fmt++;
    }
  }
  MWRITE('\0');
  return written-1; /* don't count '\0' */
}

int vprintf(char* fmt, va_list ap)
{
  unsigned long len;
  char buf[160];

  len = vsnprintf(buf, 160, fmt, ap);
  ioConsolePutString(buf);
  return len;
}


int printf(char* fmt, ...)
{
  va_list ap;
  unsigned long len;
  char buf[160];

  va_start(ap, fmt);
  /*	va_arg(ap, char*); *//* XXX ??? don't know why but it has to be here */
  /* at least on tardis and kom because there the va_arg is
  broken (ARRRGGGG) */
  len = vsnprintf(buf, 160, fmt, ap);
  ioConsolePutString(buf);
  va_end(ap);

  return len;
}

int snprintf(char *buf, unsigned long len, char *fmt, ...)
{
  va_list ap;
  int ret = 0;

  va_start(ap, fmt);
  ret = vsnprintf(buf, len, fmt, ap);
  va_end(ap);
  return ret;
}

void Debug(char *fmt, ...)
{
#ifdef DEBUG
  va_list ap;
  char buf[160];

  va_start(ap, fmt);
  vsnprintf(buf, 160, fmt, ap);
  ioConsolePutString(buf);
  va_end(ap);

#endif

  return;
}

