1 files changed, 61 insertions, 23 deletions

diff --git a/fw/esp32/components/eos/cell.c b/fw/esp32/components/eos/cell.c
index d34172e..8665616 100644
--- a/fw/esp32/components/eos/cell.c
+++ b/fw/esp32/components/eos/cell.c
@@ -17,8 +17,6 @@
 
 static const char *TAG = "EOS CELL";
 
-static uint8_t cell_mode;
-
 static EOSBufQ cell_buf_q;
 static unsigned char *cell_bufq_array[CELL_SIZE_QUEUE];
 
@@ -28,8 +26,6 @@ static QueueHandle_t cell_queue;
 static void _cell_handler(unsigned char _mtype, unsigned char *buffer, uint16_t buf_len) {
     uint8_t mtype;
 
-    if (buf_len < 1) return;
-
     mtype = buffer[0];
     switch (mtype & EOS_CELL_MTYPE_MASK) {
         case EOS_CELL_MTYPE_DEV: {
@@ -43,29 +39,47 @@ static void _cell_handler(unsigned char _mtype, unsigned char *buffer, uint16_t
                 }
 
                 case EOS_CELL_MTYPE_RESET: {
+                    int rv;
+
+                    rv = eos_modem_take(1000);
+                    if (rv) {
+                        ESP_LOGE(TAG, "Reset modem failed: %d", rv);
+                        break;
+                    }
                     eos_modem_reset();
+                    eos_modem_give();
                     break;
                 }
 
                 case EOS_CELL_MTYPE_UART_TAKE: {
-                    uint8_t mode;
-
-                    mode = eos_modem_get_mode();
-                    if (mode == EOS_CELL_UART_MODE_NONE) break;
+                    int rv;
 
-                    eos_modem_set_mode(EOS_CELL_UART_MODE_RELAY);
-                    cell_mode = mode;
+                    if (eos_modem_get_mode() == EOS_CELL_UART_MODE_ATCMD) {
+                        rv = eos_modem_set_mode(EOS_CELL_UART_MODE_RELAY);
+                        if (rv) ESP_LOGE(TAG, "Set RELAY mode failed: %d", rv);
+                    }
                     break;
                 }
 
                 case EOS_CELL_MTYPE_UART_GIVE: {
-                    eos_modem_atinit();
-                    eos_modem_set_mode(cell_mode);
+                    int rv;
+
+                    if (eos_modem_get_mode() == EOS_CELL_UART_MODE_RELAY) {
+                        rv = eos_modem_take(1000);
+                        if (!rv) {
+                            eos_modem_atinit();
+                            eos_modem_give();
+                        }
+                        rv = eos_modem_set_mode(EOS_CELL_UART_MODE_ATCMD);
+                        if (rv) ESP_LOGE(TAG, "Set ATCMD mode failed: %d", rv);
+                    }
                     break;
                 }
 
                 case EOS_CELL_MTYPE_UART_DATA: {
-                    if (eos_modem_get_mode() == EOS_CELL_UART_MODE_RELAY) eos_modem_write(buffer + 1, buf_len - 1);
+                    if (eos_modem_get_mode() == EOS_CELL_UART_MODE_RELAY) {
+                        eos_modem_write(buffer + 1, buf_len - 1);
+                    }
                     break;
                 }
             }
@@ -108,23 +122,30 @@ static void cell_handler_task(void *pvParameters) {
     vTaskDelete(NULL);
 }
 
-static void cell_handler(unsigned char type, unsigned char *buffer, uint16_t len) {
+static void cell_handler(unsigned char type, unsigned char *buffer, uint16_t buf_len) {
     EOSMsgItem mi;
-    unsigned char *buf;
+    unsigned char *_buffer;
+
+    if (buf_len < 1) return;
+
+    if (type & EOS_NET_MTYPE_FLAG_REPL) {
+        _cell_handler(type, buffer, buf_len);
+        return;
+    }
 
     xSemaphoreTake(mutex, portMAX_DELAY);
-    buf = eos_bufq_pop(&cell_buf_q);
+    _buffer = eos_bufq_pop(&cell_buf_q);
     xSemaphoreGive(mutex);
 
-    if (buf == NULL) {
-        if (len) ESP_LOGE(TAG, "Cell message NOT handled: %2x", buffer[0]);
+    if (_buffer == NULL) {
+        ESP_LOGE(TAG, "Cell message NOT handled: %2x", buffer[0]);
         return;
     }
 
-    memcpy(buf, buffer, len);
+    memcpy(_buffer, buffer, buf_len);
     mi.type = type;
-    mi.buffer = buf;
-    mi.len = len;
+    mi.buffer = _buffer;
+    mi.len = buf_len;
     xQueueSend(cell_queue, &mi, portMAX_DELAY);
 }
 
@@ -133,13 +154,30 @@ void eos_cell_init(void) {
 
     eos_bufq_init(&cell_buf_q, cell_bufq_array, CELL_SIZE_QUEUE);
     for (i=0; i<CELL_SIZE_QUEUE; i++) {
-        eos_bufq_push(&cell_buf_q, malloc(EOS_NET_SIZE_BUF));
+        unsigned char *buffer;
+
+        buffer = malloc(EOS_NET_SIZE_BUF);
+        assert(buffer != NULL);
+        eos_bufq_push(&cell_buf_q, buffer);
     }
 
     mutex = xSemaphoreCreateBinary();
+    assert(mutex != NULL);
     xSemaphoreGive(mutex);
+
     cell_queue = xQueueCreate(CELL_SIZE_QUEUE, sizeof(EOSMsgItem));
-    xTaskCreate(cell_handler_task, "cell_handler", EOS_TASK_SSIZE_CELL, NULL, EOS_TASK_PRIORITY_CELL, NULL);
+    assert(cell_queue != NULL);
 
     eos_net_set_handler(EOS_NET_MTYPE_CELL, cell_handler);
+
+    ESP_LOGI(TAG, "INIT");
+}
+
+void eos_cell_run(void) {
+    BaseType_t rv;
+
+    rv = xTaskCreate(cell_handler_task, "cell_handler", EOS_TASK_SSIZE_CELL, NULL, EOS_TASK_PRIORITY_CELL, NULL);
+    assert(rv == pdPASS);
+
+    ESP_LOGI(TAG, "RUN");
 }