main,adcs: Enhancement of power command

If an abnormal packet size or command code is received, the reply
telemetry was not sent previously. This commit addresses this by
ensuring the reply telemetry is sent. In such cases, the command
ID will be uniformly set to 0xFF, as this is necessary for
categorization on the Yamcs side.

Signed-off-by: Takuya Sasaki <takuya.sasaki@spacecubics.com>

adcs/src/cmd/pwrctrl.c

@@ -72,7 +72,7 @@ static int csp_power_cycle_cmd(uint8_t command_id, csp_packet_t *packet)
 
 int csp_pwrctrl_handler(csp_packet_t *packet)
 {
-	int ret;
+	int ret = 0;
 	uint8_t command_id;
 
 	if (packet == NULL) {
@@ -82,8 +82,9 @@ int csp_pwrctrl_handler(csp_packet_t *packet)
 
 	if (packet->length < PWRCTRL_CMD_MIN_SIZE) {
 		LOG_ERR("Invalide command size: %d", packet->length);
-		ret = -EINVAL;
-		goto free;
+		command_id = CSP_UNKNOWN_CMD_CODE;
+		ret = -EMSGSIZE;
+		goto reply;
 	}
 
 	command_id = packet->data[CSP_COMMAND_ID_OFFSET];
@@ -97,11 +98,16 @@ int csp_pwrctrl_handler(csp_packet_t *packet)
 		break;
 	default:
 		LOG_ERR("Unkown command code: %d", command_id);
+		command_id = CSP_UNKNOWN_CMD_CODE;
+		ret = -EINVAL;
 		break;
 	}
 
-free:
-	csp_buffer_free(packet);
+reply:
+	if (ret < 0) {
+		csp_send_std_reply(packet, command_id, ret);
+		csp_buffer_free(packet);
+	}
 
 end:
 	return ret;

include/sc_csp.h

@@ -20,3 +20,5 @@
 #define CSP_TELEMETRY_ID_OFFSET (0U)
 #define CSP_COMMAND_ID_OFFSET   (0U)
 #define CSP_ERROR_CODE_OFFSET   (1U)
+
+#define CSP_UNKNOWN_CMD_CODE (255U)

main/src/cmd/pwrctrl.c

@@ -72,7 +72,7 @@ static int csp_power_cycle_cmd(uint8_t command_id, csp_packet_t *packet)
 
 int csp_pwrctrl_handler(csp_packet_t *packet)
 {
-	int ret;
+	int ret = 0;
 	uint8_t command_id;
 
 	if (packet == NULL) {
@@ -82,8 +82,9 @@ int csp_pwrctrl_handler(csp_packet_t *packet)
 
 	if (packet->length < PWRCTRL_CMD_MIN_SIZE) {
 		LOG_ERR("Invalide command size: %d", packet->length);
-		ret = -EINVAL;
-		goto free;
+		command_id = CSP_UNKNOWN_CMD_CODE;
+		ret = -EMSGSIZE;
+		goto reply;
 	}
 
 	command_id = packet->data[CSP_COMMAND_ID_OFFSET];
@@ -97,12 +98,16 @@ int csp_pwrctrl_handler(csp_packet_t *packet)
 		break;
 	default:
 		LOG_ERR("Unkown command code: %d", command_id);
+		command_id = CSP_UNKNOWN_CMD_CODE;
 		ret = -EINVAL;
 		break;
 	}
 
-free:
-	csp_buffer_free(packet);
+reply:
+	if (ret < 0) {
+		csp_send_std_reply(packet, command_id, ret);
+		csp_buffer_free(packet);
+	}
 
 end:
 	return ret;