put upsilon_creole calls into creole.c

1 files changed, 128 insertions, 24 deletions

diff --git a/creole.c b/creole.c
index 000fa0d..9df59bd 100644
--- a/creole.c
+++ b/creole.c
@@ -12,8 +12,12 @@ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */
 
+#include <zephyr/kernel.h>
+#include <zephyr/sys_clock.h>
 #include "creole.h"
-#include "upsilon_creole.h"
+#include "access.h"
+#include "control_loop_cmds.h"
+#include "sock.h"
 
 /*************************************************************************
  * Static information
@@ -550,6 +554,60 @@ int creole_jump(struct creole_env *env, creole_word off)
 	return 1;
 }
 
+static size_t
+load_into_array(const struct creole_reader *start, creole_word *buf, size_t buflen)
+{
+	size_t i = 0;
+	struct creole_word w;
+	struct creole_reader r = start;
+
+	while (creole_decode(&r, &w) && i < buflen) {
+		buf[i++] = w.word;
+	}
+
+	return i;
+}
+
+static creole_word
+upsilon_load_waveform(struct creole_env *env, creole_word slot,
+                      creole_word db)
+{
+	creole_word buf[MAX_WL_SIZE];
+	size_t len = load_into_array(env->dats[db], buf, ARRAY_SIZE(buf));
+	if (len < MAX_WL_SIZE)
+		return 0;
+	return waveform_load(buf, slot, K_FOREVER);
+}
+
+static creole_word
+upsilon_sendval(struct creole_env *env, creole_word num)
+{
+	char buf[32];
+	struct bufptr bp = {buf, sizeof(buf)};
+
+	return sock_printf(env->fd, &bp, "%u", num) == BUF_OK;
+}
+
+static creole_word
+upsilon_senddat(struct creole_env *env, creole_word db)
+{
+	char buf[128];
+	struct bufptr bp = {buf, 0};
+	struct creole_word w;
+	struct creole_reader r = start;
+
+	while (creole_decode(&r, &w) && bp.left < buflen) {
+		if (w.word > 0xFF)
+			return -EINVAL;
+		buf[bp.left++] = w.word;
+	}
+
+	return sock_write_buf(env->fd, &bp);
+}
+
+
+/* Upsilon interface */
+
 #define chk_sign_op(OPER) do {                                       \
 	switch (check_sign_bits(ins.w_flags[1], ins.w_flags[2]))   { \
 	case ALL_UNSIGNED:                                           \
@@ -586,24 +644,29 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		env->dats[ins->w[0]].p = ins->datapt;
 		env->dats[ins->w[0]].left = ins->dataptlen;
 		break;
+
 	case CREOLE_PUSH:
 		check(read_val(env, &ins, 0, &a1));
 		check(creole_push(env, a1));
 		break;
+
 	case CREOLE_POP:
 		check(creole_pop(env, &a1));
 		check(creole_reg_write(env, ins.w[0], a1));
 		break;
+
 	case CREOLE_ADD:
 		check(read_val(env, &ins, 1, &a1));
 		check(read_val(env, &ins, 2, &a2));
 		check(creole_reg_write(env, ins.w[0], a1 + a2));
 		break;
+
 	case CREOLE_MUL:
 		check(read_val(env, &ins, 1, &a1));
 		check(read_val(env, &ins, 2, &a2));
 		check(creole_reg_write(env, ins.w[0], a1 * a2));
 		break;
+
 	case CREOLE_DIV:
 		check(read_val(env, &ins, 1, &a1));
 		check(read_val(env, &ins, 2, &a2));
@@ -613,10 +676,12 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		chk_sign_op(/);
 		check(creole_reg_write(env, ins.w[0], a1));
 		break;
+
 	case CREOLE_SYS:
 		check(read_val(env, &ins, 0, sc));
 		rcode = CREOLE_STEP_SYSCALL;
 		break;
+
 	case CREOLE_JL:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
@@ -625,6 +690,7 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		if (a1 && !creole_jump(env, a0))
 				return CREOLE_JUMP_OVERFLOW;
 		break;
+
 	case CREOLE_JLE:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
@@ -633,6 +699,7 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		if (a1 && !creole_jump(env, a0))
 				return CREOLE_JUMP_OVERFLOW;
 		break;
+
 	case CREOLE_JE:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
@@ -640,6 +707,7 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		if (a1 == a2 && !creole_jump(env, a0))
 			return CREOLE_JUMP_OVERFLOW;
 		break;
+
 	case CREOLE_JNE:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
@@ -647,100 +715,136 @@ enum creole_run_ret creole_step(struct creole_env *env, creole_word *sc)
 		if (a1 != a2 && !creole_jump(env, a0))
 			return CREOLE_JUMP_OVERFLOW;
 		break;
+
+
 	case CREOLE_READ_ADC:
 		check(read_val(env, &ins, 0, &a0));
-		a1 = upsilon_get_adc(a0);
-		check(creole_reg_write(env, ins.w[1], a1));
+		a1 = adc_read(a0, K_FOREVER, &a2);
+		check(creole_reg_write(env, ins.w[1], a2));
+		check(creole_push(env, a1));
 		break;
+
 	case CREOLE_READ_DAC:
 		check(read_val(env, &ins, 0, &a0));
-		a1 = upsilon_get_dac(a0);
-		check(creole_reg_write(env, ins.w[1], a1));
+		a1 = dac_read_write(a0, 0x1 << 23 | 0x1 << 20, K_FOREVER, NULL);
+		if (a1 == 0) {
+			a1 = dac_read_write(a0, 0, K_FOREVER, &a2);
+			check(creole_reg_write(env, ins.w[1], a2));
+		}
+		check(creole_push(env, a1));
 		break;
+
 	case CREOLE_WRITE_DAC:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
-		upsilon_write_dac(a0, a1);
+
+		a2 = dac_read_write(a0, 0x1 << 20 | a1, K_FOREVER, NULL);
+		check(creole_push(env, a2));
 		break;
+
 	case CREOLE_SLEEP:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_sleep(a0);
+		k_sleep(K_USEC(a0));
+		check(creole_push(env, 0));
 		break;
+
 	case CREOLE_CLOOP_READ:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 0, &a1));
 		check(read_val(env, &ins, 0, &a2));
+
 		if (valid_reglen(env, a1) && valid_reglen(env, a2)) {
-			upsilon_control_loop_read(env->reg + a1,
-			                          env->reg + a2,
-			                          a0);
+			a0 = cloop_read(a0, env->reg + a1, env->reg + a2, K_FOREVER);
+			check(creole_push(env, a0));
+		} else {
+			check(creole_push(env, -EINVAL));
 		}
+
 		break;
+
 	case CREOLE_CLOOP_WRITE:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 0, &a1));
 		check(read_val(env, &ins, 0, &a2));
-		upsilon_control_loop_write(a0, a1, a2);
+
+		a0 = cloop_write(a0, a1, a2, K_FOREVER);
+		check(creole_push(env, a0));
 		break;
+
 	case CREOLE_WF_LOAD:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
-		upsilon_load_waveform(env, a0, a1);
+		check(creole_push(env, upsilon_load_waveform(env, a0, a1)));
 		break;
+
 	case CREOLE_WF_ARM:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 1, &a1));
 		check(read_val(env, &ins, 2, &a2));
-		upsilon_arm_waveform(a0, a1, a2);
+
+		check(creole_push(env, waveform_arm(a0, a1, a2, K_FOREVER)));
 		break;
+
 	case CREOLE_WF_DISARM:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_disarm_waveform(a0);
+		check(creole_push(env, waveform_disarm(a0)));
 		break;
+
 	case CREOLE_SENDVAL:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_sendval(a0);
+		check(creole_push(env, upsilon_sendval(a0)));
 		break;
+
 	case CREOLE_SENDDAT:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_senddat(env, a0);
+		check(creole_push(env, upsilon_senddat(env, a0)));
 		break;
+
 	case CREOLE_TAKE_ADC:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 0, &a1));
-		upsilon_take_adc(a0, a1);
+		check(creole_push(env, adc_take(a0, K_USEC(a1))));
 		break;
+
 	case CREOLE_RELEASE_ADC:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_release_adc(a0);
+		check(creole_push(env, adc_release(a0)));
 		break;
+
 	case CREOLE_TAKE_DAC:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 0, &a1));
-		upsilon_take_dac(a0, a1);
+		check(creole_push(env, dac_take(a0, K_USEC(a1))));
 		break;
+
 	case CREOLE_RELEASE_DAC:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_release_dac(a0);
+		check(creole_push(env, dac_release(a0)));
 		break;
+
 	case CREOLE_TAKE_WF:
 		check(read_val(env, &ins, 0, &a0));
 		check(read_val(env, &ins, 0, &a1));
-		upsilon_take_wf(a0, a1);
+		check(creole_push(env, waveform_take(a0, K_USEC(a1))));
 		break;
+
 	case CREOLE_RELEASE_WF:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_release_wf(a0);
+		check(creole_push(env, waveform_release(a0)));
 		break;
+
 	case CREOLE_TAKE_CLOOP:
 		check(read_val(env, &ins, 0, &a0));
-		upsilon_take_cloop(a0);
+		check(creole_push(env, cloop_take(K_USEC(a0))));
 		break;
+
 	case CREOLE_RELEASE_CLOOP:
-		upsilon_release_cloop();
+		check(creole_push(env, cloop_release()));
 		break;
+
 	default:
 		rcode = CREOLE_STEP_UNKNOWN_OPCODE;
+		break;
 	}
 
 	return rcode;