From db97dd31fbf97493c56f17b222920f973658b609 Mon Sep 17 00:00:00 2001
From: Polprzewodnikowy <sc@mateuszfaderewski.pl>
Date: Sat, 24 Sep 2022 12:42:11 +0200
Subject: [PATCH] return previous cfg when setting new

---
 sw/controller/src/cfg.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/sw/controller/src/cfg.c b/sw/controller/src/cfg.c
index 9a13144..73d118f 100644
--- a/sw/controller/src/cfg.c
+++ b/sw/controller/src/cfg.c
@@ -355,6 +355,7 @@ void cfg_init (void) {
 void cfg_process (void) {
     uint32_t reg;
     uint32_t args[2];
+    uint32_t prev_cfg[2];
     usb_tx_info_t packet_info;
 
     reg = fpga_reg_get(REG_CFG_CMD);
@@ -377,10 +378,13 @@ void cfg_process (void) {
                 break;
 
             case 'C':
+                prev_cfg[0] = args[0];
+                cfg_query(prev_cfg);
                 if (cfg_update(args)) {
                     cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
                     return;
                 }
+                args[1] = prev_cfg[1];
                 break;
 
             case 't':