diff --git a/zero/meta-csp/recipes-cspd/csp-handler/files/file.c b/zero/meta-csp/recipes-cspd/csp-handler/files/file.c index 95b62eb..c9d8727 100644 --- a/zero/meta-csp/recipes-cspd/csp-handler/files/file.c +++ b/zero/meta-csp/recipes-cspd/csp-handler/files/file.c @@ -29,6 +29,19 @@ static GQueue file_work_queue; static pthread_t file_work; +void send_err_reply(csp_packet_t *packet, uint8_t command_id, int err_code) +{ + struct file_err_reply_telemetry tlm; + + tlm.telemetry_id = command_id; + tlm.error_code = htole32(err_code); + + memcpy(packet->data, &tlm, sizeof(tlm)); + packet->length = sizeof(tlm); + + csp_sendto_reply(packet, packet, CSP_O_SAME); +} + static void csp_file_work(csp_packet_t *packet) { int ret = 0; @@ -44,7 +57,7 @@ static void csp_file_work(csp_packet_t *packet) sd_journal_print(LOG_ERR, "Invalide command size: %d", packet->length); ret = -EINVAL; command_id = CSP_UNKNOWN_COMMAND_ID; - goto free; + goto reply; } command_id = packet->data[CSP_COMMAND_ID_OFFSET]; @@ -66,8 +79,9 @@ static void csp_file_work(csp_packet_t *packet) break; } -free: +reply: if (ret < 0) { + send_err_reply(packet, command_id, ret); } end: diff --git a/zero/meta-csp/recipes-cspd/csp-handler/files/file.h b/zero/meta-csp/recipes-cspd/csp-handler/files/file.h index 32d4455..153f609 100644 --- a/zero/meta-csp/recipes-cspd/csp-handler/files/file.h +++ b/zero/meta-csp/recipes-cspd/csp-handler/files/file.h @@ -8,5 +8,10 @@ #include +struct file_err_reply_telemetry { + uint8_t telemetry_id; + uint32_t error_code; +} __attribute__((__packed__)); + void file_handler_init(void); void file_handler(csp_packet_t *packet);