arch/arm/mach-berlin/modules/tzdd_cb/tzdd_cb.c - manifest_repos/salami - Git at Google

 #include <linux/module.h>
 #include <linux/types.h>
 #include <linux/ioctl.h>
 #include <linux/cdev.h>
 #include <linux/semaphore.h>
 #include <linux/slab.h>
 #include <linux/fs.h>
 #include <linux/platform_device.h>
 #include <asm/uaccess.h>

 #include "teec_cb.h"

 MODULE_LICENSE("GPL");

 #define KCB_VERSION "TZDD CB Version 0.0.1"
 #define QDEPTH 20

 /* extract from vmeta_cmd.h */
 #define TA_VEMATA_CB_UUID {0x79b77788, 0x9789, 0x4a7a,{'a', 'm', 'p', 'c', 'b', 'a', 'c', 'k'}}
 enum{
     VDEC_ENABLE_CB = 1000,
     VDEC_WAIT_CB   = 1001,
     VDEC_DISABLE_CB = 1002,
 };
 /* extract end */

 static const teec_cb_uuid_t _g_cb_uuid = TA_VEMATA_CB_UUID;

 int tzddCB_major;
 int tzddCB_minor;

 typedef struct {
     struct cdev cdev;
 } tzddCB_dev_t;

 typedef struct _vmeta_cb_arg_t
 {
     int32_t nbytes;     /* sizeof(args), max = 4K - sizeof(uint32_t) */
     uint8_t *args;   /* the arguments, sizeof(args) == nbytes */
 } vmeta_cb_arg_t;


 typedef struct __CC_Q_{
     uint32_t num;
     uint32_t w_index;
     uint32_t r_index;
     vmeta_cb_arg_t* base;
 }CB_CC_Q;

 typedef struct __tzddCB_private_data_ {
     struct semaphore sem;
     teec_cb_handle_t hcb;
     CB_CC_Q  cbQ;
 } tzddCB_private_data;

 tzddCB_dev_t *tzddCB_dev;

 static struct class* tzddCB_class;

 static int CC_Q_Init(CB_CC_Q *q){
     if(q == NULL)
         return -1;

     q->num = QDEPTH;
     q->w_index = 0;
     q->r_index = 0;
     q->base = (vmeta_cb_arg_t*)kmalloc(sizeof(vmeta_cb_arg_t)*QDEPTH,
             GFP_KERNEL|__GFP_NOWARN);
     if(q->base == NULL)
         return -ENOMEM;

     return 0;
 }

 static int CC_Q_Add(CB_CC_Q *q, vmeta_cb_arg_t* arg){
     int wr_offset;

     if(q == NULL || arg == NULL)
         return -1;

     wr_offset = q->w_index - q->r_index;
     if(wr_offset == -1 || wr_offset == (q->num - 2))
         return -2;

     memcpy((uint8_t*)(q->base+q->w_index), (uint8_t*)arg,
             sizeof(vmeta_cb_arg_t));
     q->w_index++;
     q->w_index %= q->num;

     return 0;
 }

 static int CC_Q_Read_Try(CB_CC_Q *q, vmeta_cb_arg_t* arg){
     int rd_offset;

     if(q == NULL || arg == NULL)
         return -1;

     rd_offset = q->r_index - q->w_index;
     if(rd_offset != 0){
         memcpy((uint8_t*)arg,(uint8_t*)(q->base +q->r_index),
                 sizeof(vmeta_cb_arg_t));
         return 0;
     }
     return -2;
 }

 static int CC_Q_ReadFinish(CB_CC_Q *q){
     int rd_offset;

     if(q == NULL)
         return -1;

     rd_offset = q->r_index - q->w_index;
     if (rd_offset != 0){
         q->r_index ++;
         q->r_index %= q->num;
         return 0;
     }
     return -2;
 }


 static void CC_Q_Destroy(CB_CC_Q *q){
     if(q == NULL)
         return;

     if(q->base){
         vmeta_cb_arg_t arg;

         while(1){
             if(CC_Q_Read_Try(q, &arg))
                 break;
             kfree(arg.args);
             if(CC_Q_ReadFinish(q))
                 break;
         }
         kfree(q->base);
         q->base = NULL;
     }
 }
 static teec_cb_stat_t tzddCB_callback(tee_cb_arg_t *arg, teec_cb_cntx_t cntx){
     tzddCB_private_data* pri_data = NULL;

     pri_data = cntx;
     if (NULL == pri_data){
         printk("%s,%d invalid pri data\n", __FUNCTION__, __LINE__);
         return TEEC_ERROR_BAD_PARAMETERS;
     }

     if(arg != NULL){
         vmeta_cb_arg_t p;

         p.nbytes = arg->nbytes;
         p.args = (uint8_t*)kmalloc(arg->nbytes, GFP_KERNEL|__GFP_NOWARN);
         if(p.args == NULL)
             return TEEC_ERROR_OUT_OF_MEMORY;

         memcpy(p.args, arg->args, arg->nbytes);
         if(CC_Q_Add(&pri_data->cbQ, &p)){
             kfree(p.args);
             return TEEC_ERROR_GENERIC;
         }
     }
     up(&(pri_data->sem));
     return TEEC_SUCCESS;
 }

 static int tzddCB_open(struct inode *inode, struct file *filp){
     tzddCB_private_data* pri_data = NULL;

     if (NULL == filp)
         return -EINVAL;

     pri_data = (tzddCB_private_data*)kmalloc(sizeof(tzddCB_private_data),
             GFP_KERNEL|__GFP_NOWARN);

     if (NULL == pri_data)
         return -ENOMEM;

     memset(pri_data, 0, sizeof(tzddCB_private_data));
     sema_init(&(pri_data->sem), 0);

     if(CC_Q_Init(&pri_data->cbQ)){
         kfree(pri_data);
         return -ENOMEM;
     }

     filp->private_data = pri_data;

     printk(KERN_NOTICE"%s\n", __FUNCTION__);
     return 0;
 }

 static int tzddCB_close(struct inode *inode, struct file *filp){
     tzddCB_private_data* pri_data;

     if (NULL == filp)
         return -EINVAL;

     pri_data = (tzddCB_private_data*)filp->private_data;

     if(pri_data != NULL){
         if(pri_data->hcb != NULL){
             teec_unreg_cb(pri_data->hcb);
             pri_data->hcb = NULL;
         }
         CC_Q_Destroy(&pri_data->cbQ);
         kfree(pri_data);
         filp->private_data = NULL;
     }
     return 0;
 }


 static long tzddCB_ioctl(struct file *filp, unsigned int cmd, unsigned long arg){
     tzddCB_private_data* pri_data = NULL;
     vmeta_cb_arg_t teecb;

     if (NULL == filp)
         return -EINVAL;

     pri_data = (tzddCB_private_data*)filp->private_data;
     if (NULL == pri_data)
         return -EINVAL;

     switch(cmd){
       case VDEC_ENABLE_CB:
         //register callback
         pri_data->hcb = teec_reg_cb(&_g_cb_uuid,
                 tzddCB_callback, (teec_cb_cntx_t)pri_data);
         if (NULL == pri_data->hcb){
             printk(KERN_ERR"%s,%d fail to register callback\n", __FUNCTION__, __LINE__);
             return -100;
         }
         printk(KERN_NOTICE"tzddCB_ioctl VDEC_ENABLE_CB\n");
         break;
       case VDEC_DISABLE_CB:
         teec_unreg_cb(pri_data->hcb);
         pri_data->hcb = NULL;
         printk(KERN_NOTICE"tzddCB_ioctl VDEC_DISABLE_CB\n");
         break;
       case VDEC_WAIT_CB:
         if(down_interruptible(&pri_data->sem))
             return -EAGAIN;

         if(CC_Q_Read_Try(&pri_data->cbQ,&teecb))
             return -EAGAIN;
         if(CC_Q_ReadFinish(&pri_data->cbQ))
             return -EFAULT;
         if (copy_to_user((void*)arg, teecb.args, teecb.nbytes))
             return -EFAULT;
         kfree(teecb.args);
         break;
       default:
         break;
     }
     return 0;
 }

 static struct file_operations tzddCB_fops = {
     .open = tzddCB_open,
     .release = tzddCB_close,
     .unlocked_ioctl = tzddCB_ioctl,
     .owner = THIS_MODULE,
 };

 static struct platform_driver tzddCB_drv = {
     .driver = {
         .name = "tzddCB",
         .owner = THIS_MODULE,
     },
 };


 static int32_t tzddCB_cdev_init(tzddCB_dev_t *dev)
 {
     int32_t err = 0, dev_nr;

     err = alloc_chrdev_region(&dev_nr, 0, 1, "tzddCB");
     tzddCB_major = MAJOR(dev_nr);
     tzddCB_minor = MINOR(dev_nr);

     if (err < 0)
         return err;

     printk("%s, %d MAJOR:%d, MINOR:%d\n", __FUNCTION__, __LINE__,
             tzddCB_major, tzddCB_minor);

     cdev_init(&dev->cdev, &tzddCB_fops);
     dev->cdev.owner = THIS_MODULE;
     dev->cdev.ops = &tzddCB_fops;

     err = cdev_add(&dev->cdev, dev_nr, 1);
     if (err){
         printk("%s, %d fail to add cdev\n", __FUNCTION__, __LINE__);
         goto _error_1;
     }

     err = platform_driver_register(&tzddCB_drv);
     if (err){
         printk("%s, %d fail to platform_driver_register\n", __FUNCTION__, __LINE__);
         goto _error_2;
     }

     tzddCB_class = class_create(THIS_MODULE, "tzddCB");
     if (IS_ERR(tzddCB_class)){
         printk("%s,%d fail to create class\n", __FUNCTION__, __LINE__);
         goto _error_3;
     }

     device_create(tzddCB_class, NULL, MKDEV(tzddCB_major, tzddCB_minor), NULL, "tzddCB");

     return 0;

 _error_3:
     platform_driver_unregister(&tzddCB_drv);
 _error_2:
     cdev_del(&(dev->cdev));
 _error_1:
     unregister_chrdev_region(MKDEV(tzddCB_major, tzddCB_minor), 1);
     return err;
 }

 static void tzddCB_cdev_cleanup(tzddCB_dev_t *dev)
 {
     device_destroy(tzddCB_class, MKDEV(tzddCB_major, tzddCB_minor));
     class_destroy(tzddCB_class);
     platform_driver_unregister(&tzddCB_drv);
     cdev_del(&dev->cdev);
     unregister_chrdev_region(MKDEV(tzddCB_major, tzddCB_minor), 1);
     return;
 }


 static int32_t __init tzddCB_init(void){
     unsigned int ret = 0;

     tzddCB_dev = kmalloc(sizeof(tzddCB_dev_t), GFP_KERNEL);
     if (NULL == tzddCB_dev)
         return -ENOMEM;

     memset(tzddCB_dev, 0, sizeof(tzddCB_dev_t));

     ret = tzddCB_cdev_init(tzddCB_dev);
     if (ret < 0){
         kfree(tzddCB_dev);
         tzddCB_dev = NULL;
         return ret;
     }
     printk("%s\n", KCB_VERSION);
     return 0;
 }

 static void __exit tzddCB_cleanup(void){
     if (NULL == tzddCB_dev)
         return;

     tzddCB_cdev_cleanup(tzddCB_dev);
     kfree(tzddCB_dev);
     tzddCB_dev = NULL;
 }

 module_init(tzddCB_init);
 module_exit(tzddCB_cleanup);
	#include <linux/module.h>
	#include <linux/types.h>
	#include <linux/ioctl.h>
	#include <linux/cdev.h>
	#include <linux/semaphore.h>
	#include <linux/slab.h>
	#include <linux/fs.h>
	#include <linux/platform_device.h>
	#include <asm/uaccess.h>

	#include "teec_cb.h"

	MODULE_LICENSE("GPL");

	#define KCB_VERSION "TZDD CB Version 0.0.1"
	#define QDEPTH 20

	/* extract from vmeta_cmd.h */
	#define TA_VEMATA_CB_UUID {0x79b77788, 0x9789, 0x4a7a,{'a', 'm', 'p', 'c', 'b', 'a', 'c', 'k'}}
	enum{
	VDEC_ENABLE_CB = 1000,
	VDEC_WAIT_CB = 1001,
	VDEC_DISABLE_CB = 1002,
	};
	/* extract end */

	static const teec_cb_uuid_t _g_cb_uuid = TA_VEMATA_CB_UUID;

	int tzddCB_major;
	int tzddCB_minor;

	typedef struct {
	struct cdev cdev;
	} tzddCB_dev_t;

	typedef struct _vmeta_cb_arg_t
	{
	int32_t nbytes; /* sizeof(args), max = 4K - sizeof(uint32_t) */
	uint8_t args; / the arguments, sizeof(args) == nbytes */
	} vmeta_cb_arg_t;


	typedef struct __CC_Q_{
	uint32_t num;
	uint32_t w_index;
	uint32_t r_index;
	vmeta_cb_arg_t* base;
	}CB_CC_Q;

	typedef struct __tzddCB_private_data_ {
	struct semaphore sem;
	teec_cb_handle_t hcb;
	CB_CC_Q cbQ;
	} tzddCB_private_data;

	tzddCB_dev_t *tzddCB_dev;

	static struct class* tzddCB_class;

	static int CC_Q_Init(CB_CC_Q *q){
	if(q == NULL)
	return -1;

	q->num = QDEPTH;
	q->w_index = 0;
	q->r_index = 0;
	q->base = (vmeta_cb_arg_t)kmalloc(sizeof(vmeta_cb_arg_t)QDEPTH,
	GFP_KERNEL\|__GFP_NOWARN);
	if(q->base == NULL)
	return -ENOMEM;

	return 0;
	}

	static int CC_Q_Add(CB_CC_Q q, vmeta_cb_arg_t arg){
	int wr_offset;

	if(q == NULL \|\| arg == NULL)
	return -1;

	wr_offset = q->w_index - q->r_index;
	if(wr_offset == -1 \|\| wr_offset == (q->num - 2))
	return -2;

	memcpy((uint8_t)(q->base+q->w_index), (uint8_t)arg,
	sizeof(vmeta_cb_arg_t));
	q->w_index++;
	q->w_index %= q->num;

	return 0;
	}

	static int CC_Q_Read_Try(CB_CC_Q q, vmeta_cb_arg_t arg){
	int rd_offset;

	if(q == NULL \|\| arg == NULL)
	return -1;

	rd_offset = q->r_index - q->w_index;
	if(rd_offset != 0){
	memcpy((uint8_t)arg,(uint8_t)(q->base +q->r_index),
	sizeof(vmeta_cb_arg_t));
	return 0;
	}
	return -2;
	}

	static int CC_Q_ReadFinish(CB_CC_Q *q){
	int rd_offset;

	if(q == NULL)
	return -1;

	rd_offset = q->r_index - q->w_index;
	if (rd_offset != 0){
	q->r_index ++;
	q->r_index %= q->num;
	return 0;
	}
	return -2;
	}


	static void CC_Q_Destroy(CB_CC_Q *q){
	if(q == NULL)
	return;

	if(q->base){
	vmeta_cb_arg_t arg;

	while(1){
	if(CC_Q_Read_Try(q, &arg))
	break;
	kfree(arg.args);
	if(CC_Q_ReadFinish(q))
	break;
	}
	kfree(q->base);
	q->base = NULL;
	}
	}
	static teec_cb_stat_t tzddCB_callback(tee_cb_arg_t *arg, teec_cb_cntx_t cntx){
	tzddCB_private_data* pri_data = NULL;

	pri_data = cntx;
	if (NULL == pri_data){
	printk("%s,%d invalid pri data\n", __FUNCTION__, __LINE__);
	return TEEC_ERROR_BAD_PARAMETERS;
	}

	if(arg != NULL){
	vmeta_cb_arg_t p;

	p.nbytes = arg->nbytes;
	p.args = (uint8_t*)kmalloc(arg->nbytes, GFP_KERNEL\|__GFP_NOWARN);
	if(p.args == NULL)
	return TEEC_ERROR_OUT_OF_MEMORY;

	memcpy(p.args, arg->args, arg->nbytes);
	if(CC_Q_Add(&pri_data->cbQ, &p)){
	kfree(p.args);
	return TEEC_ERROR_GENERIC;
	}
	}
	up(&(pri_data->sem));
	return TEEC_SUCCESS;
	}

	static int tzddCB_open(struct inode inode, struct file filp){
	tzddCB_private_data* pri_data = NULL;

	if (NULL == filp)
	return -EINVAL;

	pri_data = (tzddCB_private_data*)kmalloc(sizeof(tzddCB_private_data),
	GFP_KERNEL\|__GFP_NOWARN);

	if (NULL == pri_data)
	return -ENOMEM;

	memset(pri_data, 0, sizeof(tzddCB_private_data));
	sema_init(&(pri_data->sem), 0);

	if(CC_Q_Init(&pri_data->cbQ)){
	kfree(pri_data);
	return -ENOMEM;
	}

	filp->private_data = pri_data;

	printk(KERN_NOTICE"%s\n", __FUNCTION__);
	return 0;
	}

	static int tzddCB_close(struct inode inode, struct file filp){
	tzddCB_private_data* pri_data;

	if (NULL == filp)
	return -EINVAL;

	pri_data = (tzddCB_private_data*)filp->private_data;

	if(pri_data != NULL){
	if(pri_data->hcb != NULL){
	teec_unreg_cb(pri_data->hcb);
	pri_data->hcb = NULL;
	}
	CC_Q_Destroy(&pri_data->cbQ);
	kfree(pri_data);
	filp->private_data = NULL;
	}
	return 0;
	}


	static long tzddCB_ioctl(struct file *filp, unsigned int cmd, unsigned long arg){
	tzddCB_private_data* pri_data = NULL;
	vmeta_cb_arg_t teecb;

	if (NULL == filp)
	return -EINVAL;

	pri_data = (tzddCB_private_data*)filp->private_data;
	if (NULL == pri_data)
	return -EINVAL;

	switch(cmd){
	case VDEC_ENABLE_CB:
	//register callback
	pri_data->hcb = teec_reg_cb(&_g_cb_uuid,
	tzddCB_callback, (teec_cb_cntx_t)pri_data);
	if (NULL == pri_data->hcb){
	printk(KERN_ERR"%s,%d fail to register callback\n", __FUNCTION__, __LINE__);
	return -100;
	}
	printk(KERN_NOTICE"tzddCB_ioctl VDEC_ENABLE_CB\n");
	break;
	case VDEC_DISABLE_CB:
	teec_unreg_cb(pri_data->hcb);
	pri_data->hcb = NULL;
	printk(KERN_NOTICE"tzddCB_ioctl VDEC_DISABLE_CB\n");
	break;
	case VDEC_WAIT_CB:
	if(down_interruptible(&pri_data->sem))
	return -EAGAIN;

	if(CC_Q_Read_Try(&pri_data->cbQ,&teecb))
	return -EAGAIN;
	if(CC_Q_ReadFinish(&pri_data->cbQ))
	return -EFAULT;
	if (copy_to_user((void*)arg, teecb.args, teecb.nbytes))
	return -EFAULT;
	kfree(teecb.args);
	break;
	default:
	break;
	}
	return 0;
	}

	static struct file_operations tzddCB_fops = {
	.open = tzddCB_open,
	.release = tzddCB_close,
	.unlocked_ioctl = tzddCB_ioctl,
	.owner = THIS_MODULE,
	};

	static struct platform_driver tzddCB_drv = {
	.driver = {
	.name = "tzddCB",
	.owner = THIS_MODULE,
	},
	};


	static int32_t tzddCB_cdev_init(tzddCB_dev_t *dev)
	{
	int32_t err = 0, dev_nr;

	err = alloc_chrdev_region(&dev_nr, 0, 1, "tzddCB");
	tzddCB_major = MAJOR(dev_nr);
	tzddCB_minor = MINOR(dev_nr);

	if (err < 0)
	return err;

	printk("%s, %d MAJOR:%d, MINOR:%d\n", __FUNCTION__, __LINE__,
	tzddCB_major, tzddCB_minor);

	cdev_init(&dev->cdev, &tzddCB_fops);
	dev->cdev.owner = THIS_MODULE;
	dev->cdev.ops = &tzddCB_fops;

	err = cdev_add(&dev->cdev, dev_nr, 1);
	if (err){
	printk("%s, %d fail to add cdev\n", __FUNCTION__, __LINE__);
	goto _error_1;
	}

	err = platform_driver_register(&tzddCB_drv);
	if (err){
	printk("%s, %d fail to platform_driver_register\n", __FUNCTION__, __LINE__);
	goto _error_2;
	}

	tzddCB_class = class_create(THIS_MODULE, "tzddCB");
	if (IS_ERR(tzddCB_class)){
	printk("%s,%d fail to create class\n", __FUNCTION__, __LINE__);
	goto _error_3;
	}

	device_create(tzddCB_class, NULL, MKDEV(tzddCB_major, tzddCB_minor), NULL, "tzddCB");

	return 0;

	_error_3:
	platform_driver_unregister(&tzddCB_drv);
	_error_2:
	cdev_del(&(dev->cdev));
	_error_1:
	unregister_chrdev_region(MKDEV(tzddCB_major, tzddCB_minor), 1);
	return err;
	}

	static void tzddCB_cdev_cleanup(tzddCB_dev_t *dev)
	{
	device_destroy(tzddCB_class, MKDEV(tzddCB_major, tzddCB_minor));
	class_destroy(tzddCB_class);
	platform_driver_unregister(&tzddCB_drv);
	cdev_del(&dev->cdev);
	unregister_chrdev_region(MKDEV(tzddCB_major, tzddCB_minor), 1);
	return;
	}


	static int32_t __init tzddCB_init(void){
	unsigned int ret = 0;

	tzddCB_dev = kmalloc(sizeof(tzddCB_dev_t), GFP_KERNEL);
	if (NULL == tzddCB_dev)
	return -ENOMEM;

	memset(tzddCB_dev, 0, sizeof(tzddCB_dev_t));

	ret = tzddCB_cdev_init(tzddCB_dev);
	if (ret < 0){
	kfree(tzddCB_dev);
	tzddCB_dev = NULL;
	return ret;
	}
	printk("%s\n", KCB_VERSION);
	return 0;
	}

	static void __exit tzddCB_cleanup(void){
	if (NULL == tzddCB_dev)
	return;

	tzddCB_cdev_cleanup(tzddCB_dev);
	kfree(tzddCB_dev);
	tzddCB_dev = NULL;
	}

	module_init(tzddCB_init);
	module_exit(tzddCB_cleanup);