Module: xenomai-3 Branch: next Commit: ea4a140e31286aee23505c0495303c6d0825aaf4 URL: http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=ea4a140e31286aee23505c0495303c6d0825aaf4
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