text: #include <stdio.h>
  #include <string.h>
  #include <fcntl.h>
  #include <unistd.h>
  #include <stdlib.h>
  #include <errno.h>
  
  #include "semihosting.h"
  
  #include <stlink.h>
  #include <stlink/logging.h>
  
  static int mem_read_u8(stlink_t *sl, uint32_t addr, uint8_t *data)
  {
      int offset = addr % 4;
      int len = 4;
  
      if (sl == NULL || data == NULL) {
          return -1;
      }
  
      /* Read address and length must be aligned */
      if (stlink_read_mem32(sl, addr - offset, len) != 0) {
          return -1;
      }
  
      *data = sl->q_buf[offset];
      return 0;
  }
  
  #ifdef UNUSED
  static int mem_read_u16(stlink_t *sl, uint32_t addr, uint16_t *data)
  {
      int offset = addr % 4;
      int len = (offset > 2 ? 8 : 4);
  
      if (sl == NULL || data == NULL) {
          return -1;
      }
  
      /* Read address and length must be aligned */
      if (stlink_read_mem32(sl, addr - offset, len) != 0) {
          return -1;
      }
  
      memcpy(data, &sl->q_buf[offset], sizeof(*data));
      return 0;
  }
  
  static int mem_read_u32(stlink_t *sl, uint32_t addr, uint32_t *data)
  {
      int offset = addr % 4;
      int len = (offset > 0 ? 8 : 4);
  
      if (sl == NULL || data == NULL) {
          return -1;
      }
  
      /* Read address and length must be aligned */
      if (stlink_read_mem32(sl, addr - offset, len) != 0) {
          return -1;
      }
  
      memcpy(data, &sl->q_buf[offset], sizeof(*data));
      return 0;
  }
  #endif
  
  static int mem_read(stlink_t *sl, uint32_t addr, void *data, uint16_t len)
  {
      int offset = addr % 4;
      int read_len = len + offset;
  
      if (sl == NULL || data == NULL) {
          return -1;
      }
  
      /* Align read size */
      if ((read_len % 4) != 0) {
          read_len += 4 - (read_len % 4);
      }
  
      /* Address and length must be aligned */
      if (stlink_read_mem32(sl, addr - offset, read_len) != 0) {
          return -1;
      }
  
      memcpy(data, &sl->q_buf[offset], len);
      return 0;
  }
  
  static int mem_write(stlink_t *sl, uint32_t addr, void *data, uint16_t len)
  {
      // Note: this function can write more than it is asked to!
      // If addr is not an even 32 bit boundary, or len is not a multiple of 4.
      //
      // If only 32 bit values can be written to the target,
      // then this function should read the target memory at the
      // start and end of the buffer where it will write more that
      // the requested bytes. (perhaps reading the whole area is faster??).
      //
      // If 16 and 8 bit writes are available, then they could be used instead.
   
      // Just return when the length is zero avoiding unneeded work.
      if (len == 0) return 0;
  
      int offset = addr % 4;
      int write_len = len + offset;
  
      if (sl == NULL || data == NULL) {
          return -1;
      }
  
      /* Align read size */
      if ((write_len % 4) != 0) {
          write_len += 4 - (write_len % 4);
      }
  
      memcpy(&sl->q_buf[offset], data, len);
  
      /* Address and length must be aligned */
      if (stlink_write_mem32(sl, addr - offset, write_len) != 0) {
          return -1;
      }
  
      return 0;
  }
  
  /* For the SYS_WRITE0 call, we don't know the size of the null-terminated buffer
   * in the target memory. Instead of reading one byte at a time, we read by
   * chunks of WRITE0_BUFFER_SIZE bytes.
   */
  #define WRITE0_BUFFER_SIZE 64
  
  /* Define a maximum size for buffers transmitted by semihosting. There is no
   * limit in the ARM specification but this is a safety net.
   * We remove 4 byte from Q_BUF_LEN to handle alignment correction.
   */
  #define MAX_BUFFER_SIZE (Q_BUF_LEN - 4)
  
  /* Flags for Open syscall */
  
  #ifndef O_BINARY
  #define O_BINARY 0
  #endif
  
  static int open_mode_flags[12] = {
      O_RDONLY,
      O_RDONLY | O_BINARY,
      O_RDWR,
      O_RDWR   | O_BINARY,
      O_WRONLY | O_CREAT | O_TRUNC,
      O_WRONLY | O_CREAT | O_TRUNC  | O_BINARY,
      O_RDWR   | O_CREAT | O_TRUNC,
      O_RDWR   | O_CREAT | O_TRUNC  | O_BINARY,
      O_WRONLY | O_CREAT | O_APPEND,
      O_WRONLY | O_CREAT | O_APPEND | O_BINARY,
      O_RDWR   | O_CREAT | O_APPEND,
      O_RDWR   | O_CREAT | O_APPEND | O_BINARY
  };
  
  static int saved_errno = 0;
  
  int do_semihosting (stlink_t *sl, uint32_t r0, uint32_t r1, uint32_t *ret) {
  
      if (sl == NULL || ret == NULL) {
          return -1;
      }
  
      DLOG("Do semihosting R0=0x%08x R1=0x%08x\n", r0, r1);
  
      switch (r0) {
      case SEMIHOST_SYS_OPEN:
      {
          uint32_t args[3];
          uint32_t name_address;
          uint32_t mode;
          uint32_t name_len;
          char     *name;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_OPEN error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          name_address = args[0];
          mode         = args[1];
          name_len     = args[2];
  
          if (mode > 12) {
              /* Invalid mode */
              DLOG("Semihosting SYS_OPEN error: invalid mode %d\n", mode);
              *ret = -1;
              return -1;
          }
  
          /* Add the trailing zero that is not counted in the length argument (see
           * ARM semihosting specification)
           */
          name_len += 1;
  
          if (name_len > MAX_BUFFER_SIZE) {
              DLOG("Semihosting SYS_OPEN error: name buffer size is too big %d\n",
                  name_len);
              *ret = -1;
              return -1;
          }
          name = malloc(name_len);
  
          if (name == NULL) {
              DLOG("Semihosting SYS_OPEN error: cannot allocate name buffer\n");
              *ret = -1;
              return -1;
          }
  
          if (mem_read(sl, name_address, name, name_len) != 0 ) {
              free(name);
              *ret = -1;
              DLOG("Semihosting SYS_OPEN error: "
                   "cannot read name from target memory\n");
              return -1;
          }
  
          DLOG("Semihosting: open('%s', (SH open mode)%d, 0644)\n", name, mode);
  
          *ret = (uint32_t)open(name, open_mode_flags[mode], 0644);
          saved_errno = errno;
  
          DLOG("Semihosting: return %d\n", *ret);
  
          free(name);
          break;
      }
      case SEMIHOST_SYS_CLOSE:
      {
          uint32_t args[1];
          int      fd;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_CLOSE error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          fd = (int)args[0];
  
          DLOG("Semihosting: close(%d)\n", fd);
  
          *ret = (uint32_t)close(fd);
          saved_errno = errno;
  
          DLOG("Semihosting: return %d\n", *ret);
          break;
      }
      case SEMIHOST_SYS_WRITE:
      {
          uint32_t args[3];
          uint32_t buffer_address;
          int      fd;
          uint32_t buffer_len;
          void    *buffer;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_WRITE error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          fd             = (int)args[0];
          buffer_address = args[1];
          buffer_len     = args[2];
  
          if (buffer_len > MAX_BUFFER_SIZE) {
              DLOG("Semihosting SYS_WRITE error: buffer size is too big %d\n",
                  buffer_len);
              *ret = buffer_len;
              return -1;
          }
  
          buffer = malloc(buffer_len);
  
          if (buffer == NULL) {
              DLOG("Semihosting SYS_WRITE error: cannot allocate buffer\n");
              *ret = buffer_len;
              return -1;
          }
  
          if (mem_read(sl, buffer_address, buffer, buffer_len) != 0 ) {
              DLOG("Semihosting SYS_WRITE error: "
                   "cannot read buffer from target memory\n");
              free(buffer);
              *ret = buffer_len;
              return -1;
          }
  
          DLOG("Semihosting: write(%d, target_addr:0x%08x, %zu)\n", fd,
               buffer_address, buffer_len);
  
          *ret = (uint32_t)write(fd, buffer, buffer_len);
          saved_errno = errno;
  
          if (*ret == (uint32_t)-1) {
              *ret = buffer_len;
          } else {
              *ret -= buffer_len;
          }
  
          DLOG("Semihosting: return %d\n", *ret);
          free(buffer);
          break;
      }
      case SEMIHOST_SYS_READ:
      {
          uint32_t args[3];
          uint32_t buffer_address;
          int      fd;
          uint32_t buffer_len;
          void    *buffer;
  	ssize_t  read_result;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_READ error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          fd             = (int)args[0];
          buffer_address = args[1];
          buffer_len     = args[2];
  
          if (buffer_len > MAX_BUFFER_SIZE) {
              DLOG("Semihosting SYS_READ error: buffer size is too big %d\n",
                  buffer_len);
              *ret = buffer_len;
              return -1;
          }
  
          buffer = malloc(buffer_len);
  
          if (buffer == NULL) {
              DLOG("Semihosting SYS_READ error: cannot allocatebuffer\n");
              *ret = buffer_len;
              return -1;
          }
  
          DLOG("Semihosting: read(%d, target_addr:0x%08x, %zu)\n", fd,
               buffer_address, buffer_len);
  
          read_result = read(fd, buffer, buffer_len);
          saved_errno = errno;
  
          if (read_result == -1) {
              *ret = buffer_len;
          } else {
              if (mem_write(sl, buffer_address, buffer, read_result) != 0 ) {
                  DLOG("Semihosting SYS_READ error: "
                       "cannot write buffer to target memory\n");
                  free(buffer);
                  *ret = buffer_len;
                  return -1;
              } else {
                  *ret = buffer_len - (uint32_t)read_result;
              }
          }
  
          DLOG("Semihosting: return %d\n", *ret);
          free(buffer);
          break;
      }
      case SEMIHOST_SYS_ERRNO:
      {
          *ret = (uint32_t)saved_errno;
          DLOG("Semihosting: Errno return %d\n", *ret);
          break;
      }
      case SEMIHOST_SYS_REMOVE:
      {
          uint32_t args[2];
          uint32_t name_address;
          uint32_t name_len;
          char     *name;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_REMOVE error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          name_address = args[0];
          name_len     = args[1];
  
          /* Add the trailing zero that is not counted in the length argument (see
           * ARM semihosting specification)
           */
          name_len += 1;
  
          if (name_len > MAX_BUFFER_SIZE) {
              DLOG("Semihosting SYS_REMOVE error: "
                   "name buffer size is too big %d\n", name_len);
              *ret = -1;
              return -1;
          }
          name = malloc(name_len);
  
          if (name == NULL) {
              DLOG("Semihosting SYS_REMOVE error: cannot allocate name buffer\n");
              *ret = -1;
              return -1;
          }
  
          if (mem_read(sl, name_address, name, name_len) != 0 ) {
              free(name);
              *ret = -1;
              DLOG("Semihosting SYS_REMOVE error: "
                   "cannot read name from target memory\n");
              return -1;
          }
  
          DLOG("Semihosting: unlink('%s')\n", name);
  
          *ret = (uint32_t)unlink(name);
          saved_errno = errno;
  
          DLOG("Semihosting: return %d\n", *ret);
  
          free(name);
          break;
      }
      case SEMIHOST_SYS_SEEK:
      {
          uint32_t args[2];
          int      fd;
          off_t    offset;
  
          if (mem_read(sl, r1, args, sizeof (args)) != 0 ) {
              DLOG("Semihosting SYS_SEEK error: "
                   "cannot read args from target memory\n");
              *ret = -1;
              return -1;
          }
  
          fd = (int)args[0];
          offset = (off_t)args[1];
  
          DLOG("Semihosting: lseek(%d, %d, SEEK_SET)\n", fd, offset);
  
          *ret = (uint32_t)lseek(fd, offset, SEEK_SET);
          saved_errno = errno;
  
          if (*ret != (uint32_t)-1) {
              /* Success */
              *ret = 0;
          }
          DLOG("Semihosting: return %d\n", *ret);
          break;
      }
      case SEMIHOST_SYS_WRITEC:
      {
          uint8_t c;
          if (mem_read_u8(sl, r1, &c) == 0) {
              fprintf(stderr, "%c", c);
          } else {
              DLOG("Semihosting WRITEC: "
                   "cannot read target memory at 0x%08x\n", r1);
          }
          break;
      }
      case SEMIHOST_SYS_READC:
      {
          uint8_t c = getchar();
          *ret = c;
          break;
      }
      case SEMIHOST_SYS_WRITE0:
      {
          uint8_t buf[WRITE0_BUFFER_SIZE];
  
          while (true) {
              if (mem_read(sl, r1, buf, WRITE0_BUFFER_SIZE) != 0 ) {
                  DLOG("Semihosting WRITE0: "
                       "cannot read target memory at 0x%08x\n", r1);
                  return -1;
              }
              for (int i = 0; i < WRITE0_BUFFER_SIZE; i++) {
                  if (buf[i] == 0) {
                      return 0;
                  }
                  fprintf(stderr, "%c", buf[i]);
              }
              r1 += WRITE0_BUFFER_SIZE;
          }
          break;
      }
      default:
          fprintf(stderr, "semihosting: unsupported call %#x\n", r0);
          return -1;
      }
      return 0;
  }
  