Trond Enger / d7a_1x

Fork of d7a_1x by WizziLab

Revision:
37:e04613e021f2
Parent:
36:67cfe82526e6
Child:
38:c1e7f97ab396
--- a/src/d7a_modem.cpp	Tue Aug 23 08:49:15 2016 +0000
+++ b/src/d7a_modem.cpp	Wed Aug 24 17:31:51 2016 +0000
@@ -12,6 +12,8 @@
     Queue<void, 8> ready;
     Queue<void, 2> boot;
     Queue<d7a_com_rx_msg_t, 16> pkt_queue;
+    Queue<register_file_param_t, 64> registered_files;
+    uint8_t nb_files;
     int8_t status;
     DigitalInOut* reset;
     bool booted;
@@ -27,6 +29,7 @@
     
     g_modem_ctx.thread = new Thread(d7a_modem_thread, NULL, osPriorityHigh, DEFAULT_STACK_SIZE);
     g_modem_ctx.booted = false;
+    g_modem_ctx.nb_files = 0;
     
     if (reset_pin == NC)
     {
@@ -36,8 +39,7 @@
     {
         g_modem_ctx.reset = new DigitalInOut(reset_pin, PIN_OUTPUT, OpenDrain, 1);
     }
-    
-    d7a_com_start();
+
     d7a_modem_reset();
     ASSERT(d7a_sys_ping_modem(30000), "Modem does not respond\r\n");
 }
@@ -61,12 +63,12 @@
 {
     FPRINT("\r\n");
     IPRINT("MODEM Soft Reset.\r\n");
+            
+    // Clean buffer and queues
+    d7a_com_restart();
     
     // Try software reset
     d7a_sys_software_reset();
-        
-    // Clean buffer and queues
-    d7a_com_restart();
 }
 
 static void d7a_modem_hard_reset(void)
@@ -97,17 +99,17 @@
     g_modem_ctx.booted = false;
 
     d7a_modem_soft_reset();
-    reset_ok = d7a_modem_wait_boot(5000);
+    reset_ok = d7a_modem_wait_boot(3000);
     
     if (!reset_ok)
     {
         d7a_modem_hard_reset();
-        reset_ok = d7a_modem_wait_boot(5000);
+        reset_ok = d7a_modem_wait_boot(3000);
     }
     ASSERT(reset_ok, "MODEM BOOT Timeout\r\n");
     g_modem_ctx.booted = true;
     
-    ASSERT(d7a_modem_wait_ready(5000), "MODEM READY Timeout\r\n");
+    ASSERT(d7a_modem_wait_ready(3000), "MODEM READY Timeout\r\n");
     IPRINT("MODEM Ready.\r\n");
 }
 
@@ -139,34 +141,32 @@
 
 bool d7a_modem_register_file(uint8_t fid, uint8_t retry_policy)
 {
-    //g_modem_ctx.ressource->wait();
-    
     bool ret = true;
     
     FPRINT("(fid:%d, retry_policy:0x%02X)\r\n", fid, retry_policy);
-    register_file_param_t rparams = {
-        .bf.fid = fid ,
-        .bf.retry_policy = retry_policy
-    };
+    register_file_param_t* rparams = (register_file_param_t*)MALLOC(sizeof(register_file_param_t));
+    
+    rparams->bf.fid = fid;
+    rparams->bf.retry_policy = retry_policy;
 
     // Check If modem already has it
-    DPRINT("Register: %d\r\n", rparams.bf.fid);
+    PRINT("Register: %d\r\n", rparams->bf.fid);
     
-    g_modem_ctx.status = d7a_fs_distant_stat(rparams.bf.fid, NULL);
+    g_modem_ctx.status = d7a_fs_distant_stat(rparams->bf.fid, NULL);
     if (g_modem_ctx.status < 0)
     {
         d7a_fs_properties_t props;
         // retrieve local properties
-        d7a_fs_get_properties(rparams.bf.fid, KAL_FS_PROP_ALL, &props);
+        d7a_fs_get_properties(rparams->bf.fid, KAL_FS_PROP_ALL, &props);
         // create a remote Host file matching our local one
         props.type = HOST;
-        g_modem_ctx.status = d7a_fs_distant_create(rparams.bf.fid, &props);
+        g_modem_ctx.status = d7a_fs_distant_create(rparams->bf.fid, &props);
     }
     
     if (g_modem_ctx.status == 0)
     {
         // Distant file is ready, lets register a D7A Interface for it
-        d7a_modem_msg(WM_CMD_REGISTER_FILE,(uint8_t*)&rparams,sizeof(register_file_param_t));
+        d7a_modem_msg(WM_CMD_REGISTER_FILE,(uint8_t*)rparams,sizeof(register_file_param_t));
         ret = d7a_modem_wait_ready(TO_FS);
     }
     else
@@ -176,6 +176,10 @@
     
     //Thread::wait(100); // Let time to the modem to write to the flash
     
+    // save registered file
+    g_modem_ctx.registered_files.put(rparams);
+    g_modem_ctx.nb_files++;
+    
     DPRINT("Register done.\r\n");
     //g_modem_ctx.ressource->release();
     
@@ -184,8 +188,6 @@
 
 void d7a_modem_notify_file(uint8_t fid, uint32_t offset, uint32_t length)
 {
-    //g_modem_ctx.ressource->wait();
-    
     FPRINT("(fid:%d, offset:%d, length:%d)\r\n", fid, offset, length);
     DPRINT("Notify %d.\r\n", fid);
     
@@ -196,7 +198,7 @@
     };
 
     d7a_modem_msg(WM_CMD_NOTIFY_FILE,(uint8_t*)&nfp,sizeof(notify_file_param_t));
-    ASSERT(d7a_modem_wait_ready(TO_FS), "Notification Timeout on FID:%d\r\n", fid);
+    WARNING(d7a_modem_wait_ready(TO_FS), "Notification Timeout on FID:%d\r\n", fid);
     
     //Thread::wait(100); // XXX: To avoid lost packets
     
@@ -227,6 +229,31 @@
 }
 
 
+void d7a_modem_recover_thread(const void *p)
+{    
+    WARNING(false, "Modem rebooting, %d files to restore\r\n", g_modem_ctx.nb_files);
+    // Reset modem
+    ASSERT(d7a_modem_wait_ready(3000), "MODEM READY Timeout\r\n");
+        
+    d7a_com_start_tx();
+
+    uint8_t nb = g_modem_ctx.nb_files;
+    // register files again
+    for (int i = 0; i < nb; i++)
+    {
+        osEvent evt = g_modem_ctx.registered_files.get(1000);
+        register_file_param_t* file = (evt.status == osEventMessage)? (register_file_param_t*)evt.value.p: NULL;
+        
+        ASSERT(file != NULL, "NULL file while recovering\r\n");
+        
+        d7a_modem_register_file(file->bf.fid, file->bf.retry_policy);
+    }
+    
+    g_modem_ctx.nb_files = nb;
+    IPRINT("Modem reboot OK\r\n");
+}
+
+
 void d7a_modem_thread(const void *p)
 {
     FPRINT("\r\n");
@@ -248,9 +275,17 @@
                 }
                 else if (cmd == WM_CMD_BOOT)
                 {
-                    ASSERT(g_modem_ctx.booted == false, "Modem rebooted.\r\n");
-                    DPRINT("Modem booted CAUSE:%d NB_BOOT:%d\r\n", pkt->buffer[1], ((uint16_t)pkt->buffer[4])<<8 & pkt->buffer[3]);
-                    g_modem_ctx.boot.put(NULL);
+                    WARNING(false, "Modem booted CAUSE:%d NB_BOOT:%d\r\n", pkt->buffer[1], ((uint16_t)pkt->buffer[4])<<8 & pkt->buffer[3]);
+                    
+                    if (g_modem_ctx.booted == true)
+                    {
+                        d7a_com_stop_tx();
+                        Thread* reset_th = new Thread(d7a_modem_recover_thread, NULL, osPriorityRealtime);
+                    }
+                    else
+                    {
+                        g_modem_ctx.boot.put(NULL);
+                    }
                 }
                 else
                 {
@@ -265,3 +300,6 @@
         FREE(pkt);
     }
 }
+
+
+