void PulseAudioDriver::stream_write_callback(pa_stream* stream, size_t bytes, void* udata)
{
PulseAudioDriver* self = (PulseAudioDriver*)udata;
void* vdata;
pa_stream_begin_write(stream, &vdata, &bytes);
if (!vdata) return;
short* out = (short*)vdata;
unsigned num_samples = bytes / 4;
while (num_samples)
{
int n = std::min(self->m_buffer_size, num_samples);
self->m_callback(n, 0);
for (int i = 0; i < n; ++i)
{
*out++ = FLOAT_TO_SHORT(self->m_outL[i]);
*out++ = FLOAT_TO_SHORT(self->m_outR[i]);
}
num_samples -= n;
}
pa_stream_write(stream, vdata, (bytes / 4) * 4, 0, 0, PA_SEEK_RELATIVE);
}
/**
* Clear the accumulation buffer by mapping the renderbuffer and
* writing the clear color to it. Called by the driver's implementation
* of the glClear function.
*/
void
_mesa_clear_accum_buffer(struct gl_context *ctx)
{
GLuint x, y, width, height;
GLubyte *accMap;
GLint accRowStride;
struct gl_renderbuffer *accRb;
if (!ctx->DrawBuffer)
return;
accRb = ctx->DrawBuffer->Attachment[BUFFER_ACCUM].Renderbuffer;
if (!accRb)
return; /* missing accum buffer, not an error */
/* bounds, with scissor */
x = ctx->DrawBuffer->_Xmin;
y = ctx->DrawBuffer->_Ymin;
width = ctx->DrawBuffer->_Xmax - ctx->DrawBuffer->_Xmin;
height = ctx->DrawBuffer->_Ymax - ctx->DrawBuffer->_Ymin;
ctx->Driver.MapRenderbuffer(ctx, accRb, x, y, width, height,
GL_MAP_WRITE_BIT, &accMap, &accRowStride);
if (!accMap) {
_mesa_error(ctx, GL_OUT_OF_MEMORY, "glAccum");
return;
}
if (accRb->Format == MESA_FORMAT_SIGNED_RGBA_16) {
const GLshort clearR = FLOAT_TO_SHORT(ctx->Accum.ClearColor[0]);
const GLshort clearG = FLOAT_TO_SHORT(ctx->Accum.ClearColor[1]);
const GLshort clearB = FLOAT_TO_SHORT(ctx->Accum.ClearColor[2]);
const GLshort clearA = FLOAT_TO_SHORT(ctx->Accum.ClearColor[3]);
GLuint i, j;
for (j = 0; j < height; j++) {
GLshort *row = (GLshort *) accMap;
for (i = 0; i < width; i++) {
row[i * 4 + 0] = clearR;
row[i * 4 + 1] = clearG;
row[i * 4 + 2] = clearB;
row[i * 4 + 3] = clearA;
}
accMap += accRowStride;
}
}
else {
/* other types someday? */
_mesa_warning(ctx, "unexpected accum buffer type");
}
ctx->Driver.UnmapRenderbuffer(ctx, accRb);
}
/** For ADD/MULT */
static void
accum_mad(GLcontext *ctx, GLfloat scale, GLfloat bias,
GLint xpos, GLint ypos, GLint width, GLint height,
struct st_renderbuffer *acc_strb)
{
size_t stride = acc_strb->stride;
GLubyte *data = acc_strb->data;
switch (acc_strb->format) {
case PIPE_FORMAT_R16G16B16A16_SNORM:
{
int i, j;
for (i = 0; i < height; i++) {
GLshort *acc = (GLshort *) (data + (ypos + i) * stride + xpos * 8);
for (j = 0; j < width * 4; j++) {
float val = SHORT_TO_FLOAT(*acc) * scale + bias;
*acc++ = FLOAT_TO_SHORT(val);
}
}
}
break;
default:
_mesa_problem(NULL, "unexpected format in st_clear_accum_buffer()");
}
}
static int
wav_gsm610_write_f (SF_PRIVATE *psf, float *ptr, int len)
{ GSM610_PRIVATE *pgsm610 ;
short *sptr ;
int k, bufferlen, writecount = 0, count ;
int index = 0, total = 0 ;
float normfact ;
if (! psf->fdata)
return 0 ;
pgsm610 = (GSM610_PRIVATE*) psf->fdata ;
normfact = (psf->norm_float == SF_TRUE) ? ((float) 0x8000) : 1.0 ;
sptr = (short*) psf->buffer ;
bufferlen = ((SF_BUFFER_LEN / psf->blockwidth) * psf->blockwidth) / sizeof (short) ;
while (len > 0)
{ writecount = (len >= bufferlen) ? bufferlen : len ;
for (k = 0 ; k < writecount ; k++)
sptr [k] = FLOAT_TO_SHORT (normfact * ptr [index+k]) ;
count = wav_gsm610_write (psf, pgsm610, sptr, writecount) ;
index += writecount ;
total += count ;
len -= writecount ;
} ;
return total ;
} /* wav_gsm610_write_f */
void
st_clear_accum_buffer(GLcontext *ctx, struct gl_renderbuffer *rb)
{
struct st_renderbuffer *acc_strb = st_renderbuffer(rb);
const GLint xpos = ctx->DrawBuffer->_Xmin;
const GLint ypos = ctx->DrawBuffer->_Ymin;
const GLint width = ctx->DrawBuffer->_Xmax - xpos;
const GLint height = ctx->DrawBuffer->_Ymax - ypos;
size_t stride = acc_strb->stride;
GLubyte *data = acc_strb->data;
if(!data)
return;
switch (acc_strb->format) {
case PIPE_FORMAT_R16G16B16A16_SNORM:
{
GLshort r = FLOAT_TO_SHORT(ctx->Accum.ClearColor[0]);
GLshort g = FLOAT_TO_SHORT(ctx->Accum.ClearColor[1]);
GLshort b = FLOAT_TO_SHORT(ctx->Accum.ClearColor[2]);
GLshort a = FLOAT_TO_SHORT(ctx->Accum.ClearColor[3]);
int i, j;
for (i = 0; i < height; i++) {
GLshort *dst = (GLshort *) (data + (ypos + i) * stride + xpos * 8);
for (j = 0; j < width; j++) {
dst[0] = r;
dst[1] = g;
dst[2] = b;
dst[3] = a;
dst += 4;
}
}
}
break;
default:
_mesa_problem(ctx, "unexpected format in st_clear_accum_buffer()");
}
}
static void
accum_load(struct st_context *st, GLfloat value,
GLint xpos, GLint ypos, GLint width, GLint height,
struct st_renderbuffer *acc_strb,
struct st_renderbuffer *color_strb)
{
struct pipe_context *pipe = st->pipe;
struct pipe_screen *screen = pipe->screen;
struct pipe_transfer *color_trans;
size_t stride = acc_strb->stride;
GLubyte *data = acc_strb->data;
GLfloat *buf;
if (ST_DEBUG & DEBUG_FALLBACK)
debug_printf("%s: fallback processing\n", __FUNCTION__);
color_trans = st_cond_flush_get_tex_transfer(st, color_strb->texture,
0, 0, 0,
PIPE_TRANSFER_READ, xpos, ypos,
width, height);
buf = (GLfloat *) _mesa_malloc(width * height * 4 * sizeof(GLfloat));
pipe_get_tile_rgba(color_trans, 0, 0, width, height, buf);
switch (acc_strb->format) {
case PIPE_FORMAT_R16G16B16A16_SNORM:
{
const GLfloat *color = buf;
int i, j;
for (i = 0; i < height; i++) {
GLshort *acc = (GLshort *) (data + (ypos + i) * stride + xpos * 8);
for (j = 0; j < width * 4; j++) {
float val = *color++ * value;
*acc++ = FLOAT_TO_SHORT(val);
}
}
}
break;
default:
_mesa_problem(NULL, "unexpected format in st_clear_accum_buffer()");
}
_mesa_free(buf);
screen->tex_transfer_destroy(color_trans);
}
/**
* @brief If requested via inv_test_setup_accel(), test the accelerometer
* biases and calculate the necessary bias correction.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param enable_axis
* specify which axis has to be checked and corrected: provides
* a switch mode between 3 axis calibration and Z axis only
* calibration.
* @param bias
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements to store accel X, Y,
* and Z axis bias.
* @param gravity
* The gravity value given the parts' sensitivity: for example
* if the accelerometer is set to +/- 2 gee ==> the gravity
* value will be 2^14 = 16384.
* @param perform_full_test
* If 1:
* calculates offsets and noise and compare it against set
* thresholds. The final exist status will reflect if any of the
* value is outside of the expected range.
* When 0;
* skip the noise calculation and pass/fail assessment; simply
* calculates the accel biases.
*
* @return 0 on success. A non-zero error code on error.
*/
int test_accel(void *mlsl_handle, int enable_axes,
short *bias, long gravity,
uint_fast8_t perform_full_test)
{
short *p_vals;
float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f;
float rms[3];
float accel_rms_thresh = 1000000.f; /* enourmous to make the test always
passes - future deployment */
int accel_error = false;
const long sample_period = inv_get_sample_step_size_ms() * 1000;
int ii;
p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
/* collect the samples */
for(ii = 0; ii < test_setup.accel_samples; ii++) {
unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY;
int tries = 0;
long accel_data[3];
short *vals = &p_vals[3 * ii];
/* ignore data not ready errors but don't try more than 5 times */
while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) {
result = inv_get_accel_data(accel_data);
usleep(sample_period);
}
if (result || tries >= 5) {
MPL_LOGV("cannot reliably fetch data from the accelerometer");
accel_error = true;
goto accel_early_exit;
}
vals[X] = (short)accel_data[X];
vals[Y] = (short)accel_data[Y];
vals[Z] = (short)accel_data[Z];
avg[X] += 1.f * vals[X] / test_setup.accel_samples;
avg[Y] += 1.f * vals[Y] / test_setup.accel_samples;
avg[Z] += 1.f * vals[Z] / test_setup.accel_samples;
if (VERBOSE_OUT)
MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n",
vals[X], vals[Y], vals[Z]);
}
if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) {
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n",
avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity);
bias[X] = FLOAT_TO_SHORT(avg[X]);
bias[Y] = FLOAT_TO_SHORT(avg[Y]);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
bias[X], bias[Y], bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel correct.: "
"%+13.3f %+13.3f %+13.3f (gee)\n",
1.f * bias[X] / gravity,
1.f * bias[Y] / gravity,
1.f * bias[Z] / gravity);
if (perform_full_test) {
/* accel RMS - for now the threshold is only indicative */
for (ii = 0,
rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
ii < test_setup.accel_samples; ii++) {
short *vals = &p_vals[3 * ii];
rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]);
rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]);
rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]);
}
for (ii = 0; ii < 3; ii++) {
if (rms[ii] > accel_rms_thresh * accel_rms_thresh
* test_setup.accel_samples) {
MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
"(threshold = %.2f)\n", a_name[ii],
sqrt(rms[ii] / test_setup.accel_samples),
accel_rms_thresh);
accel_error = true;
goto accel_early_exit;
}
}
MPL_LOGI("Accel RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
sqrt(rms[X] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Y] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Z] / DEF_N_ACCEL_SAMPLES));
}
} else {
MPL_LOGI("Accel Z bias : %+13.3f (LSB)\n", avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z bias : %+13.3f (gee)\n", avg[Z] / gravity);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z correct.: "
"%+13.3f (gee)\n", 1.f * bias[Z] / gravity);
}
accel_early_exit:
if (accel_error) {
bias[0] = bias[1] = bias[2] = 0;
return (1); /* error */
}
inv_free(p_vals);
return (0); /* success */
}
/**
* @brief Test the gyroscope sensor.
* Implements the core logic of the MPU Self Test.
* Produces the PASS/FAIL result. Loads the calculated gyro biases
* and temperature datum into the corresponding pointers.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param gyro_biases
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements for gyro X, Y,
* and Z.
* @param temp_avg
* output pointer to store the initial average temperature as
* provided by the MPU Self Test.
* @param perform_full_test
* If 1:
* Complete calibration test:
* Calculate offset, drive frequency, and noise and compare it
* against set thresholds.
* When 0:
* Skip the noise and drive frequency calculation,
* simply calculate the gyro biases.
*
* @return 0 on success.
* On error, the return value is a bitmask representing:
* 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively
* (decimal values will be 1, 2, 4 respectively).
* 3, 4, 5 Excessive offset with X, Y, Z gyros respectively
* (decimal values will be 8, 16, 32 respectively).
* 6, 7, 8 Excessive noise with X, Y, Z gyros respectively
* (decimal values will be 64, 128, 256 respectively).
* 9 If any of the RMS noise values is zero, it may be
* due to a non-functional gyro or FIFO/register failure.
* (decimal value will be 512).
*/
int test_gyro(void *mlsl_handle,
short gyro_biases[3], short *temp_avg,
uint_fast8_t perform_full_test)
{
int ret_val = 0;
inv_error_t result;
int total_count = 0;
int total_count_axis[3] = {0, 0, 0};
int packet_count;
short x[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short y[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short z[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
int temperature = 0;
float avg[3];
float rms[3];
unsigned long test_start = inv_get_tick_count();
int i, j, tmp;
char tmpStr[200];
unsigned char regs[7] = {0};
/* make sure the DMP is disabled first */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* reset the gyro offset values */
regs[0] = MPUREG_XG_OFFS_USRH;
result = inv_serial_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
6, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* sample rate */
if (perform_full_test) {
/* = 8ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x07);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_CAL * test_setup.gyro_sens);
} else {
/* = 1ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x00);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_SELF * test_setup.gyro_sens);
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
switch (test_setup.gyro_fs) {
case 2000:
regs[0] |= 0x18;
break;
case 1000:
regs[0] |= 0x10;
break;
case 500:
regs[0] |= 0x08;
break;
case 250:
default:
regs[0] |= 0x00;
break;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_CONFIG, regs[0]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_INT_ENABLE, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* 1st, timing test */
for (j = 0; j < 3; j++) {
MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
/* turn on all gyros, use gyro X for clocking
Set to Y and Z for 2nd and 3rd iteration */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_PWR_MGMT_1, j + 1);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait for 2 ms after switching clock source */
usleep(2000);
/* enable & reset FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
tmp = test_setup.tests_per_axis;
while (tmp-- > 0) {
const unsigned char fifo_en_reg = MPUREG_FIFO_EN;
/* enable XYZ gyro in FIFO and nothing else */
result = inv_serial_single_write(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, fifo_en_reg,
BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait one period for data */
if (perform_full_test)
usleep(DEF_PERIOD_CAL * 1000);
else
usleep(DEF_PERIOD_SELF * 1000);
/* stop storing gyro in the FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
fifo_en_reg, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Getting number of bytes in FIFO */
result = inv_serial_read(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_COUNTH, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* number of 6 B packets in the FIFO */
packet_count = inv_big8_to_int16(dataout) / 6;
sprintf(tmpStr, "Packet Count: %d - ", packet_count);
if (abs(packet_count - test_setup.packet_thresh)
<= /* Within total_timing_tol % range, rounded up */
(int)(test_setup.total_timing_tol *
test_setup.packet_thresh + 1)) {
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result = inv_serial_read_fifo(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, 6, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
x[total_count + i] = inv_big8_to_int16(&dataout[0]);
y[total_count + i] = inv_big8_to_int16(&dataout[2]);
z[total_count + i] = inv_big8_to_int16(&dataout[4]);
if (VERBOSE_OUT) {
MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n",
total_count + i, x[total_count + i],
y[total_count + i], z[total_count + i]);
}
}
total_count += packet_count;
total_count_axis[j] += packet_count;
sprintf(tmpStr, "%sOK", tmpStr);
} else {
ret_val |= 1 << j;
sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
}
MPL_LOGI("%s\n", tmpStr);
}
/* remove gyros from FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_EN, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Read Temperature */
result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_TEMP_OUT_H, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
temperature += (short)inv_big8_to_int16(dataout);
}
MPL_LOGI("\n");
MPL_LOGI("Total %d samples\n", total_count);
MPL_LOGI("\n");
/* 2nd, check bias from X, Y, and Z PLL clock source */
tmp = total_count != 0 ? total_count : 1;
for (i = 0, avg[X] = .0f, avg[Y] = .0f, avg[Z] = .0f;
i < total_count; i++) {
avg[X] += 1.f * x[i] / tmp;
avg[Y] += 1.f * y[i] / tmp;
avg[Z] += 1.f * z[i] / tmp;
}
MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT) {
MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n",
avg[X] / adj_gyro_sens,
avg[Y] / adj_gyro_sens,
avg[Z] / adj_gyro_sens);
}
for (j = 0; j < 3; j++) {
if (fabs(avg[j]) > test_setup.bias_thresh) {
MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
"(threshold = %d)\n",
a_name[j], avg[j], test_setup.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 */
for (i = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
i < total_count; i++) {
rms[X] += (x[i] - avg[X]) * (x[i] - avg[X]);
rms[Y] += (y[i] - avg[Y]) * (y[i] - avg[Y]);
rms[Z] += (z[i] - avg[Z]) * (z[i] - avg[Z]);
}
if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0) {
ret_val |= 1 << 9;
}
/* 4th, temperature average */
temperature /= 3;
if (VERBOSE_OUT)
MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n",
(float)inv_decode_temperature(temperature) / (1L << 16),
"", "");
/* load into final storage */
*temp_avg = (short)temperature;
gyro_biases[X] = FLOAT_TO_SHORT(avg[X]);
gyro_biases[Y] = FLOAT_TO_SHORT(avg[Y]);
gyro_biases[Z] = FLOAT_TO_SHORT(avg[Z]);
MPL_LOGI("\n");
MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - test_start);
return ret_val;
}
