1 files changed, 34 insertions, 12 deletions

diff --git a/fw/fe310/eos/dev/ctp.c b/fw/fe310/eos/dev/ctp.c
index b6459b4..e58f929 100644
--- a/fw/fe310/eos/dev/ctp.c
+++ b/fw/fe310/eos/dev/ctp.c
@@ -32,6 +32,13 @@ int eos_ctp_init(void) {
     return rv;
 }
 
+int _eos_ctp_reset(void) {
+    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_CTP_SEL)) return EOS_ERR_BUSY;
+
+    gt911_reset();
+    return EOS_OK;
+}
+
 int eos_ctp_reset(void) {
     uint8_t status = 0;
     int rv;
@@ -49,6 +56,12 @@ int eos_ctp_reset(void) {
     return EOS_OK;
 }
 
+int _eos_ctp_sleep(void) {
+    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_CTP_SEL)) return EOS_ERR_BUSY;
+
+    return gt911_sleep();
+}
+
 int eos_ctp_sleep(void) {
     int rv;
 
@@ -67,6 +80,13 @@ int eos_ctp_sleep(void) {
     return EOS_OK;
 }
 
+int _eos_ctp_wake(void) {
+    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_CTP_SEL)) return EOS_ERR_BUSY;
+
+    gt911_wake();
+    return EOS_OK;
+}
+
 int eos_ctp_wake(void) {
     int rv;
 
@@ -85,7 +105,7 @@ int eos_ctp_wake(void) {
 }
 
 int eos_ctp_give(void) {
-    int rv;
+    int rv, _rv;
 
     if (!eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
     if (!eos_egpio_get_val(EGPIO_PIN_CTP_SEL)) return EOS_ERR;
@@ -94,22 +114,23 @@ int eos_ctp_give(void) {
     if (rv) return rv;
 
     rv = eos_egpio_set_val(EGPIO_PIN_CTP_SEL, 0);
-    if (rv) return rv;
+    if (rv) goto ctp_give_err;
 
     rv = eve_select();
-    if (rv) return rv;
+    if (rv) goto ctp_give_err;
 
     eve_touch_set_engine(EVE_TOUCH_ENGINE_GOODIX);
     eve_deselect();
 
-    rv = eos_egpio_intr_set();
-    if (rv) return rv;
+ctp_give_err:
+    _rv = eos_egpio_intr_set();
+    if (!rv && _rv) return _rv;
 
-    return EOS_OK;
+    return rv;
 }
 
 int eos_ctp_take(void) {
-    int rv;
+    int rv, _rv;
 
     if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
     if (eos_egpio_get_val(EGPIO_PIN_CTP_SEL)) return EOS_ERR;
@@ -118,20 +139,21 @@ int eos_ctp_take(void) {
     if (rv) return rv;
 
     rv = eve_select();
-    if (rv) return rv;
+    if (rv) goto ctp_take_err;
 
     eve_touch_set_engine(EVE_TOUCH_ENGINE_HOST);
     eve_deselect();
 
     rv = eos_egpio_set_val(EGPIO_PIN_CTP_SEL, 1);
-    if (rv) return rv;
+    if (rv) goto ctp_take_err;
 
     gt911_reset();
 
-    rv = eos_egpio_intr_set();
-    if (rv) return rv;
+ctp_take_err:
+    _rv = eos_egpio_intr_set();
+    if (!rv && _rv) return _rv;
 
-    return EOS_OK;
+    return rv;
 }
 
 int eos_ctp_handle_intr(void) {