commit 970069692c1695204ea4c25c40f8195f780b884d Author: Markus Lehr Date: Wed Jun 19 07:56:27 2024 +0200 1st Version diff --git a/75-mon-datafile.rules b/75-mon-datafile.rules new file mode 100644 index 0000000..75eed6e --- /dev/null +++ b/75-mon-datafile.rules @@ -0,0 +1 @@ +SUBSYSTEM=="misc", KERNEL=="mon-datafile", GROUP="dialout", MODE="0666" diff --git a/Create-Kernel-Driver.odt b/Create-Kernel-Driver.odt new file mode 100644 index 0000000..cee5ccf Binary files /dev/null and b/Create-Kernel-Driver.odt differ diff --git a/LockingWaitQueues.pdf b/LockingWaitQueues.pdf new file mode 100644 index 0000000..6d16b4b Binary files /dev/null and b/LockingWaitQueues.pdf differ diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..60c3bf0 --- /dev/null +++ b/Makefile @@ -0,0 +1,26 @@ +# Makefile for VRPMDV Monitoring Datafile +obj-m := vrpmdv-mon-datafile.o + + +SRC := $(shell pwd) + +all: + $(MAKE) -C $(KERNEL_SRC) M=$(SRC) + +modules_install: + $(MAKE) -C $(KERNEL_SRC) M=$(SRC) modules_install + +clean: + rm -f *.o *~ core .depend .*.cmd *.ko *.mod.c + rm -f Module.markers Module.symvers modules.order + rm -rf .tmp_versions Modules.symvers + +# obj-m = vrpmdv-monitoring-controler.o + +# KVERSION = $(shell uname -r) + +# all: +# make -C /lib/modules/$(KVERSION)/build M=$(PWD) modules + +# clean: +# make -C /lib/modules/$(KVERSION)/build M=$(PWD) clean diff --git a/vrpmdv-mon-datafile.c b/vrpmdv-mon-datafile.c new file mode 100644 index 0000000..4484d0d --- /dev/null +++ b/vrpmdv-mon-datafile.c @@ -0,0 +1,606 @@ +// SPDX-License-Identifier: GPL-2.0+ +/* + * Copyright (C) STMicroelectronics 2019 - All Rights Reserved + * Author: Jean-Philippe Romain + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define RPMSG_SDB_DRIVER_VERSION "1.0" + +/* + * Static global variables + */ +static const char rpmsg_sdb_driver_name[] = "vrpmdv-mon-datafile"; + +static int LastBufferId; + +struct rpmsg_sdb_ioctl_set_efd { + int bufferId, eventfd; +}; + +struct rpmsg_sdb_ioctl_get_data_size { + int bufferId; + uint32_t size; +}; + +/* ioctl numbers */ +/* _IOW means userland is writing and kernel is reading */ +/* _IOR means userland is reading and kernel is writing */ +/* _IOWR means userland and kernel can both read and write */ +#define RPMSG_SDB_IOCTL_SET_EFD _IOW('R', 0x00, struct rpmsg_sdb_ioctl_set_efd *) +#define RPMSG_SDB_IOCTL_GET_DATA_SIZE _IOWR('R', 0x01, struct rpmsg_sdb_ioctl_get_data_size *) + +struct sdb_buf_t { + int index; /* index of buffer */ + size_t size; /* buffer size */ + size_t writing_size; /* size of data written by copro */ + dma_addr_t paddr; /* physical address*/ + void *vaddr; /* virtual address */ + void *uaddr; /* mapped address for userland */ + struct eventfd_ctx *efd_ctx; /* eventfd context */ + struct list_head buflist; /* reference in the buffers list */ +}; + +struct rpmsg_sdb_t { + struct mutex mutex; /* mutex to protect the ioctls */ + struct miscdevice mdev; /* misc device ref */ + struct rpmsg_device *rpdev; /* handle rpmsg device */ + struct list_head buffer_list; /* buffer instances list */ +}; + +struct device *rpmsg_sdb_dev; + +struct vRCMDeviceData { + uint32_t packageNo; //current package Number + uint32_t packageCount; //complete package Number + uint32_t dataQuantity; //number of uint32_t in data + uint32_t data[]; //the data +}; + +static int rpmsg_sdb_format_txbuf_string(struct sdb_buf_t *buffer, char *bufinfo_str, size_t bufinfo_str_size) +{ + pr_info("rpmsg_sdb(%s): Buffer index:%d, addr:%08x, size:%08x\n", + __func__, + buffer->index, + buffer->paddr, + buffer->size); + return snprintf(bufinfo_str, bufinfo_str_size, "B%dA%08xL%08x", buffer->index, buffer->paddr, buffer->size); +} + +static long rpmsg_sdb_decode_rxbuf_string(char *rxbuf_str, int *buffer_id, size_t *size) +{ + int ret = 0; + char *sub_str; + long bsize; + long bufid; + const char delimiter[2] = {'L','\0'}; + //__u32* data = (__u32*) rxbuf_str; + struct vRCMDeviceData* pdata = (struct vRCMDeviceData*) rxbuf_str; + + + //pr_info("rpmsg_sdb(%s): rxbuf_str:%s\n", __func__, rxbuf_str); + pr_info("rpmsg_sdb(%s): packageno:%d\n", __func__, pdata->packageNo); + pr_info("rpmsg_sdb(%s): packageCount:%d\n", __func__, pdata->packageCount); + pr_info("rpmsg_sdb(%s): dataquantity:%d\n", __func__, pdata->dataQuantity); + pr_info("rpmsg_sdb(%s): data:%d\n", __func__, (pdata->data)[0]); + // pr_info("rpmsg_sdb(%s): rxbuf_str:%d\n", __func__, data[0]); + // pr_info("rpmsg_sdb(%s): rxbuf_str:%d\n", __func__, data[1]); + // pr_info("rpmsg_sdb(%s): rxbuf_str:%d\n", __func__, data[2]); + + + + /* Get first part containing the buffer id */ +// sub_str = strsep(&rxbuf_str, delimiter); + +// // pr_info("rpmsg_sdb(%s): sub_str:%s\n", __func__, sub_str); + +// /* Save Buffer id and size: template BxLyyyyyyyy*/ +// ret = kstrtol(&sub_str[1], 10, &bufid); +// if (ret < 0) { +// // pr_info("rpmsg_sdb(ERROR): Extract of buffer id failed(%d)", ret); +// goto out; +// } + +// ret = kstrtol(&rxbuf_str[2], 16, &bsize); +// if (ret < 0) { +// // pr_info("rpmsg_sdb(ERROR): Extract of buffer size failed(%d)", ret); +// goto out; +// } + +// *size = (size_t)bsize; +// *buffer_id = (int)bufid; + +// out: + return ret; +} + +static int rpmsg_sdb_send_buf_info(struct rpmsg_sdb_t *rpmsg_sdb, struct sdb_buf_t *buffer) +{ + int count = 0, ret = 0; + const unsigned char *tbuf; + char mybuf[32]; + int msg_size; + struct rpmsg_device *_rpdev; + + pr_info("rpmsg_sdb(%s): start rpmsg_sdb_send_buf_info\n", __func__); + _rpdev = rpmsg_sdb->rpdev; + msg_size = rpmsg_get_mtu(_rpdev->ept); + + pr_info("rpmsg_sdb(%s): checked msg site:%d\n", __func__, msg_size); + if (msg_size < 0) + return msg_size; + + pr_info("rpmsg_sdb(%s): Call rpmsg_sdb_format_txbuf_string\n", __func__); + count = rpmsg_sdb_format_txbuf_string(buffer, mybuf, 32); + tbuf = &mybuf[0]; + + do { + /* send a message to our remote processor */ + pr_info("rpmsg_sdb(%s): send a message to our remote processor\n", __func__); + ret = rpmsg_send(_rpdev->ept, (void *)tbuf, + count > msg_size ? msg_size : count); + if (ret) { + dev_err(&_rpdev->dev, "rpmsg_send failed: %d\n", ret); + pr_info("rpmsg_sdb(%s): error by send a message to our remote processor\n", __func__); + return ret; + } + + if (count > msg_size) { + count -= msg_size; + tbuf += msg_size; + } else { + count = 0; + } + } while (count > 0); + + return count; +} + +static int rpmsg_sdb_mmap(struct file *file, struct vm_area_struct *vma) +{ + unsigned long vsize = vma->vm_end - vma->vm_start; + unsigned long size = PAGE_ALIGN(vsize); + unsigned long NumPages = size >> PAGE_SHIFT; + unsigned long align = get_order(size); + pgprot_t prot = pgprot_noncached(vma->vm_page_prot); + struct rpmsg_sdb_t *_rpmsg_sdb; + struct sdb_buf_t *_buffer; + + pr_info("rpmsg_sdb(%s): start rpmsg_sdb_mmap\n", __func__); + if (align > CONFIG_CMA_ALIGNMENT) + align = CONFIG_CMA_ALIGNMENT; + + if (rpmsg_sdb_dev == NULL) + return -ENOMEM; + + pr_info("rpmsg_sdb(%s): start rpmsg_sdb_mmap - coherent_dma_mask\n", __func__); + rpmsg_sdb_dev->coherent_dma_mask = DMA_BIT_MASK(32); + rpmsg_sdb_dev->dma_mask = &rpmsg_sdb_dev->coherent_dma_mask; + + _rpmsg_sdb = container_of(file->private_data, struct rpmsg_sdb_t, + mdev); + + /* Field the last buffer entry which is the last one created */ + if (!list_empty(&_rpmsg_sdb->buffer_list)) { + pr_info("rpmsg_sdb(%s): start rpmsg_sdb_mmap - bufferlist not empty\n", __func__); + _buffer = list_last_entry(&_rpmsg_sdb->buffer_list, + struct sdb_buf_t, buflist); + + _buffer->uaddr = NULL; + _buffer->size = NumPages * PAGE_SIZE; + _buffer->writing_size = -1; + _buffer->vaddr = dma_alloc_coherent(rpmsg_sdb_dev, + _buffer->size, + &_buffer->paddr, + GFP_KERNEL); + + + if (!_buffer->vaddr) { + pr_info("rpmsg_sdb(ERROR): Memory allocation issue\n"); + return -ENOMEM; + } + + pr_info("rpmsg_sdb(%s): dma_alloc_coherent done - paddr[%d]:%x - vaddr[%d]:%p\n", + __func__, + _buffer->index, + _buffer->paddr, + _buffer->index, + _buffer->vaddr); + + /* Get address for userland */ + if (remap_pfn_range(vma, vma->vm_start, + (_buffer->paddr >> PAGE_SHIFT) + vma->vm_pgoff, + size, prot)) { + + pr_info("rpmsg_sdb(%s): remap_pfn_range could not be done\n"); + return -EAGAIN; + } + + + _buffer->uaddr = (void *)vma->vm_start; + pr_info("rpmsg_sdb(%s): _buffer->uaddr = %p\n", __func__, _buffer->uaddr); + + /* Send information to remote proc */ + pr_info("rpmsg_sdb(%s): Send information to remote proc\n", __func__); + rpmsg_sdb_send_buf_info(_rpmsg_sdb, _buffer); + } else { + dev_err(rpmsg_sdb_dev, "No existing buffer entry exist in the list !!!"); + pr_debug("rpmsg_sdb(%s): No existing buffer entry exist in the list !!!\n", __func__); + return -EINVAL; + } + + /* Increment for number of requested buffer */ + LastBufferId++; + + return 0; +} + +/** + * rpmsg_sdb_open - Open Session + * + * @inode: inode struct + * @file: file struct + * + * Return: + * 0 - Success + * Non-zero - Failure + */ +static int rpmsg_sdb_open(struct inode *inode, struct file *file) +{ + struct rpmsg_sdb_t *_rpmsg_sdb; + + _rpmsg_sdb = container_of(file->private_data, struct rpmsg_sdb_t, + mdev); + + /* Initialize the buffer list*/ + pr_debug("rpmsg_sdb(%s): Init bufferlist", __func__); + pr_info("rpmsg_sdb(%s): start Init bufferlist", __func__); + INIT_LIST_HEAD(&_rpmsg_sdb->buffer_list); + pr_info("rpmsg_sdb(%s): success Init bufferlist", __func__); + + mutex_init(&_rpmsg_sdb->mutex); + + return 0; +} + +/** + * rpmsg_sdb_close - Close Session + * + * @inode: inode struct + * @file: file struct + * + * Return: + * 0 - Success + * Non-zero - Failure + */ +static int rpmsg_sdb_close(struct inode *inode, struct file *file) +{ + struct rpmsg_sdb_t *_rpmsg_sdb; + struct sdb_buf_t *pos, *next; + + // pr_info("rpmsg_sdb(%s): Free the CMA allocation 1: pos->size:%08x, pos->vaddr:%08x, pos->paddr:%08x\n", + // __func__, + // pos->size, + // pos->vaddr, + // pos->paddr); + + pr_info("rpmsg_sdb(%s): Free the CMA allocation 1", __func__); + _rpmsg_sdb = container_of(file->private_data, struct rpmsg_sdb_t, + mdev); + + // pr_info("rpmsg_sdb(%s): Free the CMA allocation 2: pos->size:%08x, pos->vaddr:%08x, pos->paddr:%08x\n", + // __func__, + // pos->size, + // pos->vaddr, + // pos->paddr); + + pr_info("rpmsg_sdb(%s): Free the CMA allocation 2", __func__); + list_for_each_entry_safe(pos, next, &_rpmsg_sdb->buffer_list, buflist) { + /* Free the CMA allocation */ + // pr_info("rpmsg_sdb(%s): Free the CMA allocation 3: pos->size:%08x, pos->vaddr:%08x, pos->paddr:%08x\n", + // __func__, + // pos->size, + // pos->vaddr, + // pos->paddr); + pr_info("rpmsg_sdb(%s): Free the CMA allocation 3", __func__); + dma_free_coherent(rpmsg_sdb_dev, pos->size, pos->vaddr, pos->paddr); + /* Remove the buffer from the list */ + list_del(&pos->buflist); + /* Free the buffer */ + kfree(pos); + } + + pr_info("rpmsg_sdb(%s): Free the CMA allocation 4", __func__); + /* Reset LastBufferId */ + LastBufferId = 0; + + return 0; +} + +/** + * rpmsg_sdb_ioctl - IOCTL + * + * @session: ibmvmc_file_session struct + * @cmd: cmd field + * @arg: Argument field + * + * Return: + * 0 - Success + * Non-zero - Failure + */ +static long rpmsg_sdb_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + int idx = 0; + + struct rpmsg_sdb_t *_rpmsg_sdb; + struct sdb_buf_t *buffer, *lastbuffer; + + struct list_head *pos; + struct sdb_buf_t *datastructureptr = NULL; + + struct rpmsg_sdb_ioctl_set_efd q_set_efd; + struct rpmsg_sdb_ioctl_get_data_size q_get_dat_size; + + void __user *argp = (void __user *)arg; + + _rpmsg_sdb = container_of(file->private_data, struct rpmsg_sdb_t, + mdev); + + switch (cmd) { + case RPMSG_SDB_IOCTL_SET_EFD: + mutex_lock(&_rpmsg_sdb->mutex); + + /* Get index from the last buffer in the list */ + pr_info("mon-datafile: set EFD\n"); + if (!list_empty(&_rpmsg_sdb->buffer_list)) { + lastbuffer = list_last_entry(&_rpmsg_sdb->buffer_list, struct sdb_buf_t, buflist); + idx = lastbuffer->index; + + /* Check last index was properly initiated*/ + if (lastbuffer->vaddr == NULL) { + pr_err("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_SET_EFD - previous buffer was not allocated\n"); + pr_info("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_SET_EFD - previous buffer was not allocated\n"); + mutex_unlock(&_rpmsg_sdb->mutex); + return -EBADE; + } + + /* increment this index for the next buffer creation*/ + idx++; + } + + if (copy_from_user(&q_set_efd, (struct rpmsg_sdb_ioctl_set_efd *)argp, + sizeof(struct rpmsg_sdb_ioctl_set_efd))) { + pr_err("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_SET_EFD - copy from user failed\n"); + pr_info("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_SET_EFD - copy from user failed\n"); + + mutex_unlock(&_rpmsg_sdb->mutex); + return -EFAULT; + } + + /* create a new buffer which will be added in the buffer list */ + buffer = kmalloc(sizeof(struct sdb_buf_t), GFP_KERNEL); + + buffer->index = idx; + buffer->vaddr = NULL; + buffer->efd_ctx = eventfd_ctx_fdget(q_set_efd.eventfd); + list_add_tail(&buffer->buflist, &_rpmsg_sdb->buffer_list); + + pr_info("rpmsg_sdb(SUCCES): RPMSG_SDB_IOCTL_SET_EFD \n"); + + mutex_unlock(&_rpmsg_sdb->mutex); + break; + + case RPMSG_SDB_IOCTL_GET_DATA_SIZE: + pr_info("mon-datafile: get datasize\n"); + if (copy_from_user(&q_get_dat_size, (struct rpmsg_sdb_ioctl_get_data_size *)argp, + sizeof(struct rpmsg_sdb_ioctl_get_data_size))) { + pr_err("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_GET_DATA_SIZE - copy from user failed\n"); + return -EFAULT; + } + + /* Get the index of the requested buffer and then look-up in the buffer list*/ + idx = q_get_dat_size.bufferId; + + list_for_each(pos, &_rpmsg_sdb->buffer_list) + { + datastructureptr = list_entry(pos, struct sdb_buf_t, buflist); + if (datastructureptr->index == idx) { + /* Get the writing size*/ + q_get_dat_size.size = datastructureptr->writing_size; + break; + } + } + + if (copy_to_user((struct rpmsg_sdb_ioctl_get_data_size *)argp, &q_get_dat_size, + sizeof(struct rpmsg_sdb_ioctl_get_data_size))) { + pr_err("rpmsg_sdb(ERROR): RPMSG_SDB_IOCTL_GET_DATA_SIZE - copy to user failed\n"); + return -EFAULT; + } + + /* Reset the writing size*/ + datastructureptr->writing_size = -1; + + break; + default: + return -EINVAL; + } + + return 0; +} + +static const struct file_operations rpmsg_sdb_fops = { + .owner = THIS_MODULE, + .unlocked_ioctl = rpmsg_sdb_ioctl, + .mmap = rpmsg_sdb_mmap, + .open = rpmsg_sdb_open, + .release = rpmsg_sdb_close, +}; + +static int rpmsg_sdb_drv_cb(struct rpmsg_device *rpdev, void *data, int len, + void *priv, u32 src) +{ + int ret = 0; + int buffer_id = 0; + size_t buffer_size; + char *rpmsg_RxBuf; + struct list_head *pos; + struct sdb_buf_t *datastructureptr = NULL; + + pr_info("mon-datafile: receive msg from Copro: %s, len:%d\n", __func__, len); + struct rpmsg_sdb_t *drv = dev_get_drvdata(&rpdev->dev); + + if (len == 0) { + dev_err(rpmsg_sdb_dev, "(%s) Empty lenght requested\n", __func__); + return -EINVAL; + } + + //dev_err(rpmsg_sdb_dev, "(%s) lenght: %d\n", __func__,len); + + rpmsg_RxBuf = (char *)kmalloc(len+1, GFP_KERNEL); + memcpy(rpmsg_RxBuf, data, len); + + rpmsg_RxBuf[len] = 0; + + ret = rpmsg_sdb_decode_rxbuf_string(rpmsg_RxBuf, &buffer_id, &buffer_size); + kfree(rpmsg_RxBuf); + if (ret < 0) + goto out; + + if (buffer_id > LastBufferId) { + ret = -EINVAL; + goto out; + } + + /* Signal to User space application */ + list_for_each(pos, &drv->buffer_list) + { + datastructureptr = list_entry(pos, struct sdb_buf_t, buflist); + if (datastructureptr->index == buffer_id) { + datastructureptr->writing_size = buffer_size; + + if (datastructureptr->writing_size > datastructureptr->size) { + dev_err(rpmsg_sdb_dev, "(%s) Writing size is bigger than buffer size\n", __func__); + ret = -EINVAL; + goto out; + } + + eventfd_signal(datastructureptr->efd_ctx, 1); + break; + } + /* TODO: quid if nothing find during the loop ? */ + } + +out: + return ret; +} + +static int rpmsg_sdb_drv_probe(struct rpmsg_device *rpdev) +{ + int ret = 0; + struct device *dev = &rpdev->dev; + struct rpmsg_sdb_t *rpmsg_sdb; + + pr_info("mon-datafile: registering started\n"); + + rpmsg_sdb = devm_kzalloc(dev, sizeof(*rpmsg_sdb), GFP_KERNEL); + if (!rpmsg_sdb) + return -ENOMEM; + + mutex_init(&rpmsg_sdb->mutex); + + rpmsg_sdb->rpdev = rpdev; + + rpmsg_sdb->mdev.name = "mon-datafile";//"rpmsg-sdb"; + rpmsg_sdb->mdev.minor = MISC_DYNAMIC_MINOR; + rpmsg_sdb->mdev.fops = &rpmsg_sdb_fops; + + pr_info("mon-datafile: Set Driver data\n"); + dev_set_drvdata(&rpdev->dev, rpmsg_sdb); + + /* Register misc device */ + ret = misc_register(&rpmsg_sdb->mdev); + + if (ret) { + dev_err(dev, "Failed to register device\n"); + pr_info("mon-datafile: Failed to register device\n"); + goto err_out; + } + + rpmsg_sdb_dev = rpmsg_sdb->mdev.this_device; + + //pr_info("rpmsg_sdb: Failed to register device\n"); + dev_info(dev, "%s probed\n", rpmsg_sdb_driver_name); + +err_out: + return ret; +} + +static void rpmsg_sdb_drv_remove(struct rpmsg_device *rpmsgdev) +{ + struct rpmsg_sdb_t *drv = dev_get_drvdata(&rpmsgdev->dev); + + misc_deregister(&drv->mdev); +} + +static struct rpmsg_device_id rpmsg_driver_sdb_id_table[] = { + { .name = "vrpmdv-mon-datafile" }, + { }, +}; +MODULE_DEVICE_TABLE(rpmsg, rpmsg_driver_sdb_id_table); + +//static struct rpmsg_driver rpmsg_sdb_rmpsg_drv = { +static struct rpmsg_driver vrpmdv_monitoring_data = { + .drv.name = KBUILD_MODNAME, + .drv.owner = THIS_MODULE, + .id_table = rpmsg_driver_sdb_id_table, + .probe = rpmsg_sdb_drv_probe, + .callback = rpmsg_sdb_drv_cb, + .remove = rpmsg_sdb_drv_remove, +}; + +module_rpmsg_driver(vrpmdv_monitoring_data); + + +// static int __init rpmsg_sdb_drv_init(void) +// { +// int ret = 0; + +// /* Register rpmsg device */ +// ret = register_rpmsg_driver(&rpmsg_sdb_rmpsg_drv); + +// if (ret) { +// pr_err("rpmsg_sdb(ERROR): Failed to register device\n"); +// return ret; +// } + +// pr_info("rpmsg_sdb: Init done\n"); + +// return ret; +// } + +// static void __exit rpmsg_sdb_drv_exit(void) +// { +// unregister_rpmsg_driver(&rpmsg_sdb_rmpsg_drv); +// pr_info("rpmsg_sdb: Exit\n"); +// } + +// module_init(rpmsg_sdb_drv_init); +// module_exit(rpmsg_sdb_drv_exit); + + +MODULE_AUTHOR("Markus Lehr "); +MODULE_DESCRIPTION("shared data buffer over RPMSG"); +MODULE_VERSION(RPMSG_SDB_DRIVER_VERSION); +MODULE_LICENSE("GPL v2");