util.c

em86xx 完整启动程序,支持网络下载与串通下载

/*****************************************
 Copyright (c) 2002-2004
 Sigma Designs, Inc. All Rights Reserved
 Proprietary and Confidential
 *****************************************/

/* This file is part of the boot loader */

/*
 * util.c
 * 
 * utility functions
 *
 * first revision by Ho Lee 10/10/2002
 */

#include "config.h"
#include "util.h"
#include "vsprintf.h"
#include "uart.h"

//
// ctype
//

int isspace(int ch)
{
    return (ch == '\r' || ch == '\n' || ch == ' ' || ch == '\t') ? 1 : 0;
}

int isdigit(int ch)
{
    return (ch >= '0' && ch <= '9') ? 1 : 0;
}

int toupper(int ch)
{
    return (ch >= 'a' && ch <= 'z') ? ch - 'a' + 'A' : ch;
}

int tolower(int ch)
{
    return (ch >= 'A' && ch <= 'Z') ? ch - 'A' + 'a' : ch;
}

//
// string
//

char *trim(char *str)
{
    char *cp = str;

    while (*cp != 0 && isspace(*cp))
        ++cp;
    
    if (cp == str)
        cp += strlen(str) - 1;
    else {
        char *cpdes;
        for (cpdes = str; *cp != 0; )
            *cpdes++ = *cp++;
        cp = cpdes;
        *cp-- = 0;
    }

    while (cp > str && isspace(*cp))
        *cp-- = 0;

    return str;
}

int strlen(const char *str)
{
    int i;

    for (i = 0; str[i] != 0; ++i)
        ;

    return i;
}

char *strcpy(char *dest, const char *src)
{
    int i;

    for (i = 0; src[i] != 0; ++i)
        dest[i] = src[i];
    dest[i] = 0;
        
    return dest;
}

char *strncpy(char *dest, const char *src, int n)
{
    int i;

    for (i = 0; src[i] != 0 && i < (n - 1); ++i)
        dest[i] = src[i];
    dest[i] = src[i];

    return dest;
}

char *strncpy0(char *dest, const char *src, int n)
{
    int i;

    for (i = 0; src[i] != 0 && i < (n - 1); ++i)
        dest[i] = src[i];
    dest[i] = 0;

    return dest;
}

char *strcat(char *dest, const char *src)
{
    while (*dest)
        ++dest;

    return strcpy(dest, src);
}

int strcmp(const char *str1, const char *str2)
{
    int i;

    for (i = 0; (str1[i] == str2[i]) && (str1[i] != 0); ++i)
        ;
    
    return str1[i] - str2[i];
}

int strncmp(const char *str1, const char *str2, int num)
{
    int i;

    for (i = 0; (i < num - 1) && (str1[i] == str2[i]) && (str1[i] != 0); ++i)
        ;
    
    return str1[i] - str2[i];
}

int strcasecmp(const char *str1, const char *str2)
{
    int i;

    for (i = 0; (toupper(str1[i]) == toupper(str2[i])) && (str1[i] != 0); ++i)
        ;
    
    return toupper(str1[i]) - toupper(str2[i]);
}

char *strchr(const char *str, char ch)
{
    int i;

    for (i = 0; str[i] != 0; ++i)
        if (str[i] == ch)
            return (char *) (str + i);

    return NULL;
}

char *strtok(char *str, const char *token)
{
    static char *s_last = NULL;
    int i;

    if (str == NULL)
        str = s_last;
    if (str == NULL)
        return NULL;

    for (i = 0; str[i] != 0; ++i) 
        if (strchr(token, str[i]) != NULL)
            break;

    if (str[i] != 0)
        str[i++] = 0;

    for (; str[i] != 0; ++i)
        if (strchr(token, str[i]) == NULL)
            break;

    s_last = (str[i] == 0) ? NULL : str + i;

    return str;
}

// 
// stdlib
//

unsigned int atoi(const char *str)
{
    const char *cp;
    unsigned int data;

    if (str[0] == '0' && (str[1] == 'X' || str[1] == 'x'))
        return htoi(str + 2);
    
    for (cp = str, data = 0; *cp != 0; ++cp) {
        if (*cp >= '0' && *cp <= '9')
            data = data * 10 + *cp - '0';
        else
            break;
    }

    return data;
}

unsigned int htoi(const char *str)
{
    const char *cp;
    unsigned int data, bdata;

    for (cp = str, data = 0; *cp != 0; ++cp) {
        if (*cp >= '0' && *cp <= '9')
            bdata = *cp - '0';
        else if (*cp >= 'A' && *cp <= 'F')
            bdata = *cp - 'A' + 10;
        else if (*cp >= 'a' && *cp <= 'f')
            bdata = *cp - 'a' + 10;
        else
            break;
        data = (data << 4) | bdata;
    }

    return data;
}

void memcpy(void *to, const void *from, unsigned long n)
{
#ifndef FAST_OPERATION   
    // brute-force copying

    int i;
    unsigned char *_to = (unsigned char *) to;
    unsigned char *_from = (unsigned char *) from;
    
    for (i = 0; i < n; ++i)
        _to[i] = _from[i];
#else   
    // copy based on alignment of source and target
    // copy is fast when the source and destination have the same alignment 
    //   and the both are 4-byte aligned
    // alignment difference : 
    //   0 : copy_4
    //   1, 3 : copy_1
    //   2 : copy_2

    unsigned char *_to = (unsigned char *) to;
    unsigned char *_from = (unsigned char *) from;
    int align_to, align_from;
    unsigned int i, x, y, z;

    if (n < 8)
        goto copy_1;
    
    align_to = ((unsigned int) to) & 0x03;
    align_from = ((unsigned int) from) & 0x03;

    if (align_to == align_from) {
        x = (align_to) ? 4 - align_to : 0;
        n -= x;
        y = n >> 2;
        z = n & 0x03;
        goto copy_4;
    } else {
        if ((align_to & 0x01) == (align_from & 0x01)) {
            x = (align_to & 0x01);
            n -= x;
            y = n >> 1;
            z = n & 0x01;
            goto copy_2;
        } else
            goto copy_1;
    }
        
copy_4:
    while (x--)
        *_to++ = *_from++;
    if (y) {
        unsigned int *__to = (unsigned int *) _to;
        unsigned int *__from = (unsigned int *) _from;
        for (i = 0; i < y; ++i)
            __to[i] = __from[i];
        _to += (y << 2);
        _from += (y << 2);
    }
    while (z--)
        *_to++ = *_from++;
    goto copy_return;

copy_2:
    while (x--)
        *_to++ = *_from++;
    if (y) {
        unsigned short *__to = (unsigned short *) _to;
        unsigned short *__from = (unsigned short *) _from;
        for (i = 0; i < y; ++i)
            __to[i] = __from[i];
        _to += (y << 1);
        _from += (y << 1);
    }
    while (z--)
        *_to++ = *_from++;
    goto copy_return;

copy_1:
    for (i = 0; i < n; ++i)
        _to[i] = _from[i];

copy_return:
#endif
}

void memmove(void *to, const void *from, unsigned long n)
{
    // memcpy may overwrite when source and destination overlap and 
    // destination start before the end of source
    // otherwise, memcpy() is enough
    if (to > from && ((unsigned long) from + n > (unsigned long) to)) {
        unsigned long i;
        unsigned char *_to = (unsigned char *) to + n;
        unsigned char *_from = (unsigned char *) from + n;
    
        for (i = 0; i < n; ++i)
            *--_to = *--_from;
    } else if (to != from)
        memcpy(to, from, n);
}

int memcmp(const void *ptr1, const void *ptr2, unsigned long n)
{
    int i;
    
    unsigned char *_ptr1 = (unsigned char *) ptr1;
    unsigned char *_ptr2 = (unsigned char *) ptr2;

    if (n == 0)
        return 0;

    for (i = 0; i < (n - 1) && _ptr1[i] == _ptr2[i]; ++i)
        ;

    return _ptr1[i] - _ptr2[i];
}

void memset(void *ptr, unsigned char value, unsigned long n)
{
#ifndef FAST_OPERATION
/* Use this version, mat be it's slower but smaller */
    int i;
    unsigned char *_ptr = (unsigned char *) ptr;
    
    for (i = 0; i < n; ++i)
        _ptr[i] = 0;
#else
    unsigned char *_ptr = (unsigned char *) ptr;
    int align;
    unsigned int i, x, y, z;

    if (n < 8)
        goto memset_1;
    
    align = ((unsigned int) ptr) & 0x03;

    x = (align) ? 4 - align : 0;
    n -= x;
    y = n >> 2;
    z = n & 0x03;

// memset_4:
    while (x--)
        *_ptr++ = value;
    if (y) {
        unsigned int *__ptr = (unsigned int *) _ptr;
        unsigned int value4 = (value) | (value << 8) | (value << 16) | (value << 24);
        for (i = 0; i < y; ++i)
            __ptr[i] = value4;
        _ptr += (y << 2);
    }
    while (z--)
        *_ptr++ = value;
    return;

memset_1:
    for (i = 0; i < n; ++i)
        _ptr[i] = value;
    return;
#endif
}

//
// numerical
// 

#define DIVIDE_FACTOR       2

void unsigned_divide(unsigned int dividened, unsigned int divisor, unsigned int *pquotient, unsigned int *premainder)
{
    unsigned int quotient = 0, remainder = 0;
    int bits, mdivisor;

    // divide by zero
    if (divisor == 0)
        return;

    for (bits = 1, mdivisor = divisor; dividened >= (mdivisor << DIVIDE_FACTOR); bits <<= DIVIDE_FACTOR, mdivisor <<= DIVIDE_FACTOR)
        ;

    do {
        while (dividened >= mdivisor) {
            quotient += bits;
            dividened -= mdivisor;
        }
        bits >>= DIVIDE_FACTOR;
        mdivisor >>= DIVIDE_FACTOR;
    } while (bits > 0);
    
    remainder = dividened;

    if (pquotient)
        *pquotient = quotient;
    if (premainder)
        *premainder = remainder;
}

void signed_divide(int dividened, int divisor, int *pquotient, int *premainder)
{
    int quotient = 0, remainder = 0;
    unsigned int abs_dividened = ABS(dividened);
    unsigned int abs_divisor = ABS(divisor);
    unsigned int quot, rem;

    unsigned_divide(abs_dividened, abs_divisor, &quot, &rem);

    quotient = quot;
    if (dividened < 0) {
        remainder = -rem;
        if (divisor > 0)
            quotient = -quot;
    } else {
        remainder = rem;
        if (divisor < 0)
            quotient = -quot;
    }

    if (pquotient)
        *pquotient = quotient;
    if (premainder)
        *premainder = remainder;
}

int idivide(int dividened, int divisor)
{
    int quotient;

    signed_divide(dividened, divisor, &quotient, NULL);

    return quotient;
}

int imodulus(int dividened, int divisor)
{
    int remainder;

    signed_divide(dividened, divisor, NULL, &remainder);

    return remainder;
}

//
// Miscellaneous
//

// decode ip address or MAC address string to series of number
// retrun :
//   0 : valid address
//   non-zero : invalid address
int parse_netaddr(char *str, unsigned char *addr, int len)
{
    int i, j, value;
    char *cp;
        
    trim(str);
    cp = strtok(str, ".:");
    
    for (i = 0; i < len && cp != NULL; ++i) {
        for (j = 0; cp[j] != 0; ++j)
            if (!isdigit(cp[j]))
                return 1;
        value = atoi(cp);
        if (value < 0 || value >= 0x100)
            return 1;
        addr[i] = (unsigned char) value;
        cp = strtok(NULL, ".");
    }
    
    return (i == len && cp == NULL) ? 0 : 1;
}

int parse_ipaddr(char *str, unsigned int *ipaddr)
{
    unsigned int addr;
    unsigned char *cp = (unsigned char *) &addr;
    
    if (parse_netaddr(str, cp, 4) == 0) {
        *ipaddr = ntohl(addr);
        return 0;
    }

    return 1;
}

char *ipaddr_to_str(unsigned int ipaddr)
{
    static char s_buf[16];
    
    unsigned int addr = htonl(ipaddr);
    unsigned char *cp = (unsigned char *) &addr;

    sprintf(s_buf, "%d.%d.%d.%d", cp[0], cp[1], cp[2], cp[3]);

    return s_buf;
}

//
// memory management (heap)
//
// very simple implementation : no freeing memory
//

static unsigned long g_heap_start, g_heap_end, g_heap_cur;
static int heap_inited = 0;

void heap_init(void *startptr, unsigned int size)
{
    if (heap_inited != 0)
	return;
    g_heap_start = g_heap_cur = (unsigned long) startptr;
    g_heap_end = g_heap_start + size;
    heap_inited = 1;
}

void *malloc(int size)
{
    if (g_heap_cur + size <= g_heap_end) {
        void *ptr = (void *) g_heap_cur;
    
        // align on 16 bytes boundary
        g_heap_cur += size + 15;
        g_heap_cur &= ~0x0f;

        return ptr;
    } else
        return NULL;
}

void free(void *ptr)
{
    /* Do nothing */
}

//
// Misc.
//

static inline unsigned long get_dword_from_buf(unsigned char *buf)
{
    unsigned long val = 0;
    val = buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24); 
    return val;
}

// Given start address and size, check if checksum is OK.
unsigned binfile_crc_check(void *startptr, unsigned int size)
{
    unsigned long sum = 0;
    unsigned int i;
    unsigned char *ptr;

    uart_printf("Verify checksum from 0x%08lx, size %d (0x%x).\n",
		(unsigned long)startptr, size, size);

    ptr = (unsigned char *)startptr;
    for (i = 0; i < (size >> 2); i++, ptr += 4)
	sum += get_dword_from_buf(ptr);

    if (size & 0x3) {
	    union {
	    	unsigned char c[4];
		unsigned long l;
	    } buf;

	    buf.l = 0;
	    for (i = 0; i < (size & 0x3); i++) 
		    buf.c[i] = *ptr++;
	    sum += get_dword_from_buf(&buf.c[0]);
    }	    
    return(sum);
}

#ifdef CONFIG_ENABLE_BITMAPS
static unsigned char bmp_get_4bits(unsigned char **pin, int *toggle)
{
	unsigned char val;
	
	val = ((**pin) >> (*toggle * 4)) & 0xf;
	*toggle = 1 - *toggle;
	if (*toggle == 1)
		*pin = *pin + 1;

	return val;
}

static int bmp_inflate_4bpp(unsigned char *in_buf, unsigned char *out_buf, unsigned int width, unsigned int len)
{
	unsigned char color;
	unsigned char *pin, *pout;
	int count, color_count, line_count, i, toggle=1;
	unsigned long x;

	pout = out_buf;
	pin = in_buf;
	x = 0;
	line_count = 0;

	while (pin < in_buf + len) {
		color = bmp_get_4bits(&pin, &toggle);
		count = bmp_get_4bits(&pin, &toggle);

		if (count == 0)
			color_count = width - x;
		else {
			color_count = 0;
			i = 0;
			while (1) {
				color_count += (count & 0x7) << i;

				if ((count & 0x8) == 0)
					break;
				
				count = bmp_get_4bits(&pin, &toggle);
				i += 3;
			}
		}

		while (color_count > 0) {
			if (x & 0x1){
				*pout |= color & 0xf;
				pout++;
			}
			else {
				*pout = color << 4;
			}
			color_count--;
			x++;
		}

		if (x == width) { 
			if (x & 0x1)
				pout ++;
			
			if (toggle == 0) {
				toggle = 1;
				pin ++;
			}

			x = 0;
			line_count ++;
		}
	}

	return (line_count * ((width + 1) / 2));
}

static int bmp_inflate_7bpp(unsigned char *in_buf, unsigned char *out_buf, unsigned int width, unsigned int len)
{
	unsigned char color, b;
	unsigned char *pin = in_buf, *pout = out_buf;
	int count, line_count = 0, i, color_count, toggle=1;
	unsigned long x=0;
       
	while (pin < in_buf + len){
		b = bmp_get_4bits(&pin, &toggle);
		b = (b << 4) | bmp_get_4bits(&pin, &toggle);
		
		if (b & 0x80) {
			color = b & 0x7f;

			count = bmp_get_4bits(&pin, &toggle);
			if (count == 0)
				color_count = width - x;
			else {
				color_count = 1;
				i = 0;
				while (1) {
					color_count += (count & 0x7) << i;
					
					if ((count & 0x8) == 0)
						break;
					
					count = bmp_get_4bits(&pin, &toggle);
					i += 3;
				}
			}
		}
		else {
			color = b;
			color_count = 1;
		}
		
		while (color_count > 0) {
			(*pout) = color;
			pout++;
			x++;
			color_count --;
		}

		if (x == width) {
			if (toggle == 0) {
				pin ++;
				toggle = 1;
			}
			x = 0;
			line_count ++;
		}
	}

	return (line_count * width);
}

#define RLED_DEPTH_4BPP 0
#define RLED_DEPTH_7BPP 1
int bmp_inflate(unsigned char *in_buf, unsigned char *out_buf, unsigned int len){
	//get width
	int *pwidth, width, type;
	pwidth = (int *) in_buf;
	width = 0x0fff & (*pwidth);
	type = ((*pwidth)>>24) & 0xf;
	uart_printf("RLED section has width %d\n", width);
	uart_printf("RLED section is of type %d\n", type);
	
	switch((type)){
	case RLED_DEPTH_4BPP:
		in_buf += sizeof(int);
		return bmp_inflate_4bpp(in_buf, out_buf, width, len);
		break;
	case RLED_DEPTH_7BPP:
		in_buf += sizeof(int);
		return bmp_inflate_7bpp(in_buf, out_buf, width, len);
		break;
	default:
		break;
	}
		
	return 0;
}

#endif