File content as of revision 0:43070b2dfc87:

/*
 * -------------------------------------------------------------------------
 * 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();
}