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
15using android::pdx::LocalHandle;
16using android::pdx::LocalChannelHandle;
17using android::pdx::Status;
18using android::pdx::Transaction;
19
20#define arraysize(x) (static_cast<int32_t>(std::extent<decltype(x)>::value))
21
22namespace android {
23namespace dvr {
24
25// PoseClient is a remote interface to the pose service in sensord.
26class PoseClient : public pdx::ClientBase<PoseClient> {
27 public:
28  ~PoseClient() override {}
29
30  // Casts C handle into an instance of this class.
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.
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
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
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
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
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.
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.
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.
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
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
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
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
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.
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
255  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
274using android::dvr::PoseClient;
275
276struct DvrPose {};
277
278extern "C" {
279
280DvrPose* dvrPoseCreate() {
281  PoseClient* client = PoseClient::Create().release();
282  return reinterpret_cast<DvrPose*>(client);
283}
284
285void dvrPoseDestroy(DvrPose* client) { delete PoseClient::FromC(client); }
286
287int dvrPoseGet(DvrPose* client, uint32_t vsync_count, DvrPoseAsync* out_pose) {
288  return PoseClient::FromC(client)->GetPose(vsync_count, out_pose);
289}
290
291uint32_t dvrPoseGetVsyncCount(DvrPose* client) {
292  return PoseClient::FromC(client)->GetVsyncCount();
293}
294
295int 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
301int dvrPoseLogController(DvrPose* client, bool enable) {
302  return PoseClient::FromC(client)->LogController(enable);
303}
304
305int dvrPosePoll(DvrPose* client, DvrPoseState* state) {
306  return PoseClient::FromC(client)->Poll(state);
307}
308
309int dvrPoseFreeze(DvrPose* client, const DvrPoseState* frozen_state) {
310  return PoseClient::FromC(client)->Freeze(*frozen_state);
311}
312
313int dvrPoseSetMode(DvrPose* client, DvrPoseMode mode) {
314  return PoseClient::FromC(client)->SetMode(mode);
315}
316
317int dvrPoseGetMode(DvrPose* client, DvrPoseMode* mode) {
318  return PoseClient::FromC(client)->GetMode(mode);
319}
320
321int dvrPoseGetRingBuffer(DvrPose* client, DvrPoseRingBufferInfo* out_info) {
322  return PoseClient::FromC(client)->GetRingBuffer(out_info);
323}
324
325int 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
334int privateDvrPoseGetRingBufferFd(DvrPose* client, LocalHandle* fd) {
335  return PoseClient::FromC(client)->GetRingBufferFd(fd);
336}
337
338}  // extern "C"
339