Add support for Semihosting v2 support for ARM 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>

3 files changed, 231 insertions, 65 deletions

diff --git a/libgloss/arm/_kill.c b/libgloss/arm/_kill.c
index 81354d473..8871d3c36 100644
--- a/libgloss/arm/_kill.c
+++ b/libgloss/arm/_kill.c
@@ -10,15 +10,36 @@ _kill (int pid, int sig)
   (void) pid; (void) sig;
 #ifdef ARM_RDI_MONITOR
   /* Note: The pid argument is thrown away.  */
+  int block[2];
+  block[1] = sig;
+  int insn;
+
+#if SEMIHOST_V2
+  if (_has_ext_exit_extended ())
+    {
+      insn = AngelSWI_Reason_ReportExceptionExtended;
+    }
+  else
+#endif
+    {
+      insn = AngelSWI_Reason_ReportException;
+    }
+
   switch (sig)
     {
     case SIGABRT:
-      return do_AngelSWI (AngelSWI_Reason_ReportException,
-			  (void *) ADP_Stopped_RunTimeError);
+      {
+	block[0] = ADP_Stopped_RunTimeError;
+	break;
+      }
     default:
-      return do_AngelSWI (AngelSWI_Reason_ReportException,
-			  (void *) ADP_Stopped_ApplicationExit);
+      {
+	block[0] = ADP_Stopped_ApplicationExit;
+	break;
+      }
     }
+
+  return do_AngelSWI (insn, block);
 #else
   asm ("swi %a0" :: "i" (SWI_Exit));
 #endif
diff --git a/libgloss/arm/swi.h b/libgloss/arm/swi.h
index b7fa243f8..72cb838dc 100644
--- a/libgloss/arm/swi.h
+++ b/libgloss/arm/swi.h
@@ -1,4 +1,5 @@
 #include "arm.h"
+#include <_ansi.h>
 
 /* SWI numbers for RDP (Demon) monitor.  */
 #define SWI_WriteC                 0x0
@@ -29,45 +30,76 @@
 
 
 /* Now the SWI numbers and reason codes for RDI (Angel) monitors.  */
-#define AngelSWI_ARM 			0x123456
-#ifdef __thumb__
-#define AngelSWI 			0xAB
-#else
-#define AngelSWI 			AngelSWI_ARM
-#endif
+#if defined (SEMIHOST_V2) && defined (SEMIHOST_V2_MIXED_MODE)
+  #define AngelSWI_ARM			0xF000 /* HLT A32.  */
+  #ifdef __thumb__
+    #define AngelSWI			0x3C /* HLT T32.  */
+  #else /* __thumb__.  */
+    #define AngelSWI			AngelSWI_ARM
+  #endif /* __thumb__.  */
+#else  /* SEMIHOST_V2.  */
+  #define AngelSWI_ARM			0x123456 /* SVC A32.  */
+  #ifdef __thumb__
+    #define AngelSWI			0xAB /* SVC T32.  */
+  #else /* __thumb__.  */
+    #define AngelSWI			AngelSWI_ARM
+  #endif /* __thumb__.  */
+#endif /* SEMIHOST_V2.  */
+
 /* For thumb only architectures use the BKPT instruction instead of SWI.  */
 #ifdef THUMB_VXM
-#define AngelSWIInsn			"bkpt"
-#define AngelSWIAsm			bkpt
+  #define AngelSWIInsn			"bkpt"
+  #define AngelSWIAsm			bkpt
 #else
-#define AngelSWIInsn			"swi"
-#define AngelSWIAsm			swi
+  #define AngelSWIInsn			"swi"
+  #define AngelSWIAsm			swi
 #endif
 
 /* The reason codes:  */
-#define AngelSWI_Reason_Open		0x01
-#define AngelSWI_Reason_Close		0x02
-#define AngelSWI_Reason_WriteC		0x03
-#define AngelSWI_Reason_Write0		0x04
-#define AngelSWI_Reason_Write		0x05
-#define AngelSWI_Reason_Read		0x06
-#define AngelSWI_Reason_ReadC		0x07
-#define AngelSWI_Reason_IsTTY		0x09
-#define AngelSWI_Reason_Seek		0x0A
-#define AngelSWI_Reason_FLen		0x0C
-#define AngelSWI_Reason_TmpNam		0x0D
-#define AngelSWI_Reason_Remove		0x0E
-#define AngelSWI_Reason_Rename		0x0F
-#define AngelSWI_Reason_Clock		0x10
-#define AngelSWI_Reason_Time		0x11
-#define AngelSWI_Reason_System		0x12
-#define AngelSWI_Reason_Errno		0x13
-#define AngelSWI_Reason_GetCmdLine 	0x15
-#define AngelSWI_Reason_HeapInfo 	0x16
-#define AngelSWI_Reason_EnterSVC 	0x17
-#define AngelSWI_Reason_ReportException 0x18
-#define ADP_Stopped_ApplicationExit 	((2 << 16) + 38)
-#define ADP_Stopped_RunTimeError 	((2 << 16) + 35)
+#define AngelSWI_Reason_Open			0x01
+#define AngelSWI_Reason_Close			0x02
+#define AngelSWI_Reason_WriteC			0x03
+#define AngelSWI_Reason_Write0			0x04
+#define AngelSWI_Reason_Write			0x05
+#define AngelSWI_Reason_Read			0x06
+#define AngelSWI_Reason_ReadC			0x07
+#define AngelSWI_Reason_IsError			0x08
+#define AngelSWI_Reason_IsTTY			0x09
+#define AngelSWI_Reason_Seek			0x0A
+#define AngelSWI_Reason_FLen			0x0C
+#define AngelSWI_Reason_TmpNam			0x0D
+#define AngelSWI_Reason_Remove			0x0E
+#define AngelSWI_Reason_Rename			0x0F
+#define AngelSWI_Reason_Clock			0x10
+#define AngelSWI_Reason_Time			0x11
+#define AngelSWI_Reason_System			0x12
+#define AngelSWI_Reason_Errno			0x13
+#define AngelSWI_Reason_GetCmdLine		0x15
+#define AngelSWI_Reason_HeapInfo		0x16
+#define AngelSWI_Reason_EnterSVC		0x17
+#define AngelSWI_Reason_ReportException		0x18
+#define AngelSWI_Reason_ReportExceptionExtended 0x20
+#define AngelSWI_Reason_Elapsed			0x30
+#define AngelSWI_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/arm/syscalls.c b/libgloss/arm/syscalls.c
index 0ccad215c..80713680b 100644
--- a/libgloss/arm/syscalls.c
+++ b/libgloss/arm/syscalls.c
@@ -47,6 +47,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
@@ -95,6 +98,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*
@@ -154,15 +160,21 @@ initialise_monitor_handles (void)
   block[1] = 0;     /* mode "r" */
   monitor_stdin = do_AngelSWI (AngelSWI_Reason_Open, (void *) block);
 
-  block[0] = (int) ":tt";
-  block[2] = 3;     /* length of filename */
-  block[1] = 4;     /* mode "w" */
-  monitor_stdout = do_AngelSWI (AngelSWI_Reason_Open, (void *) block);
+  for (i = 0; i < MAX_OPEN_FILES; i ++)
+    openfiles[i].handle = -1;
 
-  block[0] = (int) ":tt";
-  block[2] = 3;     /* length of filename */
-  block[1] = 8;     /* mode "a" */
-  monitor_stderr = do_AngelSWI (AngelSWI_Reason_Open, (void *) block);
+  if (_has_ext_stdout_stderr ())
+  {
+    block[0] = (int) ":tt";
+    block[2] = 3;     /* length of filename */
+    block[1] = 4;     /* mode "w" */
+    monitor_stdout = do_AngelSWI (AngelSWI_Reason_Open, (void *) block);
+
+    block[0] = (int) ":tt";
+    block[2] = 3;     /* length of filename */
+    block[1] = 8;     /* mode "a" */
+    monitor_stderr = do_AngelSWI (AngelSWI_Reason_Open, (void *) block);
+  }
 #else
   int fh;
   const char * name;
@@ -174,34 +186,135 @@ initialise_monitor_handles (void)
        : "r0","r1");
   monitor_stdin = fh;
 
-  name = ":tt";
-  asm ("mov r0,%2; mov r1, #4; swi %a1; mov %0, r0"
-       : "=r"(fh)
-       : "i" (SWI_Open),"r"(name)
-       : "r0","r1");
-  monitor_stdout = fh;
-
-  name = ":tt";
-  asm ("mov r0,%2; mov r1, #8; swi %a1; mov %0, r0"
-       : "=r"(fh)
-       : "i" (SWI_Open),"r"(name)
-       : "r0","r1");
-  monitor_stderr = fh;
+  if (_has_ext_stdout_stderr ())
+  {
+    name = ":tt";
+    asm ("mov r0,%2; mov r1, #4; swi %a1; mov %0, r0"
+	 : "=r"(fh)
+	 : "i" (SWI_Open),"r"(name)
+	 : "r0","r1");
+    monitor_stdout = fh;
+
+    name = ":tt";
+    asm ("mov r0,%2; mov r1, #8; swi %a1; mov %0, r0"
+	 : "=r"(fh)
+	 : "i" (SWI_Open),"r"(name)
+	 : "r0","r1");
+    monitor_stderr = fh;
+  }
 #endif
 
   /* 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].pos = 0;
-  openfiles[1].handle = monitor_stdout;
-  openfiles[1].pos = 0;
-  openfiles[2].handle = monitor_stderr;
-  openfiles[2].pos = 0;
+
+  if (_has_ext_stdout_stderr ())
+  {
+    openfiles[1].handle = monitor_stdout;
+    openfiles[1].pos = 0;
+    openfiles[2].handle = monitor_stderr;
+    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 len;
+  struct fdent *pfd;
+  int fd = _open (":semihosting-features", O_RDONLY);
+  memset (features, 0, num);
+
+  if (fd == -1)
+  {
+    return -1;
+  }
+
+  pfd = findslot (fd);
+
+#ifdef ARM_RDI_MONITOR
+  len = checkerror (do_AngelSWI (AngelSWI_Reason_FLen, &pfd->handle));
+#else
+  asm ("mov r0,%2; swi %a1; mov %0, r0"
+       : "=r"(len)
+       : "i" (SWI_Flen),"r"(pfd->handle)
+       : "r0");
+#endif
+
+  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