genlib.c

umon bootloader source code, support mips cpu.

/* genlib.c:
 *	This file has some of the stuff that is typically pulled in by the c
 *	library.  It is provided as local source so that the monitor can be
 *	built without the need for a library.  Some of this code was taken from
 *	the Berkley source code (see notice below).
 *
 *	General notice:
 *	This code is part of a boot-monitor package developed as a generic base
 *	platform for embedded system designs.  As such, it is likely to be
 *	distributed to various projects beyond the control of the original
 *	author.  Please notify the author of any enhancements made or bugs found
 *	so that all may benefit from the changes.  In addition, notification back
 *	to the author will allow the new user to pick up changes that may have
 *	been made by other users after this version of the code was distributed.
 *
 *	Note1: the majority of this code was edited with 4-space tabs.
 *	Note2: as more and more contributions are accepted, the term "author"
 *		   is becoming a mis-representation of credit.
 *
 *	Original author:	Ed Sutter
 *	Email:				[e-mail]
 *	Phone:				[phone]
 */
/*
 * Copyright (c) 1988, 1993
 *	The Regents of the University of California.  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by the University of
 *	California, Berkeley and its contributors.
 * 4. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
#include "config.h"
#include "ctype.h"
#include "genlib.h"
#include "stddefs.h"

char *BSDCRN = "Copyright (c) 1988, 1993 The Regents of the University of California.  All rights reserved.";

/* If an application is using this code, then it must define MONAPP
 * when compiling this file.  This deals with a few functions that this
 * code would otherwise call within the monitor itself.
 * For example, in application space we call mon_printf() instead of
 * printf()...
 * Note: this used to be MONPRINTF, and has since been changed because
 * a few other spots not related to mon_printf() must also be modified.
 */
#ifdef MONAPP
extern int mon_printf();
#define printf mon_printf
#endif

int
abs(int arg)
{
	return (arg >= 0 ? arg : -arg);
}

int
atoi(char *p)
{
	int n, c, neg;

	neg = 0;
	if (!isdigit((c = *p))) {
		while (isspace(c))
			c = *++p;
		switch (c) {
		case '-':
			neg++;
			break;
		case '+': /* fall-through */
			c = *++p;
			break;
		}
		if (!isdigit(c))
			return (0);
	}
	for (n = '0' - c; isdigit((c = *++p)); ) {
		n *= 10;		/* two steps to avoid unnecessary overflow */
		n += '0' - c;	/* accum neg to avoid surprises at MAX */
	}
	return (neg ? n : -n);
}

unsigned char	ctypetbl[] = {
	 0,		_C,		_C,		_C,		_C,		_C,		_C,		_C,
	_C,		_S|_C,	_S|_C,	_S|_C,	_S|_C,	_S|_C,	_C,		_C,
	_C,		_C,		_C,		_C,		_C,		_C,		_C,		_C,
	_C,		_C,		_C,		_C,		_C,		_C,		_C,		_C,
	_S|_B,	_P,		_P,		_P,		_P,		_P,		_P,		_P,
	_P,		_P,		_P,		_P,		_P,		_P,		_P,		_P,
	_N|_X,	_N|_X,	_N|_X,	_N|_X,	_N|_X,	_N|_X,	_N|_X,	_N|_X,
	_N|_X,	_N|_X,	_P,		_P,		_P,		_P,		_P,		_P,
	_P,		_U|_X,	_U|_X,	_U|_X,	_U|_X,	_U|_X,	_U|_X,	_U,
	_U,		_U,		_U,		_U,		_U,		_U,		_U,		_U,
	_U,		_U,		_U,		_U,		_U,		_U,		_U,		_U,
	_U,		_U,		_U,		_P,		_P,		_P,		_P,		_P,
	_P,		_L|_X,	_L|_X,	_L|_X,	_L|_X,	_L|_X,	_L|_X,	_L,
	_L,		_L,		_L,		_L,		_L,		_L,		_L,		_L,
	_L,		_L,		_L,		_L,		_L,		_L,		_L,		_L,
	_L,		_L,		_L,		_P,		_P,		_P,		_P,		_C,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0,
	 0,		 0,		 0,		 0,		 0,		 0,		 0,		 0
};

/* memccpy():
 * Copy s2 to s1, stopping if character c is copied. Copy no more than n bytes.
 * Return a pointer to the byte after character c in the copy,
 * or NULL if c is not found in the first n bytes.
 */
char *
memccpy(register char *s1,register char *s2,register int c,register int n)
{
	while (--n >= 0)
		if ((*s1++ = *s2++) == c)
			return (s1);
	return (0);
}

/* memchr():
 * Return the ptr in sp at which the character c appears;
 *   NULL if not found in n chars; don't stop at \0.
 */
char *
memchr(register char *sp, register char c,register int n)
{
	while (--n >= 0)
		if (*sp++ == c)
			return (--sp);
	return (0);
}

/* memcmp():
 * Compare n bytes:  s1>s2: >0  s1==s2: 0  s1<s2: <0
 */
int
memcmp(register char *s1,register char *s2,register int n)
{
	int diff;

	if (s1 != s2)
		while (--n >= 0)
			if ((diff = (*s1++ - *s2++)))
				return (diff);
	return (0);
}

/* memcpy():
 *  Copy n bytes from 'from' to 'to'; return 'to'.
 *	This version of memcpy() tries to take advantage of address alignment.
 *  The goal is to do as many of the copies on 4-byte aligned addresses,
 *  falling back to 2-byte alignment, and finally, if there is no other
 *  way, simple byte-by-byte copy.
 *  Note that there is some point where the amount of overhead may exceed
 *  the byte count; hence, this will take longer for small byte counts.
 *  The assumption here is that small byte count memcpy() calls don't really
 *  care.
 */
char *
memcpy(char *to,char *from,int count)
{
	char	*to_copy, *end;

	to_copy = to;

#if INCLUDE_QUICKMEMCPY
	/* If count is greater than 8, get fancy, else just do byte-copy... */
	if (count > 8) {
		/* Attempt to optimize the transfer here... */
		if (((int)to & 3) && ((int)from & 3)) {
			/* If from and to pointers are both unaligned to the
			 * same degree then we can do a few char copies to get them
			 * 4-byte aligned and then do a lot of 4-byte aligned copies.
			 */
			if (((int)to & 3) == ((int)from & 3)) {
				while((int)to & 3) {
					*to++ = *from++;
					count--;
				}
			}
			/* If from and to pointers are both odd, but different, then
			 * we can increment them both by 1 and do a bunch of 2-byte
			 * aligned copies...
			 */
			else if (((int)to & 1) && ((int)from & 1)) {
				*to++ = *from++;
				count--;
			}
		}
	
		/* If both pointers are now 4-byte aligned or 2-byte aligned,
		 * take advantage of that here...
		 */
		if (!((int)to & 3) && !((int)from & 3)) {
			end = to + (count & ~3);
			count = count & 3;
			while(to < end) {
				*(ulong *)to = *(ulong *)from;
				from += 4;
				to += 4;
			}
		}
		else if (!((int)to & 1) && !((int)from & 1)) {
			end = to + (count & ~1);
			count = count & 1;
			while(to < end) {
				*(ushort *)to = *(ushort *)from;
				from += 2;
				to += 2;
			}
		}
	}
#endif

	if (count) {
		end = to + count;
		while(to < end)
			*to++ = *from++;
	}
	return(to_copy);
}

void
bcopy(char *from, char *to, int size)
{
	memcpy(to,from,size);
}

/* s_memcpy():
 *	Superset of memcpy().  Note, this used to be tfsmemcpy() in tfs.c;
 *  however, since it has no real TFS dependencies, and code outside of
 *  TFS uses it, it has been moved here and the name is changed.
 *
 *  Includes verbose option plus verification after copy.
 *  Takes advantage of address alignment when possible.
 *
 *  Note:
 *  If verbose is greater than one, then this function doesn't
 *  do any memory transfer, it simply displays the memory range
 *  that is to be transferred.  This is used by TFS to dump a map without
 *  actually touching memory.
 * 
 *  Return:
 *  0 if successful, else -1 indicating some part of the copy failed.
 */
int
s_memcpy(char *_to,char *_from,int count, int verbose,int verifyonly)
{
	int	err;
	volatile register char *to, *from, *end;

	to = _to;
	from = _from;

	if (verbose)
		printf("%s %7d bytes from 0x%08lx to 0x%08lx",
			verifyonly ? "vrfy" : "copy", count,(ulong)from,(ulong)to);

	if (count < 0)
		return(-1);

	if (verifyonly) {
		while(count) {
			if (*to != *from)
				break;
			to++;
			from++;
			count--;
#ifdef WATCHDOG_ENABLED
			if ((count & 0xff) == 0)
				WATCHDOG_MACRO;
#endif
		}
		if (count) {
			if (verbose) {
				printf(" FAILED\n");
				printf("            (0x%02x @ 0x%08lx should be 0x%02x)\n",
					*to,(ulong)to,*from);
			}
			return(-1);
		}
		else
			if (verbose)
				printf(" OK\n");
			return(0);
	}

	/* If verbose is greater than 1, then we don't even do a memcpy,
	 * we just let the user know what we would have done...
	 */
	if ((count == 0) || (verbose > 1))
		goto done;

	if (to != from) {

		err = 0;
		if (!((int)to & 3) && !((int)from & 3) && !(count & 3)) {
			volatile register ulong	*lto, *lfrom, *lend;
	
			count >>= 2;
			lto = (ulong *)to;
			lfrom = (ulong *)from;
			lend = lto + count;
			while(lto < lend) {
				*lto = *lfrom;
				if (*lto != *lfrom) {
					err = 1;
					break;
				}
				lto++;
				lfrom++;
#ifdef WATCHDOG_ENABLED
				if (((int)lto & 0xff) == 0)
					WATCHDOG_MACRO;
#endif
			}
		}
		else if (!((int)to & 1) && !((int)from & 1) && !(count & 1)) {
			volatile register ushort	*sto, *sfrom, *send;
	
			count >>= 1;
			sto = (ushort *)to;
			sfrom = (ushort *)from;
			send = sto + count;
			while(sto < send) {
				*sto = *sfrom;
				if (*sto != *sfrom) {
					err = 1;
					break;
				}
				sto++;
				sfrom++;
#ifdef WATCHDOG_ENABLED
				if (((int)sto & 0xff) == 0)
					WATCHDOG_MACRO;
#endif
			}
		}
		else {
			end = to + count;
			while(to < end) {
				*to = *from;
				if (*to != *from) {
					err = 1;
					break;
				}
				to++;
				from++;
#ifdef WATCHDOG_ENABLED
				if (((int)to & 0xff) == 0)
					WATCHDOG_MACRO;
#endif
			}
		}
		if (err) {
			if (verbose)
				printf(" failed\n");
			return(-1);
		}
	}

done:
	if (verbose)
		printf("\n");

	return(0);
}

/* memset():
 * Set an array of n chars starting at sp to the character c.
 * Return sp.
 */
char *
memset(register char *sp, register char c, register int n)
{
	register char *sp0 = sp;

	while (--n >= 0) {
#ifdef WATCHDOG_ENABLED
		if (((ulong)sp & 0xff) == 0)
			WATCHDOG_MACRO;
#endif
		*sp++ = c;
	}
	return (sp0);
}

/* s_memset():
 *	Superset of memset() (used to be tfsmemset()).
 *  Includes verbose option, test to make sure the destination is not
 *	within MicroMonitor's own space, plus verification after set.
 */
int
s_memset(uchar *to,uchar val,int count,int verbose,int verifyonly)
{
	int		failed;
	uchar	*end;

	failed = 0;

#ifndef MONAPP
	if (inUmonBssSpace((char *)to,(char *)to+count))
		return(-1);
#endif

	if (verbose) {
		printf("%s %7d bytes  at  0x%08lx to 0x%02x",
			verifyonly ? "vrfy" : "set ",count,(ulong)to,val);
	}

	if ((count == 0) || (verbose > 1))
		goto done;

	end = to+count;

	if (verifyonly) {
		while(to < end) {
#ifdef WATCHDOG_ENABLED
			if (((ulong)to & 0xff) == 0)
				WATCHDOG_MACRO;
#endif
			if (*to++ != val) {
				failed = 1;
				break;
			}
		}