From eaf4339cd638777d787e34c63397b23ce3b0e6c3 Mon Sep 17 00:00:00 2001
From: David Given <dg@cowlark.com>
Date: Sun, 9 Jun 2013 22:16:30 +0100
Subject: [PATCH] Implement a very crude busy-wait based select() mechanism for
 consol input.

--HG--
branch : dtrg-videocore
rename : plat/rpi/include/ack/config.h => plat/rpi/include/sys/select.h
rename : plat/rpi/libsys/time.c => plat/rpi/libsys/select.c
---
 plat/rpi/build.mk             |  3 +-
 plat/rpi/include/sys/select.h | 13 +++++++
 plat/rpi/include/unistd.h     | 12 +++++++
 plat/rpi/libsys/libsys.h      |  1 +
 plat/rpi/libsys/pi_uart.s     | 14 ++++++++
 plat/rpi/libsys/select.c      | 65 +++++++++++++++++++++++++++++++++++
 6 files changed, 107 insertions(+), 1 deletion(-)
 create mode 100644 plat/rpi/include/sys/select.h
 create mode 100644 plat/rpi/libsys/select.c

diff --git a/plat/rpi/build.mk b/plat/rpi/build.mk
index 4ddaeb489..4db4a4849 100644
--- a/plat/rpi/build.mk
+++ b/plat/rpi/build.mk
@@ -36,7 +36,8 @@ platform-libsys := \
 	time.c \
 	signal.c \
 	tcgetattr.c \
-	tcsetattr.c
+	tcsetattr.c \
+	select.c
 
 $(eval $(call build-platform))
 
diff --git a/plat/rpi/include/sys/select.h b/plat/rpi/include/sys/select.h
new file mode 100644
index 000000000..df7488da4
--- /dev/null
+++ b/plat/rpi/include/sys/select.h
@@ -0,0 +1,13 @@
+/*
+ * Raspberry Pi support library for the ACK
+ * © 2013 David Given
+ * This file is redistributable under the terms of the 3-clause BSD license.
+ * See the file 'Copying' in the root of the distribution for the full text.
+ */
+
+#ifndef _SYS_SELECT_H
+#define _SYS_SELECT_H
+
+#include <unistd.h>
+
+#endif
diff --git a/plat/rpi/include/unistd.h b/plat/rpi/include/unistd.h
index 4cbf43c58..a4d0c4507 100644
--- a/plat/rpi/include/unistd.h
+++ b/plat/rpi/include/unistd.h
@@ -90,4 +90,16 @@ typedef void (*sighandler_t)(int);
 extern sighandler_t signal(int signum, sighandler_t handler);
 extern int raise(int signum);
 
+/* Select */
+
+typedef uint32_t fd_set;
+
+extern int select(int nfds, fd_set *readfds, fd_set *writefds,
+                  fd_set *exceptfds, struct timeval *timeout);
+
+#define FD_ZERO(set) do { *set = 0; } while (0)
+#define FD_SET(fd, set) do { *set |= (1<<fd); } while (0);
+#define FD_CLR(fd, set) do { *set &= ~(1<<fd); } while (0);
+#define FD_ISSET(fd, set) (*set | (1<<fd))
+
 #endif
diff --git a/plat/rpi/libsys/libsys.h b/plat/rpi/libsys/libsys.h
index bd9d91880..44b3c7818 100644
--- a/plat/rpi/libsys/libsys.h
+++ b/plat/rpi/libsys/libsys.h
@@ -10,6 +10,7 @@
 
 extern void _sys_rawwrite(unsigned char b);
 extern unsigned char _sys_rawread(void);
+extern int _sys_rawpoll(void);
 
 extern void _sys_write_tty(char c);
 
diff --git a/plat/rpi/libsys/pi_uart.s b/plat/rpi/libsys/pi_uart.s
index 0cfed4a67..d52069d71 100644
--- a/plat/rpi/libsys/pi_uart.s
+++ b/plat/rpi/libsys/pi_uart.s
@@ -141,6 +141,20 @@ sendwait:
 1:
 	b lr
 
+! Poll to see if there's incoming data available.
+
+.define __sys_rawpoll
+.define __sys_rawpoll
+__sys_rawpoll:
+	ldb r0, __uart_status
+	b.eq r0, #0, 1b
+
+	mov r1, #AUX_MU_LSR_REG
+	ld r0, (r1)
+	and r0, #0x1 ! 0 if no data, 1 if data
+1:
+	b lr
+
 ! Receive a single byte.
 
 .define __sys_rawread
diff --git a/plat/rpi/libsys/select.c b/plat/rpi/libsys/select.c
new file mode 100644
index 000000000..280bfd694
--- /dev/null
+++ b/plat/rpi/libsys/select.c
@@ -0,0 +1,65 @@
+/*
+ * Raspberry Pi support library for the ACK
+ * © 2013 David Given
+ * This file is redistributable under the terms of the 3-clause BSD license.
+ * See the file 'Copying' in the root of the distribution for the full text.
+ */
+
+#include <stdlib.h>
+#include <errno.h>
+#include <unistd.h>
+#include <time.h>
+#include <pi.h>
+#include "libsys.h"
+
+#define TICKS_PER_SEC 1000000
+
+typedef int condition_t(void);
+
+static int nop_condition(void)
+{
+	return 0;
+}
+
+int select(int nfds, fd_set *readfds, fd_set *writefds,
+                  fd_set *exceptfds, struct timeval *timeout)
+{
+	int result = 0;
+	condition_t* condition = nop_condition;
+
+	if (FD_ISSET(0, readfds))
+		condition = _sys_rawpoll;
+
+	FD_ZERO(readfds);
+	FD_ZERO(writefds);
+	FD_ZERO(exceptfds);
+
+	if (timeout)
+	{
+		/* Wait for a specified amount of time. */
+
+		uint32_t ticks = (timeout->tv_sec * TICKS_PER_SEC) +
+			(timeout->tv_usec * (TICKS_PER_SEC/1000000));
+		uint32_t* timer_clo = pi_phys_to_user((void*) 0x7e003004);
+		uint32_t ra = *timer_clo;
+
+		while (!condition() && ((*timer_clo - ra) < ticks))
+			;
+	}
+	else
+	{
+		/* Wait forever. */
+
+		while (!condition())
+			;
+
+	}
+
+	if ((condition == _sys_rawpoll) && condition())
+	{
+		FD_SET(0, readfds);
+		result = 1;
+	}
+
+	return result;
+}