mirror of https://github.com/Desuuuu/klipper.git
linux: userspace GPIO control
Allow use host GPIO pins for non-realtime purposes. Signed-off-by: Andrey Kunitsyn <blackicebox@gmail.com>
This commit is contained in:
parent
f0c394de81
commit
8fd330c54e
|
@ -1,5 +1,6 @@
|
|||
# This is a travis-ci.org continuous integration configuration file.
|
||||
language: c
|
||||
dist: bionic
|
||||
|
||||
addons:
|
||||
apt:
|
||||
|
|
|
@ -10,6 +10,8 @@ config LINUX_SELECT
|
|||
select HAVE_GPIO_SPI
|
||||
select HAVE_GPIO_HARD_PWM
|
||||
select HAVE_GPIO_I2C
|
||||
select HAVE_GPIO_BITBANGING
|
||||
select HAVE_GPIO
|
||||
|
||||
config BOARD_DIRECTORY
|
||||
string
|
||||
|
|
|
@ -4,7 +4,7 @@ dirs-y += src/linux src/generic
|
|||
|
||||
src-y += linux/main.c linux/timer.c linux/console.c linux/watchdog.c
|
||||
src-y += linux/pca9685.c linux/spidev.c linux/analog.c linux/hard_pwm.c
|
||||
src-y += linux/i2c.c generic/crc16_ccitt.c generic/alloc.c
|
||||
src-y += linux/i2c.c linux/gpio.c generic/crc16_ccitt.c generic/alloc.c
|
||||
|
||||
CFLAGS_klipper.elf += -lutil
|
||||
|
||||
|
|
|
@ -15,15 +15,17 @@
|
|||
|
||||
DECL_CONSTANT("ADC_MAX", 4095); // Assume 12bit adc
|
||||
|
||||
DECL_ENUMERATION_RANGE("pin", "analog0", 0, 8);
|
||||
#define ANALOG_START (1<<8)
|
||||
|
||||
DECL_ENUMERATION_RANGE("pin", "analog0", ANALOG_START, 8);
|
||||
|
||||
#define IIO_PATH "/sys/bus/iio/devices/iio:device0/in_voltage%d_raw"
|
||||
|
||||
struct gpio_adc
|
||||
gpio_adc_setup(uint8_t pin)
|
||||
gpio_adc_setup(uint32_t pin)
|
||||
{
|
||||
char fname[256];
|
||||
snprintf(fname, sizeof(fname), IIO_PATH, pin);
|
||||
snprintf(fname, sizeof(fname), IIO_PATH, pin-ANALOG_START);
|
||||
|
||||
int fd = open(fname, O_RDONLY|O_CLOEXEC);
|
||||
if (fd < 0) {
|
||||
|
|
|
@ -53,7 +53,7 @@ set_non_blocking(int fd)
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
int
|
||||
set_close_on_exec(int fd)
|
||||
{
|
||||
int ret = fcntl(fd, F_SETFD, FD_CLOEXEC);
|
||||
|
|
|
@ -0,0 +1,163 @@
|
|||
// Very basic support via a Linux gpiod device
|
||||
//
|
||||
// Copyright (C) 2017-2018 Kevin O'Connor <kevin@koconnor.net>
|
||||
//
|
||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
#include "autoconf.h"
|
||||
#include "gpio.h"
|
||||
#include "command.h" // shutdown
|
||||
#include "sched.h" // shutdown
|
||||
|
||||
#include <fcntl.h> // open
|
||||
#include <stdio.h> // snprintf
|
||||
#include <string.h> // memset
|
||||
#include <stdlib.h> // atexit
|
||||
|
||||
#include "internal.h" // report_errno
|
||||
|
||||
#include </usr/include/linux/gpio.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#define CHIP_FILE_NAME "/dev/gpiochip0"
|
||||
#define GPIO_CONSUMER "klipper"
|
||||
#define NUM_LINES 40
|
||||
|
||||
DECL_ENUMERATION_RANGE("pin", "P0", 0, NUM_LINES);
|
||||
|
||||
struct gpio_line {
|
||||
int offset;
|
||||
int fd;
|
||||
int state;
|
||||
};
|
||||
|
||||
static struct gpio_line lines[NUM_LINES];
|
||||
|
||||
static int gpio_chip_fd = -1;
|
||||
|
||||
static int
|
||||
get_chip_fd(void) {
|
||||
int i = 0;
|
||||
if (gpio_chip_fd <= 0) {
|
||||
gpio_chip_fd = open(CHIP_FILE_NAME,O_RDWR | O_CLOEXEC);
|
||||
if (gpio_chip_fd < 0) {
|
||||
report_errno("open " CHIP_FILE_NAME,-1);
|
||||
shutdown("Unable to open GPIO chip device");
|
||||
}
|
||||
for (i=0; i<NUM_LINES; ++i) {
|
||||
lines[i].offset = i;
|
||||
lines[i].fd = -1;
|
||||
}
|
||||
}
|
||||
return gpio_chip_fd;
|
||||
}
|
||||
|
||||
// Dummy versions of gpio_out functions
|
||||
struct gpio_out
|
||||
gpio_out_setup(uint32_t pin, uint8_t val)
|
||||
{
|
||||
struct gpio_line* line = &lines[pin];
|
||||
line->offset = pin;
|
||||
struct gpio_out g = { .line = line };
|
||||
gpio_out_reset(g,val);
|
||||
return g;
|
||||
}
|
||||
|
||||
static void
|
||||
gpio_release_line(struct gpio_line* line)
|
||||
{
|
||||
if (line->fd > 0) {
|
||||
close(line->fd);
|
||||
line->fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
gpio_out_reset(struct gpio_out g, uint8_t val)
|
||||
{
|
||||
int rv;
|
||||
struct gpiohandle_request req;
|
||||
gpio_release_line(g.line);
|
||||
memset(&req, 0, sizeof(req));
|
||||
req.lines = 1;
|
||||
req.flags = GPIOHANDLE_REQUEST_OUTPUT;
|
||||
req.lineoffsets[0] = g.line->offset;
|
||||
req.default_values[0] = !!val;
|
||||
strncpy(req.consumer_label,GPIO_CONSUMER,sizeof(req.consumer_label) - 1);
|
||||
rv = ioctl(get_chip_fd(), GPIO_GET_LINEHANDLE_IOCTL, &req);
|
||||
if (rv < 0) {
|
||||
report_errno("gpio_out_reset get line",rv);
|
||||
shutdown("Unable to open out GPIO chip line");
|
||||
}
|
||||
set_close_on_exec(req.fd);
|
||||
g.line->fd = req.fd;
|
||||
g.line->state = !!val;
|
||||
}
|
||||
|
||||
void
|
||||
gpio_out_write(struct gpio_out g, uint8_t val)
|
||||
{
|
||||
struct gpiohandle_data data;
|
||||
memset(&data, 0, sizeof(data));
|
||||
data.values[0] = !!val;
|
||||
ioctl(g.line->fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
|
||||
g.line->state = !!val;
|
||||
}
|
||||
|
||||
void
|
||||
gpio_out_toggle(struct gpio_out g)
|
||||
{
|
||||
gpio_out_write(g,!g.line->state);
|
||||
}
|
||||
|
||||
void
|
||||
gpio_out_toggle_noirq(struct gpio_out g)
|
||||
{
|
||||
gpio_out_toggle(g);
|
||||
}
|
||||
|
||||
struct gpio_in
|
||||
gpio_in_setup(uint32_t pin, int8_t pull_up)
|
||||
{
|
||||
struct gpio_line* line = &lines[pin];
|
||||
line->offset = pin;
|
||||
struct gpio_in g = { .line = line };
|
||||
gpio_in_reset(g,pull_up);
|
||||
return g;
|
||||
}
|
||||
|
||||
void
|
||||
gpio_in_reset(struct gpio_in g, int8_t pull_up)
|
||||
{
|
||||
int rv;
|
||||
struct gpiohandle_request req;
|
||||
gpio_release_line(g.line);
|
||||
memset(&req, 0, sizeof(req));
|
||||
req.lines = 1;
|
||||
req.flags = GPIOHANDLE_REQUEST_INPUT;
|
||||
#if defined(GPIOD_LINE_REQUEST_FLAG_BIAS_PULL_UP)
|
||||
if (pull_up > 0) {
|
||||
req.flags |= GPIOD_LINE_REQUEST_FLAG_BIAS_PULL_UP;
|
||||
} else if (pull_up < 0) {
|
||||
req.flags |= GPIOD_LINE_REQUEST_FLAG_BIAS_PULL_DOWN;
|
||||
}
|
||||
#endif
|
||||
req.lineoffsets[0] = g.line->offset;
|
||||
strncpy(req.consumer_label,GPIO_CONSUMER,sizeof(req.consumer_label) - 1);
|
||||
rv = ioctl(get_chip_fd(), GPIO_GET_LINEHANDLE_IOCTL, &req);
|
||||
if (rv < 0) {
|
||||
report_errno("gpio_in_reset get line",rv);
|
||||
shutdown("Unable to open in GPIO chip line");
|
||||
}
|
||||
set_close_on_exec(req.fd);
|
||||
g.line->fd = req.fd;
|
||||
}
|
||||
|
||||
uint8_t
|
||||
gpio_in_read(struct gpio_in g)
|
||||
{
|
||||
struct gpiohandle_data data;
|
||||
memset(&data, 0, sizeof(data));
|
||||
ioctl(g.line->fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data);
|
||||
return data.values[0];
|
||||
}
|
|
@ -4,17 +4,25 @@
|
|||
#include <stdint.h> // uint8_t
|
||||
|
||||
struct gpio_out {
|
||||
uint32_t pin;
|
||||
struct gpio_line* line;
|
||||
};
|
||||
struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val);
|
||||
struct gpio_out gpio_out_setup(uint32_t pin, uint8_t val);
|
||||
void gpio_out_reset(struct gpio_out g, uint8_t val);
|
||||
void gpio_out_toggle_noirq(struct gpio_out g);
|
||||
void gpio_out_toggle(struct gpio_out g);
|
||||
void gpio_out_write(struct gpio_out g, uint8_t val);
|
||||
|
||||
struct gpio_in {
|
||||
struct gpio_line* line;
|
||||
};
|
||||
struct gpio_in gpio_in_setup(uint32_t pin, int8_t pull_up);
|
||||
void gpio_in_reset(struct gpio_in g, int8_t pull_up);
|
||||
uint8_t gpio_in_read(struct gpio_in g);
|
||||
|
||||
struct gpio_adc {
|
||||
int fd;
|
||||
};
|
||||
struct gpio_adc gpio_adc_setup(uint8_t pin);
|
||||
struct gpio_adc gpio_adc_setup(uint32_t pin);
|
||||
uint32_t gpio_adc_sample(struct gpio_adc g);
|
||||
uint16_t gpio_adc_read(struct gpio_adc g);
|
||||
void gpio_adc_cancel_sample(struct gpio_adc g);
|
||||
|
@ -32,7 +40,7 @@ struct gpio_pwm {
|
|||
int fd;
|
||||
uint32_t period;
|
||||
};
|
||||
struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint16_t val);
|
||||
struct gpio_pwm gpio_pwm_setup(uint32_t pin, uint32_t cycle_time, uint16_t val);
|
||||
void gpio_pwm_write(struct gpio_pwm g, uint16_t val);
|
||||
|
||||
struct i2c_config {
|
||||
|
|
|
@ -37,7 +37,7 @@ DECL_ENUMERATION_RANGE("pin", "pwmchip7/pwm0", HARD_PWM(7, 0), 16);
|
|||
#define PWM_PATH "/sys/class/pwm/pwmchip%u/pwm%u/%s"
|
||||
#define PWM_PATH_BB "/sys/class/pwm/pwm-%u:%u/%s"
|
||||
|
||||
struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint16_t val)
|
||||
struct gpio_pwm gpio_pwm_setup(uint32_t pin, uint32_t cycle_time, uint16_t val)
|
||||
{
|
||||
char filename[256];
|
||||
char scratch[16];
|
||||
|
|
|
@ -11,6 +11,7 @@
|
|||
// console.c
|
||||
void report_errno(char *where, int rc);
|
||||
int set_non_blocking(int fd);
|
||||
int set_close_on_exec(int fd);
|
||||
int console_setup(char *name);
|
||||
void console_sleep(struct timespec ts);
|
||||
|
||||
|
|
|
@ -107,15 +107,3 @@ spi_transfer(struct spi_config config, uint8_t receive_data
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Dummy versions of gpio_out functions
|
||||
struct gpio_out
|
||||
gpio_out_setup(uint8_t pin, uint8_t val)
|
||||
{
|
||||
shutdown("gpio_out_setup not supported");
|
||||
}
|
||||
|
||||
void
|
||||
gpio_out_write(struct gpio_out g, uint8_t val)
|
||||
{
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue