1 #define LOG_TAG "PoseClient"
2 #include <dvr/pose_client.h>
3 
4 #include <stdint.h>
5 
6 #include <log/log.h>
7 #include <pdx/client.h>
8 #include <pdx/default_transport/client_channel_factory.h>
9 #include <pdx/file_handle.h>
10 #include <private/dvr/buffer_hub_client.h>
11 #include <private/dvr/pose-ipc.h>
12 #include <private/dvr/pose_client_internal.h>
13 #include <private/dvr/sensor_constants.h>
14 
15 using android::pdx::LocalHandle;
16 using android::pdx::LocalChannelHandle;
17 using android::pdx::Status;
18 using android::pdx::Transaction;
19 
20 #define arraysize(x) (static_cast<int32_t>(std::extent<decltype(x)>::value))
21 
22 namespace android {
23 namespace dvr {
24 
25 // PoseClient is a remote interface to the pose service in sensord.
26 class PoseClient : public pdx::ClientBase<PoseClient> {
27  public:
~PoseClient()28   ~PoseClient() override {}
29 
30   // Casts C handle into an instance of this class.
FromC(DvrPose * client)31   static PoseClient* FromC(DvrPose* client) {
32     return reinterpret_cast<PoseClient*>(client);
33   }
34 
35   // Polls the pose service for the current state and stores it in *state.
36   // Returns zero on success, a negative error code otherwise.
Poll(DvrPoseState * state)37   int Poll(DvrPoseState* state) {
38     Transaction trans{*this};
39     Status<int> status =
40         trans.Send<int>(DVR_POSE_POLL, nullptr, 0, state, sizeof(*state));
41     ALOGE_IF(!status, "Pose poll() failed because: %s\n",
42              status.GetErrorMessage().c_str());
43     return ReturnStatusOrError(status);
44   }
45 
GetPose(uint32_t vsync_count,DvrPoseAsync * out_pose)46   int GetPose(uint32_t vsync_count, DvrPoseAsync* out_pose) {
47     if (!mapped_pose_buffer_) {
48       int ret = GetRingBuffer(nullptr);
49       if (ret < 0)
50         return ret;
51     }
52     *out_pose =
53         mapped_pose_buffer_->ring[vsync_count & kPoseAsyncBufferIndexMask];
54     return 0;
55   }
56 
GetVsyncCount()57   uint32_t GetVsyncCount() {
58     if (!mapped_pose_buffer_) {
59       int ret = GetRingBuffer(nullptr);
60       if (ret < 0)
61         return 0;
62     }
63     return mapped_pose_buffer_->vsync_count;
64   }
65 
GetControllerPose(int32_t controller_id,uint32_t vsync_count,DvrPoseAsync * out_pose)66   int GetControllerPose(int32_t controller_id, uint32_t vsync_count,
67                         DvrPoseAsync* out_pose) {
68     if (controller_id < 0 || controller_id >= arraysize(controllers_)) {
69       return -EINVAL;
70     }
71     if (!controllers_[controller_id].mapped_pose_buffer) {
72       int ret = GetControllerRingBuffer(controller_id);
73       if (ret < 0)
74         return ret;
75     }
76     *out_pose =
77         controllers_[controller_id]
78             .mapped_pose_buffer[vsync_count & kPoseAsyncBufferIndexMask];
79     return 0;
80   }
81 
LogController(bool enable)82   int LogController(bool enable) {
83     Transaction trans{*this};
84     Status<int> status = trans.Send<int>(DVR_POSE_LOG_CONTROLLER, &enable,
85                                          sizeof(enable), nullptr, 0);
86     ALOGE_IF(!status, "Pose LogController() failed because: %s",
87              status.GetErrorMessage().c_str());
88     return ReturnStatusOrError(status);
89   }
90 
91   // Freezes the pose to the provided state. Future poll operations will return
92   // this state until a different state is frozen or SetMode() is called with a
93   // different mode.
94   // Returns zero on success, a negative error code otherwise.
Freeze(const DvrPoseState & frozen_state)95   int Freeze(const DvrPoseState& frozen_state) {
96     Transaction trans{*this};
97     Status<int> status = trans.Send<int>(DVR_POSE_FREEZE, &frozen_state,
98                                          sizeof(frozen_state), nullptr, 0);
99     ALOGE_IF(!status, "Pose Freeze() failed because: %s\n",
100              status.GetErrorMessage().c_str());
101     return ReturnStatusOrError(status);
102   }
103 
104   // Sets the data mode for the pose service.
SetMode(DvrPoseMode mode)105   int SetMode(DvrPoseMode mode) {
106     Transaction trans{*this};
107     Status<int> status =
108         trans.Send<int>(DVR_POSE_SET_MODE, &mode, sizeof(mode), nullptr, 0);
109     ALOGE_IF(!status, "Pose SetPoseMode() failed because: %s",
110              status.GetErrorMessage().c_str());
111     return ReturnStatusOrError(status);
112   }
113 
114   // Gets the data mode for the pose service.
GetMode(DvrPoseMode * out_mode)115   int GetMode(DvrPoseMode* out_mode) {
116     int mode;
117     Transaction trans{*this};
118     Status<int> status =
119         trans.Send<int>(DVR_POSE_GET_MODE, nullptr, 0, &mode, sizeof(mode));
120     ALOGE_IF(!status, "Pose GetPoseMode() failed because: %s",
121              status.GetErrorMessage().c_str());
122     if (status)
123       *out_mode = DvrPoseMode(mode);
124     return ReturnStatusOrError(status);
125   }
126 
GetRingBuffer(DvrPoseRingBufferInfo * out_info)127   int GetRingBuffer(DvrPoseRingBufferInfo* out_info) {
128     if (pose_buffer_.get()) {
129       if (out_info) {
130         GetPoseRingBufferInfo(out_info);
131       }
132       return 0;
133     }
134 
135     Transaction trans{*this};
136     Status<LocalChannelHandle> status =
137         trans.Send<LocalChannelHandle>(DVR_POSE_GET_RING_BUFFER);
138     if (!status) {
139       ALOGE("Pose GetRingBuffer() failed because: %s",
140             status.GetErrorMessage().c_str());
141       return -status.error();
142     }
143 
144     auto buffer = BufferConsumer::Import(status.take());
145     if (!buffer) {
146       ALOGE("Pose failed to import ring buffer");
147       return -EIO;
148     }
149     void* addr = nullptr;
150     int ret = buffer->GetBlobReadOnlyPointer(sizeof(DvrPoseRingBuffer), &addr);
151     if (ret < 0 || !addr) {
152       ALOGE("Pose failed to map ring buffer: ret:%d, addr:%p", ret, addr);
153       return -EIO;
154     }
155     pose_buffer_.swap(buffer);
156     mapped_pose_buffer_ = static_cast<const DvrPoseRingBuffer*>(addr);
157     ALOGI("Mapped pose data translation %f,%f,%f quat %f,%f,%f,%f",
158           mapped_pose_buffer_->ring[0].translation[0],
159           mapped_pose_buffer_->ring[0].translation[1],
160           mapped_pose_buffer_->ring[0].translation[2],
161           mapped_pose_buffer_->ring[0].orientation[0],
162           mapped_pose_buffer_->ring[0].orientation[1],
163           mapped_pose_buffer_->ring[0].orientation[2],
164           mapped_pose_buffer_->ring[0].orientation[3]);
165     if (out_info) {
166       GetPoseRingBufferInfo(out_info);
167     }
168     return 0;
169   }
170 
GetControllerRingBuffer(int32_t controller_id)171   int GetControllerRingBuffer(int32_t controller_id) {
172     if (controller_id < 0 || controller_id >= arraysize(controllers_)) {
173       return -EINVAL;
174     }
175     ControllerClientState& client_state = controllers_[controller_id];
176     if (client_state.pose_buffer.get()) {
177       return 0;
178     }
179 
180     Transaction trans{*this};
181     Status<LocalChannelHandle> status = trans.Send<LocalChannelHandle>(
182         DVR_POSE_GET_CONTROLLER_RING_BUFFER, &controller_id,
183         sizeof(controller_id), nullptr, 0);
184     if (!status) {
185       return -status.error();
186     }
187 
188     auto buffer = BufferConsumer::Import(status.take());
189     if (!buffer) {
190       ALOGE("Pose failed to import ring buffer");
191       return -EIO;
192     }
193     constexpr size_t size = kPoseAsyncBufferTotalCount * sizeof(DvrPoseAsync);
194     void* addr = nullptr;
195     int ret = buffer->GetBlobReadOnlyPointer(size, &addr);
196     if (ret < 0 || !addr) {
197       ALOGE("Pose failed to map ring buffer: ret:%d, addr:%p", ret, addr);
198       return -EIO;
199     }
200     client_state.pose_buffer.swap(buffer);
201     client_state.mapped_pose_buffer = static_cast<const DvrPoseAsync*>(addr);
202     ALOGI(
203         "Mapped controller %d pose data translation %f,%f,%f quat %f,%f,%f,%f",
204         controller_id, client_state.mapped_pose_buffer[0].translation[0],
205         client_state.mapped_pose_buffer[0].translation[1],
206         client_state.mapped_pose_buffer[0].translation[2],
207         client_state.mapped_pose_buffer[0].orientation[0],
208         client_state.mapped_pose_buffer[0].orientation[1],
209         client_state.mapped_pose_buffer[0].orientation[2],
210         client_state.mapped_pose_buffer[0].orientation[3]);
211     return 0;
212   }
213 
NotifyVsync(uint32_t vsync_count,int64_t display_timestamp,int64_t display_period_ns,int64_t right_eye_photon_offset_ns)214   int NotifyVsync(uint32_t vsync_count, int64_t display_timestamp,
215                   int64_t display_period_ns,
216                   int64_t right_eye_photon_offset_ns) {
217     const struct iovec data[] = {
218         {.iov_base = &vsync_count, .iov_len = sizeof(vsync_count)},
219         {.iov_base = &display_timestamp, .iov_len = sizeof(display_timestamp)},
220         {.iov_base = &display_period_ns, .iov_len = sizeof(display_period_ns)},
221         {.iov_base = &right_eye_photon_offset_ns,
222          .iov_len = sizeof(right_eye_photon_offset_ns)},
223     };
224     Transaction trans{*this};
225     Status<int> status =
226         trans.SendVector<int>(DVR_POSE_NOTIFY_VSYNC, data, nullptr);
227     ALOGE_IF(!status, "Pose NotifyVsync() failed because: %s\n",
228              status.GetErrorMessage().c_str());
229     return ReturnStatusOrError(status);
230   }
231 
GetRingBufferFd(LocalHandle * fd)232   int GetRingBufferFd(LocalHandle* fd) {
233     int ret = GetRingBuffer(nullptr);
234     if (ret < 0)
235       return ret;
236     *fd = pose_buffer_->GetBlobFd();
237     return 0;
238   }
239 
240  private:
241   friend BASE;
242 
243   // Set up a channel to the pose service.
PoseClient()244   PoseClient()
245       : BASE(pdx::default_transport::ClientChannelFactory::Create(
246             DVR_POSE_SERVICE_CLIENT)) {
247     // TODO(eieio): Cache the pose and make timeout 0 so that the API doesn't
248     // block while waiting for the pose service to come back up.
249     EnableAutoReconnect(kInfiniteTimeout);
250   }
251 
252   PoseClient(const PoseClient&) = delete;
253   PoseClient& operator=(const PoseClient&) = delete;
254 
GetPoseRingBufferInfo(DvrPoseRingBufferInfo * out_info) const255   void GetPoseRingBufferInfo(DvrPoseRingBufferInfo* out_info) const {
256     out_info->min_future_count = kPoseAsyncBufferMinFutureCount;
257     out_info->total_count = kPoseAsyncBufferTotalCount;
258     out_info->buffer = mapped_pose_buffer_->ring;
259   }
260 
261   std::unique_ptr<BufferConsumer> pose_buffer_;
262   const DvrPoseRingBuffer* mapped_pose_buffer_ = nullptr;
263 
264   struct ControllerClientState {
265     std::unique_ptr<BufferConsumer> pose_buffer;
266     const DvrPoseAsync* mapped_pose_buffer = nullptr;
267   };
268   ControllerClientState controllers_[2];
269 };
270 
271 }  // namespace dvr
272 }  // namespace android
273 
274 using android::dvr::PoseClient;
275 
276 struct DvrPose {};
277 
278 extern "C" {
279 
dvrPoseCreate()280 DvrPose* dvrPoseCreate() {
281   PoseClient* client = PoseClient::Create().release();
282   return reinterpret_cast<DvrPose*>(client);
283 }
284 
dvrPoseDestroy(DvrPose * client)285 void dvrPoseDestroy(DvrPose* client) { delete PoseClient::FromC(client); }
286 
dvrPoseGet(DvrPose * client,uint32_t vsync_count,DvrPoseAsync * out_pose)287 int dvrPoseGet(DvrPose* client, uint32_t vsync_count, DvrPoseAsync* out_pose) {
288   return PoseClient::FromC(client)->GetPose(vsync_count, out_pose);
289 }
290 
dvrPoseGetVsyncCount(DvrPose * client)291 uint32_t dvrPoseGetVsyncCount(DvrPose* client) {
292   return PoseClient::FromC(client)->GetVsyncCount();
293 }
294 
dvrPoseGetController(DvrPose * client,int32_t controller_id,uint32_t vsync_count,DvrPoseAsync * out_pose)295 int dvrPoseGetController(DvrPose* client, int32_t controller_id,
296                          uint32_t vsync_count, DvrPoseAsync* out_pose) {
297   return PoseClient::FromC(client)->GetControllerPose(controller_id,
298                                                       vsync_count, out_pose);
299 }
300 
dvrPoseLogController(DvrPose * client,bool enable)301 int dvrPoseLogController(DvrPose* client, bool enable) {
302   return PoseClient::FromC(client)->LogController(enable);
303 }
304 
dvrPosePoll(DvrPose * client,DvrPoseState * state)305 int dvrPosePoll(DvrPose* client, DvrPoseState* state) {
306   return PoseClient::FromC(client)->Poll(state);
307 }
308 
dvrPoseFreeze(DvrPose * client,const DvrPoseState * frozen_state)309 int dvrPoseFreeze(DvrPose* client, const DvrPoseState* frozen_state) {
310   return PoseClient::FromC(client)->Freeze(*frozen_state);
311 }
312 
dvrPoseSetMode(DvrPose * client,DvrPoseMode mode)313 int dvrPoseSetMode(DvrPose* client, DvrPoseMode mode) {
314   return PoseClient::FromC(client)->SetMode(mode);
315 }
316 
dvrPoseGetMode(DvrPose * client,DvrPoseMode * mode)317 int dvrPoseGetMode(DvrPose* client, DvrPoseMode* mode) {
318   return PoseClient::FromC(client)->GetMode(mode);
319 }
320 
dvrPoseGetRingBuffer(DvrPose * client,DvrPoseRingBufferInfo * out_info)321 int dvrPoseGetRingBuffer(DvrPose* client, DvrPoseRingBufferInfo* out_info) {
322   return PoseClient::FromC(client)->GetRingBuffer(out_info);
323 }
324 
privateDvrPoseNotifyVsync(DvrPose * client,uint32_t vsync_count,int64_t display_timestamp,int64_t display_period_ns,int64_t right_eye_photon_offset_ns)325 int privateDvrPoseNotifyVsync(DvrPose* client, uint32_t vsync_count,
326                               int64_t display_timestamp,
327                               int64_t display_period_ns,
328                               int64_t right_eye_photon_offset_ns) {
329   return PoseClient::FromC(client)->NotifyVsync(vsync_count, display_timestamp,
330                                                 display_period_ns,
331                                                 right_eye_photon_offset_ns);
332 }
333 
privateDvrPoseGetRingBufferFd(DvrPose * client,LocalHandle * fd)334 int privateDvrPoseGetRingBufferFd(DvrPose* client, LocalHandle* fd) {
335   return PoseClient::FromC(client)->GetRingBufferFd(fd);
336 }
337 
338 }  // extern "C"
339