#include <fcntl.h>
#include <pthread.h>
#include <time.h>
+#include <math.h>
#include <sys/epoll.h>
#include <sys/socket.h>
#include <utils/Log.h>
* - a iio device number if the fd is a iio character device fd
* - THREAD_REPORT_TAG_BASE + sensor handle if the fd is the receiving end of a
* pipe used by a sysfs data acquisition thread
- * */
+ */
#define THREAD_REPORT_TAG_BASE 0x00010000
#define ENABLE_BUFFER_RETRIES 10
#define ENABLE_BUFFER_RETRY_DELAY_MS 10
-inline int is_enabled(int s)
+
+inline int is_enabled (int s)
{
- return (sensor[s].directly_enabled || sensor[s].ref_count);
+ return sensor[s].directly_enabled || sensor[s].ref_count;
}
-static int check_state_change(int s, int enabled, int from_virtual)
+
+static int check_state_change (int s, int enabled, int from_virtual)
{
- if(enabled) {
+ if (enabled) {
if (sensor[s].directly_enabled)
- return 0;
+ /*
+ * We're being enabled but already were
+ * directly activated: no change.
+ */
+ return 0;
- /* If we were enabled by Android no sample drops */
if (!from_virtual)
+ /* We're being directly enabled */
sensor[s].directly_enabled = 1;
- /*
- * If we got here it means we were not previously directly enabled - we may
- * or may not be now, whatever the case if we already had references we
- * were already in use
- */
if (sensor[s].ref_count)
+ /* We were already indirectly enabled */
return 0;
- return 1;
-
+ return 1; /* Do continue enabling this sensor */
}
- /* Spurious disable call */
+
if (!is_enabled(s))
+ /* We are being disabled but already were: no change */
return 0;
- /* We're requesting disable for a virtual sensor but the base is still active */
if (from_virtual && sensor[s].directly_enabled)
+ /* We're indirectly disabled but the base is still active */
return 0;
/* If it's disable, and it's from Android, and we still have ref counts */
/*If perhaps we are from virtual but we're disabling it*/
sensor[s].directly_enabled = 0;
- return 1;
+ return 1; /* Do continue disabling this sensor */
}
+
+
static int enable_buffer(int dev_num, int enabled)
{
char sysfs_path[PATH_MAX];
{
/*
* Adjust counters based on sensor enable action. Return values are:
- * -1 if there's an inconsistency: abort action in this case
* 0 if the operation was completed and we're all set
* 1 if we toggled the state of the sensor and there's work left
*/
int dev_num = sensor[s].dev_num;
if (!check_state_change(s, enabled, from_virtual))
+ /* The state of the sensor remains the same: we're done */
return 0;
if (enabled) {
if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD)
compass_store_data(&sensor[s]);
- if(sensor[s].type == SENSOR_TYPE_GYROSCOPE)
+ if (sensor[s].type == SENSOR_TYPE_GYROSCOPE)
gyro_store_data(&sensor[s]);
}
- /* We changed the state of a sensor - adjust per iio device counters */
- /* If this is a regular event-driven sensor */
+ /* We changed the state of a sensor: adjust device ref counts */
+
if (sensor[s].num_channels) {
if (enabled)
trig_sensors_per_dev[dev_num]--;
return 1;
- }
+ }
if (enabled) {
active_poll_sensors++;
return 1;
case SENSOR_TYPE_ROTATION_VECTOR:
- return 4;
+ return 4;
default:
ALOGE("Unknown sensor type!\n");
return NULL;
}
- ALOGI("Entering data acquisition thread S%d (%s): rate(%f), ts(%lld)\n", s,
- sensor[s].friendly_name, sensor[s].sampling_rate, sensor[s].report_ts);
+ ALOGI("Entering data acquisition thread S%d (%s), rate:%g\n",
+ s, sensor[s].friendly_name, sensor[s].sampling_rate);
if (sensor[s].sampling_rate <= 0) {
- ALOGE("Non-positive rate in acquisition routine for sensor %d: %f\n",
+ ALOGE("Invalid rate in acquisition routine for sensor %d: %g\n",
s, sensor[s].sampling_rate);
return NULL;
}
&data.timestamp, sample_size);
if (ret != sample_size)
- ALOGE("S%d acquisition thread: tried to write %d, ret: %d\n",
- s, sample_size, ret);
+ ALOGE("S%d write failure: wrote %d, got %d\n",
+ s, sample_size, ret);
}
/* Check and honor termination requests */
/* Recalculate period asumming sensor[s].sampling_rate
* can be changed dynamically during the thread run */
if (sensor[s].sampling_rate <= 0) {
- ALOGE("Non-positive rate in acquisition routine for sensor %d: %f\n",
+ ALOGE("Unexpected sampling rate for sensor %d: %g\n",
s, sensor[s].sampling_rate);
goto exit;
}
pthread_mutex_destroy(&thread_release_mutex[s]);
}
-static void sensor_activate_virtual(int s, int enabled, int from_virtual)
+
+static void sensor_activate_virtual (int s, int enabled, int from_virtual)
{
int i, base;
if (enabled) {
/* Enable all the base sensors for this virtual one */
for (i = 0; i < sensor[s].base_count; i++) {
- base = sensor[s].base_idx[i];
+ base = sensor[s].base[i];
sensor_activate(base, enabled, 1);
sensor[base].ref_count++;
}
sensor[s].report_pending = 0;
for (i = 0; i < sensor[s].base_count; i++) {
- base = sensor[s].base_idx[i];
+ base = sensor[s].base[i];
sensor_activate(base, enabled, 1);
sensor[base].ref_count--;
}
return 1;
}
+
static void tentative_switch_trigger (int s)
{
/*
setup_trigger(s, sensor[s].init_trigger_name);
}
-static int setup_delay_sysfs(int s, float new_sampling_rate)
+
+static int setup_delay_sysfs (int s, float requested_rate)
{
/* Set the rate at which a specific sensor should report events */
-
/* See Android sensors.h for indication on sensor trigger modes */
char sysfs_path[PATH_MAX];
char avail_sysfs_path[PATH_MAX];
- float cur_sampling_rate; /* Currently used sampling rate */
int dev_num = sensor[s].dev_num;
int i = sensor[s].catalog_index;
const char *prefix = sensor_catalog[i].tag;
int per_device_sampling_rate;
int32_t min_delay_us = sensor_desc[s].minDelay;
max_delay_t max_delay_us = sensor_desc[s].maxDelay;
- float min_supported_rate = max_delay_us ? (1000000.0 / max_delay_us) : 1;
+ float min_supported_rate = max_delay_us ? 1000000.0/max_delay_us : 1;
float max_supported_rate =
- (min_delay_us && min_delay_us != -1) ? (1000000.0 / min_delay_us) : 0;
+ min_delay_us && min_delay_us != -1 ? 1000000.0/min_delay_us : 0;
char freqs_buf[100];
char* cursor;
int n;
float sr;
+ float cur_sampling_rate; /* Currently used sampling rate */
+ float arb_sampling_rate; /* Granted sampling rate after arbitration */
- if (new_sampling_rate < min_supported_rate)
- new_sampling_rate = min_supported_rate;
+ ALOGV("Sampling rate %g requested on sensor %d (%s)\n", requested_rate,
+ s, sensor[s].friendly_name);
- if (max_supported_rate &&
- new_sampling_rate > max_supported_rate) {
- new_sampling_rate = max_supported_rate;
+ sensor[s].requested_rate = requested_rate;
+
+ arb_sampling_rate = requested_rate;
+
+ if (arb_sampling_rate < min_supported_rate) {
+ ALOGV("Sampling rate %g too low for %s, using %g instead\n",
+ arb_sampling_rate, sensor[s].friendly_name,
+ min_supported_rate);
+
+ arb_sampling_rate = min_supported_rate;
}
- sensor[s].sampling_rate = new_sampling_rate;
+ if (max_supported_rate && arb_sampling_rate > max_supported_rate) {
+ ALOGV("Sampling rate %g too high for %s, using %g instead\n",
+ arb_sampling_rate, sensor[s].friendly_name, max_supported_rate);
+ arb_sampling_rate = max_supported_rate;
+ }
+
+ sensor[s].sampling_rate = arb_sampling_rate;
/* If we're dealing with a poll-mode sensor */
if (!sensor[s].num_channels) {
- /* Interrupt current sleep so the new sampling gets used */
+ /* Wake up thread so the new sampling rate gets used */
pthread_cond_signal(&thread_release_cond[s]);
return 0;
}
if (n != s && sensor[n].dev_num == dev_num &&
sensor[n].num_channels &&
is_enabled(s) &&
- sensor[n].sampling_rate > new_sampling_rate)
- new_sampling_rate= sensor[n].sampling_rate;
+ sensor[n].sampling_rate > arb_sampling_rate)
+ arb_sampling_rate = sensor[n].sampling_rate;
/* Check if we have contraints on allowed sampling rates */
/* Decode a single value */
sr = strtod(cursor, NULL);
- /* If this matches the selected rate, we're happy */
- if (new_sampling_rate == sr)
+ /*
+ * If this matches the selected rate, we're happy.
+ * Have some tolerance to counter rounding errors and
+ * avoid needless jumps to higher rates.
+ */
+ if (fabs(arb_sampling_rate - sr) <= 0.001) {
+ arb_sampling_rate = sr;
break;
+ }
/*
* If we reached a higher value than the desired rate,
* assumption that rates are sorted by increasing value
* in the allowed frequencies string.
*/
- if (sr > new_sampling_rate) {
- new_sampling_rate = sr;
+ if (sr > arb_sampling_rate) {
+ arb_sampling_rate = sr;
break;
}
}
if (max_supported_rate &&
- new_sampling_rate > max_supported_rate) {
- new_sampling_rate = max_supported_rate;
+ arb_sampling_rate > max_supported_rate) {
+ arb_sampling_rate = max_supported_rate;
}
/* If the desired rate is already active we're all set */
- if (new_sampling_rate == cur_sampling_rate)
+ if (arb_sampling_rate == cur_sampling_rate)
return 0;
- ALOGI("Sensor %d sampling rate set to %g\n", s, new_sampling_rate);
+ ALOGI("Sensor %d (%s) sampling rate set to %g\n",
+ s, sensor[s].friendly_name, arb_sampling_rate);
if (trig_sensors_per_dev[dev_num])
enable_buffer(dev_num, 0);
- sysfs_write_float(sysfs_path, new_sampling_rate);
+ sysfs_write_float(sysfs_path, arb_sampling_rate);
/* Check if it makes sense to use an alternate trigger */
tentative_switch_trigger(s);
return 0;
}
+
+
/*
* We go through all the virtual sensors of the base - and the base itself
* in order to recompute the maximum requested delay of the group and setup the base
for (i = 0; i < sensor_count; i++) {
for (vidx = 0; vidx < sensor[i].base_count; vidx++)
/* If we have a virtual sensor depending on this one - handle it */
- if (sensor[i].base_idx[vidx] == s &&
+ if (sensor[i].base[vidx] == s &&
sensor[i].directly_enabled &&
sensor[i].requested_rate > arbitrated_rate)
arbitrated_rate = sensor[i].requested_rate;
return setup_delay_sysfs(s, arbitrated_rate);
}
+
/*
* Re-assesment for delays. We need to re-asses delays for all related groups
* of sensors everytime a sensor enables / disables / changes frequency.
-*/
+ */
int arbitrate_delays (int s)
{
int i;
}
/* Is virtual sensor - go through bases */
for (i = 0; i < sensor[s].base_count; i++)
- arbitrate_bases(sensor[s].base_idx[i]);
+ arbitrate_bases(sensor[s].base[i]);
return 0;
}
-int sensor_activate(int s, int enabled, int from_virtual)
+
+
+int sensor_activate (int s, int enabled, int from_virtual)
{
char device_name[PATH_MAX];
struct epoll_event ev = {0};
/* Prepare the report timestamp field for the first event, see set_report_ts method */
sensor[s].report_ts = 0;
+
ret = adjust_counters(s, enabled, from_virtual);
/* If the operation was neutral in terms of state, we're done */
enable_buffer(dev_num, 1);
}
-/* CTS acceptable thresholds:
+
+/*
+ * CTS acceptable thresholds:
* EventGapVerification.java: (th <= 1.8)
* FrequencyVerification.java: (0.9)*(expected freq) => (th <= 1.1111)
*/
return 0;
}
+
static int propagate_vsensor_report (int s, struct sensors_event_t *data)
{
/* There's a new report stored in sensor.sample for this sensor; transmit it */
return 1;
}
+
static int propagate_sensor_report (int s, struct sensors_event_t *data)
{
/* There's a sensor report pending for this sensor ; transmit it */
}
-int sensor_poll(struct sensors_event_t* data, int count)
+int sensor_poll (struct sensors_event_t* data, int count)
{
int s;
int i;
goto return_available_sensor_reports;
}
-int sensor_set_delay(int s, int64_t ns)
+
+int sensor_set_delay (int s, int64_t ns)
{
- float new_sampling_rate; /* Granted sampling rate after arbitration */
+ float requested_sampling_rate;
if (ns <= 0) {
- ALOGE("Rejecting non-positive delay request on sensor %d,required delay: %lld\n", s, ns);
+ ALOGE("Invalid delay requested on sensor %d: %lld\n", s, ns);
return -EINVAL;
}
- new_sampling_rate = 1000000000LL/ns;
+ requested_sampling_rate = 1000000000.0/ns;
- ALOGV("Entering set delay S%d (%s): old rate(%f), new rate(%f)\n",
+ ALOGV("Entering set delay S%d (%s): current rate: %f, requested: %f\n",
s, sensor[s].friendly_name, sensor[s].sampling_rate,
- new_sampling_rate);
+ requested_sampling_rate);
- sensor[s].requested_rate = new_sampling_rate;
+ sensor[s].requested_rate = requested_sampling_rate;
return arbitrate_delays(s);
}
+
int sensor_flush (int s)
{
/* If one shot or not enabled return -EINVAL */
return 0;
}
+
int allocate_control_data (void)
{
int i;
OSDN Git Service
