From b8370406c5ab624b1b2fd3acb4ef1d2aa3b6201b Mon Sep 17 00:00:00 2001
From: Alexandre Bard <alexandre.bard@netmodule.com>
Date: Thu, 16 Apr 2020 22:32:22 +0200
Subject: [PATCH] imu-setup: Add support for polling mode

Required for HW23.
The code is not really clean but helps make the difference between HW21
and HW23. Polling creates a device /dev/imu0 giving the poll frequency.

BugzID: 61346
---
 recipes-extended/imu-setup/files/imu-poll.py  | 27 +++++++++++++++++++
 .../imu-setup/files/imu-setup.service         |  3 ++-
 recipes-extended/imu-setup/files/imu-setup.sh | 17 +++++++-----
 recipes-extended/imu-setup/imu-setup_0.0.1.bb |  2 ++
 4 files changed, 41 insertions(+), 8 deletions(-)
 create mode 100755 recipes-extended/imu-setup/files/imu-poll.py

diff --git a/recipes-extended/imu-setup/files/imu-poll.py b/recipes-extended/imu-setup/files/imu-poll.py
new file mode 100755
index 0000000..544c1b6
--- /dev/null
+++ b/recipes-extended/imu-setup/files/imu-poll.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+
+import os
+import time
+
+def create_pty(dev):
+    master, slave = os.openpty()
+    slave_file = os.readlink('/proc/self/fd/{}'.format(slave))
+    if os.path.exists(dev):
+         os.remove(dev)
+    os.symlink(slave_file, dev)
+    os.close(slave)
+    return master
+
+
+def set_sampling_freq(iio_nbr):
+    f = open('/sys/bus/iio/devices/iio:device{}/sampling_frequency'.format(iio_nbr), 'w')
+    f.write('416')
+
+set_sampling_freq(1)
+set_sampling_freq(2)
+
+dev = create_pty('/dev/imu0')
+
+while True:
+    os.write(dev, b'poll\n')
+    time.sleep(0.1)
diff --git a/recipes-extended/imu-setup/files/imu-setup.service b/recipes-extended/imu-setup/files/imu-setup.service
index 7837c91..c7044ad 100644
--- a/recipes-extended/imu-setup/files/imu-setup.service
+++ b/recipes-extended/imu-setup/files/imu-setup.service
@@ -7,6 +7,7 @@ Before=gpsd.service
 Type=oneshot
 ExecStart=/usr/bin/imu-setup
 EnvironmentFile=/etc/imu-lsm6ds3.conf
+RemainAfterExit=yes
 
 [Install]
-WantedBy=multi-user.target
\ No newline at end of file
+WantedBy=multi-user.target
diff --git a/recipes-extended/imu-setup/files/imu-setup.sh b/recipes-extended/imu-setup/files/imu-setup.sh
index b63941b..64d1733 100644
--- a/recipes-extended/imu-setup/files/imu-setup.sh
+++ b/recipes-extended/imu-setup/files/imu-setup.sh
@@ -2,12 +2,15 @@
 ACC_SYSFS_PATH="/sys/bus/iio/devices/iio:device0"
 GYRO_SYSFS_PATH="/sys/bus/iio/devices/iio:device1"
 
-echo 0 > $ACC_SYSFS_PATH/buffer/enable
-echo $ACC_SAMPLING_FREQUENCY > $ACC_SYSFS_PATH/sampling_frequency
-echo 1 > $ACC_SYSFS_PATH/buffer/enable
-
-echo 0 > $GYRO_SYSFS_PATH/buffer/enable
-echo $GYRO_SAMPLING_FREQUENCY > $GYRO_SYSFS_PATH/sampling_frequency
-echo 1 > $GYRO_SYSFS_PATH/buffer/enable
+if [ -f "$ACC_SYSFS_PATH/buffer" ]; then
+	echo 0 > $ACC_SYSFS_PATH/buffer/enable
+	echo $ACC_SAMPLING_FREQUENCY > $ACC_SYSFS_PATH/sampling_frequency
+	echo 1 > $ACC_SYSFS_PATH/buffer/enable
 
+	echo 0 > $GYRO_SYSFS_PATH/buffer/enable
+	echo $GYRO_SAMPLING_FREQUENCY > $GYRO_SYSFS_PATH/sampling_frequency
+	echo 1 > $GYRO_SYSFS_PATH/buffer/enable
+else
+	imu-poll &
+fi
 exit 0
diff --git a/recipes-extended/imu-setup/imu-setup_0.0.1.bb b/recipes-extended/imu-setup/imu-setup_0.0.1.bb
index b550703..d167471 100644
--- a/recipes-extended/imu-setup/imu-setup_0.0.1.bb
+++ b/recipes-extended/imu-setup/imu-setup_0.0.1.bb
@@ -10,6 +10,7 @@ RDEPENDS_${PN} = "bash"
 SRC_URI = " \
             file://imu-setup.service \
             file://imu-setup.sh \
+            file://imu-poll.py \
             file://imu-lsm6ds3.conf \
           "
 
@@ -26,6 +27,7 @@ do_install() {
 
     install -d ${D}${bindir}
     install -m 744 ${WORKDIR}/imu-setup.sh ${D}${bindir}/imu-setup
+    install -m 755 ${WORKDIR}/imu-poll.py ${D}${bindir}/imu-poll
 
     install -d ${D}/etc
     install -m 644 ${WORKDIR}/imu-lsm6ds3.conf ${D}/etc/imu-lsm6ds3.conf