polled_io.c

RTEMS (Real-Time Executive for Multiprocessor Systems) is a free open source real-time operating sys

#error "BSP probably didn't define a console port"
#endif

void serial_putc(const u_char c)
{
	while ((INL_CONSOLE_INB(lsr) & LSR_THRE) == 0) ;
	INL_CONSOLE_OUTB(thr, c);
}
 
int serial_getc(void)
{
	while ((INL_CONSOLE_INB(lsr) & LSR_DR) == 0) ;
	return (INL_CONSOLE_INB(rbr));
}

int serial_tstc(void)
{
	return ((INL_CONSOLE_INB(lsr) & LSR_DR) != 0);
}

#ifdef USE_VGA_SUPPORT
static void scroll(void)
{
	int i;

	memcpy ( (u_char *)vidmem, (u_char *)vidmem + console_global_data.cols * 2, 
		 ( console_global_data.lines - 1 ) * console_global_data.cols * 2 );
	for ( i = ( console_global_data.lines - 1 ) * console_global_data.cols * 2; 
	      i < console_global_data.lines * console_global_data.cols * 2; 
	      i += 2 )
		vidmem[i] = ' ';
}

/*
 * cursor() sets an offset (0-1999) into the 80x25 text area   
 */
static void
cursor(int x, int y)
{
	int pos = console_global_data.cols*y + x;
	vga_outb(14, 0x14);
	vga_outb(0x15, pos>>8);
	vga_outb(0x14, 15);
	vga_outb(0x15, pos);
}

void 
vga_putc(const u_char c)
{
	int x,y;

	x = console_global_data.orig_x;
	y = console_global_data.orig_y;

	if ( c == '\n' ) {
		if ( ++y >= console_global_data.lines ) {
			scroll();
			y--;
		}
	} else if (c == '\b') {
		if (x > 0) {
			x--;
		}
	} else if (c == '\r') {
		x = 0;
	} else {
		vidmem [ ( x + console_global_data.cols * y ) * 2 ] = c; 
		if ( ++x >= console_global_data.cols ) {
			x = 0;
			if ( ++y >= console_global_data.lines ) {
				scroll();
				y--;
			}
		}
	}

	cursor(x, y);

	console_global_data.orig_x = x;
	console_global_data.orig_y = y;
}
#endif /* USE_VGA_SUPPORT */

#ifdef USE_KBD_SUPPORT
/* Keyboard support */
static int kbd_getc(void)
{
	unsigned char dt, brk, val;
	unsigned code;
loop:
	while((kbd_inb(KBD_STATUS_REG) & KBD_STAT_OBF) == 0) ;

	dt = kbd_inb(KBD_DATA_REG);

	brk = dt & 0x80;	/* brk == 1 on key release */
	dt = dt & 0x7f;		/* keycode */

	if (console_global_data.shfts)
	    code = shift_map[dt];
	else if (console_global_data.ctls)
	    code = ctrl_map[dt];
	else
	    code = plain_map[dt];

	val = KVAL(code);
	switch (KTYP(code) & 0x0f) {
	    case KT_LATIN:
		if (brk)
		    break;
		if (console_global_data.alts)
		    val |= 0x80;
		if (val == 0x7f)	/* map delete to backspace */
		    val = '\b';
		return val;

	    case KT_LETTER:
		if (brk)
		    break;
		if (console_global_data.caps)
		    val -= 'a'-'A';
		return val;

	    case KT_SPEC:
		if (brk)
		    break;
		if (val == KVAL(K_CAPS))
		    console_global_data.caps = !console_global_data.caps;
		else if (val == KVAL(K_ENTER)) {
enter:		    /* Wait for key up */
		    while (1) {
			while((kbd_inb(KBD_STATUS_REG) & KBD_STAT_OBF) == 0) ;
			dt = kbd_inb(KBD_DATA_REG);
			if (dt & 0x80) /* key up */ break;
		    }
		    return 10;
		}
		break;

	    case KT_PAD:
		if (brk)
		    break;
		if (val < 10)
		    return val;
		if (val == KVAL(K_PENTER))
		    goto enter;
		break;

	    case KT_SHIFT:
		switch (val) {
		    case KG_SHIFT:
		    case KG_SHIFTL:
		    case KG_SHIFTR:
			console_global_data.shfts = brk ? 0 : 1;
			break;
		    case KG_ALT:
		    case KG_ALTGR:
			console_global_data.alts = brk ? 0 : 1;
			break;
		    case KG_CTRL:
		    case KG_CTRLL:
		    case KG_CTRLR:
			console_global_data.ctls = brk ? 0 : 1;
			break;
		}
		break;

	    case KT_LOCK:
		switch (val) {
		    case KG_SHIFT:
		    case KG_SHIFTL:
		    case KG_SHIFTR:
			if (brk)
			    console_global_data.shfts = !console_global_data.shfts;
			break;
		    case KG_ALT:
		    case KG_ALTGR:
			if (brk)
			    console_global_data.alts = !console_global_data.alts;
			break;
		    case KG_CTRL:
		    case KG_CTRLL:
		    case KG_CTRLR:
			if (brk)
			    console_global_data.ctls = !console_global_data.ctls;
			break;
		}
		break;
	}
	/* if (brk) return (0); */  /* Ignore initial 'key up' codes */
	goto loop;
}

static int kbd_get(int ms) {
	int status, data;
	while(1) {
		status = kbd_inb(KBD_STATUS_REG);
		if (status & KBD_STAT_OBF) {
			data = kbd_inb(KBD_DATA_REG);
			if (status & (KBD_STAT_GTO | KBD_STAT_PERR))
				return -1;
			else
				return data;
		}
		if (--ms < 0) return -1; 
#ifdef __BOOT__
		boot_udelay(1000);
#else
		rtems_bsp_delay(1000);
#endif
	}
}

static void kbd_put(u_char c, int ms, int port) {
  	while (kbd_inb(KBD_STATUS_REG) & KBD_STAT_IBF) {
		if (--ms < 0) return; 
#ifdef __BOOT__
		boot_udelay(1000);
#else
		rtems_bsp_delay(1000);
#endif
	}
	kbd_outb(port, c);
}

int kbdreset(void)
{
	int c;

	/* Flush all pending data */
	while(kbd_get(10) != -1);

	/* Send self-test */
	kbd_put(KBD_CCMD_SELF_TEST, 10, KBD_CNTL_REG);
	c = kbd_get(1000);
	if (c != 0x55) return 1;

	/* Enable then reset the KB */
	kbd_put(KBD_CCMD_KBD_ENABLE, 10, KBD_CNTL_REG);
	
	while (1) {
		kbd_put(KBD_CMD_RESET, 10, KBD_DATA_REG);
		c = kbd_get(1000);
		if (c == KBD_REPLY_ACK) break;
		if (c != KBD_REPLY_RESEND) return 2;
	}
	
	if (kbd_get(1000) != KBD_REPLY_POR) return 3;

	/* Disable the keyboard while setting up the controller */
	kbd_put(KBD_CMD_DISABLE, 10, KBD_DATA_REG);
	if (kbd_get(10)!=KBD_REPLY_ACK) return 4;

	/* Enable interrupts and keyboard controller */
	kbd_put(KBD_CCMD_WRITE_MODE, 10, KBD_CNTL_REG);
	kbd_put(KBD_MODE_KBD_INT | KBD_MODE_SYS | 
		KBD_MODE_DISABLE_MOUSE | KBD_MODE_KCC,
		10, KBD_DATA_REG);

	/* Reenable the keyboard */
	kbd_put(KBD_CMD_ENABLE, 10, KBD_DATA_REG);
	if (kbd_get(10)!=KBD_REPLY_ACK) return 5;

	return 0;
}

int kbd_tstc(void)
{
	return ((kbd_inb(KBD_STATUS_REG) & KBD_STAT_OBF) != 0);
}
#endif /* USE_KBD_SUPPORT */

const struct console_io 
vacuum_console_functions = {
  	vacuum_putc, 
	vacuum_getc, 
	vacuum_tstc
};

static const struct console_io
log_console_functions = {
  	log_putc, 
	vacuum_getc, 
	vacuum_tstc
}
,
serial_console_functions = {
	serial_putc, 
	serial_getc, 
	serial_tstc
}
#if defined(USE_KBD_SUPPORT) && defined(USE_VGA_SUPPORT)
,
vga_console_functions = {
	vga_putc, 
	kbd_getc, 
	kbd_tstc
}
#endif
;

console_io* curIo = (console_io*) &vacuum_console_functions;

int select_console(ioType t) {
  static ioType curType = CONSOLE_VACUUM;
  
  switch (t) {
  case CONSOLE_VACUUM 	: curIo = (console_io*)&vacuum_console_functions; break;
  case CONSOLE_LOG 	: curIo = (console_io*)&log_console_functions; break;
  case CONSOLE_SERIAL 	: curIo = (console_io*)&serial_console_functions; break;
#if defined(USE_KBD_SUPPORT) && defined(USE_VGA_SUPPORT)
  case CONSOLE_VGA 	: curIo = (console_io*)&vga_console_functions; break;
#endif
  default	        : curIo = (console_io*)&vacuum_console_functions;break;
  }
  if (curType == CONSOLE_LOG) flush_log();
  curType = t;
  return 0;
}

/* we use this so that we can do without the ctype library */
#define is_digit(c)	((c) >= '0' && (c) <= '9')


/* provide this for the bootloader only; otherwise
 * use libcpu implementation
 */
#if defined(__BOOT__)
static int skip_atoi(const char **s)
{
	int i=0;

	while (is_digit(**s))
		i = i*10 + *((*s)++) - '0';
	return i;
}

/* Based on linux/lib/vsprintf.c and modified to suit our needs,
 * bloat has been limited since we basically only need %u, %x, %s and %c.
 * But we need 64 bit values !
 */
int k_vsprintf(char *buf, const char *fmt, va_list args);

int printk(const char *fmt, ...) {
	va_list args;
	int i;
	/* Should not be a problem with 8kB of stack */
	char buf[1024];
	
	va_start(args, fmt);
	i = k_vsprintf(buf, fmt, args);
	va_end(args);
	puts(buf);
	return  i;
}

#endif

/* Necessary to avoid including a library, and GCC won't do this inline. */
#define div10(num, rmd)							 \
do {	u32 t1, t2, t3;							 \
	asm("lis %4,0xcccd; "						 \
	    "addi %4,%4,0xffffcccd; "	/* Build 0xcccccccd */		 \
	    "mulhwu %3,%0+1,%4; "	/* (num.l*cst.l).h  */		 \
	    "mullw %2,%0,%4; "		/* (num.h*cst.l).l  */		 \
	    "addc %3,%3,%2; "						 \
	    "mulhwu %2,%0,%4; "		/* (num.h*cst.l).h  */		 \
	    "addi %4,%4,-1; " 		/* Build 0xcccccccc */		 \
	    "mullw %1,%0,%4; "		/* (num.h*cst.h).l  */		 \
	    "adde %2,%2,%1; "						 \
	    "mulhwu %1,%0,%4; "		/* (num.h*cst.h).h  */		 \
	    "addze %1,%1; "						 \
	    "mullw %0,%0+1,%4; "	/* (num.l*cst.h).l  */		 \
	    "addc %3,%3,%0; "						 \
	    "mulhwu %0,%0+1,%4; "	/* (num.l*cst.h).h  */		 \
	    "adde %2,%2,%0; "						 \
	    "addze %1,%1; "						 \
	    "srwi %2,%2,3; "						 \
	    "srwi %0,%1,3; "						 \
	    "rlwimi %2,%1,29,0,2; "					 \
	    "mulli %4,%2,10; "						 \
	    "sub %4,%0+1,%4; "						 \
	    "mr %0+1,%2; " :						 \
	    "=r" (num), "=&r" (t1), "=&r" (t2), "=&r"(t3), "=&b" (rmd) : \
	    "0" (num));							 \
									 \
} while(0); 

#define SIGN	1		/* unsigned/signed long */
#define LARGE	2		/* use 'ABCDEF' instead of 'abcdef' */
#define HEX	4		/* hexadecimal instead of decimal */
#define ADDR	8               /* Value is an addres (p) */
#define ZEROPAD	16		/* pad with zero */
#define HALF    32
#define LONG	64		/* long argument */
#define LLONG	128		/* 64 bit argument */

#if defined(__BOOT__)
static char * number(char * str, int size, int type, u64 num)
{
	char fill,sign,tmp[24];
	const char *digits="0123456789abcdef";
	int i;

	if (type & LARGE)
		digits = "0123456789ABCDEF";
	fill = (type & ZEROPAD) ? '0' : ' ';
	sign = 0;
	if (type & SIGN) {
		if ((s64)num <0) {
			sign = '-';
			num = -num;
			size--;
		} 
	}

	i = 0;
	do {
		unsigned rem;
		if (type&HEX) {
			rem = num & 0x0f;
			num >>=4;
		} else {
			div10(num, rem);
		}
		tmp[i++] = digits[rem];
	} while (num != 0);
		
	size -= i;
	if (!(type&(ZEROPAD)))
		while(size-->0)
			*str++ = ' ';
	if (sign)
		*str++ = sign;

	while (size-- > 0)
		*str++ = fill;
	while (i-- > 0)
		*str++ = tmp[i];
	while (size-- > 0)
		*str++ = ' ';
	return str;
}

int k_vsprintf(char *buf, const char *fmt, va_list args)
{
	int len;
	u64 num;
	int i;
	char * str;
	const char *s;

	int flags;		/* flags to number() and private */

	int field_width;	/* width of output field */

	for (str=buf ; *fmt ; ++fmt) {
		if (*fmt != '%') {
			*str++ = *fmt;
			continue;
		}
			
		/* process flags, only 0 padding needed */
		flags = 0;
		if (*++fmt == '0' ) {
			flags |= ZEROPAD;
			fmt++;
		}
		
		/* get field width */
		field_width = -1;
		if (is_digit(*fmt))
			field_width = skip_atoi(&fmt);

		/* get the conversion qualifier */
		if (*fmt == 'h') {
			flags |= HALF;
			fmt++;
		} else if (*fmt == 'L') {
			flags |= LLONG;
			fmt++;
		} else if (*fmt == 'l') {
			flags |= LONG;
			fmt++;
		}

		switch (*fmt) {
		case 'c':
			*str++ = (unsigned char) va_arg(args, int);
			while (--field_width > 0)
				*str++ = ' ';
			continue;

		case 's':
			s = va_arg(args, char *);
			len = strlen(s);

			for (i = 0; i < len; ++i)
				*str++ = *s++;
			while (len < field_width--)
				*str++ = ' ';
			continue;

		case 'p':
			if (field_width == -1) {
				field_width = 2*sizeof(void *);
			}
			flags |= ZEROPAD|HEX|ADDR;
			break;

		case 'X':
			flags |= LARGE;
		case 'x':
			flags |= HEX;
			break;

		case 'd':
		case 'i':
			flags |= SIGN;
		case 'u':
			break;

		default:
			if (*fmt != '%')
				*str++ = '%';
			if (*fmt)
				*str++ = *fmt;
			else
				--fmt;
			continue;
		}
		/* This ugly code tries to minimize the number of va_arg() 
		 * since they expand to a lot of code on PPC under the SYSV 
		 * calling conventions (but not with -mcall-aix which has 
		 * other problems). Arguments have at least the size of a
		 * long allocated, and we use this fact to minimize bloat.
		 * (and pointers are assimilated to unsigned long too).
		 */
		if (sizeof(long long) > sizeof(long) && flags & LLONG) 
			num = va_arg(args, unsigned long long);
		else {
			u_long n = va_arg(args, unsigned long);
			if (flags & HALF) {
			  	if (flags & SIGN)
					n = (short) n;
				else
				  	n = (unsigned short) n;
			} else if (! flags & LONG) {
				/* Here the compiler correctly removes this 
				 * do nothing code on 32 bit PPC.
				 */
				if (flags & SIGN) 
					n = (int) n;
				else
					n = (unsigned) n;
			}
			if (flags & SIGN)  num = (long) n; else num = n;
		}
		str = number(str, field_width, flags, num);
	}
	*str = '\0';
	return str-buf;
}
#endif