int parse_dataframe(struct ssp_data *data, char *pchRcvDataFrame, int iLength) {
int iDataIdx, iSensorData;
u16 length = 0;
struct sensor_value sensorsdata;
struct timespec ts;
getnstimeofday(&ts);
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;
}
data->get_sensor_data[iSensorData](pchRcvDataFrame, &iDataIdx,
&sensorsdata);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata);
data->report_sensor_data[iSensorData](data, &sensorsdata);
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;
case MSG2AP_INST_RESET:
queue_refresh_task(data, 0);
break;
}
}
if (data->bTimeSyncing)
data->timestamp = ts.tv_sec * 1000000000ULL + ts.tv_nsec;
return SUCCESS;
}
static void work_function_firmware_update(struct work_struct *work)
{
struct ssp_data *data = container_of((struct delayed_work *)work,
struct ssp_data, work_firmware);
int iRet;
ssp_infof();
iRet = forced_to_download_binary(data, KERNEL_BINARY);
if (iRet < 0) {
ssp_infof("forced_to_download_binary failed!");
data->uSensorState = 0;
return;
}
queue_refresh_task(data, SSP_SW_RESET_TIME);
}
static void work_function_firmware_update(struct work_struct *work)
{
struct ssp_data *data = container_of((struct delayed_work *)work,
struct ssp_data, work_firmware);
int iRet = 0;
pr_info("[SSP] : %s\n", __func__);
iRet = forced_to_download_binary(data, KERNEL_BINARY);
if (iRet < 0) {
ssp_dbg("[SSP]: %s - forced_to_download_binary failed!\n",
__func__);
return;
}
if (data->uCurFirmRev == SSP_INVALID_REVISION)
queue_refresh_task(data, SSP_SW_RESET_TIME);
}
void reset_mcu(struct ssp_data *data) {
#if SSP_STATUS_MONITOR
data->bRefreshing = true;
#endif
func_dbg();
ssp_enable(data, false);
clean_pending_list(data);
#if SSP_STATUS_MONITOR
if( (data->reg_hub) && ((current_cable_type==POWER_SUPPLY_TYPE_MAINS)
|| (current_cable_type==POWER_SUPPLY_TYPE_HV_MAINS)))
toggle_mcu_hw_reset(data);
else
#endif
toggle_mcu_reset(data);
ssp_enable(data, true);
#if SSP_STATUS_MONITOR
if((data->reg_hub) && ( (current_cable_type==POWER_SUPPLY_TYPE_MAINS)
|| (current_cable_type==POWER_SUPPLY_TYPE_HV_MAINS)))
queue_refresh_task(data, 0);
#endif
}
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;
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;
case MSG2AP_INST_RESET:
queue_refresh_task(data, 0);
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;
}
