void sync_sensor_state(struct ssp_data *data)
{
unsigned char uBuf[9] = {0,};
unsigned int uSensorCnt;
int iRet = 0;
iRet = set_gyro_cal(data);
if (iRet < 0)
ssp_errf("set_gyro_cal failed");
iRet = set_accel_cal(data);
if (iRet < 0)
ssp_errf("set_accel_cal failed");
udelay(10);
for (uSensorCnt = 0; uSensorCnt < SENSOR_MAX; uSensorCnt++) {
if (atomic64_read(&data->aSensorEnable) & (1 << uSensorCnt)) {
s32 dMsDelay
= get_msdelay(data->adDelayBuf[uSensorCnt]);
memcpy(&uBuf[0], &dMsDelay, 4);
memcpy(&uBuf[4], &data->batchLatencyBuf[uSensorCnt], 4);
uBuf[8] = data->batchOptBuf[uSensorCnt];
send_instruction(data, ADD_SENSOR, uSensorCnt, uBuf, 9);
udelay(10);
}
}
if (data->bProximityRawEnabled == true) {
s32 dMsDelay = 20;
memcpy(&uBuf[0], &dMsDelay, 4);
send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, uBuf, 4);
}
set_proximity_threshold(data, data->uProxHiThresh, data->uProxLoThresh);
data->buf[PROXIMITY_SENSOR].prox = 0;
report_sensordata(data, PROXIMITY_SENSOR, &data->buf[PROXIMITY_SENSOR]);
#if 1
if(sec_debug_get_debug_level() > 0)
{
data->bMcuDumpMode = true;
ssp_info("Mcu Dump Enabled");
}
iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,
data->bMcuDumpMode);
if (iRet < 0)
ssp_errf("MSG2SSP_AP_MCU_SET_DUMPMODE failed");
#else
#if CONFIG_SEC_DEBUG
data->bMcuDumpMode = sec_debug_is_enabled();
iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,
data->bMcuDumpMode);
if (iRet < 0)
ssp_errf("MSG2SSP_AP_MCU_SET_DUMPMODE failed");
#endif
#endif
}
static void generate_data(struct ssp_data *data, struct sensor_value *sensorsdata,
int sensor, u64 timestamp)
{
u64 move_timestamp = data->lastTimestamp[sensor];
if ((sensor != PROXIMITY_SENSOR) && (sensor != GESTURE_SENSOR)
&& (sensor != STEP_DETECTOR) && (sensor != SIG_MOTION_SENSOR)
&& (sensor != STEP_COUNTER)) {
while ((move_timestamp * 10 + data->adDelayBuf[sensor] * 15) < (timestamp * 10)) {
move_timestamp += data->adDelayBuf[sensor];
sensorsdata->timestamp = move_timestamp;
report_sensordata(data, sensor, sensorsdata);
}
}
}
int parse_dataframe(struct ssp_data *data, char *dataframe, int frame_len)
{
struct sensor_value sensorsdata;
struct ssp_time_diff sensortime;
int sensor, index;
u16 length = 0;
s16 caldata[3] = { 0, };
memset(&sensorsdata, 0, sizeof(sensorsdata));
for (index = 0; index < frame_len;) {
switch (dataframe[index++]) {
case MSG2AP_INST_BYPASS_DATA:
sensor = dataframe[index++];
if ((sensor < 0) || (sensor >= SENSOR_MAX)) {
ssp_errf("Mcu bypass dataframe err %d", sensor);
return ERROR;
}
memcpy(&length, dataframe + index, 2);
index += 2;
sensortime.batch_count = sensortime.batch_count_fixed = length;
sensortime.batch_mode = length > 1 ? BATCH_MODE_RUN : BATCH_MODE_NONE;
sensortime.irq_diff = data->timestamp - data->lastTimestamp[sensor];
if (sensortime.batch_mode == BATCH_MODE_RUN) {
if (data->reportedData[sensor] == true) {
u64 time;
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor]), (s64)length);
if (length > 8)
time = data->adDelayBuf[sensor] * 18;
else if (length > 4)
time = data->adDelayBuf[sensor] * 25;
else if (length > 2)
time = data->adDelayBuf[sensor] * 50;
else
time = data->adDelayBuf[sensor] * 100;
if ((sensortime.time_diff * 10) > time) {
data->lastTimestamp[sensor] = data->timestamp - (data->adDelayBuf[sensor] * length);
sensortime.time_diff = data->adDelayBuf[sensor];
} else {
time = data->adDelayBuf[sensor] * 11;
if ((sensortime.time_diff * 10) > time)
sensortime.time_diff = data->adDelayBuf[sensor];
}
} else {
if (data->lastTimestamp[sensor] < (data->timestamp - (data->adDelayBuf[sensor] * length))) {
data->lastTimestamp[sensor] = data->timestamp - (data->adDelayBuf[sensor] * length);
sensortime.time_diff = data->adDelayBuf[sensor];
} else
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor]), (s64)length);
}
} else {
if (data->reportedData[sensor] == false)
sensortime.irq_diff = data->adDelayBuf[sensor];
}
do {
get_sensordata(data, dataframe, &index,
sensor, &sensorsdata);
get_timestamp(data, dataframe, &index, &sensorsdata, &sensortime, sensor);
if (sensortime.irq_diff > 1000000)
report_sensordata(data, sensor, &sensorsdata);
else if ((sensor == PROXIMITY_SENSOR) || (sensor == PROXIMITY_RAW)
|| (sensor == GESTURE_SENSOR) || (sensor == SIG_MOTION_SENSOR))
report_sensordata(data, sensor, &sensorsdata);
else
ssp_errf("irq_diff is under 1msec (%d)", sensor);
sensortime.batch_count--;
} while ((sensortime.batch_count > 0) && (index < frame_len));
if (sensortime.batch_count > 0)
ssp_errf("batch count error (%d)", sensortime.batch_count);
data->lastTimestamp[sensor] = data->timestamp;
data->reportedData[sensor] = true;
break;
case MSG2AP_INST_DEBUG_DATA:
sensor = print_mcu_debug(dataframe, &index, frame_len);
if (sensor) {
ssp_errf("Mcu debug dataframe err %d", sensor);
return ERROR;
}
break;
case MSG2AP_INST_LIBRARY_DATA:
memcpy(&length, dataframe + index, 2);
index += 2;
ssp_sensorhub_handle_data(data, dataframe, index,
index + length);
index += length;
break;
case MSG2AP_INST_BIG_DATA:
handle_big_data(data, dataframe, &index);
break;
case MSG2AP_INST_META_DATA:
sensorsdata.meta_data.what = dataframe[index++];
sensorsdata.meta_data.sensor = dataframe[index++];
report_meta_data(data, META_SENSOR, &sensorsdata);
break;
case MSG2AP_INST_TIME_SYNC:
data->bTimeSyncing = true;
break;
case MSG2AP_INST_RESET:
ssp_infof("Reset MSG received from MCU");
queue_refresh_task(data, 0);
break;
case MSG2AP_INST_GYRO_CAL:
ssp_infof("Gyro caldata received from MCU");
memcpy(caldata, dataframe + index, sizeof(caldata));
wake_lock(&data->ssp_wake_lock);
save_gyro_caldata(data, caldata);
wake_unlock(&data->ssp_wake_lock);
index += sizeof(caldata);
break;
case MSG2AP_INST_DUMP_DATA:
debug_crash_dump(data, dataframe, frame_len);
return SUCCESS;
break;
}
}
return SUCCESS;
}
