#include <sys/times.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include "libsys.h"


// ---------------------------------------------------------------------------------
// Forward declarations
// ---------------------------------------------------------------------------------
void flush(UINT32 port);
UINT32 GetPort(int file);

// ---------------------------------------------------------------------------------
// Types
// ---------------------------------------------------------------------------------
void libsys_init(void)
{
	UINT32 volatile *pVGA_ctrl = (UINT32*)SYS_VGA_CTRL;
	UINT32 volatile *pITIM_ctrl = (UINT32*)SYS_ITIM_CTRL;

	Screen_clr();

	// Disable timers
	*pITIM_ctrl = 0;

	// Disable all VGA stuff
	*pVGA_ctrl = 0;

	// Initialzie syscall support
	syscalls_init();

	// Initalize debugger
	dbg_init();

	// flush stdin
	flush(GetPort(0));

	// Do some initializations here

}

UINT32 GetPort(int file)
{
	if (file == 0)
		return 0;

	if (file == 1)
		return 0;

	if (file == 2)
		return 2;
}

enum
{
	IF_TYPE_INVALID = 0,
	IF_TYPE_UART,
	IF_TYPE_PS2,
	IF_TYPE_VGA
};

typedef struct _suart_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
	volatile UINT32 *pBAUD;
} uart_if_t;

typedef struct _sps2_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
} ps2_if_t;

typedef struct _svga_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
	volatile UINT32 *pCSRX;
	volatile UINT32 *pCSRY;
} vga_if_t;

typedef struct _scon_if_in_t
{
	UINT32 type;
	void *pIF;
} con_if_in_t;

typedef struct _scon_if_out_t
{
	UINT32 type;
	void *pIF;
} con_if_out_t;

// ---------------------------------------------------------------------------------
// Globals
// ---------------------------------------------------------------------------------
static uart_if_t uart_if[2] =
{
	{(UINT32*)SYS_UART0_STAT, (UINT32*)SYS_UART0_DATA, (UINT32*)SYS_UART0_BAUD},
	{(UINT32*)SYS_UART1_STAT, (UINT32*)SYS_UART1_DATA, (UINT32*)SYS_UART1_BAUD}
};

extern ps2_if_t ps2_if[];

static vga_if_t vga_if =
{
	(UINT32*)SYS_VGA_CTRL, (UINT32*)SYS_VGA_ASCII, (UINT32*)SYS_VGA_POSX, (UINT32*)SYS_VGA_POSY
};

static con_if_in_t con_if_in[4] =
{
	{IF_TYPE_UART, &uart_if[0]},
	{IF_TYPE_UART, &uart_if[1]},
	{IF_TYPE_PS2, &ps2_if[0]},
	{IF_TYPE_PS2, &ps2_if[1]}
};

static con_if_out_t con_if_out[4] =
{
	{IF_TYPE_UART, &uart_if[0]},
	{IF_TYPE_UART, &uart_if[1]},
	{IF_TYPE_VGA, &vga_if},
	{IF_TYPE_INVALID, NULL}
};

// ---------------------------------------------------------------------------------
// MIPS specific
// ---------------------------------------------------------------------------------
UINT32 CP0_SR_read(void)
{
	UINT32 result;

	__asm__
	(
		"mfc0   %[val], $12\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_SR_write(UINT32 val)
{
	__asm__
	(
		"mtc0   %[val], $12\n"
		: /* no output */
		: [val] "r" (val)
	);
}

UINT32 CP0_CR_read(void)
{
	UINT32 result;

	__asm__
	(
		"mfc0   %[val], $13\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_CR_write(UINT32 val)
{
	__asm__
	(
		"mtc0   %[val], $13\n"
		:
		: [val] "r" (val)
	);
}

UINT32 CP0_TR_read(void)
{
	UINT32 result;

	__asm__
	(
		"mfc0   %[val], $31\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_TR_write(UINT32 val)
{
	__asm__
	(
		"mtc0   %[val], $31\n"
		:
		: [val] "r" (val)
	);
}

UINT32 CP0_PRID_read(void)
{
	UINT32 result;

	__asm__
	(
		"mfc0   %[val], $15\n"
		: [val] "=r" (result)
	);

	return result;
}

void CP0_TR_write_ptr(UINT32* pPtr)
{
	__asm__
	(
		"swc0   $31, 0(%[pPtr])\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void CP0_TR_read_ptr(UINT32* pPtr)
{
	__asm__
	(
		"lwc0   $31, 0(%[pPtr])\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void ICACHE_invalidate_all(void)
{
	__asm__
	(
		"cop0	32\n"
	);
}

void ICACHE_invalidate_at(UINT32* pPtr)
{
	__asm__
	(
		"mtc0   %[pPtr], $31\n"
		"cop0	33\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

void DCACHE_invalidate_all(void)
{
	__asm__
	(
		"cop0	34\n"
	);
}

void DCACHE_invalidate_at(UINT32* pPtr)
{
	__asm__
	(
		"mtc0   %[pPtr], $31\n"
		"cop0	35\n"
		:
		: [pPtr] "r" (pPtr)
	);
}

// -------------------------------------------------------------
// BEGIN QUICK AND DIRTY
// -------------------------------------------------------------
//keymap for the AT keyboard SG layout
typedef struct
{
    UINT8 scancode;
    UINT8 unshifted;
    UINT8 shifted;
    UINT8 altered;
} libsys_key_entry_t;

#define SHIFTED (1<<0)
#define ALTERED (1<<1)
#define CONTROLLED (1<<2)

static const libsys_key_entry_t __keymap_german[] =
{
    {0x1c, 'a', 'A', 'a'},
    {0x32, 'b', 'B', 'b'},
    {0x21, 'c', 'C', 'c'},
    {0x23, 'd', 'D', 'd'},
    {0x24, 'e', 'E', '€'},
    {0x2b, 'f', 'F', 'f'},
    {0x34, 'g', 'G', 'g'},
    {0x33, 'h', 'H', 'h'},
    {0x43, 'i', 'I', 'i'},
    {0x3b, 'j', 'J', 'j'},
    {0x42, 'k', 'K', 'k'},
    {0x4b, 'l', 'L', 'l'},
    {0x3a, 'm', 'M', 'µ'},
    {0x31, 'n', 'N', 'n'},
    {0x44, 'o', 'O', 'o'},
    {0x4d, 'p', 'P', 'p'},
    {0x15, 'q', 'Q', '@'},
    {0x2d, 'r', 'R', 'r'},
    {0x1b, 's', 'S', 's'},
    {0x2c, 't', 'T', 't'},
    {0x3c, 'u', 'U', 'u'},
    {0x2a, 'v', 'V', 'v'},
    {0x1d, 'w', 'W', 'w'},
    {0x22, 'x', 'X', 'x'},
    {0x1a, 'y', 'Y', 'y'},
    {0x35, 'z', 'Z', 'z'},
    {0x16, '1', '!', '1'},
    {0x1e, '2', '"', '2'},
    {0x26, '3', '§', '3'},
    {0x25, '4', '$', '4'},
    {0x2e, '5', '%', '5'},
    {0x36, '6', '&', '6'},
    {0x3d, '7', '/', '{'},
    {0x3e, '8', '(', '['},
    {0x46, '9', ')', ']'},
    {0x45, '0', '=', '}'},
    {0x4e, 'ß', '?', '\\'},
    {0x29, ' ', ' ', ' '},
    {0x41, ',', ';', ','},
    {0x49, '.', ':', '.'},
    {0x4a, '-', '_', '-'},
    {0x52, 'ä', 'Ä', 'ä'},
    {0x4c, 'ö', 'Ö', 'ö'},
    {0x54, 'ü', 'Ü', 'ü'},
    {0x5a, '\n', '\n', '\n'},       //RETURN
    {0x5b, '+', '*', '~'},
    {0x5d, '#', '\'', '#'},
    {0x0e, '^', '°', '^'},
    {0x61, '<', '>', '|'},
    {0x66, '\b', '\b', '\b'},
    {0}       //sentinel
};

static char __findkey(char scancode, int mode)
{
    const libsys_key_entry_t *p = __keymap_german;
    while (p->scancode)
    {
        if (p->scancode == scancode)
        {
            switch (mode)
            {
                case SHIFTED:
                    return p->shifted;
                case ALTERED:
                    return p->altered;
                case ALTERED|SHIFTED:
                default:
                    return p->unshifted;
            }
        }
        p++;
    }
    return 0;
}

#define RELEASE         0x8000
#define SPECIALKEY      0x4000
#define EXTENDED        0x2000
#define X_CONTROLLED    0x0400
#define X_ALTERED       0x0200
#define X_SHIFTED       0x0100
#define CAPS    (SPECIALKEY|0x58)
#define F1      (SPECIALKEY|0x05)
#define F2      (SPECIALKEY|0x06)
#define F3      (SPECIALKEY|0x04)
#define F4      (SPECIALKEY|0x0c)
#define F5      (SPECIALKEY|0x03)
#define F6      (SPECIALKEY|0x0b)
#define F7      (SPECIALKEY|0x83)
#define F8      (SPECIALKEY|0x0a)
#define F9      (SPECIALKEY|0x01)
#define F10     (SPECIALKEY|0x09)
#define F11     (SPECIALKEY|0x78)
#define F12     (SPECIALKEY|0x07)
#define ARROW_UP        (SPECIALKEY|EXTENDED|0x75)
#define ARROW_DOWN      (SPECIALKEY|EXTENDED|0x72)
#define ARROW_LEFT      (SPECIALKEY|EXTENDED|0x6b)
#define ARROW_RIGHT     (SPECIALKEY|EXTENDED|0x74)

UINT32 PS2_readchar(ps2_if_t *pIF)
{
	if (!pIF)
		return -1;

	while (!(SYS_PS2_BIT_RX_AVAIL & *pIF->pCTRL));

	return (*pIF->pDATA & 0xFF);

}

UINT32 con_readchar(ps2_if_t *pIF)
{
	UINT8 c;
	static char mode = 0;
	static char nextrelease = 0;
	static char extended = 0;
	int flags = 0;
	int char_valid = 0;
	UINT8 key;

	key = PS2_readchar(pIF);

	switch (key)
	{
		case 0xf0: //release key
			nextrelease = 1;
			break;
		case 0x14: //control key
			if (nextrelease)
			{
				mode &= ~CONTROLLED;
			} else
			{
				mode |= CONTROLLED;
			}
			nextrelease = 0;
			break;
		case 0x12: //left SHIFT
		case 0x59: //right SHIFT
			if (nextrelease)
			{
				mode &= ~SHIFTED;
			} else
			{
				mode |= SHIFTED;
			}
			nextrelease = 0;
			break;
		case 0x11: //left and right ALT (right one also with E0)
			if (nextrelease)
			{
				mode &= ~ALTERED;
			} else
			{
				mode |= ALTERED;
			}
			nextrelease = 0;
			break;
		case 0xe0:  //extended key following
			extended = 1;
			break;
		default:
			if (nextrelease) flags |= RELEASE;
			if (extended) flags |= EXTENDED;
			flags |= mode << 8;
			c = __findkey(key, mode);
			char_valid = (c && ((flags & RELEASE) != RELEASE));
			nextrelease = 0;
			extended = 0;
			break;
	}

	if (char_valid)
	{
		// fprintf(stderr, "c = %c (%d), flags = 0x%08X, mode = 0x%08X\n", c, c, flags, mode);
		return c;
	}

	return -1;
}
// -------------------------------------------------------------
// END QUICK AND DIRTY
// -------------------------------------------------------------

// ------------------------------------
// Low-level I/O
// ------------------------------------
int UART_readchar(uart_if_t *pIF)
{
	if (!pIF)
		return -1;

	if (SYS_UART_BIT_RX_AVAIL & *pIF->pCTRL)
		return (*pIF->pDATA & 0xFF);

	return -1;
}

void UART_writechar(uart_if_t *pIF, char c)
{
	if (!pIF)
		return;

	while((SYS_UART_BIT_TX_HALFFULL & *pIF->pCTRL) != 0);

	*pIF->pDATA = (UINT32)c;
}

void cg_writechar(vga_if_t *pIF, char c)
{
	UINT32 code;

	if (!pIF)
		return;

	// Output translation
	switch(c)
	{
		case 'Ä':
		code = 196;
		break;

		case 'ä':
		code = 228;
		break;

		case 'Ö':
		code = 214;
		break;

		case 'ö':
		code = 246;
		break;

		case 'Ü':
		code = 220;
		break;

		case 'ü':
		code = 252;
		break;

		case '&':
		code = 230;
		break;

		case '§':
		code = 167;
		break;

		case 'ß':
		code = 223;
		break;

		case '°':
		code = 176;
		break;

		case 'µ':
		code = 181;
		break;

		default:
		code = (UINT32)c;
		break;
	}

	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pDATA = code;
}

void cg_set_csr_x(vga_if_t *pIF, UINT32 coord)
{
	if (!pIF)
		return;

	// Set cursor
	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCSRX = coord;
}

void cg_set_csr_y(vga_if_t *pIF, UINT32 coord)
{
	if (!pIF)
		return;

	// Set cursor
	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCSRY = coord;
}

void cg_clr_line(vga_if_t *pIF)
{
	if (!pIF)
		return;

	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCTRL |= SYS_VGA_BIT_CLRLINE;
}

void cg_clr_screen(vga_if_t *pIF)
{

	if (!pIF)
		return;

	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCTRL |= SYS_VGA_BIT_CLRSCR;

	// Cursor home
	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCSRX = 0;

	while (!(*pIF->pCTRL & SYS_VGA_BIT_CGRDY));
		*pIF->pCSRY = 0;

}

void Screen_clr(void)
{
	cg_clr_screen(&vga_if);
}

void Screen_line_clr(void)
{
	cg_clr_line(&vga_if);
}

void Screen_csr_set_x(UINT32 coord)
{
	cg_set_csr_x(&vga_if, coord);
}

void Screen_csr_set_y(UINT32 coord)
{
	cg_set_csr_y(&vga_if, coord);
}

UINT32 Screen_get_resx(void)
{
	UINT32 volatile *pVGA_res = (UINT32*)SYS_VGA_RES;

	return (0xFFF & (*pVGA_res >> 0));
}

UINT32 Screen_get_resy(void)
{
	UINT32 volatile *pVGA_res = (UINT32*)SYS_VGA_RES;

	return (0xFFF & (*pVGA_res >> 12));
}

UINT32 Screen_get_fps(void)
{
	UINT32 volatile *pVGA_res = (UINT32*)SYS_VGA_RES;

	return (0xFF & (*pVGA_res >> 24));
}

// ---------------------------------------------------------------------------------
void flush(UINT32 port)
{
	int c;
	con_if_in_t *pIF;

	pIF = &con_if_in[port];

	do
	{
		switch(pIF->type)
		{
			case IF_TYPE_UART:
			c = UART_readchar((uart_if_t*)pIF->pIF);
			break;

			case IF_TYPE_PS2:
			c = con_readchar((ps2_if_t*)pIF->pIF);
			break;

			default:
			break;
		}

	} while(c > 0);
}

char readchar(UINT32 port)
{
	int c;
	con_if_in_t *pIF;

	pIF = &con_if_in[port];

	do
	{
		switch(pIF->type)
		{
			case IF_TYPE_UART:
			c = UART_readchar((uart_if_t*)pIF->pIF);
			break;

			case IF_TYPE_PS2:
			c = con_readchar((ps2_if_t*)pIF->pIF);
			break;

			default:
			break;
		}

	} while(c < 0);

	return (char)c;
}

void writechar(UINT32 port, char c)
{
	con_if_out_t *pIF;

	pIF = &con_if_out[port];

	switch(pIF->type)
	{
		case IF_TYPE_UART:
		UART_writechar((uart_if_t*)pIF->pIF, c);
		break;

		case IF_TYPE_VGA:
		cg_writechar((vga_if_t*)pIF->pIF, c);
		break;

		default:
		break;
	}
}

void _putchar(UINT32 port, char c)
{

	con_if_out_t *pIF;

	pIF = &con_if_out[port];

	switch(pIF->type)
	{
		case IF_TYPE_UART:
		if (c == 0x0A)
			UART_writechar((uart_if_t*)pIF->pIF, 0x0D);
		UART_writechar((uart_if_t*)pIF->pIF, c);
		break;

		case IF_TYPE_VGA:
		cg_writechar((vga_if_t*)pIF->pIF, c);
		if (c == 0x0A)
		{
			cg_clr_line((vga_if_t*)pIF->pIF);
			cg_writechar((vga_if_t*)pIF->pIF, 0x0D);
		}
		break;

		default:
		break;
	}
}

char _getchar(void)
{
	return readchar(GetPort(0));
}

void _exit (int exitcode)
{
	fflush(stdout);
	fflush(stderr);
	while(1);
}

void sleep(unsigned ms)
{
	unsigned stop;
	struct tms t;

	times(&t);
	stop = t.tms_utime + ms;

	do
	{
		times(&t);
	} while (t.tms_utime < stop);

}


int getrusage(int who, struct rusage *usage)
{
	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;

	// who: RUSAGE_SELF or RUSAGE_CHILDREN
	usage->ru_utime.tv_usec = *pReg_usec;
	usage->ru_utime.tv_sec = *pReg_sec;
	usage->ru_stime.tv_usec = *pReg_usec;
	usage->ru_stime.tv_sec = *pReg_sec;
}

clock_t times(struct tms *buffer)
{
	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
	long sec;
	long usec;

	sec = *pReg_sec;
	usec = *pReg_usec;

	if (buffer)
	{
		buffer->tms_utime = sec*1000 + usec/1000;
		buffer->tms_stime = 0;
		buffer->tms_cutime = 0;
		buffer->tms_cstime = 0;
	}

	return (clock_t)sec;
}

int gettimeofday(struct timeval *tp, void *tzp)
{
	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;

	if (tp)
	{
		tp->tv_sec = *pReg_sec;
		tp->tv_usec = *pReg_usec;
	}

	return 0;
}

int settimeofday(const struct timeval *tp, const struct timezone *tzp)
{
	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
	div_t res;

	res = div(tp->tv_usec, 1E6);

	if (tp)
	{
		*pReg_usec = res.rem;
		*pReg_sec  = tp->tv_sec + res.quot;
	}

	return 0;
}

caddr_t sbrk(int incr)
{
	extern char end;
	extern char stack_ptr;
	static char *heap_end;
	char *prev_heap_end;

	if (heap_end == 0)
	{
		heap_end = &end;
	}

	prev_heap_end = heap_end;
	if (heap_end + incr > &stack_ptr)
	{
//		sputs("Heap and stack collision\n");
//		sputs("Stack Ptr = ");print_word(&stack_ptr);sputs("\n");
//		sputs("Heap end  = ");print_word(heap_end);sputs("\n");
//		sputs("Increment = ");print_word(incr);sputs("\n");
		exit(1);
	}
	heap_end += incr;
	return (caddr_t) prev_heap_end;
}

int fstat(int file, struct stat *st)
{
//	sputs("fstat ("); print_word(file); sputs(")\n");
	st->st_mode = S_IFCHR;
	st->st_blksize = 0;
	return 0;
}

int lseek(int file, int ptr, int dir)
{
//	sputs("lseek ("); print_word(file); sputs(")\n");
	errno = ESPIPE;
	return -1;
}

int open(const char *name, int flags, int mode)
{
//	sputs("open (\""); sputs(name); sputs("\")\n");
	errno = EIO;
	return -1;
}

int close(int file)
{
//	sputs("close ("); print_word(file); sputs(")\n");
	return 0;
}

int read(int file, char *ptr, int len)
{
	UINT32 port;
	int i;
	char c;

//	sputs("read (file="); print_word(file); sputs(", len ="); print_word(len); sputs(")\n");
	i = 0;
	port = GetPort(file);

	while (i < len)
	{
		c = readchar(port);

		if (c == 0x04)
			break;

		if ((c == 0x0D) || (c == 0x0A))
		{
			writechar(port, 0x0D);
			ptr[i++] = 0x0D;
			writechar(port, 0x0A);
			ptr[i++] = 0x0A;
			break;
		}

		writechar(port, c);
		ptr[i++] = c;

	}
	return i;
}

int write(int file, char *ptr, int len)
{
	UINT32 port;
	int i = 0;

//	sputs("write ("); print_word(file); sputs(")\n");

	port = GetPort(file);
	for (i=0; i < len; i++)
		_putchar(port, *ptr++);

	return i;
}

int isatty(int file)
{
//	sputs("isatty ("); print_word(file); sputs(")\n");
	return 1;
}

int getpid(void)
{
	return 1;
}

#include <errno.h>
#undef errno
extern int errno;
int kill(int pid, int sig)
{
	errno = EINVAL;
	return -1;
}

#include <errno.h>
#undef errno
extern int errno;
int link(char *old, char *new)
{
	errno = EMLINK;
	return -1;
}

#include <errno.h>
#undef errno
extern int errno;
int unlink(char *name)
{
	errno = ENOENT;
	return -1;
}

int sputs(char *pStr)
{
	char *start;
	start = pStr;

	while(*pStr)
		_putchar(GetPort(0), *(pStr++));

	return pStr - start;
}

void print_byte(char byte)
{
	int i;
	unsigned char c, nibble;

	for (i=0; i < 2; i++)
	{
		nibble = (char)((byte >> 4) & 0xF);
		byte <<= 4;

		if (nibble < 10)
			c = nibble + '0';
		else
			c = nibble + 'A' - 10;

		_putchar(GetPort(0), c);
	}
}

void print_word(int word)
{
	int i;
	unsigned char c, nibble;

	for (i=0; i < 4; i++)
	{
		c = (char) (word >> 24);
		print_byte(c);
		word <<= 8;
	}
}

// ---------------------------------------------------------------------------------
// PrintBuffer8()
// Prints byte buffer as hex and ascii interpretation
// ---------------------------------------------------------------------------------
// _Parameters :
// pBuf:       : IN: Buffer to display
// nbpr        : Number of bytes per row to display
// len         : Length of input buffer
// _Return:      none
//
// ---------------------------------------------------------------------------
void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
{
	memdump(pBuf, 1, nbpr, len);
}

void memdump(UINT8 *pBuf, int print_offset_only, int num_bytes_per_row, int len)
{
    int i, j, cnt_hex, cnt_asc, base;
    unsigned char c;

    i = j = 0;
    cnt_hex = len;
    cnt_asc = len;
    base = 0;
	if (!print_offset_only)
		base = (int)pBuf;

    do
    {
        print_word(base + i);
        sputs(": ");
        for (j=0; j < num_bytes_per_row; j++)
        {
            if (cnt_hex)
            {
    		    print_byte(pBuf[i+j]);
		        sputs(" ");
                cnt_hex--;
            }
            else
            {
                sputs("   ");
                break;
            }

        }

        sputs("   ");
        for (j=0; j < num_bytes_per_row; j++)
        {
            if (cnt_asc)
            {
                c = pBuf[i+j];
                if((c < 0x20) || (c > 0x7F))
                    c = '.';

                _putchar(GetPort(0), c);
                cnt_asc--;
            }
            else
            {
                sputs(" ");
                break;
            }
        }
        sputs("\n");
        i += num_bytes_per_row;

    } while (cnt_hex);
}