Adds default implementation for ultrasonics HAL
- Replicates logic from camera for default impl.
Bug: 148619310
Fixes: b/148608401
Test: Builds, VTS passes.
Change-Id: I5c1b4c615f98cb7405a9a233a7853daba09cc63d
diff --git a/automotive/evs/1.1/default/Android.bp b/automotive/evs/1.1/default/Android.bp
index a35c9db..d843167 100644
--- a/automotive/evs/1.1/default/Android.bp
+++ b/automotive/evs/1.1/default/Android.bp
@@ -10,6 +10,7 @@
"EvsDisplay.cpp",
"ConfigManager.cpp",
"ConfigManagerUtil.cpp",
+ "EvsUltrasonicsArray.cpp",
],
init_rc: ["android.hardware.automotive.evs@1.1-service.rc"],
@@ -17,11 +18,14 @@
"android.hardware.automotive.evs@1.0",
"android.hardware.automotive.evs@1.1",
"android.hardware.camera.device@3.3",
+ "android.hidl.allocator@1.0",
+ "android.hidl.memory@1.0",
"libbase",
"libbinder",
"liblog",
"libhardware",
"libhidlbase",
+ "libhidlmemory",
"liblog",
"libui",
"libutils",
diff --git a/automotive/evs/1.1/default/EvsEnumerator.cpp b/automotive/evs/1.1/default/EvsEnumerator.cpp
index 415841c..117ee7a 100644
--- a/automotive/evs/1.1/default/EvsEnumerator.cpp
+++ b/automotive/evs/1.1/default/EvsEnumerator.cpp
@@ -19,6 +19,7 @@
#include "EvsEnumerator.h"
#include "EvsCamera.h"
#include "EvsDisplay.h"
+#include "EvsUltrasonicsArray.h"
namespace android {
namespace hardware {
@@ -31,12 +32,12 @@
// NOTE: All members values are static so that all clients operate on the same state
// That is to say, this is effectively a singleton despite the fact that HIDL
// constructs a new instance for each client.
-std::list<EvsEnumerator::CameraRecord> EvsEnumerator::sCameraList;
-wp<EvsDisplay> EvsEnumerator::sActiveDisplay;
-unique_ptr<ConfigManager> EvsEnumerator::sConfigManager;
-sp<IAutomotiveDisplayProxyService> EvsEnumerator::sDisplayProxyService;
-std::unordered_map<uint8_t, uint64_t> EvsEnumerator::sDisplayPortList;
-
+std::list<EvsEnumerator::CameraRecord> EvsEnumerator::sCameraList;
+wp<EvsDisplay> EvsEnumerator::sActiveDisplay;
+unique_ptr<ConfigManager> EvsEnumerator::sConfigManager;
+sp<IAutomotiveDisplayProxyService> EvsEnumerator::sDisplayProxyService;
+std::unordered_map<uint8_t, uint64_t> EvsEnumerator::sDisplayPortList;
+std::list<EvsEnumerator::UltrasonicsArrayRecord> EvsEnumerator::sUltrasonicsArrayRecordList;
EvsEnumerator::EvsEnumerator(sp<IAutomotiveDisplayProxyService> windowService) {
ALOGD("EvsEnumerator created");
@@ -66,6 +67,10 @@
}
});
}
+
+ // Add ultrasonics array desc.
+ sUltrasonicsArrayRecordList.emplace_back(
+ EvsUltrasonicsArray::GetDummyArrayDesc("front_array"));
}
@@ -355,25 +360,99 @@
return pRecord;
}
-// TODO(b/148608401): Add default implementation with dummy data.
+EvsEnumerator::UltrasonicsArrayRecord* EvsEnumerator::findUltrasonicsArrayById(
+ const std::string& ultrasonicsArrayId) {
+ auto recordIt = std::find_if(
+ sUltrasonicsArrayRecordList.begin(), sUltrasonicsArrayRecordList.end(),
+ [&ultrasonicsArrayId](const UltrasonicsArrayRecord& record) {
+ return ultrasonicsArrayId == record.desc.ultrasonicsArrayId;});
+
+ return (recordIt != sUltrasonicsArrayRecordList.end()) ? &*recordIt : nullptr;
+}
+
Return<void> EvsEnumerator::getUltrasonicsArrayList(getUltrasonicsArrayList_cb _hidl_cb) {
- hidl_vec<UltrasonicsArrayDesc> ultrasonicsArrayDesc;
- _hidl_cb(ultrasonicsArrayDesc);
+ hidl_vec<UltrasonicsArrayDesc> desc;
+ desc.resize(sUltrasonicsArrayRecordList.size());
+
+ // Copy over desc from sUltrasonicsArrayRecordList.
+ for (auto p = std::make_pair(sUltrasonicsArrayRecordList.begin(), desc.begin());
+ p.first != sUltrasonicsArrayRecordList.end(); p.first++, p.second++) {
+ *p.second = p.first->desc;
+ }
+
+ // Send back the results
+ ALOGD("reporting %zu ultrasonics arrays available", desc.size());
+ _hidl_cb(desc);
+
+ // HIDL convention says we return Void if we sent our result back via callback
return Void();
}
-// TODO(b/148608401): Add default implementation with dummy data.
Return<sp<IEvsUltrasonicsArray>> EvsEnumerator::openUltrasonicsArray(
const hidl_string& ultrasonicsArrayId) {
- (void)ultrasonicsArrayId;
- sp<IEvsUltrasonicsArray> pEvsUltrasonicsArray;
- return pEvsUltrasonicsArray;
+ // Find the named ultrasonic array.
+ UltrasonicsArrayRecord* pRecord = findUltrasonicsArrayById(ultrasonicsArrayId);
+
+ // Is this a recognized ultrasonic array id?
+ if (!pRecord) {
+ ALOGE("Requested ultrasonics array %s not found", ultrasonicsArrayId.c_str());
+ return nullptr;
+ }
+
+ // Has this ultrasonic array already been instantiated by another caller?
+ sp<EvsUltrasonicsArray> pActiveUltrasonicsArray = pRecord->activeInstance.promote();
+ if (pActiveUltrasonicsArray != nullptr) {
+ ALOGW("Killing previous ultrasonics array because of new caller");
+ closeUltrasonicsArray(pActiveUltrasonicsArray);
+ }
+
+ // Construct a ultrasonic array instance for the caller
+ pActiveUltrasonicsArray = EvsUltrasonicsArray::Create(ultrasonicsArrayId.c_str());
+ pRecord->activeInstance = pActiveUltrasonicsArray;
+ if (pActiveUltrasonicsArray == nullptr) {
+ ALOGE("Failed to allocate new EvsUltrasonicsArray object for %s\n",
+ ultrasonicsArrayId.c_str());
+ }
+
+ return pActiveUltrasonicsArray;
}
-// TODO(b/148608401): Add default implementation with dummy data.
Return<void> EvsEnumerator::closeUltrasonicsArray(
- const ::android::sp<IEvsUltrasonicsArray>& evsUltrasonicsArray) {
- (void)evsUltrasonicsArray;
+ const sp<IEvsUltrasonicsArray>& pEvsUltrasonicsArray) {
+
+ if (pEvsUltrasonicsArray.get() == nullptr) {
+ ALOGE("Ignoring call to closeUltrasonicsArray with null ultrasonics array");
+ return Void();
+ }
+
+ // Get the ultrasonics array id so we can find it in our list.
+ std::string ultrasonicsArrayId;
+ pEvsUltrasonicsArray->getUltrasonicArrayInfo([&ultrasonicsArrayId](UltrasonicsArrayDesc desc) {
+ ultrasonicsArrayId.assign(desc.ultrasonicsArrayId);
+ });
+
+ // Find the named ultrasonics array
+ UltrasonicsArrayRecord* pRecord = findUltrasonicsArrayById(ultrasonicsArrayId);
+ if (!pRecord) {
+ ALOGE("Asked to close a ultrasonics array whose name isnt not found");
+ return Void();
+ }
+
+ sp<EvsUltrasonicsArray> pActiveUltrasonicsArray = pRecord->activeInstance.promote();
+
+ if (pActiveUltrasonicsArray.get() == nullptr) {
+ ALOGE("Somehow a ultrasonics array is being destroyed when the enumerator didn't know "
+ "one existed");
+ } else if (pActiveUltrasonicsArray != pEvsUltrasonicsArray) {
+ // This can happen if the ultrasonics array was aggressively reopened,
+ // orphaning this previous instance
+ ALOGW("Ignoring close of previously orphaned ultrasonics array - why did a client steal?");
+ } else {
+ // Drop the active ultrasonics array
+ pActiveUltrasonicsArray->forceShutdown();
+ pRecord->activeInstance = nullptr;
+ }
+
return Void();
}
diff --git a/automotive/evs/1.1/default/EvsEnumerator.h b/automotive/evs/1.1/default/EvsEnumerator.h
index 12fd018..d80124b 100644
--- a/automotive/evs/1.1/default/EvsEnumerator.h
+++ b/automotive/evs/1.1/default/EvsEnumerator.h
@@ -21,6 +21,7 @@
#include <android/hardware/automotive/evs/1.1/IEvsCamera.h>
#include <android/hardware/automotive/evs/1.1/IEvsDisplay.h>
#include <android/frameworks/automotive/display/1.0/IAutomotiveDisplayProxyService.h>
+#include <android/hardware/automotive/evs/1.1/IEvsUltrasonicsArray.h>
#include <list>
@@ -46,6 +47,7 @@
class EvsCamera; // from EvsCamera.h
class EvsDisplay; // from EvsDisplay.h
+class EvsUltrasonicsArray; // from EvsUltrasonicsArray.h
class EvsEnumerator : public IEvsEnumerator {
@@ -85,10 +87,21 @@
CameraRecord(const char *cameraId) : desc() { desc.v1.cameraId = cameraId; }
};
+ struct UltrasonicsArrayRecord {
+ UltrasonicsArrayDesc desc;
+ wp<EvsUltrasonicsArray> activeInstance;
+
+ UltrasonicsArrayRecord(const UltrasonicsArrayDesc& arrayDesc) : desc(arrayDesc) {};
+ };
+
static CameraRecord* findCameraById(const std::string& cameraId);
static std::list<CameraRecord> sCameraList;
+ static UltrasonicsArrayRecord* findUltrasonicsArrayById(const std::string& ultrasonicsArrayId);
+
+ static std::list<UltrasonicsArrayRecord> sUltrasonicsArrayRecordList;
+
// Weak pointer. Object destructs if client dies.
static wp<EvsDisplay> sActiveDisplay;
diff --git a/automotive/evs/1.1/default/EvsUltrasonicsArray.cpp b/automotive/evs/1.1/default/EvsUltrasonicsArray.cpp
new file mode 100644
index 0000000..bc69aa4
--- /dev/null
+++ b/automotive/evs/1.1/default/EvsUltrasonicsArray.cpp
@@ -0,0 +1,551 @@
+/*
+ * Copyright 2020 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 "EvsUltrasonicsArray.h"
+
+#include <android-base/logging.h>
+#include <hidlmemory/mapping.h>
+#include <log/log.h>
+#include <time.h>
+#include <utils/SystemClock.h>
+#include <utils/Timers.h>
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace evs {
+namespace V1_1 {
+namespace implementation {
+
+// Arbitrary limit on number of data frames allowed to be allocated
+// Safeguards against unreasonable resource consumption and provides a testable limit
+const unsigned int kMaximumDataFramesInFlight = 100;
+
+const uint32_t kMaxReadingsPerSensor = 5;
+const uint32_t kMaxReceiversCount = 3;
+
+const unsigned int kSharedMemoryMaxSize =
+ kMaxReadingsPerSensor * kMaxReceiversCount * 2 * sizeof(float);
+
+// Target frame rate in frames per second.
+const int kTargetFrameRate = 10;
+
+namespace {
+
+void fillDummyArrayDesc(UltrasonicsArrayDesc& arrayDesc) {
+ arrayDesc.maxReadingsPerSensorCount = kMaxReadingsPerSensor;
+ arrayDesc.maxReceiversCount = kMaxReceiversCount;
+
+ const int kSensorCount = 3;
+ const float kMaxRange = 4000; // 4 metres.
+ const float kAngleOfMeasurement = 0.261799; // 15 degrees.
+
+ std::vector<UltrasonicSensor> sensors(kSensorCount);
+
+ // Sensor pointing forward on left side of front bumper.
+ sensors[0].maxRange = kMaxRange;
+ sensors[0].angleOfMeasurement = kAngleOfMeasurement;
+ sensors[0].pose = {{1, 0, 0, 0}, {-1000, 2000, 200}};
+
+ // Sensor pointing forward on center of front bumper.
+ sensors[1].maxRange = kMaxRange;
+ sensors[1].angleOfMeasurement = kAngleOfMeasurement;
+ sensors[1].pose = {{1, 0, 0, 0}, {0, 2000, 200}};
+
+ // Sensor pointing forward on right side of front bumper.
+ sensors[2].maxRange = kMaxRange;
+ sensors[2].angleOfMeasurement = kAngleOfMeasurement;
+ sensors[2].pose = {{1, 0, 0, 0}, {1000, 2000, 200}};
+
+ arrayDesc.sensors = sensors;
+}
+
+// Struct used by SerializeWaveformData().
+struct WaveformData {
+ uint8_t receiverId;
+ std::vector<std::pair<float, float>> readings;
+};
+
+// Serializes data provided in waveformDataList to a shared memory data pointer.
+// TODO(b/149950362): Add a common library for serialiazing and deserializing waveform data.
+void SerializeWaveformData(const std::vector<WaveformData>& waveformDataList, uint8_t* pData) {
+ for (auto& waveformData : waveformDataList) {
+ // Set Id
+ memcpy(pData, &waveformData.receiverId, sizeof(uint8_t));
+ pData += sizeof(uint8_t);
+
+ for (auto& reading : waveformData.readings) {
+ // Set the time of flight.
+ memcpy(pData, &reading.first, sizeof(float));
+ pData += sizeof(float);
+
+ // Set the resonance.
+ memcpy(pData, &reading.second, sizeof(float));
+ pData += sizeof(float);
+ }
+ }
+}
+
+// Fills dataFrameDesc with dummy data.
+bool fillDummyDataFrame(UltrasonicsDataFrameDesc& dataFrameDesc, sp<IMemory> pIMemory) {
+ dataFrameDesc.timestampNs = elapsedRealtimeNano();
+
+ const std::vector<uint8_t> transmittersIdList = {0};
+ dataFrameDesc.transmittersIdList = transmittersIdList;
+
+ const std::vector<uint8_t> recvIdList = {0, 1, 2};
+ dataFrameDesc.receiversIdList = recvIdList;
+
+ const std::vector<uint32_t> receiversReadingsCountList = {2, 2, 4};
+ dataFrameDesc.receiversReadingsCountList = receiversReadingsCountList;
+
+ const std::vector<WaveformData> waveformDataList = {
+ {recvIdList[0], { {1000, 0.1f}, {2000, 0.8f} }},
+ {recvIdList[1], { {1000, 0.1f}, {2000, 1.0f} }},
+ {recvIdList[2], { {1000, 0.1f}, {2000, 0.2f}, {4000, 0.2f}, {5000, 0.1f} }}
+ };
+
+ if (pIMemory.get() == nullptr) {
+ return false;
+ }
+
+ uint8_t* pData = (uint8_t*)((void*)pIMemory->getPointer());
+
+ pIMemory->update();
+ SerializeWaveformData(waveformDataList, pData);
+ pIMemory->commit();
+
+ return true;
+}
+
+} // namespace
+
+EvsUltrasonicsArray::EvsUltrasonicsArray(const char* deviceName)
+ : mFramesAllowed(0), mFramesInUse(0), mStreamState(STOPPED) {
+ LOG(DEBUG) << "EvsUltrasonicsArray instantiated";
+
+ // Set up dummy data for description.
+ mArrayDesc.ultrasonicsArrayId = deviceName;
+ fillDummyArrayDesc(mArrayDesc);
+
+ // Assign allocator.
+ mShmemAllocator = IAllocator::getService("ashmem");
+ if (mShmemAllocator.get() == nullptr) {
+ LOG(ERROR) << "SurroundViewHidlTest getService ashmem failed";
+ }
+}
+
+sp<EvsUltrasonicsArray> EvsUltrasonicsArray::Create(const char* deviceName) {
+ return sp<EvsUltrasonicsArray>(new EvsUltrasonicsArray(deviceName));
+}
+
+EvsUltrasonicsArray::~EvsUltrasonicsArray() {
+ LOG(DEBUG) << "EvsUltrasonicsArray being destroyed";
+ forceShutdown();
+}
+
+// This gets called if another caller "steals" ownership of the ultrasonic array.
+void EvsUltrasonicsArray::forceShutdown() {
+ LOG(DEBUG) << "EvsUltrasonicsArray forceShutdown";
+
+ // Make sure our output stream is cleaned up
+ // (It really should be already)
+ stopStream();
+
+ // Claim the lock while we work on internal state
+ std::lock_guard<std::mutex> lock(mAccessLock);
+
+ // Drop all the data frames we've been using
+ for (auto&& dataFrame : mDataFrames) {
+ if (dataFrame.inUse) {
+ LOG(ERROR) << "Error - releasing data frame despite remote ownership";
+ }
+ dataFrame.sharedMemory.clear();
+ }
+ mDataFrames.clear();
+
+ // Put this object into an unrecoverable error state since somebody else
+ // is going to own the underlying ultrasonic array now
+ mStreamState = DEAD;
+}
+
+UltrasonicsArrayDesc EvsUltrasonicsArray::GetDummyArrayDesc(const char* deviceName) {
+ UltrasonicsArrayDesc ultrasonicsArrayDesc;
+ ultrasonicsArrayDesc.ultrasonicsArrayId = deviceName;
+ fillDummyArrayDesc(ultrasonicsArrayDesc);
+ return ultrasonicsArrayDesc;
+}
+
+Return<void> EvsUltrasonicsArray::getUltrasonicArrayInfo(getUltrasonicArrayInfo_cb _get_info_cb) {
+ LOG(DEBUG) << "EvsUltrasonicsArray getUltrasonicsArrayInfo";
+
+ // Return the description for the get info callback.
+ _get_info_cb(mArrayDesc);
+
+ return Void();
+}
+
+Return<EvsResult> EvsUltrasonicsArray::setMaxFramesInFlight(uint32_t bufferCount) {
+ LOG(DEBUG) << "EvsUltrasonicsArray setMaxFramesInFlight";
+
+ // Lock mutex for performing changes to available frames.
+ std::lock_guard<std::mutex> lock(mAccessLock);
+
+ // We cannot function without at least one buffer to send data.
+ if (bufferCount < 1) {
+ LOG(ERROR) << "Ignoring setMaxFramesInFlight with less than one buffer requested";
+ return EvsResult::INVALID_ARG;
+ }
+
+ // Update our internal state of buffer count.
+ if (setAvailableFrames_Locked(bufferCount)) {
+ return EvsResult::OK;
+ } else {
+ return EvsResult::BUFFER_NOT_AVAILABLE;
+ }
+
+ return EvsResult::OK;
+}
+
+Return<void> EvsUltrasonicsArray::doneWithDataFrame(const UltrasonicsDataFrameDesc& dataFrameDesc) {
+ LOG(DEBUG) << "EvsUltrasonicsArray doneWithFrame";
+
+ std::lock_guard<std::mutex> lock(mAccessLock);
+
+ if (dataFrameDesc.dataFrameId >= mDataFrames.size()) {
+ LOG(ERROR) << "ignoring doneWithFrame called with invalid dataFrameId "
+ << dataFrameDesc.dataFrameId << "(max is " << mDataFrames.size() - 1 << ")";
+ return Void();
+ }
+
+ if (!mDataFrames[dataFrameDesc.dataFrameId].inUse) {
+ LOG(ERROR) << "ignoring doneWithFrame called on frame " << dataFrameDesc.dataFrameId
+ << "which is already free";
+ return Void();
+ }
+
+ // Mark the frame as available
+ mDataFrames[dataFrameDesc.dataFrameId].inUse = false;
+ mFramesInUse--;
+
+ // If this frame's index is high in the array, try to move it down
+ // to improve locality after mFramesAllowed has been reduced.
+ if (dataFrameDesc.dataFrameId >= mFramesAllowed) {
+ // Find an empty slot lower in the array (which should always exist in this case)
+ for (auto&& dataFrame : mDataFrames) {
+ if (!dataFrame.sharedMemory.IsValid()) {
+ dataFrame.sharedMemory = mDataFrames[dataFrameDesc.dataFrameId].sharedMemory;
+ mDataFrames[dataFrameDesc.dataFrameId].sharedMemory.clear();
+ return Void();
+ }
+ }
+ }
+
+ return Void();
+}
+
+Return<EvsResult> EvsUltrasonicsArray::startStream(
+ const ::android::sp<IEvsUltrasonicsArrayStream>& stream) {
+ LOG(DEBUG) << "EvsUltrasonicsArray startStream";
+
+ std::lock_guard<std::mutex> lock(mAccessLock);
+
+ if (mStreamState != STOPPED) {
+ LOG(ERROR) << "ignoring startStream call when a stream is already running.";
+ return EvsResult::STREAM_ALREADY_RUNNING;
+ }
+
+ // If the client never indicated otherwise, configure ourselves for a single streaming buffer
+ if (mFramesAllowed < 1) {
+ if (!setAvailableFrames_Locked(1)) {
+ LOG(ERROR)
+ << "Failed to start stream because we couldn't get shared memory data buffer";
+ return EvsResult::BUFFER_NOT_AVAILABLE;
+ }
+ }
+
+ // Record the user's callback for use when we have a frame ready
+ mStream = stream;
+
+ // Start the frame generation thread
+ mStreamState = RUNNING;
+ mCaptureThread = std::thread([this]() { generateDataFrames(); });
+
+ return EvsResult::OK;
+}
+
+Return<void> EvsUltrasonicsArray::stopStream() {
+ LOG(DEBUG) << "EvsUltrasonicsArray stopStream";
+
+ bool streamStateStopping = false;
+ {
+ std::lock_guard<std::mutex> lock(mAccessLock);
+ if (mStreamState == RUNNING) {
+ // Tell the GenerateFrames loop we want it to stop
+ mStreamState = STOPPING;
+ streamStateStopping = true;
+ }
+ }
+
+ if (streamStateStopping) {
+ // Block outside the mutex until the "stop" flag has been acknowledged
+ // We won't send any more frames, but the client might still get some already in flight
+ LOG(DEBUG) << "Waiting for stream thread to end...";
+ mCaptureThread.join();
+ }
+
+ {
+ std::lock_guard<std::mutex> lock(mAccessLock);
+ mStreamState = STOPPED;
+ mStream = nullptr;
+ LOG(DEBUG) << "Stream marked STOPPED.";
+ }
+
+ return Void();
+}
+
+bool EvsUltrasonicsArray::setAvailableFrames_Locked(unsigned bufferCount) {
+ if (bufferCount < 1) {
+ LOG(ERROR) << "Ignoring request to set buffer count to zero";
+ return false;
+ }
+ if (bufferCount > kMaximumDataFramesInFlight) {
+ LOG(ERROR) << "Rejecting buffer request in excess of internal limit";
+ return false;
+ }
+
+ // Is an increase required?
+ if (mFramesAllowed < bufferCount) {
+ // An increase is required
+ unsigned needed = bufferCount - mFramesAllowed;
+ LOG(INFO) << "Number of data frame buffers to add: " << needed;
+
+ unsigned added = increaseAvailableFrames_Locked(needed);
+ if (added != needed) {
+ // If we didn't add all the frames we needed, then roll back to the previous state
+ LOG(ERROR) << "Rolling back to previous frame queue size";
+ decreaseAvailableFrames_Locked(added);
+ return false;
+ }
+ } else if (mFramesAllowed > bufferCount) {
+ // A decrease is required
+ unsigned framesToRelease = mFramesAllowed - bufferCount;
+ LOG(INFO) << "Number of data frame buffers to reduce: " << framesToRelease;
+
+ unsigned released = decreaseAvailableFrames_Locked(framesToRelease);
+ if (released != framesToRelease) {
+ // This shouldn't happen with a properly behaving client because the client
+ // should only make this call after returning sufficient outstanding buffers
+ // to allow a clean resize.
+ LOG(ERROR) << "Buffer queue shrink failed -- too many buffers currently in use?";
+ }
+ }
+
+ return true;
+}
+
+EvsUltrasonicsArray::SharedMemory EvsUltrasonicsArray::allocateAndMapSharedMemory() {
+ SharedMemory sharedMemory;
+
+ // Check shared memory allocator is valid.
+ if (mShmemAllocator.get() == nullptr) {
+ LOG(ERROR) << "Shared memory allocator not initialized.";
+ return SharedMemory();
+ }
+
+ // Allocate memory.
+ bool allocateSuccess = false;
+ Return<void> result = mShmemAllocator->allocate(kSharedMemoryMaxSize,
+ [&](bool success, const hidl_memory& hidlMem) {
+ if (!success) {
+ return;
+ }
+ allocateSuccess = success;
+ sharedMemory.hidlMemory = hidlMem;
+ });
+
+ // Check result of allocated memory.
+ if (!result.isOk() || !allocateSuccess) {
+ LOG(ERROR) << "Shared memory allocation failed.";
+ return SharedMemory();
+ }
+
+ // Map shared memory.
+ sharedMemory.pIMemory = mapMemory(sharedMemory.hidlMemory);
+ if (sharedMemory.pIMemory.get() == nullptr) {
+ LOG(ERROR) << "Shared memory mapping failed.";
+ return SharedMemory();
+ }
+
+ // Return success.
+ return sharedMemory;
+}
+
+unsigned EvsUltrasonicsArray::increaseAvailableFrames_Locked(unsigned numToAdd) {
+ unsigned added = 0;
+
+ while (added < numToAdd) {
+ SharedMemory sharedMemory = allocateAndMapSharedMemory();
+
+ // If allocate and map fails, break.
+ if (!sharedMemory.IsValid()) {
+ break;
+ }
+
+ // Find a place to store the new buffer
+ bool stored = false;
+ for (auto&& dataFrame : mDataFrames) {
+ if (!dataFrame.sharedMemory.IsValid()) {
+ // Use this existing entry
+ dataFrame.sharedMemory = sharedMemory;
+ dataFrame.inUse = false;
+ stored = true;
+ break;
+ }
+ }
+
+ if (!stored) {
+ // Add a BufferRecord wrapping this handle to our set of available buffers
+ mDataFrames.emplace_back(sharedMemory);
+ }
+
+ mFramesAllowed++;
+ added++;
+ }
+
+ return added;
+}
+
+unsigned EvsUltrasonicsArray::decreaseAvailableFrames_Locked(unsigned numToRemove) {
+ unsigned removed = 0;
+
+ for (auto&& dataFrame : mDataFrames) {
+ // Is this record not in use, but holding a buffer that we can free?
+ if (!dataFrame.inUse && dataFrame.sharedMemory.IsValid()) {
+ // Release buffer and update the record so we can recognize it as "empty"
+ dataFrame.sharedMemory.clear();
+
+ mFramesAllowed--;
+ removed++;
+
+ if (removed == numToRemove) {
+ break;
+ }
+ }
+ }
+
+ return removed;
+}
+
+// This is the asynchronous data frame generation thread that runs in parallel with the
+// main serving thread. There is one for each active ultrasonic array instance.
+void EvsUltrasonicsArray::generateDataFrames() {
+ LOG(DEBUG) << "Data frame generation loop started";
+
+ unsigned idx = 0;
+
+ while (true) {
+ bool timeForFrame = false;
+
+ nsecs_t startTime = elapsedRealtimeNano();
+
+ // Lock scope for updating shared state
+ {
+ std::lock_guard<std::mutex> lock(mAccessLock);
+
+ if (mStreamState != RUNNING) {
+ // Break out of our main thread loop
+ break;
+ }
+
+ // Are we allowed to issue another buffer?
+ if (mFramesInUse >= mFramesAllowed) {
+ // Can't do anything right now -- skip this frame
+ LOG(WARNING) << "Skipped a frame because too many are in flight";
+ } else {
+ // Identify an available buffer to fill
+ for (idx = 0; idx < mDataFrames.size(); idx++) {
+ if (!mDataFrames[idx].inUse && mDataFrames[idx].sharedMemory.IsValid()) {
+ // Found an available record, so stop looking
+ break;
+ }
+ }
+ if (idx >= mDataFrames.size()) {
+ // This shouldn't happen since we already checked mFramesInUse vs mFramesAllowed
+ LOG(ERROR) << "Failed to find an available buffer slot";
+ } else {
+ // We're going to make the frame busy
+ mDataFrames[idx].inUse = true;
+ mFramesInUse++;
+ timeForFrame = true;
+ }
+ }
+ }
+
+ if (timeForFrame) {
+ // Assemble the buffer description we'll transmit below
+ UltrasonicsDataFrameDesc dummyDataFrameDesc;
+ dummyDataFrameDesc.dataFrameId = idx;
+ dummyDataFrameDesc.waveformsData = mDataFrames[idx].sharedMemory.hidlMemory;
+
+ // Fill dummy waveform data.
+ fillDummyDataFrame(dummyDataFrameDesc, mDataFrames[idx].sharedMemory.pIMemory);
+
+ // Issue the (asynchronous) callback to the client -- can't be holding the lock
+ auto result = mStream->deliverDataFrame(dummyDataFrameDesc);
+ if (result.isOk()) {
+ LOG(DEBUG) << "Delivered data frame id: " << dummyDataFrameDesc.dataFrameId;
+ } else {
+ // This can happen if the client dies and is likely unrecoverable.
+ // To avoid consuming resources generating failing calls, we stop sending
+ // frames. Note, however, that the stream remains in the "STREAMING" state
+ // until cleaned up on the main thread.
+ LOG(ERROR) << "Frame delivery call failed in the transport layer.";
+
+ // Since we didn't actually deliver it, mark the frame as available
+ std::lock_guard<std::mutex> lock(mAccessLock);
+ mDataFrames[idx].inUse = false;
+ mFramesInUse--;
+
+ break;
+ }
+ }
+
+ // Sleep to generate frames at kTargetFrameRate.
+ static const nsecs_t kTargetFrameTimeUs = 1000 * 1000 / kTargetFrameRate;
+ const nsecs_t now = elapsedRealtimeNano();
+ const nsecs_t workTimeUs = (now - startTime) / 1000;
+ const nsecs_t sleepDurationUs = kTargetFrameTimeUs - workTimeUs;
+ if (sleepDurationUs > 0) {
+ usleep(sleepDurationUs);
+ }
+ }
+
+ // If we've been asked to stop, send an event to signal the actual end of stream
+ EvsEventDesc event;
+ event.aType = EvsEventType::STREAM_STOPPED;
+ auto result = mStream->notify(event);
+ if (!result.isOk()) {
+ LOG(ERROR) << "Error delivering end of stream marker";
+ }
+}
+
+} // namespace implementation
+} // namespace V1_1
+} // namespace evs
+} // namespace automotive
+} // namespace hardware
+} // namespace android
diff --git a/automotive/evs/1.1/default/EvsUltrasonicsArray.h b/automotive/evs/1.1/default/EvsUltrasonicsArray.h
new file mode 100644
index 0000000..7a41012
--- /dev/null
+++ b/automotive/evs/1.1/default/EvsUltrasonicsArray.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright 2020 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.
+ */
+
+#ifndef ANDROID_HARDWARE_AUTOMOTIVE_EVS_V1_1_EVSULTRASONICSARRAY_H
+#define ANDROID_HARDWARE_AUTOMOTIVE_EVS_V1_1_EVSULTRASONICSARRAY_H
+
+#include <thread>
+#include <utility>
+
+#include <android-base/macros.h>
+#include <android/hidl/allocator/1.0/IAllocator.h>
+#include <android/hidl/memory/1.0/IMemory.h>
+#include <utils/threads.h>
+
+#include <android/hardware/automotive/evs/1.1/IEvsUltrasonicsArray.h>
+#include <android/hardware/automotive/evs/1.1/IEvsUltrasonicsArrayStream.h>
+#include <android/hardware/automotive/evs/1.1/types.h>
+
+using ::android::hardware::hidl_memory;
+using ::android::hardware::automotive::evs::V1_0::EvsResult;
+using ::android::hardware::automotive::evs::V1_1::IEvsUltrasonicsArray;
+using ::android::hardware::automotive::evs::V1_1::IEvsUltrasonicsArrayStream;
+using ::android::hardware::automotive::evs::V1_1::UltrasonicsArrayDesc;
+using ::android::hardware::automotive::evs::V1_1::UltrasonicsDataFrameDesc;
+using ::android::hidl::allocator::V1_0::IAllocator;
+using ::android::hidl::memory::V1_0::IMemory;
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace evs {
+namespace V1_1 {
+namespace implementation {
+
+class EvsUltrasonicsArray : public IEvsUltrasonicsArray {
+ public:
+ // Methods from ::android::hardware::automotive::evs::V1_1::IEvsUltrasonicsArray follow.
+ Return<void> getUltrasonicArrayInfo(getUltrasonicArrayInfo_cb _get_info_cb) override;
+ Return<EvsResult> setMaxFramesInFlight(uint32_t bufferCount) override;
+ Return<void> doneWithDataFrame(const UltrasonicsDataFrameDesc& dataFrameDesc) override;
+ Return<EvsResult> startStream(const ::android::sp<IEvsUltrasonicsArrayStream>& stream) override;
+ Return<void> stopStream() override;
+
+ // Factory function to create a array.
+ static sp<EvsUltrasonicsArray> Create(const char* deviceName);
+
+ // Returns a ultrasonics array descriptor filled with sample data.
+ static UltrasonicsArrayDesc GetDummyArrayDesc(const char* id);
+
+ DISALLOW_COPY_AND_ASSIGN(EvsUltrasonicsArray);
+ virtual ~EvsUltrasonicsArray() override;
+ void forceShutdown(); // This gets called if another caller "steals" ownership
+
+ private:
+ // Structure holding the hidl memory struct and the interface to a shared memory.
+ struct SharedMemory {
+ hidl_memory hidlMemory;
+ sp<IMemory> pIMemory;
+
+ SharedMemory() : hidlMemory(hidl_memory()), pIMemory(nullptr){};
+
+ SharedMemory(hidl_memory hidlMem, sp<IMemory> pIMem)
+ : hidlMemory(hidlMem), pIMemory(pIMem) {}
+
+ bool IsValid() { return (pIMemory.get() != nullptr && hidlMemory.valid()); }
+
+ void clear() {
+ hidlMemory = hidl_memory();
+ pIMemory.clear();
+ }
+ };
+
+ // Struct for a data frame record.
+ struct DataFrameRecord {
+ SharedMemory sharedMemory;
+ bool inUse;
+ explicit DataFrameRecord(SharedMemory shMem) : sharedMemory(shMem), inUse(false){};
+ };
+
+ enum StreamStateValues {
+ STOPPED,
+ RUNNING,
+ STOPPING,
+ DEAD,
+ };
+
+ EvsUltrasonicsArray(const char* deviceName);
+
+ // These three functions are expected to be called while mAccessLock is held
+ bool setAvailableFrames_Locked(unsigned bufferCount);
+ unsigned increaseAvailableFrames_Locked(unsigned numToAdd);
+ unsigned decreaseAvailableFrames_Locked(unsigned numToRemove);
+
+ void generateDataFrames();
+
+ SharedMemory allocateAndMapSharedMemory();
+
+ UltrasonicsArrayDesc mArrayDesc = {}; // The properties of this ultrasonic array.
+
+ std::thread mCaptureThread; // The thread we'll use to synthesize frames
+
+ sp<IEvsUltrasonicsArrayStream> mStream = nullptr; // The callback used to deliver each frame
+
+ sp<IAllocator> mShmemAllocator = nullptr; // Shared memory allocator.
+
+ std::mutex mAccessLock;
+ std::vector<DataFrameRecord> mDataFrames GUARDED_BY(mAccessLock); // Shared memory buffers.
+ unsigned mFramesAllowed GUARDED_BY(mAccessLock); // How many buffers are we currently using.
+ unsigned mFramesInUse GUARDED_BY(mAccessLock); // How many buffers are currently outstanding.
+
+ StreamStateValues mStreamState GUARDED_BY(mAccessLock);
+};
+
+} // namespace implementation
+} // namespace V1_1
+} // namespace evs
+} // namespace automotive
+} // namespace hardware
+} // namespace android
+
+#endif // ANDROID_HARDWARE_AUTOMOTIVE_EVS_V1_1_EVSULTRASONICSARRAY_H