Module: xenomai-3
Branch: next
Commit: cb4339bd38ed71567bb006f4c0bb15c3e43e1761
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=cb4339bd38ed71567bb006f4c0bb15c3e43e1761
Author: Jorge Ramirez-Ortiz <j...@xenomai.org>
Date: Tue Mar 15 19:18:15 2016 -0400
gpiopwm: pwm signal generator and servo motor control demo code
---
demo/posix/cobalt/Makefile.am | 6 +
demo/posix/cobalt/gpiopwm.c | 448 ++++++++++++++++++++++++++++++++++
include/cobalt/kernel/rtdm/gpiopwm.h | 24 ++
include/rtdm/uapi/gpiopwm.h | 56 +++++
include/rtdm/uapi/rtdm.h | 1 +
kernel/drivers/Kconfig | 1 +
kernel/drivers/Makefile | 2 +-
kernel/drivers/gpiopwm/Kconfig | 9 +
kernel/drivers/gpiopwm/Makefile | 5 +
kernel/drivers/gpiopwm/gpiopwm.c | 301 +++++++++++++++++++++++
10 files changed, 852 insertions(+), 1 deletion(-)
diff --git a/demo/posix/cobalt/Makefile.am b/demo/posix/cobalt/Makefile.am
index eedc346..9e35cf1 100644
--- a/demo/posix/cobalt/Makefile.am
+++ b/demo/posix/cobalt/Makefile.am
@@ -3,6 +3,7 @@ demodir = @XENO_DEMO_DIR@
CCLD = $(top_srcdir)/scripts/wrap-link.sh $(CC)
demo_PROGRAMS = \
+ gpiopwm \
bufp-label \
bufp-readwrite \
can_rtt \
@@ -24,6 +25,11 @@ ldadd = \
@XENO_USER_LDADD@ \
-lpthread -lrt
+gpiopwm_SOURCES = gpiopwm.c
+gpiopwm_CPPFLAGS = $(cppflags) -I$(top_srcdir)/include/rtdm/uapi
+gpiopwm_LDFLAGS = $(ldflags)
+gpiopwm_LDADD = $(ldadd)
+
bufp_label_SOURCES = bufp-label.c
bufp_label_CPPFLAGS = $(cppflags)
bufp_label_LDFLAGS = $(ldflags)
diff --git a/demo/posix/cobalt/gpiopwm.c b/demo/posix/cobalt/gpiopwm.c
new file mode 100644
index 0000000..e093fbe
--- /dev/null
+++ b/demo/posix/cobalt/gpiopwm.c
@@ -0,0 +1,448 @@
+#include <xenomai/init.h>
+#include <rtdm/rtdm.h>
+#include <semaphore.h>
+#include <pthread.h>
+#include <gpiopwm.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <errno.h>
+#include <error.h>
+#include <stdio.h>
+#include <time.h>
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <netdb.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#define MIN_DUTY_CYCLE 0
+#define MAX_DUTY_CYCLE 100
+
+typedef void *(*gpiopwm_control_thread)(void *cookie);
+#define DEVICE_NAME "/dev/rtdm/gpiopwm"
+char *device_name;
+int dev;
+
+static sem_t synch;
+static sem_t setup;
+static int stop;
+static int step = 1;
+static int port = 66666;
+
+#define GPIO_PWM_SERVO_CONFIG \
+{ \
+ .duty_cycle = 50, \
+ .range_min = 950, \
+ .range_max = 2050, \
+ .period = 20000000, \
+ .gpio = 1, \
+}
+
+static struct gpiopwm config = GPIO_PWM_SERVO_CONFIG;
+
+static void fail(const char *reason)
+{
+ perror(reason);
+ exit(EXIT_FAILURE);
+}
+
+static void sem_sync(sem_t *sem)
+{
+ int ret;
+
+ for (;;) {
+ ret = sem_wait(sem);
+ if (ret == 0)
+ return;
+ if (errno != EINTR)
+ fail("sem_wait");
+ }
+}
+
+static inline void clear_screen(void)
+{
+ const char* cmd = "\e[1;1H\e[2J";
+ int ret;
+
+ ret = write(2, cmd, strlen(cmd));
+ if (!ret)
+ error(1, ret, "clear screen error");
+}
+
+static inline void print_config(char *str)
+{
+ printf("Config: %s\n", str);
+ printf(" device : %s\n", device_name);
+ printf(" range : [%d, %d]\n", config.range_min, config.range_max);
+ printf(" period : %d nsec\n", config.period);
+ printf(" gpio pin : %d\n", config.gpio);
+ printf(" duty cycle : %d\n", config.duty_cycle);
+}
+
+static inline void input_message(void)
+{
+ print_config("");
+ printf("\n GPIO PWM Control\n");
+ printf( " Enter duty_cycle [0-100] : ");
+}
+
+static void setup_sched_parameters(pthread_attr_t *attr, int prio)
+{
+ struct sched_param p;
+ int ret;
+
+ ret = pthread_attr_init(attr);
+ if (ret)
+ error(1, ret, "pthread_attr_init()");
+
+ ret = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED);
+ if (ret)
+ error(1, ret, "pthread_attr_setinheritsched()");
+
+ ret = pthread_attr_setschedpolicy(attr, prio ? SCHED_FIFO :
SCHED_OTHER);
+ if (ret)
+ error(1, ret, "pthread_attr_setschedpolicy()");
+
+ p.sched_priority = prio;
+ ret = pthread_attr_setschedparam(attr, &p);
+ if (ret)
+ error(1, ret, "pthread_attr_setschedparam()");
+}
+
+static void *gpiopwm_init_thread(void *cookie)
+{
+ int ret;
+
+ pthread_setname_np(pthread_self(), "gpio-pwm-handler");
+ ret = ioctl(dev, GPIOPWM_RTIOC_SET_CONFIG, config);
+ if (ret)
+ error(1, ret, "failed to set config");
+
+ ioctl(dev, GPIOPWM_RTIOC_START);
+
+ /* setup completed: allow handler to run */
+ sem_post(&setup);
+
+ /* wait for completion */
+ sem_sync(&synch);
+ ioctl(dev, GPIOPWM_RTIOC_STOP);
+
+ return NULL;
+}
+
+/*
+ * Controls the motor receving the duty cycle sent over UDP
+ * ie: echo -n <duty_cycle> | nc -w1 -u <ipaddr> <port>
+ */
+static void *gpiopwm_udp_ctrl_thread(void *cookie)
+{
+ struct sockaddr_in saddr;
+ struct sockaddr_in caddr;
+ unsigned int duty_cycle;
+ const int blen = 4;
+ int optval = 1;
+ socklen_t clen;
+ char buf[blen];
+ int sockfd;
+ int ret;
+
+ pthread_setname_np(pthread_self(), "gpio-pwm.netcat");
+
+ sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+ if (sockfd < 0)
+ perror("socket");
+
+ setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(int));
+
+ bzero((char *) &saddr, sizeof(saddr));
+ saddr.sin_addr.s_addr = htonl(INADDR_ANY);
+ saddr.sin_port = htons(port);
+ saddr.sin_family = AF_INET;
+
+ if (bind(sockfd, &saddr, sizeof(saddr)) < 0)
+ perror("bind");
+
+ clen = sizeof(caddr);
+ sem_sync(&setup);
+ for (;;) {
+
+ clear_screen();
+ print_config("UDP server");
+
+ memset(buf,'\0', blen);
+ ret = recvfrom(sockfd, buf, blen - 1, 0, &caddr, &clen);
+ if (ret < 0)
+ perror("recvfrom");
+
+ duty_cycle = strtol(buf, NULL, 10);
+ if (duty_cycle < MIN_DUTY_CYCLE || duty_cycle > MAX_DUTY_CYCLE)
+ continue;
+
+ ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, duty_cycle);
+ if (ret)
+ break;
+
+ config.duty_cycle = duty_cycle;
+ }
+
+ return NULL;
+}
+
+/*
+ * Manual control of the pwm duty cycle.
+ */
+static void *gpiopwm_manual_ctrl_thread(void *cookie)
+{
+ unsigned int duty_cycle;
+ size_t len = 4;
+ char *in;
+ int ret;
+
+ pthread_setname_np(pthread_self(), "gpio-pwm.manual");
+
+ in = malloc(len * sizeof(*in));
+ if (!in)
+ goto err;
+
+ sem_sync(&setup);
+ for (;;) {
+ clear_screen();
+ input_message();
+
+ len = getline(&in, &len, stdin);
+ if (len == -1 || len == 1)
+ break;
+
+ duty_cycle = atoi(in);
+ if (!duty_cycle && strncmp(in, "000", len - 1) != 0)
+ break;
+
+ ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, duty_cycle);
+ if (ret) {
+ fprintf(stderr, "invalid duty cycle %d\n", duty_cycle);
+ break;
+ }
+
+ config.duty_cycle = duty_cycle;
+ }
+
+ free(in);
+err:
+ sem_post(&synch);
+
+ return NULL;
+}
+
+/*
+ * Continuously sweep all duty cycles 0..100 and 100..0.
+ * No mode switches should occur.
+ */
+static void *gpiopwm_sweep_ctrl_thread(void *cookie)
+{
+ struct timespec delay;
+ struct duty_values {
+ enum { fwd, bck} direction;
+ int x;
+ } values;
+ int ret;
+
+ pthread_setname_np(pthread_self(), "gpio-pwm.sweep");
+
+ delay = (struct timespec) {.tv_sec = 0, .tv_nsec = 10 * config.period};
+ values = (struct duty_values) {.direction = fwd, .x = MIN_DUTY_CYCLE};
+
+ sem_sync(&setup);
+ for (;;) {
+ if (stop)
+ break;
+
+ ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, values.x);
+ if (ret) {
+ fprintf(stderr, "invalid duty cycle %d\n", values.x);
+ break;
+ }
+
+ nanosleep(&delay, NULL);
+
+ if (values.direction == bck) {
+ if (values.x - (step - 1) > MIN_DUTY_CYCLE)
+ values.x -= step;
+ else {
+ values.direction = fwd;
+ values.x = MIN_DUTY_CYCLE;
+ continue;
+ }
+ }
+
+ if (values.direction == fwd) {
+ if (values.x + (step - 1) < MAX_DUTY_CYCLE)
+ values.x += step;
+ else {
+ values.direction = bck;
+ values.x = MAX_DUTY_CYCLE;
+ }
+ }
+ }
+ sem_post(&synch);
+
+ return NULL;
+}
+
+static void gpiopwm_sweep_sig_handler(int sig)
+{
+ stop = 1;
+}
+
+static const struct option options[] = {
+ {
+#define help_opt 0
+ .name = "help",
+ .has_arg = 0,
+ .flag = NULL,
+ },
+ {
+#define sweep_range_opt 1
+ .name = "sweep",
+ .has_arg = 1,
+ .flag = NULL,
+ },
+ {
+#define manual_opt 2
+ .name = "manual",
+ .has_arg = 0,
+ .flag = NULL,
+ },
+ {
+#define config_opt 3
+ .name = "config",
+ .has_arg = 1,
+ .flag = NULL,
+ },
+ {
+#define udp_opt 4
+ .name = "udp",
+ .has_arg = 1,
+ .flag = NULL,
+ },
+ {
+ .name = NULL,
+ }
+};
+
+static void usage(void)
+{
+ fprintf(stderr, "Usage:\n"
+ "gpiopwm --config=dev:min:max:period:gpio:duty [--sweep=<step>
| --udp=<port> | --manual]\n\n"
+ "--config=<..>\n"
+ " dev: /dev/rtdm/gpio-pwm id [0..7]\n"
+ " min: min active period in usec\n"
+ " max: max active period in usec\n"
+ " period: base signal period in nsec\n"
+ " gpio: gpio pin number\n"
+ " duty: default duty cycle [0..100]\n"
+ "--sweep=<step>\n"
+ " sweep all duty cycle ranges in a loop\n"
+ " in step increments [default 1]\n"
+ "--manual input duty cycle from the command
line\n"
+ "--udp=<port> receive duty cycle from the network\n"
+ " ie: echo -n <duty_cycle> | nc -w1 -u
<ipaddr> <port>\n"
+ );
+}
+
+int main(int argc, char *argv[])
+{
+ gpiopwm_control_thread handler = NULL;
+ pthread_t pwm_task, ctrl_task;
+ int opt, lindex, device = 0;
+ pthread_attr_t tattr;
+ char *p;
+ int ret;
+
+ for (;;) {
+ lindex = -1;
+ opt = getopt_long_only(argc, argv, "", options, &lindex);
+ if (opt == EOF)
+ break;
+
+ switch (lindex) {
+ case sweep_range_opt:
+ handler = gpiopwm_sweep_ctrl_thread;
+ signal(SIGINT, gpiopwm_sweep_sig_handler);
+ step = atoi(optarg);
+ step = step < 1 ? 1 : step;
+ break;
+ case manual_opt:
+ handler = gpiopwm_manual_ctrl_thread;
+ signal(SIGINT, SIG_IGN);
+ break;
+ case udp_opt:
+ handler = gpiopwm_udp_ctrl_thread;
+ port = atoi(optarg);
+ break;
+ case config_opt:
+ p = strtok(optarg,":");
+ device = p ? atoi(p): -1;
+ p = strtok(NULL,":");
+ config.range_min = p ? atoi(p): -1;
+ p = strtok(NULL,":");
+ config.range_max = p ? atoi(p): -1;
+ p = strtok(NULL,":");
+ config.period = p ? atoi(p): -1;
+ p = strtok(NULL,":");
+ config.gpio = p ? atoi(p): -1;
+ p = strtok(NULL,"");
+ config.duty_cycle = p ? atoi(p): -1;
+ break;
+ case help_opt:
+ default:
+ usage();
+ exit(1);
+ }
+ }
+
+ if (handler == NULL) {
+ usage();
+ exit(1);
+ }
+
+ ret = sem_init(&synch, 0, 0);
+ if (ret < 0)
+ error(1, errno, "can't create synch semaphore");
+
+ ret = sem_init(&setup, 0, 0);
+ if (ret < 0)
+ error(1, errno, "can't create setup semaphore");
+
+ ret = asprintf(&device_name, "%s%d", DEVICE_NAME, device);
+ if (ret < 0)
+ error(1, EINVAL, "can't create device name");
+
+ dev = open(device_name, O_RDWR);
+ if (dev < 0)
+ error(1, EINVAL, "can't open %s", device_name);
+
+ setup_sched_parameters(&tattr, 99);
+ ret = pthread_create(&ctrl_task, &tattr, handler, NULL);
+ if (ret)
+ error(1, ret, "pthread_create(ctrl_handler)");
+
+ setup_sched_parameters(&tattr, 98);
+ ret = pthread_create(&pwm_task, &tattr, gpiopwm_init_thread, NULL);
+ if (ret)
+ error(1, ret, "pthread_create(init thread)");
+
+ pthread_join(pwm_task, NULL);
+ pthread_join(ctrl_task, NULL);
+
+ pthread_attr_destroy(&tattr);
+
+ ret = close(dev);
+ if (ret < 0)
+ error(1, EINVAL, "can't close");
+
+ return 0;
+}
diff --git a/include/cobalt/kernel/rtdm/gpiopwm.h
b/include/cobalt/kernel/rtdm/gpiopwm.h
new file mode 100644
index 0000000..e38d241
--- /dev/null
+++ b/include/cobalt/kernel/rtdm/gpiopwm.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2015 Jorge Ramirez <j...@xenomai.org>
+ *
+ * Xenomai is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Xenomai is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Xenomai; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+#ifndef _COBALT_RTDM_PWM_H
+#define _COBALT_RTDM_PWM_H
+
+#include <rtdm/rtdm.h>
+#include <rtdm/uapi/gpiopwm.h>
+
+#endif /* !_COBALT_RTDM_PWM_H */
diff --git a/include/rtdm/uapi/gpiopwm.h b/include/rtdm/uapi/gpiopwm.h
new file mode 100644
index 0000000..512c89c
--- /dev/null
+++ b/include/rtdm/uapi/gpiopwm.h
@@ -0,0 +1,56 @@
+/**
+ * @file
+ * Real-Time Driver Model for Xenomai, pwm header
+ *
+ * @note Copyright (C) 2015 Jorge Ramirez <j...@xenomai.org>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ *
+ * @ingroup rttesting
+ */
+#ifndef _RTDM_UAPI_PWM_H
+#define _RTDM_UAPI_PWM_H
+
+#include <linux/types.h>
+
+#define RTPWM_PROFILE_VER 1
+
+struct gpiopwm {
+ unsigned int duty_cycle;
+ unsigned int range_min;
+ unsigned int range_max;
+ unsigned int period;
+ unsigned int gpio;
+};
+
+#define RTIOC_TYPE_PWM RTDM_CLASS_PWM
+
+#define GPIOPWM_RTIOC_SET_CONFIG \
+ _IOW(RTIOC_TYPE_PWM, 0x00, struct gpiopwm)
+
+#define GPIOPWM_RTIOC_GET_CONFIG \
+ _IOR(RTIOC_TYPE_PWM, 0x10, struct gpiopwm)
+
+#define GPIOPWM_RTIOC_START \
+ _IO(RTIOC_TYPE_PWM, 0x20)
+
+#define GPIOPWM_RTIOC_STOP \
+ _IO(RTIOC_TYPE_PWM, 0x30)
+
+#define GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE \
+ _IOW(RTIOC_TYPE_PWM, 0x40, unsigned int)
+
+
+#endif /* !_RTDM_UAPI_TESTING_H */
diff --git a/include/rtdm/uapi/rtdm.h b/include/rtdm/uapi/rtdm.h
index ee6de65..80c789a 100644
--- a/include/rtdm/uapi/rtdm.h
+++ b/include/rtdm/uapi/rtdm.h
@@ -81,6 +81,7 @@ typedef int64_t nanosecs_rel_t;
#define RTDM_CLASS_MEMORY 10
#define RTDM_CLASS_GPIO 11
#define RTDM_CLASS_SPI 12
+#define RTDM_CLASS_PWM 13
#define RTDM_CLASS_MISC 223
#define RTDM_CLASS_EXPERIMENTAL 224
diff --git a/kernel/drivers/Kconfig b/kernel/drivers/Kconfig
index c2bc904..197a48e 100644
--- a/kernel/drivers/Kconfig
+++ b/kernel/drivers/Kconfig
@@ -29,6 +29,7 @@ source "drivers/xenomai/analogy/Kconfig"
source "drivers/xenomai/ipc/Kconfig"
source "drivers/xenomai/udd/Kconfig"
source "drivers/xenomai/gpio/Kconfig"
+source "drivers/xenomai/gpiopwm/Kconfig"
source "drivers/xenomai/spi/Kconfig"
endmenu
diff --git a/kernel/drivers/Makefile b/kernel/drivers/Makefile
index 1402094..b8fe1b3 100644
--- a/kernel/drivers/Makefile
+++ b/kernel/drivers/Makefile
@@ -1 +1 @@
-obj-$(CONFIG_XENOMAI) += autotune/ serial/ testing/ can/ net/ analogy/ ipc/
udd/ gpio/ spi/
+obj-$(CONFIG_XENOMAI) += autotune/ serial/ testing/ can/ net/ analogy/ ipc/
udd/ gpio/ gpiopwm/ spi/
diff --git a/kernel/drivers/gpiopwm/Kconfig b/kernel/drivers/gpiopwm/Kconfig
new file mode 100644
index 0000000..532742a
--- /dev/null
+++ b/kernel/drivers/gpiopwm/Kconfig
@@ -0,0 +1,9 @@
+menu "GPIOPWM support"
+
+config XENO_DRIVERS_GPIOPWM
+ tristate "GPIOPWM driver"
+ help
+
+ An RTDM-based GPIO PWM generator driver
+
+endmenu
diff --git a/kernel/drivers/gpiopwm/Makefile b/kernel/drivers/gpiopwm/Makefile
new file mode 100644
index 0000000..c5d2bd6
--- /dev/null
+++ b/kernel/drivers/gpiopwm/Makefile
@@ -0,0 +1,5 @@
+ccflags-y += -Ikernel -Iinclude/xenomai/
+
+obj-$(CONFIG_XENO_DRIVERS_GPIOPWM) += xeno_gpiopwm.o
+
+xeno_gpiopwm-y := gpiopwm.o
diff --git a/kernel/drivers/gpiopwm/gpiopwm.c b/kernel/drivers/gpiopwm/gpiopwm.c
new file mode 100644
index 0000000..114afa7
--- /dev/null
+++ b/kernel/drivers/gpiopwm/gpiopwm.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) 2015 Jorge Ramirez <j...@xenomai.org>.
+ *
+ * Xenomai is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Xenomai is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Xenomai; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <rtdm/driver.h>
+#include <rtdm/gpiopwm.h>
+
+MODULE_AUTHOR("Jorge Ramirez <j...@xenomai.org>");
+MODULE_DESCRIPTION("PWM driver");
+MODULE_VERSION("0.0.1");
+MODULE_LICENSE("GPL");
+
+#define MAX_DUTY_CYCLE 100
+#define MAX_SAMPLES (MAX_DUTY_CYCLE + 1)
+
+struct gpiopwm_base_signal {
+ unsigned long period;
+};
+
+struct gpiopwm_duty_signal {
+ unsigned int range_min;
+ unsigned int range_max;
+ unsigned long period;
+ unsigned int cycle;
+};
+
+struct gpiopwm_control {
+ struct gpiopwm_duty_signal duty;
+ unsigned int configured;
+ unsigned int update;
+};
+
+struct gpiopwm_priv {
+ struct gpiopwm_base_signal base;
+ struct gpiopwm_duty_signal duty;
+ struct gpiopwm_control ctrl;
+
+ rtdm_timer_t base_timer;
+ rtdm_timer_t duty_timer;
+
+ int gpio;
+};
+
+static inline int div100(long long dividend)
+{
+ const long long divisor = 0x28f5c29;
+ return ((divisor * dividend) >> 32) & 0xffffffff;
+}
+
+static inline unsigned long duty_period(struct gpiopwm_duty_signal *p)
+{
+ unsigned long period;
+
+ period = p->range_min + div100((p->range_max - p->range_min) *
p->cycle);
+ return period * 1000;
+}
+
+static void gpiopwm_handle_base_timer(rtdm_timer_t *timer)
+{
+ struct gpiopwm_priv *ctx = container_of(timer, struct gpiopwm_priv,
+ base_timer);
+ gpio_set_value(ctx->gpio, 1);
+
+ /* one shot timer to avoid carrying over errors */
+ rtdm_timer_start_in_handler(&ctx->duty_timer, ctx->duty.period, 0,
+ RTDM_TIMERMODE_RELATIVE);
+
+ if (ctx->ctrl.update) {
+ ctx->duty.period = ctx->ctrl.duty.period;
+ ctx->duty.cycle = ctx->ctrl.duty.cycle;
+ ctx->ctrl.update = 0;
+ }
+}
+
+static void gpiopwm_handle_duty_timer(rtdm_timer_t *timer)
+{
+ struct gpiopwm_priv *ctx = container_of(timer, struct gpiopwm_priv,
+ duty_timer);
+ gpio_set_value(ctx->gpio, 0);
+}
+
+static inline int gpiopwm_config(struct rtdm_fd *fd, struct gpiopwm *conf)
+{
+ struct rtdm_dev_context *dev_ctx = rtdm_fd_to_context(fd);
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+ int ret;
+
+ if (ctx->ctrl.configured)
+ return -EINVAL;
+
+ if (conf->duty_cycle > MAX_DUTY_CYCLE)
+ return -EINVAL;
+
+ ret = gpio_request(conf->gpio, dev_ctx->device->name);
+ if (ret < 0) {
+ ctx->gpio = -1;
+ return ret;
+ }
+
+ ret = gpio_direction_output(conf->gpio, 0);
+ if (ret < 0)
+ return ret;
+
+ gpio_set_value(conf->gpio, 0);
+
+ ctx->duty.range_min = ctx->ctrl.duty.range_min = conf->range_min;
+ ctx->duty.range_max = ctx->ctrl.duty.range_max = conf->range_max;
+ ctx->duty.cycle = conf->duty_cycle;
+ ctx->base.period = conf->period;
+ ctx->gpio = conf->gpio;
+ ctx->duty.period = duty_period(&ctx->duty);
+
+ rtdm_timer_init(&ctx->base_timer, gpiopwm_handle_base_timer,
"base_timer");
+ rtdm_timer_init(&ctx->duty_timer, gpiopwm_handle_duty_timer,
"duty_timer");
+
+ ctx->ctrl.configured = 1;
+
+ return 0;
+}
+
+static inline int gpiopwm_change_duty_cycle(struct gpiopwm_priv *ctx, unsigned
int cycle)
+{
+ if (cycle > MAX_DUTY_CYCLE)
+ return -EINVAL;
+
+ /* prepare the new data on the calling thread */
+ ctx->ctrl.duty.cycle = cycle;
+ ctx->ctrl.duty.period = duty_period(&ctx->ctrl.duty);
+
+ /* update data on the next base signal timeout */
+ ctx->ctrl.update = 1;
+
+ return 0;
+}
+
+static inline int gpiopwm_stop(struct rtdm_fd *fd)
+{
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+
+ if (!ctx->ctrl.configured)
+ return -EINVAL;
+
+ gpio_set_value(ctx->gpio, 0);
+
+ rtdm_timer_stop(&ctx->base_timer);
+ rtdm_timer_stop(&ctx->duty_timer);
+
+ return 0;
+}
+
+static inline int gpiopwm_start(struct rtdm_fd *fd)
+{
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+
+ if (!ctx->ctrl.configured)
+ return -EINVAL;
+
+ /* update duty cycle on next timeout */
+ ctx->ctrl.update = 1;
+
+ /* start the base signal tick */
+ rtdm_timer_start(&ctx->base_timer, ctx->base.period, ctx->base.period,
+ RTDM_TIMERMODE_RELATIVE);
+
+ return 0;
+}
+
+static int gpiopwm_ioctl_rt(struct rtdm_fd *fd, unsigned int request, void
__user *arg)
+{
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+
+ switch (request) {
+ case GPIOPWM_RTIOC_SET_CONFIG:
+ return -ENOSYS;
+ case GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE:
+ return gpiopwm_change_duty_cycle(ctx, (unsigned long) arg);
+ case GPIOPWM_RTIOC_START:
+ return gpiopwm_start(fd);
+ case GPIOPWM_RTIOC_STOP:
+ return gpiopwm_stop(fd);
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int gpiopwm_ioctl_nrt(struct rtdm_fd *fd, unsigned int request, void
__user *arg)
+{
+ struct gpiopwm conf;
+
+ switch (request) {
+ case GPIOPWM_RTIOC_SET_CONFIG:
+ if (!rtdm_rw_user_ok(fd, arg, sizeof(conf)))
+ return -EFAULT;
+
+ rtdm_copy_from_user(fd, &conf, arg, sizeof(conf));
+ return gpiopwm_config(fd, &conf);
+ case GPIOPWM_RTIOC_GET_CONFIG:
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int gpiopwm_open(struct rtdm_fd *fd, int oflags)
+{
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+
+ ctx->ctrl.configured = 0;
+ ctx->gpio = -1;
+
+ return 0;
+}
+
+static void gpiopwm_close(struct rtdm_fd *fd)
+{
+ struct gpiopwm_priv *ctx = rtdm_fd_to_private(fd);
+
+ if (ctx->gpio >= 0)
+ gpio_free(ctx->gpio);
+
+ if (!ctx->ctrl.configured)
+ return;
+
+ rtdm_timer_destroy(&ctx->base_timer);
+ rtdm_timer_destroy(&ctx->duty_timer);
+}
+
+static struct rtdm_driver gpiopwm_driver = {
+ .profile_info = RTDM_PROFILE_INFO(gpiopwm,
+ RTDM_CLASS_PWM,
+ RTDM_SUBCLASS_GENERIC,
+ RTPWM_PROFILE_VER),
+ .device_flags = RTDM_NAMED_DEVICE | RTDM_EXCLUSIVE,
+ .device_count = 8,
+ .context_size = sizeof(struct gpiopwm_priv),
+ .ops = {
+ .open = gpiopwm_open,
+ .close = gpiopwm_close,
+ .ioctl_rt = gpiopwm_ioctl_rt,
+ .ioctl_nrt = gpiopwm_ioctl_nrt,
+ },
+};
+
+static struct rtdm_device device[8] = {
+ [0 ... 7] = {
+ .driver = &gpiopwm_driver,
+ .label = "gpiopwm%d",
+ }
+};
+
+static int __init __gpiopwm_init(void)
+{
+ int i, ret;
+
+ if (!realtime_core_enabled())
+ return -ENODEV;
+
+ for (i = 0; i < ARRAY_SIZE(device); i++) {
+ ret = rtdm_dev_register(device + i);
+ if (ret)
+ goto fail;
+ }
+
+ return 0;
+fail:
+ while (i-- > 0)
+ rtdm_dev_unregister(device + i);
+
+ return ret;
+}
+
+static void __exit __gpiopwm_exit(void)
+{
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(device); i++)
+ rtdm_dev_unregister(device + i);
+}
+
+module_init(__gpiopwm_init);
+module_exit(__gpiopwm_exit);
_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
https://xenomai.org/mailman/listinfo/xenomai-git
