util/nanotool/contexthub.cpp

/*
 * Copyright (C) 2016 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "contexthub.h"

#include <cstring>
#include <errno.h>
#include <vector>

#include "apptohostevent.h"
#include "log.h"
#include "resetreasonevent.h"
#include "sensorevent.h"
#include "util.h"

namespace android {

#define UNUSED_PARAM(param) (void) (param)

constexpr int kCalibrationTimeoutMs(10000);
constexpr int kTestTimeoutMs(10000);
constexpr int kBridgeVersionTimeoutMs(500);

struct SensorTypeNames {
    SensorType sensor_type;
    const char *name_abbrev;
};

static const SensorTypeNames sensor_names_[] = {
    { SensorType::Accel,                "accel" },
    { SensorType::AnyMotion,            "anymo" },
    { SensorType::NoMotion,             "nomo" },
    { SensorType::SignificantMotion,    "sigmo" },
    { SensorType::Flat,                 "flat" },
    { SensorType::Gyro,                 "gyro" },
    //{ SensorType::GyroUncal,            "gyro_uncal" },
    { SensorType::Magnetometer,         "mag" },
    //{ SensorType::MagnetometerUncal,    "mag_uncal" },
    { SensorType::Barometer,            "baro" },
    { SensorType::Temperature,          "temp" },
    { SensorType::AmbientLightSensor,   "als" },
    { SensorType::Proximity,            "prox" },
    { SensorType::Orientation,          "orien" },
    //{ SensorType::HeartRateECG,         "ecg" },
    //{ SensorType::HeartRatePPG,         "ppg" },
    { SensorType::Gravity,              "gravity" },
    { SensorType::LinearAccel,          "linear_acc" },
    { SensorType::RotationVector,       "rotation" },
    { SensorType::GeomagneticRotationVector, "geomag" },
    { SensorType::GameRotationVector,   "game" },
    { SensorType::StepCount,            "step_cnt" },
    { SensorType::StepDetect,           "step_det" },
    { SensorType::Gesture,              "gesture" },
    { SensorType::Tilt,                 "tilt" },
    { SensorType::DoubleTwist,          "twist" },
    { SensorType::DoubleTap,            "doubletap" },
    { SensorType::WindowOrientation,    "win_orien" },
    { SensorType::Hall,                 "hall" },
    { SensorType::Activity,             "activity" },
    { SensorType::Vsync,                "vsync" },
    { SensorType::WristTilt,            "wrist_tilt" },
};

struct SensorTypeAlias {
    SensorType sensor_type;
    SensorType sensor_alias;
    const char *name_abbrev;
};

static const SensorTypeAlias sensor_aliases_[] = {
    { SensorType::Accel, SensorType::CompressedAccel, "compressed_accel" },
};

bool SensorTypeIsAliasOf(SensorType sensor_type, SensorType alias) {
    for (size_t i = 0; i < ARRAY_LEN(sensor_aliases_); i++) {
        if (sensor_aliases_[i].sensor_type == sensor_type
                && sensor_aliases_[i].sensor_alias == alias) {
            return true;
        }
    }

    return false;
}

SensorType ContextHub::SensorAbbrevNameToType(const char *sensor_name_abbrev) {
    for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
        if (strcmp(sensor_names_[i].name_abbrev, sensor_name_abbrev) == 0) {
            return sensor_names_[i].sensor_type;
        }
    }

    return SensorType::Invalid_;
}

SensorType ContextHub::SensorAbbrevNameToType(const std::string& abbrev_name) {
    return ContextHub::SensorAbbrevNameToType(abbrev_name.c_str());
}

std::string ContextHub::SensorTypeToAbbrevName(SensorType sensor_type) {
    for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
        if (sensor_names_[i].sensor_type == sensor_type) {
            return std::string(sensor_names_[i].name_abbrev);
        }
    }

    for (unsigned int i = 0; i < ARRAY_LEN(sensor_aliases_); i++) {
        if (sensor_aliases_[i].sensor_alias == sensor_type) {
            return std::string(sensor_aliases_[i].name_abbrev);
        }
    }

    char buffer[24];
    snprintf(buffer, sizeof(buffer), "unknown (%d)",
             static_cast<int>(sensor_type));
    return std::string(buffer);
}

std::string ContextHub::ListAllSensorAbbrevNames() {
    std::string sensor_list;
    for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
        sensor_list += sensor_names_[i].name_abbrev;
        if (i < ARRAY_LEN(sensor_names_) - 1) {
            sensor_list += ", ";
        }
    }

    return sensor_list;
}

bool ContextHub::Flash(const std::string& filename) {
    FILE *firmware_file = fopen(filename.c_str(), "r");
    if (!firmware_file) {
        LOGE("Failed to open firmware image: %d (%s)", errno, strerror(errno));
        return false;
    }

    fseek(firmware_file, 0, SEEK_END);
    long file_size = ftell(firmware_file);
    fseek(firmware_file, 0, SEEK_SET);

    auto firmware_data = std::vector<uint8_t>(file_size);
    size_t bytes_read = fread(firmware_data.data(), sizeof(uint8_t),
        file_size, firmware_file);
    fclose(firmware_file);

    if (bytes_read != static_cast<size_t>(file_size)) {
        LOGE("Read of firmware file returned %zu, expected %ld",
            bytes_read, file_size);
        return false;
    }
    return FlashSensorHub(firmware_data);
}

bool ContextHub::CalibrateSensors(const std::vector<SensorSpec>& sensors) {
    bool success = ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
        return CalibrateSingleSensor(spec);
    });

    if (success) {
        success = SaveCalibration();
    }
    return success;
}

bool ContextHub::TestSensors(const std::vector<SensorSpec>& sensors) {
    bool success = ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
        return TestSingleSensor(spec);
    });

    return success;
}

bool ContextHub::EnableSensor(const SensorSpec& spec) {
    ConfigureSensorRequest req;

    req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
    req.config.sensor_type = static_cast<uint8_t>(spec.sensor_type);
    req.config.command = static_cast<uint8_t>(
        ConfigureSensorRequest::CommandType::Enable);
    if (spec.special_rate != SensorSpecialRate::None) {
        req.config.rate = static_cast<uint32_t>(spec.special_rate);
    } else {
        req.config.rate = ConfigureSensorRequest::FloatRateToFixedPoint(
            spec.rate_hz);
    }
    req.config.latency = spec.latency_ns;

    LOGI("Enabling sensor %d at rate %.0f Hz (special 0x%x) and latency %.2f ms",
         spec.sensor_type, spec.rate_hz, spec.special_rate,
         spec.latency_ns / 1000000.0f);
    auto result = WriteEvent(req);
    if (result == TransportResult::Success) {
        sensor_is_active_[static_cast<int>(spec.sensor_type)] = true;
        return true;
    }

    LOGE("Could not enable sensor %d", spec.sensor_type);
    return false;
}

bool ContextHub::EnableSensors(const std::vector<SensorSpec>& sensors) {
    return ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
        return EnableSensor(spec);
    });
}

bool ContextHub::DisableSensor(SensorType sensor_type) {
    ConfigureSensorRequest req;

    req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
    req.config.sensor_type = static_cast<uint8_t>(sensor_type);
    req.config.command = static_cast<uint8_t>(
        ConfigureSensorRequest::CommandType::Disable);

    // Note that nanohub treats us as a single client, so if we call enable
    // twice then disable once, the sensor will be disabled
    LOGI("Disabling sensor %d", sensor_type);
    auto result = WriteEvent(req);
    if (result == TransportResult::Success) {
        sensor_is_active_[static_cast<int>(sensor_type)] = false;
        return true;
    }

    LOGE("Could not disable sensor %d", sensor_type);
    return false;
}

bool ContextHub::DisableSensors(const std::vector<SensorSpec>& sensors) {
    return ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
        return DisableSensor(spec.sensor_type);
    });
}

bool ContextHub::DisableAllSensors() {
    bool success = true;

    for (size_t i = 0; i < ARRAY_LEN(sensor_names_); i++) {
        success &= DisableSensor(sensor_names_[i].sensor_type);
    }

    return success;
}

bool ContextHub::DisableActiveSensors() {
    bool success = true;

    LOGD("Disabling all active sensors");
    for (size_t i = 0; i < ARRAY_LEN(sensor_names_); i++) {
        if (sensor_is_active_[static_cast<int>(sensor_names_[i].sensor_type)]) {
            success &= DisableSensor(sensor_names_[i].sensor_type);
        }
    }

    return success;
}

void ContextHub::PrintAllEvents(unsigned int limit) {
    bool continuous = (limit == 0);
    auto event_printer = [&limit, continuous](const SensorEvent& event) -> bool {
        printf("%s", event.ToString().c_str());
        return (continuous || --limit > 0);
    };
    ReadSensorEvents(event_printer);
}

bool ContextHub::PrintBridgeVersion() {
    BridgeVersionInfoRequest request;
    TransportResult result = WriteEvent(request);
    if (result != TransportResult::Success) {
        LOGE("Failed to send bridge version info request: %d",
             static_cast<int>(result));
        return false;
    }

    bool success = false;
    auto event_handler = [&success](const AppToHostEvent &event) -> bool {
        bool keep_going = true;
        auto rsp = reinterpret_cast<const BrHostEventData *>(event.GetDataPtr());
        if (event.GetAppId() != kAppIdBridge) {
            LOGD("Ignored event from unexpected app");
        } else if (event.GetDataLen() < sizeof(BrHostEventData)) {
            LOGE("Got short app to host event from bridge: length %u, expected "
                 "at least %zu", event.GetDataLen(), sizeof(BrHostEventData));
        } else if (rsp->msgId != BRIDGE_HOST_EVENT_MSG_VERSION_INFO) {
            LOGD("Ignored bridge event with unexpected message ID %u", rsp->msgId);
        } else if (rsp->status) {
            LOGE("Bridge version info request failed with status %u", rsp->status);
            keep_going = false;
        } else if (event.GetDataLen() < (sizeof(BrHostEventData) +
                                         sizeof(BrVersionInfoRsp))) {
            LOGE("Got successful version info response with short payload: "
                 "length %u, expected at least %zu", event.GetDataLen(),
                 (sizeof(BrHostEventData) + sizeof(BrVersionInfoRsp)));
            keep_going = false;
        } else {
            auto ver = reinterpret_cast<const struct BrVersionInfoRsp *>(
                rsp->payload);
            printf("Bridge version info:\n"
                   "  HW type:         0x%04x\n"
                   "  OS version:      0x%04x\n"
                   "  Variant version: 0x%08x\n"
                   "  Bridge version:  0x%08x\n",
                   ver->hwType, ver->osVer, ver->variantVer, ver->bridgeVer);
            keep_going = false;
            success = true;
        }

        return keep_going;
    };

    ReadAppEvents(event_handler, kBridgeVersionTimeoutMs);
    return success;
}

void ContextHub::PrintSensorEvents(SensorType type, int limit) {
    bool continuous = (limit == 0);
    auto event_printer = [type, &limit, continuous](const SensorEvent& event) -> bool {
        SensorType event_source = event.GetSensorType();
        if (event_source == type || SensorTypeIsAliasOf(type, event_source)) {
            printf("%s", event.ToString().c_str());
            limit -= event.GetNumSamples();
        }
        return (continuous || limit > 0);
    };
    ReadSensorEvents(event_printer);
}

void ContextHub::PrintSensorEvents(const std::vector<SensorSpec>& sensors, int limit) {
    bool continuous = (limit == 0);
    auto event_printer = [&sensors, &limit, continuous](const SensorEvent& event) -> bool {
        SensorType event_source = event.GetSensorType();
        for (unsigned int i = 0; i < sensors.size(); i++) {
            if (sensors[i].sensor_type == event_source
                    || SensorTypeIsAliasOf(sensors[i].sensor_type, event_source)) {
                printf("%s", event.ToString().c_str());
                limit -= event.GetNumSamples();
                break;
            }
        }
        return (continuous || limit > 0);
    };
    ReadSensorEvents(event_printer);
}

// Protected methods -----------------------------------------------------------

bool ContextHub::CalibrateSingleSensor(const SensorSpec& sensor) {
    ConfigureSensorRequest req;

    req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
    req.config.sensor_type = static_cast<uint8_t>(sensor.sensor_type);
    req.config.command = static_cast<uint8_t>(
        ConfigureSensorRequest::CommandType::Calibrate);

    LOGI("Issuing calibration request to sensor %d (%s)", sensor.sensor_type,
         ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str());
    auto result = WriteEvent(req);
    if (result != TransportResult::Success) {
        LOGE("Failed to calibrate sensor %d", sensor.sensor_type);
        return false;
    }

    bool success = false;
    auto cal_event_handler = [this, &sensor, &success](const AppToHostEvent &event) -> bool {
        if (event.IsCalibrationEventForSensor(sensor.sensor_type)) {
            success = HandleCalibrationResult(sensor, event);
            return false;
        }
        return true;
    };

    result = ReadAppEvents(cal_event_handler, kCalibrationTimeoutMs);
    if (result != TransportResult::Success) {
      LOGE("Error reading calibration response %d", static_cast<int>(result));
      return false;
    }

    return success;
}

bool ContextHub::TestSingleSensor(const SensorSpec& sensor) {
    ConfigureSensorRequest req;

    req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
    req.config.sensor_type = static_cast<uint8_t>(sensor.sensor_type);
    req.config.command = static_cast<uint8_t>(
        ConfigureSensorRequest::CommandType::SelfTest);

    LOGI("Issuing test request to sensor %d (%s)", sensor.sensor_type,
         ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str());
    auto result = WriteEvent(req);
    if (result != TransportResult::Success) {
        LOGE("Failed to test sensor %d", sensor.sensor_type);
        return false;
    }

    bool success = false;
    auto test_event_handler = [this, &sensor, &success](const AppToHostEvent &event) -> bool {
        if (event.IsTestEventForSensor(sensor.sensor_type)) {
            success = HandleTestResult(sensor, event);
            return false;
        }
        return true;
    };

    result = ReadAppEvents(test_event_handler, kTestTimeoutMs);
    if (result != TransportResult::Success) {
      LOGE("Error reading test response %d", static_cast<int>(result));
      return false;
    }

    return success;
}

bool ContextHub::ForEachSensor(const std::vector<SensorSpec>& sensors,
        std::function<bool(const SensorSpec&)> callback) {
    bool success = true;

    for (unsigned int i = 0; success && i < sensors.size(); i++) {
        success &= callback(sensors[i]);
    }

    return success;
}

bool ContextHub::HandleCalibrationResult(const SensorSpec& sensor,
        const AppToHostEvent &event) {
    auto hdr = reinterpret_cast<const SensorAppEventHeader *>(event.GetDataPtr());
    if (hdr->status) {
        LOGE("Calibration of sensor %d (%s) failed with status %u",
             sensor.sensor_type,
             ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str(),
             hdr->status);
        return false;
    }

    bool success = false;
    switch (sensor.sensor_type) {
      case SensorType::Accel:
      case SensorType::Gyro: {
        auto result = reinterpret_cast<const TripleAxisCalibrationResult *>(
            event.GetDataPtr());
        success = SetCalibration(sensor.sensor_type, result->xBias,
                                 result->yBias, result->zBias);
        break;
      }

      case SensorType::Barometer: {
        auto result = reinterpret_cast<const FloatCalibrationResult *>(
            event.GetDataPtr());
        if (sensor.have_cal_ref) {
            success = SetCalibration(sensor.sensor_type,
                                     (sensor.cal_ref - result->value));
        }
        break;
      }

      case SensorType::Proximity: {
        auto result = reinterpret_cast<const FourAxisCalibrationResult *>(
            event.GetDataPtr());
        success = SetCalibration(sensor.sensor_type, result->xBias,
                                 result->yBias, result->zBias, result->wBias);
        break;
      }

      case SensorType::AmbientLightSensor: {
        auto result = reinterpret_cast<const FloatCalibrationResult *>(
            event.GetDataPtr());
        if (sensor.have_cal_ref && (result->value != 0.0f)) {
            success = SetCalibration(sensor.sensor_type,
                                     (sensor.cal_ref / result->value));
        }
        break;
      }

      default:
        LOGE("Calibration not supported for sensor type %d",
             static_cast<int>(sensor.sensor_type));
    }

    return success;
}

bool ContextHub::HandleTestResult(const SensorSpec& sensor,
        const AppToHostEvent &event) {
    auto hdr = reinterpret_cast<const SensorAppEventHeader *>(event.GetDataPtr());
    if (!hdr->status) {
        LOGI("Self-test of sensor %d (%s) succeeded",
             sensor.sensor_type,
             ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str());
        return true;
    } else {
        LOGE("Self-test of sensor %d (%s) failed with status %u",
             sensor.sensor_type,
             ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str(),
             hdr->status);
        return false;
    }
}

ContextHub::TransportResult ContextHub::ReadAppEvents(
        std::function<bool(const AppToHostEvent&)> callback, int timeout_ms) {
    using Milliseconds = std::chrono::milliseconds;

    TransportResult result;
    bool timeout_required = timeout_ms > 0;
    bool keep_going = true;

    while (keep_going) {
        if (timeout_required && timeout_ms <= 0) {
            return TransportResult::Timeout;
        }

        std::unique_ptr<ReadEventResponse> event;

        SteadyClock start_time = std::chrono::steady_clock::now();
        result = ReadEvent(&event, timeout_ms);
        SteadyClock end_time = std::chrono::steady_clock::now();

        auto delta = end_time - start_time;
        timeout_ms -= std::chrono::duration_cast<Milliseconds>(delta).count();

        if (result == TransportResult::Success && event->IsAppToHostEvent()) {
            AppToHostEvent *app_event = reinterpret_cast<AppToHostEvent*>(
                event.get());
            keep_going = callback(*app_event);
        } else {
            if (result != TransportResult::Success) {
                LOGE("Error %d while reading", static_cast<int>(result));
                if (result != TransportResult::ParseFailure) {
                    return result;
                }
            } else {
                LOGD("Ignoring non-app-to-host event");
            }
        }
    }

    return TransportResult::Success;
}

void ContextHub::ReadSensorEvents(std::function<bool(const SensorEvent&)> callback) {
    TransportResult result;
    bool keep_going = true;

    while (keep_going) {
        std::unique_ptr<ReadEventResponse> event;
        result = ReadEvent(&event);
        if (result == TransportResult::Success && event->IsSensorEvent()) {
            SensorEvent *sensor_event = reinterpret_cast<SensorEvent*>(
                event.get());
            keep_going = callback(*sensor_event);
        } else {
            if (result != TransportResult::Success) {
                LOGE("Error %d while reading", static_cast<int>(result));
                if (result != TransportResult::ParseFailure) {
                    break;
                }
            } else {
                LOGD("Ignoring non-sensor event");
            }
        }
    }
}

bool ContextHub::SendCalibrationData(SensorType sensor_type,
        const std::vector<uint8_t>& cal_data) {
    ConfigureSensorRequest req;

    req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
    req.config.sensor_type = static_cast<uint8_t>(sensor_type);
    req.config.command = static_cast<uint8_t>(
        ConfigureSensorRequest::CommandType::ConfigData);
    req.SetAdditionalData(cal_data);

    auto result = WriteEvent(req);
    return (result == TransportResult::Success);
}

ContextHub::TransportResult ContextHub::WriteEvent(
        const WriteEventRequest& request) {
    return WriteEvent(request.GetBytes());
}

ContextHub::TransportResult ContextHub::ReadEvent(
        std::unique_ptr<ReadEventResponse>* response, int timeout_ms) {
    std::vector<uint8_t> responseBuf(256);
    ContextHub::TransportResult result = ReadEvent(responseBuf, timeout_ms);
    if (result == TransportResult::Success) {
        *response = ReadEventResponse::FromBytes(responseBuf);
        if (*response == nullptr) {
            result = TransportResult::ParseFailure;
        }
    }
    return result;
}

// Stubs for subclasses that don't implement calibration support
bool ContextHub::LoadCalibration() {
    LOGE("Loading calibration data not implemented");
    return false;
}

bool ContextHub::SetCalibration(SensorType sensor_type, int32_t data) {
    UNUSED_PARAM(sensor_type);
    UNUSED_PARAM(data);
    return false;
}

bool ContextHub::SetCalibration(SensorType sensor_type, float data) {
    UNUSED_PARAM(sensor_type);
    UNUSED_PARAM(data);
    return false;
}

bool ContextHub::SetCalibration(SensorType sensor_type, int32_t x,
        int32_t y, int32_t z) {
    UNUSED_PARAM(sensor_type);
    UNUSED_PARAM(x);
    UNUSED_PARAM(y);
    UNUSED_PARAM(z);
    return false;
}

bool ContextHub::SetCalibration(SensorType sensor_type, int32_t x,
        int32_t y, int32_t z, int32_t w) {
    UNUSED_PARAM(sensor_type);
    UNUSED_PARAM(x);
    UNUSED_PARAM(y);
    UNUSED_PARAM(z);
    UNUSED_PARAM(w);
    return false;
}

bool ContextHub::SaveCalibration() {
    LOGE("Saving calibration data not implemented");
    return false;
}

}  // namespace android