From 1e45c8b5d74bd5974bc239d2b5624017842bcfae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mario=20H=C3=BCttel?= <mario.huettel@gmx.net>
Date: Sun, 26 Dec 2021 20:54:04 +0100
Subject: [PATCH] Issue #39: Implement DIGIO profile commands

---
 .../temp-profile/temp-profile-executer.c      | 34 ++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/stm-firmware/temp-profile/temp-profile-executer.c b/stm-firmware/temp-profile/temp-profile-executer.c
index f5631a8..ed983ef 100644
--- a/stm-firmware/temp-profile/temp-profile-executer.c
+++ b/stm-firmware/temp-profile/temp-profile-executer.c
@@ -185,6 +185,36 @@ static void cmd_ack_flags(void)
 
 }
 
+static void cmd_digio_conf(uint8_t digio_num, uint8_t mode)
+{
+	uint8_t pin_mode;
+	uint8_t alt_func = 0;
+
+	if (mode == 0 || mode == 1) {
+		pin_mode = mode;
+	} else if (mode >= 0x80 && mode <= 0x87) {
+		/* Alternate function */
+		pin_mode = 2;
+		alt_func = mode - 0x80;
+	} else {
+		return;
+	}
+
+	digio_setup_pin(digio_num, pin_mode, alt_func);
+}
+
+bool cmd_digio_wait(uint8_t digio_num, uint8_t digio_state)
+{
+	bool advance = false;
+	int res;
+
+	res = digio_get(digio_num);
+	if (res < 0 || (uint8_t)res == digio_state)
+		advance = true;
+
+	return advance;
+}
+
 int temp_profile_executer_handle(void)
 {
 	struct pl_command *current_cmd;
@@ -252,12 +282,14 @@ int temp_profile_executer_handle(void)
 		break;
 	case PL_DIGIO_CONF:
 		advance = true;
+		cmd_digio_conf((uint8_t)current_cmd->params[0], (uint8_t)current_cmd->params[1]);
 		break;
 	case PL_DIGIO_SET:
+		digio_set((uint8_t)current_cmd->params[0], current_cmd->params[1] ? 1u : 0u);
 		advance = true;
 		break;
 	case PL_DIGIO_WAIT:
-		advance = true;
+		advance = cmd_digio_wait((uint8_t)current_cmd->params[0], current_cmd->params[1] ? 1u : 0u);
 		break;
 	default:
 		tpe_abort();