btfmcodec: Add packetization and depacketizations

This change adds support for packetization and depacketization of various packet received or sent from btfmcodec driver. CRs-Fixed: 3298745 Change-Id: I30c1a7897a1f50d5dcad83757900152f2c06255b
2023-01-02 10:11:03 +05:30
parent d22c687077
commit 30209d05c3
4 changed files with 389 additions and 11 deletions
--- a/btfmcodec/btfm_codec.c
+++ b/btfmcodec/btfm_codec.c
@@ -13,7 +13,7 @@
 #include <linux/fs.h>
 #include <linux/module.h>
 #include "btfm_codec.h"
-//#include "btfm_codec_hw_interface.h"
+#include "btfm_codec_pkt.h"

 #define dev_to_btfmcodec(_dev) container_of(_dev, struct btfmcodec_data, dev)

@@ -23,6 +23,8 @@ static dev_t dev_major;
 struct btfmcodec_data *btfmcodec;
 struct device_driver driver = {.name = "btfmcodec-driver", .owner = THIS_MODULE};
 struct btfmcodec_char_device *btfmcodec_dev;
+#define cdev_to_btfmchardev(_cdev) container_of(_cdev, struct btfmcodec_char_device, cdev)
+#define MIN_PKT_LEN  0x3

 char *coverttostring(enum btfmcodec_states state) {
 	switch (state) {
@@ -47,16 +49,323 @@ char *coverttostring(enum btfmcodec_states state) {
 	}
 }

+/*
+ * btfmcodec_dev_open() - open() syscall for the btfmcodec dev node
+ * inode:	Pointer to the inode structure.
+ * file:	Pointer to the file structure.
+ *
+ * This function is used to open the btfmcodec char device when
+ * userspace client do a open() system call. All input arguments are
+ * validated by the virtual file system before calling this function.
+ * Note: btfmcodec dev node works on nonblocking mode.
+ */
+static int btfmcodec_dev_open(struct inode *inode, struct file *file)
+{
+	struct btfmcodec_char_device *btfmcodec_dev = cdev_to_btfmchardev(inode->i_cdev);
+	struct btfmcodec_data *btfmcodec = (struct btfmcodec_data *)btfmcodec_dev->btfmcodec;
+	int active_clients = refcount_read(&btfmcodec_dev->active_clients);
+
+	btfmcodec->states.current_state = IDLE; /* Just a temp*/
+	BTFMCODEC_INFO("File mode :%u", file->f_mode);
+	BTFMCODEC_INFO("for %s by %s:%d active_clients[%d]\n",
+		       btfmcodec_dev->dev_name, current->comm,
+		       task_pid_nr(current), refcount_read(&btfmcodec_dev->active_clients));
+	/* Don't allow a new client if already one is active. */
+	if (active_clients > 0) {
+		BTFMCODEC_WARN("%s: Not honoring open as other client is active", __func__);
+		return EACCES;
+	}
+	/* for now have btfmcodec and later we can think of having it btfmcodec_dev */
+	file->private_data = btfmcodec;
+	refcount_inc(&btfmcodec_dev->active_clients);
+	return 0;
+}
+
+/*
+ * btfmcodec_pkt_release() - release operation on btfmcodec device
+ * inode:	Pointer to the inode structure.
+ * file:	Pointer to the file structure.
+ *
+ * This function is used to release the btfmcodec dev node when
+ * userspace client do a close() system call. All input arguments are
+ * validated by the virtual file system before calling this function.
+ */
+static int btfmcodec_dev_release(struct inode *inode, struct file *file)
+{
+	struct btfmcodec_char_device *btfmcodec_dev = cdev_to_btfmchardev(inode->i_cdev);
+	unsigned long flags;
+
+	BTFMCODEC_INFO("for %s by %s:%d active_clients[%d]\n",
+		       btfmcodec_dev->dev_name, current->comm,
+		       task_pid_nr(current), refcount_read(&btfmcodec_dev->active_clients));
+
+	refcount_dec(&btfmcodec_dev->active_clients);
+	if (refcount_read(&btfmcodec_dev->active_clients) == 0) {
+		spin_lock_irqsave(&btfmcodec_dev->tx_queue_lock, flags);
+		skb_queue_purge(&btfmcodec_dev->txq);
+		/* Wakeup the device if waiting for the data */
+		wake_up_interruptible(&btfmcodec_dev->readq);
+		spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+		/* we need to have separte rx lock for below buff */
+		skb_queue_purge(&btfmcodec_dev->rxq);
+	}
+
+	return 0;
+}
+
+btm_opcode get_opcode (struct sk_buff *skb)
+{
+	return ((skb->data[0]<< 8) | skb->data[1]);
+}
+
+static void btfmcodec_dev_rxwork(struct work_struct *work)
+{
+	struct btfmcodec_char_device *btfmcodec_dev = container_of(work, struct btfmcodec_char_device, rx_work);
+	struct sk_buff *skb;
+	struct btm_req *req_pkt;
+
+	BTFMCODEC_DBG("start");
+	while ((skb = skb_dequeue(&btfmcodec_dev->rxq))) {
+		btm_opcode opcode = get_opcode(skb);
+//		skb_pull(skb, sizeof(btm_opcode));
+		switch (opcode) {
+		case BTM_BTFMCODEC_PREPARE_AUDIO_BEARER_SWITCH_REQ:
+			req_pkt = (void *)skb->data;
+			req_pkt->opcode  = opcode;
+			BTFMCODEC_DBG("BTM_BTFMCODEC_PREPARE_AUDIO_BEARER_SWITCH_REQ opcode %4x and len %d", req_pkt->opcode, req_pkt->len);
+			btfmcodec_dev_enqueue_pkt(btfmcodec_dev, skb->data, skb->len);
+			break;
+		case BTM_BTFMCODEC_MASTER_CONFIG_RSP:
+			BTFMCODEC_DBG("BTM_BTFMCODEC_MASTER_CONFIG_RSP");
+			break;
+		case BTM_BTFMCODEC_CTRL_MASTER_SHUTDOWN_RSP:
+			BTFMCODEC_DBG("BTM_BTFMCODEC_CTRL_MASTER_SHUTDOWN_RSP");
+			break;
+		case BTM_BTFMCODEC_BEARER_SWITCH_IND:
+			BTFMCODEC_DBG("BTM_BTFMCODEC_BEARER_SWITCH_IND");
+			break;
+		default:
+			BTFMCODEC_ERR("wrong opcode:%04x", opcode);
+		}
+		kfree_skb(skb);
+	}
+	BTFMCODEC_DBG("end");
+}
+
+/*
+ * btfmcodec_pkt_write() - write() syscall for the btfmcodec_pkt device
+ * file:	Pointer to the file structure.
+ * buf:		Pointer to the userspace buffer.
+ * count:	Number bytes to read from the file.
+ * ppos:	Pointer to the position into the file.
+ *
+ * This function is used to write the data to btfmcodec dev node when
+ * userspace client do a write() system call. All input arguments are
+ * validated by the virtual file system before calling this function.
+ */
+static ssize_t btfmcodec_dev_write(struct file *file,
+			const char __user *buf, size_t count, loff_t *ppos)
+{
+	struct btfmcodec_data *btfmcodec = file->private_data;
+	struct btfmcodec_char_device *btfmcodec_dev= NULL;
+	struct sk_buff *skb;
+	void *kbuf;
+	int ret =  0;
+
+	if (!btfmcodec || !btfmcodec->btfmcodec_dev || refcount_read(&btfmcodec->btfmcodec_dev->active_clients) == 0) {
+		BTFMCODEC_INFO("%s: %d\n", current->comm, task_pid_nr(current));
+		return -EINVAL;
+	} else {
+		btfmcodec_dev = btfmcodec->btfmcodec_dev;
+	}
+
+	if (mutex_lock_interruptible(&btfmcodec_dev->lock)) {
+		ret = -ERESTARTSYS;
+		goto free_kbuf;
+	}
+
+	/* Hack for Now */
+	if (count < MIN_PKT_LEN) {
+		BTFMCODEC_ERR("minimum packet len should be greater than 3 bytes");
+		goto free_kbuf;
+	}
+	if (refcount_read(&btfmcodec->btfmcodec_dev->active_clients) == 0) {
+		BTFMCODEC_WARN("Client disconnected");
+		ret = -ENETRESET;
+		goto free_kbuf;
+	}
+
+	BTFMCODEC_DBG("begin to %s buffer_size %zu\n", btfmcodec_dev->dev_name, count);
+	kbuf = memdup_user(buf, count);
+	if (IS_ERR(kbuf)) {
+		ret = PTR_ERR(kbuf);
+		goto free_kbuf;
+	}
+
+	/* Check whether we need a dedicated chunk of memory for this driver */
+	skb = alloc_skb(count* sizeof(size_t), GFP_KERNEL);
+	if (!skb) {
+		BTFMCODEC_ERR("failed to allocate memory for recevied packet");
+		ret = -ENOMEM;
+		goto free_kbuf;
+	}
+
+	skb_put_data(skb, (uint8_t *)kbuf, count);
+	skb_queue_tail(&btfmcodec_dev->rxq, skb);
+	schedule_work(&btfmcodec_dev->rx_work);
+
+	kfree(kbuf);
+
+free_kbuf:
+	mutex_unlock(&btfmcodec_dev->lock);
+	BTFMCODEC_DBG("finish to %s ret %d\n", btfmcodec_dev->dev_name, ret);
+	return ret;
+}
+
+int btfmcodec_dev_enqueue_pkt(struct btfmcodec_char_device *btfmcodec_dev, uint8_t *buf, int len)
+{
+	struct sk_buff *skb;
+	unsigned long flags;
+
+	BTFMCODEC_DBG("start");
+	spin_lock_irqsave(&btfmcodec_dev->tx_queue_lock, flags);
+	skb = alloc_skb(len, GFP_ATOMIC);
+	if (!skb) {
+		BTFMCODEC_ERR("failed to allocate memory");
+		spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+		return -ENOMEM;
+	}
+
+	skb_put_data(skb, buf, len);
+	skb_queue_tail(&btfmcodec_dev->txq, skb);
+	wake_up_interruptible(&btfmcodec_dev->readq);
+	spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+	BTFMCODEC_DBG("end");
+	return 0;
+}
+
+/*
+ * btfmcodec_pkt_poll() - poll() syscall for the btfmcodec device
+ * file:	Pointer to the file structure.
+ * wait:	pointer to Poll table.
+ *
+ * This function is used to poll on the btfmcodec dev node when
+ * userspace client do a poll() system call. All input arguments are
+ * validated by the virtual file system before calling this function.
+ */
+static __poll_t btfmcodec_dev_poll(struct file *file, poll_table *wait)
+{
+	struct btfmcodec_data *btfmcodec = file->private_data;
+	struct btfmcodec_char_device *btfmcodec_dev= NULL;
+	__poll_t mask = 0;
+	unsigned long flags;
+
+	BTFMCODEC_DBG("start");
+	if (!btfmcodec || !btfmcodec->btfmcodec_dev || refcount_read(&btfmcodec->btfmcodec_dev->active_clients) == 0) {
+		BTFMCODEC_INFO("%s: %d\n", current->comm, task_pid_nr(current));
+		return -EINVAL;
+	} else {
+		btfmcodec_dev = btfmcodec->btfmcodec_dev;
+	}
+
+	/* Wait here for timeout or for a wakeup signal on readq */
+	poll_wait(file, &btfmcodec_dev->readq, wait);
+
+	mutex_lock(&btfmcodec_dev->lock);
+	/* recheck if the client has released by the driver */
+	if (refcount_read(&btfmcodec_dev->active_clients) == 0) {
+		BTFMCODEC_WARN("port has been closed alreadt");
+		mutex_unlock(&btfmcodec_dev->lock);
+		return POLLHUP;
+	}
+
+	spin_lock_irqsave(&btfmcodec_dev->tx_queue_lock, flags);
+	/* Set flags if data is avilable to read */
+	if (!skb_queue_empty(&btfmcodec_dev->txq))
+		mask |= POLLIN | POLLRDNORM;
+
+	spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+	mutex_unlock(&btfmcodec_dev->lock);
+
+	BTFMCODEC_DBG("end with reason %d", mask);
+	return mask;
+}
+
+/*
+ * btfmcodec_dev_read() - read() syscall for the btfmcodec dev node
+ * file:	Pointer to the file structure.
+ * buf:		Pointer to the userspace buffer.
+ * count:	Number bytes to read from the file.
+ * ppos:	Pointer to the position into the file.
+ *
+ * This function is used to Read the data from btfmcodec pkt device when
+ * userspace client do a read() system call. All input arguments are
+ * validated by the virtual file system before calling this function.
+ */
+static ssize_t btfmcodec_dev_read(struct file *file,
+			char __user *buf, size_t count, loff_t *ppos)
+{
+	struct btfmcodec_data *btfmcodec = file->private_data;
+	struct btfmcodec_char_device *btfmcodec_dev= NULL;
+	unsigned long flags;
+	struct sk_buff *skb;
+	int use;
+
+	BTFMCODEC_DBG("start");
+	if (!btfmcodec || !btfmcodec->btfmcodec_dev || refcount_read(&btfmcodec->btfmcodec_dev->active_clients) == 0) {
+		BTFMCODEC_INFO("%s: %d\n", current->comm, task_pid_nr(current));
+		return -EINVAL;
+	} else {
+		btfmcodec_dev = btfmcodec->btfmcodec_dev;
+	}
+
+	spin_lock_irqsave(&btfmcodec_dev->tx_queue_lock, flags);
+	/* Wait for data in the queue */
+	if (skb_queue_empty(&btfmcodec_dev->txq)) {
+		spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		/* Wait until we get data*/
+		if (wait_event_interruptible(btfmcodec_dev->readq,
+					     !skb_queue_empty(&btfmcodec_dev->txq)))
+			return -ERESTARTSYS;
+
+		/* We lost the client while waiting */
+		if (!refcount_read(&btfmcodec->btfmcodec_dev->active_clients))
+			return -ENETRESET;
+
+		spin_lock_irqsave(&btfmcodec_dev->tx_queue_lock, flags);
+	}
+
+	skb = skb_dequeue(&btfmcodec_dev->txq);
+	spin_unlock_irqrestore(&btfmcodec_dev->tx_queue_lock, flags);
+	if (!skb)
+		return -EFAULT;
+
+	use = min_t(size_t, count, skb->len);
+	if (copy_to_user(buf, skb->data, use))
+		use = -EFAULT;
+
+	kfree_skb(skb);
+
+	BTFMCODEC_DBG("end for %s by %s:%d ret[%d]\n", btfmcodec_dev->dev_name,
+		       current->comm, task_pid_nr(current), use);
+
+	return use;
+}
 static const struct file_operations btfmcodec_fops = {
 	.owner = THIS_MODULE,
-/*	.open = glink_pkt_open,
-	.release = glink_pkt_release,
-	.read = glink_pkt_read,
-	.write = glink_pkt_write,
-	.poll = glink_pkt_poll,
-	.unlocked_ioctl = glink_pkt_ioctl,
-	.compat_ioctl = glink_pkt_ioctl,
-*/};
+	.open = btfmcodec_dev_open,
+	.release = btfmcodec_dev_release,
+	.write = btfmcodec_dev_write,
+	.poll = btfmcodec_dev_poll,
+	.read = btfmcodec_dev_read,
+	/* For Now add no hookups for below callbacks */
+	.unlocked_ioctl = NULL,
+	.compat_ioctl = NULL,
+};

 static ssize_t btfmcodec_attributes_store(struct device *dev,
 				  struct device_attribute *attr,
@@ -148,7 +457,7 @@ static int __init btfmcodec_init(void)

 	// ToDo Rethink of having btfmcodec alone instead of btfmcodec
 	btfmcodec->btfmcodec_dev = btfmcodec_dev;
-	refcount_set(&btfmcodec_dev->active_clients, 1);
+	refcount_set(&btfmcodec_dev->active_clients, 0);
 	mutex_init(&btfmcodec_dev->lock);
 	strlcpy(btfmcodec_dev->dev_name, "btfmcodec_dev", DEVICE_NAME_MAX_LEN);
 	device_initialize(dev);
@@ -158,6 +467,7 @@ static int __init btfmcodec_init(void)

 	cdev_init(&btfmcodec_dev->cdev, &btfmcodec_fops);
 	btfmcodec_dev->cdev.owner = THIS_MODULE;
+	btfmcodec_dev->btfmcodec = (struct btfmcodec_data *)btfmcodec;
 	dev_set_name(dev, btfmcodec_dev->dev_name, btfmcodec_dev->reuse_minor);
 	ret = cdev_add(&btfmcodec_dev->cdev, dev->devt, 1);
 	if (ret) {
@@ -183,6 +493,12 @@ static int __init btfmcodec_init(void)
 	BTFMCODEC_INFO("created a node at /dev/%s with %u:%u\n",
 		btfmcodec_dev->dev_name, dev_major, btfmcodec_dev->reuse_minor);

+	skb_queue_head_init(&btfmcodec_dev->rxq);
+	mutex_init(&btfmcodec_dev->lock);
+	INIT_WORK(&btfmcodec_dev->rx_work, btfmcodec_dev_rxwork);
+	init_waitqueue_head(&btfmcodec_dev->readq);
+	spin_lock_init(&btfmcodec_dev->tx_queue_lock);
+	skb_queue_head_init(&btfmcodec_dev->txq);
 	return ret;

 free_device:
@@ -222,6 +538,7 @@ static void __exit btfmcodec_deinit(void)
 	}

 	btfmcodec_dev = btfmcodec->btfmcodec_dev;
+	skb_queue_purge(&btfmcodec_dev->rxq);
 	idr_remove(&dev_minor, btfmcodec_dev->reuse_minor);
 	class_destroy(dev_class);
 	unregister_chrdev_region(MAJOR(dev_major), 0);