1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "chre/platform/slpi/see/see_helper.h" 18 19 #include "pb_decode.h" 20 #include "pb_encode.h" 21 #include "sns_cal.pb.h" 22 #include "sns_client.pb.h" 23 #include "sns_client_api_v01.h" 24 #include "sns_proximity.pb.h" 25 #include "sns_rc.h" 26 #include "sns_remote_proc_state.pb.h" 27 #include "sns_std.pb.h" 28 #include "sns_std_sensor.pb.h" 29 #include "stringl.h" 30 #include "timer.h" 31 32 #include <algorithm> 33 #include <cfloat> 34 #include <cinttypes> 35 #include <cmath> 36 37 #include "chre/platform/assert.h" 38 #include "chre/platform/log.h" 39 #include "chre/platform/slpi/system_time_util.h" 40 #include "chre/util/lock_guard.h" 41 #include "chre/util/macros.h" 42 43 #ifdef CHREX_SENSOR_SUPPORT 44 #include "chre/extensions/platform/vendor_sensor_types.h" 45 #endif // CHREX_SENSOR_SUPPORT 46 47 #define LOG_NANOPB_ERROR(stream) \ 48 LOGE("Nanopb error: %s:%d", PB_GET_ERROR(stream), __LINE__) 49 50 namespace chre { 51 namespace { 52 53 //! Operating mode indicating sensor is disabled. 54 const char *kOpModeOff = "OFF"; 55 56 //! The SUID of the look up sensor. 57 const sns_std_suid kSuidLookup = sns_suid_sensor_init_default; 58 59 //! A struct to facilitate SEE response handling 60 struct SeeRespCbData { 61 SeeHelper *seeHelper; 62 uint32_t txnId; 63 }; 64 65 //! A struct to facilitate pb encode/decode 66 struct SeeBufArg { 67 const void *buf; 68 size_t bufLen; 69 }; 70 71 //! A struct to facilitate pb decode of sync calls. 72 struct SeeSyncArg { 73 sns_std_suid syncSuid; 74 void *syncData; 75 const char *syncDataType; 76 bool syncIndFound; 77 }; 78 79 //! SeeFloatArg can be used to decode a vectorized 3x3 array. 80 constexpr size_t kSeeFloatArgValLen = 9; 81 82 //! A struct to facilitate decoding a float array. 83 struct SeeFloatArg { 84 size_t index; 85 float val[kSeeFloatArgValLen]; 86 }; 87 88 //! A struct to facilitate pb decode of sensor data event. 89 struct SeeDataArg { 90 uint64_t prevTimeNs; 91 uint64_t timeNs; 92 size_t sampleIndex; 93 size_t totalSamples; 94 UniquePtr<uint8_t> event; 95 UniquePtr<SeeHelperCallbackInterface::SamplingStatusData> status; 96 SeeCalData *cal; 97 SensorType sensorType; 98 bool isHostWakeSuspendEvent; 99 bool isHostAwake; 100 }; 101 102 //! A struct to facilitate pb decode 103 struct SeeInfoArg { 104 sns_client *client; 105 sns_std_suid suid; 106 uint32_t msgId; 107 SeeSyncArg *sync; 108 SeeDataArg *data; 109 bool decodeMsgIdOnly; 110 Optional<sns_std_suid> *remoteProcSuid; 111 SeeCalInfo *calInfo; 112 }; 113 114 //! A struct to facilitate decoding sensor attributes. 115 struct SeeAttrArg { 116 union { 117 char strVal[kSeeAttrStrValLen]; 118 bool boolVal; 119 struct { 120 float fltMin; 121 float fltMax; 122 }; 123 int64_t int64; 124 }; 125 bool initialized; 126 }; 127 128 size_t getCalIndexFromSensorType(SensorType sensorType) { 129 SeeCalSensor index; 130 switch (sensorType) { 131 case SensorType::Accelerometer: 132 index = SeeCalSensor::AccelCal; 133 break; 134 case SensorType::Gyroscope: 135 index = SeeCalSensor::GyroCal; 136 break; 137 case SensorType::GeomagneticField: 138 index = SeeCalSensor::MagCal; 139 break; 140 default: 141 index = SeeCalSensor::NumCalSensors; 142 } 143 return static_cast<size_t>(index); 144 } 145 146 size_t getCalIndexFromDataType(const char *dataType) { 147 SensorType sensorType = SensorType::Unknown; 148 if (strcmp(dataType, "accel_cal") == 0) { 149 sensorType = SensorType::Accelerometer; 150 } else if (strcmp(dataType, "gyro_cal") == 0) { 151 sensorType = SensorType::Gyroscope; 152 } else if (strcmp(dataType, "mag_cal") == 0) { 153 sensorType = SensorType::GeomagneticField; 154 } 155 return getCalIndexFromSensorType(sensorType); 156 } 157 158 size_t getCalIndexFromSuid(const sns_std_suid& suid, 159 const SeeCalInfo *calInfo) { 160 size_t i = 0; 161 for (; i < kNumSeeCalSensors; i++) { 162 if (calInfo[i].suid.has_value() 163 && suidsMatch(suid, calInfo[i].suid.value())) { 164 break; 165 } 166 } 167 return i; 168 } 169 170 /** 171 * Copy an encoded pb message to a wrapper proto's field. 172 */ 173 bool copyPayload(pb_ostream_t *stream, const pb_field_t *field, 174 void *const *arg) { 175 bool success = false; 176 177 auto *data = static_cast<const SeeBufArg *>(*arg); 178 if (!pb_encode_tag_for_field(stream, field)) { 179 LOG_NANOPB_ERROR(stream); 180 } else if (!pb_encode_string( 181 stream, static_cast<const pb_byte_t *>(data->buf), data->bufLen)) { 182 LOG_NANOPB_ERROR(stream); 183 } else { 184 success = true; 185 } 186 return success; 187 } 188 189 /** 190 * Encodes sns_std_attr_req pb message. 191 * 192 * @param msg A non-null pointer to the pb message unique pointer whose object 193 * will be assigned here. 194 * @param msgLen A non-null pointer to the size of the encoded pb message. 195 * 196 * @return true if the pb message and length were obtained. 197 */ 198 bool encodeSnsStdAttrReq(UniquePtr<pb_byte_t> *msg, size_t *msgLen) { 199 CHRE_ASSERT(msg); 200 CHRE_ASSERT(msgLen); 201 202 // Initialize the pb message 203 sns_std_attr_req req = {}; 204 205 bool success = pb_get_encoded_size(msgLen, sns_std_attr_req_fields, &req); 206 if (!success) { 207 LOGE("pb_get_encoded_size failed for sns_str_attr_req"); 208 } else { 209 UniquePtr<pb_byte_t> buf(static_cast<pb_byte_t *>(memoryAlloc(*msgLen))); 210 *msg = std::move(buf); 211 212 // The encoded size can be 0 as there's only one optional field. 213 if (msg->isNull() && *msgLen > 0) { 214 LOG_OOM(); 215 } else { 216 pb_ostream_t stream = pb_ostream_from_buffer(msg->get(), *msgLen); 217 218 success = pb_encode(&stream, sns_std_attr_req_fields, &req); 219 if (!success) { 220 LOG_NANOPB_ERROR(&stream); 221 } 222 } 223 } 224 return success; 225 } 226 227 /** 228 * Encodes sns_suid_req pb message. 229 * 230 * @param dataType Sensor data type, "accel" for example. 231 * @param msg A non-null pointer to the pb message unique pointer whose object 232 * will be assigned here. 233 * @param msgLen A non-null pointer to the size of the encoded pb message. 234 * 235 * @return true if the pb message and length were obtained. 236 */ 237 bool encodeSnsSuidReq(const char *dataType, 238 UniquePtr<pb_byte_t> *msg, size_t *msgLen) { 239 CHRE_ASSERT(msg); 240 CHRE_ASSERT(msgLen); 241 bool success = false; 242 243 // Initialize the pb message 244 SeeBufArg data = { 245 .buf = dataType, 246 .bufLen = strlen(dataType), 247 }; 248 sns_suid_req req = { 249 .data_type.funcs.encode = copyPayload, 250 .data_type.arg = &data, 251 }; 252 253 if (!pb_get_encoded_size(msgLen, sns_suid_req_fields, &req)) { 254 LOGE("pb_get_encoded_size failed for sns_suid_req: %s", dataType); 255 } else if (*msgLen == 0) { 256 LOGE("Invalid pb encoded size for sns_suid_req"); 257 } else { 258 UniquePtr<pb_byte_t> buf(static_cast<pb_byte_t *>(memoryAlloc(*msgLen))); 259 *msg = std::move(buf); 260 if (msg->isNull()) { 261 LOG_OOM(); 262 } else { 263 pb_ostream_t stream = pb_ostream_from_buffer(msg->get(), *msgLen); 264 265 success = pb_encode(&stream, sns_suid_req_fields, &req); 266 if (!success) { 267 LOG_NANOPB_ERROR(&stream); 268 } 269 } 270 } 271 return success; 272 } 273 274 /** 275 * Encodes sns_std_sensor_config pb message. 276 * 277 * @param request The request to be encoded. 278 * @param msg A non-null pointer to the pb message unique pointer whose object 279 * will be assigned here. 280 * @param msgLen A non-null pointer to the size of the encoded pb message. 281 * 282 * @return true if the pb message and length were obtained. 283 */ 284 bool encodeSnsStdSensorConfig(const SeeSensorRequest& request, 285 UniquePtr<pb_byte_t> *msg, size_t *msgLen) { 286 CHRE_ASSERT(msg); 287 CHRE_ASSERT(msgLen); 288 bool success = false; 289 290 // Initialize the pb message 291 sns_std_sensor_config req = { 292 .sample_rate = request.samplingRateHz, 293 }; 294 295 if (!pb_get_encoded_size(msgLen, sns_std_sensor_config_fields, &req)) { 296 LOGE("pb_get_encoded_size failed for sns_std_sensor_config"); 297 } else if (*msgLen == 0) { 298 LOGE("Invalid pb encoded size for sns_std_sensor_config"); 299 } else { 300 UniquePtr<pb_byte_t> buf(static_cast<pb_byte_t *>(memoryAlloc(*msgLen))); 301 *msg = std::move(buf); 302 if (msg->isNull()) { 303 LOG_OOM(); 304 } else { 305 pb_ostream_t stream = pb_ostream_from_buffer(msg->get(), *msgLen); 306 307 success = pb_encode(&stream, sns_std_sensor_config_fields, &req); 308 if (!success) { 309 LOG_NANOPB_ERROR(&stream); 310 } 311 } 312 } 313 return success; 314 } 315 316 bool encodeSnsRemoteProcSensorConfig(pb_byte_t *msgBuffer, size_t msgBufferSize, 317 size_t *msgLen, 318 sns_std_client_processor processorType) { 319 CHRE_ASSERT(msgBuffer); 320 CHRE_ASSERT(msgLen); 321 322 sns_remote_proc_state_config request = { 323 .proc_type = processorType, 324 }; 325 326 pb_ostream_t stream = pb_ostream_from_buffer(msgBuffer, msgBufferSize); 327 bool success = pb_encode( 328 &stream, sns_remote_proc_state_config_fields, &request); 329 if (!success) { 330 LOG_NANOPB_ERROR(&stream); 331 } else { 332 *msgLen = stream.bytes_written; 333 } 334 335 return success; 336 } 337 338 /** 339 * Prepares a sns_client_req message with provided payload. 340 */ 341 bool prepSnsClientReq(sns_std_suid suid, uint32_t msgId, 342 void *payload, size_t payloadLen, 343 bool batchValid, uint32_t batchPeriodUs, bool passive, 344 UniquePtr<sns_client_request_msg> *msg, SeeBufArg *data) { 345 CHRE_ASSERT(payload || payloadLen == 0); 346 CHRE_ASSERT(msg); 347 CHRE_ASSERT(data); 348 bool success = false; 349 350 auto req = MakeUniqueZeroFill<sns_client_request_msg>(); 351 if (req.isNull()) { 352 LOG_OOM(); 353 } else { 354 success = true; 355 356 // Initialize sns_client_request_msg to be sent 357 data->buf = payload, 358 data->bufLen = payloadLen, 359 360 req->suid = suid; 361 req->msg_id = msgId; 362 req->susp_config.client_proc_type = SNS_STD_CLIENT_PROCESSOR_SSC; 363 req->susp_config.delivery_type = SNS_CLIENT_DELIVERY_WAKEUP; 364 req->request.has_batching = batchValid; 365 req->request.batching.batch_period = batchPeriodUs; 366 req->request.payload.funcs.encode = copyPayload; 367 req->request.payload.arg = data; 368 req->request.has_is_passive = true, 369 req->request.is_passive = passive, 370 371 *msg = std::move(req); 372 } 373 return success; 374 } 375 376 /** 377 * Helps decode a pb string field and passes the string to the calling function. 378 */ 379 bool decodeStringField(pb_istream_t *stream, const pb_field_t *field, 380 void **arg) { 381 auto *data = static_cast<SeeBufArg *>(*arg); 382 data->bufLen = stream->bytes_left; 383 data->buf = stream->state; 384 385 bool success = pb_read(stream, nullptr /* buf */, stream->bytes_left); 386 if (!success) { 387 LOG_NANOPB_ERROR(stream); 388 } 389 return success; 390 } 391 392 /** 393 * Decodes each SUID. 394 */ 395 bool decodeSnsSuidEventSuid(pb_istream_t *stream, const pb_field_t *field, 396 void **arg) { 397 sns_std_suid suid = {}; 398 bool success = pb_decode(stream, sns_std_suid_fields, &suid); 399 if (!success) { 400 LOG_NANOPB_ERROR(stream); 401 } else { 402 auto *suids = static_cast<DynamicVector<sns_std_suid> *>(*arg); 403 suids->push_back(suid); 404 } 405 return success; 406 } 407 408 bool decodeSnsSuidEvent(pb_istream_t *stream, const pb_field_t *field, 409 void **arg) { 410 auto *info = static_cast<SeeInfoArg *>(*arg); 411 if (!suidsMatch(info->suid, kSuidLookup)) { 412 LOGE("SNS_SUID_MSGID_SNS_SUID_EVENT with incorrect SUID: 0x%" PRIx64 413 " %" PRIx64, info->suid.suid_high, info->suid.suid_low); 414 } 415 416 SeeBufArg data; 417 DynamicVector<sns_std_suid> suids; 418 sns_suid_event event = { 419 .data_type.funcs.decode = decodeStringField, 420 .data_type.arg = &data, 421 .suid.funcs.decode = decodeSnsSuidEventSuid, 422 .suid.arg = &suids, 423 }; 424 425 bool success = pb_decode(stream, sns_suid_event_fields, &event); 426 if (!success) { 427 LOG_NANOPB_ERROR(stream); 428 } else { 429 // If syncData == nullptr, this indication is received outside of a sync 430 // call. If the decoded data type doesn't match the one we are waiting 431 // for, this indication is from a previous call (may be findSuidSync) 432 // and happens to arrive between another sync req/ind pair. 433 // Note that req/ind misalignment can still happen if findSuidSync is 434 // called again with the same data type. 435 // Note that there's no need to compare the SUIDs as no other calls 436 // but findSuidSync populate mWaitingDataType and can lead to a data 437 // type match. 438 if (info->sync->syncData == nullptr 439 || strncmp(info->sync->syncDataType, 440 static_cast<const char *>(data.buf), 441 std::min(data.bufLen, kSeeAttrStrValLen)) != 0) { 442 LOGW("Received late SNS_SUID_MSGID_SNS_SUID_EVENT indication"); 443 } else { 444 info->sync->syncIndFound = true; 445 auto *outputSuids = static_cast<DynamicVector<sns_std_suid> *>( 446 info->sync->syncData); 447 for (const auto& suid : suids) { 448 outputSuids->push_back(suid); 449 } 450 } 451 } 452 return success; 453 } 454 455 /** 456 * Decode messages defined in sns_suid.proto 457 */ 458 bool decodeSnsSuidProtoEvent(pb_istream_t *stream, const pb_field_t *field, 459 void **arg) { 460 bool success = false; 461 462 auto *info = static_cast<SeeInfoArg *>(*arg); 463 switch (info->msgId) { 464 case SNS_SUID_MSGID_SNS_SUID_EVENT: 465 success = decodeSnsSuidEvent(stream, field, arg); 466 break; 467 468 default: 469 LOGW("Unhandled sns_suid.proto msg ID: %" PRIu32, info->msgId); 470 break; 471 } 472 return success; 473 } 474 475 /** 476 * Defined in sns_std_sensor.pb.h 477 */ 478 const char *getAttrNameFromAttrId(int32_t id) { 479 switch (id) { 480 case SNS_STD_SENSOR_ATTRID_NAME: 481 return "NAME"; 482 case SNS_STD_SENSOR_ATTRID_VENDOR: 483 return "VENDOR"; 484 case SNS_STD_SENSOR_ATTRID_TYPE: 485 return "TYPE"; 486 case SNS_STD_SENSOR_ATTRID_AVAILABLE: 487 return "AVAILABLE"; 488 case SNS_STD_SENSOR_ATTRID_VERSION: 489 return "VERSION"; 490 case SNS_STD_SENSOR_ATTRID_API: 491 return "API"; 492 case SNS_STD_SENSOR_ATTRID_RATES: 493 return "RATES"; 494 case SNS_STD_SENSOR_ATTRID_RESOLUTIONS: 495 return "RESOLUTIONS"; 496 case SNS_STD_SENSOR_ATTRID_FIFO_SIZE: 497 return "FIFO_SIZE"; 498 case SNS_STD_SENSOR_ATTRID_ACTIVE_CURRENT: 499 return "ACTIVE_CURRENT"; 500 case SNS_STD_SENSOR_ATTRID_SLEEP_CURRENT: 501 return "SLEEP_CURRENT"; 502 case SNS_STD_SENSOR_ATTRID_RANGES: 503 return "RANGES"; 504 case SNS_STD_SENSOR_ATTRID_OP_MODES: 505 return "OP_MODES"; 506 case SNS_STD_SENSOR_ATTRID_DRI: 507 return "DRI"; 508 case SNS_STD_SENSOR_ATTRID_STREAM_SYNC: 509 return "STREAM_SYNC"; 510 case SNS_STD_SENSOR_ATTRID_EVENT_SIZE: 511 return "EVENT_SIZE"; 512 case SNS_STD_SENSOR_ATTRID_STREAM_TYPE: 513 return "STREAM_TYPE"; 514 case SNS_STD_SENSOR_ATTRID_DYNAMIC: 515 return "DYNAMIC"; 516 case SNS_STD_SENSOR_ATTRID_HW_ID: 517 return "HW_ID"; 518 case SNS_STD_SENSOR_ATTRID_RIGID_BODY: 519 return "RIGID_BODY"; 520 case SNS_STD_SENSOR_ATTRID_PLACEMENT: 521 return "PLACEMENT"; 522 case SNS_STD_SENSOR_ATTRID_PHYSICAL_SENSOR: 523 return "PHYSICAL_SENSOR"; 524 case SNS_STD_SENSOR_ATTRID_PHYSICAL_SENSOR_TESTS: 525 return "PHYSICAL_SENSOR_TESTS"; 526 case SNS_STD_SENSOR_ATTRID_SELECTED_RESOLUTION: 527 return "SELECTED_RESOLUTION"; 528 case SNS_STD_SENSOR_ATTRID_SELECTED_RANGE: 529 return "SELECTED_RANGE"; 530 case SNS_STD_SENSOR_ATTRID_ADDITIONAL_LOW_LATENCY_RATES: 531 return "LOW_LATENCY_RATES"; 532 case SNS_STD_SENSOR_ATTRID_PASSIVE_REQUEST: 533 return "PASSIVE_REQUEST"; 534 default: 535 return "UNKNOWN ATTRIBUTE"; 536 } 537 } 538 539 /** 540 * Decodes each attribute field and passes the value to the calling function. 541 * For repeated fields of float or integers, only store the maximum and 542 * minimum values for the calling function. 543 */ 544 bool decodeSnsStdAttrValue(pb_istream_t *stream, const pb_field_t *field, 545 void **arg) { 546 bool success = false; 547 548 struct DecodeData { 549 SeeBufArg strData; 550 SeeAttrArg subtypeAttrArg; 551 sns_std_attr_value_data value; 552 }; 553 auto data = MakeUniqueZeroFill<DecodeData>(); 554 555 if (data.isNull()) { 556 LOG_OOM(); 557 } else { 558 data->value.str.funcs.decode = decodeStringField; 559 data->value.str.arg = &data->strData; 560 data->value.subtype.values.funcs.decode = decodeSnsStdAttrValue; 561 data->value.subtype.values.arg = &data->subtypeAttrArg; 562 563 success = pb_decode(stream, sns_std_attr_value_data_fields, &data->value); 564 if (!success) { 565 LOG_NANOPB_ERROR(stream); 566 } else { 567 auto *attrVal = static_cast<SeeAttrArg *>(*arg); 568 if (data->value.has_flt) { 569 // If this is a float (repeated) field, initialize the union as floats 570 // to store the maximum and minmum values of the repeated fields. 571 if (!attrVal->initialized) { 572 attrVal->initialized = true; 573 attrVal->fltMin = FLT_MAX; 574 attrVal->fltMax = FLT_MIN; 575 } 576 if (data->value.flt < attrVal->fltMin) { 577 attrVal->fltMin = data->value.flt; 578 } 579 if (data->value.flt > attrVal->fltMax) { 580 attrVal->fltMax = data->value.flt; 581 } 582 } else if (data->value.has_sint) { 583 attrVal->int64 = data->value.sint; 584 } else if (data->value.has_boolean) { 585 attrVal->boolVal = data->value.boolean; 586 } else if (data->strData.buf != nullptr) { 587 strlcpy(attrVal->strVal, static_cast<const char *>(data->strData.buf), 588 sizeof(attrVal->strVal)); 589 } else if (!data->value.has_subtype) { 590 LOGW("Unknown attr type"); 591 } 592 } 593 } 594 return success; 595 } 596 597 bool decodeSnsStrAttr(pb_istream_t *stream, const pb_field_t *field, 598 void **arg) { 599 bool success = false; 600 601 struct Decodedata { 602 SeeAttrArg attrArg; 603 sns_std_attr attr; 604 }; 605 auto data = MakeUniqueZeroFill<Decodedata>(); 606 607 if (data.isNull()) { 608 LOG_OOM(); 609 } else { 610 data->attr.value.values.funcs.decode = decodeSnsStdAttrValue; 611 data->attr.value.values.arg = &data->attrArg; 612 613 success = pb_decode(stream, sns_std_attr_fields, &data->attr); 614 if (!success) { 615 LOG_NANOPB_ERROR(stream); 616 } else { 617 auto *attrData = static_cast<SeeAttributes *>(*arg); 618 switch (data->attr.attr_id) { 619 case SNS_STD_SENSOR_ATTRID_NAME: 620 strlcpy(attrData->name, data->attrArg.strVal, sizeof(attrData->name)); 621 break; 622 case SNS_STD_SENSOR_ATTRID_VENDOR: 623 strlcpy(attrData->vendor, data->attrArg.strVal, 624 sizeof(attrData->vendor)); 625 break; 626 case SNS_STD_SENSOR_ATTRID_TYPE: 627 strlcpy(attrData->type, data->attrArg.strVal, sizeof(attrData->type)); 628 break; 629 case SNS_STD_SENSOR_ATTRID_AVAILABLE: 630 if (!data->attrArg.boolVal) { 631 LOGW("%s: %d", getAttrNameFromAttrId(data->attr.attr_id), 632 data->attrArg.boolVal); 633 } 634 break; 635 case SNS_STD_SENSOR_ATTRID_RATES: 636 attrData->maxSampleRate = data->attrArg.fltMax; 637 break; 638 case SNS_STD_SENSOR_ATTRID_STREAM_TYPE: 639 attrData->streamType = data->attrArg.int64; 640 break; 641 case SNS_STD_SENSOR_ATTRID_HW_ID: 642 attrData->hwId = data->attrArg.int64; 643 break; 644 case SNS_STD_SENSOR_ATTRID_PASSIVE_REQUEST: 645 attrData->passiveRequest = data->attrArg.boolVal; 646 break; 647 default: 648 break; 649 } 650 } 651 } 652 return success; 653 } 654 655 bool decodeSnsStdAttrEvent(pb_istream_t *stream, const pb_field_t *field, 656 void **arg) { 657 bool success = false; 658 659 struct DecodeData { 660 SeeAttributes attr; 661 sns_std_attr_event event; 662 }; 663 auto data = MakeUniqueZeroFill<DecodeData>(); 664 665 if (data.isNull()) { 666 LOG_OOM(); 667 } else { 668 data->event.attributes.funcs.decode = decodeSnsStrAttr; 669 data->event.attributes.arg = &data->attr; 670 671 success = pb_decode(stream, sns_std_attr_event_fields, &data->event); 672 if (!success) { 673 LOG_NANOPB_ERROR(stream); 674 } else { 675 auto *info = static_cast<SeeInfoArg *>(*arg); 676 677 // If syncData == nullptr, this indication is received outside of a sync 678 // call. If the decoded SUID doesn't match the one we are waiting for, 679 // this indication is from a previous getAttributes call and happens to 680 // arrive between a later findAttributesSync req/ind pair. 681 // Note that req/ind misalignment can still happen if getAttributesSync is 682 // called again with the same SUID. 683 if (info->sync->syncData == nullptr 684 || !suidsMatch(info->suid, info->sync->syncSuid)) { 685 LOGW("Received late SNS_STD_MSGID_SNS_STD_ATTR_EVENT indication"); 686 } else { 687 info->sync->syncIndFound = true; 688 memcpy(info->sync->syncData, &data->attr, sizeof(data->attr)); 689 } 690 } 691 } 692 return success; 693 } 694 695 /** 696 * Decode messages defined in sns_std.proto 697 */ 698 bool decodeSnsStdProtoEvent(pb_istream_t *stream, const pb_field_t *field, 699 void **arg) { 700 bool success = false; 701 702 auto *info = static_cast<SeeInfoArg *>(*arg); 703 switch (info->msgId) { 704 case SNS_STD_MSGID_SNS_STD_ATTR_EVENT: 705 success = decodeSnsStdAttrEvent(stream, field, arg); 706 break; 707 708 case SNS_STD_MSGID_SNS_STD_FLUSH_EVENT: 709 // An empty message. 710 success = true; 711 break; 712 713 case SNS_STD_MSGID_SNS_STD_ERROR_EVENT: { 714 sns_std_error_event event = {}; 715 success = pb_decode(stream, sns_std_error_event_fields, &event); 716 if (!success) { 717 LOG_NANOPB_ERROR(stream); 718 } else { 719 LOGW("SNS_STD_MSGID_SNS_STD_ERROR_EVENT: %d", event.error); 720 } 721 break; 722 } 723 724 default: 725 LOGW("Unhandled sns_std.proto msg ID %" PRIu32, info->msgId); 726 } 727 return success; 728 } 729 730 // TODO: Support compensation matrix and scaling factor calibration 731 void applyThreeAxisCalibration( 732 chreSensorThreeAxisData::chreSensorThreeAxisSampleData *sample, 733 const float *val, const SeeCalData *cal) { 734 float bias[3] = {}; 735 if (cal != nullptr && cal->hasBias) { 736 memcpy(bias, cal->bias, sizeof(bias)); 737 } 738 sample->x = val[0] - bias[0]; 739 sample->y = val[1] - bias[1]; 740 sample->z = val[2] - bias[2]; 741 } 742 743 void populateEventSample(SeeDataArg *data, const float *val) { 744 size_t index = data->sampleIndex; 745 if (!data->event.isNull() && index < data->totalSamples) { 746 SensorSampleType sampleType = getSensorSampleTypeFromSensorType( 747 data->sensorType); 748 749 uint32_t *timestampDelta = nullptr; 750 switch (sampleType) { 751 case SensorSampleType::ThreeAxis: { 752 auto *event = reinterpret_cast<chreSensorThreeAxisData *>( 753 data->event.get()); 754 applyThreeAxisCalibration(&event->readings[index], val, data->cal); 755 timestampDelta = &event->readings[index].timestampDelta; 756 break; 757 } 758 759 case SensorSampleType::Float: { 760 auto *event = reinterpret_cast<chreSensorFloatData *>( 761 data->event.get()); 762 event->readings[index].value = val[0]; 763 timestampDelta = &event->readings[index].timestampDelta; 764 break; 765 } 766 767 case SensorSampleType::Byte: { 768 auto *event = reinterpret_cast<chreSensorByteData *>(data->event.get()); 769 event->readings[index].value = 0; 770 event->readings[index].isNear = (val[0] > 0.5f); 771 timestampDelta = &event->readings[index].timestampDelta; 772 break; 773 } 774 775 case SensorSampleType::Occurrence: { 776 auto *event = reinterpret_cast<chreSensorOccurrenceData *>( 777 data->event.get()); 778 timestampDelta = &event->readings[index].timestampDelta; 779 break; 780 } 781 782 #ifdef CHREX_SENSOR_SUPPORT 783 case SensorSampleType::Vendor0: { 784 auto *event = reinterpret_cast<chrexSensorVendor0Data *>( 785 data->event.get()); 786 memcpy(event->readings[index].values, val, 787 sizeof(event->readings[index].values)); 788 timestampDelta = &event->readings[index].timestampDelta; 789 break; 790 } 791 792 case SensorSampleType::Vendor1: { 793 auto *event = reinterpret_cast<chrexSensorVendor1Data *>( 794 data->event.get()); 795 memcpy(event->readings[index].values, val, 796 sizeof(event->readings[index].values)); 797 timestampDelta = &event->readings[index].timestampDelta; 798 break; 799 } 800 801 case SensorSampleType::Vendor2: { 802 auto *event = reinterpret_cast<chrexSensorVendor2Data *>( 803 data->event.get()); 804 event->readings[index].value = *val; 805 timestampDelta = &event->readings[index].timestampDelta; 806 break; 807 } 808 #endif // CHREX_SENSOR_SUPPORT 809 810 default: 811 LOGE("Invalid sample type %" PRIu8, static_cast<uint8_t>(sampleType)); 812 } 813 814 if (data->sampleIndex == 0) { 815 auto *header = reinterpret_cast<chreSensorDataHeader *>( 816 data->event.get()); 817 header->baseTimestamp = data->timeNs; 818 *timestampDelta = 0; 819 } else { 820 uint64_t delta = data->timeNs - data->prevTimeNs; 821 if (delta > UINT32_MAX) { 822 LOGE("Sensor %" PRIu8 " timestampDelta overflow: prev %" PRIu64 823 " curr %" PRIu64, static_cast<uint8_t>(data->sensorType), 824 data->prevTimeNs, data->timeNs); 825 delta = UINT32_MAX; 826 } 827 *timestampDelta = static_cast<uint32_t>(delta); 828 } 829 data->prevTimeNs = data->timeNs; 830 } 831 } 832 833 /** 834 * Decodes a float array and ensures that the data doesn't go out of bound. 835 */ 836 bool decodeFloatData(pb_istream_t *stream, const pb_field_t *field, 837 void **arg) { 838 auto *data = static_cast<SeeFloatArg *>(*arg); 839 840 float value; 841 float *fltPtr = &value; 842 if (data->index >= ARRAY_SIZE(data->val)) { 843 LOGE("Float array length exceeds %zu", ARRAY_SIZE(data->val)); 844 } else { 845 // Decode to the provided array only if it doesn't go out of bound. 846 fltPtr = &(data->val[data->index]); 847 } 848 // Increment index whether it's gone out of bounds or not. 849 (data->index)++; 850 851 bool success = pb_decode_fixed32(stream, fltPtr); 852 if (!success) { 853 LOG_NANOPB_ERROR(stream); 854 } 855 return success; 856 } 857 858 bool decodeSnsStdSensorPhysicalConfigEvent( 859 pb_istream_t *stream, const pb_field_t *field, void **arg) { 860 SeeBufArg data = {}; 861 sns_std_sensor_physical_config_event event = { 862 .operation_mode.funcs.decode = decodeStringField, 863 .operation_mode.arg = &data, 864 }; 865 866 bool success = pb_decode(stream, sns_std_sensor_physical_config_event_fields, 867 &event); 868 if (!success) { 869 LOG_NANOPB_ERROR(stream); 870 } else { 871 auto statusData = 872 MakeUniqueZeroFill<SeeHelperCallbackInterface::SamplingStatusData>(); 873 if (statusData.isNull()) { 874 LOG_OOM(); 875 } else { 876 struct chreSensorSamplingStatus *status = &statusData->status; 877 878 if (event.has_sample_rate) { 879 statusData->intervalValid = true; 880 status->interval = static_cast<uint64_t>( 881 ceilf(Seconds(1).toRawNanoseconds() / event.sample_rate)); 882 } 883 884 // If operation_mode is populated, decoded string length will be > 0. 885 if (data.bufLen > 0) { 886 statusData->enabledValid = true; 887 status->enabled = 888 (strncmp(static_cast<const char *>(data.buf), kOpModeOff, 889 std::min(data.bufLen, sizeof(kOpModeOff))) != 0); 890 } 891 892 if (event.has_sample_rate || data.bufLen > 0) { 893 auto *info = static_cast<SeeInfoArg *>(*arg); 894 statusData->sensorType = info->data->sensorType; 895 info->data->status = std::move(statusData); 896 } 897 } 898 } 899 return success; 900 } 901 902 bool decodeSnsStdSensorEvent(pb_istream_t *stream, const pb_field_t *field, 903 void **arg) { 904 SeeFloatArg sample = {}; 905 sns_std_sensor_event event = { 906 .data.funcs.decode = decodeFloatData, 907 .data.arg = &sample, 908 }; 909 910 bool success = pb_decode(stream, sns_std_sensor_event_fields, &event); 911 if (!success) { 912 LOG_NANOPB_ERROR(stream); 913 } else { 914 auto *info = static_cast<SeeInfoArg *>(*arg); 915 populateEventSample(info->data, sample.val); 916 } 917 return success; 918 } 919 920 /** 921 * Decode messages defined in sns_std_sensor.proto 922 */ 923 bool decodeSnsStdSensorProtoEvent(pb_istream_t *stream, const pb_field_t *field, 924 void **arg) { 925 bool success = false; 926 927 auto *info = static_cast<SeeInfoArg *>(*arg); 928 switch (info->msgId) { 929 case SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_PHYSICAL_CONFIG_EVENT: 930 success = decodeSnsStdSensorPhysicalConfigEvent(stream, field, arg); 931 break; 932 933 case SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_EVENT: 934 success = decodeSnsStdSensorEvent(stream, field, arg); 935 break; 936 937 default: 938 LOGW("Unhandled sns_std_sensor.proto msg ID %" PRIu32, info->msgId); 939 } 940 return success; 941 } 942 943 bool decodeSnsCalEvent(pb_istream_t *stream, const pb_field_t *field, 944 void **arg) { 945 SeeFloatArg offset = {}; 946 SeeFloatArg scale = {}; 947 SeeFloatArg matrix = {}; 948 sns_cal_event event = { 949 .bias.funcs.decode = decodeFloatData, 950 .bias.arg = &offset, 951 .scale_factor.funcs.decode = decodeFloatData, 952 .scale_factor.arg = &scale, 953 .comp_matrix.funcs.decode = decodeFloatData, 954 .comp_matrix.arg = &matrix, 955 }; 956 957 bool success = pb_decode(stream, sns_cal_event_fields, &event); 958 if (!success) { 959 LOG_NANOPB_ERROR(stream); 960 } else { 961 auto *info = static_cast<SeeInfoArg *>(*arg); 962 SeeCalInfo *calInfo = info->calInfo; 963 size_t calIndex = getCalIndexFromSuid(info->suid, calInfo); 964 if (calIndex >= kNumSeeCalSensors) { 965 LOGW("Cal sensor index out of bounds 0x%" PRIx64 " %" PRIx64, 966 info->suid.suid_high, info->suid.suid_low); 967 } else { 968 SeeCalData *cal = &calInfo[calIndex].cal; 969 970 cal->hasBias = (offset.index == 3); 971 if (cal->hasBias) { 972 memcpy(cal->bias, offset.val, sizeof(cal->bias)); 973 } 974 975 cal->hasScale = (scale.index == 3); 976 if (cal->hasScale) { 977 memcpy(cal->scale, scale.val, sizeof(cal->scale)); 978 } 979 980 cal->hasMatrix = (matrix.index == 9); 981 if (cal->hasScale) { 982 memcpy(cal->matrix, matrix.val, sizeof(cal->matrix)); 983 } 984 985 cal->accuracy = static_cast<uint8_t>(event.status); 986 } 987 } 988 return success; 989 } 990 991 /** 992 * Decode messages defined in sns_cal.proto 993 */ 994 bool decodeSnsCalProtoEvent(pb_istream_t *stream, const pb_field_t *field, 995 void **arg) { 996 bool success = false; 997 998 auto *info = static_cast<SeeInfoArg *>(*arg); 999 switch (info->msgId) { 1000 case SNS_CAL_MSGID_SNS_CAL_EVENT: 1001 success = decodeSnsCalEvent(stream, field, arg); 1002 break; 1003 1004 default: 1005 LOGW("Unhandled sns_cal.proto msg ID %" PRIu32, info->msgId); 1006 } 1007 return success; 1008 } 1009 1010 bool decodeSnsProximityEvent(pb_istream_t *stream, const pb_field_t *field, 1011 void **arg) { 1012 sns_proximity_event event = {}; 1013 1014 bool success = pb_decode(stream, sns_proximity_event_fields, &event); 1015 if (!success) { 1016 LOG_NANOPB_ERROR(stream); 1017 } else { 1018 float value = static_cast<float>(event.proximity_event_type); 1019 auto *info = static_cast<SeeInfoArg *>(*arg); 1020 populateEventSample(info->data, &value); 1021 } 1022 return success; 1023 } 1024 1025 /** 1026 * Decode messages defined in sns_proximity.proto 1027 */ 1028 bool decodeSnsProximityProtoEvent(pb_istream_t *stream, const pb_field_t *field, 1029 void **arg) { 1030 bool success = false; 1031 1032 auto *info = static_cast<SeeInfoArg *>(*arg); 1033 switch (info->msgId) { 1034 case SNS_PROXIMITY_MSGID_SNS_PROXIMITY_EVENT: 1035 success = decodeSnsProximityEvent(stream, field, arg); 1036 break; 1037 1038 default: 1039 LOGW("Unhandled sns_proximity.proto msg ID %" PRIu32, info->msgId); 1040 } 1041 return success; 1042 } 1043 1044 bool decodeSnsRemoteProcStateEvent( 1045 pb_istream_t *stream, const pb_field_t *field, void **arg) { 1046 sns_remote_proc_state_event event = sns_remote_proc_state_event_init_default; 1047 bool success = pb_decode(stream, sns_remote_proc_state_event_fields, &event); 1048 if (!success) { 1049 LOG_NANOPB_ERROR(stream); 1050 } else if (event.proc_type == SNS_STD_CLIENT_PROCESSOR_APSS) { 1051 auto *info = static_cast<SeeInfoArg *>(*arg); 1052 info->data->isHostWakeSuspendEvent = true; 1053 info->data->isHostAwake = 1054 (event.event_type == SNS_REMOTE_PROC_STATE_AWAKE); 1055 } 1056 return success; 1057 } 1058 1059 /** 1060 * Decode messages defined in sns_remote_proc_state.proto 1061 */ 1062 bool decodeSnsRemoteProcProtoEvent( 1063 pb_istream_t *stream, const pb_field_t *field, void **arg) { 1064 bool success = false; 1065 auto *info = static_cast<SeeInfoArg *>(*arg); 1066 switch (info->msgId) { 1067 case SNS_REMOTE_PROC_STATE_MSGID_SNS_REMOTE_PROC_STATE_EVENT: 1068 success = decodeSnsRemoteProcStateEvent(stream, field, arg); 1069 break; 1070 1071 default: 1072 LOGW("Unhandled sns_remote_proc_state.proto msg ID %" PRIu32, 1073 info->msgId); 1074 } 1075 return success; 1076 } 1077 1078 bool assignPayloadCallback(const SeeInfoArg *info, pb_callback_t *payload) { 1079 bool success = true; 1080 1081 payload->arg = const_cast<SeeInfoArg *>(info); 1082 1083 if (info->remoteProcSuid->has_value() 1084 && suidsMatch(info->suid, info->remoteProcSuid->value())) { 1085 payload->funcs.decode = decodeSnsRemoteProcProtoEvent; 1086 } else if (suidsMatch(info->suid, kSuidLookup)) { 1087 payload->funcs.decode = decodeSnsSuidProtoEvent; 1088 } else { 1089 // Assumed: "real" sensors SUIDs 1090 switch (info->msgId) { 1091 case SNS_STD_MSGID_SNS_STD_ATTR_EVENT: 1092 case SNS_STD_MSGID_SNS_STD_FLUSH_EVENT: 1093 case SNS_STD_MSGID_SNS_STD_ERROR_EVENT: 1094 payload->funcs.decode = decodeSnsStdProtoEvent; 1095 break; 1096 1097 case SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_PHYSICAL_CONFIG_EVENT: 1098 case SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_EVENT: 1099 payload->funcs.decode = decodeSnsStdSensorProtoEvent; 1100 break; 1101 1102 case SNS_CAL_MSGID_SNS_CAL_EVENT: 1103 payload->funcs.decode = decodeSnsCalProtoEvent; 1104 break; 1105 1106 case SNS_PROXIMITY_MSGID_SNS_PROXIMITY_EVENT: 1107 payload->funcs.decode = decodeSnsProximityProtoEvent; 1108 break; 1109 1110 default: 1111 success = false; 1112 LOGW("Unhandled msg ID %" PRIu32, info->msgId); 1113 } 1114 } 1115 return success; 1116 } 1117 1118 /** 1119 * Decodes only msg_id and timestamp defined in sns_client_event and converts 1120 * the timestamp to nanoseconds. 1121 */ 1122 bool decodeMsgIdAndTime(pb_istream_t *stream, uint32_t *msgId, 1123 uint64_t *timeNs) { 1124 sns_client_event_msg_sns_client_event event = {}; 1125 1126 bool success = pb_decode( 1127 stream, sns_client_event_msg_sns_client_event_fields, &event); 1128 if (!success) { 1129 LOG_NANOPB_ERROR(stream); 1130 } else { 1131 *msgId = event.msg_id; 1132 *timeNs = getNanosecondsFromQTimerTicks(event.timestamp); 1133 } 1134 return success; 1135 } 1136 1137 /** 1138 * Decodes pb-encoded message 1139 */ 1140 bool decodeSnsClientEventMsg(pb_istream_t *stream, const pb_field_t *field, 1141 void **arg) { 1142 // Make a copy for data decoding. 1143 pb_istream_t streamCpy = *stream; 1144 1145 auto *info = static_cast<SeeInfoArg *>(*arg); 1146 bool success = decodeMsgIdAndTime(stream, &info->msgId, &info->data->timeNs); 1147 1148 if (success && !info->decodeMsgIdOnly) { 1149 sns_client_event_msg_sns_client_event event = {}; 1150 1151 // Payload callback must be assigned if and only if we want to decode beyond 1152 // msg ID. 1153 success = assignPayloadCallback(info, &event.payload); 1154 if (!success) { 1155 LOGE("No pb callback assigned"); 1156 } else { 1157 success = pb_decode(&streamCpy, 1158 sns_client_event_msg_sns_client_event_fields, &event); 1159 if (!success) { 1160 LOG_NANOPB_ERROR(&streamCpy); 1161 } 1162 } 1163 } 1164 1165 // Increment sample count only after sensor event decoding. 1166 if (success 1167 && (info->msgId == SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_EVENT 1168 || info->msgId == SNS_PROXIMITY_MSGID_SNS_PROXIMITY_EVENT)) { 1169 info->data->sampleIndex++; 1170 } 1171 return success; 1172 } 1173 1174 /** 1175 * Obtain the SensorType from the list of registered SensorInfos. 1176 */ 1177 SensorType getSensorTypeFromSensorInfo( 1178 sns_client *client, const sns_std_suid& suid, 1179 const DynamicVector<SeeHelper::SensorInfo>& sensorInfos) { 1180 bool suidFound = false; 1181 SensorType otherType; 1182 for (const auto& sensorInfo : sensorInfos) { 1183 if (suidsMatch(sensorInfo.suid, suid)) { 1184 suidFound = true; 1185 if (sensorInfo.client == client) { 1186 return sensorInfo.sensorType; 1187 } 1188 otherType = sensorInfo.sensorType; 1189 } 1190 } 1191 1192 if (suidFound) { 1193 LOGE("Unmatched client: %p, SUID 0x%016" PRIx64 " %016" PRIx64, 1194 client, suid.suid_high, suid.suid_low); 1195 // TODO: remove after b/79993302 is resolved. 1196 for (const auto& sensorInfo : sensorInfos) { 1197 LOGE(" %p, 0x%016" PRIx64 " %016" PRIx64, 1198 sensorInfo.client, 1199 sensorInfo.suid.suid_high, sensorInfo.suid.suid_low); 1200 } 1201 1202 // Return SensorType in the other sns_client that matches the SUID as a 1203 // backup plan. 1204 return otherType; 1205 } 1206 return SensorType::Unknown; 1207 } 1208 1209 /** 1210 * Allocate event memory according to SensorType and the number of samples. 1211 */ 1212 void *allocateEvent(SensorType sensorType, size_t numSamples) { 1213 SensorSampleType sampleType = getSensorSampleTypeFromSensorType(sensorType); 1214 size_t sampleSize = 0; 1215 switch (sampleType) { 1216 case SensorSampleType::ThreeAxis: 1217 sampleSize = sizeof( 1218 chreSensorThreeAxisData::chreSensorThreeAxisSampleData); 1219 break; 1220 1221 case SensorSampleType::Float: 1222 sampleSize = sizeof( 1223 chreSensorFloatData::chreSensorFloatSampleData); 1224 break; 1225 1226 case SensorSampleType::Byte: 1227 sampleSize = sizeof( 1228 chreSensorByteData::chreSensorByteSampleData); 1229 break; 1230 1231 case SensorSampleType::Occurrence: 1232 sampleSize = sizeof( 1233 chreSensorOccurrenceData::chreSensorOccurrenceSampleData); 1234 break; 1235 1236 #ifdef CHREX_SENSOR_SUPPORT 1237 case SensorSampleType::Vendor0: 1238 sampleSize = sizeof(chrexSensorVendor0SampleData); 1239 break; 1240 1241 case SensorSampleType::Vendor1: 1242 sampleSize = sizeof(chrexSensorVendor1SampleData); 1243 break; 1244 1245 case SensorSampleType::Vendor2: 1246 sampleSize = sizeof(chrexSensorVendor2SampleData); 1247 break; 1248 #endif // CHREX_SENSOR_SUPPORT 1249 1250 default: 1251 LOGE("Unhandled SensorSampleType for SensorType %" PRIu8, 1252 static_cast<uint8_t>(sensorType)); 1253 } 1254 1255 size_t memorySize = (sampleType == SensorSampleType::Unknown) 1256 ? 0 : (sizeof(chreSensorDataHeader) + numSamples * sampleSize); 1257 void *event = (memorySize == 0) ? nullptr : memoryAlloc(memorySize); 1258 1259 if (event == nullptr && memorySize != 0) { 1260 LOG_OOM(); 1261 } 1262 return event; 1263 } 1264 1265 // Allocates the sensor event memory and partially populates the header. 1266 bool prepareSensorEvent(SeeInfoArg& info) { 1267 bool success = false; 1268 1269 UniquePtr<uint8_t> buf(static_cast<uint8 *>( 1270 allocateEvent(info.data->sensorType, info.data->sampleIndex))); 1271 info.data->event = std::move(buf); 1272 1273 if (!info.data->event.isNull()) { 1274 success = true; 1275 1276 info.data->prevTimeNs = 0; 1277 1278 auto *header = reinterpret_cast<chreSensorDataHeader *>( 1279 info.data->event.get()); 1280 memset(header->reserved, 0, sizeof(header->reserved)); 1281 header->sensorHandle = getSensorHandleFromSensorType( 1282 info.data->sensorType); 1283 header->readingCount = info.data->sampleIndex; 1284 1285 // Protect against out of bounds access in data decoding. 1286 info.data->totalSamples = info.data->sampleIndex; 1287 1288 // Reset sampleIndex only after memory has been allocated and header 1289 // populated. 1290 info.data->sampleIndex = 0; 1291 } 1292 return success; 1293 } 1294 1295 } // anonymous namespace 1296 1297 const SeeHelper::SnsClientApi SeeHelper::kDefaultApi = { 1298 .sns_client_init = sns_client_init, 1299 .sns_client_deinit = sns_client_deinit, 1300 .sns_client_send = sns_client_send, 1301 }; 1302 1303 SeeHelper::~SeeHelper() { 1304 for (auto *client : mSeeClients) { 1305 int status = mSnsClientApi->sns_client_deinit(client); 1306 if (status != 0) { 1307 LOGE("Failed to release sensor client: %d", status); 1308 } 1309 } 1310 } 1311 1312 void SeeHelper::handleSnsClientEventMsg( 1313 sns_client *client, const void *payload, size_t payloadLen) { 1314 CHRE_ASSERT(payload); 1315 1316 pb_istream_t stream = pb_istream_from_buffer( 1317 static_cast<const pb_byte_t *>(payload), payloadLen); 1318 1319 // Make a copy of the stream for sensor data decoding. 1320 pb_istream_t streamCpy = stream; 1321 1322 struct DecodeData { 1323 SeeSyncArg syncArg = {}; 1324 SeeDataArg dataArg = {}; 1325 SeeInfoArg info = {}; 1326 sns_client_event_msg event = {}; 1327 }; 1328 auto data = MakeUnique<DecodeData>(); 1329 1330 if (data.isNull()) { 1331 LOG_OOM(); 1332 } else { 1333 // Only initialize fields that are not accessed in the main CHRE thread. 1334 data->info.client = client; 1335 data->info.sync = &data->syncArg; 1336 data->info.data = &data->dataArg; 1337 data->info.decodeMsgIdOnly = true; 1338 data->info.remoteProcSuid = &mRemoteProcSuid; 1339 data->info.calInfo = &mCalInfo[0]; 1340 data->event.events.funcs.decode = decodeSnsClientEventMsg; 1341 data->event.events.arg = &data->info; 1342 1343 // Decode only SUID and MSG ID to help further decode. 1344 if (!pb_decode(&stream, sns_client_event_msg_fields, &data->event)) { 1345 LOG_NANOPB_ERROR(&stream); 1346 } else { 1347 data->info.suid = data->event.suid; 1348 data->info.decodeMsgIdOnly = false; 1349 data->info.data->cal = getCalDataFromSuid(data->info.suid); 1350 data->info.data->sensorType = getSensorTypeFromSensorInfo( 1351 data->info.client, data->info.suid, mSensorInfos); 1352 1353 mMutex.lock(); 1354 bool synchronizedDecode = mWaitingOnInd; 1355 if (!synchronizedDecode) { 1356 // Early unlock, we're not going to use anything from the main thread. 1357 mMutex.unlock(); 1358 } else { 1359 // Populate fields set by the main thread. 1360 data->info.sync->syncData = mSyncData; 1361 data->info.sync->syncDataType = mSyncDataType; 1362 data->info.sync->syncSuid = mSyncSuid; 1363 } 1364 1365 if (data->info.data->sampleIndex > 0) { 1366 if (data->info.data->sensorType == SensorType::Unknown) { 1367 LOGE("Unhandled sensor data SUID 0x%016" PRIx64 " %016" PRIx64, 1368 data->info.suid.suid_high, data->info.suid.suid_low); 1369 } else if (!prepareSensorEvent(data->info)) { 1370 LOGE("Failed to prepare sensor event"); 1371 } 1372 } 1373 1374 if (!pb_decode(&streamCpy, sns_client_event_msg_fields, &data->event)) { 1375 LOG_NANOPB_ERROR(&streamCpy); 1376 } else if (synchronizedDecode && data->info.sync->syncIndFound) { 1377 mWaitingOnInd = false; 1378 mCond.notify_one(); 1379 } else { 1380 if (data->info.data->isHostWakeSuspendEvent) { 1381 mCbIf->onHostWakeSuspendEvent(data->info.data->isHostAwake); 1382 } 1383 if (!data->info.data->event.isNull()) { 1384 mCbIf->onSensorDataEvent( 1385 data->info.data->sensorType, std::move(data->info.data->event)); 1386 } 1387 if (!data->info.data->status.isNull()) { 1388 if (data->info.data->sensorType == SensorType::Unknown) { 1389 LOGE("Unhandled sensor status SUID 0x%016" PRIx64 " %016" PRIx64, 1390 data->info.suid.suid_high, data->info.suid.suid_low); 1391 } else { 1392 mCbIf->onSamplingStatusUpdate(std::move(data->info.data->status)); 1393 } 1394 } 1395 } 1396 1397 if (synchronizedDecode) { 1398 mMutex.unlock(); 1399 } 1400 } 1401 } 1402 } 1403 1404 void SeeHelper::handleSeeResp(uint32_t txnId, sns_std_error error) { 1405 LockGuard<Mutex> lock(mMutex); 1406 if (mWaitingOnResp && txnId == mCurrentTxnId) { 1407 mRespError = error; 1408 mWaitingOnResp = false; 1409 mCond.notify_one(); 1410 } 1411 } 1412 1413 bool SeeHelper::findSuidSync(const char *dataType, 1414 DynamicVector<sns_std_suid> *suids, 1415 uint8_t minNumSuids, uint32_t maxRetries, 1416 Milliseconds retryDelay) { 1417 CHRE_ASSERT(suids != nullptr); 1418 CHRE_ASSERT(minNumSuids > 0); 1419 1420 bool success = false; 1421 if (mSeeClients.empty()) { 1422 LOGE("Sensor client wasn't initialized"); 1423 } else { 1424 UniquePtr<pb_byte_t> msg; 1425 size_t msgLen; 1426 if (encodeSnsSuidReq(dataType, &msg, &msgLen)) { 1427 // Sensor client service may come up before SEE sensors are enumerated. A 1428 // max dwell time is set and retries are performed as currently there's no 1429 // message indicating that SEE intialization is complete. 1430 uint32_t trialCount = 0; 1431 do { 1432 suids->clear(); 1433 if (++trialCount > 1) { 1434 timer_sleep(retryDelay.getMilliseconds(), T_MSEC, 1435 true /* non_deferrable */); 1436 } 1437 1438 // Ignore failures from sendReq, we'll retry anyways (up to maxRetries) 1439 sendReq(sns_suid_sensor_init_default, 1440 suids, dataType, 1441 SNS_SUID_MSGID_SNS_SUID_REQ, msg.get(), msgLen, 1442 false /* batchValid */, 0 /* batchPeriodUs */, 1443 false /* passive */, true /* waitForIndication */); 1444 } while (suids->size() < minNumSuids && trialCount < maxRetries); 1445 1446 success = (suids->size() >= minNumSuids); 1447 if (!success) { 1448 mHaveTimedOutOnSuidLookup = true; 1449 } 1450 if (trialCount > 1) { 1451 LOGD("Waited %" PRIu32 " ms for %s (found: %d)", 1452 static_cast<uint32_t>(trialCount * retryDelay.getMilliseconds()), 1453 dataType, success); 1454 } 1455 } 1456 } 1457 1458 return success; 1459 } 1460 1461 bool SeeHelper::getAttributesSync(const sns_std_suid& suid, 1462 SeeAttributes *attr) { 1463 CHRE_ASSERT(attr); 1464 bool success = false; 1465 1466 if (mSeeClients.empty()) { 1467 LOGE("Sensor client wasn't initialized"); 1468 } else { 1469 UniquePtr<pb_byte_t> msg; 1470 size_t msgLen; 1471 success = encodeSnsStdAttrReq(&msg, &msgLen); 1472 1473 if (success) { 1474 success = sendReq(suid, attr, nullptr /* syncDataType */, 1475 SNS_STD_MSGID_SNS_STD_ATTR_REQ, msg.get(), msgLen, 1476 false /* batchValid */, 0 /* batchPeriodUs */, 1477 false /* passive */, true /* waitForIndication */); 1478 } 1479 } 1480 return success; 1481 } 1482 1483 bool SeeHelper::init(SeeHelperCallbackInterface *cbIf, Microseconds timeout) { 1484 CHRE_ASSERT(cbIf); 1485 1486 mCbIf = cbIf; 1487 sns_client *client; 1488 1489 // Initialize cal/remote_proc_state sensors before making sensor data request. 1490 return (waitForService(&client, timeout) 1491 && mSeeClients.push_back(client) 1492 && initCalSensors() 1493 && initRemoteProcSensor()); 1494 } 1495 1496 bool SeeHelper::makeRequest(const SeeSensorRequest& request) { 1497 bool success = false; 1498 1499 const SensorInfo *sensorInfo = getSensorInfo(request.sensorType); 1500 if (sensorInfo == nullptr) { 1501 LOGE("SensorType %" PRIu8 " hasn't been registered", 1502 static_cast<uint8_t>(request.sensorType)); 1503 } else { 1504 uint32_t msgId; 1505 UniquePtr<pb_byte_t> msg; 1506 size_t msgLen = 0; 1507 1508 if (!request.enable) { 1509 // An empty message 1510 msgId = SNS_CLIENT_MSGID_SNS_CLIENT_DISABLE_REQ; 1511 success = true; 1512 } else if (sensorTypeIsContinuous(request.sensorType)) { 1513 msgId = SNS_STD_SENSOR_MSGID_SNS_STD_SENSOR_CONFIG; 1514 success = encodeSnsStdSensorConfig(request, &msg, &msgLen); 1515 } else { 1516 msgId = SNS_STD_SENSOR_MSGID_SNS_STD_ON_CHANGE_CONFIG; 1517 // No sample rate needed to configure on-change or one-shot sensors. 1518 success = true; 1519 } 1520 1521 if (success) { 1522 success = sendReq(sensorInfo->client, sensorInfo->suid, 1523 nullptr /* syncData */, nullptr /* syncDataType */, 1524 msgId, msg.get(), msgLen, 1525 true /* batchValid */, request.batchPeriodUs, 1526 request.passive, false /* waitForIndication */); 1527 } 1528 } 1529 return success; 1530 } 1531 1532 const sns_std_suid& SeeHelper::getCalSuidFromSensorType( 1533 SensorType sensorType) const { 1534 static sns_std_suid suid = sns_suid_sensor_init_zero; 1535 1536 size_t calIndex = getCalIndexFromSensorType(sensorType); 1537 if (calIndex < kNumSeeCalSensors && mCalInfo[calIndex].suid.has_value()) { 1538 suid = mCalInfo[calIndex].suid.value(); 1539 } 1540 return suid; 1541 } 1542 1543 /** 1544 * Sends a request to SEE and waits for the response. 1545 */ 1546 bool SeeHelper::sendSeeReqSync( 1547 sns_client *client, sns_client_request_msg *req, Nanoseconds timeoutResp) { 1548 CHRE_ASSERT(client); 1549 CHRE_ASSERT(req); 1550 bool success = false; 1551 1552 auto *cbData = memoryAlloc<SeeRespCbData>(); 1553 if (cbData == nullptr) { 1554 LOG_OOM(); 1555 } else { 1556 cbData->seeHelper = this; 1557 1558 { 1559 LockGuard<Mutex> lock(mMutex); 1560 CHRE_ASSERT(!mWaitingOnResp); 1561 mWaitingOnResp = true; 1562 cbData->txnId = ++mCurrentTxnId; 1563 } 1564 1565 int status = mSnsClientApi->sns_client_send( 1566 client, req, SeeHelper::seeRespCb, cbData); 1567 if (status != 0) { 1568 LOGE("Error sending SEE request %d", status); 1569 memoryFree(cbData); 1570 } 1571 1572 { 1573 LockGuard<Mutex> lock(mMutex); 1574 1575 if (status == 0) { 1576 bool waitSuccess = true; 1577 1578 while (mWaitingOnResp && waitSuccess) { 1579 waitSuccess = mCond.wait_for(mMutex, timeoutResp); 1580 } 1581 1582 if (!waitSuccess) { 1583 LOGE("SEE resp timed out after %" PRIu64 " ms", 1584 Milliseconds(timeoutResp).getMilliseconds()); 1585 } else if (mRespError != SNS_STD_ERROR_NO_ERROR) { 1586 LOGE("SEE txn ID %" PRIu32 " failed with error %d", 1587 mCurrentTxnId, mRespError); 1588 } else { 1589 success = true; 1590 } 1591 } 1592 mWaitingOnResp = false; 1593 } 1594 } 1595 return success; 1596 } 1597 1598 bool SeeHelper::sendReq( 1599 sns_client *client, const sns_std_suid& suid, 1600 void *syncData, const char *syncDataType, 1601 uint32_t msgId, void *payload, size_t payloadLen, 1602 bool batchValid, uint32_t batchPeriodUs, bool passive, 1603 bool waitForIndication, Nanoseconds timeoutResp, Nanoseconds timeoutInd) { 1604 UniquePtr<sns_client_request_msg> msg; 1605 SeeBufArg data; 1606 bool success = false; 1607 1608 if (prepSnsClientReq(suid, msgId, payload, payloadLen, batchValid, 1609 batchPeriodUs, passive, &msg, &data)) { 1610 if (waitForIndication) { 1611 prepareWaitForInd(suid, syncData, syncDataType); 1612 } 1613 1614 success = sendSeeReqSync(client, msg.get(), timeoutResp); 1615 1616 if (waitForIndication) { 1617 success = waitForInd(success, timeoutInd); 1618 } 1619 } 1620 return success; 1621 } 1622 1623 void SeeHelper::prepareWaitForInd(const sns_std_suid& suid, void *syncData, 1624 const char *syncDataType) { 1625 LockGuard<Mutex> lock(mMutex); 1626 CHRE_ASSERT(!mWaitingOnInd); 1627 mWaitingOnInd = true; 1628 1629 // Specify members needed for a sync call. 1630 mSyncSuid = suid; 1631 mSyncData = syncData; 1632 mSyncDataType = syncDataType; 1633 } 1634 1635 bool SeeHelper::waitForInd(bool reqSent, Nanoseconds timeoutInd) { 1636 bool success = reqSent; 1637 1638 LockGuard<Mutex> lock(mMutex); 1639 CHRE_ASSERT(!mWaitingOnResp); 1640 if (reqSent) { 1641 bool waitSuccess = true; 1642 1643 while (mWaitingOnInd && waitSuccess) { 1644 waitSuccess = mCond.wait_for(mMutex, timeoutInd); 1645 } 1646 1647 if (!waitSuccess) { 1648 LOGE("SEE indication timed out after %" PRIu64 " ms", 1649 Milliseconds(timeoutInd).getMilliseconds()); 1650 success = false; 1651 } 1652 } 1653 mWaitingOnInd = false; 1654 1655 // Reset members needed for a sync call. 1656 mSyncSuid = sns_suid_sensor_init_zero; 1657 mSyncData = nullptr; 1658 mSyncDataType = nullptr; 1659 1660 return success; 1661 } 1662 1663 void SeeHelper::seeIndCb( 1664 sns_client *client, void *msg, uint32_t msgLen, void *cbData) { 1665 auto *obj = static_cast<SeeHelper *>(cbData); 1666 obj->handleSnsClientEventMsg(client, msg, msgLen); 1667 } 1668 1669 void SeeHelper::seeRespCb(sns_client *client, sns_std_error error, 1670 void *cbData) { 1671 auto *respCbData = static_cast<SeeRespCbData *>(cbData); 1672 respCbData->seeHelper->handleSeeResp(respCbData->txnId, error); 1673 memoryFree(cbData); 1674 } 1675 1676 bool SeeHelper::registerSensor( 1677 SensorType sensorType, const sns_std_suid& suid, bool *prevRegistered) { 1678 CHRE_ASSERT(sensorType != SensorType::Unknown); 1679 CHRE_ASSERT(prevRegistered != nullptr); 1680 bool success = false; 1681 1682 // Check whether the SUID/SensorType pair has been previously registered. 1683 // Also count how many other SensorTypes the SUID has been registered with. 1684 *prevRegistered = false; 1685 size_t suidRegCount = 0; 1686 for (const auto& sensorInfo : mSensorInfos) { 1687 if (suidsMatch(suid, sensorInfo.suid)) { 1688 suidRegCount++; 1689 if (sensorInfo.sensorType == sensorType) { 1690 *prevRegistered = true; 1691 } 1692 } 1693 } 1694 1695 // Initialize another SEE client if the SUID has been previously 1696 // registered with more SensorTypes than the number of SEE clients can 1697 // disambiguate. 1698 bool clientAvailable = true; 1699 if (mSeeClients.size() <= suidRegCount) { 1700 sns_client *client; 1701 clientAvailable = waitForService(&client); 1702 if (clientAvailable) { 1703 clientAvailable = mSeeClients.push_back(client); 1704 } 1705 } 1706 1707 // Add a new entry only if this SUID/SensorType pair hasn't been registered. 1708 if (!*prevRegistered && clientAvailable) { 1709 SensorInfo sensorInfo = { 1710 .suid = suid, 1711 .sensorType = sensorType, 1712 .client = mSeeClients[suidRegCount], 1713 }; 1714 success = mSensorInfos.push_back(sensorInfo); 1715 } 1716 return success; 1717 } 1718 1719 bool SeeHelper::sensorIsRegistered(SensorType sensorType) const { 1720 return (getSensorInfo(sensorType) != nullptr); 1721 } 1722 1723 bool SeeHelper::waitForService(sns_client **client, 1724 Microseconds timeout) { 1725 CHRE_ASSERT(client); 1726 1727 // TODO: add error_cb and error_cb_data. 1728 int status = mSnsClientApi->sns_client_init( 1729 client, timeout.getMilliseconds(), 1730 SeeHelper::seeIndCb, this /* ind_cb_data */, 1731 nullptr /* error_cb */, nullptr /* error_cb_data */); 1732 1733 bool success = (status == 0); 1734 if (!success) { 1735 LOGE("Failed to initialize the sensor client: %d", status); 1736 } 1737 return success; 1738 } 1739 1740 bool SeeHelper::initCalSensors() { 1741 bool success = true; 1742 1743 // Zero out mCalInfo to avoid accidental suid and data match. 1744 memset(mCalInfo, 0, sizeof(mCalInfo)); 1745 1746 const char *kCalTypes[] = { 1747 "accel_cal", 1748 "gyro_cal", 1749 "mag_cal", 1750 }; 1751 1752 // Find the cal sensor's SUID, assign it to mCalInfo, and make cal sensor data 1753 // request. 1754 DynamicVector<sns_std_suid> suids; 1755 for (size_t i = 0; i < ARRAY_SIZE(kCalTypes); i++) { 1756 const char *calType = kCalTypes[i]; 1757 if (!findSuidSync(calType, &suids)) { 1758 success = false; 1759 LOGE("Failed to find sensor '%s'", calType); 1760 } else { 1761 size_t index = getCalIndexFromDataType(calType); 1762 if (index >= kNumSeeCalSensors) { 1763 success = false; 1764 LOGE("Cal sensor '%s' index out of bounds", calType); 1765 } else { 1766 mCalInfo[index].suid = suids[0]; 1767 1768 if (!sendReq(suids[0], nullptr /* syncData */, 1769 nullptr /* syncDataType */, 1770 SNS_STD_SENSOR_MSGID_SNS_STD_ON_CHANGE_CONFIG, 1771 nullptr /* msg */, 0 /* msgLen */, 1772 false /* batchValid */, 0 /* batchPeriodUs */, 1773 false /* passive */, false /* waitForIndication */)) { 1774 success = false; 1775 LOGE("Failed to request '%s' data", calType); 1776 } 1777 } 1778 } 1779 } 1780 1781 return success; 1782 } 1783 1784 bool SeeHelper::initRemoteProcSensor() { 1785 bool success = false; 1786 1787 const char *kRemoteProcType = "remote_proc_state"; 1788 DynamicVector<sns_std_suid> suids; 1789 if (!findSuidSync(kRemoteProcType, &suids)) { 1790 LOGE("Failed to find sensor '%s'", kRemoteProcType); 1791 } else { 1792 mRemoteProcSuid = suids[0]; 1793 1794 uint32_t msgId = SNS_REMOTE_PROC_STATE_MSGID_SNS_REMOTE_PROC_STATE_CONFIG; 1795 constexpr size_t kBufferSize = sns_remote_proc_state_config_size; 1796 pb_byte_t msgBuffer[kBufferSize]; 1797 size_t msgLen; 1798 if (encodeSnsRemoteProcSensorConfig(msgBuffer, kBufferSize, &msgLen, 1799 SNS_STD_CLIENT_PROCESSOR_APSS)) { 1800 success = sendReq(mRemoteProcSuid.value(), 1801 nullptr /* syncData */, nullptr /* syncDataType */, 1802 msgId, msgBuffer, msgLen, 1803 false /* batchValid */, 0 /* batchPeriodUs */, 1804 false /* passive */, false /* waitForIndication */); 1805 if (!success) { 1806 LOGE("Failed to request '%s' config", kRemoteProcType); 1807 } 1808 } 1809 } 1810 1811 return success; 1812 } 1813 1814 SeeCalData *SeeHelper::getCalDataFromSuid(const sns_std_suid& suid) { 1815 size_t calIndex = getCalIndexFromSuid(suid, mCalInfo); 1816 return (calIndex < kNumSeeCalSensors) ? &mCalInfo[calIndex].cal : nullptr; 1817 } 1818 1819 const SeeHelper::SensorInfo *SeeHelper::getSensorInfo( 1820 SensorType sensorType) const { 1821 for (const auto& sensorInfo : mSensorInfos) { 1822 if (sensorInfo.sensorType == sensorType) { 1823 return &sensorInfo; 1824 } 1825 } 1826 return nullptr; 1827 } 1828 1829 } // namespace chre 1830