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