From 903e7484731e6970954651300a1095f25fba2f21 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=A4ggi?= <tobias.jaeggi@netmodule.com>
Date: Wed, 22 Jan 2020 16:55:00 +0100
Subject: [PATCH] config-gnss: baud rate configuration for gnss receiver and
 ttyS3
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Added recipes aswell as scripts to automatically configure the baud
rate of ttyS3 and the GNSS receiver (NEO-M8L) with the baud rate
specified in a new config file called /etc/gnss.conf

tty interface also configured if receiver config gets launched manually.
If config-gnss-receiver was launched manually it now executes
config-gnss-tty to configure the tty interface before starting gpsd.

BugzID: 60698

Signed-off-by: Tobias Jäggi <tobias.jaeggi@netmodule.com>
Signed-off-by: Ramon Moesching <ramon.moesching@netmodule.com>
---
 .../gpsd/config-gnss-receiver.bb              |  40 +++
 recipes-connectivity/gpsd/config-gnss-tty.bb  |  32 +++
 .../gpsd/files/config-gnss-receiver.service   |  12 +
 .../gpsd/files/config-gnss-receiver.sh        | 270 ++++++++++++++++++
 .../gpsd/files/config-gnss-tty.service        |  12 +
 .../gpsd/files/config-gnss-tty.sh             |  33 +++
 .../gpsd/files/gnss-receiver-save-state       |   1 +
 recipes-connectivity/gpsd/files/gnss.conf     |  12 +
 8 files changed, 412 insertions(+)
 create mode 100644 recipes-connectivity/gpsd/config-gnss-receiver.bb
 create mode 100644 recipes-connectivity/gpsd/config-gnss-tty.bb
 create mode 100755 recipes-connectivity/gpsd/files/config-gnss-receiver.service
 create mode 100755 recipes-connectivity/gpsd/files/config-gnss-receiver.sh
 create mode 100755 recipes-connectivity/gpsd/files/config-gnss-tty.service
 create mode 100644 recipes-connectivity/gpsd/files/config-gnss-tty.sh
 create mode 100644 recipes-connectivity/gpsd/files/gnss-receiver-save-state
 create mode 100644 recipes-connectivity/gpsd/files/gnss.conf

diff --git a/recipes-connectivity/gpsd/config-gnss-receiver.bb b/recipes-connectivity/gpsd/config-gnss-receiver.bb
new file mode 100644
index 0000000..55a4c47
--- /dev/null
+++ b/recipes-connectivity/gpsd/config-gnss-receiver.bb
@@ -0,0 +1,40 @@
+# Copyright (C) 2020 Tobias Jäggi <tobias.jaeggi@netmodule.com>
+# Released under the MIT license (see COPYING.MIT for the terms)
+DESCRIPTION = "NEO-M8L baud rate config service"
+HOMEPAGE = "www.netmodule.com"
+LICENSE = "MIT"
+SECTION = "bsp/firmware"
+LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302"
+
+inherit systemd
+
+SRC_URI = " \
+           file://config-gnss-receiver.service \
+           file://config-gnss-receiver.sh \
+           file://gnss.conf \
+           file://gnss-receiver-save-state \
+           "
+
+S = "${WORKDIR}"
+
+SYSTEMD_SERVICE_${PN} = " \
+                          config-gnss-receiver.service \
+                        "
+
+FILES_${PN} = "${systemd_unitdir}/system ${bindir} ${localstatedir} ${sysconfdir}"
+
+do_install() {
+    install -d ${D}${systemd_unitdir}/system
+    install -m 644 ${WORKDIR}/config-gnss-receiver.service ${D}${systemd_unitdir}/system/
+
+    install -d ${D}${bindir}
+    install -m 744 ${WORKDIR}/config-gnss-receiver.sh ${D}${bindir}/config-gnss-receiver
+   
+    install -d ${D}${sysconfdir}
+    install -m 644 ${WORKDIR}/gnss.conf ${D}${sysconfdir}/gnss.conf
+
+    install -d ${D}${localstatedir}
+    install -m 644 ${WORKDIR}/gnss-receiver-save-state ${D}${localstatedir}/gnss-receiver-save-state
+}
+
+RDEPENDS_${PN} = "gpsd"
diff --git a/recipes-connectivity/gpsd/config-gnss-tty.bb b/recipes-connectivity/gpsd/config-gnss-tty.bb
new file mode 100644
index 0000000..8b4c71a
--- /dev/null
+++ b/recipes-connectivity/gpsd/config-gnss-tty.bb
@@ -0,0 +1,32 @@
+# Copyright (C) 2020 Tobias Jäggi <tobias.jaeggi@netmodule.com>
+# Released under the MIT license (see COPYING.MIT for the terms)
+DESCRIPTION = "ttyS3 config service"
+HOMEPAGE = "www.netmodule.com"
+LICENSE = "MIT"
+SECTION = "bsp/firmware"
+LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302"
+
+
+inherit systemd
+
+SRC_URI = " \ 
+           file://config-gnss-tty.service \
+           file://config-gnss-tty.sh \
+           "
+
+S = "${WORKDIR}"
+
+SYSTEMD_SERVICE_${PN} = " \
+                          config-gnss-tty.service \
+                        "
+
+FILES_${PN} = "${systemd_unitdir}/system ${bindir}"
+
+do_install() {
+    install -d ${D}${systemd_unitdir}/system
+    install -m 644 ${WORKDIR}/config-gnss-tty.service ${D}${systemd_unitdir}/system/
+
+    install -d ${D}${bindir}
+    install -m 744 ${WORKDIR}/config-gnss-tty.sh ${D}${bindir}/config-gnss-tty
+
+}
diff --git a/recipes-connectivity/gpsd/files/config-gnss-receiver.service b/recipes-connectivity/gpsd/files/config-gnss-receiver.service
new file mode 100755
index 0000000..b0face5
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/config-gnss-receiver.service
@@ -0,0 +1,12 @@
+[Unit]
+Description=config baud rate on NEO-M8L
+Before=config-gnss-tty.service
+
+[Service]
+
+Type=oneshot
+ExecStart=/usr/bin/config-gnss-receiver
+RemainAfterExit=no
+
+[Install]
+WantedBy=multi-user.target
diff --git a/recipes-connectivity/gpsd/files/config-gnss-receiver.sh b/recipes-connectivity/gpsd/files/config-gnss-receiver.sh
new file mode 100755
index 0000000..e08c7a0
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/config-gnss-receiver.sh
@@ -0,0 +1,270 @@
+#!/bin/sh
+
+# This script tries to figure out the baud rate of $DEVICE_NAME connected to $TTY_INTERFACE.
+# If the script successfully determines the baud rate of the receiver,
+# it changes both the baud rate of the ttyS3 interface and the receiver to the
+# value stored in DESIRED_BAUD_RATE
+
+# exit codes:
+# 0 success
+# 1 unable to detect baud rate
+# 2 set baud rate was not successful
+# 3 baud rate not set to $DESIRED_BAUD_RATE or verifiying baud rate failed
+# 4 saving to flash failed
+# 5 baud rate specified in $CONFIG_FILE not supported by $DEVICE_NAME
+
+# static variables
+DEVICE_NAME="NEO-M8L"
+TTY_INTERFACE="/dev/ttyS3"
+CONFIG_FILE="/etc/gnss.conf"
+SAVE_FILE="/var/gnss-receiver-save-state"
+# if the config file is missing, a new one gets created with $DEFAULT_BAUD_RATE baud
+DEFAULT_BAUD_RATE=115200
+# first baud rate in PRESELECTION_BAUD_RATES should be the one which is most likely the current baud rate of the GNSS receiver,
+# the last one should be the one least likely used by the reciever.
+# This can reduce the runtime of this script
+# Note that this array can be expanded to test more baud rates if needed
+PRESELECTION_BAUD_RATES=(9600 115200 4800)
+
+# this array contains the baud rates that can be specified to be used by $DEVICE_NAME
+# There is no guarantee that those baud rates work in combination with your device
+# Note: 921600 is supported by M8L but not ubxtool
+#SUPPORTED_BAUD_RATES=(460800 230400 115200 57600 38400 19200 9600 4800)
+SUPPORTED_BAUD_RATES=(4800 9600 19200 38400 57600 115200 230400 460800)
+
+ACK_MESSAGE="UBX-ACK-ACK:"
+MAX_ALLOWED_RETRIES=4
+
+# non static variables
+# this is ugly, nobody likes non static global variables :(
+DETECTED_BAUD_RATE=0
+DESIRED_BAUD_RATE=0
+
+# pass baudrate of ttyS3 as parameter $1
+# echo'es back the polled baud rate
+# if no baud rate is returned by the receiver this function echo'es back "0"
+poll_baud_rate_receiver () {
+  local _br_ttys3
+  _br_ttys3=$1
+  read -r _br_receiver <<< "$(ubxtool -f "$TTY_INTERFACE" -p CFG-PRT -s "$_br_ttys3" | awk '/mode .* baudRate .*/{print $4}')"
+  if test -z "$_br_receiver"; then
+    # reading baud rate of receiver failed
+    echo "0"
+  else
+    echo "$_br_receiver"
+  fi
+}
+
+# pass baud rate of ttyS3 as paramater $1 and baud rate to set on receiver as $2
+set_baud_rate_receiver () {
+  local _br_ttys3
+  _br_ttys3="$1"
+  local _br_receiver
+  _br_receiver="$2"
+  # note: there is no ack when setting new baud rate
+  # -> there is no way to confirm config change without sending an additional message
+  ubxtool -f "$TTY_INTERFACE" -s "$1" -S "$2" > /dev/null
+  return 0
+}
+
+# args[0]: allowed retries
+# args[1:n]: baud rates to test
+detect_baud_rate () {
+  local _args
+  _args=("$@")
+  local _allowed_retries
+  _allowed_retries="${_args[0]}"
+  local _baud_rates
+  for i in "${_args[@]:1}"
+  do
+    _baud_rates+=("$i")
+  done
+  local _polled_baud_rate
+  for br in "${_baud_rates[@]}";
+    do
+      printf "testing %s baud\n" "$br"
+        for i in $(eval echo {1.."$_allowed_retries"})
+        do
+          _polled_baud_rate="$(poll_baud_rate_receiver "$br")"
+          if [ "$_polled_baud_rate" == "0" ];then
+            # increasing timeout for each try
+            t=$(echo "$i" "0.1"| awk '{printf "%4.3f\n",$1*$2}')
+            sleep "$t"
+          else
+            DETECTED_BAUD_RATE=$_polled_baud_rate
+            return 0
+          fi
+        done
+      printf "no luck so far\n"
+  done
+# failed to detect baud rate
+printf "failed to detect baud rate\n"
+return 1
+}
+
+save_receiver_config () {
+  local _ack
+  read -r _ack <<< "$(ubxtool -f "$TTY_INTERFACE" -p SAVE -s $DESIRED_BAUD_RATE | grep $ACK_MESSAGE)"
+  if [ "$_ack" == "$ACK_MESSAGE" ]; then
+    # saved successfully
+    # log succe save
+    echo 1 > $SAVE_FILE
+    #touch
+    return 0
+  fi
+  return 1
+}
+
+# start of main
+
+# check if script was launched by systemd
+if test -z "$INVOCATION_ID"; then
+  # launched by user
+  # TODO: get state of gpsd and decide accordingly
+  systemctl stop gpsd.socket && systemctl stop gpsd
+  RESTART_GPSD=1
+else
+  RESTART_GPSD=0
+fi
+
+# TODO: remove this hack in favour of a proper python environment
+# hack to get pyserial working
+PYTHONPATH=/usr/lib/python3.7/site-packages/ && export PYTHONPATH
+
+# TODO: parse input args to display help which would explain usage of $CONFIG_FILE
+
+# check if CONFIG_FILE exists
+if ! test -f "$CONFIG_FILE"; then
+  # CONFIG_FILE doesn't exist
+  printf "config file missing\n"
+  touch $CONFIG_FILE
+  echo -e "#\n# Define a baud rate to be used by the GNSS receiver\n# This file is part of ublox-gnss-config service\n#\n# Required fields:\n#     br:       baud rate to be used by the GNSS receiver\n#\n\n\n[default]\nbr=$DEFAULT_BAUD_RATE\n" > $CONFIG_FILE
+  printf "created default config file %s with %s baud\n" "$CONFIG_FILE" "$DEFAULT_BAUD_RATE"
+fi
+
+# check if SAVE_FILE exists
+if ! test -f "$SAVE_FILE"; then
+  # SAVE_FILE doesn't exist
+  printf "save file missing\n"
+  touch $SAVE_FILE
+  echo 0 > $SAVE_FILE
+  printf "created save file %s\n" "$SAVE_FILE"
+fi
+
+# read $DESIRED_BAUD_RATE specified in $CONFIG_FILE
+read -r DESIRED_BAUD_RATE <<< "$(awk '/br=(\d*)/{print $0}' < $CONFIG_FILE)"
+read -r DESIRED_BAUD_RATE <<< "$(echo "$DESIRED_BAUD_RATE"| cut -d'=' -f 2)"
+
+# check if $DESIRED_BAUD_RATE is supported by $DEVICE_NAME
+baud_rate_supported_flag=0
+for i in "${SUPPORTED_BAUD_RATES[@]}"
+do
+  if [ "$i" == "$DESIRED_BAUD_RATE" ];then
+    baud_rate_supported_flag=1
+    break
+  fi
+done
+if [ "$baud_rate_supported_flag" == "0" ];then
+  printf "baud rate specified in %s is not supported by receiver %s\n" "$CONFIG_FILE" "$DEVICE_NAME"
+  printf "supported baud rates are:\n"
+  for j in "${SUPPORTED_BAUD_RATES[@]}"
+  do
+    printf "%s " "$j"
+  done
+  printf "\n"
+  exit 5 # baud rate specified in $CONFIG_FILE not supported by $DEVICE_NAME
+fi
+
+# check if baud rate of $DEVICE_NAME matches $DESIRED_BAUD_RATE
+if br_receiver="$(poll_baud_rate_receiver "$DESIRED_BAUD_RATE")";then
+  # read worked
+  if [ "$br_receiver" == "$DESIRED_BAUD_RATE" ];then
+    # baud rate of receiver is matching baud rate specified in CONFIG_FILE
+    if [ "$(cat "$SAVE_FILE")" == "1" ];then
+      printf "baud rate of %s is already at %s baud\n" "$DEVICE_NAME" "$DESIRED_BAUD_RATE"
+      printf "configuration is saved in flash of %s\n" "$DEVICE_NAME"
+      # script can exit
+      exit 0 # baud rate of receiver is already matching $DESIRED_BAUD_RATE
+    else
+      # TODO: this could be way faster by telling the detector which baud rate to use since we already know it
+      printf "baud rate of %s is already at %s baud\n" "$DEVICE_NAME" "$DESIRED_BAUD_RATE"
+      printf "configuration is not saved in flash of %s\n" "$DEVICE_NAME"
+    fi
+  else
+    printf "baud rates are not matching\n"
+  fi
+else
+  printf "read failed\n"
+fi
+
+# determine current baud rate of receiver
+args=("2" "${PRESELECTION_BAUD_RATES[@]}")
+detect_baud_rate "${args[@]}"
+if [ "$?" == "1" ];then
+  # retry with more baud rates and more tries per baud rate
+  printf "retrying\n"
+  # maybe a time out increase would also work when dealing with lower baud rates?
+  args=("$MAX_ALLOWED_RETRIES" "${SUPPORTED_BAUD_RATES[@]}")
+  detect_baud_rate "${args[@]}"
+  if [ "$?" == "1" ];then
+    printf "baud rate not detected, even after retry\n"
+    exit 1 # unable to detect baud rate
+  fi
+fi
+printf "detected baud rate of %s is %s baud\n" "$DEVICE_NAME" "$DETECTED_BAUD_RATE"
+
+# upgrade to $DESIRED_BAUD_RATE baud
+new_baud_rate_set_flag=0
+for i in $(eval echo {0.."$MAX_ALLOWED_RETRIES"})
+do
+  set_baud_rate_receiver "$DETECTED_BAUD_RATE" "$DESIRED_BAUD_RATE"
+
+  if br_receiver="$(poll_baud_rate_receiver "$DESIRED_BAUD_RATE")";then
+    # poll of baud rate worked
+    if [ "$br_receiver" == "$DESIRED_BAUD_RATE" ];then
+      # baud rate of receiver is matching $DESIRED_BAUD_RATE
+      new_baud_rate_set_flag=1
+      # reset save log
+      echo 0 > $SAVE_FILE
+      break
+    fi
+  fi
+  # increasing timeout for each try
+  t=$(echo "$i" "0.1"| awk '{printf "%4.3f\n",$1*$2}')
+  sleep "$t"
+done
+
+if [ "$new_baud_rate_set_flag" == "0" ];then
+  printf "baud rate of %s is not set to %s baud or unable to verify, retried %s times\n" "$DEVICE_NAME" "$DESIRED_BAUD_RATE" "$i"
+  exit 3 # baud rate not set to $DESIRED_BAUD_RATE or verifiying baud rate failed
+else
+  printf "baud rate of %s is set to desired %s baud, it took %s retries\n" "$DEVICE_NAME" "$DESIRED_BAUD_RATE" "$i"
+fi
+
+# save config to flash
+config_saved_flag=0
+for i in $(eval echo {0.."$MAX_ALLOWED_RETRIES"})
+do
+  if save_receiver_config;then
+    config_saved_flag=1
+    break # success
+  fi
+  # increasing timeout for each try
+  t=$(echo "$i" "0.1"| awk '{printf "%4.3f\n",$1*$2}')
+  sleep "$t"
+done
+
+if [ "$config_saved_flag" == "0" ];then
+  printf "saving to flash was not successful, retried %s times\n" "$i"
+  exit 4 # saving to flash failed
+else
+  printf "saving to flash was successful, it took %s retries\n" "$i"
+fi
+
+# TODO: verify or error handling
+if [ "$RESTART_GPSD" -eq "1" ]; then
+  config-gnss-tty # call config-gnss-tty to set baud rate of ttyS3 to the baud rate specified in $CONFIG_FILE before launching gpsd
+  systemctl start gpsd
+fi
+
+exit 0 # successfully changed baud rate to $DESIRED_BAUD_RATE specified in $CONFIG_FILE
diff --git a/recipes-connectivity/gpsd/files/config-gnss-tty.service b/recipes-connectivity/gpsd/files/config-gnss-tty.service
new file mode 100755
index 0000000..8e10872
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/config-gnss-tty.service
@@ -0,0 +1,12 @@
+[Unit]
+Description=config baud rate of tty used by gnss receiver
+Before=gpsd.service
+
+[Service]
+
+Type=oneshot
+ExecStart=/usr/bin/config-gnss-tty
+RemainAfterExit=no
+
+[Install]
+WantedBy=multi-user.target
diff --git a/recipes-connectivity/gpsd/files/config-gnss-tty.sh b/recipes-connectivity/gpsd/files/config-gnss-tty.sh
new file mode 100644
index 0000000..7d07c17
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/config-gnss-tty.sh
@@ -0,0 +1,33 @@
+#!/bin/sh
+
+# exit codes:
+# 0: success
+# 1: $CONFIG_FILE is missing (should have been created by config-gnss-receiver)
+# 2: failed to set baud rate of $TTY_INTERFACE to $DESIRED_BAUD_RATE
+
+TTY_INTERFACE="/dev/ttyS3"
+CONFIG_FILE="/etc/gnss.conf"
+
+# check if CONFIG_FILE exists
+if ! test -f "$CONFIG_FILE"; then
+  # CONFIG_FILE doesn't exist
+  printf "config file missing\n"
+  exit 1 # $CONFIG_FILE is missing (should have been created by config-gnss-receiver)
+fi
+
+# read $DESIRED_BAUD_RATE specified in $CONFIG_FILE
+read -r DESIRED_BAUD_RATE <<< "$(awk '/br=(\d*)/{print $0}' < $CONFIG_FILE)"
+read -r DESIRED_BAUD_RATE <<< "$(echo "$DESIRED_BAUD_RATE" | cut -d'=' -f 2)"
+
+# set baud rate
+stty -F "$TTY_INTERFACE" speed "$DESIRED_BAUD_RATE" > /dev/null
+
+# verify baud rate
+read -r TTYS3_BAUD_RATE <<< "$(stty -F "$TTY_INTERFACE" | awk '/speed .* baud;/{print $2}')"
+if [ "$TTYS3_BAUD_RATE" == "$DESIRED_BAUD_RATE" ];then
+  printf "successfully set baud rate of %s to %s baud\n" "$TTY_INTERFACE" "$DESIRED_BAUD_RATE"
+  exit 0 # success
+else
+  printf "failed to set baud rate of %s to %s\n" "$TTY_INTERFACE" "$DESIRED_BAUD_RATE"
+  exit 2
+fi
diff --git a/recipes-connectivity/gpsd/files/gnss-receiver-save-state b/recipes-connectivity/gpsd/files/gnss-receiver-save-state
new file mode 100644
index 0000000..573541a
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/gnss-receiver-save-state
@@ -0,0 +1 @@
+0
diff --git a/recipes-connectivity/gpsd/files/gnss.conf b/recipes-connectivity/gpsd/files/gnss.conf
new file mode 100644
index 0000000..4e98fbd
--- /dev/null
+++ b/recipes-connectivity/gpsd/files/gnss.conf
@@ -0,0 +1,12 @@
+#
+# Define a baud rate to be used by the GNSS receiver
+# This file is part of ublox-gnss-config service
+#
+# Required fields:
+#     br:       baud rate to be used by the GNSS receiver
+#
+
+
+[default]
+br=115200
+