Revision 6:748062f3de21, committed 2016-03-23

Comitter:

TNU

Date:

Wed Mar 23 15:24:50 2016 +0000

Parent:

5:87e211a23654

Child:

7:c47612b25c77

Commit message:

Patch load: Fragments load now

Changed in this revision

--- a/LidarSpi.cpp	Fri Mar 04 14:17:32 2016 +0000
+++ b/LidarSpi.cpp	Wed Mar 23 15:24:50 2016 +0000
@@ -5,7 +5,9 @@
 #include <string.h>
 #include "LidarSpi.h"
 #include "MLX_BaseSPI.h"
+#include <inttypes.h>
 #define SPIFREQ 8000
+
 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr, PinName smpl):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr), sampling(smpl)
 {
     //resetPin.write(1);
@@ -399,7 +401,7 @@
     
     res = ReadReg(470, &val);     // PORT_READY 
     val=val>>16;
-    pc->printf("PORT READY: %x\n\r",val);
+    //pc->printf("PORT READY: %x\n\r",val);
     trigger.write(0);
     // Encode the request and send it
     res = MLX_ReqReadEch(tx);
@@ -1098,8 +1100,8 @@
 }
 
 
-int LidarSpi::LoadPatch ( uint16_t address, uint8_t  *buf, uint16_t nBytes){
-    int res;
+int LidarSpi::LoadPatch (const char *patch, Serial * pc){
+    int res=0;
     uint16_t rSz;
     PACK_SHORT rx[1], tx[1];
     PACK_LONG2 rxL, txL;
@@ -1108,80 +1110,204 @@
     //uint32_t val = 0;
     uint16_t nWordsRemaining;
     uint16_t  nPack;
-    uint16_t nWords = nBytes / 2;
-
-
-    // Ensure all packets are all zeros to not send trash
-    memset(tx, 0, sizeof(tx));
-    memset(&rxL, 0, sizeof(rxL));
-
-    //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
-    //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
-    nPack = nWords / (MLX_LONG2_DATA_SZ / 2)  + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0  :1);
-
-    // Encode the request and send it
-    res = MLX_ReqWriteFW(tx, nPack, address);
-    if (res < 0)
-        goto END;
+    uint16_t nWords;
+    
+    uint8_t memory[512];
+    uint16_t addrStart, startLine, nBytes ;
+    startLine=0;
+    memset(memory, 0, 512);
+    
 
-    res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
-    if (res < 0)
+    res=WriteReg(0x1d8, 128);
+    if (res<0){
+        pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
         goto END;
-
-    // Optional status decoding
-    res = MLX_DecodeStatusS(rx, &iserr, &err);
-    if (res < 0 || iserr)
-        goto END;
+    }
+    
+    wait_us(5);
+    pc->printf("Start loading fragments \n\r");
+    int count=0;
+    while(LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc)==-5){
+        Trigger(0);
+        pc->printf("Load fragment %d\n\r", count);
+        for(int i =0; i<nBytes;i++){
+            //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );    
+        }
+        pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
+        nWords= nBytes / 2;
+        
+        // Ensure all packets are all zeros to not send trash
+        memset(tx, 0, sizeof(tx));
+        memset(&rxL, 0, sizeof(rxL));
+    
+        //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
+        //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
+        nPack = nWords / (MLX_LONG2_DATA_SZ / 2)  + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0  :1);
+        pc->printf("npack: %d\n\r", nPack);
+        // Encode the request and send it
+        res = MLX_ReqWriteFW(tx, nPack, addrStart);
+        if (res < 0){
+            pc->printf("Err @ 1");
+            goto END;   }
+            
+        wait_us(16);
+        res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
+        if (res < 0){
+            pc->printf("Err @ 2");
+            goto END;   }
+        //Trigger(1);
+             /*   
+        pc->printf("ReqWriteFirmware MOSI: \n\r0x");
+        for(int k=0;k<(sizeof(PACK_LONG2));k++){
+            uint8_t* pnt=(uint8_t*)(&txL);
+            pc->printf("%02X", *(pnt+k));   
+            if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+        }
+        pc->printf("\n\r");
+        pc->printf("ReqWriteFirmware MISO: \n\r0x");
+        for(int k=0;k<(sizeof(PACK_LONG2));k++){
+            uint8_t* pnt=(uint8_t*)(&rxL);
+            pc->printf("%02X", *(pnt+k));   
+            if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+        }
+        pc->printf("\n\r-------------------\n\r");
+        */
+        
+        
+        wait_us(5);
+    
+        // Optional status decoding
+        res = MLX_DecodeStatusS(rx, &iserr, &err);
+        if (res < 0 || iserr){
+            pc->printf("Err @ 3");
+            
+            pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
+                for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                    uint8_t* pnt=(uint8_t*)(&rxL);
+                    pc->printf("%02X", *(pnt+k));   
+                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+                }
+            pc->printf("\n\r");
+            
+            
+            goto END;   }
+    
+        nWordsRemaining = nWords;
+        for (uint a = 0; a < nPack; ++a)
+        {
+            uint16_t size; 
+            if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
+                size = MLX_LONG2_DATA_SZ / 2;
+            else
+                size = nWordsRemaining;
 
-    nWordsRemaining = nWords;
-    for (uint a = 0; a < nPack; ++a)
-    {
-        uint size; 
-        if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
-            size = MLX_LONG2_DATA_SZ / 2;
-        else
-            size = nWordsRemaining;
+            res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
+            if (res < 0){
+                res = -7;
+                goto END;
+            }
+
+            res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+
+            //Trigger(0);
+            wait_us(8);
 
-        res = MLX_WriteDataL2(&txL, size, a, &buf[a*MLX_LONG2_DATA_SZ]);
-        if (res < 0){
+            /*    
+            pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d    MOSI: \n\r0x", a,count);
+            for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                uint8_t* pnt=(uint8_t*)(&txL);
+                pc->printf("%02X", *(pnt+k));   
+                if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+            }
+            pc->printf("\n\r");
+            pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d   MISO: \n\r0x", a,count);
+            for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                uint8_t* pnt=(uint8_t*)(&rxL);
+                pc->printf("%02X", *(pnt+k));   
+                if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+            }
+            pc->printf("\n\r-----------\n\r");
+            */
+            
+            
+            res = MLX_DecodeResL2(&rxL);
+            if (res < 0){
+                pc->printf("LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
+                
+                pc->printf("LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a);
+                for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                    uint8_t* pnt=(uint8_t*)(&txL);
+                    pc->printf("%02X", *(pnt+k));   
+                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+                }
+                pc->printf("\n\r");
+                pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
+                for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                    uint8_t* pnt=(uint8_t*)(&rxL);
+                    pc->printf("%02X", *(pnt+k));   
+                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+                }
+                pc->printf("\n\r");
+                res = -9;
+                goto END;
+            }
+    
+            nWordsRemaining = nWords - size;
+            
+        }
+        count++;
+        //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
+        res = MLX_EncodeStatusL2(&txL, 0, 0);
+        if (res < 0) {
             res = -7;
             goto END;
         }
-
-        //Tools::Wait(10);
-
-        // Clock the remaining long packets 
+    
+        // Clock the remaining long packets
         res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
-
-        // Decode the long responses
-        res = MLX_DecodeResL2(&rxL);
+        wait_us(11);
         if (res < 0){
-            printf("LONG WRITE ERROR: Stopped at the %d long message\n", a);
-            res = -9;
-            goto END;
-        }
-
-        nWordsRemaining = nWords - size;
+            pc->printf("Err @ 4");
+            goto END;   }
+        
+        
+        
+        
+        memset(memory, 0, 512);
+        wait_us(12);
+    }
+    /*
+    pc->printf("Status long packet to check status after last fragment : %d, res=%d\n", count, res);
+                
+    pc->printf("LOAD PATCH LONG STATUS %d MOSI: \n\r0x", count);
+    for(int k=0;k<(sizeof(PACK_LONG2));k++){
+        uint8_t* pnt=(uint8_t*)(&txL);
+        pc->printf("%02X", *(pnt+k));   
+        if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
     }
-
-    //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
-    res = MLX_EncodeStatusL2(&txL, 0, 0);
-    if (res < 0) {
-        res = -7;
-        goto END;
+    pc->printf("\n\r");
+    pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", count);
+    for(int k=0;k<(sizeof(PACK_LONG2));k++){
+        uint8_t* pnt=(uint8_t*)(&rxL);
+        pc->printf("%02X", *(pnt+k));   
+        if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
     }
-
-    //Tools::Wait(10);
-    // Clock the remaining long packets
-    res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
-
-    if (res < 0)
-        goto END;
-
+    pc->printf("\n\r-----------\n\r");
+*/
     // Change jump table pointer.
-    res = WriteReg(0x1000, 0x7000);     // write addr: 0x1000 value:0x7000
-    if (res < 0)
-        goto END;
+    //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
+    //pc->printf("Return from a loadPatchFragment: res = %d \n\r", res);
+    wait_ms(100);
+    
+    if (count>3){
+        pc->printf("Change jumptable... \n\r");
+        res = WriteReg(0x1000, 0x7000, pc);     // write addr: 0x1000 value:0x7000
+        if (res < 0){
+            pc->printf("Err @ 5");
+            goto END;   
+        }
+    }
+    else pc->printf("Failed to read the file\n\r");
     
 END:
     return res;     
@@ -1190,3 +1316,123 @@
 void LidarSpi::Trigger(int level){
     trigger.write(level);    
 }
+
+
+int LidarSpi::LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc){
+    char line[100];
+    FILE *fin;
+    uint16_t addr, n, status;
+    uint8_t bytes[256];
+    uint16_t i, total = 0, lineno = 1;
+    uint16_t minaddr = 65535, maxaddr = 0;
+    
+    uint16_t prevAddr = 0;
+    uint16_t lineCount = *startLine;
+    uint16_t lines = 0;
+    
+    bool isFirst = true;
+    
+    *nBytes = 0;
+    if (strlen(patch) == 0) {
+        //printf("   Can't load a file without the filename.");
+        return -9;
+    }
+    fin = fopen(patch, "r");
+    if (fin == NULL) {
+        pc->printf("   Can't open file '%s' for reading.\n\r", patch);
+        return -10;
+    }
+    //  printf("Patch file %s opened\n", filename);
+    
+    while (true) {
+        line[0] = '\0';
+        do {
+            fgets(line, 1000, fin);
+            lines++;
+        } while (lines < (*startLine));
+        //pc->printf("Startline: %d, lines: %d\n\r", *startLine,lines);
+
+        if (line[strlen(line) - 1] == '\n') line[strlen(line) - 1] = '\0';
+        if (line[strlen(line) - 1] == '\r') line[strlen(line) - 1] = '\0';
+        *addrStart= 0;
+        if (parse_hex_line(line, bytes, &addr, &n, &status)) {
+            //cout << "Parse OK\n\r";
+            //pc->printf("Line: %s\n\r", line);
+            if (!(isFirst | ((addr == prevAddr + 1) && (((*nBytes)+n) < 512)))) {
+                *addrStart = minaddr;
+                *startLine = lines;
+                fclose(fin);
+                return -5;
+            }
+            
+            if (status == 0) {  /* data */
+                                //cout << "Status=0";
+                isFirst = false;
+                for (i = 0; i <= (n - 1); i++) {
+                    if (i % 2 == 0)
+                        memory[*nBytes+i] = bytes[i + 1] & 255;  //Assumption even no of bytes per line
+                    else
+                        memory[*nBytes+i] = bytes[i - 1] & 255;
+                    total++;
+                    if (addr < minaddr) minaddr = addr;
+                    if (addr > maxaddr) maxaddr = addr;
+                    prevAddr = addr;
+                    addr++;
+                }
+            }
+            if (status == 1) {  /* end of file */
+                fclose(fin);
+                //printf("load_file parsed %d bytes between:", total);
+                //printf(" %04X to %04X\n", minaddr, maxaddr);
+                *addrStart= minaddr;
+                pc->printf("End of file\n\r");
+                return total;
+            }
+            if (status == 2)  /* begin of file */
+                goto NEXT;
+            *nBytes += n;
+        }
+        else {
+            printf("   Error: '%s', line: %d\n", patch, lineno);
+        }
+        lineCount++;
+    NEXT:   lineno++;
+    }
+    fclose(fin);
+    return 0;
+    
+}
+int LidarSpi::parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code){
+    unsigned int sum, len, cksum;
+    unsigned int newAddr, newCode, newByte;
+    
+    
+    char *ptr;
+    //cout << "Start parsing\n\r";
+    *num = 0;
+    if (theline[0] != ':') return 0;
+    if (strlen(theline) < 11) return 0;
+    ptr = theline + 1;
+    if (!sscanf(ptr, "%02x", &len)) return 0;
+    ptr += 2;
+    if (strlen(theline) < (11 + (len * 2))) return 0;
+    if (!sscanf(ptr, "%04x", &newAddr)) return 0;
+    *addr=(uint16_t)newAddr;
+    ptr += 4;
+    //printf("Line: length=%d Addr=%d\n", len, *addr);
+    if (!sscanf(ptr, "%02x", &newCode)) return 0;
+    *code=(uint16_t)newCode;
+    ptr += 2;
+    sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
+    while (*num != len) {
+        if (!sscanf(ptr, "%02x", &newByte )) return 0;
+        bytes[*num]=(uint8_t)newByte;
+        ptr += 2;
+        sum += bytes[*num] & 255;
+        (*num)++;
+        if (*num >= 256) return 0;
+    }
+    if (!sscanf(ptr, "%02x", &cksum)) return 0;
+    if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */
+    return 1;
+}

--- a/LidarSpi.h	Fri Mar 04 14:17:32 2016 +0000
+++ b/LidarSpi.h	Wed Mar 23 15:24:50 2016 +0000
@@ -6,6 +6,7 @@
 #include "FunctionPointer.h"
 
 
+
 class LidarSpi
 {
     public:
@@ -173,7 +174,8 @@
         int GetEchoes ( Echo *ech, uint16_t maxN, uint16_t mode, Serial* pc);
         int GetTrace  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc);
         int GetTraceOne  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc);
-        int LoadPatch ( uint16_t address, uint8_t  *buf, uint16_t nBytes);
+        int LoadPatch (const char *patch, Serial *pc);
+        
         int PrintAllReg (uint16_t * regs, uint32_t * val, uint16_t size);
         
         void Trigger(int level);
@@ -193,7 +195,8 @@
         DigitalOut trigger;
         DigitalIn sampling;
         
-
+        int parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code);
+        int LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc);
 
           
 };