/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** * * File Name : l3g4200d.c * Authors : MH - C&I BU - Application Team * : Carmine Iascone (carmine.iascone@st.com) * : Matteo Dameno (matteo.dameno@st.com) * Version : V 0.1 * Date : 29/01/2010 * Description : L3G4200D digital output gyroscope sensor API * ******************************************************************************** * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES * OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE * PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT. * AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, * INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE * CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. * * THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS. * ******************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define L3G4200D_MAJOR 102 #define L3G4200D_MINOR 4 /* l3g4200d gyroscope registers */ #define WHO_AM_I 0x0F #define CTRL_REG1 0x20 /* power control reg */ #define CTRL_REG2 0x21 /* power control reg */ #define CTRL_REG3 0x22 /* power control reg */ #define CTRL_REG4 0x23 /* interrupt control reg */ #define CTRL_REG5 0x24 /* interrupt control reg */ #define OUT_TEMP 0x26 /* Temperature data */ #define AXISDATA_REG 0x28 #define STATUS_REG 0x27 #define PM_OFF 0x00 #define PM_NORMAL 0x08 #define ENABLE_ALL_AXES 0x07 #define ODR200_BW12_5 0x00 /* ODR = 200Hz; BW = 12.5Hz */ #define ODR200_BW25 0x10 /* ODR = 200Hz; BW = 25Hz */ #define ODR200_BW50 0x20 /* ODR = 200Hz; BW = 50Hz */ #define ODR100_BW12_5 0x40 /* ODR = 100Hz; BW = 12.5Hz */ #define ODR100_BW25 0x50 /* ODR = 100Hz; BW = 25Hz */ #define ODR400_BW25 0x90 /* ODR = 400Hz; BW = 25Hz */ #define ODR400_BW50 0xA0 /* ODR = 400Hz; BW = 50Hz */ #define ODR400_BW110 0xB0 /* ODR = 400Hz; BW = 110Hz */ #define ODR800_BW50 0xE0 /* ODR = 800Hz; BW = 50Hz */ #define ODR800_BW100 0xF0 /* ODR = 800Hz; BW = 100Hz */ #define MIN_ST 175 #define MAX_ST 875 #define MAX_ENTRY 1 #define MAX_DELAY (MAX_ENTRY * 9523809LL) /*#define SHIFT_ADJ_2G 4 #define SHIFT_ADJ_4G 3 #define SHIFT_ADJ_8G 2*/ #define DEBUG 0 static DEFINE_MUTEX(l3g4200d_mutex); static unsigned char reg_backup[5]; static unsigned int fd_cnt = 0; extern unsigned int HWREV; /* * L3G4200D gyroscope data * brief structure containing gyroscope values for yaw, pitch and roll in * signed short */ static const struct odr_delay { u8 odr; /* odr reg setting */ u32 delay_ns; /* odr in ns */ } odr_delay_table[] = { { ODR800_BW100, 1190476LL }, /* 840Hz */ { ODR400_BW110, 2380952LL }, /* 420Hz */ { ODR200_BW50, 4761904LL }, /* 210Hz */ { ODR100_BW25, 9523809LL }, /* 105Hz */ }; struct l3g4200d_t { short y, /* yaw data. Range -2048 to 2047. */ p, /* pitch data. Range -2048 to 2047. */ r; /* roll data. Range -2048 to 2047. */ }; /* static struct i2c_client *l3g4200d_client; */ struct l3g4200d_data { struct i2c_client *client; struct l3g4200d_platform_data *pdata; struct input_dev *input_dev; struct mutex lock; struct workqueue_struct *l3g_wq; struct work_struct work; struct hrtimer timer; bool enable; bool drop_next_event; bool interruptible; /* interrupt or polling? */ int entries; /* number of fifo entries */ u8 ctrl_regs[5]; /* saving register settings */ u32 time_to_read; /* time needed to read one entry */ ktime_t polling_delay; /* polling time for timer */ /* u8 shift_adj; */ }; static struct l3g4200d_data *gyro; static struct class *l3g_gyro_dev_class; static int device_init(void); int l3g4200d_set_mode(char ); int l3g4200d_read_gyro_values(struct l3g4200d_t *data); static char l3g4200d_i2c_write(unsigned char reg_addr, unsigned char *data, unsigned char len); static char l3g4200d_i2c_read(unsigned char reg_addr, unsigned char *data, unsigned char len); static void l3g_report_gyro_values(void) { struct l3g4200d_t data; l3g4200d_read_gyro_values(&data); input_report_rel(gyro->input_dev, REL_RX, data.y); input_report_rel(gyro->input_dev, REL_RY, data.p); input_report_rel(gyro->input_dev, REL_RZ, data.r); input_sync(gyro->input_dev); } static enum hrtimer_restart l3g_timer_func(struct hrtimer *timer) { queue_work(gyro->l3g_wq, &gyro->work); hrtimer_forward_now(&gyro->timer, gyro->polling_delay); return HRTIMER_RESTART; } static void l3g_work_func(struct work_struct *work) { l3g_report_gyro_values(); } static ssize_t l3g_show_enable(struct device *dev, struct device_attribute *attr, char *buf) { return sprintf(buf, "%d\n", gyro->enable); } static ssize_t l3g_set_enable(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { int err = 0; bool new_enable; if (sysfs_streq(buf, "1")) new_enable = true; else if (sysfs_streq(buf, "0")) new_enable = false; else { printk("%s: invalid value %d\n", __func__, *buf); return -EINVAL; } if (new_enable == gyro->enable) return size; mutex_lock(&gyro->lock); if (new_enable) { /* turning on */ device_init(); mdelay(300); printk("%s: Gyro_sensor Turn on \n", __func__); hrtimer_start(&gyro->timer, gyro->polling_delay, HRTIMER_MODE_REL); printk("%s: Timer on \n", __func__); } else { hrtimer_cancel(&gyro->timer); cancel_work_sync(&gyro->work); printk("%s: Timer off \n", __func__); l3g4200d_set_mode(PM_OFF); } /* turning off */ gyro->enable = new_enable; mutex_unlock(&gyro->lock); return err ? err : size; } static ssize_t l3g_show_delay(struct device *dev, struct device_attribute *attr, char *buf) { return sprintf(buf, "%lld\n", ktime_to_ns(gyro->polling_delay)); } static ssize_t l3g_set_delay(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { int res = 0; u64 delay_ns; res = strict_strtoll(buf, 10, &delay_ns); if (res < 0) return res; mutex_lock(&gyro->lock); if (delay_ns != ktime_to_ns(gyro->polling_delay)) { hrtimer_cancel(&gyro->timer); gyro->polling_delay = ns_to_ktime(delay_ns); if (gyro->enable) hrtimer_start(&gyro->timer, gyro->polling_delay, HRTIMER_MODE_REL); } mutex_unlock(&gyro->lock); return size; } static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, l3g_show_enable, l3g_set_enable); static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP, l3g_show_delay, l3g_set_delay); /* set l3g4200d digital gyroscope bandwidth */ int l3g4200d_set_bandwidth(char bw) { int res = 0; unsigned char data = 0; res = i2c_smbus_read_word_data(gyro->client, CTRL_REG1); if (res >= 0) { data = res & 0x00cf; data = data + bw; res = l3g4200d_i2c_write(CTRL_REG1, &data, 1); } return res; } /* read selected bandwidth from l3g4200d */ int l3g4200d_get_bandwidth(unsigned char *bw) { int res = 1; /* TO DO */ return res; } int l3g4200d_set_mode(char mode) { int res = 0; unsigned char data = 0; res = i2c_smbus_read_word_data(gyro->client, CTRL_REG1); if (res >= 0) { data = res & 0x00f7; data = mode + data; /* printk(KERN_INFO "set mode CTRL_REG1=%x\n",data); */ res = l3g4200d_i2c_write(CTRL_REG1, &data, 1); } return res; } int l3g4200d_set_range(char range) { int res = 0; unsigned char data = 0; res = i2c_smbus_read_word_data(gyro->client, CTRL_REG4); if (res >= 0) { data = res & 0x00cf; data = range + data; res = l3g4200d_i2c_write(CTRL_REG4, &data, 1); } return res; } /* gyroscope data readout */ int l3g4200d_read_gyro_values(struct l3g4200d_t *data) { int res; unsigned char gyro_data[6]; /* x,y,z hardware data */ int hw_d[3] = { 0 }; res = l3g4200d_i2c_read(AXISDATA_REG, &gyro_data[0], 6); hw_d[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]); hw_d[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]); hw_d[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]); /* hw_d[0] >>= gyro->shift_adj; hw_d[1] >>= gyro->shift_adj; hw_d[2] >>= gyro->shift_adj; */ #if 0 data->y = ((gyro->pdata->negate_x) ? (-hw_d[gyro->pdata->axis_map_x]) : (hw_d[gyro->pdata->axis_map_x])); data->p = ((gyro->pdata->negate_y) ? (-hw_d[gyro->pdata->axis_map_y]) : (hw_d[gyro->pdata->axis_map_y])); data->r = ((gyro->pdata->negate_z) ? (-hw_d[gyro->pdata->axis_map_z]) : (hw_d[gyro->pdata->axis_map_z])); #else gyro->pdata->negate_x = 0; gyro->pdata->negate_y = 0; gyro->pdata->negate_z = 0; data->y = ((gyro->pdata->negate_x) ? (-hw_d[2]) : (hw_d[2])); data->p = ((gyro->pdata->negate_y) ? (-hw_d[1]) : (hw_d[1])); data->r = ((gyro->pdata->negate_z) ? (-hw_d[0]) : (hw_d[0])); #endif // printk(KERN_INFO "read x=%d, y=%d, z=%d\n", data->y, data->p, data->r); return res; } /* Device Initialization */ static int device_init(void) { int res; unsigned char buf[5]; buf[0] = 0x6f; buf[1] = 0x00; buf[2] = 0x00; buf[3] = 0x10; buf[4] = 0x00; res = l3g4200d_i2c_write(CTRL_REG1, &buf[0], 5); return res; } /* i2c write routine for l3g4200d digital gyroscope */ static char l3g4200d_i2c_write(unsigned char reg_addr, unsigned char *data, unsigned char len) { int dummy; int i; if (gyro->client == NULL) /* No global client pointer? */ return -1; for (i = 0; i < len; i++) { dummy = i2c_smbus_write_byte_data(gyro->client, reg_addr++, data[i]); if (dummy) { printk(KERN_INFO "i2c write error\n"); return dummy; } } return 0; } /* i2c read routine for l3g4200d digital gyroscope */ static char l3g4200d_i2c_read(unsigned char reg_addr, unsigned char *data, unsigned char len) { int dummy = 0; int i = 0; if (gyro->client == NULL) /* No global client pointer? */ return -1; while (i < len) { dummy = i2c_smbus_read_word_data(gyro->client, reg_addr++); if (dummy >= 0) { data[i] = dummy & 0x00ff; /*printk("data[%d]=%x",i,dummy);*/ i++; } else { printk(KERN_INFO" i2c read error\n "); return dummy; } dummy = len; } return dummy; } /* read command for l3g4200d device file */ static ssize_t l3g4200d_read(struct file *file, char __user *buf, size_t count, loff_t *offset) { #if DEBUG struct l3g4200d_t data; #endif if (gyro->client == NULL) return -1; #if DEBUG l3g4200d_read_gyro_values(&data); printk(KERN_INFO "Yaw axis: %d\n", data.y); printk(KERN_INFO "Pitch axis: %d\n", data.p); printk(KERN_INFO "Roll axis: %d\n", data.r); #endif return 0; } /* write command for l3g4200d device file */ static ssize_t l3g4200d_write(struct file *file, const char __user *buf, size_t count, loff_t *offset) { if (gyro->client == NULL) return -1; #if DEBUG printk(KERN_INFO "l3g4200d should be accessed with ioctl command\n"); #endif return 0; } /*************************************************************************/ /* Start of L3G4200D Sysfs */ /*************************************************************************/ //TEST //#define POWER_ON_TEST static ssize_t l3g4200d_power_on(struct device *dev, struct device_attribute *attr, char *buf) { int count; int res; #ifdef POWER_ON_TEST unsigned char gyro_data[6] = {0,0,0,0,0,0}; unsigned char bZYXDA = 0; unsigned char temp[1] = {0}; short raw[3] = {0,0,0}; #endif res = device_init(); mdelay(300); #ifdef POWER_ON_TEST // For Test // check ZYXDA ready bit l3g4200d_i2c_read(STATUS_REG, &temp[0], 1); bZYXDA = ((unsigned int)temp[0] & 0x08) >> 3; printk("[l3g4200d_power_on] temp = 0x%x, bZYXDA = %d\n", temp[0], bZYXDA); res = l3g4200d_i2c_read(AXISDATA_REG, &gyro_data[0], 6); raw[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]); raw[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]); raw[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]); printk("[l3g4200d_power_on] raw[0] = %d\n", raw[0]); printk("[l3g4200d_power_on] raw[1] = %d\n", raw[1]); printk("[l3g4200d_power_on] raw[2] = %d\n", raw[2]); printk("\n"); #endif printk("[%s] result of device init = %d\n", __func__, res); count = sprintf(buf,"%d\n", (res < 0 ? 0:1)); return count; } static ssize_t l3g4200d_get_temp(struct device *dev, struct device_attribute *attr, char *buf) { int count = 0; char temp = 0; // before starting self-test, backup register l3g4200d_i2c_read(OUT_TEMP, &temp, 1); printk("[%s] read temperature : %d\n", __func__, temp); count = sprintf(buf,"%d\n", temp); return count; } static ssize_t l3g4200d_self_test(struct device *dev, struct device_attribute *attr, char *buf) { int count; int res; unsigned char temp[1] = {0}; unsigned char gyro_data[6] = {0,0,0,0,0,0}; short raw[3] = {0,0,0}; int NOST[3], ST[3]; int differ_x = 0, differ_y = 0, differ_z = 0; unsigned char bak_reg[5] = {0,0,0,0,0}; unsigned char reg[5] = {0,0,0,0,0}; unsigned char bZYXDA = 0; unsigned char pass = 0; int i = 0; int fail_count = 0; memset(NOST, 0, sizeof(int)*3); memset(ST, 0, sizeof(int)*3); // before starting self-test, backup register l3g4200d_i2c_read(CTRL_REG1, &bak_reg[0], 5); for(i = 0; i < 5; i++) printk("[gyro_self_test] backup reg[%d] = %2x\n", i, bak_reg[i]); // Initialize Sensor, turn on sensor, enable P/R/Y // Set BDU=1, Set ODR=200Hz, Cut-Off Frequency=50Hz, FS=2000dps reg[0] = 0x6f; reg[1] = 0x00; reg[2] = 0x00; reg[3] = 0xA0; reg[4] = 0x02; l3g4200d_i2c_write(CTRL_REG1, ®[0], 5); // Power up, wait for 800ms for stable output mdelay(800); // Read 5 samples output before self-test on i = 0; fail_count = 0; while(i < 5) { // check ZYXDA ready bit l3g4200d_i2c_read(STATUS_REG, &temp[0], 1); bZYXDA = ((unsigned int)temp[0] & 0x08) >> 3; if(!bZYXDA) { fail_count++; mdelay(100); if(fail_count < 10) continue; else goto exit; } res = l3g4200d_i2c_read(AXISDATA_REG, &gyro_data[0], 6); raw[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]); raw[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]); raw[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]); NOST[0] += raw[0]; NOST[1] += raw[1]; NOST[2] += raw[2]; printk("[gyro_self_test] raw[0] = %d\n", raw[0]); printk("[gyro_self_test] raw[1] = %d\n", raw[1]); printk("[gyro_self_test] raw[2] = %d\n", raw[2]); printk("\n"); i++; } for(i = 0; i < 3; i++) printk("[gyro_self_test] SUM of NOST[%d] = %d\n", i, NOST[i]); // calculate average of NOST and covert from ADC to DPS for(i = 0; i < 3; i++) { NOST[i] = (NOST[i] / 5) * 70 / 1000; printk("[gyro_self_test] AVG of NOST[%d] = %d\n", i, NOST[i]); } printk("\n"); // Enable Self Test reg[0] = 0xA2; l3g4200d_i2c_write(CTRL_REG4, ®[0], 1); mdelay(100); // Read 5 samples output after self-test on i = 0; fail_count = 0; while(i < 5) { // check ZYXDA ready bit l3g4200d_i2c_read(STATUS_REG, &temp[0], 1); bZYXDA = ((unsigned int)temp[0] & 0x08) >> 3; if(!bZYXDA) { fail_count++; mdelay(100); if(fail_count < 10) continue; else goto exit; } res = l3g4200d_i2c_read(AXISDATA_REG, &gyro_data[0], 6); raw[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]); raw[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]); raw[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]); ST[0] += raw[0]; ST[1] += raw[1]; ST[2] += raw[2]; printk("[gyro_self_test] raw[0] = %d\n", raw[0]); printk("[gyro_self_test] raw[1] = %d\n", raw[1]); printk("[gyro_self_test] raw[2] = %d\n", raw[2]); printk("\n"); i++; } for(i = 0; i < 3; i++) printk("[gyro_self_test] SUM of ST[%d] = %d\n", i, ST[i]); // calculate average of ST and convert from ADC to dps for(i = 0; i < 3; i++) { ST[i] = (ST[i] / 5) * 70 / 1000; // When FS=2000, 70 mdps/digit printk("[gyro_self_test] AVG of ST[%d] = %d\n", i, ST[i]); } // check whether pass or not if( ST[0] >= NOST[0] ) // for x differ_x = ST[0] - NOST[0]; else differ_x = NOST[0] - ST[0]; if( ST[1] >= NOST[1] ) // for y differ_y = ST[1] - NOST[1]; else differ_y = NOST[1] - ST[1]; if( ST[2] >= NOST[2] ) // for z differ_z = ST[2] - NOST[2]; else differ_z = NOST[2] - ST[2]; printk("[gyro_self_test] differ x:%d, y:%d, z:%d\n", differ_x, differ_y, differ_z); if( (MIN_ST <= differ_x && differ_x <= MAX_ST) && (MIN_ST <= differ_y && differ_y <= MAX_ST) && (MIN_ST <= differ_z && differ_z <= MAX_ST) ) pass = 1; exit: // restore backup register l3g4200d_i2c_write(CTRL_REG1, &bak_reg[0], 5); printk("[gyro_self_test] self-test result : %s\n", pass ? "pass" : "fail"); count = sprintf(buf,"%d,%d,%d,%d,%d,%d,%d\n", NOST[0], NOST[1], NOST[2], ST[0], ST[1], ST[2], pass); return count; } static ssize_t l3g4200d_fs_write(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { //buf[size]=0; printk("input data --> %s\n", buf); return size; } static DEVICE_ATTR(gyro_power_on, S_IRUGO | S_IWUSR | S_IWGRP, l3g4200d_power_on, NULL); static DEVICE_ATTR(gyro_get_temp, S_IRUGO | S_IWUSR | S_IWGRP, l3g4200d_get_temp, NULL); static DEVICE_ATTR(gyro_selftest, S_IRUGO | S_IWUSR | S_IWGRP, l3g4200d_self_test, l3g4200d_fs_write); /*************************************************************************/ /* End of L3G4200D Sysfs */ /*************************************************************************/ /* open command for l3g4200d device file */ static int l3g4200d_open(struct inode *inode, struct file *file) { if (gyro->client == NULL) { #if DEBUG printk(KERN_ERR "I2C driver not install\n"); #endif return -1; } device_init(); mdelay(300); fd_cnt++; #if DEBUG printk(KERN_INFO "l3g4200d has been opened\n"); #endif return 0; } /* release command for l3g4200d device file */ static int l3g4200d_close(struct inode *inode, struct file *file) { #if DEBUG printk(KERN_INFO "L3G4200D has been closed\n"); #endif fd_cnt--; #if DEBUG printk(KERN_INFO "[%s] fd count = %d\n", __func__, fd_cnt); #endif if(fd_cnt == 0) { #if DEBUG printk(KERN_INFO "L3G4200D has been powered off.\n"); #endif l3g4200d_set_mode(PM_OFF); } return 0; } /* ioctl command for l3g4200d device file */ static int l3g4200d_ioctl(struct file *file, unsigned int cmd, unsigned long arg) { int err = 0; unsigned char data[6]; mutex_lock(&l3g4200d_mutex); /* check l3g4200d_client */ if (gyro->client == NULL) { #if DEBUG printk(KERN_ERR "I2C driver not install\n"); #endif mutex_unlock(&l3g4200d_mutex); return -EFAULT; } /* cmd mapping */ switch (cmd) { /*case L3G4200D_SELFTEST: //TO DO return err;*/ case L3G4200D_SET_RANGE: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG printk(KERN_ERR "copy_from_user error\n"); #endif } err = l3g4200d_set_range(*data); break; case L3G4200D_SET_MODE: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG printk(KERN_ERR "copy_to_user error\n"); #endif } err = l3g4200d_set_mode(*data); break; case L3G4200D_SET_BANDWIDTH: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG printk(KERN_ERR "copy_from_user error\n"); #endif } err = l3g4200d_set_bandwidth(*data); break; case L3G4200D_READ_GYRO_VALUES: err = l3g4200d_read_gyro_values( (struct l3g4200d_t *)data); if (copy_to_user((struct l3g4200d_t *)arg, (struct l3g4200d_t *)data, 6) != 0) { #if DEBUG printk(KERN_ERR "copy_to error\n"); #endif } break; default: err = 0; } mutex_unlock(&l3g4200d_mutex); return err; } static const struct file_operations l3g4200d_fops = { .owner = THIS_MODULE, .read = l3g4200d_read, .write = l3g4200d_write, .open = l3g4200d_open, .release = l3g4200d_close, .unlocked_ioctl = l3g4200d_ioctl, }; static int l3g4200d_validate_pdata(struct l3g4200d_data *gyro) { if (gyro->pdata->axis_map_x > 2 || gyro->pdata->axis_map_y > 2 || gyro->pdata->axis_map_z > 2) { dev_err(&gyro->client->dev, "invalid axis_map value x:%u y:%u z%u\n", gyro->pdata->axis_map_x, gyro->pdata->axis_map_y, gyro->pdata->axis_map_z); return -EINVAL; } printk(KERN_INFO "%s : check map!!\n", __func__); /* Only allow 0 and 1 for negation boolean flag */ if (gyro->pdata->negate_x > 1 || gyro->pdata->negate_y > 1 || gyro->pdata->negate_z > 1) { dev_err(&gyro->client->dev, "invalid negate value x:%u y:%u z:%u\n", gyro->pdata->negate_x, gyro->pdata->negate_y, gyro->pdata->negate_z); return -EINVAL; } printk(KERN_INFO "%s : check negate!!\n", __func__); return 0; } static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id *devid) { struct l3g4200d_data *data; struct input_dev *input_dev; struct device *dev; int err = 0; int tempvalue; if (client->dev.platform_data == NULL) { dev_err(&client->dev, "platform data is NULL. exiting.\n"); err = -ENODEV; goto exit; } printk(KERN_INFO "%s : platform data ok!!\n", __func__); if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { err = -ENODEV; goto exit; } printk(KERN_INFO "%s : i2c_check_functionality I2C_FUNC_I2C!!\n", __func__); if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) { err = -ENODEV; goto exit; } printk(KERN_INFO "%s : i2c_check_functionality I2C_FUNC_SMBUS_I2C_BLOCK!!\n", __func__); /* * OK. For now, we presume we have a valid client. We now create the * client structure, even though we cannot fill it completely yet. */ data = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL); if (data == NULL) { dev_err(&client->dev, "failed to allocate memory for module data\n"); err = -ENOMEM; goto exit; } printk(KERN_INFO "%s : kzalloc!!\n", __func__); i2c_set_clientdata(client, data); data->client = client; printk(KERN_INFO "%s : i2c_set_clientdata!!\n", __func__); data->pdata = kmalloc(sizeof(*data->pdata), GFP_KERNEL); printk(KERN_INFO "%s : kmalloc!!\n", __func__); if (data->pdata == NULL) { err = -ENOMEM; goto exit_kfree; } memcpy(data->pdata, client->dev.platform_data, sizeof(*data->pdata)); printk(KERN_INFO "%s : memcpy!!\n", __func__); err = l3g4200d_validate_pdata(data); if (err < 0) { dev_err(&client->dev, "failed to validate platform data\n"); goto exit_kfree_pdata; } printk(KERN_INFO "%s : validate_pdata!!\n", __func__); printk(KERN_INFO "%s : slave addr = %x\n", __func__, client->addr); if ( (err = i2c_smbus_read_byte(client)) < 0) { printk(KERN_ERR "%s : i2c_smbus_read_byte error!!. err = %d\n", __func__, err); goto exit_kfree; } else { printk(KERN_INFO "%s : L3G4200D Device detected!\n", __func__); } /* read chip id */ tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I); if ((tempvalue & 0x00FF) == 0x00D3) { printk(KERN_INFO "%s : I2C driver registered!\n", __func__); } else { printk(KERN_ERR "%s : It's not L3G4200D device. tempvalue = %d\n", __func__, tempvalue); err = -ENODEV; data->client = NULL; goto exit_kfree; } mutex_init(&data->lock); /* hrtimer settings. we poll for gyro values using a timer. */ hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); data->polling_delay = ns_to_ktime(200 * NSEC_PER_MSEC); data->time_to_read = 10000000LL; data->timer.function = l3g_timer_func; /* the timer just fires off a work queue request. We need a thread to read i2c (can be slow and blocking). */ data->l3g_wq = create_singlethread_workqueue("l3g_wq"); if (!data->l3g_wq) { err = -ENOMEM; pr_err("%s: could not create workqueue\n", __func__); goto err_create_workqueue; } /* this is the thread function we run on the work queue */ INIT_WORK(&data->work, l3g_work_func); /* allocate gyro input_device */ input_dev = input_allocate_device(); if (!input_dev) { pr_err("%s: could not allocate input device\n", __func__); err = -ENOMEM; goto err_input_allocate_device; } data->input_dev = input_dev; input_set_drvdata(input_dev, data); input_dev->name = "gyro"; /* X */ input_set_capability(input_dev, EV_REL, REL_RX); input_set_abs_params(input_dev, REL_RX, -32768, 32768, 0, 0); /* Y */ input_set_capability(input_dev, EV_REL, REL_RY); input_set_abs_params(input_dev, REL_RY, -32768, 32768, 0, 0); /* Z */ input_set_capability(input_dev, EV_REL, REL_RZ); input_set_abs_params(input_dev, REL_RZ, -32768, 32768, 0, 0); err = input_register_device(input_dev); if (err < 0) { pr_err("%s: could not register input device\n", __func__); input_free_device(data->input_dev); goto err_input_register_device; } if (device_create_file(&input_dev->dev, &dev_attr_enable) < 0) { pr_err("Failed to create device file(%s)!\n", dev_attr_enable.attr.name); goto err_device_create_file; } if (device_create_file(&input_dev->dev, &dev_attr_poll_delay) < 0) { pr_err("Failed to create device file(%s)!\n", dev_attr_poll_delay.attr.name); goto err_device_create_file2; } dev_set_drvdata(&input_dev->dev, data); /* create l3g-dev device class */ l3g_gyro_dev_class = class_create(THIS_MODULE, "L3G_GYRO-dev"); if (IS_ERR(l3g_gyro_dev_class)) { err = PTR_ERR(l3g_gyro_dev_class); goto out_unreg_chrdev; } dev = device_create(l3g_gyro_dev_class, NULL, MKDEV(L3G4200D_MAJOR, 0), NULL, "l3g4200d"); if (IS_ERR(dev)) { err = PTR_ERR(dev); goto error_destroy; } if ( (err = device_create_file(dev, &dev_attr_gyro_power_on)) < 0) { printk("Failed to create device file(%s)!\n", dev_attr_gyro_power_on.attr.name); goto error_destroy; } if ( (err = device_create_file(dev, &dev_attr_gyro_get_temp)) < 0) { printk("Failed to create device file(%s)!\n", dev_attr_gyro_get_temp.attr.name); goto error_destroy; } if ( (err = device_create_file(dev, &dev_attr_gyro_selftest)) < 0) { printk("Failed to create device file(%s)!\n", dev_attr_gyro_selftest.attr.name); goto error_destroy; } printk(KERN_INFO "%s : L3G4200D device created successfully\n", __func__); gyro = data; printk(KERN_INFO "%s : L3G4200D device created successfully\n", __func__); return 0; error_destroy: class_destroy(l3g_gyro_dev_class); out_unreg_chrdev: unregister_chrdev(L3G4200D_MAJOR, "l3g4200d"); err_device_create_file2: device_remove_file(&input_dev->dev, &dev_attr_enable); err_device_create_file: input_unregister_device(data->input_dev); //out_unreg_chrdev: // unregister_chrdev(L3G4200D_MAJOR, "l3g4200d"); err_create_workqueue: err_input_allocate_device: err_input_register_device: mutex_destroy(&data->lock); exit_kfree_pdata: printk(KERN_INFO "%s : Prima di Kfree data->pdata\n", __func__); kfree(data->pdata); exit_kfree: printk(KERN_INFO "%s : Prima di Kfree data\n", __func__); kfree(data); exit: printk(KERN_INFO "%s : Prima di return err=%d\n", __func__, err); return err; } static int l3g4200d_remove(struct i2c_client *client) { struct l3g4200d_data *lis = i2c_get_clientdata(client); #if DEBUG printk(KERN_INFO "L3G4200D driver removing\n"); #endif device_remove_file(&lis->input_dev->dev, &dev_attr_enable); device_remove_file(&lis->input_dev->dev, &dev_attr_poll_delay); printk(KERN_INFO "Sono in out\n"); device_destroy(l3g_gyro_dev_class, MKDEV(L3G4200D_MAJOR, 0)); printk(KERN_INFO "Dopo device_destroy\n"); printk(KERN_INFO "Dopo 12c_del_driver\n"); class_destroy(l3g_gyro_dev_class); printk(KERN_INFO "Dopo class_destroy\n"); unregister_chrdev(L3G4200D_MAJOR, "l3g4200d"); printk(KERN_INFO "Dopo unregister_chrdev\n"); input_unregister_device(lis->input_dev); destroy_workqueue(lis->l3g_wq); mutex_destroy(&lis->lock); kfree(lis); gyro->client = NULL; return 0; } #ifdef CONFIG_PM static int l3g4200d_suspend(struct i2c_client *client, pm_message_t state) { int i; #if DEBUG printk(KERN_INFO "l3g4200d_suspend\n"); #endif /* TO DO */ // before starting self-test, backup register l3g4200d_i2c_read(CTRL_REG1, ®_backup[0], 5); #if DEBUG for(i = 0; i < 5; i++) printk("[l3g4200d_suspend] backup reg[%d] = %2x\n", i, reg_backup[i]); #endif if (gyro->enable) { mutex_lock(&gyro->lock); hrtimer_cancel(&gyro->timer); cancel_work_sync(&gyro->work); mutex_unlock(&gyro->lock); } l3g4200d_set_mode(PM_OFF); return 0; } static int l3g4200d_resume(struct i2c_client *client) { int i; #if DEBUG printk(KERN_INFO "l3g4200d_resume\n"); #endif /* TO DO */ // restore backup register l3g4200d_i2c_write(CTRL_REG1, ®_backup[0], 5); #if DEBUG for(i = 0; i < 5; i++) printk("[l3g4200d_resume] backup reg[%d] = %2x\n", i, reg_backup[i]); #endif if (gyro->enable) { mutex_lock(&gyro->lock); hrtimer_start(&gyro->timer,gyro->polling_delay, HRTIMER_MODE_REL); mutex_unlock(&gyro->lock); } return 0; } #endif static const struct i2c_device_id l3g4200d_id[] = { { "l3g4200d", 0 }, {}, }; MODULE_DEVICE_TABLE(i2c, l3g4200d_id); static struct i2c_driver l3g4200d_driver = { .class = I2C_CLASS_HWMON, .probe = l3g4200d_probe, .remove = __devexit_p(l3g4200d_remove), .id_table = l3g4200d_id, #ifdef CONFIG_PM .suspend = l3g4200d_suspend, .resume = l3g4200d_resume, #endif .driver = { .owner = THIS_MODULE, .name = "l3g4200d", }, /* .detect = l3g4200d_detect, */ }; static int __init l3g4200d_init(void) { printk("%s \n",__func__); printk(KERN_INFO "L3G4200D init driver\n"); return i2c_add_driver(&l3g4200d_driver); } static void __exit l3g4200d_exit(void) { #if DEBUG printk(KERN_INFO "L3G4200D exit\n"); #endif i2c_del_driver(&l3g4200d_driver); return; } module_init(l3g4200d_init); module_exit(l3g4200d_exit); MODULE_DESCRIPTION("l3g4200d digital gyroscope driver"); MODULE_AUTHOR("STMicroelectronics"); MODULE_LICENSE("GPL");