[GitHub/mt8127/android_kernel_alcatel_ttab.git] / drivers / misc / mediatek / gyroscope / gyro_factory.c

#include "gyro_factory.h"

static int gyro_factory_open(struct inode *inode, struct file *file)
{
    file->private_data = gyro_context_obj;

    if (file->private_data == NULL)
    {
        GYRO_ERR("null pointer!!\n");
        return -EINVAL;
    }
    return nonseekable_open(inode, file);
}

static int gyro_factory_release(struct inode *inode, struct file *file)
{
    file->private_data = NULL;
    return 0;
}

static int gyro_set_cali(int data[GYRO_AXES_NUM])
{
	struct gyro_context *cxt = gyro_context_obj;
	GYRO_LOG(" factory gyro cali %d,%d,%d \n",data[GYRO_AXIS_X],data[GYRO_AXIS_Y],data[GYRO_AXIS_Z] );
	GYRO_LOG(" original gyro  cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);
	cxt->cali_sw[GYRO_AXIS_X] += data[GYRO_AXIS_X];
    cxt->cali_sw[GYRO_AXIS_Y] += data[GYRO_AXIS_Y];
    cxt->cali_sw[GYRO_AXIS_Z] += data[GYRO_AXIS_Z];
	GYRO_LOG(" GYRO new cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);

	return 0;
}

static int gyro_clear_cali(void)
{
	struct gyro_context *cxt = gyro_context_obj;
	cxt->cali_sw[GYRO_AXIS_X] = 0;
    cxt->cali_sw[GYRO_AXIS_Y] = 0;
    cxt->cali_sw[GYRO_AXIS_Z] = 0;
	GYRO_LOG(" GYRO after clear cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z] );
	return 0;
}

static long gyro_factory_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
    void __user *data;
    long err = 0;
	struct gyro_context *cxt = gyro_context_obj;
	int x,y,z,status;
	char strbuf[256];
    int cali[3] = {0};
    SENSOR_DATA sensor_data = {0};
	int smtRes;
	
    if (_IOC_DIR(cmd) & _IOC_READ)
    {
        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
    }
    else if (_IOC_DIR(cmd) & _IOC_WRITE)
    {
        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
    }

    if (err)
    {
        GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
        return -EFAULT;
    }

    switch (cmd)
    {
    case GYROSCOPE_IOCTL_INIT:
		if(cxt->gyro_ctl.enable_nodata!= NULL){
			err = cxt->gyro_ctl.enable_nodata(1);
			if(err < 0)
			{
				GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
				break;
			}
			GYRO_LOG("GYROSCOPE_IOCTL_INIT\n");

		}else{
			GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
		}
	    break;
    case GYROSCOPE_IOCTL_SMT_DATA:
		data = (void __user *) arg;
        if (data == NULL)
        {
            err = -EINVAL;
            break;    
        }
		smtRes = 1;
        err = copy_to_user(data, &smtRes,  sizeof(smtRes));
        if (err)
        {
            err = -EINVAL;
            GYRO_ERR("copy gyro data to user failed!\n");
        }

        break;    
    case GYROSCOPE_IOCTL_READ_SENSORDATA:
		data = (void __user *) arg;
        if (data == NULL)
        {
            err = -EINVAL;
            break;    
        }
		if(cxt->gyro_data.get_data != NULL){
			err = cxt->gyro_data.get_data(&x, &y, &z, &status);
			if(err < 0)
			{
				GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
				break;
			}
			x+=cxt->cali_sw[0];
			y+=cxt->cali_sw[1];
			z+=cxt->cali_sw[2];
			sprintf(strbuf, "%x %x %x", x, y, z);
			GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data : (%d, %d, %d)!\n", x, y, z);
			GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read strbuf : (%s)!\n", strbuf);
			
	        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
	        {
	            err = -EFAULT;
	            break;    
	        }
		}else{
			GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
		}
        break;
		
    case GYROSCOPE_IOCTL_READ_SENSORDATA_RAW:
		data = (void __user *) arg;
        if (data == NULL)
        {
            err = -EINVAL;
            break;    
        }
		if(cxt->gyro_data.get_raw_data != NULL){
			err = cxt->gyro_data.get_raw_data(&x, &y, &z);
			if(err < 0)
			{
				GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data fail!\n");
				break;
			}
			x+=cxt->cali_sw[0];
			y+=cxt->cali_sw[1];
			z+=cxt->cali_sw[2];
			sprintf(strbuf, "%x %x %x", x, y, z);
			GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data : (%d, %d, %d)!\n", x, y, z);
	        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
	        {
	            err = -EFAULT;
	            break;    
	        }
		}else{
			GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA FAIL!\n ");
		}
        break;   
    case GYROSCOPE_IOCTL_SET_CALI:
        data = (void __user*)arg;
        if (data == NULL)
        {
            err = -EINVAL;
            break;    
        }
        if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
        {
            err = -EFAULT;
            break;    
        }
        cali[0] = sensor_data.x ;
        cali[1] = sensor_data.y ;
        cali[2] = sensor_data.z ;
		GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI data : (%d, %d, %d)!\n", cali[0], cali[1], cali[2]);
        gyro_set_cali(cali);
/*
		if(cxt->gyro_ctl.gyro_calibration != NULL)
		{
			err = cxt->gyro_ctl.gyro_calibration(SETCALI, cali);
			if(err < 0)
			{
				GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI fail!\n");
				break;
			}
		}     
		*/
        break;

    case GYROSCOPE_IOCTL_CLR_CALI:
		/*
        if(cxt->gyro_ctl.gyro_calibration != NULL)
		{
			err = cxt->gyro_ctl.gyro_calibration(CLRCALI, cali);
			if(err < 0)
			{
				GYRO_LOG("GYROSCOPE_IOCTL_CLR_CALI fail!\n");
				break;
			}
		}   
		*/
		gyro_clear_cali();
        break;

    case GYROSCOPE_IOCTL_GET_CALI:
        data = (void __user*)arg;
        if (data == NULL)
        {
            err = -EINVAL;
            break;    
        }
		/*
        if(cxt->gyro_ctl.gyro_calibration != NULL)
		{
			err = cxt->gyro_ctl.gyro_calibration(GETCALI, cali);
			if(err < 0)
			{
				GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI fail!\n");
				break;
			}
		}
		*/
		GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI data : (%d, %d, %d)!\n", cxt->cali_sw[0] , cxt->cali_sw[1], cxt->cali_sw[2]);
        sensor_data.x = cxt->cali_sw[0]; ; 
        sensor_data.y = cxt->cali_sw[1]; ;
        sensor_data.z = cxt->cali_sw[2]; ;
        if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
        {
            err = -EFAULT;
            break;
        }
        break;
		
    default:
        GYRO_LOG("unknown IOCTL: 0x%08x\n", cmd);
        err = -ENOIOCTLCMD;
        break;

    }
    return err;
}


static struct file_operations gyro_factory_fops = {
    .open = gyro_factory_open,
    .release = gyro_factory_release,
    .unlocked_ioctl = gyro_factory_unlocked_ioctl,
};

static struct miscdevice gyro_factory_device = {
    .minor = MISC_DYNAMIC_MINOR,
    .name = "gyroscope",
    .fops = &gyro_factory_fops,
};

int gyro_factory_device_init()
{
	int error = 0;
	struct gyro_context *cxt = gyro_context_obj;

	if (!cxt->gyro_ctl.is_use_common_factory) {
		GYRO_LOG("Node of '/dev/gyroscope' has already existed!\n");
		return -1;
	}
    if ((error = misc_register(&gyro_factory_device)))
    {
        GYRO_LOG("gyro_factory_device register failed\n");
		error = -1;
    }	
	return error;
}
Commit	Line	Data
6fa3eb70 S	1	#include "gyro_factory.h"
	2
	3	static int gyro_factory_open(struct inode inode, struct file file)
	4	{
	5	file->private_data = gyro_context_obj;
	6
	7	if (file->private_data == NULL)
	8	{
	9	GYRO_ERR("null pointer!!\n");
	10	return -EINVAL;
	11	}
	12	return nonseekable_open(inode, file);
	13	}
	14
	15	static int gyro_factory_release(struct inode inode, struct file file)
	16	{
	17	file->private_data = NULL;
	18	return 0;
	19	}
	20
	21	static int gyro_set_cali(int data[GYRO_AXES_NUM])
	22	{
	23	struct gyro_context *cxt = gyro_context_obj;
	24	GYRO_LOG(" factory gyro cali %d,%d,%d \n",data[GYRO_AXIS_X],data[GYRO_AXIS_Y],data[GYRO_AXIS_Z] );
	25	GYRO_LOG(" original gyro cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);
	26	cxt->cali_sw[GYRO_AXIS_X] += data[GYRO_AXIS_X];
	27	cxt->cali_sw[GYRO_AXIS_Y] += data[GYRO_AXIS_Y];
	28	cxt->cali_sw[GYRO_AXIS_Z] += data[GYRO_AXIS_Z];
	29	GYRO_LOG(" GYRO new cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);
	30
	31	return 0;
	32	}
	33
	34	static int gyro_clear_cali(void)
	35	{
	36	struct gyro_context *cxt = gyro_context_obj;
	37	cxt->cali_sw[GYRO_AXIS_X] = 0;
	38	cxt->cali_sw[GYRO_AXIS_Y] = 0;
	39	cxt->cali_sw[GYRO_AXIS_Z] = 0;
	40	GYRO_LOG(" GYRO after clear cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z] );
	41	return 0;
	42	}
	43
	44	static long gyro_factory_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
	45	{
	46	void __user *data;
	47	long err = 0;
	48	struct gyro_context *cxt = gyro_context_obj;
	49	int x,y,z,status;
	50	char strbuf[256];
	51	int cali[3] = {0};
	52	SENSOR_DATA sensor_data = {0};
	53	int smtRes;
	54
	55	if (_IOC_DIR(cmd) & _IOC_READ)
	56	{
	57	err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
	58	}
	59	else if (_IOC_DIR(cmd) & _IOC_WRITE)
	60	{
	61	err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
	62	}
	63
	64	if (err)
65	{
66	GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
67	return -EFAULT;
68	}
69
70	switch (cmd)
71	{
72	case GYROSCOPE_IOCTL_INIT:
73	if(cxt->gyro_ctl.enable_nodata!= NULL){
74	err = cxt->gyro_ctl.enable_nodata(1);
75	if(err < 0)
76	{
77	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
78	break;
79	}
80	GYRO_LOG("GYROSCOPE_IOCTL_INIT\n");
81
82	}else{
83	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
84	}
85	break;
86	case GYROSCOPE_IOCTL_SMT_DATA:
87	data = (void __user *) arg;
88	if (data == NULL)
89	{
90	err = -EINVAL;
91	break;
92	}
93	smtRes = 1;
94	err = copy_to_user(data, &smtRes, sizeof(smtRes));
95	if (err)
96	{
97	err = -EINVAL;
98	GYRO_ERR("copy gyro data to user failed!\n");
99	}
100
101	break;
102	case GYROSCOPE_IOCTL_READ_SENSORDATA:
103	data = (void __user *) arg;
104	if (data == NULL)
105	{
106	err = -EINVAL;
107	break;
108	}
109	if(cxt->gyro_data.get_data != NULL){
110	err = cxt->gyro_data.get_data(&x, &y, &z, &status);
111	if(err < 0)
112	{
113	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
114	break;
115	}
116	x+=cxt->cali_sw[0];
117	y+=cxt->cali_sw[1];
118	z+=cxt->cali_sw[2];
119	sprintf(strbuf, "%x %x %x", x, y, z);
120	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data : (%d, %d, %d)!\n", x, y, z);
121	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read strbuf : (%s)!\n", strbuf);
122
123	if (copy_to_user(data, strbuf, strlen(strbuf)+1))
124	{
125	err = -EFAULT;
126	break;
127	}
128	}else{
129	GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
130	}
131	break;
132
133	case GYROSCOPE_IOCTL_READ_SENSORDATA_RAW:
134	data = (void __user *) arg;
135	if (data == NULL)
136	{
137	err = -EINVAL;
138	break;
139	}
140	if(cxt->gyro_data.get_raw_data != NULL){
141	err = cxt->gyro_data.get_raw_data(&x, &y, &z);
142	if(err < 0)
143	{
144	GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data fail!\n");
145	break;
146	}
147	x+=cxt->cali_sw[0];
148	y+=cxt->cali_sw[1];
149	z+=cxt->cali_sw[2];
150	sprintf(strbuf, "%x %x %x", x, y, z);
151	GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data : (%d, %d, %d)!\n", x, y, z);
152	if (copy_to_user(data, strbuf, strlen(strbuf)+1))
153	{
154	err = -EFAULT;
155	break;
156	}
157	}else{
158	GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA FAIL!\n ");
159	}
160	break;
161	case GYROSCOPE_IOCTL_SET_CALI:
162	data = (void __user*)arg;
163	if (data == NULL)
164	{
165	err = -EINVAL;
166	break;
167	}
168	if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
169	{
170	err = -EFAULT;
171	break;
172	}
173	cali[0] = sensor_data.x ;
174	cali[1] = sensor_data.y ;
175	cali[2] = sensor_data.z ;
176	GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI data : (%d, %d, %d)!\n", cali[0], cali[1], cali[2]);
177	gyro_set_cali(cali);
178	/*
179	if(cxt->gyro_ctl.gyro_calibration != NULL)
180	{
181	err = cxt->gyro_ctl.gyro_calibration(SETCALI, cali);
182	if(err < 0)
183	{
184	GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI fail!\n");
185	break;
186	}
187	}
188	*/
189	break;
190
191	case GYROSCOPE_IOCTL_CLR_CALI:
192	/*
193	if(cxt->gyro_ctl.gyro_calibration != NULL)
194	{
195	err = cxt->gyro_ctl.gyro_calibration(CLRCALI, cali);
196	if(err < 0)
197	{
198	GYRO_LOG("GYROSCOPE_IOCTL_CLR_CALI fail!\n");
199	break;
200	}
201	}
202	*/
203	gyro_clear_cali();
204	break;
205
206	case GYROSCOPE_IOCTL_GET_CALI:
207	data = (void __user*)arg;
208	if (data == NULL)
209	{
210	err = -EINVAL;
211	break;
212	}
213	/*
214	if(cxt->gyro_ctl.gyro_calibration != NULL)
215	{
216	err = cxt->gyro_ctl.gyro_calibration(GETCALI, cali);
217	if(err < 0)
218	{
219	GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI fail!\n");
220	break;
221	}
222	}
223	*/
224	GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI data : (%d, %d, %d)!\n", cxt->cali_sw[0] , cxt->cali_sw[1], cxt->cali_sw[2]);
225	sensor_data.x = cxt->cali_sw[0]; ;
226	sensor_data.y = cxt->cali_sw[1]; ;
227	sensor_data.z = cxt->cali_sw[2]; ;
228	if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
229	{
230	err = -EFAULT;
231	break;
232	}
233	break;
234
235	default:
236	GYRO_LOG("unknown IOCTL: 0x%08x\n", cmd);
237	err = -ENOIOCTLCMD;
238	break;
239
240	}
241	return err;
242	}
243
244
245	static struct file_operations gyro_factory_fops = {
246	.open = gyro_factory_open,
247	.release = gyro_factory_release,
248	.unlocked_ioctl = gyro_factory_unlocked_ioctl,
249	};
250
251	static struct miscdevice gyro_factory_device = {
252	.minor = MISC_DYNAMIC_MINOR,
253	.name = "gyroscope",
254	.fops = &gyro_factory_fops,
255	};
256
257	int gyro_factory_device_init()
258	{
259	int error = 0;
260	struct gyro_context *cxt = gyro_context_obj;
261
262	if (!cxt->gyro_ctl.is_use_common_factory) {
263	GYRO_LOG("Node of '/dev/gyroscope' has already existed!\n");
264	return -1;
265	}
266	if ((error = misc_register(&gyro_factory_device)))
267	{
268	GYRO_LOG("gyro_factory_device register failed\n");
269	error = -1;
270	}
271	return error;
272	}
273
274
275
276