static int
geomagnetic_i2c_read(uint8_t slave, uint8_t addr, uint8_t *buf, int len)
{
struct i2c_msg msg[2];
int err;
msg[0].addr = slave;
msg[0].flags = 0;
msg[0].len = 1;
msg[0].buf = &addr;
msg[1].addr = slave;
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", slave, addr, err);
return err;
}
#if DEBUG
if (len == 1) {
YLOGD(("[R] addr[%02x] [%02x]\n", addr, buf[0]));
}
else if (len == 6) {
YLOGD(("[R] addr[%02x] "
"[%02x%02x%02x%02x%02x%02x]\n",
addr, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]));
}
else if (len == 8) {
YLOGD(("[R] addr[%02x] "
"[%02x%02x%02x%02x%02x%02x%02x%02x]\n",
addr, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]));
}
else if (len == 9) {
YLOGD(("[R] addr[%02x] "
"[%02x%02x%02x%02x%02x%02x%02x%02x%02x]\n",
addr, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8]));
}
else if (len == 16) {
YLOGD(("[R] addr[%02x] "
"[%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x]\n",
addr,
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7],
buf[8], buf[9], buf[10], buf[11], buf[12], buf[13], buf[14], buf[15]));
}
#endif
return 0;
}
static int
resume(void)
{
/* implement resume of the sensor */
YLOGD(("%s: resume\n", SENSOR_NAME));
if (strcmp(SENSOR_NAME, "gyroscope") == 0) {
/* resume gyroscope */
}
else if (strcmp(SENSOR_NAME, "light") == 0) {
/* resume light */
}
else if (strcmp(SENSOR_NAME, "pressure") == 0) {
/* resume pressure */
}
else if (strcmp(SENSOR_NAME, "temperature") == 0) {
/* resume temperature */
}
else if (strcmp(SENSOR_NAME, "proximity") == 0) {
/* resume proximity */
}
#if DEBUG
{
struct sensor_data *data = input_get_drvdata(this_data);
data->suspend = 0;
}
#endif /* DEBUG */
return 0;
}
static int32_t kc_itg3500_reset( void )
{
int32_t reset = atomic_read( &at_device_reset );
if(reset == 1) {
YLOGD(("%s(): reset start\n",__func__));
atomic_set( &at_device_reset, 2 );
sensor_power_reset(SENSOR_INDEX_GYRO);
reset = atomic_read( &at_device_reset );
if (reset != 3)
atomic_set( &at_device_reset, 0 );
YLOGD(("%s(): reset end reset=%d\n",__func__,reset));
}
return YAS_NO_ERROR;
}
static int
geomagnetic_i2c_write(uint8_t slave, 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 slave, uint8_t *buf, int len)
{
if (i2c_master_recv(this_client, buf, len) < 0) {
return -1;
}
#if DEBUG
if (len == 1) {
YLOGD(("[R] [%02x]\n", buf[0]));
}
else if (len == 6) {
YLOGD(("[R] "
"[%02x%02x%02x%02x%02x%02x]\n",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]));
}
else if (len == 8) {
YLOGD(("[R] "
"[%02x%02x%02x%02x%02x%02x%02x%02x]\n",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]));
}
else if (len == 9) {
YLOGD(("[R] "
"[%02x%02x%02x%02x%02x%02x%02x%02x%02x]\n",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8]));
}
else if (len == 16) {
YLOGD(("[R] "
"[%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x]\n",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7],
buf[8], buf[9], buf[10], buf[11], buf[12], buf[13], buf[14], buf[15]));
}
#endif
return 0;
}
static int
geomagnetic_i2c_write(uint8_t slave, 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
suspend(void)
{
/* implement suspend of the sensor */
YLOGD(("%s: suspend\n", SENSOR_NAME));
if (strcmp(SENSOR_NAME, "gyroscope") == 0) {
/* suspend gyroscope */
}
else if (strcmp(SENSOR_NAME, "light") == 0) {
/* suspend light */
}
else if (strcmp(SENSOR_NAME, "pressure") == 0) {
/* suspend pressure */
}
else if (strcmp(SENSOR_NAME, "temperature") == 0) {
/* suspend temperature */
}
else if (strcmp(SENSOR_NAME, "proximity") == 0) {
/* suspend proximity */
}
return 0;
}
static int
geomagnetic_work(struct yas_mag_data *magdata)
{
struct geomagnetic_data *data = i2c_get_clientdata(this_client);
uint32_t time_delay_ms = 100;
struct yas_mag_offset offset;
int rt, i, accuracy;
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(&offset);
geomagnetic_multi_lock();
data->driver_offset = offset;
if (rt & YAS_REPORT_OVERFLOW_OCCURED) {
atomic_set(&data->last_status, 0);
accuracy = 0;
}
geomagnetic_multi_unlock();
/* 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);
input_report_abs(data->input_raw, ABS_RAW_REPORT, value);
}
if (rt & YAS_REPORT_DATA) {
for (i = 0; i < 3; i++) {
atomic_set(&data->last_data[i], magdata->xyz.v[i]);
}
/* 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]);
input_report_abs(data->input_data, ABS_STATUS, accuracy);
input_sync(data->input_data);
}
/* 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);
}
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 = 100;
struct yas_mag_offset offset;
struct yas_mag_data magdata;
int rt, i, accuracy;
if (hwdep_driver.measure == NULL || hwdep_driver.get_offset == NULL) {
return;
}
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(&offset);
geomagnetic_multi_lock();
data->driver_offset = offset;
if (rt & YAS_REPORT_OVERFLOW_OCCURED) {
atomic_set(&data->last_status, 0);
accuracy = 0;
}
geomagnetic_multi_unlock();
/* 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);
input_report_abs(data->input_raw, ABS_RAW_REPORT, value);
}
if (rt & YAS_REPORT_DATA) {
for (i = 0; i < 3; i++) {
atomic_set(&data->last_data[i], magdata.xyz.v[i]);
}
/* 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]);
input_report_abs(data->input_data, ABS_STATUS, accuracy);
input_sync(data->input_data);
}
if (rt & YAS_REPORT_CALIB) {
/* 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);
}
}
else {
time_delay_ms = 100;
}
if (time_delay_ms > 0) {
schedule_delayed_work(&data->work, msecs_to_jiffies(time_delay_ms) + 1);
}
else {
schedule_delayed_work(&data->work, 0);
}
}