[PATCH v3 1/3] misc: servo-pwm: driver for controlling servo motors via PWM
From: Angelo Compagnucci
Date: Fri Feb 17 2023 - 11:11:07 EST
This patch adds a simple driver to control servo motor position via
PWM signal.
The driver allows to set the angle from userspace, while min/max
positions duty cycle and the motor degrees aperture are defined in
the dts.
Signed-off-by: Angelo Compagnucci <angelo@xxxxxxxxxxxxxxxxxxxx>
---
v2:
* Driver mostly rewritten for kernel 6.2
v3:
* Fixed sysfs_emit (greg k-h)
MAINTAINERS | 6 ++
drivers/misc/Kconfig | 11 +++
drivers/misc/Makefile | 1 +
drivers/misc/servo-pwm.c | 149 +++++++++++++++++++++++++++++++++++++++
4 files changed, 167 insertions(+)
create mode 100644 drivers/misc/servo-pwm.c
diff --git a/MAINTAINERS b/MAINTAINERS
index 39ff1a717625..8f4af64deb1b 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -8737,6 +8737,12 @@ F: Documentation/devicetree/bindings/power/power?domain*
F: drivers/base/power/domain*.c
F: include/linux/pm_domain.h
+GENERIC PWM SERVO DRIVER
+M: "Angelo Compagnucci" <angelo@xxxxxxxxxxxxxxxxxxxx>
+L: linux-pwm@xxxxxxxxxxxxxxx
+S: Maintained
+F: drivers/misc/servo-pwm.c
+
GENERIC RESISTIVE TOUCHSCREEN ADC DRIVER
M: Eugen Hristev <eugen.hristev@xxxxxxxxxxxxx>
L: linux-input@xxxxxxxxxxxxxxx
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 9947b7892bd5..8a74087149ac 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -518,6 +518,17 @@ config VCPU_STALL_DETECTOR
If you do not intend to run this kernel as a guest, say N.
+config SERVO_PWM
+ tristate "Servo motor positioning"
+ depends on PWM
+ help
+ Driver to control generic servo motor positioning.
+ Writing to the "angle" device attribute, the motor will move to
+ the angle position.
+
+ To compile this driver as a module, choose M here: the module
+ will be called servo-pwm.
+
source "drivers/misc/c2port/Kconfig"
source "drivers/misc/eeprom/Kconfig"
source "drivers/misc/cb710/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 87b54a4a4422..936629b648a9 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -64,3 +64,4 @@ obj-$(CONFIG_HI6421V600_IRQ) += hi6421v600-irq.o
obj-$(CONFIG_OPEN_DICE) += open-dice.o
obj-$(CONFIG_GP_PCI1XXXX) += mchp_pci1xxxx/
obj-$(CONFIG_VCPU_STALL_DETECTOR) += vcpu_stall_detector.o
+obj-$(CONFIG_SERVO_PWM) += servo-pwm.o
diff --git a/drivers/misc/servo-pwm.c b/drivers/misc/servo-pwm.c
new file mode 100644
index 000000000000..1303ddda8d07
--- /dev/null
+++ b/drivers/misc/servo-pwm.c
@@ -0,0 +1,149 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (c) 2023 Angelo Compagnucci <angelo@xxxxxxxxxxxxxxxxxxxx>
+ * servo-pwm.c - driver for controlling servo motors via pwm.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/pwm.h>
+
+#define DEFAULT_DUTY_MIN 500000
+#define DEFAULT_DUTY_MAX 2500000
+#define DEFAULT_DEGREES 175
+#define DEFAULT_ANGLE 0
+
+struct servo_pwm_data {
+ u32 duty_min;
+ u32 duty_max;
+ u32 degrees;
+ u32 angle;
+
+ struct mutex lock;
+ struct pwm_device *pwm;
+ struct pwm_state pwmstate;
+};
+
+static int servo_pwm_set(struct servo_pwm_data *data, int val)
+{
+ u64 new_duty = (((data->duty_max - data->duty_min) /
+ data->degrees) * val) + data->duty_min;
+ int ret;
+
+ mutex_lock(&data->lock);
+
+ data->pwmstate.duty_cycle = new_duty;
+ data->pwmstate.enabled = 1;
+ ret = pwm_apply_state(data->pwm, &data->pwmstate);
+
+ if (!ret)
+ data->angle = val;
+
+ mutex_unlock(&data->lock);
+
+ return ret;
+}
+
+static ssize_t angle_show(struct device *dev, struct device_attribute *attr,
+ char *buf)
+{
+ struct servo_pwm_data *data = dev_get_drvdata(dev);
+
+ return sysfs_emit(buf, PAGE_SIZE, "%u\n", data->angle);
+}
+
+static ssize_t angle_store(struct device *dev, struct device_attribute *attr,
+ const char *buf, size_t count)
+{
+ struct servo_pwm_data *data = dev_get_drvdata(dev);
+ int ret, val = 0;
+
+ ret = kstrtoint(buf, 10, &val);
+ if (ret < 0)
+ return -EINVAL;
+
+ if (val < 0 || val > data->degrees)
+ return -EINVAL;
+
+ ret = servo_pwm_set(data, val);
+ if (ret)
+ return ret;
+
+ return count;
+}
+
+static DEVICE_ATTR_RW(angle);
+
+static ssize_t degrees_show(struct device *dev, struct device_attribute *attr,
+ char *buf)
+{
+ struct servo_pwm_data *data = dev_get_drvdata(dev);
+
+ return sysfs_emit(buf, PAGE_SIZE, "%u\n", data->degrees);
+}
+
+static DEVICE_ATTR_RO(degrees);
+
+static struct attribute *servo_pwm_attrs[] = {
+ &dev_attr_angle.attr,
+ &dev_attr_degrees.attr,
+ NULL,
+};
+
+ATTRIBUTE_GROUPS(servo_pwm);
+
+static int servo_pwm_probe(struct platform_device *pdev)
+{
+ struct fwnode_handle *fwnode = dev_fwnode(&pdev->dev);
+ struct servo_pwm_data *data;
+
+ data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+ if (!data)
+ return -ENOMEM;
+
+ mutex_init(&data->lock);
+
+ if (!fwnode_property_read_u32(fwnode, "duty-min", &data->duty_min) == 0)
+ data->duty_min = DEFAULT_DUTY_MIN;
+
+ if (!fwnode_property_read_u32(fwnode, "duty-max", &data->duty_max) == 0)
+ data->duty_max = DEFAULT_DUTY_MAX;
+
+ if (!fwnode_property_read_u32(fwnode, "degrees", &data->degrees) == 0)
+ data->degrees = DEFAULT_DEGREES;
+
+ data->pwm = devm_fwnode_pwm_get(&pdev->dev, fwnode, NULL);
+ if (IS_ERR(data->pwm)) {
+ return dev_err_probe(&pdev->dev, PTR_ERR(data->pwm),
+ "unable to request PWM\n");
+ }
+
+ pwm_init_state(data->pwm, &data->pwmstate);
+
+ platform_set_drvdata(pdev, data);
+
+ return 0;
+}
+
+static const struct of_device_id of_servo_pwm_match[] = {
+ { .compatible = "servo-pwm", },
+ {},
+};
+MODULE_DEVICE_TABLE(of, of_servo_pwm_match);
+
+static struct platform_driver servo_pwm_driver = {
+ .probe = servo_pwm_probe,
+ .driver = {
+ .name = "servo-pwm",
+ .of_match_table = of_servo_pwm_match,
+ .dev_groups = servo_pwm_groups,
+ },
+};
+
+module_platform_driver(servo_pwm_driver);
+
+MODULE_AUTHOR("Angelo Compagnucci <angelo@xxxxxxxxxxxxxxxxxxxx>");
+MODULE_DESCRIPTION("generic PWM servo motor driver");
+MODULE_LICENSE("GPL");
--
2.34.1