[PATCH 3/4] cmd: Add vibrator command

[Samuel Dionne-Riel]

Signed-off-by: Samuel Dionne-Riel <samuel at dionne-riel.com>
---
 cmd/Kconfig    |  10 ++++
 cmd/Makefile   |   1 +
 cmd/vibrator.c | 148 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 159 insertions(+)
 create mode 100644 cmd/vibrator.c

diff --git a/cmd/Kconfig b/cmd/Kconfig
index e538e69a11..51e79ad806 100644
--- a/cmd/Kconfig
+++ b/cmd/Kconfig
@@ -1391,6 +1391,16 @@ config CMD_PVBLOCK
 	help
 	  Xen para-virtualized block device support
 
+config CMD_VIBRATOR
+	bool "vibrator"
+	depends on VIBRATOR
+	default y if VIBRATOR
+	help
+	  Enable the 'vibrator' command which allows for control of vibrator
+	  motors available on the board. The vibrator motors can be listed with
+	  'vibrator list' and controlled with vibrator on/off/time. Any
+	  vibrator driver can be controlled with this command.
+
 config CMD_VIRTIO
 	bool "virtio"
 	depends on VIRTIO
diff --git a/cmd/Makefile b/cmd/Makefile
index 6c4db4ed2e..49bf184bd9 100644
--- a/cmd/Makefile
+++ b/cmd/Makefile
@@ -164,6 +164,7 @@ obj-$(CONFIG_CMD_UBIFS) += ubifs.o
 obj-$(CONFIG_CMD_UNIVERSE) += universe.o
 obj-$(CONFIG_CMD_UNLZ4) += unlz4.o
 obj-$(CONFIG_CMD_UNZIP) += unzip.o
+obj-$(CONFIG_CMD_VIBRATOR) += vibrator.o
 obj-$(CONFIG_CMD_VIRTIO) += virtio.o
 obj-$(CONFIG_CMD_WDT) += wdt.o
 obj-$(CONFIG_CMD_LZMADEC) += lzmadec.o
diff --git a/cmd/vibrator.c b/cmd/vibrator.c
new file mode 100644
index 0000000000..b77cb4867a
--- /dev/null
+++ b/cmd/vibrator.c
@@ -0,0 +1,148 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2021 Samuel Dionne-Riel <samuel at dionne-riel.com>
+ * Copyright (c) 2017 Google, Inc
+ * Largely derived from `cmd/led.c`
+ * Original written by Simon Glass <sjg at chromium.org>
+ */
+
+#include <common.h>
+#include <command.h>
+#include <dm.h>
+#include <vibrator.h>
+#include <dm/uclass-internal.h>
+#include <linux/delay.h>
+
+static const char *const state_label[] = {
+	[VIBRATOR_STATE_OFF]	= "off",
+	[VIBRATOR_STATE_ON]	= "on",
+	[VIBRATOR_STATE_TOGGLE]	= "toggle",
+};
+
+enum vibrator_state_t get_vibrator_cmd(char *var)
+{
+	int i;
+
+	for (i = 0; i < VIBRATOR_STATE_COUNT; i++) {
+		if (!strncmp(var, state_label[i], strlen(var)))
+			return i;
+	}
+
+	return -1;
+}
+
+static int show_vibrator_state(struct udevice *dev)
+{
+	int ret;
+
+	ret = vibrator_get_state(dev);
+	if (ret >= VIBRATOR_STATE_COUNT)
+		ret = -EINVAL;
+	if (ret >= 0)
+		printf("%s\n", state_label[ret]);
+
+	return ret;
+}
+
+static int list_vibrators(void)
+{
+	struct udevice *dev;
+	int ret;
+
+	for (uclass_find_first_device(UCLASS_VIBRATOR, &dev);
+	     dev;
+	     uclass_find_next_device(&dev)) {
+		struct vibrator_uc_plat *plat = dev_get_uclass_plat(dev);
+
+		if (!plat->label)
+			continue;
+		printf("%-15s ", plat->label);
+		if (device_active(dev)) {
+			ret = show_vibrator_state(dev);
+			if (ret < 0)
+				printf("Error %d\n", ret);
+		} else {
+			printf("<inactive>\n");
+		}
+	}
+
+	return 0;
+}
+
+int timed_vibration(struct udevice *dev, int duration_ms)
+{
+	int ret;
+
+	ret = vibrator_set_state(dev, VIBRATOR_STATE_ON);
+	if (ret < 0) {
+		printf("Vibrator operation failed (err=%d)\n", ret);
+		return CMD_RET_FAILURE;
+	}
+
+	udelay(duration_ms * 1000);
+
+	ret = vibrator_set_state(dev, VIBRATOR_STATE_OFF);
+	if (ret < 0) {
+		printf("Vibrator operation failed (err=%d)\n", ret);
+		return CMD_RET_FAILURE;
+	}
+
+	return CMD_RET_SUCCESS;
+}
+
+int do_vibrator(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
+{
+	enum vibrator_state_t cmd;
+	const char *vibrator_label;
+	struct udevice *dev;
+	int ret;
+	int duration_ms = 0;
+
+	/* Validate arguments */
+	if (argc < 2)
+		return CMD_RET_USAGE;
+	vibrator_label = argv[1];
+	if (strncmp(vibrator_label, "list", 4) == 0)
+		return list_vibrators();
+
+	cmd = argc > 2 ? get_vibrator_cmd(argv[2]) : VIBRATOR_STATE_COUNT;
+	ret = vibrator_get_by_label(vibrator_label, &dev);
+	if (ret) {
+		printf("Vibrator '%s' not found (err=%d)\n", vibrator_label, ret);
+		return CMD_RET_FAILURE;
+	}
+
+	if (strncmp(argv[2], "timed", 5) == 0) {
+		if (argc < 4)
+			return CMD_RET_USAGE;
+		duration_ms = dectoul(argv[3], NULL);
+
+		return timed_vibration(dev, duration_ms);
+	}
+
+	switch (cmd) {
+	case VIBRATOR_STATE_OFF:
+	case VIBRATOR_STATE_ON:
+	case VIBRATOR_STATE_TOGGLE:
+		ret = vibrator_set_state(dev, cmd);
+		break;
+	case VIBRATOR_STATE_COUNT:
+		printf("Vibrator '%s': ", vibrator_label);
+		ret = show_vibrator_state(dev);
+		break;
+	}
+	if (ret < 0) {
+		printf("Vibrator '%s' operation failed (err=%d)\n", vibrator_label, ret);
+		return CMD_RET_FAILURE;
+	}
+
+	return 0;
+}
+
+U_BOOT_CMD(vibrator, 4, 1, do_vibrator,
+	   "manage vibration motors",
+	   "<vibrator_label> on|off\tChange vibration motor state\n"
+	   "vibrator <vibrator_label> timed <ms duration>\t\tVibrate for the given duration (will block)\n"
+	   "vibrator <vibrator_label>\tGet vibration motor state\n"
+	   "vibrator list\t\tShow a list of vibration motors"
+);
-- 
2.34.0