queue_drv.c - master - matv3ys/QueueDriver

queue_drv.c
350 строк9 KB
Matvey Sukhanov
class_create fix
16 дек 2025, 14:05
16 дек 2025, 14:053c7a054
#include <linux/init.h>
#include <linux/module.h>
#include <linux/version.h>
#include <linux/cdev.h>
#include <linux/fs.h>
#include <linux/sysfs.h>
#include <linux/types.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/poll.h>
#include <linux/wait.h>
#include <linux/mutex.h>
#include <linux/kfifo.h>
#include <linux/atomic.h>

#include "q_share.h"

MODULE_AUTHOR("Matvey Sukhanov nikomu@ne.skaju");
MODULE_LICENSE("GPL");
MODULE_DESCRIPTION("Queue boilerplate with sysfs and poll");

#define DRV_NAME "queue_drv"
#define MINOR_BASE  0
#define MINOR_COUNT 4 

static struct class *device_class;
static dev_t first_assigned_number;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 2, 0)
static char *devnode(const struct device *dev, umode_t *mode)
#else
static char *devnode(struct device *dev, umode_t *mode)
#endif
{
	if (!mode)
		return NULL;
	*mode = (S_IWUGO | S_IRUGO); /* 0666 */
	return NULL;
}

static int drv_resources_init(void)
{
    int ret;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 4, 0)
    device_class = class_create(DRV_NAME);
#else
    device_class = class_create(THIS_MODULE, DRV_NAME);
#endif
	if (IS_ERR(device_class)) {
		pr_err("Failed to create sys_class\n");
		return PTR_ERR(device_class);
	}
	device_class->devnode = devnode;

	ret = alloc_chrdev_region(&first_assigned_number, MINOR_BASE, MINOR_COUNT, DRV_NAME);
	if (ret < 0) {
		pr_err("Failed to alloc chrdev_region\n");
        class_destroy(device_class);
		return ret;
	}

    return 0;
}

static void drv_resources_destroy(void)
{
    unregister_chrdev_region(first_assigned_number, MINOR_COUNT);
    class_destroy(device_class);
}

/* --------------------------------------------------------- */
/* Interface Driver <-> Kernel <--> Userspace */
/* --------------------------------------------------------- */

struct drv_chardev {
    unsigned int number;
    dev_t cdevno;
    struct cdev cdev;
    struct device *c_device;

    DECLARE_KFIFO_PTR(fifo, struct queue_msg);
    wait_queue_head_t wq;
    
    /* Мьютекс для защиты очереди и для Single Open */
    struct mutex lock;
    int open_count;

    /* Счетчик потерь для sysfs */
    atomic_t lost_counter;
};

static struct drv_chardev chardevs[MINOR_COUNT];

/* SYSFS CALLBACKS */

static ssize_t lost_count_show(struct device *dev, struct device_attribute *attr, char *buf)
{
    struct drv_chardev *dev_data = (struct drv_chardev *)dev_get_drvdata(dev);
    return sysfs_emit(buf, "%d\n", atomic_read(&dev_data->lost_counter));
}

static ssize_t lost_count_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
{
    struct drv_chardev *dev_data = (struct drv_chardev *)dev_get_drvdata(dev);
    unsigned int user_value = 0;
    int ret = 0;

    ret = kstrtouint(buf, 10, &user_value);
    if (ret)
        return ret;

    atomic_set(&dev_data->lost_counter, user_value);

    return count;
}

static DEVICE_ATTR_RW(lost_count);


/* --- FILE OPERATIONS --- */

static int drv_open(struct inode *inode, struct file *filp)
{
    struct drv_chardev *drv_cdev = container_of(inode->i_cdev, struct drv_chardev, cdev);

    mutex_lock(&drv_cdev->lock);

    if (drv_cdev->open_count > 0) {
        mutex_unlock(&drv_cdev->lock);
        pr_err("Device %d already open\n", drv_cdev->number);
        return -EBUSY;
    }
    drv_cdev->open_count++;

    mutex_unlock(&drv_cdev->lock);

    filp->private_data = drv_cdev;
    return 0;
}

static int drv_release(struct inode *inode, struct file *filp)
{
    struct drv_chardev *drv_cdev = (struct drv_chardev *)filp->private_data;

    mutex_lock(&drv_cdev->lock);
    drv_cdev->open_count--;
    mutex_unlock(&drv_cdev->lock);

    filp->private_data = NULL;
    return 0;
}

static ssize_t drv_read(struct file *filp, char __user *buf, size_t count, loff_t *pos)
{
    struct drv_chardev *drv_cdev = (struct drv_chardev *)filp->private_data;
    int ret;
    unsigned int copied;

    if (count < sizeof(struct queue_msg))
        return -EINVAL;

    mutex_lock(&drv_cdev->lock);

    /* Если очередь пуста */
    if (kfifo_is_empty(&drv_cdev->fifo)) {
        mutex_unlock(&drv_cdev->lock);

        /* неблокирующий ввод-вывод */
        if (filp->f_flags & O_NONBLOCK)
            return -EAGAIN;

        /* Если открыли без O_NONBLOCK, спим */
        if (wait_event_interruptible(drv_cdev->wq, !kfifo_is_empty(&drv_cdev->fifo)))
            return -ERESTARTSYS;
        
        mutex_lock(&drv_cdev->lock);
    }

    ret = kfifo_to_user(&drv_cdev->fifo, buf, sizeof(struct queue_msg), &copied);

    mutex_unlock(&drv_cdev->lock);

    return ret ? ret : copied;
}

static ssize_t drv_write(struct file *filp, const char __user *buf, size_t count, loff_t *pos)
{
    struct drv_chardev *drv_cdev = (struct drv_chardev *)filp->private_data;
    int ret;
    unsigned int copied;
    struct queue_msg temp;

    if (count != sizeof(struct queue_msg))
        return -EINVAL;

    mutex_lock(&drv_cdev->lock);

    /* при переполнении перезаписываем старые */
    if (kfifo_is_full(&drv_cdev->fifo)) {
        ret = kfifo_out(&drv_cdev->fifo, &temp, 1);
        if (ret > 0) {
            atomic_inc(&drv_cdev->lost_counter);
        }
    }

    ret = kfifo_from_user(&drv_cdev->fifo, buf, sizeof(struct queue_msg), &copied);

    mutex_unlock(&drv_cdev->lock);

    if (ret == 0) {
        wake_up_interruptible(&drv_cdev->wq);
        return copied;
    }

    return ret;
}

static unsigned int drv_poll(struct file *filp, poll_table *wait)
{
    struct drv_chardev *drv_cdev = (struct drv_chardev *)filp->private_data;
    unsigned int mask = 0;

    poll_wait(filp, &drv_cdev->wq, wait);

    mutex_lock(&drv_cdev->lock);

    if (!kfifo_is_empty(&drv_cdev->fifo))
        mask |= POLLIN | POLLRDNORM;

    /* Всегда готовы к записи, т.к. перезаписываем старые */
    mask |= POLLOUT | POLLWRNORM;

    mutex_unlock(&drv_cdev->lock);

    return mask;
}

static const struct file_operations drv_fops = {
	.owner = THIS_MODULE,
	.open = drv_open,
	.release = drv_release,
    .read = drv_read,
    .write = drv_write,
    .poll = drv_poll,
};

static int drv_chardev_init(struct drv_chardev *drv_cdev)
{
    int ret;

    /* Инициализация cdev */
    cdev_init(&drv_cdev->cdev, &drv_fops);
	drv_cdev->cdev.owner = THIS_MODULE;
	drv_cdev->cdevno = MKDEV(MAJOR(first_assigned_number), drv_cdev->number);

    /* Инициализация внутренней логики */
    ret = kfifo_alloc(&drv_cdev->fifo, FIFO_ELEM_NUM, GFP_KERNEL);
    if (ret) {
        pr_err("kfifo_alloc failed\n");
        return ret;
    }

    init_waitqueue_head(&drv_cdev->wq);
    mutex_init(&drv_cdev->lock);
    atomic_set(&drv_cdev->lost_counter, 0);
    drv_cdev->open_count = 0;

	/* Добавление cdev */
	ret = cdev_add(&drv_cdev->cdev, drv_cdev->cdevno, 1);
	if (ret < 0) {
		pr_err("cdev_add failed, code %d\n", ret);
        kfifo_free(&drv_cdev->fifo);
		return ret;
	}

    /* Создание /dev ноды */
	drv_cdev->c_device = device_create(device_class, NULL, drv_cdev->cdevno, (void *)drv_cdev,
        DRV_NAME "%d", drv_cdev->number);
	if (IS_ERR(drv_cdev->c_device)) {
		pr_err("device_create failed\n");
        ret = PTR_ERR(drv_cdev->c_device);
        goto device_create_error;
	}

    /* Создание файла в sysfs */
    ret = device_create_file(drv_cdev->c_device, &dev_attr_lost_count);
    if (ret < 0) {
        pr_err("device_create_file failed\n");
        goto attributes_error;
    }

    return 0;

attributes_error:
    device_destroy(device_class, drv_cdev->cdevno);
device_create_error:
    cdev_del(&drv_cdev->cdev);
    kfifo_free(&drv_cdev->fifo);
    return ret;
}

static void drv_chardev_destroy(struct drv_chardev *drv_cdev)
{
    device_remove_file(drv_cdev->c_device, &dev_attr_lost_count);
    device_destroy(device_class, drv_cdev->cdevno);
    cdev_del(&drv_cdev->cdev);
    kfifo_free(&drv_cdev->fifo);
}

int init_module(void)
{
    int i;

    if (drv_resources_init() < 0) {
        pr_err("drv_resources_init failure\n");
        return -ENODEV;
    }

    for (i = 0; i < MINOR_COUNT; ++i) {
        chardevs[i].number = i;
        if (drv_chardev_init(&chardevs[i]) < 0) {
            pr_err("drv_chardev[%i] init failure\n", i);
            goto drv_resources_destroy;
        }
    }

    pr_info("init_module: " DRV_NAME " loaded\n");

	return 0;

drv_resources_destroy:
    while (--i >= 0) {
        drv_chardev_destroy(&chardevs[i]);
    }
    drv_resources_destroy();
    return -ENODEV;
}

void cleanup_module(void)
{
    int i;

    for (i = 0; i < MINOR_COUNT; ++i) {
        drv_chardev_destroy(&chardevs[i]);
    }
    drv_resources_destroy();

	pr_info("cleanup_module: " DRV_NAME "\n");
}