automotive/evs/1.1/default/EvsUltrasonicsArray.cpp - LeafOS-Project/android_hardware_interfaces - Gitiles

 /*
  * 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 fillMockArrayDesc(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 mock data.
 bool fillMockDataFrame(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 mock data for description.
     mArrayDesc.ultrasonicsArrayId = deviceName;
     fillMockArrayDesc(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::GetMockArrayDesc(const char* deviceName) {
     UltrasonicsArrayDesc ultrasonicsArrayDesc;
     ultrasonicsArrayDesc.ultrasonicsArrayId = deviceName;
     fillMockArrayDesc(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 mockDataFrameDesc;
             mockDataFrameDesc.dataFrameId = idx;
             mockDataFrameDesc.waveformsData = mDataFrames[idx].sharedMemory.hidlMemory;

             // Fill mock waveform data.
             fillMockDataFrame(mockDataFrameDesc, mDataFrames[idx].sharedMemory.pIMemory);

             // Issue the (asynchronous) callback to the client -- can't be holding the lock
             auto result = mStream->deliverDataFrame(mockDataFrameDesc);
             if (result.isOk()) {
                 LOG(DEBUG) << "Delivered data frame id: " << mockDataFrameDesc.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
	/*
	* 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 fillMockArrayDesc(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 mock data.
	bool fillMockDataFrame(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 mock data for description.
	mArrayDesc.ultrasonicsArrayId = deviceName;
	fillMockArrayDesc(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::GetMockArrayDesc(const char* deviceName) {
	UltrasonicsArrayDesc ultrasonicsArrayDesc;
	ultrasonicsArrayDesc.ultrasonicsArrayId = deviceName;
	fillMockArrayDesc(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 mockDataFrameDesc;
	mockDataFrameDesc.dataFrameId = idx;
	mockDataFrameDesc.waveformsData = mDataFrames[idx].sharedMemory.hidlMemory;

	// Fill mock waveform data.
	fillMockDataFrame(mockDataFrameDesc, mDataFrames[idx].sharedMemory.pIMemory);

	// Issue the (asynchronous) callback to the client -- can't be holding the lock
	auto result = mStream->deliverDataFrame(mockDataFrameDesc);
	if (result.isOk()) {
	LOG(DEBUG) << "Delivered data frame id: " << mockDataFrameDesc.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