Teseo-LIV3F.cpp

00001 /*
00002  * -------------------------------------------------------------------------
00003  * Copyright (C) 2017  STMicroelectronics
00004  * Author: Francesco M. Virlinzi  <francesco.virlinzi@st.com>
00005  *
00006  * May be copied or modified under the terms of the GNU General Public
00007  * License V.2 ONLY.  See linux/COPYING for more information.
00008  *
00009  * -------------------------------------------------------------------------
00010  */
00011 #include "string.h"
00012 #include "Teseo-LIV3F.h"
00013 
00014 
00015 static char T3_name[] = "Teseo-LIV3F";
00016 
00017 char _OK[] = "OK";
00018 char _Failed[] = "Failed";
00019 char X_Nucleo_name[] = "X-Nucleo-GNSS1A1";
00020 
00021 struct teseo_cmd {
00022     char *cmd;
00023 };
00024 
00025 static struct teseo_cmd teseo_cmds[] = {
00026     [Teseo_LIV3F::GETSWVER] = {
00027             .cmd = "$PSTMGETSWVER,7",
00028     },
00029     [Teseo_LIV3F::FORCESTANDBY] = {
00030         .cmd = "$PSTMFORCESTANDBY,00007",
00031     },
00032     [Teseo_LIV3F::RFTESTON] = {
00033         .cmd  = "$PSTMRFTESTON,16",
00034     },
00035     [Teseo_LIV3F::RFTESTOFF] = {
00036         .cmd  = "$PSTMRFTESTOFF\n\r",
00037     },
00038     [Teseo_LIV3F::LOWPOWER] = {
00039         .cmd  = "$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01",
00040     },
00041     [Teseo_LIV3F::FWUPDATE] = {
00042         .cmd  = "$PSTMFWUPGRADE",
00043     },
00044     [Teseo_LIV3F::ENABLEALL] = {
00045         .cmd  = "$PSTMSETPAR,201,0xBFBBFFDF",
00046     },
00047     [Teseo_LIV3F::ENABLEALL2] = {
00048         .cmd  = "$PSTMSETPAR,228,0x3397",
00049     },
00050     [Teseo_LIV3F::SAVE] = {
00051         .cmd  = "$PSTMSAVEPAR",
00052     },
00053     [Teseo_LIV3F::REBOOT] = {
00054         .cmd  = "$PSTMSRR",
00055     },
00056 };
00057 
00058 
00059 Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin,
00060         PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin,
00061         Serial *serial_debug):
00062         _reset(reset_pin, 1),
00063         _pps(pps_pin),
00064         _wakeup(wakeup_pin, 0),
00065         _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD),
00066         _serial_debug(serial_debug)
00067 {
00068         wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
00069         _uart.baud(SDT_UART_BAUD);
00070         _uart.format(8, SerialBase::None, 1);
00071         _i2c = NULL;
00072         _uart_interleaded = false;
00073         _uart_discard = false;
00074 }
00075 
00076 Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin,
00077         PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin,
00078         I2C *bus, Serial *serial_debug):
00079         _reset(reset_pin, 1),
00080         _pps(pps_pin),
00081         _wakeup(wakeup_pin, 0),
00082         _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD),
00083         _i2c(bus),
00084         _serial_debug(serial_debug)
00085 {
00086     wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS);
00087     _uart.baud(SDT_UART_BAUD);
00088     _uart.format(8, SerialBase::None, 1);
00089     _uart_interleaded = false;
00090     _uart_discard = false;
00091 }
00092 
00093 int Teseo_LIV3F::EnableLowPower()
00094 {
00095     SendCommand(LOWPOWER);
00096     return 0;
00097 }
00098 
00099 void Teseo_LIV3F::Reset(Serial *serial_debug)
00100 {
00101     if (serial_debug)
00102         serial_debug->printf("%s: Resetting...", T3_name);
00103 
00104     _reset.write(0);
00105 
00106     wait_ms(50);
00107 
00108     _reset.write(1);
00109 
00110     wait_ms(70);
00111 
00112     if (serial_debug)
00113             serial_debug->printf("Done...\n\r");
00114 }
00115 
00116 enum {
00117         TESEO_FLASHER_IDENTIFIER = 0,   //  0xBCD501F4
00118         TESEO_FLASHER_SYNC,             //  0x83984073
00119         DEVICE_START_COMMUNICATION,     //  0xA3
00120         FLASHER_READY,                  //  0x4A
00121         ACK,                            //  0xCC
00122         NACK,
00123 };
00124 
00125 struct firmware_ctrl {
00126     char *cmd;
00127     unsigned char len;
00128     char *n;
00129 } ;
00130 
00131 /*
00132  * #define FWUPG_IDENTIFIER          0xBC D5 01 F4
00133  * #define FWUPG_SYNC                0x83 98 40 73
00134  */
00135 static struct firmware_ctrl fw_data[] = {
00136     [TESEO_FLASHER_IDENTIFIER] = {
00137             .cmd = (char *)(char[]){ 0xF4, 0x01, 0xD5, 0xBC},
00138             .len = 4,
00139             .n = "TESEO_FLASHER_IDENTIFIER",
00140     },
00141     [TESEO_FLASHER_SYNC] = {
00142             .cmd =(char *)(char[]){ 0x73, 0x40, 0x98, 0x83 },
00143             .len = 4,
00144             .n = "TESEO_FLASHER_SYNC",
00145     },
00146     [DEVICE_START_COMMUNICATION] = {
00147             .cmd = (char *)(char[]){0xA3},
00148             .len = 1,
00149             .n = "DEVICE_START_COMMUNICATION",
00150     },
00151     [FLASHER_READY] = {
00152             .cmd = (char *)(char[]){0x4A},
00153             .len = 1,
00154             .n = "FLASHER_READY",
00155     },
00156     [ACK]  = {
00157             .cmd = (char *)(char[]){0xCC},
00158             .len = 1,
00159             .n = "ACK",
00160     },
00161     [NACK]  = {
00162             .cmd = (char *)(char[]){0xDD},
00163             .len = 1,
00164             .n = "NACK",
00165         },
00166 };
00167 
00168 int Teseo_LIV3F::SendString(char *buf, int len)
00169 {
00170     for (int i = 0; i < len; ++i) {
00171         while (!_uart.writeable());
00172         printf ("%c",buf[i]);
00173         _uart.putc(buf[i]);
00174         }
00175 }
00176 
00177 struct ImageOptions
00178 {
00179     unsigned char eraseNVM;
00180     unsigned char programOnly;
00181     unsigned char reserved;
00182     unsigned char baudRate;
00183     unsigned int firmwareSize;
00184     unsigned int firmwareCRC;
00185     unsigned int nvmAddressOffset;
00186     unsigned int nvmSize;
00187 } liv3f_img_option = {
00188         .eraseNVM       = 1,
00189         .programOnly    = 0,
00190         .reserved       = 0,
00191         .baudRate       = 1,
00192         .firmwareSize   = 0,
00193         .firmwareCRC    = 0,
00194         .nvmAddressOffset = 0x00100000,
00195         .nvmSize          = 0x00100000,
00196 
00197 };
00198 
00199 int Teseo_LIV3F::FwWaitAck()
00200 {
00201     while (!_uart.readable());
00202 
00203     char c = _uart.getc();
00204 
00205     if (fw_data[ACK].cmd[0] == c) {
00206         if (_serial_debug)
00207             _serial_debug->printf("%s (0x%x)\n\r", _OK,  c);
00208         return 0;
00209     }
00210 
00211     if (fw_data[NACK].cmd[0] == c) {
00212         if (_serial_debug)
00213             _serial_debug->printf("%s (%x)\n\r", _Failed, c);
00214         return -1;
00215     }
00216 
00217     if (_serial_debug)
00218         _serial_debug->printf("%s - Char not allowed (%x)\n\r", _Failed, c);
00219     return -1;
00220 }
00221 
00222 bool Teseo_LIV3F::FirmwareUpdate(bool is_recovery, char *data,
00223         unsigned int data_len,
00224         unsigned long crc,
00225         Serial *serial_debug)
00226 {
00227     unsigned int i;
00228 
00229     char _buf[4] = { 0xff, 0xff, 0xff, 0xff };
00230 
00231     liv3f_img_option.firmwareSize   = data_len;
00232     liv3f_img_option.firmwareCRC    = crc;
00233 
00234     if (data == NULL || !data_len)
00235         return false;
00236 
00237     if (is_recovery)
00238         Reset();
00239 
00240     {
00241 
00242         _uart.baud(FWU_UART_BAUD);
00243 
00244 #if 1
00245         while (1) {
00246             /* send TESEO_FLASHER_IDENTIFIER */
00247 /*
00248  * Device is under reset. Host sends continuously “TESEO2_FLASHER_IDENTIFIER� word.
00249  */
00250             SendString(fw_data[TESEO_FLASHER_IDENTIFIER].cmd, fw_data[TESEO_FLASHER_IDENTIFIER].len);
00251 
00252             /* try to read... TESEO_FLASHER_SYNC */
00253             if (_uart.readable())
00254                     for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len; ) {
00255 
00256                         while (!_uart.readable());
00257 
00258                         _buf[i] = _uart.getc();
00259 
00260                         if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == _buf[i]) {
00261                                 if (serial_debug)
00262                                     serial_debug->printf("-- %d -- 0x%x -- ok--\n\r", i, _buf[i]);
00263                                 i++;
00264                                 goto exit_step_1; /* FMV: WA to have firmware update working.... */
00265                             } else {
00266                                 i = 0;
00267                             }
00268                     }
00269                     if (i == fw_data[TESEO_FLASHER_SYNC].len)
00270                         goto exit_step_1;
00271             }
00272 
00273 exit_step_1:
00274 
00275     _uart.abort_read();
00276 
00277     if (serial_debug)
00278         serial_debug->printf("Got: %s from %s\n\r", fw_data[TESEO_FLASHER_SYNC].n, T3_name);
00279 
00280 /*
00281  * Host sends “DEVICE_START_COMMUNICATION� word.
00282  */
00283     serial_debug->printf("\n\r%s Step: %s ",T3_name, fw_data[DEVICE_START_COMMUNICATION].n);
00284     SendString(fw_data[DEVICE_START_COMMUNICATION].cmd, fw_data[DEVICE_START_COMMUNICATION].len);
00285 
00286     FwWaitAck();
00287 
00288 /*
00289  * Host sends the binary image options. Both host and
00290  * device change UART baud rates. Host sends continuously
00291  * the “FLASHER_READY� word.
00292  */
00293     if (serial_debug)
00294         serial_debug->printf("%s Step: Send ImageOption\n\r",T3_name);
00295 
00296     SendString((char*)&liv3f_img_option, sizeof(ImageOptions));
00297 
00298     if (serial_debug)
00299             serial_debug->printf("%s Step: Send %s\n\r",T3_name, fw_data[FLASHER_READY].n);
00300 
00301     while (1) {
00302         SendString(fw_data[FLASHER_READY].cmd, fw_data[FLASHER_READY].len);
00303         if (_uart.readable())
00304             goto exit_step_3;
00305     }
00306 
00307     exit_step_3:
00308     FwWaitAck();
00309 
00310     if (serial_debug)
00311         serial_debug->printf("%s Step: Erasing flash area ",T3_name);
00312 
00313 
00314     FwWaitAck();
00315 
00316 /*
00317  * Device is erasing flash program. Host is waiting for an “ACK�.
00318  */
00319 
00320     if (serial_debug)
00321         serial_debug->printf("%s Step: Erasing NVM ",T3_name);
00322 
00323     while (!_uart.readable());
00324 
00325     FwWaitAck();
00326 
00327     if (serial_debug)
00328         serial_debug->printf("%s Step: Sending data... ",T3_name);
00329 
00330     for (i = 0; i < (data_len / (16*1024)); ++i) {
00331         SendString(&data[i], 16*1024);
00332 
00333         FwWaitAck();
00334     }
00335 
00336     serial_debug->printf("\n\r");
00337     /*
00338      * send remaining data...
00339      */
00340     if (data_len != (i * 16*1024)) {
00341         SendString(&data[i*16*1024], data_len-i*16*1024);
00342         FwWaitAck();
00343         }
00344     /*
00345      * wait CRC ack
00346      */
00347     FwWaitAck();
00348     }
00349 
00350 #else
00351     //_uart.format(8, SerialBase::Forced0, 1);
00352     while (1)
00353             {
00354                 /* send TESEO_FLASHER_IDENTIFIER */
00355                 for (i = 0; i < fw_data[TESEO_FLASHER_IDENTIFIER].len; ++i) {
00356                      while (!_uart.writeable());
00357                     _uart.putc(fw_data[TESEO_FLASHER_IDENTIFIER].cmd[i]);
00358                 }
00359 
00360 
00361                 /* try to read... TESEO_FLASHER_SYNC */
00362                 //while (!_uart.readable());
00363                 for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len && _uart.readable(); )
00364                     if (_uart.readable())
00365                     {
00366                         char c = _uart.getc();
00367     /*
00368                         if (!c)
00369                                 break;
00370     */
00371                         if (serial_debug)
00372                             serial_debug->printf("%x vs %x\n\r", c, fw_data[TESEO_FLASHER_SYNC].cmd[i]);
00373 
00374                         if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == c)
00375                             i++;
00376                         else
00377                             i = 0;
00378                     }
00379 
00380                 if (i == fw_data[TESEO_FLASHER_SYNC].len && serial_debug)
00381                     serial_debug->printf("Got %s from %s\n\r",fw_data[TESEO_FLASHER_SYNC].n, T3_name);
00382             }
00383         }
00384 #endif
00385     if (serial_debug)
00386         serial_debug->printf("END\n\r");
00387 
00388     return true;
00389 }
00390 
00391 int Teseo_LIV3F::WakeUp()
00392 {
00393     wait_ms(100);
00394 
00395     _wakeup.write(1);
00396 
00397     wait_ms(500);
00398 
00399     _wakeup.write(0);
00400 
00401     return 0;
00402 }
00403 
00404 bool Teseo_LIV3F::CheckPPSWorking()
00405 {
00406     int val_0, val_1;
00407 
00408     wait_ms(500);
00409 
00410     val_0 = _pps.read();
00411 
00412     wait_ms(500);
00413     val_1 = _pps.read();
00414 
00415     if (val_0 != val_1)
00416         return true;
00417 
00418     return false;
00419 }
00420 
00421 int Teseo_LIV3F::CRC_(char *buf, int size)
00422 {
00423     int i = 0, ch = 0;
00424 
00425     if (buf[0] == '$')
00426         ++i;
00427 
00428     if (size)
00429         for (; i < size; ++i)
00430             ch ^= buf[i];
00431     else
00432         for (; buf[i] != 0; ++i)
00433             ch ^= buf[i];
00434 
00435     return ch;
00436 }
00437 
00438 bool Teseo_LIV3F::WaitBooting(Timer *t, float timeout)
00439 {
00440     unsigned int now = t->read_ms();;
00441     while (1) {
00442         if (CheckPPSWorking() == true)
00443             return true;
00444 
00445     if ((now + timeout*1000) < t->read_ms())
00446             break;
00447     }
00448 
00449     return false;
00450 }
00451 
00452 void Teseo_LIV3F::SendCommand(enum Teseo_LIV3F::cmd_enum c)
00453 {
00454     char crc[5];
00455 
00456     sprintf(crc, "*%02X\n\r", CRC_(teseo_cmds[c].cmd, 0));
00457     //printf ("%s",crc);
00458     _uart_mutex_lock();
00459     _uart_interleaded = true;
00460     SendString(teseo_cmds[c].cmd, strlen(teseo_cmds[c].cmd));
00461     SendString(crc, 5);
00462     _uart_mutex_unlock();
00463 }
00464 
00465 char *Teseo_LIV3F::DetectSentence(const char *cmd, char *buf, unsigned long len)
00466 {
00467     char *result = NULL;
00468     unsigned int i = 0;
00469     const unsigned long cmd_len = strlen(cmd);
00470     len -= strlen(cmd);
00471 
00472     while (!result && i < len) {
00473         for (; buf[i] != '$' && i < len; ++i); /* 1. check '$' char */
00474         if (i == len)
00475             break; /* no more char.... */
00476 
00477         ++i; /* to point to the char after '$' */
00478 
00479         if (strncmp(&buf[i], cmd, cmd_len) == 0) {
00480             result = &buf[i];
00481         }
00482     }
00483 
00484     if (result) {
00485         for (i = 0; result[i] != '*'; ++i);
00486         result[i] = 0;
00487     }
00488 #if 0
00489     if (_serial_debug)
00490             _serial_debug->printf("%s: %s: %s %s FOUND\n\r", T3_name, __FUNCTION__, cmd, result ? " " : "NOT");
00491 #endif
00492     return result;
00493 }
00494 
00495 
00496 int Teseo_LIV3F::CheckI2C()
00497 {
00498 
00499     if (!_i2c)
00500         return -1;
00501 
00502     _i2c->start();
00503     int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1);
00504     _i2c->stop();
00505     /*
00506     *  @returns
00507     *    '0' - NAK was received
00508     *    '1' - ACK was received,
00509     *    '2' - timeout
00510     */
00511     return res == 1 ? 0 : -1;
00512 }
00513 
00514  int Teseo_LIV3F::ReadMessage(char *buf, unsigned long len, Timer *t, float timeout)
00515 {
00516     memset(buf, 0, len);
00517 
00518     for (unsigned int i = 0; i < len; ++i){
00519         if (t) {
00520             unsigned int now = t->read_ms();;
00521             while (!_uart.readable() && (now + timeout*1000) > t->read_ms());
00522         } else
00523             while (!_uart.readable());
00524         if (_uart.readable())
00525             buf[i] = _uart.getc();;
00526     }
00527 #if 0
00528     if (_serial_debug) {
00529             unsigned int i;
00530             _serial_debug->printf("\n\r---------------------\n\r");
00531             for (i = 0; i < len ; ++i)
00532                 _serial_debug->putc((int)buf[i]);
00533             _serial_debug->printf("\n\r---------------------\n\r");
00534     }
00535 #endif
00536     return 0;
00537 }
00538 
00539 
00540  void Teseo_LIV3F::RFTest(bool enable)
00541  {
00542     if (enable)
00543         SendCommand(Teseo_LIV3F::RFTESTON);
00544     else
00545         SendCommand(Teseo_LIV3F::RFTESTOFF);
00546  }
00547 
00548 void Teseo_LIV3F::ReadLoop(Serial *serial_debug)
00549 {
00550     while (1)
00551         if (_uart.readable()) {
00552             int c = _uart.getc();
00553             serial_debug->putc(c);
00554         }
00555 }
00556 
00557 char *Teseo_LIV3F::ReadSentence(const char *cmd, char *buf, unsigned long len)
00558 {
00559     int ret = ReadMessage(buf, len);
00560     if (ret)
00561         return NULL;
00562      return DetectSentence(cmd, buf, len);
00563 }
00564 
00565 struct ___msg {
00566     unsigned char len;
00567     char *str;
00568 };
00569 
00570 
00571 static const struct ___msg teseo_msgs[] = {
00572     [ NMEA_GPGGA ]   = {    .len = 5,   .str = "GPGGA",     },
00573     [ NMEA_GPGLL ]   = {    .len = 5,   .str = "GPGLL",     },
00574     [ NMEA_GNGSA ]   = {    .len = 5,   .str = "GNGSA",     },
00575     [ NMEA_GPTXT ]   = {    .len = 5,   .str = "GPTXT",     },
00576     [ NMEA_GPVTG ]   = {    .len = 5,   .str = "GPVTG",     },
00577     [ NMEA_GPRMC ]   = {    .len = 5,   .str = "GPRMC",     },
00578     [ NMEA_PSTMCPU ] = {    .len = 7,   .str = "PSTMCPU",   },
00579     [ NMEA_PSTMVER ] = {    .len = 7,   .str = "PSTMVER",   },
00580 };
00581 
00582 
00583 enum nmea_msg_id Teseo_LIV3F::MsgDetect(char  *buf, int buf_len, Serial *serial_debug)
00584 {
00585     int i;
00586 
00587     if (buf[0] == '$')
00588         ++buf;
00589 
00590     for (i = 0; i < NMEA_END__; ++i)
00591         if (memcmp((void*)teseo_msgs[i].str, (void*)buf, teseo_msgs[i].len) == 0)
00592             return (enum nmea_msg_id) i;
00593 #if 0
00594     if (serial_debug) {
00595         serial_debug->puts("MESSAGE NOT FOUND: ");
00596         for (int i = 0; i < 5; ++i)
00597             serial_debug->putc(lbuf[i]);
00598         serial_debug->puts("\n\r");
00599     }
00600 #endif
00601     return NMEA_END__;
00602 }
00603 
00604 void Teseo_LIV3F::UARTStreamProcess(Serial *serial_debug)
00605 {
00606     enum nmea_msg_id id;
00607     char c;
00608 
00609     struct teseo_msg *msg = mpool.alloc();
00610     msg->len = 0;
00611 
00612     while (true) {
00613           _uart_mutex_lock();
00614 #if 0
00615           if (_uart_interleaded == true) {
00616               msg->len = 0;
00617               _uart_interleaded = false;
00618               _uart_discard = true;
00619           }
00620 #endif
00621           if (_uart.readable()) {
00622               c = _uart.getc();
00623               _uart_mutex_unlock();
00624               if (c == '$') {
00625                   queue.put(msg);
00626                   msg = mpool.alloc();
00627                   msg->len = 0;
00628                   _uart_discard = false;
00629               }
00630               if (!_uart_discard)
00631                   msg->buf[msg->len++] = c;
00632           } else {
00633               _uart_mutex_unlock();
00634               wait_us(100);
00635           }
00636     }
00637 }
00638 
00639 struct thr_data {
00640     Teseo_LIV3F *gnss;
00641     Serial *serial_debug;
00642 };
00643 
00644 static void Teseo_LIV3F_UARTStreamProcess(struct thr_data *data)
00645 {
00646     data->gnss->UARTStreamProcess(data->serial_debug);
00647 }
00648 
00649 void Teseo_LIV3F::startListener(Serial *serial_debug)
00650 {
00651     if (serialStreamThread.get_state() == Thread::Running)
00652         return;
00653 
00654     static struct thr_data data = {
00655         .gnss = this,
00656         .serial_debug = serial_debug,
00657     };
00658 
00659     serialStreamThread.start(Teseo_LIV3F_UARTStreamProcess, &data);
00660 }
00661 
00662 void Teseo_LIV3F::stopListener(Serial *serial_debug)
00663 {
00664     if (serialStreamThread.get_state() != Thread::Running)
00665         return;
00666     serialStreamThread.terminate();
00667 }
00668