[PATCH u-boot-marvell v2 30/39] tools: kwboot: Check whether baudrate was set to requested value

[Marek Behún]

The tcsetattr() function can return 0 even if baudrate was not changed.
Check whether baudrate was changed to requested value, and in case of
arbitrary baudrate, check whether the set value is within 3% tolerance.

Signed-off-by: Marek Behún <marek.behun at nic.cz>
---
 tools/kwboot.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c
index f1fd184382..2572096267 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -567,6 +567,13 @@ kwboot_tty_baudrate_to_speed(int baudrate)
 	}
 }
 
+static int
+_is_within_tolerance(int value, int reference, int tolerance)
+{
+	return 100 * value >= reference * (100 - tolerance) &&
+	       100 * value <= reference * (100 + tolerance);
+}
+
 static int
 kwboot_tty_change_baudrate(int fd, int baudrate)
 {
@@ -601,7 +608,32 @@ kwboot_tty_change_baudrate(int fd, int baudrate)
 	if (rc)
 		return rc;
 
+	rc = tcgetattr(fd, &tio);
+	if (rc)
+		return rc;
+
+	if (cfgetospeed(&tio) != speed || cfgetispeed(&tio) != speed)
+		goto baud_fail;
+
+#ifdef BOTHER
+	/*
+	 * Check whether set baudrate is within 3% tolerance.
+	 * If BOTHER is defined, Linux always fills out c_ospeed / c_ispeed
+	 * with real values.
+	 */
+	if (!_is_within_tolerance(tio.c_ospeed, baudrate, 3))
+		goto baud_fail;
+
+	if (!_is_within_tolerance(tio.c_ispeed, baudrate, 3))
+		goto baud_fail;
+#endif
+
 	return 0;
+
+baud_fail:
+	fprintf(stderr, "Could not set baudrate to requested value\n");
+	errno = EINVAL;
+	return -1;
 }
 
 static int
-- 
2.32.0