// 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");