inline void update_step_time (long long diff, struct det_time_struct* det_time, unsigned long current_clock) {
unsigned long actual_step_time;
unsigned long count = current_clock - det_time->last_clock;
actual_step_time = div64_long (diff, count);
DPRINT ("update_step_time: diff:%lld, count:%ld, step_time:%ld, threshold:%ld, actual_step_time:%ld\n", diff, count, det_time->step_time, det_time->threshold, actual_step_time);
det_time->threshold = actual_step_time*9/10;
det_time->step_time = actual_step_time*9/10;
}
static void get_timestamp(struct ssp_data *data, int iSensorData,
struct sensor_value *sensorsdata, struct ssp_time_diff *sensortime)
{
if (((sensortime->irq_diff * 10) >
(data->adDelayBuf[iSensorData] * 18))
&& ((sensortime->irq_diff * 10) <
(data->adDelayBuf[iSensorData] * 100))) {
u64 move_timestamp;
u64 shift_timestamp =
div64_long(data->adDelayBuf[iSensorData], 2);
for (move_timestamp = data->lastTimestamp[iSensorData] +
data->adDelayBuf[iSensorData];
move_timestamp < (data->timestamp - shift_timestamp);
move_timestamp += data->adDelayBuf[iSensorData]) {
sensorsdata->timestamp = move_timestamp;
data->report_sensor_data[iSensorData](data,
sensorsdata);
}
}
sensorsdata->timestamp = data->timestamp;
}
int parse_dataframe(struct ssp_data *data, char *pchRcvDataFrame, int iLength)
{
int iDataIdx, iSensorData;
u16 length = 0;
struct sensor_value sensorsdata;
struct ssp_time_diff sensortime;
sensortime.time_diff = 0;
data->uIrqCnt++;
for (iDataIdx = 0; iDataIdx < iLength;) {
switch (pchRcvDataFrame[iDataIdx++]) {
case MSG2AP_INST_BYPASS_DATA:
iSensorData = pchRcvDataFrame[iDataIdx++];
if ((iSensorData < 0) || (iSensorData >= SENSOR_MAX)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d\n", __func__,
iSensorData);
return ERROR;
}
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 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[iSensorData];
if (sensortime.batch_mode == BATCH_MODE_RUN) {
if (data->reportedData[iSensorData] == true) {
u64 time;
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[iSensorData]), (s64)length);
if (length > 8)
time = data->adDelayBuf[iSensorData] * 18;
else if (length > 4)
time = data->adDelayBuf[iSensorData] * 25;
else if (length > 2)
time = data->adDelayBuf[iSensorData] * 50;
else
time = data->adDelayBuf[iSensorData] * 100;
if ((sensortime.time_diff * 10) > time) {
data->lastTimestamp[iSensorData] = data->timestamp - (data->adDelayBuf[iSensorData] * length);
sensortime.time_diff = data->adDelayBuf[iSensorData];
} else {
time = data->adDelayBuf[iSensorData] * 18;
if ((sensortime.time_diff * 10) > time)
sensortime.time_diff = data->adDelayBuf[iSensorData];
}
} else {
if (data->lastTimestamp[iSensorData] < (data->timestamp - (data->adDelayBuf[iSensorData] * length))) {
data->lastTimestamp[iSensorData] = data->timestamp - (data->adDelayBuf[iSensorData] * length);
sensortime.time_diff = data->adDelayBuf[iSensorData];
} else
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[iSensorData]), (s64)length);
}
} else {
if (data->reportedData[iSensorData] == false)
sensortime.irq_diff = data->adDelayBuf[iSensorData];
}
do {
data->get_sensor_data[iSensorData](pchRcvDataFrame, &iDataIdx, &sensorsdata);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata, &sensortime, iSensorData);
if (sensortime.irq_diff > 1000000)
data->report_sensor_data[iSensorData](data, &sensorsdata);
else if ((iSensorData == PROXIMITY_SENSOR) || (iSensorData == PROXIMITY_RAW)
|| (iSensorData == GESTURE_SENSOR) || (iSensorData == SIG_MOTION_SENSOR))
data->report_sensor_data[iSensorData](data, &sensorsdata);
else
pr_err("[SSP]: %s irq_diff is under 1msec (%d)\n", __func__, iSensorData);
sensortime.batch_count--;
} while ((sensortime.batch_count > 0) && (iDataIdx < iLength));
if (sensortime.batch_count > 0)
pr_err("[SSP]: %s batch count error (%d)\n", __func__, sensortime.batch_count);
data->lastTimestamp[iSensorData] = data->timestamp;
data->reportedData[iSensorData] = true;
break;
case MSG2AP_INST_DEBUG_DATA:
iSensorData = print_mcu_debug(pchRcvDataFrame, &iDataIdx, iLength);
if (iSensorData) {
pr_err("[SSP]: %s - Mcu data frame3 error %d\n", __func__,
iSensorData);
return ERROR;
}
break;
case MSG2AP_INST_LIBRARY_DATA:
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
ssp_sensorhub_handle_data(data, pchRcvDataFrame, iDataIdx,
iDataIdx + length);
iDataIdx += length;
break;
case MSG2AP_INST_BIG_DATA:
handle_big_data(data, pchRcvDataFrame, &iDataIdx);
break;
case MSG2AP_INST_META_DATA:
sensorsdata.meta_data.what = pchRcvDataFrame[iDataIdx++];
sensorsdata.meta_data.sensor = pchRcvDataFrame[iDataIdx++];
report_meta_data(data, &sensorsdata);
break;
case MSG2AP_INST_TIME_SYNC:
data->bTimeSyncing = true;
break;
}
}
return SUCCESS;
}
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;
}
int parse_dataframe(struct ssp_data *data, char *pchRcvDataFrame, int iLength)
{
int iDataIdx;
int sensor_type;
u16 length = 0;
struct sensor_value sensorsdata;
s16 caldata[3] = { 0, };
#ifdef CONFIG_SENSORS_SSP_HIFI_BATCHING // HIFI batch
u16 batch_event_count;
u16 batch_mode;
u64 ts = 0;
#else
struct ssp_time_diff sensortime;
sensortime.time_diff = 0;
#endif
data->uIrqCnt++;
for (iDataIdx = 0; iDataIdx < iLength;) {
switch (pchRcvDataFrame[iDataIdx++]) {
#ifdef CONFIG_SENSORS_SSP_HIFI_BATCHING // HIFI batch
case MSG2AP_INST_BYPASS_DATA:
sensor_type = pchRcvDataFrame[iDataIdx++];
if ((sensor_type < 0) || (sensor_type >= SENSOR_MAX)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d\n", __func__,
sensor_type);
return ERROR;
}
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
batch_event_count = length;
batch_mode = length > 1 ? BATCH_MODE_RUN : BATCH_MODE_NONE;
//pr_err("[SSP]: %s batch count (%d)\n", __func__, batch_event_count);
// TODO: When batch_event_count = 0, we should not run.
do {
data->get_sensor_data[sensor_type](pchRcvDataFrame, &iDataIdx, &sensorsdata);
// TODO: Integrate get_sensor_data function.
// TODO: get_sensor_data(pchRcvDataFrame, &iDataIdx, &sensorsdata, data->sensor_data_size[sensor_type]);
// TODO: Divide control data batch and non batch.
data->skipEventReport = false;
//if(sensor_type == GYROSCOPE_SENSOR) //check packet recieve time
// pr_err("[SSP_PARSE] bbd event time %lld\n", data->timestamp);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata, batch_mode, sensor_type);
if (data->skipEventReport == false)
data->report_sensor_data[sensor_type](data, &sensorsdata);
batch_event_count--;
//pr_err("[SSP]: %s batch count (%d)\n", __func__, batch_event_count);
} while ((batch_event_count > 0) && (iDataIdx < iLength));
if (batch_event_count > 0)
pr_err("[SSP]: %s batch count error (%d)\n", __func__, batch_event_count);
data->reportedData[sensor_type] = true;
//pr_err("[SSP]: (%d / %d)\n", iDataIdx, iLength);
ts = get_current_timestamp();
if(ts > 10000000000ULL + data->lastTimestamp[sensor_type]) {
data->lastTimestamp[sensor_type] = ts;
pr_err("[SSP_PARSE] %d late 10 sec sync to %lld\n",sensor_type, ts);
}
break;
case MSG2AP_INST_DEBUG_DATA:
sensor_type = print_mcu_debug(pchRcvDataFrame, &iDataIdx, iLength);
if (sensor_type) {
pr_err("[SSP]: %s - Mcu data frame3 error %d\n", __func__,
sensor_type);
return ERROR;
}
break;
#else
case MSG2AP_INST_BYPASS_DATA:
sensor_type = pchRcvDataFrame[iDataIdx++];
if ((sensor_type < 0) || (sensor_type >= SENSOR_MAX)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d\n", __func__,
sensor_type);
return ERROR;
}
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 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_type];
if (sensortime.batch_mode == BATCH_MODE_RUN) {
if (data->reportedData[sensor_type] == true) {
u64 time;
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor_type]), (s64)length);
if (length > 8)
time = data->adDelayBuf[sensor_type] * 18;
else if (length > 4)
time = data->adDelayBuf[sensor_type] * 25;
else if (length > 2)
time = data->adDelayBuf[sensor_type] * 50;
else
time = data->adDelayBuf[sensor_type] * 130;
if ((sensortime.time_diff * 10) > time) {
data->lastTimestamp[sensor_type] = data->timestamp - (data->adDelayBuf[sensor_type] * length);
sensortime.time_diff = data->adDelayBuf[sensor_type];
} else {
time = data->adDelayBuf[sensor_type] * 11;
if ((sensortime.time_diff * 10) > time)
sensortime.time_diff = data->adDelayBuf[sensor_type];
}
} else {
if (data->lastTimestamp[sensor_type] < (data->timestamp - (data->adDelayBuf[sensor_type] * length))) {
data->lastTimestamp[sensor_type] = data->timestamp - (data->adDelayBuf[sensor_type] * length);
sensortime.time_diff = data->adDelayBuf[sensor_type];
} else
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor_type]), (s64)length);
}
} else {
if (data->reportedData[sensor_type] == false)
sensortime.irq_diff = data->adDelayBuf[sensor_type];
}
do {
data->get_sensor_data[sensor_type](pchRcvDataFrame, &iDataIdx, &sensorsdata);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata, &sensortime, sensor_type);
if (sensortime.irq_diff > 1000000)
data->report_sensor_data[sensor_type](data, &sensorsdata);
else if ((sensor_type == PROXIMITY_SENSOR) || (sensor_type == PROXIMITY_RAW)
|| (sensor_type == STEP_COUNTER) || (sensor_type == STEP_DETECTOR)
|| (sensor_type == GESTURE_SENSOR) || (sensor_type == SIG_MOTION_SENSOR))
data->report_sensor_data[sensor_type](data, &sensorsdata);
else
pr_err("[SSP]: %s irq_diff is under 1msec (%d)\n", __func__, sensor_type);
sensortime.batch_count--;
} while ((sensortime.batch_count > 0) && (iDataIdx < iLength));
if (sensortime.batch_count > 0)
pr_err("[SSP]: %s batch count error (%d)\n", __func__, sensortime.batch_count);
data->lastTimestamp[sensor_type] = data->timestamp;
data->reportedData[sensor_type] = true;
break;
case MSG2AP_INST_DEBUG_DATA:
sensor_type = print_mcu_debug(pchRcvDataFrame, &iDataIdx, iLength);
if (sensor_type) {
pr_err("[SSP]: %s - Mcu data frame3 error %d\n", __func__,
sensor_type);
return ERROR;
}
break;
#endif
case MSG2AP_INST_LIBRARY_DATA:
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
ssp_sensorhub_handle_data(data, pchRcvDataFrame, iDataIdx,
iDataIdx + length);
iDataIdx += length;
break;
case MSG2AP_INST_BIG_DATA:
handle_big_data(data, pchRcvDataFrame, &iDataIdx);
break;
case MSG2AP_INST_META_DATA:
sensorsdata.meta_data.what = pchRcvDataFrame[iDataIdx++];
sensorsdata.meta_data.sensor = pchRcvDataFrame[iDataIdx++];
report_meta_data(data, &sensorsdata);
break;
case MSG2AP_INST_TIME_SYNC:
data->bTimeSyncing = true;
break;
case MSG2AP_INST_GYRO_CAL:
pr_info("[SSP]: %s - Gyro caldata received from MCU\n", __func__);
memcpy(caldata, pchRcvDataFrame + iDataIdx, sizeof(caldata));
wake_lock(&data->ssp_wake_lock);
save_gyro_caldata(data, caldata);
wake_unlock(&data->ssp_wake_lock);
iDataIdx += sizeof(caldata);
break;
case SH_MSG2AP_GYRO_CALIBRATION_EVENT_OCCUR:
data->gyro_lib_state = GYRO_CALIBRATION_STATE_EVENT_OCCUR;
break;
}
}
return SUCCESS;
}
