libs/vr/libsensor/pose_client.cpp

   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:
  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
 274 using android::dvr::PoseClient;
 275
 276 struct DvrPose {};
 277
 278 extern "C" {
 279
 280 DvrPose* dvrPoseCreate() {
 281   PoseClient* client = PoseClient::Create().release();
 282   return reinterpret_cast<DvrPose*>(client);
 283 }
 284
 285 void dvrPoseDestroy(DvrPose* client) { delete PoseClient::FromC(client); }
 286
 287 int dvrPoseGet(DvrPose* client, uint32_t vsync_count, DvrPoseAsync* out_pose) {
 288   return PoseClient::FromC(client)->GetPose(vsync_count, out_pose);
 289 }
 290
 291 uint32_t dvrPoseGetVsyncCount(DvrPose* client) {
 292   return PoseClient::FromC(client)->GetVsyncCount();
 293 }
 294
 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
 301 int dvrPoseLogController(DvrPose* client, bool enable) {
 302   return PoseClient::FromC(client)->LogController(enable);
 303 }
 304
 305 int dvrPosePoll(DvrPose* client, DvrPoseState* state) {
 306   return PoseClient::FromC(client)->Poll(state);
 307 }
 308
 309 int dvrPoseFreeze(DvrPose* client, const DvrPoseState* frozen_state) {
 310   return PoseClient::FromC(client)->Freeze(*frozen_state);
 311 }
 312
 313 int dvrPoseSetMode(DvrPose* client, DvrPoseMode mode) {
 314   return PoseClient::FromC(client)->SetMode(mode);
 315 }
 316
 317 int dvrPoseGetMode(DvrPose* client, DvrPoseMode* mode) {
 318   return PoseClient::FromC(client)->GetMode(mode);
 319 }
 320
 321 int dvrPoseGetRingBuffer(DvrPose* client, DvrPoseRingBufferInfo* out_info) {
 322   return PoseClient::FromC(client)->GetRingBuffer(out_info);
 323 }
 324
 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
 334 int privateDvrPoseGetRingBufferFd(DvrPose* client, LocalHandle* fd) {
 335   return PoseClient::FromC(client)->GetRingBufferFd(fd);
 336 }
 337
 338 }  // extern "C"