![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
hue
Teseo-LIV3F/Teseo-LIV3F.cpp
- Committer:
- etiene32
- Date:
- 2019-02-19
- Revision:
- 1:d6e2c8220523
- Parent:
- 0:4e088cbb2dbf
File content as of revision 1:d6e2c8220523:
/* * ------------------------------------------------------------------------- * Copyright (C) 2017 STMicroelectronics * Author: Francesco M. Virlinzi <francesco.virlinzi@st.com> * * May be copied or modified under the terms of the GNU General Public * License V.2 ONLY. See linux/COPYING for more information. * * ------------------------------------------------------------------------- */ #include "string.h" #include "Teseo-LIV3F.h" static char T3_name[] = "Teseo-LIV3F"; char _OK[] = "OK"; char _Failed[] = "Failed"; char X_Nucleo_name[] = "X-Nucleo-GNSS1A1"; struct teseo_cmd { char *cmd; }; static struct teseo_cmd teseo_cmds[] = { [Teseo_LIV3F::GETSWVER] = { .cmd = "$PSTMGETSWVER,7", }, [Teseo_LIV3F::FORCESTANDBY] = { .cmd = "$PSTMFORCESTANDBY,00007", }, [Teseo_LIV3F::RFTESTON] = { .cmd = "$PSTMRFTESTON,16", }, [Teseo_LIV3F::RFTESTOFF] = { .cmd = "$PSTMRFTESTOFF\n\r", }, [Teseo_LIV3F::LOWPOWER] = { .cmd = "$PSTMLOWPOWERONOFF,1,0,000,05,0,1,000,1,00010,01,0,0,1,01", }, [Teseo_LIV3F::FWUPDATE] = { .cmd = "$PSTMFWUPGRADE", }, }; Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin, PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin, Serial *serial_debug): _reset(reset_pin, 1), _pps(pps_pin), _wakeup(wakeup_pin, 0), _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD), _serial_debug(serial_debug) { wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); _uart.baud(SDT_UART_BAUD); _uart.format(8, SerialBase::None, 1); _i2c = NULL; _uart_interleaded = false; _uart_discard = false; } Teseo_LIV3F::Teseo_LIV3F(PinName reset_pin, PinName wakeup_pin, PinName pps_pin, PinName uart_tx_pin, PinName uart_rx_pin, I2C *bus, Serial *serial_debug): _reset(reset_pin, 1), _pps(pps_pin), _wakeup(wakeup_pin, 0), _uart(uart_rx_pin, uart_tx_pin, SDT_UART_BAUD), _i2c(bus), _serial_debug(serial_debug) { wait_ms(POWERON_STABLE_SIGNAL_DELAY_MS); _uart.baud(SDT_UART_BAUD); _uart.format(8, SerialBase::None, 1); _uart_interleaded = false; _uart_discard = false; } int Teseo_LIV3F::EnableLowPower() { SendCommand(LOWPOWER); return 0; } void Teseo_LIV3F::Reset(Serial *serial_debug) { if (serial_debug) serial_debug->printf("%s: Resetting...", T3_name); _reset.write(0); wait_ms(50); _reset.write(1); wait_ms(70); if (serial_debug) serial_debug->printf("Done...\n\r"); } enum { TESEO_FLASHER_IDENTIFIER = 0, // 0xBCD501F4 TESEO_FLASHER_SYNC, // 0x83984073 DEVICE_START_COMMUNICATION, // 0xA3 FLASHER_READY, // 0x4A ACK, // 0xCC NACK, }; struct firmware_ctrl { char *cmd; unsigned char len; char *n; } ; /* * #define FWUPG_IDENTIFIER 0xBC D5 01 F4 * #define FWUPG_SYNC 0x83 98 40 73 */ static struct firmware_ctrl fw_data[] = { [TESEO_FLASHER_IDENTIFIER] = { .cmd = (char *)(char[]){ 0xF4, 0x01, 0xD5, 0xBC}, .len = 4, .n = "TESEO_FLASHER_IDENTIFIER", }, [TESEO_FLASHER_SYNC] = { .cmd =(char *)(char[]){ 0x73, 0x40, 0x98, 0x83 }, .len = 4, .n = "TESEO_FLASHER_SYNC", }, [DEVICE_START_COMMUNICATION] = { .cmd = (char *)(char[]){0xA3}, .len = 1, .n = "DEVICE_START_COMMUNICATION", }, [FLASHER_READY] = { .cmd = (char *)(char[]){0x4A}, .len = 1, .n = "FLASHER_READY", }, [ACK] = { .cmd = (char *)(char[]){0xCC}, .len = 1, .n = "ACK", }, [NACK] = { .cmd = (char *)(char[]){0xDD}, .len = 1, .n = "NACK", }, }; int Teseo_LIV3F::SendString(char *buf, int len) { for (int i = 0; i < len; ++i) { while (!_uart.writeable()); _uart.putc(buf[i]); } } struct ImageOptions { unsigned char eraseNVM; unsigned char programOnly; unsigned char reserved; unsigned char baudRate; unsigned int firmwareSize; unsigned int firmwareCRC; unsigned int nvmAddressOffset; unsigned int nvmSize; } liv3f_img_option = { .eraseNVM = 1, .programOnly = 0, .reserved = 0, .baudRate = 1, .firmwareSize = 0, .firmwareCRC = 0, .nvmAddressOffset = 0x00100000, .nvmSize = 0x00100000, }; int Teseo_LIV3F::FwWaitAck() { while (!_uart.readable()); char c = _uart.getc(); if (fw_data[ACK].cmd[0] == c) { if (_serial_debug) _serial_debug->printf("%s (0x%x)\n\r", _OK, c); return 0; } if (fw_data[NACK].cmd[0] == c) { if (_serial_debug) _serial_debug->printf("%s (%x)\n\r", _Failed, c); return -1; } if (_serial_debug) _serial_debug->printf("%s - Char not allowed (%x)\n\r", _Failed, c); return -1; } bool Teseo_LIV3F::FirmwareUpdate(bool is_recovery, char *data, unsigned int data_len, unsigned long crc, Serial *serial_debug) { unsigned int i; char _buf[4] = { 0xff, 0xff, 0xff, 0xff }; liv3f_img_option.firmwareSize = data_len; liv3f_img_option.firmwareCRC = crc; if (data == NULL || !data_len) return false; if (is_recovery) Reset(); { _uart.baud(FWU_UART_BAUD); #if 1 while (1) { /* send TESEO_FLASHER_IDENTIFIER */ /* * Device is under reset. Host sends continuously “TESEO2_FLASHER_IDENTIFIER� word. */ SendString(fw_data[TESEO_FLASHER_IDENTIFIER].cmd, fw_data[TESEO_FLASHER_IDENTIFIER].len); /* try to read... TESEO_FLASHER_SYNC */ if (_uart.readable()) for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len; ) { while (!_uart.readable()); _buf[i] = _uart.getc(); if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == _buf[i]) { if (serial_debug) serial_debug->printf("-- %d -- 0x%x -- ok--\n\r", i, _buf[i]); i++; goto exit_step_1; /* FMV: WA to have firmware update working.... */ } else { i = 0; } } if (i == fw_data[TESEO_FLASHER_SYNC].len) goto exit_step_1; } exit_step_1: _uart.abort_read(); if (serial_debug) serial_debug->printf("Got: %s from %s\n\r", fw_data[TESEO_FLASHER_SYNC].n, T3_name); /* * Host sends “DEVICE_START_COMMUNICATION� word. */ serial_debug->printf("\n\r%s Step: %s ",T3_name, fw_data[DEVICE_START_COMMUNICATION].n); SendString(fw_data[DEVICE_START_COMMUNICATION].cmd, fw_data[DEVICE_START_COMMUNICATION].len); FwWaitAck(); /* * Host sends the binary image options. Both host and * device change UART baud rates. Host sends continuously * the “FLASHER_READY� word. */ if (serial_debug) serial_debug->printf("%s Step: Send ImageOption\n\r",T3_name); SendString((char*)&liv3f_img_option, sizeof(ImageOptions)); if (serial_debug) serial_debug->printf("%s Step: Send %s\n\r",T3_name, fw_data[FLASHER_READY].n); while (1) { SendString(fw_data[FLASHER_READY].cmd, fw_data[FLASHER_READY].len); if (_uart.readable()) goto exit_step_3; } exit_step_3: FwWaitAck(); if (serial_debug) serial_debug->printf("%s Step: Erasing flash area ",T3_name); FwWaitAck(); /* * Device is erasing flash program. Host is waiting for an “ACK�. */ if (serial_debug) serial_debug->printf("%s Step: Erasing NVM ",T3_name); while (!_uart.readable()); FwWaitAck(); if (serial_debug) serial_debug->printf("%s Step: Sending data... ",T3_name); for (i = 0; i < (data_len / (16*1024)); ++i) { SendString(&data[i], 16*1024); FwWaitAck(); } serial_debug->printf("\n\r"); /* * send remaining data... */ if (data_len != (i * 16*1024)) { SendString(&data[i*16*1024], data_len-i*16*1024); FwWaitAck(); } /* * wait CRC ack */ FwWaitAck(); } #else //_uart.format(8, SerialBase::Forced0, 1); while (1) { /* send TESEO_FLASHER_IDENTIFIER */ for (i = 0; i < fw_data[TESEO_FLASHER_IDENTIFIER].len; ++i) { while (!_uart.writeable()); _uart.putc(fw_data[TESEO_FLASHER_IDENTIFIER].cmd[i]); } /* try to read... TESEO_FLASHER_SYNC */ //while (!_uart.readable()); for (i = 0; i < fw_data[TESEO_FLASHER_SYNC].len && _uart.readable(); ) if (_uart.readable()) { char c = _uart.getc(); /* if (!c) break; */ if (serial_debug) serial_debug->printf("%x vs %x\n\r", c, fw_data[TESEO_FLASHER_SYNC].cmd[i]); if (fw_data[TESEO_FLASHER_SYNC].cmd[i] == c) i++; else i = 0; } if (i == fw_data[TESEO_FLASHER_SYNC].len && serial_debug) serial_debug->printf("Got %s from %s\n\r",fw_data[TESEO_FLASHER_SYNC].n, T3_name); } } #endif if (serial_debug) serial_debug->printf("END\n\r"); return true; } int Teseo_LIV3F::WakeUp() { wait_ms(100); _wakeup.write(1); wait_ms(500); _wakeup.write(0); return 0; } bool Teseo_LIV3F::CheckPPSWorking() { int val_0, val_1; wait_ms(500); val_0 = _pps.read(); wait_ms(500); val_1 = _pps.read(); if (val_0 != val_1) return true; return false; } int Teseo_LIV3F::CRC_(char *buf, int size) { int i = 0, ch = 0; if (buf[0] == '$') ++i; if (size) for (; i < size; ++i) ch ^= buf[i]; else for (; buf[i] != 0; ++i) ch ^= buf[i]; return ch; } bool Teseo_LIV3F::WaitBooting(Timer *t, float timeout) { unsigned int now = t->read_ms();; while (1) { if (CheckPPSWorking() == true) return true; if ((now + timeout*1000) < t->read_ms()) break; } return false; } void Teseo_LIV3F::SendCommand(enum Teseo_LIV3F::cmd_enum c) { char crc[5]; sprintf(crc, "*%02X\n\r", CRC_(teseo_cmds[c].cmd, 0)); _uart_mutex_lock(); _uart_interleaded = true; SendString(teseo_cmds[c].cmd, strlen(teseo_cmds[c].cmd)); SendString(crc, 5); _uart_mutex_unlock(); } char *Teseo_LIV3F::DetectSentence(const char *cmd, char *buf, unsigned long len) { char *result = NULL; unsigned int i = 0; const unsigned long cmd_len = strlen(cmd); len -= strlen(cmd); while (!result && i < len) { for (; buf[i] != '$' && i < len; ++i); /* 1. check '$' char */ if (i == len) break; /* no more char.... */ ++i; /* to point to the char after '$' */ if (strncmp(&buf[i], cmd, cmd_len) == 0) { result = &buf[i]; } } if (result) { for (i = 0; result[i] != '*'; ++i); result[i] = 0; } #if 0 if (_serial_debug) _serial_debug->printf("%s: %s: %s %s FOUND\n\r", T3_name, __FUNCTION__, cmd, result ? " " : "NOT"); #endif return result; } int Teseo_LIV3F::CheckI2C() { if (!_i2c) return -1; _i2c->start(); int res = _i2c->write((TESEO_I2C_ADDRESS << 1) | 1); _i2c->stop(); /* * @returns * '0' - NAK was received * '1' - ACK was received, * '2' - timeout */ return res == 1 ? 0 : -1; } int Teseo_LIV3F::ReadMessage(char *buf, unsigned long len, Timer *t, float timeout) { memset(buf, 0, len); for (unsigned int i = 0; i < len; ++i){ if (t) { unsigned int now = t->read_ms();; while (!_uart.readable() && (now + timeout*1000) > t->read_ms()); } else while (!_uart.readable()); if (_uart.readable()) buf[i] = _uart.getc();; } #if 0 if (_serial_debug) { unsigned int i; _serial_debug->printf("\n\r---------------------\n\r"); for (i = 0; i < len ; ++i) _serial_debug->putc((int)buf[i]); _serial_debug->printf("\n\r---------------------\n\r"); } #endif return 0; } void Teseo_LIV3F::RFTest(bool enable) { if (enable) SendCommand(Teseo_LIV3F::RFTESTON); else SendCommand(Teseo_LIV3F::RFTESTOFF); } void Teseo_LIV3F::ReadLoop(Serial *serial_debug) { while (1) if (_uart.readable()) { int c = _uart.getc(); serial_debug->putc(c); } } char *Teseo_LIV3F::ReadSentence(const char *cmd, char *buf, unsigned long len) { int ret = ReadMessage(buf, len); if (ret) return NULL; return DetectSentence(cmd, buf, len); } struct ___msg { unsigned char len; char *str; }; static const struct ___msg teseo_msgs[] = { [ NMEA_GPGGA ] = { .len = 5, .str = "GPGGA", }, [ NMEA_GPGLL ] = { .len = 5, .str = "GPGLL", }, [ NMEA_GNGSA ] = { .len = 5, .str = "GNGSA", }, [ NMEA_GPTXT ] = { .len = 5, .str = "GPTXT", }, [ NMEA_GPVTG ] = { .len = 5, .str = "GPVTG", }, [ NMEA_GPRMC ] = { .len = 5, .str = "GPRMC", }, [ NMEA_PSTMCPU ] = { .len = 7, .str = "PSTMCPU", }, [ NMEA_PSTMVER ] = { .len = 7, .str = "PSTMVER", }, }; enum nmea_msg_id Teseo_LIV3F::MsgDetect(char *buf, int buf_len, Serial *serial_debug) { int i; if (buf[0] == '$') ++buf; for (i = 0; i < NMEA_END__; ++i) if (memcmp((void*)teseo_msgs[i].str, (void*)buf, teseo_msgs[i].len) == 0) return (enum nmea_msg_id) i; #if 0 if (serial_debug) { serial_debug->puts("MESSAGE NOT FOUND: "); for (int i = 0; i < 5; ++i) serial_debug->putc(lbuf[i]); serial_debug->puts("\n\r"); } #endif return NMEA_END__; } void Teseo_LIV3F::UARTStreamProcess(Serial *serial_debug) { enum nmea_msg_id id; char c; struct teseo_msg *msg = mpool.alloc(); msg->len = 0; while (true) { _uart_mutex_lock(); #if 0 if (_uart_interleaded == true) { msg->len = 0; _uart_interleaded = false; _uart_discard = true; } #endif if (_uart.readable()) { c = _uart.getc(); _uart_mutex_unlock(); if (c == '$') { queue.put(msg); msg = mpool.alloc(); msg->len = 0; _uart_discard = false; } if (!_uart_discard) msg->buf[msg->len++] = c; } else { _uart_mutex_unlock(); wait_us(100); } } } struct thr_data { Teseo_LIV3F *gnss; Serial *serial_debug; }; static void Teseo_LIV3F_UARTStreamProcess(struct thr_data *data) { data->gnss->UARTStreamProcess(data->serial_debug); } void Teseo_LIV3F::startListener(Serial *serial_debug) { if (serialStreamThread.get_state() == Thread::Running) return; static struct thr_data data = { .gnss = this, .serial_debug = serial_debug, }; serialStreamThread.start(Teseo_LIV3F_UARTStreamProcess, &data); } void Teseo_LIV3F::stopListener(Serial *serial_debug) { if (serialStreamThread.get_state() != Thread::Running) return; serialStreamThread.terminate(); }