forked from Archive/PX4-Autopilot
2376 lines
61 KiB
C
2376 lines
61 KiB
C
/****************************************************************************
|
|
* pexec.c
|
|
*
|
|
* Copyright (C) 200-2009 Gregory Nutt. All rights reserved.
|
|
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name NuttX nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Included Files
|
|
****************************************************************************/
|
|
|
|
#include <sys/types.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
|
|
#include "keywords.h"
|
|
#include "pdefs.h"
|
|
#include "pinsn16.h"
|
|
#include "pfdefs.h"
|
|
#include "pxdefs.h"
|
|
#include "pedefs.h"
|
|
|
|
#include "paslib.h"
|
|
#include "pexec.h"
|
|
|
|
#ifdef CONFIG_HAVE_LIBM
|
|
#include <math.h>
|
|
#endif
|
|
|
|
/****************************************************************************
|
|
* Definitions
|
|
****************************************************************************/
|
|
|
|
#define PTRUE ((ustack_t)-1)
|
|
#define PFALSE ((ustack_t) 0)
|
|
|
|
/****************************************************************************
|
|
* Macros
|
|
****************************************************************************/
|
|
|
|
/* Remove the value from the top of the stack */
|
|
|
|
#define POP(st, dest) \
|
|
do { \
|
|
dest = (st)->dstack.i[BTOISTACK((st)->sp)]; \
|
|
(st)->sp -= BPERI; \
|
|
} while (0)
|
|
|
|
/* Add the value to top of the stack */
|
|
|
|
#define PUSH(st, src) \
|
|
do { \
|
|
(st)->sp += BPERI; \
|
|
(st)->dstack.i[BTOISTACK((st)->sp)] = src; \
|
|
} while (0)
|
|
|
|
/* Return an rvalue for the (word) offset from the top of the stack */
|
|
|
|
#define TOS(st, off) \
|
|
(st)->dstack.i[BTOISTACK((st)->sp)-(off)]
|
|
|
|
/* Save the src (word) at the dest (word) stack position */
|
|
|
|
#define PUTSTACK(st, src, dest) \
|
|
do { \
|
|
(st)->dstack.i[BTOISTACK(dest)] = src; \
|
|
} while (0)
|
|
|
|
/* Return an rvalue for the (word) from the absolute stack position */
|
|
|
|
#define GETSTACK(st, src) \
|
|
(st)->dstack.i[BTOISTACK(src)]
|
|
|
|
/* Store a byte to an absolute (byte) stack position */
|
|
|
|
#define PUTBSTACK(st, src,dest) \
|
|
do { \
|
|
(st)->dstack.b[dest] = dest; \
|
|
} while (0)
|
|
|
|
/* Return an rvalue for the absolute (byte) stack position */
|
|
|
|
#define GETBSTACK(st, src) \
|
|
(st)->dstack.b[src]
|
|
|
|
/* Return the address for an absolute (byte) stack position. */
|
|
|
|
#define ATSTACK(st, src) \
|
|
&(st)->dstack.b[src]
|
|
|
|
/* Discard n words from the top of the stack */
|
|
|
|
#define DISCARD(st, n) \
|
|
do { \
|
|
(st)->sp -= BPERI*(n); \
|
|
} while (0)
|
|
|
|
/* Release a C string */
|
|
|
|
#define free_cstring(a) \
|
|
free(a)
|
|
|
|
/****************************************************************************
|
|
* Private Type Definitions
|
|
****************************************************************************/
|
|
|
|
union fparg_u
|
|
{
|
|
double f;
|
|
uint16_t hw[4];
|
|
};
|
|
|
|
typedef union fparg_u fparg_t;
|
|
|
|
/****************************************************************************
|
|
* Private Function Prototypes
|
|
****************************************************************************/
|
|
|
|
static uint16_t pexec_sysio(struct pexec_s *st, uint8_t fno, uint16_t subfunc);
|
|
static uint16_t pexec_libcall(struct pexec_s *st, uint16_t subfunc);
|
|
static uint16_t pexec_execfp(struct pexec_s *st, uint8_t fpop);
|
|
static void pexec_getfparguments(struct pexec_s *st, uint8_t fpop, fparg_t *arg1, fparg_t *arg2);
|
|
static ustack_t pexec_readinteger(uint8_t *ioptr);
|
|
static void pexec_readreal(uint16_t *dest, uint8_t *ioptr);
|
|
static ustack_t pexec_getbaseaddress(struct pexec_s *st, level_t leveloffset);
|
|
static uint8_t *pexec_mkcstring(uint8_t *buffer, int buflen);
|
|
|
|
/****************************************************************************
|
|
* Private Variables
|
|
****************************************************************************/
|
|
|
|
static uint8_t ioline[LINE_SIZE+1];
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_sysio
|
|
*
|
|
* Description:
|
|
* This function process a system I/O operation.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static uint16_t pexec_sysio(struct pexec_s *st, uint8_t fno, uint16_t subfunc)
|
|
{
|
|
ustack_t uparm1;
|
|
fparg_t fp;
|
|
|
|
uint8_t *ptr;
|
|
|
|
switch (subfunc)
|
|
{
|
|
case xEOF :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
case xEOLN :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
case xRESET :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
case xREWRITE :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
|
|
case xREADLN :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
case xREAD_BINARY :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
|
|
/* xREAD_INT:
|
|
* STACK INPUTS: TOS(st, 0) = address to store integer */
|
|
case xREAD_INT :
|
|
(void)fgets((char*)ioline, LINE_SIZE, stdin);
|
|
PUTSTACK(st, pexec_readinteger(ioline),TOS(st, 0));
|
|
break;
|
|
|
|
/* xREAD_CHAR:
|
|
* STACK INPUTS: TOS(st, 0) = address to store integer */
|
|
|
|
case xREAD_CHAR:
|
|
(void)fgets((char*)ioline, LINE_SIZE, stdin);
|
|
PUTBSTACK(st, ioline[0],TOS(st, 0));
|
|
break;
|
|
|
|
/* XREAD_STRING:
|
|
|
|
* STACK INPUTS:
|
|
* TOS = Number of bytes to read
|
|
* TOS-1 = Address to store byte(s) */
|
|
case xREAD_STRING :
|
|
(void)fgets((char*)ATSTACK(st, TOS(st, 1)), TOS(st, 0), stdin);
|
|
break;
|
|
|
|
/* xREAD_REAL:
|
|
* STACK INPUTS: TOS = address to store REAL */
|
|
|
|
case xREAD_REAL :
|
|
(void)fgets((char*)ioline, LINE_SIZE, stdin);
|
|
pexec_readreal((uint16_t*)ATSTACK(st, TOS(st, 0)), ioline);
|
|
break;
|
|
|
|
case xWRITELN :
|
|
putchar('\n');
|
|
break;
|
|
case xWRITE_PAGE :
|
|
putchar('\f');
|
|
break;
|
|
case xWRITE_BINARY :
|
|
/* FINISH ME -- > */
|
|
break;
|
|
|
|
/* xWRITE_INT:
|
|
* STACK INPUTS: TOS = integer value to write. */
|
|
|
|
case xWRITE_INT :
|
|
printf("%ld", signExtend16(TOS(st, 0)));
|
|
break;
|
|
|
|
/* xWRITE_CHAR:
|
|
* STACK INPUTS: TOS = char value to write. */
|
|
|
|
case xWRITE_CHAR :
|
|
putchar(TOS(st, 0));
|
|
break;
|
|
|
|
/* xWRITE_STRING:
|
|
* STACK INPUTS:
|
|
* TOS = Number of bytes to write
|
|
* TOS-1 = Address of src data */
|
|
|
|
case xWRITE_STRING :
|
|
uparm1 = TOS(st, 0);
|
|
for (ptr = (uint8_t*)ATSTACK(st, TOS(st, 1)); uparm1; uparm1--, ptr++)
|
|
putchar(*ptr);
|
|
break;
|
|
|
|
/* xWRITE_REAL:
|
|
* STACK INPUTS: TOS = value of double */
|
|
|
|
case xWRITE_REAL :
|
|
fp.hw[0] = TOS(st, 3);
|
|
fp.hw[1] = TOS(st, 2);
|
|
fp.hw[2] = TOS(st, 1);
|
|
fp.hw[3] = TOS(st, 0);;
|
|
printf("%f", fp.f);
|
|
break;
|
|
|
|
default :
|
|
return eBADSYSIOFUNC;
|
|
|
|
}
|
|
|
|
return eNOERROR;
|
|
|
|
} /* end pexec_sysio */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_libcall
|
|
*
|
|
* Description:
|
|
* This function process a system I/O operation
|
|
*
|
|
****************************************************************************/
|
|
|
|
static uint16_t pexec_libcall(struct pexec_s *st, uint16_t subfunc)
|
|
{
|
|
ustack_t uparm1;
|
|
ustack_t uparm2;
|
|
paddr_t addr1;
|
|
paddr_t addr2;
|
|
uint16_t *tmp;
|
|
uint16_t *ref;
|
|
uint8_t *src;
|
|
uint8_t *dest;
|
|
uint8_t *name;
|
|
int len;
|
|
int32_t value;
|
|
|
|
switch (subfunc)
|
|
{
|
|
/* Get the value of an environment string
|
|
*
|
|
* ON INPUT:
|
|
* TOS(st, 0) = Number of bytes in environment identifier string
|
|
* TOS(st, 1) = Address environment identifier string
|
|
* ON RETURN (above replaced with):
|
|
* TOS(st, 0) = MS 16-bits of 32-bit C string pointer
|
|
* TOS(st, 1) = LS 16-bits of 32-bit C string pointer
|
|
*/
|
|
|
|
case lbGETENV :
|
|
len = TOS(st, 0); /* Number of bytes in string */
|
|
src = (uint8_t*)&GETSTACK(st, TOS(st, 1)); /* Pointer to string */
|
|
|
|
/* Make a C string out of the pascal string */
|
|
|
|
name = pexec_mkcstring(src, len);
|
|
if (name == NULL)
|
|
{
|
|
return eNOMEMORY;
|
|
}
|
|
|
|
/* Make the C-library call and free the string copy */
|
|
|
|
src = (uint8_t*)getenv((char*)name);
|
|
free_cstring(name);
|
|
|
|
/* Save the returned pointer in the stack */
|
|
|
|
TOS(st, 0) = (ustack_t)((uint32_t)src >> 16);
|
|
TOS(st, 1) = (ustack_t)((uint32_t)src & 0x0000ffff);
|
|
break;
|
|
|
|
/* Copy pascal string to a pascal string
|
|
*
|
|
* ON INPUT:
|
|
* TOS(st, 0) = address of dest string hdr
|
|
* TOS(st, 1) = length of source string
|
|
* TOS(st, 2) = pointer to source string
|
|
* ON RETURN (input consumed):
|
|
*/
|
|
|
|
case lbSTR2STR :
|
|
/* "Pop" in the input parameters from the stack */
|
|
|
|
POP(st, addr1); /* addr of dest string header */
|
|
POP(st, uparm1); /* length of source data */
|
|
POP(st, addr2); /* addr of source string data */
|
|
|
|
/* Do nothing if the source and destinations are the same
|
|
* string. This happens normally on cases like:
|
|
* string name;
|
|
* char c;
|
|
* name := name + c;
|
|
*/
|
|
|
|
if (addr1 != addr2)
|
|
{
|
|
/* The source and destination strings are different.
|
|
* Make sure that the string length will fit into the destination.
|
|
*/
|
|
|
|
if (uparm1 >= sSTRING_MAX_SIZE)
|
|
{
|
|
/* Clip to the maximum size */
|
|
|
|
uparm1 = sSTRING_MAX_SIZE;
|
|
len = sSTRING_MAX_SIZE;
|
|
}
|
|
else
|
|
{
|
|
/* We have space */
|
|
|
|
len = (int)uparm1;
|
|
}
|
|
|
|
/* Get proper string pointers */
|
|
|
|
dest = ATSTACK(st, addr1);
|
|
src = ATSTACK(st, addr2);
|
|
|
|
/* Transfer the (16-bit) string length (must be aligned!) */
|
|
|
|
tmp = (uint16_t*)dest;
|
|
*tmp++ = uparm1;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
/* Then transfer the string contents */
|
|
|
|
memcpy(dest, src, len);
|
|
}
|
|
break;
|
|
|
|
/* Copy C string to a pascal string
|
|
*
|
|
* ON INPUT:
|
|
* TOS(st, 0) = address of dest hdr
|
|
* TOS(st, 1) = MS 16-bits of 32-bit C string pointer
|
|
* TOS(st, 2) = LS 16-bits of 32-bit C string pointer
|
|
* ON RETURN (input consumed):
|
|
*/
|
|
case lbCSTR2STR :
|
|
/* "Pop" in the input parameters from the stack */
|
|
|
|
POP(st, addr1); /* addr of dest string header */
|
|
POP(st, uparm1); /* MS 16-bits of 32-bit C string pointer */
|
|
POP(st, uparm2); /* LS 16-bits of 32-bit C string pointer */
|
|
|
|
/* Get proper string pointers */
|
|
|
|
dest = ATSTACK(st, addr1);
|
|
src = (uint8_t*)((unsigned long)uparm1 << 16 | (unsigned long)uparm2);
|
|
|
|
/* Handle null src pointer */
|
|
|
|
if (src == NULL)
|
|
{
|
|
*dest = 0;
|
|
}
|
|
else
|
|
{
|
|
/* Get the length of the string */
|
|
|
|
uparm1 = strlen((char*)src);
|
|
|
|
/* Make sure that the string length will fit into the
|
|
* destination. */
|
|
|
|
if (uparm1 >= sSTRING_MAX_SIZE)
|
|
{
|
|
/* Clip to the maximum size */
|
|
|
|
uparm1 = sSTRING_MAX_SIZE;
|
|
len = sSTRING_MAX_SIZE;
|
|
}
|
|
else
|
|
{
|
|
/* We have space */
|
|
|
|
len = (int)uparm1;
|
|
}
|
|
|
|
/* Transfer the (16-bit) string length (must be aligned!) */
|
|
|
|
tmp = (uint16_t*)dest;
|
|
*tmp++ = uparm1;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
/* Then transfer the string contents */
|
|
|
|
memcpy(dest, src, len);
|
|
}
|
|
break;
|
|
|
|
/* Copy pascal string to a pascal string reference
|
|
* procedure str2rstr(src : string; var dest : rstring)
|
|
* ON INPUT:
|
|
* TOS(st, 0)=address of dest string reference
|
|
* TOS(st, 1)=length of source string
|
|
* TOS(st, 2)=pointer to source string
|
|
* ON RETURN: actual parameters released.
|
|
*/
|
|
|
|
case lbSTR2RSTR :
|
|
/* "Pop" in the input parameters from the stack */
|
|
|
|
POP(st, addr1); /* addr of dest string reference */
|
|
POP(st, uparm1); /* length of source data */
|
|
POP(st, addr2); /* addr of source string data */
|
|
|
|
/* Make sure that the string length will fit into the destination. */
|
|
|
|
if (uparm1 >= sSTRING_MAX_SIZE)
|
|
{
|
|
return eSTRSTKOVERFLOW;
|
|
}
|
|
|
|
/* Get a pointer to the destination reference */
|
|
|
|
ref = (uint16_t*)ATSTACK(st, addr1);
|
|
|
|
/* Get proper string pointers */
|
|
|
|
dest = ATSTACK(st, ref[0] - 2);
|
|
src = ATSTACK(st, addr2);
|
|
|
|
/* Transfer the (16-bit) string length (must be aligned!) */
|
|
|
|
tmp = (uint16_t*)dest;
|
|
*tmp++ = uparm1;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
/* Then transfer the string contents and save the new size */
|
|
|
|
memcpy(dest, src, uparm1);
|
|
ref[1] = uparm1;
|
|
break;
|
|
|
|
/* Copy C string to a pascal string reference
|
|
* procedure cstr2str(src : cstring; var dest : string)
|
|
* ON INPUT:
|
|
* TOS(st, 0)=address of dest string reference
|
|
* TOS(st, 0)=MS 16-bits of 32-bit C source string pointer
|
|
* TOS(st, 1)=LS 16-bits of 32-bit C source string pointer
|
|
* ON RETURN: actual parameters released
|
|
*/
|
|
|
|
case lbCSTR2RSTR :
|
|
/* "Pop" in the input parameters from the stack */
|
|
|
|
POP(st, addr1); /* addr of dest string reference */
|
|
POP(st, uparm1); /* MS 16-bits of 32-bit C string pointer */
|
|
POP(st, uparm2); /* LS 16-bits of 32-bit C string pointer */
|
|
|
|
/* Get a pointer to the destination reference */
|
|
|
|
ref = (uint16_t*)ATSTACK(st, addr1);
|
|
|
|
/* Get proper string pointers */
|
|
|
|
dest = ATSTACK(st, ref[0] - 2);
|
|
src = (uint8_t*)((unsigned long)uparm1 << 16 | (unsigned long)uparm2);
|
|
|
|
/* Handle null src pointer */
|
|
|
|
if (src == NULL)
|
|
{
|
|
*dest = 0;
|
|
}
|
|
else
|
|
{
|
|
/* Get the length of the string */
|
|
|
|
uparm1 = strlen((char*)src);
|
|
|
|
/* Make sure that the string length will fit into the
|
|
* destination. */
|
|
|
|
if (uparm1 >= sSTRING_MAX_SIZE)
|
|
{
|
|
return eSTRSTKOVERFLOW;
|
|
}
|
|
|
|
/* Transfer the (16-bit) string length (must be aligned!) */
|
|
|
|
tmp = (uint16_t*)dest;
|
|
*tmp++ = uparm1;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
/* Then transfer the string contents */
|
|
|
|
memcpy(dest, src, uparm1);
|
|
ref[1] = uparm1;
|
|
}
|
|
break;
|
|
|
|
/* Convert a string to a numeric value
|
|
* procedure val(const s : string; var v; var code : word);
|
|
*
|
|
* Description:
|
|
* val() converts the value represented in the string S to a numerical
|
|
* value, and stores this value in the variable V, which can be of type
|
|
* Longint, Real and Byte. If the conversion isn't succesfull, then the
|
|
* parameter Code contains the index of the character in S which
|
|
* prevented the conversion. The string S is allowed to contain spaces
|
|
* in the beginning.
|
|
*
|
|
* The string S can contain a number in decimal, hexadecimal, binary or
|
|
* octal format, as described in the language reference.
|
|
*
|
|
* Errors:
|
|
* If the conversion doesn¡Çt succeed, the value of Code indicates the
|
|
* position where the conversion went wrong.
|
|
*
|
|
* ON INPUT
|
|
* TOS(st, 0)=address of code
|
|
* TOS(st, 1)=address of value
|
|
* TOS(st, 2)=length of source string
|
|
* TOS(st, 3)=pointer to source string
|
|
* ON RETURN: actual parameters released
|
|
*/
|
|
|
|
case lbVAL :
|
|
/* Get the string information */
|
|
|
|
len = TOS(st, 2); /* Number of bytes in string */
|
|
src = (uint8_t*)&GETSTACK(st, TOS(st, 3)); /* Pointer to string */
|
|
|
|
/* Make a C string out of the pascal string */
|
|
|
|
name = pexec_mkcstring(src, len);
|
|
if (name == NULL)
|
|
{
|
|
return eNOMEMORY;
|
|
}
|
|
|
|
/* Convert the string to an integer */
|
|
|
|
value = atoi((char*)name);
|
|
if ((value < MININT) || (value > MAXINT))
|
|
{
|
|
return eINTEGEROVERFLOW;
|
|
}
|
|
PUTSTACK(st, TOS(st, 0), 0);
|
|
PUTSTACK(st, TOS(st, 1), value);
|
|
DISCARD(st, 4);
|
|
break;
|
|
|
|
/* Create an empty string
|
|
* function mkstk : string;
|
|
* ON INPUT
|
|
* ON RETURN
|
|
* TOS(st, 0)=length of new string
|
|
* TOS(st, 1)=pointer to new string
|
|
*/
|
|
|
|
case lbMKSTK :
|
|
/* Allocate space on the string stack for the new string
|
|
* FIXME: This logic does not handle strings with other than the
|
|
* default size!
|
|
*/
|
|
|
|
addr1 = ((st->csp + 1) & ~1);
|
|
st->csp += sSTRING_SIZE; /* Allocate max size */
|
|
|
|
/* Save the length at the beginning of the copy */
|
|
|
|
tmp = (uint16_t*)&GETSTACK(st, addr1); /* Pointer to new string */
|
|
*tmp++ = 0; /* Save current size */
|
|
|
|
/* Update the stack content */
|
|
|
|
PUSH(st, addr1 + sSTRING_HDR_SIZE); /* Pointer to new string */
|
|
PUSH(st, 0); /* Current size */
|
|
break;
|
|
|
|
/* Replace a string with a duplicate string residing in allocated
|
|
* string stack.
|
|
* function mkstkstr(name : string) : string;
|
|
* ON INPUT
|
|
* TOS(st, 0)=length of original string
|
|
* TOS(st, 1)=pointer to original string data
|
|
* ON RETURN
|
|
* TOS(st, 0)=length of new string (unchanged)
|
|
* TOS(st, 1)=pointer to new string data
|
|
*/
|
|
|
|
case lbMKSTKSTR :
|
|
/* Get the parameters from the stack (leaving the string reference
|
|
* in place.
|
|
*/
|
|
|
|
uparm1 = TOS(st, 0); /* Original string size */
|
|
addr1 = TOS(st, 1); /* Original string data pointer */
|
|
|
|
/* Check if there is space on the string stack for the new string
|
|
* FIXME: This logic does not handle strings with other than the
|
|
* default size!
|
|
*/
|
|
|
|
if (st->csp + sSTRING_SIZE >= st->spb)
|
|
{
|
|
return eSTRSTKOVERFLOW;
|
|
}
|
|
|
|
/* Allocate space on the string stack for the new string */
|
|
|
|
addr2 = ((st->csp + 1) & ~1);
|
|
st->csp += sSTRING_SIZE; /* Allocate max size */
|
|
|
|
/* Save the length at the beginning of the copy */
|
|
|
|
tmp = (uint16_t*)&GETSTACK(st, addr2); /* Pointer to new string */
|
|
*tmp++ = uparm1; /* Save current size */
|
|
dest = (uint8_t*)tmp; /* Pointer to string data */
|
|
|
|
/* Copy the string into the string stack */
|
|
|
|
src = (uint8_t*)&GETSTACK(st, addr1); /* Pointer to original string */
|
|
memcpy(dest, src, uparm1);
|
|
|
|
/* Update the stack content */
|
|
|
|
TOS(st, 1) = addr2 + sSTRING_HDR_SIZE;
|
|
break;
|
|
|
|
/* Replace a character with a string residing in allocated string stack.
|
|
* function mkstkc(c : char) : string;
|
|
* ON INPUT
|
|
* TOS(st, 0)=Character value
|
|
* ON RETURN
|
|
* TOS(st, 0)=length of new string
|
|
* TOS(st, 1)=pointer to new string
|
|
*/
|
|
|
|
case lbMKSTKC :
|
|
/* Check if there is space on the string stack for the new string
|
|
* FIXME: This logic does not handle strings with other than the
|
|
* default size!
|
|
*/
|
|
|
|
if (st->csp + sSTRING_SIZE >= st->spb)
|
|
{
|
|
return eSTRSTKOVERFLOW;
|
|
}
|
|
|
|
/* Allocate space on the string stack for the new string */
|
|
|
|
addr2 = ((st->csp + 1) & ~1);
|
|
st->csp += sSTRING_SIZE; /* Allocate max size */
|
|
|
|
/* Save the length at the beginning of the copy */
|
|
|
|
tmp = (uint16_t*)&GETSTACK(st, addr2); /* Pointer to new string */
|
|
*tmp++ = 1; /* Save initial size */
|
|
dest = (uint8_t*)tmp; /* Pointer to string data */
|
|
|
|
/* Copy the character into the string stack */
|
|
|
|
*dest++ = TOS(st, 0); /* Save character as string */
|
|
|
|
/* Update the stack content */
|
|
|
|
TOS(st, 0) = addr2 + sSTRING_HDR_SIZE; /* String address */
|
|
PUSH(st, 1); /* String length */
|
|
break;
|
|
|
|
/* Concatenate a string to the end of a string.
|
|
* function strcat(name : string, c : char) : string;
|
|
*
|
|
* ON INPUT
|
|
* TOS(st, 0)=length of string1
|
|
* TOS(st, 1)=pointer to string1 data
|
|
* TOS(st, 2)=length of string2
|
|
* TOS(st, 3)=pointer to string2 data
|
|
* ON OUTPUT
|
|
* TOS(st, 1)=new length of string2
|
|
* TOS(st, 2)=pointer to string2
|
|
*/
|
|
|
|
case lbSTRCAT :
|
|
/* Get the parameters from the stack (leaving the string reference
|
|
* in place.
|
|
*/
|
|
|
|
POP(st, uparm1); /* string1 size */
|
|
POP(st, addr1); /* string1 data stack addr */
|
|
uparm2 = TOS(st, 0); /* string2 size */
|
|
|
|
/* Check for string overflow. FIXME: This logic does not handle
|
|
* strings with other than the default size!
|
|
*/
|
|
|
|
if (uparm1 + uparm2 > sSTRING_MAX_SIZE)
|
|
return eSTRSTKOVERFLOW;
|
|
else
|
|
{
|
|
/* Get a pointer to string1 data */
|
|
|
|
src = ATSTACK(st, addr1);
|
|
|
|
/* Get a pointer to string2 header, set new size then, get
|
|
* a pointer to string2 data.
|
|
*/
|
|
|
|
tmp = ((uint16_t*)&GETSTACK(st, TOS(st, 1))) - 1;
|
|
*tmp++ = uparm1 + uparm2;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
memcpy(&dest[uparm2], src, uparm1); /* cat strings */
|
|
TOS(st, 0) = uparm1 + uparm2; /* Save new size */
|
|
}
|
|
break;
|
|
|
|
/* Concatenate a character to the end of a string.
|
|
* function strcatc(name : string, c : char) : string;
|
|
*
|
|
* ON INPUT
|
|
* TOS(st, 0)=character to concatenate
|
|
* TOS(st, 1)=length of string
|
|
* TOS(st, 2)=pointer to string
|
|
* ON OUTPUT
|
|
* TOS(st, 1)=new length of string
|
|
* TOS(st, 2)=pointer to string
|
|
*/
|
|
|
|
case lbSTRCATC :
|
|
/* Get the parameters from the stack (leaving the string reference
|
|
* in place.
|
|
*/
|
|
|
|
POP(st, uparm1); /* Character to concatenate */
|
|
uparm2 = TOS(st, 0); /* Current length of string */
|
|
|
|
/* Check for string overflow. FIXME: This logic does not handle
|
|
* strings with other than the default size!
|
|
*/
|
|
|
|
if (uparm2 >= sSTRING_MAX_SIZE)
|
|
return eSTRSTKOVERFLOW;
|
|
else
|
|
{
|
|
/* Get a pointer to string header, set size new size then, get
|
|
* a pointer to string data.
|
|
*/
|
|
|
|
tmp = ((uint16_t*)&GETSTACK(st, TOS(st, 1))) - 1;
|
|
*tmp++ = uparm2 + 1;
|
|
dest = (uint8_t*)tmp;
|
|
|
|
/* Add the new charcter */
|
|
|
|
dest[uparm2] = (uint8_t)uparm1;
|
|
|
|
/* Save the new string size */
|
|
|
|
TOS(st, 0) = uparm2 + 1;
|
|
}
|
|
break;
|
|
|
|
/* Compare two pascal strings
|
|
* function strcmp(name1 : string, name2 : string) : integer;
|
|
* ON INPUT
|
|
* TOS(st, 1)=length of string2
|
|
* TOS(st, 2)=address of string2 data
|
|
* TOS(st, 3)=length of string1
|
|
* TOS(st, 4)=address of string1 data
|
|
* ON OUTPUT
|
|
* TOS(st, 0)=(-1=less than, 0=equal, 1=greater than}
|
|
*/
|
|
|
|
case lbSTRCMP :
|
|
{
|
|
int result;
|
|
|
|
/* Get the parameters from the stack (leaving space for the
|
|
* return value);
|
|
*/
|
|
|
|
POP(st, uparm2); /* length of string2 */
|
|
POP(st, addr2); /* address of string2 data */
|
|
POP(st, uparm1); /* length of string1 */
|
|
addr1 = TOS(st, 0); /* address of string1 data */
|
|
|
|
/* Get full address */
|
|
|
|
dest = ATSTACK(st, addr1);
|
|
src = ATSTACK(st, addr2);
|
|
|
|
/* If name1 is shorter than name2, then we can only return
|
|
* -1 (less than) or +1 greater than. If the substrings
|
|
* of length of name1 are equal, then we return less than.
|
|
*/
|
|
|
|
if (uparm1 < uparm2)
|
|
{
|
|
result = memcmp(dest, src, uparm1);
|
|
if (result == 0) result = -1;
|
|
}
|
|
|
|
/* If name1 is longer than name2, then we can only return
|
|
* -1 (less than) or +1 greater than. If the substrings
|
|
* of length of name2 are equal, then we return greater than.
|
|
*/
|
|
|
|
else if (uparm1 > uparm2)
|
|
{
|
|
result = memcmp(dest, src, uparm2);
|
|
if (result == 0) result = 1;
|
|
}
|
|
|
|
/* The strings are of equal length. Return the result of
|
|
* the comparison.
|
|
*/
|
|
|
|
else
|
|
{
|
|
result = memcmp(dest, src, uparm1);
|
|
}
|
|
TOS(st, 0) = result;
|
|
}
|
|
break;
|
|
|
|
default :
|
|
return eBADSYSLIBCALL;
|
|
|
|
}
|
|
|
|
return eNOERROR;
|
|
|
|
} /* end pexec_libcall */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_execfp
|
|
*
|
|
* Description:
|
|
* This function processes a floating point operation.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static uint16_t pexec_execfp(struct pexec_s *st, uint8_t fpop)
|
|
{
|
|
int16_t intValue;
|
|
fparg_t arg1;
|
|
fparg_t arg2;
|
|
fparg_t result;
|
|
|
|
switch (fpop & fpMASK)
|
|
{
|
|
/* Floating Pointer Conversions (On stack argument: FP or Integer) */
|
|
|
|
case fpFLOAT :
|
|
POP(st, intValue);
|
|
result.f = (double)intValue;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
|
|
case fpTRUNC :
|
|
case fpROUND :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
intValue = (int16_t)arg1.f;
|
|
PUSH(st, intValue);
|
|
break;
|
|
|
|
/* Floating Point arithmetic instructions (Two FP stack arguments) */
|
|
|
|
case fpADD :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
result.f = arg1.f + arg2.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpSUB :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
result.f = arg1.f - arg2.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpMUL :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
result.f = arg1.f * arg2.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpDIV :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
result.f = arg1.f / arg2.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpMOD :
|
|
return eBADFPOPCODE;
|
|
#if 0 /* Not yet */
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
result.f = arg1.f % arg2.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
#endif
|
|
|
|
/* Floating Point Comparisons (Two FP stack arguments) */
|
|
|
|
case fpEQU :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f == arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
case fpNEQ :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f != arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
case fpLT :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f < arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
case fpGTE :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f >= arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
case fpGT :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f > arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
case fpLTE :
|
|
pexec_getfparguments(st, fpop, &arg1, &arg2);
|
|
intValue = PFALSE;
|
|
if (arg1.f <= arg2.f)
|
|
intValue = PTRUE;
|
|
PUSH(st, intValue);
|
|
break;
|
|
|
|
/* Floating Point arithmetic instructions (One FP stack arguments) */
|
|
|
|
case fpNEG :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = -arg1.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
#ifdef CONFIG_HAVE_LIBM
|
|
case fpABS :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = fabs(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
#endif
|
|
case fpSQR :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = arg1.f * arg1.f;
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
#ifdef CONFIG_HAVE_LIBM
|
|
case fpSQRT :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = sqrt(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpSIN :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = sin(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpCOS :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = cos(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpATAN :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = atan(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpLN :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = log(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
case fpEXP :
|
|
pexec_getfparguments(st, fpop, &arg1, NULL);
|
|
result.f = exp(arg1.f);
|
|
PUSH(st, result.hw[0]);
|
|
PUSH(st, result.hw[1]);
|
|
PUSH(st, result.hw[2]);
|
|
PUSH(st, result.hw[3]);
|
|
break;
|
|
#endif
|
|
|
|
default :
|
|
return eBADFPOPCODE;
|
|
|
|
}
|
|
return eNOERROR;
|
|
|
|
} /* end pexec_execfp */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_getfparguments
|
|
*
|
|
* Description:
|
|
* This function retrieves the floating point arguments and performs
|
|
* integer to REAL conversions as necessary
|
|
*
|
|
****************************************************************************/
|
|
|
|
static void pexec_getfparguments(struct pexec_s *st, uint8_t fpop, fparg_t *arg1, fparg_t *arg2)
|
|
{
|
|
int16_t sparm;
|
|
|
|
/* Extract arg2 from the stack */
|
|
|
|
if (arg2)
|
|
{
|
|
/* Convert an integer argument to type REAL */
|
|
|
|
if ((fpop & fpARG2) != 0)
|
|
{
|
|
POP(st, sparm);
|
|
arg2->f = (double)sparm;
|
|
}
|
|
else
|
|
{
|
|
POP(st, arg2->hw[3]);
|
|
POP(st, arg2->hw[2]);
|
|
POP(st, arg2->hw[1]);
|
|
POP(st, arg2->hw[0]);
|
|
}
|
|
}
|
|
|
|
/* Extract arg1 from the stack */
|
|
|
|
if (arg1)
|
|
{
|
|
/* Convert an integer argument to type REAL */
|
|
|
|
if ((fpop & fpARG1) != 0)
|
|
{
|
|
POP(st, sparm);
|
|
arg1->f = (double)sparm;
|
|
}
|
|
else
|
|
{
|
|
POP(st, arg1->hw[3]);
|
|
POP(st, arg1->hw[2]);
|
|
POP(st, arg1->hw[1]);
|
|
POP(st, arg1->hw[0]);
|
|
}
|
|
}
|
|
|
|
} /* end pexec_getfparguments */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_readinteger
|
|
*
|
|
* Description:
|
|
* This function parses a decimal integer from ioptr
|
|
****************************************************************************/
|
|
|
|
static ustack_t pexec_readinteger(uint8_t *ioptr)
|
|
{
|
|
sstack_t value = 0;
|
|
|
|
while (isspace(*ioptr)) ioptr++;
|
|
while ((*ioptr >= '0') && (*ioptr <= '9'))
|
|
{
|
|
value = 10*value
|
|
+ (sstack_t)(*ioptr)
|
|
- (sstack_t)'0';
|
|
ioptr++;
|
|
}
|
|
|
|
return (ustack_t)value;
|
|
|
|
} /* end pexec_readinteger */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_readreal
|
|
*
|
|
* Description:
|
|
* This function parses a decimal integer from ioptr.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static void pexec_readreal(uint16_t *dest, uint8_t *inPtr)
|
|
{
|
|
int32_t intpart;
|
|
fparg_t result;
|
|
double fraction;
|
|
uint8_t unaryop;
|
|
|
|
intpart = 0;
|
|
unaryop = '+';
|
|
|
|
/* Check for a leading unary - */
|
|
|
|
if ((*inPtr == '-') || (*inPtr == '+'))
|
|
unaryop = *inPtr++;
|
|
|
|
/* Get the integer part of the real */
|
|
|
|
while ((*inPtr >= '0') && (*inPtr <= '9'))
|
|
intpart = 10*intpart + ((int32_t)*inPtr++) - ((int32_t)'0');
|
|
|
|
result.f = ((double)intpart);
|
|
|
|
/* Check for the a fractional part */
|
|
|
|
if (*inPtr == '.')
|
|
{
|
|
inPtr++;
|
|
fraction = 0.1;
|
|
while ((*inPtr >= '0') && (*inPtr <= '9'))
|
|
{
|
|
result.f += fraction * (double)(((int32_t)*inPtr++) - ((int32_t)'0'));
|
|
fraction /= 10.0;
|
|
}
|
|
}
|
|
|
|
/* Correct the sign of the result */
|
|
|
|
if (unaryop == '-')
|
|
result.f = -result.f;
|
|
|
|
/* Return the value into the P-Machine stack */
|
|
|
|
*dest++ = result.hw[0];
|
|
*dest++ = result.hw[1];
|
|
*dest++ = result.hw[2];
|
|
*dest = result.hw[3];
|
|
|
|
} /* end pexec_readreal */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_getbaseaddress
|
|
*
|
|
* Description:
|
|
* This function binds the base address corresponding to a given level
|
|
* offset.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static ustack_t pexec_getbaseaddress(struct pexec_s *st, level_t leveloffset)
|
|
{
|
|
/* Start with the base register of the current frame */
|
|
|
|
ustack_t baseAddress = st->fp;
|
|
|
|
/* Search backware "leveloffset" frames until the correct frame is
|
|
* found
|
|
*/
|
|
|
|
while (leveloffset > 0)
|
|
{
|
|
baseAddress = st->dstack.i[BTOISTACK(baseAddress)];
|
|
leveloffset--;
|
|
}
|
|
|
|
/* Offset that value by two words (one for the st->fp and one for the
|
|
* return value
|
|
*/
|
|
|
|
return baseAddress + 2*BPERI;
|
|
|
|
} /* end pexec_getbaseaddress */
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_mkcstring
|
|
****************************************************************************/
|
|
|
|
static uint8_t *pexec_mkcstring(uint8_t *buffer, int buflen)
|
|
{
|
|
uint8_t *string;
|
|
|
|
string = malloc(buflen + 1);
|
|
if (string != NULL)
|
|
{
|
|
memcpy(string, buffer, buflen);
|
|
string[buflen] = '\0';
|
|
}
|
|
return string;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec8
|
|
*
|
|
* Descripton:
|
|
* Handle 8-bit instructions with no immediate data
|
|
*
|
|
****************************************************************************/
|
|
|
|
static inline int pexec8(FAR struct pexec_s *st, uint8_t opcode)
|
|
{
|
|
sstack_t sparm;
|
|
ustack_t uparm1;
|
|
ustack_t uparm2;
|
|
ustack_t uparm3;
|
|
|
|
switch (opcode)
|
|
{
|
|
/* Arithmetic & logical & and integer conversions (One stack argument) */
|
|
case oNEG :
|
|
TOS(st, 0) = (ustack_t)(-(sstack_t)TOS(st, 0));
|
|
break;
|
|
case oABS :
|
|
if (signExtend16(TOS(st, 0)) < 0)
|
|
{
|
|
TOS(st, 0) = (ustack_t)(-signExtend16(TOS(st, 0)));
|
|
}
|
|
break;
|
|
case oINC :
|
|
TOS(st, 0)++;
|
|
break;
|
|
case oDEC :
|
|
TOS(st, 0)--;
|
|
break;
|
|
case oNOT :
|
|
TOS(st, 0) = ~TOS(st, 0);
|
|
break;
|
|
|
|
/* Arithmetic & logical (Two stack arguments) */
|
|
|
|
case oADD :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) + sparm);
|
|
break;
|
|
case oSUB :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) - sparm);
|
|
break;
|
|
case oMUL :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) * sparm);
|
|
break;
|
|
case oDIV :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) / sparm);
|
|
break;
|
|
case oMOD :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) % sparm);
|
|
break;
|
|
case oSLL :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) << sparm);
|
|
break;
|
|
case oSRL :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (TOS(st, 0) >> sparm);
|
|
break;
|
|
case oSRA :
|
|
POP(st, sparm);
|
|
TOS(st, 0) = (ustack_t)(((sstack_t)TOS(st, 0)) >> sparm);
|
|
break;
|
|
case oOR :
|
|
POP(st, uparm1);
|
|
TOS(st, 0) = (TOS(st, 0) | uparm1);
|
|
break;
|
|
case oAND :
|
|
POP(st, uparm1);
|
|
TOS(st, 0) = (TOS(st, 0) & uparm1);
|
|
break;
|
|
case oBIT :
|
|
POP(st, uparm1);
|
|
uparm2 = TOS(st, 0);
|
|
if ((uparm1 & (1 << uparm2)) != 0)
|
|
{
|
|
TOS(st, 0) = PTRUE;
|
|
}
|
|
else
|
|
{
|
|
TOS(st, 0) = PFALSE;
|
|
}
|
|
break;
|
|
|
|
/* Comparisons (One stack argument) */
|
|
|
|
case oEQUZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm == 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oNEQZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm != 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oLTZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm < 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oGTEZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm >= 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oGTZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm > 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oLTEZ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm <= 0)
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
PUSH(st, uparm1);
|
|
break;
|
|
|
|
/* Comparisons (Two stack arguments) */
|
|
|
|
case oEQU :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm == (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
case oNEQ :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm != (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
case oLT :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm < (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
case oGTE :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm >= (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
case oGT :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm > (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
case oLTE :
|
|
POP(st, sparm);
|
|
uparm1 = PFALSE;
|
|
if (sparm <= (sstack_t)TOS(st, 0))
|
|
{
|
|
uparm1 = PTRUE;
|
|
}
|
|
TOS(st, 0) = uparm1;
|
|
break;
|
|
|
|
/* Load (One stack argument) */
|
|
|
|
case oLDI :
|
|
POP(st, uparm1); /* Address */
|
|
PUSH(st, GETSTACK(st, uparm1));
|
|
PUSH(st, GETSTACK(st, uparm1 + BPERI));
|
|
break;
|
|
case oLDIH :
|
|
TOS(st, 0) = GETSTACK(st, TOS(st, 0));
|
|
break;
|
|
case oLDIB :
|
|
TOS(st, 0) = GETBSTACK(st, TOS(st, 0));
|
|
break;
|
|
case oLDIM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size */
|
|
POP(st, uparm2); /* Stack offset */
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUSH(st, GETSTACK(st, uparm2));
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
}
|
|
else
|
|
{
|
|
PUSH(st, GETBSTACK(st, uparm2));
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
break;
|
|
case oDUP :
|
|
uparm1 = TOS(st, 0);
|
|
uparm2 = TOS(st, 1);
|
|
PUSH(st, uparm2);
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oDUPH :
|
|
uparm1 = TOS(st, 0);
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oPUSHS :
|
|
PUSH(st, st->csp);
|
|
break;
|
|
case oPOPS :
|
|
POP(st, st->csp);
|
|
break;
|
|
|
|
/* Store (Two stack arguments) */
|
|
|
|
case oSTIH :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
PUTSTACK(st, uparm1,uparm2);
|
|
break;
|
|
case oSTIB :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
PUTBSTACK(st, uparm1, uparm2);
|
|
break;
|
|
case oSTIM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size in bytes */
|
|
uparm3 = uparm1; /* Save for stack discard */
|
|
sparm = ROUNDBTOI(uparm1); /* Size in words */
|
|
uparm2 = TOS(st, sparm); /* Stack offset */
|
|
sparm--;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUTSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
sparm--;
|
|
}
|
|
else
|
|
{
|
|
PUTBSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
|
|
/* Discard the stored data + the stack offset */
|
|
|
|
DISCARD(st, (ROUNDBTOI(uparm3) + 1));
|
|
break;
|
|
|
|
/* Program control (No stack arguments) */
|
|
|
|
case oNOP :
|
|
break;
|
|
case oRET :
|
|
POP(st, st->pc);
|
|
POP(st, st->fp);
|
|
DISCARD(st, 1);
|
|
return eNOERROR;
|
|
|
|
/* System Functions (No stack arguments) */
|
|
|
|
case oEND :
|
|
return eEXIT;
|
|
|
|
default :
|
|
return eILLEGALOPCODE;
|
|
}
|
|
|
|
st->pc += 1;
|
|
return eNOERROR;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec16
|
|
*
|
|
* Descripton:
|
|
* Handle 16-bit instructions with 8-bits of immediate data (imm8)
|
|
*
|
|
****************************************************************************/
|
|
|
|
static inline int pexec16(FAR struct pexec_s *st, uint8_t opcode, uint8_t imm8)
|
|
{
|
|
int ret = eNOERROR;
|
|
|
|
st->pc += 2;
|
|
switch (opcode)
|
|
{
|
|
/* Data stack: imm8 = 8 bit unsigned data (no stack arguments) */
|
|
|
|
case oPUSHB :
|
|
PUSH(st, imm8);
|
|
break;
|
|
|
|
/* Floating Point: imm8 = FP op-code (varying number of stack arguments) */
|
|
case oFLOAT :
|
|
ret = pexec_execfp(st, imm8);
|
|
break;
|
|
|
|
default :
|
|
ret = eILLEGALOPCODE;
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec24
|
|
*
|
|
* Descripton:
|
|
* Handle 24-bit instructions with 16-bits of immediate data (imm16)
|
|
*
|
|
****************************************************************************/
|
|
|
|
static inline int pexec24(FAR struct pexec_s *st, uint8_t opcode, uint16_t imm16)
|
|
{
|
|
sstack_t sparm1;
|
|
sstack_t sparm2;
|
|
ustack_t uparm1;
|
|
ustack_t uparm2;
|
|
ustack_t uparm3;
|
|
int ret = eNOERROR;
|
|
|
|
switch (opcode)
|
|
{
|
|
/* Program control: imm16 = unsigned label (no stack arguments) */
|
|
|
|
case oJMP :
|
|
goto branch_out;
|
|
|
|
/* Program control: imm16 = unsigned label (One stack argument) */
|
|
|
|
case oJEQUZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 == 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJNEQZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 != 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJLTZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 < 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJGTEZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 >= 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJGTZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 > 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJLTEZ :
|
|
POP(st, sparm1);
|
|
if (sparm1 <= 0)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
|
|
/* Program control: imm16 = unsigned label (Two stack arguments) */
|
|
|
|
case oJEQU :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 == sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJNEQ :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 != sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJLT :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 < sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJGTE :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 >= sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJGT :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 > sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
case oJLTE :
|
|
POP(st, sparm1);
|
|
POP(st, sparm2);
|
|
if (sparm2 <= sparm1)
|
|
{
|
|
goto branch_out;
|
|
}
|
|
break;
|
|
|
|
/* Load: imm16 = usigned offset (no stack arguments) */
|
|
|
|
case oLD :
|
|
uparm1 = st->spb + imm16;
|
|
PUSH(st, GETSTACK(st, uparm1));
|
|
PUSH(st, GETSTACK(st, uparm1 + BPERI));
|
|
break;
|
|
case oLDH :
|
|
uparm1 = st->spb + imm16;
|
|
PUSH(st, GETSTACK(st, uparm1));
|
|
break;
|
|
case oLDB :
|
|
uparm1 = st->spb + imm16;
|
|
PUSH(st, GETBSTACK(st, uparm1));
|
|
break;
|
|
case oLDM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1);
|
|
uparm2 = st->spb + imm16;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUSH(st, GETSTACK(st, uparm2));
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
}
|
|
else
|
|
{
|
|
PUSH(st, GETBSTACK(st, uparm2));
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
break;
|
|
|
|
/* Load & store: imm16 = unsigned base offset (One stack argument) */
|
|
|
|
case oST :
|
|
uparm1 = st->spb + imm16;
|
|
POP(st, uparm2);
|
|
PUTSTACK(st, uparm2, uparm1 + BPERI);
|
|
POP(st, uparm2);
|
|
PUTSTACK(st, uparm2, uparm1);
|
|
break;
|
|
case oSTH :
|
|
uparm1 = st->spb + imm16;
|
|
POP(st, uparm2);
|
|
PUTSTACK(st, uparm2, uparm1);
|
|
break;
|
|
case oSTB :
|
|
uparm1 = st->spb + imm16;
|
|
POP(st, uparm2);
|
|
PUTBSTACK(st, uparm2, uparm1);
|
|
break;
|
|
case oSTM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size */
|
|
uparm3 = uparm1; /* Save for stack discard */
|
|
uparm2 = st->spb + imm16;
|
|
sparm1 = ROUNDBTOI(uparm1) - 1;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUTSTACK(st, TOS(st, sparm1), uparm2);
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
sparm1--;
|
|
}
|
|
else
|
|
{
|
|
PUTBSTACK(st, TOS(st, sparm1), uparm2);
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
|
|
/* Discard the stored data */
|
|
|
|
DISCARD(st, ROUNDBTOI(uparm3));
|
|
break;
|
|
case oLDX :
|
|
uparm1 = st->spb + imm16 + TOS(st, 0);
|
|
TOS(st, 0) = GETSTACK(st, uparm1);
|
|
PUSH(st, GETSTACK(st, uparm1 + BPERI));
|
|
break;
|
|
case oLDXH :
|
|
uparm1 = st->spb + imm16 + TOS(st, 0);
|
|
TOS(st, 0) = GETSTACK(st, uparm1);
|
|
break;
|
|
case oLDXB :
|
|
uparm1 = st->spb + imm16 + TOS(st, 0);
|
|
TOS(st, 0) = GETBSTACK(st, uparm1);
|
|
break;
|
|
case oLDXM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += st->spb + imm16;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUSH(st, GETSTACK(st, uparm2));
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
}
|
|
else
|
|
{
|
|
PUSH(st, GETBSTACK(st, uparm2));
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
break;
|
|
|
|
/* Store: imm16 = unsigned base offset (Two stack arguments) */
|
|
|
|
case oSTXH :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += st->spb + imm16;
|
|
PUTSTACK(st, uparm1,uparm2);
|
|
break;
|
|
case oSTXB :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += st->spb + imm16;
|
|
PUTBSTACK(st, uparm1, uparm2);
|
|
break;
|
|
case oSTXM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size */
|
|
uparm3 = uparm1; /* Save for stack discard */
|
|
sparm1 = ROUNDBTOI(uparm1); /* Size in 16-bit words */
|
|
uparm2 = TOS(st, sparm1); /* index */
|
|
sparm1--;
|
|
uparm2 += st->spb + imm16;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUTSTACK(st, TOS(st, sparm1), uparm2);
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
sparm1--;
|
|
}
|
|
else
|
|
{
|
|
PUTBSTACK(st, TOS(st, sparm1), uparm2);
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
|
|
/* Discard the stored data + the index */
|
|
|
|
DISCARD(st, (ROUNDBTOI(uparm3) + 1));
|
|
break;
|
|
|
|
case oLA :
|
|
uparm1 = st->spb + imm16;
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oLAX :
|
|
TOS(st, 0) = st->spb + imm16 + TOS(st, 0);
|
|
break;
|
|
|
|
/* Data stack: imm16 = 16 bit signed data (no stack arguments) */
|
|
|
|
case oPUSH :
|
|
PUSH(st, imm16);
|
|
break;
|
|
case oINDS :
|
|
st->sp += signExtend16(imm16);
|
|
break;
|
|
|
|
/* System Functions:
|
|
* For LIB: imm16 = sub-function code
|
|
*/
|
|
|
|
case oLIB :
|
|
ret = pexec_libcall(st, imm16);
|
|
break;
|
|
|
|
/* Program control: imm16 = unsigned label (no stack arguments) */
|
|
|
|
case oLAC :
|
|
uparm1 = imm16 + st->rop;
|
|
PUSH(st, uparm1);
|
|
break;
|
|
|
|
case oLABEL :
|
|
default:
|
|
ret = eILLEGALOPCODE;
|
|
break;
|
|
}
|
|
|
|
st->pc += 3;
|
|
return ret;
|
|
|
|
branch_out:
|
|
st->pc = (paddr_t)imm16;
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec32
|
|
*
|
|
* Descripton:
|
|
* Handle 32-bit instructions with 24-bits of immediate data (imm8+imm16)
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int pexec32(FAR struct pexec_s *st, uint8_t opcode, uint8_t imm8, uint16_t imm16)
|
|
{
|
|
sstack_t sparm;
|
|
ustack_t uparm1;
|
|
ustack_t uparm2;
|
|
ustack_t uparm3;
|
|
int ret = eNOERROR;
|
|
|
|
switch (opcode)
|
|
{
|
|
/* Load: imm8 = level; imm16 = signed frame offset (no stack arguments) */
|
|
case oLDS :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUSH(st, GETSTACK(st, uparm1));
|
|
PUSH(st, GETSTACK(st, uparm1 + BPERI));
|
|
break;
|
|
case oLDSH :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUSH(st, GETSTACK(st, uparm1));
|
|
break;
|
|
case oLDSB :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUSH(st, GETBSTACK(st, uparm1));
|
|
break;
|
|
case oLDSM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1);
|
|
uparm2 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUSH(st, GETSTACK(st, uparm2));
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
}
|
|
else
|
|
{
|
|
PUSH(st, GETBSTACK(st, uparm2));
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
break;
|
|
|
|
/* Load & store: imm8 = level; imm16 = signed frame offset (One stack argument) */
|
|
|
|
case oSTSH :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
POP(st, uparm2);
|
|
PUTSTACK(st, uparm2, uparm1);
|
|
break;
|
|
case oSTSB :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
POP(st, uparm2);
|
|
PUTBSTACK(st, uparm2, uparm1);
|
|
break;
|
|
case oSTSM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size */
|
|
uparm3 = uparm1; /* Save for stack discard */
|
|
uparm2 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
sparm = ROUNDBTOI(uparm1) - 1;
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUTSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
sparm--;
|
|
}
|
|
else
|
|
{
|
|
PUTBSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
|
|
/* Discard the stored data */
|
|
|
|
DISCARD(st, ROUNDBTOI(uparm3));
|
|
break;
|
|
case oLDSX :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16) + TOS(st, 0);
|
|
TOS(st, 0) = GETSTACK(st, uparm1);
|
|
PUSH(st, GETSTACK(st, uparm1 + BPERI));
|
|
break;
|
|
case oLDSXH :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16) + TOS(st, 0);
|
|
TOS(st, 0) = GETSTACK(st, uparm1);
|
|
break;
|
|
case oLDSXB :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16) + TOS(st, 0);
|
|
TOS(st, 0) = GETBSTACK(st, uparm1);
|
|
break;
|
|
case oLDSXM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUSH(st, GETSTACK(st, uparm2));
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
}
|
|
else
|
|
{
|
|
PUSH(st, GETBSTACK(st, uparm2));
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
break;
|
|
|
|
/* Store: imm8 = level; imm16 = signed frame offset (Two stack arguments) */
|
|
|
|
case oSTSXH :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUTSTACK(st, uparm1,uparm2);
|
|
break;
|
|
case oSTSXB :
|
|
POP(st, uparm1);
|
|
POP(st, uparm2);
|
|
uparm2 += pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUTBSTACK(st, uparm1, uparm2);
|
|
break;
|
|
case oSTSXM :
|
|
/* FIX ME --> Need to handle the unaligned case */
|
|
POP(st, uparm1); /* Size */
|
|
uparm3 = uparm1; /* Save for stack discard */
|
|
sparm = ROUNDBTOI(uparm1); /* Size in 16-bit words */
|
|
uparm2 = TOS(st, sparm); /* index */
|
|
sparm--;
|
|
uparm2 += pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
while (uparm1 > 0)
|
|
{
|
|
if (uparm1 >= BPERI)
|
|
{
|
|
PUTSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2 += BPERI;
|
|
uparm1 -= BPERI;
|
|
sparm--;
|
|
}
|
|
else
|
|
{
|
|
PUTBSTACK(st, TOS(st, sparm), uparm2);
|
|
uparm2++;
|
|
uparm1--;
|
|
}
|
|
}
|
|
|
|
/* Discard the stored data + the index */
|
|
|
|
DISCARD(st, (ROUNDBTOI(uparm3) + 1));
|
|
break;
|
|
|
|
case oLAS :
|
|
uparm1 = pexec_getbaseaddress(st, imm8) + signExtend16(imm16);
|
|
PUSH(st, uparm1);
|
|
break;
|
|
case oLASX :
|
|
TOS(st, 0) = pexec_getbaseaddress(st, imm8) + signExtend16(imm16) + TOS(st, 0);
|
|
break;
|
|
|
|
/* Program Control: imm8 = level; imm16 = unsigned label (No
|
|
* stack arguments)
|
|
*/
|
|
|
|
case oPCAL :
|
|
PUSH(st, pexec_getbaseaddress(st, imm8));
|
|
PUSH(st, st->fp);
|
|
uparm1 = st->sp;
|
|
PUSH(st, st->pc + 4);
|
|
st->fp = uparm1;
|
|
st->pc = (paddr_t)imm16;
|
|
return eNOERROR;
|
|
|
|
/* System Functions:
|
|
* For SYSIO: imm8 = file number; imm16 = sub-function code
|
|
*/
|
|
|
|
case oSYSIO :
|
|
ret = pexec_sysio(st, imm8, imm16);
|
|
break;
|
|
|
|
/* Psuedo-operations: (No stack arguments)
|
|
* For LINE: imm8 = file number; imm16 = line number
|
|
*/
|
|
|
|
case oLINE :
|
|
default :
|
|
ret = eILLEGALOPCODE;
|
|
break;
|
|
}
|
|
|
|
st->pc += 4;
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Public Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_init
|
|
****************************************************************************/
|
|
|
|
FAR struct pexec_s *pexec_init(struct pexec_attr_s *attr)
|
|
{
|
|
struct pexec_s *st;
|
|
paddr_t stacksize;
|
|
paddr_t adjusted_rosize;
|
|
|
|
/* Allocate the p-machine state stucture */
|
|
|
|
st = (struct pexec_s *)malloc(sizeof(struct pexec_s));
|
|
if (!st)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
/* Set up I-Space */
|
|
|
|
st->ispace = attr->ispace;
|
|
st->maxpc = attr->maxpc;
|
|
|
|
/* Align size of read-only data to 16-bit boundary. */
|
|
|
|
adjusted_rosize = (attr->rosize + 1) & ~1;
|
|
|
|
/* Allocate the pascal stack. Organization is string stack, then
|
|
* constant data, then "normal" pascal stack.
|
|
*/
|
|
|
|
stacksize = attr->varsize + adjusted_rosize + attr->strsize;
|
|
st->dstack.b = (uint8_t*)malloc(stacksize);
|
|
if (!st->dstack.b)
|
|
{
|
|
free(st);
|
|
return NULL;
|
|
}
|
|
|
|
/* Copy the rodata into the stack */
|
|
|
|
if (attr->rodata && attr->rosize)
|
|
{
|
|
memcpy(&st->dstack.b[attr->strsize], attr->rodata, attr->rosize);
|
|
}
|
|
|
|
/* Set up info needed to perform a simulated reset */
|
|
|
|
st->strsize = attr->strsize;
|
|
st->rosize = adjusted_rosize;
|
|
st->entry = attr->entry;
|
|
st->stacksize = stacksize;
|
|
|
|
/* Then perform a simulated reset */
|
|
|
|
pexec_reset(st);
|
|
return st;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec
|
|
****************************************************************************/
|
|
|
|
int pexec(FAR struct pexec_s *st)
|
|
{
|
|
uint8_t opcode;
|
|
int ret;
|
|
|
|
/* Make sure that the program counter is within range */
|
|
|
|
if (st->pc >= st->maxpc)
|
|
{
|
|
ret = eBADPC;
|
|
}
|
|
else
|
|
{
|
|
/* Get the instruction to execute */
|
|
|
|
opcode = st->ispace[st->pc];
|
|
if ((opcode & o8) != 0)
|
|
{
|
|
/* Get the immediate, 8-bit value */
|
|
|
|
uint8_t imm8 = st->ispace[st->pc + 1];
|
|
if ((opcode & o16) != 0)
|
|
{
|
|
/* Get the immediate, big-endian 16-bit value */
|
|
|
|
uint16_t imm16 = ((st->ispace[st->pc + 2]) << 8) | st->ispace[st->pc + 3];
|
|
|
|
/* Handle 32 bit instructions */
|
|
|
|
ret = pexec32(st, opcode, imm8, imm16);
|
|
}
|
|
else
|
|
{
|
|
/* Handle 16-bit instructions */
|
|
|
|
ret = pexec16(st, opcode, imm8);
|
|
}
|
|
}
|
|
else if ((opcode & o16) != 0)
|
|
{
|
|
/* Get the immediate, big-endian 16-bit value */
|
|
|
|
uint16_t imm16 = ((st->ispace[st->pc + 1]) << 8) | st->ispace[st->pc + 2];
|
|
|
|
/* Handle 24-bit instructions */
|
|
|
|
ret = pexec24(st, opcode, imm16);
|
|
}
|
|
else
|
|
{
|
|
/* Handle 8-bit instructions */
|
|
|
|
ret = pexec8(st, opcode);
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_reset
|
|
****************************************************************************/
|
|
|
|
void pexec_reset(struct pexec_s *st)
|
|
{
|
|
int dndx;
|
|
|
|
/* Setup the bottom of the "normal" pascal stack */
|
|
|
|
st->rop = st->strsize;
|
|
st->spb = st->strsize + st->rosize;
|
|
|
|
/* Initialize the emulated P-Machine registers */
|
|
|
|
st->csp = 0;
|
|
st->sp = st->spb + 2*BPERI;
|
|
st->fp = st->spb + BPERI;
|
|
st->pc = st->entry;
|
|
|
|
/* Initialize the P-Machine stack */
|
|
|
|
dndx = BTOISTACK(st->spb);
|
|
st->dstack.i[dndx] = 0;
|
|
st->dstack.i[dndx+1] = 0;
|
|
st->dstack.i[dndx+2] = -1;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pexec_release
|
|
****************************************************************************/
|
|
|
|
void pexec_release(struct pexec_s *st)
|
|
{
|
|
if (st)
|
|
{
|
|
if (st->dstack.i)
|
|
{
|
|
free(st->dstack.i);
|
|
}
|
|
|
|
if (st->ispace)
|
|
{
|
|
free(st->ispace);
|
|
}
|
|
|
|
free(st);
|
|
}
|
|
}
|