Add support for Semihosting v2 support for AArch64 in libgloss.

Semihosting v2 changes are documented here:
https://developer.arm.com/docs/100863/latest/

The biggest change is the addition of an extensions mechanism
to add more extensions in the future.

Signed-off-by: Tamar Christina <tamar.christina@arm.com>

4 files changed, 186 insertions, 47 deletions

diff --git a/libgloss/aarch64/_exit.c b/libgloss/aarch64/_exit.c
index a4f9d9eb5..1a6b01532 100644
--- a/libgloss/aarch64/_exit.c
+++ b/libgloss/aarch64/_exit.c
@@ -36,6 +36,12 @@ _exit (int status)
       param_block_t block[2];
       block[0] = ADP_Stopped_ApplicationExit;
       block[1] = status;
-      do_AngelSVC (AngelSVC_Reason_ReportException, block);
+
+#if SEMIHOST_V2
+      if (_has_ext_exit_extended ())
+	 do_AngelSVC (AngelSVC_Reason_ReportExceptionExtended, block);
+      else
+#endif
+	 do_AngelSVC (AngelSVC_Reason_ReportException, block);
     }
 }
diff --git a/libgloss/aarch64/_kill.c b/libgloss/aarch64/_kill.c
index af33f07b0..1d42371f0 100644
--- a/libgloss/aarch64/_kill.c
+++ b/libgloss/aarch64/_kill.c
@@ -45,5 +45,11 @@ _kill (int pid, int sig)
 
   block[0] = ADP_Stopped_RunTimeError;
   block[1] = sig;
-  return do_AngelSVC (AngelSVC_Reason_ReportException, block);
+
+#if SEMIHOST_V2
+  if (_has_ext_exit_extended ())
+    return do_AngelSVC (AngelSVC_Reason_ReportExceptionExtended, block);
+  else
+#endif
+    return do_AngelSVC (AngelSVC_Reason_ReportException, block);
 }
diff --git a/libgloss/aarch64/svc.h b/libgloss/aarch64/svc.h
index fe249f08a..60bd00fca 100644
--- a/libgloss/aarch64/svc.h
+++ b/libgloss/aarch64/svc.h
@@ -23,37 +23,59 @@
  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
 
+#include <_ansi.h>
+
 /* Now the SWI numbers and reason codes for RDI (Angel) monitors.  */
-#define AngelSVC			0xF000
-#define AngelSVCInsn			"hlt"
-#define AngelSVCAsm			hlt
+#define AngelSVC				0xF000
+#define AngelSVCInsn				"hlt"
+#define AngelSVCAsm				hlt
 
 /* The reason codes:  */
-#define AngelSVC_Reason_Open		0x01
-#define AngelSVC_Reason_Close		0x02
-#define AngelSVC_Reason_WriteC		0x03
-#define AngelSVC_Reason_Write0		0x04
-#define AngelSVC_Reason_Write		0x05
-#define AngelSVC_Reason_Read		0x06
-#define AngelSVC_Reason_ReadC		0x07
-#define AngelSVC_Reason_IsTTY		0x09
-#define AngelSVC_Reason_Seek		0x0A
-#define AngelSVC_Reason_FLen		0x0C
-#define AngelSVC_Reason_TmpNam		0x0D
-#define AngelSVC_Reason_Remove		0x0E
-#define AngelSVC_Reason_Rename		0x0F
-#define AngelSVC_Reason_Clock		0x10
-#define AngelSVC_Reason_Time		0x11
-#define AngelSVC_Reason_System		0x12
-#define AngelSVC_Reason_Errno		0x13
-#define AngelSVC_Reason_GetCmdLine	0x15
-#define AngelSVC_Reason_HeapInfo	0x16
-#define AngelSVC_Reason_EnterSVC	0x17
-#define AngelSVC_Reason_ReportException 0x18
-#define AngelSVC_Reason_SyncCacheRange	0x19
-#define AngelSVC_Reason_Elapsed         0x30
-#define ADP_Stopped_ApplicationExit	((2 << 16) + 38)
-#define ADP_Stopped_RunTimeError	((2 << 16) + 35)
+#define AngelSVC_Reason_Open			0x01
+#define AngelSVC_Reason_Close			0x02
+#define AngelSVC_Reason_WriteC			0x03
+#define AngelSVC_Reason_Write0			0x04
+#define AngelSVC_Reason_Write			0x05
+#define AngelSVC_Reason_Read			0x06
+#define AngelSVC_Reason_ReadC			0x07
+#define AngelSVC_Reason_IsError			0x08
+#define AngelSVC_Reason_IsTTY			0x09
+#define AngelSVC_Reason_Seek			0x0A
+#define AngelSVC_Reason_FLen			0x0C
+#define AngelSVC_Reason_TmpNam			0x0D
+#define AngelSVC_Reason_Remove			0x0E
+#define AngelSVC_Reason_Rename			0x0F
+#define AngelSVC_Reason_Clock			0x10
+#define AngelSVC_Reason_Time			0x11
+#define AngelSVC_Reason_System			0x12
+#define AngelSVC_Reason_Errno			0x13
+#define AngelSVC_Reason_GetCmdLine		0x15
+#define AngelSVC_Reason_HeapInfo		0x16
+#define AngelSVC_Reason_EnterSVC		0x17
+#define AngelSVC_Reason_ReportException 	0x18
+#define AngelSVC_Reason_SyncCacheRange		0x19
+#define AngelSVC_Reason_ReportExceptionExtended 0x20
+#define AngelSVC_Reason_Elapsed			0x30
+#define AngelSVC_Reason_TickFreq		0x31
+#define ADP_Stopped_ApplicationExit		((2 << 16) + 38)
+#define ADP_Stopped_RunTimeError		((2 << 16) + 35)
+
+/* Semihosting feature magic numbers.  */
+#define NUM_SHFB_MAGIC			4
+#define SHFB_MAGIC_0			0x53
+#define SHFB_MAGIC_1			0x48
+#define SHFB_MAGIC_2			0x46
+#define SHFB_MAGIC_3			0x42
+
+/* Semihosting extensions.  */
+#define SH_EXT_EXIT_EXTENDED_BITNUM	0x0
+#define SH_EXT_STDOUT_STDERR_BITNUM	0x1
+
+#if !defined (__ASSEMBLER__)
+extern int _get_semihosting_exts _PARAMS ((char*, int, int));
+extern int _has_ext_exit_extended _PARAMS ((void));
+extern int _has_ext_stdout_stderr _PARAMS ((void));
+#endif
 
 #if defined(ARM_RDI_MONITOR) && !defined(__ASSEMBLER__)
 
diff --git a/libgloss/aarch64/syscalls.c b/libgloss/aarch64/syscalls.c
index 058cef5cb..af206a1d4 100644
--- a/libgloss/aarch64/syscalls.c
+++ b/libgloss/aarch64/syscalls.c
@@ -76,6 +76,9 @@ static int checkerror _PARAMS ((int));
 static int error _PARAMS ((int));
 static int get_errno _PARAMS ((void));
 
+/* Semihosting utilities.  */
+static void initialise_semihosting_exts _PARAMS ((void));
+
 /* Struct used to keep track of the file position, just so we
    can implement fseek(fh,x,SEEK_CUR).  */
 struct fdent
@@ -130,6 +133,9 @@ static int monitor_stdin;
 static int monitor_stdout;
 static int monitor_stderr;
 
+static int supports_ext_exit_extended = -1;
+static int supports_ext_stdout_stderr = -1;
+
 /* Return a pointer to the structure associated with
    the user file descriptor fd. */
 static struct fdent *
@@ -188,32 +194,131 @@ initialise_monitor_handles (void)
   block[1] = 0;			/* mode "r" */
   monitor_stdin = do_AngelSVC (AngelSVC_Reason_Open, block);
 
-  block[0] = POINTER_TO_PARAM_BLOCK_T (":tt");
-  block[2] = 3;			/* length of filename */
-  block[1] = 4;			/* mode "w" */
-  monitor_stdout = do_AngelSVC (AngelSVC_Reason_Open, block);
+  for (i = 0; i < MAX_OPEN_FILES; i++)
+    openfiles[i].handle = -1;;
 
-  block[0] = POINTER_TO_PARAM_BLOCK_T (":tt");
-  block[2] = 3;			/* length of filename */
-  block[1] = 8;			/* mode "a" */
-  monitor_stderr = do_AngelSVC (AngelSVC_Reason_Open, block);
+  if (_has_ext_stdout_stderr ())
+  {
+    block[0] = POINTER_TO_PARAM_BLOCK_T (":tt");
+    block[2] = 3;			/* length of filename */
+    block[1] = 4;			/* mode "w" */
+    monitor_stdout = do_AngelSVC (AngelSVC_Reason_Open, block);
+
+    block[0] = POINTER_TO_PARAM_BLOCK_T (":tt");
+    block[2] = 3;			/* length of filename */
+    block[1] = 8;			/* mode "a" */
+    monitor_stderr = do_AngelSVC (AngelSVC_Reason_Open, block);
+  }
 
   /* If we failed to open stderr, redirect to stdout. */
   if (monitor_stderr == -1)
     monitor_stderr = monitor_stdout;
 
-  for (i = 0; i < MAX_OPEN_FILES; i++)
-    openfiles[i].handle = -1;
-
   openfiles[0].handle = monitor_stdin;
   openfiles[0].flags = _FREAD;
   openfiles[0].pos = 0;
-  openfiles[1].handle = monitor_stdout;
-  openfiles[0].flags = _FWRITE;
-  openfiles[1].pos = 0;
-  openfiles[2].handle = monitor_stderr;
-  openfiles[0].flags = _FWRITE;
-  openfiles[2].pos = 0;
+
+  if (_has_ext_stdout_stderr ())
+  {
+    openfiles[1].handle = monitor_stdout;
+    openfiles[0].flags = _FWRITE;
+    openfiles[1].pos = 0;
+    openfiles[2].handle = monitor_stderr;
+    openfiles[0].flags = _FWRITE;
+    openfiles[2].pos = 0;
+  }
+}
+
+int
+_has_ext_exit_extended (void)
+{
+  if (supports_ext_exit_extended < 0)
+  {
+    initialise_semihosting_exts ();
+  }
+
+  return supports_ext_exit_extended;
+}
+
+int
+_has_ext_stdout_stderr (void)
+{
+  if (supports_ext_stdout_stderr < 0)
+  {
+    initialise_semihosting_exts ();
+  }
+
+  return supports_ext_stdout_stderr;
+}
+
+static void
+initialise_semihosting_exts (void)
+{
+  supports_ext_exit_extended = 0;
+  supports_ext_stdout_stderr = 1;
+
+#if SEMIHOST_V2
+  char features[1];
+  if (_get_semihosting_exts (features, 0, 1) > 0)
+  {
+     supports_ext_exit_extended
+       = features[0] & (1 << SH_EXT_EXIT_EXTENDED_BITNUM);
+     supports_ext_stdout_stderr
+       = features[0] & (1 << SH_EXT_STDOUT_STDERR_BITNUM);
+  }
+#endif
+}
+
+int
+_get_semihosting_exts (char* features, int offset, int num)
+{
+  int fd = _open (":semihosting-features", O_RDONLY);
+  memset (features, 0, num);
+
+  if (fd == -1)
+  {
+    return -1;
+  }
+
+  struct fdent *pfd;
+  pfd = findslot (fd);
+
+  param_block_t block[1];
+  block[0] = pfd->handle;
+
+  int len = do_AngelSVC (AngelSVC_Reason_FLen, block);
+
+  if (len < NUM_SHFB_MAGIC
+      || num > (len - NUM_SHFB_MAGIC))
+  {
+     _close (fd);
+     return -1;
+  }
+
+  char buffer[NUM_SHFB_MAGIC];
+  int n_read = _read (fd, buffer, NUM_SHFB_MAGIC);
+
+  if (n_read < NUM_SHFB_MAGIC
+      || buffer[0] != SHFB_MAGIC_0
+      || buffer[1] != SHFB_MAGIC_1
+      || buffer[2] != SHFB_MAGIC_2
+      || buffer[3] != SHFB_MAGIC_3)
+  {
+     _close (fd);
+     return -1;
+  }
+
+  if (_lseek (fd, offset, SEEK_CUR) < 0)
+  {
+     _close (fd);
+     return -1;
+  }
+
+  n_read = _read (fd, features, num);
+
+  _close (fd);
+
+  return checkerror (n_read);
 }
 
 static int