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; }
void ssp_batch_report(struct ssp_data *data) { u8 sensor_type = 0; struct sensor_value sensor_data; int idx_data = 0; int count = 0; u64 timestamp = get_current_timestamp(); ssp_dbg("[SSP_BAT] LENGTH = %d, start index = %d ts %lld\n", data->batch_event.batch_length, idx_data, timestamp); while (idx_data < data->batch_event.batch_length) { //ssp_dbg("[SSP_BAT] bcnt %d\n", count); sensor_type = data->batch_event.batch_data[idx_data++]; if(sensor_type == META_SENSOR) { sensor_data.meta_data.sensor = data->batch_event.batch_data[idx_data++]; report_meta_data(data, &sensor_data); count++; continue; } if ((sensor_type != ACCELEROMETER_SENSOR) && (sensor_type != GEOMAGNETIC_UNCALIB_SENSOR) && (sensor_type != PRESSURE_SENSOR) && (sensor_type != GAME_ROTATION_VECTOR) && (sensor_type != PROXIMITY_SENSOR)) { pr_err("[SSP]: %s - Mcu data frame1 error %d, idx_data %d\n", __func__, sensor_type, idx_data - 1); return ; } if(count%80 == 0) usleep_range(1000,1000); //ssp_dbg("[SSP_BAT] cnt %d\n", count); data->get_sensor_data[sensor_type](data->batch_event.batch_data, &idx_data, &sensor_data); data->skipEventReport = false; get_timestamp(data, data->batch_event.batch_data, &idx_data, &sensor_data, BATCH_MODE_RUN, sensor_type); if (data->skipEventReport == false) { data->report_sensor_data[sensor_type](data, &sensor_data); } data->reportedData[sensor_type] = true; count++; } ssp_dbg("[SSP_BAT] max cnt %d\n", count); }
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; }