操作gpio - mmap and gpiohandle_request, gpiochip

gpio.h

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <sched.h>
#include <sys/mman.h>
#include <errno.h>
#include <linux/ioctl.h>
#include <linux/types.h>

#define MMIO_SUCCESS 0
#define MMIO_ERROR_DEVMEM -1
#define MMIO_ERROR_MMAP -2

#define PAD_TOP_BK_ADDR     (0x00000B00)
#define TOP_GPIO_OUT_OFFSET (0x0000006A)
#define TOP_GPIO_IN_OFFSET  (0x00000019)
#define PAD_TOP_BK_ADDR     (0x00000B00)
#define TOP_GPIO_OUT_OFFSET (0x0000006A)
#define TOP_GPIO_IN_OFFSET  (0x00000019)
#define GPIO_OUT_REG_ADDR   (0x1F000000+(PAD_TOP_BK_ADDR<<1)+(TOP_GPIO_OUT_OFFSET<<2))
#define GPIO_IN_REG_ADDR    (0x1F000000+(PAD_TOP_BK_ADDR<<1)+(TOP_GPIO_IN_OFFSET <<2))

#define BASE 0x1F001000

#define GPIO_NUM_RANGE_MIN 84

/* Linerequest flags */
#define GPIOHANDLE_REQUEST_INPUT        (1UL << 0)
#define GPIOHANDLE_REQUEST_OUTPUT       (1UL << 1)
#define GPIOHANDLE_REQUEST_ACTIVE_LOW   (1UL << 2)
#define GPIOHANDLE_REQUEST_OPEN_DRAIN   (1UL << 3)
#define GPIOHANDLE_REQUEST_OPEN_SOURCE  (1UL << 4)


#define GPIO_GET_CHIPINFO_IOCTL   _IOR(0xB4,  0x01, struct gpiochip_info)
#define GPIO_GET_LINEINFO_IOCTL   _IOWR(0xB4, 0x02, struct gpioline_info)
#define GPIO_GET_LINEHANDLE_IOCTL _IOWR(0xB4, 0x03, struct gpiohandle_request)
#define GPIO_GET_LINEEVENT_IOCTL  _IOWR(0xB4, 0x04, struct gpioevent_request)



#define GPIOHANDLES_MAX 64
struct gpiohandle_request {
        __u32 lineoffsets[GPIOHANDLES_MAX];
        __u32 flags;
        __u8 default_values[GPIOHANDLES_MAX];
        char consumer_label[32];
        __u32 lines;
        int fd;
};


#ifdef DEBUG_ENV
#define fprintff(fmt,args...) fprintf(fmt ,##args)
#define DEBINFO __FILE__, __LINE__, __FUNCTION__
#else
#define fprintff(fmt,args...)
#endif

int  s500_mmio_init(void);
int  s500_mmio_set_output(int gpio_number, char *device_name);
int  s500_mmio_set_input(int gpio_number, char *device_name);
void s500_mmio_set_high(int gpio_number);
void s500_mmio_set_low(int gpio_number);
int  s500_mmio_read_input(int gpio_number);

void set_max_priority(void);
void set_default_priority(void);

gpio.c

#include "gpio.h"

volatile unsigned char *s500_mmio_gpio_out = NULL;
volatile unsigned char *s500_mmio_gpio_in = NULL;
off_t page_base;
off_t page_offset_in, page_offset_out;

int s500_mmio_init(void) {
  if (s500_mmio_gpio_in == NULL && s500_mmio_gpio_out == NULL) 
  {
    size_t pagesize = sysconf(_SC_PAGE_SIZE);
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd == -1) {
      // Error opening /dev/mem.  Probably not running as root.
      return MMIO_ERROR_DEVMEM;
    }
    // Map GPIO memory to location in process space.
    page_base = (GPIO_IN_REG_ADDR / pagesize) * pagesize;
    page_offset_in = GPIO_IN_REG_ADDR - page_base;
    fprintff(stdout, "page_offset_in=0x%X, page_offset_out=0x%x\n", page_offset_in, page_offset_out); 
    s500_mmio_gpio_in = mmap(NULL,  page_offset_in+1, PROT_READ | PROT_WRITE, MAP_SHARED, fd, page_base);

    page_base = (GPIO_OUT_REG_ADDR / pagesize) * pagesize;
    page_offset_out = GPIO_OUT_REG_ADDR - page_base;
    s500_mmio_gpio_out = mmap(NULL, page_offset_out+1, PROT_READ | PROT_WRITE, MAP_SHARED, fd, page_base);
    close(fd);
    if (s500_mmio_gpio_in  == MAP_FAILED || s500_mmio_gpio_out == MAP_FAILED) 
    {
      // Don't save the result if the memory mapping failed.
      s500_mmio_gpio_in  = NULL;
      s500_mmio_gpio_out = NULL;
      return MMIO_ERROR_MMAP;
    }
  }
  return MMIO_SUCCESS;
}

int s500_mmio_set_input(int gpio_number, char *device_name) 
{
    int fd=0;  
    int ret=0;
    char *chrdev_name;
    struct gpiohandle_request req;
    ret = asprintf(&chrdev_name, "/dev/%s", device_name);
    if (ret < 0) 
    {
        fprintf(stderr, "can't open /dev/%s", device_name);
        return -ENOMEM;
    }
    fd = open(chrdev_name, 0);
    if (fd == -1) 
    {
        ret = -errno;
        fprintf(stderr, "Failed to open %s\n", chrdev_name);
        goto exit_close_error;
    }

    req.lineoffsets[0] = gpio_number;
    req.flags = GPIOHANDLE_REQUEST_INPUT;
    req.lines = 1;
    ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &req);
    if (ret == -1) 
    {
        ret = -errno;
        fprintf(stderr, "Failed to issue GET LINEHANDLE IOCTL (%d)\n",ret);
        goto exit_close_error;
    }
exit_close_error:
    if (close(fd) == -1)
        perror("Failed to close GPIO character device file");
    free(chrdev_name);
    return ret;
}

int s500_mmio_set_output(int gpio_number, char *device_name)
{
    int fd=0;  
    int ret=0;
    char *chrdev_name;
    struct gpiohandle_request req;
    ret = asprintf(&chrdev_name, "/dev/%s", device_name);
    if (ret < 0) 
    {
        fprintf(stderr, "can't open /dev/%s", device_name);
        return -ENOMEM;
    }
    fd = open(chrdev_name, 0);
    if (fd == -1) 
    {
        ret = -errno;
        fprintf(stderr, "Failed to open %s\n", chrdev_name);
        goto exit_close_error;
    }

    req.lineoffsets[0] = gpio_number;
    req.flags = GPIOHANDLE_REQUEST_OUTPUT;
    req.lines = 1;
    ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &req);
    if (ret == -1) 
    {
      ret = -errno;
      fprintf(stderr, "Failed to issue GET LINEHANDLE IOCTL (%d)\n",ret);
        goto exit_close_error;
    }
exit_close_error:
    if (close(fd) == -1)
        perror("Failed to close GPIO character device file");
    free(chrdev_name);
    return ret;
}

int s500_mmio_read_input(int gpio_number) {
  return  (*(s500_mmio_gpio_in+page_offset_in) & (1 << (gpio_number - GPIO_NUM_RANGE_MIN )) ? 1 : 0);
}

void s500_mmio_set_high(int gpio_number)
{
    *(s500_mmio_gpio_out+page_offset_out) |= (1 << (gpio_number-GPIO_NUM_RANGE_MIN));
}
void s500_mmio_set_low(int gpio_number)
{
    *(s500_mmio_gpio_out+page_offset_out) &= (1 << (gpio_number-GPIO_NUM_RANGE_MIN));
}

void set_max_priority(void) {
  struct sched_param sched;
  memset(&sched, 0, sizeof(sched));
  // Use FIFO scheduler with highest priority for the lowest chance of the kernel context switching.
  sched.sched_priority = sched_get_priority_max(SCHED_FIFO);
  sched_setscheduler(0, SCHED_FIFO, &sched);
}

void set_default_priority(void) {
  struct sched_param sched;
  memset(&sched, 0, sizeof(sched));
  // Go back to default scheduler with default 0 priority.
  sched.sched_priority = 0;
  sched_setscheduler(0, SCHED_OTHER, &sched);
}

main.c

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "gpio.h"

int main (int argc, char **argv)
{
    if (argc < 2)
    {
        printf("ex: ./main 88");
        return;
    }

    int gpio_num=atoi(argv[1]);
    int ret=0;
    printf("num=%d\n", gpio_num);

    if (s500_mmio_init() < 0)
        printf("error open mem!n");

    set_max_priority();
    ret=s500_mmio_set_input(gpio_num, "gpiochip0");
    for(;;)
    {
        ret=s500_mmio_read_input(gpio_num);
        usleep(500*1000);
        fprintf(stdout, "\rret=%d", ret);
        fflush(stdout);
    }
    set_default_priority();
}