616 lines
14 KiB
616 lines
14 KiB
#include "forth_internal.h"
|
|
|
|
static enum fh_error wp_setbase(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
fh_setbase(fh, w->param);
|
|
return FH_OK;
|
|
}
|
|
|
|
enum fh_error wp_const(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
enum fh_error rv;
|
|
TRY(ds_push(fh, w->param));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_plus(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_push(fh, a + b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_minus(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, a - b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_star(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_push(fh, a * b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_and(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_push(fh, a & b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_or(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_push(fh, a | b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_xor(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_push(fh, a ^ b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_lshift(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, a << b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_rshift(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, a >> b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_zero_less(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL((int32_t) a < 0)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_zero_greater(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL((int32_t) a > 0)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_zero_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a == 0)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_zero_not_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a != 0)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_min(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, (int32_t) a < (int32_t) b ? a : b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_max(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, (int32_t) a < (int32_t) b ? b : a));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_less(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL((int32_t) a < (int32_t) b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_greater(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL((int32_t) a > (int32_t) b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_within(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t test, low, high;
|
|
TRY(ds_pop(fh, &high));
|
|
TRY(ds_pop(fh, &low));
|
|
TRY(ds_pop(fh, &test));
|
|
|
|
// This should work, according to the spec:
|
|
// : WITHIN ( test low high -- flag ) OVER - >R - R> U< ;
|
|
|
|
// t l h | OVER
|
|
// t l h l | -
|
|
// t l (h-l) | >r
|
|
// t l | R:(h-l) | -
|
|
// (t-l) | R:(h-l) | R>
|
|
// (t-l) (h-l) | U<
|
|
// =within
|
|
|
|
const int32_t ti = (int32_t)test;
|
|
const int32_t li = (int32_t)low;
|
|
const int32_t hi = (int32_t)high;
|
|
|
|
const bool within = (uint32_t)(ti-li) < (uint32_t)(hi-li);
|
|
|
|
TRY(ds_push(fh, TOBOOL(within)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_u_less(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a < b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_u_greater(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a > b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a == b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_not_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, TOBOOL(a != b)));
|
|
return FH_OK;
|
|
}
|
|
|
|
enum fh_error wp_add(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, a + w->param));
|
|
return FH_OK;
|
|
}
|
|
|
|
enum fh_error wp_mul(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, a * w->param));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error wp_div(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, (int32_t) a / (int32_t) w->param));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_2div(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, (a & 0x80000000) | (a >> 1)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_star_slash(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0, c = 0;
|
|
TRY(ds_pop(fh, &c));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
if (c == 0) {
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
int64_t v = ((int64_t) (int32_t)a * (int64_t) (int32_t)b) / (int64_t) (int32_t)c;
|
|
|
|
TRY(ds_push(fh, (uint32_t) v));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_star_slash_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0, c = 0;
|
|
TRY(ds_pop(fh, &c));
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
if (c == 0) {
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
int64_t product = ((int64_t) (int32_t)a * (int64_t) (int32_t)b);
|
|
int64_t v = product / (int64_t) (int32_t)c;
|
|
int64_t m = product % (int64_t) (int32_t)c;
|
|
|
|
TRY(ds_push(fh, (uint32_t) (int32_t)m));
|
|
TRY(ds_push(fh, (uint32_t) (int32_t)v));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_slash(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
if (b == 0) {
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
TRY(ds_push(fh, (int32_t)a / (int32_t)b));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_abs(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
int32_t sa = (int32_t) a; // TODO is this right?
|
|
|
|
if (sa < 0) { sa = -sa; }
|
|
|
|
TRY(ds_push(fh, sa));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_invert(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, ~a));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_negate(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_push(fh, (uint32_t) (-(uint32_t) a)));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_slash_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
if (b == 0) {
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
int32_t rem = (int32_t)a % (int32_t)b;
|
|
int32_t div = (int32_t)a / (int32_t)b;
|
|
|
|
TRY(ds_push(fh, rem));
|
|
TRY(ds_push(fh, div));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &b));
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
if (b == 0) {
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
int32_t rem = (int32_t)a % (int32_t)b;
|
|
|
|
TRY(ds_push(fh, rem));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_s_to_d(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
int32_t as = (int32_t) a; // because of sign extend
|
|
int64_t a64 = as;
|
|
|
|
TRY(ds_push_dw(fh, (uint64_t) a64));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_m_star(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
|
|
// make signed and then sign extend
|
|
int64_t res = (int64_t) (int32_t) a * (int64_t) (int32_t) b;
|
|
|
|
TRY(ds_push_dw(fh, (uint64_t) res));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_um_star(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t a = 0, b = 0;
|
|
TRY(ds_pop(fh, &a));
|
|
TRY(ds_pop(fh, &b));
|
|
|
|
// make signed and then sign extend
|
|
uint64_t res = (uint64_t) a * (uint64_t) b;
|
|
|
|
TRY(ds_push_dw(fh, res));
|
|
return FH_OK;
|
|
}
|
|
|
|
// Copied from https://stackoverflow.com/a/51457071/2180189
|
|
void floor_div64(int64_t *q, int64_t *r, int64_t a, int64_t b)
|
|
{
|
|
int64_t q0 = a / b;
|
|
int64_t r0 = a % b;
|
|
if (b > 0){
|
|
*q = r0 >= 0 ? q0 : q0 - 1;
|
|
*r = r0 >= 0 ? r0 : r0 + b;
|
|
}
|
|
else {
|
|
*q = r0 <= 0 ? q0 : q0 - 1;
|
|
*r = r0 <= 0 ? r0 : r0 + b;
|
|
}
|
|
}
|
|
|
|
static enum fh_error w_fm_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
int32_t div;
|
|
int64_t num;
|
|
TRY(ds_pop(fh, (uint32_t*)&div));
|
|
TRY(ds_pop_dw(fh, (uint64_t*)&num));
|
|
|
|
int64_t res, rem;
|
|
|
|
floor_div64(&res, &rem, num, div);
|
|
|
|
if ((int64_t)(int32_t)rem != rem) {
|
|
LOGE("Remainder too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
if ((int64_t)(int32_t)res != res) {
|
|
LOGE("Division result too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
TRY(ds_push(fh, (int32_t)rem));
|
|
TRY(ds_push(fh, (int32_t)res));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_um_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
uint32_t div;
|
|
uint64_t num;
|
|
TRY(ds_pop(fh, &div));
|
|
TRY(ds_pop_dw(fh, &num));
|
|
|
|
uint64_t res = num / (uint64_t)div;
|
|
uint64_t rem = num % (uint64_t)div;
|
|
|
|
if ((uint64_t)(uint32_t)rem != rem) {
|
|
LOGE("Remainder too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
if ((uint64_t)(uint32_t)res != res) {
|
|
LOGE("Division result too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
TRY(ds_push(fh, (uint32_t)rem));
|
|
TRY(ds_push(fh, (uint32_t)res));
|
|
return FH_OK;
|
|
}
|
|
|
|
static enum fh_error w_sm_rem(struct fh_thread_s *fh, const struct fh_word_s *w)
|
|
{
|
|
(void) w;
|
|
enum fh_error rv;
|
|
int32_t div;
|
|
int64_t num;
|
|
TRY(ds_pop(fh, (uint32_t*)&div));
|
|
TRY(ds_pop_dw(fh, (uint64_t*)&num));
|
|
|
|
int64_t res = num / (int64_t)div;
|
|
int64_t rem = num % (int64_t)div;
|
|
|
|
if ((int64_t)(int32_t)rem != rem) {
|
|
LOGE("Remainder too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
if ((int64_t)(int32_t)res != res) {
|
|
LOGE("Division result too large");
|
|
return FH_ERR_ARITH;
|
|
}
|
|
|
|
TRY(ds_push(fh, (int32_t)rem));
|
|
TRY(ds_push(fh, (int32_t)res));
|
|
return FH_OK;
|
|
}
|
|
|
|
const struct name_and_handler fh_builtins_arith[] = {
|
|
/* Arithmetics */
|
|
{"base", wp_const, 0, MAGICADDR_BASE},
|
|
{"decimal", wp_setbase, 0, 10},
|
|
{"hex", wp_setbase, 0, 16},
|
|
{"false", wp_const, 0, 0},
|
|
{"true", wp_const, 0, 0xFFFFFFFF},
|
|
{"+", w_plus, 0, 0},
|
|
{"-", w_minus, 0, 0},
|
|
{"*", w_star, 0, 0},
|
|
{"*/", w_star_slash, 0, 0},
|
|
{"*/mod", w_star_slash_mod, 0, 0},
|
|
{"or", w_or, 0, 0},
|
|
{"and", w_and, 0, 0},
|
|
{"xor", w_xor, 0, 0},
|
|
{"/", w_slash, 0, 0},
|
|
{"abs", w_abs, 0, 0},
|
|
{"/mod", w_slash_mod, 0, 0},
|
|
{"mod", w_mod, 0, 0},
|
|
{"invert", w_invert, 0, 0},
|
|
{"negate", w_negate, 0, 0},
|
|
{"lshift", w_lshift, 0, 0},
|
|
{"rshift", w_rshift, 0, 0},
|
|
{"min", w_min, 0, 0},
|
|
{"max", w_max, 0, 0},
|
|
{"0<", w_zero_less, 0, 0},
|
|
{"0=", w_zero_equals, 0, 0},
|
|
{"0<>", w_zero_not_equals, 0, 0},
|
|
{"0>", w_zero_greater, 0, 0},
|
|
{"<", w_less, 0, 0},
|
|
{"u<", w_u_less, 0, 0},
|
|
{"=", w_equals, 0, 0},
|
|
{"<>", w_not_equals, 0, 0},
|
|
{">", w_greater, 0, 0},
|
|
{"u>", w_u_greater, 0, 0},
|
|
{"1+", wp_add, 0, 1},
|
|
{"1-", wp_add, 0, -1},
|
|
{"2+", wp_add, 0, 2},
|
|
{"2-", wp_add, 0, -2},
|
|
{"2*", wp_mul, 0, 2},
|
|
{"2/", w_2div, 0, 0},
|
|
{"s>d", w_s_to_d, 0, 0},
|
|
{"m*", w_m_star, 0, 0},
|
|
{"um*", w_um_star, 0, 0},
|
|
{"fm/mod", w_fm_mod, 0, 0},
|
|
{"sm/rem", w_sm_rem, 0, 0},
|
|
{"um/mod", w_um_mod, 0, 0},
|
|
{"within", w_within, 0, 0},
|
|
{ /* end marker */ }
|
|
};
|
|
|