Module: xenomai-3
Branch: next
Commit: 477006f93072807a16272c60c3dcdecb6d17a7bb
URL:    
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=477006f93072807a16272c60c3dcdecb6d17a7bb

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 cba9247..2a22967 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

Reply via email to