114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi/* 214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Copyright (C) 2016 The Android Open Source Project 314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Licensed under the Apache License, Version 2.0 (the "License"); 514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * you may not use this file except in compliance with the License. 614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * You may obtain a copy of the License at 714986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 814986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * http://www.apache.org/licenses/LICENSE-2.0 914986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 1014986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Unless required by applicable law or agreed to in writing, software 1114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * distributed under the License is distributed on an "AS IS" BASIS, 1214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * See the License for the specific language governing permissions and 1414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * limitations under the License. 1514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi */ 1614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi 1777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <inttypes.h> 1877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <stdint.h> 1977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <sys/endian.h> 207062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#include <string.h> 21d3434ca8f337e220270179191af396678566f5f0Ben Fennema#include <alloca.h> 2277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 23f805306b53d82eef67d8891a5dd5c32d3794a3abAlexey Polyudov#include <variant/variant.h> 24c36763df4fb478679f842e9dd9bb7a1ae314a60aAlexey Polyudov#include <eventnums.h> 25c36763df4fb478679f842e9dd9bb7a1ae314a60aAlexey Polyudov 26f805306b53d82eef67d8891a5dd5c32d3794a3abAlexey Polyudov#include <plat/pwr.h> 279a3f18fe94753868ecede6b1dc509568084f2808Ben Fennema 28b62d65a623f9c09a5936c69dacae829e0cc9a2baAlexey Polyudov#include <nanohub/crc.h> 29b62d65a623f9c09a5936c69dacae829e0cc9a2baAlexey Polyudov 309a3f18fe94753868ecede6b1dc509568084f2808Ben Fennema#include <platform.h> 319c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema#include <cpu.h> 32774ea97d18117e0d13108a0968315f23102cbc40Peng Xu#include <halIntf.h> 3377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <hostIntf.h> 3477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <hostIntf_priv.h> 356cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema#include <nanohubCommand.h> 3677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <nanohubPacket.h> 3777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <seos.h> 3877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann#include <util.h> 3967edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema#include <atomicBitset.h> 4067edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema#include <atomic.h> 4167edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema#include <gpio.h> 4267edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema#include <apInt.h> 437062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#include <sensors.h> 447062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#include <timer.h> 45f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema#include <heap.h> 46f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema#include <simpleQ.h> 47f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 4893d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define HOSTINTF_MAX_ERR_MSG 8 4993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define MAX_NUM_BLOCKS 280 /* times 256 = 71680 bytes */ 5093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define MIN_NUM_BLOCKS 10 /* times 256 = 2560 bytes */ 5193d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define SENSOR_INIT_DELAY 500000000 /* ns */ 52fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema#define SENSOR_INIT_ERROR_MAX 4 5393d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define CHECK_LATENCY_TIME 500000000 /* ns */ 5493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian#define EVT_LATENCY_TIMER EVT_NO_FIRST_USER_EVENT 5577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 565f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic const uint32_t delta_time_multiplier_order = 9; 575f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic const uint32_t delta_time_coarse_mask = ~1; 585f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic const uint32_t delta_time_fine_mask = 1; 595f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic const uint32_t delta_time_rounding = 0x200; /* 1ul << delta_time_multiplier_order */ 605f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic const uint64_t delta_time_max = 0x1FFFFFFFE00; /* UINT32_MAX << delta_time_multiplier_order */ 615f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian 624d17cb52d252591db12cee09b08eed4ee173b939Ben Fennemaenum ConfigCmds 634d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema{ 644d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema CONFIG_CMD_DISABLE = 0, 654d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema CONFIG_CMD_ENABLE = 1, 664d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema CONFIG_CMD_FLUSH = 2, 674d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema CONFIG_CMD_CFG_DATA = 3, 684d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema CONFIG_CMD_CALIBRATE = 4, 694c8f35e88775d6adef1b79505b8353938996935eAndrew Rossignol CONFIG_CMD_SELF_TEST = 5, 704d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema}; 714d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 727062239aaa63d8a5260d7d325483755acbb8c35cBen Fennemastruct ConfigCmd 737062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema{ 747062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint64_t latency; 757062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint32_t rate; 76f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t sensType; 774d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema uint8_t cmd; 784d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema uint16_t flags; 79f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} __attribute__((packed)); 80f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 817062239aaa63d8a5260d7d325483755acbb8c35cBen Fennemastruct ActiveSensor 827062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema{ 837062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint64_t latency; 847c00b1c182d841a5f82bfca9381025873e77084dBen Fennema uint64_t firstTime; 85f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint64_t lastTime; 86e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema struct HostIntfDataBuffer buffer; 877062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint32_t rate; 88a02d34c3c4e91dc99d123a40d52f05dda4fc3296Ben Fennema uint32_t sensorHandle; 89d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema float rawScale; 90f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint16_t minSamples; 91f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint16_t curSamples; 927062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint8_t numAxis; 937062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema uint8_t interrupt; 94f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t numSamples; 95f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t packetSamples; 96d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // The sensorType used to report bias samples; normally the same as 97d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // buffer.sensorType, but in the case of raw, this gets set to the base 98d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // sensorType matching struct SensorInfo (because the sensor can have a 99d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // different rawType). Note that this is different than biasType in struct 100d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // SensorInfo. 101d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema uint8_t biasReportType; 102a02d34c3c4e91dc99d123a40d52f05dda4fc3296Ben Fennema uint8_t oneshot : 1; 103f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t discard : 1; 104f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema uint8_t raw : 1; 105f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema uint8_t reserved : 5; 1067062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema} __attribute__((packed)); 1077062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 1087062239aaa63d8a5260d7d325483755acbb8c35cBen Fennemastatic uint8_t mSensorList[SENS_TYPE_LAST_USER]; 109f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic struct SimpleQueue *mOutputQ; 110f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic struct ActiveSensor *mActiveSensorTable; 111f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic uint8_t mNumSensors; 112f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic uint8_t mLastSensor; 11377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 114be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic const struct HostIntfComm *mComm; 1154e48865ca752192f7614223d48c641dbdbe16389Ben Fennemastatic bool mBusy; 116be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic uint64_t mRxTimestamp; 117be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic uint8_t mRxBuf[NANOHUB_PACKET_SIZE_MAX]; 118be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic size_t mRxSize; 11971484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemastatic struct 12071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema{ 12171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema const struct NanohubCommand *cmd; 12271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema uint32_t seq; 12371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema bool seqMatch; 12471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema} mTxRetrans; 12505aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennemastatic struct 12605aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema{ 127f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t pad; // packet header is 10 bytes. + 2 to word align 12805aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema uint8_t prePreamble; 12905aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema uint8_t buf[NANOHUB_PACKET_SIZE_MAX]; 13005aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema uint8_t postPreamble; 131be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema} mTxBuf; 132e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennemastatic struct 133e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema{ 134e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema uint8_t pad; // packet header is 10 bytes. + 2 to word align 135e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema uint8_t prePreamble; 136e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema uint8_t buf[NANOHUB_PACKET_SIZE_MIN]; 137e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema uint8_t postPreamble; 138e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema} mTxNakBuf; 139be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic size_t mTxSize; 140be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic uint8_t *mTxBufPtr; 141be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic const struct NanohubCommand *mRxCmd; 1429c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben FennemaATOMIC_BITSET_DECL(mInterrupt, HOSTINTF_MAX_INTERRUPTS, static); 1439c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben FennemaATOMIC_BITSET_DECL(mInterruptMask, HOSTINTF_MAX_INTERRUPTS, static); 1449c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemastatic uint32_t mInterruptCntWkup, mInterruptCntNonWkup; 1453b29ae466154d7953425796516470fe5d2f79fd3Ben Fennemastatic uint32_t mWakeupBlocks, mNonWakeupBlocks, mTotalBlocks; 146be5a075a4c483bfa6704a16faecb001e74973065Ben Fennemastatic uint32_t mHostIntfTid; 1473b29ae466154d7953425796516470fe5d2f79fd3Ben Fennemastatic uint32_t mLatencyTimer; 1483b29ae466154d7953425796516470fe5d2f79fd3Ben Fennemastatic uint8_t mLatencyCnt; 14977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 15071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemastatic uint8_t mRxIdle; 15171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemastatic uint8_t mWakeActive; 15271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemastatic uint8_t mActiveWrite; 15371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemastatic uint8_t mRestartRx; 15493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstatic uint8_t mIntErrMsgIdx; 15593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstatic volatile uint32_t mIntErrMsgCnt; 15693d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian 15793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianenum hostIntfIntErrReason 15893d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian{ 15993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_PKG_INCOMPELETE = 0, 16093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_PGK_SIZE, 16193d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_PKG_PAYLOAD_SIZE, 16293d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_PKG_CRC, 16393d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_RECEIVE, 16493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_SEND, 16593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_ACK, 166e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema HOSTINTF_ERR_NAK, 16793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian HOSTINTF_ERR_UNKNOWN 16893d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian}; 16993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian 17093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstruct hostIntfIntErrMsg 17193d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian{ 17293d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian enum LogLevel level; 17393d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian enum hostIntfIntErrReason reason; 17493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian const char *func; 17593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian}; 17693d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstatic struct hostIntfIntErrMsg mIntErrMsg[HOSTINTF_MAX_ERR_MSG]; 17771484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema 1784e48865ca752192f7614223d48c641dbdbe16389Ben Fennemastatic void hostIntfTxPacket(uint32_t reason, uint8_t len, uint32_t seq, 1793ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmann HostIntfCommCallbackF callback); 18077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 1813ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfRxDone(size_t rx, int err); 18277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic void hostIntfGenerateAck(void *cookie); 18377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 1843ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfTxAckDone(size_t tx, int err); 18577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic void hostIntfGenerateResponse(void *cookie); 18677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 1873ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfTxPayloadDone(size_t tx, int err); 18877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 18977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic inline void *hostIntfGetPayload(uint8_t *buf) 19077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 19177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann struct NanohubPacket *packet = (struct NanohubPacket *)buf; 19277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return packet->data; 19377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 19477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 1956cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennemastatic inline uint8_t hostIntfGetPayloadLen(uint8_t *buf) 1966cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema{ 1976cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema struct NanohubPacket *packet = (struct NanohubPacket *)buf; 1986cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema return packet->len; 1996cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema} 2006cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema 20177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic inline struct NanohubPacketFooter *hostIntfGetFooter(uint8_t *buf) 20277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 20377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann struct NanohubPacket *packet = (struct NanohubPacket *)buf; 20477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return (struct NanohubPacketFooter *)(buf + sizeof(*packet) + packet->len); 20577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 20677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 20777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic inline __le32 hostIntfComputeCrc(uint8_t *buf) 20877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 20977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann struct NanohubPacket *packet = (struct NanohubPacket *)buf; 2104e48865ca752192f7614223d48c641dbdbe16389Ben Fennema uint32_t crc = crc32(packet, packet->len + sizeof(*packet), CRC_INIT); 21177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return htole32(crc); 21277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 21377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 21493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstatic void hostIntfPrintErrMsg(void *cookie) 21593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian{ 21693d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian struct hostIntfIntErrMsg *msg = (struct hostIntfIntErrMsg *)cookie; 21793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian osLog(msg->level, "%s failed with: %d\n", msg->func, msg->reason); 2188cf410ff3b2d8a09f3aa2b5ca10253732e8e6f78Alexey Polyudov atomicAdd32bits(&mIntErrMsgCnt, -1UL); 21993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian} 22093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian 22193d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qianstatic void hostIntfDeferErrLog(enum LogLevel level, enum hostIntfIntErrReason reason, const char *func) 22293d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian{ 22393d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian // If the message buffer is full, we drop the newer messages. 22493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian if (atomicRead32bits(&mIntErrMsgCnt) == HOSTINTF_MAX_ERR_MSG) 22593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian return; 22693d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian 22793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian mIntErrMsg[mIntErrMsgIdx].level = level; 22893d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian mIntErrMsg[mIntErrMsgIdx].reason = reason; 22993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian mIntErrMsg[mIntErrMsgIdx].func = func; 23093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian if (osDefer(hostIntfPrintErrMsg, &mIntErrMsg[mIntErrMsgIdx], false)) { 2318cf410ff3b2d8a09f3aa2b5ca10253732e8e6f78Alexey Polyudov atomicAdd32bits(&mIntErrMsgCnt, 1UL); 23293d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian mIntErrMsgIdx = (mIntErrMsgIdx + 1) % HOSTINTF_MAX_ERR_MSG; 23393d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian } 23493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian} 23593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian 2364e48865ca752192f7614223d48c641dbdbe16389Ben Fennemastatic inline const struct NanohubCommand *hostIntfFindHandler(uint8_t *buf, size_t size, uint32_t *seq) 23777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 23877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann struct NanohubPacket *packet = (struct NanohubPacket *)buf; 23977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann struct NanohubPacketFooter *footer; 24077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann __le32 packetCrc; 24177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann uint32_t packetReason; 2426cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema const struct NanohubCommand *cmd; 24377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 24477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (size < NANOHUB_PACKET_SIZE(0)) { 24593d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_INCOMPELETE, __func__); 24677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return NULL; 24777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 24877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 24977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (size != NANOHUB_PACKET_SIZE(packet->len)) { 25093d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PGK_SIZE, __func__); 25177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return NULL; 25277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 25377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 25477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann footer = hostIntfGetFooter(buf); 25577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann packetCrc = hostIntfComputeCrc(buf); 25677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (footer->crc != packetCrc) { 25793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_CRC, __func__); 25877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return NULL; 25977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 26077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 26171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (mTxRetrans.seq == packet->seq) { 26271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mTxRetrans.seqMatch = true; 26371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema return mTxRetrans.cmd; 26405aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema } else { 26571484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mTxRetrans.seqMatch = false; 26605aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema } 26705aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema 2684e48865ca752192f7614223d48c641dbdbe16389Ben Fennema *seq = packet->seq; 2694e48865ca752192f7614223d48c641dbdbe16389Ben Fennema 2704e48865ca752192f7614223d48c641dbdbe16389Ben Fennema if (mBusy) 2714e48865ca752192f7614223d48c641dbdbe16389Ben Fennema return NULL; 2724e48865ca752192f7614223d48c641dbdbe16389Ben Fennema 27377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann packetReason = le32toh(packet->reason); 2746cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema 2756cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema if ((cmd = nanohubFindCommand(packetReason)) != NULL) { 2766cccf8d2b00fe70ac6843b1a66f2ff0208e954b6Ben Fennema if (packet->len < cmd->minDataLen || packet->len > cmd->maxDataLen) { 27793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_PAYLOAD_SIZE, __func__); 27877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return NULL; 27977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 28077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 28177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return cmd; 28277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 28377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 28493d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_UNKNOWN, __func__); 28577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return NULL; 28677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 28777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 28805aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennemastatic void hostIntfTxBuf(int size, uint8_t *buf, HostIntfCommCallbackF callback) 28905aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema{ 290be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxSize = size; 291be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxBufPtr = buf; 292be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mComm->txPacket(mTxBufPtr, mTxSize, callback); 29305aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema} 29405aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema 2954e48865ca752192f7614223d48c641dbdbe16389Ben Fennemastatic void hostIntfTxPacket(__le32 reason, uint8_t len, uint32_t seq, 2963ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmann HostIntfCommCallbackF callback) 29777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 298be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxBuf.buf); 29977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann txPacket->reason = reason; 3004e48865ca752192f7614223d48c641dbdbe16389Ben Fennema txPacket->seq = seq; 30177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann txPacket->sync = NANOHUB_SYNC_BYTE; 30277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann txPacket->len = len; 30377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 304be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxBuf.buf); 305be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema txFooter->crc = hostIntfComputeCrc(mTxBuf.buf); 30677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 30705aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema // send starting with the prePremable byte 308be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema hostIntfTxBuf(1+NANOHUB_PACKET_SIZE(len), &mTxBuf.prePreamble, callback); 30975bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann} 31075bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann 311e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennemastatic void hostIntfTxNakPacket(__le32 reason, uint32_t seq, 312e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema HostIntfCommCallbackF callback) 313e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema{ 314e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxNakBuf.buf); 315e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema txPacket->reason = reason; 316e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema txPacket->seq = seq; 317e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema txPacket->sync = NANOHUB_SYNC_BYTE; 318e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema txPacket->len = 0; 319e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema 320e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxNakBuf.buf); 321e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema txFooter->crc = hostIntfComputeCrc(mTxNakBuf.buf); 322e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema 323e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema // send starting with the prePremable byte 324e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxBuf(1+NANOHUB_PACKET_SIZE_MIN, &mTxNakBuf.prePreamble, callback); 325e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema} 326e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema 3279c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemastatic inline bool hostIntfTxPacketDone(int err, size_t tx, 3283ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmann HostIntfCommCallbackF callback) 32975bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann{ 330be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema if (!err && tx < mTxSize) { 331be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxSize -= tx; 332be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxBufPtr += tx; 33375bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann 334be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mComm->txPacket(mTxBufPtr, mTxSize, callback); 3359c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema return false; 33675bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann } 3379c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema 3389c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema return true; 33977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 34077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 3417062239aaa63d8a5260d7d325483755acbb8c35cBen Fennemastatic bool hostIntfRequest(uint32_t tid) 34277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 343be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mHostIntfTid = tid; 3449c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema atomicBitsetInit(mInterrupt, HOSTINTF_MAX_INTERRUPTS); 3459c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema atomicBitsetInit(mInterruptMask, HOSTINTF_MAX_INTERRUPTS); 346650f09fc5dcbc46c0e398f78a361683bec1542f3Ben Fennema#ifdef AP_INT_NONWAKEUP 3472e781101b1a6b63de27bc9af304ce76e0ddddd3aBen Fennema hostIntfSetInterruptMask(NANOHUB_INT_NONWAKEUP); 348650f09fc5dcbc46c0e398f78a361683bec1542f3Ben Fennema#endif 349be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxBuf.prePreamble = NANOHUB_PREAMBLE_BYTE; 350be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mTxBuf.postPreamble = NANOHUB_PREAMBLE_BYTE; 351e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxNakBuf.prePreamble = NANOHUB_PREAMBLE_BYTE; 352e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxNakBuf.postPreamble = NANOHUB_PREAMBLE_BYTE; 3539d0aa0e9e154f4591705316976821c24b4d336b6Ben Fennema 354be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mComm = platHostIntfInit(); 355be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema if (mComm) { 356be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema int err = mComm->request(); 3577062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema if (!err) { 35885dd97b39820717be3cb088795bc4ca911ef5609Ben Fennema nanohubInitCommand(); 35971484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone); 360be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema osEventSubscribe(mHostIntfTid, EVT_APP_START); 3617062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema return true; 3627062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 3633ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmann } 3647062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 3657062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema return false; 36677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 36777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 36871484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennemavoid hostIntfRxPacket(bool wakeupActive) 36971484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema{ 37071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (mWakeActive) { 37171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (atomicXchgByte(&mRxIdle, false)) { 37271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (!wakeupActive) 37371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE); 37471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone); 37571484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (wakeupActive) 37671484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE); 37771484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else if (atomicReadByte(&mActiveWrite)) { 37871484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema atomicWriteByte(&mRestartRx, true); 37971484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else { 38071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (!wakeupActive) 38171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE); 38271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema else 38371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE); 38471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } 38571484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else if (wakeupActive && !atomicReadByte(&mActiveWrite)) 38671484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE); 38771484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema 38871484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mWakeActive = wakeupActive; 3896e7c2b346eba1e06da140ae2248b57fd67bc4d0dGreg Hackmann} 3906e7c2b346eba1e06da140ae2248b57fd67bc4d0dGreg Hackmann 3913ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfRxDone(size_t rx, int err) 39277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 393ff2d397716fa332b2771e9719101e52970c7c657Ben Fennema mRxTimestamp = sensorGetTime(); 394be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mRxSize = rx; 39577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 39677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (err != 0) { 39793d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_RECEIVE, __func__); 39877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return; 39977bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 40077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 401f22f291bd2bd29fd6c68f0a1208c196ab57c6be7Ben Fennema hostIntfGenerateAck(NULL); 40277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 40377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 404ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennemastatic void hostIntfTxSendAck(uint32_t resp) 405ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema{ 406ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema void *txPayload = hostIntfGetPayload(mTxBuf.buf); 407ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 408ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema if (resp == NANOHUB_FAST_UNHANDLED_ACK) { 409ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema hostIntfCopyInterrupts(txPayload, HOSTINTF_MAX_INTERRUPTS); 410ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema hostIntfTxPacket(NANOHUB_REASON_ACK, 32, mTxRetrans.seq, hostIntfTxAckDone); 411ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema } else if (resp == NANOHUB_FAST_DONT_ACK) { 412ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema // do nothing. something else will do the ack 413ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema } else { 414ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema hostIntfTxPacket(mRxCmd->reason, resp, mTxRetrans.seq, hostIntfTxPayloadDone); 415ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema } 416ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema} 417ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 418ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennemavoid hostIntfTxAck(void *buffer, uint8_t len) 419ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema{ 420ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema void *txPayload = hostIntfGetPayload(mTxBuf.buf); 421ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 422ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema memcpy(txPayload, buffer, len); 423ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 424ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema hostIntfTxSendAck(len); 425ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema} 426ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 42777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic void hostIntfGenerateAck(void *cookie) 42877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 4294e48865ca752192f7614223d48c641dbdbe16389Ben Fennema uint32_t seq = 0; 4309c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema void *txPayload = hostIntfGetPayload(mTxBuf.buf); 431ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema void *rxPayload = hostIntfGetPayload(mRxBuf); 432ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf); 433ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema uint32_t resp = NANOHUB_FAST_UNHANDLED_ACK; 4344e48865ca752192f7614223d48c641dbdbe16389Ben Fennema 43571484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema atomicWriteByte(&mActiveWrite, true); 43671484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE); 4374e48865ca752192f7614223d48c641dbdbe16389Ben Fennema mRxCmd = hostIntfFindHandler(mRxBuf, mRxSize, &seq); 43877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 439be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema if (mRxCmd) { 44071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (mTxRetrans.seqMatch) { 441be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema hostIntfTxBuf(mTxSize, &mTxBuf.prePreamble, hostIntfTxPayloadDone); 4424e48865ca752192f7614223d48c641dbdbe16389Ben Fennema } else { 44371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mTxRetrans.seq = seq; 44471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mTxRetrans.cmd = mRxCmd; 445ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema if (mRxCmd->fastHandler) 446ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema resp = mRxCmd->fastHandler(rxPayload, rx_len, txPayload, mRxTimestamp); 447ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 448ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema hostIntfTxSendAck(resp); 4494e48865ca752192f7614223d48c641dbdbe16389Ben Fennema } 45005aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema } else { 4514e48865ca752192f7614223d48c641dbdbe16389Ben Fennema if (mBusy) 452e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxNakPacket(NANOHUB_REASON_NAK_BUSY, seq, hostIntfTxAckDone); 4534e48865ca752192f7614223d48c641dbdbe16389Ben Fennema else 454e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxNakPacket(NANOHUB_REASON_NAK, seq, hostIntfTxAckDone); 45505aa54afac1c3ae205dfed5b8518207b52457c7eBen Fennema } 45677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 45777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 458ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 459e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennemastatic void hostIntfTxComplete(bool clearInt, bool restartRx) 46071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema{ 461e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema if (restartRx || clearInt || !mWakeActive) 462e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE); 46371484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema atomicWriteByte(&mActiveWrite, false); 46471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema atomicWriteByte(&mRestartRx, false); 465e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema if (restartRx) { 46671484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone); 46771484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE); 46871484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else { 46971484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema atomicWriteByte(&mRxIdle, true); 47071484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } 47171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema} 47271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema 4733ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfTxAckDone(size_t tx, int err) 47477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 47575bbd0065b00091f6114025dead36032cbf707eaGreg Hackmann hostIntfTxPacketDone(err, tx, hostIntfTxAckDone); 47677bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 47777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (err) { 47893d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_ACK, __func__); 479e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(false, false); 48077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return; 48177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 48271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema 483be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema if (!mRxCmd) { 4844e48865ca752192f7614223d48c641dbdbe16389Ben Fennema if (!mBusy) 485e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfDeferErrLog(LOG_DEBUG, HOSTINTF_ERR_NAK, __func__); 48671484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (atomicReadByte(&mRestartRx)) 487e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(false, true); 48871484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema else 489e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(false, false); 49077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann return; 49171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else if (atomicReadByte(&mRestartRx)) { 492e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxRetrans.seq = 0; 493e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxRetrans.cmd = NULL; 494e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(false, true); 49571484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema } else { 496e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema if (!osDefer(hostIntfGenerateResponse, NULL, true)) { 497e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxRetrans.seq = 0; 498e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema mTxRetrans.cmd = NULL; 499e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(false, false); 500e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema } 50177bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann } 50277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 50377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 50477bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmannstatic void hostIntfGenerateResponse(void *cookie) 50577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 506be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema void *rxPayload = hostIntfGetPayload(mRxBuf); 507be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf); 508be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema void *txPayload = hostIntfGetPayload(mTxBuf.buf); 509be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema uint8_t respLen = mRxCmd->handler(rxPayload, rx_len, txPayload, mRxTimestamp); 51077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 51171484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema hostIntfTxPacket(mRxCmd->reason, respLen, mTxRetrans.seq, hostIntfTxPayloadDone); 51277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 51377bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 5143ac137884ddf797216309dfb58730c8a6ae8636aGreg Hackmannstatic void hostIntfTxPayloadDone(size_t tx, int err) 51577bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 5169c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema bool done = hostIntfTxPacketDone(err, tx, hostIntfTxPayloadDone); 51777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 51877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann if (err) 51993d65987705a8ab827e8db39ad3f846d40db4ad5Zhengyin Qian hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_SEND, __func__); 5206e7c2b346eba1e06da140ae2248b57fd67bc4d0dGreg Hackmann 5219c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema if (done) { 52271484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema if (atomicReadByte(&mRestartRx)) 523e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(true, true); 52471484039e4e4f2eefee733bd2869ef3bb00847a6Ben Fennema else 525e17de2fcb80b0b8bb6370feff58b0e2c3cfa1389Ben Fennema hostIntfTxComplete(true, false); 5269c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 52777bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 52877bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann 5297062239aaa63d8a5260d7d325483755acbb8c35cBen Fennemastatic void hostIntfRelease() 53077bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann{ 531be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema mComm->release(); 53277bdb9a9ce2aace386595e6f2ea12bcbee759473Greg Hackmann} 53367edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 534f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic void resetBuffer(struct ActiveSensor *sensor) 5357062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema{ 536f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->discard = true; 537f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length = 0; 538f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema memset(&sensor->buffer.firstSample, 0x00, sizeof(struct SensorFirstSample)); 5397062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema} 5407062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 5414e48865ca752192f7614223d48c641dbdbe16389Ben Fennemavoid hostIntfSetBusy(bool busy) 5424e48865ca752192f7614223d48c641dbdbe16389Ben Fennema{ 5434e48865ca752192f7614223d48c641dbdbe16389Ben Fennema mBusy = busy; 5444e48865ca752192f7614223d48c641dbdbe16389Ben Fennema} 5454e48865ca752192f7614223d48c641dbdbe16389Ben Fennema 5467467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudovstatic inline struct ActiveSensor *getActiveSensorByType(uint32_t sensorType) 5477467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov{ 5487467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov struct ActiveSensor *sensor = NULL; 5497467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 5507467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensorType > SENS_TYPE_INVALID && sensorType <= SENS_TYPE_LAST_USER && 5517467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov mSensorList[sensorType - 1] < MAX_REGISTERED_SENSORS) 5527467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov sensor = mActiveSensorTable + mSensorList[sensorType - 1]; 5537467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 5547467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov return sensor; 5557467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov} 5567467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 5573b29ae466154d7953425796516470fe5d2f79fd3Ben Fennemabool hostIntfPacketDequeue(void *data, uint32_t *wakeup, uint32_t *nonwakeup) 5587062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema{ 559e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema struct HostIntfDataBuffer *buffer = data; 5603b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema bool ret; 561f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema struct ActiveSensor *sensor; 5622166f5d1d68a304348e807701eebba774041c5d5Ben Fennema uint32_t i; 5633b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema 5642166f5d1d68a304348e807701eebba774041c5d5Ben Fennema ret = simpleQueueDequeue(mOutputQ, buffer); 5653b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema while (ret) { 5667467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov sensor = getActiveSensorByType(buffer->sensType); 5677467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor) { 5687467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov // do not sent sensor data if sensor is not requested; only maintain stats 5693b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->sensorHandle == 0 && !buffer->firstSample.biasPresent && !buffer->firstSample.numFlushes) { 5703b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 5713b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks--; 5723b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 5733b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks--; 5743b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->curSamples -= buffer->firstSample.numSamples; 5752166f5d1d68a304348e807701eebba774041c5d5Ben Fennema ret = simpleQueueDequeue(mOutputQ, buffer); 5763b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } else { 5773b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema break; 5783b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } 57950a59c96d1807472a657773d82cbc7b82fbb186dBen Fennema } else { 58050a59c96d1807472a657773d82cbc7b82fbb186dBen Fennema break; 5813b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } 5823b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } 5833b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema 5843b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (!ret) { 585f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema // nothing in queue. look for partial buffers to flush 586f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema for (i = 0; i < mNumSensors; i++, mLastSensor = (mLastSensor + 1) % mNumSensors) { 5872166f5d1d68a304348e807701eebba774041c5d5Ben Fennema sensor = mActiveSensorTable + mLastSensor; 5882166f5d1d68a304348e807701eebba774041c5d5Ben Fennema 5892166f5d1d68a304348e807701eebba774041c5d5Ben Fennema if (sensor->curSamples != sensor->buffer.firstSample.numSamples) { 5902166f5d1d68a304348e807701eebba774041c5d5Ben Fennema osLog(LOG_ERROR, "hostIntfPacketDequeue: sensor(%d)->curSamples=%d != buffer->numSamples=%d\n", sensor->buffer.sensType, sensor->curSamples, sensor->buffer.firstSample.numSamples); 5912166f5d1d68a304348e807701eebba774041c5d5Ben Fennema sensor->curSamples = sensor->buffer.firstSample.numSamples; 5922166f5d1d68a304348e807701eebba774041c5d5Ben Fennema } 5932166f5d1d68a304348e807701eebba774041c5d5Ben Fennema 5942166f5d1d68a304348e807701eebba774041c5d5Ben Fennema if (sensor->buffer.length > 0) { 5952166f5d1d68a304348e807701eebba774041c5d5Ben Fennema memcpy(buffer, &sensor->buffer, sizeof(struct HostIntfDataBuffer)); 5962166f5d1d68a304348e807701eebba774041c5d5Ben Fennema resetBuffer(sensor); 597f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema ret = true; 5985d70aa7fd23e470f81bf488b048f16ce3ebe363bBen Fennema mLastSensor = (mLastSensor + 1) % mNumSensors; 599f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema break; 600f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 601f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 602f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 6037062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 604f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (ret) { 6057467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov sensor = getActiveSensorByType(buffer->sensType); 6067467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor) { 6073b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 6083b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks--; 6093b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 6103b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks--; 6115d70aa7fd23e470f81bf488b048f16ce3ebe363bBen Fennema sensor->curSamples -= buffer->firstSample.numSamples; 6127c00b1c182d841a5f82bfca9381025873e77084dBen Fennema sensor->firstTime = 0ull; 6133b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } else { 6143b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (buffer->interrupt == NANOHUB_INT_WAKEUP) 6153b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks--; 6163b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP) 6173b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks--; 6185d70aa7fd23e470f81bf488b048f16ce3ebe363bBen Fennema } 619f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 6207062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 6213b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema *wakeup = mWakeupBlocks; 6223b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema *nonwakeup = mNonWakeupBlocks; 6233b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema 6247062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema return ret; 6257062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema} 6267062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 627f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic void initCompleteCallback(uint32_t timerId, void *data) 628f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema{ 629be5a075a4c483bfa6704a16faecb001e74973065Ben Fennema osEnqueuePrivateEvt(EVT_APP_START, NULL, NULL, mHostIntfTid); 630f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} 631f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 632f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic bool queueDiscard(void *data, bool onDelete) 633f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema{ 634e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema struct HostIntfDataBuffer *buffer = data; 6357467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov struct ActiveSensor *sensor = getActiveSensorByType(buffer->sensType); 636f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 6377467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor) { 638f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (sensor->curSamples - buffer->firstSample.numSamples >= sensor->minSamples || onDelete) { 6393b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 6403b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks--; 6413b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 6423b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks--; 643f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->curSamples -= buffer->firstSample.numSamples; 644f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 645f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema return true; 646f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 647f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema return false; 648f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 649f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 6503b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (buffer->interrupt == NANOHUB_INT_WAKEUP) 6513b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks--; 6523b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP) 6533b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks--; 654f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema return true; 655f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 656f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} 657f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 6583b29ae466154d7953425796516470fe5d2f79fd3Ben Fennemastatic void latencyTimerCallback(uint32_t timerId, void* data) 6593b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema{ 6603b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema osEnqueuePrivateEvt(EVT_LATENCY_TIMER, data, NULL, mHostIntfTid); 6613b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema} 6623b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema 663f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic bool initSensors() 664f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema{ 6653b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema uint32_t i, j, blocks, maxBlocks, numAxis, packetSamples; 666f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema bool present, error; 667f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema const struct SensorInfo *si; 668f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t handle; 669fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema static uint8_t errorCnt = 0; 670a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema uint32_t totalBlocks = 0; 671a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema uint8_t numSensors = 0; 672a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema ATOMIC_BITSET_DECL(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID,); 673f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 674a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema atomicBitsetInit(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID); 675f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 676f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema for (i = SENS_TYPE_INVALID + 1; i <= SENS_TYPE_LAST_USER; i++) { 677f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema for (j = 0, present = 0, error = 0; (si = sensorFind(i, j, &handle)) != NULL; j++) { 678f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (!sensorGetInitComplete(handle)) { 679fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema if (errorCnt >= SENSOR_INIT_ERROR_MAX) { 680fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema osLog(LOG_ERROR, "initSensors: %s not ready - skipping!\n", si->sensorName); 681fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema continue; 682fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema } else { 683fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema osLog(LOG_INFO, "initSensors: %s not ready!\n", si->sensorName); 684fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema timTimerSet(SENSOR_INIT_DELAY, 0, 50, initCompleteCallback, NULL, true); 685fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema errorCnt ++; 686fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema return false; 687fa6dd561bc6092681a60e26eef9b794fe5c7a8f1Ben Fennema } 6880b54e03f62758f07a970de7458e4ea096815668fBrian Duddie } else if (!(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) { 689f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (!present) { 690f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema present = 1; 691f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema numAxis = si->numAxis; 692f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema switch (si->numAxis) { 693f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_EMBEDDED: 694f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_ONE: 695e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint); 696f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema break; 697f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_THREE: 698f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) 699f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint); 700f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema else 701f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint); 702f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema break; 703f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema default: 704f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema packetSamples = 1; 705f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema error = true; 706f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 707f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (si->minSamples > MAX_MIN_SAMPLES) 708f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema maxBlocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples; 709f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema else 710f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema maxBlocks = (si->minSamples + packetSamples - 1) / packetSamples; 711f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 712f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (si->numAxis != numAxis) { 713f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema error = true; 714f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 715f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (si->minSamples > MAX_MIN_SAMPLES) 716f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema blocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples; 717f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema else 718f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema blocks = (si->minSamples + packetSamples - 1) / packetSamples; 719f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 720f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema maxBlocks = maxBlocks > blocks ? maxBlocks : blocks; 721f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 722f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 723f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 724f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 725f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 726f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (present && !error) { 727a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema atomicBitsetSetBit(sensorPresent, i - 1); 728a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema numSensors++; 729a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema totalBlocks += maxBlocks; 730f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 731f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 732f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 733a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema if (totalBlocks > MAX_NUM_BLOCKS) { 734a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema osLog(LOG_INFO, "initSensors: totalBlocks of %ld exceeds maximum of %d\n", totalBlocks, MAX_NUM_BLOCKS); 735a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema totalBlocks = MAX_NUM_BLOCKS; 736a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema } else if (totalBlocks < MIN_NUM_BLOCKS) { 737a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema totalBlocks = MIN_NUM_BLOCKS; 738f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 739f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 740a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema mOutputQ = simpleQueueAlloc(totalBlocks, sizeof(struct HostIntfDataBuffer), queueDiscard); 741a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema mActiveSensorTable = heapAlloc(numSensors * sizeof(struct ActiveSensor)); 742a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema memset(mActiveSensorTable, 0x00, numSensors * sizeof(struct ActiveSensor)); 743f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 74469e75a2575055ba5ca84230615f10bee6e4ff4e0Jarmo Torvinen for (i = SENS_TYPE_INVALID; i < SENS_TYPE_LAST_USER; i++) { 74569e75a2575055ba5ca84230615f10bee6e4ff4e0Jarmo Torvinen mSensorList[i] = MAX_REGISTERED_SENSORS; 74669e75a2575055ba5ca84230615f10bee6e4ff4e0Jarmo Torvinen } 74769e75a2575055ba5ca84230615f10bee6e4ff4e0Jarmo Torvinen 748a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema for (i = SENS_TYPE_INVALID + 1, j = 0; i <= SENS_TYPE_LAST_USER && j < numSensors; i++) { 749a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema if (atomicBitsetGetBit(sensorPresent, i - 1) 750a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema && (si = sensorFind(i, 0, &handle)) != NULL 751a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema && !(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) { 752f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mSensorList[i - 1] = j; 753f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema resetBuffer(mActiveSensorTable + j); 754f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].buffer.sensType = i; 755d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema mActiveSensorTable[j].biasReportType = 0; 7569c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema mActiveSensorTable[j].rate = 0; 7579c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema mActiveSensorTable[j].latency = 0; 758f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].numAxis = si->numAxis; 759f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].interrupt = si->interrupt; 760f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) { 761f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mSensorList[si->rawType - 1] = j; 762f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].buffer.sensType = si->rawType; 763f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].raw = true; 764f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].rawScale = si->rawScale; 765d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 766d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (si->flags1 & SENSOR_INFO_FLAGS1_BIAS) { 7674d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema mSensorList[si->biasType - 1] = j; 768d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema mActiveSensorTable[j].biasReportType = i; 7694d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema osEventSubscribe(mHostIntfTid, sensorGetMyEventType(si->biasType)); 7704d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema } 771f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (si->minSamples > MAX_MIN_SAMPLES) { 772f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].minSamples = MAX_MIN_SAMPLES; 773f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema osLog(LOG_INFO, "initSensors: %s: minSamples of %d exceeded max of %d\n", si->sensorName, si->minSamples, MAX_MIN_SAMPLES); 774f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 775f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].minSamples = si->minSamples; 776f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 777f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].curSamples = 0; 778f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mActiveSensorTable[j].oneshot = false; 7797c00b1c182d841a5f82bfca9381025873e77084dBen Fennema mActiveSensorTable[j].firstTime = 0ull; 780f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema switch (si->numAxis) { 781f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_EMBEDDED: 782f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_ONE: 783e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint); 784f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema break; 785f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema case NUM_AXIS_THREE: 786f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema if (mActiveSensorTable[j].raw) 787f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint); 788f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema else 789f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint); 790f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema break; 791f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 792f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema j++; 793f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 794f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 795f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 796a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema mTotalBlocks = totalBlocks; 797a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema mNumSensors = numSensors; 798a107817aebfd8afdd0e6d25d0f5f62e0067ad9beBen Fennema 799f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema return true; 800f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} 801f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 802f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennemastatic inline int16_t floatToInt16(float val) 803f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema{ 804f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema if (val < (INT16_MIN + 0.5f)) 805f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema return INT16_MIN; 806f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema else if (val > (INT16_MAX - 0.5f)) 807f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema return INT16_MAX; 808f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema else if (val >= 0.0f) 809f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema return val + 0.5f; 810f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema else 811f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema return val - 0.5f; 812f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema} 813f7fd31f9f089d3856bc58233e825f46104f73f5bBen Fennema 8145f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qianstatic uint32_t encodeDeltaTime(uint64_t time) 8155f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian{ 8165f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian uint32_t deltaTime; 8175f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian 8185f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian if (time <= UINT32_MAX) { 8195f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian deltaTime = time | delta_time_fine_mask; 8205f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian } else { 8215f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian deltaTime = ((time + delta_time_rounding) >> delta_time_multiplier_order) & delta_time_coarse_mask; 8225f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian } 8235f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian return deltaTime; 8245f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian} 8255f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian 8267467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudovstatic bool enqueueSensorBuffer(struct ActiveSensor *sensor) 8277467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov{ 8287467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov bool queued = simpleQueueEnqueue(mOutputQ, &sensor->buffer, 8297467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov sizeof(uint32_t) + sensor->buffer.length, sensor->discard); 8307467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 8317467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (!queued) { 8327467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov // undo counters if failed to add buffer 8337467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->interrupt == NANOHUB_INT_WAKEUP) 8347467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov mWakeupBlocks--; 8357467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 8367467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov mNonWakeupBlocks--; 8372166f5d1d68a304348e807701eebba774041c5d5Ben Fennema sensor->curSamples -= sensor->buffer.firstSample.numSamples; 8387467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov } 8397467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov resetBuffer(sensor); 8407467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov return queued; 8417467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov} 8427467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 843f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic void copySingleSamples(struct ActiveSensor *sensor, const struct SingleAxisDataEvent *single) 844f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema{ 845f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema int i; 846f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t deltaTime; 847f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t numSamples; 8487467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov uint8_t evtNumSamples = single->samples[0].firstSample.numSamples; 849f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 8507467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov for (i = 0; i < evtNumSamples; i++) { 8517467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) 8527467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 853f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 854f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (sensor->buffer.firstSample.numSamples == 0) { 855f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (i == 0) { 856f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime = sensor->buffer.referenceTime = single->referenceTime; 857f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 858f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime += single->samples[i].deltaTime; 859f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.referenceTime = sensor->lastTime; 860f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 861f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint); 862f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.single[0].idata = single->samples[i].idata; 8633b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 8643b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks++; 8653b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 8663b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks++; 867f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.firstSample.numSamples = 1; 868b097a39d5f40a3ba1fc72634229a998086014be6Ben Fennema sensor->buffer.firstSample.interrupt = sensor->interrupt; 8697c00b1c182d841a5f82bfca9381025873e77084dBen Fennema if (sensor->curSamples++ == 0) 8707c00b1c182d841a5f82bfca9381025873e77084dBen Fennema sensor->firstTime = sensor->buffer.referenceTime; 871f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 872f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (i == 0) { 873f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (sensor->lastTime > single->referenceTime) { 874f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema // shouldn't happen. flush current packet 8757467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 8763b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema i--; 8775f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian } else if (single->referenceTime - sensor->lastTime >= delta_time_max) { 8787467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 8793b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema i--; 880f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 8815f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian deltaTime = encodeDeltaTime(single->referenceTime - sensor->lastTime); 882f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema numSamples = sensor->buffer.firstSample.numSamples; 883f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 884f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length += sizeof(struct SingleAxisDataPoint); 885f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.single[numSamples].deltaTime = deltaTime; 886f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.single[numSamples].idata = single->samples[0].idata; 887f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime = single->referenceTime; 8883b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->buffer.firstSample.numSamples++; 8893b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->curSamples++; 890f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 891f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 892f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema deltaTime = single->samples[i].deltaTime; 893f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema numSamples = sensor->buffer.firstSample.numSamples; 894f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 895f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length += sizeof(struct SingleAxisDataPoint); 8965f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian sensor->buffer.single[numSamples].deltaTime = deltaTime | delta_time_fine_mask; 897f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.single[numSamples].idata = single->samples[i].idata; 898f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime += deltaTime; 8993b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->buffer.firstSample.numSamples++; 9003b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->curSamples++; 901f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 902f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 903f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 904f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} 905f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 906f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennemastatic void copyTripleSamples(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple) 907f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema{ 908f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema int i; 909f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t deltaTime; 910f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint8_t numSamples; 911f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 9129c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) { 9137467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) 9147467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 915f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 916f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (sensor->buffer.firstSample.numSamples == 0) { 917f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (i == 0) { 918f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime; 919f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 920f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime += triple->samples[i].deltaTime; 921f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.referenceTime = sensor->lastTime; 922f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 923f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint); 924f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[0].ix = triple->samples[i].ix; 925f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[0].iy = triple->samples[i].iy; 926f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[0].iz = triple->samples[i].iz; 9274d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) { 9284d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent; 9294d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasPresent = 1; 9304d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasSample = 0; 9314d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->discard = false; 9324d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema } 9333b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 9343b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mWakeupBlocks++; 9353b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 9363b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema mNonWakeupBlocks++; 937f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.firstSample.numSamples = 1; 938b097a39d5f40a3ba1fc72634229a998086014be6Ben Fennema sensor->buffer.firstSample.interrupt = sensor->interrupt; 9397c00b1c182d841a5f82bfca9381025873e77084dBen Fennema if (sensor->curSamples++ == 0) 9407c00b1c182d841a5f82bfca9381025873e77084dBen Fennema sensor->firstTime = sensor->buffer.referenceTime; 941f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 942f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (i == 0) { 943f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (sensor->lastTime > triple->referenceTime) { 944f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema // shouldn't happen. flush current packet 9457467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 9463b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema i--; 9475f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) { 9487467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 9493b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema i--; 950f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 9515f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime); 952f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema numSamples = sensor->buffer.firstSample.numSamples; 953f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 954f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length += sizeof(struct TripleAxisDataPoint); 955f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].deltaTime = deltaTime; 956f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].ix = triple->samples[0].ix; 957f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].iy = triple->samples[0].iy; 958f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].iz = triple->samples[0].iz; 959f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime = triple->referenceTime; 9604d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == 0) { 9614d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent; 9624d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasPresent = 1; 9634d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasSample = numSamples; 9644d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->discard = false; 9654d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema } 9663b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->buffer.firstSample.numSamples++; 9673b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->curSamples++; 968f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 969f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } else { 970f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema deltaTime = triple->samples[i].deltaTime; 971f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema numSamples = sensor->buffer.firstSample.numSamples; 972f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 973f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.length += sizeof(struct TripleAxisDataPoint); 9745f5610548cd0a03d043b573d8385e53864db2f31Zhengyin Qian sensor->buffer.triple[numSamples].deltaTime = deltaTime | delta_time_fine_mask; 975f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].ix = triple->samples[i].ix; 976f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].iy = triple->samples[i].iy; 977f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->buffer.triple[numSamples].iz = triple->samples[i].iz; 978f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sensor->lastTime += deltaTime; 9794d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) { 9804d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent; 9814d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasPresent = 1; 9824d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->buffer.firstSample.biasSample = numSamples; 9834d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema sensor->discard = false; 9844d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema } 9853b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->buffer.firstSample.numSamples++; 9863b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema sensor->curSamples++; 987f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 988f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 989f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 990f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema} 991f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 992d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennemastatic void copyTripleSamplesBias(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple) 993d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema{ 994d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema uint8_t sensType = sensor->buffer.sensType; 995d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 996d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (sensType == sensor->biasReportType) { 997d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema copyTripleSamples(sensor, triple); 998d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else { 999d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // Bias needs to be sent with a different sensType, so enqueue any pending buffer, enqueue 1000d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // bias with a different sensor type, then restore the sensType 10017467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.firstSample.numSamples > 0) 10027467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1003d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.sensType = sensor->biasReportType; 1004d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema copyTripleSamples(sensor, triple); 10057467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.firstSample.numSamples > 0) 10067467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1007d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.sensType = sensType; 1008d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1009d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema} 1010d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1011d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennemastatic void copyTripleSamplesRaw(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple) 1012d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema{ 1013d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema int i; 1014d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema uint32_t deltaTime; 1015d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema uint8_t numSamples; 1016d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1017d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // Bias not supported in raw format; treat as regular format triple samples (potentially 1018d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // handling alternate bias report type) 1019d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (triple->samples[0].firstSample.biasPresent) { 1020d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema copyTripleSamplesBias(sensor, triple); 1021d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema return; 1022d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1023d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1024d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) { 10257467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) 10267467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1027d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1028d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (sensor->buffer.firstSample.numSamples == 0) { 1029d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (i == 0) { 1030d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime; 1031d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else { 1032d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->lastTime += triple->samples[i].deltaTime; 1033d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.referenceTime = sensor->lastTime; 1034d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1035d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.length = sizeof(struct RawTripleAxisDataEvent) + sizeof(struct RawTripleAxisDataPoint); 1036d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[0].ix = floatToInt16(triple->samples[i].x * sensor->rawScale); 1037d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[0].iy = floatToInt16(triple->samples[i].y * sensor->rawScale); 1038d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[0].iz = floatToInt16(triple->samples[i].z * sensor->rawScale); 1039d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (sensor->interrupt == NANOHUB_INT_WAKEUP) 1040d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema mWakeupBlocks++; 1041d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 1042d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema mNonWakeupBlocks++; 1043d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.firstSample.numSamples = 1; 1044d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.firstSample.interrupt = sensor->interrupt; 1045d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (sensor->curSamples++ == 0) 1046d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->firstTime = sensor->buffer.referenceTime; 1047d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else { 1048d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (i == 0) { 1049d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema if (sensor->lastTime > triple->referenceTime) { 1050d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema // shouldn't happen. flush current packet 10517467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1052d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema i--; 1053d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) { 10547467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1055d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema i--; 1056d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else { 1057d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime); 1058d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema numSamples = sensor->buffer.firstSample.numSamples; 1059d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1060d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint); 1061d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime; 1062d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[0].x * sensor->rawScale); 1063d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[0].y * sensor->rawScale); 1064d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[0].z * sensor->rawScale); 1065d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->lastTime = triple->referenceTime; 1066d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.firstSample.numSamples++; 1067d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->curSamples++; 1068d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1069d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } else { 1070d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema deltaTime = triple->samples[i].deltaTime; 1071d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema numSamples = sensor->buffer.firstSample.numSamples; 1072d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1073d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint); 1074d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime | delta_time_fine_mask; 1075d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[i].x * sensor->rawScale); 1076d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[i].y * sensor->rawScale); 1077d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[i].z * sensor->rawScale); 1078d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->lastTime += deltaTime; 1079d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->buffer.firstSample.numSamples++; 1080d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema sensor->curSamples++; 1081d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1082d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1083d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema } 1084d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema} 1085d8b09891f186b2356ebc4fe37e00ffb3fad06405Ben Fennema 1086b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennemastatic void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable, bool interrupt) 1087e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema{ 10887467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (!simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, discardable)) 10897467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov return; 10907467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 1091e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema if (data->interrupt == NANOHUB_INT_WAKEUP) 1092e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema mWakeupBlocks++; 1093e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema else if (data->interrupt == NANOHUB_INT_NONWAKEUP) 1094e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema mNonWakeupBlocks++; 1095b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennema nanohubPrefetchTx(interrupt ? data->interrupt : HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks); 1096e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema} 1097e9098ab13e66bf980eb6bdd089ba6065379826e9Ben Fennema 10985a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudovstatic void hostIntfNotifyReboot(uint32_t reason) 10995a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov{ 11005a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp)); 11015a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov __le32 raw_reason = htole32(reason); 11025a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov 11035a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov if (resp) { 11045a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov resp->hdr = (struct NanohubHalHdr){ 110554794375c7f69172894e9f365db835440c2bfdf6Alexey Polyudov .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 11065a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg), 11075a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov .msg = NANOHUB_HAL_REBOOT, 11085a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov }; 11095a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov memcpy(&resp->reason, &raw_reason, sizeof(resp->reason)); 11105a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); 11115a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov } 11125a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov} 11135a4366e5b9abbf4f9a8026ac2db162c963f5f8aeAlexey Polyudov 1114df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinenstatic void queueFlush(struct ActiveSensor *sensor) 1115df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen{ 1116df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen if (sensor->buffer.length == 0) { 1117df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen sensor->buffer.length = sizeof(sensor->buffer.referenceTime) + sizeof(struct SensorFirstSample); 1118df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen sensor->buffer.referenceTime = 0ull; 1119df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen if (sensor->interrupt == NANOHUB_INT_WAKEUP) 1120df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen mWakeupBlocks++; 1121df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 1122df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen mNonWakeupBlocks++; 1123df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen sensor->buffer.firstSample.numFlushes = 1; 1124df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen } else { 1125df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen sensor->buffer.firstSample.numFlushes++; 1126df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen } 1127df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen sensor->discard = false; 1128df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen hostIntfSetInterrupt(sensor->interrupt); 1129df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen} 1130df3d2be9a9a09d011c391f4b5e4417ca7e3b554aJarmo Torvinen 11311bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qianstatic void fakeFlush(struct ConfigCmd *cmd) 11321bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian{ 11331bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian struct HostIntfDataBuffer *buffer; 11341bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian uint8_t size = sizeof(buffer->evtType) + sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample); 11351bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian buffer = alloca(size); 11361bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian memset(buffer, 0x00, size); 11371bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian 11381bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian buffer->sensType = cmd->sensType; 11391bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian buffer->length = sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample); 11401bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian buffer->interrupt = NANOHUB_INT_WAKEUP; 11417467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov mWakeupBlocks++; 11421bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian buffer->firstSample.numFlushes = 1; 11437467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (!simpleQueueEnqueue(mOutputQ, buffer, size, false)) 11447467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov mWakeupBlocks--; 11451bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian} 11461bfdbfb2b3a5778b1a46b4b30ebcda705a6c8021Zhengyin Qian 1147e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtAppStart(const void *evtData) 11487062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema{ 1149e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (initSensors()) { 1150e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t reason; 1151e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov struct HostIntfDataBuffer *data; 11527062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 1153e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventUnsubscribe(mHostIntfTid, EVT_APP_START); 1154e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT); 1155774ea97d18117e0d13108a0968315f23102cbc40Peng Xu osEventSubscribe(mHostIntfTid, EVT_APP_TO_SENSOR_HAL_DATA); 1156e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST); 1157f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema#ifdef DEBUG_LOG_EVT 1158e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG); 1159e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov platEarlyLogFlush(); 1160f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema#endif 1161e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov reason = pwrResetReason(); 1162e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data = alloca(sizeof(uint32_t) + sizeof(reason)); 1163e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->sensType = SENS_TYPE_INVALID; 1164e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->length = sizeof(reason); 1165e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON; 1166e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->interrupt = NANOHUB_INT_WAKEUP; 1167e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov memcpy(data->buffer, &reason, sizeof(reason)); 1168b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennema hostIntfAddBlock(data, false, true); 1169e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov hostIntfNotifyReboot(reason); 1170f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema } 1171e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1172e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1173e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtAppToHost(const void *evtData) 1174e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1175e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov const struct HostHubRawPacket *hostMsg = evtData; 1176e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1177e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (hostMsg->dataLen <= HOST_HUB_RAW_PACKET_MAX_LEN) { 1178e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->dataLen); 1179e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1180e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->sensType = SENS_TYPE_INVALID; 1181e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->length = sizeof(*hostMsg) + hostMsg->dataLen; 1182e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST; 1183e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov data->interrupt = NANOHUB_INT_WAKEUP; 1184e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov memcpy(data->buffer, evtData, data->length); 1185b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennema hostIntfAddBlock(data, false, true); 1186e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1187e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1188e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1189e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtAppFromHost(const void *evtData) 1190e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1191e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov const uint8_t *halMsg = evtData; 1192e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov const struct NanohubHalCommand *halCmd = nanohubHalFindCommand(halMsg[1]); 1193e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (halCmd) 1194e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov halCmd->handler((void *)&halMsg[2], halMsg[0] - 1); 1195e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1196e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 11977062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#ifdef DEBUG_LOG_EVT 1198e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtDebugLog(const void *evtData) 1199e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1200e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData; 1201e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 12027467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG) 1203b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennema hostIntfAddBlock(data, true, true); 1204e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 12057062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#endif 1206e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1207e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtLatencyTimer(const void *evtData) 1208e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1209e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint64_t sensorTime = sensorGetTime(); 1210e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t i, cnt; 1211e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1212e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov for (i = 0, cnt = 0; i < mNumSensors && cnt < mLatencyCnt; i++) { 1213e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (mActiveSensorTable[i].latency > 0) { 1214e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov cnt++; 1215e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (mActiveSensorTable[i].firstTime && 1216e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorTime >= mActiveSensorTable[i].firstTime + mActiveSensorTable[i].latency) { 1217e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov hostIntfSetInterrupt(mActiveSensorTable[i].interrupt); 12183b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } 12193b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema } 1220e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1221e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1222f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 1223e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdFlushOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1224e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1225e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorFlush(sensor->sensorHandle); 1226e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1227e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1228e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdEnableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1229e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1230e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensorRequestRateChange(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) { 1231e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->rate = cmd->rate; 1232e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->latency != cmd->latency) { 1233e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (!sensor->latency) { 1234e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (mLatencyCnt++ == 0) 1235e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false); 1236e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else if (!cmd->latency) { 1237e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (--mLatencyCnt == 0) { 1238e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov timTimerCancel(mLatencyTimer); 1239e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mLatencyTimer = 0; 12407062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 12415e1901d3f916a57246646f41b8f452db2d3a42f7Zhengyin Qian } 1242e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->latency = cmd->latency; 12437062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 1244e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1245e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1246f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 1247e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdEnableAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1248e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1249e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov for (uint32_t i = 0; sensorFind(cmd->sensType, i, &sensor->sensorHandle) != NULL; i++) { 1250e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (cmd->rate == SENSOR_RATE_ONESHOT) { 1251e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov cmd->rate = SENSOR_RATE_ONCHANGE; 1252e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->oneshot = true; 1253e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else { 1254e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->oneshot = false; 1255e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1256f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema 1257e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensorRequest(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) { 1258e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (cmd->latency) { 1259e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (mLatencyCnt++ == 0) 1260e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false); 12617062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 1262e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->rate = cmd->rate; 1263e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->latency = cmd->latency; 1264e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventSubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType)); 1265e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1266e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else { 1267e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->sensorHandle = 0; 1268e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1269e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1270e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1271e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1272e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdDisableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1273e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1274e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorRelease(mHostIntfTid, sensor->sensorHandle); 1275e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventUnsubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType)); 1276e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->latency) { 1277e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (--mLatencyCnt == 0) { 1278e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov timTimerCancel(mLatencyTimer); 1279e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mLatencyTimer = 0; 1280e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1281e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1282e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->rate = 0; 1283e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->latency = 0; 1284e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->oneshot = false; 1285e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->sensorHandle = 0; 1286e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->buffer.length) { 12877467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 1288e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov hostIntfSetInterrupt(sensor->interrupt); 1289e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1290e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 12917062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 1292e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdCalibrateAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1293e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1294e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t tempSensorHandle; 1295e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++) 1296e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorCalibrate(tempSensorHandle); 1297e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1298ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 1299e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdSelfTestAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1300e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1301e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t tempSensorHandle; 1302e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++) 1303e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorSelfTest(tempSensorHandle); 1304e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1305e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1306e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onConfigCmdCfgDataAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd) 1307e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1308e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t tempSensorHandle; 1309e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++) 1310e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorCfgData(tempSensorHandle, (void *)(cmd+1)); 1311e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1312a02d34c3c4e91dc99d123a40d52f05dda4fc3296Ben Fennema 1313e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtNoSensorConfigEvent(const void *evtData) 1314e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1315e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov struct ConfigCmd *cmd = (struct ConfigCmd *)evtData; 13167467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov struct ActiveSensor *sensor = getActiveSensorByType(cmd->sensType); 13177467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor) { 1318e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->sensorHandle) { 1319e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov switch (cmd->cmd) { 1320e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_FLUSH: 1321e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdFlushOne(sensor, cmd); 1322e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1323e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_ENABLE: 1324e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdEnableOne(sensor, cmd); 1325e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1326e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_DISABLE: 1327e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdDisableOne(sensor, cmd); 1328e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1329d62fb52a5e4e6bc6ece53220107e02aa72f40a69Peng Xu case CONFIG_CMD_CFG_DATA: 1330d62fb52a5e4e6bc6ece53220107e02aa72f40a69Peng Xu onConfigCmdCfgDataAll(sensor, cmd); 1331d62fb52a5e4e6bc6ece53220107e02aa72f40a69Peng Xu break; 1332a02d34c3c4e91dc99d123a40d52f05dda4fc3296Ben Fennema } 1333e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else { 1334e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov switch (cmd->cmd) { 1335e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_ENABLE: 1336e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdEnableAll(sensor, cmd); 1337e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1338e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_CALIBRATE: 1339e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdCalibrateAll(sensor, cmd); 13404d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema break; 1341e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_SELF_TEST: 1342e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdSelfTestAll(sensor, cmd); 1343e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1344e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_CFG_DATA: 1345e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onConfigCmdCfgDataAll(sensor, cmd); 1346e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1347e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case CONFIG_CMD_FLUSH: 1348e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov queueFlush(sensor); 13494d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema break; 13504d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema } 13517062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 1352e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else if (cmd->cmd == CONFIG_CMD_FLUSH && cmd->sensType > SENS_TYPE_INVALID) { 1353e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov // if a flush event is for an unknown sensor, we just return a fake flush event. 1354e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osLog(LOG_INFO, "Flush request from unrecognized sensor, returning a fake flush\n"); 1355e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov fakeFlush(cmd); 1356e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1357e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1358e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1359774ea97d18117e0d13108a0968315f23102cbc40Peng Xustatic void onEvtAppToSensorHalData(const void *evtData) 1360774ea97d18117e0d13108a0968315f23102cbc40Peng Xu{ 1361774ea97d18117e0d13108a0968315f23102cbc40Peng Xu struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData; 1362774ea97d18117e0d13108a0968315f23102cbc40Peng Xu if (data->sensType == SENS_TYPE_INVALID 1363774ea97d18117e0d13108a0968315f23102cbc40Peng Xu && data->dataType == HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL) { 1364774ea97d18117e0d13108a0968315f23102cbc40Peng Xu struct AppToSensorHalDataBuffer *buffer = (struct AppToSensorHalDataBuffer *)data; 1365b7a837f3d0848922f553ade310c3558053bd18c9Ben Fennema hostIntfAddBlock(data, (buffer->payload.type & EVENT_TYPE_BIT_DISCARDABLE) != 0, false); 1366774ea97d18117e0d13108a0968315f23102cbc40Peng Xu } 1367774ea97d18117e0d13108a0968315f23102cbc40Peng Xu} 1368774ea97d18117e0d13108a0968315f23102cbc40Peng Xu 1369e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void copyEmbeddedSamples(struct ActiveSensor *sensor, const void* evtData) 1370e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1371e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint64_t sensorTime = sensorGetTime(); 13727467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 13737467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.length > 0 && sensorTime - sensor->lastTime >= delta_time_max) 13747467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov enqueueSensorBuffer(sensor); 13757467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov 1376e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->buffer.length == 0) { 1377e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint); 1378e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->lastTime = sensor->buffer.referenceTime = sensorTime; 1379e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->interrupt == NANOHUB_INT_WAKEUP) 1380e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mWakeupBlocks++; 1381e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP) 1382e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov mNonWakeupBlocks++; 1383e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.firstSample.numSamples = 1; 1384e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.firstSample.interrupt = sensor->interrupt; 1385e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.single[0].idata = (uint32_t)evtData; 1386e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else { 1387e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.length += sizeof(struct SingleAxisDataPoint); 1388e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.single[sensor->buffer.firstSample.numSamples].deltaTime = 1389e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov encodeDeltaTime(sensorTime - sensor->lastTime); 1390e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->lastTime = sensorTime; 1391e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.single[sensor->buffer.firstSample.numSamples].idata = (uint32_t)evtData; 1392e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->buffer.firstSample.numSamples++; 1393e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1394e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->curSamples++ == 0) 1395e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->firstTime = sensor->buffer.referenceTime; 1396e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1397e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1398e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic uint32_t getSensorInterrupt(struct ActiveSensor *sensor) 1399e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1400e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint32_t interrupt = HOSTINTF_MAX_INTERRUPTS; 1401e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov uint64_t sensorTime = sensorGetTime(); 1402e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1403e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->firstTime && 1404e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov ((sensorTime >= sensor->firstTime + sensor->latency) || 1405e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov ((sensor->latency > sensorGetCurLatency(sensor->sensorHandle)) && 1406e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov (sensorTime + sensorGetCurLatency(sensor->sensorHandle) > sensor->firstTime + sensor->latency)))) { 1407e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov interrupt = sensor->interrupt; 1408e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else if (mWakeupBlocks + mNonWakeupBlocks >= mTotalBlocks) { 1409e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov interrupt = sensor->interrupt; 1410e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1411e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1412e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov return interrupt; 1413e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1414e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1415e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtSensorDataActive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData) 1416e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1417e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (evtData == SENSOR_DATA_EVENT_FLUSH) { 1418e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov queueFlush(sensor); 1419e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } else { 14207467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov bool haveFlush = sensor->buffer.firstSample.numFlushes > 0; 14217467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->buffer.length > 0 && 14227467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov (haveFlush || sensor->buffer.firstSample.numSamples == sensor->packetSamples)) { 14237467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov // processing will be aborted if we have pending flush and are not able to send 14247467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov // in this case, send eventually will be retried, otherwise data will be lost 14257467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (!enqueueSensorBuffer(sensor) && haveFlush) 14267467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov return; 1427e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1428e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1429e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov switch (sensor->numAxis) { 1430e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case NUM_AXIS_EMBEDDED: 1431e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov copyEmbeddedSamples(sensor, evtData); 1432e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1433e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case NUM_AXIS_ONE: 1434e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov copySingleSamples(sensor, evtData); 1435e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1436e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case NUM_AXIS_THREE: 1437e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->raw) 1438e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov copyTripleSamplesRaw(sensor, evtData); 1439e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov else 1440e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov copyTripleSamples(sensor, evtData); 1441e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1442e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov default: 1443e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov return; 1444e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1445e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1446e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1447e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov nanohubPrefetchTx(getSensorInterrupt(sensor), mWakeupBlocks, mNonWakeupBlocks); 1448e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1449e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (sensor->oneshot) { 1450e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensorRelease(mHostIntfTid, sensor->sensorHandle); 1451e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov osEventUnsubscribe(mHostIntfTid, evtType); 1452e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->sensorHandle = 0; 1453e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov sensor->oneshot = false; 1454e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1455e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1456e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1457e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtSensorDataInactive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData) 1458e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1459e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (evtData != SENSOR_DATA_EVENT_FLUSH) { 1460e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov // handle bias data which can be generated for sensors that are 1461e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov // not currently requested by the AP 1462e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov switch (sensor->numAxis) { 1463e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case NUM_AXIS_THREE: 1464e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov if (((const struct TripleAxisDataEvent *)evtData)->samples[0].firstSample.biasPresent) { 1465e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov copyTripleSamplesBias(sensor, evtData); 14665a3e6b90a5790e228dc3c43392fcfbaa74a2d3a0Ben Fennema nanohubPrefetchTx(HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks); 1467e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1468e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1469e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1470e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1471e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1472e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1473e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void onEvtSensorData(uint32_t evtType, const void* evtData) 1474e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 14757467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (evtType > EVT_NO_FIRST_SENSOR_EVENT && evtType < EVT_NO_SENSOR_CONFIG_EVENT) { 14767467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov struct ActiveSensor *sensor = getActiveSensorByType(evtType & 0xFF); 14777467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor) { 14787467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov if (sensor->sensorHandle) 14797467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov onEvtSensorDataActive(sensor, evtType, evtData); 14807467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov else 14817467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov onEvtSensorDataInactive(sensor, evtType, evtData); 14827467a75602cdb0b32abc2893fb901c516807eb4eAlexey Polyudov } 1483e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov } 1484e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov} 1485e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov 1486e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudovstatic void hostIntfHandleEvent(uint32_t evtType, const void* evtData) 1487e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov{ 1488e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov switch (evtType) { 1489e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_APP_START: 1490e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtAppStart(evtData); 1491e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1492e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_APP_TO_HOST: 1493e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtAppToHost(evtData); 1494e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1495e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_APP_FROM_HOST: 1496e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtAppFromHost(evtData); 1497e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1498e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov#ifdef DEBUG_LOG_EVT 1499e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_DEBUG_LOG: 1500e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtDebugLog(evtData); 1501e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1502e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov#endif 1503e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_LATENCY_TIMER: 1504e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtLatencyTimer(evtData); 1505e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1506e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov case EVT_NO_SENSOR_CONFIG_EVENT: 1507e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtNoSensorConfigEvent(evtData); 1508e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 1509774ea97d18117e0d13108a0968315f23102cbc40Peng Xu case EVT_APP_TO_SENSOR_HAL_DATA: 1510774ea97d18117e0d13108a0968315f23102cbc40Peng Xu onEvtAppToSensorHalData(evtData); 1511774ea97d18117e0d13108a0968315f23102cbc40Peng Xu break; 1512e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov default: 1513e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov onEvtSensorData(evtType, evtData); 1514e673392dfca4a31fff5e93f320848e8181ef5bd9Alexey Polyudov break; 15157062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema } 15167062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema} 15177062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 15189c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemavoid hostIntfCopyInterrupts(void *dst, uint32_t numBits) 151967edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema{ 15209c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema if (mInterrupt->numBits != numBits) 15219c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema return; 152267edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 15239c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema atomicBitsetBulkRead(mInterrupt, dst, numBits); 15249c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema} 152567edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 15269c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemavoid hostIntfClearInterrupts() 15279c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema{ 15283b29ae466154d7953425796516470fe5d2f79fd3Ben Fennema uint32_t i; 15299c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema 15309c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema for (i = 0; i < HOSTINTF_MAX_INTERRUPTS; i++) { 15319c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema if (atomicBitsetGetBit(mInterrupt, i)) 15329c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema hostIntfClearInterrupt(i); 15339c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 153467edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema} 153567edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 153667edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennemavoid hostIntfSetInterrupt(uint32_t bit) 153767edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema{ 15389c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema uint64_t state = cpuIntsOff(); 1539de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mHostIntfTid) { 1540de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (!atomicBitsetGetBit(mInterrupt, bit)) { 1541de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema atomicBitsetSetBit(mInterrupt, bit); 1542de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (!atomicBitsetGetBit(mInterruptMask, bit)) { 1543de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mInterruptCntWkup++ == 0) 1544de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntSet(true); 1545de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } else { 1546de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mInterruptCntNonWkup++ == 0) 1547de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntSet(false); 1548de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } 15499c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 15502e781101b1a6b63de27bc9af304ce76e0ddddd3aBen Fennema } 15519c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema cpuIntsRestore(state); 155267edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema} 155367edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 1554ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennemabool hostIntfGetInterrupt(uint32_t bit) 1555ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema{ 1556ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema return atomicBitsetGetBit(mInterrupt, bit); 1557ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema} 1558ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 15599c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemavoid hostIntfClearInterrupt(uint32_t bit) 156067edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema{ 15619c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema uint64_t state = cpuIntsOff(); 1562de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mHostIntfTid) { 1563de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (atomicBitsetGetBit(mInterrupt, bit)) { 1564de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema atomicBitsetClearBit(mInterrupt, bit); 1565de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (!atomicBitsetGetBit(mInterruptMask, bit)) { 1566de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (--mInterruptCntWkup == 0) 1567de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntClear(true); 1568de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } else { 1569de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (--mInterruptCntNonWkup == 0) 1570de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntClear(false); 1571de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } 15729c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 15739c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 15749c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema cpuIntsRestore(state); 157567edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema} 157667edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 157767edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennemavoid hostIntfSetInterruptMask(uint32_t bit) 157867edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema{ 15799c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema uint64_t state = cpuIntsOff(); 1580de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mHostIntfTid) { 1581de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (!atomicBitsetGetBit(mInterruptMask, bit)) { 1582de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema atomicBitsetSetBit(mInterruptMask, bit); 1583de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (atomicBitsetGetBit(mInterrupt, bit)) { 1584de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (--mInterruptCntWkup == 0) 1585de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntClear(true); 1586de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mInterruptCntNonWkup++ == 0) 1587de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntSet(false); 1588de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } 15899c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 15909c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 15919c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema cpuIntsRestore(state); 159267edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema} 159367edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema 1594ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennemabool hostIntfGetInterruptMask(uint32_t bit) 1595ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema{ 1596ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema return atomicBitsetGetBit(mInterruptMask, bit); 1597ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema} 1598ee0d3076769654b62bc19d94de3b5eaa480703aeBen Fennema 15999c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennemavoid hostIntfClearInterruptMask(uint32_t bit) 160067edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema{ 16019c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema uint64_t state = cpuIntsOff(); 1602de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mHostIntfTid) { 1603de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (atomicBitsetGetBit(mInterruptMask, bit)) { 1604de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema atomicBitsetClearBit(mInterruptMask, bit); 1605de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (atomicBitsetGetBit(mInterrupt, bit)) { 1606de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (mInterruptCntWkup++ == 0) 1607de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntSet(true); 1608de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema if (--mInterruptCntNonWkup == 0) 1609de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema apIntClear(false); 1610de71e09ddb929de07063d12192ca9e5d1818c361Ben Fennema } 16119c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 16129c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema } 16139c851fb611fda8d0e5b2e73acc1f5bddd4094317Ben Fennema cpuIntsRestore(state); 161467edb1d56c071082e4ebf31fc751a969cff43a5aBen Fennema} 16157062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema 161654794375c7f69172894e9f365db835440c2bfdf6Alexey PolyudovINTERNAL_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent); 1617