diff options
Diffstat (limited to 'drivers/sensor')
| -rw-r--r-- | drivers/sensor/Kconfig | 54 | ||||
| -rw-r--r-- | drivers/sensor/Makefile | 6 | ||||
| -rw-r--r-- | drivers/sensor/al3201.c | 51 | ||||
| -rw-r--r-- | drivers/sensor/gp2a_proximity.c | 763 | ||||
| -rw-r--r-- | drivers/sensor/k3dh_kona.c | 1080 | ||||
| -rw-r--r-- | drivers/sensor/k3dh_reg.h | 1 | ||||
| -rw-r--r-- | drivers/sensor/yas_mag_driver-yas532.c | 2909 | ||||
| -rw-r--r-- | drivers/sensor/yas_mag_driver.c | 27 | ||||
| -rw-r--r-- | drivers/sensor/yas_mag_kernel_driver.c | 2192 | ||||
| -rw-r--r-- | drivers/sensor/yas_ori_kernel_driver.c | 695 | ||||
| -rw-r--r-- | drivers/sensor/yas_pcb_test.c | 1282 | ||||
| -rw-r--r-- | drivers/sensor/yas_pcb_test.h | 106 | ||||
| -rw-r--r-- | drivers/sensor/yas_types.h | 48 |
13 files changed, 8862 insertions, 352 deletions
diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index f7888b9..36464eb 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -112,6 +112,17 @@ config SENSORS_AL3201 Say N here if you do not use AL3201. +config SENSORS_K2DH + tristate "K2DH acceleration sensor support" + depends on I2C + default n + help + Say Y here if you use K2DH. + This option enables accelerometer sensors using + STM K2DH in K2DH device driver. + + Say N here if you do not use K2DH. + config SENSORS_K3DH tristate "K3DH acceleration sensor support" depends on I2C @@ -119,6 +130,17 @@ config SENSORS_K3DH help Driver for STMicroelectronic K3DH accelerometer. +config SENSOR_K3DH_INPUTDEV + bool "K3DH acceleration sensor input dev support" + depends on SENSORS_K3DH + default n + help + Say Y here if you use K3DH. + This option enables accelerometer sensor using + K3DH device driver. + + Say N here if you do not use K3DH. + config SENSORS_K3G tristate "K3G driver for s5pc210" depends on I2C @@ -139,4 +161,36 @@ config SENSORS_LPS331 depends on I2C help Driver for STMicro LPS331 + +config SENSORS_YAS532 + depends on I2C + tristate "yas532 Sensor Support" + default n + help + Say Y to enable YAS532 Magnetic Sensor support. + This allows control of supported Magnetic Sensor. + +config SENSORS_YAS_ORI + depends on I2C + tristate "yas orientation Sensor Support" + default n + help + Say Y to enable YAS532 Magnetic Sensor support. + This allows control of supported Magnetic Sensor. + +config INPUT_YAS_MAGNETOMETER_POSITION + int "YAS Geomagnetic Sensor Mounting Position on Board" + depends on I2C + default "0" + help + Chip mounting position (pin 1). + 0: top, upper-left + 1: top, upper-right + 2: top, lower-right + 3: top, lower-left + 4: bottom, upper-left + 5: bottom, upper-right + 6: bottom, lower-right + 7: bottom, lower-left + endif diff --git a/drivers/sensor/Makefile b/drivers/sensor/Makefile index 44adf31..195f0f9 100644 --- a/drivers/sensor/Makefile +++ b/drivers/sensor/Makefile @@ -6,7 +6,11 @@ obj-$(CONFIG_SENSORS_CORE) += sensors_core.o # accelerometer_sensor +ifeq ($(CONFIG_MACH_KONA_SENSOR),y) +obj-$(CONFIG_SENSORS_K3DH) += k3dh_kona.o +else obj-$(CONFIG_SENSORS_K3DH) += k3dh.o +endif obj-$(CONFIG_SENSORS_BMA254) += bma254_driver.o # gyro_sensor obj-$(CONFIG_SENSORS_K3G) += k3g.o @@ -15,6 +19,8 @@ obj-$(CONFIG_SENSORS_LSM330DLC) += lsm330dlc_accel.o lsm330dlc_gyro.o # magnetic_sensor obj-$(CONFIG_SENSORS_AK8975C) += ak8975.o obj-$(CONFIG_SENSORS_AK8963C) += ak8963.o +obj-$(CONFIG_SENSORS_YAS532) += yas_mag_kernel_driver.o +obj-$(CONFIG_SENSORS_YAS_ORI) += yas_ori_kernel_driver.o # optical_sensor obj-$(CONFIG_SENSORS_CM3663) += cm3663.o obj-$(CONFIG_SENSORS_TAOS) += taos.o diff --git a/drivers/sensor/al3201.c b/drivers/sensor/al3201.c index a225b1a..df21515 100644 --- a/drivers/sensor/al3201.c +++ b/drivers/sensor/al3201.c @@ -304,10 +304,35 @@ static ssize_t get_chip_name(struct device *dev, { return sprintf(buf, "%s\n", CHIP_NAME); } +#if defined(CONFIG_MACH_KONA) +static ssize_t al3201_lux_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int result; + struct input_dev *input = to_input_dev(dev); + struct al3201_data *data = input_get_drvdata(input); + + /* No LUX data if not operational */ + if (data->state == OFF) { + al3201_set_power_state(data->client, ON); + msleep(180); + } + + result = al3201_get_adc_value(data->client); + + if (data->state == OFF) + al3201_set_power_state(data->client, OFF); + + return sprintf(buf, "%d\n", result); +} +#endif static DEVICE_ATTR(raw_data, 0644, al3201_raw_data_show, NULL); static DEVICE_ATTR(vendor, 0644, get_vendor_name, NULL); static DEVICE_ATTR(name, 0644, get_chip_name, NULL); +#if defined(CONFIG_MACH_KONA) +static DEVICE_ATTR(lux, 0644, al3201_lux_show, NULL); +#endif /* factory test*/ #ifdef LSC_DBG @@ -582,26 +607,38 @@ static int __devinit al3201_probe(struct i2c_client *client, dev_attr_name.attr.name); goto err_light_device_create_file3; } + +#if defined(CONFIG_MACH_KONA) + if (device_create_file(data->light_dev, &dev_attr_lux) < 0) { + pr_err("%s: could not create device file(%s)!\n", __func__, + dev_attr_lux.attr.name); + goto err_light_device_create_file4; + } +#endif dev_set_drvdata(data->light_dev, data); pr_info("%s: success!\n", __func__); goto done; - err_light_device_create_file3: +#if defined(CONFIG_MACH_KONA) +err_light_device_create_file4: + device_remove_file(data->light_dev, &dev_attr_lux); +#endif +err_light_device_create_file3: device_remove_file(data->light_dev, &dev_attr_vendor); err_light_device_create_file2: device_remove_file(data->light_dev, &dev_attr_raw_data); err_light_device_create_file1: sensors_classdev_unregister(data->light_dev); - err_light_device_create: +err_light_device_create: sysfs_remove_group(&data->input->dev.kobj, &al3201_attribute_group); - err_sysfs_create_group_light: +err_sysfs_create_group_light: input_unregister_device(data->input); - err_input_register_device_light: - err_input_allocate_device_light: +err_input_register_device_light: +err_input_allocate_device_light: destroy_workqueue(data->wq); - err_create_workqueue: - err_initializ_chip: +err_create_workqueue: +err_initializ_chip: mutex_destroy(&data->lock); kfree(data); done: diff --git a/drivers/sensor/gp2a_proximity.c b/drivers/sensor/gp2a_proximity.c index 7efbf3e..c1624e6 100644 --- a/drivers/sensor/gp2a_proximity.c +++ b/drivers/sensor/gp2a_proximity.c @@ -47,8 +47,6 @@ #define VENDOR "SHARP" #define CHIP_ID "GP2AP" -#define OFFSET_FILE_PATH "/efs/prox_cal" - #if 1 #define gprintk(fmt, x...) printk(KERN_INFO "%s(%d): "\ fmt, __func__ , __LINE__, ## x) @@ -56,17 +54,31 @@ fmt, __func__ , __LINE__, ## x) #define gprintk(x...) do { } while (0) #endif /**************************************************/ -#if defined(CONFIG_MACH_BAFFIN) -#define PROX_READ_NUM 20 +/* Calibration*/ +#define GP2A_CALIBRATION +#ifdef GP2A_CALIBRATION +#ifdef CONFIG_SLP +#define CALIBRATION_FILE_PATH "/csa/sensor/prox_cal_data" #else -#define PROX_READ_NUM 40 +#define CALIBRATION_FILE_PATH "/efs/prox_cal" +#endif +#endif +#if defined(CONFIG_MACH_KONA_SENSOR) +#define DEFAULT_THRESHOLD_DIFF 2 +#else +#define DEFAULT_THRESHOLD_DIFF 1 #endif +#define PROX_READ_NUM 40 #define PS_LOW_THD_L 0x08 #define PS_LOW_THD_H 0x09 #define PS_HIGH_THD_L 0x0A #define PS_HIGH_THD_H 0x0B +#if defined(CONFIG_MACH_KONA_SENSOR) +#define XTALK 8 +#define THDL 10 +#endif /* global var */ static struct i2c_driver opt_i2c_driver; static struct i2c_client *opt_i2c_client; @@ -85,17 +97,17 @@ struct gp2a_data { struct hrtimer prox_timer; struct workqueue_struct *prox_wq; struct work_struct work_prox; - int enabled; int proximity_data; int irq; int average[3]; /*for proximity adc average */ ktime_t prox_poll_delay; - - /* Auto Calibration */ - int offset_value; - int cal_result; - uint16_t threshold_high; - bool offset_cal_high; + u8 enabled; + u8 thresh_diff; + u8 power_state; +#ifdef GP2A_CALIBRATION + u8 default_threshold; + u8 cal_data; +#endif }; /* initial value for sensor register */ @@ -113,13 +125,17 @@ static u8 gp2a_original_image_030a[COL][2] = { /* {0x05 , 0x00}, */ /* {0x06 , 0xFF}, */ /* {0x07 , 0xFF}, */ -#if defined(CONFIG_MACH_BAFFIN) - {0x08, 0x08}, +#if defined(CONFIG_MACH_KONA_SENSOR) + {0x08, 0x08}, /*PS mode LTH(Loff): (??mm) */ #else - {0x08, 0x09}, /*PS mode LTH(Loff): (??mm) */ + {0x08, 0x09}, /*PS mode LTH(Loff): (??mm) */ #endif {0x09, 0x00}, /*PS mode LTH(Loff) : */ - {0x0A, 0x0A}, /*PS mode HTH(Lon) : (??mm) */ +#if defined(CONFIG_MACH_KONA_SENSOR) + {0x0A, 0x0A}, /*PS mode HTH(Lon) : (??mm) */ +#else + {0x0A, 0x0A}, /*PS mode HTH(Lon) : (??mm) */ +#endif {0x0B, 0x00}, /* PS mode HTH(Lon) : */ /* {0x13 , 0x08}, by sharp for internal calculation (type:0) */ /*alternating mode (PS+ALS), TYPE=1 @@ -150,27 +166,22 @@ static u8 gp2a_original_image[COL][2] = { {0x00, 0xC0} }; -#define THR_REG_LSB(data, reg) \ - { \ - reg = (u8)data & 0xff; \ - } -#define THR_REG_MSB(data, reg) \ - { \ - reg = (u8)data >> 8; \ - } - static int proximity_onoff(u8 onoff); +#ifdef GP2A_CALIBRATION +static int proximity_open_calibration(struct gp2a_data *data); +#endif int is_gp2a030a(void) { -#if defined(CONFIG_MACH_C1) || \ - defined(CONFIG_MACH_M0) || \ - defined(CONFIG_MACH_GRANDE) || \ +#if defined(CONFIG_MACH_GRANDE) || \ defined(CONFIG_MACH_IRON) return (system_rev != 0 && system_rev != 3); #endif -#if defined(CONFIG_MACH_M3) || \ - defined(CONFIG_MACH_BAFFIN) +#if defined(CONFIG_MACH_M3_USA_TMO) || \ + defined(CONFIG_MACH_BAFFIN) || \ + defined(CONFIG_MACH_KONA_SENSOR) ||\ + defined(CONFIG_MACH_TAB3) ||\ + defined(CONFIG_MACH_GC2PD) return 1; #endif #if defined(CONFIG_MACH_REDWOOD) @@ -180,17 +191,23 @@ int is_gp2a030a(void) return 0; } -static int gp2a_update_threshold(u8 (*selected_image)[2], - unsigned long new_threshold, bool update_reg) +static int gp2a_update_threshold(struct gp2a_data *data, + u8 (*selected_image)[2], u8 new_threshold, bool update_reg) { int i, err = 0; u8 set_value; + pr_info("%s, new = 0x%x, thresh_diff = %d\n", __func__, + new_threshold, data->thresh_diff); for (i = 0; i < COL; i++) { switch (selected_image[i][0]) { case PS_LOW_THD_L: /*PS mode LTH(Loff) for low 8bit*/ +#if defined(CONFIG_MACH_KONA_SENSOR) + set_value = (new_threshold-data->thresh_diff) & 0x00FF; +#else set_value = new_threshold & 0x00FF; +#endif break; case PS_LOW_THD_H: @@ -200,12 +217,17 @@ static int gp2a_update_threshold(u8 (*selected_image)[2], case PS_HIGH_THD_L: /*PS mode HTH(Lon) for low 8bit*/ - set_value = (new_threshold+1) & 0x00FF; +#if defined(CONFIG_MACH_KONA_SENSOR) + set_value = (new_threshold) & 0x00FF; +#else + set_value = (new_threshold+data->thresh_diff) & 0x00FF; +#endif break; case PS_HIGH_THD_H: /* PS mode HTH(Lon) for high 8bit*/ - set_value = ((new_threshold+1) & 0xFF00) >> 8; + set_value = ((new_threshold + +data->thresh_diff) & 0xFF00) >> 8; break; default: @@ -219,238 +241,14 @@ static int gp2a_update_threshold(u8 (*selected_image)[2], pr_err("%s : setting error i = %d, err=%d\n", __func__, i, err); return err; - } else - selected_image[i][1] = set_value; - } - - return err; -} - -static int proximity_adc_read(struct gp2a_data *data) -{ - int sum[OFFSET_ARRAY_LENGTH]; - int i = OFFSET_ARRAY_LENGTH-1; - int avg; - int min; - int max; - int total; - int D2_data; - unsigned char get_D2_data[2]; - - mutex_lock(&data->data_mutex); - do { - msleep(50); - opt_i2c_read(DATA2_LSB, get_D2_data, sizeof(get_D2_data)); - D2_data = (get_D2_data[1] << 8) | get_D2_data[0]; - sum[i] = D2_data; - if (i == 0) { - min = sum[i]; - max = sum[i]; } else { - if (sum[i] < min) - min = sum[i]; - else if (sum[i] > max) - max = sum[i]; + selected_image[i][1] = set_value; } - total += sum[i]; - } while (i--); - mutex_unlock(&data->data_mutex); - - total -= (min + max); - avg = (int)(total / (OFFSET_ARRAY_LENGTH - 2)); - pr_info("%s: offset = %d\n", __func__, avg); - - return avg; -} - - -static int proximity_open_calibration(struct gp2a_data *data) -{ - struct file *cal_filp = NULL; - int err = 0; - mm_segment_t old_fs; - - old_fs = get_fs(); - set_fs(KERNEL_DS); - - cal_filp = filp_open(OFFSET_FILE_PATH, - O_RDONLY, S_IRUGO | S_IWUSR | S_IWGRP); - - if (IS_ERR(cal_filp)) { - pr_err("%s: Can't open calibration file\n", __func__); - set_fs(old_fs); - err = PTR_ERR(cal_filp); - goto done; } - err = cal_filp->f_op->read(cal_filp, - (char *)&data->offset_value, - sizeof(int), &cal_filp->f_pos); - if (err != sizeof(int)) { - pr_err("%s: Can't read the cal data from file\n", __func__); - err = -EIO; - } - - pr_info("%s: (%d)\n", __func__, - data->offset_value); - - filp_close(cal_filp, current->files); -done: - set_fs(old_fs); return err; } -static int proximity_do_calibrate(struct gp2a_data *data, - bool do_calib, bool thresh_set) -{ - struct file *cal_filp; - int err; - int xtalk_avg = 0; - int offset_change = 0; - uint16_t thrd = 0; - u8 reg; - mm_segment_t old_fs; - - u8 (*cal_image)[2] = (is_gp2a030a() ? - gp2a_original_image_030a : gp2a_original_image); - - if (do_calib) { - if (thresh_set) { - /* for proximity_thresh_store */ - data->offset_value = - data->threshold_high - - (cal_image[6][1] << 8 | cal_image[5][1]); - } else { - /* tap offset button */ - /* get offset value */ - xtalk_avg = proximity_adc_read(data); - offset_change = - (cal_image[6][1] << 8 | cal_image[5][1]) - - DEFAULT_HI_THR; - if (xtalk_avg < offset_change) { - /* do not need calibration */ - data->cal_result = 0; - err = 0; - goto no_cal; - } - data->offset_value = xtalk_avg - offset_change; - } - /* update threshold */ - thrd = (cal_image[4][1] << 8 | cal_image[3][1]) - + (data->offset_value); - THR_REG_LSB(thrd, reg); - opt_i2c_write(cal_image[3][0], ®); - THR_REG_MSB(thrd, reg); - opt_i2c_write(cal_image[4][0], ®); - - thrd = (cal_image[4][1] << 8 | cal_image[5][1]) - +(data->offset_value); - THR_REG_LSB(thrd, reg); - opt_i2c_write(cal_image[5][0], ®); - THR_REG_MSB(thrd, reg); - opt_i2c_write(cal_image[6][0], ®); - - /* calibration result */ - if (!thresh_set) - data->cal_result = 1; - } else { - /* tap reset button */ - data->offset_value = 0; - /* update threshold */ - opt_i2c_write(cal_image[3][0], &cal_image[3][1]); - opt_i2c_write(cal_image[4][0], &cal_image[4][1]); - opt_i2c_write(cal_image[5][0], &cal_image[5][1]); - opt_i2c_write(cal_image[6][0], &cal_image[6][1]); - /* calibration result */ - data->cal_result = 2; - } - - old_fs = get_fs(); - set_fs(KERNEL_DS); - - cal_filp = filp_open(OFFSET_FILE_PATH, - O_CREAT | O_TRUNC | O_WRONLY, - S_IRUGO | S_IWUSR | S_IWGRP); - - if (IS_ERR(cal_filp)) { - pr_err("%s: Can't open calibration file\n", __func__); - set_fs(old_fs); - err = PTR_ERR(cal_filp); - goto done; - } - - err = cal_filp->f_op->write(cal_filp, - (char *)&data->offset_value, sizeof(int), - &cal_filp->f_pos); - if (err != sizeof(int)) { - pr_err("%s: Can't write the cal data to file\n", __func__); - err = -EIO; - } - - filp_close(cal_filp, current->files); -done: - set_fs(old_fs); -no_cal: - return err; -} - -static int proximity_manual_offset(struct gp2a_data *data, u8 change_on) -{ - struct file *cal_filp; - int err; - int16_t thrd; - u8 reg; - mm_segment_t old_fs; - - u8 (*manual_image)[2] = (is_gp2a030a() ? - gp2a_original_image_030a : gp2a_original_image); - - data->offset_value = change_on; - /* update threshold */ - thrd = manual_image[3][1]+(data->offset_value); - THR_REG_LSB(thrd, reg); - opt_i2c_write(manual_image[3][0], ®); - THR_REG_MSB(thrd, reg); - opt_i2c_write(manual_image[4][0], ®); - - thrd = manual_image[5][1]+(data->offset_value); - THR_REG_LSB(thrd, reg); - opt_i2c_write(manual_image[5][0], ®); - THR_REG_MSB(thrd, reg); - opt_i2c_write(manual_image[6][0], ®); - - /* calibration result */ - data->cal_result = 1; - - old_fs = get_fs(); - set_fs(KERNEL_DS); - - cal_filp = filp_open(OFFSET_FILE_PATH, - O_CREAT | O_TRUNC | O_WRONLY, - S_IRUGO | S_IWUSR | S_IWGRP); - - if (IS_ERR(cal_filp)) { - pr_err("%s: Can't open calibration file\n", __func__); - set_fs(old_fs); - err = PTR_ERR(cal_filp); - goto done; - } - - err = cal_filp->f_op->write(cal_filp, - (char *)&data->offset_value, sizeof(int), - &cal_filp->f_pos); - if (err != sizeof(int)) { - pr_err("%s: Can't write the cal data to file\n", __func__); - err = -EIO; - } - - filp_close(cal_filp, current->files); -done: - set_fs(old_fs); - return err; -} - - /* Proximity Sysfs interface */ static ssize_t proximity_enable_show(struct device *dev, @@ -477,28 +275,63 @@ proximity_enable_store(struct device *dev, err = kstrtoint(buf, 10, &value); if (err) - printk(KERN_ERR "%s, kstrtoint failed.", __func__); + pr_err("%s, kstrtoint failed.", __func__); if (value != 0 && value != 1) return count; gprintk("value = %d\n", value); - if (data->enabled && !value) { /* Proximity power off */ + if (data->enabled && !value) { /* proximity disable */ disable_irq(data->irq); - proximity_enable = value; + data->enabled = 0; proximity_onoff(0); disable_irq_wake(data->irq); - data->pdata->gp2a_led_on(false); - } else if (!data->enabled && value) { /* proximity power on */ - data->pdata->gp2a_led_on(true); - /*msleep(1); */ - proximity_enable = value; + /* proximity power off */ + if (data->pdata->gp2a_led_on + && data->power_state) { + data->pdata->gp2a_led_on(false); + data->power_state = 0; + } + +#ifdef CONFIG_SENSORS_GP2A_VDD_CONTROL + /* gp2a vdd power off */ + if (data->pdata->gp2a_vdd_on + && !(data->enabled & LIGHT_ENABLED)) { + data->pdata->gp2a_vdd_on(false); + data->power_state &= ~LIGHT_ENABLED; + } +#endif + } else if (!(data->enabled) && value) { /* proximity enable */ + /* proximity power on */ + if (data->pdata->gp2a_led_on + && !(data->power_state)) { + data->pdata->gp2a_led_on(true); + data->power_state = 1; + } + +#ifdef CONFIG_SENSORS_GP2A_VDD_CONTROL + /* gp2a vdd power on */ + if (data->pdata->gp2a_vdd_on + && !(data->power_state & LIGHT_ENABLED)) { + data->pdata->gp2a_vdd_on(true); + data->power_state |= LIGHT_ENABLED; + } +#endif + + /*msleep(1); */ +#ifdef GP2A_CALIBRATION + /* open cancelation data */ + err = proximity_open_calibration(data); + if (err < 0 && err != -ENOENT) + pr_err("%s: proximity_open_calibration() failed\n", + __func__); +#endif + data->enabled = 1; proximity_onoff(1); enable_irq_wake(data->irq); - msleep(160); input = gpio_get_value(data->pdata->p_out); input_report_abs(data->input_dev, ABS_DISTANCE, input); @@ -506,7 +339,6 @@ proximity_enable_store(struct device *dev, enable_irq(data->irq); } - data->enabled = value; return count; } @@ -515,8 +347,6 @@ static ssize_t proximity_state_show(struct device *dev, struct device_attribute *attr, char *buf) { struct gp2a_data *data = dev_get_drvdata(dev); - static int count; /*count for proximity average */ - int D2_data = 0; unsigned char get_D2_data[2] = { 0, }; @@ -525,12 +355,7 @@ static ssize_t proximity_state_show(struct device *dev, mutex_unlock(&data->data_mutex); D2_data = (get_D2_data[1] << 8) | get_D2_data[0]; - data->average[count] = D2_data; - count++; - if (count == PROX_READ_NUM) - count = 0; - pr_debug("%s: D2_data = %d\n", __func__, D2_data); - return snprintf(buf, PAGE_SIZE, "%d\n", D2_data); + return sprintf(buf, "%d\n", D2_data); } static ssize_t proximity_avg_show(struct device *dev, @@ -559,7 +384,7 @@ static ssize_t proximity_avg_store(struct device *dev, } if (new_value && !proximity_avg_on) { - if (!(proximity_enable)) { + if (!(data->enabled)) { /*data->pdata->gp2a_led_on(true);*/ proximity_onoff(1); } @@ -573,7 +398,7 @@ static ssize_t proximity_avg_store(struct device *dev, hrtimer_cancel(&data->prox_timer); proximity_avg_on = 0; - if (!(proximity_enable)) { + if (!(data->enabled)) { proximity_onoff(0); /*data->pdata->gp2a_led_on(false);*/ } @@ -589,7 +414,7 @@ static ssize_t proximity_thresh_show(struct device *dev, struct device_attribute *attr, char *buf) { int i; - int threshold = 0; + u8 threshold = 0; u8 (*selected_image)[2] = (is_gp2a030a() ? gp2a_original_image_030a : gp2a_original_image); @@ -609,10 +434,11 @@ static ssize_t proximity_thresh_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { - unsigned long threshold; + struct gp2a_data *data = dev_get_drvdata(dev); + u8 threshold = 0; int err = 0; - err = strict_strtoul(buf, 10, &threshold); + err = kstrtou8(buf, 10, &threshold); if (err) { pr_err("%s, conversion %s to number.\n", @@ -620,7 +446,7 @@ static ssize_t proximity_thresh_store(struct device *dev, return err; } - err = gp2a_update_threshold(is_gp2a030a() ? + err = gp2a_update_threshold(data, is_gp2a030a() ? gp2a_original_image_030a : gp2a_original_image, threshold, true); @@ -632,53 +458,262 @@ static ssize_t proximity_thresh_store(struct device *dev, return size; } +#ifdef GP2A_CALIBRATION +static u8 proximity_adc_read(struct gp2a_data *gp2a) +{ + int adc_arr[OFFSET_ARRAY_LENGTH]; + int total = 0, min = 0, max = 0; + u8 avg = 0; + int D2_data = 0; + unsigned char get_D2_data[2] = {0,}; + int i; + + for (i = 0; i < OFFSET_ARRAY_LENGTH; i++) { + mdelay(50); + mutex_lock(&gp2a->data_mutex); + opt_i2c_read(0x10, get_D2_data, sizeof(get_D2_data)); + mutex_unlock(&gp2a->data_mutex); + D2_data = (get_D2_data[1] << 8) | get_D2_data[0]; + adc_arr[i] = D2_data; + if (i == 0) { + min = adc_arr[i]; + max = adc_arr[i]; + } else { + if (adc_arr[i] < min) + min = adc_arr[i]; + else if (adc_arr[i] > max) + max = adc_arr[i]; + } + total += adc_arr[i]; + } + + total -= (min + max); + avg = (u8)(total / (OFFSET_ARRAY_LENGTH - 2)); + pr_info("%s: offset = %d\n", __func__, avg); + + return avg; +} + +static int proximity_open_calibration(struct gp2a_data *data) +{ + struct file *cancel_filp = NULL; + int err = 0; + mm_segment_t old_fs; + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cancel_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666); + if (IS_ERR(cancel_filp)) { + err = PTR_ERR(cancel_filp); + if (err != -ENOENT) + pr_err("%s: Can't open cancelation file\n", __func__); + set_fs(old_fs); + return err; + } + + err = cancel_filp->f_op->read(cancel_filp, + (char *)&data->cal_data, sizeof(u8), &cancel_filp->f_pos); + if (err != sizeof(u8)) { + pr_err("%s: Can't read the cancel data from file\n", __func__); + err = -EIO; + } + + if (data->cal_data != 0) {/*If there is an offset cal data. */ +#if defined(CONFIG_MACH_KONA_SENSOR) + if (is_gp2a030a()) { + gp2a_original_image_030a[5][1] = + data->default_threshold + data->cal_data; + gp2a_original_image_030a[3][1] = + data->default_threshold + + data->cal_data - DEFAULT_THRESHOLD_DIFF; + } +#else + if (is_gp2a030a()) + gp2a_original_image_030a[3][1] = + data->default_threshold + data->cal_data; + +#endif + else + gp2a_original_image[3][1] = + data->default_threshold + data->cal_data; + + pr_info("%s: prox_cal = %d, prox_thresh = 0x%x\n", + __func__, data->cal_data, (is_gp2a030a() ? + gp2a_original_image_030a[3][1] : + gp2a_original_image[3][1])); + } + + filp_close(cancel_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int proximity_store_calibration(struct device *dev, bool do_calib) +{ + struct gp2a_data *gp2a = dev_get_drvdata(dev); + struct file *cancel_filp = NULL; + mm_segment_t old_fs; + int err = 0; + u8 thresh_x; + + if (do_calib) { + gp2a->cal_data = proximity_adc_read(gp2a); + if (is_gp2a030a()) { +#if defined(CONFIG_MACH_KONA_SENSOR) + gp2a_original_image_030a[3][1] = gp2a->cal_data+XTALK; + gp2a->cal_data = gp2a_original_image_030a[3][1]-THDL; +#else + /* if x-talk + 8 > threshold, threhold = x-talk + 8 */ + /* LTH = threshold - thresh_diff */ + thresh_x = gp2a->cal_data + 8 - gp2a->thresh_diff; + + if (thresh_x > gp2a_original_image_030a[3][1]) + gp2a_original_image_030a[3][1]= thresh_x; +#endif + } + else + gp2a_original_image[3][1] += gp2a->cal_data; + } else { /* reset */ + gp2a->cal_data = 0; + if (is_gp2a030a()) + gp2a_original_image_030a[3][1] = + gp2a->default_threshold; + else + gp2a_original_image[3][1] = gp2a->default_threshold; + } + + /* Update cal data. */ + gp2a_update_threshold(gp2a, is_gp2a030a() ? + gp2a_original_image_030a : gp2a_original_image, + (is_gp2a030a() ? + gp2a_original_image_030a[3][1] : + gp2a_original_image[3][1]), true); + + pr_info("%s: prox_cal = %d, prox_thresh = 0x%x\n", + __func__, gp2a->cal_data, (is_gp2a030a() ? + gp2a_original_image_030a[3][1] : + gp2a_original_image[3][1])); + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cancel_filp = filp_open(CALIBRATION_FILE_PATH, + O_CREAT | O_TRUNC | O_WRONLY | O_SYNC, 0666); + if (IS_ERR(cancel_filp)) { + pr_err("%s: Can't open cancelation file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cancel_filp); + return err; + } + + err = cancel_filp->f_op->write(cancel_filp, + (char *)&gp2a->cal_data, sizeof(u8), &cancel_filp->f_pos); + if (err != sizeof(u8)) { + pr_err("%s: Can't write the cancel data to file\n", __func__); + err = -EIO; + } + + filp_close(cancel_filp, current->files); + set_fs(old_fs); + + return err; +} + static ssize_t proximity_cal_show(struct device *dev, - struct device_attribute *attr, char *buf) + struct device_attribute *attr, char *buf) { struct gp2a_data *data = dev_get_drvdata(dev); - int thresh_hi; - unsigned char get_D2_data[2]; - - msleep(20); - opt_i2c_read(PS_HT_LSB, get_D2_data, sizeof(get_D2_data)); - thresh_hi = (get_D2_data[1] << 8) | get_D2_data[0]; - data->threshold_high = thresh_hi; - return sprintf(buf, "%d,%d\n", - data->offset_value, data->threshold_high); +#if defined(CONFIG_MACH_KONA_SENSOR) + return sprintf(buf, "%d,%d\n", data->cal_data, + gp2a_original_image_030a[5][1]); +#else + return sprintf(buf, "%d,%d\n", data->cal_data, (is_gp2a030a() ? + gp2a_original_image_030a[5][1] : + gp2a_original_image[5][1])); +#endif } static ssize_t proximity_cal_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t size) + struct device_attribute *attr, + const char *buf, size_t size) { - struct gp2a_data *data = dev_get_drvdata(dev); bool do_calib; int err; - if (sysfs_streq(buf, "1")) { /* calibrate cancelation value */ + if (sysfs_streq(buf, "1")) /* calibrate cancelation value */ do_calib = true; - } else if (sysfs_streq(buf, "0")) { /* reset cancelation value */ + else if (sysfs_streq(buf, "0")) /* reset cancelation value */ do_calib = false; - } else { + else { pr_err("%s: invalid value %d\n", __func__, *buf); - err = -EINVAL; - goto done; + return -EINVAL; } - err = proximity_do_calibrate(data, do_calib, false); + + err = proximity_store_calibration(dev, do_calib); if (err < 0) { - pr_err("%s: proximity_store_offset() failed\n", __func__); - goto done; + pr_err("%s: proximity_store_calibration() failed\n", __func__); + return err; } -done: - return err; + + return size; } +static ssize_t proximity_thresh_diff_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct gp2a_data *gp2a = dev_get_drvdata(dev); + + return sprintf(buf, "%d\n", gp2a->thresh_diff); +} + +static ssize_t proximity_thresh_diff_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct gp2a_data *gp2a = dev_get_drvdata(dev); + struct gp2a_platform_data *pdata = gp2a->pdata; + u8 threshold_diff = 0; + int err; + + err = kstrtou8(buf, 10, &threshold_diff); + if (err) { + pr_err("%s, conversion %s to number.\n", + __func__, buf); + return err; + } + + if ((threshold_diff > 0) && (threshold_diff < 5)) { /* update diff */ + gp2a->thresh_diff = threshold_diff; + } else if (threshold_diff == 0) { /* reset to default */ + pdata->gp2a_get_threshold(&gp2a->thresh_diff); + } else { + pr_err("%s: invalid value %d\n", __func__, *buf); + return -EINVAL; + } + + gp2a_update_threshold(gp2a, is_gp2a030a() ? + gp2a_original_image_030a : gp2a_original_image, + (is_gp2a030a() ? + gp2a_original_image_030a[3][1] : + gp2a_original_image[3][1]), true); + + return size; +} +#endif + static DEVICE_ATTR(enable, 0664, proximity_enable_show, proximity_enable_store); static DEVICE_ATTR(prox_avg, 0664, proximity_avg_show, proximity_avg_store); static DEVICE_ATTR(state, 0664, proximity_state_show, NULL); -static DEVICE_ATTR(prox_thresh, S_IRUGO | S_IWUSR, +static DEVICE_ATTR(prox_thresh, 0664, proximity_thresh_show, proximity_thresh_store); -static DEVICE_ATTR(prox_cal, 0664, proximity_cal_show, proximity_cal_store); +#ifdef GP2A_CALIBRATION +static DEVICE_ATTR(prox_cal, 0664, + proximity_cal_show, proximity_cal_store); +static DEVICE_ATTR(prox_diff, 0664, + proximity_thresh_diff_show, proximity_thresh_diff_store); +#endif static struct attribute *proximity_attributes[] = { &dev_attr_enable.attr, @@ -715,7 +750,6 @@ static ssize_t proximity_raw_data_show(struct device *dev, unsigned char get_D2_data[2] = { 0, }; struct gp2a_data *data = dev_get_drvdata(dev); - msleep(20); mutex_lock(&data->data_mutex); opt_i2c_read(0x10, get_D2_data, sizeof(get_D2_data)); mutex_unlock(&data->data_mutex); @@ -735,11 +769,10 @@ static void proxsensor_get_avgvalue(struct gp2a_data *data) u8 proximity_value = 0; unsigned char get_D2_data[2] = { 0, }; - mutex_lock(&data->data_mutex); - for (i = 0; i < PROX_READ_NUM; i++) { - msleep(20); + mutex_lock(&data->data_mutex); opt_i2c_read(0x10, get_D2_data, sizeof(get_D2_data)); + mutex_unlock(&data->data_mutex); proximity_value = (get_D2_data[1] << 8) | get_D2_data[0]; avg += proximity_value; @@ -751,7 +784,6 @@ static void proxsensor_get_avgvalue(struct gp2a_data *data) if (proximity_value > max) max = proximity_value; } - mutex_unlock(&data->data_mutex); avg /= PROX_READ_NUM; data->average[0] = min; @@ -901,7 +933,7 @@ static void gp2a_work_func_prox(struct work_struct *work) static int opt_i2c_init(void) { if (i2c_add_driver(&opt_i2c_driver)) { - printk(KERN_ERR "i2c_add_driver failed\n"); + pr_info("i2c_add_driver failed\n"); return -ENODEV; } return 0; @@ -911,16 +943,13 @@ int opt_i2c_read(u8 reg, unsigned char *rbuf, int len) { int ret = -1; struct i2c_msg msg; - /*int i;*/ if ((opt_i2c_client == NULL) || (!opt_i2c_client->adapter)) { - printk(KERN_ERR "%s %d (opt_i2c_client == NULL)\n", + pr_err("%s %d (opt_i2c_client == NULL)\n", __func__, __LINE__); return -ENODEV; } - /*gprintk("register num : 0x%x\n", reg); */ - msg.addr = opt_i2c_client->addr; msg.flags = I2C_M_WR; msg.len = 1; @@ -936,12 +965,9 @@ int opt_i2c_read(u8 reg, unsigned char *rbuf, int len) } if (ret < 0) - printk(KERN_ERR "%s, i2c transfer error ret=%d\n"\ + pr_err("%s, i2c transfer error ret=%d\n"\ , __func__, ret); - /* for (i=0;i<len;i++) - gprintk("0x%x, 0x%x\n", reg++,rbuf[i]); */ - return ret; } @@ -970,7 +996,7 @@ int opt_i2c_write(u8 reg, u8 *val) if (err >= 0) return 0; } - printk(KERN_ERR "%s, i2c transfer error(%d)\n", __func__, err); + pr_err("%s, i2c transfer error(%d)\n", __func__, err); return err; } @@ -982,7 +1008,7 @@ static int proximity_input_init(struct gp2a_data *data) data->input_dev = input_allocate_device(); if (!data->input_dev) { - printk(KERN_ERR "%s, error\n", __func__); + pr_err("%s, error\n", __func__); return -ENOMEM; } @@ -1021,14 +1047,9 @@ static int gp2a_opt_probe(struct platform_device *pdev) return err; } /* gp2a power on */ +#if !defined(CONFIG_MACH_TAB3) || !defined(CONFIG_MACH_GC2PD) pdata->gp2a_led_on(true); - - if (pdata->gp2a_get_threshold) { - gp2a_update_threshold(is_gp2a030a() ? - gp2a_original_image_030a : gp2a_original_image, - pdata->gp2a_get_threshold(), false); - } - +#endif /* allocate driver_data */ gp2a = kzalloc(sizeof(struct gp2a_data), GFP_KERNEL); if (!gp2a) { @@ -1036,10 +1057,35 @@ static int gp2a_opt_probe(struct platform_device *pdev) return -ENOMEM; } - proximity_enable = 0; +#ifdef CONFIG_SLP + gp2a->thresh_diff = DEFAULT_THRESHOLD_DIFF; +#endif +#if defined(CONFIG_MACH_KONA_SENSOR) + gp2a->thresh_diff = DEFAULT_THRESHOLD_DIFF; +#else + if (pdata->gp2a_get_threshold) + gp2a_update_threshold(gp2a, is_gp2a030a() ? + gp2a_original_image_030a : gp2a_original_image, + pdata->gp2a_get_threshold(&gp2a->thresh_diff), false); + else + gp2a->thresh_diff = DEFAULT_THRESHOLD_DIFF; +#endif +#ifdef GP2A_CALIBRATION +#if defined(CONFIG_MACH_KONA_SENSOR) + gp2a->default_threshold = (is_gp2a030a() ? + gp2a_original_image_030a[5][1] : + gp2a_original_image[5][1]); +#else + gp2a->default_threshold = (is_gp2a030a() ? + gp2a_original_image_030a[3][1] : + gp2a_original_image[3][1]); +#endif +#endif + + gp2a->enabled = 0; + proximity_sensor_detection = 0; proximity_avg_on = 0; - gp2a->enabled = 0; gp2a->pdata = pdata; /* prox_timer settings. we poll for prox_avg values using a timer. */ @@ -1082,9 +1128,19 @@ static int gp2a_opt_probe(struct platform_device *pdev) pr_err("opt_probe failed : i2c_client is NULL\n"); goto err_no_device; } else - printk(KERN_INFO "opt_i2c_client : (0x%p), address = %x\n", + pr_info("opt_i2c_client : (0x%p), address = %x\n", opt_i2c_client, opt_i2c_client->addr); +#ifdef CONFIG_SENSORS_GP2A_VDD_CONTROL + /* gp2a power on */ + if (pdata->gp2a_vdd_on) { + pdata->gp2a_vdd_on(true); + pr_info("%s, power : %d\n", __func__, gp2a-->power_state); + } + gp2a->power_state |= LIGHT_ENABLED; + msleep(20); +#endif + /* GP2A Regs INIT SETTINGS and Check I2C communication */ value = 0x00; /* shutdown mode op[3]=0 */ @@ -1148,12 +1204,20 @@ static int gp2a_opt_probe(struct platform_device *pdev) goto err_proximity_device_create_file6; } +#ifdef GP2A_CALIBRATION if (device_create_file(gp2a->proximity_dev, &dev_attr_prox_cal) < 0) { pr_err("%s: could not create device file(%s)!\n", __func__, dev_attr_prox_cal.attr.name); goto err_proximity_device_create_file7; } + if (device_create_file(gp2a->proximity_dev, &dev_attr_prox_diff) < 0) { + pr_err("%s: could not create device file(%s)!\n", __func__, + dev_attr_prox_diff.attr.name); + goto err_proximity_device_create_file8; + } +#endif + #ifdef CONFIG_SLP device_init_wakeup(gp2a->proximity_dev, true); #endif @@ -1165,14 +1229,18 @@ static int gp2a_opt_probe(struct platform_device *pdev) return 0; -err_proximity_device_create_file7: +#ifdef GP2A_CALIBRATION +err_proximity_device_create_file8: device_remove_file(gp2a->proximity_dev, &dev_attr_prox_cal); -err_proximity_device_create_file6: +err_proximity_device_create_file7: device_remove_file(gp2a->proximity_dev, &dev_attr_raw_data); -err_proximity_device_create_file5: +#endif +err_proximity_device_create_file6: device_remove_file(gp2a->proximity_dev, &dev_attr_name); -err_proximity_device_create_file4: +err_proximity_device_create_file5: device_remove_file(gp2a->proximity_dev, &dev_attr_vendor); +err_proximity_device_create_file4: + device_remove_file(gp2a->proximity_dev, &dev_attr_prox_thresh); err_proximity_device_create_file3: device_remove_file(gp2a->proximity_dev, &dev_attr_prox_avg); err_proximity_device_create_file2: @@ -1201,19 +1269,20 @@ static int gp2a_opt_remove(struct platform_device *pdev) struct gp2a_data *gp2a = platform_get_drvdata(pdev); if (gp2a == NULL) { - printk(KERN_ERR "%s, gp2a_data is NULL!!!!!\n", __func__); + pr_err("%s, gp2a_data is NULL!!!!!\n", __func__); return -1; } if (gp2a->enabled) { disable_irq(gp2a->irq); - proximity_enable = 0; + gp2a->enabled = 0; + proximity_onoff(0); disable_irq_wake(gp2a->irq); -#ifndef CONFIG_MACH_MIDAS_02_BD +#if !defined(CONFIG_MACH_MIDAS_02_BD) gp2a->pdata->gp2a_led_on(false); + gp2a->power_state = 0; #endif - gp2a->enabled = 0; } hrtimer_cancel(&gp2a->prox_timer); @@ -1228,6 +1297,10 @@ static int gp2a_opt_remove(struct platform_device *pdev) device_remove_file(gp2a->proximity_dev, &dev_attr_vendor); device_remove_file(gp2a->proximity_dev, &dev_attr_name); device_remove_file(gp2a->proximity_dev, &dev_attr_raw_data); +#ifdef GP2A_CALIBRATION + device_remove_file(gp2a->proximity_dev, &dev_attr_prox_cal); + device_remove_file(gp2a->proximity_dev, &dev_attr_prox_diff); +#endif sensors_classdev_unregister(gp2a->proximity_dev); if (gp2a->input_dev != NULL) { @@ -1274,6 +1347,7 @@ static int gp2a_opt_resume(struct platform_device *pdev) gprintk("\n"); if (gp2a->enabled) { + if (device_may_wakeup(&pdev->dev)) enable_irq_wake(gp2a->irq); } @@ -1308,8 +1382,7 @@ static int proximity_onoff(u8 onoff) opt_i2c_write(gp2a_original_image[i][0], &gp2a_original_image[i][1]); if (err < 0) - printk(KERN_ERR - "%s : turnning on error i = %d, err=%d\n", + pr_err("%s : turnning on error i = %d, err=%d\n", __func__, i, err); lightsensor_mode = 0; } @@ -1342,7 +1415,7 @@ static int opt_i2c_probe(struct i2c_client *client, gprintk("start!!!\n"); if (client == NULL) - printk(KERN_ERR "GP2A i2c client is NULL !!!\n"); + pr_err("GP2A i2c client is NULL !!!\n"); opt_i2c_client = client; gprintk("end!!!\n"); diff --git a/drivers/sensor/k3dh_kona.c b/drivers/sensor/k3dh_kona.c new file mode 100644 index 0000000..04e1f8b --- /dev/null +++ b/drivers/sensor/k3dh_kona.c @@ -0,0 +1,1080 @@ +/* + * STMicroelectronics k3dh acceleration sensor driver + * + * Copyright (C) 2010 Samsung Electronics Co.Ltd + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/fs.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/miscdevice.h> +#include <linux/uaccess.h> +#include <linux/sensor/sensors_core.h> +#include <linux/sensor/k3dh.h> +#include "k3dh_reg.h" +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV +#include <linux/input.h> +#endif + +/* For Debugging */ +#if 1 +#define k3dh_dbgmsg(str, args...) pr_debug("%s: " str, __func__, ##args) +#endif +#define k3dh_infomsg(str, args...) pr_info("%s: " str, __func__, ##args) + +#define VENDOR "STM" +#define CHIP_ID "K3DH" + +/* The default settings when sensor is on is for all 3 axis to be enabled + * and output data rate set to 400Hz. Output is via a ioctl read call. + */ +#define DEFAULT_POWER_ON_SETTING (ODR400 | ENABLE_ALL_AXES) +#define ACC_DEV_MAJOR 241 + +#define CALIBRATION_FILE_PATH "/efs/calibration_data" +#define CAL_DATA_AMOUNT 20 + +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV +/* ABS axes parameter range [um/s^2] (for input event) */ +#define GRAVITY_EARTH 9806550 +#define ABSMAX_2G (GRAVITY_EARTH * 2) +#define ABSMIN_2G (-GRAVITY_EARTH * 2) +#define MIN_DELAY 5 +#define MAX_DELAY 200 +#endif + +static const struct odr_delay { + u8 odr; /* odr reg setting */ + s64 delay_ns; /* odr in ns */ +} odr_delay_table[] = { + { ODR1344, 744047LL }, /* 1344Hz */ + { ODR400, 2500000LL }, /* 400Hz */ + { ODR200, 5000000LL }, /* 200Hz */ + { ODR100, 10000000LL }, /* 100Hz */ + { ODR50, 20000000LL }, /* 50Hz */ + { ODR25, 40000000LL }, /* 25Hz */ + { ODR10, 100000000LL }, /* 10Hz */ + { ODR1, 1000000000LL }, /* 1Hz */ +}; + +/* K3DH acceleration data */ +struct k3dh_acc { + s16 x; + s16 y; + s16 z; +}; + +struct k3dh_data { + struct i2c_client *client; + struct miscdevice k3dh_device; + struct mutex read_lock; + struct mutex write_lock; + struct completion data_ready; +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) + struct class *acc_class; +#else + struct device *dev; +#endif + struct k3dh_acc cal_data; + struct k3dh_acc acc_xyz; + u8 ctrl_reg1_shadow; + atomic_t opened; /* opened implies enabled */ +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + struct input_dev *input; + struct delayed_work work; + atomic_t delay; + atomic_t enable; +#endif + bool axis_adjust; + int position; +}; + +static struct k3dh_data *g_k3dh; + + +static void k3dh_xyz_position_adjust(struct k3dh_acc *acc, + int position) +{ + const int position_map[][3][3] = { + {{ 0, 1, 0}, {-1, 0, 0}, { 0, 0, 1} }, /* 0 top/upper-left */ + {{-1, 0, 0}, { 0, -1, 0}, { 0, 0, 1} }, /* 1 top/upper-right */ + {{ 0, -1, 0}, { 1, 0, 0}, { 0, 0, 1} }, /* 2 top/lower-right */ + {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1} }, /* 3 top/lower-left */ + {{ 0, -1, 0}, {-1, 0, 0}, { 0, 0, -1} }, /* 4 bottom/upper-left */ + {{ 1, 0, 0}, { 0, -1, 0}, { 0, 0, -1} }, /* 5 bottom/upper-right */ + {{ 0, 1, 0}, { 1, 0, 0}, { 0, 0, -1} }, /* 6 bottom/lower-right */ + {{-1, 0, 0}, { 0, 1, 0}, { 0, 0, -1} }, /* 7 bottom/lower-left*/ + }; + + struct k3dh_acc xyz_adjusted = {0,}; + s16 raw[3] = {0,}; + int j; + raw[0] = acc->x; + raw[1] = acc->y; + raw[2] = acc->z; + for (j = 0; j < 3; j++) { + xyz_adjusted.x += + (position_map[position][0][j] * raw[j]); + xyz_adjusted.y += + (position_map[position][1][j] * raw[j]); + xyz_adjusted.z += + (position_map[position][2][j] * raw[j]); + } + acc->x = xyz_adjusted.x; + acc->y = xyz_adjusted.y; + acc->z = xyz_adjusted.z; +} + +/* Read X,Y and Z-axis acceleration raw data */ +static int k3dh_read_accel_raw_xyz(struct k3dh_data *data, + struct k3dh_acc *acc) +{ + int err; + s8 reg = OUT_X_L | AC; /* read from OUT_X_L to OUT_Z_H by auto-inc */ + u8 acc_data[6]; + + err = i2c_smbus_read_i2c_block_data(data->client, reg, + sizeof(acc_data), acc_data); + if (err != sizeof(acc_data)) { + pr_err("%s : failed to read 6 bytes for getting x/y/z\n", + __func__); + return -EIO; + } + + acc->x = (acc_data[1] << 8) | acc_data[0]; + acc->y = (acc_data[3] << 8) | acc_data[2]; + acc->z = (acc_data[5] << 8) | acc_data[4]; + + acc->x = acc->x >> 4; + acc->y = acc->y >> 4; + +#if defined(CONFIG_MACH_U1_NA_SPR_REV05) \ + || defined(CONFIG_MACH_U1_NA_SPR_EPIC2_REV00) \ + || defined(CONFIG_MACH_U1_NA_USCC_REV05) \ + || defined(CONFIG_MACH_Q1_BD) \ + || defined(CONFIG_MACH_U1_NA_USCC) \ + || defined(CONFIG_MACH_U1_NA_SPR) + acc->z = -acc->z >> 4; +#else + acc->z = acc->z >> 4; +#endif + + if (data->axis_adjust) + k3dh_xyz_position_adjust(acc, data->position); + return 0; +} + +static int k3dh_read_accel_xyz(struct k3dh_data *data, + struct k3dh_acc *acc) +{ + int err = 0; + + mutex_lock(&data->read_lock); + err = k3dh_read_accel_raw_xyz(data, acc); + mutex_unlock(&data->read_lock); + if (err < 0) { + pr_err("%s: k3dh_read_accel_raw_xyz() failed\n", __func__); + return err; + } + + acc->x -= data->cal_data.x; + acc->y -= data->cal_data.y; + acc->z -= data->cal_data.z; + + return err; +} + +static int k3dh_open_calibration(struct k3dh_data *data) +{ + struct file *cal_filp = NULL; + int err = 0; + mm_segment_t old_fs; + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666); + if (IS_ERR(cal_filp)) { + err = PTR_ERR(cal_filp); + if (err != -ENOENT) + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + return err; + } + + err = cal_filp->f_op->read(cal_filp, + (char *)&data->cal_data, 3 * sizeof(s16), &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't read the cal data from file\n", __func__); + err = -EIO; + } + + k3dh_dbgmsg("%s: (%u,%u,%u)\n", __func__, + data->cal_data.x, data->cal_data.y, data->cal_data.z); + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int k3dh_do_calibrate(struct device *dev, bool do_calib) +{ + struct k3dh_data *acc_data = dev_get_drvdata(dev); + struct k3dh_acc data = { 0, }; + struct file *cal_filp = NULL; + int sum[3] = { 0, }; + int err = 0; + int i; + mm_segment_t old_fs; + + if (do_calib) { + for (i = 0; i < CAL_DATA_AMOUNT; i++) { + mutex_lock(&acc_data->read_lock); + err = k3dh_read_accel_raw_xyz(acc_data, &data); + mutex_unlock(&acc_data->read_lock); + if (err < 0) { + pr_err("%s: k3dh_read_accel_raw_xyz() " + "failed in the %dth loop\n", + __func__, i); + return err; + } + + sum[0] += data.x; + sum[1] += data.y; + sum[2] += data.z; + } + + acc_data->cal_data.x = sum[0] / CAL_DATA_AMOUNT; + acc_data->cal_data.y = sum[1] / CAL_DATA_AMOUNT; + if (sum[2] >= 0) + acc_data->cal_data.z = (sum[2] / CAL_DATA_AMOUNT)-1024; + else + acc_data->cal_data.z = (sum[2] / CAL_DATA_AMOUNT)+1024; + } else { + acc_data->cal_data.x = 0; + acc_data->cal_data.y = 0; + acc_data->cal_data.z = 0; + } + + printk(KERN_INFO "%s: cal data (%d,%d,%d)\n", __func__, + acc_data->cal_data.x, acc_data->cal_data.y, acc_data->cal_data.z); + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, + O_CREAT | O_TRUNC | O_WRONLY, 0666); + if (IS_ERR(cal_filp)) { + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cal_filp); + return err; + } + + err = cal_filp->f_op->write(cal_filp, + (char *)&acc_data->cal_data, 3 * sizeof(s16), &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't write the cal data to file\n", __func__); + err = -EIO; + } + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int k3dh_accel_enable(struct k3dh_data *data) +{ + int err = 0; + + if (atomic_read(&data->opened) == 0) { + err = k3dh_open_calibration(data); + if (err < 0 && err != -ENOENT) + pr_err("%s: k3dh_open_calibration() failed\n", + __func__); + data->ctrl_reg1_shadow = DEFAULT_POWER_ON_SETTING; + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + DEFAULT_POWER_ON_SETTING); + if (err) + pr_err("%s: i2c write ctrl_reg1 failed\n", __func__); + +#if defined(CONFIG_SENSORS_K2DH) + err = i2c_smbus_write_byte_data(data->client, CTRL_REG4, + CTRL_REG4_HR | CTRL_REG4_BDU); +#else + err = i2c_smbus_write_byte_data(data->client, CTRL_REG4, + CTRL_REG4_HR); +#endif + if (err) + pr_err("%s: i2c write ctrl_reg4 failed\n", __func__); + } + + atomic_add(1, &data->opened); + + return err; +} + +static int k3dh_accel_disable(struct k3dh_data *data) +{ + int err = 0; + + atomic_sub(1, &data->opened); + if (atomic_read(&data->opened) == 0) { + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + PM_OFF); + data->ctrl_reg1_shadow = PM_OFF; + } + + return err; +} + +/* open command for K3DH device file */ +static int k3dh_open(struct inode *inode, struct file *file) +{ + k3dh_infomsg("is called.\n"); + return 0; +} + +/* release command for K3DH device file */ +static int k3dh_close(struct inode *inode, struct file *file) +{ + k3dh_infomsg("is called.\n"); + return 0; +} + +static s64 k3dh_get_delay(struct k3dh_data *data) +{ + int i; + u8 odr; + s64 delay = -1; + + odr = data->ctrl_reg1_shadow & ODR_MASK; + for (i = 0; i < ARRAY_SIZE(odr_delay_table); i++) { + if (odr == odr_delay_table[i].odr) { + delay = odr_delay_table[i].delay_ns; + break; + } + } + return delay; +} + +static int k3dh_set_delay(struct k3dh_data *data, s64 delay_ns) +{ + int odr_value = ODR1; + int res = 0; + int i; + + /* round to the nearest delay that is less than + * the requested value (next highest freq) + */ + for (i = 0; i < ARRAY_SIZE(odr_delay_table); i++) { + if (delay_ns < odr_delay_table[i].delay_ns) + break; + } + if (i > 0) + i--; + odr_value = odr_delay_table[i].odr; + delay_ns = odr_delay_table[i].delay_ns; + + k3dh_infomsg("old=%lldns, new=%lldns, odr=0x%x/0x%x\n", + k3dh_get_delay(data), delay_ns, odr_value, + data->ctrl_reg1_shadow); + mutex_lock(&data->write_lock); + if (odr_value != (data->ctrl_reg1_shadow & ODR_MASK)) { + u8 ctrl = (data->ctrl_reg1_shadow & ~ODR_MASK); + ctrl |= odr_value; + data->ctrl_reg1_shadow = ctrl; + res = i2c_smbus_write_byte_data(data->client, CTRL_REG1, ctrl); + } + mutex_unlock(&data->write_lock); + return res; +} + +/* ioctl command for K3DH device file */ +static long k3dh_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + int err = 0; + struct k3dh_data *data = container_of(file->private_data, + struct k3dh_data, k3dh_device); + s64 delay_ns; + int enable = 0; + + /* cmd mapping */ + switch (cmd) { + case K3DH_IOCTL_SET_ENABLE: + if (copy_from_user(&enable, (void __user *)arg, + sizeof(enable))) + return -EFAULT; + k3dh_infomsg("opened = %d, enable = %d\n", + atomic_read(&data->opened), enable); + if (enable) + err = k3dh_accel_enable(data); + else + err = k3dh_accel_disable(data); + break; + case K3DH_IOCTL_SET_DELAY: + if (copy_from_user(&delay_ns, (void __user *)arg, + sizeof(delay_ns))) + return -EFAULT; + err = k3dh_set_delay(data, delay_ns); + break; + case K3DH_IOCTL_GET_DELAY: + delay_ns = k3dh_get_delay(data); + if (put_user(delay_ns, (s64 __user *)arg)) + return -EFAULT; + break; + case K3DH_IOCTL_READ_ACCEL_XYZ: + err = k3dh_read_accel_xyz(data, &data->acc_xyz); + if (err) + break; + if (copy_to_user((void __user *)arg, + &data->acc_xyz, sizeof(data->acc_xyz))) + return -EFAULT; + break; + default: + err = -EINVAL; + break; + } + + return err; +} + +static int k3dh_suspend(struct device *dev) +{ + int res = 0; + struct k3dh_data *data = dev_get_drvdata(dev); +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + if (atomic_read(&data->enable)) + cancel_delayed_work_sync(&data->work); +#endif + if (atomic_read(&data->opened) > 0) + res = i2c_smbus_write_byte_data(data->client, + CTRL_REG1, PM_OFF); + + return res; +} + +static int k3dh_resume(struct device *dev) +{ + int res = 0; + struct k3dh_data *data = dev_get_drvdata(dev); + + if (atomic_read(&data->opened) > 0) + res = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + data->ctrl_reg1_shadow); +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + if (atomic_read(&data->enable)) + schedule_delayed_work(&data->work, + msecs_to_jiffies(5)); +#endif + return res; +} + +static const struct dev_pm_ops k3dh_pm_ops = { + .suspend = k3dh_suspend, + .resume = k3dh_resume, +}; + +static const struct file_operations k3dh_fops = { + .owner = THIS_MODULE, + .open = k3dh_open, + .release = k3dh_close, + .unlocked_ioctl = k3dh_ioctl, +}; + +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV +static ssize_t k3dh_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input = to_input_dev(dev); + struct k3dh_data *data = input_get_drvdata(input); + + return sprintf(buf, "%d\n", atomic_read(&data->enable)); +} + +static ssize_t k3dh_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input = to_input_dev(dev); + struct k3dh_data *data = input_get_drvdata(input); + unsigned long enable = 0; + int err; + + if (strict_strtoul(buf, 10, &enable)) + return -EINVAL; + k3dh_open_calibration(data); + + if (enable) { + err = k3dh_accel_enable(data); + if (err < 0) + goto done; + schedule_delayed_work(&data->work, + msecs_to_jiffies(5)); + } else { + cancel_delayed_work_sync(&data->work); + err = k3dh_accel_disable(data); + if (err < 0) + goto done; + } + atomic_set(&data->enable, enable); + pr_info("%s, enable = %ld\n", __func__, enable); +done: + return count; +} +static DEVICE_ATTR(enable, + S_IRUGO | S_IWUSR | S_IWGRP, + k3dh_enable_show, k3dh_enable_store); + +static ssize_t k3dh_delay_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input = to_input_dev(dev); + struct k3dh_data *data = input_get_drvdata(input); + + return sprintf(buf, "%d\n", atomic_read(&data->delay)); +} + +static ssize_t k3dh_delay_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input = to_input_dev(dev); + struct k3dh_data *data = input_get_drvdata(input); + unsigned long delay = 0; + if (strict_strtoul(buf, 10, &delay)) + return -EINVAL; + + if (delay > MAX_DELAY) + delay = MAX_DELAY; + if (delay < MIN_DELAY) + delay = MIN_DELAY; + atomic_set(&data->delay, delay); + k3dh_set_delay(data, delay * 1000000); + pr_info("%s, delay = %ld\n", __func__, delay); + return count; +} +static DEVICE_ATTR(poll_delay, + S_IRUGO | S_IWUSR | S_IWGRP, + k3dh_delay_show, k3dh_delay_store); + +static struct attribute *k3dh_attributes[] = { + &dev_attr_enable.attr, + &dev_attr_poll_delay.attr, + NULL +}; + +static struct attribute_group k3dh_attribute_group = { + .attrs = k3dh_attributes +}; +#endif + +static ssize_t k3dh_fs_read(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct k3dh_data *data = dev_get_drvdata(dev); + +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) + int err = 0; + int on; + + mutex_lock(&data->write_lock); + on = atomic_read(&data->opened); + if (on == 0) { + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + DEFAULT_POWER_ON_SETTING); + } + mutex_unlock(&data->write_lock); + + if (err < 0) { + pr_err("%s: i2c write ctrl_reg1 failed\n", __func__); + return err; + } + + err = k3dh_read_accel_xyz(data, &data->acc_xyz); + if (err < 0) { + pr_err("%s: k3dh_read_accel_xyz failed\n", __func__); + return err; + } + + if (on == 0) { + mutex_lock(&data->write_lock); + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + PM_OFF); + mutex_unlock(&data->write_lock); + if (err) + pr_err("%s: i2c write ctrl_reg1 failed\n", __func__); + } +#endif + return sprintf(buf, "%d,%d,%d\n", + data->acc_xyz.x, data->acc_xyz.y, data->acc_xyz.z); +} + +static ssize_t k3dh_calibration_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int err; + struct k3dh_data *data = dev_get_drvdata(dev); + + err = k3dh_open_calibration(data); + if (err < 0) + pr_err("%s: k3dh_open_calibration() failed\n", __func__); + + if (!data->cal_data.x && !data->cal_data.y && !data->cal_data.z) + err = -1; + + return sprintf(buf, "%d %d %d %d\n", + err, data->cal_data.x, data->cal_data.y, data->cal_data.z); +} + +static ssize_t k3dh_calibration_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct k3dh_data *data = dev_get_drvdata(dev); + bool do_calib; + int err; + + if (sysfs_streq(buf, "1")) + do_calib = true; + else if (sysfs_streq(buf, "0")) + do_calib = false; + else { + pr_debug("%s: invalid value %d\n", __func__, *buf); + return -EINVAL; + } + + if (atomic_read(&data->opened) == 0) { + /* if off, turn on the device.*/ + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + DEFAULT_POWER_ON_SETTING); + if (err) { + pr_err("%s: i2c write ctrl_reg1 failed(err=%d)\n", + __func__, err); + } + } + + err = k3dh_do_calibrate(dev, do_calib); + if (err < 0) { + pr_err("%s: k3dh_do_calibrate() failed\n", __func__); + return err; + } + + if (atomic_read(&data->opened) == 0) { + /* if off, turn on the device.*/ + err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, + PM_OFF); + if (err) { + pr_err("%s: i2c write ctrl_reg1 failed(err=%d)\n", + __func__, err); + } + } + + return count; +} + +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) +static DEVICE_ATTR(acc_file, 0664, k3dh_fs_read, NULL); +#else +static ssize_t +k3dh_accel_position_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct k3dh_data *data = dev_get_drvdata(dev); + + return sprintf(buf, "%d\n", data->position); +} + +static ssize_t +k3dh_accel_position_store(struct device *dev, + struct device_attribute *attr, + const char *buf, + size_t count) +{ + struct k3dh_data *data = dev_get_drvdata(dev); + int err = 0; + + err = kstrtoint(buf, 10, &data->position); + if (err < 0) + pr_err("%s, kstrtoint failed.", __func__); + + return count; +} + +static ssize_t k3dh_accel_vendor_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", VENDOR); +} + +static ssize_t k3dh_accel_name_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", CHIP_ID); +} + +static DEVICE_ATTR(name, 0664, k3dh_accel_name_show, NULL); +static DEVICE_ATTR(vendor, 0664, k3dh_accel_vendor_show, NULL); +static DEVICE_ATTR(raw_data, 0664, k3dh_fs_read, NULL); +static DEVICE_ATTR(position, 0664, + k3dh_accel_position_show, k3dh_accel_position_store); +#endif +static DEVICE_ATTR(calibration, 0664, + k3dh_calibration_show, k3dh_calibration_store); + +void k3dh_shutdown(struct i2c_client *client) +{ + int res = 0; + struct k3dh_data *data = i2c_get_clientdata(client); + + k3dh_infomsg("is called.\n"); + +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + if (atomic_read(&data->enable)) + cancel_delayed_work_sync(&data->work); +#endif + res = i2c_smbus_write_byte_data(data->client, + CTRL_REG1, PM_OFF); + if (res < 0) + pr_err("%s: pm_off failed %d\n", __func__, res); +} + +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV +static void k3dh_work_func(struct work_struct *work) +{ + k3dh_read_accel_xyz(g_k3dh, &g_k3dh->acc_xyz); + pr_debug("%s: x: %d, y: %d, z: %d\n", __func__, + g_k3dh->acc_xyz.x, g_k3dh->acc_xyz.y, g_k3dh->acc_xyz.z); + input_report_abs(g_k3dh->input, ABS_X, g_k3dh->acc_xyz.x); + input_report_abs(g_k3dh->input, ABS_Y, g_k3dh->acc_xyz.y); + input_report_abs(g_k3dh->input, ABS_Z, g_k3dh->acc_xyz.z); + input_sync(g_k3dh->input); + schedule_delayed_work(&g_k3dh->work, msecs_to_jiffies( + atomic_read(&g_k3dh->delay))); +} + +/* ----------------- * + Input device interface + * ------------------ */ +static int k3dh_input_init(struct k3dh_data *data) +{ + struct input_dev *dev; + int err = 0; + + dev = input_allocate_device(); + if (!dev) + return -ENOMEM; + dev->name = "accelerometer"; + dev->id.bustype = BUS_I2C; + + input_set_capability(dev, EV_ABS, ABS_MISC); + input_set_abs_params(dev, ABS_X, ABSMIN_2G, ABSMAX_2G, 0, 0); + input_set_abs_params(dev, ABS_Y, ABSMIN_2G, ABSMAX_2G, 0, 0); + input_set_abs_params(dev, ABS_Z, ABSMIN_2G, ABSMAX_2G, 0, 0); + input_set_drvdata(dev, data); + + err = input_register_device(dev); + if (err < 0) + goto done; + data->input = dev; +done: + return 0; +} +#endif +static int k3dh_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct k3dh_data *data; +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) + struct device *dev_t, *dev_cal; +#endif + struct accel_platform_data *pdata; + int err; + + k3dh_infomsg("is started.\n"); + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_WRITE_BYTE_DATA | + I2C_FUNC_SMBUS_READ_I2C_BLOCK)) { + pr_err("%s: i2c functionality check failed!\n", __func__); + err = -ENODEV; + goto exit; + } + + data = kzalloc(sizeof(struct k3dh_data), GFP_KERNEL); + if (data == NULL) { + dev_err(&client->dev, + "failed to allocate memory for module data\n"); + err = -ENOMEM; + goto exit; + } + + /* Checking device */ + err = i2c_smbus_write_byte_data(client, CTRL_REG1, + PM_OFF); + if (err) { + pr_err("%s: there is no such device, err = %d\n", + __func__, err); + goto err_read_reg; + } + + data->client = client; + g_k3dh = data; + i2c_set_clientdata(client, data); + + init_completion(&data->data_ready); + mutex_init(&data->read_lock); + mutex_init(&data->write_lock); + atomic_set(&data->opened, 0); + + /* sensor HAL expects to find /dev/accelerometer */ + data->k3dh_device.minor = MISC_DYNAMIC_MINOR; + data->k3dh_device.name = ACC_DEV_NAME; + data->k3dh_device.fops = &k3dh_fops; + + err = misc_register(&data->k3dh_device); + if (err) { + pr_err("%s: misc_register failed\n", __FILE__); + goto err_misc_register; + } + + pdata = client->dev.platform_data; + if (!pdata) { + /*Set by default position 3, it doesn't adjust raw value*/ + data->position = 3; + data->axis_adjust = false; + pr_err("using defualt position = %d\n", data->position); + } else { + if (pdata->accel_get_position) + data->position = pdata->accel_get_position(); + data->axis_adjust = pdata->axis_adjust; + pr_info("successful, position = %d\n", data->position); + } +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + atomic_set(&data->enable, 0); + atomic_set(&data->delay, 200); + k3dh_input_init(data); + + /* Setup sysfs */ + err = + sysfs_create_group(&data->input->dev.kobj, + &k3dh_attribute_group); + if (err < 0) + goto err_sysfs_create_group; + + /* Setup driver interface */ + INIT_DELAYED_WORK(&data->work, k3dh_work_func); +#endif +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) + /* creating class/device for test */ + data->acc_class = class_create(THIS_MODULE, "accelerometer"); + if (IS_ERR(data->acc_class)) { + pr_err("%s: class create failed(accelerometer)\n", __func__); + err = PTR_ERR(data->acc_class); + goto err_class_create; + } + + dev_t = device_create(data->acc_class, NULL, + MKDEV(ACC_DEV_MAJOR, 0), "%s", "accelerometer"); + if (IS_ERR(dev_t)) { + pr_err("%s: class create failed(accelerometer)\n", __func__); + err = PTR_ERR(dev_t); + goto err_acc_device_create; + } + + err = device_create_file(dev_t, &dev_attr_acc_file); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_acc_file.attr.name); + goto err_acc_device_create_file; + } + dev_set_drvdata(dev_t, data); + + /* creating device for calibration */ + dev_cal = device_create(sec_class, NULL, 0, NULL, "gsensorcal"); + if (IS_ERR(dev_cal)) { + pr_err("%s: class create failed(gsensorcal)\n", __func__); + err = PTR_ERR(dev_cal); + goto err_cal_device_create; + } + + err = device_create_file(dev_cal, &dev_attr_calibration); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_calibration.attr.name); + goto err_cal_device_create_file; + } + dev_set_drvdata(dev_cal, data); +#else + /* creating device for test & calibration */ + data->dev = sensors_classdev_register("accelerometer_sensor"); + if (IS_ERR(data->dev)) { + pr_err("%s: class create failed(accelerometer_sensor)\n", + __func__); + err = PTR_ERR(data->dev); + goto err_acc_device_create; + } + + err = device_create_file(data->dev, &dev_attr_position); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_position.attr.name); + goto err_position_device_create_file; + } + + err = device_create_file(data->dev, &dev_attr_raw_data); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_raw_data.attr.name); + goto err_acc_device_create_file; + } + + err = device_create_file(data->dev, &dev_attr_calibration); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_calibration.attr.name); + goto err_cal_device_create_file; + } + + err = device_create_file(data->dev, &dev_attr_vendor); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_vendor.attr.name); + goto err_vendor_device_create_file; + } + + err = device_create_file(data->dev, &dev_attr_name); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_name.attr.name); + goto err_name_device_create_file; + } + + dev_set_drvdata(data->dev, data); +#endif + + k3dh_infomsg("is successful.\n"); + + return 0; + +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) +err_cal_device_create_file: + device_destroy(sec_class, 0); +err_cal_device_create: + device_remove_file(dev_t, &dev_attr_acc_file); +err_acc_device_create_file: + device_destroy(data->acc_class, MKDEV(ACC_DEV_MAJOR, 0)); +err_acc_device_create: + class_destroy(data->acc_class); +err_class_create: +#else +err_name_device_create_file: + device_remove_file(data->dev, &dev_attr_vendor); +err_vendor_device_create_file: + device_remove_file(data->dev, &dev_attr_calibration); +err_cal_device_create_file: + device_remove_file(data->dev, &dev_attr_raw_data); +err_acc_device_create_file: + device_remove_file(data->dev, &dev_attr_position); +err_position_device_create_file: + sensors_classdev_unregister(data->dev); + +err_acc_device_create: +#endif +#ifdef CONFIG_SENSOR_K3DH_INPUTDEV + input_free_device(data->input); +err_sysfs_create_group: +#endif +misc_deregister(&data->k3dh_device); +err_misc_register: + mutex_destroy(&data->read_lock); + mutex_destroy(&data->write_lock); +err_read_reg: + kfree(data); +exit: + return err; +} + +static int k3dh_remove(struct i2c_client *client) +{ + struct k3dh_data *data = i2c_get_clientdata(client); + int err = 0; + + if (atomic_read(&data->opened) > 0) { + err = i2c_smbus_write_byte_data(data->client, + CTRL_REG1, PM_OFF); + if (err < 0) + pr_err("%s: pm_off failed %d\n", __func__, err); + } + +#if defined(CONFIG_MACH_U1) || defined(CONFIG_MACH_TRATS) + device_destroy(sec_class, 0); + device_destroy(data->acc_class, MKDEV(ACC_DEV_MAJOR, 0)); + class_destroy(data->acc_class); +#else + device_remove_file(data->dev, &dev_attr_name); + device_remove_file(data->dev, &dev_attr_vendor); + device_remove_file(data->dev, &dev_attr_calibration); + device_remove_file(data->dev, &dev_attr_raw_data); + device_remove_file(data->dev, &dev_attr_position); + sensors_classdev_unregister(data->dev); +#endif + misc_deregister(&data->k3dh_device); + mutex_destroy(&data->read_lock); + mutex_destroy(&data->write_lock); + kfree(data); + + return 0; +} + +static const struct i2c_device_id k3dh_id[] = { + { "k3dh", 0 }, + { } +}; +MODULE_DEVICE_TABLE(i2c, k3dh_id); + +static struct i2c_driver k3dh_driver = { + .probe = k3dh_probe, + .shutdown = k3dh_shutdown, + .remove = __devexit_p(k3dh_remove), + .id_table = k3dh_id, + .driver = { + .pm = &k3dh_pm_ops, + .owner = THIS_MODULE, + .name = "k3dh", + }, +}; + +static int __init k3dh_init(void) +{ + return i2c_add_driver(&k3dh_driver); +} + +static void __exit k3dh_exit(void) +{ + i2c_del_driver(&k3dh_driver); +} + +module_init(k3dh_init); +module_exit(k3dh_exit); + +MODULE_DESCRIPTION("k3dh accelerometer driver"); +MODULE_AUTHOR("Samsung Electronics"); +MODULE_LICENSE("GPL"); diff --git a/drivers/sensor/k3dh_reg.h b/drivers/sensor/k3dh_reg.h index 846d0d7..e6c9fa4 100644 --- a/drivers/sensor/k3dh_reg.h +++ b/drivers/sensor/k3dh_reg.h @@ -102,6 +102,7 @@ #define I1_OVERRUN (1 << 1) /* CTRL_REG4 */ +#define CTRL_REG4_BDU (1 << 7) #define CTRL_REG4_BLE (1 << 6) #define CTRL_REG4_FS1 (1 << 5) #define CTRL_REG4_FS0 (1 << 4) diff --git a/drivers/sensor/yas_mag_driver-yas532.c b/drivers/sensor/yas_mag_driver-yas532.c new file mode 100644 index 0000000..14259b8 --- /dev/null +++ b/drivers/sensor/yas_mag_driver-yas532.c @@ -0,0 +1,2909 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +#include <linux/sensor/yas.h> + +struct utimeval { + int32_t tv_sec; + int32_t tv_msec; +}; + +struct utimer { + struct utimeval prev_time; + struct utimeval total_time; + struct utimeval delay_ms; +}; + +static int utimeval_init(struct utimeval *val); +static int utimeval_is_initial(struct utimeval *val); +static int utimeval_is_overflow(struct utimeval *val); +static struct utimeval utimeval_plus(struct utimeval *first, + struct utimeval *second); +static struct utimeval utimeval_minus(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_than(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_or_equal(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_than_zero(struct utimeval *val); +static int utimeval_less_than_zero(struct utimeval *val); +static struct utimeval *msec_to_utimeval(struct utimeval *result, + uint32_t msec); +static uint32_t utimeval_to_msec(struct utimeval *val); + +static struct utimeval utimer_calc_next_time(struct utimer *ut, + struct utimeval *cur); +static struct utimeval utimer_current_time(void); +static int utimer_is_timeout(struct utimer *ut); +static int utimer_clear_timeout(struct utimer *ut); +static uint32_t utimer_get_total_time(struct utimer *ut); +static uint32_t utimer_get_delay(struct utimer *ut); +static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms); +static int utimer_update(struct utimer *ut); +static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur); +static uint32_t utimer_sleep_time(struct utimer *ut); +static uint32_t utimer_sleep_time_with_curtime(struct utimer *ut, + struct utimeval *cur); +static int utimer_init(struct utimer *ut, uint32_t delay_ms); +static int utimer_clear(struct utimer *ut); +static void utimer_lib_init(void (*func) (int *sec, int *msec)); + +#define YAS_REGADDR_DEVICE_ID (0x80) +#define YAS_REGADDR_ACTUATE_INIT_COIL (0x81) +#define YAS_REGADDR_MEASURE_COMMAND (0x82) +#define YAS_REGADDR_CONFIG (0x83) +#define YAS_REGADDR_MEASURE_INTERVAL (0x84) +#define YAS_REGADDR_OFFSET_X (0x85) +#define YAS_REGADDR_OFFSET_Y1 (0x86) +#define YAS_REGADDR_OFFSET_Y2 (0x87) +#define YAS_REGADDR_TEST1 (0x88) +#define YAS_REGADDR_TEST2 (0x89) +#define YAS_REGADDR_CAL (0x90) +#define YAS_REGADDR_MEASURE_DATA (0xb0) +#define YAS_YAS530_DEVICE_ID (0x01) /* YAS530 (MS-3E) */ +#define YAS_YAS530_VERSION_A (0) /* YAS530 (MS-3E Aver) */ +#define YAS_YAS530_VERSION_B (1) /* YAS530B (MS-3E Bver) */ +#define YAS_YAS530_VERSION_A_COEF (380) +#define YAS_YAS530_VERSION_B_COEF (550) +#define YAS_YAS530_DATA_CENTER (2048) +#define YAS_YAS530_DATA_OVERFLOW (4095) + +#define YAS_YAS532_DEVICE_ID (0x02) /* YAS532 (MS-3R) */ +#define YAS_YAS532_VERSION_AB (0) /* YAS532AB (MS-3R ABver) */ +#define YAS_YAS532_VERSION_AC (1) /* YAS532AC (MS-3R ACver) */ +#define YAS_YAS532_VERSION_AB_COEF (1800) +#define YAS_YAS532_VERSION_AC_COEF (900) +#define YAS_YAS532_DATA_CENTER (4096) +#define YAS_YAS532_DATA_OVERFLOW (8190) + +#undef YAS_YAS530_CAL_SINGLE_READ + +struct yas_machdep_func { + int (*device_open) (void); + int (*device_close) (void); + int (*device_write) (uint8_t addr, const uint8_t *buf, int len); + int (*device_read) (uint8_t addr, uint8_t *buf, int len); + void (*msleep) (int msec); +}; + +static int yas_cdrv_actuate_initcoil(void); +static int yas_cdrv_set_offset(const int8_t *offset); +static int yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, + int32_t *new_calib_offset, + int8_t *prev_offset, + int8_t *new_offset); +static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform); +static int yas_cdrv_measure_and_set_offset(int8_t *offset); +static int yas_cdrv_measure(int32_t *msens, int32_t *raw, int16_t *t); +static int yas_cdrv_init(const int8_t *transform, + struct yas_machdep_func *func); +static int yas_cdrv_term(void); + +static void (*current_time) (int *sec, int *msec) = { +0}; + +static int utimeval_init(struct utimeval *val) +{ + if (unlikely(!val)) + return -1; + val->tv_sec = val->tv_msec = 0; + return 0; +} + +static int utimeval_is_initial(struct utimeval *val) +{ + if (unlikely(!val)) + return 0; + return val->tv_sec == 0 && val->tv_msec == 0; +} + +static int utimeval_is_overflow(struct utimeval *val) +{ + int32_t max; + + if (unlikely(!val)) + return 0; + + max = (int32_t) (0xffffffff / 1000); + if (val->tv_sec > max) { + return 1; /* overflow */ + } else if (val->tv_sec == max) { + if (val->tv_msec > + (int32_t) (0xffffffff % 1000)) { + return 1; /* overflow */ + } + } + + return 0; +} + +static struct utimeval +utimeval_plus(struct utimeval *first, struct utimeval *second) +{ + struct utimeval result = { 0, 0 }; + int32_t tmp; + + if (unlikely(!first || !second)) + return result; + + tmp = first->tv_sec + second->tv_sec; + if (first->tv_sec >= 0 && second->tv_sec >= 0 && tmp < 0) + goto overflow; + if (first->tv_sec < 0 && second->tv_sec < 0 && tmp >= 0) + goto underflow; + + result.tv_sec = tmp; + result.tv_msec = first->tv_msec + second->tv_msec; + if (1000 <= result.tv_msec) { + tmp = result.tv_sec + result.tv_msec / 1000; + if (result.tv_sec >= 0 && result.tv_msec >= 0 && tmp < 0) + goto overflow; + result.tv_sec = tmp; + result.tv_msec = result.tv_msec % 1000; + } + if (result.tv_msec < 0) { + tmp = result.tv_sec + result.tv_msec / 1000 - 1; + if (result.tv_sec < 0 && result.tv_msec < 0 && tmp >= 0) + goto underflow; + result.tv_sec = tmp; + result.tv_msec = result.tv_msec % 1000 + 1000; + } + + return result; + +overflow: + result.tv_sec = 0x7fffffff; + result.tv_msec = 999; + return result; + +underflow: + result.tv_sec = 0x80000000; + result.tv_msec = 0; + return result; +} + +static struct utimeval +utimeval_minus(struct utimeval *first, struct utimeval *second) +{ + struct utimeval result = { 0, 0 }, tmp; + + if (first == NULL || second == NULL + || second->tv_sec == (int)0x80000000) + return result; + + tmp.tv_sec = -second->tv_sec; + tmp.tv_msec = -second->tv_msec; + return utimeval_plus(first, &tmp); +} + +static int utimeval_less_than(struct utimeval *first, struct utimeval *second) +{ + if (unlikely(!first || !second)) + return 0; + + if (first->tv_sec > second->tv_sec) + return 1; + else if (first->tv_sec < second->tv_sec) + return 0; + else + if (first->tv_msec > second->tv_msec) + return 1; + else + return 0; +} + +static int +utimeval_greater_than(struct utimeval *first, struct utimeval *second) +{ + if (unlikely(!first || !second)) + return 0; + + if (first->tv_sec < second->tv_sec) + return 1; + else if (first->tv_sec > second->tv_sec) + return 0; + else + if (first->tv_msec < second->tv_msec) + return 1; + else + return 0; +} + +static int +utimeval_greater_or_equal(struct utimeval *first, struct utimeval *second) +{ + return !utimeval_less_than(first, second); +} + +static int utimeval_greater_than_zero(struct utimeval *val) +{ + struct utimeval zero = { 0, 0 }; + return utimeval_greater_than(&zero, val); +} + +static int utimeval_less_than_zero(struct utimeval *val) +{ + struct utimeval zero = { 0, 0 }; + return utimeval_less_than(&zero, val); +} + +static struct utimeval *msec_to_utimeval(struct utimeval *result, uint32_t msec) +{ + if (unlikely(!result)) + return result; + result->tv_sec = msec / 1000; + result->tv_msec = msec % 1000; + + return result; +} + +static uint32_t utimeval_to_msec(struct utimeval *val) +{ + if (unlikely(!val)) + return 0; + if (utimeval_less_than_zero(val)) + return 0; + + if (utimeval_is_overflow(val)) + return 0xffffffff; + + return val->tv_sec * 1000 + val->tv_msec; +} + +static struct utimeval +utimer_calc_next_time(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval result = { 0, 0 }, delay; + + if (ut == NULL || cur == NULL) + return result; + utimer_update_with_curtime(ut, cur); + if (utimer_is_timeout(ut)) { + result = *cur; + } else { + delay = utimeval_minus(&ut->delay_ms, &ut->total_time); + result = utimeval_plus(cur, &delay); + } + + return result; +} + +static struct utimeval utimer_current_time(void) +{ + struct utimeval tv; + int sec, msec; + + if (current_time != NULL) + current_time(&sec, &msec); + else + sec = 0, msec = 0; + tv.tv_sec = sec; + tv.tv_msec = msec; + + return tv; +} + +static int utimer_clear(struct utimer *ut) +{ + if (unlikely(!ut)) + return -1; + utimeval_init(&ut->prev_time); + utimeval_init(&ut->total_time); + + return 0; +} + +static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval tmp; + + if (unlikely(!ut || !cur)) + return -1; + if (utimeval_is_initial(&ut->prev_time)) + ut->prev_time = *cur; + if (utimeval_greater_than_zero(&ut->delay_ms)) { + tmp = utimeval_minus(cur, &ut->prev_time); + if (utimeval_less_than_zero(&tmp)) + utimeval_init(&ut->total_time); + else { + ut->total_time = utimeval_plus(&tmp, &ut->total_time); + if (utimeval_is_overflow(&ut->total_time)) + utimeval_init(&ut->total_time); + } + ut->prev_time = *cur; + } + + return 0; +} + +static int utimer_update(struct utimer *ut) +{ + struct utimeval cur; + + if (unlikely(!ut)) + return -1; + + cur = utimer_current_time(); + utimer_update_with_curtime(ut, &cur); + return 0; +} + +static int utimer_is_timeout(struct utimer *ut) +{ + if (unlikely(!ut)) + return 0; + + if (utimeval_greater_than_zero(&ut->delay_ms)) + return utimeval_greater_or_equal(&ut->delay_ms, + &ut->total_time); + else + return 1; +} + +static int utimer_clear_timeout(struct utimer *ut) +{ + uint32_t delay, total; + + if (unlikely(!ut)) + return -1; + + delay = utimeval_to_msec(&ut->delay_ms); + if (delay == 0 || utimeval_is_overflow(&ut->total_time)) + total = 0; + else + if (utimeval_is_overflow(&ut->total_time)) + total = 0; + else { + total = utimeval_to_msec(&ut->total_time); + total = total % delay; + } + msec_to_utimeval(&ut->total_time, total); + + return 0; +} + +static uint32_t +utimer_sleep_time_with_curtime(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval tv; + + if (unlikely(!ut || !cur)) + return 0; + + tv = utimer_calc_next_time(ut, cur); + tv = utimeval_minus(&tv, cur); + if (utimeval_less_than_zero(&tv)) + return 0; + + return utimeval_to_msec(&tv); +} + +static uint32_t utimer_sleep_time(struct utimer *ut) +{ + struct utimeval cur; + + if (unlikely(!ut)) + return 0; + + cur = utimer_current_time(); + return utimer_sleep_time_with_curtime(ut, &cur); +} + +static int utimer_init(struct utimer *ut, uint32_t delay_ms) +{ + if (unlikely(!ut)) + return -1; + utimer_clear(ut); + msec_to_utimeval(&ut->delay_ms, delay_ms); + + return 0; +} + +static uint32_t utimer_get_total_time(struct utimer *ut) +{ + return utimeval_to_msec(&ut->total_time); +} + +static uint32_t utimer_get_delay(struct utimer *ut) +{ + if (unlikely(!ut)) + return -1; + return utimeval_to_msec(&ut->delay_ms); +} + +static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms) +{ + return utimer_init(ut, delay_ms); +} + +static void utimer_lib_init(void (*func) (int *sec, int *msec)) +{ + current_time = func; +} + +struct yas_cal_data { + uint8_t dx, dy1, dy2; + uint8_t d2, d3, d4, d5, d6, d7, d8, d9, d0; + uint8_t dck; + uint8_t ver; +}; +struct yas_correction_data { + int32_t Cx, Cy1, Cy2; + int32_t a2, a3, a4, a5, a6, a7, a8, a9, k; +}; +struct yas_cdriver { + struct yas_cal_data cal; + struct yas_correction_data correct; + struct yas_machdep_func func; + int8_t transform[9]; + int16_t temperature; + uint8_t dev_id; + int32_t coef; + int16_t center; + int16_t overflow; +}; +static struct yas_cdriver cdriver; + +static int device_open(void) +{ + if (cdriver.func.device_open == NULL) + return -1; + return cdriver.func.device_open(); +} + +static int device_close(void) +{ + if (cdriver.func.device_close == NULL) + return -1; + return cdriver.func.device_close(); +} + +static int device_write(uint8_t addr, const uint8_t *buf, int len) +{ + if (cdriver.func.device_write == NULL) + return -1; + return cdriver.func.device_write(addr, buf, len); +} + +static int device_read(uint8_t addr, uint8_t *buf, int len) +{ + if (cdriver.func.device_read == NULL) + return -1; + return cdriver.func.device_read(addr, buf, len); +} + +static void sleep(int millisec) +{ + if (cdriver.func.msleep == NULL) + return; + cdriver.func.msleep(millisec); +} + +static int init_test_register(void) +{ + uint8_t data; + + data = 0x00; + if (device_write(YAS_REGADDR_TEST1, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + data = 0x00; + if (device_write(YAS_REGADDR_TEST2, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int get_device_id(uint8_t *id) +{ + uint8_t data = 0; + + if (device_read(YAS_REGADDR_DEVICE_ID, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + *id = data; + + return YAS_NO_ERROR; +} + +static int get_cal_data_yas530(struct yas_cal_data *cal) +{ + uint8_t data[16]; +#ifdef YAS_YAS530_CAL_SINGLE_READ + int i; + + for (i = 0; i < 16; i++) { /* dummy read */ + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + for (i = 0; i < 16; i++) { + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } +#else + if (device_read(YAS_REGADDR_CAL, data, 16) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (device_read(YAS_REGADDR_CAL, data, 16) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; +#endif + + cal->dx = data[0]; + cal->dy1 = data[1]; + cal->dy2 = data[2]; + cal->d2 = (data[3] >> 2) & 0x03f; + cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + cal->d4 = data[4] & 0x3f; + cal->d5 = (data[5] >> 2) & 0x3f; + cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + cal->d0 = (data[9] >> 2) & 0x1f; + cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + cal->ver = (data[15]) & 0x03; + + return YAS_NO_ERROR; +} + +static void +get_correction_value_yas530(struct yas_cal_data *cal, + struct yas_correction_data *correct) +{ + correct->Cx = cal->dx * 6 - 768; + correct->Cy1 = cal->dy1 * 6 - 768; + correct->Cy2 = cal->dy2 * 6 - 768; + correct->a2 = cal->d2 - 32; + correct->a3 = cal->d3 - 8; + correct->a4 = cal->d4 - 32; + correct->a5 = cal->d5 + 38; + correct->a6 = cal->d6 - 32; + correct->a7 = cal->d7 - 64; + correct->a8 = cal->d8 - 32; + correct->a9 = cal->d9; + correct->k = cal->d0 + 10; +} + +static int get_cal_data_yas532(struct yas_cal_data *cal) +{ + uint8_t data[14]; +#ifdef YAS_YAS530_CAL_SINGLE_READ + int i; + + for (i = 0; i < 14; i++) { /* dummy read */ + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + for (i = 0; i < 14; i++) { + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } +#else + if (device_read(YAS_REGADDR_CAL, data, 14) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (device_read(YAS_REGADDR_CAL, data, 14) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; +#endif + + cal->dx = data[0]; + cal->dy1 = data[1]; + cal->dy2 = data[2]; + cal->d2 = (data[3] >> 2) & 0x03f; + cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + cal->d4 = data[4] & 0x3f; + cal->d5 = (data[5] >> 2) & 0x3f; + cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + cal->d0 = (data[9] >> 2) & 0x1f; + cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + cal->ver = (data[13]) & 0x01; + + return YAS_NO_ERROR; +} + +static void +get_correction_value_yas532(struct yas_cal_data *cal, + struct yas_correction_data *correct) +{ + correct->Cx = cal->dx * 10 - 1280; + correct->Cy1 = cal->dy1 * 10 - 1280; + correct->Cy2 = cal->dy2 * 10 - 1280; + correct->a2 = cal->d2 - 32; + correct->a3 = cal->d3 - 8; + correct->a4 = cal->d4 - 32; + correct->a5 = cal->d5 + 38; + correct->a6 = cal->d6 - 32; + correct->a7 = cal->d7 - 64; + correct->a8 = cal->d8 - 32; + correct->a9 = cal->d9; + correct->k = cal->d0; +} + +static int set_configuration(int inton, int inthact, int cck) +{ + uint8_t data = 0; + + data |= (!!inton) & 0x01; + data |= ((!!inthact) << 1) & 0x02; + data |= (cck << 2) & 0x1c; + + if (device_write(YAS_REGADDR_CONFIG, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int get_measure_interval(int32_t *msec) +{ + uint8_t data; + int mult = 7; + + if (device_read(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + mult = 4; + break; + case YAS_YAS530_DEVICE_ID: + default: + mult = 7; + break; + } + + *msec = data * mult; + + return YAS_NO_ERROR; +} + +static int set_measure_interval(int32_t msec) +{ + uint8_t data = 0; + int mult = 7; + + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + mult = 4; + break; + case YAS_YAS530_DEVICE_ID: + default: + mult = 7; + break; + } + + if (msec > mult*0xff) + data = 0xff; + else + if (msec % mult == 0) + data = (uint8_t)(msec / mult); + else + data = (uint8_t)(msec / mult + 1); + if (device_write(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int set_measure_command(int ldtc, int fors, int dlymes) +{ + uint8_t data = 0; + + data |= 0x01; /* bit 0 must be 1 */ + data |= ((!(!ldtc)) << 1) & 0x02; + data |= ((!(!fors)) << 2) & 0x04; + data |= ((!(!dlymes)) << 4) & 0x10; + + if (device_write(YAS_REGADDR_MEASURE_COMMAND, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int +measure_normal_yas530(int *busy, int16_t *t, int16_t *x, int16_t *y1, + int16_t *y2) +{ + uint8_t data[8]; + + if (set_measure_command(0, 0, 0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + sleep(2); + + if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + *busy = (data[0] >> 7) & 0x01; + *t = (((int32_t) data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03); + *x = (((int32_t) data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f); + *y1 = (((int32_t) data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f); + *y2 = (((int32_t) data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f); + /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", + *busy, *t, *x, *y1, *y2)); */ + + return YAS_NO_ERROR; +} + +static int +measure_normal_yas532(int *busy, int16_t *t, int16_t *x, int16_t *y1, + int16_t *y2) +{ + uint8_t data[8]; + + if (set_measure_command(0, 0, 0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + sleep(2); + + if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + *busy = (data[0] >> 7) & 0x01; + *t = (((int32_t) data[0] << 3) & 0x3f8) | ((data[1] >> 5) & 0x07); + *x = (((int32_t) data[2] << 6) & 0x1fc0) | ((data[3] >> 2) & 0x3f); + *y1 = (((int32_t) data[4] << 6) & 0x1fc0) | ((data[5] >> 2) & 0x3f); + *y2 = (((int32_t) data[6] << 6) & 0x1fc0) | ((data[7] >> 2) & 0x3f); + /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", + *busy, *t, *x, *y1, *y2)); */ + + return YAS_NO_ERROR; +} + +static int +measure_normal(int *busy, int16_t *t, int16_t *x, int16_t *y1, int16_t *y2) +{ + int result; + + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + result = measure_normal_yas532(busy, t, x, y1, y2); + break; + case YAS_YAS530_DEVICE_ID: + default: + result = measure_normal_yas530(busy, t, x, y1, y2); + break; + } + + return result; +} + +static int +coordinate_conversion(int32_t x, int32_t y1, int32_t y2, int16_t t, + int32_t *xo, int32_t *yo, int32_t *zo, + struct yas_correction_data *c) +{ + int32_t sx, sy1, sy2, sy, sz; + int32_t hx, hy, hz; + + sx = x - (c->Cx * t) / 100; + sy1 = y1 - (c->Cy1 * t) / 100; + sy2 = y2 - (c->Cy2 * t) / 100; + + sy = sy1 - sy2; + sz = -sy1 - sy2; + + hx = c->k * ((100 * sx + c->a2 * sy + c->a3 * sz) / 10); + hy = c->k * ((c->a4 * sx + c->a5 * sy + c->a6 * sz) / 10); + hz = c->k * ((c->a7 * sx + c->a8 * sy + c->a9 * sz) / 10); + + *xo = cdriver.transform[0] * hx + + cdriver.transform[1] * hy + cdriver.transform[2] * hz; + *yo = cdriver.transform[3] * hx + + cdriver.transform[4] * hy + cdriver.transform[5] * hz; + *zo = cdriver.transform[6] * hx + + cdriver.transform[7] * hy + cdriver.transform[8] * hz; + + return YAS_NO_ERROR; +} + +static int +set_hardware_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2) +{ + uint8_t data; + + data = offset_x & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_X, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + data = offset_y1 & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_Y1, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + data = offset_y2 & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_Y2, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_actuate_initcoil(void) +{ + uint8_t data = 0; + + if (device_write(YAS_REGADDR_ACTUATE_INIT_COIL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + return YAS_NO_ERROR; +} + +static int +check_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2, + int *flag_x, int *flag_y1, int *flag_y2) +{ + int busy; + int16_t t, x, y1, y2; + + if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + *flag_x = *flag_y1 = *flag_y2 = 0; + if (x > cdriver.center) + *flag_x = 1; + if (y1 > cdriver.center) + *flag_y1 = 1; + if (y2 > cdriver.center) + *flag_y2 = 1; + if (x < cdriver.center) + *flag_x = -1; + if (y1 < cdriver.center) + *flag_y1 = -1; + if (y2 < cdriver.center) + *flag_y2 = -1; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_measure_and_set_offset(int8_t *offset) +{ + int i; + int8_t offset_x = 0, offset_y1 = 0, offset_y2 = 0; + int flag_x = 0, flag_y1 = 0, flag_y2 = 0; + static const int correct[5] = { 16, 8, 4, 2, 1 }; + + for (i = 0; i < 5; i++) { + if (check_offset(offset_x, offset_y1, offset_y2, + &flag_x, &flag_y1, &flag_y2) < 0) { + return YAS_ERROR_DEVICE_COMMUNICATION; + } + YLOGD(("offset[%d][%d][%d] flag[%d][%d][%d]\n", + offset_x, offset_y1, offset_y2, + flag_x, flag_y1, flag_y2)); + if (flag_x) + offset_x += flag_x * correct[i]; + if (flag_y1) + offset_y1 += flag_y1 * correct[i]; + if (flag_y2) + offset_y2 += flag_y2 * correct[i]; + } + if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + offset[0] = offset_x; + offset[1] = offset_y1; + offset[2] = offset_y2; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_set_offset(const int8_t *offset) +{ + if (set_hardware_offset(offset[0], offset[1], offset[2]) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + return YAS_NO_ERROR; +} + +static int +yas_cdrv_measure(int32_t *data, int32_t *raw, int16_t *temperature) +{ + int busy; + int16_t x, y1, y2, t; + int32_t xx, yy1, yy2; + int result = 0; + + if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (x == 0) + result |= 0x01; + if (x == cdriver.overflow) + result |= 0x02; + if (y1 == 0) + result |= 0x04; + if (y1 == cdriver.overflow) + result |= 0x08; + if (y2 == 0) + result |= 0x10; + if (y2 == cdriver.overflow) + result |= 0x20; + + xx = x; + yy1 = y1; + yy2 = y2; + + if (coordinate_conversion(xx, yy1, yy2, t, &data[0], &data[1], + &data[2], &cdriver.correct) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + cdriver.temperature = t; + + if (raw != NULL) + raw[0] = xx, raw[1] = yy1, raw[2] = yy2; + if (temperature != NULL) + *temperature = t; + + return result; +} + +static int +yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, + int32_t *new_calib_offset, + int8_t *prev_offset, int8_t *new_offset) +{ + int32_t tmp[3], resolution[9], base[3]; + int32_t raw[3]; + int32_t diff, i; + + if (prev_calib_offset == NULL || new_calib_offset == NULL + || prev_offset == NULL || new_offset == NULL) + return YAS_ERROR_ARG; + + raw[0] = raw[1] = raw[2] = 0; + if (coordinate_conversion(raw[0], raw[1], raw[2], cdriver.temperature, + &base[0], &base[1], &base[2], + &cdriver.correct) < 0) + return YAS_ERROR_ERROR; + for (i = 0; i < 3; i++) { + raw[0] = raw[1] = raw[2] = 0; + raw[i] = cdriver.coef; + if (coordinate_conversion(raw[0], raw[1], raw[2], + cdriver.temperature, + &resolution[i * 3 + 0], + &resolution[i * 3 + 1], + &resolution[i * 3 + 2], + &cdriver.correct) < 0) + return YAS_ERROR_ERROR; + resolution[i * 3 + 0] -= base[0]; + resolution[i * 3 + 1] -= base[1]; + resolution[i * 3 + 2] -= base[2]; + } + + for (i = 0; i < 3; i++) + tmp[i] = prev_calib_offset[i]; + + for (i = 0; i < 3; i++) { + diff = (int32_t) new_offset[i] - (int32_t) prev_offset[i]; + while (diff > 0) { + tmp[0] -= resolution[i * 3 + 0]; + tmp[1] -= resolution[i * 3 + 1]; + tmp[2] -= resolution[i * 3 + 2]; + diff--; + } + while (diff < 0) { + tmp[0] += resolution[i * 3 + 0]; + tmp[1] += resolution[i * 3 + 1]; + tmp[2] += resolution[i * 3 + 2]; + diff++; + } + } + for (i = 0; i < 3; i++) + new_calib_offset[i] = tmp[i]; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform) +{ + int i; + + if (transform == NULL) + return YAS_ERROR_ARG; + for (i = 0; i < 9; i++) + cdriver.transform[i] = transform[i]; + + return YAS_NO_ERROR; +} + +static int +yas_cdrv_init(const int8_t *transform, struct yas_machdep_func *func) +{ + int interval, i; + uint8_t id; + + if (transform == NULL || func == NULL) + return YAS_ERROR_ARG; + + for (i = 0; i < 9; i++) + cdriver.transform[i] = transform[i]; + + cdriver.func = *func; + + if (device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (init_test_register() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (get_device_id(&id) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + YLOGD(("device id:%02x\n", id)); + + switch (id) { + case YAS_YAS530_DEVICE_ID: + if (get_cal_data_yas530(&cdriver.cal) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + get_correction_value_yas530(&cdriver.cal, &cdriver.correct); + cdriver.center = YAS_YAS530_DATA_CENTER; + cdriver.overflow = YAS_YAS530_DATA_OVERFLOW; + switch (cdriver.cal.ver) { + case YAS_YAS530_VERSION_B: + cdriver.coef = YAS_YAS530_VERSION_B_COEF; + break; + case YAS_YAS530_VERSION_A: + default: + cdriver.coef = YAS_YAS530_VERSION_A_COEF; + break; + } + break; + + case YAS_YAS532_DEVICE_ID: + if (get_cal_data_yas532(&cdriver.cal) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + get_correction_value_yas532(&cdriver.cal, &cdriver.correct); + cdriver.center = YAS_YAS532_DATA_CENTER; + cdriver.overflow = YAS_YAS532_DATA_OVERFLOW; + switch (cdriver.cal.ver) { + case YAS_YAS532_VERSION_AC: + cdriver.coef = YAS_YAS532_VERSION_AC_COEF; + break; + case YAS_YAS532_VERSION_AB: + default: + cdriver.coef = YAS_YAS532_VERSION_AB_COEF; + break; + } + break; + + default: + return YAS_ERROR_DEVICE_COMMUNICATION; + } + cdriver.dev_id = id; + + if (set_configuration(0, 0, cdriver.cal.dck) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (set_measure_interval(0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (get_measure_interval(&interval) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + YLOGD(("interval[%d]\n", interval)); + + return YAS_NO_ERROR; +} + +static int yas_cdrv_term(void) +{ + device_close(); + return YAS_NO_ERROR; +} + +#define YAS_DEFAULT_CALIB_INTERVAL (50) /* 50 msecs */ +#define YAS_DEFAULT_DATA_INTERVAL (200) /* 200 msecs */ +#define YAS_INITCOIL_INTERVAL (200) /* 200 msec */ +#define YAS_INITCOIL_GIVEUP_INTERVAL (180000) /* 180 seconds */ +#define YAS_DETECT_OVERFLOW_INTERVAL (0) /* 0 second */ + +#define YAS_MAG_ERROR_DELAY (200) +#define YAS_MAG_STATE_NORMAL (0) +#define YAS_MAG_STATE_INIT_COIL (1) +#define YAS_MAG_STATE_MEASURE_OFFSET (2) + +static const int8_t YAS_TRANSFORMATION[][9] = { + {0, 1, 0, -1, 0, 0, 0, 0, 1}, + {-1, 0, 0, 0, -1, 0, 0, 0, 1}, + {0, -1, 0, 1, 0, 0, 0, 0, 1}, + {1, 0, 0, 0, 1, 0, 0, 0, 1}, + {0, -1, 0, -1, 0, 0, 0, 0, -1}, + {1, 0, 0, 0, -1, 0, 0, 0, -1}, + {0, 1, 0, 1, 0, 0, 0, 0, -1}, + {-1, 0, 0, 0, 1, 0, 0, 0, -1}, +}; + +static const int supported_data_interval[] = { 10, 20, 50, 60, 100, 200, 1000 }; +static const int supported_calib_interval[] = { 60, 60, 50, 60, 50, 50, 50 }; +static const int32_t INVALID_CALIB_OFFSET[] = { 0x7fffffff, + 0x7fffffff, 0x7fffffff }; +static const int8_t INVALID_OFFSET[] = { 0x7f, 0x7f, 0x7f }; + +struct yas_adaptive_filter { + int num; + int index; + int filter_len; + int filter_noise; + int32_t sequence[YAS_MAG_MAX_FILTER_LEN]; +}; + +struct yas_thresh_filter { + int32_t threshold; + int32_t last; +}; + +struct yas_driver { + int initialized; + struct yas_mag_driver_callback callback; + struct utimer data_timer; + struct utimer initcoil_timer; + struct utimer initcoil_giveup_timer; + struct utimer detect_overflow_timer; + int32_t prev_mag[3]; + int32_t prev_xy1y2[3]; + int32_t prev_mag_w_offset[3]; + int16_t prev_temperature; + int measure_state; + int active; + int overflow; + int initcoil_gaveup; + int position; + int delay_timer_use_data; + int delay_timer_interval; + int delay_timer_counter; + int filter_enable; + int filter_len; + int filter_thresh; + int filter_noise[3]; + struct yas_adaptive_filter adap_filter[3]; + struct yas_thresh_filter thresh_filter[3]; + struct yas_mag_offset offset; +#ifdef YAS_MAG_MANUAL_OFFSET + struct yas_vector manual_offset; +#endif + struct yas_matrix static_matrix; + struct yas_matrix dynamic_matrix; +}; + +static struct yas_driver this_driver; + +static int lock(void) +{ + if (this_driver.callback.lock != NULL) { + if (this_driver.callback.lock() < 0) + return YAS_ERROR_RESTARTSYS; + } + return 0; +} + +static int unlock(void) +{ + if (this_driver.callback.unlock != NULL) { + if (this_driver.callback.unlock() < 0) + return YAS_ERROR_RESTARTSYS; + } + return 0; +} + +static int32_t square(int32_t data) +{ + return data * data; +} + +static void +adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, + int noise) +{ + int i; + + adap_filter->num = 0; + adap_filter->index = 0; + adap_filter->filter_noise = noise; + adap_filter->filter_len = len; + + for (i = 0; i < adap_filter->filter_len; ++i) + adap_filter->sequence[i] = 0; +} + +static int32_t +adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, int32_t in) +{ + int32_t avg, sum; + int i; + + if (adap_filter->filter_len == 0) + return in; + + if (adap_filter->num < adap_filter->filter_len) { + adap_filter->sequence[adap_filter->index++] = in / 100; + adap_filter->num++; + return in; + } + if (adap_filter->filter_len <= adap_filter->index) + adap_filter->index = 0; + + adap_filter->sequence[adap_filter->index++] = in / 100; + + avg = 0; + for (i = 0; i < adap_filter->filter_len; i++) + avg += adap_filter->sequence[i]; + + avg /= adap_filter->filter_len; + + sum = 0; + for (i = 0; i < adap_filter->filter_len; i++) + sum += square(avg - adap_filter->sequence[i]); + + sum /= adap_filter->filter_len; + + if (sum <= adap_filter->filter_noise) + return avg * 100; + + return ((in / 100 - avg) * (sum - adap_filter->filter_noise) / sum + + avg) * 100; +} + +static void +thresh_filter_init(struct yas_thresh_filter *thresh_filter, int threshold) +{ + thresh_filter->threshold = threshold; + thresh_filter->last = 0; +} + +static int32_t +thresh_filter_filter(struct yas_thresh_filter *thresh_filter, int32_t in) +{ + if (in < thresh_filter->last - thresh_filter->threshold + || thresh_filter->last + thresh_filter->threshold < in) { + thresh_filter->last = in; + return in; + } else + return thresh_filter->last; +} + +static void filter_init(struct yas_driver *d) +{ + int i; + + for (i = 0; i < 3; i++) { + adaptive_filter_init(&d->adap_filter[i], d->filter_len, + d->filter_noise[i]); + thresh_filter_init(&d->thresh_filter[i], d->filter_thresh); + } +} + +static void +filter_filter(struct yas_driver *d, int32_t *orig, int32_t *filtered) +{ + int i; + + for (i = 0; i < 3; i++) { + filtered[i] = + adaptive_filter_filter(&d->adap_filter[i], orig[i]); + filtered[i] = + thresh_filter_filter(&d->thresh_filter[i], filtered[i]); + } +} + +static int is_valid_offset(const int8_t *p) +{ + return (p != NULL && (p[0] <= 31) && (p[1] <= 31) && (p[2] <= 31) + && (-31 <= p[0]) && (-31 <= p[1]) && (-31 <= p[2])); +} + +static int is_valid_calib_offset(const int32_t *p) +{ + int i; + for (i = 0; i < 3; i++) { + if (p[i] != INVALID_CALIB_OFFSET[i]) + return 1; + } + return 0; +} + +static int is_offset_differ(const int8_t *p0, const int8_t *p1) +{ + if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) + return 1; + else + return 0; +} + +static int is_calib_offset_differ(const int32_t *p0, const int32_t *p1) +{ + if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) + return 1; + else + return 0; +} + +static int get_overflow(struct yas_driver *d) +{ + return d->overflow; +} + +static void set_overflow(struct yas_driver *d, const int overflow) +{ + if (d->overflow != overflow) + d->overflow = overflow; +} + +static int get_initcoil_gaveup(struct yas_driver *d) +{ + return d->initcoil_gaveup; +} + +static void set_initcoil_gaveup(struct yas_driver *d, const int initcoil_gaveup) +{ + d->initcoil_gaveup = initcoil_gaveup; +} + +static int32_t *get_calib_offset(struct yas_driver *d) +{ + return d->offset.calib_offset.v; +} + +static void set_calib_offset(struct yas_driver *d, const int32_t *offset) +{ + int i; + + if (is_calib_offset_differ(d->offset.calib_offset.v, offset)) { + for (i = 0; i < 3; i++) + d->offset.calib_offset.v[i] = offset[i]; + } +} + +#ifdef YAS_MAG_MANUAL_OFFSET +static int32_t *get_manual_offset(struct yas_driver *d) +{ + return d->manual_offset.v; +} + +static void set_manual_offset(struct yas_driver *d, const int32_t *offset) +{ + int i; + + for (i = 0; i < 3; i++) + d->manual_offset.v[i] = offset[i]; +} +#endif + +static int32_t *get_static_matrix(struct yas_driver *d) +{ + return d->static_matrix.matrix; +} + +static void set_static_matrix(struct yas_driver *d, const int32_t *matrix) +{ + int i; + + for (i = 0; i < 9; i++) + d->static_matrix.matrix[i] = matrix[i]; +} + +static int32_t *get_dynamic_matrix(struct yas_driver *d) +{ + return d->dynamic_matrix.matrix; +} + +static void set_dynamic_matrix(struct yas_driver *d, const int32_t *matrix) +{ + int i; + + for (i = 0; i < 9; i++) + d->dynamic_matrix.matrix[i] = matrix[i]; +} + +static int8_t *get_offset(struct yas_driver *d) +{ + return d->offset.hard_offset; +} + +static void set_offset(struct yas_driver *d, const int8_t *offset) +{ + int i; + + if (is_offset_differ(d->offset.hard_offset, offset)) { + for (i = 0; i < 3; i++) + d->offset.hard_offset[i] = offset[i]; + } +} + +static int get_active(struct yas_driver *d) +{ + return d->active; +} + +static void set_active(struct yas_driver *d, const int active) +{ + d->active = active; +} + +static int get_position(struct yas_driver *d) +{ + return d->position; +} + +static void set_position(struct yas_driver *d, const int position) +{ + d->position = position; +} + +static int get_measure_state(struct yas_driver *d) +{ + return d->measure_state; +} + +static void set_measure_state(struct yas_driver *d, const int state) +{ + d->measure_state = state; +} + +static struct utimer *get_data_timer(struct yas_driver *d) +{ + return &d->data_timer; +} + +static struct utimer *get_initcoil_timer(struct yas_driver *d) +{ + return &d->initcoil_timer; +} + +static struct utimer *get_initcoil_giveup_timer(struct yas_driver *d) +{ + return &d->initcoil_giveup_timer; +} + +static struct utimer *get_detect_overflow_timer(struct yas_driver *d) +{ + return &d->detect_overflow_timer; +} + +static int get_delay_timer_use_data(struct yas_driver *d) +{ + return d->delay_timer_use_data; +} + +static void set_delay_timer_use_data(struct yas_driver *d, int flag) +{ + d->delay_timer_use_data = !(!flag); +} + +static int get_delay_timer_interval(struct yas_driver *d) +{ + return d->delay_timer_interval; +} + +static void set_delay_timer_interval(struct yas_driver *d, int interval) +{ + d->delay_timer_interval = interval; +} + +static int get_delay_timer_counter(struct yas_driver *d) +{ + return d->delay_timer_counter; +} + +static void set_delay_timer_counter(struct yas_driver *d, int counter) +{ + d->delay_timer_counter = counter; +} + +static int get_filter_enable(struct yas_driver *d) +{ + return d->filter_enable; +} + +static void set_filter_enable(struct yas_driver *d, int enable) +{ + if (enable) + filter_init(d); + + d->filter_enable = !(!enable); +} + +static int get_filter_len(struct yas_driver *d) +{ + return d->filter_len; +} + +static void set_filter_len(struct yas_driver *d, int len) +{ + if (len < 0) + return; + + if (len > YAS_MAG_MAX_FILTER_LEN) + return; + + d->filter_len = len; + filter_init(d); +} + +static int get_filter_noise(struct yas_driver *d, int *noise) +{ + int i; + + for (i = 0; i < 3; i++) + noise[i] = d->filter_noise[i]; + + return 0; +} + +static void set_filter_noise(struct yas_driver *d, int *noise) +{ + int i; + + if (noise == NULL) + return; + + for (i = 0; i < 3; i++) { + if (noise[i] < 0) + noise[i] = 0; + + d->filter_noise[i] = noise[i]; + } + filter_init(d); +} + +static int get_filter_thresh(struct yas_driver *d) +{ + return d->filter_thresh; +} + +static void set_filter_thresh(struct yas_driver *d, int threshold) +{ + if (threshold < 0) + return; + + d->filter_thresh = threshold; + filter_init(d); +} + +static int32_t *get_previous_mag(struct yas_driver *d) +{ + return d->prev_mag; +} + +static void set_previous_mag(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_mag[i] = data[i]; +} + +static int32_t *get_previous_xy1y2(struct yas_driver *d) +{ + return d->prev_xy1y2; +} + +static void set_previous_xy1y2(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_xy1y2[i] = data[i]; +} + +static int32_t *get_previous_mag_w_offset(struct yas_driver *d) +{ + return d->prev_mag_w_offset; +} + +static void set_previous_mag_w_offset(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_mag_w_offset[i] = data[i]; +} + +static int16_t get_previous_temperature(struct yas_driver *d) +{ + return d->prev_temperature; +} + +static void set_previous_temperature(struct yas_driver *d, int16_t temperature) +{ + d->prev_temperature = temperature; +} + +static int init_coil(struct yas_driver *d) +{ + int rt; + + YLOGD(("init_coil IN\n")); + + utimer_update(get_initcoil_timer(d)); + if (!get_initcoil_gaveup(d)) { + utimer_update(get_initcoil_giveup_timer(d)); + if (utimer_is_timeout(get_initcoil_giveup_timer(d))) { + utimer_clear_timeout(get_initcoil_giveup_timer(d)); + set_initcoil_gaveup(d, TRUE); + } + } + if (utimer_is_timeout(get_initcoil_timer(d)) && + !get_initcoil_gaveup(d)) { + utimer_clear_timeout(get_initcoil_timer(d)); + YLOGI(("init_coil!\n")); + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + return rt; + } + if (get_overflow(d) || !is_valid_offset(get_offset(d))) + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + else + set_measure_state(d, YAS_MAG_STATE_NORMAL); + } + + YLOGD(("init_coil OUT\n")); + + return 0; +} + +static int measure_offset(struct yas_driver *d) +{ + int8_t offset[3]; + int32_t moffset[3]; + int rt, result = 0, i; + + YLOGI(("measure_offset IN\n")); + rt = yas_cdrv_measure_and_set_offset(offset); + if (rt < 0) { + YLOGE(("yas_cdrv_measure_offset failed[%d]\n", rt)); + return rt; + } + + YLOGI(("offset[%d][%d][%d]\n", offset[0], offset[1], offset[2])); + + for (i = 0; i < 3; i++) + moffset[i] = get_calib_offset(d)[i]; + + if (is_offset_differ(get_offset(d), offset)) { + if (is_valid_offset(get_offset(d)) + && is_valid_calib_offset(get_calib_offset(d))) { + yas_cdrv_recalc_calib_offset(get_calib_offset(d), + moffset, + get_offset(d), offset); + result |= YAS_REPORT_CALIB_OFFSET_CHANGED; + } + } + result |= YAS_REPORT_HARD_OFFSET_CHANGED; + + set_offset(d, offset); + if (is_valid_calib_offset(moffset)) + set_calib_offset(d, moffset); + + set_measure_state(d, YAS_MAG_STATE_NORMAL); + + YLOGI(("measure_offset OUT\n")); + + return result; +} + +static int +measure_msensor_normal(struct yas_driver *d, int32_t *magnetic, + int32_t *mag_w_offset, int32_t * xy1y2, + int16_t *temperature) +{ + int rt = 0, result, i; + int32_t tmp[3]; + + YLOGD(("measure_msensor_normal IN\n")); + + result = 0; + rt = yas_cdrv_measure(mag_w_offset, tmp, temperature); + if (rt < 0) { + YLOGE(("yas_cdrv_measure failed[%d]\n", rt)); + return rt; + } + for (i = 0; i < 3; i++) + xy1y2[i] = tmp[i]; + +#ifdef YAS_MAG_MANUAL_OFFSET + for (i = 0; i < 3; i++) + mag_w_offset[i] -= get_manual_offset(d)[i]; +#endif + if (rt > 0) { + utimer_update(get_detect_overflow_timer(d)); + set_overflow(d, TRUE); + if (utimer_is_timeout(get_detect_overflow_timer(d))) { + utimer_clear_timeout(get_detect_overflow_timer(d)); + result |= YAS_REPORT_OVERFLOW_OCCURED; + } + if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + + } else { + utimer_clear(get_detect_overflow_timer(d)); + set_overflow(d, FALSE); + if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) { + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + } + } + + for (i = 0; i < 3; i++) { + tmp[i] + = (get_static_matrix(d)[i * 3 + 0] / 10 * + (mag_w_offset[0] / 10)) / 100 + + (get_static_matrix(d)[i * 3 + 1] / 10 * + (mag_w_offset[1] / 10)) / 100 + + (get_static_matrix(d)[i * 3 + 2] / 10 * + (mag_w_offset[2] / 10)) / 100; + } + for (i = 0; i < 3; i++) + magnetic[i] = mag_w_offset[i] = tmp[i]; + if (is_valid_calib_offset(get_calib_offset(d))) { + for (i = 0; i < 3; i++) + magnetic[i] -= get_calib_offset(d)[i]; + } + for (i = 0; i < 3; i++) { + tmp[i] + = (get_dynamic_matrix(d)[i * 3 + 0] / 10 * + (magnetic[0] / 10)) / 100 + + (get_dynamic_matrix(d)[i * 3 + 1] / 10 * + (magnetic[1] / 10)) / 100 + (get_dynamic_matrix(d)[i * 3 + + 2] / + 10 * (magnetic[2] / 10)) / + 100; + } + for (i = 0; i < 3; i++) + magnetic[i] = tmp[i]; + + if (get_filter_enable(d)) + filter_filter(d, magnetic, magnetic); + + YLOGD(("measure_msensor_normal OUT\n")); + + return result; +} + +static int +measure_msensor(struct yas_driver *d, int32_t *magnetic, + int32_t *mag_w_offset, int32_t *xy1y2, int16_t *temperature) +{ + int result, i; + + YLOGD(("measure_msensor IN\n")); + + for (i = 0; i < 3; i++) { + magnetic[i] = get_previous_mag(d)[i]; + mag_w_offset[i] = get_previous_mag_w_offset(d)[i]; + xy1y2[i] = get_previous_xy1y2(d)[i]; + *temperature = get_previous_temperature(d); + } + + result = 0; + switch (get_measure_state(d)) { + case YAS_MAG_STATE_INIT_COIL: + result = init_coil(d); + break; + case YAS_MAG_STATE_MEASURE_OFFSET: + result = measure_offset(d); + break; + case YAS_MAG_STATE_NORMAL: + result = 0; + break; + default: + result = -1; + break; + } + + if (result < 0) + return result; + + if (!(result & YAS_REPORT_OVERFLOW_OCCURED)) + result |= + measure_msensor_normal(d, magnetic, mag_w_offset, xy1y2, + temperature); + set_previous_mag(d, magnetic); + set_previous_xy1y2(d, xy1y2); + set_previous_mag_w_offset(d, mag_w_offset); + set_previous_temperature(d, *temperature); + + YLOGD(("measure_msensor OUT\n")); + + return result; +} + +static int +measure(struct yas_driver *d, int32_t *magnetic, int32_t *mag_w_offset, + int32_t *xy1y2, int16_t *temperature, uint32_t *time_delay) +{ + int result; + int counter; + uint32_t total = 0; + + YLOGD(("measure IN\n")); + + utimer_update(get_data_timer(d)); + result = measure_msensor(d, magnetic, mag_w_offset, xy1y2, + temperature); + if (result < 0) + return result; + + counter = get_delay_timer_counter(d); + total = utimer_get_total_time(get_data_timer(d)); + if (utimer_get_delay(get_data_timer(d)) > 0) + counter -= total / utimer_get_delay(get_data_timer(d)); + else + counter = 0; + + if (utimer_is_timeout(get_data_timer(d))) { + utimer_clear_timeout(get_data_timer(d)); + + if (get_delay_timer_use_data(d)) { + result |= YAS_REPORT_DATA; + if (counter <= 0) + result |= YAS_REPORT_CALIB; + } else { + result |= YAS_REPORT_CALIB; + if (counter <= 0) + result |= YAS_REPORT_DATA; + } + } + + if (counter <= 0) + set_delay_timer_counter(d, get_delay_timer_interval(d)); + else + set_delay_timer_counter(d, counter); + + *time_delay = utimer_sleep_time(get_data_timer(d)); + + YLOGD(("measure OUT [%d]\n", result)); + + return result; +} + +static int resume(struct yas_driver *d) +{ + int32_t zero[] = { 0, 0, 0 }; + struct yas_machdep_func func; + int rt; + + YLOGI(("resume IN\n")); + + func.device_open = d->callback.device_open; + func.device_close = d->callback.device_close; + func.device_write = d->callback.device_write; + func.device_read = d->callback.device_read; + func.msleep = d->callback.msleep; + + rt = + yas_cdrv_init(YAS_TRANSFORMATION[get_position(d)], &func); + + if (rt < 0) { + YLOGE(("yas_cdrv_init failed[%d]\n", rt)); + return rt; + } + + utimer_clear(get_data_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_detect_overflow_timer(d)); + + set_previous_mag(d, zero); + set_previous_xy1y2(d, zero); + set_previous_mag_w_offset(d, zero); + set_previous_temperature(d, 0); + set_overflow(d, FALSE); + set_initcoil_gaveup(d, FALSE); + + filter_init(d); + + if (is_valid_offset(d->offset.hard_offset)) { + yas_cdrv_set_offset(d->offset.hard_offset); + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_NORMAL); + } else { + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + } + + YLOGI(("resume OUT\n")); + return 0; +} + +static int suspend(struct yas_driver *d) +{ + YLOGI(("suspend IN\n")); + + (void)d; + yas_cdrv_term(); + + YLOGI(("suspend OUT\n")); + return 0; +} + +static int check_interval(int ms) +{ + int index = -1; + + if (ms <= supported_data_interval[0]) + ms = supported_data_interval[0]; + for (index = 0; index < NELEMS(supported_data_interval); index++) { + if (ms == supported_data_interval[index]) + goto done; + else if (ms < supported_data_interval[index]) { + if (index != 0) + index -= 1; + goto done; + } + } +done: + return index; +} + +static int yas_get_delay_nolock(struct yas_driver *d, int *ms) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (get_delay_timer_use_data(d)) + *ms = utimer_get_delay(get_data_timer(d)); + else { + *ms = + utimer_get_delay(get_data_timer(d)) * + get_delay_timer_interval(d); + } + return YAS_NO_ERROR; +} + +static int yas_set_delay_nolock(struct yas_driver *d, int ms) +{ + int index; + uint32_t delay_data, delay_calib; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + index = check_interval(ms); + if (index < 0) + return YAS_ERROR_ARG; + delay_data = supported_data_interval[index]; + delay_calib = supported_calib_interval[index]; + set_delay_timer_use_data(d, delay_data < delay_calib); + if (delay_data < delay_calib) { + set_delay_timer_interval(d, delay_calib / delay_data); + set_delay_timer_counter(d, delay_calib / delay_data); + utimer_set_delay(get_data_timer(d), + supported_data_interval[index]); + } else { + set_delay_timer_interval(d, delay_data / delay_calib); + set_delay_timer_counter(d, delay_data / delay_calib); + utimer_set_delay(get_data_timer(d), + supported_calib_interval[index]); + } + + return YAS_NO_ERROR; +} + +static int +yas_get_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *offset = d->offset; + return YAS_NO_ERROR; +} + +static int +yas_set_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) +{ + int32_t zero[] = { 0, 0, 0 }; + int rt; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + d->offset = *offset; + return YAS_NO_ERROR; + } + + if (!is_valid_offset(offset->hard_offset) + || is_offset_differ(offset->hard_offset, d->offset.hard_offset)) { + filter_init(d); + + utimer_clear(get_data_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_detect_overflow_timer(d)); + + set_previous_mag(d, zero); + set_previous_xy1y2(d, zero); + set_previous_mag_w_offset(d, zero); + set_previous_temperature(d, 0); + set_overflow(d, FALSE); + set_initcoil_gaveup(d, FALSE); + } + d->offset = *offset; + + if (is_valid_offset(d->offset.hard_offset)) + yas_cdrv_set_offset(d->offset.hard_offset); + else { + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + } + + return YAS_NO_ERROR; +} + +#ifdef YAS_MAG_MANUAL_OFFSET + +static int +yas_get_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + *offset = d->manual_offset; + + return YAS_NO_ERROR; +} + +static int +yas_set_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_manual_offset(d, offset->v); + + return YAS_NO_ERROR; +} + +#endif + +static int +yas_get_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + int i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + for (i = 0; i < 9; i++) + matrix->matrix[i] = get_static_matrix(d)[i]; + + return YAS_NO_ERROR; +} + +static int +yas_set_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_static_matrix(d, matrix->matrix); + + return YAS_NO_ERROR; +} + +static int +yas_get_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + int i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + for (i = 0; i < 9; i++) + matrix->matrix[i] = get_dynamic_matrix(d)[i]; + + return YAS_NO_ERROR; +} + +static int +yas_set_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_dynamic_matrix(d, matrix->matrix); + + return YAS_NO_ERROR; +} + +static int yas_get_enable_nolock(struct yas_driver *d) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + return get_active(d); +} + +static int yas_set_enable_nolock(struct yas_driver *d, int active) +{ + int rt; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + if (active) { + if (get_active(d)) + return YAS_NO_ERROR; + rt = resume(d); + if (rt < 0) + return rt; + set_active(d, TRUE); + } else { + if (!get_active(d)) + return YAS_NO_ERROR; + rt = suspend(d); + if (rt < 0) + return rt; + set_active(d, FALSE); + } + return YAS_NO_ERROR; +} + +static int +yas_get_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + filter->len = get_filter_len(d); + get_filter_noise(d, filter->noise); + filter->threshold = get_filter_thresh(d); + return YAS_NO_ERROR; +} + +static int +yas_set_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + set_filter_len(d, filter->len); + set_filter_noise(d, filter->noise); + set_filter_thresh(d, filter->threshold); + return YAS_NO_ERROR; +} + +static int yas_get_filter_enable_nolock(struct yas_driver *d) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + return get_filter_enable(d); +} + +static int yas_set_filter_enable_nolock(struct yas_driver *d, int enable) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + set_filter_enable(d, enable); + return YAS_NO_ERROR; +} + +static int yas_get_position_nolock(struct yas_driver *d, int *position) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *position = get_position(d); + return YAS_NO_ERROR; +} + +static int yas_set_position_nolock(struct yas_driver *d, int position) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (get_active(d)) + yas_cdrv_set_transformatiom_matrix(YAS_TRANSFORMATION + [position]); + set_position(d, position); + filter_init(d); + return YAS_NO_ERROR; +} + +static int +yas_read_reg_nolock(struct yas_driver *d, uint8_t addr, uint8_t * buf, int len) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + if (d->callback.device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + if (d->callback.device_read(addr, buf, len) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (!get_active(d)) { + if (d->callback.device_close() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + + return YAS_NO_ERROR; +} + +static int +yas_write_reg_nolock(struct yas_driver *d, uint8_t addr, const uint8_t * buf, + int len) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + if (d->callback.device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + if (d->callback.device_write(addr, buf, len) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (!get_active(d)) { + if (d->callback.device_close() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + + return YAS_NO_ERROR; +} + +static int +yas_measure_nolock(struct yas_driver *d, struct yas_mag_data *data, + int *time_delay_ms) +{ + uint32_t time_delay = YAS_MAG_ERROR_DELAY; + int rt, i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *time_delay_ms = YAS_MAG_ERROR_DELAY; + + if (!get_active(d)) { + for (i = 0; i < 3; i++) { + data->xyz.v[i] = get_previous_mag(d)[i]; + data->raw.v[i] = get_previous_mag_w_offset(d)[i]; + data->xy1y2.v[i] = get_previous_xy1y2(d)[i]; + } + data->temperature = get_previous_temperature(d); + return YAS_NO_ERROR; + } + + rt = measure(d, data->xyz.v, data->raw.v, data->xy1y2.v, + &data->temperature, &time_delay); + if (rt >= 0) { + *time_delay_ms = time_delay; + if (*time_delay_ms > 0) + *time_delay_ms += 1; + } + + return rt; +} + +static int yas_init_nolock(struct yas_driver *d) +{ +#ifdef YAS_MAG_MANUAL_OFFSET + int32_t zero[] = { 0, 0, 0 }; +#endif + int32_t notransform[] = { 10000, 0, 0, 0, 10000, 0, 0, 0, 10000 }; + int noise[] = { + YAS_MAG_DEFAULT_FILTER_NOISE_X, + YAS_MAG_DEFAULT_FILTER_NOISE_Y, + YAS_MAG_DEFAULT_FILTER_NOISE_Z + }; + + YLOGI(("yas_init_nolock IN\n")); + + utimer_lib_init(this_driver.callback.current_time); + utimer_init(get_data_timer(d), 50); + utimer_init(get_initcoil_timer(d), YAS_INITCOIL_INTERVAL); + utimer_init(get_initcoil_giveup_timer(d), YAS_INITCOIL_GIVEUP_INTERVAL); + utimer_init(get_detect_overflow_timer(d), YAS_DETECT_OVERFLOW_INTERVAL); + + set_delay_timer_use_data(d, 0); + set_delay_timer_interval(d, + YAS_DEFAULT_DATA_INTERVAL / + YAS_DEFAULT_CALIB_INTERVAL); + set_delay_timer_counter(d, + YAS_DEFAULT_DATA_INTERVAL / + YAS_DEFAULT_CALIB_INTERVAL); + + set_filter_enable(d, FALSE); + set_filter_len(d, YAS_MAG_DEFAULT_FILTER_LEN); + set_filter_thresh(d, YAS_MAG_DEFAULT_FILTER_THRESH); + set_filter_noise(d, noise); + filter_init(d); + set_calib_offset(d, INVALID_CALIB_OFFSET); +#ifdef YAS_MAG_MANUAL_OFFSET + set_manual_offset(d, zero); +#endif + set_static_matrix(d, notransform); + set_dynamic_matrix(d, notransform); + set_offset(d, INVALID_OFFSET); + set_active(d, FALSE); + set_position(d, 0); + + d->initialized = 1; + + YLOGI(("yas_init_nolock OUT\n")); + + return YAS_NO_ERROR; +} + +static int yas_term_nolock(struct yas_driver *d) +{ + YLOGI(("yas_term_nolock\n")); + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + if (get_active(d)) + suspend(d); + d->initialized = 0; + + YLOGI(("yas_term_nolock out\n")); + return YAS_NO_ERROR; +} + +static int yas_get_delay(void) +{ + int ms = 0, rt; + + YLOGI(("yas_get_delay\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_delay_nolock(&this_driver, &ms); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_delay[%d] OUT\n", ms)); + + return (rt < 0 ? rt : ms); +} + +static int yas_set_delay(int delay) +{ + int rt; + + YLOGI(("yas_set_delay\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_delay_nolock(&this_driver, delay); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_delay OUT\n")); + + return rt; +} + +static int yas_get_offset(struct yas_mag_offset *offset) +{ + int rt; + + YLOGI(("yas_get_offset\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_offset[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_offset(struct yas_mag_offset *offset) +{ + int rt; + + YLOGI(("yas_set_offset IN\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_offset OUT\n")); + + return rt; +} + +#ifdef YAS_MAG_MANUAL_OFFSET + +static int yas_get_manual_offset(struct yas_vector *offset) +{ + int rt; + + YLOGI(("yas_get_manual_offset\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_manual_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_manual_offset[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_manual_offset(struct yas_vector *offset) +{ + int rt; + + YLOGI(("yas_set_manual_offset IN\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_manual_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_manual_offset OUT\n")); + + return rt; +} + +#endif + +static int yas_get_static_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_get_static_matrix\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_static_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_static_matrix[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_static_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_set_static_matrix IN\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_static_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_static_matrix OUT\n")); + + return rt; +} + +static int yas_get_dynamic_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_get_dynamic_matrix\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_dynamic_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_dynamic_matrix[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_dynamic_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_set_dynamic_matrix IN\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_dynamic_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_dynamic_matrix OUT\n")); + + return rt; +} + +static int yas_get_enable(void) +{ + int rt; + + YLOGI(("yas_get_enable\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_enable_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_enable OUT[%d]\n", rt)); + + return rt; +} + +static int yas_set_enable(int enable) +{ + int rt; + + YLOGI(("yas_set_enable IN\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_enable_nolock(&this_driver, enable); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_enable OUT\n")); + + return rt; +} + +static int yas_get_filter(struct yas_mag_filter *filter) +{ + int rt; + + YLOGI(("yas_get_filter\n")); + + if (filter == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_filter_nolock(&this_driver, filter); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_filter[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_filter(struct yas_mag_filter *filter) +{ + int rt, i; + + YLOGI(("yas_set_filter IN\n")); + + if (filter == NULL + || filter->len < 0 + || YAS_MAG_MAX_FILTER_LEN < filter->len || filter->threshold < 0) { + return YAS_ERROR_ARG; + } + for (i = 0; i < 3; i++) { + if (filter->noise[i] < 0) + return YAS_ERROR_ARG; + } + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_filter_nolock(&this_driver, filter); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_filter OUT\n")); + + return rt; +} + +static int yas_get_filter_enable(void) +{ + int rt; + + YLOGI(("yas_get_filter_enable\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_filter_enable_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_filter_enable OUT[%d]\n", rt)); + + return rt; +} + +static int yas_set_filter_enable(int enable) +{ + int rt; + + YLOGI(("yas_set_filter_enable IN\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_filter_enable_nolock(&this_driver, enable); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_filter_enable OUT\n")); + + return rt; +} + +static int yas_get_position(void) +{ + int position = 0; + int rt; + + YLOGI(("yas_get_position\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_position_nolock(&this_driver, &position); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_position[%d] OUT\n", position)); + + return (rt < 0 ? rt : position); +} + +static int yas_set_position(int position) +{ + int rt; + + YLOGI(("yas_set_position\n")); + + if (position < 0 || 7 < position) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_position_nolock(&this_driver, position); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_position[%d] OUT\n", position)); + + return rt; +} + +static int yas_read_reg(uint8_t addr, uint8_t *buf, int len) +{ + int rt; + + YLOGI(("yas_read_reg\n")); + + if (buf == NULL || len <= 0) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_read_reg_nolock(&this_driver, addr, buf, len); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_read_reg[%d] OUT\n", rt)); + + return rt; +} + +static int yas_write_reg(uint8_t addr, const uint8_t *buf, int len) +{ + int rt; + + YLOGI(("yas_write_reg\n")); + + if (buf == NULL || len <= 0) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_write_reg_nolock(&this_driver, addr, buf, len); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_write_reg[%d] OUT\n", rt)); + + return rt; +} + +static int yas_measure(struct yas_mag_data *data, int *time_delay_ms) +{ + int rt; + + YLOGD(("yas_measure IN\n")); + + if (data == NULL || time_delay_ms == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_measure_nolock(&this_driver, data, time_delay_ms); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGD(("yas_measure OUT[%d]\n", rt)); + + return rt; +} + +static int yas_init(void) +{ + int rt; + + YLOGI(("yas_init\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_init_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + return rt; +} + +static int yas_term(void) +{ + int rt; + YLOGI(("yas_term\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_term_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + return rt; +} + +int yas_mag_driver_init(struct yas_mag_driver *f) +{ + if (f == NULL) + return YAS_ERROR_ARG; + if (f->callback.device_open == NULL + || f->callback.device_close == NULL + || f->callback.device_read == NULL + || f->callback.device_write == NULL + || f->callback.msleep == NULL || f->callback.current_time == NULL) + return YAS_ERROR_ARG; + + f->init = yas_init; + f->term = yas_term; + f->get_delay = yas_get_delay; + f->set_delay = yas_set_delay; + f->get_offset = yas_get_offset; + f->set_offset = yas_set_offset; +#ifdef YAS_MAG_MANUAL_OFFSET + f->get_manual_offset = yas_get_manual_offset; + f->set_manual_offset = yas_set_manual_offset; +#endif + f->get_static_matrix = yas_get_static_matrix; + f->set_static_matrix = yas_set_static_matrix; + f->get_dynamic_matrix = yas_get_dynamic_matrix; + f->set_dynamic_matrix = yas_set_dynamic_matrix; + f->get_enable = yas_get_enable; + f->set_enable = yas_set_enable; + f->get_filter = yas_get_filter; + f->set_filter = yas_set_filter; + f->get_filter_enable = yas_get_filter_enable; + f->set_filter_enable = yas_set_filter_enable; + f->get_position = yas_get_position; + f->set_position = yas_set_position; + f->read_reg = yas_read_reg; + f->write_reg = yas_write_reg; + f->measure = yas_measure; + + if ((f->callback.lock == NULL && f->callback.unlock != NULL) + || (f->callback.lock != NULL && f->callback.unlock == NULL)) { + this_driver.callback.lock = NULL; + this_driver.callback.unlock = NULL; + } else { + this_driver.callback.lock = f->callback.lock; + this_driver.callback.unlock = f->callback.unlock; + } + this_driver.callback.device_open = f->callback.device_open; + this_driver.callback.device_close = f->callback.device_close; + this_driver.callback.device_write = f->callback.device_write; + this_driver.callback.device_read = f->callback.device_read; + this_driver.callback.msleep = f->callback.msleep; + this_driver.callback.current_time = f->callback.current_time; + yas_term(); + + return YAS_NO_ERROR; +} diff --git a/drivers/sensor/yas_mag_driver.c b/drivers/sensor/yas_mag_driver.c new file mode 100644 index 0000000..805ec32 --- /dev/null +++ b/drivers/sensor/yas_mag_driver.c @@ -0,0 +1,27 @@ +/* + * Copyright (c) 2010 Yamaha Corporation + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +#include <linux/sensor/yas.h> + +#if CONFIG_SENSORS_YAS532 +#include "yas_mag_driver-yas532.c" +#else +#include "yas_mag_driver-none.c" +#endif diff --git a/drivers/sensor/yas_mag_kernel_driver.c b/drivers/sensor/yas_mag_kernel_driver.c new file mode 100644 index 0000000..0106958 --- /dev/null +++ b/drivers/sensor/yas_mag_kernel_driver.c @@ -0,0 +1,2192 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include <linux/atomic.h> +#include <linux/types.h> +#include <linux/i2c.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/list.h> +#include <linux/module.h> +#include <linux/init.h> +#include <linux/input.h> +#include <linux/mutex.h> +#include <linux/delay.h> +#include <linux/poll.h> +#include <linux/slab.h> +#include <linux/uaccess.h> +#include <linux/workqueue.h> + +#define __LINUX_KERNEL_DRIVER__ +#include <linux/sensor/yas.h> +#include <linux/sensor/sensors_core.h> +#include "yas_mag_driver.c" + +#define SYSFS_PCBTEST +#ifdef SYSFS_PCBTEST +#include "yas_pcb_test.h" +#include "yas_pcb_test.c" +#endif + +#define GEOMAGNETIC_I2C_DEVICE_NAME "yas532" +#define GEOMAGNETIC_INPUT_NAME "geomagnetic" +#define GEOMAGNETIC_INPUT_RAW_NAME "geomagnetic_raw" +#undef GEOMAGNETIC_PLATFORM_API + +#define ABS_STATUS (ABS_BRAKE) +#define ABS_WAKE (ABS_MISC) + +#define ABS_RAW_DISTORTION (ABS_THROTTLE) +#define ABS_RAW_THRESHOLD (ABS_RUDDER) +#define ABS_RAW_SHAPE (ABS_WHEEL) +#define ABS_RAW_MODE (ABS_HAT0X) +#define ABS_RAW_REPORT (ABS_GAS) + +struct geomagnetic_data { + struct input_dev *input_data; + struct input_dev *input_raw; + struct delayed_work work; + struct semaphore driver_lock; + struct semaphore multi_lock; + atomic_t last_data[3]; + atomic_t last_status; + atomic_t enable; + int filter_enable; + int filter_len; + int32_t filter_noise[3]; + int32_t filter_threshold; + int delay; + int32_t threshold; + int32_t distortion[3]; + int32_t shape; + int32_t ellipsoid_mode; + struct yas_mag_offset driver_offset; +#if DEBUG + int suspend; +#endif +#ifdef YAS_MAG_MANUAL_OFFSET + struct yas_vector manual_offset; +#endif + struct yas_matrix static_matrix; + struct yas_matrix dynamic_matrix; +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct list_head devfile_list; + struct list_head raw_devfile_list; +#endif + struct device *magnetic_sensor_device; + struct mag_platform_data *mag_pdata; + uint8_t dev_id; + int noise_test_init; +}; + +static struct i2c_client *this_client; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + +#include <linux/miscdevice.h> +#define MAX_COUNT (64) +#define SENSOR_NAME "geomagnetic" +#define SENSOR_RAW_NAME "geomagnetic_raw" + +struct sensor_device { + struct list_head list; + struct mutex lock; + wait_queue_head_t waitq; + struct input_event events[MAX_COUNT]; + int head, num_event; +}; + +static void get_time_stamp(struct timeval *tv) +{ + struct timespec ts; + ktime_get_ts(&ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; +} + +static void make_event(struct input_event *ev, int type, int code, int value) +{ + struct timeval tv; + get_time_stamp(&tv); + ev->type = type; + ev->code = code; + ev->value = value; + ev->time = tv; +} + +static void +make_event_w_time(struct input_event *ev, int type, int code, int value, + struct timeval *tv) +{ + ev->type = type; + ev->code = code; + ev->value = value; + ev->time = *tv; +} + +static void sensor_enq(struct sensor_device *kdev, struct input_event *ev) +{ + int idx; + + idx = kdev->head + kdev->num_event; + if (MAX_COUNT <= idx) + idx -= MAX_COUNT; + kdev->events[idx] = *ev; + kdev->num_event++; + if (MAX_COUNT < kdev->num_event) { + kdev->num_event = MAX_COUNT; + kdev->head++; + if (MAX_COUNT <= kdev->head) + kdev->head -= MAX_COUNT; + } +} + +static int sensor_deq(struct sensor_device *kdev, struct input_event *ev) +{ + if (kdev->num_event == 0) + return 0; + + *ev = kdev->events[kdev->head]; + kdev->num_event--; + kdev->head++; + if (MAX_COUNT <= kdev->head) + kdev->head -= MAX_COUNT; + return 1; +} + +static void +sensor_event(struct list_head *devlist, struct input_event *ev, int num) +{ + struct sensor_device *kdev; + int i; + + list_for_each_entry(kdev, devlist, list) { + mutex_lock(&kdev->lock); + for (i = 0; i < num; i++) + sensor_enq(kdev, &ev[i]); + mutex_unlock(&kdev->lock); + wake_up_interruptible(&kdev->waitq); + } +} + +static ssize_t +sensor_write(struct file *f, const char __user *buf, size_t count, + loff_t *pos) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + struct sensor_device *kdev; + struct input_event ev[MAX_COUNT]; + int num, i; + + if (count < sizeof(struct input_event)) + return -EINVAL; + num = count / sizeof(struct input_event); + if (MAX_COUNT < num) + num = MAX_COUNT; + + if (copy_from_user(ev, buf, num * sizeof(struct input_event))) + return -EFAULT; + + list_for_each_entry(kdev, &data->devfile_list, list) { + mutex_lock(&kdev->lock); + for (i = 0; i < num; i++) + sensor_enq(kdev, &ev[i]); + mutex_unlock(&kdev->lock); + wake_up_interruptible(&kdev->waitq); + } + + return count; +} + +static ssize_t +sensor_read(struct file *f, char __user *buf, size_t count, loff_t *pos) +{ + struct sensor_device *kdev = f->private_data; + int rt, num; + struct input_event ev[MAX_COUNT]; + + if (count < sizeof(struct input_event)) + return -EINVAL; + + rt = wait_event_interruptible(kdev->waitq, kdev->num_event != 0); + if (rt) + return rt; + + mutex_lock(&kdev->lock); + for (num = 0; num < count / sizeof(struct input_event); num++) { + if (!sensor_deq(kdev, &ev[num])) + break; + } + mutex_unlock(&kdev->lock); + + if (copy_to_user(buf, ev, num * sizeof(struct input_event))) + return -EFAULT; + + return num * sizeof(struct input_event); +} + +static unsigned int sensor_poll(struct file *f, struct poll_table_struct *wait) +{ + struct sensor_device *kdev = f->private_data; + + poll_wait(f, &kdev->waitq, wait); + if (kdev->num_event != 0) + return POLLIN | POLLRDNORM; + + return 0; +} + +static int sensor_open(struct inode *inode, struct file *f) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + struct sensor_device *kdev; + + kdev = kzalloc(sizeof(struct sensor_device), GFP_KERNEL); + if (!kdev) + return -ENOMEM; + + mutex_init(&kdev->lock); + init_waitqueue_head(&kdev->waitq); + f->private_data = kdev; + kdev->head = 0; + kdev->num_event = 0; + list_add(&kdev->list, &data->devfile_list); + + return 0; +} + +static int sensor_release(struct inode *inode, struct file *f) +{ + struct sensor_device *kdev = f->private_data; + + list_del(&kdev->list); + kfree(kdev); + + return 0; +} + +static int sensor_raw_open(struct inode *inode, struct file *f) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + struct sensor_device *kdev; + + kdev = kzalloc(sizeof(struct sensor_device), GFP_KERNEL); + if (!kdev) + return -ENOMEM; + + mutex_init(&kdev->lock); + init_waitqueue_head(&kdev->waitq); + f->private_data = kdev; + kdev->head = 0; + kdev->num_event = 0; + list_add(&kdev->list, &data->raw_devfile_list); + + return 0; +} + +const struct file_operations sensor_fops = { + .owner = THIS_MODULE, + .open = sensor_open, + .release = sensor_release, + .write = sensor_write, + .read = sensor_read, + .poll = sensor_poll, +}; + +static struct miscdevice sensor_devfile = { + .name = SENSOR_NAME, + .fops = &sensor_fops, + .minor = MISC_DYNAMIC_MINOR, +}; + +const struct file_operations sensor_raw_fops = { + .owner = THIS_MODULE, + .open = sensor_raw_open, + .release = sensor_release, + .write = sensor_write, + .read = sensor_read, + .poll = sensor_poll, +}; + +static struct miscdevice sensor_raw_devfile = { + .name = SENSOR_RAW_NAME, + .fops = &sensor_raw_fops, + .minor = MISC_DYNAMIC_MINOR, +}; + +#endif + +static int geomagnetic_i2c_open(void) +{ + return 0; +} + +static int geomagnetic_i2c_close(void) +{ + return 0; +} + +#if YAS_MAG_DRIVER == YAS_MAG_DRIVER_YAS529 +static int geomagnetic_i2c_write(const uint8_t *buf, int len) +{ + if (i2c_master_send(this_client, buf, len) < 0) + return -1; +#if DEBUG + YLOGD(("[W] [%02x]\n", buf[0])); +#endif + + return 0; +} + +static int geomagnetic_i2c_read(uint8_t *buf, int len) +{ + if (i2c_master_recv(this_client, buf, len) < 0) + return -1; + return 0; +} + +#else + +static int geomagnetic_i2c_write(uint8_t addr, const uint8_t *buf, int len) +{ + uint8_t tmp[16]; + + if (sizeof(tmp) - 1 < len) + return -1; + + tmp[0] = addr; + memcpy(&tmp[1], buf, len); + + if (i2c_master_send(this_client, tmp, len + 1) < 0) + return -1; +#if DEBUG + YLOGD(("[W] addr[%02x] [%02x]\n", addr, buf[0])); +#endif + + return 0; +} + +static int geomagnetic_i2c_read(uint8_t addr, uint8_t *buf, int len) +{ + struct i2c_msg msg[2]; + int err; + + msg[0].addr = this_client->addr; + msg[0].flags = 0; + msg[0].len = 1; + msg[0].buf = &addr; + msg[1].addr = this_client->addr; + msg[1].flags = I2C_M_RD; + msg[1].len = len; + msg[1].buf = buf; + + err = i2c_transfer(this_client->adapter, msg, 2); + if (err != 2) { + dev_err(&this_client->dev, + "i2c_transfer() read error: slave_addr=%02x, reg_addr=%02x, err=%d\n", + this_client->addr, addr, err); + return err; + } + + return 0; +} + +#endif + +static int geomagnetic_lock(void) +{ + struct geomagnetic_data *data = NULL; + int rt; + + if (this_client == NULL) + return -1; + + data = i2c_get_clientdata(this_client); + rt = down_interruptible(&data->driver_lock); + if (rt < 0) + up(&data->driver_lock); + return rt; +} + +static int geomagnetic_unlock(void) +{ + struct geomagnetic_data *data = NULL; + + if (this_client == NULL) + return -1; + + data = i2c_get_clientdata(this_client); + up(&data->driver_lock); + return 0; +} + +static void geomagnetic_msleep(int ms) +{ + usleep_range(ms * 999, ms * 1000); +} + +static void geomagnetic_current_time(int32_t *sec, int32_t *msec) +{ + struct timeval tv; + + do_gettimeofday(&tv); + + *sec = tv.tv_sec; + *msec = tv.tv_usec / 1000; +} + +static struct yas_mag_driver hwdep_driver = { + .callback = { + .lock = geomagnetic_lock, + .unlock = geomagnetic_unlock, + .device_open = geomagnetic_i2c_open, + .device_close = geomagnetic_i2c_close, + .device_read = geomagnetic_i2c_read, + .device_write = geomagnetic_i2c_write, + .msleep = geomagnetic_msleep, + .current_time = geomagnetic_current_time, + }, +}; + +static int geomagnetic_multi_lock(void) +{ + struct geomagnetic_data *data = NULL; + int rt; + + if (this_client == NULL) + return -1; + + data = i2c_get_clientdata(this_client); + rt = down_interruptible(&data->multi_lock); + if (rt < 0) + up(&data->multi_lock); + return rt; +} + +static int geomagnetic_multi_unlock(void) +{ + struct geomagnetic_data *data = NULL; + + if (this_client == NULL) + return -1; + + data = i2c_get_clientdata(this_client); + up(&data->multi_lock); + return 0; +} + +static int geomagnetic_enable(struct geomagnetic_data *data) +{ + if (!atomic_cmpxchg(&data->enable, 0, 1)) + schedule_delayed_work(&data->work, 0); + + return 0; +} + +static int geomagnetic_disable(struct geomagnetic_data *data) +{ + if (atomic_cmpxchg(&data->enable, 1, 0)) + cancel_delayed_work_sync(&data->work); + + return 0; +} + +/* Sysfs interface */ +static ssize_t +geomagnetic_delay_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int delay; + + geomagnetic_multi_lock(); + + delay = data->delay; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", delay); +} + +static ssize_t +geomagnetic_delay_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + if (hwdep_driver.set_delay == NULL) + return -ENOTTY; + + geomagnetic_multi_lock(); + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + if (hwdep_driver.set_delay(value) == 0) + data->delay = value; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + + return sprintf(buf, "%d\n", atomic_read(&data->enable)); +} + +static ssize_t +geomagnetic_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + value = !(!value); + + if (hwdep_driver.set_enable == NULL) + return -ENOTTY; + + if (geomagnetic_multi_lock() < 0) + return count; + + if (hwdep_driver.set_enable(value) == 0) { + if (value) + geomagnetic_enable(data); + else + geomagnetic_disable(data); + } + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_filter_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int filter_enable; + + geomagnetic_multi_lock(); + + filter_enable = data->filter_enable; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", filter_enable); +} + +static ssize_t +geomagnetic_filter_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + unsigned long value; + int error; + + if (hwdep_driver.set_filter_enable == NULL) + return -ENOTTY; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + if (geomagnetic_multi_lock() < 0) + return count; + + if (hwdep_driver.set_filter_enable(value) == 0) + data->filter_enable = !!value; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_filter_len_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int filter_len; + + geomagnetic_multi_lock(); + + filter_len = data->filter_len; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", filter_len); +} + +static ssize_t +geomagnetic_filter_len_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + struct yas_mag_filter filter; + unsigned long value; + int error; + + if (hwdep_driver.get_filter == NULL || hwdep_driver.set_filter == NULL) + return -ENOTTY; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + if (geomagnetic_multi_lock() < 0) + return count; + + hwdep_driver.get_filter(&filter); + filter.len = value; + if (hwdep_driver.set_filter(&filter) == 0) + data->filter_len = value; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_filter_noise_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int rt; + + geomagnetic_multi_lock(); + + rt = sprintf(buf, "%d %d %d\n", + data->filter_noise[0], + data->filter_noise[1], data->filter_noise[2]); + + geomagnetic_multi_unlock(); + + return rt; +} + +static ssize_t +geomagnetic_filter_noise_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct yas_mag_filter filter; + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int32_t filter_noise[3]; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d", + &filter_noise[0], &filter_noise[1], &filter_noise[2]); + hwdep_driver.get_filter(&filter); + memcpy(filter.noise, filter_noise, sizeof(filter.noise)); + if (hwdep_driver.set_filter(&filter) == 0) + memcpy(data->filter_noise, filter_noise, + sizeof(data->filter_noise)); + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_filter_threshold_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int32_t filter_threshold; + + geomagnetic_multi_lock(); + + filter_threshold = data->filter_threshold; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", filter_threshold); +} + +static ssize_t +geomagnetic_filter_threshold_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + struct yas_mag_filter filter; + unsigned long value; + int error; + + if (hwdep_driver.get_filter == NULL || hwdep_driver.set_filter == NULL) + return -ENOTTY; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + if (geomagnetic_multi_lock() < 0) + return count; + + hwdep_driver.get_filter(&filter); + filter.threshold = value; + if (hwdep_driver.set_filter(&filter) == 0) + data->filter_threshold = value; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_position_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + if (hwdep_driver.get_position == NULL) + return -ENOTTY; + return sprintf(buf, "%d\n", hwdep_driver.get_position()); +} + +static ssize_t +geomagnetic_position_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + if (hwdep_driver.set_position == NULL) + return -ENOTTY; + hwdep_driver.set_position(value); + + return count; +} + +static ssize_t +geomagnetic_data_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int rt; + + rt = sprintf(buf, "%d %d %d\n", + atomic_read(&data->last_data[0]), + atomic_read(&data->last_data[1]), + atomic_read(&data->last_data[2])); + + return rt; +} + +static ssize_t +geomagnetic_status_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + int rt; + + rt = sprintf(buf, "%d\n", atomic_read(&data->last_status)); + + return rt; +} + +static ssize_t +geomagnetic_status_store(struct device *dev, struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + static int16_t cnt = 1; +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev; +#endif + int accuracy = 0; + int code = 0; + int value = 0; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d", &accuracy); + if (0 <= accuracy && accuracy <= 3) + atomic_set(&data->last_status, accuracy); + code |= YAS_REPORT_CALIB_OFFSET_CHANGED; + value = (cnt++ << 16) | (code); + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + mkev(&ev, EV_ABS, ABS_RAW_REPORT, value); + sensor_event(&data->raw_devfile_list, &ev, 1); +#else + input_report_abs(data->input_raw, ABS_RAW_REPORT, value); + input_sync(data->input_raw); +#endif + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_wake_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_data); + static int16_t cnt = 1; +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_WAKE, cnt++); + sensor_event(&data->devfile_list, ev, 1); +#else + input_report_abs(data->input_data, ABS_WAKE, cnt++); + input_sync(data->input_data); +#endif + + return count; +} + +#if DEBUG + +static int geomagnetic_suspend(struct i2c_client *client, pm_message_t mesg); +static int geomagnetic_resume(struct i2c_client *client); + +static ssize_t +geomagnetic_debug_suspend_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input); + + return sprintf(buf, "%d\n", data->suspend); +} + +static ssize_t +geomagnetic_debug_suspend_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long suspend; + int error; + + error = strict_strtoul(buf, 10, &suspend); + if (unlikely(error)) + return error; + + if (suspend) { + pm_message_t msg; + memset(&msg, 0, sizeof(msg)); + geomagnetic_suspend(this_client, msg); + } else + geomagnetic_resume(this_client); + + return count; +} + +#endif /* DEBUG */ + +static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_delay_show, geomagnetic_delay_store); +static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_enable_show, geomagnetic_enable_store); +static DEVICE_ATTR(filter_enable, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_filter_enable_show, + geomagnetic_filter_enable_store); +static DEVICE_ATTR(filter_len, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_filter_len_show, geomagnetic_filter_len_store); +static DEVICE_ATTR(filter_threshold, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_filter_threshold_show, + geomagnetic_filter_threshold_store); +static DEVICE_ATTR(filter_noise, S_IRUGO | S_IWUSR | S_IWGRP, + geomagnetic_filter_noise_show, + geomagnetic_filter_noise_store); +static DEVICE_ATTR(data, S_IRUGO, geomagnetic_data_show, NULL); +static DEVICE_ATTR(status, S_IRUGO|S_IWUSR|S_IWGRP, geomagnetic_status_show, + geomagnetic_status_store); +static DEVICE_ATTR(wake, S_IWUSR | S_IWGRP, NULL, geomagnetic_wake_store); +static DEVICE_ATTR(position, S_IRUGO | S_IWUSR, + geomagnetic_position_show, geomagnetic_position_store); +#if DEBUG +static DEVICE_ATTR(debug_suspend, S_IRUGO | S_IWUSR, + geomagnetic_debug_suspend_show, + geomagnetic_debug_suspend_store); +#endif /* DEBUG */ + +static struct attribute *geomagnetic_attributes[] = { + &dev_attr_delay.attr, + &dev_attr_enable.attr, + &dev_attr_filter_enable.attr, + &dev_attr_filter_len.attr, + &dev_attr_filter_threshold.attr, + &dev_attr_filter_noise.attr, + &dev_attr_data.attr, + &dev_attr_status.attr, + &dev_attr_wake.attr, + &dev_attr_position.attr, +#if DEBUG + &dev_attr_debug_suspend.attr, +#endif /* DEBUG */ + NULL +}; + +static struct attribute_group geomagnetic_attribute_group = { + .attrs = geomagnetic_attributes +}; + +static ssize_t +geomagnetic_raw_threshold_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int threshold; + + geomagnetic_multi_lock(); + + threshold = data->threshold; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", threshold); +} + +static ssize_t +geomagnetic_raw_threshold_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + geomagnetic_multi_lock(); + + if (0 <= value && value <= 2) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_RAW_THRESHOLD, value); + sensor_event(&data->raw_devfile_list, ev, 1); +#endif + data->threshold = value; +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(data->input_raw, ABS_RAW_THRESHOLD, value); + input_sync(data->input_raw); +#endif + } + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_raw_distortion_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int rt; + + geomagnetic_multi_lock(); + + rt = sprintf(buf, "%d %d %d\n", + data->distortion[0], + data->distortion[1], data->distortion[2]); + + geomagnetic_multi_unlock(); + + return rt; +} + +static ssize_t +geomagnetic_raw_distortion_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int32_t distortion[3]; + static int32_t val = 1; + int i; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d", &distortion[0], &distortion[1], &distortion[2]); + if (distortion[0] > 0 && distortion[1] > 0 && distortion[2] > 0) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_RAW_DISTORTION, val++); + sensor_event(&data->raw_devfile_list, ev, 1); +#endif + for (i = 0; i < 3; i++) + data->distortion[i] = distortion[i]; +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(data->input_raw, ABS_RAW_DISTORTION, val++); + input_sync(data->input_raw); +#endif + } + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_raw_shape_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int shape; + + geomagnetic_multi_lock(); + + shape = data->shape; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", shape); +} + +static ssize_t +geomagnetic_raw_shape_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + + geomagnetic_multi_lock(); + + if (0 == value || value == 1) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_RAW_SHAPE, value); + sensor_event(&data->raw_devfile_list, ev, 1); +#endif + data->shape = value; +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(data->input_raw, ABS_RAW_SHAPE, value); + input_sync(data->input_raw); +#endif + } + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_raw_offsets_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_mag_offset offset; + int accuracy; + + geomagnetic_multi_lock(); + + offset = data->driver_offset; + accuracy = atomic_read(&data->last_status); + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d %d %d %d %d %d %d\n", + offset.hard_offset[0], + offset.hard_offset[1], + offset.hard_offset[2], + offset.calib_offset.v[0], + offset.calib_offset.v[1], + offset.calib_offset.v[2], accuracy); +} + +static ssize_t +geomagnetic_raw_offsets_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_mag_offset offset; + int32_t hard_offset[3]; + int i, accuracy; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d %d %d %d %d", + &hard_offset[0], + &hard_offset[1], + &hard_offset[2], + &offset.calib_offset.v[0], + &offset.calib_offset.v[1], &offset.calib_offset.v[2], &accuracy); + if (0 <= accuracy && accuracy <= 3) { + for (i = 0; i < 3; i++) + offset.hard_offset[i] = (int8_t) hard_offset[i]; + + if (hwdep_driver.set_offset(&offset) == 0) { + atomic_set(&data->last_status, accuracy); + data->driver_offset = offset; + } + } + + geomagnetic_multi_unlock(); + + return count; +} + +#ifdef YAS_MAG_MANUAL_OFFSET +static ssize_t +geomagnetic_raw_manual_offsets_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_vector offset; + + geomagnetic_multi_lock(); + + offset = data->manual_offset; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d %d %d\n", offset.v[0], offset.v[1], + offset.v[2]); +} + +static ssize_t +geomagnetic_raw_manual_offsets_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_vector offset; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d", &offset.v[0], &offset.v[1], &offset.v[2]); + if (hwdep_driver.set_manual_offset(&offset) == 0) + data->manual_offset = offset; + + geomagnetic_multi_unlock(); + + return count; +} +#endif + +static ssize_t +geomagnetic_raw_static_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_matrix matrix; + + geomagnetic_multi_lock(); + + matrix = data->static_matrix; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d %d %d %d %d %d %d %d %d\n", + matrix.matrix[0], matrix.matrix[1], matrix.matrix[2], + matrix.matrix[3], matrix.matrix[4], matrix.matrix[5], + matrix.matrix[6], matrix.matrix[7], matrix.matrix[8]); +} + +static ssize_t +geomagnetic_raw_static_matrix_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_matrix matrix; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d %d %d %d %d %d %d", + &matrix.matrix[0], &matrix.matrix[1], &matrix.matrix[2], + &matrix.matrix[3], &matrix.matrix[4], &matrix.matrix[5], + &matrix.matrix[6], &matrix.matrix[7], &matrix.matrix[8]); + if (hwdep_driver.set_static_matrix(&matrix) == 0) + data->static_matrix = matrix; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_raw_dynamic_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_matrix matrix; + + geomagnetic_multi_lock(); + + matrix = data->dynamic_matrix; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d %d %d %d %d %d %d %d %d\n", + matrix.matrix[0], matrix.matrix[1], matrix.matrix[2], + matrix.matrix[3], matrix.matrix[4], matrix.matrix[5], + matrix.matrix[6], matrix.matrix[7], matrix.matrix[8]); +} + +static ssize_t +geomagnetic_raw_dynamic_matrix_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + struct yas_matrix matrix; + + geomagnetic_multi_lock(); + + sscanf(buf, "%d %d %d %d %d %d %d %d %d", + &matrix.matrix[0], &matrix.matrix[1], &matrix.matrix[2], + &matrix.matrix[3], &matrix.matrix[4], &matrix.matrix[5], + &matrix.matrix[6], &matrix.matrix[7], &matrix.matrix[8]); + if (hwdep_driver.set_dynamic_matrix(&matrix) == 0) + data->dynamic_matrix = matrix; + + geomagnetic_multi_unlock(); + + return count; +} + +static ssize_t +geomagnetic_raw_ellipsoid_mode_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + int ellipsoid_mode; + + geomagnetic_multi_lock(); + + ellipsoid_mode = data->ellipsoid_mode; + + geomagnetic_multi_unlock(); + + return sprintf(buf, "%d\n", ellipsoid_mode); +} + +static ssize_t +geomagnetic_raw_ellipsoid_mode_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_raw = to_input_dev(dev); + struct geomagnetic_data *data = input_get_drvdata(input_raw); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + value = !(!value); +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; +#endif + + geomagnetic_multi_lock(); + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + make_event(ev, EV_ABS, ABS_RAW_MODE, value); + sensor_event(&data->raw_devfile_list, ev, 1); +#endif + data->ellipsoid_mode = value; +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(data->input_raw, ABS_RAW_MODE, value); + input_sync(data->input_raw); +#endif + + geomagnetic_multi_unlock(); + + return count; +} + +#ifdef SYSFS_PCBTEST + +static int +pcbtest_i2c_write(uint8_t slave, uint8_t addr, const uint8_t *buf, int len) +{ + return geomagnetic_i2c_write(addr, buf, len); +} + +static int +pcbtest_i2c_read(uint8_t slave, uint8_t addr, uint8_t *buf, int len) +{ + return geomagnetic_i2c_read(addr, buf, len); +} + +static struct yas_pcb_test pcbtest = { + .callback = { + .power_on = NULL, + .power_off = NULL, + .i2c_open = geomagnetic_i2c_open, + .i2c_close = geomagnetic_i2c_close, + .i2c_read = pcbtest_i2c_read, + .i2c_write = pcbtest_i2c_write, + .msleep = geomagnetic_msleep, + .read_intpin = NULL, + }, +}; + + +static ssize_t +geomagnetic_raw_self_test_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + int id, x, y1, y2, dir, sx, sy, ohx, ohy, ohz; + int err1, err2, err3, err4, err5, err6, err7; + + if (data->noise_test_init) + pcbtest.power_off(); + err1 = pcbtest.power_on_and_device_check(&id); + err3 = pcbtest.initialization(); + err4 = + pcbtest.offset_control_measurement_and_set_offset_register( + &x, &y1, &y2); + err5 = pcbtest.direction_measurement(&dir); + err6 = + pcbtest.sensitivity_measurement_of_magnetic_sensor_by_test_coil( + &sx, &sy); + err7 = pcbtest.magnetic_field_level_check(&ohx, &ohy, &ohz); + err2 = pcbtest.power_off(); + data->noise_test_init = 0; + if (unlikely(id != 0x2)) + err1 = -1; + if (unlikely(x < -30 || x > 30)) + err4 = -1; + if (unlikely(y1 < -30 || y1 > 30)) + err4 = -1; + if (unlikely(y2 < -30 || y2 > 30)) + err4 = -1; + if (unlikely(sx < 17 || sy < 22)) + err6 = -1; + if (unlikely(ohx < -600 || ohx > 600)) + err7 = -1; + if (unlikely(ohy < -600 || ohy > 600)) + err7 = -1; + if (unlikely(ohz < -600 || ohz > 600)) + err7 = -1; + + pr_info("%s\n" + "Test1 - err = %d, id = %d\n" + "Test3 - err = %d\n" + "Test4 - err = %d, offset = %d,%d,%d\n" + "Test5 - err = %d, direction = %d\n" + "Test6 - err = %d, sensitivity = %d,%d\n" + "Test7 - err = %d, offset = %d,%d,%d\n" + "Test2 - err = %d\n", __func__, + err1, id, err3, err4, x, y1, y2, err5, dir, err6, sx, sy, + err7, ohx, ohy, ohz, err2); + + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + err1, id, err3, err4, x, y1, y2, err5, dir, err6, sx, + sy, err7, ohx, ohy, ohz, err2); +} + +static ssize_t +geomagnetic_raw_self_test_noise_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + int id, x, y1, y2, dir, hx0, hy0, hz0; + int err8; +#if CONFIG_MACH_KONA_SENSOR + pcbtest.power_on_and_device_check(&id); + pcbtest.initialization(); + pcbtest.offset_control_measurement_and_set_offset_register( + &x, &y1, &y2); +#else + if (!data->noise_test_init) { + pcbtest.power_on_and_device_check(&id); + pcbtest.initialization(); + pcbtest.offset_control_measurement_and_set_offset_register( + &x, &y1, &y2); + } +#endif + pcbtest.direction_measurement(&dir); + err8 = pcbtest.noise_level_check(&hx0, &hy0, &hz0); + if (err8 < 0) { + pr_err("%s: err8=%d\n", __func__, err8); + hx0 = 0; + hy0 = 0; + hz0 = 0; + } + usleep_range(3000, 3100); + data->noise_test_init = 1; + pr_debug("%s: %d, %d, %d\n", __func__, hx0, hy0, hz0); + return snprintf(buf, PAGE_SIZE, "%d,%d,%d\n", hx0, hy0, hz0); +} + +static DEVICE_ATTR(self_test, S_IRUSR, geomagnetic_raw_self_test_show, NULL); +static DEVICE_ATTR(self_test_noise, S_IRUSR, + geomagnetic_raw_self_test_noise_show, NULL); + +#endif + + +static DEVICE_ATTR(threshold, S_IRUGO | S_IWUSR, + geomagnetic_raw_threshold_show, + geomagnetic_raw_threshold_store); +static DEVICE_ATTR(distortion, S_IRUGO | S_IWUSR, + geomagnetic_raw_distortion_show, + geomagnetic_raw_distortion_store); +static DEVICE_ATTR(shape, S_IRUGO | S_IWUSR, geomagnetic_raw_shape_show, + geomagnetic_raw_shape_store); +static DEVICE_ATTR(offsets, S_IRUGO | S_IWUSR, geomagnetic_raw_offsets_show, + geomagnetic_raw_offsets_store); +#ifdef YAS_MAG_MANUAL_OFFSET +static DEVICE_ATTR(manual_offsets, S_IRUGO | S_IWUSR, + geomagnetic_raw_manual_offsets_show, + geomagnetic_raw_manual_offsets_store); +#endif +static DEVICE_ATTR(static_matrix, S_IRUGO | S_IWUSR, + geomagnetic_raw_static_matrix_show, + geomagnetic_raw_static_matrix_store); +static DEVICE_ATTR(dynamic_matrix, S_IRUGO | S_IWUSR, + geomagnetic_raw_dynamic_matrix_show, + geomagnetic_raw_dynamic_matrix_store); +static DEVICE_ATTR(ellipsoid_mode, S_IRUGO | S_IWUSR, + geomagnetic_raw_ellipsoid_mode_show, + geomagnetic_raw_ellipsoid_mode_store); +static struct attribute *geomagnetic_raw_attributes[] = { +#ifdef SYSFS_PCBTEST + &dev_attr_self_test.attr, + &dev_attr_self_test_noise.attr, +#endif + &dev_attr_threshold.attr, + &dev_attr_distortion.attr, + &dev_attr_shape.attr, + &dev_attr_offsets.attr, +#ifdef YAS_MAG_MANUAL_OFFSET + &dev_attr_manual_offsets.attr, +#endif + &dev_attr_static_matrix.attr, + &dev_attr_dynamic_matrix.attr, + &dev_attr_ellipsoid_mode.attr, + NULL +}; + +static struct attribute_group geomagnetic_raw_attribute_group = { + .attrs = geomagnetic_raw_attributes +}; +static struct device_attribute dev_attr_magnetic_sensor_selftest = + __ATTR(selftest, S_IRUSR | S_IRGRP, + geomagnetic_raw_self_test_show, NULL); + +static struct device_attribute dev_attr_magnetic_sensor_raw_data = + __ATTR(raw_data, S_IRUSR | S_IRGRP, + geomagnetic_raw_self_test_noise_show, NULL); + +static ssize_t magnetic_vendor_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%s\n", "YAMAHA"); +} +static struct device_attribute dev_attr_magnetic_sensor_vendor = + __ATTR(vendor, S_IRUSR | S_IRGRP, magnetic_vendor_show, NULL); + +static ssize_t magnetic_name_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + int ret; + if (data->dev_id == YAS_YAS532_DEVICE_ID) + ret = sprintf(buf, "%s\n", "YAS532"); + else + ret = sprintf(buf, "%s\n", "YAS530C"); + return ret; +} +static struct device_attribute dev_attr_magnetic_sensor_name = + __ATTR(name, S_IRUSR | S_IRGRP, magnetic_name_show, NULL); + +static struct device_attribute *magnetic_sensor_attrs[] = { + &dev_attr_magnetic_sensor_selftest, + &dev_attr_magnetic_sensor_raw_data, + &dev_attr_magnetic_sensor_vendor, + &dev_attr_magnetic_sensor_name, + NULL, +}; + + +/* Interface Functions for Lower Layer */ +#ifdef YAS_MAG_MANUAL_OFFSET +void geomagnetic_manual_offset(void) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + struct yas_vector offset; + if (data->mag_pdata->chg_status == CABLE_TYPE_NONE) { + offset = data->mag_pdata->full_offset; + if (hwdep_driver.set_manual_offset(&offset) == 0) + data->manual_offset = offset; + } else if (data->mag_pdata->chg_status == CABLE_TYPE_USB) { + offset = data->mag_pdata->usb_offset; + if (hwdep_driver.set_manual_offset(&offset) == 0) + data->manual_offset = offset; + } else if (data->mag_pdata->chg_status == CABLE_TYPE_AC) { + offset = data->mag_pdata->ta_offset; + if (hwdep_driver.set_manual_offset(&offset) == 0) + data->manual_offset = offset; + } else { + offset = data->mag_pdata->full_offset; + if (hwdep_driver.set_manual_offset(&offset) == 0) + data->manual_offset = offset; + } + data->mag_pdata->offset_enable = 0; +} +#endif + +static int geomagnetic_work(struct yas_mag_data *magdata) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + uint32_t time_delay_ms = 100; + static int cnt; + int rt, i, accuracy; +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[5]; + struct timeval tv; +#endif + +#ifdef YAS_MAG_MANUAL_OFFSET + if (data->mag_pdata) { + if (data->mag_pdata->offset_enable) + geomagnetic_manual_offset(); + } +#endif + + if (hwdep_driver.measure == NULL || hwdep_driver.get_offset == NULL) + return time_delay_ms; + + rt = hwdep_driver.measure(magdata, &time_delay_ms); + if (rt < 0) + YLOGE(("measure failed[%d]\n", rt)); + YLOGD(("xy1y2 [%d][%d][%d] raw[%d][%d][%d]\n", + magdata->xy1y2.v[0], magdata->xy1y2.v[1], magdata->xy1y2.v[2], + magdata->xyz.v[0], magdata->xyz.v[1], magdata->xyz.v[2])); + + if (rt >= 0) { + accuracy = atomic_read(&data->last_status); + + if ((rt & YAS_REPORT_OVERFLOW_OCCURED) + || (rt & YAS_REPORT_HARD_OFFSET_CHANGED) + || (rt & YAS_REPORT_CALIB_OFFSET_CHANGED)) { + static uint16_t count = 1; + int code = 0; + int value = 0; + + hwdep_driver.get_offset(&data->driver_offset); + if (rt & YAS_REPORT_OVERFLOW_OCCURED) { + atomic_set(&data->last_status, 0); + accuracy = 0; + } + + /* report event */ + code |= (rt & YAS_REPORT_OVERFLOW_OCCURED); + code |= (rt & YAS_REPORT_HARD_OFFSET_CHANGED); + code |= (rt & YAS_REPORT_CALIB_OFFSET_CHANGED); + value = (count++ << 16) | (code); +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + make_event(ev, EV_ABS, ABS_RAW_REPORT, value); + sensor_event(&data->raw_devfile_list, ev, 1); +#else + input_report_abs(data->input_raw, ABS_RAW_REPORT, + value); + input_sync(data->input_raw); +#endif + } + + if (rt & YAS_REPORT_DATA) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + get_time_stamp(&tv); + make_event_w_time(&ev[0], EV_ABS, ABS_X, + magdata->xyz.v[0], &tv); + make_event_w_time(&ev[1], EV_ABS, ABS_Y, + magdata->xyz.v[1], &tv); + make_event_w_time(&ev[2], EV_ABS, ABS_Z, + magdata->xyz.v[2], &tv); + make_event_w_time(&ev[3], EV_ABS, ABS_STATUS, accuracy, + &tv); + make_event_w_time(&ev[4], EV_SYN, 0, 0, &tv); + sensor_event(&data->devfile_list, ev, 5); +#else + /* report magnetic data in [nT] */ + input_report_abs(data->input_data, ABS_X, + magdata->xyz.v[0]); + input_report_abs(data->input_data, ABS_Y, + magdata->xyz.v[1]); + input_report_abs(data->input_data, ABS_Z, + magdata->xyz.v[2]); + if (atomic_read(&data->last_data[0]) == + magdata->xyz.v[0] + && atomic_read(&data->last_data[1]) == + magdata->xyz.v[1] + && atomic_read(&data->last_data[2]) == + magdata->xyz.v[2]) { + input_report_abs(data->input_data, ABS_RUDDER, + cnt++); + } + input_report_abs(data->input_data, ABS_STATUS, + accuracy); + input_sync(data->input_data); +#endif + + for (i = 0; i < 3; i++) + atomic_set(&data->last_data[i], + magdata->xyz.v[i]); + } + + if (rt & YAS_REPORT_CALIB) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + get_time_stamp(&tv); + make_event_w_time(&ev[0], EV_ABS, ABS_X, + magdata->raw.v[0], &tv); + make_event_w_time(&ev[1], EV_ABS, ABS_Y, + magdata->raw.v[1], &tv); + make_event_w_time(&ev[2], EV_ABS, ABS_Z, + magdata->raw.v[2], &tv); + make_event_w_time(&ev[3], EV_SYN, 0, 0, &tv); + sensor_event(&data->raw_devfile_list, ev, 4); +#else + /* report raw magnetic data */ + input_report_abs(data->input_raw, ABS_X, + magdata->raw.v[0]); + input_report_abs(data->input_raw, ABS_Y, + magdata->raw.v[1]); + input_report_abs(data->input_raw, ABS_Z, + magdata->raw.v[2]); + input_sync(data->input_raw); +#endif + } + } else { + time_delay_ms = 100; + } + + return time_delay_ms; + +} + +static void geomagnetic_input_work_func(struct work_struct *work) +{ + struct geomagnetic_data *data = + container_of((struct delayed_work *)work, + struct geomagnetic_data, work); + uint32_t time_delay_ms; + struct yas_mag_data magdata; + + time_delay_ms = geomagnetic_work(&magdata); + + if (time_delay_ms > 60) + schedule_delayed_work(&data->work, + msecs_to_jiffies(time_delay_ms)); + else { + if (time_delay_ms > 0) + usleep_range(time_delay_ms * 1000, + time_delay_ms * 1100); + schedule_delayed_work(&data->work, 0); + } +} + +static int geomagnetic_suspend(struct i2c_client *client, pm_message_t mesg) +{ + struct geomagnetic_data *data = i2c_get_clientdata(client); + + if (atomic_read(&data->enable)) + cancel_delayed_work_sync(&data->work); +#if DEBUG + data->suspend = 1; +#endif + + return 0; +} + +static int geomagnetic_resume(struct i2c_client *client) +{ + struct geomagnetic_data *data = i2c_get_clientdata(client); + + if (atomic_read(&data->enable)) + schedule_delayed_work(&data->work, 0); +#if DEBUG + data->suspend = 0; +#endif + + return 0; +} + +static int +geomagnetic_probe(struct i2c_client *client, const struct i2c_device_id *id) +{ + struct geomagnetic_data *data = NULL; + struct input_dev *input_data = NULL, *input_raw = NULL; + int rt, sysfs_created = 0, sysfs_raw_created = 0; + int data_registered = 0, raw_registered = 0, i; + struct yas_mag_filter filter; + struct mag_platform_data *pdata; + + pr_err("%s, is called\n", __func__); + + i2c_set_clientdata(client, NULL); + data = kzalloc(sizeof(struct geomagnetic_data), GFP_KERNEL); + if (data == NULL) { + rt = -ENOMEM; + goto err; + } + + pdata = (struct mag_platform_data *) client->dev.platform_data; + data->mag_pdata = pdata; + + if (pdata) { + if (pdata->power_on) + pdata->power_on(true); + } + data->threshold = YAS_DEFAULT_MAGCALIB_THRESHOLD; + for (i = 0; i < 3; i++) + data->distortion[i] = YAS_DEFAULT_MAGCALIB_DISTORTION; + data->shape = 0; + atomic_set(&data->enable, 0); + for (i = 0; i < 3; i++) + atomic_set(&data->last_data[i], 0); + atomic_set(&data->last_status, 0); + INIT_DELAYED_WORK(&data->work, geomagnetic_input_work_func); + sema_init(&data->driver_lock, 1); + sema_init(&data->multi_lock, 1); + + input_data = input_allocate_device(); + if (input_data == NULL) { + rt = -ENOMEM; + YLOGE(("mag Failed to allocate input_data device\n")); + goto err; + } + + input_data->name = GEOMAGNETIC_INPUT_NAME; + input_data->id.bustype = BUS_I2C; + set_bit(EV_ABS, input_data->evbit); + input_set_abs_params(input_data, ABS_X, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_Y, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_Z, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_RUDDER, 0x80000000, 0x7fffffff, 0, + 0); + input_set_abs_params(input_data, ABS_STATUS, 0, 3, 0, 0); + input_set_abs_params(input_data, ABS_WAKE, 0x80000000, 0x7fffffff, 0, + 0); + input_data->dev.parent = &client->dev; + + rt = input_register_device(input_data); + if (rt) { + YLOGE(("mag: Unable to reg input_data %s\n", input_data->name)); + goto err; + } + data_registered = 1; + + rt = sysfs_create_group(&input_data->dev.kobj, + &geomagnetic_attribute_group); + if (rt) { + YLOGE(("mag sysfs_create failed[%s]\n", input_data->name)); + goto err; + } + sysfs_created = 1; + + input_raw = input_allocate_device(); + if (input_raw == NULL) { + rt = -ENOMEM; + YLOGE(("mag Failed to alloc input_raw dev\n")); + goto err; + } + + input_raw->name = GEOMAGNETIC_INPUT_RAW_NAME; + input_raw->id.bustype = BUS_I2C; + set_bit(EV_ABS, input_raw->evbit); + input_set_abs_params(input_raw, ABS_X, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_raw, ABS_Y, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_raw, ABS_Z, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_raw, ABS_RAW_DISTORTION, 0, 0x7fffffff, 0, + 0); + input_set_abs_params(input_raw, ABS_RAW_THRESHOLD, 0, 2, 0, 0); + input_set_abs_params(input_raw, ABS_RAW_SHAPE, 0, 1, 0, 0); + input_set_abs_params(input_raw, ABS_RAW_MODE, 0, 1, 0, 0); + input_set_abs_params(input_raw, ABS_RAW_REPORT, 0x80000000, 0x7fffffff, + 0, 0); + input_raw->dev.parent = &client->dev; + + rt = input_register_device(input_raw); + if (rt) { + YLOGE(("mag: Unable to reg input_raw dev\n")); + goto err; + } + raw_registered = 1; + + rt = sysfs_create_group(&input_raw->dev.kobj, + &geomagnetic_raw_attribute_group); + if (rt) { + YLOGE(("geomagnetic_probe: sysfs_create_group failed[%s]\n", + input_data->name)); + goto err; + } + sysfs_raw_created = 1; + + this_client = client; + data->input_raw = input_raw; + data->input_data = input_data; + input_set_drvdata(input_data, data); + input_set_drvdata(input_raw, data); + i2c_set_clientdata(client, data); + rt = yas_mag_driver_init(&hwdep_driver); + if (rt < 0) { + YLOGE(("yas_mag_driver_init failed[%d]\n", rt)); + goto err; + } + if (hwdep_driver.init != NULL) { + rt = hwdep_driver.init(); + if (rt < 0) { + YLOGE(("hwdep_driver.init() failed[%d]\n", rt)); + goto err; + } + } + if (hwdep_driver.set_position != NULL) { + if (pdata) { + if (pdata->orientation) { + pr_info("%s: set from board file.\n", __func__); + if (hwdep_driver. + set_position(pdata->orientation + - YAS532_POSITION_OFFSET) < 0) { + pr_err("set_position failed %d\n", rt); + goto err; + } + } else { + pr_info("%s: set from defconfig.\n", __func__); + if (hwdep_driver. + set_position( + CONFIG_INPUT_YAS_MAGNETOMETER_POSITION) + < 0) { + pr_err("set_position failed %d\n", rt); + goto err; + } + } + } else { + if (hwdep_driver. + set_position( + CONFIG_INPUT_YAS_MAGNETOMETER_POSITION) + < 0) { + pr_err("set_position() failed[%d]\n", rt); + goto err; + } + } + pr_info("%s: yas magnetic position is %d\n", __func__, + hwdep_driver.get_position()); + } + if (hwdep_driver.get_offset != NULL) { + if (hwdep_driver.get_offset(&data->driver_offset) < 0) { + YLOGE(("hwdep_driver get_driver_state failed\n")); + goto err; + } + } + if (hwdep_driver.get_delay != NULL) + data->delay = hwdep_driver.get_delay(); + + if (hwdep_driver.set_filter_enable != NULL) { + /* default to enable */ + if (hwdep_driver.set_filter_enable(1) == 0) + data->filter_enable = 1; + } + if (hwdep_driver.get_filter != NULL) { + if (hwdep_driver.get_filter(&filter) < 0) { + YLOGE(("hwdep_driver get_filter failed\n")); + goto err; + } + data->filter_len = filter.len; + for (i = 0; i < 3; i++) + data->filter_noise[i] = filter.noise[i]; + data->filter_threshold = filter.threshold; + } + if (hwdep_driver.get_static_matrix != NULL) + hwdep_driver.get_static_matrix(&data->static_matrix); + if (hwdep_driver.get_dynamic_matrix != NULL) + hwdep_driver.get_dynamic_matrix(&data->dynamic_matrix); +#ifdef SYSFS_PCBTEST + rt = yas_pcb_test_init(&pcbtest); + if (rt < 0) { + YLOGE(("yas_pcb_test_init failed[%d]\n", rt)); + goto err; + } +#endif +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + INIT_LIST_HEAD(&data->devfile_list); + INIT_LIST_HEAD(&data->raw_devfile_list); + if (misc_register(&sensor_devfile) < 0) + goto err; + if (misc_register(&sensor_raw_devfile) < 0) { + misc_deregister(&sensor_devfile); + goto err; + } +#endif + + rt = + geomagnetic_i2c_read(YAS_REGADDR_DEVICE_ID, &data->dev_id, 1); + if (rt) { + pr_err("%s: cound not read device id(%d).\n", + __func__, rt); + goto err; + } else + pr_info("%s: yamaha magnetic sensor id = %x\n", + __func__, data->dev_id); + + rt = sensors_register(data->magnetic_sensor_device, + NULL, magnetic_sensor_attrs, + "magnetic_sensor"); + if (rt) { + pr_err("%s: cound not register magnetic sensor device(%d).\n", + __func__, rt); + goto out_sensor_register_failed; + } + + return 0; + +out_sensor_register_failed: +err: + if (data != NULL) { + if (input_raw != NULL) { + if (sysfs_raw_created) + sysfs_remove_group(&input_raw->dev.kobj, + &geomagnetic_raw_attribute_group); + + if (raw_registered) + input_unregister_device(input_raw); + else + input_free_device(input_raw); + } + if (input_data != NULL) { + if (sysfs_created) + sysfs_remove_group(&input_data->dev.kobj, + &geomagnetic_attribute_group); + if (data_registered) + input_unregister_device(input_data); + else + input_free_device(input_data); + } + if (pdata) { + if (pdata->power_on) + pdata->power_on(false); + } + kfree(data); + } + + return rt; +} + +static int geomagnetic_remove(struct i2c_client *client) +{ + struct geomagnetic_data *data = i2c_get_clientdata(client); + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + misc_deregister(&sensor_devfile); + misc_deregister(&sensor_raw_devfile); +#endif + if (data != NULL) { + geomagnetic_disable(data); + if (hwdep_driver.term != NULL) + hwdep_driver.term(); + + input_unregister_device(data->input_raw); + sysfs_remove_group(&data->input_data->dev.kobj, + &geomagnetic_attribute_group); + sysfs_remove_group(&data->input_raw->dev.kobj, + &geomagnetic_raw_attribute_group); + input_unregister_device(data->input_data); + kfree(data); + } + + return 0; +} + +static void geomagnetic_shutdown(struct i2c_client *client) +{ + struct geomagnetic_data *data = i2c_get_clientdata(client); + + if (data != NULL) { + geomagnetic_disable(data); + if (data->mag_pdata) { + if (data->mag_pdata->power_on) + data->mag_pdata->power_on(false); + } + } +} + +/* I2C Device Driver */ +static struct i2c_device_id geomagnetic_idtable[] = { + {GEOMAGNETIC_I2C_DEVICE_NAME, 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, geomagnetic_idtable); + +static struct i2c_driver geomagnetic_i2c_driver = { + .driver = { + .name = GEOMAGNETIC_I2C_DEVICE_NAME, + .owner = THIS_MODULE, + }, + + .id_table = geomagnetic_idtable, + .probe = geomagnetic_probe, + .remove = geomagnetic_remove, + .suspend = geomagnetic_suspend, + .resume = geomagnetic_resume, + .shutdown = geomagnetic_shutdown, +}; + +static int __init geomagnetic_init(void) +{ + return i2c_add_driver(&geomagnetic_i2c_driver); +} + +static void __exit geomagnetic_term(void) +{ + i2c_del_driver(&geomagnetic_i2c_driver); +} + +#ifdef GEOMAGNETIC_PLATFORM_API +static int geomagnetic_api_enable(int enable) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + int rt; + + if (geomagnetic_multi_lock() < 0) + return -1; + enable = !!enable; + rt = hwdep_driver.set_enable(enable); + if (rt == 0) { + atomic_set(&data->enable, enable); + if (enable) + rt = hwdep_driver.set_delay(20); + } + + geomagnetic_multi_unlock(); + + return rt; +} + +int geomagnetic_api_resume(void) +{ + return geomagnetic_api_enable(1); +} +EXPORT_SYMBOL(geomagnetic_api_resume); + +int geomagnetic_api_suspend(void) +{ + return geomagnetic_api_enable(0); +} +EXPORT_SYMBOL(geomagnetic_api_suspend); + +int geomagnetic_api_read(int *xyz, int *raw, int *xy1y2, int *accuracy) +{ + struct geomagnetic_data *data = i2c_get_clientdata(this_client); + struct yas_mag_data magdata; + int i; + + geomagnetic_work(&magdata); + if (xyz != NULL) { + for (i = 0; i < 3; i++) + xyz[i] = magdata.xyz.v[i]; + } + if (raw != NULL) { + for (i = 0; i < 3; i++) + raw[i] = magdata.raw.v[i]; + } + if (xy1y2 != NULL) { + for (i = 0; i < 3; i++) + xy1y2[i] = magdata.xy1y2.v[i]; + } + if (accuracy != NULL) + *accuracy = atomic_read(&data->last_status); + + return 0; +} +EXPORT_SYMBOL(geomagnetic_api_read); +#endif + +module_init(geomagnetic_init); +module_exit(geomagnetic_term); + +MODULE_AUTHOR("Yamaha Corporation"); +#if YAS_MAG_DRIVER == YAS_MAG_DRIVER_YAS529 +MODULE_DESCRIPTION("YAS529 Geomagnetic Sensor Driver"); +#elif YAS_MAG_DRIVER == YAS_MAG_DRIVER_YAS530 +MODULE_DESCRIPTION("YAS530 Geomagnetic Sensor Driver"); +#elif CONFIG_SENSORS_YAS532 +MODULE_DESCRIPTION("YAS532 Geomagnetic Sensor Driver"); +#endif +MODULE_LICENSE("GPL"); +MODULE_VERSION("4.4.702a"); diff --git a/drivers/sensor/yas_ori_kernel_driver.c b/drivers/sensor/yas_ori_kernel_driver.c new file mode 100644 index 0000000..20003aa --- /dev/null +++ b/drivers/sensor/yas_ori_kernel_driver.c @@ -0,0 +1,695 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include <linux/delay.h> +#include <linux/errno.h> +#include <linux/i2c.h> +#include <linux/init.h> +#include <linux/input.h> +#include <linux/kernel.h> +#include <linux/list.h> +#include <linux/module.h> +#include <linux/mutex.h> +#include <linux/platform_device.h> +#include <linux/poll.h> +#include <linux/slab.h> +#include <linux/types.h> +#include <linux/uaccess.h> +#include <linux/version.h> +#include <linux/workqueue.h> + +#define __LINUX_KERNEL_DRIVER__ +#include <linux/sensor/yas.h> + +#define SENSOR_NAME "orientation" +#define SENSOR_DEFAULT_DELAY (200) /* 200 ms */ +#define SENSOR_MAX_DELAY (2000) /* 2000 ms */ +#define ABS_STATUS (ABS_BRAKE) +#define ABS_WAKE (ABS_MISC) +#define ABS_CONTROL_REPORT (ABS_THROTTLE) + +static int suspend(void); +static int resume(void); + +struct sensor_data { + struct mutex mutex; + int enabled; + int delay; +#if DEBUG + int suspend; +#endif +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct list_head devfile_list; +#endif +}; + +static struct platform_device *sensor_pdev; +static struct input_dev *this_data; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE +#include <linux/miscdevice.h> +#define MAX_COUNT (64) + +struct sensor_device { + struct list_head list; + struct mutex lock; + wait_queue_head_t waitq; + struct input_event events[MAX_COUNT]; + int head, num_event; +}; + +static void get_time_stamp(struct timeval *tv) +{ + struct timespec ts; + ktime_get_ts(&ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; +} + +static void make_event(struct input_event *ev, int type, int code, int value) +{ + struct timeval tv; + get_time_stamp(&tv); + ev->type = type; + ev->code = code; + ev->value = value; + ev->time = tv; +} + +static void +make_event_w_time(struct input_event *ev, int type, int code, int value, + struct timeval *tv) +{ + ev->type = type; + ev->code = code; + ev->value = value; + ev->time = *tv; +} + +static void sensor_enq(struct sensor_device *kdev, struct input_event *ev) +{ + int idx; + + idx = kdev->head + kdev->num_event; + if (MAX_COUNT <= idx) + idx -= MAX_COUNT; + kdev->events[idx] = *ev; + kdev->num_event++; + if (MAX_COUNT < kdev->num_event) { + kdev->num_event = MAX_COUNT; + kdev->head++; + if (MAX_COUNT <= kdev->head) + kdev->head -= MAX_COUNT; + } +} + +static int sensor_deq(struct sensor_device *kdev, struct input_event *ev) +{ + if (kdev->num_event == 0) + return 0; + + *ev = kdev->events[kdev->head]; + kdev->num_event--; + kdev->head++; + if (MAX_COUNT <= kdev->head) + kdev->head -= MAX_COUNT; + return 1; +} + +static void +sensor_event(struct list_head *devlist, struct input_event *ev, int num) +{ + struct sensor_device *kdev; + int i; + + list_for_each_entry(kdev, devlist, list) { + mutex_lock(&kdev->lock); + for (i = 0; i < num; i++) + sensor_enq(kdev, &ev[i]); + + mutex_unlock(&kdev->lock); + wake_up_interruptible(&kdev->waitq); + } +} + +static ssize_t +sensor_write(struct file *f, const char __user *buf, size_t count, + loff_t *pos) +{ + struct sensor_data *data = input_get_drvdata(this_data); + struct sensor_device *kdev; + struct input_event ev[MAX_COUNT]; + int num, i; + + if (count < sizeof(struct input_event)) + return -EINVAL; + + num = count / sizeof(struct input_event); + if (MAX_COUNT < num) + num = MAX_COUNT; + + if (copy_from_user(ev, buf, num * sizeof(struct input_event))) + return -EFAULT; + + list_for_each_entry(kdev, &data->devfile_list, list) { + mutex_lock(&kdev->lock); + for (i = 0; i < num; i++) + sensor_enq(kdev, &ev[i]); + mutex_unlock(&kdev->lock); + wake_up_interruptible(&kdev->waitq); + } + + return count; +} + +static ssize_t +sensor_read(struct file *f, char __user *buf, size_t count, loff_t *pos) +{ + struct sensor_device *kdev = f->private_data; + int rt, num; + struct input_event ev[MAX_COUNT]; + + if (count < sizeof(struct input_event)) + return -EINVAL; + + rt = wait_event_interruptible(kdev->waitq, kdev->num_event != 0); + if (rt) + return rt; + + mutex_lock(&kdev->lock); + for (num = 0; num < count / sizeof(struct input_event); num++) { + if (!sensor_deq(kdev, &ev[num])) + break; + } + mutex_unlock(&kdev->lock); + + if (copy_to_user(buf, ev, num * sizeof(struct input_event))) + return -EFAULT; + + return num * sizeof(struct input_event); +} + +static unsigned int sensor_poll(struct file *f, struct poll_table_struct *wait) +{ + struct sensor_device *kdev = f->private_data; + + poll_wait(f, &kdev->waitq, wait); + if (kdev->num_event != 0) + return POLLIN | POLLRDNORM; + + return 0; +} + +static int sensor_open(struct inode *inode, struct file *f) +{ + struct sensor_data *data = input_get_drvdata(this_data); + struct sensor_device *kdev; + + kdev = kzalloc(sizeof(struct sensor_device), GFP_KERNEL); + if (!kdev) + return -ENOMEM; + + mutex_init(&kdev->lock); + init_waitqueue_head(&kdev->waitq); + f->private_data = kdev; + kdev->head = 0; + kdev->num_event = 0; + list_add(&kdev->list, &data->devfile_list); + + return 0; +} + +static int sensor_release(struct inode *inode, struct file *f) +{ + struct sensor_device *kdev = f->private_data; + + list_del(&kdev->list); + kfree(kdev); + + return 0; +} + +const struct file_operations sensor_fops = { + .owner = THIS_MODULE, + .open = sensor_open, + .release = sensor_release, + .write = sensor_write, + .read = sensor_read, + .poll = sensor_poll, +}; + +static struct miscdevice sensor_devfile = { + .name = SENSOR_NAME, + .fops = &sensor_fops, + .minor = MISC_DYNAMIC_MINOR, +}; + +#endif + +static int suspend(void) +{ + /* implement suspend of the sensor */ + YLOGD(("%s: suspend\n", SENSOR_NAME)); + + return 0; +} + +static int resume(void) +{ + /* implement resume of the sensor */ + YLOGD(("%s: resume\n", SENSOR_NAME)); + + return 0; +} + +/* Sysfs interface */ +static ssize_t +sensor_delay_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct sensor_data *data = input_get_drvdata(input_data); + int delay; + + mutex_lock(&data->mutex); + + delay = data->delay; + + mutex_unlock(&data->mutex); + + return sprintf(buf, "%d\n", delay); +} + +static ssize_t +sensor_delay_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct sensor_data *data = input_get_drvdata(input_data); + long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; +#endif + + if (value < 0) + return count; + + if (SENSOR_MAX_DELAY < value) + value = SENSOR_MAX_DELAY; + + mutex_lock(&data->mutex); + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + make_event(ev, EV_ABS, ABS_CONTROL_REPORT, + (data->enabled << 16) | value); + sensor_event(&data->devfile_list, ev, 1); +#endif + data->delay = value; +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(input_data, ABS_CONTROL_REPORT, + (data->enabled << 16) | value); +#endif + + mutex_unlock(&data->mutex); + + return count; +} + +static ssize_t +sensor_enable_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct sensor_data *data = input_get_drvdata(input_data); + int enabled; + + mutex_lock(&data->mutex); + + enabled = data->enabled; + + mutex_unlock(&data->mutex); + + return sprintf(buf, "%d\n", enabled); +} + +static ssize_t +sensor_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct sensor_data *data = input_get_drvdata(input_data); + unsigned long value; + int error; + + error = strict_strtoul(buf, 10, &value); + if (unlikely(error)) + return error; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; +#endif + + value = !(!value); + + mutex_lock(&data->mutex); + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + make_event(ev, EV_ABS, ABS_CONTROL_REPORT, (value << 16) | data->delay); + sensor_event(&data->devfile_list, ev, 1); +#else + input_report_abs(input_data, ABS_CONTROL_REPORT, + (value << 16) | data->delay); + input_sync(input_data); +#endif + + if (data->enabled && !value) + suspend(); + if (!data->enabled && value) + resume(); + data->enabled = value; + + mutex_unlock(&data->mutex); + + return count; +} + +static ssize_t +sensor_wake_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + static int cnt = 1; +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct sensor_data *data = input_get_drvdata(input_data); + struct input_event ev[2]; + struct timeval tv; + get_time_stamp(&tv); + make_event_w_time(&ev[0], EV_ABS, ABS_WAKE, cnt++, &tv); + make_event_w_time(&ev[1], EV_SYN, 0, 0, &tv); + sensor_event(&data->devfile_list, ev, 2); +#else + input_report_abs(input_data, ABS_WAKE, cnt++); + input_sync(input_data); +#endif + + return count; +} + +#if DEBUG + +static int sensor_suspend(struct platform_device *pdev, pm_message_t state); +static int sensor_resume(struct platform_device *pdev); + +static ssize_t +sensor_debug_suspend_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input = to_input_dev(dev); + struct sensor_data *data = input_get_drvdata(input); + + return sprintf(buf, "%d\n", data->suspend); +} + +static ssize_t +sensor_debug_suspend_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long suspend; + int error; + + error = strict_strtoul(buf, 10, &suspend); + if (unlikely(error)) + return error; + + + if (suspend) { + pm_message_t msg; + memset(&msg, 0, sizeof(msg)); + sensor_suspend(sensor_pdev, msg); + } else + sensor_resume(sensor_pdev); + + return count; +} + +#endif /* DEBUG */ + +static ssize_t +sensor_data_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + int x, y, z; + + x = input_abs_get_val(input_data, ABS_X); + y = input_abs_get_val(input_data, ABS_Y); + z = input_abs_get_val(input_data, ABS_Z); + + return sprintf(buf, "%d %d %d\n", x, y, z); +} + +static ssize_t +sensor_status_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + int status; + + status = input_abs_get_val(input_data, ABS_STATUS); + + return sprintf(buf, "%d\n", status); +} + +static DEVICE_ATTR(delay, S_IRUGO | S_IWUSR | S_IWGRP, + sensor_delay_show, sensor_delay_store); +static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, + sensor_enable_show, sensor_enable_store); +static DEVICE_ATTR(wake, S_IWUSR | S_IWGRP, NULL, sensor_wake_store); +static DEVICE_ATTR(data, S_IRUGO, sensor_data_show, NULL); +static DEVICE_ATTR(status, S_IRUGO, sensor_status_show, NULL); + +#if DEBUG +static DEVICE_ATTR(debug_suspend, S_IRUGO | S_IWUSR, + sensor_debug_suspend_show, sensor_debug_suspend_store); +#endif /* DEBUG */ + +static struct attribute *sensor_attributes[] = { + &dev_attr_delay.attr, + &dev_attr_enable.attr, + &dev_attr_wake.attr, + &dev_attr_data.attr, + &dev_attr_status.attr, +#if DEBUG + &dev_attr_debug_suspend.attr, +#endif /* DEBUG */ + NULL +}; + +static struct attribute_group sensor_attribute_group = { + .attrs = sensor_attributes +}; + +static int sensor_suspend(struct platform_device *pdev, pm_message_t state) +{ + struct sensor_data *data = input_get_drvdata(this_data); + int rt = 0; + + mutex_lock(&data->mutex); + + if (data->enabled) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_CONTROL_REPORT, + (0 << 16) | data->delay); + sensor_event(&data->devfile_list, ev, 1); +#else + input_report_abs(this_data, ABS_CONTROL_REPORT, + (0 << 16) | data->delay); +#endif + rt = suspend(); + } + + mutex_unlock(&data->mutex); + + return rt; +} + +static int sensor_resume(struct platform_device *pdev) +{ + struct sensor_data *data = input_get_drvdata(this_data); + int rt = 0; + + mutex_lock(&data->mutex); + + if (data->enabled) { +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + struct input_event ev[1]; + make_event(ev, EV_ABS, ABS_CONTROL_REPORT, + (1 << 16) | data->delay); + sensor_event(&data->devfile_list, ev, 1); +#endif + rt = resume(); +#ifndef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + input_report_abs(this_data, ABS_CONTROL_REPORT, + (1 << 16) | data->delay); +#endif + } + + mutex_unlock(&data->mutex); + + return rt; +} + +static int sensor_probe(struct platform_device *pdev) +{ + struct sensor_data *data = NULL; + struct input_dev *input_data = NULL; + int input_registered = 0, sysfs_created = 0; + int rt; + + data = kzalloc(sizeof(struct sensor_data), GFP_KERNEL); + if (!data) { + rt = -ENOMEM; + goto err; + } + data->enabled = 0; + data->delay = SENSOR_DEFAULT_DELAY; + + input_data = input_allocate_device(); + if (!input_data) { + rt = -ENOMEM; + YLOGE(("sensor_probe: Failed to allocate input_data device\n")); + goto err; + } + + set_bit(EV_ABS, input_data->evbit); + input_set_abs_params(input_data, ABS_X, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_Y, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_Z, 0x80000000, 0x7fffffff, 0, 0); + input_set_abs_params(input_data, ABS_RUDDER, 0x80000000, 0x7fffffff, 0, + 0); + input_set_abs_params(input_data, ABS_STATUS, 0, 3, 0, 0); + input_set_abs_params(input_data, ABS_WAKE, 0x80000000, 0x7fffffff, 0, + 0); + input_set_abs_params(input_data, ABS_CONTROL_REPORT, 0x80000000, + 0x7fffffff, 0, 0); + input_data->name = SENSOR_NAME; + + rt = input_register_device(input_data); + if (rt) { + YLOGE(("ori Unable to reg input_data %s\n", input_data->name)); + goto err; + } + input_set_drvdata(input_data, data); + input_registered = 1; + + rt = sysfs_create_group(&input_data->dev.kobj, &sensor_attribute_group); + if (rt) { + YLOGE(("sensor_probe: sysfs_create_group failed[%s]\n", + input_data->name)); + goto err; + } + sysfs_created = 1; + mutex_init(&data->mutex); + this_data = input_data; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + INIT_LIST_HEAD(&data->devfile_list); + if (misc_register(&sensor_devfile) < 0) + goto err; +#endif + + return 0; + +err: + if (data != NULL) { + if (input_data != NULL) { + if (sysfs_created) + sysfs_remove_group(&input_data->dev.kobj, + &sensor_attribute_group); + if (input_registered) + input_unregister_device(input_data); + else + input_free_device(input_data); + input_data = NULL; + } + kfree(data); + } + + return rt; +} + +static int sensor_remove(struct platform_device *pdev) +{ + struct sensor_data *data; + +#ifdef YAS_SENSOR_KERNEL_DEVFILE_INTERFACE + misc_deregister(&sensor_devfile); +#endif + if (this_data != NULL) { + data = input_get_drvdata(this_data); + sysfs_remove_group(&this_data->dev.kobj, + &sensor_attribute_group); + input_unregister_device(this_data); + if (data != NULL) + kfree(data); + } + + return 0; +} + +/* + * Module init and exit + */ +static struct platform_driver sensor_driver = { + .probe = sensor_probe, + .remove = sensor_remove, + .suspend = sensor_suspend, + .resume = sensor_resume, + .driver = { + .name = SENSOR_NAME, + .owner = THIS_MODULE, + }, +}; + +static int __init sensor_init(void) +{ + sensor_pdev = platform_device_register_simple(SENSOR_NAME, 0, NULL, 0); + if (IS_ERR(sensor_pdev)) + return -1; + return platform_driver_register(&sensor_driver); +} + +module_init(sensor_init); + +static void __exit sensor_exit(void) +{ + platform_driver_unregister(&sensor_driver); + platform_device_unregister(sensor_pdev); +} + +module_exit(sensor_exit); + +MODULE_AUTHOR("Yamaha Corporation"); +MODULE_LICENSE("GPL"); +MODULE_VERSION("4.4.702a"); diff --git a/drivers/sensor/yas_pcb_test.c b/drivers/sensor/yas_pcb_test.c new file mode 100644 index 0000000..686c8c6 --- /dev/null +++ b/drivers/sensor/yas_pcb_test.c @@ -0,0 +1,1282 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +/* + * File yas_pcb_test.c + * Brief pcb test program for yas530/yas532 + * Date 2013/1/22 + * Revision 1.4.3 + */ +#include "yas_pcb_test.h" + +/* define */ +/* reg num */ +#define YAS530_CAL_REG_NUM (16) +#define YAS532_CAL_REG_NUM (14) +#define YAS_PCB_MEASURE_DATA_REG_NUM (8) + +/* default value */ +#define YAS_PCB_TEST1_DEFAULT (0x00) +#define YAS_PCB_TEST2_DEFAULT (0x00) +#define YAS_PCB_INTERVAL_DEFAULT (0x00) +#define YAS_PCB_CONFIG_DEFAULT (0x01) /* INTON = 1 */ +#define YAS_PCB_COIL_DEFAULT (0x00) + +/* measure command */ +#define YAS_PCB_MEASURE_COMMAND_START (0x01) +#define YAS_PCB_MEASURE_COMMAND_LDTC (0x02) +#define YAS_PCB_MEASURE_COMMAND_FORS (0x04) + +#define YAS_PCB_MEASURE_BUSY (0x80) + +#define YAS_PCB_MEASURE_WAIT_TIME (2) /* ms */ +#define YAS_PCB_HARD_OFFSET_CORRECT (16) +#define YAS_PCB_COIL_INIT_CALC_NUM (5) + +#define YAS_PCB_HARD_OFFSET_MASK (0x3F) + +#define YAS_PCB_INT_CHECK (1) +#define YAS_PCB_INT_NOTCHECK (0) +#define YAS_PCB_INT_HIGH (1) +#define YAS_PCB_INT_LOW (0) + +#define YAS_PCB_ACC_Z (9806550L) /* m/s2 */ + +#define YAS530_DEVICE_ID (0x01) /* MS-3E */ +#define YAS532_DEVICE_ID (0x02) /* MS-3R */ + +#define YAS530_VERSION_A (0) /* MS-3E Aver */ +#define YAS530_VERSION_B (1) /* MS-3E Bver */ +/*#define YAS530_VERSION_AB (0)*/ /* MS-3R ABver */ +#define YAS532_VERSION_AC (1) /* MS-3R ACver */ + +#define YAS530_COEF_VERSION_A (380) +#define YAS530_COEF_VERSION_B (550) +/*#define YAS532_COEF_VERSION_AB (1800)*/ +/*#define YAS532_COEF_VERSION_AC (900)*/ +#define YAS532_COEFX_VERSION_AC (850) +#define YAS532_COEFY1_VERSION_AC (750) +#define YAS532_COEFY2_VERSION_AC (750) + +#define YAS530_RAWDATA_CENTER (2048) +#define YAS530_RAWDATA_OVERFLOW (4095) +#define YAS532_RAWDATA_CENTER (4096) +#define YAS532_RAWDATA_OVERFLOW (8190) + +#define YAS_PCB_DIR_DIVIDER (400) + +#define YAS_PCB_TEST1 (0) +#define YAS_PCB_TEST3 (1) +#define YAS_PCB_TEST4 (2) +#define YAS_PCB_TEST5 (3) +#define YAS_PCB_TEST6 (4) +#define YAS_PCB_TEST7 (5) +#define YAS_PCB_TEST8 (6) +#define YAS_PCB_TEST2 (7) +#define YAS_PCB_TEST_NUM (8) + +/* typedef */ +struct yas_pcb_vector { + int32_t v[3]; +}; + +struct yas_pcb_correction { + int32_t s32Cx, s32Cy1, s32Cy2; + int32_t s32A2, s32A3, s32A4, s32A5, s32A6, s32A7, s32A8, s32A9, s32K; + int32_t s32ZFlag; + int32_t s32Rx, s32Ry1, s32Ry2; + int32_t s32Fx, s32Fy1, s32Fy2; + int32_t s32Ver; +}; + +struct yas_pcb_sensitivity { + int32_t s32Sx, s32Sy, s32Sz; +}; + +/* values */ +static uint16_t gu16State; +static struct yas_pcb_test_callback g_callback; +static struct yas_pcb_vector gstXy1y2; +static int8_t gs08HardOffset[3]; +static struct yas_pcb_correction gstCorrect; +static uint8_t gu08DevId; +static int32_t gs32Center; +static int32_t gs32Overflow; +#ifdef YAS_PCBTEST_EXTRA +static uint8_t gu08Recalc; +static int32_t gs32RecalcWait; +#endif + +/* functions */ +static int Ms3AxesLibAtan8(int, int, short *); +static int Ms3AxesLibDir8(int, int, unsigned short *); +static int yas_pcb_check_state(int); +static void yas_pcb_update_state(int); +static int yas_pcb_power_on(void); +static int yas_pcb_power_off(void); +static int yas_pcb_reset_coil(void); +static int yas530_read_cal(uint8_t *); +static int yas532_read_cal(uint8_t *); +static void yas530_calc_correction(const uint8_t *); +static void yas532_calc_correction(const uint8_t *); +static int yas_pcb_set_offset(const int8_t *); +static int yas_pcb_measure(struct yas_pcb_vector *, int *, uint8_t, uint8_t); +static int yas_pcb_is_flow_occued(struct yas_pcb_vector *, int32_t, int32_t); +static void yas_pcb_calc_sensitivity(struct yas_pcb_vector *, + int, struct yas_pcb_sensitivity *); +static void yas_pcb_calc_position(struct yas_pcb_vector *, + struct yas_pcb_vector *, int); +static int yas_pcb_calc_magnetic_field(struct yas_pcb_vector *, + struct yas_pcb_vector *); +static int yas_pcb_test1(int *); +static int yas_pcb_test2(void); +static int yas_pcb_test3(void); +static int yas_pcb_test4(int *, int *, int *); +static int yas_pcb_test5(int *); +static int yas_pcb_test6(int *, int *); +#ifdef YAS_PCBTEST_EXTRA +static int yas_pcb_test7(int *, int *, int *); +#endif + +static int Ms3AxesLibAtan8(int ss, int cc, short *ans) +{ + static const unsigned char AtanTable[] = { + 0, 1, 3, 5, 6, 8, 11, 13, + 15, 18, 21, 24, 27, 31, 34, 39, + 43, 48, 53, 58, 63, 69, 75, 82, + 89, 96, 103, 110, 118, 126, 134, 143, + 152 + }; + + unsigned char idx; + unsigned short idx_mul64; + signed char sign = 1; + unsigned int ucc; + unsigned int uss; + unsigned short ans_mul8; + unsigned char idx_mod64; + unsigned short ans_diff8; + + if (cc < 0) { + sign = -sign; + ucc = -cc; + } else { + ucc = cc; + } + + if (ss < 0) { + sign = -sign; + uss = -ss; + } else { + uss = ss; + } + + while (ucc >= 0x400) { + ucc >>= 1; + uss >>= 1; + } + + if (ucc == 0) + return -1; + + idx_mul64 = (uss << 11) / ucc; + + idx = idx_mul64 >> 6; + ans_mul8 = (idx << 4) - AtanTable[idx]; + + idx_mod64 = (unsigned char)idx_mul64 & 0x3f; + + if (idx < 32) { + idx++; + ans_diff8 = (idx << 4) - AtanTable[idx] - ans_mul8; + ans_mul8 += (ans_diff8 * idx_mod64) >> 6; + } + + *ans = (sign == 1) ? ans_mul8 : (-ans_mul8); + + return 0; +} + +static int Ms3AxesLibDir8(int ss, int cc, unsigned short *ans) +{ + short temp_ans = 0; + int ucc = cc; + int uss = ss; + int ret = 0; + + if (cc < -2147483647) + cc++; + if (ss < -2147483647) + ss++; + + if (cc < 0) + ucc = -cc; + + if (ss < 0) + uss = -ss; + + if (uss <= ucc) { + ret = Ms3AxesLibAtan8(ss, cc, &temp_ans); + if (ret < 0) + return 1; + + if (cc > 0) { + if (temp_ans < 0) + temp_ans += 2880; + } else + temp_ans += 1440; + } else { + ret = Ms3AxesLibAtan8(cc, ss, &temp_ans); + if (ret < 0) + return 1; + + if (ss > 0) + temp_ans = 720 - temp_ans; + else + temp_ans = 2160 - temp_ans; + } + + *ans = temp_ans; + + return 0; +} + +static int yas_pcb_check_state(int id) +{ + int result = YAS_PCB_ERROR_TEST_ORDER; + uint16_t u16Mask; + const uint16_t u16TestTable[] = { + 0x0000, /* 1 */ + 0x0000, /* 3 */ + (1 << YAS_PCB_TEST1) | (1 << YAS_PCB_TEST3), /* 4 */ + (1 << YAS_PCB_TEST1) | (1 << YAS_PCB_TEST3) + | (1 << YAS_PCB_TEST4), /* 5 */ + (1 << YAS_PCB_TEST1) | (1 << YAS_PCB_TEST3) + | (1 << YAS_PCB_TEST4), /* 6 */ + (1 << YAS_PCB_TEST1) | (1 << YAS_PCB_TEST3) + | (1 << YAS_PCB_TEST4) /* 7 */ + | (1 << YAS_PCB_TEST5), + (1 << YAS_PCB_TEST1) | (1 << YAS_PCB_TEST3) + | (1 << YAS_PCB_TEST4), /* 8 */ + (1 << YAS_PCB_TEST1), /* 2 */ + }; + + if ((YAS_PCB_TEST1 <= id) && (id < YAS_PCB_TEST_NUM)) { + u16Mask = u16TestTable[id]; + if (u16Mask == 0) { + switch (id) { + case YAS_PCB_TEST1: + if ((gu16State == 0) + || (gu16State == (1 << YAS_PCB_TEST1))) + result = YAS_PCB_NO_ERROR; + break; + + case YAS_PCB_TEST3: + if ((gu16State == (1 << YAS_PCB_TEST1)) + || (gu16State == + ((1 << YAS_PCB_TEST1) + | (1 << YAS_PCB_TEST3)))) + result = YAS_PCB_NO_ERROR; + break; + + default: + break; + } + } else { + if ((gu16State & u16Mask) == u16Mask) + result = YAS_PCB_NO_ERROR; + } + } + + return result; +} + +static void yas_pcb_update_state(int id) +{ + if ((YAS_PCB_TEST1 <= id) && (id < YAS_PCB_TEST2)) + gu16State |= (uint16_t)(1 << id); + else + gu16State = 0; +} + +static int yas_pcb_power_on(void) +{ + int result = YAS_PCB_NO_ERROR; + int ret; + + if (NULL != g_callback.power_on) { + ret = g_callback.power_on(); + if (0 != ret) + result = YAS_PCB_ERROR_POWER; + } + + return result; +} + +static int yas_pcb_power_off(void) +{ + int result = YAS_PCB_NO_ERROR; + int ret; + + if (NULL != g_callback.power_off) { + ret = g_callback.power_off(); + if (0 != ret) + result = YAS_PCB_ERROR_POWER; + } + + return result; +} + +static int yas_pcb_reset_coil(void) +{ + int ret; + uint8_t u08Data; + u08Data = YAS_PCB_COIL_DEFAULT; + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_COIL, + &u08Data, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + return YAS_PCB_NO_ERROR; +} +static int yas530_read_cal(uint8_t *pu08Buf) +{ + int i; + int ret; + int size = YAS530_CAL_REG_NUM; + + /* Dummy read */ + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_CAL, + pu08Buf, size); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_CAL, + pu08Buf, size); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + /* cal register is all 0 */ + for (i = 0; i < size; i++) { + if (pu08Buf[i] != 0x00) + return YAS_PCB_NO_ERROR; + } + + return YAS_PCB_ERROR_CALREG; +} + + +static int yas532_read_cal(uint8_t *pu08Buf) +{ + int i; + int ret; + int size = YAS532_CAL_REG_NUM; + int len = size - 1; + + /* Dummy read */ + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_CAL, + pu08Buf, size); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_CAL, + pu08Buf, size); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + /* cal register is all 0 */ + for (i = 0; i < len; i++) { + if (pu08Buf[i] != 0x00) + return YAS_PCB_NO_ERROR; + } + + /* MSB is not 0 */ + if (pu08Buf[len] & 0x80) + return YAS_PCB_NO_ERROR; + + return YAS_PCB_ERROR_CALREG; +} + +static void yas530_calc_correction(const uint8_t *pu08Data) +{ + uint8_t u08Dx = pu08Data[0]; + uint8_t u08Dy1 = pu08Data[1]; + uint8_t u08Dy2 = pu08Data[2]; + uint8_t u08D2 = (uint8_t)((pu08Data[3] >> 2) & 0x3F); + uint8_t u08D3 = (uint8_t)(((pu08Data[3] << 2) & 0x0C) + | ((pu08Data[4] >> 6) & 0x03)); + uint8_t u08D4 = (uint8_t)(pu08Data[4] & 0x3F); + uint8_t u08D5 = (uint8_t)((pu08Data[5] >> 2) & 0x3f); + uint8_t u08D6 = (uint8_t)(((pu08Data[5] << 4) & 0x30) + | ((pu08Data[6] >> 4) & 0x0F)); + uint8_t u08D7 = (uint8_t)(((pu08Data[6] << 3) & 0x78) + | ((pu08Data[7] >> 5) & 0x07)); + uint8_t u08D8 = (uint8_t)(((pu08Data[7] << 1) & 0x3E) + | ((pu08Data[8] >> 7) & 0x01)); + uint8_t u08D9 = (uint8_t)(((pu08Data[8] << 1) & 0xFE) + | ((pu08Data[9] >> 7) & 0x01)); + uint8_t u08D0 = (uint8_t)((pu08Data[9] >> 2) & 0x1F); + uint8_t u08ZFlag = (uint8_t)((pu08Data[11] >> 5) & 0x01); + uint8_t u08Rx = (uint8_t)(((pu08Data[11] << 1) & 0x3E) + | ((pu08Data[12] >> 7) & 0x01)); + uint8_t u08Fx = (uint8_t)((pu08Data[12] >> 5) & 0x03); + uint8_t u08Ry1 = (uint8_t)(((pu08Data[12] << 1) & 0x3E) + | ((pu08Data[13] >> 7) & 0x01)); + uint8_t u08Fy1 = (uint8_t)((pu08Data[13] >> 5) & 0x03); + uint8_t u08Ry2 = (uint8_t)(((pu08Data[13] << 1) & 0x3E) + | ((pu08Data[14] >> 7) & 0x01)); + uint8_t u08Fy2 = (uint8_t)((pu08Data[14] >> 5) & 0x03); + uint8_t u08Ver = pu08Data[15] & 0x07; + + gstCorrect.s32Cx = (int32_t)((u08Dx * 6) - 768); + gstCorrect.s32Cy1 = (int32_t)((u08Dy1 * 6) - 768); + gstCorrect.s32Cy2 = (int32_t)((u08Dy2 * 6) - 768); + gstCorrect.s32A2 = (int32_t)(u08D2 - 32); + gstCorrect.s32A3 = (int32_t)(u08D3 - 8); + gstCorrect.s32A4 = (int32_t)(u08D4 - 32); + gstCorrect.s32A5 = (int32_t)(u08D5 + 38); + gstCorrect.s32A6 = (int32_t)(u08D6 - 32); + gstCorrect.s32A7 = (int32_t)(u08D7 - 64); + gstCorrect.s32A8 = (int32_t)(u08D8 - 32); + gstCorrect.s32A9 = (int32_t)u08D9; + gstCorrect.s32K = (int32_t)(u08D0) + 10; + gstCorrect.s32ZFlag = (int32_t)u08ZFlag; + gstCorrect.s32Rx = (int32_t)((int8_t)(u08Rx << 2) >> 2); + gstCorrect.s32Fx = (int32_t)u08Fx; + gstCorrect.s32Ry1 = (int32_t)((int8_t)(u08Ry1 << 2) >> 2); + gstCorrect.s32Fy1 = (int32_t)u08Fy1; + gstCorrect.s32Ry2 = (int32_t)((int8_t)(u08Ry2 << 2) >> 2); + gstCorrect.s32Fy2 = (int32_t)u08Fy2; + gstCorrect.s32Ver = (int32_t)u08Ver; +} + +static void yas532_calc_correction(const uint8_t *pu08Data) +{ + uint8_t u08Dx = pu08Data[0]; + uint8_t u08Dy1 = pu08Data[1]; + uint8_t u08Dy2 = pu08Data[2]; + uint8_t u08D2 = (uint8_t)((pu08Data[3] >> 2) & 0x3F); + uint8_t u08D3 = (uint8_t)(((pu08Data[3] << 2) & 0x0C) + | ((pu08Data[4] >> 6) & 0x03)); + uint8_t u08D4 = (uint8_t)(pu08Data[4] & 0x3F); + uint8_t u08D5 = (uint8_t)((pu08Data[5] >> 2) & 0x3f); + uint8_t u08D6 = (uint8_t)(((pu08Data[5] << 4) & 0x30) + | ((pu08Data[6] >> 4) & 0x0F)); + uint8_t u08D7 = (uint8_t)(((pu08Data[6] << 3) & 0x78) + | ((pu08Data[7] >> 5) & 0x07)); + uint8_t u08D8 = (uint8_t)(((pu08Data[7] << 1) & 0x3E) + | ((pu08Data[8] >> 7) & 0x01)); + uint8_t u08D9 = (uint8_t)(((pu08Data[8] << 1) & 0xFE) + | ((pu08Data[9] >> 7) & 0x01)); + uint8_t u08D0 = (uint8_t)((pu08Data[9] >> 2) & 0x1F); + uint8_t u08Rx = (uint8_t)((pu08Data[10] >> 1) & 0x3F); + uint8_t u08Fx = (uint8_t)(((pu08Data[10] & 0x01) << 1) + | ((pu08Data[11] >> 7) & 0x01)); + uint8_t u08Ry1 = (uint8_t)((pu08Data[11] >> 1) & 0x3F); + uint8_t u08Fy1 = (uint8_t)(((pu08Data[11] & 0x01) << 1) + | ((pu08Data[12] >> 7) & 0x01)); + uint8_t u08Ry2 = (uint8_t)((pu08Data[12] >> 1) & 0x3F); + uint8_t u08Fy2 = (uint8_t)(((pu08Data[12] & 0x01) << 1) + | ((pu08Data[13] >> 7) & 0x01)); + uint8_t u08Ver = pu08Data[13] & 0x01; + + gstCorrect.s32Cx = (int32_t)((u08Dx * 10) - 1280); + gstCorrect.s32Cy1 = (int32_t)((u08Dy1 * 10) - 1280); + gstCorrect.s32Cy2 = (int32_t)((u08Dy2 * 10) - 1280); + gstCorrect.s32A2 = (int32_t)(u08D2 - 32); + gstCorrect.s32A3 = (int32_t)(u08D3 - 8); + gstCorrect.s32A4 = (int32_t)(u08D4 - 32); + gstCorrect.s32A5 = (int32_t)(u08D5 + 38); + gstCorrect.s32A6 = (int32_t)(u08D6 - 32); + gstCorrect.s32A7 = (int32_t)(u08D7 - 64); + gstCorrect.s32A8 = (int32_t)(u08D8 - 32); + gstCorrect.s32A9 = (int32_t)u08D9; + gstCorrect.s32K = (int32_t)u08D0; + gstCorrect.s32ZFlag = (int32_t)1; + gstCorrect.s32Rx = (int32_t)((int8_t)(u08Rx << 2) >> 2); + gstCorrect.s32Fx = (int32_t)u08Fx; + gstCorrect.s32Ry1 = (int32_t)((int8_t)(u08Ry1 << 2) >> 2); + gstCorrect.s32Fy1 = (int32_t)u08Fy1; + gstCorrect.s32Ry2 = (int32_t)((int8_t)(u08Ry2 << 2) >> 2); + gstCorrect.s32Fy2 = (int32_t)u08Fy2; + gstCorrect.s32Ver = (int32_t)u08Ver; +} + +static int yas_pcb_set_offset(const int8_t *ps08Offset) +{ + int result = YAS_PCB_NO_ERROR; + int ret; + uint8_t u08Addr; + uint8_t u08Data; + uint8_t i; + + for (i = 0; i < 3; i++) { + u08Addr = (uint8_t)(YAS_PCB_ADDR_OFFSET + i); + u08Data = (uint8_t)ps08Offset[i] & YAS_PCB_HARD_OFFSET_MASK; + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, + u08Addr, &u08Data, 1); + if (0 != ret) { + result = YAS_PCB_ERROR_I2C; + break; + } + } + + return result; +} + +static int yas_pcb_measure(struct yas_pcb_vector *pstXy1y2, int *temperature, + uint8_t u08Command, uint8_t u08CheckIni) +{ + int ret; + uint8_t u08Buf[YAS_PCB_MEASURE_DATA_REG_NUM]; + int low_or_high; + + if ((YAS_PCB_INT_CHECK == u08CheckIni) + && (NULL != g_callback.read_intpin)) { + ret = g_callback.read_intpin(&low_or_high); + if ((0 != ret) || (YAS_PCB_INT_HIGH != low_or_high)) + return YAS_PCB_ERROR_INTERRUPT; + } + + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, + YAS_PCB_ADDR_MEASURE_COMMAND, + &u08Command, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + g_callback.msleep(YAS_PCB_MEASURE_WAIT_TIME); + + if ((YAS_PCB_INT_CHECK == u08CheckIni) + && (NULL != g_callback.read_intpin)) { + ret = g_callback.read_intpin(&low_or_high); + if ((0 != ret) || (YAS_PCB_INT_LOW != low_or_high)) + return YAS_PCB_ERROR_INTERRUPT; + } + + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_MEASURE_DATA, + u08Buf, YAS_PCB_MEASURE_DATA_REG_NUM); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + /* calc measure data */ + if (YAS532_DEVICE_ID == gu08DevId) { + *temperature = (((int32_t)(u08Buf[0] & 0x7F) << 3) + | ((u08Buf[1] >> 5) & 0x07)); + pstXy1y2->v[0] = (int32_t)(((int32_t)(u08Buf[2] & 0x7F) << 6) + | ((u08Buf[3] >> 2) & 0x3F)); + pstXy1y2->v[1] = (int32_t)(((int32_t)(u08Buf[4] & 0x7F) << 6) + | ((u08Buf[5] >> 2) & 0x3F)); + pstXy1y2->v[2] = (int32_t)(((int32_t)(u08Buf[6] & 0x7F) << 6) + | ((u08Buf[7] >> 2) & 0x3F)); + } else { + *temperature = (((int32_t)(u08Buf[0] & 0x7F) << 2) + | ((u08Buf[1] >> 6) & 0x03)); + pstXy1y2->v[0] = (int32_t)(((int32_t)(u08Buf[2] & 0x7F) << 5) + | ((u08Buf[3] >> 3) & 0x1F)); + pstXy1y2->v[1] = (int32_t)(((int32_t)(u08Buf[4] & 0x7F) << 5) + | ((u08Buf[5] >> 3) & 0x1F)); + pstXy1y2->v[2] = (int32_t)(((int32_t)(u08Buf[6] & 0x7F) << 5) + | ((u08Buf[7] >> 3) & 0x1F)); + } + + if (YAS_PCB_MEASURE_BUSY == (u08Buf[0] & YAS_PCB_MEASURE_BUSY)) + return YAS_PCB_ERROR_BUSY; + + return YAS_PCB_NO_ERROR; +} + +static int yas_pcb_is_flow_occued(struct yas_pcb_vector *pstXy1y2, + int32_t underflow, int32_t overflow) +{ + int result = YAS_PCB_NO_ERROR; + int32_t s32Tmp; + uint8_t i; + + for (i = 0; i < 3; i++) { + s32Tmp = pstXy1y2->v[i]; + if (s32Tmp <= underflow) + result = YAS_PCB_ERROR_UNDERFLOW; + else + if (overflow <= s32Tmp) + result = YAS_PCB_ERROR_OVERFLOW; + } + + return result; +} + +static void yas_pcb_calc_sensitivity(struct yas_pcb_vector *pstXy1y2, + int temperature, struct yas_pcb_sensitivity *pstYasSensitivity) +{ + /* calc XYZ data from xy1y2 data */ + int32_t s32Sx = pstXy1y2->v[0] + - ((gstCorrect.s32Cx * temperature) / 100); + int32_t s32Sy1 = pstXy1y2->v[1] + - ((gstCorrect.s32Cy1 * temperature) / 100); + int32_t s32Sy2 = pstXy1y2->v[2] + - ((gstCorrect.s32Cy2 * temperature) / 100); + int32_t s32Sy = s32Sy1 - s32Sy2; + int32_t s32Sz = -s32Sy1 - s32Sy2; + + pstYasSensitivity->s32Sx = s32Sx; + pstYasSensitivity->s32Sy = s32Sy; + pstYasSensitivity->s32Sz = s32Sz; +} + +static void yas_pcb_calc_position(struct yas_pcb_vector *pstXy1y2, + struct yas_pcb_vector *pstXyz, int temperature) +{ + struct yas_pcb_sensitivity stSensitivity; + struct yas_pcb_sensitivity *pst; + + yas_pcb_calc_sensitivity(pstXy1y2, temperature, &stSensitivity); + + pst = &stSensitivity; + pstXyz->v[0] = (gstCorrect.s32K + * ((100 * pst->s32Sx) + (gstCorrect.s32A2 * pst->s32Sy) + + (gstCorrect.s32A3 * pst->s32Sz))) / 10; + pstXyz->v[1] = (gstCorrect.s32K * ((gstCorrect.s32A4 * pst->s32Sx) + + (gstCorrect.s32A5 * pst->s32Sy) + + (gstCorrect.s32A6 * pst->s32Sz))) / 10; + pstXyz->v[2] = (gstCorrect.s32K * ((gstCorrect.s32A7 * pst->s32Sx) + + (gstCorrect.s32A8 * pst->s32Sy) + + (gstCorrect.s32A9 * pst->s32Sz))) / 10; +} + +static void yas530_calc_magnetic_field(struct yas_pcb_vector *pstXy1y2, + struct yas_pcb_vector *pstXyz, int32_t s32Coef) +{ + int32_t s32Oy; + int32_t s32Oz; + static const int32_t s32HTbl[] = { + 1748, 1948, 2148, 2348 + }; + + pstXy1y2->v[0] = gstXy1y2.v[0] - s32HTbl[gstCorrect.s32Fx] + + (gs08HardOffset[0] - gstCorrect.s32Rx) * s32Coef; + pstXy1y2->v[1] = gstXy1y2.v[1] - s32HTbl[gstCorrect.s32Fy1] + + (gs08HardOffset[1] - gstCorrect.s32Ry1) * s32Coef; + pstXy1y2->v[2] = gstXy1y2.v[2] - s32HTbl[gstCorrect.s32Fy2] + + (gs08HardOffset[2] - gstCorrect.s32Ry2) * s32Coef; + s32Oy = pstXy1y2->v[1] - pstXy1y2->v[2]; + s32Oz = -pstXy1y2->v[1] - pstXy1y2->v[2]; + + pstXyz->v[0] = (gstCorrect.s32K + * ((100 * pstXy1y2->v[0]) + + (gstCorrect.s32A2 * s32Oy) + + (gstCorrect.s32A3 * s32Oz))) / 10; + pstXyz->v[1] = (gstCorrect.s32K + * ((gstCorrect.s32A4 * pstXy1y2->v[0]) + + (gstCorrect.s32A5 * s32Oy) + + (gstCorrect.s32A6 * s32Oz))) / 10; + pstXyz->v[2] = (gstCorrect.s32K + * ((gstCorrect.s32A7 * pstXy1y2->v[0]) + + (gstCorrect.s32A8 * s32Oy) + + (gstCorrect.s32A9 * s32Oz))) / 10; +} + +static void yas532_calc_magnetic_field(struct yas_pcb_vector *pstXy1y2, + struct yas_pcb_vector *pstXyz, + int32_t s32CoefX, int32_t s32CoefY1, int32_t s32CoefY2) +{ + int32_t s32Oy; + int32_t s32Oz; + static const int32_t s32HTbl[] = { + 3721, 3971, 4221, 4471 + }; + + pstXy1y2->v[0] = gstXy1y2.v[0] - s32HTbl[gstCorrect.s32Fx] + + (gs08HardOffset[0] - gstCorrect.s32Rx) * s32CoefX; + pstXy1y2->v[1] = gstXy1y2.v[1] - s32HTbl[gstCorrect.s32Fy1] + + (gs08HardOffset[1] - gstCorrect.s32Ry1) * s32CoefY1; + pstXy1y2->v[2] = gstXy1y2.v[2] - s32HTbl[gstCorrect.s32Fy2] + + (gs08HardOffset[2] - gstCorrect.s32Ry2) * s32CoefY2; + s32Oy = pstXy1y2->v[1] - pstXy1y2->v[2]; + s32Oz = -pstXy1y2->v[1] - pstXy1y2->v[2]; + + pstXyz->v[0] = (gstCorrect.s32K + * ((100 * pstXy1y2->v[0]) + + (gstCorrect.s32A2 * s32Oy) + + (gstCorrect.s32A3 * s32Oz))) / 10; + pstXyz->v[1] = (gstCorrect.s32K + * ((gstCorrect.s32A4 * pstXy1y2->v[0]) + + (gstCorrect.s32A5 * s32Oy) + + (gstCorrect.s32A6 * s32Oz))) / 10; + pstXyz->v[2] = (gstCorrect.s32K + * ((gstCorrect.s32A7 * pstXy1y2->v[0]) + + (gstCorrect.s32A8 * s32Oy) + + (gstCorrect.s32A9 * s32Oz))) / 10; +} + +static int yas_pcb_calc_magnetic_field(struct yas_pcb_vector *pstXy1y2, + struct yas_pcb_vector *pstXyz) +{ + int32_t s32Coef; + + if (YAS532_DEVICE_ID == gu08DevId) { + switch (gstCorrect.s32Ver) { + case YAS532_VERSION_AC: + break; + + default: + return YAS_PCB_ERROR_I2C; + /* break; */ + } + + /* calculate Ohx/y/z[nT] */ + yas532_calc_magnetic_field(pstXy1y2, pstXyz, + YAS532_COEFX_VERSION_AC, + YAS532_COEFY1_VERSION_AC, + YAS532_COEFY2_VERSION_AC); + } else { + switch (gstCorrect.s32Ver) { + case YAS530_VERSION_A: + s32Coef = YAS530_COEF_VERSION_A; + break; + + case YAS530_VERSION_B: + s32Coef = YAS530_COEF_VERSION_B; + break; + + default: + return YAS_PCB_ERROR_I2C; + /* break; */ + } + + /* calculate Ohx/y/z[nT] */ + yas530_calc_magnetic_field(pstXy1y2, pstXyz, s32Coef); + } + + return YAS_PCB_NO_ERROR; +} + +static int yas_pcb_test1(int *id) +{ + int result; + int ret; + + result = yas_pcb_power_on(); + if (YAS_PCB_NO_ERROR == result) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_read(YAS_PCB_ADDR_SLAVE, + YAS_PCB_ADDR_ID, &gu08DevId, 1); + if (0 == ret) { + *id = (int)gu08DevId; + result = YAS_PCB_NO_ERROR; + } + } + + return result; +} + +static int yas_pcb_test2(void) +{ + return yas_pcb_power_off(); +} + +static int yas_pcb_test3(void) +{ + int result; + int ret; + uint8_t u08Data; + uint8_t pu08Buf[YAS530_CAL_REG_NUM]; + + u08Data = YAS_PCB_TEST1_DEFAULT; + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_TEST1, + &u08Data, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + u08Data = YAS_PCB_TEST2_DEFAULT; + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, YAS_PCB_ADDR_TEST2, + &u08Data, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + u08Data = YAS_PCB_INTERVAL_DEFAULT; + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, + YAS_PCB_ADDR_MEASURE_INTERVAL, &u08Data, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + if (YAS532_DEVICE_ID == gu08DevId) { + gs32Center = YAS532_RAWDATA_CENTER; + gs32Overflow = YAS532_RAWDATA_OVERFLOW; + result = yas532_read_cal(pu08Buf); + if (YAS_PCB_NO_ERROR == result) + yas532_calc_correction(pu08Buf); + } else { + gs32Center = YAS530_RAWDATA_CENTER; + gs32Overflow = YAS530_RAWDATA_OVERFLOW; + result = yas530_read_cal(pu08Buf); + if (YAS_PCB_NO_ERROR == result) + yas530_calc_correction(pu08Buf); + } + + if (YAS_PCB_NO_ERROR != result) + return result; + + u08Data = (uint8_t)(YAS_PCB_CONFIG_DEFAULT + | (uint8_t)((pu08Buf[9] & 0x03) << 3) + | (uint8_t)((pu08Buf[10] & 0x80) >> 5)); + ret = g_callback.i2c_write(YAS_PCB_ADDR_SLAVE, + YAS_PCB_ADDR_CONFIG, &u08Data, 1); + if (0 != ret) + return YAS_PCB_ERROR_I2C; + + ret = yas_pcb_reset_coil(); + if (YAS_PCB_NO_ERROR != ret) + return ret; + return YAS_PCB_NO_ERROR; +} + +static int yas_pcb_test4(int *x, int *y1, int *y2) +{ + int result; + struct yas_pcb_vector stXy1y2; + int temperature; + int32_t s32Tmp; + int8_t s08Correct = YAS_PCB_HARD_OFFSET_CORRECT; + uint8_t i; + uint8_t k; + + gs08HardOffset[0] = 0; + gs08HardOffset[1] = 0; + gs08HardOffset[2] = 0; + result = yas_pcb_set_offset(&gs08HardOffset[0]); + if (YAS_PCB_NO_ERROR == result) { + /* calc hard offset */ + for (i = 0; i < YAS_PCB_COIL_INIT_CALC_NUM; i++) { + result = yas_pcb_measure(&stXy1y2, &temperature, + YAS_PCB_MEASURE_COMMAND_START, + YAS_PCB_INT_NOTCHECK); + if (YAS_PCB_NO_ERROR != result) + break; + + for (k = 0; k < 3; k++) { + s32Tmp = stXy1y2.v[k]; + if (gs32Center < s32Tmp) + gs08HardOffset[k] += s08Correct; + else { + if (s32Tmp < gs32Center) + gs08HardOffset[k] -= s08Correct; + } + } + + result = yas_pcb_set_offset(&gs08HardOffset[0]); + if (YAS_PCB_NO_ERROR != result) + break; + + s08Correct = (int8_t)((uint8_t)s08Correct >> 1); + } + + if (YAS_PCB_NO_ERROR == result) { + *x = (int)gs08HardOffset[0]; + *y1 = (int)gs08HardOffset[1]; + *y2 = (int)gs08HardOffset[2]; + result = yas_pcb_is_flow_occued(&stXy1y2, + 0, gs32Overflow); + } + } + + return result; +} + +static int yas_pcb_test5(int *direction) +{ + uint16_t dir; + int result; + int ret; + int x; + int y; + int nTemp; + struct yas_pcb_vector stXyz; + + result = yas_pcb_measure(&gstXy1y2, &nTemp, + YAS_PCB_MEASURE_COMMAND_START, YAS_PCB_INT_NOTCHECK); + if (YAS_PCB_NO_ERROR == result) { + result = YAS_PCB_ERROR_DIRCALC; + yas_pcb_calc_position(&gstXy1y2, &stXyz, nTemp); + + x = -stXyz.v[0] / YAS_PCB_DIR_DIVIDER; + y = stXyz.v[1] / YAS_PCB_DIR_DIVIDER; + ret = Ms3AxesLibDir8(x, y, &dir); + if (0 == ret) { + *direction = (int)(dir / 8); + result = yas_pcb_is_flow_occued(&gstXy1y2, + 0, gs32Overflow); + } + } + + return result; +} + +static int yas_pcb_test6(int *sx, int *sy) +{ + int result; + struct yas_pcb_vector stXy1y2P; + struct yas_pcb_vector stXy1y2N; + int temperature; + uint8_t u08Command; + struct yas_pcb_vector *pP = &stXy1y2P; + struct yas_pcb_vector *pN = &stXy1y2N; + struct yas_pcb_correction *pC = &gstCorrect; + + u08Command = YAS_PCB_MEASURE_COMMAND_START + | YAS_PCB_MEASURE_COMMAND_LDTC; + result = yas_pcb_measure(pP, &temperature, + u08Command, YAS_PCB_INT_CHECK); + + if (YAS_PCB_NO_ERROR == result) { + u08Command = YAS_PCB_MEASURE_COMMAND_START + | YAS_PCB_MEASURE_COMMAND_LDTC + | YAS_PCB_MEASURE_COMMAND_FORS; + result = yas_pcb_measure(pN, &temperature, u08Command, + YAS_PCB_INT_NOTCHECK); + + if (YAS_PCB_NO_ERROR == result) { + if (YAS532_DEVICE_ID == gu08DevId) { + *sx = (int)(pC->s32K * 100 + * (pP->v[0] - pN->v[0])); + *sx /= 1000; + *sx /= YAS_VCORE; + *sy = (int)(pC->s32K * pC->s32A5 + * ((pP->v[1] - pN->v[1]) + - (pP->v[2] - pN->v[2]))); + *sy /= 1000; + *sy /= YAS_VCORE; + } else { + *sx = (int)(pN->v[0] - pP->v[0]); + *sy = (int)((pN->v[1] - pP->v[1]) + - (pN->v[2] - pP->v[2])); + } + + result = yas_pcb_is_flow_occued(pP, 0, gs32Overflow); + if (YAS_PCB_NO_ERROR == result) + result = yas_pcb_is_flow_occued(pN, + 0, gs32Overflow); + } + } + + return result; +} + +#ifdef YAS_PCBTEST_EXTRA +static int yas_pcb_test7(int *ohx, int *ohy, int *ohz) +{ + int nRet = YAS_PCB_ERROR_NOT_SUPPORTED; + struct yas_pcb_vector stOhxy1y2, stOhxyz; + + if (0 != gstCorrect.s32ZFlag) { + nRet = yas_pcb_calc_magnetic_field(&stOhxy1y2, &stOhxyz); + if (YAS_PCB_NO_ERROR == nRet) { + /* [nT]->[uT] */ + *ohx = stOhxyz.v[0] / 1000; + *ohy = stOhxyz.v[1] / 1000; + *ohz = stOhxyz.v[2] / 1000; + } + } + + return nRet; +} + +static int yas_pcb_test8(int *hx0, int *hy0, int *hz0) +{ + int nRet; + int nTemp; + int nX, nY1, nY2; + int32_t s32Underflow = 0; + int32_t s32Overflow = gs32Overflow; + struct yas_pcb_vector stOhxy1y2, stOhxyz; + + if (YAS532_DEVICE_ID == gu08DevId) { + s32Underflow = YAS_PCB_NOISE_UNDERFLOW; + s32Overflow = YAS_PCB_NOISE_OVERFLOW; + } + + if (gu08Recalc != 0) { + gs32RecalcWait++; + if (YAS_PCB_NOISE_INTERVAL <= gs32RecalcWait) { + nRet = yas_pcb_reset_coil(); + if (YAS_PCB_NO_ERROR == nRet) + nRet = yas_pcb_test4(&nX, &nY1, &nY2); + + if (YAS_PCB_NO_ERROR == nRet) + gu08Recalc = 0; + + gs32RecalcWait = 0; + } + } + + if (0 != gstCorrect.s32ZFlag) { + nRet = yas_pcb_measure(&gstXy1y2, &nTemp, + YAS_PCB_MEASURE_COMMAND_START, + YAS_PCB_INT_NOTCHECK); + if (YAS_PCB_NO_ERROR == nRet) { + nRet = yas_pcb_calc_magnetic_field(&stOhxy1y2, + &stOhxyz); + if (YAS_PCB_NO_ERROR == nRet) { + *hx0 = stOhxy1y2.v[0]; + *hy0 = stOhxy1y2.v[1] - stOhxy1y2.v[2]; + *hz0 = -stOhxy1y2.v[1] - stOhxy1y2.v[2]; + nRet = yas_pcb_is_flow_occued(&gstXy1y2, + s32Underflow, s32Overflow); + if (YAS_PCB_NO_ERROR != nRet) { + if (gu08Recalc == 0) { + gu08Recalc++; + gs32RecalcWait = 0; + } + } + } + } + } else + nRet = YAS_PCB_ERROR_NOT_SUPPORTED; + + return nRet; +} + +#endif + +/* test 1 */ +static int power_on_and_device_check(int *id) +{ + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST1); + + if (YAS_PCB_NO_ERROR == result) { + if (id != NULL) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test1(id); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST1); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +} + +/* test 2 */ +static int power_off(void) +{ + int result = yas_pcb_check_state(YAS_PCB_TEST2); + + if (YAS_PCB_NO_ERROR == result) { + result = yas_pcb_test2(); + + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST2); + } + + return result; +} + +/* test 3 */ +static int initialization(void) +{ + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST3); + + if (YAS_PCB_NO_ERROR == result) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test3(); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST3); + } + + return result; +} + +/* test 4 */ +static int offset_control_measurement_and_set_offset_register(int *x, + int *y1, int *y2) +{ + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST4); + + if (YAS_PCB_NO_ERROR == result) { + if ((x != NULL) && (y1 != NULL) && (y2 != NULL)) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test4(x, y1, y2); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST4); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +} + +/* test 5 */ +static int direction_measurement(int *direction) +{ + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST5); + + if (YAS_PCB_NO_ERROR == result) { + if (direction != NULL) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test5(direction); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST5); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +} + +/* test 6 */ +static int sensitivity_measurement_of_magnetic_sensor_by_test_coil( + int *sx, int *sy) +{ + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST6); + + if (YAS_PCB_NO_ERROR == result) { + if ((sx != NULL) && (sy != NULL)) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test6(sx, sy); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST6); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +} + +/* test 7 */ +static int magnetic_field_level_check(int *ohx, int *ohy, int *ohz) +{ +#ifdef YAS_PCBTEST_EXTRA + int result = yas_pcb_check_state(YAS_PCB_TEST7); + + if (YAS_PCB_NO_ERROR == result) { + if ((ohx != NULL) && (ohy != NULL) && (ohz != NULL)) { + result = yas_pcb_test7(ohx, ohy, ohz); + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST7); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +#else + return YAS_PCB_ERROR_NOT_SUPPORTED; +#endif +} + +/* test 8 */ +static int noise_level_check(int *hx0, int *hy0, int *hz0) +{ +#ifdef YAS_PCBTEST_EXTRA + int ret; + int result = yas_pcb_check_state(YAS_PCB_TEST8); + + if (YAS_PCB_NO_ERROR == result) { + if ((hx0 != NULL) && (hy0 != NULL) && (hz0 != NULL)) { + result = YAS_PCB_ERROR_I2C; + ret = g_callback.i2c_open(); + if (0 == ret) { + result = yas_pcb_test8(hx0, hy0, hz0); + ret = g_callback.i2c_close(); + if (0 != ret) + result = YAS_PCB_ERROR_I2C; + } + if (YAS_PCB_NO_ERROR == result) + yas_pcb_update_state(YAS_PCB_TEST8); + } else + result = YAS_PCB_ERROR_ARG; + } + + return result; +#else + return YAS_PCB_ERROR_NOT_SUPPORTED; +#endif +} + +/* pcb test module initialize */ +int yas_pcb_test_init(struct yas_pcb_test *func) +{ + int result = YAS_PCB_ERROR_ARG; + + if ((NULL != func) + && (NULL != func->callback.i2c_open) + && (NULL != func->callback.i2c_close) + && (NULL != func->callback.i2c_write) + && (NULL != func->callback.i2c_read) + && (NULL != func->callback.msleep)) { + func->power_on_and_device_check = power_on_and_device_check; + func->initialization = initialization; + func->offset_control_measurement_and_set_offset_register + = offset_control_measurement_and_set_offset_register; + func->direction_measurement = direction_measurement; + func->sensitivity_measurement_of_magnetic_sensor_by_test_coil + = sensitivity_measurement_of_magnetic_sensor_by_test_coil; + func->magnetic_field_level_check = magnetic_field_level_check; + func->noise_level_check = noise_level_check; + func->power_off = power_off; + + g_callback = func->callback; + + if (0 != gu16State) { + gu16State = 0; + yas_pcb_power_off(); + } +#ifdef YAS_PCBTEST_EXTRA + gu08Recalc = 0; + gs32RecalcWait = 0; +#endif + result = YAS_PCB_NO_ERROR; + } + + return result; +} +/* end of file */ diff --git a/drivers/sensor/yas_pcb_test.h b/drivers/sensor/yas_pcb_test.h new file mode 100644 index 0000000..40d9dbd --- /dev/null +++ b/drivers/sensor/yas_pcb_test.h @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +/* + * File yas_pcb_test.h + * Date 2013/1/22 + * Revision 1.4.3 + */ + +#ifndef __YAS_PCB_TEST_H__ +#define __YAS_PCB_TEST_H__ + +#include "yas_types.h" + +/* extra */ +#define YAS_PCBTEST_EXTRA + +/* error code */ +#define YAS_PCB_NO_ERROR (0) +#define YAS_PCB_ERROR_I2C (-1) +#define YAS_PCB_ERROR_POWER (-2) +#define YAS_PCB_ERROR_TEST_ORDER (-3) +#define YAS_PCB_ERROR_INTERRUPT (-4) +#define YAS_PCB_ERROR_BUSY (-5) +#define YAS_PCB_ERROR_OVERFLOW (-6) +#define YAS_PCB_ERROR_UNDERFLOW (-7) +#define YAS_PCB_ERROR_DIRCALC (-8) +#define YAS_PCB_ERROR_NOT_SUPPORTED (-9) +#define YAS_PCB_ERROR_CALREG (-10) +#define YAS_PCB_ERROR_ARG (-128) + +/* addr */ +#define YAS_PCB_ADDR_SLAVE (0x2E) + +#define YAS_PCB_ADDR_ID (0x80) +#define YAS_PCB_ADDR_COIL (0x81) +#define YAS_PCB_ADDR_MEASURE_COMMAND (0x82) +#define YAS_PCB_ADDR_CONFIG (0x83) +#define YAS_PCB_ADDR_MEASURE_INTERVAL (0x84) +#define YAS_PCB_ADDR_OFFSET (0x85) +#define YAS_PCB_ADDR_TEST1 (0x88) +#define YAS_PCB_ADDR_TEST2 (0x89) +#define YAS_PCB_ADDR_CAL (0x90) +#define YAS_PCB_ADDR_MEASURE_DATA (0xB0) + +/* V Core */ +#define YAS_VCORE (18) +#define YAS_PCB_NOISE_OVERFLOW (6000) +#define YAS_PCB_NOISE_UNDERFLOW (2000) +#define YAS_PCB_NOISE_INTERVAL (50) + +struct yas_pcb_test_callback { + int (*power_on)(void); + int (*power_off)(void); + int (*i2c_open)(void); + int (*i2c_close)(void); + int (*i2c_write)(uint8_t, uint8_t, const uint8_t *, int); + int (*i2c_read)(uint8_t, uint8_t, uint8_t *, int); + void (*msleep)(int); + int (*read_intpin)(int *); +}; + +struct yas_pcb_test { + int (*power_on_and_device_check)(int *); + int (*initialization)(void); + int (*offset_control_measurement_and_set_offset_register) + (int *, int *, int *); + int (*direction_measurement)(int *); + int (*sensitivity_measurement_of_magnetic_sensor_by_test_coil) + (int *, int *); + int (*magnetic_field_level_check)(int *, int *, int *); + int (*noise_level_check)(int *, int *, int *); + int (*power_off)(void); + struct yas_pcb_test_callback callback; +}; + +/* prototype functions */ +#ifdef __cplusplus +extern "C" { +#endif + +int yas_pcb_test_init(struct yas_pcb_test *); + +#ifdef __cplusplus +} +#endif + +#endif /* ! __YAS_PCB_TEST_H__ */ + +/* end of file */ diff --git a/drivers/sensor/yas_types.h b/drivers/sensor/yas_types.h new file mode 100644 index 0000000..97aa3f3 --- /dev/null +++ b/drivers/sensor/yas_types.h @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +/* + * File yas_types.h + * Date 2012/10/05 + * Revision 1.4.1 + */ + +#ifndef __YAS_TYPES_H__ +#define __YAS_TYPES_H__ + +/* macro */ +#ifndef NULL +#define NULL ((void *)0) +#endif + +#if defined(__KERNEL__) +#include <linux/types.h> +#else +#include <stdint.h> +/*typedef signed char int8_t;*/ +/*typedef unsigned char uint8_t;*/ +/*typedef signed short int16_t;*/ +/*typedef unsigned short uint16_t;*/ +/*typedef signed int int32_t;*/ +/*typedef unsigned int uint32_t;*/ +#endif + +#endif /* __YASTYPES_H__ */ + +/* end of file */ |