|
|
|
#include "forth_internal.h"
|
|
|
|
|
|
|
|
// TODO stacks should grow down, not up!
|
|
|
|
|
|
|
|
enum fh_error ds_roll(struct fh_thread_s *fh, int n)
|
|
|
|
{
|
|
|
|
if (fh->data_stack_top <= n) {
|
|
|
|
LOGE("DS roll UNDERFLOW");
|
|
|
|
return FH_ERR_DS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t yn = fh->data_stack_top - 1 - n;
|
|
|
|
uint32_t yoinked = fh->data_stack[yn];
|
|
|
|
for (uint32_t i = yn; i < fh->data_stack_top; i++) {
|
|
|
|
fh->data_stack[i] = fh->data_stack[i + 1];
|
|
|
|
}
|
|
|
|
fh->data_stack[fh->data_stack_top - 1] = yoinked;
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Peek top of data stack */
|
|
|
|
enum fh_error ds_peek_n(struct fh_thread_s *fh, uint32_t *out, int n)
|
|
|
|
{
|
|
|
|
if (fh->data_stack_top <= n) {
|
|
|
|
LOGE("DS peek_n UNDERFLOW");
|
|
|
|
return FH_ERR_DS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
*out = fh->data_stack[fh->data_stack_top - 1 - n];
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Peek top of return stack */
|
|
|
|
enum fh_error rs_peek_n(struct fh_thread_s *fh, uint32_t *out, int n)
|
|
|
|
{
|
|
|
|
if (fh->return_stack_top <= n) {
|
|
|
|
LOGE("RS peek_n UNDERFLOW");
|
|
|
|
return FH_ERR_RS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
*out = fh->return_stack[fh->return_stack_top - 1 - n];
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Replace value on return stack */
|
|
|
|
enum fh_error rs_poke_n(struct fh_thread_s *fh, uint32_t value, int n)
|
|
|
|
{
|
|
|
|
if (fh->return_stack_top <= n) {
|
|
|
|
LOGE("RS peek_n UNDERFLOW");
|
|
|
|
return FH_ERR_RS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
fh->return_stack[fh->return_stack_top - 1 - n] = value;
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Pop from data stack */
|
|
|
|
enum fh_error ds_pop(struct fh_thread_s *fh, uint32_t *out)
|
|
|
|
{
|
|
|
|
if (fh->data_stack_top == 0) {
|
|
|
|
LOGE("DS pop UNDERFLOW");
|
|
|
|
return FH_ERR_DS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
*out = fh->data_stack[--fh->data_stack_top];
|
|
|
|
LOG("DS pop %d (0x%08x)", *out, *out);
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Pop from return stack */
|
|
|
|
enum fh_error rs_pop(struct fh_thread_s *fh, uint32_t *out)
|
|
|
|
{
|
|
|
|
if (fh->return_stack_top == 0) {
|
|
|
|
LOGE("RS pop UNDERFLOW");
|
|
|
|
return FH_ERR_RS_UNDERFLOW;
|
|
|
|
}
|
|
|
|
*out = fh->return_stack[--fh->return_stack_top];
|
|
|
|
LOG("RS pop %d (0x%08x)", *out, *out);
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
#define UPDATE_HWM(hwm, top) \
|
|
|
|
do { \
|
|
|
|
if((hwm) < (top)) { \
|
|
|
|
(hwm) = (top); \
|
|
|
|
} \
|
|
|
|
} while(0)
|
|
|
|
|
|
|
|
enum fh_error ds_push_dw(struct fh_thread_s *fh, uint64_t in)
|
|
|
|
{
|
|
|
|
enum fh_error rv;
|
|
|
|
TRY(ds_push(fh, in & 0xFFFFFFFFULL));
|
|
|
|
TRY(ds_push(fh, (in & 0xFFFFFFFF00000000ULL) >> 32));
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
enum fh_error ds_pop_dw(struct fh_thread_s *fh, uint64_t *out)
|
|
|
|
{
|
|
|
|
enum fh_error rv;
|
|
|
|
uint32_t a, b;
|
|
|
|
TRY(ds_pop(fh, &a));
|
|
|
|
TRY(ds_pop(fh, &b));
|
|
|
|
*out = ((uint64_t) a << 32) | ((uint64_t) b);
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Push to data stack */
|
|
|
|
enum fh_error ds_push(struct fh_thread_s *fh, uint32_t in)
|
|
|
|
{
|
|
|
|
LOG("DS push %d (0x%08x)", in, in);
|
|
|
|
if (fh->data_stack_top == DATA_STACK_DEPTH) {
|
|
|
|
return FH_ERR_DS_OVERFLOW;
|
|
|
|
}
|
|
|
|
fh->data_stack[fh->data_stack_top++] = in;
|
|
|
|
UPDATE_HWM(fh->data_stack_hwm, fh->data_stack_top);
|
|
|
|
return FH_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Push to return stack */
|
|
|
|
enum fh_error rs_push(struct fh_thread_s *fh, uint32_t in)
|
|
|
|
{
|
|
|
|
LOG("RS push %d (0x%08x)", in, in);
|
|
|
|
if (fh->return_stack_top == RETURN_STACK_DEPTH) {
|
|
|
|
return FH_ERR_RS_OVERFLOW;
|
|
|
|
}
|
|
|
|
fh->return_stack[fh->return_stack_top++] = in;
|
|
|
|
UPDATE_HWM(fh->return_stack_hwm, fh->return_stack_top);
|
|
|
|
return FH_OK;
|
|
|
|
}
|