ssize_t mpu6500_gyro_selftest(char *buf, struct ssp_data *data)
{
char chTempBuf[2] = { 3, 200};
u8 initialized = 0;
s8 hw_result = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 shift_ratio[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iDelayCnt = 0, iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
int bias_thresh = DEF_BIAS_LSB_THRESH_SELF_6500;
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 150)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
initialized = data->uFactorydata[0];
shift_ratio[0] = (s16)((data->uFactorydata[2] << 8) +
data->uFactorydata[1]);
shift_ratio[1] = (s16)((data->uFactorydata[4] << 8) +
data->uFactorydata[3]);
shift_ratio[2] = (s16)((data->uFactorydata[6] << 8) +
data->uFactorydata[5]);
hw_result = (s8)data->uFactorydata[7];
total_count = (int)((data->uFactorydata[11] << 24) +
(data->uFactorydata[10] << 16) +
(data->uFactorydata[9] << 8) +
data->uFactorydata[8]);
avg[0] = (long)((data->uFactorydata[15] << 24) +
(data->uFactorydata[14] << 16) +
(data->uFactorydata[13] << 8) +
data->uFactorydata[12]);
avg[1] = (long)((data->uFactorydata[19] << 24) +
(data->uFactorydata[18] << 16) +
(data->uFactorydata[17] << 8) +
data->uFactorydata[16]);
avg[2] = (long)((data->uFactorydata[23] << 24) +
(data->uFactorydata[22] << 16) +
(data->uFactorydata[21] << 8) +
data->uFactorydata[20]);
rms[0] = (long)((data->uFactorydata[27] << 24) +
(data->uFactorydata[26] << 16) +
(data->uFactorydata[25] << 8) +
data->uFactorydata[24]);
rms[1] = (long)((data->uFactorydata[31] << 24) +
(data->uFactorydata[30] << 16) +
(data->uFactorydata[29] << 8) +
data->uFactorydata[28]);
rms[2] = (long)((data->uFactorydata[35] << 24) +
(data->uFactorydata[34] << 16) +
(data->uFactorydata[33] << 8) +
data->uFactorydata[32]);
pr_info("[SSP] init: %d, total cnt: %d\n", initialized, total_count);
pr_info("[SSP] hw_result: %d, %d, %d, %d\n", hw_result,
shift_ratio[0], shift_ratio[1], shift_ratio[2]);
pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_info("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
if (hw_result < 0) {
pr_err("[SSP] %s - hw selftest fail(%d), sw selftest skip\n",
__func__, hw_result);
return sprintf(buf, "-1,0,0,0,0,0,0,%d.%d,%d.%d,%d.%d,0,0,0\n",
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10);
}
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
if (data->ap_rev < 3)
bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
for (j = 0; j < 3; j++) {
if (unlikely(abs(avg[j]) > bias_thresh)) {
pr_err("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], bias_thresh);
ret_val |= 1 << (3 + j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (unlikely(rms[j] / total_count > DEF_RMS_THRESH)) {
pr_err("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
exit:
ssp_dbg("[SSP]: %s - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
__func__, ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
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;
}
ssize_t k330_gyro_selftest(char *buf, struct ssp_data *data)
{
char chTempBuf[2] = { 3, 200};
u8 uFifoPass = 2;
u8 uBypassPass = 2;
u8 uCalPass = 0;
u8 dummy[2] = {0,};
s16 iNOST[3] = {0,}, iST[3] = {0,}, iCalData[3] = {0,};
s16 iZeroRateData[3] = {0,}, fifo_data[4] = {0,};
int iDelayCnt = 0, iRet = 0;
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 250)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 250) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
mdelay(5);
iNOST[0] = (s16)((data->uFactorydata[0] << 8) + data->uFactorydata[1]);
iNOST[1] = (s16)((data->uFactorydata[2] << 8) + data->uFactorydata[3]);
iNOST[2] = (s16)((data->uFactorydata[4] << 8) + data->uFactorydata[5]);
iST[0] = (s16)((data->uFactorydata[6] << 8) + data->uFactorydata[7]);
iST[1] = (s16)((data->uFactorydata[8] << 8) + data->uFactorydata[9]);
iST[2] = (s16)((data->uFactorydata[10] << 8) + data->uFactorydata[11]);
iCalData[0] =
(s16)((data->uFactorydata[12] << 8) + data->uFactorydata[13]);
iCalData[1] =
(s16)((data->uFactorydata[14] << 8) + data->uFactorydata[15]);
iCalData[2] =
(s16)((data->uFactorydata[16] << 8) + data->uFactorydata[17]);
iZeroRateData[0] =
(s16)((data->uFactorydata[18] << 8) + data->uFactorydata[19]);
iZeroRateData[1] =
(s16)((data->uFactorydata[20] << 8) + data->uFactorydata[21]);
iZeroRateData[2] =
(s16)((data->uFactorydata[22] << 8) + data->uFactorydata[23]);
fifo_data[0] = data->uFactorydata[24];
fifo_data[1] =
(s16)((data->uFactorydata[25] << 8) + data->uFactorydata[26]);
fifo_data[2] =
(s16)((data->uFactorydata[27] << 8) + data->uFactorydata[28]);
fifo_data[3] =
(s16)((data->uFactorydata[29] << 8) + data->uFactorydata[30]);
uCalPass = data->uFactorydata[31];
uFifoPass = data->uFactorydata[32];
uBypassPass = data->uFactorydata[33];
dummy[0] = data->uFactorydata[34];
dummy[1] = data->uFactorydata[35];
pr_info("[SSP] %s dummy = 0x%X, 0x%X\n", __func__, dummy[0], dummy[1]);
if (uFifoPass && uBypassPass && uCalPass)
save_gyro_caldata(data, iCalData);
exit:
ssp_dbg("[SSP]: %s - %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
__func__, iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
fifo_data[0], fifo_data[1], fifo_data[2], fifo_data[3],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
fifo_data[0], fifo_data[1], fifo_data[2], fifo_data[3],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
}
static ssize_t mpu6500_gyro_selftest(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[36] = { 0,};
u8 initialized = 0;
s8 hw_result = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 shift_ratio[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
int bias_thresh = DEF_BIAS_LSB_THRESH_SELF_6500;
struct ssp_data *data = dev_get_drvdata(dev);
struct ssp_msg *msg = kzalloc(sizeof(*msg), GFP_KERNEL);
if (msg == NULL) {
pr_err("[SSP] %s, failed to alloc memory for ssp_msg\n", __func__);
goto exit;
}
msg->cmd = GYROSCOPE_FACTORY;
msg->length = 36;
msg->options = AP2HUB_READ;
msg->buffer = chTempBuf;
msg->free_buffer = 0;
iRet = ssp_spi_sync(data, msg, 7000);
if (iRet != SUCCESS) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
ret_val = 1;
goto exit;
}
data->uTimeOutCnt = 0;
pr_err("[SSP]%d %d %d %d %d %d %d %d %d %d %d %d", chTempBuf[0],
chTempBuf[1], chTempBuf[2], chTempBuf[3], chTempBuf[4],
chTempBuf[5], chTempBuf[6], chTempBuf[7], chTempBuf[8],
chTempBuf[9], chTempBuf[10], chTempBuf[11]);
initialized = chTempBuf[0];
shift_ratio[0] = (s16)((chTempBuf[2] << 8) +
chTempBuf[1]);
shift_ratio[1] = (s16)((chTempBuf[4] << 8) +
chTempBuf[3]);
shift_ratio[2] = (s16)((chTempBuf[6] << 8) +
chTempBuf[5]);
hw_result = (s8)chTempBuf[7];
total_count = (int)((chTempBuf[11] << 24) +
(chTempBuf[10] << 16) +
(chTempBuf[9] << 8) +
chTempBuf[8]);
avg[0] = (long)((chTempBuf[15] << 24) +
(chTempBuf[14] << 16) +
(chTempBuf[13] << 8) +
chTempBuf[12]);
avg[1] = (long)((chTempBuf[19] << 24) +
(chTempBuf[18] << 16) +
(chTempBuf[17] << 8) +
chTempBuf[16]);
avg[2] = (long)((chTempBuf[23] << 24) +
(chTempBuf[22] << 16) +
(chTempBuf[21] << 8) +
chTempBuf[20]);
rms[0] = (long)((chTempBuf[27] << 24) +
(chTempBuf[26] << 16) +
(chTempBuf[25] << 8) +
chTempBuf[24]);
rms[1] = (long)((chTempBuf[31] << 24) +
(chTempBuf[30] << 16) +
(chTempBuf[29] << 8) +
chTempBuf[28]);
rms[2] = (long)((chTempBuf[35] << 24) +
(chTempBuf[34] << 16) +
(chTempBuf[33] << 8) +
chTempBuf[32]);
pr_info("[SSP] init: %d, total cnt: %d\n", initialized, total_count);
pr_info("[SSP] hw_result: %d, %d, %d, %d\n", hw_result,
shift_ratio[0], shift_ratio[1], shift_ratio[2]);
pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_info("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
if (total_count == 0) {
pr_err("[SSP] %s, total_count is 0. goto exit\n", __func__);
ret_val = 2;
goto exit;
}
if (hw_result < 0) {
pr_err("[SSP] %s - hw selftest fail(%d), sw selftest skip\n",
__func__, hw_result);
return sprintf(buf, "-1,0,0,0,0,0,0,%d.%d,%d.%d,%d.%d,0,0,0\n",
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10);
}
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (unlikely(abs(avg[j]) > bias_thresh)) {
pr_err("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], bias_thresh);
ret_val |= 1 << (3 + j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (unlikely(rms[j] / total_count > DEF_RMS_THRESH)) {
pr_err("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
exit:
ssp_dbg("[SSP]: %s - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
__func__, ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
static ssize_t bmi058_gyro_selftest_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[19] = { 0,};
u8 bist=0, selftest = 0;
int datax_check = 0;
int datay_check = 0;
int dataz_check = 0;
s16 iCalData[3] = {0,};
int iRet = 0;
struct ssp_data *data = dev_get_drvdata(dev);
struct ssp_msg *msg = kzalloc(sizeof(*msg), GFP_KERNEL);
if (msg == NULL) {
pr_err("[SSP] %s, failed to alloc memory for ssp_msg\n", __func__);
goto exit;
}
msg->cmd = GYROSCOPE_FACTORY;
msg->length = 19;
msg->options = AP2HUB_READ;
msg->buffer = chTempBuf;
msg->free_buffer = 0;
iRet = ssp_spi_sync(data, msg, 3000);
if (iRet != SUCCESS) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
data->uTimeOutCnt = 0;
pr_err("[SSP]%d %d %d %d %d %d %d %d %d %d %d %d %d\n", chTempBuf[0],
chTempBuf[1], chTempBuf[2], chTempBuf[3], chTempBuf[4],
chTempBuf[5], chTempBuf[6], chTempBuf[7], chTempBuf[8],
chTempBuf[9], chTempBuf[10], chTempBuf[11], chTempBuf[12]);
/* Should I get bist? */
selftest = chTempBuf[0];
if (selftest == 0)
bist = 1;
else
bist =0;
datax_check = (int)((chTempBuf[4] << 24) + (chTempBuf[3] <<16)
+(chTempBuf[2] << 8) + chTempBuf[1]);
datay_check = (int)((chTempBuf[8] << 24) + (chTempBuf[7] <<16)
+(chTempBuf[6] << 8) + chTempBuf[5]);
dataz_check = (int)((chTempBuf[12] << 24) + (chTempBuf[11] <<16)
+(chTempBuf[10] << 8) + chTempBuf[9]);
iCalData[0] = (s16)((chTempBuf[14] << 8) +
chTempBuf[13]);
iCalData[1] = (s16)((chTempBuf[16] << 8) +
chTempBuf[15]);
iCalData[2] = (s16)((chTempBuf[18] << 8) +
chTempBuf[17]);
//shift_ratio[1] = (s16)((chTempBuf[4] << 8) +chTempBuf[3]);
//total_count = (int)((chTempBuf[11] << 24) +(chTempBuf[10] << 16) + (chTempBuf[9] << 8) +chTempBuf[8]);
pr_info("[SSP] gyro bist: %d, selftest: %d\n", bist, selftest);
pr_info("[SSP] gyro X: %d, Y: %d, Z: %d\n", datax_check, datay_check, dataz_check);
pr_info("[SSP] iCalData X: %d, Y: %d, Z: %d\n", iCalData[0], iCalData[1], iCalData[2]);
//pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
if ((datax_check <= SELFTEST_LIMITATION_OF_ERROR)
&& (datay_check <= SELFTEST_LIMITATION_OF_ERROR)
&& (dataz_check <= SELFTEST_LIMITATION_OF_ERROR)) {
pr_info("[SENSOR]: %s - Gyro zero rate OK!- Gyro selftest Pass\n", __func__);
//bmg160_get_caldata(data);
save_gyro_caldata(data, iCalData);
} else {
pr_info("[SENSOR]: %s - Gyro zero rate NG!- Gyro selftest fail!\n", __func__);
selftest |= 1;
}
#if 0 /*Do Not saveing gyro cal after selftest NOW at BMI058*/
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
#endif
exit:
pr_info("%d,%d,%d.%03d,%d.%03d,%d.%03d\n",
selftest ? 0 : 1, bist,
(datax_check / 1000), (datax_check % 1000),
(datay_check / 1000), (datay_check % 1000),
(dataz_check / 1000), (dataz_check % 1000));
return sprintf(buf, "%d,%d,%d.%03d,%d.%03d,%d.%03d," "%d,%d,%d,%d,%d,%d,%d,%d" "\n",
selftest ? 0 : 1, bist,
(datax_check / 1000), (datax_check % 1000),
(datay_check / 1000), (datay_check % 1000),
(dataz_check / 1000), (dataz_check % 1000),
iRet ,iRet ,iRet ,iRet ,iRet ,iRet ,iRet ,iRet);
}
static ssize_t k6ds3tr_gyro_selftest(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[36] = { 0,};
u8 initialized = 0;
s8 hw_result = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0, gyro_lib_dl_fail = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 shift_ratio[3] = {0,}; //self_diff value
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
int bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
int fifo_ret = 0;
int cal_ret = 0;
struct ssp_data *data = dev_get_drvdata(dev);
s16 st_zro[3] = {0, };
s16 st_bias[3] = {0, };
int gyro_fifo_avg[3] = {0,}, gyro_self_zro[3] = {0,};
int gyro_self_bias[3] = {0,}, gyro_self_diff[3] = {0,};
struct ssp_msg *msg = kzalloc(sizeof(*msg), GFP_KERNEL);
if (msg == NULL) {
pr_err("[SSP] %s, failed to alloc memory for ssp_msg\n", __func__);
goto exit;
}
msg->cmd = GYROSCOPE_FACTORY;
msg->length = 36;
msg->options = AP2HUB_READ;
msg->buffer = chTempBuf;
msg->free_buffer = 0;
iRet = ssp_spi_sync(data, msg, 7000);
if (iRet != SUCCESS) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
ret_val = 1;
goto exit;
}
data->uTimeOutCnt = 0;
pr_err("[SSP]%d %d %d %d %d %d %d %d %d %d %d %d\n", chTempBuf[0],
chTempBuf[1], chTempBuf[2], chTempBuf[3], chTempBuf[4],
chTempBuf[5], chTempBuf[6], chTempBuf[7], chTempBuf[8],
chTempBuf[9], chTempBuf[10], chTempBuf[11]);
initialized = chTempBuf[0];
shift_ratio[0] = (s16)((chTempBuf[2] << 8) +
chTempBuf[1]);
shift_ratio[1] = (s16)((chTempBuf[4] << 8) +
chTempBuf[3]);
shift_ratio[2] = (s16)((chTempBuf[6] << 8) +
chTempBuf[5]);
hw_result = (s8)chTempBuf[7];
total_count = (int)((chTempBuf[11] << 24) +
(chTempBuf[10] << 16) +
(chTempBuf[9] << 8) +
chTempBuf[8]);
avg[0] = (long)((chTempBuf[15] << 24) +
(chTempBuf[14] << 16) +
(chTempBuf[13] << 8) +
chTempBuf[12]);
avg[1] = (long)((chTempBuf[19] << 24) +
(chTempBuf[18] << 16) +
(chTempBuf[17] << 8) +
chTempBuf[16]);
avg[2] = (long)((chTempBuf[23] << 24) +
(chTempBuf[22] << 16) +
(chTempBuf[21] << 8) +
chTempBuf[20]);
rms[0] = (long)((chTempBuf[27] << 24) +
(chTempBuf[26] << 16) +
(chTempBuf[25] << 8) +
chTempBuf[24]);
rms[1] = (long)((chTempBuf[31] << 24) +
(chTempBuf[30] << 16) +
(chTempBuf[29] << 8) +
chTempBuf[28]);
rms[2] = (long)((chTempBuf[35] << 24) +
(chTempBuf[34] << 16) +
(chTempBuf[33] << 8) +
chTempBuf[32]);
st_zro[0] = (s16)((chTempBuf[25] << 8) +
chTempBuf[24]);
st_zro[1] = (s16)((chTempBuf[27] << 8) +
chTempBuf[26]);
st_zro[2] = (s16)((chTempBuf[29] << 8) +
chTempBuf[28]);
st_bias[0] = (s16)((chTempBuf[31] << 8) +
chTempBuf[30]);
st_bias[1] = (s16)((chTempBuf[33] << 8) +
chTempBuf[32]);
st_bias[2] = (s16)((chTempBuf[35] << 8) +
chTempBuf[34]);
pr_info("[SSP] init: %d, total cnt: %d\n", initialized, total_count);
pr_info("[SSP] hw_result: %d, %d, %d, %d\n", hw_result,
shift_ratio[0], shift_ratio[1], shift_ratio[2]);
pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_info("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
//FIFO ZRO check pass / fail
gyro_fifo_avg[0] = avg[0] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_fifo_avg[1] = avg[1] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_fifo_avg[2] = avg[2] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
// ZRO self test
gyro_self_zro[0] = st_zro[0] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_zro[1] = st_zro[1] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_zro[2] = st_zro[2] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
//bias
gyro_self_bias[0] = st_bias[0] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_bias[1] = st_bias[1] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_bias[2] = st_bias[2] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
//diff = bias - ZRO
gyro_self_diff[0] = shift_ratio[0] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_diff[1] = shift_ratio[1] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
gyro_self_diff[2] = shift_ratio[2] * DEF_GYRO_SENS / DEF_SCALE_FOR_FLOAT;
if (total_count != 128) {
pr_err("[SSP] %s, total_count is not 128. goto exit\n", __func__);
ret_val = 2;
goto exit;
}
else
cal_ret = fifo_ret = 1;
if (hw_result < 0) {
pr_err("[SSP] %s - hw selftest fail(%d), sw selftest skip\n",
__func__, hw_result);
if (shift_ratio[0] == GYRO_LIB_DL_FAIL &&
shift_ratio[1] == GYRO_LIB_DL_FAIL &&
shift_ratio[2] == GYRO_LIB_DL_FAIL) {
pr_err("[SSP] %s - gyro lib download fail\n", __func__);
gyro_lib_dl_fail = 1;
} else {
/*
ssp_dbg("[SSP]: %s - %d,%d,%d fail.\n",
__func__,
shift_ratio[0] / 10,
shift_ratio[1] / 10,
shift_ratio[2] / 10);
return sprintf(buf, "%d,%d,%d\n",
shift_ratio[0] / 10,
shift_ratio[1] / 10,
shift_ratio[2] / 10);
*/
ssp_dbg("[SSP]: %s - %d,%d,%d fail.\n", __func__, gyro_self_diff[0], gyro_self_diff[1], gyro_self_diff[2]);
return sprintf(buf, "%d,%d,%d\n", gyro_self_diff[0], gyro_self_diff[1], gyro_self_diff[2]);
}
}
// AVG value range test +/- 40
if( (ABS(gyro_fifo_avg[0]) > 40) || (ABS(gyro_fifo_avg[1]) > 40) || (ABS(gyro_fifo_avg[2]) > 40) )
{
ssp_dbg("[SSP]: %s - %d,%d,%d fail.\n", __func__, gyro_fifo_avg[0], gyro_fifo_avg[1], gyro_fifo_avg[2]);
return sprintf(buf, "%d,%d,%d\n", gyro_fifo_avg[0], gyro_fifo_avg[1], gyro_fifo_avg[2]);
}
// STMICRO
gyro_bias[0] = avg[0] * DEF_GYRO_SENS;
gyro_bias[1] = avg[1] * DEF_GYRO_SENS;
gyro_bias[2] = avg[2] * DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (unlikely(abs(avg[j]) > bias_thresh)) {
pr_err("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], bias_thresh);
ret_val |= 1 << (3 + j);
}
}
// STMICRO
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
//if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
//ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mk6ds3tr_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (gyro_lib_dl_fail) {
pr_err("[SSP] gyro_lib_dl_fail, Don't save cal data\n");
ret_val = -1;
goto exit;
}
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
exit:
ssp_dbg("[SSP]: %s - "
"%d,%d,%d,"
"%d,%d,%d,"
"%d,%d,%d,"
"%d,%d,%d,%d,%d\n",
__func__,
gyro_fifo_avg[0], gyro_fifo_avg[1], gyro_fifo_avg[2],
gyro_self_zro[0], gyro_self_zro[1], gyro_self_zro[2],
gyro_self_bias[0], gyro_self_bias[1], gyro_self_bias[2],
gyro_self_diff[0], gyro_self_diff[1], gyro_self_diff[2],
fifo_ret,
cal_ret);
// Gyro Calibration pass / fail, buffer 1~6 values.
if( (fifo_ret == 0) || (cal_ret == 0) )
return sprintf(buf, "%d,%d,%d\n", gyro_self_diff[0], gyro_self_diff[1], gyro_self_diff[2]);
return sprintf(buf,
"%d,%d,%d,"
"%d,%d,%d,"
"%d,%d,%d,"
"%d,%d,%d,%d,%d\n",
gyro_fifo_avg[0], gyro_fifo_avg[1], gyro_fifo_avg[2],
gyro_self_zro[0], gyro_self_zro[1], gyro_self_zro[2],
gyro_self_bias[0], gyro_self_bias[1], gyro_self_bias[2],
gyro_self_diff[0], gyro_self_diff[1], gyro_self_diff[2],
fifo_ret,
cal_ret);
}
static ssize_t gyro_selftest_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[2] = { 3, 200};
u8 uFifoPass = 2;
u8 uBypassPass = 2;
u8 uCalPass = 0;
s16 iNOST[3] = {0,}, iST[3] = {0,}, iCalData[3] = {0,};
int iZeroRateData[3] = {0,};
int iDelayCnt = 0, iRet = 0;
struct ssp_data *data = dev_get_drvdata(dev);
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 150)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
iNOST[0] = (s16)((data->uFactorydata[0] << 8) + data->uFactorydata[1]);
iNOST[1] = (s16)((data->uFactorydata[2] << 8) + data->uFactorydata[3]);
iNOST[2] = (s16)((data->uFactorydata[4] << 8) + data->uFactorydata[5]);
iST[0] = (s16)((data->uFactorydata[6] << 8) + data->uFactorydata[7]);
iST[1] = (s16)((data->uFactorydata[8] << 8) + data->uFactorydata[9]);
iST[2] = (s16)((data->uFactorydata[10] << 8) + data->uFactorydata[11]);
iCalData[0] =
(s16)((data->uFactorydata[12] << 8) + data->uFactorydata[13]);
iCalData[1] =
(s16)((data->uFactorydata[14] << 8) + data->uFactorydata[15]);
iCalData[2] =
(s16)((data->uFactorydata[16] << 8) + data->uFactorydata[17]);
uCalPass = data->uFactorydata[18];
uFifoPass = data->uFactorydata[19];
uBypassPass = data->uFactorydata[20];
if (uFifoPass && uBypassPass && uCalPass)
save_gyro_caldata(data, iCalData);
iZeroRateData[0] = (int)(iCalData[0] * 175) / 10000;
iZeroRateData[1] = (int)(iCalData[1] * 175) / 10000;
iZeroRateData[2] = (int)(iCalData[2] * 175) / 10000;
exit:
ssp_dbg("[SSP]: Gyro Selftest - %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
}
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;
}
ssize_t k330_gyro_selftest(char *buf, struct ssp_data *data)
{
char chTempBuf[36] = { 0,};
u8 uFifoPass = 2;
u8 uBypassPass = 2;
u8 uCalPass = 0;
u8 dummy[2] = {0,};
s16 iNOST[3] = {0,}, iST[3] = {0,}, iCalData[3] = {0,};
s16 iZeroRateData[3] = {0,}, fifo_data[4] = {0,};
int iRet = 0;
struct ssp_msg *msg = kzalloc(sizeof(*msg), GFP_KERNEL);
msg->cmd = GYROSCOPE_FACTORY;
msg->length = 36;
msg->options = AP2HUB_READ;
msg->buffer = chTempBuf;
msg->free_buffer = 0;
iRet = ssp_spi_sync(data, msg, 5000);
if (iRet != SUCCESS) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
data->uTimeOutCnt = 0;
iNOST[0] = (s16)((chTempBuf[0] << 8) + chTempBuf[1]);
iNOST[1] = (s16)((chTempBuf[2] << 8) + chTempBuf[3]);
iNOST[2] = (s16)((chTempBuf[4] << 8) + chTempBuf[5]);
iST[0] = (s16)((chTempBuf[6] << 8) + chTempBuf[7]);
iST[1] = (s16)((chTempBuf[8] << 8) + chTempBuf[9]);
iST[2] = (s16)((chTempBuf[10] << 8) + chTempBuf[11]);
iCalData[0] = (s16)((chTempBuf[12] << 8) + chTempBuf[13]);
iCalData[1] =( s16)((chTempBuf[14] << 8) + chTempBuf[15]);
iCalData[2] = (s16)((chTempBuf[16] << 8) + chTempBuf[17]);
iZeroRateData[0] = (s16)((chTempBuf[18] << 8) + chTempBuf[19]);
iZeroRateData[1] = (s16)((chTempBuf[20] << 8) + chTempBuf[21]);
iZeroRateData[2] = (s16)((chTempBuf[22] << 8) + chTempBuf[23]);
fifo_data[0] = chTempBuf[24];
fifo_data[1] = (s16)((chTempBuf[25] << 8) + chTempBuf[26]);
fifo_data[2] = (s16)((chTempBuf[27] << 8) + chTempBuf[28]);
fifo_data[3] = (s16)((chTempBuf[29] << 8) + chTempBuf[30]);
uCalPass = chTempBuf[31];
uFifoPass = chTempBuf[32];
uBypassPass = chTempBuf[33];
dummy[0] = chTempBuf[34];
dummy[1] = chTempBuf[35];
pr_info("[SSP] %s dummy = 0x%X, 0x%X\n", __func__, dummy[0], dummy[1]);
if (uFifoPass && uBypassPass && uCalPass)
save_gyro_caldata(data, iCalData);
ssp_dbg("[SSP]: %s - %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
__func__, iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
fifo_data[0], fifo_data[1], fifo_data[2], fifo_data[3],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
exit:
return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
iNOST[0], iNOST[1], iNOST[2], iST[0], iST[1], iST[2],
iZeroRateData[0], iZeroRateData[1], iZeroRateData[2],
fifo_data[0], fifo_data[1], fifo_data[2], fifo_data[3],
uFifoPass & uBypassPass & uCalPass, uFifoPass, uCalPass);
}
static ssize_t gyro_selftest_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[2] = { 3, 200};
u8 initialized = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iDelayCnt = 0, iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
struct ssp_data *data = dev_get_drvdata(dev);
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 150)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
for (i = 0; i < 29; i++)
pr_err("[SSP]: %s - uFactorydata[%d] = 0x%x\n", __func__, i,
data->uFactorydata[i]);
initialized = data->uFactorydata[0];
total_count = (int)((data->uFactorydata[4] << 24) +
(data->uFactorydata[3] << 16) +
(data->uFactorydata[2] << 8) +
data->uFactorydata[1]);
avg[0] = (long)((data->uFactorydata[8] << 24) +
(data->uFactorydata[7] << 16) +
(data->uFactorydata[6] << 8) +
data->uFactorydata[5]);
avg[1] = (long)((data->uFactorydata[12] << 24) +
(data->uFactorydata[11] << 16) +
(data->uFactorydata[10] << 8) +
data->uFactorydata[9]);
avg[2] = (long)((data->uFactorydata[16] << 24) +
(data->uFactorydata[15] << 16) +
(data->uFactorydata[14] << 8) +
data->uFactorydata[13]);
rms[0] = (long)((data->uFactorydata[20] << 24) +
(data->uFactorydata[19] << 16) +
(data->uFactorydata[18] << 8) +
data->uFactorydata[17]);
rms[1] = (long)((data->uFactorydata[24] << 24) +
(data->uFactorydata[23] << 16) +
(data->uFactorydata[22] << 8) +
data->uFactorydata[21]);
rms[2] = (long)((data->uFactorydata[28] << 24) +
(data->uFactorydata[27] << 16) +
(data->uFactorydata[26] << 8) +
data->uFactorydata[25]);
pr_err("[SSP] init: %d, total cnt: %d", initialized, total_count);
pr_err("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_err("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
/*
avg[0] /= total_count;
avg[1] /= total_count;
avg[2] /= total_count;
*/
pr_info("[SSP] bias : %+8ld %+8ld %+8ld (LSB)\n",
avg[0], avg[1], avg[2]);
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (abs(avg[j]) > DEF_BIAS_LSB_THRESH_SELF) {
pr_info("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], DEF_BIAS_LSB_THRESH_SELF);
ret_val |= 1 << (3 + j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (rms[j] / total_count > DEF_RMS_THRESH) {
pr_info("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (!ret_val) {
pr_err("[SSP] ret_val == 0\n");
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
/*initialized = -1;*/
}
exit:
ssp_dbg("[SSP]: Gyro Selftest - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
