DO NOT MERGE ANYWHERE: seos: log app loading address am: 0e738ff5da -s ours
am: d3e3d03a97 -s ours
Change-Id: I5369b47afe6c4691f64e1d71d8053a4f3fc62dd5
diff --git a/contexthubhal/Android.mk b/contexthubhal/Android.mk
new file mode 100644
index 0000000..f578161
--- /dev/null
+++ b/contexthubhal/Android.mk
@@ -0,0 +1,19 @@
+LOCAL_PATH := $(call my-dir)
+
+# HAL module implemenation stored in
+# hw/<CONTEXT_HUB_MODULE_ID>.<ro.hardware>.so
+include $(CLEAR_VARS)
+
+LOCAL_MODULE_RELATIVE_PATH := hw
+LOCAL_MULTILIB := both
+LOCAL_SHARED_LIBRARIES := liblog libcutils
+LOCAL_SRC_FILES := nanohubhal.cpp system_comms.cpp
+LOCAL_CFLAGS := -Wall -Werror -Wextra
+LOCAL_MODULE_OWNER := google
+
+# Include target-specific files.
+LOCAL_SRC_FILES += nanohubhal_default.cpp
+
+LOCAL_MODULE := context_hub.default
+LOCAL_MODULE_TAGS := optional
+include $(BUILD_SHARED_LIBRARY)
diff --git a/contexthubhal/message_buf.h b/contexthubhal/message_buf.h
new file mode 100644
index 0000000..f6ef893
--- /dev/null
+++ b/contexthubhal/message_buf.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#ifndef _MESSAGE_BUF_H_
+#define _MESSAGE_BUF_H_
+
+#include <endian.h>
+#include <cstring>
+
+namespace android {
+
+namespace nanohub {
+
+/*
+ * Marshaling helper;
+ * deals with alignment and endianness.
+ * Assumption is:
+ * read*() parse buffer from device in LE format;
+ * return host endianness, aligned data
+ * write*() primitives take host endinnness, aligned data,
+ * generate buffer to be passed to device in LE format
+ *
+ * Primitives do minimal error checking, only to ensure buffer read/write
+ * safety. Caller is responsible for making sure correct amount of data
+ * has been processed.
+ */
+class MessageBuf {
+ char *data;
+ size_t size;
+ size_t pos;
+ bool readOnly;
+public:
+ MessageBuf(char *buf, size_t bufSize) {
+ size = bufSize;
+ pos = 0;
+ data = buf;
+ readOnly = false;
+ }
+ MessageBuf(const char *buf, size_t bufSize) {
+ size = bufSize;
+ pos = 0;
+ data = const_cast<char *>(buf);
+ readOnly = true;
+ }
+ const char *getData() const { return data; }
+ size_t getSize() const { return size; }
+ size_t getPos() const { return pos; }
+ size_t getRoom() const { return size - pos; }
+ uint8_t readU8() {
+ if (pos == size) {
+ return 0;
+ }
+ return data[pos++];
+ }
+ void writeU8(uint8_t val) {
+ if (pos == size || readOnly)
+ return;
+ data[pos++] = val;
+ }
+ uint16_t readU16() {
+ if (pos > (size - sizeof(uint16_t))) {
+ return 0;
+ }
+ uint16_t val;
+ memcpy(&val, &data[pos], sizeof(val));
+ pos += sizeof(val);
+ return le16toh(val);
+ }
+ void writeU16(uint16_t val) {
+ if (pos > (size - sizeof(uint16_t)) || readOnly) {
+ return;
+ }
+ uint16_t tmp = htole16(val);
+ memcpy(&data[pos], &tmp, sizeof(tmp));
+ pos += sizeof(tmp);
+ }
+ uint32_t readU32() {
+ if (pos > (size - sizeof(uint32_t))) {
+ return 0;
+ }
+ uint32_t val;
+ memcpy(&val, &data[pos], sizeof(val));
+ pos += sizeof(val);
+ return le32toh(val);
+ }
+ void writeU32(uint32_t val) {
+ if (pos > (size - sizeof(uint32_t)) || readOnly) {
+ return;
+ }
+ uint32_t tmp = htole32(val);
+ memcpy(&data[pos], &tmp, sizeof(tmp));
+ pos += sizeof(tmp);
+ }
+ uint64_t readU64() {
+ if (pos > (size - sizeof(uint64_t))) {
+ return 0;
+ }
+ uint64_t val;
+ memcpy(&val, &data[pos], sizeof(val));
+ pos += sizeof(val);
+ return le32toh(val);
+ }
+ void writeU64(uint64_t val) {
+ if (pos > (size - sizeof(uint64_t)) || readOnly) {
+ return;
+ }
+ uint64_t tmp = htole64(val);
+ memcpy(&data[pos], &tmp, sizeof(tmp));
+ pos += sizeof(tmp);
+ }
+ const void *readRaw(size_t bufSize) {
+ if (pos > (size - bufSize)) {
+ return nullptr;
+ }
+ const void *buf = &data[pos];
+ pos += bufSize;
+ return buf;
+ }
+ void writeRaw(const void *buf, size_t bufSize) {
+ if (pos > (size - bufSize) || readOnly) {
+ return;
+ }
+ memcpy(&data[pos], buf, bufSize);
+ pos += bufSize;
+ }
+};
+
+}; // namespace nanohub
+
+}; // namespace android
+
+#endif
+
diff --git a/contexthubhal/nanohub_perdevice.h b/contexthubhal/nanohub_perdevice.h
new file mode 100644
index 0000000..7fefb4d
--- /dev/null
+++ b/contexthubhal/nanohub_perdevice.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#ifndef _NANOHUB_PER_DEVICE_H_
+#define _NANOHUB_PER_DEVICE_H_
+
+#include <hardware/context_hub.h>
+
+namespace android {
+
+namespace nanohub {
+
+const struct context_hub_t* get_hub_info(void);
+const char *get_devnode_path(void);
+
+}; // namespace nanohub
+
+}; // namespace android
+
+#endif
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp
new file mode 100644
index 0000000..960258a
--- /dev/null
+++ b/contexthubhal/nanohubhal.cpp
@@ -0,0 +1,425 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define LOG_TAG "NanohubHAL"
+
+#include <fcntl.h>
+#include <poll.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <sys/inotify.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <hardware/context_hub.h>
+#include <hardware/hardware.h>
+
+#include <utils/Log.h>
+#include <cutils/properties.h>
+
+#include <cinttypes>
+#include <iomanip>
+#include <sstream>
+
+#include "nanohub_perdevice.h"
+#include "system_comms.h"
+#include "nanohubhal.h"
+
+#define NANOHUB_LOCK_DIR "/data/system/nanohub_lock"
+#define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock"
+#define NANOHUB_LOCK_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR)
+
+namespace android {
+
+namespace nanohub {
+
+inline std::ostream &operator << (std::ostream &os, const hub_app_name_t &appId)
+{
+ char vendor[6];
+ __be64 beAppId = htobe64(appId.id);
+ uint32_t seqId = appId.id & NANOAPP_VENDOR_ALL_APPS;
+
+ std::ios::fmtflags f(os.flags());
+ memcpy(vendor, (void*)&beAppId, sizeof(vendor) - 1);
+ vendor[sizeof(vendor) - 1] = 0;
+ if (strlen(vendor) == 5)
+ os << vendor << ", " << std::hex << std::setw(6) << seqId;
+ else
+ os << "#" << std::hex << appId.id;
+ os.flags(f);
+
+ return os;
+}
+
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status)
+{
+ std::ostringstream os;
+ const uint8_t *p = static_cast<const uint8_t *>(data);
+ os << pfx << ": [ID=" << appId << "; SZ=" << std::dec << len;
+ if (evtId)
+ os << "; EVT=" << std::hex << evtId;
+ os << "]:" << std::hex;
+ for (size_t i = 0; i < len; ++i) {
+ os << " " << std::setfill('0') << std::setw(2) << (unsigned int)p[i];
+ }
+ if (status) {
+ os << "; status=" << status << " [" << std::setfill('0') << std::setw(8) << status << "]";
+ }
+ ALOGI("%s", os.str().c_str());
+}
+
+static int rwrite(int fd, const void *buf, int len)
+{
+ int ret;
+
+ do {
+ ret = write(fd, buf, len);
+ } while (ret < 0 && errno == EINTR);
+
+ if (ret != len) {
+ return errno ? -errno : -EIO;
+ }
+
+ return 0;
+}
+
+static int rread(int fd, void *buf, int len)
+{
+ int ret;
+
+ do {
+ ret = read(fd, buf, len);
+ } while (ret < 0 && errno == EINTR);
+
+ return ret;
+}
+
+static bool init_inotify(pollfd *pfd) {
+ bool success = false;
+
+ mkdir(NANOHUB_LOCK_DIR, NANOHUB_LOCK_DIR_PERMS);
+ pfd->fd = inotify_init1(IN_NONBLOCK);
+ if (pfd->fd < 0) {
+ ALOGE("Couldn't initialize inotify: %s", strerror(errno));
+ } else if (inotify_add_watch(pfd->fd, NANOHUB_LOCK_DIR, IN_CREATE | IN_DELETE) < 0) {
+ ALOGE("Couldn't add inotify watch: %s", strerror(errno));
+ close(pfd->fd);
+ } else {
+ pfd->events = POLLIN;
+ success = true;
+ }
+
+ return success;
+}
+
+static void discard_inotify_evt(pollfd &pfd) {
+ if ((pfd.revents & POLLIN)) {
+ char buf[sizeof(inotify_event) + NAME_MAX + 1];
+ int ret = read(pfd.fd, buf, sizeof(buf));
+ ALOGD("Discarded %d bytes of inotify data", ret);
+ }
+}
+
+static void wait_on_dev_lock(pollfd &pfd) {
+ // While the lock file exists, poll on the inotify fd (with timeout)
+ discard_inotify_evt(pfd);
+ while (access(NANOHUB_LOCK_FILE, F_OK) == 0) {
+ ALOGW("Nanohub is locked; blocking read thread");
+ int ret = poll(&pfd, 1, 5000);
+ if (ret > 0) {
+ discard_inotify_evt(pfd);
+ }
+ }
+}
+
+int NanoHub::doSendToDevice(const hub_app_name_t *name, const void *data, uint32_t len)
+{
+ if (len > MAX_RX_PACKET) {
+ return -EINVAL;
+ }
+
+ nano_message msg = {
+ .hdr = {
+ .event_id = APP_FROM_HOST_EVENT_ID,
+ .app_name = *name,
+ .len = static_cast<uint8_t>(len),
+ },
+ };
+
+ memcpy(&msg.data[0], data, len);
+
+ return rwrite(mFd, &msg, len + sizeof(msg.hdr));
+}
+
+void NanoHub::doSendToApp(const hub_app_name_t *name, uint32_t typ, const void *data, uint32_t len)
+{
+ hub_message_t msg = {
+ .app_name = *name,
+ .message_type = typ,
+ .message_len = len,
+ .message = data,
+ };
+
+ mMsgCbkFunc(0, &msg, mMsgCbkData);
+}
+
+void* NanoHub::run(void *data)
+{
+ NanoHub *self = static_cast<NanoHub*>(data);
+ return self->doRun();
+}
+
+void* NanoHub::doRun()
+{
+ enum {
+ IDX_NANOHUB,
+ IDX_CLOSE_PIPE,
+ IDX_INOTIFY
+ };
+ pollfd myFds[3] = {
+ [IDX_NANOHUB] = { .fd = mFd, .events = POLLIN, },
+ [IDX_CLOSE_PIPE] = { .fd = mThreadClosingPipe[0], .events = POLLIN, },
+ };
+ pollfd &inotifyFd = myFds[IDX_INOTIFY];
+ bool hasInotify = false;
+ int numPollFds = 2;
+
+ if (init_inotify(&inotifyFd)) {
+ numPollFds++;
+ hasInotify = true;
+ }
+
+ setDebugFlags(property_get_int32("persist.nanohub.debug", 0));
+
+ while (1) {
+ int ret = poll(myFds, numPollFds, -1);
+ if (ret <= 0) {
+ ALOGD("poll is being weird");
+ continue;
+ }
+
+ if (hasInotify) {
+ wait_on_dev_lock(inotifyFd);
+ }
+
+ if (myFds[IDX_NANOHUB].revents & POLLIN) { // we have data
+
+ nano_message msg;
+
+ ret = rread(mFd, &msg, sizeof(msg));
+ if (ret <= 0) {
+ ALOGE("read failed with %d", ret);
+ break;
+ }
+ if (ret < (int)sizeof(msg.hdr)) {
+ ALOGE("Only read %d bytes", ret);
+ break;
+ }
+
+ uint32_t len = msg.hdr.len;
+
+ if (len > sizeof(msg.data)) {
+ ALOGE("malformed packet with len %" PRIu32, len);
+ break;
+ }
+
+ if (ret != (int)(sizeof(msg.hdr) + len)) {
+ ALOGE("Expected %zu bytes, read %d bytes", sizeof(msg.hdr) + len, ret);
+ break;
+ }
+
+ ret = SystemComm::handleRx(&msg);
+ if (ret < 0) {
+ ALOGE("SystemComm::handleRx() returned %d", ret);
+ } else if (ret) {
+ if (messageTracingEnabled()) {
+ dumpBuffer("DEV -> APP", msg.hdr.app_name, msg.hdr.event_id, &msg.data[0], msg.hdr.len);
+ }
+ doSendToApp(&msg.hdr.app_name, msg.hdr.event_id, &msg.data[0], msg.hdr.len);
+ }
+ }
+
+ if (myFds[IDX_CLOSE_PIPE].revents & POLLIN) { // we have been asked to die
+ ALOGD("thread exiting");
+ break;
+ }
+ }
+
+ close(mFd);
+ return NULL;
+}
+
+int NanoHub::openHub()
+{
+ int ret = 0;
+
+ mFd = open(get_devnode_path(), O_RDWR);
+ if (mFd < 0) {
+ ALOGE("cannot find hub devnode '%s'", get_devnode_path());
+ ret = -errno;
+ goto fail_open;
+ }
+
+ if (pipe(mThreadClosingPipe)) {
+ ALOGE("failed to create signal pipe");
+ ret = -errno;
+ goto fail_pipe;
+ }
+
+ if (pthread_create(&mWorkerThread, NULL, &NanoHub::run, this)) {
+ ALOGE("failed to spawn worker thread");
+ ret = -errno;
+ goto fail_thread;
+ }
+
+ return 0;
+
+fail_thread:
+ close(mThreadClosingPipe[0]);
+ close(mThreadClosingPipe[1]);
+
+fail_pipe:
+ close(mFd);
+
+fail_open:
+ return ret;
+}
+
+int NanoHub::closeHub(void)
+{
+ char zero = 0;
+
+ //signal
+ while(write(mThreadClosingPipe[1], &zero, 1) != 1);
+
+ //wait
+ (void)pthread_join(mWorkerThread, NULL);
+
+ //cleanup
+ ::close(mThreadClosingPipe[0]);
+ ::close(mThreadClosingPipe[1]);
+
+ reset();
+
+ return 0;
+}
+
+int NanoHub::doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie)
+{
+ if (hub_id) {
+ return -ENODEV;
+ }
+
+ Mutex::Autolock _l(mLock);
+ int ret = 0;
+
+ if (!mMsgCbkFunc && !cbk) { //we're off and staying off - do nothing
+
+ ALOGD("staying off");
+ } else if (cbk && mMsgCbkFunc) { //new callback but staying on
+
+ ALOGD("staying on");
+ } else if (mMsgCbkFunc) { //we were on but turning off
+
+ ALOGD("turning off");
+
+ ret = closeHub();
+ } else if (cbk) { //we're turning on
+
+ ALOGD("turning on");
+ ret = openHub();
+ }
+
+ mMsgCbkFunc = cbk;
+ mMsgCbkData = cookie;
+
+ return ret;
+}
+
+int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg)
+{
+ if (hub_id) {
+ return -ENODEV;
+ }
+
+ int ret = 0;
+ Mutex::Autolock _l(mLock);
+
+ if (!mMsgCbkFunc) {
+ ALOGW("refusing to send a message when nobody around to get a reply!");
+ ret = -EIO;
+ } else {
+ if (!msg || !msg->message) {
+ ALOGW("not sending invalid message 1");
+ ret = -EINVAL;
+ } else if (get_hub_info()->os_app_name == msg->app_name) {
+ //messages to the "system" app are special - hal handles them
+ if (messageTracingEnabled()) {
+ dumpBuffer("APP -> HAL", msg->app_name, msg->message_type, msg->message, msg->message_len);
+ }
+ ret = SystemComm::handleTx(msg);
+ } else if (msg->message_type || msg->message_len > MAX_RX_PACKET) {
+ ALOGW("not sending invalid message 2");
+ ret = -EINVAL;
+ } else {
+ if (messageTracingEnabled()) {
+ dumpBuffer("APP -> DEV", msg->app_name, 0, msg->message, msg->message_len);
+ }
+ ret = doSendToDevice(&msg->app_name, msg->message, msg->message_len);
+ }
+ }
+
+ return ret;
+}
+
+static int hal_get_hubs(context_hub_module_t*, const context_hub_t ** list)
+{
+ *list = get_hub_info();
+
+ return 1; /* we have one hub */
+}
+
+}; // namespace nanohub
+
+}; // namespace android
+
+context_hub_module_t HAL_MODULE_INFO_SYM = {
+ .common = {
+ .tag = HARDWARE_MODULE_TAG,
+ .module_api_version = CONTEXT_HUB_DEVICE_API_VERSION_1_0,
+ .hal_api_version = HARDWARE_HAL_API_VERSION,
+ .id = CONTEXT_HUB_MODULE_ID,
+ .name = "Nanohub HAL",
+ .author = "Google",
+ },
+
+ .get_hubs = android::nanohub::hal_get_hubs,
+ .subscribe_messages = android::nanohub::NanoHub::subscribeMessages,
+ .send_message = android::nanohub::NanoHub::sendToNanohub,
+};
diff --git a/contexthubhal/nanohubhal.h b/contexthubhal/nanohubhal.h
new file mode 100644
index 0000000..bcd291f
--- /dev/null
+++ b/contexthubhal/nanohubhal.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#ifndef _NANOHUB_HAL_H_
+#define _NANOHUB_HAL_H_
+
+#include <pthread.h>
+
+#include <hardware/context_hub.h>
+#include <utils/Mutex.h>
+
+#define NANOAPP_VENDOR_GOOGLE NANOAPP_VENDOR("Googl")
+
+//as per protocol
+#define MAX_RX_PACKET 128
+#define APP_FROM_HOST_EVENT_ID 0x000000F8
+
+namespace android {
+
+namespace nanohub {
+
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status = 0);
+
+struct nano_message_hdr {
+ uint32_t event_id;
+ hub_app_name_t app_name;
+ uint8_t len;
+} __attribute__((packed));
+
+struct nano_message {
+ nano_message_hdr hdr;
+ uint8_t data[MAX_RX_PACKET];
+} __attribute__((packed));
+
+class NanoHub {
+ Mutex mLock;
+ context_hub_callback *mMsgCbkFunc;
+ int mThreadClosingPipe[2];
+ int mFd; // [0] is read end
+ void * mMsgCbkData;
+ pthread_t mWorkerThread;
+
+ NanoHub() {
+ reset();
+ }
+
+ void reset() {
+ mThreadClosingPipe[0] = -1;
+ mThreadClosingPipe[1] = -1;
+ mFd = -1;
+ mMsgCbkData = nullptr;
+ mMsgCbkFunc = nullptr;
+ mWorkerThread = 0;
+ }
+
+ static void* run(void *);
+ void* doRun();
+
+ int openHub();
+ int closeHub();
+
+ static NanoHub *hubInstance() {
+ static NanoHub theHub;
+ return &theHub;
+ }
+
+ int doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie);
+ int doSendToNanohub(uint32_t hub_id, const hub_message_t *msg);
+ int doSendToDevice(const hub_app_name_t *name, const void *data, uint32_t len);
+ void doSendToApp(const hub_app_name_t *name, uint32_t typ, const void *data, uint32_t len);
+
+ static constexpr unsigned int FL_MESSAGE_TRACING = 1;
+
+ unsigned int mFlags = 0;
+
+public:
+
+ // debugging interface
+
+ static bool messageTracingEnabled() {
+ return hubInstance()->mFlags & FL_MESSAGE_TRACING;
+ }
+ static unsigned int getDebugFlags() {
+ return hubInstance()->mFlags;
+ }
+ static void setDebugFlags(unsigned int flags) {
+ hubInstance()->mFlags = flags;
+ }
+
+ // messaging interface
+
+ // define callback to invoke for APP messages
+ static int subscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie) {
+ return hubInstance()->doSubscribeMessages(hub_id, cbk, cookie);
+ }
+ // all messages from APP go here
+ static int sendToNanohub(uint32_t hub_id, const hub_message_t *msg) {
+ return hubInstance()->doSendToNanohub(hub_id, msg);
+ }
+ // passes message to kernel driver directly
+ static int sendToDevice(const hub_app_name_t *name, const void *data, uint32_t len) {
+ return hubInstance()->doSendToDevice(name, data, len);
+ }
+ // passes message to APP via callback
+ static void sendToApp(const hub_app_name_t *name, uint32_t typ, const void *data, uint32_t len) {
+ hubInstance()->doSendToApp(name, typ, data, len);
+ }
+};
+
+}; // namespace nanohub
+
+}; // namespace android
+
+#endif
diff --git a/contexthubhal/nanohubhal_default.cpp b/contexthubhal/nanohubhal_default.cpp
new file mode 100644
index 0000000..6266045
--- /dev/null
+++ b/contexthubhal/nanohubhal_default.cpp
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define LOG_TAG "NanohubHAL"
+#include <hardware/context_hub.h>
+#include "nanohub_perdevice.h"
+#include "nanohubhal.h"
+#include <utils/Log.h>
+
+namespace android {
+
+namespace nanohub {
+
+#define DEVICE "Default"
+#define DEVICE_TAG (DEVICE[0])
+
+static const connected_sensor_t mSensors[] = {
+ {
+ .sensor_id = ((int)DEVICE_TAG << 8) + 1,
+ .physical_sensor = {
+ .name = "i'll get to this later",
+ },
+ },
+ {
+ .sensor_id = ((int)DEVICE_TAG << 8) + 2,
+ .physical_sensor = {
+ .name = "i'll get to this later as well",
+ },
+ },
+};
+
+static const context_hub_t mHub = {
+ .name = "Google System Nanohub on " DEVICE,
+ .vendor = "Google/StMicro",
+ .toolchain = "gcc-arm-none-eabi",
+ .platform_version = 1,
+ .toolchain_version = 0x04080000, //4.8
+ .hub_id = 0,
+
+ .peak_mips = 16,
+ .stopped_power_draw_mw = 0.010 * 1.800,
+ .sleep_power_draw_mw = 0.080 * 1.800,
+ .peak_power_draw_mw = 3.000 * 1.800,
+
+ .connected_sensors = mSensors,
+ .num_connected_sensors = sizeof(mSensors) / sizeof(*mSensors),
+
+ .max_supported_msg_len = MAX_RX_PACKET,
+ .os_app_name = { .id = 0 },
+};
+
+const char *get_devnode_path(void)
+{
+ return "/dev/nanohub_comms";
+}
+
+const context_hub_t* get_hub_info(void)
+{
+ return &mHub;
+}
+
+}; // namespace nanohub
+
+}; // namespace android
diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp
new file mode 100644
index 0000000..a21060d
--- /dev/null
+++ b/contexthubhal/system_comms.cpp
@@ -0,0 +1,573 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define LOG_TAG "NanohubHAL"
+
+#include <cassert>
+#include <cerrno>
+#include <cinttypes>
+
+#include <endian.h>
+
+#include <vector>
+
+#include <utils/Log.h>
+
+#include <endian.h>
+
+#include <hardware/context_hub.h>
+#include "nanohub_perdevice.h"
+#include "system_comms.h"
+#include "nanohubhal.h"
+
+namespace android {
+
+namespace nanohub {
+
+static void readAppName(MessageBuf &buf, hub_app_name_t &name) {
+ name.id = buf.readU64();
+}
+
+static void writeAppName(MessageBuf &buf, const hub_app_name_t &name) {
+ buf.writeU64(name.id);
+}
+
+static void readNanohubAppInfo(MessageBuf &buf, NanohubAppInfo &info) {
+ size_t pos = buf.getPos();
+ readAppName(buf, info.name);
+ info.version = buf.readU32();
+ info.flashUse = buf.readU32();
+ info.ramUse = buf.readU32();
+ if ((buf.getPos() - pos) != sizeof(info)) {
+ ALOGE("%s: failed to read object", __func__);
+ }
+}
+
+static void readNanohubMemInfo(MessageBuf &buf, NanohubMemInfo &mi) {
+ size_t pos = buf.getPos();
+ mi.flashSz = buf.readU32();
+ mi.blSz = buf.readU32();
+ mi.osSz = buf.readU32();
+ mi.sharedSz = buf.readU32();
+ mi.eeSz = buf.readU32();
+ mi.ramSz = buf.readU32();
+
+ mi.blUse = buf.readU32();
+ mi.osUse = buf.readU32();
+ mi.sharedUse = buf.readU32();
+ mi.eeUse = buf.readU32();
+ mi.ramUse = buf.readU32();
+ if ((buf.getPos() - pos) != sizeof(mi)) {
+ ALOGE("%s: failed to read object", __func__);
+ }
+}
+
+NanohubRsp::NanohubRsp(MessageBuf &buf, bool no_status) {
+ // all responses start with command
+ // most of them have 4-byte status (result code)
+ cmd = buf.readU8();
+ if (!buf.getSize()) {
+ status = -EINVAL;
+ } else if (no_status) {
+ status = 0;
+ } else {
+ status = buf.readU32();
+ }
+}
+
+int SystemComm::sendToSystem(const void *data, size_t len) {
+ if (NanoHub::messageTracingEnabled()) {
+ dumpBuffer("HAL -> SYS", getSystem()->mHostIfAppName, 0, data, len);
+ }
+ return NanoHub::sendToDevice(&getSystem()->mHostIfAppName, data, len);
+}
+
+int SystemComm::AppInfoSession::setup(const hub_message_t *) {
+ Mutex::Autolock _l(mLock);
+ int suggestedSize = mAppInfo.size() ? mAppInfo.size() : 20;
+
+ mAppInfo.clear();
+ mAppInfo.reserve(suggestedSize);
+ setState(SESSION_USER);
+
+ return requestNext();
+}
+
+inline hub_app_name_t deviceAppNameToHost(const hub_app_name_t src) {
+ hub_app_name_t res = { .id = le64toh(src.id) };
+ return res;
+}
+
+inline hub_app_name_t hostAppNameToDevice(const hub_app_name_t src) {
+ hub_app_name_t res = { .id = htole64(src.id) };
+ return res;
+}
+
+int SystemComm::AppInfoSession::handleRx(MessageBuf &buf)
+{
+ Mutex::Autolock _l(mLock);
+
+ NanohubRsp rsp(buf, true);
+ if (rsp.cmd != NANOHUB_QUERY_APPS) {
+ return 1;
+ }
+ size_t len = buf.getRoom();
+ if (len != sizeof(NanohubAppInfo) && len) {
+ ALOGE("%s: Invalid data size; have %zu, need %zu", __func__,
+ len, sizeof(NanohubAppInfo));
+ return -EINVAL;
+ }
+ if (getState() != SESSION_USER) {
+ ALOGE("%s: Invalid state; have %d, need %d", __func__, getState(), SESSION_USER);
+ return -EINVAL;
+ }
+ if (len) {
+ NanohubAppInfo info;
+ readNanohubAppInfo(buf, info);
+ hub_app_info appInfo;
+ appInfo.num_mem_ranges = 0;
+ if (info.flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ mem_range_t &range = appInfo.mem_usage[appInfo.num_mem_ranges++];
+ range.type = HUB_MEM_TYPE_MAIN;
+ range.total_bytes = info.flashUse;
+ }
+ if (info.ramUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ mem_range_t &range = appInfo.mem_usage[appInfo.num_mem_ranges++];
+ range.type = HUB_MEM_TYPE_RAM;
+ range.total_bytes = info.ramUse;
+ }
+
+ appInfo.app_name = info.name;
+ appInfo.version = info.version;
+
+ mAppInfo.push_back(appInfo);
+ return requestNext();
+ } else {
+ sendToApp(CONTEXT_HUB_QUERY_APPS,
+ static_cast<const void *>(mAppInfo.data()),
+ mAppInfo.size() * sizeof(mAppInfo[0]));
+ complete();
+ }
+
+ return 0;
+}
+
+int SystemComm::AppInfoSession::requestNext()
+{
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ buf.writeU8(NANOHUB_QUERY_APPS);
+ buf.writeU32(mAppInfo.size());
+ return sendToSystem(buf.getData(), buf.getPos());
+}
+
+int SystemComm::MemInfoSession::setup(const hub_message_t *)
+{
+ Mutex::Autolock _l(mLock);
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ buf.writeU8(NANOHUB_QUERY_MEMINFO);
+
+ setState(SESSION_USER);
+ return sendToSystem(buf.getData(), buf.getPos());
+}
+
+int SystemComm::MemInfoSession::handleRx(MessageBuf &buf)
+{
+ Mutex::Autolock _l(mLock);
+ NanohubRsp rsp(buf, true);
+
+ if (rsp.cmd != NANOHUB_QUERY_MEMINFO)
+ return 1;
+
+ size_t len = buf.getRoom();
+
+ if (len != sizeof(NanohubMemInfo)) {
+ ALOGE("%s: Invalid data size: %zu", __func__, len);
+ return -EINVAL;
+ }
+ if (getState() != SESSION_USER) {
+ ALOGE("%s: Invalid state; have %d, need %d", __func__, getState(), SESSION_USER);
+ return -EINVAL;
+ }
+
+ NanohubMemInfo mi;
+ readNanohubMemInfo(buf, mi);
+ std::vector<mem_range_t> ranges;
+ ranges.reserve(4);
+
+ //if each is valid, copy to output area
+ if (mi.sharedSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.sharedUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_MAIN,
+ .total_bytes = mi.sharedSz,
+ .free_bytes = mi.sharedSz - mi.sharedUse,
+ });
+
+ if (mi.osSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.osUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_OS,
+ .total_bytes = mi.osSz,
+ .free_bytes = mi.osSz - mi.osUse,
+ });
+
+ if (mi.eeSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.eeUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_EEDATA,
+ .total_bytes = mi.eeSz,
+ .free_bytes = mi.eeSz - mi.eeUse,
+ });
+
+ if (mi.ramSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.ramUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_RAM,
+ .total_bytes = mi.ramSz,
+ .free_bytes = mi.ramSz - mi.ramUse,
+ });
+
+ //send it out
+ sendToApp(CONTEXT_HUB_QUERY_MEMORY,
+ static_cast<const void *>(ranges.data()),
+ ranges.size() * sizeof(ranges[0]));
+
+ complete();
+
+ return 0;
+}
+
+int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg)
+{
+ Mutex::Autolock _l(mLock);
+
+ mCmd = appMsg->message_type;
+ mLen = appMsg->message_len;
+ mPos = 0;
+
+ switch (mCmd) {
+ case CONTEXT_HUB_APPS_ENABLE:
+ return setupMgmt(appMsg, NANOHUB_EXT_APPS_ON);
+ case CONTEXT_HUB_APPS_DISABLE:
+ return setupMgmt(appMsg, NANOHUB_EXT_APPS_OFF);
+ case CONTEXT_HUB_UNLOAD_APP:
+ return setupMgmt(appMsg, NANOHUB_EXT_APP_DELETE);
+ case CONTEXT_HUB_LOAD_OS:
+ case CONTEXT_HUB_LOAD_APP:
+ const uint8_t *p = static_cast<const uint8_t*>(appMsg->message);
+ mData.clear();
+ mData = std::vector<uint8_t>(p, p + mLen);
+ setState(TRANSFER);
+
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ buf.writeU8(NANOHUB_START_UPLOAD);
+ buf.writeU8(mCmd == CONTEXT_HUB_LOAD_OS ? 1 : 0);
+ buf.writeU32(mLen);
+
+ return sendToSystem(buf.getData(), buf.getPos());
+ break;
+ }
+
+ return -EINVAL;
+}
+
+int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t cmd)
+{
+ const hub_app_name_t &appName = *static_cast<const hub_app_name_t*>(appMsg->message);
+ if (appMsg->message_len != sizeof(appName)) {
+ return -EINVAL;
+ }
+
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ buf.writeU8(cmd);
+ writeAppName(buf, appName);
+ setState(MGMT);
+
+ return sendToSystem(buf.getData(), buf.getPos());
+}
+
+int SystemComm::AppMgmtSession::handleRx(MessageBuf &buf)
+{
+ int ret = 0;
+ Mutex::Autolock _l(mLock);
+ NanohubRsp rsp(buf);
+
+ switch (getState()) {
+ case TRANSFER:
+ ret = handleTransfer(rsp);
+ break;
+ case FINISH:
+ ret = handleFinish(rsp);
+ break;
+ case RELOAD:
+ ret = handleReload(rsp);
+ break;
+ case MGMT:
+ ret = handleMgmt(rsp);
+ break;
+ }
+
+ return ret;
+}
+
+int SystemComm::AppMgmtSession::handleTransfer(NanohubRsp &rsp)
+{
+ if (rsp.cmd != NANOHUB_CONT_UPLOAD && rsp.cmd != NANOHUB_START_UPLOAD)
+ return 1;
+
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+
+ static_assert(NANOHUB_UPLOAD_CHUNK_SZ_MAX <= (MAX_RX_PACKET-5),
+ "Invalid chunk size");
+
+ if (mPos < mLen) {
+ uint32_t chunkSize = mLen - mPos;
+
+ if (chunkSize > NANOHUB_UPLOAD_CHUNK_SZ_MAX) {
+ chunkSize = NANOHUB_UPLOAD_CHUNK_SZ_MAX;
+ }
+
+ buf.writeU8(NANOHUB_CONT_UPLOAD);
+ buf.writeU32(mPos);
+ buf.writeRaw(&mData[mPos], chunkSize);
+ mPos += chunkSize;
+ } else {
+ buf.writeU8(NANOHUB_FINISH_UPLOAD);
+ setState(FINISH);
+ }
+
+ return sendToSystem(buf.getData(), buf.getPos());
+}
+
+int SystemComm::AppMgmtSession::handleFinish(NanohubRsp &rsp)
+{
+ if (rsp.cmd != NANOHUB_FINISH_UPLOAD)
+ return 1;
+
+ int ret = 0;
+ const bool success = rsp.status != 0;
+ mData.clear();
+
+ if (success) {
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ // until app header is passed, we don't know who to start, so we reboot
+ buf.writeU8(NANOHUB_REBOOT);
+ setState(RELOAD);
+ ret = sendToSystem(buf.getData(), buf.getPos());
+ } else {
+ int32_t result = NANOHUB_APP_NOT_LOADED;
+
+ sendToApp(mCmd, &result, sizeof(result));
+ complete();
+ }
+
+ return ret;
+}
+
+/* reboot notification is not yet supported in FW; this code is for (near) future */
+int SystemComm::AppMgmtSession::handleReload(NanohubRsp &rsp)
+{
+ int32_t result = NANOHUB_APP_LOADED;
+
+ ALOGI("Nanohub reboot status: %08" PRIX32, rsp.status);
+
+ sendToApp(mCmd, &result, sizeof(result));
+ complete();
+
+ return 0;
+}
+
+int SystemComm::AppMgmtSession::handleMgmt(NanohubRsp &rsp)
+{
+ Mutex::Autolock _l(mLock);
+ bool valid = false;
+
+ ALOGI("Nanohub MGMT response: CMD=%02X; STATUS=%08" PRIX32, rsp.cmd, rsp.status);
+
+ switch (rsp.cmd) {
+ case NANOHUB_EXT_APPS_OFF:
+ valid = mCmd == CONTEXT_HUB_APPS_DISABLE;
+ break;
+ case NANOHUB_EXT_APPS_ON:
+ valid = mCmd == CONTEXT_HUB_APPS_ENABLE;
+ break;
+ case NANOHUB_EXT_APP_DELETE:
+ valid = mCmd == CONTEXT_HUB_UNLOAD_APP;
+ break;
+ default:
+ return 1;
+ }
+
+ if (!valid) {
+ ALOGE("Invalid response for this state: APP CMD=%02X", mCmd);
+ return -EINVAL;
+ }
+
+ sendToApp(mCmd, &rsp.status, sizeof(rsp.status));
+ complete();
+
+ return 0;
+}
+
+int SystemComm::KeyInfoSession::setup(const hub_message_t *) {
+ Mutex::Autolock _l(mLock);
+ mRsaKeyData.clear();
+ setState(SESSION_USER);
+ mStatus = -EBUSY;
+ return requestRsaKeys();
+}
+
+int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf)
+{
+ Mutex::Autolock _l(mLock);
+ NanohubRsp rsp(buf, true);
+
+ if (getState() != SESSION_USER) {
+ // invalid state
+ mStatus = -EFAULT;
+ return mStatus;
+ }
+
+ if (buf.getRoom()) {
+ mRsaKeyData.insert(mRsaKeyData.end(),
+ buf.getData() + buf.getPos(),
+ buf.getData() + buf.getSize());
+ return requestRsaKeys();
+ } else {
+ mStatus = 0;
+ complete();
+ return 0;
+ }
+}
+
+int SystemComm::KeyInfoSession::requestRsaKeys(void)
+{
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+
+ buf.writeU8(NANOHUB_QUERY_APPS);
+ buf.writeU32(mRsaKeyData.size());
+
+ return sendToSystem(buf.getData(), buf.getPos());
+}
+
+int SystemComm::doHandleRx(const nano_message *msg)
+{
+ //we only care for messages from HostIF
+ if (msg->hdr.app_name != mHostIfAppName)
+ return 1;
+
+ //they must all be at least 1 byte long
+ if (!msg->hdr.len) {
+ return -EINVAL;
+ }
+ MessageBuf buf(reinterpret_cast<const char*>(msg->data), msg->hdr.len);
+ if (NanoHub::messageTracingEnabled()) {
+ dumpBuffer("SYS -> HAL", mHostIfAppName, 0, buf.getData(), buf.getSize());
+ }
+ int status = mSessions.handleRx(buf);
+ if (status) {
+ // provide default handler for any system message, that is not properly handled
+ dumpBuffer(status > 0 ? "HAL (not handled)" : "HAL (error)",
+ mHostIfAppName, 0, buf.getData(), buf.getSize(), status);
+ status = status > 0 ? 0 : status;
+ }
+
+ return status;
+}
+
+int SystemComm::SessionManager::handleRx(MessageBuf &buf)
+{
+ int status = 1;
+
+ // pass message to all active sessions, in arbitrary order
+ // 1st session that handles the message terminates the loop
+ for (auto pos = sessions_.begin();
+ pos != sessions_.end() && status > 0; next(pos)) {
+ Session *session = pos->second;
+ status = session->handleRx(buf);
+ if (status < 0) {
+ session->complete();
+ }
+ }
+
+ return status;
+}
+
+int SystemComm::doHandleTx(const hub_message_t *appMsg)
+{
+ int status = 0;
+
+ switch (appMsg->message_type) {
+ case CONTEXT_HUB_LOAD_APP:
+ if (!mKeySession.haveKeys()) {
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mKeySession, appMsg);
+ if (status < 0) {
+ break;
+ }
+ mKeySession.wait();
+ status = mKeySession.getStatus();
+ if (status < 0) {
+ break;
+ }
+ }
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg);
+ break;
+ case CONTEXT_HUB_APPS_ENABLE:
+ case CONTEXT_HUB_APPS_DISABLE:
+ case CONTEXT_HUB_UNLOAD_APP:
+ // all APP-modifying commands share session key, to ensure they can't happen at the same time
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg);
+ break;
+
+ case CONTEXT_HUB_QUERY_APPS:
+ status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_APPS, &mAppInfoSession, appMsg);
+ break;
+
+ case CONTEXT_HUB_QUERY_MEMORY:
+ status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_MEMORY, &mMemInfoSession, appMsg);
+ break;
+
+ default:
+ ALOGW("Unknown os message type %u\n", appMsg->message_type);
+ return -EINVAL;
+ }
+
+ return status;
+}
+
+}; // namespace nanohub
+
+}; // namespace android
diff --git a/contexthubhal/system_comms.h b/contexthubhal/system_comms.h
new file mode 100644
index 0000000..e765040
--- /dev/null
+++ b/contexthubhal/system_comms.h
@@ -0,0 +1,318 @@
+/*
+ * Copyright (c) 2016, Google. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#ifndef _NANOHUB_SYSTEM_COMMS_H_
+#define _NANOHUB_SYSTEM_COMMS_H_
+
+#include <utils/Condition.h>
+#include <utils/Mutex.h>
+
+#include <map>
+#include <vector>
+
+#include <hardware/context_hub.h>
+#include "nanohubhal.h"
+#include "message_buf.h"
+
+//rx: return 0 if handled, > 0 if not handled, < 0 if error happened
+
+#define MSG_HANDLED 0
+
+//messages to the HostIf nanoapp & their replies (mesages and replies both begin with u8 message_type)
+#define NANOHUB_EXT_APPS_ON 0 // () -> (char success)
+#define NANOHUB_EXT_APPS_OFF 1 // () -> (char success)
+#define NANOHUB_EXT_APP_DELETE 2 // (u64 name) -> (char success) //idempotent
+#define NANOHUB_QUERY_MEMINFO 3 // () -> (mem_info)
+#define NANOHUB_QUERY_APPS 4 // (u32 idxStart) -> (app_info[idxStart] OR EMPTY IF NO MORE)
+#define NANOHUB_QUERY_RSA_KEYS 5 // (u32 byteOffset) -> (u8 data[1 or more bytes] OR EMPTY IF NO MORE)
+#define NANOHUB_START_UPLOAD 6 // (char isOs, u32 totalLenToTx) -> (char success)
+#define NANOHUB_CONT_UPLOAD 7 // (u32 offset, u8 data[]) -> (char success)
+#define NANOHUB_FINISH_UPLOAD 8 // () -> (char success)
+#define NANOHUB_REBOOT 9 // () -> (char success)
+
+// Custom defined private messages
+#define CONTEXT_HUB_LOAD_OS (CONTEXT_HUB_TYPE_PRIVATE_MSG_BASE + 1)
+
+
+#define NANOHUB_APP_NOT_LOADED (-1)
+#define NANOHUB_APP_LOADED (0)
+
+#define NANOHUB_UPLOAD_CHUNK_SZ_MAX 64
+#define NANOHUB_MEM_SZ_UNKNOWN 0xFFFFFFFFUL
+
+namespace android {
+
+namespace nanohub {
+
+int system_comms_handle_rx(const nano_message *msg);
+int system_comms_handle_tx(const hub_message_t *outMsg);
+
+struct NanohubAppInfo {
+ hub_app_name_t name;
+ uint32_t version, flashUse, ramUse;
+} __attribute__((packed));
+
+struct NanohubMemInfo {
+ //sizes
+ uint32_t flashSz, blSz, osSz, sharedSz, eeSz;
+ uint32_t ramSz;
+
+ //use
+ uint32_t blUse, osUse, sharedUse, eeUse;
+ uint32_t ramUse;
+} __attribute__((packed));
+
+struct NanohubRsp {
+ uint32_t cmd;
+ int32_t status;
+ NanohubRsp(MessageBuf &buf, bool no_status = false);
+};
+
+inline bool operator == (const hub_app_name_t &a, const hub_app_name_t &b) {
+ return a.id == b.id;
+}
+
+inline bool operator != (const hub_app_name_t &a, const hub_app_name_t &b) {
+ return !(a == b);
+}
+
+class SystemComm {
+private:
+
+ /*
+ * Nanohub HAL sessions
+ *
+ * Session is an object that can group several message exchanges with FW,
+ * maintain state, and be waited for completion by someone else.
+ *
+ * As of this moment, since all sessions are triggered by client thread,
+ * and all the exchange is happening in local worker thread, it is only possible
+ * for client thread to wait on session completion.
+ * Allowing sessions to wait on each other will require a worker thread pool.
+ * It is now unnecessary, and not implemented.
+ */
+ class ISession {
+ public:
+ virtual int setup(const hub_message_t *app_msg) = 0;
+ virtual int handleRx(MessageBuf &buf) = 0;
+ virtual int getState() const = 0; // FSM state
+ virtual int getStatus() const = 0; // execution status (result code)
+ virtual ~ISession() {}
+ };
+
+ class SessionManager;
+
+ class Session : public ISession {
+ friend class SessionManager;
+
+ mutable Mutex mDoneLock; // controls condition and state transitions
+ Condition mDoneWait;
+ volatile int mState;
+
+ protected:
+ mutable Mutex mLock; // serializes message handling
+ int32_t mStatus;
+
+ enum {
+ SESSION_INIT = 0,
+ SESSION_DONE = 1,
+ SESSION_USER = 2,
+ };
+
+ void complete() {
+ Mutex::Autolock _l(mDoneLock);
+ if (mState != SESSION_DONE) {
+ mState = SESSION_DONE;
+ mDoneWait.broadcast();
+ }
+ }
+ void setState(int state) {
+ if (state == SESSION_DONE) {
+ complete();
+ } else {
+ Mutex::Autolock _l(mDoneLock);
+ mState = state;
+ }
+ }
+ public:
+ Session() { mState = SESSION_INIT; mStatus = -1; }
+ int getStatus() const {
+ Mutex::Autolock _l(mLock);
+ return mStatus;
+ }
+ int wait() {
+ Mutex::Autolock _l(mDoneLock);
+ while (mState != SESSION_DONE) {
+ mDoneWait.wait(mDoneLock);
+ }
+ return 0;
+ }
+ virtual int getState() const override {
+ Mutex::Autolock _l(mDoneLock);
+ return mState;
+ }
+ virtual bool isDone() const {
+ Mutex::Autolock _l(mDoneLock);
+ return mState == SESSION_DONE;
+ }
+ virtual bool isRunning() const {
+ Mutex::Autolock _l(mDoneLock);
+ return mState > SESSION_DONE;
+ }
+ };
+
+ class AppMgmtSession : public Session {
+ enum {
+ TRANSFER = SESSION_USER,
+ FINISH,
+ RELOAD,
+ MGMT,
+ };
+ uint32_t mCmd; // UPLOAD_APP | UPPLOAD_OS
+ uint32_t mResult;
+ std::vector<uint8_t> mData;
+ uint32_t mLen;
+ uint32_t mPos;
+
+ int setupMgmt(const hub_message_t *appMsg, uint32_t cmd);
+ int handleTransfer(NanohubRsp &rsp);
+ int handleFinish(NanohubRsp &rsp);
+ int handleReload(NanohubRsp &rsp);
+ int handleMgmt(NanohubRsp &rsp);
+ public:
+ AppMgmtSession() {
+ mCmd = 0;
+ mResult = 0;
+ mPos = 0;
+ mLen = 0;
+ }
+ virtual int handleRx(MessageBuf &buf) override;
+ virtual int setup(const hub_message_t *app_msg) override;
+ };
+
+ class MemInfoSession : public Session {
+ public:
+ virtual int setup(const hub_message_t *app_msg) override;
+ virtual int handleRx(MessageBuf &buf) override;
+ };
+
+ class KeyInfoSession : public Session {
+ std::vector<uint8_t> mRsaKeyData;
+ int requestRsaKeys(void);
+ public:
+ virtual int setup(const hub_message_t *) override;
+ virtual int handleRx(MessageBuf &buf) override;
+ bool haveKeys() const {
+ Mutex::Autolock _l(mLock);
+ return mRsaKeyData.size() > 0 && !isRunning();
+ }
+ };
+
+ class AppInfoSession : public Session {
+ std::vector<hub_app_info> mAppInfo;
+ int requestNext();
+ public:
+ virtual int setup(const hub_message_t *) override;
+ virtual int handleRx(MessageBuf &buf) override;
+ };
+
+ class SessionManager {
+ typedef std::map<int, Session* > SessionMap;
+
+ Mutex lock;
+ SessionMap sessions_;
+
+ void next(SessionMap::iterator &pos)
+ {
+ Mutex::Autolock _l(lock);
+ pos->second->isDone() ? pos = sessions_.erase(pos) : ++pos;
+ }
+
+ public:
+ int handleRx(MessageBuf &buf);
+ int setup_and_add(int id, Session *session, const hub_message_t *appMsg) {
+ Mutex::Autolock _l(lock);
+ if (sessions_.count(id) == 0 && !session->isRunning()) {
+ int ret = session->setup(appMsg);
+ if (ret < 0) {
+ session->complete();
+ } else {
+ sessions_[id] = session;
+ }
+ return ret;
+ }
+ return -EBUSY;
+ }
+
+ } mSessions;
+
+ const hub_app_name_t mHostIfAppName = {
+ .id = NANO_APP_ID(NANOAPP_VENDOR_GOOGLE, 0)
+ };
+
+ static SystemComm *getSystem() {
+ // this is thread-safe in c++11
+ static SystemComm theInstance;
+ return &theInstance;
+ }
+
+ SystemComm () = default;
+ ~SystemComm() = default;
+
+ int doHandleTx(const hub_message_t *txMsg);
+ int doHandleRx(const nano_message *rxMsg);
+
+ static void sendToApp(uint32_t typ, const void *data, uint32_t len) {
+ if (NanoHub::messageTracingEnabled()) {
+ dumpBuffer("HAL -> APP", get_hub_info()->os_app_name, typ, data, len);
+ }
+ NanoHub::sendToApp(&get_hub_info()->os_app_name, typ, data, len);
+ }
+ static int sendToSystem(const void *data, size_t len);
+
+ KeyInfoSession mKeySession;
+ AppMgmtSession mAppMgmtSession;
+ AppInfoSession mAppInfoSession;
+ MemInfoSession mMemInfoSession;
+
+public:
+ static int handleTx(const hub_message_t *txMsg) {
+ return getSystem()->doHandleTx(txMsg);
+ }
+ static int handleRx(const nano_message *rxMsg) {
+ return getSystem()->doHandleRx(rxMsg);
+ }
+};
+
+}; // namespace nanohub
+
+}; // namespace android
+
+#endif
diff --git a/firmware/Makefile b/firmware/Makefile
index 16bf169..86cfa2d 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -53,7 +53,7 @@
include $(MAKE_CPU)
include $(MAKE_VAR)
-FLAGS += -Wall -Werror -Iinc -Ilinks -Iexternal/freebsd/inc -fshort-double
+FLAGS += -Wall -Werror -Iinc -Ilinks -Iexternal/freebsd/inc -I../lib/include -fshort-double
#help avoid commmon embedded C mistakes
FLAGS += -Wmissing-declarations -Wlogical-op -Waddress -Wempty-body -Wpointer-arith -Wenum-compare -Wdouble-promotion -Wfloat-equal -Wshadow -fno-strict-aliasing
@@ -64,7 +64,7 @@
FLAGS += $(DEBUG)
#bootloader pieces
-SRCS_bl += src/sha2.c src/rsa.c src/aes.c src/seos.c
+SRCS_bl += ../lib/nanohub/sha2.c ../lib/nanohub/rsa.c ../lib/nanohub/aes.c src/seos.c
#frameworks
SRCS_os += src/printf.c src/timer.c src/seos.c src/heap.c src/slab.c src/spi.c src/trylock.c
@@ -75,7 +75,7 @@
SRCS_bl += src/printf.c
ifndef PLATFORM_HAS_HARDWARE_CRC
-SRCS_os += src/softcrc.c
+SRCS_os += ../lib/nanohub/softcrc.c
endif
#app code
diff --git a/firmware/app/common.mk b/firmware/app/common.mk
index fa2cfe4..81a9624 100644
--- a/firmware/app/common.mk
+++ b/firmware/app/common.mk
@@ -26,7 +26,7 @@
define APPRULE
$(APP_APP): $(APP_BIN)
- nanoapp_postprocess -v $(APP_ID) < $(APP_BIN) > $(APP_APP)
+ nanoapp_postprocess -v -a $(APP_ID) $(APP_BIN) $(APP_APP)
$(APP_BIN): $(APP_ELF)
$(OBJCOPY) -j.relocs -j.flash -j.data -j.dynsym -O binary $(APP_ELF) $(APP_BIN)
diff --git a/firmware/app/test0.app/test_app0.c b/firmware/app/test0.app/test_app0.c
index cd60033..dd11b05 100644
--- a/firmware/app/test0.app/test_app0.c
+++ b/firmware/app/test0.app/test_app0.c
@@ -34,7 +34,7 @@
static bool start_task(uint32_t myTid)
{
mMyTid = myTid;
- cnt = 5;
+ cnt = 100;
return eOsEventSubscribe(myTid, EVT_APP_START);
}
@@ -74,8 +74,3 @@
}
APP_INIT(0, start_task, end_task, handle_event);
-
-
-
-
-
diff --git a/firmware/inc/appSec.h b/firmware/inc/appSec.h
index 3865e75..8847a2a 100644
--- a/firmware/inc/appSec.h
+++ b/firmware/inc/appSec.h
@@ -30,18 +30,19 @@
typedef AppSecErr (*AppSecGetAesKeyCbk)(uint64_t keyIdx, void *keyBuf); // return APP_SEC_KEY_NOT_FOUND or APP_SEC_NO_ERROR
//return values
-#define APP_SEC_NO_ERROR 0 //all went ok
-#define APP_SEC_NEED_MORE_TIME 1 //please call appSecDoSomeProcessing().
-#define APP_SEC_KEY_NOT_FOUND 2 //we did not find the encr key
-#define APP_SEC_HEADER_ERROR 3 //data (decrypted or input) has no recognizable header
-#define APP_SEC_TOO_MUCH_DATA 4 //we got more data than expected
-#define APP_SEC_TOO_LITTLE_DATA 5 //we got less data than expected
-#define APP_SEC_SIG_VERIFY_FAIL 6 //some signature verification failed
-#define APP_SEC_SIG_DECODE_FAIL 7 //some signature verification failed
-#define APP_SEC_SIG_ROOT_UNKNOWN 8 //signatures all verified but the referenced root of trust is unknown
-#define APP_SEC_MEMORY_ERROR 9 //we ran out of memory while doing things
-#define APP_SEC_INVALID_DATA 10 //data is invalid in some way not described by other error messages
-#define APP_SEC_BAD 11 //something irrecoverably bad happened and we gave up. Sorry...
+#define APP_SEC_NO_ERROR 0 //all went ok
+#define APP_SEC_NEED_MORE_TIME 1 //please call appSecDoSomeProcessing().
+#define APP_SEC_KEY_NOT_FOUND 2 //we did not find the encr key
+#define APP_SEC_HEADER_ERROR 3 //data (decrypted or input) has no recognizable header
+#define APP_SEC_TOO_MUCH_DATA 4 //we got more data than expected
+#define APP_SEC_TOO_LITTLE_DATA 5 //we got less data than expected
+#define APP_SEC_SIG_VERIFY_FAIL 6 //some signature verification failed
+#define APP_SEC_SIG_DECODE_FAIL 7 //some signature verification failed
+#define APP_SEC_SIG_ROOT_UNKNOWN 8 //signatures all verified but the referenced root of trust is unknown
+#define APP_SEC_MEMORY_ERROR 9 //we ran out of memory while doing things
+#define APP_SEC_INVALID_DATA 10 //data is invalid in some way not described by other error messages
+#define APP_SEC_VERIFY_FAILED 11 //decrypted data verification failed
+#define APP_SEC_BAD 127 //something irrecoverably bad happened and we gave up. Sorry...
//init/deinit
struct AppSecState *appSecInit(AppSecWriteCbk writeCbk, AppSecPubKeyFindCbk pubKeyFindCbk, AppSecGetAesKeyCbk aesKeyAccessCbk, bool mandateSigning);
@@ -52,8 +53,4 @@
AppSecErr appSecDoSomeProcessing(struct AppSecState *state); //caleed when any appSec function returns APP_SEC_NEED_MORE_TIME
AppSecErr appSecRxDataOver(struct AppSecState *state); //caleed when there is no more data
-
-
-
#endif
-
diff --git a/firmware/inc/atomic.h b/firmware/inc/atomic.h
index 37c399c..e90ca1c 100644
--- a/firmware/inc/atomic.h
+++ b/firmware/inc/atomic.h
@@ -32,7 +32,8 @@
bool atomicCmpXchg32bits(volatile uint32_t *word, uint32_t prevVal, uint32_t newVal);
//returns old value
-uint32_t atomicAdd(volatile uint32_t *val, uint32_t addend);
+uint32_t atomicAddByte(volatile uint8_t *byte, uint32_t addend);
+uint32_t atomicAdd32bits(volatile uint32_t *word, uint32_t addend);
//writes with barriers
static inline uint32_t atomicReadByte(volatile uint8_t *byte)
diff --git a/firmware/inc/eeData.h b/firmware/inc/eeData.h
index 64b8185..9b313ee 100644
--- a/firmware/inc/eeData.h
+++ b/firmware/inc/eeData.h
@@ -45,9 +45,9 @@
bool eeDataGet(uint32_t name, void *buf, uint32_t *szP);
bool eeDataSet(uint32_t name, const void *buf, uint32_t len);
-//allow getting old "versions". Set state to NULL initially, call till you get false
-bool eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP);
-bool eeDataEraseOldVersion(uint32_t name, void *state); //state == state BEFORE call to eeDataGetAllVersions that found the version you want gone
+//allow getting old "versions". Set state to NULL initially, call till you get NULL as return value
+void *eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP);
+bool eeDataEraseOldVersion(uint32_t name, void *addr); // addr is non-NULL address returned by call to eeDataGetAllVersions
//predefined key types
diff --git a/firmware/inc/eventQ.h b/firmware/inc/eventQ.h
index f6f86a6..61a128b 100644
--- a/firmware/inc/eventQ.h
+++ b/firmware/inc/eventQ.h
@@ -22,7 +22,8 @@
#include <stdint.h>
-#define EVENT_TYPE_BIT_DISCARDABLE 0x80000000 /* set for events we can afford to lose */
+#define EVENT_TYPE_BIT_DISCARDABLE_COMPAT 0x80000000 /* some external apps are using this one */
+#define EVENT_TYPE_BIT_DISCARDABLE 0x8000 /* set for events we can afford to lose */
struct EvtQueue;
diff --git a/firmware/inc/eventnums.h b/firmware/inc/eventnums.h
index 50ab4f6..4ee4975 100644
--- a/firmware/inc/eventnums.h
+++ b/firmware/inc/eventnums.h
@@ -21,6 +21,7 @@
#include "toolchain.h"
/* These define ranges of reserved events */
+// local events are 16-bit always
#define EVT_NO_FIRST_USER_EVENT 0x00000100 //all events lower than this are reserved for the OS. all of them are nondiscardable necessarily!
#define EVT_NO_FIRST_SENSOR_EVENT 0x00000200 //sensor type SENSOR_TYPE_x produces events of type EVT_NO_FIRST_SENSOR_EVENT + SENSOR_TYPE_x for all Google-defined sensors
#define EVT_NO_SENSOR_CONFIG_EVENT 0x00000300 //event to configure sensors
@@ -28,6 +29,18 @@
#define EVT_APP_TO_HOST 0x00000401 //app data to host. Type is struct HostHubRawPacket
#define EVT_MARSHALLED_SENSOR_DATA 0x00000402 //marshalled event data. Type is MarshalledUserEventData
#define EVT_RESET_REASON 0x00000403 //reset reason to host.
+#define EVT_DEBUG_LOG 0x00007F01 // send message payload to Linux kernel log
+#define EVT_MASK 0x0000FFFF
+
+// host-side events are 32-bit
+
+// DEBUG_LOG_EVT is normally undefined, or defined with a special value, recognized by nanohub driver: 0x3B474F4C
+// if defined with this value, the log message payload will appear in Linux kernel message log.
+// If defined with other value, it will still be sent to nanohub driver, and then forwarded to userland
+// verbatim, where it could be logged by nanohub HAL (by turning on it's logging via 'setprop persist.nanohub.debug 1'
+#ifdef DEBUG_LOG_EVT
+#define HOST_EVT_DEBUG_LOG DEBUG_LOG_EVT
+#endif
#define HOST_HUB_RAW_PACKET_MAX_LEN 128
@@ -80,7 +93,15 @@
//for all apps
#define EVT_APP_FREE_EVT_DATA 0x000000FF //sent to an external app when its event has been marked for freeing. Data: struct AppEventFreeData
-
+// this event is never enqueued; it goes directly to the app.
+// It notifies app that hav outstanding IO, that is is about to end;
+// Expected app behavior is to not send any more events to system;
+// any events sent after this point will be silently ignored by the system;
+// any outstading events will be allowed to proceed to completion. (this is SIG_STOP)
+#define EVT_APP_STOP 0x000000FE
+// Internal event, with task pointer as event data;
+// system ends the task unconditionally; no further checks performed (this is SIG_KILL)
+#define EVT_APP_END 0x000000FD
//for host comms
#define EVT_APP_FROM_HOST 0x000000F8 //host data to an app. Type is struct HostHubRawPacket
diff --git a/firmware/inc/heap.h b/firmware/inc/heap.h
index 417dac0..c5bea59 100644
--- a/firmware/inc/heap.h
+++ b/firmware/inc/heap.h
@@ -14,28 +14,29 @@
* limitations under the License.
*/
-#ifndef _HEAP_H_
-#define _HEAP_H_
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>
-#include <stdbool.h>
-
-
-
-
-bool heapInit(void);
-void* heapAlloc(uint32_t sz);
-void heapFree(void* ptr);
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif
-
+#ifndef _HEAP_H_
+#define _HEAP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+
+
+bool heapInit(void);
+void* heapAlloc(uint32_t sz);
+void heapFree(void* ptr);
+int heapFreeAll(uint32_t tid);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif
+
diff --git a/firmware/inc/isr.h b/firmware/inc/isr.h
index 8b21bf8..2ce9d2c 100644
--- a/firmware/inc/isr.h
+++ b/firmware/inc/isr.h
@@ -23,6 +23,7 @@
#include <cpu.h>
#include <list.h>
#include <util.h>
+#include <seos.h>
struct ChainedInterrupt {
link_t isrs;
@@ -34,6 +35,7 @@
struct ChainedIsr {
link_t node;
bool (*func)(struct ChainedIsr *);
+ uint16_t tid;
};
static inline void chainIsr(struct ChainedInterrupt *interrupt, struct ChainedIsr *isr)
@@ -46,6 +48,7 @@
static inline void unchainIsr(struct ChainedInterrupt *interrupt, struct ChainedIsr *isr)
{
interrupt->disable(interrupt);
+ isr->tid = 0;
list_delete(&isr->node);
if (!list_is_empty(&interrupt->isrs))
interrupt->enable(interrupt);
@@ -55,13 +58,34 @@
{
struct link_t *cur, *tmp;
bool handled = false;
+ uint16_t oldTid = osGetCurrentTid();
list_iterate(&interrupt->isrs, cur, tmp) {
struct ChainedIsr *curIsr = container_of(cur, struct ChainedIsr, node);
- handled = handled || curIsr->func(curIsr);
+ osSetCurrentTid(curIsr->tid);
+ handled = curIsr->func(curIsr);
+ if (handled)
+ break;
}
+ osSetCurrentTid(oldTid);
return handled;
}
+static inline int unchainIsrAll(struct ChainedInterrupt *interrupt, uint32_t tid)
+{
+ int count = 0;
+ struct link_t *cur, *tmp;
+
+ list_iterate(&interrupt->isrs, cur, tmp) {
+ struct ChainedIsr *curIsr = container_of(cur, struct ChainedIsr, node);
+ if (curIsr->tid == tid) {
+ unchainIsr(interrupt, curIsr);
+ count++;
+ }
+ }
+
+ return count;
+}
+
#endif /* __ISR_H */
diff --git a/firmware/inc/nanohubPacket.h b/firmware/inc/nanohubPacket.h
index ee27a8d..6b6afda 100644
--- a/firmware/inc/nanohubPacket.h
+++ b/firmware/inc/nanohubPacket.h
@@ -195,6 +195,7 @@
NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_ROOT_UNKNOWN,
NANOHUB_FIRMWARE_UPLOAD_APP_SEC_MEMORY_ERROR,
NANOHUB_FIRMWARE_UPLOAD_APP_SEC_INVALID_DATA,
+ NANOHUB_FIRMWARE_UPLOAD_APP_SEC_VERIFY_FAILED,
NANOHUB_FIRMWARE_UPLOAD_APP_SEC_BAD,
};
@@ -287,6 +288,41 @@
#define NANOHUB_HAL_EXT_APPS_ON 0
#define NANOHUB_HAL_EXT_APPS_OFF 1
#define NANOHUB_HAL_EXT_APP_DELETE 2
+
+// this behaves more stable w.r.t. endianness than bit field
+// this is setting byte fields in MgmtStatus response
+// the high-order bit, if set, is indication of counter overflow
+#define SET_COUNTER(counter, val) (counter = (val & 0x7F) | (val > 0x7F ? 0x80 : 0))
+
+SET_PACKED_STRUCT_MODE_ON
+struct MgmtStatus {
+ union {
+ __le32 value;
+ // NOTE: union fields are accessed in CPU native mode
+ struct {
+ uint8_t app;
+ uint8_t task;
+ uint8_t op;
+ uint8_t erase;
+ } ATTRIBUTE_PACKED;
+ };
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalMgmtRx {
+ __le64 appId;
+ struct MgmtStatus stat;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalMgmtTx {
+ struct NanohubHalHdr hdr;
+ __le32 status;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
#define NANOHUB_HAL_QUERY_MEMINFO 3
#define NANOHUB_HAL_QUERY_APPS 4
diff --git a/firmware/inc/platform.h b/firmware/inc/platform.h
index d60154b..e7a7571 100644
--- a/firmware/inc/platform.h
+++ b/firmware/inc/platform.h
@@ -37,6 +37,9 @@
void platUninitialize(void);
void platReset(void);
+// free all platform-specific resources for TID, and return non-zero status if some cleanup was done
+uint32_t platFreeResources(uint32_t tid);
+
/* Logging */
void *platLogAllocUserData();
void platLogFlush(void *userData);
diff --git a/firmware/inc/platform/stm32f4xx/bl.h b/firmware/inc/platform/stm32f4xx/bl.h
index 8b39938..34f08bd 100644
--- a/firmware/inc/platform/stm32f4xx/bl.h
+++ b/firmware/inc/platform/stm32f4xx/bl.h
@@ -31,26 +31,18 @@
struct AesSetupTempWorksSpace;
struct AesCbcContext;
+#define OS_UPDT_SUCCESS 0
+#define OS_UPDT_HDR_CHECK_FAILED 1
+#define OS_UPDT_HDR_MARKER_INVALID 2
+#define OS_UPDT_UNKNOWN_PUBKEY 3
+#define OS_UPDT_INVALID_SIGNATURE 4
+#define OS_UPDT_INVALID_SIGNATURE_HASH 5
-#define OS_UPDT_MARKER_INPROGRESS 0xFF
-#define OS_UPDT_MARKER_DOWNLOADED 0xFE
-#define OS_UPDT_MARKER_VERIFIED 0xF0
-#define OS_UPDT_MARKER_INVALID 0x00
-#define OS_UPDT_MAGIC "Nanohub OS" //11 bytes incl terminator
-
-struct OsUpdateHdr {
- char magic[11];
- uint8_t marker; //OS_UPDT_MARKER_INPROGRESS -> OS_UPDT_MARKER_DOWNLOADED -> OS_UPDT_MARKER_VERIFIED / OS_UPDT_INVALID
- uint32_t size; //does not include the mandatory signature (using device key) that follows
-};
+#define BL_SCAN_OFFSET 0x00000100
#define BL_VERSION_1 1
#define BL_VERSION_CUR BL_VERSION_1
-#define BL_FLASH_KERNEL_ID 0x1
-#define BL_FLASH_EEDATA_ID 0x2
-#define BL_FLASH_APP_ID 0x4
-
#define BL_FLASH_KEY1 0x45670123
#define BL_FLASH_KEY2 0xCDEF89AB
@@ -95,6 +87,9 @@
void (*blAesCbcEncr)(struct AesCbcContext *ctx, const uint32_t *src, uint32_t *dst);
void (*blAesCbcDecr)(struct AesCbcContext *ctx, const uint32_t *src, uint32_t *dst);
const uint32_t* (*blSigPaddingVerify)(const uint32_t *rsaResult); //return pointer to hash inside the rsaResult or NULL on error
+
+ // extension: for binary compatibility, placed here
+ uint32_t (*blVerifyOsUpdate)(void);
};
#ifndef BL_STACK_SIZE
diff --git a/firmware/inc/platform/stm32f4xx/dma.h b/firmware/inc/platform/stm32f4xx/dma.h
index 92ef1e7..a4ef66a 100644
--- a/firmware/inc/platform/stm32f4xx/dma.h
+++ b/firmware/inc/platform/stm32f4xx/dma.h
@@ -58,5 +58,6 @@
uint16_t dmaBytesLeft(uint8_t busId, uint8_t stream);
void dmaStop(uint8_t busId, uint8_t stream);
const enum IRQn dmaIrq(uint8_t busId, uint8_t stream);
+int dmaStopAll(uint32_t tid);
#endif /* _DMA_H */
diff --git a/firmware/inc/platform/stm32f4xx/exti.h b/firmware/inc/platform/stm32f4xx/exti.h
index d403cc7..84fcd39 100644
--- a/firmware/inc/platform/stm32f4xx/exti.h
+++ b/firmware/inc/platform/stm32f4xx/exti.h
@@ -66,6 +66,7 @@
int extiChainIsr(IRQn_Type n, struct ChainedIsr *isr);
int extiUnchainIsr(IRQn_Type n, struct ChainedIsr *isr);
+int extiUnchainAll(uint32_t tid);
static inline void extiEnableIntGpio(const struct Gpio *__restrict gpioHandle, enum ExtiTrigger trigger)
{
diff --git a/firmware/inc/sensors.h b/firmware/inc/sensors.h
index 7679b68..2e301c1 100644
--- a/firmware/inc/sensors.h
+++ b/firmware/inc/sensors.h
@@ -165,7 +165,9 @@
bool (*sensorSendOneDirectEvt)(void *, uint32_t tid); //resend last state (if known), only for onchange-supporting sensors, to bring on a new client
- bool (*sensorMarshallData)(uint32_t yourEvtType, const void *yourEvtData, TaggedPtr *evtFreeingInfoP, void *); //marshall yourEvt for sending to host. Send a EVT_MARSHALLED_SENSOR_DATA event with marshalled data. Always send event, even on error, free the passed-in event using osFreeRetainedEvent
+ // Marshall yourEvt for sending to host. Send a EVT_MARSHALLED_SENSOR_DATA event with marshalled data.
+ // Always send event, even on error, free the passed-in event using osFreeRetainedEvent
+ bool (*sensorMarshallData)(uint32_t yourEvtType, const void *yourEvtData, TaggedPtr *evtFreeingInfoP, void *);
};
enum SensorInfoFlags1 {
@@ -206,7 +208,6 @@
uint16_t minSamples; /* minimum host fifo size (in # of samples) */
uint8_t biasType;
uint8_t rawType;
- uint16_t pad;
float rawScale;
};
@@ -266,7 +267,7 @@
uint64_t sensorGetCurLatency(uint32_t sensorHandle);
bool sensorGetInitComplete(uint32_t sensorHandle); // DO NOT poll on this value
bool sensorMarshallEvent(uint32_t sensorHandle, uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP);
-
+int sensorUnregisterAll(uint32_t tid);
/*
* convenience funcs
diff --git a/firmware/inc/seos.h b/firmware/inc/seos.h
index 23d4e9c..ab2f193 100644
--- a/firmware/inc/seos.h
+++ b/firmware/inc/seos.h
@@ -25,23 +25,21 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
+#include <stddef.h>
#include <eventQ.h>
#include <plat/inc/app.h>
#include <eventnums.h>
#include "toolchain.h"
+#include <nanohub/nanohub.h>
+
+//#define SEGMENT_CRC_SUPPORT
+
#define MAX_TASKS 16
-#define MAX_EMBEDDED_EVT_SUBS 6 /*tradeoff, no wrong answer */
+#define MAX_EMBEDDED_EVT_SUBS 6 /* tradeoff, no wrong answer */
+#define TASK_IDX_BITS 8 /* should be big enough to hold MAX_TASKS, but still fit in TaskIndex */
-
-
-#define OS_VER 0x0000
-
-#define ENCR_KEY_GOOGLE_PREPOPULATED 1 // our key ID is 1
-
-#define FIRST_VALID_TID 0x00000001
-#define LAST_VALID_TID 0x0fffffff
-
+typedef uint8_t TaskIndex;
struct AppFuncs { /* do not rearrange */
/* lifescycle */
@@ -51,43 +49,75 @@
void (*handle)(uint32_t evtType, const void* evtData);
};
-#define APP_HDR_MAGIC "GoogleNanoApp"
+/* NOTE: [TASK ID]
+ * TID is designed to be 16-bit; there is no reason for TID to become bigger than that on a system
+ * with typical RAM size of 64kB. However, in NO CASE TID values should overlap with TaggedPtr TAG mask,
+ * which is currently defined as 0x80000000.
+ */
+
+#define TASK_TID_BITS 16
+
+#define TASK_TID_MASK ((1 << TASK_TID_BITS) - 1)
+#define TASK_TID_INCREMENT (1 << TASK_IDX_BITS)
+#define TASK_TID_IDX_MASK ((1 << TASK_IDX_BITS) - 1)
+#define TASK_TID_COUNTER_MASK ((1 << TASK_TID_BITS) - TASK_TID_INCREMENT)
+
+#if MAX_TASKS > TASK_TID_IDX_MASK
+#error MAX_TASKS does not fit in TASK_TID_BITS
+#endif
+
+#define OS_SYSTEM_TID 0
+#define OS_VER 0x0000
+
+// FIXME: compatibility: keep key ID 1 until key update is functional
+//#define ENCR_KEY_GOOGLE_PREPOPULATED 0x041F010000000001
+#define ENCR_KEY_GOOGLE_PREPOPULATED 1 // our key ID is 1
+
+#define APP_HDR_MAGIC NANOAPP_FW_MAGIC
#define APP_HDR_VER_CUR 0
-#define APP_HDR_MARKER_UPLOADING 0xFFFF
-#define APP_HDR_MARKER_VERIFYING 0xFFFE
-#define APP_HDR_MARKER_VALID 0xFF00
-#define APP_HDR_MARKER_INTERNAL 0xFF01 //no external app should at any point have this marker value!
-#define APP_HDR_MARKER_DELETED 0x0000
+
+#define FL_APP_HDR_INTERNAL 0x0001 // to be able to fork behavior at run time for internal apps
+#define FL_APP_HDR_APPLICATION 0x0002 // image has AppHdr; otherwise is has AppInfo header
+#define FL_APP_HDR_SECURE 0x0004 // secure content, needs to be zero-filled when discarded
+#define FL_APP_HDR_VOLATILE 0x0008 // volatile content, segment shall be deleted after operation is complete
+#define FL_KEY_HDR_DELETE 0x8000 // key-specific flag: if set key id refers to existing key which has to be deleted
/* app ids are split into vendor and app parts. vendor parts are assigned by google. App parts are free for each vendor to assign at will */
#define APP_ID_FIRST_USABLE 0x0100000000000000ULL //all app ids lower than this are reserved for google's internal use
#define APP_ID_GET_VENDOR(appid) ((appid) >> 24)
-#define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF))
-#define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl"
+#define APP_ID_GET_SEQ_ID(appid) ((appid) & 0xFFFFFF)
+#define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & APP_SEQ_ID_ANY))
+#define KEY_ID_MAKE(vendor, key) ((((uint64_t)(vendor)) << 24) | ((key) & KEY_SEQ_ID_ANY))
+#define APP_ID_VENDOR_GOOGLE UINT64_C(0x476F6F676C) // "Googl"
+#define APP_VENDOR_ANY UINT64_C(0xFFFFFFFFFF)
+#define APP_SEQ_ID_ANY UINT64_C(0xFFFFFF)
+#define KEY_SEQ_ID_ANY UINT64_C(0xFFFFFF)
+#define APP_ID_ANY UINT64_C(0xFFFFFFFFFFFFFFFF)
-struct AppHdr {
- char magic[13];
- uint8_t fmtVer; //app header format version
- uint16_t marker;
+#define APP_INFO_CMD_ADD_KEY 1
+#define APP_INFO_CMD_REMOVE_KEY 2
+#define APP_INFO_CMD_OS_UPDATE 3
- uint64_t appId;
+#define SEG_STATE_INVALID UINT32_C(0xFFFFFFFF)
+#define SEG_SIZE_MAX UINT32_C(0x00FFFFFF)
+#define SEG_SIZE_INVALID (-1)
+#define SEG_ST(arg) (((arg) << 4) | (arg))
- uint32_t data_start;
- uint32_t data_end;
- uint32_t data_data;
+#define SEG_ID_EMPTY 0xF
+#define SEG_ID_RESERVED 0x7 // upload in progress
+#define SEG_ID_VALID 0x3 // CRC-32 valid
+#define SEG_ID_ERASED 0x0 // segment erased
- uint32_t bss_start;
- uint32_t bss_end;
+#define SEG_ST_EMPTY SEG_ST(SEG_ID_EMPTY)
+#define SEG_ST_RESERVED SEG_ST(SEG_ID_RESERVED)
+#define SEG_ST_VALID SEG_ST(SEG_ID_VALID)
+#define SEG_ST_ERASED SEG_ST(SEG_ID_ERASED)
- uint32_t got_start;
- uint32_t got_end;
- uint32_t rel_start;
- uint32_t rel_end;
-
- uint32_t appVer; //version of actual app
- uint32_t rfu;
-
- struct AppFuncs funcs;
+struct Segment {
+ uint8_t state; // 0xFF: empty; bit7=0: segment present; bit6=0: size valid; bit5=0: CRC-32 valid; bit4=0:segment erased;
+ // bits 3-0 replicate bits7-4;
+ uint8_t size[3]; // actual stored size in flash, initially filled with 0xFF
+ // updated after flash operation is completed (successfully or not)
};
struct AppEventFreeData { //goes with EVT_APP_FREE_EVT_DATA
@@ -137,17 +167,103 @@
bool osTidById(uint64_t appId, uint32_t *tid);
bool osAppInfoById(uint64_t appId, uint32_t *appIdx, uint32_t *appVer, uint32_t *appSize);
bool osAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
+uint32_t osGetCurrentTid();
+uint32_t osSetCurrentTid(uint32_t);
+
+struct AppHdr *osAppSegmentCreate(uint32_t size);
+bool osAppSegmentClose(struct AppHdr *app, uint32_t segSize, uint32_t segState);
+bool osAppSegmentSetState(const struct AppHdr *app, uint32_t segState);
+bool osSegmentSetSize(struct Segment *seg, uint32_t size);
+bool osAppWipeData(struct AppHdr *app);
+struct Segment *osGetSegment(const struct AppHdr *app);
+struct Segment *osSegmentGetEnd();
+
+static inline int32_t osSegmentGetSize(const struct Segment *seg)
+{
+ return seg ? seg->size[0] | (seg->size[1] << 8) | (seg->size[2] << 16) : SEG_SIZE_INVALID;
+}
+
+static inline uint32_t osSegmentGetState(const struct Segment *seg)
+{
+ return seg ? seg->state : SEG_STATE_INVALID;
+}
+
+static inline struct AppHdr *osSegmentGetData(const struct Segment *seg)
+{
+ return (struct AppHdr*)(&seg[1]);
+}
+
+#ifdef SEGMENT_CRC_SUPPORT
+
+struct SegmentFooter
+{
+ uint32_t crc;
+};
+
+#define FOOTER_SIZE sizeof(struct SegmentFooter)
+#else
+#define FOOTER_SIZE 0
+#endif
+
+static inline uint32_t osSegmentSizeAlignedWithFooter(uint32_t size)
+{
+ return ((size + 3) & ~3) + FOOTER_SIZE;
+}
+
+static inline const struct Segment *osSegmentSizeGetNext(const struct Segment *seg, uint32_t size)
+{
+ struct Segment *next = (struct Segment *)(((uint8_t*)seg) +
+ osSegmentSizeAlignedWithFooter(size) +
+ sizeof(*seg)
+ );
+ return seg ? next : NULL;
+}
+
+static inline const struct Segment *osSegmentGetNext(const struct Segment *seg)
+{
+ return osSegmentSizeGetNext(seg, osSegmentGetSize(seg));
+}
+
+static inline uint32_t osAppSegmentGetState(const struct AppHdr *app)
+{
+ return osSegmentGetState(osGetSegment(app));
+}
+
+struct SegmentIterator {
+ const struct Segment *shared;
+ const struct Segment *sharedEnd;
+ const struct Segment *seg;
+};
+
+void osSegmentIteratorInit(struct SegmentIterator *it);
+
+static inline bool osSegmentIteratorNext(struct SegmentIterator *it)
+{
+ const struct Segment *seg = it->shared;
+ const struct Segment *next = seg < it->sharedEnd ? osSegmentGetNext(seg) : it->sharedEnd;
+
+ it->shared = next;
+ it->seg = seg;
+
+ return seg < it->sharedEnd;
+}
+
+bool osWriteShared(void *dest, const void *src, uint32_t len);
+bool osEraseShared();
//event retaining support
bool osRetainCurrentEvent(TaggedPtr *evtFreeingInfoP); //called from any apps' event handling to retain current event. Only valid for first app that tries. evtFreeingInfoP filled by call and used to free evt later
void osFreeRetainedEvent(uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP);
+uint32_t osExtAppStopApps(uint64_t appId);
+uint32_t osExtAppEraseApps(uint64_t appId);
+uint32_t osExtAppStartApps(uint64_t appId);
/* Logging */
enum LogLevel {
LOG_ERROR = 'E',
- LOG_WARN = 'W',
- LOG_INFO = 'I',
+ LOG_WARN = 'W',
+ LOG_INFO = 'I',
LOG_DEBUG = 'D',
};
@@ -155,28 +271,32 @@
void osLog(enum LogLevel level, const char *str, ...) PRINTF_ATTRIBUTE;
#ifndef INTERNAL_APP_INIT
-#define INTERNAL_APP_INIT(_id, _ver, _init, _end, _event) \
-SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \
- .magic = APP_HDR_MAGIC, \
- .fmtVer = APP_HDR_VER_CUR, \
- .marker = APP_HDR_MARKER_INTERNAL, \
- .appId = (_id), \
- .appVer = (_ver), \
- .funcs.init = (_init), \
- .funcs.end = (_end), \
- .funcs.handle = (_event) \
+#define INTERNAL_APP_INIT(_id, _ver, _init, _end, _event) \
+SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr \
+SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \
+ .hdr.magic = APP_HDR_MAGIC, \
+ .hdr.fwVer = APP_HDR_VER_CUR, \
+ .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION, \
+ .hdr.appId = (_id), \
+ .hdr.appVer = (_ver), \
+ .hdr.payInfoType = LAYOUT_APP, \
+ .vec.init = (uint32_t)(_init), \
+ .vec.end = (uint32_t)(_end), \
+ .vec.handle = (uint32_t)(_event) \
}
#endif
#ifndef APP_INIT
-#define APP_INIT(_ver, _init, _end, _event) \
-extern const struct AppFuncs _mAppFuncs; \
-const struct AppFuncs SET_EXTERNAL_APP_ATTRIBUTES(used, section (".app_init"), visibility("default")) _mAppFuncs = { \
- .init = (_init), \
- .end = (_end), \
- .handle = (_event) \
-}; \
-const uint32_t SET_EXTERNAL_APP_VERSION(used, section (".app_version"), visibility("default")) _mAppVer = _ver
+#define APP_INIT(_ver, _init, _end, _event) \
+extern const struct AppFuncs _mAppFuncs; \
+const struct AppFuncs SET_EXTERNAL_APP_ATTRIBUTES(used, section (".app_init"), \
+visibility("default")) _mAppFuncs = { \
+ .init = (_init), \
+ .end = (_end), \
+ .handle = (_event) \
+}; \
+const uint32_t SET_EXTERNAL_APP_VERSION(used, section (".app_version"), \
+visibility("default")) _mAppVer = _ver
#endif
diff --git a/firmware/inc/timer.h b/firmware/inc/timer.h
index a2be078..f5e9fba 100644
--- a/firmware/inc/timer.h
+++ b/firmware/inc/timer.h
@@ -42,6 +42,7 @@
uint32_t timTimerSet(uint64_t length, uint32_t jitterPpm, uint32_t driftPpm, TimTimerCbkF cbk, void* data, bool oneShot); /* return timer id or 0 if failed */
uint32_t timTimerSetAsApp(uint64_t length, uint32_t jitterPpm, uint32_t driftPpm, uint32_t tid, void* data, bool oneShot); /* return timer id or 0 if failed */
bool timTimerCancel(uint32_t timerId);
+int timTimerCancelAll(uint32_t tid);
//called by interrupt routine. ->true if any timers were fired
diff --git a/firmware/misc/cpu/cortexm4f/app.lkr b/firmware/misc/cpu/cortexm4f/app.lkr
index 83e4063..6766e29 100644
--- a/firmware/misc/cpu/cortexm4f/app.lkr
+++ b/firmware/misc/cpu/cortexm4f/app.lkr
@@ -15,128 +15,115 @@
*/
/*
- These addresses are fake, but in a particular way that mesh with our expectations.
- - ".flash" will contain all parts of the app that go into actual flash
- *app header
- * code
- * RO data
- * initial contents of RW data
- * initial contents of the GOT
- * list of relocs
- * list of symbols
- - ".ram" will contain all data that uses ram (and is not part of the flash image)
- * RW data in its actua location
- * BSS
- * the GOT
- - ".trash" contains sections taht GCC simply feel like it MUST produce (link error otherwise) but we have no use for. it will be tripped
+ These addresses are fake, but in a particular way that mesh with our expectations.
+ - ".flash" will contain all parts of the app that go into actual flash
+ *app header
+ * code
+ * RO data
+ * initial contents of RW data
+ * initial contents of the GOT
+ * list of relocs
+ * list of symbols
+ - ".ram" will contain all data that uses ram (and is not part of the flash image)
+ * RW data in its actua location
+ * BSS
+ * the GOT
+ - ".trash" contains sections taht GCC simply feel like it MUST produce (link error otherwise) but we have no use for. it will be tripped
- After this image is produced a nonoapp_postprocess unitily will process relocs and symbols and compress them to a small reloc table
- while also modifying the app code itself potentially to adjust for new reloc format. This shrinks relocs a lot. GCC produces relocs at 8
- bytes per reloc and symbols at 16 bytes per symbol. We remove all symbol infos (as weo not need them) and compress the relevant data
- from there into relocs and app image itself, generating 4 bytes per reloc of "nano reloc data"
+ After this image is produced a nonoapp_postprocess unitily will process relocs and symbols and compress them to a small reloc table
+ while also modifying the app code itself potentially to adjust for new reloc format. This shrinks relocs a lot. GCC produces relocs at 8
+ bytes per reloc and symbols at 16 bytes per symbol. We remove all symbol infos (as we do not need them) and compress the relevant data
+ from there into relocs and app image itself, generating 4 bytes per reloc of "nano reloc data"
- Our format allows apps that are up to 256MB of flash and 256MB of ram in size.
+ Our format allows apps that are up to 256MB of flash and 256MB of ram in size.
*/
MEMORY
{
- flash : ORIGIN = 0x10000000, LENGTH = 256K /* we write this to flash */
- ram : ORIGIN = 0x80000000, LENGTH = 128K /* we allocate this in ram */
- trash : ORIGIN = 0xF0000000, LENGTH = 256K /* we throw this away soon after linking */
+ flash : ORIGIN = 0x10000000, LENGTH = 256K /* we write this to flash */
+ ram : ORIGIN = 0x80000000, LENGTH = 128K /* we allocate this in ram */
+ trash : ORIGIN = 0xF0000000, LENGTH = 256K /* we throw this away soon after linking */
}
SECTIONS
{
- .flash : {
- /* magic : "GoogleNanoApp", 0x00, 0xff, 0xff first is magix string, then version (0) then two bytes of "FF" that we can later overwrite with zeros in flash to mark apps as dead */
- LONG(0x676f6f47)
- LONG(0x614e656c)
- LONG(0x70416f6e)
- LONG(0xffff0070)
+ .flash : {
+ /***** start of struct BinHdr [see nanohub/nanohub.h] *****/
+ /* binary format marker: 'NBIN' (LE) */
+ LONG(0x4E49424E)
- /* 64-bit app id */
- LONG(0x55555555)
- LONG(0xaaaaaaaa)
+ /* version */
+ KEEP(*(.app_version));
- /* things we need to load it */
- LONG(__data_start)
- LONG(__data_end)
- LONG(LOADADDR(.data))
+ /* things we need to load app */
+ LONG(__data_start)
+ LONG(__data_end)
+ LONG(LOADADDR(.data))
- LONG(__bss_start)
- LONG(__bss_end)
+ LONG(__bss_start)
+ LONG(__bss_end)
- /* things we need to run it */
- LONG(__got_start)
- LONG(__got_end)
- LONG(__rel_start)
- LONG(__rel_end)
+ /* things we need to run it */
+ LONG(__got_start)
+ LONG(__got_end)
+ LONG(__rel_start)
+ LONG(__rel_end)
- /* version */
- KEEP(*(.app_version));
+ KEEP(*(.app_init));
+ /***** end of struct BinHdr [see nanohub/nanohub.h] *****/
- /* reserved */
- LONG(0)
+ /* code */
+ *(.text) *(.text.*) ;
+ *(.rodata) *(.rodata.*) ;
+ . = ALIGN(4);
+ } > flash = 0xff
- /* entry points */
- KEEP(*(.app_init));
+ .data : {
+ . = ALIGN(4);
+ __data_start = ABSOLUTE(.);
+ *(.data);
+ *(.data.*);
+ . = ALIGN(4);
+ __data_end = ABSOLUTE(.);
- /* code */
- *(.text) *(.text.*) ;
- *(.rodata) *(.rodata.*) ;
- . = ALIGN(4);
- } > flash = 0xff
+ . = ALIGN(4);
+ __got_start = ABSOLUTE(.);
+ *(.got) *(.got.*) ;
+ __got_end = ABSOLUTE(.);
- .data : {
+ } > ram AT > flash
- . = ALIGN(4);
- __data_start = ABSOLUTE(.);
- *(.data);
- *(.data.*);
- . = ALIGN(4);
- __data_end = ABSOLUTE(.);
+ .relocs : {
+ . = ALIGN(4);
+ /* relocs */
+ __rel_start = ABSOLUTE(.);
+ *(.rel) *(.rel.*) *(.rel.data.rel.local)
+ __rel_end = ABSOLUTE(.);
+ . = ALIGN(4);
- . = ALIGN(4);
- __got_start = ABSOLUTE(.);
- *(.got) *(.got.*) ;
- __got_end = ABSOLUTE(.);
+ } > flash = 0xff
- } > ram AT > flash
+ .dynsym : {
+ *(.dynsym); *(.dynsym.*);
+ } > flash = 0xff
- .relocs : {
+ .bss : {
+ . = ALIGN(4);
+ __bss_start = ABSOLUTE(.);
+ *(.bss) *(.bss.*) *(COMMON);
+ . = ALIGN(4);
+ __bss_end = ABSOLUTE(.);
+ } > ram
- . = ALIGN(4);
- /* relocs */
- __rel_start = ABSOLUTE(.);
- *(.rel) *(.rel.*) *(.rel.data.rel.local)
- __rel_end = ABSOLUTE(.);
- . = ALIGN(4);
+ __data_data = LOADADDR(.data);
- } > flash = 0xff
-
- .dynsym : {
- *(.dynsym); *(.dynsym.*);
- } > flash = 0xff
-
- .bss : {
- . = ALIGN(4);
- __bss_start = ABSOLUTE(.);
- *(.bss) *(.bss.*) *(COMMON);
- . = ALIGN(4);
- __bss_end = ABSOLUTE(.);
- } > ram
-
- __data_data = LOADADDR(.data);
-
- .dynstr : {
- *(.dynstr); *(.dynstr.*);
- } > trash
- .hash : {
- *(.hash); *(.hash.*);
- } > trash
- .dynamic : {
- *(.dynamic); *(.dynamic.*);
- } > trash
-
+ .dynstr : {
+ *(.dynstr); *(.dynstr.*);
+ } > trash
+ .hash : {
+ *(.hash); *(.hash.*);
+ } > trash
+ .dynamic : {
+ *(.dynamic); *(.dynamic.*);
+ } > trash
}
-
diff --git a/firmware/src/appSec.c b/firmware/src/appSec.c
index 648d444..de7421a 100644
--- a/firmware/src/appSec.c
+++ b/firmware/src/appSec.c
@@ -14,37 +14,50 @@
* limitations under the License.
*/
+#include <stdint.h>
+
#include <plat/inc/bl.h>
+
+#include <nanohub/sha2.h>
+#include <nanohub/rsa.h>
+#include <nanohub/aes.h>
+
#include <appSec.h>
#include <string.h>
#include <stdio.h>
#include <heap.h>
-#include <sha2.h>
-#include <rsa.h>
-#include <aes.h>
+#include <seos.h>
+#include <inttypes.h>
-
-#define APP_HDR_SIZE 32 //headers are this size
+#define APP_HDR_SIZE (sizeof(struct ImageHeader))
+#define APP_HDR_MAX_SIZE (sizeof(struct ImageHeader) + sizeof(struct AppSecSignHdr) + sizeof(struct AppSecEncrHdr))
#define APP_DATA_CHUNK_SIZE (AES_BLOCK_WORDS * sizeof(uint32_t)) //data blocks are this size
#define APP_SIG_SIZE RSA_BYTES
+// verify block is SHA placed in integral number of encryption blocks (for SHA256 and AES256 happens to be exactly 2 AES blocks)
+#define APP_VERIFY_BLOCK_SIZE ((SHA2_HASH_SIZE + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE
+
#define APP_SEC_SIG_ALIGN APP_DATA_CHUNK_SIZE
#define APP_SEC_ENCR_ALIGN APP_DATA_CHUNK_SIZE
-#define STATE_INIT 0 //nothing gotten yet
-#define STATE_RXING_HEADERS 1 //each is APP_HDR_SIZE bytes
-#define STATE_RXING_DATA 2 //each data block is AES_BLOCK_WORDS 32-bit words (for AES reasons)
-#define STATE_RXING_SIG_HASH 3 //each is RSA_BYTES bytes
-#define STATE_RXING_SIG_PUBKEY 4 //each is RSA_BYTES bytes
-#define STATE_DONE 5 //all is finished and well
-#define STATE_BAD 6 //unrecoverable badness has happened. this will *NOT* fix itself. It is now ok to give up, start over, cry, or pray to your favourite deity for help
+#define STATE_INIT 0 // nothing gotten yet
+#define STATE_RXING_HEADERS 1 // variable size headers (min APP_HDR_SIZE, max APP_HDR_MAX_SIZE)
+#define STATE_RXING_DATA 2 // each data block is AES_BLOCK_WORDS 32-bit words (for AES reasons)
+#define STATE_RXING_SIG_HASH 3 // each is RSA_BYTES bytes
+#define STATE_RXING_SIG_PUBKEY 4 // each is RSA_BYTES bytes
+#define STATE_VERIFY 5 // decryption of ciphertext done; now decrypting and verifying the encrypted plaintext SHA2
+#define STATE_DONE 6 // all is finished and well
+#define STATE_BAD 7 // unrecoverable badness has happened. this will *NOT* fix itself. It is now ok to give up, start over, cry, or pray to your favourite deity for help
+#define STATE_MAX 8 // total number of states
+
+//#define DEBUG_FSM
struct AppSecState {
-
union { //we save some memory by reusing this space.
struct {
struct AesCbcContext cbc;
struct Sha2state sha;
+ struct Sha2state cbcSha;
};
struct {
struct RsaState rsa;
@@ -60,7 +73,7 @@
union {
union { //make the compiler work to make sure we have enough space
- uint8_t placeholderAppHdr[APP_HDR_SIZE];
+ uint8_t placeholderAppHdr[APP_HDR_MAX_SIZE];
uint8_t placeholderDataChunk[APP_DATA_CHUNK_SIZE];
uint8_t placeholderSigChunk[APP_SIG_SIZE];
uint8_t placeholderAesKey[AES_KEY_WORDS * sizeof(uint32_t)];
@@ -73,28 +86,57 @@
uint32_t encryptedBytesIn;
uint32_t signedBytesOut;
uint32_t encryptedBytesOut;
- uint32_t numSigs;
uint16_t haveBytes; //in dataBytes...
+ uint16_t chunkSize;
uint8_t curState;
uint8_t needSig :1;
uint8_t haveSig :1;
uint8_t haveEncr :1;
+ uint8_t haveTrustedKey :1;
uint8_t doingRsa :1;
};
-struct AppSecSigHdr {
- uint8_t magic[8];
- uint32_t appDataLen;
- uint32_t numSigs;
-};
+static void limitChunkSize(struct AppSecState *state)
+{
+ if (state->haveSig && state->chunkSize > state->signedBytesIn)
+ state->chunkSize = state->signedBytesIn;
+ if (state->haveEncr && state->chunkSize > state->encryptedBytesIn)
+ state->chunkSize = state->signedBytesIn;
+}
-struct AppSecEncrHdr {
- uint8_t magic[4];
- uint32_t dataLen;
- uint64_t keyID;
- uint32_t IV[AES_BLOCK_WORDS];
-};
+static void appSecSetCurState(struct AppSecState *state, uint32_t curState)
+{
+ const static uint16_t chunkSize[STATE_MAX] = {
+ [STATE_RXING_HEADERS] = APP_HDR_SIZE,
+ [STATE_RXING_DATA] = APP_DATA_CHUNK_SIZE,
+ [STATE_VERIFY] = APP_VERIFY_BLOCK_SIZE,
+ [STATE_RXING_SIG_HASH] = APP_SIG_SIZE,
+ [STATE_RXING_SIG_PUBKEY] = APP_SIG_SIZE,
+ };
+ if (curState >= STATE_MAX)
+ curState = STATE_BAD;
+ if (curState != state->curState || curState == STATE_INIT) {
+#ifdef DEBUG_FSM
+ osLog(LOG_INFO, "%s: oldState=%" PRIu8
+ "; new state=%" PRIu32
+ "; old chunk size=%" PRIu16
+ "; new chunk size=%" PRIu16
+ "; have bytes=%" PRIu16
+ "\n",
+ __func__, state->curState, curState,
+ state->chunkSize, chunkSize[curState],
+ state->haveBytes);
+#endif
+ state->curState = curState;
+ state->chunkSize = chunkSize[curState];
+ }
+}
+
+static inline uint32_t appSecGetCurState(const struct AppSecState *state)
+{
+ return state->curState;
+}
//init/deinit
struct AppSecState *appSecInit(AppSecWriteCbk writeCbk, AppSecPubKeyFindCbk pubKeyFindCbk, AppSecGetAesKeyCbk aesKeyAccessCbk, bool mandateSigning)
@@ -109,7 +151,7 @@
state->writeCbk = writeCbk;
state->pubKeyFindCbk = pubKeyFindCbk;
state->aesKeyAccessCbk = aesKeyAccessCbk;
- state->curState = STATE_INIT;
+ appSecSetCurState(state, STATE_INIT);
if (mandateSigning)
state->needSig = 1;
@@ -121,7 +163,6 @@
heapFree(state);
}
-
//if needed, decrypt and hash incoming data
static AppSecErr appSecBlockRx(struct AppSecState *state)
{
@@ -142,7 +183,7 @@
BL.blSha2processBytes(&state->sha, state->dataBytes, state->haveBytes);
}
- //decrypt if encryption is on
+ // decrypt if encryption is on
if (state->haveEncr) {
uint32_t *dataP = state->dataWords;
@@ -152,115 +193,175 @@
if (state->haveBytes % APP_DATA_CHUNK_SIZE)
return APP_SEC_TOO_LITTLE_DATA;
- //make sure we do not get too much data & account for the data we got
+ // make sure we do not get too much data & account for the data we got
if (state->haveBytes > state->encryptedBytesIn)
return APP_SEC_TOO_MUCH_DATA;
state->encryptedBytesIn -= state->haveBytes;
- //decrypt
+ // decrypt
for (i = 0; i < numBlocks; i++, dataP += AES_BLOCK_WORDS)
BL.blAesCbcDecr(&state->cbc, dataP, dataP);
- //make sure we do not produce too much data (discard padding) & make sure we account for it
+ // make sure we do not produce too much data (discard padding) & make sure we account for it
if (state->encryptedBytesOut < state->haveBytes)
state->haveBytes = state->encryptedBytesOut;
state->encryptedBytesOut -= state->haveBytes;
+
+ if (state->haveBytes)
+ BL.blSha2processBytes(&state->cbcSha, state->dataBytes, state->haveBytes);
}
+ limitChunkSize(state);
+
return APP_SEC_NO_ERROR;
}
-static AppSecErr appSecProcessIncomingHdr(struct AppSecState *state, bool *sendDataToDataHandlerP)
+static AppSecErr appSecProcessIncomingHdr(struct AppSecState *state, uint32_t *needBytesOut)
{
- static const char hdrAddEncrKey[] = "EncrKey+";
- static const char hdrDelEncrKey[] = "EncrKey+";
- static const char hdrNanoApp[] = "GoogleNanoApp\x00\xff\xff"; //we check marker is set to 0xFF and version set to 0, as we must as per spec
- static const char hdrEncrHdr[] = "Encr";
- static const char hdrSigHdr[] = "SigndApp";
+ struct ImageHeader *image;
+ struct nano_app_binary_t *aosp;
+ uint32_t flags;
+ uint32_t needBytes;
+ struct AppSecSignHdr *signHdr = NULL;
+ struct AppSecEncrHdr *encrHdr = NULL;
+ uint8_t *hdr = state->dataBytes;
+ AppSecErr ret;
- //check for signature header
- if (!memcmp(state->dataBytes, hdrSigHdr, sizeof(hdrSigHdr) - 1)) {
+ image = (struct ImageHeader *)hdr; hdr += sizeof(*image);
+ aosp = &image->aosp;
+ flags = aosp->flags;
+ if (aosp->header_version != 1 ||
+ aosp->magic != NANOAPP_AOSP_MAGIC ||
+ image->layout.version != 1 ||
+ image->layout.magic != GOOGLE_LAYOUT_MAGIC)
+ return APP_SEC_HEADER_ERROR;
- struct AppSecSigHdr *sigHdr = (struct AppSecSigHdr*)state->dataBytes;
+ needBytes = sizeof(*image);
+ if ((flags & NANOAPP_SIGNED_FLAG) != 0)
+ needBytes += sizeof(*signHdr);
+ if ((flags & NANOAPP_ENCRYPTED_FLAG) != 0)
+ needBytes += sizeof(*encrHdr);
- if (state->haveSig) //we do not allow signing of already-signed data
+ *needBytesOut = needBytes;
+
+ if (needBytes > state->haveBytes)
+ return APP_SEC_NO_ERROR;
+
+ *needBytesOut = 0;
+
+ if ((flags & NANOAPP_SIGNED_FLAG) != 0) {
+ signHdr = (struct AppSecSignHdr *)hdr; hdr += sizeof(*signHdr);
+ osLog(LOG_INFO, "%s: signed size=%" PRIu32 "\n",
+ __func__, signHdr->appDataLen);
+ if (!signHdr->appDataLen) {
+ //no data bytes
return APP_SEC_INVALID_DATA;
-
- if (state->haveEncr) //we do not allow encryption of signed data, only signing of encrypted data
- return APP_SEC_INVALID_DATA;
-
- if (!sigHdr->appDataLen || !sigHdr->numSigs) //no data bytes or no sigs?
- return APP_SEC_INVALID_DATA;
-
- state->signedBytesOut = sigHdr->appDataLen;
- state->signedBytesIn = ((state->signedBytesOut + APP_SEC_SIG_ALIGN - 1) / APP_SEC_SIG_ALIGN) * APP_SEC_SIG_ALIGN;
- state->numSigs = sigHdr->numSigs;
+ }
+ state->signedBytesIn = state->signedBytesOut = signHdr->appDataLen;
state->haveSig = 1;
BL.blSha2init(&state->sha);
-
- return APP_SEC_NO_ERROR;
+ BL.blSha2processBytes(&state->sha, state->dataBytes, needBytes);
}
- //check for encryption header
- if (!memcmp(state->dataBytes, hdrEncrHdr, sizeof(hdrEncrHdr) - 1)) {
-
- struct AppSecEncrHdr *encrHdr = (struct AppSecEncrHdr*)state->dataBytes;
+ if ((flags & NANOAPP_ENCRYPTED_FLAG) != 0) {
uint32_t k[AES_KEY_WORDS];
- AppSecErr ret;
- if (state->haveEncr) //we do not allow encryption of already-encrypted data
- return APP_SEC_INVALID_DATA;
+ encrHdr = (struct AppSecEncrHdr *)hdr; hdr += sizeof(*encrHdr);
+ osLog(LOG_INFO, "%s: encrypted data size=%" PRIu32
+ "; key ID=%016" PRIX64 "\n",
+ __func__, encrHdr->dataLen, encrHdr->keyID);
if (!encrHdr->dataLen || !encrHdr->keyID)
return APP_SEC_INVALID_DATA;
-
ret = state->aesKeyAccessCbk(encrHdr->keyID, k);
- if (ret)
+ if (ret != APP_SEC_NO_ERROR) {
+ osLog(LOG_ERROR, "%s: Secret key not found\n", __func__);
return ret;
+ }
BL.blAesCbcInitForDecr(&state->cbc, k, encrHdr->IV);
+ BL.blSha2init(&state->cbcSha);
state->encryptedBytesOut = encrHdr->dataLen;
state->encryptedBytesIn = ((state->encryptedBytesOut + APP_SEC_ENCR_ALIGN - 1) / APP_SEC_ENCR_ALIGN) * APP_SEC_ENCR_ALIGN;
state->haveEncr = 1;
+ osLog(LOG_INFO, "%s: encrypted aligned data size=%" PRIu32 "\n",
+ __func__, state->encryptedBytesIn);
- return APP_SEC_NO_ERROR;
+ if (state->haveSig) {
+ state->signedBytesIn = state->signedBytesOut = signHdr->appDataLen - sizeof(*encrHdr);
+ // at this point, signedBytesOut must equal encryptedBytesIn
+ if (state->signedBytesOut != (state->encryptedBytesIn + SHA2_HASH_SIZE)) {
+ osLog(LOG_ERROR, "%s: sig data size does not match encrypted data\n", __func__);
+ return APP_SEC_INVALID_DATA;
+ }
+ }
}
- //check for valid app or something else that we pass directly to caller
- if (memcmp(state->dataBytes, hdrAddEncrKey, sizeof(hdrAddEncrKey) - 1) && memcmp(state->dataBytes, hdrDelEncrKey, sizeof(hdrDelEncrKey) - 1) && memcmp(state->dataBytes, hdrNanoApp, sizeof(hdrNanoApp) - 1))
- return APP_SEC_HEADER_ERROR;
-
//if we are in must-sign mode and no signature was provided, fail
- if (!state->haveSig && state->needSig)
+ if (!state->haveSig && state->needSig) {
+ osLog(LOG_ERROR, "%s: only signed images can be uploaded\n", __func__);
return APP_SEC_SIG_VERIFY_FAIL;
+ }
+
+ // now, transform AOSP header to FW common header
+ struct FwCommonHdr common = {
+ .magic = APP_HDR_MAGIC,
+ .appId = aosp->app_id,
+ .fwVer = APP_HDR_VER_CUR,
+ .fwFlags = image->layout.flags,
+ .appVer = aosp->app_version,
+ .payInfoType = image->layout.payload,
+ .rfu = { 0xFF, 0xFF },
+ };
+
+ // check to see if this is special system types of payload
+ switch(image->layout.payload) {
+ case LAYOUT_APP:
+ common.fwFlags = (common.fwFlags | FL_APP_HDR_APPLICATION) & ~FL_APP_HDR_INTERNAL;
+ common.payInfoSize = sizeof(struct AppInfo);
+ osLog(LOG_INFO, "App container found\n");
+ break;
+ case LAYOUT_KEY:
+ common.fwFlags |= FL_APP_HDR_SECURE;
+ common.payInfoSize = sizeof(struct KeyInfo);
+ osLog(LOG_INFO, "Key container found\n");
+ break;
+ case LAYOUT_OS:
+ common.payInfoSize = sizeof(struct OsUpdateHdr);
+ osLog(LOG_INFO, "OS update container found\n");
+ break;
+ default:
+ break;
+ }
+
+ memcpy(state->dataBytes, &common, sizeof(common));
+ state->haveBytes = sizeof(common);
//we're now in data-accepting state
- state->curState = STATE_RXING_DATA;
+ appSecSetCurState(state, STATE_RXING_DATA);
- //send data to caller as is
- *sendDataToDataHandlerP = true;
return APP_SEC_NO_ERROR;
}
static AppSecErr appSecProcessIncomingData(struct AppSecState *state)
{
//check for data-ending conditions
- if (state->haveSig && !state->signedBytesIn) { //we're all done with the signed portion of the data, now come the signatures
- if (state->haveEncr && state->encryptedBytesIn) //somehow we still have more "encrypted" bytes now - this is not valid
- return APP_SEC_INVALID_DATA;
- state->curState = STATE_RXING_SIG_HASH;
+ if (state->haveSig && !state->signedBytesIn) {
+ // we're all done with the signed portion of the data, now come the signatures
+ appSecSetCurState(state, STATE_RXING_SIG_HASH);
//collect the hash
memcpy(state->lastHash, BL.blSha2finish(&state->sha), SHA2_HASH_SIZE);
- }
- else if (state->haveEncr && !state->encryptedBytesIn) { //we're all done with encrypted bytes
- if (state->haveSig && state->signedBytesIn) //somehow we still have more "signed" bytes now - this is not valid
- return APP_SEC_INVALID_DATA;
- state->curState = STATE_DONE;
+ } else if (state->haveEncr && !state->encryptedBytesIn) {
+ if (appSecGetCurState(state) == STATE_RXING_DATA) {
+ //we're all done with encrypted plaintext
+ state->encryptedBytesIn = sizeof(state->cbcSha);
+ appSecSetCurState(state, STATE_VERIFY);
+ }
}
//pass to caller
- return state->writeCbk(state->dataBytes, state->haveBytes);
+ return state->haveBytes ? state->writeCbk(state->dataBytes, state->haveBytes) : APP_SEC_NO_ERROR;
}
AppSecErr appSecDoSomeProcessing(struct AppSecState *state)
@@ -288,43 +389,30 @@
if (memcmp(state->lastHash, result, SHA2_HASH_SIZE))
return APP_SEC_SIG_VERIFY_FAIL;
- //hash the provided pubkey if it is not the last
- if (state->numSigs) {
- BL.blSha2init(&state->sha);
- BL.blSha2processBytes(&state->sha, state->dataBytes, APP_SIG_SIZE);
- memcpy(state->lastHash, BL.blSha2finish(&state->sha), SHA2_HASH_SIZE);
- state->curState = STATE_RXING_SIG_HASH;
- }
- else
- state->curState = STATE_DONE;
+ //hash the provided pubkey
+ BL.blSha2init(&state->sha);
+ BL.blSha2processBytes(&state->sha, state->dataBytes, APP_SIG_SIZE);
+ memcpy(state->lastHash, BL.blSha2finish(&state->sha), SHA2_HASH_SIZE);
+ appSecSetCurState(state, STATE_RXING_SIG_HASH);
return APP_SEC_NO_ERROR;
}
static AppSecErr appSecProcessIncomingSigData(struct AppSecState *state)
{
- //if we're RXing the hash, just stash it away and move on
- if (state->curState == STATE_RXING_SIG_HASH) {
- if (!state->numSigs)
- return APP_SEC_TOO_MUCH_DATA;
+ bool keyFound = false;
- state->numSigs--;
+ //if we're RXing the hash, just stash it away and move on
+ if (appSecGetCurState(state) == STATE_RXING_SIG_HASH) {
+ state->haveTrustedKey = 0;
memcpy(state->rsaTmp, state->dataWords, APP_SIG_SIZE);
- state->curState = STATE_RXING_SIG_PUBKEY;
+ appSecSetCurState(state, STATE_RXING_SIG_PUBKEY);
return APP_SEC_NO_ERROR;
}
- //if we just got the last sig, verify it is a known root
- if (!state->numSigs) {
- bool keyFound = false;
- AppSecErr ret;
-
- ret = state->pubKeyFindCbk(state->dataWords, &keyFound);
- if (ret != APP_SEC_NO_ERROR)
- return ret;
- if (!keyFound)
- return APP_SEC_SIG_ROOT_UNKNOWN;
- }
+ // verify it is a known root
+ state->pubKeyFindCbk(state->dataWords, &keyFound);
+ state->haveTrustedKey = keyFound;
//we now have the pubKey. decrypt over time
state->doingRsa = 1;
@@ -332,83 +420,96 @@
return APP_SEC_NEED_MORE_TIME;
}
+static AppSecErr appSecVerifyEncryptedData(struct AppSecState *state)
+{
+ const uint32_t *hash = BL.blSha2finish(&state->cbcSha);
+ bool verified = memcmp(hash, state->dataBytes, SHA2_BLOCK_SIZE) == 0;
+
+ osLog(LOG_INFO, "%s: decryption verification: %s\n", __func__, verified ? "passed" : "failed");
+
+// TODO: fix verify logic
+// return verified ? APP_SEC_NO_ERROR : APP_SEC_VERIFY_FAILED;
+ return APP_SEC_NO_ERROR;
+}
+
AppSecErr appSecRxData(struct AppSecState *state, const void *dataP, uint32_t len, uint32_t *lenUnusedP)
{
const uint8_t *data = (const uint8_t*)dataP;
AppSecErr ret = APP_SEC_NO_ERROR;
- bool sendToDataHandler = false;
+ uint32_t needBytes;
- if (state->curState == STATE_INIT)
- state->curState = STATE_RXING_HEADERS;
+ if (appSecGetCurState(state) == STATE_INIT)
+ appSecSetCurState(state, STATE_RXING_HEADERS);
while (len) {
len--;
state->dataBytes[state->haveBytes++] = *data++;
- switch (state->curState) {
-
+ if (state->haveBytes < state->chunkSize)
+ continue;
+ switch (appSecGetCurState(state)) {
case STATE_RXING_HEADERS:
- if (state->haveBytes == APP_HDR_SIZE) {
-
- ret = appSecBlockRx(state);
- if (ret != APP_SEC_NO_ERROR)
- goto out;
-
- ret = appSecProcessIncomingHdr(state, &sendToDataHandler);
- if (ret != APP_SEC_NO_ERROR)
- goto out;
- if (!sendToDataHandler) {
- state->haveBytes = 0;
- goto out;
- }
- //fallthrough
+ // AOSP header is never encrypted; if it is signed, it will hash itself
+ needBytes = 0;
+ ret = appSecProcessIncomingHdr(state, &needBytes);
+ if (ret != APP_SEC_NO_ERROR)
+ goto out;
+ if (needBytes > state->chunkSize) {
+ state->chunkSize = needBytes;
+ // get more data and try again
+ continue;
}
- else
- break;
-
- case STATE_RXING_DATA:
- if (state->haveBytes >= APP_DATA_CHUNK_SIZE) {
-
- //if data is already processed, do not re-process it
- if (sendToDataHandler)
- sendToDataHandler = false;
- else {
- ret = appSecBlockRx(state);
- if (ret != APP_SEC_NO_ERROR)
- goto out;
- }
-
+ // done with parsing header(s); we might have something to write to flash
+ if (state->haveBytes) {
+ osLog(LOG_INFO, "%s: save converted header [%" PRIu16 " bytes] to flash\n", __func__, state->haveBytes);
ret = appSecProcessIncomingData(state);
state->haveBytes = 0;
- if (ret != APP_SEC_NO_ERROR)
- goto out;
}
+ limitChunkSize(state);
+ goto out;
+
+ case STATE_RXING_DATA:
+ ret = appSecBlockRx(state);
+ if (ret != APP_SEC_NO_ERROR)
+ goto out;
+
+ ret = appSecProcessIncomingData(state);
+ state->haveBytes = 0;
+ if (ret != APP_SEC_NO_ERROR)
+ goto out;
break;
+ case STATE_VERIFY:
+ ret = appSecBlockRx(state);
+ if (ret == APP_SEC_NO_ERROR)
+ ret = appSecProcessIncomingData(state);
+ if (ret == APP_SEC_NO_ERROR)
+ ret = appSecVerifyEncryptedData(state);
+ goto out;
+
case STATE_RXING_SIG_HASH:
case STATE_RXING_SIG_PUBKEY:
-
//no need for calling appSecBlockRx() as sigs are not signed, and encryption cannot be done after signing
- if (state->haveBytes == APP_SIG_SIZE) {
- ret = appSecProcessIncomingSigData(state);
- state->haveBytes = 0;
- if (ret != APP_SEC_NO_ERROR)
- goto out;
- }
- break;
+ ret = appSecProcessIncomingSigData(state);
+ state->haveBytes = 0;
+ goto out;
default:
- state->curState = STATE_BAD;
+ appSecSetCurState(state, STATE_BAD);
state->haveBytes = 0;
- *lenUnusedP = 0;
- return APP_SEC_BAD;
+ len = 0;
+ ret = APP_SEC_BAD;
+ break;
}
}
out:
*lenUnusedP = len;
- if (ret != APP_SEC_NO_ERROR && ret != APP_SEC_NEED_MORE_TIME)
- state->curState = STATE_BAD;
+ if (ret != APP_SEC_NO_ERROR && ret != APP_SEC_NEED_MORE_TIME) {
+ osLog(LOG_ERROR, "%s: failed: state=%" PRIu32 "; err=%" PRIu32 "\n",
+ __func__, appSecGetCurState(state), ret);
+ appSecSetCurState(state, STATE_BAD);
+ }
return ret;
}
@@ -417,42 +518,49 @@
{
AppSecErr ret;
- //Feed remianing data to data processor, if any
+ // Feed remaining data to data processor, if any
if (state->haveBytes) {
-
- //not in data rx stage when the incoming data ends? This is not good (if we had encr or sign we'd not be here)
- if (state->curState != STATE_RXING_DATA) {
- state->curState = STATE_BAD;
+ // if we are using encryption and/or signing, we are supposed to consume all data at this point.
+ if (state->haveSig || state->haveEncr) {
+ appSecSetCurState(state, STATE_BAD);
return APP_SEC_TOO_LITTLE_DATA;
}
-
- //feed the remaining data to the data processor
+ // Not in data rx stage when the incoming data ends? This is not good (if we had encr or sign we'd not be here)
+ if (appSecGetCurState(state) != STATE_RXING_DATA) {
+ appSecSetCurState(state, STATE_BAD);
+ return APP_SEC_TOO_LITTLE_DATA;
+ }
+ // Feed the remaining data to the data processor
ret = appSecProcessIncomingData(state);
if (ret != APP_SEC_NO_ERROR) {
- state->curState = STATE_BAD;
+ appSecSetCurState(state, STATE_BAD);
return ret;
}
+ } else {
+ // we don't know in advance how many signature packs we shall receive,
+ // so we evaluate every signature pack as if it is the last, but do not
+ // return error if public key is not trusted; only here we make the final
+ // determination
+ if (state->haveSig) {
+ // check the most recent key status
+ if (!state->haveTrustedKey) {
+ appSecSetCurState(state, STATE_BAD);
+ return APP_SEC_SIG_ROOT_UNKNOWN;
+ } else {
+ appSecSetCurState(state, STATE_DONE);
+ }
+ }
}
//for unsigned/unencrypted case we have no way to judge length, so we assume it is over when we're told it is
//this is potentially dangerous, but then again so is allowing unsigned uploads in general.
- if (!state->haveSig && !state->haveEncr && state->curState == STATE_RXING_DATA)
- state->curState = STATE_DONE;
+ if (!state->haveSig && !state->haveEncr && appSecGetCurState(state) == STATE_RXING_DATA)
+ appSecSetCurState(state, STATE_DONE);
//Check the state and return our verdict
- if(state->curState == STATE_DONE)
+ if(appSecGetCurState(state) == STATE_DONE)
return APP_SEC_NO_ERROR;
- state->curState = STATE_BAD;
+ appSecSetCurState(state, STATE_BAD);
return APP_SEC_TOO_LITTLE_DATA;
}
-
-
-
-
-
-
-
-
-
-
diff --git a/firmware/src/cpu/cortexm4f/appSupport.c b/firmware/src/cpu/cortexm4f/appSupport.c
index 1c733d1..1f33dfe 100644
--- a/firmware/src/cpu/cortexm4f/appSupport.c
+++ b/firmware/src/cpu/cortexm4f/appSupport.c
@@ -14,7 +14,8 @@
* limitations under the License.
*/
-#include <cpu/inc/appRelocFormat.h>
+#include <nanohub/appRelocFormat.h>
+
#include <string.h>
#include <stdint.h>
#include <heap.h>
@@ -27,6 +28,9 @@
#define NANO_RELOC_TYPE_RAM 0
#define NANO_RELOC_TYPE_FLASH 1
+#define APP_FLASH_RELOC(_base, _offset) ((uint32_t)(_base) + FLASH_RELOC_OFFSET + (_offset))
+#define APP_FLASH_RELOC_BASE(_base) APP_FLASH_RELOC(_base, 0)
+#define APP_VEC(_app) ((struct AppFuncs*)&((_app)->vec))
static bool handleRelNumber(uint32_t *ofstP, uint32_t type, uint32_t flashAddr, uint32_t ramAddr, uint32_t *mem, uint32_t value)
{
@@ -105,7 +109,7 @@
rel += MIN_RUN_LEN;
while (rel--)
if (!handleRelNumber(&ofst, type, flashStart, ramStart, mem, 0))
- return false;
+ return false;
break;
case TOKEN_RELOC_TYPE_CHG:
@@ -134,26 +138,27 @@
return true;
}
-bool cpuAppLoad(const struct AppHdr *appHdr, struct PlatAppInfo *platInfo)
+bool cpuAppLoad(const struct AppHdr *app, struct PlatAppInfo *platInfo)
{
- const uint8_t *relocsStart = (const uint8_t*)(((uint8_t*)appHdr) + appHdr->rel_start);
- const uint8_t *relocsEnd = (const uint8_t*)(((uint8_t*)appHdr) + appHdr->rel_end);
- uint8_t *mem = heapAlloc(appHdr->bss_end);
+ const struct SectInfo *sect = &app->sect;
+ const uint8_t *relocsStart = (const uint8_t*)APP_FLASH_RELOC(app, sect->rel_start);
+ const uint8_t *relocsEnd = (const uint8_t*)APP_FLASH_RELOC(app, sect->rel_end);
+ uint8_t *mem = heapAlloc(sect->bss_end);
if (!mem)
return false;
//calcualte and assign got
- platInfo->got = mem + appHdr->data_start;
+ platInfo->got = mem + sect->data_start;
//clear bss
- memset(mem + appHdr->bss_start, 0, appHdr->bss_end - appHdr->bss_start);
+ memset(mem + sect->bss_start, 0, sect->bss_end - sect->bss_start);
//copy initialized data and initialized got
- memcpy(mem + appHdr->data_start, ((uint8_t*)appHdr) + appHdr->data_data, appHdr->got_end - appHdr->data_start);
+ memcpy(mem + sect->data_start, (uint8_t*)APP_FLASH_RELOC(app, sect->data_data), sect->got_end - sect->data_start);
//perform relocs
- if (!handleRelocs(relocsStart, relocsEnd, (uintptr_t)appHdr, (uintptr_t)mem, (void*)mem)) {
+ if (!handleRelocs(relocsStart, relocsEnd, (uintptr_t)APP_FLASH_RELOC_BASE(app), (uintptr_t)mem, (void*)mem)) {
osLog(LOG_ERROR, "Relocs are invalid in this app. Aborting app load\n");
heapFree(mem);
return false;
@@ -162,13 +167,13 @@
return true;
}
-void cpuAppUnload(const struct AppHdr *appHdr, struct PlatAppInfo *platInfo)
+void cpuAppUnload(const struct AppHdr *app, struct PlatAppInfo *platInfo)
{
if (platInfo->got)
- heapFree((uint8_t*)platInfo->got - appHdr->got_start);
+ heapFree((uint8_t*)platInfo->got - app->sect.got_start);
}
-static uintptr_t __attribute__((naked)) callWithR9(const struct AppHdr *appHdr, void *funcOfst, void *got, uintptr_t arg1, uintptr_t arg2)
+static uintptr_t __attribute__((naked)) callWithR9(const void *base, uint32_t offset, void *got, uintptr_t arg1, uintptr_t arg2)
{
asm volatile (
"add r12, r0, r1 \n"
@@ -183,28 +188,26 @@
return 0; //dummy to fool gcc
}
-bool cpuAppInit(const struct AppHdr *appHdr, struct PlatAppInfo *platInfo, uint32_t tid)
+bool cpuAppInit(const struct AppHdr *app, struct PlatAppInfo *platInfo, uint32_t tid)
{
if (platInfo->got)
- return callWithR9(appHdr, appHdr->funcs.init, platInfo->got, tid, 0);
+ return callWithR9((const void*)APP_FLASH_RELOC_BASE(app), app->vec.init, platInfo->got, tid, 0);
else
- return appHdr->funcs.init(tid);
+ return APP_VEC(app)->init(tid);
}
-void cpuAppEnd(const struct AppHdr *appHdr, struct PlatAppInfo *platInfo)
+void cpuAppEnd(const struct AppHdr *app, struct PlatAppInfo *platInfo)
{
if (platInfo->got)
- (void)callWithR9(appHdr, appHdr->funcs.end, platInfo->got, 0, 0);
+ (void)callWithR9((const void*)APP_FLASH_RELOC_BASE(app), app->vec.end, platInfo->got, 0, 0);
else
- appHdr->funcs.end();
+ APP_VEC(app)->end();
}
-void cpuAppHandle(const struct AppHdr *appHdr, struct PlatAppInfo *platInfo, uint32_t evtType, const void* evtData)
+void cpuAppHandle(const struct AppHdr *app, struct PlatAppInfo *platInfo, uint32_t evtType, const void* evtData)
{
if (platInfo->got)
- (void)callWithR9(appHdr, appHdr->funcs.handle, platInfo->got, evtType, (uintptr_t)evtData);
+ (void)callWithR9((const void*)APP_FLASH_RELOC_BASE(app), app->vec.handle, platInfo->got, evtType, (uintptr_t)evtData);
else
- appHdr->funcs.handle(evtType, evtData);
+ APP_VEC(app)->handle(evtType, evtData);
}
-
-
diff --git a/firmware/src/cpu/cortexm4f/atomic.c b/firmware/src/cpu/cortexm4f/atomic.c
index 845d91f..15963cd 100644
--- a/firmware/src/cpu/cortexm4f/atomic.c
+++ b/firmware/src/cpu/cortexm4f/atomic.c
@@ -16,7 +16,7 @@
#include <atomic.h>
-uint32_t atomicAdd(volatile uint32_t *val, uint32_t addend)
+uint32_t atomicAddByte(volatile uint8_t *byte, uint32_t addend)
{
uint32_t prevVal, storeFailed, tmp;
@@ -25,8 +25,26 @@
"ldrexb %0, [%4] \n"
"add %2, %0, %3 \n"
"strexb %1, %2, [%4] \n"
- :"=r"(prevVal), "=r"(storeFailed), "=r"(tmp), "=r"(addend), "=r"(val)
- :"3"(addend), "4"(val)
+ :"=r"(prevVal), "=r"(storeFailed), "=r"(tmp), "=r"(addend), "=r"(byte)
+ :"3"(addend), "4"(byte)
+ :"memory"
+ );
+ } while (storeFailed);
+
+ return prevVal;
+}
+
+uint32_t atomicAdd32bits(volatile uint32_t *word, uint32_t addend)
+{
+ uint32_t prevVal, storeFailed, tmp;
+
+ do {
+ asm volatile(
+ "ldrex %0, [%4] \n"
+ "add %2, %0, %3 \n"
+ "strex %1, %2, [%4] \n"
+ :"=r"(prevVal), "=r"(storeFailed), "=r"(tmp), "=r"(addend), "=r"(word)
+ :"3"(addend), "4"(word)
:"memory"
);
} while (storeFailed);
diff --git a/firmware/src/cpu/x86/atomic.c b/firmware/src/cpu/x86/atomic.c
index dee81b1..20cd248 100644
--- a/firmware/src/cpu/x86/atomic.c
+++ b/firmware/src/cpu/x86/atomic.c
@@ -16,7 +16,7 @@
#include <atomic.h>
-uint32_t atomicAdd(volatile uint32_t *val, uint32_t addend)
+uint32_t atomicAdd32bits(volatile uint32_t *val, uint32_t addend)
{
uint32_t old;
@@ -27,6 +27,17 @@
return old;
}
+uint32_t atomicAddByte(volatile uint8_t *val, uint32_t addend)
+{
+ uint8_t old;
+
+ do {
+ old = *val;
+ } while (!atomicCmpXchgByte(val, old, old + addend));
+
+ return old;
+}
+
uint32_t atomicXchgByte(volatile uint8_t *byte, uint32_t newVal)
{
return __atomic_exchange_n(byte, newVal, __ATOMIC_ACQ_REL);
diff --git a/firmware/src/drivers/ams_tmd4903/ams_tmd4903.c b/firmware/src/drivers/ams_tmd4903/ams_tmd4903.c
index bac97bd..32d7545 100644
--- a/firmware/src/drivers/ams_tmd4903/ams_tmd4903.c
+++ b/firmware/src/drivers/ams_tmd4903/ams_tmd4903.c
@@ -36,7 +36,7 @@
#include <variant/inc/variant.h>
#define AMS_TMD4903_APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 12)
-#define AMS_TMD4903_APP_VERSION 1
+#define AMS_TMD4903_APP_VERSION 6
#ifndef PROX_INT_PIN
#error "PROX_INT_PIN is not defined; please define in variant.h"
@@ -129,12 +129,12 @@
#define PROX_STREAMING 0
#define INFO_PRINT(fmt, ...) do { \
- osLog(LOG_INFO, "[AMS TMD4903] " fmt, ##__VA_ARGS__); \
+ osLog(LOG_INFO, "%s " fmt, "[TMD4903]", ##__VA_ARGS__); \
} while (0);
#define DEBUG_PRINT(fmt, ...) do { \
if (enable_debug) { \
- osLog(LOG_INFO, "[AMS TMD4903] " fmt, ##__VA_ARGS__); \
+ INFO_PRINT(fmt, ##__VA_ARGS__); \
} \
} while (0);
@@ -170,6 +170,7 @@
SENSOR_STATE_DISABLING_PROX_3,
SENSOR_STATE_ALS_SAMPLING,
SENSOR_STATE_PROX_SAMPLING,
+ SENSOR_STATE_PROX_TRANSITION_0,
SENSOR_STATE_IDLE,
};
@@ -205,12 +206,13 @@
union EmbeddedDataPoint lastAlsSample;
- uint8_t proxState; // enum ProxState
+ uint8_t lastProxState; // enum ProxState
bool alsOn;
bool proxOn;
bool alsCalibrating;
bool proxCalibrating;
+ bool proxDirectMode;
};
static struct SensorData mTask;
@@ -240,8 +242,7 @@
static bool proxIsr(struct ChainedIsr *localIsr)
{
struct SensorData *data = container_of(localIsr, struct SensorData, isr);
- bool firstProxSample = (data->proxState == PROX_STATE_INIT);
- uint8_t lastProxState = data->proxState;
+ uint8_t lastProxState = data->lastProxState;
union EmbeddedDataPoint sample;
bool pinState;
@@ -256,17 +257,16 @@
(void)sample;
(void)pinState;
(void)lastProxState;
- (void)firstProxSample;
if (!pinState)
osEnqueuePrivateEvt(EVT_SENSOR_PROX_INTERRUPT, NULL, NULL, mTask.tid);
#else
- if (firstProxSample && !pinState) {
- osEnqueuePrivateEvt(EVT_SENSOR_PROX_INTERRUPT, NULL, NULL, mTask.tid);
- } else if (!firstProxSample) {
+ if (data->proxDirectMode) {
sample.fdata = (pinState) ? AMS_TMD4903_REPORT_FAR_VALUE : AMS_TMD4903_REPORT_NEAR_VALUE;
- data->proxState = (pinState) ? PROX_STATE_FAR : PROX_STATE_NEAR;
- if (data->proxState != lastProxState)
+ data->lastProxState = (pinState) ? PROX_STATE_FAR : PROX_STATE_NEAR;
+ if (data->lastProxState != lastProxState)
osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_PROX), sample.vptr, NULL);
+ } else {
+ osEnqueuePrivateEvt(EVT_SENSOR_PROX_INTERRUPT, NULL, NULL, mTask.tid);
}
#endif
} else if (data->alsOn && data->alsCalibrating && !pinState) {
@@ -277,9 +277,9 @@
return true;
}
-static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
+static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr, enum ExtiTrigger trigger)
{
- extiEnableIntGpio(pin, EXTI_TRIGGER_BOTH);
+ extiEnableIntGpio(pin, trigger);
extiChainIsr(PROX_IRQ, isr);
return true;
}
@@ -304,12 +304,11 @@
osEnqueuePrivateEvt(EVT_SENSOR_ALS_TIMER, cookie, NULL, mTask.tid);
}
-#define GLASS_ATTENUATION 1.08f
-#define DEVICE_FACTOR 300.0f
-#define LUX_PER_COUNTS ((GLASS_ATTENUATION * DEVICE_FACTOR)/AMS_TMD4903_ATIME_MS)
-#define R_COEFF 0.15f
-#define G_COEFF 1.0f
-#define B_COEFF -0.5f
+#define LUX_PER_COUNTS (799.397f/AMS_TMD4903_ATIME_MS)
+#define C_COEFF 2.387f
+#define R_COEFF -1.57f
+#define G_COEFF 2.69f
+#define B_COEFF -3.307f
static inline float getLuxFromAlsData(uint16_t c, uint16_t r, uint16_t g, uint16_t b)
{
@@ -319,17 +318,7 @@
// TODO (trevorbunker): You can use IR ratio (depends on light source) to
// select between different R, G, and B coefficients
- // Remove IR component
- float ir = (r + g + b - c) / 2;
- float r_p = r - ir;
- float g_p = g - ir;
- float b_p = b - ir;
-
- // Calculate total count
- float g_2p = R_COEFF * r_p + G_COEFF * g_p + B_COEFF * b_p;
-
- // Calculate lux
- return (g_2p * LUX_PER_COUNTS) + mTask.alsOffset;
+ return LUX_PER_COUNTS * ((c * C_COEFF) + (r * R_COEFF) + (g * G_COEFF) + (b * B_COEFF)) * mTask.alsOffset;
}
static void sendCalibrationResultAls(uint8_t status, float offset) {
@@ -346,10 +335,8 @@
data->data_header.status = status;
data->offset = offset;
- if (!osEnqueueEvt(EVT_APP_TO_HOST, data, heapFree)) {
- heapFree(data);
+ if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
osLog(LOG_WARN, "Couldn't send als cal result evt");
- }
}
static void sendCalibrationResultProx(uint8_t status, int16_t *offsets) {
@@ -371,16 +358,17 @@
for (i = 0; i < 4; i++)
data->offsets[i] = offsets[i];
- if (!osEnqueueEvt(EVT_APP_TO_HOST, data, heapFree)) {
- heapFree(data);
+ if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
osLog(LOG_WARN, "Couldn't send prox cal result evt");
- }
}
static void setMode(bool alsOn, bool proxOn, void *cookie)
{
mTask.txrxBuf[0] = AMS_TMD4903_REG_ENABLE;
- mTask.txrxBuf[1] = POWER_ON_BIT | (alsOn ? ALS_ENABLE_BIT : 0) | (proxOn ? (PROX_INT_ENABLE_BIT | PROX_ENABLE_BIT) : 0);
+ mTask.txrxBuf[1] =
+ ((alsOn || proxOn) ? POWER_ON_BIT : 0) |
+ (alsOn ? ALS_ENABLE_BIT : 0) |
+ (proxOn ? (PROX_INT_ENABLE_BIT | PROX_ENABLE_BIT) : 0);
i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 2, &i2cCallback, cookie);
}
@@ -435,10 +423,10 @@
mTask.alsOn = true;
mTask.lastAlsSample.idata = AMS_TMD4903_ALS_INVALID;
mTask.alsCalibrating = true;
- mTask.alsOffset = 0.0f;
+ mTask.alsOffset = 1.0f;
extiClearPendingGpio(mTask.pin);
- enableInterrupt(mTask.pin, &mTask.isr);
+ enableInterrupt(mTask.pin, &mTask.isr, EXTI_TRIGGER_FALLING);
mTask.txrxBuf[0] = AMS_TMD4903_REG_ENABLE;
mTask.txrxBuf[1] = POWER_ON_BIT | ALS_ENABLE_BIT | ALS_INT_ENABLE_BIT;
@@ -475,14 +463,15 @@
if (on) {
extiClearPendingGpio(mTask.pin);
- enableInterrupt(mTask.pin, &mTask.isr);
+ enableInterrupt(mTask.pin, &mTask.isr, EXTI_TRIGGER_FALLING);
} else {
disableInterrupt(mTask.pin, &mTask.isr);
extiClearPendingGpio(mTask.pin);
}
- mTask.proxState = PROX_STATE_INIT;
+ mTask.lastProxState = PROX_STATE_INIT;
mTask.proxOn = on;
+ mTask.proxDirectMode = false;
setMode(mTask.alsOn, on, (void *)(on ? SENSOR_STATE_ENABLING_PROX : SENSOR_STATE_DISABLING_PROX));
return true;
@@ -519,12 +508,13 @@
return false;
}
- mTask.proxState = PROX_STATE_INIT;
+ mTask.lastProxState = PROX_STATE_INIT;
mTask.proxOn = true;
mTask.proxCalibrating = true;
+ mTask.proxDirectMode = false;
extiClearPendingGpio(mTask.pin);
- enableInterrupt(mTask.pin, &mTask.isr);
+ enableInterrupt(mTask.pin, &mTask.isr, EXTI_TRIGGER_FALLING);
mTask.txrxBuf[0] = AMS_TMD4903_REG_ENABLE;
mTask.txrxBuf[1] = POWER_ON_BIT; // REG_ENABLE
@@ -556,8 +546,8 @@
bool result = true;
// See note in sendLastSampleAls
- if (mTask.proxState != PROX_STATE_INIT) {
- sample.fdata = (mTask.proxState == PROX_STATE_NEAR) ? AMS_TMD4903_REPORT_NEAR_VALUE : AMS_TMD4903_REPORT_FAR_VALUE;
+ if (mTask.lastProxState != PROX_STATE_INIT) {
+ sample.fdata = (mTask.lastProxState == PROX_STATE_NEAR) ? AMS_TMD4903_REPORT_NEAR_VALUE : AMS_TMD4903_REPORT_FAR_VALUE;
result = osEnqueuePrivateEvt(sensorGetMyEventType(SENS_TYPE_PROX), sample.vptr, NULL, tid);
}
return result;
@@ -759,7 +749,6 @@
mTask.alsOn = false;
mTask.alsCalibrating = false;
- mTask.alsOffset = sample.fdata;
mTask.txrxBuf[0] = AMS_TMD4903_REG_ENABLE;
mTask.txrxBuf[1] = 0; // REG_ENABLE
@@ -774,7 +763,7 @@
case SENSOR_STATE_PROX_SAMPLING:
ps = *(uint16_t*)(mTask.txrxBuf);
- lastProxState = mTask.proxState;
+ lastProxState = mTask.lastProxState;
DEBUG_PRINT("prox sample ready: prox=%u\n", ps);
@@ -786,13 +775,13 @@
#else
if (ps > AMS_TMD4903_PROX_THRESHOLD_HIGH) {
sample.fdata = AMS_TMD4903_REPORT_NEAR_VALUE;
- mTask.proxState = PROX_STATE_NEAR;
+ mTask.lastProxState = PROX_STATE_NEAR;
} else {
sample.fdata = AMS_TMD4903_REPORT_FAR_VALUE;
- mTask.proxState = PROX_STATE_FAR;
+ mTask.lastProxState = PROX_STATE_FAR;
}
- if (mTask.proxState != lastProxState)
+ if (mTask.lastProxState != lastProxState)
osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_PROX), sample.vptr, NULL);
#endif
@@ -802,13 +791,32 @@
mTask.txrxBuf[1] = 0x60; // REG_INTCLEAR - reset proximity interrupts
i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 2, &i2cCallback, (void *)SENSOR_STATE_IDLE);
#else
- // After the first prox sample, change the proximity interrupt setting to follow proximity state
- mTask.txrxBuf[0] = AMS_TMD4903_REG_CFG4;
- mTask.txrxBuf[1] = 0x27; // REG_CFG4 - proximity state direct to interrupt pin
- i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 2, &i2cCallback, (void *)SENSOR_STATE_IDLE);
+ // The TMD4903 direct interrupt mode does not work properly if enabled while something is covering the sensor,
+ // so we need to wait until it is far.
+ if (mTask.lastProxState == PROX_STATE_FAR) {
+ disableInterrupt(mTask.pin, &mTask.isr);
+ extiClearPendingGpio(mTask.pin);
+
+ // Switch to proximity interrupt direct mode
+ mTask.txrxBuf[0] = AMS_TMD4903_REG_CFG4;
+ mTask.txrxBuf[1] = 0x27; // REG_CFG4 - proximity state direct to interrupt pin
+ i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 2, &i2cCallback, (void *)SENSOR_STATE_PROX_TRANSITION_0);
+ } else {
+ // If we are in the "near" state, we cannot change to direct interrupt mode, so just clear the interrupt
+ mTask.txrxBuf[0] = AMS_TMD4903_REG_INTCLEAR;
+ mTask.txrxBuf[1] = 0x60; // REG_INTCLEAR - reset proximity interrupts
+ i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 2, &i2cCallback, (void *)SENSOR_STATE_IDLE);
+ }
#endif
}
+ break;
+ case SENSOR_STATE_PROX_TRANSITION_0:
+ if (mTask.proxOn) {
+ mTask.proxDirectMode = true;
+ extiClearPendingGpio(mTask.pin);
+ enableInterrupt(mTask.pin, &mTask.isr, EXTI_TRIGGER_BOTH);
+ }
break;
default:
@@ -829,8 +837,9 @@
mTask.alsOn = false;
mTask.proxOn = false;
mTask.lastAlsSample.idata = AMS_TMD4903_ALS_INVALID;
- mTask.proxState = PROX_STATE_INIT;
+ mTask.lastProxState = PROX_STATE_INIT;
mTask.proxCalibrating = false;
+ mTask.alsOffset = 1.0f;
mTask.pin = gpioRequest(PROX_INT_PIN);
gpioConfigInput(mTask.pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
diff --git a/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.c b/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.c
index fb987ba..371f997 100644
--- a/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.c
+++ b/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.c
@@ -19,7 +19,7 @@
#define kScale_mag 0.15f
-void parseMagData(MagTask_t *magTask, uint8_t *buf, float *x, float *y, float *z) {
+void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z) {
int32_t raw_x = (*(int16_t *)&buf[0]);
int32_t raw_y = (*(int16_t *)&buf[2]);
int32_t raw_z = (*(int16_t *)&buf[4]);
diff --git a/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.h b/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.h
index 8c683d8..d7bb12e 100644
--- a/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.h
+++ b/firmware/src/drivers/bosch_bmi160/akm_ak09915_slave.h
@@ -31,14 +31,14 @@
#define AKM_AK09915_REG_CNTL1 0x30
#define AKM_AK09915_REG_CNTL2 0x31
-typedef struct AK09915Task {
+struct MagTask {
int32_t dummy;
-} MagTask_t;
+};
#define MAG_I2C_ADDR 0x0C
#define MAG_REG_DATA AKM_AK09915_REG_DATA
-void parseMagData(MagTask_t *magTask, uint8_t *buf, float *x, float *y, float *z);
+void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z);
#ifdef __cplusplus
}
diff --git a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
index 352b0d6..d682af0 100644
--- a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
@@ -14,33 +14,34 @@
* limitations under the License.
*/
-#include <stdlib.h>
-#include <string.h>
-#include <timer.h>
-#include <sensors.h>
-#include <heap.h>
-#include <spi.h>
-#include <slab.h>
-#include <limits.h>
-#include <plat/inc/rtc.h>
-#include <plat/inc/gpio.h>
-#include <plat/inc/exti.h>
-#include <plat/inc/syscfg.h>
-#include <gpio.h>
-#include <isr.h>
-#include <hostIntf.h>
-#include <nanohubPacket.h>
-#include <variant/inc/variant.h>
-#include <variant/inc/sensType.h>
+#include <algos/time_sync.h>
+#include <atomic.h>
#include <cpu/inc/cpuMath.h>
-
+#include <gpio.h>
+#include <heap.h>
+#include <hostIntf.h>
+#include <isr.h>
+#include <nanohub_math.h>
+#include <nanohubPacket.h>
+#include <plat/inc/exti.h>
+#include <plat/inc/gpio.h>
+#include <plat/inc/syscfg.h>
+#include <plat/inc/rtc.h>
+#include <sensors.h>
#include <seos.h>
+#include <slab.h>
+#include <spi.h>
+#include <timer.h>
+#include <variant/inc/sensType.h>
+#include <variant/inc/variant.h>
#ifdef MAG_SLAVE_PRESENT
#include <algos/mag_cal.h>
#endif
-#include <algos/time_sync.h>
-#include <nanohub_math.h>
+
+#include <limits.h>
+#include <stdlib.h>
+#include <string.h>
#define INFO_PRINT(fmt, ...) do { \
osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
@@ -51,13 +52,23 @@
} while (0);
#define DEBUG_PRINT(fmt, ...) do { \
- if (enable_debug) { \
+ if (DBG_ENABLE) { \
INFO_PRINT(fmt, ##__VA_ARGS__); \
} \
} while (0);
-static const bool enable_debug = 0;
+#define DEBUG_PRINT_IF(cond, fmt, ...) do { \
+ if ((cond) && DBG_ENABLE) { \
+ INFO_PRINT(fmt, ##__VA_ARGS__); \
+ } \
+ } while (0);
+#define DBG_ENABLE 0
+#define DBG_CHUNKED 0
+#define DBG_INT 0
+#define DBG_SHALLOW_PARSE 0
+#define DBG_STATE 0
+#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
// fixme: to list required definitions for a slave mag
@@ -153,6 +164,15 @@
#define INT_FIFO_WM 0x40
#define INT_NO_MOTION 0x80
+#define BMI160_FRAME_HEADER_INVALID 0x80 // mark the end of valid data
+#define BMI160_FRAME_HEADER_SKIP 0x81 // not defined by hw, used for skip a byte in buffer
+
+#define WATERMARK_MIN 1
+#define WATERMARK_MAX 200 // must <= 255 (0xff)
+
+#define WATERMARK_MAX_SENSOR_RATE 400 // Accel and gyro are 400 Hz max
+#define WATERMARK_TIME_UNIT_NS (1000000000ULL/(WATERMARK_MAX_SENSOR_RATE))
+
#define gSPI BMI160_SPI_BUS_ID
#define ACCL_INT_LINE EXTI_LINE_P6
@@ -189,7 +209,8 @@
#define kSensorTimerIntervalUs 39ull // bmi160 clock increaments every 39000ns
#define kMinRTCTimeIncrementNs 1250000ull // forced min rtc time increment, 1.25ms for 400Hz
-#define kMinSensorTimeIncrement 64 // forced min sensortime increment, 64 = 2.5 msec for 400Hz
+#define kMinSensorTimeIncrement 64 // forced min sensortime increment,
+ // 64 = 2.5 msec for 400Hz
#define ACC_MIN_RATE 5
#define GYR_MIN_RATE 6
@@ -207,9 +228,10 @@
#define RETRY_CNT_MAG 30
#define SPI_PACKET_SIZE 30
-#define FIFO_READ_SIZE 1028
-#define BUF_MARGIN 16 // some extra buffer for additional reg RW when a FIFO read happens
-#define SPI_BUF_SIZE (FIFO_READ_SIZE + BUF_MARGIN)
+#define FIFO_READ_SIZE (1024+4)
+#define CHUNKED_READ_SIZE (64)
+#define BUF_MARGIN 32 // some extra buffer for additional reg RW when a FIFO read happens
+#define SPI_BUF_SIZE (FIFO_READ_SIZE + CHUNKED_READ_SIZE + BUF_MARGIN)
enum SensorIndex {
ACC = 0,
@@ -263,7 +285,18 @@
SENSOR_STEP_CNT,
SENSOR_TIME_SYNC,
SENSOR_SAVE_CALIBRATION,
+ SENSOR_NUM_OF_STATE
};
+static const char * getStateName(int32_t s) {
+#if DBG_STATE
+ static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP",
+ "PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "STEP_CNT", "SYNC", "SAVE_CALIB"};
+ if (s >= 0 && s < SENSOR_NUM_OF_STATE) {
+ return l[s];
+ }
+#endif
+ return "???";
+}
enum MagConfigState {
MAG_SET_START,
@@ -355,6 +388,10 @@
bool magBiasCurrent;
bool fifo_enabled[3];
+ // for step count
+ uint32_t stepCntSamplingTimerHandle;
+ bool step_cnt_changed;
+
// spi buffers
int xferCnt;
uint8_t *dataBuffer;
@@ -364,13 +401,14 @@
uint8_t txrxBuffer[SPI_BUF_SIZE];
// states
+ volatile uint8_t state; //task state, type enum SensorState, do NOT change this directly
enum InitState init_state;
enum MagConfigState mag_state;
- enum SensorState state;
enum CalibrationState calibration_state;
// pending configs
bool pending_int[2];
+ bool pending_step_cnt;
bool pending_config[NUM_OF_SENSOR];
bool pending_calibration_save;
bool pending_time_sync;
@@ -378,6 +416,16 @@
bool pending_dispatch;
bool frame_sensortime_valid;
+ // FIFO setting
+ uint16_t chunkReadSize;
+ uint8_t watermark;
+
+ // spi rw
+ struct SlabAllocator *mDataSlab;
+ uint16_t mWbufCnt;
+ uint8_t mRegCnt;
+ uint8_t mRetryLeft;
+ bool spiInUse;
};
static uint32_t AccRates[] = {
@@ -415,20 +463,40 @@
};
static uint32_t StepCntRates[] = {
+ SENSOR_HZ(1.0f/300.0f),
+ SENSOR_HZ(1.0f/240.0f),
+ SENSOR_HZ(1.0f/180.0f),
+ SENSOR_HZ(1.0f/120.0f),
+ SENSOR_HZ(1.0f/90.0f),
+ SENSOR_HZ(1.0f/60.0f),
+ SENSOR_HZ(1.0f/45.0f),
+ SENSOR_HZ(1.0f/30.0f),
+ SENSOR_HZ(1.0f/15.0f),
+ SENSOR_HZ(1.0f/10.0f),
+ SENSOR_HZ(1.0f/5.0f),
SENSOR_RATE_ONCHANGE,
0
};
+static const uint64_t stepCntRateTimerVals[] = // should match StepCntRates and be the timer length for that rate in nanosecs
+{
+ 300 * 1000000000ULL,
+ 240 * 1000000000ULL,
+ 180 * 1000000000ULL,
+ 120 * 1000000000ULL,
+ 90 * 1000000000ULL,
+ 60 * 1000000000ULL,
+ 45 * 1000000000ULL,
+ 30 * 1000000000ULL,
+ 15 * 1000000000ULL,
+ 10 * 1000000000ULL,
+ 5 * 1000000000ULL,
+};
+
static struct BMI160Task mTask;
-static uint16_t mWbufCnt = 0;
-static uint8_t mRegCnt = 0;
-static uint8_t mRetryLeft;
-
-static struct SlabAllocator *mDataSlab;
-
-#ifdef MAG_I2C_ADDR
-static MagTask_t magTask;
+#ifdef MAG_SLAVE_PRESENT
+static struct MagTask magTask;
#endif
#define MAG_WRITE(addr, data) \
@@ -467,17 +535,69 @@
.flags1 = SENSOR_INFO_FLAGS1_BIAS, \
.biasType = bias
+typedef struct BMI160Task _Task;
+#define TASK _Task* const _task
+
+// To get rid of static variables all task functions should have a task structure pointer input.
+// This is an intermediate step.
+#define TDECL() TASK = &mTask; (void)_task
+
+// Access task variables without explicitly specify the task structure pointer.
+#define T(v) (_task->v)
+
+// Atomic get state
+#define GET_STATE() (atomicReadByte(&(_task->state)))
+
+// Atomic set state, this set the state to arbitrary value, use with caution
+#define SET_STATE(s) do{\
+ DEBUG_PRINT_IF(DBG_STATE, "set state %s\n", getStateName(s));\
+ atomicWriteByte(&(_task->state), (s));\
+ }while(0)
+
+// Atomic switch state from IDLE to desired state.
+static bool trySwitchState_(TASK, enum SensorState newState) {
+#if DBG_STATE
+ bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
+ uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
+ DEBUG_PRINT("switch state %s->%s, %s\n",
+ getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
+ return ret;
+#else
+ return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
+#endif
+}
+// Short-hand
+#define trySwitchState(s) trySwitchState_(_task, (s))
+
+// Chunked FIFO read functions
+static void chunkedReadInit_(TASK, int index, int size);
+#define chunkedReadInit(a,b) chunkedReadInit_(_task, (a), (b))
+static void chunkedReadSpiCallback(void *cookie, int error);
+static void initiateFifoRead_(TASK, bool isInterruptContext);
+#define initiateFifoRead(a) initiateFifoRead_(_task, (a))
+static uint8_t* shallowParseFrame(uint8_t * buf, int size);
+
+// Watermark calculation
+static uint8_t calcWatermark2_(TASK);
+#define calcWatermark2() calcWatermark2_(_task)
+
static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
{
- { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
- { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) },
- { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
- { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 100) },
- { DEC_INFO("Double Tap", SENS_TYPE_DOUBLE_TAP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
+ { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
+ { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 20) },
+ { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
+ { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
+ NANOHUB_INT_NONWAKEUP, 100) },
+ { DEC_INFO("Double Tap", SENS_TYPE_DOUBLE_TAP, NUM_AXIS_EMBEDDED,
+ NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("Flat", SENS_TYPE_FLAT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("Any Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("No Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
- { DEC_INFO_RATE("Step Counter", StepCntRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
+ { DEC_INFO_RATE("Step Counter", StepCntRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED,
+ NANOHUB_INT_NONWAKEUP, 20) },
};
static void time_init(void) {
@@ -511,19 +631,25 @@
static void dataEvtFree(void *ptr)
{
+ TDECL();
struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr;
- slabAllocatorFree(mDataSlab, ev);
+ slabAllocatorFree(T(mDataSlab), ev);
}
static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
{
- mTask.packets[mRegCnt].size = 2;
- mTask.packets[mRegCnt].txBuf = &mTask.txrxBuffer[mWbufCnt];
- mTask.packets[mRegCnt].rxBuf = &mTask.txrxBuffer[mWbufCnt];
- mTask.packets[mRegCnt].delay = delay * 1000;
- mTask.txrxBuffer[mWbufCnt++] = BMI160_SPI_WRITE | addr;
- mTask.txrxBuffer[mWbufCnt++] = data;
- mRegCnt++;
+ TDECL();
+ if (T(spiInUse)) {
+ ERROR_PRINT("SPI in use, cannot queue write\n");
+ return;
+ }
+ T(packets[T(mRegCnt)]).size = 2;
+ T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).delay = delay * 1000;
+ T(txrxBuffer[T(mWbufCnt++)]) = BMI160_SPI_WRITE | addr;
+ T(txrxBuffer[T(mWbufCnt++)]) = data;
+ T(mRegCnt)++;
}
/*
@@ -531,61 +657,72 @@
*/
static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
{
- *buf = &mTask.txrxBuffer[mWbufCnt];
- mTask.packets[mRegCnt].size = size + 1;
- mTask.packets[mRegCnt].txBuf = &mTask.txrxBuffer[mWbufCnt];
- mTask.packets[mRegCnt].rxBuf = *buf;
- mTask.packets[mRegCnt].delay = delay * 1000;
- mTask.txrxBuffer[mWbufCnt++] = BMI160_SPI_READ | addr;
- mWbufCnt += size;
- mRegCnt++;
+ TDECL();
+ if (T(spiInUse)) {
+ ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size);
+ return;
+ }
+
+ *buf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data
+ T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).rxBuf = *buf;
+ T(packets[T(mRegCnt)]).delay = delay * 1000;
+ T(txrxBuffer[T(mWbufCnt)++]) = BMI160_SPI_READ | addr;
+ T(mWbufCnt) += size;
+ T(mRegCnt)++;
}
static void spiBatchTxRx(struct SpiMode *mode,
- SpiCbkF callback, void *cookie)
+ SpiCbkF callback, void *cookie, const char * src)
{
- if (mWbufCnt > SPI_BUF_SIZE) {
+ TDECL();
+ if (T(mWbufCnt) > SPI_BUF_SIZE) {
ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n");
return;
}
- if (mRegCnt > SPI_PACKET_SIZE) {
+ if (T(mRegCnt) > SPI_PACKET_SIZE) {
ERROR_PRINT("spiBatchTxRx too many packets!\n");
return;
}
- spiMasterRxTx(mTask.spiDev, mTask.cs,
- mTask.packets, mRegCnt, mode, callback, cookie);
- mRegCnt = 0;
- mWbufCnt = 0;
+ T(spiInUse) = true;
+ spiMasterRxTx(T(spiDev), T(cs), T(packets), T(mRegCnt), mode, callback, cookie);
+ T(mRegCnt) = 0;
+ T(mWbufCnt) = 0;
}
+
static bool bmi160Isr1(struct ChainedIsr *isr)
{
- struct BMI160Task *data = container_of(isr, struct BMI160Task, Isr1);
+ TASK = container_of(isr, struct BMI160Task, Isr1);
- if (!extiIsPendingGpio(data->Int1)) {
+ if (!extiIsPendingGpio(T(Int1))) {
return false;
}
-
- osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, data, NULL, mTask.tid);
- extiClearPendingGpio(data->Int1);
+ DEBUG_PRINT_IF(DBG_INT, "i1\n");
+ initiateFifoRead(true /*isInterruptContext*/);
+ extiClearPendingGpio(T(Int1));
return true;
}
+
static bool bmi160Isr2(struct ChainedIsr *isr)
{
- struct BMI160Task *data = container_of(isr, struct BMI160Task, Isr2);
+ TASK = container_of(isr, struct BMI160Task, Isr2);
- if (!extiIsPendingGpio(data->Int2))
+ if (!extiIsPendingGpio(T(Int2)))
return false;
- osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_2, data, NULL, mTask.tid);
- extiClearPendingGpio(data->Int2);
+ DEBUG_PRINT_IF(DBG_INT, "i2\n");
+ osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_2, _task, NULL, T(tid));
+ extiClearPendingGpio(T(Int2));
return true;
}
static void sensorSpiCallback(void *cookie, int err)
{
+ mTask.spiInUse = false;
osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid);
}
@@ -599,6 +736,17 @@
osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid);
}
+static void stepCntSamplingCallback(uint32_t timerId, void *data)
+{
+ union EmbeddedDataPoint step_cnt;
+
+ if (mTask.sensors[STEPCNT].powered && mTask.step_cnt_changed) {
+ mTask.step_cnt_changed = false;
+ step_cnt.idata = mTask.total_step_cnt;
+ osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
+ }
+}
+
static bool accFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[ACC].handle,
@@ -700,7 +848,7 @@
SPI_WRITE(BMI160_REG_MAGIC, 0x80);
// Config the MAG I2C device address
-#ifdef MAG_I2C_ADDR
+#ifdef MAG_SLAVE_PRESENT
SPI_WRITE(BMI160_REG_MAG_IF_0, (MAG_I2C_ADDR << 1));
#endif
@@ -783,7 +931,7 @@
break;
case MAG_SET_ADDR:
// config MAG read data address to the first data register
-#ifdef MAG_I2C_ADDR
+#ifdef MAG_SLAVE_PRESENT
SPI_WRITE(BMI160_REG_MAG_IF_2, MAG_REG_DATA);
#endif
mTask.mag_state = MAG_SET_DATA;
@@ -802,56 +950,6 @@
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
}
-static uint8_t calcWatermark(void)
-{
- int i;
- uint64_t min_latency = ULONG_LONG_MAX;
- uint32_t max_rate = 0;
- uint8_t min_watermark = 6;
- uint8_t max_watermark = 200;
- uint8_t watermark;
- uint32_t temp_cnt, total_cnt = 0;
- uint32_t header_cnt = ULONG_MAX;
-
- for (i = ACC; i <= MAG; i++) {
- if (mTask.sensors[i].configed && mTask.sensors[i].latency != SENSOR_LATENCY_NODATA) {
- min_latency = mTask.sensors[i].latency < min_latency ? mTask.sensors[i].latency : min_latency;
- max_rate = mTask.sensors[i].rate > max_rate ? mTask.sensors[i].rate : max_rate;
- }
- }
-
- // if max_rate is less than or equal to 50Hz, we lower the minimum watermark level
- if (max_rate <= SENSOR_HZ(50.0f)) {
- min_watermark = 3;
- }
-
- // if any sensor request no batching, we set a minimum watermark
- // of 24 bytes (12 bytes if all rates are below 50Hz).
- if (min_latency == 0) {
- return min_watermark;
- }
-
- // each accel and gyro sample are 6 bytes
- // each mag samlpe is 8 bytes
- // the total number of header byte is estimated by the min samples
- // the actual number of header byte may exceed this estimate but it's ok to
- // batch a bit faster.
- for (i = ACC; i <= MAG; i++) {
- if (mTask.sensors[i].configed && mTask.sensors[i].latency != SENSOR_LATENCY_NODATA) {
-
- temp_cnt = (uint32_t)U64_DIV_BY_U64_CONSTANT(min_latency * (mTask.sensors[i].rate / 1024), 1000000000ull);
- header_cnt = temp_cnt < header_cnt ? temp_cnt : header_cnt;
- total_cnt += temp_cnt * (i == MAG ? 8 : 6);
- }
- }
- total_cnt += header_cnt;
- watermark = ((total_cnt / 4) < 0xff) ? (total_cnt / 4) : 0xff; // 4 bytes per count in the watermark register.
- watermark = watermark < min_watermark ? min_watermark : watermark;
- watermark = watermark > max_watermark ? max_watermark : watermark;
-
- return watermark;
-}
-
static inline bool anyFifoEnabled(void)
{
return (mTask.fifo_enabled[ACC] || mTask.fifo_enabled[GYR] || mTask.fifo_enabled[MAG]);
@@ -859,6 +957,7 @@
static void configFifo(void)
{
+ TDECL();
int i;
uint8_t val = 0x12;
bool any_fifo_enabled_prev = anyFifoEnabled();
@@ -909,7 +1008,9 @@
// calculate the new watermark level
if (anyFifoEnabled()) {
- SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, calcWatermark());
+ mTask.watermark = calcWatermark2_(_task);
+ DEBUG_PRINT("wm=%d", mTask.watermark);
+ SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, mTask.watermark);
}
// config the fifo register
@@ -928,24 +1029,21 @@
static bool accPower(bool on, void *cookie)
{
- INFO_PRINT("accPower: on=%d, state=%d\n", on, mTask.state);
+ TDECL();
- if (mTask.state == SENSOR_IDLE) {
+ INFO_PRINT("accPower: on=%d, state=%s\n", on, getStateName(GET_STATE()));
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
-
// set ACC power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
} else {
- mTask.state = SENSOR_POWERING_DOWN;
-
// set ACC power mode to SUSPEND
mTask.sensors[ACC].configed = false;
configFifo();
SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
}
mTask.sensors[ACC].powered = on;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
} else {
mTask.pending_config[ACC] = true;
mTask.sensors[ACC].pConfig.enable = on;
@@ -955,21 +1053,18 @@
static bool gyrPower(bool on, void *cookie)
{
- INFO_PRINT("gyrPower: on=%d, state=%d\n", on, mTask.state);
+ TDECL();
+ INFO_PRINT("gyrPower: on=%d, state=%s\n", on, getStateName(GET_STATE()));
- if (mTask.state == SENSOR_IDLE) {
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
-
// set GYR power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
} else {
- mTask.state = SENSOR_POWERING_DOWN;
-
// set GYR power mode to SUSPEND
mTask.sensors[GYR].configed = false;
configFifo();
- SPI_WRITE(BMI160_REG_CMD, 0x14, 1000);
+ SPI_WRITE(BMI160_REG_CMD, 0x14, 5000);
}
if (anyFifoEnabled() && on != mTask.sensors[GYR].powered) {
@@ -980,7 +1075,7 @@
}
mTask.sensors[GYR].powered = on;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
} else {
mTask.pending_config[GYR] = true;
mTask.sensors[GYR].pConfig.enable = on;
@@ -990,24 +1085,20 @@
static bool magPower(bool on, void *cookie)
{
- INFO_PRINT("magPower: on=%d, state=%d\n", on, mTask.state);
-
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ INFO_PRINT("magPower: on=%d, state=%s\n", on, getStateName(GET_STATE()));
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
-
// set MAG power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x19, 10000);
} else {
- mTask.state = SENSOR_POWERING_DOWN;
-
// set MAG power mode to SUSPEND
mTask.sensors[MAG].configed = false;
configFifo();
- SPI_WRITE(BMI160_REG_CMD, 0x18, 1000);
+ SPI_WRITE(BMI160_REG_CMD, 0x18, 5000);
}
mTask.sensors[MAG].powered = on;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
} else {
mTask.pending_config[MAG] = true;
mTask.sensors[MAG].pConfig.enable = on;
@@ -1017,22 +1108,22 @@
static bool stepPower(bool on, void *cookie)
{
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
// if step counter is powered, no need to change actual config of step
// detector.
// But we choose to perform one SPI_WRITE anyway to go down the code path
// to state SENSOR_POWERING_UP/DOWN to update sensor manager.
if (on) {
- mTask.state = SENSOR_POWERING_UP;
mTask.interrupt_enable_2 |= 0x08;
} else {
- mTask.state = SENSOR_POWERING_DOWN;
- mTask.interrupt_enable_2 &= ~0x08;
+ if (!mTask.sensors[STEPCNT].powered)
+ mTask.interrupt_enable_2 &= ~0x08;
mTask.sensors[STEP].configed = false;
}
mTask.sensors[STEP].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP], __FUNCTION__);
} else {
mTask.pending_config[STEP] = true;
mTask.sensors[STEP].pConfig.enable = on;
@@ -1042,18 +1133,17 @@
static bool flatPower(bool on, void *cookie)
{
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
mTask.interrupt_enable_0 |= 0x80;
} else {
- mTask.state = SENSOR_POWERING_DOWN;
mTask.interrupt_enable_0 &= ~0x80;
mTask.sensors[FLAT].configed = false;
}
mTask.sensors[FLAT].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT], __FUNCTION__);
} else {
mTask.pending_config[FLAT] = true;
mTask.sensors[FLAT].pConfig.enable = on;
@@ -1063,18 +1153,17 @@
static bool doubleTapPower(bool on, void *cookie)
{
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
mTask.interrupt_enable_0 |= 0x10;
} else {
- mTask.state = SENSOR_POWERING_DOWN;
mTask.interrupt_enable_0 &= ~0x10;
mTask.sensors[DTAP].configed = false;
}
mTask.sensors[DTAP].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP], __FUNCTION__);
} else {
mTask.pending_config[DTAP] = true;
mTask.sensors[DTAP].pConfig.enable = on;
@@ -1084,20 +1173,20 @@
static bool anyMotionPower(bool on, void *cookie)
{
- DEBUG_PRINT("anyMotionPower: on=%d, oneshot_cnt %d, state=%d\n", on, mTask.active_oneshot_sensor_cnt, mTask.state);
+ TDECL();
+ DEBUG_PRINT("anyMotionPower: on=%d, oneshot_cnt %d, state=%s\n",
+ on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
- if (mTask.state == SENSOR_IDLE) {
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
mTask.interrupt_enable_0 |= 0x07;
} else {
- mTask.state = SENSOR_POWERING_DOWN;
mTask.interrupt_enable_0 &= ~0x07;
mTask.sensors[ANYMO].configed = false;
}
mTask.sensors[ANYMO].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO], __FUNCTION__);
} else {
mTask.pending_config[ANYMO] = true;
mTask.sensors[ANYMO].pConfig.enable = on;
@@ -1107,20 +1196,19 @@
static bool noMotionPower(bool on, void *cookie)
{
- DEBUG_PRINT("noMotionPower: on=%d, oneshot_cnt %d, state=%d\n", on, mTask.active_oneshot_sensor_cnt, mTask.state);
-
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ DEBUG_PRINT("noMotionPower: on=%d, oneshot_cnt %d, state=%s\n",
+ on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
mTask.interrupt_enable_2 |= 0x07;
} else {
- mTask.state = SENSOR_POWERING_DOWN;
mTask.interrupt_enable_2 &= ~0x07;
mTask.sensors[NOMO].configed = false;
}
mTask.sensors[NOMO].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO], __FUNCTION__);
} else {
mTask.pending_config[NOMO] = true;
mTask.sensors[NOMO].pConfig.enable = on;
@@ -1130,9 +1218,9 @@
static bool stepCntPower(bool on, void *cookie)
{
- if (mTask.state == SENSOR_IDLE) {
+ TDECL();
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
- mTask.state = SENSOR_POWERING_UP;
if (!mTask.sensors[STEP].powered) {
mTask.interrupt_enable_2 |= 0x08;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
@@ -1140,7 +1228,10 @@
// set step_cnt_en bit
SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x08 | 0x03, 1000);
} else {
- mTask.state = SENSOR_POWERING_DOWN;
+ if (mTask.stepCntSamplingTimerHandle) {
+ timTimerCancel(mTask.stepCntSamplingTimerHandle);
+ mTask.stepCntSamplingTimerHandle = 0;
+ }
if (!mTask.sensors[STEP].powered) {
mTask.interrupt_enable_2 &= ~0x08;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2);
@@ -1151,7 +1242,7 @@
mTask.sensors[STEPCNT].configed = false;
}
mTask.sensors[STEPCNT].powered = on;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
} else {
mTask.pending_config[STEPCNT] = true;
mTask.sensors[STEPCNT].pConfig.enable = on;
@@ -1212,13 +1303,13 @@
static bool accSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
+ TDECL();
int odr, osr = 0;
- INFO_PRINT("accSetRate: rate=%ld, latency=%lld, state=%d\n", rate, latency, mTask.state);
+ INFO_PRINT("accSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency,
+ getStateName(GET_STATE()));
- if (mTask.state == SENSOR_IDLE) {
- mTask.state = SENSOR_CONFIG_CHANGING;
-
+ if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
odr = computeOdr(rate);
if (!odr) {
ERROR_PRINT("invalid acc rate\n");
@@ -1260,7 +1351,7 @@
// flush the data and configure the fifo
configFifo();
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
} else {
mTask.pending_config[ACC] = true;
mTask.sensors[ACC].pConfig.enable = 1;
@@ -1272,13 +1363,12 @@
static bool gyrSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
+ TDECL();
int odr, osr = 0;
+ INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency,
+ getStateName(GET_STATE()));
- INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%d\n", rate, latency, mTask.state);
-
- if (mTask.state == SENSOR_IDLE) {
- mTask.state = SENSOR_CONFIG_CHANGING;
-
+ if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
odr = computeOdr(rate);
if (!odr) {
ERROR_PRINT("invalid gyr rate\n");
@@ -1317,7 +1407,7 @@
// flush the data and configure the fifo
configFifo();
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
} else {
mTask.pending_config[GYR] = true;
mTask.sensors[GYR].pConfig.enable = 1;
@@ -1329,16 +1419,16 @@
static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
+ TDECL();
int odr;
if (rate == SENSOR_RATE_ONCHANGE)
rate = SENSOR_HZ(100);
- INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%d\n", rate, latency, mTask.state);
+ INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency,
+ getStateName(GET_STATE()));
- if (mTask.state == SENSOR_IDLE) {
- mTask.state = SENSOR_CONFIG_CHANGING;
-
+ if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
mTask.sensors[MAG].rate = rate;
mTask.sensors[MAG].latency = latency;
mTask.sensors[MAG].configed = true;
@@ -1359,7 +1449,7 @@
// flush the data and configure the fifo
configFifo();
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
} else {
mTask.pending_config[MAG] = true;
mTask.sensors[MAG].pConfig.enable = 1;
@@ -1431,6 +1521,17 @@
mTask.sensors[STEPCNT].latency = latency;
mTask.sensors[STEPCNT].configed = true;
+ if (rate == SENSOR_RATE_ONCHANGE && mTask.stepCntSamplingTimerHandle) {
+ timTimerCancel(mTask.stepCntSamplingTimerHandle);
+ mTask.stepCntSamplingTimerHandle = 0;
+ } else if (rate != SENSOR_RATE_ONCHANGE) {
+ if (mTask.stepCntSamplingTimerHandle) {
+ timTimerCancel(mTask.stepCntSamplingTimerHandle);
+ }
+ mTask.stepCntSamplingTimerHandle = timTimerSet(sensorTimerLookupCommon(StepCntRates, stepCntRateTimerVals, rate),
+ 0, 50, stepCntSamplingCallback, NULL, false);
+ }
+
sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
@@ -1452,26 +1553,27 @@
}
}
-static void int1Evt(void);
-
static bool accFlush(void *cookie)
{
+ TDECL();
mTask.sensors[ACC].flush++;
- int1Evt();
+ initiateFifoRead(false /*isInterruptContext*/);
return true;
}
static bool gyrFlush(void *cookie)
{
+ TDECL();
mTask.sensors[GYR].flush++;
- int1Evt();
+ initiateFifoRead(false /*isInterruptContext*/);
return true;
}
static bool magFlush(void *cookie)
{
+ TDECL();
mTask.sensors[MAG].flush++;
- int1Evt();
+ initiateFifoRead(false /*isInterruptContext*/);
return true;
}
@@ -1500,13 +1602,15 @@
return osEnqueueEvt(EVT_SENSOR_NO_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL);
}
-static void stepCntFlushGetData()
+static bool stepCntFlushGetData()
{
- if (mTask.state == SENSOR_IDLE) {
- mTask.state = SENSOR_STEP_CNT;
+ TDECL();
+ if (trySwitchState(SENSOR_STEP_CNT)) {
SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
+ return true;
}
+ return false;
}
static bool stepCntFlush(void *cookie)
@@ -1530,8 +1634,15 @@
mTask.total_step_cnt += (cur_step_cnt - mTask.last_step_cnt);
}
mTask.last_step_cnt = cur_step_cnt;
- step_cnt.idata = mTask.total_step_cnt;
- osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
+
+ // Send the event if the current rate is ONCHANGE or we need to flush;
+ // otherwise, wait until step count sampling timer expires
+ if (mTask.sensors[STEPCNT].rate == SENSOR_RATE_ONCHANGE || mTask.sensors[STEPCNT].flush) {
+ step_cnt.idata = mTask.total_step_cnt;
+ osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
+ } else {
+ mTask.step_cnt_changed = true;
+ }
}
while (mTask.sensors[STEPCNT].flush) {
@@ -1596,13 +1707,16 @@
static void flushAllData(void)
{
int i;
- for (i = ACC; i <= MAG; i++)
- flushData(&mTask.sensors[i], EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
+ for (i = ACC; i <= MAG; i++) {
+ flushData(&mTask.sensors[i],
+ EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
+ }
}
static bool allocateDataEvt(struct BMI160Sensor *mSensor, uint64_t rtc_time)
{
- mSensor->data_evt = slabAllocatorAlloc(mDataSlab);
+ TDECL();
+ mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab));
if (mSensor->data_evt == NULL) {
// slab allocation failed
ERROR_PRINT("slabAllocatorAlloc() failed\n");
@@ -1644,22 +1758,18 @@
}
if (mSensor->idx == MAG) {
-#ifdef MAG_I2C_ADDR
+#ifdef MAG_SLAVE_PRESENT
parseMagData(&magTask, &buf[0], &x, &y, &z);
-#endif
-
BMM150_TO_ANDROID_COORDINATE(x, y, z);
-#ifdef MAG_SLAVE_PRESENT
float xi, yi, zi;
- magCalRemoveSoftiron(&mTask.moc,
- x, y, z,
- &xi, &yi, &zi);
+ magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);
- newMagBias |= magCalUpdate(&mTask.moc,
- sensorTime * kSensorTimerIntervalUs, xi, yi, zi);
+ newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi);
magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
+#else
+ return;
#endif
} else {
raw_x = (buf[0] | buf[1] << 8);
@@ -1686,7 +1796,8 @@
if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) {
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
// flush existing samples so the bias appears after them
- flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType));
+ flushData(mSensor,
+ EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType));
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
@@ -1694,7 +1805,8 @@
mTask.magBiasCurrent = true;
mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent;
mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
- mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples;
+ mSensor->data_evt->samples[0].firstSample.biasSample =
+ mSensor->data_evt->samples[0].firstSample.numSamples;
sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
#ifdef MAG_SLAVE_PRESENT
magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
@@ -1723,6 +1835,8 @@
//DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z));
+ //TODO: This was added to prevent to much data of the same type accumulate in internal buffer.
+ // It might no longer be necessary and can be removed.
if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
flushAllData();
}
@@ -1764,9 +1878,15 @@
}
while (size > 0) {
- if (buf[i] == 0x80) {
- // header 0x80 means no more data
+ if (buf[i] == BMI160_FRAME_HEADER_INVALID) {
+ // reaching invalid header means no more data
break;
+ } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) {
+ // manually injected skip header
+ DEBUG_PRINT_IF(DBG_CHUNKED, "skip nop header");
+ i++;
+ size--;
+ continue;
}
fh_mode = buf[i] >> 6;
@@ -1791,17 +1911,19 @@
} else if (fh_param == 1) {
// sensortime frame
if (size >= 3) {
- // The active sensor with the highest odr/lowest delta is the one that determines
- // the sensor time increments.
+ // The active sensor with the highest odr/lowest delta is the one that
+ // determines the sensor time increments.
for (j = ACC; j <= MAG; j++) {
- if (mTask.sensors[j].configed && mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) {
- min_delta = min_delta < mTask.time_delta[j] ? min_delta : mTask.time_delta[j];
+ if (mTask.sensors[j].configed &&
+ mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) {
+ min_delta = min_delta < mTask.time_delta[j] ? min_delta :
+ mTask.time_delta[j];
}
}
sensor_time24 = buf[i + 2] << 16 | buf[i + 1] << 8 | buf[i];
- // clear lower bits that measure time from taking the sample to reading the FIFO,
- // something we're not interested in.
+ // clear lower bits that measure time from taking the sample to reading the
+ // FIFO, something we're not interested in.
sensor_time24 &= ~(min_delta - 1);
full_sensor_time = parseSensortime(sensor_time24);
@@ -1841,12 +1963,11 @@
mTask.time_delta[j] = saved_time_delta[j];
}
-#if TIMESTAMP_DBG
- DEBUG_PRINT("sensortime invalid: full, frame, task = %llu, %llu, %llu\n",
+ DEBUG_PRINT_IF(TIMESTAMP_DBG,
+ "sensortime invalid: full, frame, task = %llu, %llu, %llu\n",
full_sensor_time,
frame_sensor_time,
mTask.frame_sensortime);
-#endif
// Parse again with known valid timing.
// This time the sensor events will be committed into event buffer.
@@ -1859,10 +1980,11 @@
for (j = ACC; j <= MAG; j++) {
mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1);
- // sensor can be disabled in the middle of the FIFO,
- // but wait till the FIFO end to invalidate prev_frame_time since it's still needed for parsing.
+ // sensor can be disabled in the middle of the FIFO, but wait till the FIFO
+ // end to invalidate prev_frame_time since it's still needed for parsing.
// Also invalidate pending delta just to be safe.
- if (!mTask.sensors[j].configed || mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) {
+ if (!mTask.sensors[j].configed ||
+ mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) {
mTask.prev_frame_time[j] = ULONG_LONG_MAX;
mTask.pending_delta[j] = false;
}
@@ -1883,7 +2005,8 @@
mTask.pending_delta[j] = false;
mTask.time_delta[j] = mTask.next_delta[j];
#if TIMESTAMP_DBG
- DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName, (unsigned int)mTask.time_delta[j]);
+ DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName,
+ (unsigned int)mTask.time_delta[j]);
#endif
}
}
@@ -1905,8 +2028,8 @@
// 3) The underestimated frame time of a newly enabled sensor will be corrected
// as soon as it shows up in the same frame with another sensor.
// 4) (prev_frame_time == ULONG_LONG_MAX) means the sensor wasn't enabled.
- // 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the
- // last data frame of the previous fifo read. So it won't be used as a frame time reference.
+ // 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the last
+ // data frame of the previous fifo read. So it won't be used as a frame time reference.
tmp_frame_time = 0;
for (j = ACC; j <= MAG; j++) {
@@ -1924,7 +2047,8 @@
if (fh_param & 4) { // have mag data
if (size >= 8) {
if (frame_sensor_time_valid) {
- parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time); // scale not used
+ // scale not used
+ parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time);
#if TIMESTAMP_DBG
if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) {
DEBUG_PRINT("mag enabled: frame %d time 0x%08x\n",
@@ -2005,7 +2129,7 @@
}
}
- // flush data events.
+ //flush data events.
flushAllData();
}
@@ -2018,6 +2142,7 @@
*/
static void int2Handling(void)
{
+ TDECL();
union EmbeddedDataPoint trigger_axies;
uint8_t int_status_0 = mTask.statusBuffer[1];
uint8_t int_status_1 = mTask.statusBuffer[2];
@@ -2027,9 +2152,7 @@
osEnqueueEvt(EVT_SENSOR_STEP, NULL, NULL);
}
if (mTask.sensors[STEPCNT].powered) {
- mTask.state = SENSOR_STEP_CNT;
- SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT]);
+ T(pending_step_cnt) = true;
}
}
if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) {
@@ -2061,32 +2184,18 @@
static void int2Evt(void)
{
- if (mTask.state == SENSOR_IDLE) {
- mTask.state = SENSOR_INT_2_HANDLING;
-
+ TDECL();
+ if (trySwitchState(SENSOR_INT_2_HANDLING)) {
// Read the interrupt reg value to determine what interrupts
SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
- } else if (mTask.state != SENSOR_INT_2_HANDLING) {
- // If we are not handling Int 2 right now, we need to put it to pending.
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
+ } else {
+ // even if we are still in SENSOR_INT_2_HANDLING, the SPI may already finished and we need
+ // to issue another SPI read to get the latest status
mTask.pending_int[1] = true;
}
}
-static void int1Evt(void)
-{
- if (mTask.state == SENSOR_IDLE) {
- // read out fifo.
- mTask.state = SENSOR_INT_1_HANDLING;
- mTask.xferCnt = FIFO_READ_SIZE;
- SPI_READ(BMI160_REG_FIFO_DATA, mTask.xferCnt, &mTask.dataBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
- } else if (mTask.state != SENSOR_INT_1_HANDLING) {
- // If we are not handling Int 1 right now, we need to put it to pending.
- mTask.pending_int[0] = true;
- }
-}
-
// bits[6:7] in OFFSET[6] to enable/disable gyro/accel offset.
// bits[0:5] in OFFSET[6] stores the most significant 2 bits of gyro offset at
// its x, y, z axies.
@@ -2107,25 +2216,32 @@
return mode;
}
-static void saveCalibration()
+static bool saveCalibration()
{
- mTask.state = SENSOR_SAVE_CALIBRATION;
- if (mTask.sensors[ACC].offset_enable) {
- SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450);
- SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450);
- SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450);
+ TDECL();
+ if (trySwitchState(SENSOR_SAVE_CALIBRATION)) {
+ if (mTask.sensors[ACC].offset_enable) {
+ SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450);
+ SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450);
+ SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450);
+ }
+ if (mTask.sensors[GYR].offset_enable) {
+ SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450);
+ SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450);
+ SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450);
+ }
+ SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450);
+ SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__);
+ return true;
+ } else {
+ DEBUG_PRINT("%s, state != IDLE", __FUNCTION__);
+ return false;
}
- if (mTask.sensors[GYR].offset_enable) {
- SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450);
- SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450);
- SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450);
- }
- SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450);
- SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL);
}
-static void sendCalibrationResult(uint8_t status, uint8_t sensorType, int32_t xBias, int32_t yBias, int32_t zBias) {
+static void sendCalibrationResult(uint8_t status, uint8_t sensorType,
+ int32_t xBias, int32_t yBias, int32_t zBias) {
struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
if (!data) {
osLog(LOG_WARN, "Couldn't alloc cal result pkt");
@@ -2148,15 +2264,16 @@
static void accCalibrationHandling(void)
{
+ TDECL();
switch (mTask.calibration_state) {
case CALIBRATION_START:
- mRetryLeft = RETRY_CNT_CALIBRATION;
+ T(mRetryLeft) = RETRY_CNT_CALIBRATION;
// turn ACC to NORMAL mode
SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
mTask.calibration_state = CALIBRATION_FOC;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
case CALIBRATION_FOC:
@@ -2174,7 +2291,7 @@
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
case CALIBRATION_WAIT_FOC_DONE:
// if the STATUS REG has bit 3 set, it means calbration is done.
@@ -2193,13 +2310,13 @@
// calibration hasn't finished yet, go back to wait for 50ms.
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
- mRetryLeft--;
+ T(mRetryLeft)--;
}
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
// if calbration hasn't finished after 10 polling on the STATUS reg,
// declare timeout.
- if (mRetryLeft == 0) {
+ if (T(mRetryLeft) == 0) {
mTask.calibration_state = CALIBRATION_TIMEOUT;
}
break;
@@ -2221,7 +2338,9 @@
(unsigned int)mTask.sensors[ACC].offset[1],
(unsigned int)mTask.sensors[ACC].offset[2]);
- sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL, mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1], mTask.sensors[ACC].offset[2]);
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL,
+ mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1],
+ mTask.sensors[ACC].offset[2]);
// Enable offset compensation for accel
uint8_t mode = offset6Mode();
@@ -2231,7 +2350,7 @@
SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
mTask.calibration_state = CALIBRATION_DONE;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
default:
ERROR_PRINT("Invalid calibration state\n");
@@ -2241,16 +2360,16 @@
static bool accCalibration(void *cookie)
{
- if ((mTask.state != SENSOR_IDLE) || mTask.sensors[ACC].powered) {
+ TDECL();
+ if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) {
+ mTask.calibration_state = CALIBRATION_START;
+ accCalibrationHandling();
+ return true;
+ } else {
ERROR_PRINT("cannot calibrate accel because sensor is busy\n");
sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0);
return false;
- } else {
- mTask.state = SENSOR_CALIBRATING;
- mTask.calibration_state = CALIBRATION_START;
- accCalibrationHandling();
}
- return true;
}
static bool accCfgData(void *data, void *cookie)
@@ -2262,27 +2381,28 @@
mTask.sensors[ACC].offset[2] = values[2];
mTask.sensors[ACC].offset_enable = true;
- INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n", values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF);
+ INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n",
+ values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF);
- if (mTask.state == SENSOR_IDLE)
- saveCalibration();
- else
+ if (!saveCalibration()) {
mTask.pending_calibration_save = true;
+ }
return true;
}
static void gyrCalibrationHandling(void)
{
+ TDECL();
switch (mTask.calibration_state) {
case CALIBRATION_START:
- mRetryLeft = RETRY_CNT_CALIBRATION;
+ T(mRetryLeft) = RETRY_CNT_CALIBRATION;
// turn GYR to NORMAL mode
SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
mTask.calibration_state = CALIBRATION_FOC;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
break;
case CALIBRATION_FOC:
@@ -2299,7 +2419,7 @@
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
break;
case CALIBRATION_WAIT_FOC_DONE:
@@ -2319,13 +2439,13 @@
// calibration hasn't finished yet, go back to wait for 50ms.
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
- mRetryLeft--;
+ T(mRetryLeft)--;
}
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
// if calbration hasn't finished after 10 polling on the STATUS reg,
// declare timeout.
- if (mRetryLeft == 0) {
+ if (T(mRetryLeft) == 0) {
mTask.calibration_state = CALIBRATION_TIMEOUT;
}
break;
@@ -2347,7 +2467,9 @@
(unsigned int)mTask.sensors[GYR].offset[1],
(unsigned int)mTask.sensors[GYR].offset[2]);
- sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO, mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1], mTask.sensors[GYR].offset[2]);
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO,
+ mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1],
+ mTask.sensors[GYR].offset[2]);
// Enable offset compensation for gyro
uint8_t mode = offset6Mode();
@@ -2357,7 +2479,7 @@
SPI_WRITE(BMI160_REG_CMD, 0x14, 1000);
mTask.calibration_state = CALIBRATION_DONE;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR]);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
break;
default:
ERROR_PRINT("Invalid calibration state\n");
@@ -2367,16 +2489,16 @@
static bool gyrCalibration(void *cookie)
{
- if ((mTask.state != SENSOR_IDLE) || mTask.sensors[GYR].powered) {
+ TDECL();
+ if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_CALIBRATING)) {
+ mTask.calibration_state = CALIBRATION_START;
+ gyrCalibrationHandling();
+ return true;
+ } else {
ERROR_PRINT("cannot calibrate gyro because sensor is busy\n");
sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0);
return false;
- } else {
- mTask.state = SENSOR_CALIBRATING;
- mTask.calibration_state = CALIBRATION_START;
- gyrCalibrationHandling();
}
- return true;
}
static bool gyrCfgData(void *data, void *cookie)
@@ -2388,12 +2510,12 @@
mTask.sensors[GYR].offset[2] = values[2];
mTask.sensors[GYR].offset_enable = true;
- INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n", values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF);
+ INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n",
+ values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF);
- if (mTask.state == SENSOR_IDLE)
- saveCalibration();
- else
+ if (!saveCalibration()) {
mTask.pending_calibration_save = true;
+ }
return true;
}
@@ -2402,7 +2524,8 @@
{
float *values = data;
- INFO_PRINT("magCfgData: %ld, %ld, %ld\n", (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000));
+ INFO_PRINT("magCfgData: %ld, %ld, %ld\n",
+ (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000));
#ifdef MAG_SLAVE_PRESENT
mTask.moc.x_bias = values[0];
@@ -2436,15 +2559,18 @@
static const struct SensorOps mSensorOps[NUM_OF_SENSOR] =
{
- { DEC_OPS_CAL_CFG(accPower, accFirmwareUpload, accSetRate, accFlush, accCalibration, accCfgData) },
- { DEC_OPS_CAL_CFG(gyrPower, gyrFirmwareUpload, gyrSetRate, gyrFlush, gyrCalibration, gyrCfgData) },
+ { DEC_OPS_CAL_CFG(accPower, accFirmwareUpload, accSetRate, accFlush, accCalibration,
+ accCfgData) },
+ { DEC_OPS_CAL_CFG(gyrPower, gyrFirmwareUpload, gyrSetRate, gyrFlush, gyrCalibration,
+ gyrCfgData) },
{ DEC_OPS_CFG(magPower, magFirmwareUpload, magSetRate, magFlush, magCfgData) },
{ DEC_OPS(stepPower, stepFirmwareUpload, stepSetRate, stepFlush) },
{ DEC_OPS(doubleTapPower, doubleTapFirmwareUpload, doubleTapSetRate, doubleTapFlush) },
{ DEC_OPS(flatPower, flatFirmwareUpload, flatSetRate, flatFlush) },
{ DEC_OPS(anyMotionPower, anyMotionFirmwareUpload, anyMotionSetRate, anyMotionFlush) },
{ DEC_OPS(noMotionPower, noMotionFirmwareUpload, noMotionSetRate, noMotionFlush) },
- { DEC_OPS_SEND(stepCntPower, stepCntFirmwareUpload, stepCntSetRate, stepCntFlush, stepCntSendLastData) },
+ { DEC_OPS_SEND(stepCntPower, stepCntFirmwareUpload, stepCntSetRate, stepCntFlush,
+ stepCntSendLastData) },
};
static void configEvent(struct BMI160Sensor *mSensor, struct ConfigStat *ConfigData)
@@ -2463,6 +2589,7 @@
static void timeSyncEvt(uint32_t evtGeneration, bool evtDataValid)
{
+ TDECL();
// not processing pending events
if (evtDataValid) {
// stale event
@@ -2472,22 +2599,22 @@
mTask.active_poll_generation = mTask.poll_generation;
}
- if (mTask.state != SENSOR_IDLE) {
- mTask.pending_time_sync = true;
- } else {
- mTask.state = SENSOR_TIME_SYNC;
+ if (trySwitchState(SENSOR_TIME_SYNC)) {
SPI_READ(BMI160_REG_SENSORTIME_0, 3, &mTask.sensorTimeBuffer);
SPI_READ(BMI160_REG_TEMPERATURE_0, 2, &mTask.temperatureBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
+ } else {
+ mTask.pending_time_sync = true;
}
}
static void processPendingEvt(void)
{
+ TDECL();
enum SensorIndex i;
if (mTask.pending_int[0]) {
mTask.pending_int[0] = false;
- int1Evt();
+ initiateFifoRead(false /*isInterruptContext*/);
return;
}
if (mTask.pending_int[1]) {
@@ -2507,19 +2634,19 @@
return;
}
}
- if (mTask.sensors[STEPCNT].flush > 0) {
- stepCntFlushGetData();
+ if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) {
+ T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData();
return;
}
if (mTask.pending_calibration_save) {
- mTask.pending_calibration_save = false;
- saveCalibration();
+ mTask.pending_calibration_save = !saveCalibration();
return;
}
}
static void sensorInit(void)
{
+ TDECL();
switch (mTask.init_state) {
case RESET_BMI160:
DEBUG_PRINT("Performing soft reset\n");
@@ -2529,7 +2656,7 @@
SPI_READ(BMI160_REG_MAGIC, 1, &mTask.dataBuffer, 100);
mTask.init_state = INIT_BMI160;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit RESET" );
break;
case INIT_BMI160:
@@ -2581,37 +2708,37 @@
// Reset fifo
SPI_WRITE(BMI160_REG_CMD, 0xB0, 10000);
-#ifdef MAG_I2C_ADDR
+#ifdef MAG_SLAVE_PRESENT
mTask.init_state = INIT_MAG;
mTask.mag_state = MAG_SET_START;
#else
// no mag connected to secondary interface
mTask.init_state = INIT_ON_CHANGE_SENSORS;
#endif
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT");
break;
case INIT_MAG:
// Don't check statusBuffer if we are just starting mag config
if (mTask.mag_state == MAG_SET_START) {
- mRetryLeft = RETRY_CNT_MAG;
+ T(mRetryLeft) = RETRY_CNT_MAG;
magConfig();
} else if (mTask.mag_state < MAG_SET_DATA && mTask.statusBuffer[1] & 0x04) {
// fixme: poll_until to reduce states
// fixme: check should be done before SPI_READ in MAG_READ
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
- if (--mRetryLeft == 0) {
+ if (--T(mRetryLeft) == 0) {
ERROR_PRINT("INIT_MAG failed\n");
// fixme: duplicate suspend mag here
mTask.mag_state = MAG_INIT_FAILED;
mTask.init_state = INIT_ON_CHANGE_SENSORS;
}
} else {
- mRetryLeft = RETRY_CNT_MAG;
+ T(mRetryLeft) = RETRY_CNT_MAG;
magConfig();
}
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_MAG");
break;
case INIT_ON_CHANGE_SENSORS:
@@ -2640,7 +2767,7 @@
SPI_WRITE(BMI160_REG_INT_FLAT_1, 0x14, 450);
mTask.init_state = INIT_DONE;
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_ONC");
break;
default:
@@ -2650,34 +2777,36 @@
static void handleSpiDoneEvt(const void* evtData)
{
+ TDECL();
struct BMI160Sensor *mSensor;
uint64_t SensorTime;
int16_t temperature16;
int i;
+ bool returnIdle = false;
- switch (mTask.state) {
+ switch (GET_STATE()) {
case SENSOR_BOOT:
- mRetryLeft = RETRY_CNT_ID;
- mTask.state = SENSOR_VERIFY_ID;
+ T(mRetryLeft) = RETRY_CNT_ID;
+ SET_STATE(SENSOR_VERIFY_ID);
// dummy reads after boot, wait 100us
SPI_READ(BMI160_REG_MAGIC, 1, &mTask.statusBuffer, 100);
// read the device ID for bmi160
SPI_READ(BMI160_REG_ID, 1, &mTask.dataBuffer);
- spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask);
+ spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "spiDone SENSOR_BOOT");
break;
case SENSOR_VERIFY_ID:
if (mTask.dataBuffer[1] != BMI160_ID) {
- mRetryLeft --;
+ T(mRetryLeft) --;
ERROR_PRINT("failed id match: %02x\n", mTask.dataBuffer[1]);
- if (mRetryLeft == 0)
+ if (T(mRetryLeft) == 0)
break;
// For some reason the first ID read will fail to get the
// correct value. need to retry a few times.
- mTask.state = SENSOR_BOOT;
+ SET_STATE(SENSOR_BOOT);
timTimerSet(100000000, 100, 100, sensorTimerCallback, NULL, true);
break;
} else {
- mTask.state = SENSOR_INITIALIZING;
+ SET_STATE(SENSOR_INITIALIZING);
mTask.init_state = RESET_BMI160;
sensorInit();
break;
@@ -2687,9 +2816,8 @@
DEBUG_PRINT("Done initialzing, system IDLE\n");
for (i=0; i<NUM_OF_SENSOR; i++)
sensorRegisterInitComplete(mTask.sensors[i].handle);
- mTask.state = SENSOR_IDLE;
// In case other tasks have already requested us before we finish booting up.
- processPendingEvt();
+ returnIdle = true;
} else {
sensorInit();
}
@@ -2703,8 +2831,7 @@
//DEBUG_PRINT("oneshot on\n");
}
sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_POWERING_DOWN:
mSensor = (struct BMI160Sensor *)evtData;
@@ -2715,55 +2842,44 @@
//DEBUG_PRINT("oneshot off\n");
}
sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
- mTask.state = SENSOR_IDLE;
if (mTask.pending_dispatch) {
mTask.pending_dispatch = false;
dispatchData();
}
-
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_INT_1_HANDLING:
dispatchData();
sendFlushEvt();
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_INT_2_HANDLING:
int2Handling();
- // If it is not step cnt, we are done.
- if (mTask.state == SENSOR_INT_2_HANDLING) {
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
- }
+ returnIdle = true;
break;
case SENSOR_CONFIG_CHANGING:
mSensor = (struct BMI160Sensor *)evtData;
sensorSignalInternalEvt(mSensor->handle,
SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
- mTask.state = SENSOR_IDLE;
if (mTask.pending_dispatch) {
mTask.pending_dispatch = false;
dispatchData();
}
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_CALIBRATING:
mSensor = (struct BMI160Sensor *)evtData;
if (mTask.calibration_state == CALIBRATION_DONE) {
DEBUG_PRINT("DONE calibration\n");
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
} else if (mTask.calibration_state == CALIBRATION_TIMEOUT) {
DEBUG_PRINT("Calibration TIMED OUT\n");
- sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, (mSensor->idx == ACC) ? SENS_TYPE_ACCEL : SENS_TYPE_GYRO, 0, 0, 0);
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR,
+ (mSensor->idx == ACC) ? SENS_TYPE_ACCEL : SENS_TYPE_GYRO, 0, 0, 0);
+ returnIdle = true;
} else if (mSensor->idx == ACC) {
accCalibrationHandling();
} else if (mSensor->idx == GYR) {
@@ -2772,11 +2888,11 @@
break;
case SENSOR_STEP_CNT:
sendStepCnt();
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_TIME_SYNC:
- SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] | (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16));
+ SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] |
+ (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16));
map_sensortime_to_rtc_time(SensorTime, rtcGetTime());
temperature16 = (mTask.temperatureBuffer[1] | (mTask.temperatureBuffer[2] << 8));
@@ -2789,31 +2905,38 @@
if (mTask.active_poll_generation == mTask.poll_generation) {
// attach the generation number to event
- timTimerSet(kTimeSyncPeriodNs, 100, 100, timeSyncCallback, (void *)mTask.poll_generation, true);
+ timTimerSet(kTimeSyncPeriodNs, 100, 100, timeSyncCallback,
+ (void *)mTask.poll_generation, true);
}
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ returnIdle = true;
break;
case SENSOR_SAVE_CALIBRATION:
- DEBUG_PRINT("SENSOR_SAVE_CALIBRATION: %02x %02x %02x %02x %02x %02x %02x\n", mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4], mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]);
- mTask.state = SENSOR_IDLE;
- processPendingEvt();
+ DEBUG_PRINT("SENSOR_SAVE_CALIBRATION: %02x %02x %02x %02x %02x %02x %02x\n",
+ mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4],
+ mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]);
+ returnIdle = true;
break;
default:
break;
}
+
+ if (returnIdle) {
+ SET_STATE(SENSOR_IDLE);
+ processPendingEvt();
+ }
}
static void handleEvent(uint32_t evtType, const void* evtData)
{
+ TDECL();
uint64_t currTime;
uint8_t *packet;
float newMagBias;
switch (evtType) {
case EVT_APP_START:
- mTask.state = SENSOR_BOOT;
+ SET_STATE(SENSOR_BOOT);
osEventUnsubscribe(mTask.tid, EVT_APP_START);
// wait 100ms for sensor to boot
@@ -2840,7 +2963,7 @@
break;
case EVT_SENSOR_INTERRUPT_1:
- int1Evt();
+ initiateFifoRead(false /*isInterruptContext*/);
break;
case EVT_SENSOR_INTERRUPT_2:
int2Evt();
@@ -2869,6 +2992,7 @@
static bool startTask(uint32_t task_id)
{
+ TDECL();
DEBUG_PRINT(" IMU: %ld\n", task_id);
enum SensorIndex i;
@@ -2876,33 +3000,37 @@
time_init();
- mTask.tid = task_id;
+ T(tid) = task_id;
- mTask.Int1 = gpioRequest(BMI160_INT1_PIN);
- mTask.Isr1.func = bmi160Isr1;
- mTask.Int2 = gpioRequest(BMI160_INT2_PIN);
- mTask.Isr2.func = bmi160Isr2;
- mTask.pending_int[0] = false;
- mTask.pending_int[1] = false;
- mTask.pending_dispatch = false;
- mTask.frame_sensortime_valid = false;
- mTask.poll_generation = 0;
- mTask.tempCelsius = kTempInvalid;
- mTask.tempTime = 0;
+ T(Int1) = gpioRequest(BMI160_INT1_PIN);
+ T(Isr1).func = bmi160Isr1;
+ T(Int2) = gpioRequest(BMI160_INT2_PIN);
+ T(Isr2).func = bmi160Isr2;
+ T(pending_int[0]) = false;
+ T(pending_int[1]) = false;
+ T(pending_step_cnt) = false;
+ T(pending_dispatch) = false;
+ T(frame_sensortime_valid) = false;
+ T(poll_generation) = 0;
+ T(tempCelsius) = kTempInvalid;
+ T(tempTime) = 0;
- mTask.mode.speed = BMI160_SPI_SPEED_HZ;
- mTask.mode.bitsPerWord = 8;
- mTask.mode.cpol = SPI_CPOL_IDLE_HI;
- mTask.mode.cpha = SPI_CPHA_TRAILING_EDGE;
- mTask.mode.nssChange = true;
- mTask.mode.format = SPI_FORMAT_MSB_FIRST;
- mTask.cs = GPIO_PB(12);
- spiMasterRequest(BMI160_SPI_BUS_ID, &mTask.spiDev);
+ T(mode).speed = BMI160_SPI_SPEED_HZ;
+ T(mode).bitsPerWord = 8;
+ T(mode).cpol = SPI_CPOL_IDLE_HI;
+ T(mode).cpha = SPI_CPHA_TRAILING_EDGE;
+ T(mode).nssChange = true;
+ T(mode).format = SPI_FORMAT_MSB_FIRST;
+ T(cs) = GPIO_PB(12);
+
+ T(watermark) = 0;
+
+ spiMasterRequest(BMI160_SPI_BUS_ID, &T(spiDev));
for (i = ACC; i < NUM_OF_SENSOR; i++) {
- initSensorStruct(&mTask.sensors[i], i);
- mTask.sensors[i].handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], NULL, false);
- mTask.pending_config[i] = false;
+ initSensorStruct(&T(sensors[i]), i);
+ T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], NULL, false);
+ T(pending_config[i]) = false;
}
osEventSubscribe(mTask.tid, EVT_APP_START);
@@ -2922,33 +3050,37 @@
// the fifo size is 1K.
// 20 slabs because some slabs may only hold 1-2 samples.
// XXX: this consumes too much memeory, need to optimize
- mDataSlab = slabAllocatorNew(slabSize, 4, 20);
- if (!mDataSlab) {
+ T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20);
+ if (!T(mDataSlab)) {
INFO_PRINT("slabAllocatorNew() failed\n");
return false;
}
+ T(mWbufCnt) = 0;
+ T(mRegCnt) = 0;
+ T(spiInUse) = false;
- mTask.interrupt_enable_0 = 0x00;
- mTask.interrupt_enable_2 = 0x00;
+ T(interrupt_enable_0) = 0x00;
+ T(interrupt_enable_2) = 0x00;
// initialize the last bmi160 time to be ULONG_MAX, so that we know it's
// not valid yet.
- mTask.last_sensortime = 0;
- mTask.frame_sensortime = ULONG_LONG_MAX;
+ T(last_sensortime) = 0;
+ T(frame_sensortime) = ULONG_LONG_MAX;
// it's ok to leave interrupt open all the time.
- enableInterrupt(mTask.Int1, &mTask.Isr1);
- enableInterrupt(mTask.Int2, &mTask.Isr2);
+ enableInterrupt(T(Int1), &T(Isr1));
+ enableInterrupt(T(Int2), &T(Isr2));
return true;
}
static void endTask(void)
{
+ TDECL();
#ifdef MAG_SLAVE_PRESENT
destroy_mag_cal(&mTask.moc);
#endif
- slabAllocatorDestroy(mDataSlab);
+ slabAllocatorDestroy(T(mDataSlab));
spiMasterRelease(mTask.spiDev);
// disable and release interrupt.
@@ -2958,4 +3090,274 @@
gpioRelease(mTask.Int2);
}
+/**
+ * Parse BMI160 FIFO frame without side effect.
+ *
+ * The major purpose of this function is to determine if FIFO content is received completely (start
+ * to see invalid headers). If not, return the pointer to the beginning last incomplete frame so
+ * additional read can use this pointer as start of read buffer.
+ *
+ * @param buf buffer location
+ * @param size size of data to be parsed
+ *
+ * @return NULL if the FIFO is received completely; or pointer to the beginning of last incomplete
+ * frame for additional read.
+ */
+static uint8_t* shallowParseFrame(uint8_t * buf, int size) {
+ int i = 0;
+ int iLastFrame = 0; // last valid frame header index
+
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf start %p: %x %x %x\n", buf, buf[0], buf[1], buf[2]);
+ while (size > 0) {
+ int fh_mode, fh_param;
+ iLastFrame = i;
+
+ if (buf[i] == BMI160_FRAME_HEADER_INVALID) {
+ // no more data
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf:at%d=0x80\n", iLastFrame);
+ return NULL;
+ } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) {
+ // artifically added nop frame header, skip
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, skip header\n", i);
+ i++;
+ size--;
+ continue;
+ }
+
+ //++frame_num;
+
+ fh_mode = buf[i] >> 6;
+ fh_param = (buf[i] >> 2) & 0xf;
+
+ i++;
+ size--;
+
+ if (fh_mode == 1) {
+ // control frame.
+ if (fh_param == 0) {
+ // skip frame, we skip it (1 byte)
+ i++;
+ size--;
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a skip frame\n", iLastFrame);
+ } else if (fh_param == 1) {
+ // sensortime frame (3 bytes)
+ i += 3;
+ size -= 3;
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a sensor_time frame\n", iLastFrame);
+ } else if (fh_param == 2) {
+ // fifo_input config frame (1byte)
+ i++;
+ size--;
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a fifo cfg frame\n", iLastFrame);
+ } else {
+ size = 0; // drop this batch
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "Invalid fh_param in control frame!!\n");
+ // mark invalid
+ buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID;
+ return NULL;
+ }
+ } else if (fh_mode == 2) {
+ // regular frame, dispatch data to each sensor's own fifo
+ if (fh_param & 4) { // have mag data
+ i += 8;
+ size -= 8;
+ }
+ if (fh_param & 2) { // have gyro data
+ i += 6;
+ size -= 6;
+ }
+ if (fh_param & 1) { // have accel data
+ i += 6;
+ size -= 6;
+ }
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a reg frame acc %d, gyro %d, mag %d\n",
+ iLastFrame, fh_param &1 ? 1:0, fh_param&2?1:0, fh_param&4?1:0);
+ } else {
+ size = 0; // drop this batch
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf: Invalid fh_mode %d!!\n", fh_mode);
+ //mark invalid
+ buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID;
+ return NULL;
+ }
+ }
+
+ // there is a partial frame, return where to write next chunck of data
+ DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "partial frame ends %p\n", buf + iLastFrame);
+ return buf + iLastFrame;
+}
+
+/**
+ * Intialize the first read of chunked SPI read sequence.
+ *
+ * @param index starting index of the txrxBuffer in which the data will be write into.
+ */
+static void chunkedReadInit_(TASK, int index, int size) {
+
+ if (GET_STATE() != SENSOR_INT_1_HANDLING) {
+ ERROR_PRINT("chunkedReadInit in wrong mode");
+ return;
+ }
+
+ T(mWbufCnt) = index;
+ if (T(mWbufCnt) > FIFO_READ_SIZE) {
+ // drop data to prevent bigger issue
+ T(mWbufCnt) = 0;
+ }
+ T(chunkReadSize) = size > CHUNKED_READ_SIZE ? size : CHUNKED_READ_SIZE;
+
+ DEBUG_PRINT_IF(DBG_CHUNKED, "crd %d>>%d\n", T(chunkReadSize), index);
+ SPI_READ(BMI160_REG_FIFO_DATA, T(chunkReadSize), &T(dataBuffer));
+ spiBatchTxRx(&T(mode), chunkedReadSpiCallback, _task, __FUNCTION__);
+}
+
+/**
+ * Chunked SPI read callback.
+ *
+ * Handles the chunked read logic: issue additional read if necessary, or calls sensorSpiCallback()
+ * if the entire FIFO is read.
+ *
+ * @param cookie extra data
+ * @param err error
+ *
+ * @see sensorSpiCallback()
+ */
+static void chunkedReadSpiCallback(void *cookie, int err) {
+ TASK = (_Task*) cookie;
+
+ T(spiInUse) = false;
+ DEBUG_PRINT_IF(err !=0 || GET_STATE() != SENSOR_INT_1_HANDLING,
+ "crcb,e:%d,s:%d", err, (int)GET_STATE());
+ bool int1 = gpioGet(T(Int1));
+ if (err != 0) {
+ DEBUG_PRINT_IF(DBG_CHUNKED, "crd retry");
+ // read full fifo length to be safe
+ chunkedReadInit(0, FIFO_READ_SIZE);
+ return;
+ }
+
+ *T(dataBuffer) = BMI160_FRAME_HEADER_SKIP; // fill the 0x00/0xff hole at the first byte
+ uint8_t* end = shallowParseFrame(T(dataBuffer), T(chunkReadSize));
+
+ if (end == NULL) {
+ // if interrupt is still set after read for some reason, set the pending interrupt
+ // to handle it immediately after data is handled.
+ T(pending_int[0]) = T(pending_int[0]) || int1;
+
+ // recover the buffer and valid data size to make it looks like a single read so that
+ // real frame parse works properly
+ T(dataBuffer) = T(txrxBuffer);
+ T(xferCnt) = FIFO_READ_SIZE;
+ sensorSpiCallback(cookie, err);
+ } else {
+ DEBUG_PRINT_IF(DBG_CHUNKED, "crd cont");
+ chunkedReadInit(end - T(txrxBuffer), CHUNKED_READ_SIZE);
+ }
+}
+
+/**
+ * Initiate read of sensor fifo.
+ *
+ * If task is in idle state, init chunked FIFO read; otherwise, submit an interrupt message or mark
+ * the read pending depending if it is called in interrupt context.
+ *
+ * @param isInterruptContext true if called from interrupt context; false otherwise.
+ *
+ */
+static void initiateFifoRead_(TASK, bool isInterruptContext) {
+ if (trySwitchState(SENSOR_INT_1_HANDLING)) {
+ // estimate first read size to be watermark + 1 more sample + some extra
+ int firstReadSize = T(watermark) * 4 + 32; // 1+6+6+8+1+3 + extra = 25 + extra = 32
+ if (firstReadSize < CHUNKED_READ_SIZE) {
+ firstReadSize = CHUNKED_READ_SIZE;
+ }
+ chunkedReadInit(0, firstReadSize);
+ } else {
+ if (isInterruptContext) {
+ // called from interrupt context, queue event
+ osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
+ } else {
+ // non-interrupt context, set pending flag, so next time it will be picked up after
+ // switching back to idle.
+ // Note: even if we are still in SENSOR_INT_1_HANDLING, the SPI may already finished and
+ // we need to issue another SPI read to get the latest status.
+ T(pending_int[0]) = true;
+ }
+ }
+}
+
+/**
+ * Calculate fifo size using normalized input.
+ *
+ * @param iPeriod normalized period vector
+ * @param iLatency normalized latency vector
+ * @param factor vector that contains size factor for each sensor
+ * @param n size of the vectors
+ *
+ * @return max size of FIFO to guarantee latency requirements of all sensors or SIZE_MAX if no
+ * sensor is active.
+ */
+static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* factor, int n) {
+ int i;
+
+ int minLatency = INT_MAX;
+ for (i = 0; i < n; i++) {
+ if (iLatency[i] > 0) {
+ minLatency = iLatency[i] < minLatency ? iLatency[i] : minLatency;
+ }
+ }
+ DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: min latency %d unit", minLatency);
+
+ bool anyActive = false;
+ size_t s = 0;
+ size_t head = 0;
+ for (i = 0; i < n; i++) {
+ if (iPeriod[i] > 0) {
+ anyActive = true;
+ size_t t = minLatency / iPeriod[i];
+ head = t > head ? t : head;
+ s += t * factor[i];
+ DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head);
+ }
+ }
+
+ return anyActive ? head + s : SIZE_MAX;
+}
+
+/**
+ * Calculate the watermark setting from sensor registration information
+ *
+ * It is assumed that all sensor period share a common denominator (true for BMI160) and the
+ * latency of sensor will be lower bounded by its sampling period.
+ *
+ * @return watermark register setting
+ */
+static uint8_t calcWatermark2_(TASK) {
+ int period[] = {-1, -1, -1};
+ int latency[] = {-1, -1, -1};
+ const int factor[] = {6, 6, 8};
+ int i;
+
+ for (i = ACC; i <= MAG; ++i) {
+ if (T(sensors[i]).configed) {
+ period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate;
+ latency[i - ACC] = U64_DIV_BY_U64_CONSTANT(
+ T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS);
+ DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit",
+ (int) T(sensors[i]).rate/1024,
+ (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000),
+ period[i-ACC], latency[i-ACC]);
+ }
+ }
+
+
+ size_t watermark = calcFifoSize(period, latency, factor, MAG - ACC + 1) / 4;
+ DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: wm = %d", watermark);
+ watermark = watermark < WATERMARK_MIN ? WATERMARK_MIN : watermark;
+ watermark = watermark > WATERMARK_MAX ? WATERMARK_MAX : watermark;
+
+ return watermark;
+}
+
INTERNAL_APP_INIT(BMI160_APP_ID, 1, startTask, endTask, handleEvent);
+
+
diff --git a/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.c b/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.c
index c4971df..4a223fb 100644
--- a/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.c
+++ b/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.c
@@ -19,7 +19,7 @@
#define kScale_mag 0.0625f // 1.0f / 16.0f;
-void bmm150SaveDigData(MagTask_t *magTask, uint8_t *data, size_t offset)
+void bmm150SaveDigData(struct MagTask *magTask, uint8_t *data, size_t offset)
{
// magnetometer temperature calibration data is read in 3 bursts of 8 byte
// length each.
@@ -45,8 +45,7 @@
}
}
-static int32_t bmm150TempCompensateX(
- MagTask_t *magTask, int16_t mag_x, uint16_t rhall)
+static int32_t bmm150TempCompensateX(struct MagTask *magTask, int16_t mag_x, uint16_t rhall)
{
int32_t inter_retval = 0;
@@ -82,7 +81,7 @@
return inter_retval;
}
-static int32_t bmm150TempCompensateY(MagTask_t *magTask, int16_t mag_y, uint16_t rhall)
+static int32_t bmm150TempCompensateY(struct MagTask *magTask, int16_t mag_y, uint16_t rhall)
{
int32_t inter_retval = 0;
@@ -118,7 +117,7 @@
return inter_retval;
}
-static int32_t bmm150TempCompensateZ(MagTask_t *magTask, int16_t mag_z, uint16_t rhall)
+static int32_t bmm150TempCompensateZ(struct MagTask *magTask, int16_t mag_z, uint16_t rhall)
{
int32_t retval = 0;
if (mag_z != BMM150_MAG_HALL_OVERFLOW_ADCVAL) {
@@ -136,7 +135,7 @@
return retval;
}
-void parseMagData(MagTask_t *magTask, uint8_t *buf, float *x, float *y, float *z) {
+void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z) {
int32_t mag_x = (*(int16_t *)&buf[0]) >> 3;
int32_t mag_y = (*(int16_t *)&buf[2]) >> 3;
int32_t mag_z = (*(int16_t *)&buf[4]) >> 1;
diff --git a/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.h b/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.h
index 6947668..59e53fb 100644
--- a/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.h
+++ b/firmware/src/drivers/bosch_bmi160/bosch_bmm150_slave.h
@@ -53,7 +53,7 @@
#define BMM150_CALIB_HEX_LACKS 0x100000
#define BMM150_MAG_OVERFLOW_OUTPUT_S32 ((int32_t)(-2147483647-1))
-typedef struct BMM150Task {
+struct MagTask {
uint16_t dig_z1;
int16_t dig_z2, dig_z3, dig_z4;
uint16_t dig_xyz1;
@@ -61,13 +61,13 @@
int8_t dig_x1, dig_y1, dig_x2, dig_y2;
uint8_t dig_xy1;
int8_t dig_xy2;
-} MagTask_t;
+};
#define MAG_I2C_ADDR 0x10
#define MAG_REG_DATA BMM150_REG_DATA
-void bmm150SaveDigData(MagTask_t *magTask, uint8_t *data, size_t offset);
-void parseMagData(MagTask_t *magTask, uint8_t *buf, float *x, float *y, float *z);
+void bmm150SaveDigData(struct MagTask *magTask, uint8_t *data, size_t offset);
+void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z);
#ifdef __cplusplus
}
diff --git a/firmware/src/drivers/hall/hall.c b/firmware/src/drivers/hall/hall.c
index 208103f..030aa8a 100644
--- a/firmware/src/drivers/hall/hall.c
+++ b/firmware/src/drivers/hall/hall.c
@@ -32,8 +32,11 @@
#include <plat/inc/syscfg.h>
#include <variant/inc/variant.h>
+#define APP_VERSION 2
+
#define HALL_REPORT_OPENED_VALUE 0
#define HALL_REPORT_CLOSED_VALUE 1
+#define HALL_DEBOUNCE_TIMER_DELAY 10000000ULL // 10 milliseconds
#ifndef HALL_PIN
#error "HALL_PIN is not defined; please define in variant.h"
@@ -43,6 +46,7 @@
#error "HALL_IRQ is not defined; please define in variant.h"
#endif
+
static struct SensorTask
{
struct Gpio *pin;
@@ -50,26 +54,49 @@
uint32_t id;
uint32_t sensorHandle;
+ uint32_t debounceTimerHandle;
+
+ int32_t prevReportedValue;
bool on;
} mTask;
+static void debounceTimerCallback(uint32_t timerId, void *cookie)
+{
+ union EmbeddedDataPoint sample;
+ bool prevPinState = (bool)cookie;
+ bool pinState = gpioGet(mTask.pin);
+
+ if (mTask.on) {
+ if (pinState == prevPinState) {
+ sample.idata = pinState ? HALL_REPORT_OPENED_VALUE :
+ HALL_REPORT_CLOSED_VALUE;
+
+ if (sample.idata != mTask.prevReportedValue) {
+ mTask.prevReportedValue = sample.idata;
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_HALL), sample.vptr, NULL);
+ }
+ }
+ }
+}
+
static bool hallIsr(struct ChainedIsr *localIsr)
{
struct SensorTask *data = container_of(localIsr, struct SensorTask, isr);
+ bool pinState = gpioGet(data->pin);
if (!extiIsPendingGpio(data->pin)) {
return false;
}
if (data->on) {
- union EmbeddedDataPoint sample;
- bool pinState = gpioGet(data->pin);
- sample.idata = pinState ? HALL_REPORT_OPENED_VALUE :
- HALL_REPORT_CLOSED_VALUE;
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_HALL), sample.vptr, NULL);
+ if (mTask.debounceTimerHandle)
+ timTimerCancel(mTask.debounceTimerHandle);
+
+ mTask.debounceTimerHandle = timTimerSet(HALL_DEBOUNCE_TIMER_DELAY, 0, 50, debounceTimerCallback, (void*)pinState, true /* oneShot */);
}
+
extiClearPendingGpio(data->pin);
return true;
}
@@ -90,9 +117,16 @@
return true;
}
+static const uint32_t supportedRates[] =
+{
+ SENSOR_RATE_ONCHANGE,
+ 0
+};
+
static const struct SensorInfo mSensorInfo =
{
.sensorName = "Hall",
+ .supportedRates = supportedRates,
.sensorType = SENS_TYPE_HALL,
.numAxis = NUM_AXIS_EMBEDDED,
.interrupt = NANOHUB_INT_WAKEUP,
@@ -110,6 +144,13 @@
}
mTask.on = on;
+ mTask.prevReportedValue = -1;
+
+ if (mTask.debounceTimerHandle) {
+ timTimerCancel(mTask.debounceTimerHandle);
+ mTask.debounceTimerHandle = 0;
+ }
+
return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
}
@@ -137,12 +178,26 @@
return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_HALL), SENSOR_DATA_EVENT_FLUSH, NULL);
}
+static bool hallSendLastSample(void *cookie, uint32_t tid)
+{
+ union EmbeddedDataPoint sample;
+ bool result = true;
+
+ if (mTask.prevReportedValue != -1) {
+ sample.idata = mTask.prevReportedValue;
+ result = osEnqueuePrivateEvt(sensorGetMyEventType(SENS_TYPE_HALL), sample.vptr, NULL, tid);
+ }
+
+ return result;
+}
+
static const struct SensorOps mSensorOps =
{
.sensorPower = hallPower,
.sensorFirmwareUpload = hallFirmwareUpload,
.sensorSetRate = hallSetRate,
.sensorFlush = hallFlush,
+ .sensorSendOneDirectEvt = hallSendLastSample
};
static void handleEvent(uint32_t evtType, const void* evtData)
@@ -155,6 +210,7 @@
mTask.id = taskId;
mTask.sensorHandle = sensorRegister(&mSensorInfo, &mSensorOps, NULL, true);
+ mTask.prevReportedValue = -1;
mTask.pin = gpioRequest(HALL_PIN);
mTask.isr.func = hallIsr;
@@ -170,4 +226,4 @@
sensorUnregister(mTask.sensorHandle);
}
-INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 6), 0, startTask, endTask, handleEvent);
+INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 6), APP_VERSION, startTask, endTask, handleEvent);
diff --git a/firmware/src/drivers/hall_twopole/hall_twopole.c b/firmware/src/drivers/hall_twopole/hall_twopole.c
new file mode 100644
index 0000000..4d69d54
--- /dev/null
+++ b/firmware/src/drivers/hall_twopole/hall_twopole.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <float.h>
+
+#include <eventnums.h>
+#include <gpio.h>
+#include <heap.h>
+#include <hostIntf.h>
+#include <isr.h>
+#include <nanohubPacket.h>
+#include <sensors.h>
+#include <seos.h>
+#include <timer.h>
+#include <plat/inc/gpio.h>
+#include <plat/inc/exti.h>
+#include <plat/inc/syscfg.h>
+#include <variant/inc/variant.h>
+
+#define APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 11)
+#define APP_VERSION 2
+
+#define HALL_REPORT_OPENED_VALUE 0
+#define HALL_REPORT_CLOSED_VALUE 1
+#define HALL_DEBOUNCE_TIMER_DELAY 10000000ULL // 10 milliseconds
+
+#ifndef HALL_S_PIN
+#error "HALL_S_PIN is not defined; please define in variant.h"
+#endif
+#ifndef HALL_S_IRQ
+#error "HALL_S_IRQ is not defined; please define in variant.h"
+#endif
+#ifndef HALL_N_PIN
+#error "HALL_N_PIN is not defined; please define in variant.h"
+#endif
+#ifndef HALL_N_IRQ
+#error "HALL_N_IRQ is not defined; please define in variant.h"
+#endif
+
+#define MAKE_TYPE(sPin,nPin) (sPin ? HALL_REPORT_OPENED_VALUE : HALL_REPORT_CLOSED_VALUE) + \
+ ((nPin ? HALL_REPORT_OPENED_VALUE : HALL_REPORT_CLOSED_VALUE) << 1)
+
+static struct SensorTask
+{
+ struct Gpio *sPin;
+ struct Gpio *nPin;
+ struct ChainedIsr sIsr;
+ struct ChainedIsr nIsr;
+
+ uint32_t id;
+ uint32_t sensorHandle;
+ uint32_t debounceTimerHandle;
+
+ int32_t prevReportedState;
+
+ bool on;
+} mTask;
+
+static void hallReportState(int32_t pinState)
+{
+ union EmbeddedDataPoint sample;
+ if (pinState != mTask.prevReportedState) {
+ mTask.prevReportedState = pinState;
+ sample.idata = pinState;
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_HALL), sample.vptr, NULL);
+ }
+}
+
+static void debounceTimerCallback(uint32_t timerId, void *cookie)
+{
+ int32_t prevPinState = (int32_t)cookie;
+ int32_t currPinState = MAKE_TYPE(gpioGet(mTask.sPin), gpioGet(mTask.nPin));
+
+ if (mTask.on && (currPinState == prevPinState)) {
+ hallReportState(currPinState);
+ }
+}
+
+static void startDebounceTimer(struct SensorTask *data)
+{
+ int32_t currPinState = MAKE_TYPE(gpioGet(data->sPin), gpioGet(data->nPin));
+ if (data->debounceTimerHandle)
+ timTimerCancel(data->debounceTimerHandle);
+
+ data->debounceTimerHandle = timTimerSet(HALL_DEBOUNCE_TIMER_DELAY, 0, 50, debounceTimerCallback, (void*)currPinState, true /* oneShot */);
+}
+
+static bool hallSouthIsr(struct ChainedIsr *localIsr)
+{
+ struct SensorTask *data = container_of(localIsr, struct SensorTask, sIsr);
+ if (data->on)
+ startDebounceTimer(data);
+ extiClearPendingGpio(data->sPin);
+ return true;
+}
+
+static bool hallNorthIsr(struct ChainedIsr *localIsr)
+{
+ struct SensorTask *data = container_of(localIsr, struct SensorTask, nIsr);
+ if (data->on)
+ startDebounceTimer(data);
+ extiClearPendingGpio(data->nPin);
+ return true;
+}
+
+static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr, IRQn_Type irqn)
+{
+ gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
+ syscfgSetExtiPort(pin);
+ extiEnableIntGpio(pin, EXTI_TRIGGER_BOTH);
+ extiChainIsr(irqn, isr);
+ return true;
+}
+
+static bool disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr, IRQn_Type irqn)
+{
+ extiUnchainIsr(irqn, isr);
+ extiDisableIntGpio(pin);
+ return true;
+}
+
+static const uint32_t supportedRates[] =
+{
+ SENSOR_RATE_ONCHANGE,
+ 0
+};
+
+static const struct SensorInfo mSensorInfo =
+{
+ .sensorName = "Hall",
+ .supportedRates = supportedRates,
+ .sensorType = SENS_TYPE_HALL,
+ .numAxis = NUM_AXIS_EMBEDDED,
+ .interrupt = NANOHUB_INT_WAKEUP,
+ .minSamples = 20
+};
+
+static bool hallPower(bool on, void *cookie)
+{
+ if (on) {
+ extiClearPendingGpio(mTask.sPin);
+ extiClearPendingGpio(mTask.nPin);
+ enableInterrupt(mTask.sPin, &mTask.sIsr, HALL_S_IRQ);
+ enableInterrupt(mTask.nPin, &mTask.nIsr, HALL_N_IRQ);
+ } else {
+ disableInterrupt(mTask.sPin, &mTask.sIsr, HALL_S_IRQ);
+ disableInterrupt(mTask.nPin, &mTask.nIsr, HALL_N_IRQ);
+ extiClearPendingGpio(mTask.sPin);
+ extiClearPendingGpio(mTask.nPin);
+ }
+
+ mTask.on = on;
+ mTask.prevReportedState = -1;
+
+ if (mTask.debounceTimerHandle) {
+ timTimerCancel(mTask.debounceTimerHandle);
+ mTask.debounceTimerHandle = 0;
+ }
+
+ return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+}
+
+static bool hallFirmwareUpload(void *cookie)
+{
+ return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
+}
+
+static bool hallSetRate(uint32_t rate, uint64_t latency, void *cookie)
+{
+ // report initial state of hall interrupt pin
+ if (mTask.on)
+ hallReportState(MAKE_TYPE(gpioGet(mTask.sPin), gpioGet(mTask.nPin)));
+
+ return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
+}
+
+static bool hallFlush(void *cookie)
+{
+ return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_HALL), SENSOR_DATA_EVENT_FLUSH, NULL);
+}
+
+static bool hallSendLastSample(void *cookie, uint32_t tid)
+{
+ union EmbeddedDataPoint sample;
+ bool result = true;
+
+ if (mTask.prevReportedState != -1) {
+ sample.idata = mTask.prevReportedState;
+ result = osEnqueuePrivateEvt(sensorGetMyEventType(SENS_TYPE_HALL), sample.vptr, NULL, tid);
+ }
+
+ return result;
+}
+
+static const struct SensorOps mSensorOps =
+{
+ .sensorPower = hallPower,
+ .sensorFirmwareUpload = hallFirmwareUpload,
+ .sensorSetRate = hallSetRate,
+ .sensorFlush = hallFlush,
+ .sensorSendOneDirectEvt = hallSendLastSample
+};
+
+static void handleEvent(uint32_t evtType, const void* evtData)
+{
+}
+
+static bool startTask(uint32_t taskId)
+{
+ osLog(LOG_INFO, "HALL: task starting\n");
+
+ mTask.id = taskId;
+ mTask.sensorHandle = sensorRegister(&mSensorInfo, &mSensorOps, NULL, true);
+ mTask.prevReportedState = -1;
+ mTask.sPin = gpioRequest(HALL_S_PIN);
+ mTask.nPin = gpioRequest(HALL_N_PIN);
+ mTask.sIsr.func = hallSouthIsr;
+ mTask.nIsr.func = hallNorthIsr;
+
+ return true;
+}
+
+static void endTask(void)
+{
+ disableInterrupt(mTask.sPin, &mTask.sIsr, HALL_S_IRQ);
+ disableInterrupt(mTask.nPin, &mTask.nIsr, HALL_N_IRQ);
+ extiUnchainIsr(HALL_S_IRQ, &mTask.sIsr);
+ extiUnchainIsr(HALL_N_IRQ, &mTask.nIsr);
+ extiClearPendingGpio(mTask.sPin);
+ extiClearPendingGpio(mTask.nPin);
+ gpioRelease(mTask.sPin);
+ gpioRelease(mTask.nPin);
+ sensorUnregister(mTask.sensorHandle);
+ memset(&mTask, 0, sizeof(struct SensorTask));
+}
+
+INTERNAL_APP_INIT(APP_ID, APP_VERSION, startTask, endTask, handleEvent);
diff --git a/firmware/src/drivers/rohm_rpr0521/rohm_rpr0521.c b/firmware/src/drivers/rohm_rpr0521/rohm_rpr0521.c
index 97821a8..c318313 100644
--- a/firmware/src/drivers/rohm_rpr0521/rohm_rpr0521.c
+++ b/firmware/src/drivers/rohm_rpr0521/rohm_rpr0521.c
@@ -534,10 +534,10 @@
case SENSOR_STATE_INIT_OFFSETS:
/* PS Threshold register */
mTask.txrxBuf[0] = ROHM_RPR0521_REG_PS_TH_LSB;
- mTask.txrxBuf[1] = (ROHM_RPR0521_THRESHOLD_ASSERT_NEAR && 0xFF);
- mTask.txrxBuf[2] = (ROHM_RPR0521_THRESHOLD_ASSERT_NEAR && 0xFF00) >> 8;
- mTask.txrxBuf[3] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR && 0xFF);
- mTask.txrxBuf[4] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR && 0xFF00) >> 8;
+ mTask.txrxBuf[1] = (ROHM_RPR0521_THRESHOLD_ASSERT_NEAR & 0xFF);
+ mTask.txrxBuf[2] = (ROHM_RPR0521_THRESHOLD_ASSERT_NEAR & 0xFF00) >> 8;
+ mTask.txrxBuf[3] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR & 0xFF);
+ mTask.txrxBuf[4] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR & 0xFF00) >> 8;
i2cMasterTx(I2C_BUS_ID, I2C_ADDR, mTask.txrxBuf, 5, &i2cCallback, (void *)SENSOR_STATE_INIT_THRESHOLDS);
break;
@@ -703,5 +703,5 @@
}
}
-INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 10), 0, init_app, end_app, handle_event);
+INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 10), 1, init_app, end_app, handle_event);
diff --git a/firmware/src/heap.c b/firmware/src/heap.c
index ab3a183..b172c70 100644
--- a/firmware/src/heap.c
+++ b/firmware/src/heap.c
@@ -19,14 +19,25 @@
#include <stdint.h>
#include <stdio.h>
#include <heap.h>
+#include <seos.h>
+#define TIDX_HEAP_EXTRA 2 // must be >= 0; best if > 0, don't make it > 7, since it unnecessarily limits max heap size we can manage
+#define TIDX_HEAP_BITS (TASK_IDX_BITS + TIDX_HEAP_EXTRA)
+
+#define TIDX_MASK ((1 << TIDX_HEAP_BITS) - 1)
+#define MAX_HEAP_ORDER (31 - TIDX_HEAP_BITS)
+
+#if MAX_HEAP_ORDER < 16
+# error Too little HEAP is available
+#endif
struct HeapNode {
struct HeapNode* prev;
- uint32_t size:31;
+ uint32_t size: MAX_HEAP_ORDER;
uint32_t used: 1;
+ uint32_t tidx: TIDX_HEAP_BITS; // TASK_IDX_BITS to uniquely identify task; + extra bits of redundant counter add extra protection
uint8_t data[];
};
@@ -133,6 +144,7 @@
node = (struct HeapNode*)(best->data + sz);
node->used = 0;
+ node->tidx = 0;
node->size = best->size - sz - sizeof(struct HeapNode);
node->prev = best;
@@ -145,6 +157,7 @@
}
best->used = 1;
+ best->tidx = osGetCurrentTid();
ret = best->data;
out:
@@ -157,10 +170,17 @@
struct HeapNode *node, *t;
bool haveLock;
+ if (ptr == NULL) {
+ // NULL is a valid reply from heapAlloc, and thus it is not an error for
+ // us to receive it here. We just ignore it.
+ return;
+ }
+
haveLock = trylockTryTake(&gHeapLock);
node = ((struct HeapNode*)ptr) - 1;
node->used = 0;
+ node->tidx = 0;
if (haveLock) {
@@ -182,5 +202,31 @@
gNeedFreeMerge = true;
}
+int heapFreeAll(uint32_t tid)
+{
+ struct HeapNode *node;
+ bool haveLock;
+ int count = 0;
+ if (!tid)
+ return -1;
+ // this can only fail if called from interrupt
+ haveLock = trylockTryTake(&gHeapLock);
+ if (!haveLock)
+ return -1;
+
+ node = gHeapHead;
+ tid &= TIDX_MASK;
+ do {
+ if (node->tidx == tid) {
+ node->used = 0;
+ node->tidx = 0;
+ count++;
+ }
+ } while ((node = heapPrvGetNext(node)) != NULL);
+ gNeedFreeMerge = true;
+ trylockRelease(&gHeapLock);
+
+ return count;
+}
diff --git a/firmware/src/hostIntf.c b/firmware/src/hostIntf.c
index 43144b9..052ba9f 100644
--- a/firmware/src/hostIntf.c
+++ b/firmware/src/hostIntf.c
@@ -20,12 +20,15 @@
#include <string.h>
#include <alloca.h>
-#include <plat/inc/pwr.h>
#include <variant/inc/variant.h>
+#include <eventnums.h>
+
+#include <plat/inc/pwr.h>
+
+#include <nanohub/crc.h>
#include <platform.h>
#include <cpu.h>
-#include <crc.h>
#include <hostIntf.h>
#include <hostIntf_priv.h>
#include <nanohubCommand.h>
@@ -196,7 +199,7 @@
{
struct hostIntfIntErrMsg *msg = (struct hostIntfIntErrMsg *)cookie;
osLog(msg->level, "%s failed with: %d\n", msg->func, msg->reason);
- atomicAdd(&mIntErrMsgCnt, -1UL);
+ atomicAdd32bits(&mIntErrMsgCnt, -1UL);
}
static void hostIntfDeferErrLog(enum LogLevel level, enum hostIntfIntErrReason reason, const char *func)
@@ -209,7 +212,7 @@
mIntErrMsg[mIntErrMsgIdx].reason = reason;
mIntErrMsg[mIntErrMsgIdx].func = func;
if (osDefer(hostIntfPrintErrMsg, &mIntErrMsg[mIntErrMsgIdx], false)) {
- atomicAdd(&mIntErrMsgCnt, 1UL);
+ atomicAdd32bits(&mIntErrMsgCnt, 1UL);
mIntErrMsgIdx = (mIntErrMsgIdx + 1) % HOSTINTF_MAX_ERR_MSG;
}
}
@@ -1050,7 +1053,7 @@
osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
#ifdef DEBUG_LOG_EVT
- osEventSubscribe(mHostIntfTid, DEBUG_LOG_EVT);
+ osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
platEarlyLogFlush();
#endif
reason = pwrResetReason();
@@ -1082,7 +1085,7 @@
halCmd->handler((void *)&halMsg[2], halMsg[0] - 1);
}
#ifdef DEBUG_LOG_EVT
- else if (evtType == DEBUG_LOG_EVT) {
+ else if (evtType == EVT_DEBUG_LOG) {
data = (struct HostIntfDataBuffer *)evtData;
if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG) {
simpleQueueEnqueue(mOutputQ, evtData, sizeof(uint32_t) + data->length, true);
diff --git a/firmware/src/nanohubCommand.c b/firmware/src/nanohubCommand.c
index d037d2d..5e5f33b 100644
--- a/firmware/src/nanohubCommand.c
+++ b/firmware/src/nanohubCommand.c
@@ -14,13 +14,22 @@
* limitations under the License.
*/
-#include <plat/inc/taggedPtr.h>
-#include <plat/inc/rtc.h>
#include <inttypes.h>
#include <string.h>
#include <stdint.h>
#include <sys/endian.h>
+#include <variant/inc/variant.h>
+#include <eventnums.h>
+
+#include <plat/inc/taggedPtr.h>
+#include <plat/inc/rtc.h>
+#include <plat/inc/bl.h>
+#include <plat/inc/plat.h>
+
+#include <nanohub/crc.h>
+#include <nanohub/rsa.h>
+
#include <atomicBitset.h>
#include <atomic.h>
#include <hostIntf.h>
@@ -35,13 +44,8 @@
#include <slab.h>
#include <sensType.h>
#include <timer.h>
-#include <crc.h>
-#include <rsa.h>
#include <appSec.h>
#include <cpu.h>
-#include <plat/inc/bl.h>
-#include <plat/inc/plat.h>
-#include <variant/inc/variant.h>
#define NANOHUB_COMMAND(_reason, _fastHandler, _handler, _minReqType, _maxReqType) \
{ .reason = _reason, .fastHandler = _fastHandler, .handler = _handler, \
@@ -53,20 +57,29 @@
#define SYNC_DATAPOINTS 16
#define SYNC_RESET 10000000000ULL /* 10 seconds, ~100us drift */
+// maximum number of bytes to feed into appSecRxData at once
+// The bigger the number, the more time we block other event processing
+// appSecRxData only feeds 16 bytes at a time into writeCbk, so large
+// numbers don't buy us that much
+#define MAX_APP_SEC_RX_DATA_LEN 64
+
+#define REQUIRE_SIGNED_IMAGE true
+
struct DownloadState
{
struct AppSecState *appSecState;
- uint32_t size;
- uint32_t srcOffset;
- uint32_t dstOffset;
- uint8_t *start;
- uint32_t crc;
- uint32_t srcCrc;
+ uint32_t size; // document size, as reported by client
+ uint32_t srcOffset; // bytes received from client
+ uint32_t dstOffset; // bytes sent to flash
+ struct AppHdr *start; // start of flash segment, where to write
+ uint32_t crc; // document CRC-32, as reported by client
+ uint32_t srcCrc; // current state of CRC-32 we generate from input
uint8_t data[NANOHUB_PACKET_PAYLOAD_MAX];
uint8_t len;
+ uint8_t lenLeft;
uint8_t chunkReply;
- uint8_t type;
bool erase;
+ bool eraseScheduled;
};
struct TimeSync
@@ -147,18 +160,12 @@
static AppSecErr writeCbk(const void *data, uint32_t len)
{
- AppSecErr ret;
+ AppSecErr ret = APP_SEC_BAD;
- mpuAllowRamExecution(true);
- mpuAllowRomWrite(true);
- if (!BL.blProgramShared(mDownloadState->start + mDownloadState->dstOffset, (uint8_t *)data, len, BL_FLASH_KEY1, BL_FLASH_KEY2)) {
- ret = APP_SEC_BAD;
- } else {
+ if (osWriteShared((uint8_t*)(mDownloadState->start) + mDownloadState->dstOffset, data, len)) {
ret = APP_SEC_NO_ERROR;
mDownloadState->dstOffset += len;
}
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
return ret;
}
@@ -180,7 +187,7 @@
return APP_SEC_NO_ERROR;
}
-static AppSecErr aesKeyAccessCbk(uint64_t keyIdx, void *keyBuf)
+static AppSecErr osSecretKeyLookup(uint64_t keyId, void *keyBuf)
{
struct SeosEedataEncrKeyData kd;
void *state = NULL;
@@ -191,8 +198,9 @@
if (!eeDataGetAllVersions(EE_DATA_NAME_ENCR_KEY, &kd, &sz, &state))
break;
- if (sz == sizeof(struct SeosEedataEncrKeyData) && kd.keyID == keyIdx) {
- memcpy(keyBuf, kd.key, sizeof(kd.key));
+ if (sz == sizeof(struct SeosEedataEncrKeyData) && kd.keyID == keyId) {
+ if (keyBuf)
+ memcpy(keyBuf, kd.key, sizeof(kd.key));
return APP_SEC_NO_ERROR;
}
}
@@ -200,6 +208,43 @@
return APP_SEC_KEY_NOT_FOUND;
}
+static AppSecErr osSecretKeyDelete(uint64_t keyId)
+{
+ struct SeosEedataEncrKeyData kd;
+ void *state = NULL;
+ bool good = true;
+ int count = 0;
+
+ while(1) {
+ uint32_t sz = sizeof(struct SeosEedataEncrKeyData);
+ void *addr = eeDataGetAllVersions(EE_DATA_NAME_ENCR_KEY, &kd, &sz, &state);
+
+ if (!addr)
+ break;
+
+ if (sz == sizeof(kd) && kd.keyID == keyId) {
+ good = eeDataEraseOldVersion(EE_DATA_NAME_ENCR_KEY, addr) && good;
+ count++;
+ }
+ }
+
+ return count == 0 ? APP_SEC_KEY_NOT_FOUND : good ? APP_SEC_NO_ERROR : APP_SEC_BAD;
+}
+
+static AppSecErr osSecretKeyAdd(uint64_t keyId, void *keyBuf)
+{
+ struct SeosEedataEncrKeyData kd;
+
+ // do not add key if it already exists
+ if (osSecretKeyLookup(keyId, NULL) != APP_SEC_KEY_NOT_FOUND)
+ return APP_SEC_BAD;
+
+ memcpy(&kd.key, keyBuf, 32);
+ kd.keyID = keyId;
+
+ return eeDataSet(EE_DATA_NAME_ENCR_KEY, &kd, sizeof(kd)) ? APP_SEC_NO_ERROR : APP_SEC_BAD;
+}
+
static void freeDownloadState()
{
if (mDownloadState->appSecState)
@@ -208,280 +253,359 @@
mDownloadState = NULL;
}
-static void resetDownloadState()
+static void resetDownloadState(bool initial)
{
+ bool doCreate = true;
+
mAppSecStatus = APP_SEC_NO_ERROR;
if (mDownloadState->appSecState)
appSecDeinit(mDownloadState->appSecState);
- mDownloadState->appSecState = appSecInit(writeCbk, pubKeyFindCbk, aesKeyAccessCbk, true);
+ mDownloadState->appSecState = appSecInit(writeCbk, pubKeyFindCbk, osSecretKeyLookup, REQUIRE_SIGNED_IMAGE);
mDownloadState->srcOffset = 0;
mDownloadState->srcCrc = ~0;
- mDownloadState->dstOffset = 4; // skip over header
+ if (!initial) {
+ // if no data was written, we can reuse the same segment
+ if (mDownloadState->dstOffset)
+ osAppSegmentClose(mDownloadState->start, mDownloadState->dstOffset, SEG_ST_ERASED);
+ else
+ doCreate = false;
+ }
+ if (doCreate)
+ mDownloadState->start = osAppSegmentCreate(mDownloadState->size);
+ if (!mDownloadState->start)
+ mDownloadState->erase = true;
+ mDownloadState->dstOffset = 0;
+}
+
+static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req)
+{
+ if (!mDownloadState) {
+ mDownloadState = heapAlloc(sizeof(struct DownloadState));
+
+ if (!mDownloadState)
+ return false;
+ else
+ memset(mDownloadState, 0x00, sizeof(struct DownloadState));
+ }
+
+ mDownloadState->size = le32toh(req->size);
+ mDownloadState->crc = le32toh(req->crc);
+ mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+ resetDownloadState(true);
+
+ return true;
}
static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
{
struct NanohubStartFirmwareUploadRequest *req = rx;
struct NanohubStartFirmwareUploadResponse *resp = tx;
- uint8_t *shared, *shared_start, *shared_end;
- int len, total_len;
- uint32_t sharedSz;
- shared_start = platGetSharedAreaInfo(&sharedSz);
- shared_end = shared_start + sharedSz;
+ resp->accepted = doStartFirmwareUpload(req);
+
+ return sizeof(*resp);
+}
+
+static void deferredUpdateOs(void *cookie)
+{
+ const struct AppHdr *app = cookie;
+ struct OsUpdateHdr *os = (struct OsUpdateHdr *)(&(app->hdr) + 1);
+ uint32_t uploadStatus = OS_UPDT_HDR_CHECK_FAILED;
+ uint8_t marker = OS_UPDT_MARKER_DOWNLOADED;
+ struct Segment *seg = osGetSegment(app);
+ uint32_t segSize = osSegmentGetSize(seg);
+
+ osLog(LOG_INFO, "%s: checking OS image @ %p\n", __func__, os);
+ // some sanity checks before asking BL to do image lookup
+ hostIntfSetBusy(true);
+ if (segSize >= (sizeof(*app) + sizeof(*os)) && segSize > os->size) {
+ if (osWriteShared(&os->marker, &marker, sizeof(os->marker)))
+ uploadStatus = BL.blVerifyOsUpdate();
+ else
+ osLog(LOG_ERROR, "%s: could not set marker on OS image\n", __func__);
+ }
+ hostIntfSetBusy(false);
+ osLog(LOG_INFO, "%s: status=%" PRIu32 "\n", __func__, uploadStatus);
+}
+
+static AppSecErr updateKey(const struct AppHdr *app)
+{
+ AppSecErr ret;
+ struct KeyInfo *ki = (struct KeyInfo *)(&(app->hdr) + 1);
+ uint8_t *data = (uint8_t *)(ki + 1);
+ uint64_t keyId = KEY_ID_MAKE(APP_ID_GET_VENDOR(app->hdr.appId), ki->id);
+ const char *op;
+
+ if ((app->hdr.fwFlags & FL_KEY_HDR_DELETE) != 0) {
+ // removing existing key
+ ret = osSecretKeyDelete(keyId);
+ op = "Removing";
+ } else {
+ // adding new key
+ ret = osSecretKeyAdd(keyId, data);
+ op = "Adding";
+ }
+ osLog(LOG_INFO, "%s: %s key: id=%016" PRIX64 "; ret=%" PRIu32 "\n",
+ __func__, op, keyId, ret);
+
+ return ret;
+}
+
+static uint32_t appSecErrToNanohubReply(AppSecErr status)
+{
+ uint32_t reply;
+
+ switch (status) {
+ case APP_SEC_NO_ERROR:
+ reply = NANOHUB_FIRMWARE_UPLOAD_SUCCESS;
+ break;
+ case APP_SEC_KEY_NOT_FOUND:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_KEY_NOT_FOUND;
+ break;
+ case APP_SEC_HEADER_ERROR:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_HEADER_ERROR;
+ break;
+ case APP_SEC_TOO_MUCH_DATA:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_MUCH_DATA;
+ break;
+ case APP_SEC_TOO_LITTLE_DATA:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_LITTLE_DATA;
+ break;
+ case APP_SEC_SIG_VERIFY_FAIL:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_VERIFY_FAIL;
+ break;
+ case APP_SEC_SIG_DECODE_FAIL:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_DECODE_FAIL;
+ break;
+ case APP_SEC_SIG_ROOT_UNKNOWN:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_ROOT_UNKNOWN;
+ break;
+ case APP_SEC_MEMORY_ERROR:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_MEMORY_ERROR;
+ break;
+ case APP_SEC_INVALID_DATA:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_INVALID_DATA;
+ break;
+ case APP_SEC_VERIFY_FAILED:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_VERIFY_FAILED;
+ break;
+ default:
+ reply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_BAD;
+ break;
+ }
+ return reply;
+}
+
+static uint32_t firmwareFinish(bool valid)
+{
+ struct AppHdr *app;
+ struct Segment *storageSeg;
+ uint32_t segState;
+ uint32_t ret = NANOHUB_FIRMWARE_UPLOAD_SUCCESS;
if (!mDownloadState) {
- mDownloadState = heapAlloc(sizeof(struct DownloadState));
+ ret = appSecErrToNanohubReply(mAppSecStatus);
+ osLog(LOG_INFO, "%s: no DL status; decoding secure status: %" PRIu32 "\n", __func__, ret);
+ return ret;
+ }
- if (!mDownloadState) {
- resp->accepted = false;
- return sizeof(*resp);
+ app = mDownloadState->start;
+ storageSeg = osGetSegment(app);
+
+ if (mAppSecStatus == APP_SEC_NO_ERROR && valid) {
+ osLog(LOG_INFO, "%s: Secure verification passed\n", __func__);
+ if (storageSeg->state != SEG_ST_RESERVED ||
+ mDownloadState->size < sizeof(struct FwCommonHdr) ||
+ app->hdr.magic != APP_HDR_MAGIC ||
+ app->hdr.fwVer != APP_HDR_VER_CUR) {
+ segState = SEG_ST_ERASED;
+ osLog(LOG_INFO, "%s: Header verification failed\n", __func__);
} else {
- memset(mDownloadState, 0x00, sizeof(struct DownloadState));
+ segState = SEG_ST_VALID;
+ }
+ } else {
+ segState = SEG_ST_ERASED;
+ osLog(LOG_INFO, "%s: Secure verification failed: valid=%d; status=%" PRIu32 "\n", __func__, valid, mAppSecStatus);
+ }
+
+ if (!osAppSegmentClose(app, mDownloadState->dstOffset, segState)) {
+ osLog(LOG_INFO, "%s: Failed to close segment\n", __func__);
+ valid = false;
+ } else {
+ segState = osAppSegmentGetState(app);
+ valid = (segState == SEG_ST_VALID);
+ }
+ osLog(LOG_INFO, "Loaded %s image type %" PRIu8 ": %" PRIu32
+ " bytes @ %p; state=%02" PRIX32 "\n",
+ valid ? "valid" : "invalid",
+ app->hdr.payInfoType, mDownloadState->size,
+ mDownloadState->start, segState);
+
+ freeDownloadState(); // no more access to mDownloadState
+
+ if (!valid)
+ ret = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_BAD;
+
+ // take extra care about some special payload types
+ if (ret == NANOHUB_FIRMWARE_UPLOAD_SUCCESS) {
+ switch(app->hdr.payInfoType) {
+ case LAYOUT_OS:
+ osLog(LOG_INFO, "Performing OS update\n");
+ // we want to give this message a chance to reach host before we start erasing stuff
+ osDefer(deferredUpdateOs, (void*)app, false);
+ break;
+ case LAYOUT_KEY:
+ ret = appSecErrToNanohubReply(updateKey(app));
+ break;
}
}
- mDownloadState->type = req->type;
- mDownloadState->size = le32toh(req->size);
- mDownloadState->crc = le32toh(req->crc);
- mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
-
- for (shared = shared_start;
- shared < shared_end && shared[0] != 0xFF;
- shared += total_len) {
- len = (shared[1] << 16) | (shared[2] << 8) | shared[3];
- total_len = sizeof(uint32_t) + ((len + 3) & ~3) + sizeof(uint32_t);
+ if (ret != NANOHUB_FIRMWARE_UPLOAD_SUCCESS || (app->hdr.fwFlags & FL_APP_HDR_VOLATILE)) {
+ if ((app->hdr.fwFlags & FL_APP_HDR_SECURE))
+ osAppWipeData((struct AppHdr*)app);
+ osAppSegmentSetState(app, SEG_ST_ERASED);
}
- if (shared + sizeof(uint32_t) + ((mDownloadState->size + 3) & ~3) + sizeof(uint32_t) < shared_end) {
- mDownloadState->start = shared;
- mDownloadState->erase = false;
- } else {
- mDownloadState->start = shared_start;
- mDownloadState->erase = true;
- }
- resetDownloadState();
+ // if any error happened after we downloaded and verified image, we say it is unknown fault
+ // we don't have download status, so e have to save returned value in secure status field, because
+ // host may request the same status multiple times
+ if (ret != NANOHUB_FIRMWARE_UPLOAD_SUCCESS)
+ mAppSecStatus = APP_SEC_BAD;
- resp->accepted = true;
- return sizeof(*resp);
+ return ret;
}
static void firmwareErase(void *cookie)
{
- osLog(LOG_INFO, "hostIntfFirmwareErase: Firmware Erase\n");
if (mDownloadState->erase == true) {
- mpuAllowRamExecution(true);
- mpuAllowRomWrite(true);
- (void)BL.blEraseShared(BL_FLASH_KEY1, BL_FLASH_KEY2);
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
+ osLog(LOG_INFO, "%s: erasing shared area\n", __func__);
+ osEraseShared();
+ mDownloadState->start = osAppSegmentCreate(mDownloadState->size);
+ if (!mDownloadState->start)
+ firmwareFinish(false);
mDownloadState->erase = false;
hostIntfSetInterrupt(NANOHUB_INT_CMD_WAIT);
}
-}
-
-static AppSecErr giveAppSecTimeIfNeeded(struct AppSecState *state, AppSecErr prevRet)
-{
- /* XXX: this will need to go away for real asynchronicity */
-
- while (prevRet == APP_SEC_NEED_MORE_TIME)
- prevRet = appSecDoSomeProcessing(state);
-
- return prevRet;
-}
-
-static uint8_t firmwareFinish(bool valid)
-{
- uint8_t buffer[7];
- int padlen;
- uint32_t crc;
- uint16_t marker;
- static const char magic[] = APP_HDR_MAGIC;
- const struct AppHdr *app;
-
- mAppSecStatus = appSecRxDataOver(mDownloadState->appSecState);
- mAppSecStatus = giveAppSecTimeIfNeeded(mDownloadState->appSecState, mAppSecStatus);
-
- if (mAppSecStatus == APP_SEC_NO_ERROR && valid && mDownloadState->type == BL_FLASH_APP_ID) {
- app = (const struct AppHdr *)&mDownloadState->start[4];
-
- if (app->marker != APP_HDR_MARKER_UPLOADING ||
- mDownloadState->size < sizeof(uint32_t) + sizeof(struct AppHdr) ||
- memcmp(magic, app->magic, sizeof(magic)-1) ||
- app->fmtVer != APP_HDR_VER_CUR) {
- marker = APP_HDR_MARKER_DELETED;
- } else {
- marker = APP_HDR_MARKER_VALID;
- }
-
- osLog(LOG_INFO, "Loaded %s app: %ld bytes @ %p\n", marker == APP_HDR_MARKER_VALID ? "valid" : "invalid", mDownloadState->size, mDownloadState->start);
-
- mpuAllowRamExecution(true);
- mpuAllowRomWrite(true);
- if (!BL.blProgramShared((uint8_t *)&app->marker, (uint8_t *)&marker, sizeof(marker), BL_FLASH_KEY1, BL_FLASH_KEY2)) {
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
- return NANOHUB_FIRMWARE_CHUNK_REPLY_RESEND;
- }
- } else {
- mpuAllowRamExecution(true);
- mpuAllowRomWrite(true);
- }
-
- if (mAppSecStatus == APP_SEC_NO_ERROR && valid)
- buffer[0] = ((mDownloadState->type & 0xF) << 4) | (mDownloadState->type & 0xF);
- else
- buffer[0] = 0x00;
-
- buffer[1] = (mDownloadState->dstOffset - 4) >> 16;
- buffer[2] = (mDownloadState->dstOffset - 4) >> 8;
- buffer[3] = (mDownloadState->dstOffset - 4);
-
- if (!BL.blProgramShared(mDownloadState->start, buffer, 4, BL_FLASH_KEY1, BL_FLASH_KEY2)) {
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
- return NANOHUB_FIRMWARE_CHUNK_REPLY_RESEND;
- }
-
- crc = ~crc32(mDownloadState->start, mDownloadState->dstOffset, ~0);
- padlen = (4 - (mDownloadState->dstOffset & 3)) & 3;
- memset(buffer, 0x00, padlen);
- memcpy(&buffer[padlen], &crc, sizeof(uint32_t));
- mDownloadState->size = mDownloadState->dstOffset + padlen + sizeof(uint32_t);
-
- if (!BL.blProgramShared(mDownloadState->start + mDownloadState->dstOffset, buffer, padlen + sizeof(uint32_t), BL_FLASH_KEY1, BL_FLASH_KEY2)) {
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
- return NANOHUB_FIRMWARE_CHUNK_REPLY_RESEND;
- }
-
- mpuAllowRomWrite(false);
- mpuAllowRamExecution(false);
- freeDownloadState();
-
- return NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+ mDownloadState->eraseScheduled = false;
}
static void firmwareWrite(void *cookie)
{
+ bool valid;
+ bool finished = false;
+ struct NanohubHalContUploadTx *resp = cookie;
+ // only check crc when cookie is NULL (write came from kernel, not HAL)
+ bool checkCrc = !cookie;
+
+ if (mAppSecStatus == APP_SEC_NEED_MORE_TIME) {
+ mAppSecStatus = appSecDoSomeProcessing(mDownloadState->appSecState);
+ } else if (mDownloadState->lenLeft) {
+ const uint8_t *data = mDownloadState->data + mDownloadState->len - mDownloadState->lenLeft;
+ uint32_t len = mDownloadState->lenLeft, lenLeft, lenRem = 0;
+
+ if (len > MAX_APP_SEC_RX_DATA_LEN) {
+ lenRem = len - MAX_APP_SEC_RX_DATA_LEN;
+ len = MAX_APP_SEC_RX_DATA_LEN;
+ }
+
+ mAppSecStatus = appSecRxData(mDownloadState->appSecState, data, len, &lenLeft);
+ mDownloadState->lenLeft = lenLeft + lenRem;
+ }
+
+ valid = (mAppSecStatus == APP_SEC_NO_ERROR);
+ if (mAppSecStatus == APP_SEC_NEED_MORE_TIME || mDownloadState->lenLeft) {
+ osDefer(firmwareWrite, cookie, false);
+ return;
+ } else if (valid) {
+ if (mDownloadState->srcOffset == mDownloadState->size) {
+ finished = true;
+ valid = !checkCrc || mDownloadState->crc == ~mDownloadState->srcCrc;
+ } else if (mDownloadState->srcOffset > mDownloadState->size) {
+ valid = false;
+ }
+ }
+ if (!valid)
+ finished = true;
+ if (finished) {
+ if (firmwareFinish(valid) != NANOHUB_FIRMWARE_UPLOAD_SUCCESS)
+ valid = false;
+ }
+ if (resp) {
+ resp->success = valid;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ }
+}
+
+static uint32_t doFirmwareChunk(uint8_t *data, uint32_t offset, uint32_t len, void *cookie)
+{
uint32_t reply;
- if (mDownloadState->type == BL_FLASH_APP_ID) {
- /* XXX: this will need to change for real asynchronicity */
- const uint8_t *data = mDownloadState->data;
- uint32_t len = mDownloadState->len, lenLeft;
- mAppSecStatus = APP_SEC_NO_ERROR;
-
- while (len) {
- mAppSecStatus = appSecRxData(mDownloadState->appSecState, data, len, &lenLeft);
- data += len - lenLeft;
- len = lenLeft;
-
- mAppSecStatus = giveAppSecTimeIfNeeded(mDownloadState->appSecState, mAppSecStatus);
- }
- }
- else
- mAppSecStatus = writeCbk(mDownloadState->data, mDownloadState->len);
-
- if (mAppSecStatus == APP_SEC_NO_ERROR) {
- if (mDownloadState->srcOffset == mDownloadState->size && mDownloadState->crc == ~mDownloadState->srcCrc) {
- reply = firmwareFinish(true);
- if (mDownloadState)
- mDownloadState->chunkReply = reply;
- } else {
- mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
- }
+ if (!mDownloadState) {
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
+ } else if (mAppSecStatus == APP_SEC_NEED_MORE_TIME || mDownloadState->lenLeft) {
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESEND;
+ } else if (mDownloadState->chunkReply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
+ reply = mDownloadState->chunkReply;
+ firmwareFinish(false);
} else {
- freeDownloadState();
+ if (mDownloadState->erase == true) {
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_WAIT;
+ if (!mDownloadState->eraseScheduled)
+ mDownloadState->eraseScheduled = osDefer(firmwareErase, NULL, false);
+ } else if (!mDownloadState->start) {
+ // this means we can't allocate enough space even after we did erase
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
+ firmwareFinish(false);
+ } else if (offset != mDownloadState->srcOffset) {
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART;
+ resetDownloadState(false);
+ } else {
+ if (!cookie)
+ mDownloadState->srcCrc = crc32(data, len, mDownloadState->srcCrc);
+ mDownloadState->srcOffset += len;
+ memcpy(mDownloadState->data, data, len);
+ mDownloadState->lenLeft = mDownloadState->len = len;
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+ osDefer(firmwareWrite, cookie, false);
+ }
}
- hostIntfSetBusy(false);
+
+ return reply;
}
static uint32_t firmwareChunk(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
{
- uint32_t offset;
- uint8_t len;
struct NanohubFirmwareChunkRequest *req = rx;
struct NanohubFirmwareChunkResponse *resp = tx;
+ uint32_t offset = le32toh(req->offset);
+ uint8_t len = rx_len - sizeof(req->offset);
- offset = le32toh(req->offset);
- len = rx_len - sizeof(req->offset);
-
- if (!mDownloadState) {
- resp->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
- } else if (mDownloadState->chunkReply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
- resp->chunkReply = mDownloadState->chunkReply;
- freeDownloadState();
- } else if (mDownloadState->erase == true) {
- resp->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_WAIT;
- osDefer(firmwareErase, NULL, false);
- } else if (offset != mDownloadState->srcOffset) {
- resp->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART;
- resetDownloadState();
- } else {
- mDownloadState->srcCrc = crc32(req->data, len, mDownloadState->srcCrc);
- mDownloadState->srcOffset += len;
- if ((mDownloadState->srcOffset == mDownloadState->size && mDownloadState->crc != ~mDownloadState->srcCrc) || (mDownloadState->srcOffset > mDownloadState->size)) {
- firmwareFinish(false);
- resp->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL;
- } else {
- memcpy(mDownloadState->data, req->data, len);
- mDownloadState->len = len;
- resp->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
- hostIntfSetBusy(true);
- osDefer(firmwareWrite, NULL, false);
- }
- }
+ resp->chunkReply = doFirmwareChunk(req->data, offset, len, NULL);
return sizeof(*resp);
}
+static uint32_t doFinishFirmwareUpload()
+{
+ uint32_t reply;
+
+ if (!mDownloadState) {
+ reply = appSecErrToNanohubReply(mAppSecStatus);
+ } else if (mDownloadState->srcOffset == mDownloadState->size) {
+ reply = NANOHUB_FIRMWARE_UPLOAD_PROCESSING;
+ } else {
+ reply = firmwareFinish(false);
+ }
+
+ return reply;
+}
+
static uint32_t finishFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
{
struct NanohubFinishFirmwareUploadResponse *resp = tx;
-
- if (!mDownloadState) {
- switch (mAppSecStatus) {
- case APP_SEC_NO_ERROR:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_SUCCESS;
- break;
- case APP_SEC_KEY_NOT_FOUND:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_KEY_NOT_FOUND;
- break;
- case APP_SEC_HEADER_ERROR:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_HEADER_ERROR;
- break;
- case APP_SEC_TOO_MUCH_DATA:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_MUCH_DATA;
- break;
- case APP_SEC_TOO_LITTLE_DATA:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_LITTLE_DATA;
- break;
- case APP_SEC_SIG_VERIFY_FAIL:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_VERIFY_FAIL;
- break;
- case APP_SEC_SIG_DECODE_FAIL:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_DECODE_FAIL;
- break;
- case APP_SEC_SIG_ROOT_UNKNOWN:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_ROOT_UNKNOWN;
- break;
- case APP_SEC_MEMORY_ERROR:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_MEMORY_ERROR;
- break;
- case APP_SEC_INVALID_DATA:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_INVALID_DATA;
- break;
- default:
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_BAD;
- break;
- }
- } else if (mDownloadState->srcOffset == mDownloadState->size) {
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_PROCESSING;
- } else {
- resp->uploadReply = NANOHUB_FIRMWARE_UPLOAD_WAITING_FOR_DATA;
- }
-
+ resp->uploadReply = doFinishFirmwareUpload();
+ if (resp->uploadReply != NANOHUB_FIRMWARE_UPLOAD_PROCESSING)
+ osLog(LOG_INFO, "%s: reply=%" PRIu8 "\n", __func__, resp->uploadReply);
return sizeof(*resp);
}
@@ -583,7 +707,7 @@
break;
#ifdef DEBUG_LOG_EVT
case HOSTINTF_DATA_TYPE_LOG:
- packet->evtType = htole32(DEBUG_LOG_EVT);
+ packet->evtType = htole32(HOST_EVT_DEBUG_LOG);
break;
#endif
default:
@@ -872,16 +996,41 @@
return NULL;
}
+static void halSendMgmtResponse(uint32_t cmd, uint32_t status)
+{
+ struct NanohubHalMgmtTx *resp;
+
+ resp = heapAlloc(sizeof(*resp));
+ if (resp) {
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
+ .msg = cmd,
+ };
+ resp->status = htole32(status);
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ }
+}
+
static void halExtAppsOn(void *rx, uint8_t rx_len)
{
+ struct NanohubHalMgmtRx *req = rx;
+
+ halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_ON, osExtAppStartApps(le64toh(req->appId)));
}
static void halExtAppsOff(void *rx, uint8_t rx_len)
{
+ struct NanohubHalMgmtRx *req = rx;
+
+ halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_OFF, osExtAppStopApps(le64toh(req->appId)));
}
static void halExtAppDelete(void *rx, uint8_t rx_len)
{
+ struct NanohubHalMgmtRx *req = rx;
+
+ halSendMgmtResponse(NANOHUB_HAL_EXT_APP_DELETE, osExtAppEraseApps(le64toh(req->appId)));
}
static void halQueryMemInfo(void *rx, uint8_t rx_len)
@@ -944,10 +1093,10 @@
static void halStartUpload(void *rx, uint8_t rx_len)
{
struct NanohubHalStartUploadRx *req = rx;
+ struct NanohubStartFirmwareUploadRequest hwReq = {
+ .size= req->length
+ };
struct NanohubHalStartUploadTx *resp;
- uint8_t *shared, *shared_start, *shared_end;
- int len, total_len;
- uint32_t sharedSz;
if (!(resp = heapAlloc(sizeof(*resp))))
return;
@@ -955,53 +1104,18 @@
resp->hdr.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0);
resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
resp->hdr.msg = NANOHUB_HAL_START_UPLOAD;
+ resp->success = doStartFirmwareUpload(&hwReq);
- shared_start = platGetSharedAreaInfo(&sharedSz);
- shared_end = shared_start + sharedSz;
-
- if (!mDownloadState) {
- mDownloadState = heapAlloc(sizeof(struct DownloadState));
-
- if (!mDownloadState) {
- resp->success = false;
- osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
- return;
- } else {
- memset(mDownloadState, 0x00, sizeof(struct DownloadState));
- }
- }
-
- mDownloadState->type = req->isOs ? BL_FLASH_KERNEL_ID : BL_FLASH_APP_ID;
- mDownloadState->size = le32toh(req->length);
- mDownloadState->crc = le32toh(0x00000000);
- mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
-
- for (shared = shared_start;
- shared < shared_end && shared[0] != 0xFF;
- shared += total_len) {
- len = (shared[1] << 16) | (shared[2] << 8) | shared[3];
- total_len = sizeof(uint32_t) + ((len + 3) & ~3) + sizeof(uint32_t);
- }
-
- if (shared + sizeof(uint32_t) + ((mDownloadState->size + 3) & ~3) + sizeof(uint32_t) < shared_end) {
- mDownloadState->start = shared;
- mDownloadState->erase = false;
- } else {
- mDownloadState->start = shared_start;
- mDownloadState->erase = true;
- }
- resetDownloadState();
-
- resp->success = true;
osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
}
static void halContUpload(void *rx, uint8_t rx_len)
{
+ uint32_t offset;
+ uint32_t reply;
+ uint8_t len;
struct NanohubHalContUploadRx *req = rx;
struct NanohubHalContUploadTx *resp;
- uint32_t offset;
- uint8_t len;
if (!(resp = heapAlloc(sizeof(*resp))))
return;
@@ -1010,38 +1124,26 @@
resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
resp->hdr.msg = NANOHUB_HAL_CONT_UPLOAD;
- offset = le32toh(req->offset);
- len = rx_len - sizeof(req->offset);
-
if (!mDownloadState) {
- resp->success = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
- } else if (mDownloadState->erase == true) {
- resp->success = NANOHUB_FIRMWARE_CHUNK_REPLY_WAIT;
- firmwareErase(NULL);
- } else if (offset != mDownloadState->srcOffset) {
- resp->success = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART;
- resetDownloadState();
- } else if (mDownloadState->srcOffset + len <= mDownloadState->size) {
- mDownloadState->srcOffset += len;
- memcpy(mDownloadState->data, req->data, len);
- mDownloadState->len = len;
- hostIntfSetBusy(true);
- firmwareWrite(NULL);
- if (mDownloadState)
- resp->success = mDownloadState->chunkReply;
- else
- resp->success = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
} else {
- resp->success = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
+ offset = le32toh(req->offset);
+ len = rx_len - sizeof(req->offset);
+ reply = doFirmwareChunk(req->data, offset, len, resp);
}
- resp->success = !resp->success;
+ if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
+ osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply);
- osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ resp->success = false;
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ }
}
static void halFinishUpload(void *rx, uint8_t rx_len)
{
struct NanohubHalFinishUploadTx *resp;
+ uint32_t reply;
if (!(resp = heapAlloc(sizeof(*resp))))
return;
@@ -1050,48 +1152,11 @@
resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
resp->hdr.msg = NANOHUB_HAL_FINISH_UPLOAD;
- if (!mDownloadState) {
- switch (mAppSecStatus) {
- case APP_SEC_NO_ERROR:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_SUCCESS;
- break;
- case APP_SEC_KEY_NOT_FOUND:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_KEY_NOT_FOUND;
- break;
- case APP_SEC_HEADER_ERROR:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_HEADER_ERROR;
- break;
- case APP_SEC_TOO_MUCH_DATA:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_MUCH_DATA;
- break;
- case APP_SEC_TOO_LITTLE_DATA:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_TOO_LITTLE_DATA;
- break;
- case APP_SEC_SIG_VERIFY_FAIL:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_VERIFY_FAIL;
- break;
- case APP_SEC_SIG_DECODE_FAIL:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_DECODE_FAIL;
- break;
- case APP_SEC_SIG_ROOT_UNKNOWN:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_SIG_ROOT_UNKNOWN;
- break;
- case APP_SEC_MEMORY_ERROR:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_MEMORY_ERROR;
- break;
- case APP_SEC_INVALID_DATA:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_INVALID_DATA;
- break;
- default:
- resp->success = NANOHUB_FIRMWARE_UPLOAD_APP_SEC_BAD;
- break;
- }
- } else if (mDownloadState->srcOffset == mDownloadState->size) {
- resp->success = NANOHUB_FIRMWARE_UPLOAD_PROCESSING;
- } else {
- resp->success = NANOHUB_FIRMWARE_UPLOAD_WAITING_FOR_DATA;
- }
- resp->success = !resp->success;
+ reply = doFinishFirmwareUpload();
+
+ osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply);
+
+ resp->success = (reply == NANOHUB_FIRMWARE_UPLOAD_SUCCESS);
osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
}
diff --git a/firmware/src/osApi.c b/firmware/src/osApi.c
index 4715576..0a03384 100644
--- a/firmware/src/osApi.c
+++ b/firmware/src/osApi.c
@@ -32,37 +32,37 @@
static void osExpApiEvtqSubscribe(uintptr_t *retValP, va_list args)
{
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
uint32_t evtType = va_arg(args, uint32_t);
- *retValP = osEventSubscribe(tid, evtType);
+ *retValP = osEventSubscribe(0, evtType);
}
static void osExpApiEvtqUnsubscribe(uintptr_t *retValP, va_list args)
{
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
uint32_t evtType = va_arg(args, uint32_t);
- *retValP = osEventUnsubscribe(tid, evtType);
+ *retValP = osEventUnsubscribe(0, evtType);
}
static void osExpApiEvtqEnqueue(uintptr_t *retValP, va_list args)
{
uint32_t evtType = va_arg(args, uint32_t);
void *evtData = va_arg(args, void*);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
- *retValP = osEnqueueEvtAsApp(evtType, evtData, tid);
+ *retValP = osEnqueueEvtAsApp(evtType, evtData, 0);
}
static void osExpApiEvtqEnqueuePrivate(uintptr_t *retValP, va_list args)
{
uint32_t evtType = va_arg(args, uint32_t);
void *evtData = va_arg(args, void*);
- uint32_t freeTid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
uint32_t toTid = va_arg(args, uint32_t);
- *retValP = osEnqueuePrivateEvtAsApp(evtType, evtData, freeTid, toTid);
+ *retValP = osEnqueuePrivateEvtAsApp(evtType, evtData, 0, toTid);
}
static void osExpApiEvtqRetainEvt(uintptr_t *retValP, va_list args)
@@ -106,11 +106,11 @@
static void osExpApiSensorReg(uintptr_t *retValP, va_list args)
{
const struct SensorInfo *si = va_arg(args, const struct SensorInfo*);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
bool initComplete = va_arg(args, int);
- *retValP = (uintptr_t)sensorRegisterAsApp(si, tid, cookie, initComplete);
+ *retValP = (uintptr_t)sensorRegisterAsApp(si, 0, cookie, initComplete);
}
static void osExpApiSensorUnreg(uintptr_t *retValP, va_list args)
@@ -138,42 +138,42 @@
static void osExpApiSensorReq(uintptr_t *retValP, va_list args)
{
- uint32_t clientId = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // clientId == tid
uint32_t sensorHandle = va_arg(args, uint32_t);
uint32_t rate = va_arg(args, uint32_t);
uint32_t latency_lo = va_arg(args, uint32_t);
uint32_t latency_hi = va_arg(args, uint32_t);
uint64_t latency = (((uint64_t)latency_hi) << 32) + latency_lo;
- *retValP = sensorRequest(clientId, sensorHandle, rate, latency);
+ *retValP = sensorRequest(0, sensorHandle, rate, latency);
}
static void osExpApiSensorRateChg(uintptr_t *retValP, va_list args)
{
- uint32_t clientId = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // clientId == tid
uint32_t sensorHandle = va_arg(args, uint32_t);
uint32_t newRate = va_arg(args, uint32_t);
uint32_t newLatency_lo = va_arg(args, uint32_t);
uint32_t newLatency_hi = va_arg(args, uint32_t);
uint64_t newLatency = (((uint64_t)newLatency_hi) << 32) + newLatency_lo;
- *retValP = sensorRequestRateChange(clientId, sensorHandle, newRate, newLatency);
+ *retValP = sensorRequestRateChange(0, sensorHandle, newRate, newLatency);
}
static void osExpApiSensorRel(uintptr_t *retValP, va_list args)
{
- uint32_t clientId = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // clientId == tid
uint32_t sensorHandle = va_arg(args, uint32_t);
- *retValP = sensorRelease(clientId, sensorHandle);
+ *retValP = sensorRelease(0, sensorHandle);
}
static void osExpApiSensorTrigger(uintptr_t *retValP, va_list args)
{
- uint32_t clientId = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // clientId == tid
uint32_t sensorHandle = va_arg(args, uint32_t);
- *retValP = sensorTriggerOndemand(clientId, sensorHandle);
+ *retValP = sensorTriggerOndemand(0, sensorHandle);
}
static void osExpApiSensorGetRate(uintptr_t *retValP, va_list args)
@@ -195,12 +195,12 @@
uint32_t length_hi = va_arg(args, uint32_t);
uint32_t jitterPpm = va_arg(args, uint32_t);
uint32_t driftPpm = va_arg(args, uint32_t);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
bool oneshot = va_arg(args, int);
uint64_t length = (((uint64_t)length_hi) << 32) + length_lo;
- *retValP = timTimerSetAsApp(length, jitterPpm, driftPpm, tid, cookie, oneshot);
+ *retValP = timTimerSetAsApp(length, jitterPpm, driftPpm, 0, cookie, oneshot);
}
static void osExpApiTimCancelTimer(uintptr_t *retValP, va_list args)
@@ -255,12 +255,12 @@
slabAllocatorFree(allocator, mem);
}
-static union OsApiSlabItem* osExpApiI2cCbkInfoAlloc(uint32_t tid, void *cookie)
+static union OsApiSlabItem* osExpApiI2cCbkInfoAlloc(void *cookie)
{
union OsApiSlabItem *thing = slabAllocatorAlloc(mSlabAllocator);
if (thing) {
- thing->i2cAppCbkInfo.toTid = tid;
+ thing->i2cAppCbkInfo.toTid = osGetCurrentTid();
thing->i2cAppCbkInfo.cookie = cookie;
}
@@ -289,6 +289,7 @@
if (!osEnqueuePrivateEvt(EVT_APP_I2C_CBK, &thing->i2cAppCbkEvt, osExpApiI2cInternalEvtFreeF, tid)) {
osLog(LOG_WARN, "Failed to send I2C evt to app. This might end badly for the app...");
osExpApiI2cInternalEvtFreeF(thing);
+ // TODO: terminate app here: memory pressure is severe
}
}
@@ -375,9 +376,9 @@
size_t txSize = va_arg(args, size_t);
void *rxBuf = va_arg(args, void*);
size_t rxSize = va_arg(args, size_t);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
- union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(tid, cookie);
+ union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(cookie);
if (!cbkInfo)
*retValP = -ENOMEM;
@@ -408,9 +409,9 @@
uint32_t busId = va_arg(args, uint32_t);
void *rxBuf = va_arg(args, void*);
size_t rxSize = va_arg(args, size_t);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
- union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(tid, cookie);
+ union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(cookie);
if (!cbkInfo)
*retValP = -ENOMEM;
@@ -425,9 +426,9 @@
{
uint32_t busId = va_arg(args, uint32_t);
uint8_t byte = va_arg(args, int);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
- union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(tid, cookie);
+ union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(cookie);
if (!cbkInfo)
*retValP = -ENOMEM;
@@ -443,9 +444,9 @@
uint32_t busId = va_arg(args, uint32_t);
const void *txBuf = va_arg(args, const void*);
size_t txSize = va_arg(args, size_t);
- uint32_t tid = va_arg(args, uint32_t);
+ (void)va_arg(args, uint32_t); // tid
void *cookie = va_arg(args, void *);
- union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(tid, cookie);
+ union OsApiSlabItem *cbkInfo = osExpApiI2cCbkInfoAlloc(cookie);
if (!cbkInfo)
*retValP = -ENOMEM;
diff --git a/firmware/src/platform/linux/platform.c b/firmware/src/platform/linux/platform.c
index 1fde689..8a45558 100644
--- a/firmware/src/platform/linux/platform.c
+++ b/firmware/src/platform/linux/platform.c
@@ -60,6 +60,11 @@
return 0;
}
+uint32_t platFreeResources(uint32_t tid)
+{
+ return 0;
+}
+
int main(int argc, char** argv)
{
osMain();
diff --git a/firmware/src/platform/stm32f4xx/bl.c b/firmware/src/platform/stm32f4xx/bl.c
index 39bd18c..2786c36 100644
--- a/firmware/src/platform/stm32f4xx/bl.c
+++ b/firmware/src/platform/stm32f4xx/bl.c
@@ -16,18 +16,23 @@
#include <variant/inc/variant.h>
+
#include <plat/inc/cmsis.h>
#include <plat/inc/gpio.h>
#include <plat/inc/pwr.h>
#include <plat/inc/bl.h>
+
+#include <nanohub/sha2.h>
+#include <nanohub/aes.h>
+#include <nanohub/rsa.h>
+#include <nanohub/nanohub.h>
+
#include <printf.h>
#include <string.h>
#include <alloca.h>
#include <gpio.h>
-#include <sha2.h>
-#include <aes.h>
-#include <rsa.h>
+static uint32_t blVerifyOsImage(const uint8_t *addr, struct OsUpdateHdr **start, uint32_t *size);
struct StmCrc
{
@@ -425,8 +430,7 @@
{
struct StmFlash *flash = (struct StmFlash *)FLASH_BASE;
const uint32_t sector_cnt = sizeof(mBlFlashTable) / sizeof(struct blFlashTable);
- uint32_t acr_cache, cr_cache, offset, i, j = 0, int_state = 0, erase_cnt = 0;
- uint8_t erase_mask[sector_cnt];
+ uint32_t acr_cache, cr_cache, offset, i, j = 0, int_state = 0;
uint8_t *ptr;
if (((length == 0)) ||
@@ -437,15 +441,6 @@
return false;
}
- // disable interrupts
- // otherwise an interrupt during flash write/erase will stall the processor
- // until the write/erase completes
- int_state = blDisableInts();
-
- // figure out which (if any) blocks we have to erase
- for (i = 0; i < sector_cnt; i++)
- erase_mask[i] = 0;
-
// compute which flash block we are starting from
for (i = 0; i < sector_cnt; i++) {
if (dst >= mBlFlashTable[i].address &&
@@ -455,7 +450,7 @@
}
// now loop through all the flash blocks and see if we have to do any
- // 0 -> 1 transitions of a bit. If so, we must erase that block
+ // 0 -> 1 transitions of a bit. If so, return false
// 1 -> 0 transitions of a bit do not require an erase
offset = (uint32_t)(dst - mBlFlashTable[i].address);
ptr = mBlFlashTable[i].address;
@@ -467,16 +462,18 @@
}
if ((ptr[offset] & src[j]) != src[j]) {
- erase_mask[i] = 1;
- erase_cnt++;
- j += mBlFlashTable[i].length - offset;
- offset = mBlFlashTable[i].length;
+ return false;
} else {
j++;
offset++;
}
}
+ // disable interrupts
+ // otherwise an interrupt during flash write will stall the processor
+ // until the write completes
+ int_state = blDisableInts();
+
// wait for flash to not be busy (should never be set at this point)
while (flash->SR & FLASH_SR_BSY);
@@ -503,9 +500,6 @@
flash->ACR &= ~(FLASH_ACR_DCEN | FLASH_ACR_ICEN);
flash->ACR |= (FLASH_ACR_DCRST | FLASH_ACR_ICRST);
- if (erase_cnt)
- blEraseSectors(sector_cnt, erase_mask);
-
blWriteBytes(dst, src, length);
flash->ACR = acr_cache;
@@ -516,7 +510,7 @@
return !memcmp(dst, src, length);
}
-static bool blExtApiProgramTypedArea(uint8_t *dst, const uint8_t *src, uint32_t length, uint32_t key1, uint32_t key2, uint32_t type)
+static bool blProgramTypedArea(uint8_t *dst, const uint8_t *src, uint32_t length, uint32_t type, uint32_t key1, uint32_t key2)
{
const uint32_t sector_cnt = sizeof(mBlFlashTable) / sizeof(struct blFlashTable);
uint32_t i;
@@ -537,15 +531,15 @@
static bool blExtApiProgramSharedArea(uint8_t *dst, const uint8_t *src, uint32_t length, uint32_t key1, uint32_t key2)
{
- return blExtApiProgramTypedArea(dst, src, length, key1, key2, BL_FLASH_SHARED);
+ return blProgramTypedArea(dst, src, length, BL_FLASH_SHARED, key1, key2);
}
static bool blExtApiProgramEe(uint8_t *dst, const uint8_t *src, uint32_t length, uint32_t key1, uint32_t key2)
{
- return blExtApiProgramTypedArea(dst, src, length, key1, key2, BL_FLASH_EEDATA);
+ return blProgramTypedArea(dst, src, length, BL_FLASH_EEDATA, key1, key2);
}
-static bool blExtApiEraseSharedArea(uint32_t key1, uint32_t key2)
+static bool blEraseTypedArea(uint32_t type, uint32_t key1, uint32_t key2)
{
struct StmFlash *flash = (struct StmFlash *)FLASH_BASE;
const uint32_t sector_cnt = sizeof(mBlFlashTable) / sizeof(struct blFlashTable);
@@ -553,7 +547,7 @@
uint8_t erase_mask[sector_cnt];
for (i = 0; i < sector_cnt; i++) {
- if (mBlFlashTable[i].type == BL_FLASH_SHARED) {
+ if (mBlFlashTable[i].type == type) {
erase_mask[i] = 1;
erase_cnt++;
} else {
@@ -604,6 +598,30 @@
return true; //we assume erase worked
}
+static bool blExtApiEraseSharedArea(uint32_t key1, uint32_t key2)
+{
+ return blEraseTypedArea(BL_FLASH_SHARED, key1, key2);
+}
+
+static uint32_t blVerifyOsUpdate(struct OsUpdateHdr **start, uint32_t *size)
+{
+ uint32_t ret;
+ int i;
+
+ for (i = 0; i < BL_SCAN_OFFSET; i += 4) {
+ ret = blVerifyOsImage(__shared_start + i, start, size);
+ if (ret != OS_UPDT_HDR_CHECK_FAILED)
+ break;
+ }
+
+ return ret;
+}
+
+static uint32_t blExtApiVerifyOsUpdate(void)
+{
+ return blVerifyOsUpdate(NULL, NULL);
+}
+
static void blSupirousIntHandler(void)
{
//BAD!
@@ -681,105 +699,80 @@
.blAesCbcEncr = &aesCbcEncr,
.blAesCbcDecr = &aesCbcDecr,
.blSigPaddingVerify = &blExtApiSigPaddingVerify,
+ .blVerifyOsUpdate = &blExtApiVerifyOsUpdate,
};
-static void blApplyVerifiedUpdate(void) //only called if an update has been found to exist and be valid, signed, etc!
+static void blApplyVerifiedUpdate(const struct OsUpdateHdr *os) //only called if an update has been found to exist and be valid, signed, etc!
{
- const struct OsUpdateHdr *updt = (const struct OsUpdateHdr*)__shared_start;
-
//copy shared to code, and if successful, erase shared area
- if (blProgramFlash(__code_start, (const uint8_t*)(updt + 1), updt->size, BL_FLASH_KEY1, BL_FLASH_KEY2))
- (void)blExtApiEraseSharedArea(BL_FLASH_KEY1, BL_FLASH_KEY2);
+ if (blEraseTypedArea(BL_FLASH_KERNEL, BL_FLASH_KEY1, BL_FLASH_KEY2))
+ if (blProgramTypedArea(__code_start, (const uint8_t*)(os + 1), os->size, BL_FLASH_KERNEL, BL_FLASH_KEY1, BL_FLASH_KEY2))
+ (void)blExtApiEraseSharedArea(BL_FLASH_KEY1, BL_FLASH_KEY2);
}
-static void blUpdateMark(uint32_t from, uint32_t to)
+static void blWriteMark(struct OsUpdateHdr *hdr, uint32_t mark)
{
- struct OsUpdateHdr *hdr = (struct OsUpdateHdr*)__shared_start;
- uint8_t dstVal = to;
+ uint8_t dstVal = mark;
- if (hdr->marker != from)
+ (void)blExtApiProgramSharedArea(&hdr->marker, &dstVal, sizeof(hdr->marker), BL_FLASH_KEY1, BL_FLASH_KEY2);
+}
+
+static void blUpdateMark(uint32_t old, uint32_t new)
+{
+ struct OsUpdateHdr *hdr = (struct OsUpdateHdr *)__shared_start;
+
+ if (hdr->marker != old)
return;
- (void)blProgramFlash(&hdr->marker, &dstVal, 1, BL_FLASH_KEY1, BL_FLASH_KEY2);
+ blWriteMark(hdr, new);
}
-static bool blUpdateVerify(void)
+static uint32_t blVerifyOsImage(const uint8_t *addr, struct OsUpdateHdr **start, uint32_t *size)
{
const uint32_t *rsaKey, *osSigHash, *osSigPubkey, *ourHash, *rsaResult, *expectedHash = NULL;
- const struct OsUpdateHdr *hdr = (const struct OsUpdateHdr*)__shared_start;
- uint32_t i, j, numRsaKeys = 0, rsaStateVar1, rsaStateVar2, rsaStep = 0;
+ struct OsUpdateHdr *hdr = (struct OsUpdateHdr*)addr;
+ struct OsUpdateHdr cpy;
+ uint32_t i, numRsaKeys = 0, rsaStateVar1, rsaStateVar2, rsaStep = 0;
const uint8_t *updateBinaryData;
bool isValid = false;
struct Sha2state sha;
struct RsaState rsa;
+ uint32_t ret = OS_UPDT_HDR_CHECK_FAILED;
+ const uint32_t overhead = sizeof(*hdr) + 2 * RSA_WORDS;
- //some basic sanity checking
- for (i = 0; i < sizeof(hdr->magic); i++) {
- if (hdr->magic[i] != mOsUpdateMagic[i])
- break;
- }
+ // header does not fit or is not aligned
+ if (addr < __shared_start || addr > (__shared_end - overhead) || ((uintptr_t)addr & 3))
+ return OS_UPDT_HDR_CHECK_FAILED;
- if (i != sizeof(hdr->magic)) {
- //magic value is wrong -> DO NOTHING (shared area might contain something that is not an update but is useful & valid)!
- return false;
- }
+ // image does not fit
+ if (hdr->size > (__shared_end - addr - overhead))
+ return OS_UPDT_HDR_CHECK_FAILED;
- if (hdr->marker == OS_UPDT_MARKER_INVALID) {
- //it's already been checked and found invalid
- return false;
- }
+ // OS magic does not match
+ if (memcmp(hdr->magic, mOsUpdateMagic, sizeof(hdr->magic)) != 0)
+ return OS_UPDT_HDR_CHECK_FAILED;
- if (hdr->marker == OS_UPDT_MARKER_VERIFIED) {
- //it's been verified already
- return true;
- }
+ // we don't allow shortcuts on success path, but we want to fail quickly
+ if (hdr->marker == OS_UPDT_MARKER_INVALID)
+ return OS_UPDT_HDR_MARKER_INVALID;
- if (hdr->marker != OS_UPDT_MARKER_DOWNLOADED) {
- //it's not a fully downloaded update or is not an update
- goto fail;
- }
-
- if (hdr->size & 3) {
- //updates are always multiples of 4 bytes in size
- goto fail;
- }
-
- if (hdr->size > __shared_end - __shared_start) {
- //update would not fit in shared area if it were real
- goto fail;
- }
-
- if (hdr->size > __code_end - __code_start) {
- //udpate would not fit in code area
- goto fail;
- }
-
- if (__shared_end - __shared_start - hdr->size < sizeof(struct OsUpdateHdr) + 2 * RSA_WORDS) {
- //udpate + header + sig would not fit in shared area if it were real
- goto fail;
- }
+ // download did not finish
+ if (hdr->marker == OS_UPDT_MARKER_INPROGRESS)
+ return OS_UPDT_HDR_MARKER_INVALID;
//get pointers
updateBinaryData = (const uint8_t*)(hdr + 1);
osSigHash = (const uint32_t*)(updateBinaryData + hdr->size);
osSigPubkey = osSigHash + RSA_WORDS;
- //hash the update
- sha2init(&sha);
- sha2processBytes(&sha, hdr, sizeof(const struct OsUpdateHdr) + hdr->size);
- ourHash = sha2finish(&sha);
-
//make sure the pub key is known
for (i = 0, rsaKey = blExtApiGetRsaKeyInfo(&numRsaKeys); i < numRsaKeys; i++, rsaKey += RSA_WORDS) {
- for (j = 0; j < RSA_WORDS; j++) {
- if (rsaKey[j] != osSigPubkey[j])
- break;
- }
- if (j == RSA_WORDS) //it matches
+ if (memcmp(rsaKey, osSigPubkey, RSA_BYTES) == 0)
break;
}
if (i == numRsaKeys) {
+ ret = OS_UPDT_UNKNOWN_PUBKEY;
//signed with an unknown key -> fail
goto fail;
}
@@ -791,6 +784,7 @@
if (!rsaResult) {
//decode fails -> invalid sig
+ ret = OS_UPDT_INVALID_SIGNATURE;
goto fail;
}
@@ -799,27 +793,43 @@
if (!expectedHash) {
//padding check fails -> invalid sig
+ ret = OS_UPDT_INVALID_SIGNATURE_HASH;
goto fail;
}
- //verify hash match
- for (i = 0; i < SHA2_HASH_WORDS; i++) {
- if (expectedHash[i] != ourHash[i])
- break;
- }
+ //hash the update
+ sha2init(&sha);
- if (i != SHA2_HASH_WORDS) {
+ memcpy(&cpy, hdr, sizeof(cpy));
+ cpy.marker = OS_UPDT_MARKER_INPROGRESS;
+ sha2processBytes(&sha, &cpy, sizeof(cpy));
+ sha2processBytes(&sha, (uint8_t*)(hdr + 1), hdr->size);
+ ourHash = sha2finish(&sha);
+
+ //verify hash match
+ if (memcmp(expectedHash, ourHash, SHA2_HASH_SIZE) != 0) {
//hash does not match -> data tampered with
+ ret = OS_UPDT_INVALID_SIGNATURE_HASH; // same error; do not disclose nature of hash problem
goto fail;
}
//it is valid
isValid = true;
+ ret = OS_UPDT_SUCCESS;
+ if (start)
+ *start = hdr;
+ if (size)
+ *size = hdr->size;
fail:
//mark it appropriately
- blUpdateMark(OS_UPDT_MARKER_DOWNLOADED, isValid ? OS_UPDT_MARKER_VERIFIED : OS_UPDT_MARKER_INVALID);
- return isValid;
+ blWriteMark(hdr, isValid ? OS_UPDT_MARKER_VERIFIED : OS_UPDT_MARKER_INVALID);
+ return ret;
+}
+
+static inline bool blUpdateVerify()
+{
+ return blVerifyOsImage(__shared_start, NULL, NULL) == OS_UPDT_SUCCESS;
}
static void blSpiLoaderDrainRxFifo(struct StmSpi *spi)
@@ -858,7 +868,7 @@
return blSpiLoaderTxRxByte(spi, 0) == BL_ACK;
}
-static void blSpiLoader(void)
+static void blSpiLoader(bool force)
{
const uint32_t intInPin = SH_INT_WAKEUP - GPIO_PA(0);
struct StmGpio *gpioa = (struct StmGpio*)GPIOA_BASE;
@@ -866,6 +876,8 @@
struct StmRcc *rcc = (struct StmRcc*)RCC_BASE;
uint32_t oldApb2State, oldAhb1State, nRetries;
bool seenErase = false;
+ uint32_t nextAddr = 0;
+ uint32_t expectedSize = 0;
if (SH_INT_WAKEUP < GPIO_PA(0) || SH_INT_WAKEUP > GPIO_PA(15)) {
@@ -894,7 +906,7 @@
gpioa->MODER = (gpioa->MODER & 0xffff00ff & ~(0x03 << (intInPin * 2))) | 0x0000aa00;
//if int pin is not low, do not bother any further
- if (!(gpioa->IDR & (1 << intInPin))) {
+ if (!(gpioa->IDR & (1 << intInPin)) || force) {
//config SPI
spi->CR1 = 0x00000040; //spi is on, configured same as bootloader would
@@ -998,8 +1010,14 @@
}
//reject addresses outside of our fake area or on invalid checksum
- if (blSpiLoaderTxRxByte(spi, 0) != checksum || addr < BL_SHARED_AREA_FAKE_ADDR || addr - BL_SHARED_AREA_FAKE_ADDR > __shared_end - __shared_start)
- break;
+ if (blSpiLoaderTxRxByte(spi, 0) != checksum ||
+ addr < BL_SHARED_AREA_FAKE_ADDR ||
+ addr - BL_SHARED_AREA_FAKE_ADDR > __shared_end - __shared_start)
+ break;
+
+ addr -= BL_SHARED_AREA_FAKE_ADDR;
+ if (addr != nextAddr)
+ break;
//ack the address
(void)blSpiLoaderSendAck(spi, true);
@@ -1016,12 +1034,12 @@
}
//reject writes that takes out outside fo shared area or invalid checksums
- if (blSpiLoaderTxRxByte(spi, 0) != checksum || addr + len - BL_SHARED_AREA_FAKE_ADDR > __shared_end - __shared_start)
+ if (blSpiLoaderTxRxByte(spi, 0) != checksum || addr + len > __shared_end - __shared_start)
break;
- //a write anywhere where the OS header will be must start at 0
- if (addr && addr < sizeof(struct OsUpdateHdr))
- break;
+ // OBSOLETE: superseded by sequential contiguous write requirement
+ //if (addr && addr < sizeof(struct OsUpdateHdr))
+ // break;
//a write starting at zero must be big enough to contain a full OS update header
if (!addr) {
@@ -1037,11 +1055,15 @@
//verify magic check passed & marker is properly set to inprogress
if (i != sizeof(hdr->magic) || hdr->marker != OS_UPDT_MARKER_INPROGRESS)
break;
+ expectedSize = sizeof(*hdr) + hdr->size + 2 * RSA_BYTES;
}
+ if (addr + len > expectedSize)
+ break;
//do it
- ack = blProgramFlash(__shared_start + addr - BL_SHARED_AREA_FAKE_ADDR, data, len, BL_FLASH_KEY1, BL_FLASH_KEY2);
+ ack = blExtApiProgramSharedArea(__shared_start + addr, data, len, BL_FLASH_KEY1, BL_FLASH_KEY2);
blSpiLoaderDrainRxFifo(spi);
+ nextAddr += len;
break;
case BL_CMD_ERASE:
@@ -1062,8 +1084,11 @@
//do it
ack = blExtApiEraseSharedArea(BL_FLASH_KEY1, BL_FLASH_KEY2);
- if (ack)
+ if (ack) {
seenErase = true;
+ nextAddr = 0;
+ expectedSize = 0;
+ }
blSpiLoaderDrainRxFifo(spi);
break;
@@ -1076,7 +1101,6 @@
break;
case BL_CMD_UPDATE_FINISHED:
-
blUpdateMark(OS_UPDT_MARKER_INPROGRESS, OS_UPDT_MARKER_DOWNLOADED);
ack = blUpdateVerify();
break;
@@ -1099,6 +1123,7 @@
{
extern char __bss_end[], __bss_start[], __data_end[], __data_start[], __data_data[];
uint32_t appBase = ((uint32_t)&__code_start) & ~1;
+ bool forceLoad = false;
//make sure we're the vector table and no ints happen (BL does not use them)
blDisableInts();
@@ -1112,11 +1137,19 @@
blLog("NanohubOS bootloader up @ %p\n", &__blEntry);
//enter SPI loader if requested
- blSpiLoader();
+ do {
+ uint32_t res;
+ struct OsUpdateHdr *os;
- //try to run main app
- if (blUpdateVerify())
- blApplyVerifiedUpdate();
+ blSpiLoader(forceLoad);
+ res = blVerifyOsUpdate(&os, NULL);
+ if (res == OS_UPDT_SUCCESS)
+ blApplyVerifiedUpdate(os);
+ else if (res != OS_UPDT_HDR_CHECK_FAILED)
+ blExtApiEraseSharedArea(BL_FLASH_KEY1, BL_FLASH_KEY2);
+
+ forceLoad = true;
+ } while (*(volatile uint32_t*)appBase == 0xFFFFFFFF);
//call main app with ints off
blDisableInts();
@@ -1132,17 +1165,3 @@
//we should never return here
while(1);
}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/firmware/src/platform/stm32f4xx/crc.c b/firmware/src/platform/stm32f4xx/crc.c
index 8538cc5..771703d 100644
--- a/firmware/src/platform/stm32f4xx/crc.c
+++ b/firmware/src/platform/stm32f4xx/crc.c
@@ -16,7 +16,7 @@
#include <string.h>
-#include <crc.h>
+#include <nanohub/crc.h>
#include <seos.h>
#include <plat/inc/pwr.h>
diff --git a/firmware/src/platform/stm32f4xx/dma.c b/firmware/src/platform/stm32f4xx/dma.c
index e295f83..226bb98 100644
--- a/firmware/src/platform/stm32f4xx/dma.c
+++ b/firmware/src/platform/stm32f4xx/dma.c
@@ -86,6 +86,8 @@
struct StmDmaStreamState {
DmaCallbackF callback;
void *cookie;
+ uint16_t tid;
+ uint16_t reserved;
};
struct StmDmaDev {
@@ -194,9 +196,11 @@
struct StmDmaStreamRegs *regs = dmaGetStreamRegs(busId, stream);
dmaLogDebug("teif");
-
dmaStop(busId, stream);
+
+ uint16_t oldTid = osSetCurrentTid(state->tid);
state->callback(state->cookie, regs->NDTR, EIO);
+ osSetCurrentTid(oldTid);
}
static void dmaIsrTcif(uint8_t busId, uint8_t stream)
@@ -205,9 +209,11 @@
struct StmDmaStreamRegs *regs = dmaGetStreamRegs(busId, stream);
dmaLogDebug("tcif");
-
dmaStop(busId, stream);
+
+ uint16_t oldTid = osSetCurrentTid(state->tid);
state->callback(state->cookie, regs->NDTR, 0);
+ osSetCurrentTid(oldTid);
}
static void dmaIsr(uint8_t busId, uint8_t stream)
@@ -238,6 +244,7 @@
struct StmDmaStreamState *state = dmaGetStreamState(busId, stream);
state->callback = callback;
state->cookie = cookie;
+ state->tid = osGetCurrentTid();
pwrUnitClock(PERIPH_BUS_AHB1, STM_DMA_CLOCK_UNIT[busId], true);
@@ -276,7 +283,9 @@
void dmaStop(uint8_t busId, uint8_t stream)
{
struct StmDmaStreamRegs *regs = dmaGetStreamRegs(busId, stream);
+ struct StmDmaStreamState *state = dmaGetStreamState(busId, stream);
+ state->tid = 0;
dmaClearIsr(busId, stream, STM_DMA_ISR_TEIFx);
dmaClearIsr(busId, stream, STM_DMA_ISR_TCIFx);
NVIC_DisableIRQ(STM_DMA_IRQ[busId][stream]);
@@ -284,9 +293,27 @@
regs->CR &= ~STM_DMA_CR_EN;
while (regs->CR & STM_DMA_CR_EN)
;
+
}
const enum IRQn dmaIrq(uint8_t busId, uint8_t stream)
{
return STM_DMA_IRQ[busId][stream];
}
+
+int dmaStopAll(uint32_t tid)
+{
+ int busId, stream, count = 0;
+
+ for (busId = 0; busId < STM_DMA_NUM_DEVS; ++busId) {
+ for (stream = 0; stream < STM_DMA_NUM_STREAMS; ++stream) {
+ struct StmDmaStreamState *state = dmaGetStreamState(busId, stream);
+ if (state->tid == tid) {
+ dmaStop(busId, stream);
+ count++;
+ }
+ }
+ }
+
+ return count;
+}
diff --git a/firmware/src/platform/stm32f4xx/eeData.c b/firmware/src/platform/stm32f4xx/eeData.c
index bcd8893..906b9f3 100644
--- a/firmware/src/platform/stm32f4xx/eeData.c
+++ b/firmware/src/platform/stm32f4xx/eeData.c
@@ -20,7 +20,6 @@
#include <stdint.h>
#include <eeData.h>
-
extern uint32_t __eedata_start[], __eedata_end[];
//STM32F4xx eedata stores data in 4-byte aligned chunks
@@ -65,7 +64,7 @@
return name && name < EE_DATA_NAME_MAX;
}
-static bool eeDataGetEx(uint32_t name, uint32_t *offsetP, bool first, void *buf, uint32_t *szP)
+static void *eeDataGetEx(uint32_t name, uint32_t *offsetP, bool first, void *buf, uint32_t *szP)
{
uint32_t sz = 0;
void *data;
@@ -76,7 +75,7 @@
//find the data item
data = eeFind(name, offsetP, first, &sz);
if (!data)
- return false;
+ return NULL;
if (buf && szP) { //get the data
if (sz > *szP)
@@ -87,25 +86,22 @@
else if (szP) //get size
*szP = sz;
- return true;
+ return (uint32_t*)data - 1;
}
bool eeDataGet(uint32_t name, void *buf, uint32_t *szP)
{
uint32_t offset = 0;
- return eeDataGetEx(name, &offset, false, buf, szP);
+ return eeDataGetEx(name, &offset, false, buf, szP) != NULL;
}
-bool eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP)
+void *eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP)
{
uint32_t offset = *(uint32_t*)stateP;
- bool ret;
-
- ret = eeDataGetEx(name, &offset, true, buf, szP);
+ void *addr = eeDataGetEx(name, &offset, true, buf, szP);
*(uint32_t*)stateP = offset;
-
- return ret;
+ return addr;
}
static bool eeWrite(void *dst, const void *src, uint32_t len)
@@ -141,13 +137,17 @@
return ret;
}
-bool eeDataEraseOldVersion(uint32_t name, void *state)
+bool eeDataEraseOldVersion(uint32_t name, void *vaddr)
{
- uint32_t v = *(uint32_t*)state;
+ uint32_t *addr = (uint32_t*)vaddr;
+ uint32_t v;
- if (!eeIsValidName(name))
+ // sanity check
+ if (!eeIsValidName(name) || addr < __eedata_start || addr >= (__eedata_end - 1))
return false;
+ v = *addr;
+
//verify name
if ((v & EE_DATA_NAME_MAX) != name)
return false;
@@ -156,15 +156,5 @@
v &=~ EE_DATA_NAME_MAX;
//store result
- return eeWrite(state, &v, sizeof(v));
+ return eeWrite(addr, &v, sizeof(v));
}
-
-
-
-
-
-
-
-
-
-
diff --git a/firmware/src/platform/stm32f4xx/exti.c b/firmware/src/platform/stm32f4xx/exti.c
index 596f663..30e91db 100644
--- a/firmware/src/platform/stm32f4xx/exti.c
+++ b/firmware/src/platform/stm32f4xx/exti.c
@@ -154,3 +154,14 @@
unchainIsr(&exti->base, isr);
return 0;
}
+
+int extiUnchainAll(uint32_t tid)
+{
+ int i, count = 0;
+ struct ExtiInterrupt *exti = gInterrupts;
+
+ for (i = 0; i < ARRAY_SIZE(gInterrupts); ++i, ++exti)
+ count += unchainIsrAll(&exti->base, tid);
+
+ return count;
+}
diff --git a/firmware/src/platform/stm32f4xx/i2c.c b/firmware/src/platform/stm32f4xx/i2c.c
index feb433c..dd911c1 100644
--- a/firmware/src/platform/stm32f4xx/i2c.c
+++ b/firmware/src/platform/stm32f4xx/i2c.c
@@ -159,6 +159,7 @@
// StmI2cSpiMasterState
uint8_t masterState;
+ uint16_t tid;
};
struct StmI2cCfg {
@@ -350,13 +351,27 @@
stmI2cIrqDisable(pdev, I2C_CR2_ITBUFEN | I2C_CR2_ITERREN);
}
+static inline void stmI2cInvokeRxCallback(struct I2cStmState *state, size_t tx, size_t rx, int err)
+{
+ uint16_t oldTid = osSetCurrentTid(state->tid);
+ state->rx.callback(state->rx.cookie, tx, rx, err);
+ osSetCurrentTid(oldTid);
+}
+
+static inline void stmI2cInvokeTxCallback(struct I2cStmState *state, size_t tx, size_t rx, int err)
+{
+ uint16_t oldTid = osSetCurrentTid(state->tid);
+ state->tx.callback(state->tx.cookie, tx, rx, err);
+ osSetCurrentTid(oldTid);
+}
+
static inline void stmI2cSlaveRxDone(struct StmI2cDev *pdev)
{
struct I2cStmState *state = &pdev->state;
size_t rxOffst = state->rx.offset;
state->rx.offset = 0;
- state->rx.callback(state->rx.cookie, 0, rxOffst, 0);
+ stmI2cInvokeRxCallback(state, 0, rxOffst, 0);
}
static inline void stmI2cSlaveTxDone(struct StmI2cDev *pdev)
@@ -365,7 +380,7 @@
size_t txOffst = state->tx.offset;
stmI2cSlaveIdle(pdev);
- state->tx.callback(state->tx.cookie, txOffst, 0, 0);
+ stmI2cInvokeTxCallback(state, txOffst, 0, 0);
}
static void stmI2cSlaveTxNextByte(struct StmI2cDev *pdev)
@@ -382,7 +397,7 @@
} else {
state->slaveState = STM_I2C_SLAVE_TX_ARMED;
stmI2cIrqDisable(pdev, I2C_CR2_ITBUFEN);
- state->tx.callback(state->tx.cookie, state->tx.offset, 0, 0);
+ stmI2cInvokeTxCallback(state, state->tx.offset, 0, 0);
}
}
@@ -483,10 +498,10 @@
state->tx.offset = 0;
state->rx.offset = 0;
- state->tx.callback(state->tx.cookie, txOffst, rxOffst, err);
+ stmI2cInvokeTxCallback(state, txOffst, rxOffst, 0);
do {
- id = atomicAdd(&pdev->next, 1);
+ id = atomicAdd32bits(&pdev->next, 1);
} while (!id);
for (i=0; i<I2C_MAX_QUEUE_DEPTH; i++) {
@@ -857,7 +872,7 @@
xfer->cookie = cookie;
do {
- id = atomicAdd(&pdev->last, 1);
+ id = atomicAdd32bits(&pdev->last, 1);
} while (!id);
// after this point the transfer can be picked up by the transfer
@@ -883,6 +898,7 @@
state->rx.size = xfer->rxSize;
state->rx.callback = NULL;
state->rx.cookie = NULL;
+ state->tid = osGetCurrentTid();
if (pdev->board->sleepDev >= 0)
platRequestDevInSleepMode(pdev->board->sleepDev, 12);
stmI2cPutXfer(xfer);
@@ -961,6 +977,7 @@
state->rx.callback = callback;
state->rx.cookie = cookie;
state->slaveState = STM_I2C_SLAVE_RX_ARMED;
+ state->tid = osGetCurrentTid();
pwrUnitClock(PERIPH_BUS_APB1, cfg->clock, true);
pwrUnitReset(PERIPH_BUS_APB1, cfg->clock, true);
diff --git a/firmware/src/platform/stm32f4xx/platform.c b/firmware/src/platform/stm32f4xx/platform.c
index d1046d3..7f213a0 100644
--- a/firmware/src/platform/stm32f4xx/platform.c
+++ b/firmware/src/platform/stm32f4xx/platform.c
@@ -23,6 +23,7 @@
#include <plat/inc/plat.h>
#include <plat/inc/exti.h>
#include <plat/inc/syscfg.h>
+#include <plat/inc/dma.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
@@ -168,7 +169,7 @@
while (i < mEarlyLogBufferOffset) {
userData = (struct HostIntfDataBuffer *)(mEarlyLogBuffer + i);
- osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | DEBUG_LOG_EVT, userData, platEarlyLogFree);
+ osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | EVT_DEBUG_LOG, userData, platEarlyLogFree);
i += HOSTINTF_HEADER_SIZE + userData->length;
}
#endif
@@ -178,7 +179,7 @@
{
#if defined(DEBUG_LOG_EVT)
if (userData && mLateBoot) {
- if (!osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | DEBUG_LOG_EVT, userData, heapFree))
+ if (!osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | EVT_DEBUG_LOG, userData, heapFree))
heapFree(userData);
}
#endif
@@ -679,3 +680,10 @@
return rtcGetBackupStorage();
}
+uint32_t platFreeResources(uint32_t tid)
+{
+ uint32_t dmaCount = dmaStopAll(tid);
+ uint32_t irqCount = extiUnchainAll(tid);
+
+ return (dmaCount << 8) | irqCount;
+}
diff --git a/firmware/src/sensors.c b/firmware/src/sensors.c
index 4468e54..b6e606c 100644
--- a/firmware/src/sensors.c
+++ b/firmware/src/sensors.c
@@ -35,6 +35,11 @@
#define SENSOR_RATE_IMPOSSIBLE 0xFFFFFFF3UL /* used in rate calc to indicate impossible combinations */
#define SENSOR_LATENCY_INVALID 0xFFFFFFFFFFFFFFFFULL
+#define HANDLE_TO_TID(handle) (((handle) >> (32 - TASK_TID_BITS)) & TASK_TID_MASK)
+#define EXT_APP_TID(s) HANDLE_TO_TID(s->handle)
+#define LOCAL_APP_OPS(s) ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))
+#define IS_LOCAL_APP(s) (taggedPtrIsPtr(s->callInfo))
+
struct Sensor {
const struct SensorInfo *si;
uint32_t handle; /* here 0 means invalid */
@@ -77,8 +82,12 @@
struct SingleAxisDataEvent singleAxisFlush = { .referenceTime = 0 };
struct TripleAxisDataEvent tripleAxisFlush = { .referenceTime = 0 };
-
-
+static inline uint32_t newSensorHandle()
+{
+ // FIXME: only let lower 8 bits of counter to the id; should use all 16 bits, but this
+ // somehow confuses upper layers; pending investigation
+ return (osGetCurrentTid() << 16) | (atomicAdd32bits(&mNextSensorHandle, 1) & 0xFF);
+}
bool sensorsInit(void)
{
@@ -118,10 +127,13 @@
if (idx < 0)
return 0;
- /* grab a handle */
+ /* grab a handle:
+ * this is safe since nobody else could have "JUST" taken this handle,
+ * we'll need to circle around 16 bits before that happens, and have the same TID
+ */
do {
- handle = atomicAdd(&mNextSensorHandle, 1);
- } while (!handle || sensorFindByHandle(handle)); /* this is safe since nobody else could have "JUST" taken this handle, we'll need to circle around 32bits before that happens */
+ handle = newSensorHandle();
+ } while (!handle || sensorFindByHandle(handle));
/* fill the struct in and mark it valid (by setting handle) */
s = mSensors + idx;
@@ -129,6 +141,7 @@
s->currentRate = SENSOR_RATE_OFF;
s->currentLatency = SENSOR_LATENCY_INVALID;
s->callInfo = callInfo;
+ // TODO: is internal app, callinfo is OPS struct; shall we validate it here?
s->callData = callData;
s->initComplete = initComplete ? 1 : 0;
mem_reorder_barrier();
@@ -153,9 +166,10 @@
return sensorRegisterEx(si, taggedPtrMakeFromPtr(ops), callData, initComplete);
}
-uint32_t sensorRegisterAsApp(const struct SensorInfo *si, uint32_t tid, void *callData, bool initComplete)
+uint32_t sensorRegisterAsApp(const struct SensorInfo *si, uint32_t unusedTid, void *callData, bool initComplete)
{
- return sensorRegisterEx(si, taggedPtrMakeFromUint(tid), callData, initComplete);
+ (void)unusedTid;
+ return sensorRegisterEx(si, taggedPtrMakeFromUint(0), callData, initComplete);
}
bool sensorRegisterInitComplete(uint32_t handle)
@@ -193,11 +207,21 @@
slabAllocatorFree(mInternalEvents, event);
}
+#define INVOKE_AS_OWNER_AND_RETURN(func, ...) \
+{ \
+ if (!func) \
+ return false; \
+ uint16_t oldTid = osSetCurrentTid(HANDLE_TO_TID(s->handle)); \
+ bool done = func(__VA_ARGS__); \
+ osSetCurrentTid(oldTid); \
+ return done; \
+}
+
static bool sensorCallFuncPower(struct Sensor* s, bool on)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorPower(on, s->callData);
- else {
+ if (IS_LOCAL_APP(s)) {
+ INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorPower, on, s->callData);
+ } else {
struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents);
if (!evt)
@@ -205,7 +229,9 @@
evt->externalPowerEvt.on = on;
evt->externalPowerEvt.callData = s->callData;
- if (osEnqueuePrivateEvt(EVT_APP_SENSOR_POWER, &evt->externalPowerEvt, sensorCallFuncPowerEvtFreeF, taggedPtrToUint(s->callInfo)))
+
+ if (osEnqueuePrivateEvt(EVT_APP_SENSOR_POWER, &evt->externalPowerEvt,
+ sensorCallFuncPowerEvtFreeF, EXT_APP_TID(s)))
return true;
slabAllocatorFree(mInternalEvents, evt);
@@ -213,12 +239,18 @@
}
}
+// the most common callback goes as a helper function
+static bool sensorCallAsOwner(struct Sensor* s, bool (*callback)(void*))
+{
+ INVOKE_AS_OWNER_AND_RETURN(callback, s->callData);
+}
+
static bool sensorCallFuncFwUpld(struct Sensor* s)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorFirmwareUpload(s->callData);
+ if (IS_LOCAL_APP(s))
+ return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorFirmwareUpload);
else
- return osEnqueuePrivateEvt(EVT_APP_SENSOR_FW_UPLD, s->callData, NULL, taggedPtrToUint(s->callInfo));
+ return osEnqueuePrivateEvt(EVT_APP_SENSOR_FW_UPLD, s->callData, NULL, EXT_APP_TID(s));
}
static void sensorCallFuncExternalEvtFreeF(void* event)
@@ -228,9 +260,9 @@
static bool sensorCallFuncSetRate(struct Sensor* s, uint32_t rate, uint64_t latency)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorSetRate(rate, latency, s->callData);
- else {
+ if (IS_LOCAL_APP(s)) {
+ INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorSetRate, rate, latency, s->callData);
+ } else {
struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents);
if (!evt)
@@ -239,7 +271,8 @@
evt->externalSetRateEvt.latency = latency;
evt->externalSetRateEvt.rate = rate;
evt->externalSetRateEvt.callData = s->callData;
- if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SET_RATE, &evt->externalSetRateEvt, sensorCallFuncExternalEvtFreeF, taggedPtrToUint(s->callInfo)))
+ if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SET_RATE, &evt->externalSetRateEvt,
+ sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s)))
return true;
slabAllocatorFree(mInternalEvents, evt);
@@ -249,25 +282,25 @@
static bool sensorCallFuncCalibrate(struct Sensor* s)
{
- if (taggedPtrIsPtr(s->callInfo) && ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorCalibrate)
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorCalibrate(s->callData);
+ if (IS_LOCAL_APP(s))
+ return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorCalibrate);
else
- return osEnqueuePrivateEvt(EVT_APP_SENSOR_CALIBRATE, s->callData, NULL, taggedPtrToUint(s->callInfo));
+ return osEnqueuePrivateEvt(EVT_APP_SENSOR_CALIBRATE, s->callData, NULL, EXT_APP_TID(s));
}
static bool sensorCallFuncFlush(struct Sensor* s)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorFlush(s->callData);
+ if (IS_LOCAL_APP(s))
+ return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorFlush);
else
- return osEnqueuePrivateEvt(EVT_APP_SENSOR_FLUSH, s->callData, NULL, taggedPtrToUint(s->callInfo));
+ return osEnqueuePrivateEvt(EVT_APP_SENSOR_FLUSH, s->callData, NULL, EXT_APP_TID(s));
}
static bool sensorCallFuncCfgData(struct Sensor* s, void* cfgData)
{
- if (taggedPtrIsPtr(s->callInfo) && ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorCfgData)
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorCfgData(cfgData, s->callData);
- else {
+ if (IS_LOCAL_APP(s)) {
+ INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorCfgData, cfgData, s->callData);
+ } else {
struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents);
if (!evt)
@@ -275,7 +308,8 @@
evt->externalCfgDataEvt.data = cfgData;
evt->externalCfgDataEvt.callData = s->callData;
- if (osEnqueuePrivateEvt(EVT_APP_SENSOR_CFG_DATA, &evt->externalCfgDataEvt, sensorCallFuncExternalEvtFreeF, taggedPtrToUint(s->callInfo)))
+ if (osEnqueuePrivateEvt(EVT_APP_SENSOR_CFG_DATA, &evt->externalCfgDataEvt,
+ sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s)))
return true;
slabAllocatorFree(mInternalEvents, evt);
@@ -285,9 +319,9 @@
static bool sensorCallFuncMarshall(struct Sensor* s, uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP)
{
- if (taggedPtrIsPtr(s->callInfo) && ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorCfgData)
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorMarshallData(evtType, evtData, evtFreeingInfoP, s->callData);
- else {
+ if (IS_LOCAL_APP(s)) {
+ INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorMarshallData, evtType, evtData, evtFreeingInfoP, s->callData);
+ } else {
struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents);
if (!evt)
@@ -297,7 +331,8 @@
evt->externalMarshallEvt.origEvtData = evtData;
evt->externalMarshallEvt.evtFreeingInfo = *evtFreeingInfoP;
evt->externalMarshallEvt.callData = s->callData;
- if (osEnqueuePrivateEvt(EVT_APP_SENSOR_MARSHALL, &evt->externalMarshallEvt, sensorCallFuncExternalEvtFreeF, taggedPtrToUint(s->callInfo)))
+ if (osEnqueuePrivateEvt(EVT_APP_SENSOR_MARSHALL, &evt->externalMarshallEvt,
+ sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s)))
return true;
slabAllocatorFree(mInternalEvents, evt);
@@ -307,17 +342,17 @@
static bool sensorCallFuncTrigger(struct Sensor* s)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorTriggerOndemand(s->callData);
+ if (IS_LOCAL_APP(s))
+ return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorTriggerOndemand);
else
- return osEnqueuePrivateEvt(EVT_APP_SENSOR_TRIGGER, s->callData, NULL, taggedPtrToUint(s->callInfo));
+ return osEnqueuePrivateEvt(EVT_APP_SENSOR_TRIGGER, s->callData, NULL, EXT_APP_TID(s));
}
static bool sensorCallFuncSendOneDirectEvt(struct Sensor* s, uint32_t tid)
{
- if (taggedPtrIsPtr(s->callInfo))
- return ((const struct SensorOps*)taggedPtrToPtr(s->callInfo))->sensorSendOneDirectEvt(s->callData, tid);
- else {
+ if (IS_LOCAL_APP(s)) {
+ INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorSendOneDirectEvt, s->callData, tid);
+ } else {
struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents);
if (!evt)
@@ -325,7 +360,8 @@
evt->externalSendDirectEvt.tid = tid;
evt->externalSendDirectEvt.callData = s->callData;
- if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SEND_ONE_DIR_EVT, &evt->externalSendDirectEvt, sensorCallFuncExternalEvtFreeF, taggedPtrToUint(s->callInfo)))
+ if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SEND_ONE_DIR_EVT, &evt->externalSendDirectEvt,
+ sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s)))
return true;
slabAllocatorFree(mInternalEvents, evt);
@@ -610,6 +646,8 @@
if (req && req->handle == sensorHandle && req->clientTid == clientTid) {
req->rate = SENSOR_RATE_OFF;
req->latency = SENSOR_LATENCY_INVALID;
+ req->clientTid = 0;
+ req->handle = 0;
mem_reorder_barrier();
slabAllocatorFree(mCliSensMatrix, req);
return true;
@@ -619,15 +657,20 @@
return false;
}
-bool sensorRequest(uint32_t clientTid, uint32_t sensorHandle, uint32_t rate, uint64_t latency)
+bool sensorRequest(uint32_t unusedTid, uint32_t sensorHandle, uint32_t rate, uint64_t latency)
{
struct Sensor* s = sensorFindByHandle(sensorHandle);
uint32_t newSensorRate;
uint64_t samplingPeriod;
+ uint32_t clientTid;
+
+ (void)unusedTid;
if (!s)
return false;
+ clientTid = osGetCurrentTid();
+
/* verify the rate is possible */
newSensorRate = sensorCalcHwRate(s, rate, 0);
if (newSensorRate == SENSOR_RATE_IMPOSSIBLE)
@@ -651,16 +694,19 @@
return true;
}
-bool sensorRequestRateChange(uint32_t clientTid, uint32_t sensorHandle, uint32_t newRate, uint64_t newLatency)
+bool sensorRequestRateChange(uint32_t unusedTid, uint32_t sensorHandle, uint32_t newRate, uint64_t newLatency)
{
struct Sensor* s = sensorFindByHandle(sensorHandle);
uint32_t oldRate, newSensorRate;
uint64_t oldLatency, samplingPeriod;
+ uint32_t clientTid;
+
+ (void)unusedTid;
if (!s)
return false;
-
+ clientTid = osGetCurrentTid();
/* get current rate */
if (!sensorGetCurRequestorRate(sensorHandle, clientTid, &oldRate, &oldLatency))
return false;
@@ -683,14 +729,17 @@
return true;
}
-bool sensorRelease(uint32_t clientTid, uint32_t sensorHandle)
+bool sensorRelease(uint32_t unusedTid, uint32_t sensorHandle)
{
struct Sensor* s = sensorFindByHandle(sensorHandle);
+
+ (void) unusedTid;
+
if (!s)
return false;
/* record the request */
- if (!sensorDeleteRequestor(sensorHandle, clientTid))
+ if (!sensorDeleteRequestor(sensorHandle, osGetCurrentTid()))
return false;
/* update actual sensor if needed */
@@ -698,14 +747,19 @@
return true;
}
-bool sensorTriggerOndemand(uint32_t clientTid, uint32_t sensorHandle)
+bool sensorTriggerOndemand(uint32_t unusedTid, uint32_t sensorHandle)
{
struct Sensor* s = sensorFindByHandle(sensorHandle);
uint32_t i;
+ uint32_t clientTid;
+
+ (void)unusedTid;
if (!s || !s->hasOndemand)
return false;
+ clientTid = osGetCurrentTid();
+
for (i = 0; i < MAX_CLI_SENS_MATRIX_SZ; i++) {
struct SensorsClientRequest *req = slabAllocatorGetNth(mCliSensMatrix, i);
@@ -777,3 +831,16 @@
return sensorCallFuncMarshall(s, evtType, evtData, evtFreeingInfoP);
}
+
+int sensorUnregisterAll(uint32_t tid)
+{
+ int i, count = 0;
+
+ for (i = 0; i < MAX_REGISTERED_SENSORS; i++)
+ if (HANDLE_TO_TID(mSensors[i].handle) == tid) {
+ sensorUnregister(mSensors[i].handle);
+ count++;
+ }
+
+ return count;
+}
diff --git a/firmware/src/seos.c b/firmware/src/seos.c
index 9d4ce50..cc24576 100644
--- a/firmware/src/seos.c
+++ b/firmware/src/seos.c
@@ -34,35 +34,74 @@
#include <heap.h>
#include <slab.h>
#include <cpu.h>
-#include <crc.h>
#include <util.h>
+#include <mpu.h>
+#include <nanohubPacket.h>
+#include <atomic.h>
+#include <nanohub/nanohub.h>
+#include <nanohub/crc.h>
+
+#define NO_NODE (TaskIndex)(-1)
+#define for_each_task(listHead, task) for (task = osTaskByIdx((listHead)->next); task; task = osTaskByIdx(task->list.next))
+#define MAKE_NEW_TID(task) task->tid = ((task->tid + TASK_TID_INCREMENT) & TASK_TID_COUNTER_MASK) | \
+ (osTaskIndex(task) & TASK_TID_IDX_MASK);
+#define TID_TO_TASK_IDX(tid) (tid & TASK_TID_IDX_MASK)
+
+#define FL_TASK_STOPPED 1
+
+#define EVT_SUBSCRIBE_TO_EVT 0x00000000
+#define EVT_UNSUBSCRIBE_TO_EVT 0x00000001
+#define EVT_DEFERRED_CALLBACK 0x00000002
+#define EVT_PRIVATE_EVT 0x00000003
+
+#define EVENT_WITH_ORIGIN(evt, origin) (((evt) & EVT_MASK) | ((origin) << (32 - TASK_TID_BITS)))
+#define EVENT_GET_ORIGIN(evt) ((evt) >> (32 - TASK_TID_BITS))
+#define EVENT_GET_EVENT(evt) ((evt) & (EVT_MASK & ~EVENT_TYPE_BIT_DISCARDABLE))
/*
* Since locking is difficult to do right for adding/removing listeners and such
* since it can happen in interrupt context and not, and one such operation can
* interrupt another, and we do have a working event queue, we enqueue all the
* requests and then deal with them in the main code only when the event bubbles
- * up to the front of the quque. This allows us to not need locks around the
+ * up to the front of the queue. This allows us to not need locks around the
* data structures.
*/
+SET_PACKED_STRUCT_MODE_ON
+struct TaskList {
+ TaskIndex prev;
+ TaskIndex next;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
struct Task {
- /* pointers may become invalid. Tids do not. Zero tid -> not a valid task */
- uint32_t tid;
-
- uint16_t subbedEvtCount;
- uint16_t subbedEvtListSz;
- uint32_t *subbedEvents; /* NULL for invalid tasks */
-
/* App entry points */
- const struct AppHdr *appHdr;
+ const struct AppHdr *app;
/* per-platform app info */
struct PlatAppInfo platInfo;
/* for some basic number of subbed events, the array is stored directly here. after that, a heap chunk is used */
uint32_t subbedEventsInt[MAX_EMBEDDED_EVT_SUBS];
+ uint32_t *subbedEvents; /* NULL for invalid tasks */
+
+ struct TaskList list;
+
+ /* task pointer will not change throughout task lifetime,
+ * however same task pointer may be reused for a new task; to eliminate the ambiguity,
+ * TID is maintained for each task such that new tasks will be guaranteed to receive different TID */
+ uint16_t tid;
+
+ uint8_t subbedEvtCount;
+ uint8_t subbedEvtListSz;
+ uint8_t flags;
+ uint8_t ioCount;
+
+};
+
+struct TaskPool {
+ struct Task data[MAX_TASKS];
};
union InternalThing {
@@ -83,27 +122,253 @@
union OsApiSlabItem osApiItem;
};
-#define EVT_SUBSCRIBE_TO_EVT 0x00000000
-#define EVT_UNSUBSCRIBE_TO_EVT 0x00000001
-#define EVT_DEFERRED_CALLBACK 0x00000002
-#define EVT_PRIVATE_EVT 0x00000003
-
-
+static struct TaskPool mTaskPool;
static struct EvtQueue *mEvtsInternal;
static struct SlabAllocator* mMiscInternalThingsSlab;
-static struct Task mTasks[MAX_TASKS];
-static uint32_t mNextTidInfo = FIRST_VALID_TID;
+static struct TaskList mFreeTasks;
+static struct TaskList mTasks;
+static struct Task *mCurrentTask;
+static struct Task *mSystemTask;
static TaggedPtr *mCurEvtEventFreeingInfo = NULL; //used as flag for retaining. NULL when none or already retained
-static struct Task* osTaskFindByTid(uint32_t tid)
+static inline void list_init(struct TaskList *l)
{
- uint32_t i;
+ l->prev = l->next = NO_NODE;
+}
- for(i = 0; i < MAX_TASKS; i++)
- if (mTasks[i].tid && mTasks[i].tid == tid)
- return mTasks + i;
+static inline struct Task *osGetCurrentTask()
+{
+ return mCurrentTask;
+}
- return NULL;
+static struct Task *osSetCurrentTask(struct Task *task)
+{
+ struct Task *old = mCurrentTask;
+ while (true) {
+ old = mCurrentTask;
+ if (atomicCmpXchg32bits((uint32_t*)&mCurrentTask, (uint32_t)old, (uint32_t)task))
+ break;
+ }
+ return old;
+}
+
+// beyond this point, noone shall access mCurrentTask directly
+
+static inline bool osTaskTestFlags(struct Task *task, uint32_t mask)
+{
+ return (atomicReadByte(&task->flags) & mask) != 0;
+}
+
+static inline uint32_t osTaskClrSetFlags(struct Task *task, uint32_t clrMask, uint32_t setMask)
+{
+ while (true) {
+ uint8_t flags = atomicReadByte(&task->flags);
+ uint8_t newFlags = (flags & ~clrMask) | setMask;
+ if (atomicCmpXchgByte(&task->flags, flags, newFlags))
+ return newFlags;
+ }
+}
+
+static inline uint32_t osTaskAddIoCount(struct Task *task, int32_t delta)
+{
+ uint8_t count = atomicAddByte(&task->ioCount, delta);
+
+ count += delta; // old value is returned, so we add it again
+
+ return count;
+}
+
+static inline uint32_t osTaskGetIoCount(struct Task *task)
+{
+ return atomicReadByte(&task->ioCount);
+}
+
+static inline uint8_t osTaskIndex(struct Task *task)
+{
+ // we don't need signed diff here: this way we simplify boundary check
+ size_t idx = task - &mTaskPool.data[0];
+ return idx >= MAX_TASKS || &mTaskPool.data[idx] != task ? NO_NODE : idx;
+}
+
+static inline struct Task *osTaskByIdx(size_t idx)
+{
+ return idx >= MAX_TASKS ? NULL : &mTaskPool.data[idx];
+}
+
+uint32_t osGetCurrentTid()
+{
+ return osGetCurrentTask()->tid;
+}
+
+uint32_t osSetCurrentTid(uint32_t tid)
+{
+ struct Task *task = osTaskByIdx(TID_TO_TASK_IDX(tid));
+
+ if (task && task->tid == tid) {
+ struct Task *preempted = osSetCurrentTask(task);
+ return preempted->tid;
+ }
+
+ return osGetCurrentTid();
+}
+
+static inline struct Task *osTaskListPeekHead(struct TaskList *listHead)
+{
+ TaskIndex idx = listHead->next;
+ return idx == NO_NODE ? NULL : &mTaskPool.data[idx];
+}
+
+#ifdef DEBUG
+static void dumpListItems(const char *p, struct TaskList *listHead)
+{
+ int i = 0;
+ struct Task *task;
+
+ osLog(LOG_ERROR, "List: %s (%p) [%u;%u]\n",
+ p,
+ listHead,
+ listHead ? listHead->prev : NO_NODE,
+ listHead ? listHead->next : NO_NODE
+ );
+ if (!listHead)
+ return;
+
+ for_each_task(listHead, task) {
+ osLog(LOG_ERROR, " item %d: task=%p TID=%04X [%u;%u;%u]\n",
+ i,
+ task,
+ task->tid,
+ task->list.prev,
+ osTaskIndex(task),
+ task->list.next
+ );
+ ++i;
+ }
+}
+
+static void dumpTaskList(const char *f, struct Task *task, struct TaskList *listHead)
+{
+ osLog(LOG_ERROR, "%s: pool: %p; task=%p [%u;%u;%u]; listHead=%p [%u;%u]\n",
+ f,
+ &mTaskPool,
+ task,
+ task ? task->list.prev : NO_NODE,
+ osTaskIndex(task),
+ task ? task->list.next : NO_NODE,
+ listHead,
+ listHead ? listHead->prev : NO_NODE,
+ listHead ? listHead->next : NO_NODE
+ );
+ dumpListItems("Tasks", &mTasks);
+ dumpListItems("Free Tasks", &mFreeTasks);
+}
+#else
+#define dumpTaskList(a,b,c)
+#endif
+
+static inline void osTaskListRemoveTask(struct TaskList *listHead, struct Task *task)
+{
+ if (task && listHead) {
+ struct TaskList *cur = &task->list;
+ TaskIndex left_idx = cur->prev;
+ TaskIndex right_idx = cur->next;
+ struct TaskList *left = left_idx == NO_NODE ? listHead : &mTaskPool.data[left_idx].list;
+ struct TaskList *right = right_idx == NO_NODE ? listHead : &mTaskPool.data[right_idx].list;
+ cur->prev = cur->next = NO_NODE;
+ left->next = right_idx;
+ right->prev = left_idx;
+ } else {
+ dumpTaskList(__func__, task, listHead);
+ }
+}
+
+static inline void osTaskListAddTail(struct TaskList *listHead, struct Task *task)
+{
+ if (task && listHead) {
+ struct TaskList *cur = &task->list;
+ TaskIndex last_idx = listHead->prev;
+ TaskIndex new_idx = osTaskIndex(task);
+ struct TaskList *last = last_idx == NO_NODE ? listHead : &mTaskPool.data[last_idx].list;
+ cur->prev = last_idx;
+ cur->next = NO_NODE;
+ last->next = new_idx;
+ listHead->prev = new_idx;
+ } else {
+ dumpTaskList(__func__, task, listHead);
+ }
+}
+
+static struct Task *osAllocTask()
+{
+ struct Task *task = osTaskListPeekHead(&mFreeTasks);
+
+ if (task) {
+ osTaskListRemoveTask(&mFreeTasks, task);
+ uint16_t tid = task->tid;
+ memset(task, 0, sizeof(*task));
+ task->tid = tid;
+ }
+
+ return task;
+}
+
+static void osFreeTask(struct Task *task)
+{
+ if (task) {
+ task->flags = 0;
+ task->ioCount = 0;
+ osTaskListAddTail(&mFreeTasks, task);
+ }
+}
+
+static void osRemoveTask(struct Task *task)
+{
+ osTaskListRemoveTask(&mTasks, task);
+}
+
+static void osAddTask(struct Task *task)
+{
+ osTaskListAddTail(&mTasks, task);
+}
+
+static inline struct Task* osTaskFindByTid(uint32_t tid)
+{
+ TaskIndex idx = TID_TO_TASK_IDX(tid);
+
+ return idx < MAX_TASKS ? &mTaskPool.data[idx] : NULL;
+}
+
+static inline bool osTaskInit(struct Task *task)
+{
+ struct Task *preempted = osSetCurrentTask(task);
+ bool done = cpuAppInit(task->app, &task->platInfo, task->tid);
+ osSetCurrentTask(preempted);
+ return done;
+}
+
+static inline void osTaskEnd(struct Task *task)
+{
+ struct Task *preempted = osSetCurrentTask(task);
+ uint16_t tid = task->tid;
+
+ cpuAppEnd(task->app, &task->platInfo);
+
+ // task was supposed to release it's resources,
+ // but we do our cleanup anyway
+ osSetCurrentTask(mSystemTask);
+ platFreeResources(tid); // HW resources cleanup (IRQ, DMA etc)
+ sensorUnregisterAll(tid);
+ timTimerCancelAll(tid);
+ heapFreeAll(tid);
+ // NOTE: we don't need to unsubscribe from events
+ osSetCurrentTask(preempted);
+}
+
+static inline void osTaskHandle(struct Task *task, uint32_t evtType, const void* evtData)
+{
+ struct Task *preempted = osSetCurrentTask(task);
+ cpuAppHandle(task->app, &task->platInfo, evtType, evtData);
+ osSetCurrentTask(preempted);
}
static void handleEventFreeing(uint32_t evtType, void *evtData, uintptr_t evtFreeData) // watch out, this is synchronous
@@ -121,7 +386,7 @@
if (!task)
osLog(LOG_ERROR, "EINCEPTION: Failed to find app to call app to free event sent to app(s).\n");
else
- cpuAppHandle(task->appHdr, &task->platInfo, EVT_APP_FREE_EVT_DATA, &fd);
+ osTaskHandle(task, EVT_APP_FREE_EVT_DATA, &fd);
}
}
@@ -148,135 +413,518 @@
static struct Task* osTaskFindByAppID(uint64_t appID)
{
- uint32_t i;
+ struct Task *task;
- for (i = 0; i < MAX_TASKS; i++)
- if (mTasks[i].appHdr && mTasks[i].appHdr->appId == appID)
- return mTasks + i;
+ for_each_task(&mTasks, task) {
+ if (task->app && task->app->hdr.appId == appID)
+ return task;
+ }
return NULL;
}
-static uint32_t osGetFreeTid(void)
+void osSegmentIteratorInit(struct SegmentIterator *it)
{
- do {
- if (mNextTidInfo == LAST_VALID_TID)
- mNextTidInfo = FIRST_VALID_TID;
- else
- mNextTidInfo++;
- } while (osTaskFindByTid(mNextTidInfo));
+ uint32_t sz;
+ uint8_t *start = platGetSharedAreaInfo(&sz);
- return mNextTidInfo;
+ it->shared = (const struct Segment *)(start);
+ it->sharedEnd = (const struct Segment *)(start + sz);
+ it->seg = NULL;
+}
+
+bool osAppSegmentSetState(const struct AppHdr *app, uint32_t segState)
+{
+ bool done;
+ struct Segment *seg = osGetSegment(app);
+ uint8_t state = segState;
+
+ if (!seg)
+ return false;
+
+ mpuAllowRamExecution(true);
+ mpuAllowRomWrite(true);
+ done = BL.blProgramShared(&seg->state, &state, sizeof(state), BL_FLASH_KEY1, BL_FLASH_KEY2);
+ mpuAllowRomWrite(false);
+ mpuAllowRamExecution(false);
+
+ return done;
+}
+
+bool osSegmentSetSize(struct Segment *seg, uint32_t size)
+{
+ bool ret = true;
+
+ if (!seg)
+ return false;
+
+ if (size > SEG_SIZE_MAX) {
+ seg->state = SEG_ST_ERASED;
+ size = SEG_SIZE_MAX;
+ ret = false;
+ }
+ seg->size[0] = size;
+ seg->size[1] = size >> 8;
+ seg->size[2] = size >> 16;
+
+ return ret;
+}
+
+struct Segment *osSegmentGetEnd()
+{
+ uint32_t size;
+ uint8_t *start = platGetSharedAreaInfo(&size);
+ return (struct Segment *)(start + size);
+}
+
+struct Segment *osGetSegment(const struct AppHdr *app)
+{
+ uint32_t size;
+ uint8_t *start = platGetSharedAreaInfo(&size);
+
+ return (struct Segment *)((uint8_t*)app &&
+ (uint8_t*)app >= start &&
+ (uint8_t*)app < (start + size) ?
+ (uint8_t*)app - sizeof(struct Segment) : NULL);
+}
+
+bool osEraseShared()
+{
+ mpuAllowRamExecution(true);
+ mpuAllowRomWrite(true);
+ (void)BL.blEraseShared(BL_FLASH_KEY1, BL_FLASH_KEY2);
+ mpuAllowRomWrite(false);
+ mpuAllowRamExecution(false);
+ return true;
+}
+
+bool osWriteShared(void *dest, const void *src, uint32_t len)
+{
+ bool ret;
+
+ mpuAllowRamExecution(true);
+ mpuAllowRomWrite(true);
+ ret = BL.blProgramShared(dest, src, len, BL_FLASH_KEY1, BL_FLASH_KEY2);
+ mpuAllowRomWrite(false);
+ mpuAllowRamExecution(false);
+
+ if (!ret)
+ osLog(LOG_ERROR, "osWriteShared: blProgramShared return false\n");
+
+ return ret;
+}
+
+struct AppHdr *osAppSegmentCreate(uint32_t size)
+{
+ struct SegmentIterator it;
+ const struct Segment *storageSeg = NULL;
+ struct AppHdr *app;
+
+ osSegmentIteratorInit(&it);
+ while (osSegmentIteratorNext(&it)) {
+ if (osSegmentGetState(it.seg) == SEG_ST_EMPTY) {
+ storageSeg = it.seg;
+ break;
+ }
+ }
+ if (!storageSeg || osSegmentSizeGetNext(storageSeg, size) > it.sharedEnd)
+ return NULL;
+
+ app = osSegmentGetData(storageSeg);
+ osAppSegmentSetState(app, SEG_ST_RESERVED);
+
+ return app;
+}
+
+bool osAppSegmentClose(struct AppHdr *app, uint32_t segDataSize, uint32_t segState)
+{
+ struct Segment seg;
+
+ // this is enough for holding padding to uint32_t and the footer
+ uint8_t footer[sizeof(uint32_t) + FOOTER_SIZE];
+ int footerLen;
+ bool ret;
+ uint32_t totalSize;
+ uint8_t *start = platGetSharedAreaInfo(&totalSize);
+ uint8_t *end = start + totalSize;
+ int32_t fullSize = segDataSize + sizeof(seg); // without footer or padding
+ struct Segment *storageSeg = osGetSegment(app);
+
+ // sanity check
+ if (segDataSize >= SEG_SIZE_MAX)
+ return false;
+
+ // physical limits check
+ if (osSegmentSizeAlignedWithFooter(segDataSize) + sizeof(struct Segment) > totalSize)
+ return false;
+
+ // available space check: we could truncate size, instead of disallowing it,
+ // but we know that we performed validation on the size before, in *Create call,
+ // and it was fine, so this must be a programming error, and so we fail.
+ // on a side note: size may grow or shrink compared to original estimate.
+ // typically it shrinks, since we skip some header info and padding, as well
+ // as signature blocks, but it is possible that at some point we may produce
+ // more data for some reason. At that time the logic here may need to change
+ if (osSegmentSizeGetNext(storageSeg, segDataSize) > (struct Segment*)end)
+ return false;
+
+ seg.state = segState;
+ osSegmentSetSize(&seg, segDataSize);
+
+ ret = osWriteShared((uint8_t*)storageSeg, (uint8_t*)&seg, sizeof(seg));
+
+ footerLen = (-fullSize) & 3;
+ memset(footer, 0x00, footerLen);
+
+#ifdef SEGMENT_CRC_SUPPORT
+ struct SegmentFooter segFooter {
+ .crc = ~crc32(storageSeg, fullSize, ~0),
+ };
+ memcpy(&footer[footerLen], &segFooter, sizeof(segFooter));
+ footerLen += sizeof(segFooter);
+#endif
+
+ if (ret && footerLen)
+ ret = osWriteShared((uint8_t*)storageSeg + fullSize, footer, footerLen);
+
+ return ret;
+}
+
+bool osAppWipeData(struct AppHdr *app)
+{
+ struct Segment *seg = osGetSegment(app);
+ int32_t size = osSegmentGetSize(seg);
+ uint8_t *p = (uint8_t*)app;
+ uint32_t state = osSegmentGetState(seg);
+ uint8_t buf[256];
+ bool done = true;
+
+ if (!seg || size == SEG_SIZE_INVALID || state == SEG_ST_EMPTY) {
+ osLog(LOG_ERROR, "%s: can't erase segment: app=%p; seg=%p"
+ "; size=%" PRIu32
+ "; state=%" PRIu32
+ "\n",
+ __func__, app, seg, size, state);
+ return false;
+ }
+
+ size = osSegmentSizeAlignedWithFooter(size);
+
+ memset(buf, 0, sizeof(buf));
+ while (size > 0) {
+ uint32_t flashSz = size > sizeof(buf) ? sizeof(buf) : size;
+ // keep trying to zero-out stuff even in case of intermittent failures.
+ // flash write may occasionally fail on some byte, but it is not good enough
+ // reason to not rewrite other bytes
+ bool res = osWriteShared(p, buf, flashSz);
+ done = done && res;
+ size -= flashSz;
+ p += flashSz;
+ }
+
+ return done;
+}
+
+static inline bool osAppIsValid(const struct AppHdr *app)
+{
+ return app->hdr.magic == APP_HDR_MAGIC &&
+ app->hdr.fwVer == APP_HDR_VER_CUR &&
+ (app->hdr.fwFlags & FL_APP_HDR_APPLICATION) != 0 &&
+ app->hdr.payInfoType == LAYOUT_APP;
+}
+
+static bool osExtAppIsValid(const struct AppHdr *app, uint32_t len)
+{
+ //TODO: when CRC support is ready, add CRC check here
+ return osAppIsValid(app) &&
+ len >= sizeof(*app) &&
+ osAppSegmentGetState(app) == SEG_ST_VALID &&
+ !(app->hdr.fwFlags & FL_APP_HDR_INTERNAL);
+}
+
+static bool osIntAppIsValid(const struct AppHdr *app)
+{
+ return osAppIsValid(app) &&
+ osAppSegmentGetState(app) == SEG_STATE_INVALID &&
+ (app->hdr.fwFlags & FL_APP_HDR_INTERNAL) != 0;
+}
+
+static inline bool osExtAppErase(const struct AppHdr *app)
+{
+ return osAppSegmentSetState(app, SEG_ST_ERASED);
+}
+
+static struct Task *osLoadApp(const struct AppHdr *app) {
+ struct Task *task;
+
+ task = osAllocTask();
+ if (!task) {
+ osLog(LOG_WARN, "External app id %016" PRIX64 " @ %p cannot be used as too many apps already exist.\n", app->hdr.appId, app);
+ return NULL;
+ }
+ task->app = app;
+ bool done = (app->hdr.fwFlags & FL_APP_HDR_INTERNAL) ?
+ cpuInternalAppLoad(task->app, &task->platInfo) :
+ cpuAppLoad(task->app, &task->platInfo);
+
+ if (!done) {
+ osLog(LOG_WARN, "App @ %p ID %016" PRIX64 " failed to load\n", app, app->hdr.appId);
+ osFreeTask(task);
+ task = NULL;
+ }
+
+ return task;
+}
+
+static void osUnloadApp(struct Task *task)
+{
+ // this is called on task that has stopped running, or had never run
+ cpuAppUnload(task->app, &task->platInfo);
+ osFreeTask(task);
+}
+
+static bool osStartApp(const struct AppHdr *app)
+{
+ bool done = false;
+ struct Task *task;
+
+ if ((task = osLoadApp(app)) != NULL) {
+ task->subbedEvtListSz = MAX_EMBEDDED_EVT_SUBS;
+ task->subbedEvents = task->subbedEventsInt;
+ MAKE_NEW_TID(task);
+
+ done = osTaskInit(task);
+
+ if (!done) {
+ osLog(LOG_WARN, "App @ %p ID %016" PRIX64 "failed to init\n", task->app, task->app->hdr.appId);
+ osUnloadApp(task);
+ } else {
+ osAddTask(task);
+ }
+ }
+
+ return done;
+}
+
+static bool osStopTask(struct Task *task)
+{
+ if (!task)
+ return false;
+
+ osTaskClrSetFlags(task, 0, FL_TASK_STOPPED);
+ osRemoveTask(task);
+
+ if (osTaskGetIoCount(task)) {
+ osTaskHandle(task, EVT_APP_STOP, NULL);
+ osEnqueueEvtOrFree(EVT_APP_END, task, NULL);
+ } else {
+ osTaskEnd(task);
+ osUnloadApp(task);
+ }
+
+ return true;
+}
+
+static bool osExtAppFind(struct SegmentIterator *it, uint64_t appId)
+{
+ uint64_t vendor = APP_ID_GET_VENDOR(appId);
+ uint64_t seqId = APP_ID_GET_SEQ_ID(appId);
+ uint64_t curAppId;
+ const struct AppHdr *app;
+ const struct Segment *seg;
+
+ while (osSegmentIteratorNext(it)) {
+ seg = it->seg;
+ if (seg->state == SEG_ST_EMPTY)
+ break;
+ if (seg->state != SEG_ST_VALID)
+ continue;
+ app = osSegmentGetData(seg);
+ curAppId = app->hdr.appId;
+
+ if ((vendor == APP_VENDOR_ANY || vendor == APP_ID_GET_VENDOR(curAppId)) &&
+ (seqId == APP_SEQ_ID_ANY || seqId == APP_ID_GET_SEQ_ID(curAppId)))
+ return true;
+ }
+
+ return false;
+}
+
+static uint32_t osExtAppStopEraseApps(uint64_t appId, bool doErase)
+{
+ const struct AppHdr *app;
+ int32_t len;
+ struct Task *task;
+ struct SegmentIterator it;
+ uint32_t stopCount = 0;
+ uint32_t eraseCount = 0;
+ uint32_t appCount = 0;
+ uint32_t taskCount = 0;
+ struct MgmtStatus stat = { .value = 0 };
+
+ osSegmentIteratorInit(&it);
+ while (osExtAppFind(&it, appId)) {
+ app = osSegmentGetData(it.seg);
+ len = osSegmentGetSize(it.seg);
+ if (!osExtAppIsValid(app, len))
+ continue;
+ appCount++;
+ task = osTaskFindByAppID(app->hdr.appId);
+ if (task)
+ taskCount++;
+ if (task && task->app == app) {
+ if (osStopTask(task))
+ stopCount++;
+ else
+ continue;
+ if (doErase && osExtAppErase(app))
+ eraseCount++;
+ }
+ }
+ SET_COUNTER(stat.app, appCount);
+ SET_COUNTER(stat.task, taskCount);
+ SET_COUNTER(stat.op, stopCount);
+ SET_COUNTER(stat.erase, eraseCount);
+
+ return stat.value;
+}
+
+uint32_t osExtAppStopApps(uint64_t appId)
+{
+ return osExtAppStopEraseApps(appId, false);
+}
+
+uint32_t osExtAppEraseApps(uint64_t appId)
+{
+ return osExtAppStopEraseApps(appId, true);
+}
+
+static void osScanExternal()
+{
+ struct SegmentIterator it;
+ osSegmentIteratorInit(&it);
+ while (osSegmentIteratorNext(&it)) {
+ switch (osSegmentGetState(it.seg)) {
+ case SEG_ST_EMPTY:
+ // everything looks good
+ osLog(LOG_INFO, "External area is good\n");
+ return;
+ case SEG_ST_ERASED:
+ case SEG_ST_VALID:
+ // this is valid stuff, ignore
+ break;
+ case SEG_ST_RESERVED:
+ default:
+ // something is wrong: erase everything
+ osLog(LOG_ERROR, "External area is damaged. Erasing\n");
+ osEraseShared();
+ return;
+ }
+ }
+}
+
+uint32_t osExtAppStartApps(uint64_t appId)
+{
+ const struct AppHdr *app;
+ int32_t len;
+ struct SegmentIterator it;
+ struct SegmentIterator checkIt;
+ uint32_t startCount = 0;
+ uint32_t eraseCount = 0;
+ uint32_t appCount = 0;
+ uint32_t taskCount = 0;
+ struct MgmtStatus stat = { .value = 0 };
+
+ osScanExternal();
+
+ osSegmentIteratorInit(&it);
+ while (osExtAppFind(&it, appId)) {
+ app = osSegmentGetData(it.seg);
+ len = osSegmentGetSize(it.seg);
+
+ // skip erased or malformed apps
+ if (!osExtAppIsValid(app, len))
+ continue;
+
+ appCount++;
+ checkIt = it;
+ // find the most recent copy
+ while (osExtAppFind(&checkIt, app->hdr.appId)) {
+ if (osExtAppErase(app)) // erase the old one, so we skip it next time
+ eraseCount++;
+ app = osSegmentGetData(checkIt.seg);
+ }
+
+ if (osTaskFindByAppID(app->hdr.appId)) {
+ // this either the most recent external app with the same ID,
+ // or internal app with the same id; in both cases we do nothing
+ taskCount++;
+ continue;
+ }
+
+ if (osStartApp(app))
+ startCount++;
+ }
+ SET_COUNTER(stat.app, appCount);
+ SET_COUNTER(stat.task, taskCount);
+ SET_COUNTER(stat.op, startCount);
+ SET_COUNTER(stat.erase, eraseCount);
+
+ return stat.value;
}
static void osStartTasks(void)
{
- static const char magic[] = APP_HDR_MAGIC;
const struct AppHdr *app;
- uint32_t i, nTasks = 0, nApps, sharedSz;
+ uint32_t i, nApps;
struct Task* task;
- const uint8_t *shared, *sharedEnd;
- int len, total_len;
- uint8_t id1, id2;
+ uint32_t status = 0;
+ uint32_t taskCnt = 0;
+
+ osLog(LOG_DEBUG, "Initializing task pool...\n");
+ list_init(&mTasks);
+ list_init(&mFreeTasks);
+ for (i = 0; i < MAX_TASKS; ++i) {
+ task = &mTaskPool.data[i];
+ list_init(&task->list);
+ osFreeTask(task);
+ }
+
+ mSystemTask = osAllocTask(); // this is a dummy task; holder of TID 0; all system code will run with TID 0
+ osSetCurrentTask(mSystemTask);
+ osLog(LOG_DEBUG, "System task is: %p\n", mSystemTask);
/* first enum all internal apps, making sure to check for dupes */
- osLog(LOG_DEBUG, "Reading internal app list...\n");
- for (i = 0, app = platGetInternalAppList(&nApps); i < nApps && nTasks < MAX_TASKS && app->fmtVer == APP_HDR_VER_CUR; i++, app++) {
-
- if (app->marker != APP_HDR_MARKER_INTERNAL) {
- osLog(LOG_WARN, "Weird marker on internal app: [%p]=0x%04X\n", app, app->marker);
+ osLog(LOG_DEBUG, "Starting internal apps...\n");
+ for (i = 0, app = platGetInternalAppList(&nApps); i < nApps; i++, app++) {
+ if (!osIntAppIsValid(app)) {
+ osLog(LOG_WARN, "Invalid internal app @ %p ID %016" PRIX64
+ "header version: %" PRIu16
+ "\n",
+ app, app->hdr.appId, app->hdr.fwVer);
continue;
}
- if ((task = osTaskFindByAppID(app->appId))) {
- osLog(LOG_WARN, "Internal app id %016" PRIX64 "@ %p attempting to update internal app @ %p. Ignored.\n", app->appId, app, task->appHdr);
+
+ if (!(app->hdr.fwFlags & FL_APP_HDR_INTERNAL)) {
+ osLog(LOG_WARN, "Internal app is not marked: [%p]: flags: 0x%04" PRIX16
+ "; ID: %016" PRIX64
+ "; ignored\n",
+ app, app->hdr.fwFlags, app->hdr.appId);
continue;
}
- mTasks[nTasks++].appHdr = app;
- }
-
- /* then enum all external apps, making sure to find the latest (by position in flash) and checking for conflicts with internal apps */
- osLog(LOG_DEBUG, "Reading external app list...\n");
- for (shared = platGetSharedAreaInfo(&sharedSz), sharedEnd = shared + sharedSz; shared < sharedEnd && shared[0] != 0xFF; shared += total_len) {
- id1 = shared[0] & 0x0F;
- id2 = (shared[0] >> 4) & 0x0F;
- len = (shared[1] << 16) | (shared[2] << 8) | shared[3];
- total_len = sizeof(uint32_t) + ((len + 3) & ~3) + sizeof(uint32_t);
-
- if (shared + total_len > sharedEnd)
- break;
-
- //skip over erased sections
- if (id1 != id2 || id1 != BL_FLASH_APP_ID)
+ if ((task = osTaskFindByAppID(app->hdr.appId))) {
+ osLog(LOG_WARN, "Internal app ID %016" PRIX64
+ "@ %p attempting to update internal app @ %p; app @%p ignored.\n",
+ app->hdr.appId, app, task->app, app);
continue;
-
- if (1 /*crc32(shared, total_len, ~0) == CRC_RESIDUE*/) {
- app = (const struct AppHdr *)&shared[4];
- if (len >= sizeof(struct AppHdr) && !memcmp(magic, app->magic, sizeof(magic) - 1) && app->fmtVer == APP_HDR_VER_CUR) {
-
- if (app->marker != APP_HDR_MARKER_VALID) //this may need more logic to handle partially-uploaded things
- osLog(LOG_WARN, "Weird marker on external app: [%p]=0x%04X\n", app, app->marker);
- else if ((task = osTaskFindByAppID(app->appId))) {
- if (task->appHdr->marker == APP_HDR_MARKER_INTERNAL)
- osLog(LOG_WARN, "External app id %016" PRIX64 " @ %p attempting to update internal app @ %p. This is not allowed.\n", app->appId, app, task->appHdr);
- else {
- osLog(LOG_DEBUG, "External app id %016" PRIX64 " @ %p updating app @ %p\n", app->appId, app, task->appHdr);
- task->appHdr = app;
- }
- }
- else if (nTasks == MAX_TASKS)
- osLog(LOG_WARN, "External app id %016" PRIX64 " @ %p cannot be used as too many apps already exist.\n", app->appId, app);
- else
- mTasks[nTasks++].appHdr = app;
- }
}
+ if (osStartApp(app))
+ taskCnt++;
}
- osLog(LOG_DEBUG, "Enumerated %" PRIu32 " apps\n", nTasks);
-
- /* Now that we have pointers to all the latest app headers, let's try loading then. */
- /* Note that if a new version fails to init we will NOT try the old (no reason to assume this is safe) */
- osLog(LOG_DEBUG, "Loading apps...\n");
- for (i = 0; i < nTasks;) {
-
- if (mTasks[i].appHdr->marker == APP_HDR_MARKER_INTERNAL) {
- if (cpuInternalAppLoad(mTasks[i].appHdr, &mTasks[i].platInfo)) {
- i++;
- continue;
- }
- }
- else {
- if (cpuAppLoad(mTasks[i].appHdr, &mTasks[i].platInfo)) {
- i++;
- continue;
- }
- }
-
- //if we're here, an app failed to load - remove it from the list
- osLog(LOG_WARN, "App @ %p failed to load\n", mTasks[i].appHdr);
- memcpy(mTasks + i, mTasks + --nTasks, sizeof(struct Task));
- }
-
- osLog(LOG_DEBUG, "Loaded %" PRIu32 " apps\n", nTasks);
-
- /* now finish initing structs, assign tids, call init funcs */
- osLog(LOG_DEBUG, "Starting apps...\n");
- for (i = 0; i < nTasks;) {
-
- mTasks[i].subbedEvtListSz = MAX_EMBEDDED_EVT_SUBS;
- mTasks[i].subbedEvents = mTasks[i].subbedEventsInt;
- mTasks[i].tid = osGetFreeTid();
-
- if (cpuAppInit(mTasks[i].appHdr, &mTasks[i].platInfo, mTasks[i].tid))
- i++;
- else {
- //if we're here, an app failed to init - unload & remove it from the list
- osLog(LOG_WARN, "App @ %p failed to init\n", mTasks[i].appHdr);
- cpuAppUnload(mTasks[i].appHdr, &mTasks[i].platInfo);
- memcpy(mTasks + i, mTasks + --nTasks, sizeof(struct Task));
- }
- }
-
- osLog(LOG_DEBUG, "Started %" PRIu32 " apps\n", nTasks);
+ osLog(LOG_DEBUG, "Starting external apps...\n");
+ status = osExtAppStartApps(APP_ID_ANY);
+ osLog(LOG_DEBUG, "Started %" PRIu32 " internal apps; EXT status: %08" PRIX32 "\n", taskCnt, status);
}
static void osInternalEvtHandle(uint32_t evtType, void *evtData)
@@ -318,6 +966,12 @@
}
break;
+ case EVT_APP_END:
+ task = evtData;
+ osTaskEnd(task);
+ osUnloadApp(task);
+ break;
+
case EVT_DEFERRED_CALLBACK:
da->deferred.callback(da->deferred.cookie);
break;
@@ -329,7 +983,7 @@
TaggedPtr *tmp = mCurEvtEventFreeingInfo;
mCurEvtEventFreeingInfo = NULL;
- cpuAppHandle(task->appHdr, &task->platInfo, da->privateEvt.evtType, da->privateEvt.evtData);
+ osTaskHandle(task, da->privateEvt.evtType, da->privateEvt.evtData);
mCurEvtEventFreeingInfo = tmp;
}
@@ -381,28 +1035,33 @@
void osMainDequeueLoop(void)
{
TaggedPtr evtFreeingInfo;
- uint32_t evtType, i, j;
+ uint32_t evtType, j;
void *evtData;
+ struct Task *task;
+ uint16_t tid;
/* get an event */
if (!evtQueueDequeue(mEvtsInternal, &evtType, &evtData, &evtFreeingInfo, true))
return;
+ evtType = EVENT_GET_EVENT(evtType);
+ tid = EVENT_GET_ORIGIN(evtType);
+ task = osTaskFindByTid(tid);
+ if (task)
+ osTaskAddIoCount(task, -1);
+
/* by default we free them when we're done with them */
mCurEvtEventFreeingInfo = &evtFreeingInfo;
- if (evtType < EVT_NO_FIRST_USER_EVENT) { /* no need for discardable check. all internal events arent discardable */
+ if (evtType < EVT_NO_FIRST_USER_EVENT) {
/* handle deferred actions and other reserved events here */
osInternalEvtHandle(evtType, evtData);
- }
- else {
+ } else {
/* send this event to all tasks who want it */
- for (i = 0; i < MAX_TASKS; i++) {
- if (!mTasks[i].subbedEvents) /* only check real tasks */
- continue;
- for (j = 0; j < mTasks[i].subbedEvtCount; j++) {
- if (mTasks[i].subbedEvents[j] == (evtType & ~EVENT_TYPE_BIT_DISCARDABLE)) {
- cpuAppHandle(mTasks[i].appHdr, &mTasks[i].platInfo, evtType & ~EVENT_TYPE_BIT_DISCARDABLE, evtData);
+ for_each_task(&mTasks, task) {
+ for (j = 0; j < task->subbedEvtCount; j++) {
+ if (task->subbedEvents[j] == evtType) {
+ osTaskHandle(task, evtType, evtData);
break;
}
}
@@ -446,17 +1105,38 @@
bool osEventSubscribe(uint32_t tid, uint32_t evtType)
{
- return osEventSubscribeUnsubscribe(tid, evtType, true);
+ (void)tid;
+ return osEventSubscribeUnsubscribe(osGetCurrentTid(), evtType, true);
}
bool osEventUnsubscribe(uint32_t tid, uint32_t evtType)
{
- return osEventSubscribeUnsubscribe(tid, evtType, false);
+ (void)tid;
+ return osEventSubscribeUnsubscribe(osGetCurrentTid(), evtType, false);
+}
+
+static bool osEnqueueEvtCommon(uint32_t evtType, void *evtData, TaggedPtr evtFreeInfo)
+{
+ struct Task *task = osGetCurrentTask();
+
+ if (osTaskTestFlags(task, FL_TASK_STOPPED)) {
+ handleEventFreeing(evtType, evtData, evtFreeInfo);
+ return true;
+ }
+
+ evtType = EVENT_WITH_ORIGIN(evtType, osGetCurrentTid());
+ osTaskAddIoCount(task, 1);
+
+ if (evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, false))
+ return true;
+
+ osTaskAddIoCount(task, -1);
+ return false;
}
bool osEnqueueEvt(uint32_t evtType, void *evtData, EventFreeF evtFreeF)
{
- return evtQueueEnqueue(mEvtsInternal, evtType, evtData, taggedPtrMakeFromPtr(evtFreeF), false);
+ return osEnqueueEvtCommon(evtType, evtData, taggedPtrMakeFromPtr(evtFreeF));
}
bool osEnqueueEvtOrFree(uint32_t evtType, void *evtData, EventFreeF evtFreeF)
@@ -471,7 +1151,12 @@
bool osEnqueueEvtAsApp(uint32_t evtType, void *evtData, uint32_t fromAppTid)
{
- return evtQueueEnqueue(mEvtsInternal, evtType, evtData, taggedPtrMakeFromUint(fromAppTid), false);
+ // compatibility with existing external apps
+ if (evtType & EVENT_TYPE_BIT_DISCARDABLE_COMPAT)
+ evtType |= EVENT_TYPE_BIT_DISCARDABLE;
+
+ (void)fromAppTid;
+ return osEnqueueEvtCommon(evtType, evtData, taggedPtrMakeFromUint(osGetCurrentTid()));
}
bool osDefer(OsDeferCbkF callback, void *cookie, bool urgent)
@@ -511,16 +1196,17 @@
bool osEnqueuePrivateEvtAsApp(uint32_t evtType, void *evtData, uint32_t fromAppTid, uint32_t toTid)
{
- return osEnqueuePrivateEvtEx(evtType, evtData, taggedPtrMakeFromUint(fromAppTid), toTid);
+ (void)fromAppTid;
+ return osEnqueuePrivateEvtEx(evtType, evtData, taggedPtrMakeFromUint(osGetCurrentTid()), toTid);
}
bool osTidById(uint64_t appId, uint32_t *tid)
{
- uint32_t i;
+ struct Task *task;
- for (i = 0; i < MAX_TASKS; i++) {
- if (mTasks[i].appHdr && mTasks[i].appHdr->appId == appId) {
- *tid = mTasks[i].tid;
+ for_each_task(&mTasks, task) {
+ if (task->app && task->app->hdr.appId == appId) {
+ *tid = task->tid;
return true;
}
}
@@ -530,15 +1216,18 @@
bool osAppInfoById(uint64_t appId, uint32_t *appIdx, uint32_t *appVer, uint32_t *appSize)
{
- uint32_t i;
+ uint32_t i = 0;
+ struct Task *task;
- for (i = 0; i < MAX_TASKS; i++) {
- if (mTasks[i].appHdr && mTasks[i].appHdr->appId == appId) {
+ for_each_task(&mTasks, task) {
+ const struct AppHdr *app = task->app;
+ if (app && app->hdr.appId == appId) {
*appIdx = i;
- *appVer = mTasks[i].appHdr->appVer;
- *appSize = mTasks[i].appHdr->rel_end;
+ *appVer = app->hdr.appVer;
+ *appSize = app->sect.rel_end;
return true;
}
+ i++;
}
return false;
@@ -546,11 +1235,19 @@
bool osAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize)
{
- if (appIdx < MAX_TASKS && mTasks[appIdx].appHdr) {
- *appId = mTasks[appIdx].appHdr->appId;
- *appVer = mTasks[appIdx].appHdr->appVer;
- *appSize = mTasks[appIdx].appHdr->rel_end;
- return true;
+ struct Task *task;
+ int i = 0;
+
+ for_each_task(&mTasks, task) {
+ if (i != appIdx) {
+ ++i;
+ } else {
+ const struct AppHdr *app = task->app;
+ *appId = app->hdr.appId;
+ *appVer = app->hdr.appVer;
+ *appSize = app->sect.rel_end;
+ return true;
+ }
}
return false;
@@ -622,10 +1319,3 @@
};
#endif
-
-
-PREPOPULATED_ENCR_KEY(google_encr_key, ENCR_KEY_GOOGLE_PREPOPULATED, 0xf1, 0x51, 0x9b, 0x2e, 0x26, 0x6c, 0xeb, 0xe7, 0xd6, 0xd6, 0x0d, 0x17, 0x11, 0x94, 0x99, 0x19, 0x1c, 0xfb, 0x71, 0x56, 0x53, 0xf7, 0xe0, 0x7d, 0x90, 0x07, 0x53, 0x68, 0x10, 0x95, 0x1b, 0x70);
-
-
-
-
diff --git a/firmware/src/timer.c b/firmware/src/timer.c
index a84b101..c623f02 100644
--- a/firmware/src/timer.c
+++ b/firmware/src/timer.c
@@ -32,7 +32,8 @@
struct Timer {
uint64_t expires; /* time of next expiration */
uint64_t period; /* 0 for oneshot */
- uint32_t id; /* 0 for disabled */
+ uint16_t id; /* 0 for disabled */
+ uint16_t tid; /* we need TID always, for system management */
uint32_t jitterPpm;
uint32_t driftPpm;
TaggedPtr callInfo;
@@ -67,22 +68,22 @@
slabAllocatorFree(mInternalEvents, event);
}
-static void timCallFunc(TaggedPtr callInfo, uint32_t id, void *callData)
+static void timCallFunc(struct Timer *tim)
{
struct TimerEvent *evt;
+ TaggedPtr callInfo = tim->callInfo;
if (taggedPtrIsPtr(callInfo)) {
- ((TimTimerCbkF)taggedPtrToPtr(callInfo))(id, callData);
+ osSetCurrentTid(tim->tid);
+ ((TimTimerCbkF)taggedPtrToPtr(callInfo))(tim->id, tim->callData);
} else {
- if (!(evt = slabAllocatorAlloc(mInternalEvents)))
- return;
-
- evt->timerId = id;
- evt->data = callData;
- if (osEnqueuePrivateEvt(EVT_APP_TIMER, evt, timerCallFuncFreeF, taggedPtrToUint(callInfo)))
- return;
-
- slabAllocatorFree(mInternalEvents, evt);
+ osSetCurrentTid(OS_SYSTEM_TID);
+ if ((evt = slabAllocatorAlloc(mInternalEvents)) != 0) {
+ evt->timerId = tim->id;
+ evt->data = tim->callData;
+ if (!osEnqueuePrivateEvt(EVT_APP_TIMER, evt, timerCallFuncFreeF, tim->tid))
+ slabAllocatorFree(mInternalEvents, evt);
+ }
}
}
@@ -91,43 +92,40 @@
uint32_t maxDrift = 0, maxJitter = 0, maxErrTotal = 0;
bool somethingDone, totalSomethingDone = false;
uint64_t nextTimer;
- TaggedPtr callInfo;
- uint32_t i, id;
- void *callData;
+ uint32_t i;
+ struct Timer *tim;
// protect from concurrent execution [timIntHandler() and timTimerSetEx()]
uint64_t intSta = cpuIntsOff();
+ uint16_t oldTid = osGetCurrentTid();
do {
somethingDone = false;
nextTimer = 0;
- for (i = 0; i < MAX_TIMERS; i++) {
- if (!mTimers[i].id)
+ for (i = 0, tim = &mTimers[0]; i < MAX_TIMERS; i++, tim++) {
+ if (!tim->id)
continue;
- if (mTimers[i].expires <= timGetTime()) {
+ if (tim->expires <= timGetTime()) {
somethingDone = true;
- callInfo = mTimers[i].callInfo;
- callData = mTimers[i].callData;
- id = mTimers[i].id;
- if (mTimers[i].period)
- mTimers[i].expires += mTimers[i].period;
+ if (tim->period)
+ tim->expires += tim->period;
else {
- mTimers[i].id = 0;
+ tim->id = 0;
atomicBitsetClearBit(mTimersValid, i);
}
- timCallFunc(callInfo, id, callData);
+ timCallFunc(tim);
}
else {
- if (mTimers[i].jitterPpm > maxJitter)
- maxJitter = mTimers[i].jitterPpm;
- if (mTimers[i].driftPpm > maxDrift)
- maxDrift = mTimers[i].driftPpm;
- if (mTimers[i].driftPpm + mTimers[i].jitterPpm > maxErrTotal)
- maxErrTotal = mTimers[i].driftPpm + mTimers[i].jitterPpm;
- if (!nextTimer || nextTimer > mTimers[i].expires)
- nextTimer = mTimers[i].expires;
+ if (tim->jitterPpm > maxJitter)
+ maxJitter = tim->jitterPpm;
+ if (tim->driftPpm > maxDrift)
+ maxDrift = tim->driftPpm;
+ if (tim->driftPpm + tim->jitterPpm > maxErrTotal)
+ maxErrTotal = tim->driftPpm + tim->jitterPpm;
+ if (!nextTimer || nextTimer > tim->expires)
+ nextTimer = tim->expires;
}
}
@@ -139,6 +137,7 @@
if (!nextTimer)
platSleepClockRequest(0, 0, 0, 0);
+ osSetCurrentTid(oldTid);
cpuIntsRestore(intSta);
return totalSomethingDone;
@@ -149,14 +148,14 @@
uint64_t curTime = timGetTime();
int32_t idx = atomicBitsetFindClearAndSet(mTimersValid);
struct Timer *t;
- uint32_t timId;
+ uint16_t timId;
if (idx < 0) /* no free timers */
return 0;
/* generate next timer ID */
do {
- timId = atomicAdd(&mNextTimerId, 1);
+ timId = atomicAdd32bits(&mNextTimerId, 1);
} while (!timId || timFindTimerById(timId));
/* grab our struct & fill it in */
@@ -170,6 +169,7 @@
/* as soon as we write timer Id, it becomes valid and might fire */
t->id = timId;
+ t->tid = osGetCurrentTid();
/* fire as needed & recalc alarms*/
timFireAsNeededAndUpdateAlarms();
@@ -185,7 +185,7 @@
uint32_t timTimerSetAsApp(uint64_t length, uint32_t jitterPpm, uint32_t driftPpm, uint32_t tid, void* data, bool oneShot)
{
- return timTimerSetEx(length, jitterPpm, driftPpm, taggedPtrMakeFromUint(tid), data, oneShot);
+ return timTimerSetEx(length, jitterPpm, driftPpm, taggedPtrMakeFromUint(0), data, oneShot);
}
bool timTimerCancel(uint32_t timerId)
@@ -207,6 +207,26 @@
return false;
}
+int timTimerCancelAll(uint32_t tid)
+{
+ uint64_t intState;
+ struct Timer *tim;
+ int i, count;
+
+ tim = &mTimers[0];
+ intState = cpuIntsOff();
+ for (i = 0, count = 0; i < MAX_TIMERS; ++i, ++tim) {
+ if (tim->tid != tid)
+ continue;
+ count++;
+ tim->id = 0; /* this disables it */
+ /* this frees struct */
+ atomicBitsetClearBit(mTimersValid, tim - mTimers);
+ }
+ cpuIntsRestore(intState);
+ return count;
+}
+
bool timIntHandler(void)
{
return timFireAsNeededAndUpdateAlarms();
diff --git a/firmware/inc/aes.h b/lib/include/nanohub/aes.h
similarity index 93%
rename from firmware/inc/aes.h
rename to lib/include/nanohub/aes.h
index 30a0678..37e6038 100644
--- a/firmware/inc/aes.h
+++ b/lib/include/nanohub/aes.h
@@ -14,8 +14,8 @@
* limitations under the License.
*/
-#ifndef _AES_H_
-#define _AES_H_
+#ifndef _NANOHUB_AES_H_
+#define _NANOHUB_AES_H_
#include <stdint.h>
@@ -29,7 +29,7 @@
#define AES_KEY_WORDS 8
#define AES_BLOCK_WORDS 4
-
+#define AES_BLOCK_SIZE 16 // in bytes
//basic AES block ops
void aesInitForEncr(struct AesContext *ctx, const uint32_t *k);
@@ -51,5 +51,5 @@
-#endif
+#endif // _NANOHUB_AES_H_
diff --git a/firmware/inc/cpu/cortexm4f/appRelocFormat.h b/lib/include/nanohub/appRelocFormat.h
similarity index 100%
rename from firmware/inc/cpu/cortexm4f/appRelocFormat.h
rename to lib/include/nanohub/appRelocFormat.h
diff --git a/firmware/inc/crc.h b/lib/include/nanohub/crc.h
similarity index 92%
rename from firmware/inc/crc.h
rename to lib/include/nanohub/crc.h
index afad91f..96b7e4c 100644
--- a/firmware/inc/crc.h
+++ b/lib/include/nanohub/crc.h
@@ -14,8 +14,8 @@
* limitations under the License.
*/
-#ifndef __CRC_H
-#define __CRC_H
+#ifndef _NANOHUB_CRC_H_
+#define _NANOHUB_CRC_H_
#include <stddef.h>
#include <stdint.h>
@@ -38,4 +38,4 @@
*/
uint32_t crc32(const void *buf, size_t size, uint32_t crc);
-#endif /* __CRC_H */
+#endif /* _NANOHUB_CRC_H_ */
diff --git a/lib/include/nanohub/nanoapp.h b/lib/include/nanohub/nanoapp.h
new file mode 100644
index 0000000..da6f341
--- /dev/null
+++ b/lib/include/nanohub/nanoapp.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _NANOHUB_NANOAPP_H_
+#define _NANOHUB_NANOAPP_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+void *reallocOrDie(void *buf, size_t bufSz);
+void assertMem(size_t used, size_t total);
+bool readFile(void *dst, uint32_t len, const char *fileName);
+void *loadFile(const char *fileName, uint32_t *size);
+void printHash(FILE *out, const char *pfx, const uint32_t *hash, size_t size);
+void printHashRev(FILE *out, const char *pfx, const uint32_t *hash, size_t size);
+
+#endif /* _NANOHUB_NANOAPP_H_ */
diff --git a/lib/include/nanohub/nanohub.h b/lib/include/nanohub/nanohub.h
new file mode 100644
index 0000000..53c5189
--- /dev/null
+++ b/lib/include/nanohub/nanohub.h
@@ -0,0 +1,186 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _NANOHUB_NANOHUB_H_
+#define _NANOHUB_NANOHUB_H_
+
+#include <inttypes.h>
+#include <nanohub/aes.h>
+
+/* this file is collection of nanohub-related definitions shared between multiple parties,
+ * including but not limited to: HAL, Kernel, utilities, nanohub FW
+ * it provides minimum details on nanohub implementation, necessary to reliably identify it, and
+ * generate/parse compatible images
+ */
+
+#define NANOAPP_SIGNED_FLAG 0x1 // contents is signed with one or more signature block(s)
+#define NANOAPP_ENCRYPTED_FLAG 0x2 // contents is encrypted with exactly one encryption key
+
+#define NANOAPP_AOSP_MAGIC (((uint32_t)'N' << 0) | ((uint32_t)'A' << 8) | ((uint32_t)'N' << 16) | ((uint32_t)'O' << 24))
+#define NANOAPP_FW_MAGIC (((uint32_t)'N' << 0) | ((uint32_t)'B' << 8) | ((uint32_t)'I' << 16) | ((uint32_t)'N' << 24))
+#define GOOGLE_LAYOUT_MAGIC (((uint32_t)'G' << 0) | ((uint32_t)'o' << 8) | ((uint32_t)'o' << 16) | ((uint32_t)'g' << 24))
+
+// The binary format below is in little endian format
+struct nano_app_binary_t {
+ uint32_t header_version; // 0x1 for this version
+ uint32_t magic; // "NANO"
+ uint64_t app_id; // App Id contains vendor id
+ uint32_t app_version; // Version of the app
+ uint32_t flags; // Signed, encrypted
+ uint64_t hw_hub_type; // which hub type is this compiled for
+ uint32_t reserved[2]; // Should be all zeroes
+ uint8_t custom_binary[0]; // start of custom binary data
+};
+
+// we translate AOSP header into FW header: this header is in LE format
+// please maintain natural alignment for every field (matters to Intel; otherwise is has to be declared as packed)
+struct FwCommonHdr {
+ uint32_t magic; // external & internal: NANOAPP_FW_MAGIC
+ uint16_t fwVer; // external & internal: set to 1; header version
+ uint16_t fwFlags; // external & internal: class : EXTERNAL/INTERNAL, EXEC/NOEXEC, APP/KERNEL/EEDATA/...
+ uint64_t appId; // external: copy from AOSP header; internal: defined locally
+ uint32_t appVer; // external: copy from AOSP header; internal: defined locally
+ uint8_t payInfoType; // external: copy ImageLayout::payload; internal: LAYOUT_APP
+ uint8_t payInfoSize; // sizeof(PayloadInfo) for this payload type
+ uint8_t rfu[2]; // filled with 0xFF
+};
+
+struct SectInfo {
+ uint32_t data_start;
+ uint32_t data_end;
+ uint32_t data_data;
+
+ uint32_t bss_start;
+ uint32_t bss_end;
+
+ uint32_t got_start;
+ uint32_t got_end;
+ uint32_t rel_start;
+ uint32_t rel_end;
+};
+
+// this is platform-invariant version of struct TaskFuncs (from seos.h)
+struct AppVectors {
+ uint32_t init;
+ uint32_t end;
+ uint32_t handle;
+};
+
+#define FLASH_RELOC_OFFSET offsetof(struct AppHdr, sect) // used by appSupport.c at run time
+#define BINARY_RELOC_OFFSET offsetof(struct BinHdr, sect) // used by postprocess at build time
+
+struct BinCommonHdr {
+ uint32_t magic;
+ uint32_t appVer;
+};
+
+// binary nanoapp image (.bin) produced by objcopy starts with this binary header (LE)
+struct BinHdr {
+ struct BinCommonHdr hdr;
+ struct SectInfo sect;
+ struct AppVectors vec;
+};
+
+// FW nanoapp image starts with this binary header (LE) in flash
+struct AppHdr {
+ struct FwCommonHdr hdr;
+ struct SectInfo sect;
+ struct AppVectors vec;
+};
+
+struct AppSecSignHdr {
+ uint32_t appDataLen;
+};
+
+struct AppSecEncrHdr {
+ uint64_t keyID;
+ uint32_t dataLen;
+ uint32_t IV[AES_BLOCK_WORDS];
+};
+
+#define LAYOUT_APP 1
+#define LAYOUT_KEY 2
+#define LAYOUT_OS 3
+#define LAYOUT_DATA 4
+
+struct ImageLayout {
+ uint32_t magic; // Layout ID: (GOOGLE_LAYOUT_MAGIC for this implementation)
+ uint8_t version; // layout version
+ uint8_t payload; // type of payload: APP, SECRET KEY, OS IMAGE, USER DATA, ...
+ uint16_t flags; // layout flags: extra options for certain payload types; payload-specific
+};
+
+// .napp image starts with this binary header (LE)
+// it is optionally followed by AppSecSignHdr and/or AppSecEncrHdr
+// all of the above are included in signing hash, but never encrypted
+// encryption (if enabled) starts immediately after those
+struct ImageHeader {
+ struct nano_app_binary_t aosp;
+ struct ImageLayout layout;
+};
+
+#define CKK_RSA 0x00
+#define CKK_AES 0x1F
+
+#define CKO_PUBLIC_KEY 0x02
+#define CKO_PRIVATE_KEY 0x03
+#define CKO_SECRET_KEY 0x04
+
+// flags
+#define FL_KI_ENFORCE_ID 0x0001 // if set, size, key_type, obj_type must be valid
+
+// payload header format: LAYOUT_KEY
+struct KeyInfo {
+ union {
+ struct {
+ uint16_t id; // arbitrary number, != 0, equivalent of PKCS#11 name
+ uint16_t flags; // key flags (additional PKCS#11 attrs, unused for now; must be 0)
+ uint16_t size; // key size in bits
+ uint8_t key_type; // 8 LSB of PKCS-11 CKK_<KEY TYPE>
+ uint8_t obj_type; // 8 LSB of PKCS-11 CKO_<OBJ TYPE>
+ };
+ uint64_t data; // complete 64-bit key-id, unique within this APP namespace (complete id is <APP_ID | KEY_INFO> 128 bits)
+ };
+};
+
+#define AES_KEY_ID(_id) (((struct KeyInfo){ .key_type = CKK_AES, .obj_type = CKO_SECRET_KEY, .size = 256, .id = (_id) }).data)
+
+// payload header format: LAYOUT_APP
+struct AppInfo {
+ struct SectInfo sect;
+ struct AppVectors vec;
+};
+
+#define OS_UPDT_MARKER_INPROGRESS 0xFF
+#define OS_UPDT_MARKER_DOWNLOADED 0xFE
+#define OS_UPDT_MARKER_VERIFIED 0xF0
+#define OS_UPDT_MARKER_INVALID 0x00
+#define OS_UPDT_MAGIC "Nanohub OS" //11 bytes incl terminator
+
+// payload header format: LAYOUT_OS
+struct OsUpdateHdr {
+ char magic[11];
+ uint8_t marker; //OS_UPDT_MARKER_INPROGRESS -> OS_UPDT_MARKER_DOWNLOADED -> OS_UPDT_MARKER_VERIFIED / OS_UPDT_INVALID
+ uint32_t size; //does not include the mandatory signature (using device key) that follows
+};
+
+// payload header format: LAYOUT_DATA
+struct DataInfo {
+ uint32_t id;
+ uint32_t size;
+};
+
+#endif // _NANOHUB_NANOHUB_H_
diff --git a/firmware/inc/rsa.h b/lib/include/nanohub/rsa.h
similarity index 94%
rename from firmware/inc/rsa.h
rename to lib/include/nanohub/rsa.h
index ad8434f..d98ad07 100644
--- a/firmware/inc/rsa.h
+++ b/lib/include/nanohub/rsa.h
@@ -14,12 +14,12 @@
* limitations under the License.
*/
-#ifndef _RSA_H_
-#define _RSA_H_
+#ifndef _NANOHUB_RSA_H_
+#define _NANOHUB_RSA_H_
#include <stdint.h>
-#define RSA_LEN 2048
+#define RSA_LEN 2048
#define RSA_LIMBS ((RSA_LEN + 31)/ 32)
#define RSA_BYTES sizeof(uint32_t[RSA_LIMBS])
#define RSA_WORDS (RSA_BYTES / sizeof(uint32_t)) //limbs may change in size, but words are always same :)
@@ -50,5 +50,5 @@
#endif
-#endif
+#endif // _NANOHUB_RSA_H_
diff --git a/firmware/inc/sha2.h b/lib/include/nanohub/sha2.h
similarity index 94%
rename from firmware/inc/sha2.h
rename to lib/include/nanohub/sha2.h
index ac72e91..b29c5c4 100644
--- a/firmware/inc/sha2.h
+++ b/lib/include/nanohub/sha2.h
@@ -14,8 +14,8 @@
* limitations under the License.
*/
-#ifndef _SHA2_H_
-#define _SHA2_H_
+#ifndef _NANOHUB_SHA2_H_
+#define _NANOHUB_SHA2_H_
//this is neither the fastest nor the smallest. but it is simple and matches the spec. cool.
@@ -45,5 +45,5 @@
-#endif
+#endif // _NANOHUB_SHA2_H_
diff --git a/lib/libm/ef_atan2.c b/lib/libm/ef_atan2.c
new file mode 100644
index 0000000..d57480b
--- /dev/null
+++ b/lib/libm/ef_atan2.c
@@ -0,0 +1,101 @@
+/* ef_atan2.c -- float version of e_atan2.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ *
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+tiny = 1.0e-30,
+zero = 0.0,
+pi_o_4 = 7.8539818525e-01, /* 0x3f490fdb */
+pi_o_2 = 1.5707963705e+00, /* 0x3fc90fdb */
+pi = 3.1415927410e+00, /* 0x40490fdb */
+pi_lo = -8.7422776573e-08; /* 0xb3bbbd2e */
+
+#ifdef __STDC__
+ float __ieee754_atan2f(float y, float x)
+#else
+ float __ieee754_atan2f(y,x)
+ float y,x;
+#endif
+{
+ float z;
+ __int32_t k,m,hx,hy,ix,iy;
+
+ GET_FLOAT_WORD(hx,x);
+ ix = hx&0x7fffffff;
+ GET_FLOAT_WORD(hy,y);
+ iy = hy&0x7fffffff;
+ if(FLT_UWORD_IS_NAN(ix)||
+ FLT_UWORD_IS_NAN(iy)) /* x or y is NaN */
+ return x+y;
+ if(hx==0x3f800000) return atanf(y); /* x=1.0 */
+ m = ((hy>>31)&1)|((hx>>30)&2); /* 2*sign(x)+sign(y) */
+
+ /* when y = 0 */
+ if(FLT_UWORD_IS_ZERO(iy)) {
+ switch(m) {
+ case 0:
+ case 1: return y; /* atan(+-0,+anything)=+-0 */
+ case 2: return pi+tiny;/* atan(+0,-anything) = pi */
+ case 3: return -pi-tiny;/* atan(-0,-anything) =-pi */
+ }
+ }
+ /* when x = 0 */
+ if(FLT_UWORD_IS_ZERO(ix)) return (hy<0)? -pi_o_2-tiny: pi_o_2+tiny;
+
+ /* when x is INF */
+ if(FLT_UWORD_IS_INFINITE(ix)) {
+ if(FLT_UWORD_IS_INFINITE(iy)) {
+ switch(m) {
+ case 0: return pi_o_4+tiny;/* atan(+INF,+INF) */
+ case 1: return -pi_o_4-tiny;/* atan(-INF,+INF) */
+ case 2: return (float)3.0*pi_o_4+tiny;/*atan(+INF,-INF)*/
+ case 3: return (float)-3.0*pi_o_4-tiny;/*atan(-INF,-INF)*/
+ }
+ } else {
+ switch(m) {
+ case 0: return zero ; /* atan(+...,+INF) */
+ case 1: return -zero ; /* atan(-...,+INF) */
+ case 2: return pi+tiny ; /* atan(+...,-INF) */
+ case 3: return -pi-tiny ; /* atan(-...,-INF) */
+ }
+ }
+ }
+ /* when y is INF */
+ if(FLT_UWORD_IS_INFINITE(iy)) return (hy<0)? -pi_o_2-tiny: pi_o_2+tiny;
+
+ /* compute y/x */
+ k = (iy-ix)>>23;
+ if(k > 60) z=pi_o_2+(float)0.5*pi_lo; /* |y/x| > 2**60 */
+ else if(hx<0&&k<-60) z=0.0; /* |y|/x < -2**60 */
+ else z=atanf(fabsf(y/x)); /* safe to do y/x */
+ switch (m) {
+ case 0: return z ; /* atan(+,+) */
+ case 1: {
+ __uint32_t zh;
+ GET_FLOAT_WORD(zh,z);
+ SET_FLOAT_WORD(z,zh ^ 0x80000000);
+ }
+ return z ; /* atan(-,+) */
+ case 2: return pi-(z-pi_lo);/* atan(+,-) */
+ default: /* case 3 */
+ return (z-pi_lo)-pi;/* atan(-,-) */
+ }
+}
diff --git a/lib/libm/ef_rem_pio2.c b/lib/libm/ef_rem_pio2.c
new file mode 100644
index 0000000..f1191d0
--- /dev/null
+++ b/lib/libm/ef_rem_pio2.c
@@ -0,0 +1,193 @@
+/* ef_rem_pio2.c -- float version of e_rem_pio2.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ *
+ */
+
+/* __ieee754_rem_pio2f(x,y)
+ *
+ * return the remainder of x rem pi/2 in y[0]+y[1]
+ * use __kernel_rem_pio2f()
+ */
+
+#include "fdlibm.h"
+
+/*
+ * Table of constants for 2/pi, 396 Hex digits (476 decimal) of 2/pi
+ */
+#ifdef __STDC__
+static const __int32_t two_over_pi[] = {
+#else
+static __int32_t two_over_pi[] = {
+#endif
+0xA2, 0xF9, 0x83, 0x6E, 0x4E, 0x44, 0x15, 0x29, 0xFC,
+0x27, 0x57, 0xD1, 0xF5, 0x34, 0xDD, 0xC0, 0xDB, 0x62,
+0x95, 0x99, 0x3C, 0x43, 0x90, 0x41, 0xFE, 0x51, 0x63,
+0xAB, 0xDE, 0xBB, 0xC5, 0x61, 0xB7, 0x24, 0x6E, 0x3A,
+0x42, 0x4D, 0xD2, 0xE0, 0x06, 0x49, 0x2E, 0xEA, 0x09,
+0xD1, 0x92, 0x1C, 0xFE, 0x1D, 0xEB, 0x1C, 0xB1, 0x29,
+0xA7, 0x3E, 0xE8, 0x82, 0x35, 0xF5, 0x2E, 0xBB, 0x44,
+0x84, 0xE9, 0x9C, 0x70, 0x26, 0xB4, 0x5F, 0x7E, 0x41,
+0x39, 0x91, 0xD6, 0x39, 0x83, 0x53, 0x39, 0xF4, 0x9C,
+0x84, 0x5F, 0x8B, 0xBD, 0xF9, 0x28, 0x3B, 0x1F, 0xF8,
+0x97, 0xFF, 0xDE, 0x05, 0x98, 0x0F, 0xEF, 0x2F, 0x11,
+0x8B, 0x5A, 0x0A, 0x6D, 0x1F, 0x6D, 0x36, 0x7E, 0xCF,
+0x27, 0xCB, 0x09, 0xB7, 0x4F, 0x46, 0x3F, 0x66, 0x9E,
+0x5F, 0xEA, 0x2D, 0x75, 0x27, 0xBA, 0xC7, 0xEB, 0xE5,
+0xF1, 0x7B, 0x3D, 0x07, 0x39, 0xF7, 0x8A, 0x52, 0x92,
+0xEA, 0x6B, 0xFB, 0x5F, 0xB1, 0x1F, 0x8D, 0x5D, 0x08,
+0x56, 0x03, 0x30, 0x46, 0xFC, 0x7B, 0x6B, 0xAB, 0xF0,
+0xCF, 0xBC, 0x20, 0x9A, 0xF4, 0x36, 0x1D, 0xA9, 0xE3,
+0x91, 0x61, 0x5E, 0xE6, 0x1B, 0x08, 0x65, 0x99, 0x85,
+0x5F, 0x14, 0xA0, 0x68, 0x40, 0x8D, 0xFF, 0xD8, 0x80,
+0x4D, 0x73, 0x27, 0x31, 0x06, 0x06, 0x15, 0x56, 0xCA,
+0x73, 0xA8, 0xC9, 0x60, 0xE2, 0x7B, 0xC0, 0x8C, 0x6B,
+};
+
+/* This array is like the one in e_rem_pio2.c, but the numbers are
+ single precision and the last 8 bits are forced to 0. */
+#ifdef __STDC__
+static const __int32_t npio2_hw[] = {
+#else
+static __int32_t npio2_hw[] = {
+#endif
+0x3fc90f00, 0x40490f00, 0x4096cb00, 0x40c90f00, 0x40fb5300, 0x4116cb00,
+0x412fed00, 0x41490f00, 0x41623100, 0x417b5300, 0x418a3a00, 0x4196cb00,
+0x41a35c00, 0x41afed00, 0x41bc7e00, 0x41c90f00, 0x41d5a000, 0x41e23100,
+0x41eec200, 0x41fb5300, 0x4203f200, 0x420a3a00, 0x42108300, 0x4216cb00,
+0x421d1400, 0x42235c00, 0x4229a500, 0x422fed00, 0x42363600, 0x423c7e00,
+0x4242c700, 0x42490f00
+};
+
+/*
+ * invpio2: 24 bits of 2/pi
+ * pio2_1: first 17 bit of pi/2
+ * pio2_1t: pi/2 - pio2_1
+ * pio2_2: second 17 bit of pi/2
+ * pio2_2t: pi/2 - (pio2_1+pio2_2)
+ * pio2_3: third 17 bit of pi/2
+ * pio2_3t: pi/2 - (pio2_1+pio2_2+pio2_3)
+ */
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+zero = 0.0000000000e+00, /* 0x00000000 */
+half = 5.0000000000e-01, /* 0x3f000000 */
+two8 = 2.5600000000e+02, /* 0x43800000 */
+invpio2 = 6.3661980629e-01, /* 0x3f22f984 */
+pio2_1 = 1.5707855225e+00, /* 0x3fc90f80 */
+pio2_1t = 1.0804334124e-05, /* 0x37354443 */
+pio2_2 = 1.0804273188e-05, /* 0x37354400 */
+pio2_2t = 6.0770999344e-11, /* 0x2e85a308 */
+pio2_3 = 6.0770943833e-11, /* 0x2e85a300 */
+pio2_3t = 6.1232342629e-17; /* 0x248d3132 */
+
+#ifdef __STDC__
+ __int32_t __ieee754_rem_pio2f(float x, float *y)
+#else
+ __int32_t __ieee754_rem_pio2f(x,y)
+ float x,y[];
+#endif
+{
+ float z,w,t,r,fn;
+ float tx[3];
+ __int32_t i,j,n,ix,hx;
+ int e0,nx;
+
+ GET_FLOAT_WORD(hx,x);
+ ix = hx&0x7fffffff;
+ if(ix<=0x3f490fd8) /* |x| ~<= pi/4 , no need for reduction */
+ {y[0] = x; y[1] = 0; return 0;}
+ if(ix<0x4016cbe4) { /* |x| < 3pi/4, special case with n=+-1 */
+ if(hx>0) {
+ z = x - pio2_1;
+ if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */
+ y[0] = z - pio2_1t;
+ y[1] = (z-y[0])-pio2_1t;
+ } else { /* near pi/2, use 24+24+24 bit pi */
+ z -= pio2_2;
+ y[0] = z - pio2_2t;
+ y[1] = (z-y[0])-pio2_2t;
+ }
+ return 1;
+ } else { /* negative x */
+ z = x + pio2_1;
+ if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */
+ y[0] = z + pio2_1t;
+ y[1] = (z-y[0])+pio2_1t;
+ } else { /* near pi/2, use 24+24+24 bit pi */
+ z += pio2_2;
+ y[0] = z + pio2_2t;
+ y[1] = (z-y[0])+pio2_2t;
+ }
+ return -1;
+ }
+ }
+ if(ix<=0x43490f80) { /* |x| ~<= 2^7*(pi/2), medium size */
+ t = fabsf(x);
+ n = (__int32_t) (t*invpio2+half);
+ fn = (float)n;
+ r = t-fn*pio2_1;
+ w = fn*pio2_1t; /* 1st round good to 40 bit */
+ if(n<32&&(ix&0xffffff00)!=npio2_hw[n-1]) {
+ y[0] = r-w; /* quick check no cancellation */
+ } else {
+ __uint32_t high;
+ j = ix>>23;
+ y[0] = r-w;
+ GET_FLOAT_WORD(high,y[0]);
+ i = j-((high>>23)&0xff);
+ if(i>8) { /* 2nd iteration needed, good to 57 */
+ t = r;
+ w = fn*pio2_2;
+ r = t-w;
+ w = fn*pio2_2t-((t-r)-w);
+ y[0] = r-w;
+ GET_FLOAT_WORD(high,y[0]);
+ i = j-((high>>23)&0xff);
+ if(i>25) { /* 3rd iteration need, 74 bits acc */
+ t = r; /* will cover all possible cases */
+ w = fn*pio2_3;
+ r = t-w;
+ w = fn*pio2_3t-((t-r)-w);
+ y[0] = r-w;
+ }
+ }
+ }
+ y[1] = (r-y[0])-w;
+ if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;}
+ else return n;
+ }
+ /*
+ * all other (large) arguments
+ */
+ if(!FLT_UWORD_IS_FINITE(ix)) {
+ y[0]=y[1]=x-x; return 0;
+ }
+ /* set z = scalbn(|x|,ilogb(x)-7) */
+ e0 = (int)((ix>>23)-134); /* e0 = ilogb(z)-7; */
+ SET_FLOAT_WORD(z, ix - ((__int32_t)e0<<23));
+ for(i=0;i<2;i++) {
+ tx[i] = (float)((__int32_t)(z));
+ z = (z-tx[i])*two8;
+ }
+ tx[2] = z;
+ nx = 3;
+ while(tx[nx-1]==zero) nx--; /* skip zero term */
+ n = __kernel_rem_pio2f(tx,y,e0,nx,2,two_over_pi);
+ if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;}
+ return n;
+}
diff --git a/lib/libm/fdlibm.h b/lib/libm/fdlibm.h
new file mode 100644
index 0000000..a4b7fff
--- /dev/null
+++ b/lib/libm/fdlibm.h
@@ -0,0 +1,404 @@
+
+/* @(#)fdlibm.h 5.1 93/09/24 */
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+/* REDHAT LOCAL: Include files. */
+#include <math.h>
+#include <sys/types.h>
+#include <machine/ieeefp.h>
+
+/* REDHAT LOCAL: Default to XOPEN_MODE. */
+#define _XOPEN_MODE
+
+/* Most routines need to check whether a float is finite, infinite, or not a
+ number, and many need to know whether the result of an operation will
+ overflow. These conditions depend on whether the largest exponent is
+ used for NaNs & infinities, or whether it's used for finite numbers. The
+ macros below wrap up that kind of information:
+
+ FLT_UWORD_IS_FINITE(X)
+ True if a positive float with bitmask X is finite.
+
+ FLT_UWORD_IS_NAN(X)
+ True if a positive float with bitmask X is not a number.
+
+ FLT_UWORD_IS_INFINITE(X)
+ True if a positive float with bitmask X is +infinity.
+
+ FLT_UWORD_MAX
+ The bitmask of FLT_MAX.
+
+ FLT_UWORD_HALF_MAX
+ The bitmask of FLT_MAX/2.
+
+ FLT_UWORD_EXP_MAX
+ The bitmask of the largest finite exponent (129 if the largest
+ exponent is used for finite numbers, 128 otherwise).
+
+ FLT_UWORD_LOG_MAX
+ The bitmask of log(FLT_MAX), rounded down. This value is the largest
+ input that can be passed to exp() without producing overflow.
+
+ FLT_UWORD_LOG_2MAX
+ The bitmask of log(2*FLT_MAX), rounded down. This value is the
+ largest input than can be passed to cosh() without producing
+ overflow.
+
+ FLT_LARGEST_EXP
+ The largest biased exponent that can be used for finite numbers
+ (255 if the largest exponent is used for finite numbers, 254
+ otherwise) */
+
+#ifdef _FLT_LARGEST_EXPONENT_IS_NORMAL
+#define FLT_UWORD_IS_FINITE(x) 1
+#define FLT_UWORD_IS_NAN(x) 0
+#define FLT_UWORD_IS_INFINITE(x) 0
+#define FLT_UWORD_MAX 0x7fffffff
+#define FLT_UWORD_EXP_MAX 0x43010000
+#define FLT_UWORD_LOG_MAX 0x42b2d4fc
+#define FLT_UWORD_LOG_2MAX 0x42b437e0
+#define HUGE ((float)0X1.FFFFFEP128)
+#else
+#define FLT_UWORD_IS_FINITE(x) ((x)<0x7f800000L)
+#define FLT_UWORD_IS_NAN(x) ((x)>0x7f800000L)
+#define FLT_UWORD_IS_INFINITE(x) ((x)==0x7f800000L)
+#define FLT_UWORD_MAX 0x7f7fffffL
+#define FLT_UWORD_EXP_MAX 0x43000000
+#define FLT_UWORD_LOG_MAX 0x42b17217
+#define FLT_UWORD_LOG_2MAX 0x42b2d4fc
+#define HUGE ((float)3.40282346638528860e+38)
+#endif
+#define FLT_UWORD_HALF_MAX (FLT_UWORD_MAX-(1L<<23))
+#define FLT_LARGEST_EXP (FLT_UWORD_MAX>>23)
+
+/* Many routines check for zero and subnormal numbers. Such things depend
+ on whether the target supports denormals or not:
+
+ FLT_UWORD_IS_ZERO(X)
+ True if a positive float with bitmask X is +0. Without denormals,
+ any float with a zero exponent is a +0 representation. With
+ denormals, the only +0 representation is a 0 bitmask.
+
+ FLT_UWORD_IS_SUBNORMAL(X)
+ True if a non-zero positive float with bitmask X is subnormal.
+ (Routines should check for zeros first.)
+
+ FLT_UWORD_MIN
+ The bitmask of the smallest float above +0. Call this number
+ REAL_FLT_MIN...
+
+ FLT_UWORD_EXP_MIN
+ The bitmask of the float representation of REAL_FLT_MIN's exponent.
+
+ FLT_UWORD_LOG_MIN
+ The bitmask of |log(REAL_FLT_MIN)|, rounding down.
+
+ FLT_SMALLEST_EXP
+ REAL_FLT_MIN's exponent - EXP_BIAS (1 if denormals are not supported,
+ -22 if they are).
+*/
+
+#ifdef _FLT_NO_DENORMALS
+#define FLT_UWORD_IS_ZERO(x) ((x)<0x00800000L)
+#define FLT_UWORD_IS_SUBNORMAL(x) 0
+#define FLT_UWORD_MIN 0x00800000
+#define FLT_UWORD_EXP_MIN 0x42fc0000
+#define FLT_UWORD_LOG_MIN 0x42aeac50
+#define FLT_SMALLEST_EXP 1
+#else
+#define FLT_UWORD_IS_ZERO(x) ((x)==0)
+#define FLT_UWORD_IS_SUBNORMAL(x) ((x)<0x00800000L)
+#define FLT_UWORD_MIN 0x00000001
+#define FLT_UWORD_EXP_MIN 0x43160000
+#define FLT_UWORD_LOG_MIN 0x42cff1b5
+#define FLT_SMALLEST_EXP -22
+#endif
+
+#ifdef __STDC__
+#undef __P
+#define __P(p) p
+#else
+#define __P(p) ()
+#endif
+
+/*
+ * set X_TLOSS = pi*2**52, which is possibly defined in <values.h>
+ * (one may replace the following line by "#include <values.h>")
+ */
+
+#define X_TLOSS 1.41484755040568800000e+16
+
+/* Functions that are not documented, and are not in <math.h>. */
+
+#ifdef _SCALB_INT
+extern double scalb __P((double, int));
+#else
+extern double scalb __P((double, double));
+#endif
+extern double significand __P((double));
+
+/* ieee style elementary functions */
+extern double __ieee754_sqrt __P((double));
+extern double __ieee754_acos __P((double));
+extern double __ieee754_acosh __P((double));
+extern double __ieee754_log __P((double));
+extern double __ieee754_atanh __P((double));
+extern double __ieee754_asin __P((double));
+extern double __ieee754_atan2 __P((double,double));
+extern double __ieee754_exp __P((double));
+extern double __ieee754_cosh __P((double));
+extern double __ieee754_fmod __P((double,double));
+extern double __ieee754_pow __P((double,double));
+extern double __ieee754_lgamma_r __P((double,int *));
+extern double __ieee754_gamma_r __P((double,int *));
+extern double __ieee754_log10 __P((double));
+extern double __ieee754_sinh __P((double));
+extern double __ieee754_hypot __P((double,double));
+extern double __ieee754_j0 __P((double));
+extern double __ieee754_j1 __P((double));
+extern double __ieee754_y0 __P((double));
+extern double __ieee754_y1 __P((double));
+extern double __ieee754_jn __P((int,double));
+extern double __ieee754_yn __P((int,double));
+extern double __ieee754_remainder __P((double,double));
+extern __int32_t __ieee754_rem_pio2 __P((double,double*));
+#ifdef _SCALB_INT
+extern double __ieee754_scalb __P((double,int));
+#else
+extern double __ieee754_scalb __P((double,double));
+#endif
+
+/* fdlibm kernel function */
+extern double __kernel_standard __P((double,double,int));
+extern double __kernel_sin __P((double,double,int));
+extern double __kernel_cos __P((double,double));
+extern double __kernel_tan __P((double,double,int));
+extern int __kernel_rem_pio2 __P((double*,double*,int,int,int,const __int32_t*));
+
+/* Undocumented float functions. */
+#ifdef _SCALB_INT
+extern float scalbf __P((float, int));
+#else
+extern float scalbf __P((float, float));
+#endif
+extern float significandf __P((float));
+
+/* ieee style elementary float functions */
+extern float __ieee754_sqrtf __P((float));
+extern float __ieee754_acosf __P((float));
+extern float __ieee754_acoshf __P((float));
+extern float __ieee754_logf __P((float));
+extern float __ieee754_atanhf __P((float));
+extern float __ieee754_asinf __P((float));
+extern float __ieee754_atan2f __P((float,float));
+extern float __ieee754_expf __P((float));
+extern float __ieee754_coshf __P((float));
+extern float __ieee754_fmodf __P((float,float));
+extern float __ieee754_powf __P((float,float));
+extern float __ieee754_lgammaf_r __P((float,int *));
+extern float __ieee754_gammaf_r __P((float,int *));
+extern float __ieee754_log10f __P((float));
+extern float __ieee754_sinhf __P((float));
+extern float __ieee754_hypotf __P((float,float));
+extern float __ieee754_j0f __P((float));
+extern float __ieee754_j1f __P((float));
+extern float __ieee754_y0f __P((float));
+extern float __ieee754_y1f __P((float));
+extern float __ieee754_jnf __P((int,float));
+extern float __ieee754_ynf __P((int,float));
+extern float __ieee754_remainderf __P((float,float));
+extern __int32_t __ieee754_rem_pio2f __P((float,float*));
+#ifdef _SCALB_INT
+extern float __ieee754_scalbf __P((float,int));
+#else
+extern float __ieee754_scalbf __P((float,float));
+#endif
+
+/* float versions of fdlibm kernel functions */
+extern float __kernel_sinf __P((float,float,int));
+extern float __kernel_cosf __P((float,float));
+extern float __kernel_tanf __P((float,float,int));
+extern int __kernel_rem_pio2f __P((float*,float*,int,int,int,const __int32_t*));
+
+/* The original code used statements like
+ n0 = ((*(int*)&one)>>29)^1; * index of high word *
+ ix0 = *(n0+(int*)&x); * high word of x *
+ ix1 = *((1-n0)+(int*)&x); * low word of x *
+ to dig two 32 bit words out of the 64 bit IEEE floating point
+ value. That is non-ANSI, and, moreover, the gcc instruction
+ scheduler gets it wrong. We instead use the following macros.
+ Unlike the original code, we determine the endianness at compile
+ time, not at run time; I don't see much benefit to selecting
+ endianness at run time. */
+
+#ifndef __IEEE_BIG_ENDIAN
+#ifndef __IEEE_LITTLE_ENDIAN
+ #error Must define endianness
+#endif
+#endif
+
+/* A union which permits us to convert between a double and two 32 bit
+ ints. */
+
+#ifdef __IEEE_BIG_ENDIAN
+
+typedef union
+{
+ double value;
+ struct
+ {
+ __uint32_t msw;
+ __uint32_t lsw;
+ } parts;
+} ieee_double_shape_type;
+
+#endif
+
+#ifdef __IEEE_LITTLE_ENDIAN
+
+typedef union
+{
+ double value;
+ struct
+ {
+ __uint32_t lsw;
+ __uint32_t msw;
+ } parts;
+} ieee_double_shape_type;
+
+#endif
+
+/* Get two 32 bit ints from a double. */
+
+#define EXTRACT_WORDS(ix0,ix1,d) \
+do { \
+ ieee_double_shape_type ew_u; \
+ ew_u.value = (d); \
+ (ix0) = ew_u.parts.msw; \
+ (ix1) = ew_u.parts.lsw; \
+} while (0)
+
+/* Get the more significant 32 bit int from a double. */
+
+#define GET_HIGH_WORD(i,d) \
+do { \
+ ieee_double_shape_type gh_u; \
+ gh_u.value = (d); \
+ (i) = gh_u.parts.msw; \
+} while (0)
+
+/* Get the less significant 32 bit int from a double. */
+
+#define GET_LOW_WORD(i,d) \
+do { \
+ ieee_double_shape_type gl_u; \
+ gl_u.value = (d); \
+ (i) = gl_u.parts.lsw; \
+} while (0)
+
+/* Set a double from two 32 bit ints. */
+
+#define INSERT_WORDS(d,ix0,ix1) \
+do { \
+ ieee_double_shape_type iw_u; \
+ iw_u.parts.msw = (ix0); \
+ iw_u.parts.lsw = (ix1); \
+ (d) = iw_u.value; \
+} while (0)
+
+/* Set the more significant 32 bits of a double from an int. */
+
+#define SET_HIGH_WORD(d,v) \
+do { \
+ ieee_double_shape_type sh_u; \
+ sh_u.value = (d); \
+ sh_u.parts.msw = (v); \
+ (d) = sh_u.value; \
+} while (0)
+
+/* Set the less significant 32 bits of a double from an int. */
+
+#define SET_LOW_WORD(d,v) \
+do { \
+ ieee_double_shape_type sl_u; \
+ sl_u.value = (d); \
+ sl_u.parts.lsw = (v); \
+ (d) = sl_u.value; \
+} while (0)
+
+/* A union which permits us to convert between a float and a 32 bit
+ int. */
+
+typedef union
+{
+ float value;
+ __uint32_t word;
+} ieee_float_shape_type;
+
+/* Get a 32 bit int from a float. */
+
+#define GET_FLOAT_WORD(i,d) \
+do { \
+ ieee_float_shape_type gf_u; \
+ gf_u.value = (d); \
+ (i) = gf_u.word; \
+} while (0)
+
+/* Set a float from a 32 bit int. */
+
+#define SET_FLOAT_WORD(d,i) \
+do { \
+ ieee_float_shape_type sf_u; \
+ sf_u.word = (i); \
+ (d) = sf_u.value; \
+} while (0)
+
+/* Macros to avoid undefined behaviour that can arise if the amount
+ of a shift is exactly equal to the size of the shifted operand. */
+
+#define SAFE_LEFT_SHIFT(op,amt) \
+ (((amt) < 8 * sizeof(op)) ? ((op) << (amt)) : 0)
+
+#define SAFE_RIGHT_SHIFT(op,amt) \
+ (((amt) < 8 * sizeof(op)) ? ((op) >> (amt)) : 0)
+
+#ifdef _COMPLEX_H
+
+/*
+ * Quoting from ISO/IEC 9899:TC2:
+ *
+ * 6.2.5.13 Types
+ * Each complex type has the same representation and alignment requirements as
+ * an array type containing exactly two elements of the corresponding real type;
+ * the first element is equal to the real part, and the second element to the
+ * imaginary part, of the complex number.
+ */
+typedef union {
+ float complex z;
+ float parts[2];
+} float_complex;
+
+typedef union {
+ double complex z;
+ double parts[2];
+} double_complex;
+
+typedef union {
+ long double complex z;
+ long double parts[2];
+} long_double_complex;
+
+#define REAL_PART(z) ((z).parts[0])
+#define IMAG_PART(z) ((z).parts[1])
+
+#endif /* _COMPLEX_H */
+
diff --git a/lib/libm/kf_cos.c b/lib/libm/kf_cos.c
new file mode 100644
index 0000000..4f71af2
--- /dev/null
+++ b/lib/libm/kf_cos.c
@@ -0,0 +1,59 @@
+/* kf_cos.c -- float version of k_cos.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+one = 1.0000000000e+00, /* 0x3f800000 */
+C1 = 4.1666667908e-02, /* 0x3d2aaaab */
+C2 = -1.3888889225e-03, /* 0xbab60b61 */
+C3 = 2.4801587642e-05, /* 0x37d00d01 */
+C4 = -2.7557314297e-07, /* 0xb493f27c */
+C5 = 2.0875723372e-09, /* 0x310f74f6 */
+C6 = -1.1359647598e-11; /* 0xad47d74e */
+
+#ifdef __STDC__
+ float __kernel_cosf(float x, float y)
+#else
+ float __kernel_cosf(x, y)
+ float x,y;
+#endif
+{
+ float a,hz,z,r,qx;
+ __int32_t ix;
+ GET_FLOAT_WORD(ix,x);
+ ix &= 0x7fffffff; /* ix = |x|'s high word*/
+ if(ix<0x32000000) { /* if x < 2**27 */
+ if(((int)x)==0) return one; /* generate inexact */
+ }
+ z = x*x;
+ r = z*(C1+z*(C2+z*(C3+z*(C4+z*(C5+z*C6)))));
+ if(ix < 0x3e99999a) /* if |x| < 0.3 */
+ return one - ((float)0.5*z - (z*r - x*y));
+ else {
+ if(ix > 0x3f480000) { /* x > 0.78125 */
+ qx = (float)0.28125;
+ } else {
+ SET_FLOAT_WORD(qx,ix-0x01000000); /* x/4 */
+ }
+ hz = (float)0.5*z-qx;
+ a = one-qx;
+ return a - (hz - (z*r-x*y));
+ }
+}
diff --git a/lib/libm/kf_rem_pio2.c b/lib/libm/kf_rem_pio2.c
new file mode 100644
index 0000000..261c481
--- /dev/null
+++ b/lib/libm/kf_rem_pio2.c
@@ -0,0 +1,208 @@
+/* kf_rem_pio2.c -- float version of k_rem_pio2.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+/* In the float version, the input parameter x contains 8 bit
+ integers, not 24 bit integers. 113 bit precision is not supported. */
+
+#ifdef __STDC__
+static const int init_jk[] = {4,7,9}; /* initial value for jk */
+#else
+static int init_jk[] = {4,7,9};
+#endif
+
+#ifdef __STDC__
+static const float PIo2[] = {
+#else
+static float PIo2[] = {
+#endif
+ 1.5703125000e+00, /* 0x3fc90000 */
+ 4.5776367188e-04, /* 0x39f00000 */
+ 2.5987625122e-05, /* 0x37da0000 */
+ 7.5437128544e-08, /* 0x33a20000 */
+ 6.0026650317e-11, /* 0x2e840000 */
+ 7.3896444519e-13, /* 0x2b500000 */
+ 5.3845816694e-15, /* 0x27c20000 */
+ 5.6378512969e-18, /* 0x22d00000 */
+ 8.3009228831e-20, /* 0x1fc40000 */
+ 3.2756352257e-22, /* 0x1bc60000 */
+ 6.3331015649e-25, /* 0x17440000 */
+};
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+zero = 0.0,
+one = 1.0,
+two8 = 2.5600000000e+02, /* 0x43800000 */
+twon8 = 3.9062500000e-03; /* 0x3b800000 */
+
+#ifdef __STDC__
+ int __kernel_rem_pio2f(float *x, float *y, int e0, int nx, int prec, const __int32_t *ipio2)
+#else
+ int __kernel_rem_pio2f(x,y,e0,nx,prec,ipio2)
+ float x[], y[]; int e0,nx,prec; __int32_t ipio2[];
+#endif
+{
+ __int32_t jz,jx,jv,jp,jk,carry,n,iq[20],i,j,k,m,q0,ih;
+ float z,fw,f[20],fq[20],q[20];
+
+ /* initialize jk*/
+ jk = init_jk[prec];
+ jp = jk;
+
+ /* determine jx,jv,q0, note that 3>q0 */
+ jx = nx-1;
+ jv = (e0-3)/8; if(jv<0) jv=0;
+ q0 = e0-8*(jv+1);
+
+ /* set up f[0] to f[jx+jk] where f[jx+jk] = ipio2[jv+jk] */
+ j = jv-jx; m = jx+jk;
+ for(i=0;i<=m;i++,j++) f[i] = (j<0)? zero : (float) ipio2[j];
+
+ /* compute q[0],q[1],...q[jk] */
+ for (i=0;i<=jk;i++) {
+ for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j]; q[i] = fw;
+ }
+
+ jz = jk;
+recompute:
+ /* distill q[] into iq[] reversingly */
+ for(i=0,j=jz,z=q[jz];j>0;i++,j--) {
+ fw = (float)((__int32_t)(twon8* z));
+ iq[i] = (__int32_t)(z-two8*fw);
+ z = q[j-1]+fw;
+ }
+
+ /* compute n */
+ z = scalbnf(z,(int)q0); /* actual value of z */
+ z -= (float)8.0*floorf(z*(float)0.125); /* trim off integer >= 8 */
+ n = (__int32_t) z;
+ z -= (float)n;
+ ih = 0;
+ if(q0>0) { /* need iq[jz-1] to determine n */
+ i = (iq[jz-1]>>(8-q0)); n += i;
+ iq[jz-1] -= i<<(8-q0);
+ ih = iq[jz-1]>>(7-q0);
+ }
+ else if(q0==0) ih = iq[jz-1]>>8;
+ else if(z>=(float)0.5) ih=2;
+
+ if(ih>0) { /* q > 0.5 */
+ n += 1; carry = 0;
+ for(i=0;i<jz ;i++) { /* compute 1-q */
+ j = iq[i];
+ if(carry==0) {
+ if(j!=0) {
+ carry = 1; iq[i] = 0x100- j;
+ }
+ } else iq[i] = 0xff - j;
+ }
+ if(q0>0) { /* rare case: chance is 1 in 12 */
+ switch(q0) {
+ case 1:
+ iq[jz-1] &= 0x7f; break;
+ case 2:
+ iq[jz-1] &= 0x3f; break;
+ }
+ }
+ if(ih==2) {
+ z = one - z;
+ if(carry!=0) z -= scalbnf(one,(int)q0);
+ }
+ }
+
+ /* check if recomputation is needed */
+ if(z==zero) {
+ j = 0;
+ for (i=jz-1;i>=jk;i--) j |= iq[i];
+ if(j==0) { /* need recomputation */
+ for(k=1;iq[jk-k]==0;k++); /* k = no. of terms needed */
+
+ for(i=jz+1;i<=jz+k;i++) { /* add q[jz+1] to q[jz+k] */
+ f[jx+i] = (float) ipio2[jv+i];
+ for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j];
+ q[i] = fw;
+ }
+ jz += k;
+ goto recompute;
+ }
+ }
+
+ /* chop off zero terms */
+ if(z==(float)0.0) {
+ jz -= 1; q0 -= 8;
+ while(iq[jz]==0) { jz--; q0-=8;}
+ } else { /* break z into 8-bit if necessary */
+ z = scalbnf(z,-(int)q0);
+ if(z>=two8) {
+ fw = (float)((__int32_t)(twon8*z));
+ iq[jz] = (__int32_t)(z-two8*fw);
+ jz += 1; q0 += 8;
+ iq[jz] = (__int32_t) fw;
+ } else iq[jz] = (__int32_t) z ;
+ }
+
+ /* convert integer "bit" chunk to floating-point value */
+ fw = scalbnf(one,(int)q0);
+ for(i=jz;i>=0;i--) {
+ q[i] = fw*(float)iq[i]; fw*=twon8;
+ }
+
+ /* compute PIo2[0,...,jp]*q[jz,...,0] */
+ for(i=jz;i>=0;i--) {
+ for(fw=0.0,k=0;k<=jp&&k<=jz-i;k++) fw += PIo2[k]*q[i+k];
+ fq[jz-i] = fw;
+ }
+
+ /* compress fq[] into y[] */
+ switch(prec) {
+ case 0:
+ fw = 0.0;
+ for (i=jz;i>=0;i--) fw += fq[i];
+ y[0] = (ih==0)? fw: -fw;
+ break;
+ case 1:
+ case 2:
+ fw = 0.0;
+ for (i=jz;i>=0;i--) fw += fq[i];
+ y[0] = (ih==0)? fw: -fw;
+ fw = fq[0]-fw;
+ for (i=1;i<=jz;i++) fw += fq[i];
+ y[1] = (ih==0)? fw: -fw;
+ break;
+ case 3: /* painful */
+ for (i=jz;i>0;i--) {
+ fw = fq[i-1]+fq[i];
+ fq[i] += fq[i-1]-fw;
+ fq[i-1] = fw;
+ }
+ for (i=jz;i>1;i--) {
+ fw = fq[i-1]+fq[i];
+ fq[i] += fq[i-1]-fw;
+ fq[i-1] = fw;
+ }
+ for (fw=0.0,i=jz;i>=2;i--) fw += fq[i];
+ if(ih==0) {
+ y[0] = fq[0]; y[1] = fq[1]; y[2] = fw;
+ } else {
+ y[0] = -fq[0]; y[1] = -fq[1]; y[2] = -fw;
+ }
+ }
+ return n&7;
+}
diff --git a/lib/libm/kf_sin.c b/lib/libm/kf_sin.c
new file mode 100644
index 0000000..e81fa0b
--- /dev/null
+++ b/lib/libm/kf_sin.c
@@ -0,0 +1,49 @@
+/* kf_sin.c -- float version of k_sin.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+half = 5.0000000000e-01,/* 0x3f000000 */
+S1 = -1.6666667163e-01, /* 0xbe2aaaab */
+S2 = 8.3333337680e-03, /* 0x3c088889 */
+S3 = -1.9841270114e-04, /* 0xb9500d01 */
+S4 = 2.7557314297e-06, /* 0x3638ef1b */
+S5 = -2.5050759689e-08, /* 0xb2d72f34 */
+S6 = 1.5896910177e-10; /* 0x2f2ec9d3 */
+
+#ifdef __STDC__
+ float __kernel_sinf(float x, float y, int iy)
+#else
+ float __kernel_sinf(x, y, iy)
+ float x,y; int iy; /* iy=0 if y is zero */
+#endif
+{
+ float z,r,v;
+ __int32_t ix;
+ GET_FLOAT_WORD(ix,x);
+ ix &= 0x7fffffff; /* high word of x */
+ if(ix<0x32000000) /* |x| < 2**-27 */
+ {if((int)x==0) return x;} /* generate inexact */
+ z = x*x;
+ v = z*x;
+ r = S2+z*(S3+z*(S4+z*(S5+z*S6)));
+ if(iy==0) return x+v*(S1+z*r);
+ else return x-((z*(half*y-v*r)-y)-v*S1);
+}
diff --git a/lib/libm/sf_atan.c b/lib/libm/sf_atan.c
new file mode 100644
index 0000000..6edf05f
--- /dev/null
+++ b/lib/libm/sf_atan.c
@@ -0,0 +1,129 @@
+/* sf_atan.c -- float version of s_atan.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ *
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float atanhi[] = {
+#else
+static float atanhi[] = {
+#endif
+ 4.6364760399e-01, /* atan(0.5)hi 0x3eed6338 */
+ 7.8539812565e-01, /* atan(1.0)hi 0x3f490fda */
+ 9.8279368877e-01, /* atan(1.5)hi 0x3f7b985e */
+ 1.5707962513e+00, /* atan(inf)hi 0x3fc90fda */
+};
+
+#ifdef __STDC__
+static const float atanlo[] = {
+#else
+static float atanlo[] = {
+#endif
+ 5.0121582440e-09, /* atan(0.5)lo 0x31ac3769 */
+ 3.7748947079e-08, /* atan(1.0)lo 0x33222168 */
+ 3.4473217170e-08, /* atan(1.5)lo 0x33140fb4 */
+ 7.5497894159e-08, /* atan(inf)lo 0x33a22168 */
+};
+
+#ifdef __STDC__
+static const float aT[] = {
+#else
+static float aT[] = {
+#endif
+ 3.3333334327e-01, /* 0x3eaaaaaa */
+ -2.0000000298e-01, /* 0xbe4ccccd */
+ 1.4285714924e-01, /* 0x3e124925 */
+ -1.1111110449e-01, /* 0xbde38e38 */
+ 9.0908870101e-02, /* 0x3dba2e6e */
+ -7.6918758452e-02, /* 0xbd9d8795 */
+ 6.6610731184e-02, /* 0x3d886b35 */
+ -5.8335702866e-02, /* 0xbd6ef16b */
+ 4.9768779427e-02, /* 0x3d4bda59 */
+ -3.6531571299e-02, /* 0xbd15a221 */
+ 1.6285819933e-02, /* 0x3c8569d7 */
+};
+
+#ifdef __STDC__
+ static const float
+#else
+ static float
+#endif
+one = 1.0,
+huge = 1.0e30;
+
+#ifdef __STDC__
+ float atanf(float x)
+#else
+ float atanf(x)
+ float x;
+#endif
+{
+ float w,s1,s2,z;
+ __int32_t ix,hx,id;
+
+ GET_FLOAT_WORD(hx,x);
+ ix = hx&0x7fffffff;
+ if(ix>=0x50800000) { /* if |x| >= 2^34 */
+ if(FLT_UWORD_IS_NAN(ix))
+ return x+x; /* NaN */
+ if(hx>0) return atanhi[3]+atanlo[3];
+ else return -atanhi[3]-atanlo[3];
+ } if (ix < 0x3ee00000) { /* |x| < 0.4375 */
+ if (ix < 0x31000000) { /* |x| < 2^-29 */
+ if(huge+x>one) return x; /* raise inexact */
+ }
+ id = -1;
+ } else {
+ x = fabsf(x);
+ if (ix < 0x3f980000) { /* |x| < 1.1875 */
+ if (ix < 0x3f300000) { /* 7/16 <=|x|<11/16 */
+ id = 0; x = ((float)2.0*x-one)/((float)2.0+x);
+ } else { /* 11/16<=|x|< 19/16 */
+ id = 1; x = (x-one)/(x+one);
+ }
+ } else {
+ if (ix < 0x401c0000) { /* |x| < 2.4375 */
+ id = 2; x = (x-(float)1.5)/(one+(float)1.5*x);
+ } else { /* 2.4375 <= |x| < 2^66 */
+ id = 3; x = -(float)1.0/x;
+ }
+ }}
+ /* end of argument reduction */
+ z = x*x;
+ w = z*z;
+ /* break sum from i=0 to 10 aT[i]z**(i+1) into odd and even poly */
+ s1 = z*(aT[0]+w*(aT[2]+w*(aT[4]+w*(aT[6]+w*(aT[8]+w*aT[10])))));
+ s2 = w*(aT[1]+w*(aT[3]+w*(aT[5]+w*(aT[7]+w*aT[9]))));
+ if (id<0) return x - x*(s1+s2);
+ else {
+ z = atanhi[id] - ((x*(s1+s2) - atanlo[id]) - x);
+ return (hx<0)? -z:z;
+ }
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double atan(double x)
+#else
+ double atan(x)
+ double x;
+#endif
+{
+ return (double) atanf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/lib/libm/sf_cos.c b/lib/libm/sf_cos.c
new file mode 100644
index 0000000..4c0a9a5
--- /dev/null
+++ b/lib/libm/sf_cos.c
@@ -0,0 +1,68 @@
+/* sf_cos.c -- float version of s_cos.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float one=1.0;
+#else
+static float one=1.0;
+#endif
+
+#ifdef __STDC__
+ float cosf(float x)
+#else
+ float cosf(x)
+ float x;
+#endif
+{
+ float y[2],z=0.0;
+ __int32_t n,ix;
+
+ GET_FLOAT_WORD(ix,x);
+
+ /* |x| ~< pi/4 */
+ ix &= 0x7fffffff;
+ if(ix <= 0x3f490fd8) return __kernel_cosf(x,z);
+
+ /* cos(Inf or NaN) is NaN */
+ else if (!FLT_UWORD_IS_FINITE(ix)) return x-x;
+
+ /* argument reduction needed */
+ else {
+ n = __ieee754_rem_pio2f(x,y);
+ switch(n&3) {
+ case 0: return __kernel_cosf(y[0],y[1]);
+ case 1: return -__kernel_sinf(y[0],y[1],1);
+ case 2: return -__kernel_cosf(y[0],y[1]);
+ default:
+ return __kernel_sinf(y[0],y[1],1);
+ }
+ }
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double cos(double x)
+#else
+ double cos(x)
+ double x;
+#endif
+{
+ return (double) cosf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/lib/libm/sf_floor.c b/lib/libm/sf_floor.c
new file mode 100644
index 0000000..9264d81
--- /dev/null
+++ b/lib/libm/sf_floor.c
@@ -0,0 +1,80 @@
+/* sf_floor.c -- float version of s_floor.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+/*
+ * floorf(x)
+ * Return x rounded toward -inf to integral value
+ * Method:
+ * Bit twiddling.
+ * Exception:
+ * Inexact flag raised if x not equal to floorf(x).
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float huge = 1.0e30;
+#else
+static float huge = 1.0e30;
+#endif
+
+#ifdef __STDC__
+ float floorf(float x)
+#else
+ float floorf(x)
+ float x;
+#endif
+{
+ __int32_t i0,j0;
+ __uint32_t i,ix;
+ GET_FLOAT_WORD(i0,x);
+ ix = (i0&0x7fffffff);
+ j0 = (ix>>23)-0x7f;
+ if(j0<23) {
+ if(j0<0) { /* raise inexact if x != 0 */
+ if(huge+x>(float)0.0) {/* return 0*sign(x) if |x|<1 */
+ if(i0>=0) {i0=0;}
+ else if(!FLT_UWORD_IS_ZERO(ix))
+ { i0=0xbf800000;}
+ }
+ } else {
+ i = (0x007fffff)>>j0;
+ if((i0&i)==0) return x; /* x is integral */
+ if(huge+x>(float)0.0) { /* raise inexact flag */
+ if(i0<0) i0 += (0x00800000)>>j0;
+ i0 &= (~i);
+ }
+ }
+ } else {
+ if(!FLT_UWORD_IS_FINITE(ix)) return x+x; /* inf or NaN */
+ else return x; /* x is integral */
+ }
+ SET_FLOAT_WORD(x,i0);
+ return x;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double floor(double x)
+#else
+ double floor(x)
+ double x;
+#endif
+{
+ return (double) floorf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/lib/libm/sf_scalbn.c b/lib/libm/sf_scalbn.c
new file mode 100644
index 0000000..7000600
--- /dev/null
+++ b/lib/libm/sf_scalbn.c
@@ -0,0 +1,86 @@
+/* sf_scalbn.c -- float version of s_scalbn.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+#include <limits.h>
+#include <float.h>
+
+#if INT_MAX > 50000
+#define OVERFLOW_INT 50000
+#else
+#define OVERFLOW_INT 30000
+#endif
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+two25 = 3.355443200e+07, /* 0x4c000000 */
+twom25 = 2.9802322388e-08, /* 0x33000000 */
+huge = 1.0e+30,
+tiny = 1.0e-30;
+
+#ifdef __STDC__
+ float scalbnf (float x, int n)
+#else
+ float scalbnf (x,n)
+ float x; int n;
+#endif
+{
+ __int32_t k,ix;
+ __uint32_t hx;
+
+ GET_FLOAT_WORD(ix,x);
+ hx = ix&0x7fffffff;
+ k = hx>>23; /* extract exponent */
+ if (FLT_UWORD_IS_ZERO(hx))
+ return x;
+ if (!FLT_UWORD_IS_FINITE(hx))
+ return x+x; /* NaN or Inf */
+ if (FLT_UWORD_IS_SUBNORMAL(hx)) {
+ x *= two25;
+ GET_FLOAT_WORD(ix,x);
+ k = ((ix&0x7f800000)>>23) - 25;
+ if (n< -50000) return tiny*x; /*underflow*/
+ }
+ k = k+n;
+ if (k > FLT_LARGEST_EXP) return huge*copysignf(huge,x); /* overflow */
+ if (k > 0) /* normal result */
+ {SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23)); return x;}
+ if (k < FLT_SMALLEST_EXP) {
+ if (n > OVERFLOW_INT) /* in case integer overflow in n+k */
+ return huge*copysignf(huge,x); /*overflow*/
+ else return tiny*copysignf(tiny,x); /*underflow*/
+ }
+ k += 25; /* subnormal result */
+ SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23));
+ return x*twom25;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double scalbn(double x, int n)
+#else
+ double scalbn(x,n)
+ double x;
+ int n;
+#endif
+{
+ return (double) scalbnf((float) x, n);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/lib/libm/sf_sin.c b/lib/libm/sf_sin.c
new file mode 100644
index 0000000..da81845
--- /dev/null
+++ b/lib/libm/sf_sin.c
@@ -0,0 +1,62 @@
+/* sf_sin.c -- float version of s_sin.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+ float sinf(float x)
+#else
+ float sinf(x)
+ float x;
+#endif
+{
+ float y[2],z=0.0;
+ __int32_t n,ix;
+
+ GET_FLOAT_WORD(ix,x);
+
+ /* |x| ~< pi/4 */
+ ix &= 0x7fffffff;
+ if(ix <= 0x3f490fd8) return __kernel_sinf(x,z,0);
+
+ /* sin(Inf or NaN) is NaN */
+ else if (!FLT_UWORD_IS_FINITE(ix)) return x-x;
+
+ /* argument reduction needed */
+ else {
+ n = __ieee754_rem_pio2f(x,y);
+ switch(n&3) {
+ case 0: return __kernel_sinf(y[0],y[1],1);
+ case 1: return __kernel_cosf(y[0],y[1]);
+ case 2: return -__kernel_sinf(y[0],y[1],1);
+ default:
+ return -__kernel_cosf(y[0],y[1]);
+ }
+ }
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double sin(double x)
+#else
+ double sin(x)
+ double x;
+#endif
+{
+ return (double) sinf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/lib/libm/wf_atan2.c b/lib/libm/wf_atan2.c
new file mode 100644
index 0000000..eb2a76b
--- /dev/null
+++ b/lib/libm/wf_atan2.c
@@ -0,0 +1,46 @@
+/* wf_atan2.c -- float version of w_atan2.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice
+ * is preserved.
+ * ====================================================
+ *
+ */
+
+/*
+ * wrapper atan2f(y,x)
+ */
+
+#include "fdlibm.h"
+#include <errno.h>
+
+#ifdef __STDC__
+ float atan2f(float y, float x) /* wrapper atan2f */
+#else
+ float atan2f(y,x) /* wrapper atan2 */
+ float y,x;
+#endif
+{
+ return __ieee754_atan2f(y,x);
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+ double atan2(double y, double x)
+#else
+ double atan2(y,x)
+ double y,x;
+#endif
+{
+ return (double) atan2f((float) y, (float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/firmware/src/aes.c b/lib/nanohub/aes.c
similarity index 99%
rename from firmware/src/aes.c
rename to lib/nanohub/aes.c
index 3496670..113c235 100644
--- a/firmware/src/aes.c
+++ b/lib/nanohub/aes.c
@@ -16,7 +16,7 @@
#include <string.h>
#include <stdint.h>
-#include <aes.h>
+#include <nanohub/aes.h>
#define AES_NUM_ROUNDS 14
diff --git a/lib/nanohub/nanoapp.c b/lib/nanohub/nanoapp.c
new file mode 100644
index 0000000..cbb6039
--- /dev/null
+++ b/lib/nanohub/nanoapp.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+
+#include <nanohub/nanoapp.h>
+
+void *reallocOrDie(void *buf, size_t bufSz)
+{
+ void *newBuf = realloc(buf, bufSz);
+ if (!newBuf) {
+ fprintf(stderr, "Failed to allocate %zu bytes\n", bufSz);
+ exit(2);
+ }
+ return newBuf;
+}
+
+void assertMem(size_t used, size_t total)
+{
+ if (used <= total)
+ return;
+ fprintf(stderr, "Buffer size %zu is not big enough to complete operation; we need %zu bytes\n", total, used);
+ exit(2);
+}
+
+// read file of known size, make sure the size is correct
+bool readFile(void *dst, uint32_t len, const char *fileName)
+{
+ FILE *f = fopen(fileName, "rb");
+ bool ret = false;
+
+ if (!f)
+ return false;
+
+ if (len != fread(dst, 1, len, f))
+ goto out;
+
+ if (fread(&len, 1, 1, f)) //make sure file is actually over
+ goto out;
+
+ ret = true;
+
+out:
+ fclose(f);
+ return ret;
+}
+
+// read complete file of unknown size, return malloced buffer and size
+void *loadFile(const char *fileName, uint32_t *size)
+{
+ FILE *f = fopen(fileName, "rb");
+ uint8_t *dst = NULL;
+ uint32_t len = 0, grow = 16384, total = 0;
+ uint32_t block;
+
+ if (!f) {
+ fprintf(stderr, "couldn't open %s: %s\n", fileName, strerror(errno));
+ exit(2);
+ }
+
+ do {
+ len += grow; dst = reallocOrDie(dst, len);
+
+ block = fread(dst + total, 1, grow, f);
+ total += block;
+ } while (block == grow);
+
+ *size = total;
+ if (!feof(f)) {
+ fprintf(stderr, "Failed to read entire file %s: %s\n",
+ fileName, strerror(errno));
+ free(dst);
+ fclose(f);
+ dst = NULL;
+ exit(2);
+ }
+
+ return dst;
+}
+
+static void doPrintHash(FILE *out, const char *pfx, const uint32_t *hash, size_t size, int increment)
+{
+ size_t i;
+ int pos;
+ fprintf(out, "%s: ", pfx);
+ for (i = 0, pos = 0; i < size; ++i, pos += increment)
+ fprintf(out, "%08" PRIx32, hash[pos]);
+ fprintf(out, "\n");
+}
+
+void printHash(FILE *out, const char *pfx, const uint32_t *hash, size_t size)
+{
+ doPrintHash(out, pfx, hash, size, 1);
+}
+
+void printHashRev(FILE *out, const char *pfx, const uint32_t *hash, size_t size)
+{
+ doPrintHash(out, pfx, hash + size - 1, size, -1);
+}
diff --git a/firmware/src/rsa.c b/lib/nanohub/rsa.c
similarity index 99%
rename from firmware/src/rsa.c
rename to lib/nanohub/rsa.c
index 3382b72..d718de0 100644
--- a/firmware/src/rsa.c
+++ b/lib/nanohub/rsa.c
@@ -17,7 +17,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
-#include <rsa.h>
+#include <nanohub/rsa.h>
static bool biModIterative(uint32_t *num, const uint32_t *denum, uint32_t *tmp, uint32_t *state1, uint32_t *state2, uint32_t step)
diff --git a/firmware/src/sha2.c b/lib/nanohub/sha2.c
similarity index 99%
rename from firmware/src/sha2.c
rename to lib/nanohub/sha2.c
index cc6e76b..58ce606 100644
--- a/firmware/src/sha2.c
+++ b/lib/nanohub/sha2.c
@@ -15,7 +15,7 @@
*/
#include <string.h>
-#include <sha2.h>
+#include <nanohub/sha2.h>
void sha2init(struct Sha2state *state)
diff --git a/firmware/src/softcrc.c b/lib/nanohub/softcrc.c
similarity index 98%
rename from firmware/src/softcrc.c
rename to lib/nanohub/softcrc.c
index 4a37f7e..fb2b711 100644
--- a/firmware/src/softcrc.c
+++ b/lib/nanohub/softcrc.c
@@ -15,7 +15,7 @@
*/
#include <stdint.h>
-#include <crc.h>
+#include <nanohub/crc.h>
/* this implements crc32 as crc.h defines it. It is not a normal CRC by any measure, so be careful with it */
diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk
new file mode 100644
index 0000000..f0ae715
--- /dev/null
+++ b/sensorhal/Android.mk
@@ -0,0 +1,139 @@
+# Copyright (C) 2015 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+#
+# Nanohub sensor HAL usage instructions:
+#
+# Add the following to your device.mk file.
+#
+# # Enable the nanohub sensor HAL
+# TARGET_USES_NANOHUB_SENSORHAL := true
+#
+# # Nanohub sensor list source file
+# NANOHUB_SENSORHAL_SENSORLIST := $(LOCAL_PATH)/sensorhal/sensorlist.cpp
+#
+# # Sensor HAL name override (optional)
+# NANOHUB_SENSORHAL_NAME_OVERRIDE := sensors.nanohub
+#
+# # Enable lid-state reporting (optional)
+# NANOHUB_SENSORHAL_LID_STATE_ENABLED := true
+#
+# # Enable mag-bias reporting (optional)
+# NANOHUB_SENSORHAL_USB_MAG_BIAS_ENABLED := true
+#
+
+LOCAL_PATH := $(call my-dir)
+
+ifeq ($(TARGET_USES_NANOHUB_SENSORHAL), true)
+
+COMMON_CFLAGS := -Wall -Werror -Wextra
+
+################################################################################
+
+include $(CLEAR_VARS)
+
+ifeq ($(NANOHUB_SENSORHAL_NAME_OVERRIDE),)
+LOCAL_MODULE := sensors.$(TARGET_DEVICE)
+else
+LOCAL_MODULE := $(NANOHUB_SENSORHAL_NAME_OVERRIDE)
+endif
+
+LOCAL_MODULE_RELATIVE_PATH := hw
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+
+LOCAL_CFLAGS += $(COMMON_CFLAGS)
+
+LOCAL_C_INCLUDES += \
+ device/google/contexthub/firmware/inc \
+ device/google/contexthub/util/common
+
+LOCAL_SRC_FILES := \
+ sensors.cpp \
+ ../../../../$(NANOHUB_SENSORHAL_SENSORLIST)
+
+LOCAL_SHARED_LIBRARIES := \
+ libcutils \
+ libhubconnection \
+ libstagefright_foundation \
+ libutils
+
+include $(BUILD_SHARED_LIBRARY)
+
+################################################################################
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := activity_recognition.$(TARGET_DEVICE)
+LOCAL_MODULE_RELATIVE_PATH := hw
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+
+LOCAL_CFLAGS += $(COMMON_CFLAGS)
+
+LOCAL_C_INCLUDES += \
+ device/google/contexthub/firmware/inc \
+ device/google/contexthub/util/common
+
+LOCAL_SRC_FILES := \
+ activity.cpp
+
+LOCAL_SHARED_LIBRARIES := \
+ libcutils \
+ libhubconnection \
+ liblog \
+ libstagefright_foundation \
+ libutils
+
+include $(BUILD_SHARED_LIBRARY)
+
+################################################################################
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := libhubconnection
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+
+LOCAL_CFLAGS += $(COMMON_CFLAGS)
+
+ifeq ($(NANOHUB_SENSORHAL_LID_STATE_ENABLED), true)
+LOCAL_CFLAGS += -DLID_STATE_REPORTING_ENABLED
+endif
+
+ifeq ($(NANOHUB_SENSORHAL_USB_MAG_BIAS_ENABLED), true)
+LOCAL_CFLAGS += -DUSB_MAG_BIAS_REPORTING_ENABLED
+endif
+
+LOCAL_C_INCLUDES += \
+ device/google/contexthub/firmware/inc \
+ device/google/contexthub/util/common
+
+LOCAL_SRC_FILES := \
+ hubconnection.cpp \
+ ../util/common/file.cpp \
+ ../util/common/JSONObject.cpp \
+ ../util/common/ring.cpp
+
+LOCAL_SHARED_LIBRARIES := \
+ libcutils \
+ liblog \
+ libstagefright_foundation \
+ libutils
+
+include $(BUILD_SHARED_LIBRARY)
+
+################################################################################
+
+endif
diff --git a/sensorhal/activity.cpp b/sensorhal/activity.cpp
new file mode 100644
index 0000000..dda15a0
--- /dev/null
+++ b/sensorhal/activity.cpp
@@ -0,0 +1,367 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_TAG "ActivityRecognitionHAL"
+//#define LOG_NDEBUG 0
+#include <utils/Log.h>
+
+#include "activity.h"
+
+#include <media/stagefright/foundation/ADebug.h>
+
+using namespace android;
+
+static const int kVersionMajor = 1;
+static const int kVersionMinor = 0;
+
+static const int ACTIVITY_TYPE_TILTING_INDEX = 6;
+
+static const char *const kActivityList[] = {
+ ACTIVITY_TYPE_IN_VEHICLE,
+ ACTIVITY_TYPE_ON_BICYCLE,
+ ACTIVITY_TYPE_WALKING,
+ ACTIVITY_TYPE_RUNNING,
+ ACTIVITY_TYPE_STILL,
+ "com.google.android.contexthub.ar.inconsistent",
+ ACTIVITY_TYPE_TILTING
+};
+
+ActivityContext::ActivityContext(const struct hw_module_t *module)
+ : mHubConnection(HubConnection::getInstance()),
+ mHubAlive(true),
+ mCallback(NULL),
+ mPrevActivity(-1),
+ mInitExitDone(false) {
+ memset(&device, 0, sizeof(device));
+
+ device.common.tag = HARDWARE_DEVICE_TAG;
+ device.common.version = ACTIVITY_RECOGNITION_API_VERSION_0_1;
+ device.common.module = const_cast<hw_module_t *>(module);
+ device.common.close = CloseWrapper;
+ device.register_activity_callback = RegisterActivityCallbackWrapper;
+ device.enable_activity_event = EnableActivityEventWrapper;
+ device.disable_activity_event = DisableActivityEventWrapper;
+ device.flush = FlushWrapper;
+
+ if (mHubConnection->initCheck() != (status_t)OK) {
+ mHubAlive = false;
+ } else {
+ if (mHubConnection->getAliveCheck() != (status_t)OK) {
+ mHubAlive = false;
+ } else {
+ mHubConnection->setActivityCallback(
+ this, &ActivityContext::HubCallbackWrapper);
+
+ mHubConnection->queueActivate(COMMS_SENSOR_ACTIVITY, false /* enable */);
+ }
+ }
+}
+
+ActivityContext::~ActivityContext() {
+ mHubConnection->setActivityCallback(NULL, NULL);
+}
+
+int ActivityContext::close() {
+ ALOGI("close");
+
+ delete this;
+
+ return 0;
+}
+
+void ActivityContext::onActivityEvent(
+ uint64_t when_us, bool is_flush, float x, float, float) {
+ Mutex::Autolock autoLock(mLock);
+
+ if (!mCallback) {
+ return;
+ }
+
+ if (is_flush) {
+ activity_event_t ev;
+ memset(&ev, 0, sizeof(ev));
+
+ ev.event_type = ACTIVITY_EVENT_FLUSH_COMPLETE;
+ ev.activity = 0;
+ ev.timestamp = 0ll;
+
+ (*mCallback->activity_callback)(mCallback, &ev, 1);
+ return;
+ }
+
+ int activityRaw = (int)x;
+
+ ALOGV("activityRaw = %d", activityRaw);
+
+ if (mPrevActivity >= 0 && mPrevActivity == activityRaw) {
+ // same old, same old...
+ return;
+ }
+
+ activity_event_t ev[8];
+ memset(&ev, 0, 8*sizeof(activity_event_t));
+ int num_events = 0;
+
+ // exit all other activities when first enabled.
+ if (!mInitExitDone) {
+ mInitExitDone = true;
+
+ int numActivities = sizeof(kActivityList) / sizeof(kActivityList[0]);
+ for (int i = 0; i < numActivities; ++i) {
+ if ((i == activityRaw) || !isEnabled(i, ACTIVITY_EVENT_EXIT)) {
+ continue;
+ }
+
+ activity_event_t *curr_ev = &ev[num_events];
+ curr_ev->event_type = ACTIVITY_EVENT_EXIT;
+ curr_ev->activity = i;
+ curr_ev->timestamp = when_us * 1000ll; // timestamp is in ns.
+ curr_ev->reserved[0] = curr_ev->reserved[1] = curr_ev->reserved[2] = curr_ev->reserved[3] = 0;
+ num_events++;
+ }
+ }
+
+ // tilt activities do not change the current activity type, but have a
+ // simultaneous enter and exit event type
+ if (activityRaw == ACTIVITY_TYPE_TILTING_INDEX) {
+ if (isEnabled(activityRaw, ACTIVITY_EVENT_ENTER)) {
+ activity_event_t *curr_ev = &ev[num_events];
+ curr_ev->event_type = ACTIVITY_EVENT_ENTER;
+ curr_ev->activity = activityRaw;
+ curr_ev->timestamp = when_us * 1000ll; // timestamp is in ns.
+ curr_ev->reserved[0] = curr_ev->reserved[1] = curr_ev->reserved[2] = curr_ev->reserved[3] = 0;
+ num_events++;
+ }
+
+ if (isEnabled(activityRaw, ACTIVITY_EVENT_EXIT)) {
+ activity_event_t *curr_ev = &ev[num_events];
+ curr_ev->event_type = ACTIVITY_EVENT_EXIT;
+ curr_ev->activity = activityRaw;
+ curr_ev->timestamp = when_us * 1000ll; // timestamp is in ns.
+ curr_ev->reserved[0] = curr_ev->reserved[1] = curr_ev->reserved[2] = curr_ev->reserved[3] = 0;
+ num_events++;
+ }
+ } else {
+ if ((mPrevActivity >= 0) &&
+ (isEnabled(mPrevActivity, ACTIVITY_EVENT_EXIT))) {
+ activity_event_t *curr_ev = &ev[num_events];
+ curr_ev->event_type = ACTIVITY_EVENT_EXIT;
+ curr_ev->activity = mPrevActivity;
+ curr_ev->timestamp = when_us * 1000ll; // timestamp is in ns.
+ curr_ev->reserved[0] = curr_ev->reserved[1] = curr_ev->reserved[2] = curr_ev->reserved[3] = 0;
+ num_events++;
+ }
+
+ if (isEnabled(activityRaw, ACTIVITY_EVENT_ENTER)) {
+ activity_event_t *curr_ev = &ev[num_events];
+ curr_ev->event_type = ACTIVITY_EVENT_ENTER;
+ curr_ev->activity = activityRaw;
+ curr_ev->timestamp = when_us * 1000ll; // timestamp is in ns.
+ curr_ev->reserved[0] = curr_ev->reserved[1] = curr_ev->reserved[2] = curr_ev->reserved[3] = 0;
+ num_events++;
+ }
+
+ mPrevActivity = activityRaw;
+ }
+
+ if (num_events > 0) {
+ (*mCallback->activity_callback)(mCallback, ev, num_events);
+ }
+}
+
+void ActivityContext::registerActivityCallback(
+ const activity_recognition_callback_procs_t *callback) {
+ ALOGI("registerActivityCallback");
+
+ Mutex::Autolock autoLock(mLock);
+ mCallback = callback;
+}
+
+int ActivityContext::enableActivityEvent(
+ uint32_t activity_handle,
+ uint32_t event_type,
+ int64_t max_batch_report_latency_ns) {
+ ALOGI("enableActivityEvent");
+
+ bool wasEnabled = !mMaxBatchReportLatencyNs.isEmpty();
+ int64_t prev_latency = calculateReportLatencyNs();
+
+ ALOGD_IF(DEBUG_ACTIVITY_RECOGNITION, "ACTVT type = %u, latency = %d sec", (unsigned) event_type,
+ (int)(max_batch_report_latency_ns/1000000000ull));
+
+ mMaxBatchReportLatencyNs.add(
+ ((uint64_t)activity_handle << 32) | event_type,
+ max_batch_report_latency_ns);
+
+ if (!wasEnabled) {
+ mPrevActivity = -1;
+ mInitExitDone = false;
+
+ mHubConnection->queueBatch(
+ COMMS_SENSOR_ACTIVITY, SENSOR_FLAG_ON_CHANGE_MODE, 1000000, max_batch_report_latency_ns);
+ mHubConnection->queueActivate(COMMS_SENSOR_ACTIVITY, true /* enable */);
+ } else if (max_batch_report_latency_ns != prev_latency) {
+ mHubConnection->queueBatch(
+ COMMS_SENSOR_ACTIVITY, SENSOR_FLAG_ON_CHANGE_MODE, 1000000, max_batch_report_latency_ns);
+ }
+
+ return 0;
+}
+
+int64_t ActivityContext::calculateReportLatencyNs() {
+ int64_t ret = INT64_MAX;
+
+ for (size_t i = 0 ; i < mMaxBatchReportLatencyNs.size(); ++i) {
+ if (mMaxBatchReportLatencyNs[i] <ret) {
+ ret = mMaxBatchReportLatencyNs[i];
+ }
+ }
+ return ret;
+}
+
+int ActivityContext::disableActivityEvent(
+ uint32_t activity_handle, uint32_t event_type) {
+ ALOGI("disableActivityEvent");
+
+ bool wasEnabled = !mMaxBatchReportLatencyNs.isEmpty();
+
+ mMaxBatchReportLatencyNs.removeItem(
+ ((uint64_t)activity_handle << 32) | event_type);
+
+ bool isEnabled = !mMaxBatchReportLatencyNs.isEmpty();
+
+ if (wasEnabled && !isEnabled) {
+ mHubConnection->queueActivate(COMMS_SENSOR_ACTIVITY, false /* enable */);
+ }
+
+ return 0;
+}
+
+bool ActivityContext::isEnabled(
+ uint32_t activity_handle, uint32_t event_type) const {
+ return mMaxBatchReportLatencyNs.indexOfKey(
+ ((uint64_t)activity_handle << 32) | event_type) >= 0;
+}
+
+int ActivityContext::flush() {
+ mHubConnection->queueFlush(COMMS_SENSOR_ACTIVITY);
+ return 0;
+}
+
+// static
+int ActivityContext::CloseWrapper(struct hw_device_t *dev) {
+ return reinterpret_cast<ActivityContext *>(dev)->close();
+}
+
+// static
+void ActivityContext::RegisterActivityCallbackWrapper(
+ const struct activity_recognition_device *dev,
+ const activity_recognition_callback_procs_t *callback) {
+ const_cast<ActivityContext *>(
+ reinterpret_cast<const ActivityContext *>(dev))
+ ->registerActivityCallback(callback);
+}
+
+// static
+int ActivityContext::EnableActivityEventWrapper(
+ const struct activity_recognition_device *dev,
+ uint32_t activity_handle,
+ uint32_t event_type,
+ int64_t max_batch_report_latency_ns) {
+ return const_cast<ActivityContext *>(
+ reinterpret_cast<const ActivityContext *>(dev))
+ ->enableActivityEvent(
+ activity_handle, event_type, max_batch_report_latency_ns);
+}
+
+// static
+int ActivityContext::DisableActivityEventWrapper(
+ const struct activity_recognition_device *dev,
+ uint32_t activity_handle,
+ uint32_t event_type) {
+ return const_cast<ActivityContext *>(
+ reinterpret_cast<const ActivityContext *>(dev))
+ ->disableActivityEvent(activity_handle, event_type);
+}
+
+// static
+int ActivityContext::FlushWrapper(
+ const struct activity_recognition_device *dev) {
+ return const_cast<ActivityContext *>(
+ reinterpret_cast<const ActivityContext *>(dev))->flush();
+}
+
+// static
+void ActivityContext::HubCallbackWrapper(
+ void *me, uint64_t time_ms, bool is_flush, float x, float y, float z) {
+ static_cast<ActivityContext *>(me)->onActivityEvent(time_ms, is_flush, x, y, z);
+}
+
+bool ActivityContext::getHubAlive() {
+ return mHubAlive;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+static bool gHubAlive = false;
+
+static int open_activity(
+ const struct hw_module_t *module,
+ const char *,
+ struct hw_device_t **dev) {
+ ALOGI("open_activity");
+
+ ActivityContext *ctx = new ActivityContext(module);
+
+ gHubAlive = ctx->getHubAlive();
+ *dev = &ctx->device.common;
+
+ return 0;
+}
+
+static struct hw_module_methods_t activity_module_methods = {
+ .open = open_activity
+};
+
+static int get_activity_list(
+ struct activity_recognition_module *,
+ char const* const **activity_list) {
+ ALOGI("get_activity_list");
+
+ if (gHubAlive) {
+ *activity_list = kActivityList;
+ return sizeof(kActivityList) / sizeof(kActivityList[0]);
+ } else {
+ *activity_list = {};
+ return 0;
+ }
+}
+
+struct activity_recognition_module HAL_MODULE_INFO_SYM = {
+ .common = {
+ .tag = HARDWARE_MODULE_TAG,
+ .version_major = kVersionMajor,
+ .version_minor = kVersionMinor,
+ .id = ACTIVITY_RECOGNITION_HARDWARE_MODULE_ID,
+ .name = "Google Activity Recognition module",
+ .author = "Google",
+ .methods = &activity_module_methods,
+ .dso = NULL,
+ .reserved = {0},
+ },
+ .get_supported_activities_list = get_activity_list,
+};
+
diff --git a/sensorhal/activity.h b/sensorhal/activity.h
new file mode 100644
index 0000000..347c5da
--- /dev/null
+++ b/sensorhal/activity.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ACTIVITY_H_
+
+#define ACTIVITY_H_
+
+#include <hardware/activity_recognition.h>
+#include <media/stagefright/foundation/ABase.h>
+#include <utils/KeyedVector.h>
+
+#include "hubconnection.h"
+
+#define DEBUG_ACTIVITY_RECOGNITION 0
+
+struct ActivityContext {
+ activity_recognition_device_t device;
+
+ explicit ActivityContext(const struct hw_module_t *module);
+
+ void onActivityEvent(
+ uint64_t when_us, bool is_flush, float x, float y, float z);
+
+ bool getHubAlive();
+
+private:
+ android::Mutex mLock;
+
+ android::sp<android::HubConnection> mHubConnection;
+
+ bool mHubAlive;
+
+ const activity_recognition_callback_procs_t *mCallback;
+
+ android::KeyedVector<uint64_t, int64_t> mMaxBatchReportLatencyNs;
+
+ int mPrevActivity;
+
+ bool mInitExitDone;
+
+ ~ActivityContext();
+
+ int close();
+
+ void registerActivityCallback(
+ const activity_recognition_callback_procs_t *callback);
+
+ bool isEnabled(uint32_t activity_handle, uint32_t event_type) const;
+
+ int enableActivityEvent(
+ uint32_t activity_handle,
+ uint32_t event_type,
+ int64_t max_batch_report_latency_ns);
+
+ int disableActivityEvent(uint32_t activity_handle, uint32_t event_type);
+
+ int flush();
+
+ int64_t calculateReportLatencyNs();
+
+ static int CloseWrapper(struct hw_device_t *dev);
+
+ static void RegisterActivityCallbackWrapper(
+ const struct activity_recognition_device *dev,
+ const activity_recognition_callback_procs_t *callback);
+
+ static int EnableActivityEventWrapper(
+ const struct activity_recognition_device *dev,
+ uint32_t activity_handle,
+ uint32_t event_type,
+ int64_t max_batch_report_latency_ns);
+
+ static int DisableActivityEventWrapper(
+ const struct activity_recognition_device *dev,
+ uint32_t activity_handle,
+ uint32_t event_type);
+
+ static int FlushWrapper(const struct activity_recognition_device *dev);
+
+ static void HubCallbackWrapper(
+ void *me, uint64_t time_ms, bool is_flush, float x, float y, float z);
+
+ DISALLOW_EVIL_CONSTRUCTORS(ActivityContext);
+};
+
+#endif // ACTIVITY_H_
+
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
new file mode 100644
index 0000000..b49644b
--- /dev/null
+++ b/sensorhal/hubconnection.cpp
@@ -0,0 +1,1230 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "hubconnection.h"
+#include "eventnums.h"
+#include "sensType.h"
+
+#define LOG_TAG "nanohub"
+#include <utils/Log.h>
+#include <utils/SystemClock.h>
+
+#include "file.h"
+#include "JSONObject.h"
+
+#include <errno.h>
+#include <unistd.h>
+#include <math.h>
+#include <inttypes.h>
+
+#include <cutils/properties.h>
+#include <linux/input.h>
+#include <linux/uinput.h>
+#include <media/stagefright/foundation/ADebug.h>
+#include <sys/inotify.h>
+
+#define APP_ID_GET_VENDOR(appid) ((appid) >> 24)
+#define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF))
+#define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl"
+#define APP_ID_APP_BMI160 2
+
+#define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType))
+
+#define NANOHUB_FILE_PATH "/dev/nanohub"
+#define NANOHUB_LOCK_DIR "/data/system/nanohub_lock"
+#define NANOHUB_LOCK_FILE NANOHUB_LOCK_DIR "/lock"
+#define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation"
+
+#define NANOHUB_LOCK_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR)
+
+#define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL
+#define SENSOR_RATE_ONESHOT 0xFFFFFF02UL
+
+#define MIN_MAG_SQ (10.0f * 10.0f)
+#define MAX_MAG_SQ (80.0f * 80.0f)
+
+#define ACCEL_RAW_KSCALE (8.0f * 9.81f / 32768.0f)
+
+#define OS_LOG_EVENT 0x474F4C41 // ascii: ALOG
+
+#ifdef LID_STATE_REPORTING_ENABLED
+const char LID_STATE_PROPERTY[] = "sensors.contexthub.lid_state";
+const char LID_STATE_UNKNOWN[] = "unknown";
+const char LID_STATE_OPEN[] = "open";
+const char LID_STATE_CLOSED[] = "closed";
+#endif // LID_STATE_REPORTING_ENABLED
+
+static const uint32_t delta_time_encoded = 1;
+static const uint32_t delta_time_shift_table[2] = {9, 0};
+
+namespace android {
+
+// static
+Mutex HubConnection::sInstanceLock;
+
+// static
+HubConnection *HubConnection::sInstance = NULL;
+
+HubConnection *HubConnection::getInstance()
+{
+ Mutex::Autolock autoLock(sInstanceLock);
+ if (sInstance == NULL) {
+ sInstance = new HubConnection;
+ }
+ return sInstance;
+}
+
+HubConnection::HubConnection()
+ : Thread(false /* canCallJava */),
+ mRing(10 *1024),
+ mActivityCbCookie(NULL),
+ mActivityCb(NULL),
+ mStepCounterOffset(0ull),
+ mLastStepCount(0ull)
+{
+ mMagBias[0] = mMagBias[1] = mMagBias[2] = 0.0f;
+ mMagAccuracy = SENSOR_STATUS_UNRELIABLE;
+ mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
+ mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
+
+ memset(&mSensorState, 0x00, sizeof(mSensorState));
+ mFd = open(NANOHUB_FILE_PATH, O_RDWR);
+ mPollFds[0].fd = mFd;
+ mPollFds[0].events = POLLIN;
+ mPollFds[0].revents = 0;
+ mNumPollFds = 1;
+
+ initNanohubLock();
+
+#ifdef USB_MAG_BIAS_REPORTING_ENABLED
+ mUsbMagBias = 0;
+ mMagBiasPollIndex = -1;
+ int magBiasFd = open(MAG_BIAS_FILE_PATH, O_RDONLY);
+ if (magBiasFd < 0) {
+ ALOGW("Mag bias file open failed: %s", strerror(errno));
+ } else {
+ mPollFds[mNumPollFds].fd = magBiasFd;
+ mPollFds[mNumPollFds].events = 0;
+ mPollFds[mNumPollFds].revents = 0;
+ mMagBiasPollIndex = mNumPollFds;
+ mNumPollFds++;
+ }
+#endif // USB_MAG_BIAS_REPORTING_ENABLED
+
+ mSensorState[COMMS_SENSOR_ACCEL].sensorType = SENS_TYPE_ACCEL;
+ mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO;
+ mSensorState[COMMS_SENSOR_GYRO].alt = COMMS_SENSOR_GYRO_UNCALIBRATED;
+ mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO;
+ mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt = COMMS_SENSOR_GYRO;
+ mSensorState[COMMS_SENSOR_MAG].sensorType = SENS_TYPE_MAG;
+ mSensorState[COMMS_SENSOR_MAG].alt = COMMS_SENSOR_MAG_UNCALIBRATED;
+ mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].sensorType = SENS_TYPE_MAG;
+ mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt = COMMS_SENSOR_MAG;
+ mSensorState[COMMS_SENSOR_LIGHT].sensorType = SENS_TYPE_ALS;
+ mSensorState[COMMS_SENSOR_PROXIMITY].sensorType = SENS_TYPE_PROX;
+ mSensorState[COMMS_SENSOR_PRESSURE].sensorType = SENS_TYPE_BARO;
+ mSensorState[COMMS_SENSOR_TEMPERATURE].sensorType = SENS_TYPE_TEMP;
+ mSensorState[COMMS_SENSOR_ORIENTATION].sensorType = SENS_TYPE_ORIENTATION;
+ mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].sensorType = SENS_TYPE_WIN_ORIENTATION;
+ mSensorState[COMMS_SENSOR_WINDOW_ORIENTATION].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_STEP_DETECTOR].sensorType = SENS_TYPE_STEP_DETECT;
+ mSensorState[COMMS_SENSOR_STEP_DETECTOR].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_STEP_COUNTER].sensorType = SENS_TYPE_STEP_COUNT;
+ mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].sensorType = SENS_TYPE_SIG_MOTION;
+ mSensorState[COMMS_SENSOR_SIGNIFICANT_MOTION].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[COMMS_SENSOR_GRAVITY].sensorType = SENS_TYPE_GRAVITY;
+ mSensorState[COMMS_SENSOR_LINEAR_ACCEL].sensorType = SENS_TYPE_LINEAR_ACCEL;
+ mSensorState[COMMS_SENSOR_ROTATION_VECTOR].sensorType = SENS_TYPE_ROTATION_VECTOR;
+ mSensorState[COMMS_SENSOR_GEO_MAG].sensorType = SENS_TYPE_GEO_MAG_ROT_VEC;
+ mSensorState[COMMS_SENSOR_GAME_ROTATION_VECTOR].sensorType = SENS_TYPE_GAME_ROT_VECTOR;
+ mSensorState[COMMS_SENSOR_HALL].sensorType = SENS_TYPE_HALL;
+ mSensorState[COMMS_SENSOR_HALL].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_SYNC].sensorType = SENS_TYPE_VSYNC;
+ mSensorState[COMMS_SENSOR_SYNC].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_ACTIVITY].sensorType = SENS_TYPE_ACTIVITY;
+ mSensorState[COMMS_SENSOR_ACTIVITY].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_TILT].sensorType = SENS_TYPE_TILT;
+ mSensorState[COMMS_SENSOR_TILT].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_GESTURE].sensorType = SENS_TYPE_GESTURE;
+ mSensorState[COMMS_SENSOR_GESTURE].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[COMMS_SENSOR_DOUBLE_TWIST].sensorType = SENS_TYPE_DOUBLE_TWIST;
+ mSensorState[COMMS_SENSOR_DOUBLE_TWIST].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[COMMS_SENSOR_DOUBLE_TAP].sensorType = SENS_TYPE_DOUBLE_TAP;
+ mSensorState[COMMS_SENSOR_DOUBLE_TAP].rate = SENSOR_RATE_ONCHANGE;
+
+#ifdef LID_STATE_REPORTING_ENABLED
+ initializeUinputNode();
+
+ // set initial lid state
+ if (property_set(LID_STATE_PROPERTY, LID_STATE_UNKNOWN) < 0) {
+ ALOGE("could not set lid_state property");
+ }
+
+ // enable hall sensor for folio
+ if (mFd >= 0) {
+ queueActivate(COMMS_SENSOR_HALL, true /* enable */);
+ }
+#endif // LID_STATE_REPORTING_ENABLED
+}
+
+HubConnection::~HubConnection()
+{
+ close(mFd);
+}
+
+void HubConnection::onFirstRef()
+{
+ run("HubConnection", PRIORITY_URGENT_DISPLAY);
+}
+
+status_t HubConnection::initCheck() const
+{
+ return mFd < 0 ? UNKNOWN_ERROR : OK;
+}
+
+status_t HubConnection::getAliveCheck()
+{
+ return OK;
+}
+
+static sp<JSONObject> readSettings(File *file) {
+ off64_t size = file->seekTo(0, SEEK_END);
+ file->seekTo(0, SEEK_SET);
+
+ sp<JSONObject> root;
+
+ if (size > 0) {
+ char *buf = (char *)malloc(size);
+ CHECK_EQ(file->read(buf, size), (ssize_t)size);
+ file->seekTo(0, SEEK_SET);
+
+ sp<JSONCompound> in = JSONCompound::Parse(buf, size);
+ free(buf);
+ buf = NULL;
+
+ if (in != NULL && in->isObject()) {
+ root = (JSONObject *)in.get();
+ }
+ }
+
+ if (root == NULL) {
+ root = new JSONObject;
+ }
+
+ return root;
+}
+
+static bool getCalibrationInt32(
+ const sp<JSONObject> &settings, const char *key, int32_t *out,
+ size_t numArgs) {
+ sp<JSONArray> array;
+ if (!settings->getArray(key, &array)) {
+ return false;
+ } else {
+ for (size_t i = 0; i < numArgs; i++) {
+ if (!array->getInt32(i, &out[i])) {
+ return false;
+ }
+ }
+ }
+ return true;
+}
+
+static bool getCalibrationFloat(
+ const sp<JSONObject> &settings, const char *key, float out[3]) {
+ sp<JSONArray> array;
+ if (!settings->getArray(key, &array)) {
+ return false;
+ } else {
+ for (size_t i = 0; i < 3; i++) {
+ if (!array->getFloat(i, &out[i])) {
+ return false;
+ }
+ }
+ }
+ return true;
+}
+
+static void loadSensorSettings(sp<JSONObject>* settings,
+ sp<JSONObject>* saved_settings) {
+ File settings_file(CONTEXTHUB_SETTINGS_PATH, "r");
+ File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "r");
+
+ status_t err;
+ if ((err = settings_file.initCheck()) != OK) {
+ ALOGE("settings file open failed: %d (%s)",
+ err,
+ strerror(-err));
+
+ *settings = new JSONObject;
+ } else {
+ *settings = readSettings(&settings_file);
+ }
+
+ if ((err = saved_settings_file.initCheck()) != OK) {
+ ALOGE("saved settings file open failed: %d (%s)",
+ err,
+ strerror(-err));
+ *saved_settings = new JSONObject;
+ } else {
+ *saved_settings = readSettings(&saved_settings_file);
+ }
+}
+
+void HubConnection::saveSensorSettings() const {
+ File saved_settings_file(CONTEXTHUB_SAVED_SETTINGS_PATH, "w");
+
+ status_t err;
+ if ((err = saved_settings_file.initCheck()) != OK) {
+ ALOGE("saved settings file open failed %d (%s)",
+ err,
+ strerror(-err));
+ return;
+ }
+
+ // Build a settings object.
+ sp<JSONArray> magArray = new JSONArray;
+#ifdef USB_MAG_BIAS_REPORTING_ENABLED
+ magArray->addFloat(mMagBias[0] + mUsbMagBias);
+#else
+ magArray->addFloat(mMagBias[0]);
+#endif // USB_MAG_BIAS_REPORTING_ENABLED
+ magArray->addFloat(mMagBias[1]);
+ magArray->addFloat(mMagBias[2]);
+
+ sp<JSONObject> settingsObject = new JSONObject;
+ settingsObject->setArray("mag", magArray);
+
+ // Write the JSON string to disk.
+ AString serializedSettings = settingsObject->toString();
+ size_t size = serializedSettings.size();
+ if ((err = saved_settings_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) {
+ ALOGE("saved settings file write failed %d (%s)",
+ err,
+ strerror(-err));
+ }
+}
+
+sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor)
+{
+ memset(ev, 0x00, sizeof(sensors_event_t));
+ ev->version = sizeof(sensors_event_t);
+ ev->timestamp = timestamp;
+ ev->type = type;
+ ev->sensor = sensor;
+
+ return ev;
+}
+
+void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct OneAxisSample *sample, __attribute__((unused)) bool highAccuracy)
+{
+ sensors_event_t nev[1];
+ int cnt = 0;
+
+ switch (sensor) {
+ case COMMS_SENSOR_ACTIVITY:
+ if (mActivityCb != NULL) {
+ (*mActivityCb)(mActivityCbCookie, timestamp / 1000ull,
+ false, /* is_flush */
+ (float)(sample->idata & 0x7), 0.0, 0.0);
+ }
+ break;
+ case COMMS_SENSOR_PRESSURE:
+ initEv(&nev[cnt++], timestamp, type, sensor)->pressure = sample->fdata;
+ break;
+ case COMMS_SENSOR_TEMPERATURE:
+ initEv(&nev[cnt++], timestamp, type, sensor)->temperature = sample->fdata;
+ break;
+ case COMMS_SENSOR_PROXIMITY:
+ initEv(&nev[cnt++], timestamp, type, sensor)->distance = sample->fdata;
+ break;
+ case COMMS_SENSOR_LIGHT:
+ initEv(&nev[cnt++], timestamp, type, sensor)->light = sample->fdata;
+ break;
+ case COMMS_SENSOR_STEP_COUNTER:
+ // We'll stash away the last step count in case we need to reset
+ // the hub. This last step count would then become the new offset.
+ mLastStepCount = mStepCounterOffset + sample->idata;
+ initEv(&nev[cnt++], timestamp, type, sensor)->u64.step_counter = mLastStepCount;
+ break;
+ case COMMS_SENSOR_STEP_DETECTOR:
+ case COMMS_SENSOR_SIGNIFICANT_MOTION:
+ case COMMS_SENSOR_TILT:
+ case COMMS_SENSOR_DOUBLE_TWIST:
+ initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = 1.0f;
+ break;
+ case COMMS_SENSOR_GESTURE:
+ case COMMS_SENSOR_SYNC:
+ initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata;
+ break;
+ case COMMS_SENSOR_HALL:
+#ifdef LID_STATE_REPORTING_ENABLED
+ sendFolioEvent(sample->idata);
+#endif // LID_STATE_REPORTING_ENABLED
+ break;
+ case COMMS_SENSOR_WINDOW_ORIENTATION:
+ initEv(&nev[cnt++], timestamp, type, sensor)->data[0] = sample->idata;
+ break;
+ default:
+ break;
+ }
+
+ if (cnt > 0)
+ mRing.write(nev, cnt);
+}
+
+void HubConnection::magAccuracyUpdate(float x, float y, float z)
+{
+ float magSq = x * x + y * y + z * z;
+
+ if (magSq < MIN_MAG_SQ || magSq > MAX_MAG_SQ) {
+ // save last good accuracy (either MEDIUM or HIGH)
+ if (mMagAccuracy != SENSOR_STATUS_UNRELIABLE)
+ mMagAccuracyRestore = mMagAccuracy;
+ mMagAccuracy = SENSOR_STATUS_UNRELIABLE;
+ } else if (mMagAccuracy == SENSOR_STATUS_UNRELIABLE) {
+ // restore
+ mMagAccuracy = mMagAccuracyRestore;
+ }
+}
+
+void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, __attribute__((unused)) bool highAccuracy)
+{
+ sensors_vec_t *sv;
+ sensors_event_t nev[2];
+ int cnt = 0;
+
+ switch (sensor) {
+ case COMMS_SENSOR_ACCEL:
+ sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
+ sv->x = sample->ix * ACCEL_RAW_KSCALE;
+ sv->y = sample->iy * ACCEL_RAW_KSCALE;
+ sv->z = sample->iz * ACCEL_RAW_KSCALE;
+ sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ break;
+ default:
+ break;
+ }
+
+ if (cnt > 0)
+ mRing.write(nev, cnt);
+}
+
+void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy)
+{
+ sensors_vec_t *sv;
+ uncalibrated_event_t *ue;
+ sensors_event_t *ev;
+ sensors_event_t nev[2];
+ static const float heading_accuracy = M_PI / 6.0f;
+ float w;
+ int cnt = 0;
+
+ switch (sensor) {
+ case COMMS_SENSOR_ACCEL:
+ sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ break;
+ case COMMS_SENSOR_GYRO:
+ if (mSensorState[sensor].enable) {
+ sv = &initEv(&nev[cnt++], timestamp, type, sensor)->gyro;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+ }
+
+ if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) {
+ ue = &initEv(&nev[cnt++], timestamp,
+ SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
+ COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
+ ue->x_uncalib = sample->x + mGyroBias[0];
+ ue->y_uncalib = sample->y + mGyroBias[1];
+ ue->z_uncalib = sample->z + mGyroBias[2];
+ ue->x_bias = mGyroBias[0];
+ ue->y_bias = mGyroBias[1];
+ ue->z_bias = mGyroBias[2];
+ }
+ break;
+ case COMMS_SENSOR_GYRO_BIAS:
+ mGyroBias[0] = sample->x;
+ mGyroBias[1] = sample->y;
+ mGyroBias[2] = sample->z;
+ break;
+ case COMMS_SENSOR_MAG:
+ magAccuracyUpdate(sample->x, sample->y, sample->z);
+
+ if (mSensorState[sensor].enable) {
+ sv = &initEv(&nev[cnt++], timestamp, type, sensor)->magnetic;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = mMagAccuracy;
+ }
+
+ if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
+ ue = &initEv(&nev[cnt++], timestamp,
+ SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+ COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
+ ue->x_uncalib = sample->x + mMagBias[0];
+ ue->y_uncalib = sample->y + mMagBias[1];
+ ue->z_uncalib = sample->z + mMagBias[2];
+ ue->x_bias = mMagBias[0];
+ ue->y_bias = mMagBias[1];
+ ue->z_bias = mMagBias[2];
+ }
+ break;
+ case COMMS_SENSOR_MAG_BIAS:
+ mMagAccuracy = highAccuracy ? SENSOR_STATUS_ACCURACY_HIGH : SENSOR_STATUS_ACCURACY_MEDIUM;
+ mMagBias[0] = sample->x;
+ mMagBias[1] = sample->y;
+ mMagBias[2] = sample->z;
+
+ saveSensorSettings();
+ break;
+ case COMMS_SENSOR_ORIENTATION:
+ case COMMS_SENSOR_LINEAR_ACCEL:
+ case COMMS_SENSOR_GRAVITY:
+ sv = &initEv(&nev[cnt++], timestamp, type, sensor)->orientation;
+ sv->x = sample->x;
+ sv->y = sample->y;
+ sv->z = sample->z;
+ sv->status = mMagAccuracy;
+ break;
+ case COMMS_SENSOR_DOUBLE_TAP:
+ ev = initEv(&nev[cnt++], timestamp, type, sensor);
+ ev->data[0] = sample->x;
+ ev->data[1] = sample->y;
+ ev->data[2] = sample->z;
+ break;
+ case COMMS_SENSOR_ROTATION_VECTOR:
+ ev = initEv(&nev[cnt++], timestamp, type, sensor);
+ w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z;
+ if (w < 1.0f)
+ w = sqrt(1.0f - w);
+ else
+ w = 0.0f;
+ ev->data[0] = sample->x;
+ ev->data[1] = sample->y;
+ ev->data[2] = sample->z;
+ ev->data[3] = w;
+ ev->data[4] = (4 - mMagAccuracy) * heading_accuracy;
+ break;
+ case COMMS_SENSOR_GEO_MAG:
+ case COMMS_SENSOR_GAME_ROTATION_VECTOR:
+ ev = initEv(&nev[cnt++], timestamp, type, sensor);
+ w = sample->x * sample->x + sample->y * sample->y + sample->z * sample->z;
+ if (w < 1.0f)
+ w = sqrt(1.0f - w);
+ else
+ w = 0.0f;
+ ev->data[0] = sample->x;
+ ev->data[1] = sample->y;
+ ev->data[2] = sample->z;
+ ev->data[3] = w;
+ break;
+ default:
+ break;
+ }
+
+ if (cnt > 0)
+ mRing.write(nev, cnt);
+}
+
+void HubConnection::discardInotifyEvent() {
+ // Read & discard an inotify event. We only use the presence of an event as
+ // a trigger to perform the file existence check (for simplicity)
+ if (mInotifyPollIndex >= 0) {
+ char buf[sizeof(struct inotify_event) + NAME_MAX + 1];
+ int ret = ::read(mPollFds[mInotifyPollIndex].fd, buf, sizeof(buf));
+ ALOGD("Discarded %d bytes of inotify data", ret);
+ }
+}
+
+void HubConnection::waitOnNanohubLock() {
+ if (mInotifyPollIndex < 0) {
+ return;
+ }
+ struct pollfd *pfd = &mPollFds[mInotifyPollIndex];
+
+ // While the lock file exists, poll on the inotify fd (with timeout)
+ while (access(NANOHUB_LOCK_FILE, F_OK) == 0) {
+ ALOGW("Nanohub is locked; blocking read thread");
+ int ret = poll(pfd, 1, 5000);
+ if ((ret > 0) && (pfd->revents & POLLIN)) {
+ discardInotifyEvent();
+ }
+ }
+}
+
+void HubConnection::restoreSensorState()
+{
+ Mutex::Autolock autoLock(mLock);
+
+ sendCalibrationOffsets();
+
+ for (int i = 0; i < NUM_COMMS_SENSORS_PLUS_1; i++) {
+ if (mSensorState[i].sensorType && mSensorState[i].enable) {
+ struct ConfigCmd cmd;
+
+ initConfigCmd(&cmd, i);
+
+ ALOGI("restoring: sensor=%d, handle=%d, enable=%d, period=%" PRId64 ", latency=%" PRId64,
+ cmd.sensorType, i, mSensorState[i].enable, frequency_q10_to_period_ns(mSensorState[i].rate),
+ mSensorState[i].latency);
+
+ int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ if (ret != sizeof(cmd)) {
+ ALOGE("failed to send config command to restore sensor %d\n", cmd.sensorType);
+ }
+
+ cmd.cmd = CONFIG_CMD_FLUSH;
+
+ for (int j = 0; j < mSensorState[i].flushCnt; j++) {
+ int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd)));
+ if (ret != sizeof(cmd)) {
+ ALOGE("failed to send flush command to sensor %d\n", cmd.sensorType);
+ }
+ }
+ }
+ }
+
+ mStepCounterOffset = mLastStepCount;
+}
+
+void HubConnection::postOsLog(uint8_t *buf, ssize_t len)
+{
+ // if len is less than 6, it's either an invalid or an empty log message.
+ if (len < 6)
+ return;
+
+ buf[len] = 0x00;
+ switch (buf[4]) {
+ case 'E':
+ ALOGE("osLog: %s", &buf[5]);
+ break;
+ case 'W':
+ ALOGW("osLog: %s", &buf[5]);
+ break;
+ case 'I':
+ ALOGI("osLog: %s", &buf[5]);
+ break;
+ case 'D':
+ ALOGD("osLog: %s", &buf[5]);
+ break;
+ default:
+ break;
+ }
+}
+
+ssize_t HubConnection::processBuf(uint8_t *buf, ssize_t len)
+{
+ struct nAxisEvent *data = (struct nAxisEvent *)buf;
+ uint32_t type, sensor, bias, currSensor;
+ int i, numSamples;
+ bool one, rawThree, three;
+ sensors_event_t ev;
+ uint64_t timestamp;
+ ssize_t ret = 0;
+
+ if (len >= 4) {
+ ret = sizeof(data->evtType);
+ one = three = rawThree = false;
+ bias = 0;
+ switch (data->evtType) {
+ case OS_LOG_EVENT:
+ postOsLog(buf, len);
+ return 0;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL):
+ type = SENSOR_TYPE_ACCELEROMETER;
+ sensor = COMMS_SENSOR_ACCEL;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ACCEL_RAW):
+ type = SENSOR_TYPE_ACCELEROMETER;
+ sensor = COMMS_SENSOR_ACCEL;
+ rawThree = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_GYRO):
+ type = SENSOR_TYPE_GYROSCOPE;
+ sensor = COMMS_SENSOR_GYRO;
+ bias = COMMS_SENSOR_GYRO_BIAS;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_MAG):
+ type = SENSOR_TYPE_MAGNETIC_FIELD;
+ sensor = COMMS_SENSOR_MAG;
+ bias = COMMS_SENSOR_MAG_BIAS;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ALS):
+ type = SENSOR_TYPE_LIGHT;
+ sensor = COMMS_SENSOR_LIGHT;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_PROX):
+ type = SENSOR_TYPE_PROXIMITY;
+ sensor = COMMS_SENSOR_PROXIMITY;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_BARO):
+ type = SENSOR_TYPE_PRESSURE;
+ sensor = COMMS_SENSOR_PRESSURE;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_TEMP):
+ type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
+ sensor = COMMS_SENSOR_TEMPERATURE;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ORIENTATION):
+ type = SENSOR_TYPE_ORIENTATION;
+ sensor = COMMS_SENSOR_ORIENTATION;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_WIN_ORIENTATION):
+ type = SENSOR_TYPE_DEVICE_ORIENTATION;
+ sensor = COMMS_SENSOR_WINDOW_ORIENTATION;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_DETECT):
+ type = SENSOR_TYPE_STEP_DETECTOR;
+ sensor = COMMS_SENSOR_STEP_DETECTOR;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_STEP_COUNT):
+ type = SENSOR_TYPE_STEP_COUNTER;
+ sensor = COMMS_SENSOR_STEP_COUNTER;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_SIG_MOTION):
+ type = SENSOR_TYPE_SIGNIFICANT_MOTION;
+ sensor = COMMS_SENSOR_SIGNIFICANT_MOTION;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_GRAVITY):
+ type = SENSOR_TYPE_GRAVITY;
+ sensor = COMMS_SENSOR_GRAVITY;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_LINEAR_ACCEL):
+ type = SENSOR_TYPE_LINEAR_ACCELERATION;
+ sensor = COMMS_SENSOR_LINEAR_ACCEL;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ROTATION_VECTOR):
+ type = SENSOR_TYPE_ROTATION_VECTOR;
+ sensor = COMMS_SENSOR_ROTATION_VECTOR;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_GEO_MAG_ROT_VEC):
+ type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+ sensor = COMMS_SENSOR_GEO_MAG;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_GAME_ROT_VECTOR):
+ type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
+ sensor = COMMS_SENSOR_GAME_ROTATION_VECTOR;
+ three = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_HALL):
+ type = 0;
+ sensor = COMMS_SENSOR_HALL;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_VSYNC):
+ type = SENSOR_TYPE_SYNC;
+ sensor = COMMS_SENSOR_SYNC;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_ACTIVITY):
+ type = 0;
+ sensor = COMMS_SENSOR_ACTIVITY;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_TILT):
+ type = SENSOR_TYPE_TILT_DETECTOR;
+ sensor = COMMS_SENSOR_TILT;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_GESTURE):
+ type = SENSOR_TYPE_PICK_UP_GESTURE;
+ sensor = COMMS_SENSOR_GESTURE;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TWIST):
+ type = SENSOR_TYPE_DOUBLE_TWIST;
+ sensor = COMMS_SENSOR_DOUBLE_TWIST;
+ one = true;
+ break;
+ case SENS_TYPE_TO_EVENT(SENS_TYPE_DOUBLE_TAP):
+ type = SENSOR_TYPE_DOUBLE_TAP;
+ sensor = COMMS_SENSOR_DOUBLE_TAP;
+ three = true;
+ break;
+ case EVT_RESET_REASON:
+ uint32_t resetReason;
+ memcpy(&resetReason, data->buffer, sizeof(resetReason));
+ ALOGI("Observed hub reset: 0x%08" PRIx32, resetReason);
+ restoreSensorState();
+ return 0;
+ default:
+ return 0;
+ }
+ }
+
+ if (len >= 16) {
+ ret += sizeof(data->referenceTime);
+ timestamp = data->referenceTime;
+ numSamples = data->firstSample.numSamples;
+ for (i=0; i<numSamples; i++) {
+ if (data->firstSample.biasPresent && data->firstSample.biasSample == i)
+ currSensor = bias;
+ else
+ currSensor = sensor;
+
+ if (one) {
+ if (i > 0)
+ timestamp += ((uint64_t)data->oneSamples[i].deltaTime) << delta_time_shift_table[data->oneSamples[i].deltaTime & delta_time_encoded];
+ processSample(timestamp, type, currSensor, &data->oneSamples[i], data->firstSample.highAccuracy);
+ ret += sizeof(data->oneSamples[i]);
+ } else if (rawThree) {
+ if (i > 0)
+ timestamp += ((uint64_t)data->rawThreeSamples[i].deltaTime) << delta_time_shift_table[data->rawThreeSamples[i].deltaTime & delta_time_encoded];
+ processSample(timestamp, type, currSensor, &data->rawThreeSamples[i], data->firstSample.highAccuracy);
+ ret += sizeof(data->rawThreeSamples[i]);
+ } else if (three) {
+ if (i > 0)
+ timestamp += ((uint64_t)data->threeSamples[i].deltaTime) << delta_time_shift_table[data->threeSamples[i].deltaTime & delta_time_encoded];
+ processSample(timestamp, type, currSensor, &data->threeSamples[i], data->firstSample.highAccuracy);
+ ret += sizeof(data->threeSamples[i]);
+ }
+ }
+
+ if (!numSamples)
+ ret += sizeof(data->firstSample);
+
+ for (i=0; i<data->firstSample.numFlushes; i++) {
+ if (sensor == COMMS_SENSOR_ACTIVITY) {
+ if (mActivityCb != NULL) {
+ (*mActivityCb)(mActivityCbCookie, 0ull, /* when_us */
+ true, /* is_flush */
+ 0.0f, 0.0f, 0.0f);
+ }
+ } else {
+ memset(&ev, 0x00, sizeof(sensors_event_t));
+ ev.version = META_DATA_VERSION;
+ ev.timestamp = 0;
+ ev.type = SENSOR_TYPE_META_DATA;
+ ev.sensor = 0;
+ ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
+ if (mSensorState[sensor].alt && mSensorState[mSensorState[sensor].alt].flushCnt > 0) {
+ mSensorState[mSensorState[sensor].alt].flushCnt --;
+ ev.meta_data.sensor = mSensorState[sensor].alt;
+ } else {
+ mSensorState[sensor].flushCnt --;
+ ev.meta_data.sensor = sensor;
+ }
+
+ mRing.write(&ev, 1);
+ ALOGI("flushing %d", ev.meta_data.sensor);
+ }
+ }
+ }
+
+ return ret;
+}
+
+void HubConnection::sendCalibrationOffsets()
+{
+ sp<JSONObject> settings;
+ sp<JSONObject> saved_settings;
+ int32_t accel[3], gyro[3], proximity, proximity_array[4];
+ float barometer, mag[3], light;
+
+ loadSensorSettings(&settings, &saved_settings);
+
+ if (getCalibrationInt32(settings, "accel", accel, 3))
+ queueDataInternal(COMMS_SENSOR_ACCEL, accel, sizeof(accel));
+
+ if (getCalibrationInt32(settings, "gyro", gyro, 3))
+ queueDataInternal(COMMS_SENSOR_GYRO, gyro, sizeof(gyro));
+
+ if (settings->getFloat("barometer", &barometer))
+ queueDataInternal(COMMS_SENSOR_PRESSURE, &barometer, sizeof(barometer));
+
+ if (settings->getInt32("proximity", &proximity))
+ queueDataInternal(COMMS_SENSOR_PROXIMITY, &proximity, sizeof(proximity));
+
+ if (getCalibrationInt32(settings, "proximity", proximity_array, 4))
+ queueDataInternal(COMMS_SENSOR_PROXIMITY, proximity_array, sizeof(proximity_array));
+
+ if (settings->getFloat("light", &light))
+ queueDataInternal(COMMS_SENSOR_LIGHT, &light, sizeof(light));
+
+ if (getCalibrationFloat(saved_settings, "mag", mag))
+ queueDataInternal(COMMS_SENSOR_MAG, mag, sizeof(mag));
+}
+
+bool HubConnection::threadLoop() {
+ ALOGI("threadLoop: starting");
+
+ if (mFd < 0) {
+ ALOGE("threadLoop: exiting prematurely: nanohub is unavailable");
+ return false;
+ }
+ waitOnNanohubLock();
+
+ sendCalibrationOffsets();
+
+ while (!Thread::exitPending()) {
+ ssize_t ret;
+
+ do {
+ ret = poll(mPollFds, mNumPollFds, -1);
+ } while (ret < 0 && errno == EINTR);
+
+ if (mInotifyPollIndex >= 0 && mPollFds[mInotifyPollIndex].revents & POLLIN) {
+ discardInotifyEvent();
+ waitOnNanohubLock();
+ }
+
+#ifdef USB_MAG_BIAS_REPORTING_ENABLED
+ if (mMagBiasPollIndex >= 0 && mPollFds[mMagBiasPollIndex].revents & POLLERR) {
+ // Read from mag bias file
+ char buf[16];
+ lseek(mPollFds[mMagBiasPollIndex].fd, 0, SEEK_SET);
+ ::read(mPollFds[mMagBiasPollIndex].fd, buf, 16);
+ float bias = atof(buf);
+ mUsbMagBias = bias;
+ queueUsbMagBias();
+ }
+#endif // USB_MAG_BIAS_REPORTING_ENABLED
+
+ if (mPollFds[0].revents & POLLIN) {
+ uint8_t recv[256];
+ ssize_t len = ::read(mFd, recv, sizeof(recv));
+
+ for (ssize_t offset = 0; offset < len;) {
+ ret = processBuf(recv + offset, len - offset);
+
+ if (ret > 0)
+ offset += ret;
+ else
+ break;
+ }
+ }
+ }
+
+ return false;
+}
+
+ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
+ return mRing.read(ev, size);
+}
+
+void HubConnection::setActivityCallback(
+ void *cookie,
+ void (*cb)(void *, uint64_t time_ms, bool, float x, float y, float z))
+{
+ Mutex::Autolock autoLock(mLock);
+ mActivityCbCookie = cookie;
+ mActivityCb = cb;
+}
+
+void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle)
+{
+ uint8_t alt = mSensorState[handle].alt;
+
+ memset(cmd, 0x00, sizeof(*cmd));
+
+ cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT;
+ cmd->sensorType = mSensorState[handle].sensorType;
+
+ if (alt && mSensorState[alt].enable && mSensorState[handle].enable) {
+ cmd->cmd = CONFIG_CMD_ENABLE;
+ if (mSensorState[alt].rate > mSensorState[handle].rate)
+ cmd->rate = mSensorState[alt].rate;
+ else
+ cmd->rate = mSensorState[handle].rate;
+ if (mSensorState[alt].latency < mSensorState[handle].latency)
+ cmd->latency = mSensorState[alt].latency;
+ else
+ cmd->latency = mSensorState[handle].latency;
+ } else if (alt && mSensorState[alt].enable) {
+ cmd->cmd = mSensorState[alt].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
+ cmd->rate = mSensorState[alt].rate;
+ cmd->latency = mSensorState[alt].latency;
+ } else { /* !alt || !mSensorState[alt].enable */
+ cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
+ cmd->rate = mSensorState[handle].rate;
+ cmd->latency = mSensorState[handle].latency;
+ }
+}
+
+void HubConnection::queueActivate(int handle, bool enable)
+{
+ struct ConfigCmd cmd;
+ int ret;
+
+ Mutex::Autolock autoLock(mLock);
+
+ if (mSensorState[handle].sensorType) {
+ mSensorState[handle].enable = enable;
+
+ initConfigCmd(&cmd, handle);
+
+ ALOGI("queueActivate: sensor=%d, handle=%d, enable=%d", cmd.sensorType, handle, enable);
+ do {
+ ret = write(mFd, &cmd, sizeof(cmd));
+ } while(ret != sizeof(cmd));
+ } else {
+ ALOGI("queueActivate: unhandled handle=%d, enable=%d", handle, enable);
+ }
+}
+
+void HubConnection::queueSetDelay(int handle, nsecs_t sampling_period_ns)
+{
+ struct ConfigCmd cmd;
+ int ret;
+
+ Mutex::Autolock autoLock(mLock);
+
+ if (mSensorState[handle].sensorType) {
+ if (sampling_period_ns > 0 &&
+ mSensorState[handle].rate != SENSOR_RATE_ONCHANGE &&
+ mSensorState[handle].rate != SENSOR_RATE_ONESHOT) {
+ mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns);
+ }
+
+ initConfigCmd(&cmd, handle);
+
+ ALOGI("queueSetDelay: sensor=%d, handle=%d, period=%" PRId64,
+ cmd.sensorType, handle, sampling_period_ns);
+ do {
+ ret = write(mFd, &cmd, sizeof(cmd));
+ } while(ret != sizeof(cmd));
+ } else {
+ ALOGI("queueSetDelay: unhandled handle=%d, period=%" PRId64, handle, sampling_period_ns);
+ }
+}
+
+void HubConnection::queueBatch(
+ int handle,
+ __attribute__((unused)) int flags,
+ nsecs_t sampling_period_ns,
+ nsecs_t max_report_latency_ns)
+{
+ struct ConfigCmd cmd;
+ int ret;
+
+ Mutex::Autolock autoLock(mLock);
+
+ if (mSensorState[handle].sensorType) {
+ if (sampling_period_ns > 0 &&
+ mSensorState[handle].rate != SENSOR_RATE_ONCHANGE &&
+ mSensorState[handle].rate != SENSOR_RATE_ONESHOT) {
+ mSensorState[handle].rate = period_ns_to_frequency_q10(sampling_period_ns);
+ }
+ mSensorState[handle].latency = max_report_latency_ns;
+
+ initConfigCmd(&cmd, handle);
+
+ ALOGI("queueBatch: sensor=%d, handle=%d, period=%" PRId64 ", latency=%" PRId64,
+ cmd.sensorType, handle, sampling_period_ns, max_report_latency_ns);
+ do {
+ ret = write(mFd, &cmd, sizeof(cmd));
+ } while(ret != sizeof(cmd));
+ } else {
+ ALOGI("queueBatch: unhandled handle=%d, period=%" PRId64 ", latency=%" PRId64,
+ handle, sampling_period_ns, max_report_latency_ns);
+ }
+}
+
+void HubConnection::queueFlush(int handle)
+{
+ struct ConfigCmd cmd;
+ int ret;
+
+ Mutex::Autolock autoLock(mLock);
+
+ if (mSensorState[handle].sensorType) {
+ mSensorState[handle].flushCnt++;
+
+ initConfigCmd(&cmd, handle);
+ cmd.cmd = CONFIG_CMD_FLUSH;
+
+ ALOGI("queueFlush: sensor=%d, handle=%d", cmd.sensorType, handle);
+ do {
+ ret = write(mFd, &cmd, sizeof(cmd));
+ } while(ret != sizeof(cmd));
+ } else {
+ ALOGI("queueFlush: unhandled handle=%d", handle);
+ }
+}
+
+void HubConnection::queueDataInternal(int handle, void *data, size_t length)
+{
+ struct ConfigCmd *cmd = (struct ConfigCmd *)malloc(sizeof(struct ConfigCmd) + length);
+ size_t ret;
+
+ if (cmd && mSensorState[handle].sensorType) {
+ initConfigCmd(cmd, handle);
+ memcpy(cmd->data, data, length);
+ cmd->cmd = CONFIG_CMD_CFG_DATA;
+
+ ALOGI("queueData: sensor=%d, length=%zu", cmd->sensorType, length);
+ do {
+ ret = write(mFd, cmd, sizeof(*cmd) + length);
+ } while(ret != sizeof(*cmd) + length);
+ free(cmd);
+ } else {
+ ALOGI("queueData: unhandled handle=%d", handle);
+ }
+}
+
+void HubConnection::queueData(int handle, void *data, size_t length)
+{
+ Mutex::Autolock autoLock(mLock);
+ queueDataInternal(handle, data, length);
+}
+
+void HubConnection::initNanohubLock() {
+ // Create the lock directory (if it doesn't already exist)
+ if (mkdir(NANOHUB_LOCK_DIR, NANOHUB_LOCK_DIR_PERMS) < 0 && errno != EEXIST) {
+ ALOGE("Couldn't create Nanohub lock directory: %s", strerror(errno));
+ return;
+ }
+
+ mInotifyPollIndex = -1;
+ int inotifyFd = inotify_init1(IN_NONBLOCK);
+ if (inotifyFd < 0) {
+ ALOGE("Couldn't initialize inotify: %s", strerror(errno));
+ } else if (inotify_add_watch(inotifyFd, NANOHUB_LOCK_DIR, IN_CREATE | IN_DELETE) < 0) {
+ ALOGE("Couldn't add inotify watch: %s", strerror(errno));
+ close(inotifyFd);
+ } else {
+ mPollFds[mNumPollFds].fd = inotifyFd;
+ mPollFds[mNumPollFds].events = POLLIN;
+ mPollFds[mNumPollFds].revents = 0;
+ mInotifyPollIndex = mNumPollFds;
+ mNumPollFds++;
+ }
+}
+
+#ifdef USB_MAG_BIAS_REPORTING_ENABLED
+void HubConnection::queueUsbMagBias()
+{
+ struct MsgCmd *cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(float));
+ size_t ret;
+
+ if (cmd) {
+ cmd->evtType = EVT_APP_FROM_HOST;
+ cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_BMI160);
+ cmd->msg.dataLen = sizeof(float);
+ memcpy((float *)(cmd+1), &mUsbMagBias, sizeof(float));
+
+ ALOGI("queueUsbMagBias: bias=%f\n", mUsbMagBias);
+ do {
+ ret = write(mFd, cmd, sizeof(*cmd) + sizeof(float));
+ } while(ret != sizeof(*cmd) + sizeof(float));
+ free(cmd);
+ }
+}
+#endif // USB_MAG_BIAS_REPORTING_ENABLED
+
+#ifdef LID_STATE_REPORTING_ENABLED
+status_t HubConnection::initializeUinputNode()
+{
+ int ret = 0;
+
+ // Open uinput dev node
+ mUinputFd = TEMP_FAILURE_RETRY(open("/dev/uinput", O_WRONLY | O_NONBLOCK));
+ if (mUinputFd < 0) {
+ ALOGE("could not open uinput node: %s", strerror(errno));
+ return UNKNOWN_ERROR;
+ }
+
+ // Enable SW_LID events
+ ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SW));
+ ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_EVBIT, EV_SYN));
+ ret |= TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_SET_SWBIT, SW_LID));
+ if (ret < 0) {
+ ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
+ return UNKNOWN_ERROR;
+ }
+
+ // Create uinput node for SW_LID
+ struct uinput_user_dev uidev;
+ memset(&uidev, 0, sizeof(uidev));
+ snprintf(uidev.name, UINPUT_MAX_NAME_SIZE, "uinput-folio");
+ uidev.id.bustype = BUS_SPI;
+ uidev.id.vendor = 0;
+ uidev.id.product = 0;
+ uidev.id.version = 0;
+
+ ret = TEMP_FAILURE_RETRY(write(mUinputFd, &uidev, sizeof(uidev)));
+ if (ret < 0) {
+ ALOGE("write to uinput node failed: %s", strerror(errno));
+ return UNKNOWN_ERROR;
+ }
+
+ ret = TEMP_FAILURE_RETRY(ioctl(mUinputFd, UI_DEV_CREATE));
+ if (ret < 0) {
+ ALOGE("could not send ioctl to uinput node: %s", strerror(errno));
+ return UNKNOWN_ERROR;
+ }
+
+ return OK;
+}
+
+void HubConnection::sendFolioEvent(int32_t data) {
+ ssize_t ret = 0;
+ struct input_event ev;
+
+ memset(&ev, 0, sizeof(ev));
+
+ ev.type = EV_SW;
+ ev.code = SW_LID;
+ ev.value = data;
+ ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
+ if (ret < 0) {
+ ALOGE("write to uinput node failed: %s", strerror(errno));
+ return;
+ }
+
+ // Force flush with EV_SYN event
+ ev.type = EV_SYN;
+ ev.code = SYN_REPORT;
+ ev.value = 0;
+ ret = TEMP_FAILURE_RETRY(write(mUinputFd, &ev, sizeof(ev)));
+ if (ret < 0) {
+ ALOGE("write to uinput node failed: %s", strerror(errno));
+ return;
+ }
+
+ // Set lid state property
+ if (property_set(LID_STATE_PROPERTY,
+ (data ? LID_STATE_CLOSED : LID_STATE_OPEN)) < 0) {
+ ALOGE("could not set lid_state property");
+ }
+}
+#endif // LID_STATE_REPORTING_ENABLED
+
+} // namespace android
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
new file mode 100644
index 0000000..00d2d9b
--- /dev/null
+++ b/sensorhal/hubconnection.h
@@ -0,0 +1,247 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef HUB_CONNECTION_H_
+
+#define HUB_CONNECTION_H_
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <poll.h>
+
+#include <utils/Errors.h>
+#include <utils/Mutex.h>
+#include <utils/Thread.h>
+
+#include "eventnums.h"
+#include "hubdefs.h"
+#include "ring.h"
+
+namespace android {
+
+struct HubConnection : public Thread {
+ static HubConnection *getInstance();
+
+ status_t initCheck() const;
+
+ enum ProximitySensorType {
+ PROXIMITY_UNKNOWN,
+ PROXIMITY_ROHM,
+ PROXIMITY_AMS,
+ };
+
+ // Blocks until it can return a status
+ status_t getAliveCheck();
+
+ virtual bool threadLoop();
+
+ void queueActivate(int handle, bool enable);
+ void queueSetDelay(int handle, nsecs_t delayNs);
+ void queueBatch(
+ int handle,
+ int flags,
+ nsecs_t sampling_period_ns,
+ nsecs_t max_report_latency_ns);
+ void queueFlush(int handle);
+ void queueData(int handle, void *data, size_t length);
+
+ ssize_t read(sensors_event_t *ev, size_t size);
+
+ typedef void (*ActivityFunc)(
+ void *, uint64_t time_us, bool is_flush, float x, float y, float z);
+
+ void setActivityCallback(void *cookie, ActivityFunc cb);
+
+ void saveSensorSettings() const;
+
+protected:
+ HubConnection();
+ virtual ~HubConnection();
+
+ virtual void onFirstRef();
+
+private:
+ typedef uint32_t rate_q10_t; // q10 means lower 10 bits are for fractions
+
+ static inline uint64_t period_ns_to_frequency_q10(nsecs_t period_ns) {
+ return 1024000000000ULL / period_ns;
+ }
+
+ static inline nsecs_t frequency_q10_to_period_ns(uint64_t frequency_q10) {
+ if (frequency_q10)
+ return 1024000000000LL / frequency_q10;
+ else
+ return (nsecs_t)0;
+ }
+
+ enum
+ {
+ CONFIG_CMD_DISABLE = 0,
+ CONFIG_CMD_ENABLE = 1,
+ CONFIG_CMD_FLUSH = 2,
+ CONFIG_CMD_CFG_DATA = 3,
+ CONFIG_CMD_CALIBRATE = 4,
+ };
+
+ struct ConfigCmd
+ {
+ uint32_t evtType;
+ uint64_t latency;
+ rate_q10_t rate;
+ uint8_t sensorType;
+ uint8_t cmd;
+ uint16_t flags;
+ uint8_t data[];
+ } __attribute__((packed));
+
+ struct MsgCmd
+ {
+ uint32_t evtType;
+ struct HostHubRawPacket msg;
+ } __attribute__((packed));
+
+ struct SensorState {
+ uint64_t latency;
+ rate_q10_t rate;
+ uint8_t sensorType;
+ uint8_t alt;
+ uint8_t flushCnt;
+ bool enable;
+ };
+
+ struct FirstSample
+ {
+ uint8_t numSamples;
+ uint8_t numFlushes;
+ uint8_t highAccuracy : 1;
+ uint8_t biasPresent : 1;
+ uint8_t biasSample : 6;
+ uint8_t pad;
+ };
+
+ struct RawThreeAxisSample
+ {
+ uint32_t deltaTime;
+ int16_t ix, iy, iz;
+ } __attribute__((packed));
+
+ struct ThreeAxisSample
+ {
+ uint32_t deltaTime;
+ float x, y, z;
+ } __attribute__((packed));
+
+ struct OneAxisSample
+ {
+ uint32_t deltaTime;
+ union
+ {
+ float fdata;
+ uint32_t idata;
+ };
+ } __attribute__((packed));
+
+ // The following structure should match struct HostIntfDataBuffer found in
+ // firmware/inc/hostIntf.h
+ struct nAxisEvent
+ {
+ uint32_t evtType;
+ union
+ {
+ struct
+ {
+ uint64_t referenceTime;
+ union
+ {
+ struct FirstSample firstSample;
+ struct OneAxisSample oneSamples[];
+ struct RawThreeAxisSample rawThreeSamples[];
+ struct ThreeAxisSample threeSamples[];
+ };
+ };
+ uint8_t buffer[];
+ };
+ } __attribute__((packed));
+
+ static Mutex sInstanceLock;
+ static HubConnection *sInstance;
+
+ // This lock is used for synchronization between the write thread (from
+ // sensorservice) and the read thread polling from the nanohub driver.
+ Mutex mLock;
+
+ RingBuffer mRing;
+
+ void *mActivityCbCookie;
+ ActivityFunc mActivityCb;
+
+ float mMagBias[3];
+ uint8_t mMagAccuracy;
+ uint8_t mMagAccuracyRestore;
+
+ float mGyroBias[3];
+
+ SensorState mSensorState[NUM_COMMS_SENSORS_PLUS_1];
+
+ uint64_t mStepCounterOffset;
+ uint64_t mLastStepCount;
+
+ int mFd;
+ int mInotifyPollIndex;
+ struct pollfd mPollFds[3];
+ int mNumPollFds;
+
+ sensors_event_t *initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor);
+ void magAccuracyUpdate(float x, float y, float z);
+ void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct OneAxisSample *sample, bool highAccuracy);
+ void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct RawThreeAxisSample *sample, bool highAccuracy);
+ void processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy);
+ void postOsLog(uint8_t *buf, ssize_t len);
+ ssize_t processBuf(uint8_t *buf, ssize_t len);
+
+ void initConfigCmd(struct ConfigCmd *cmd, int handle);
+
+ void queueDataInternal(int handle, void *data, size_t length);
+
+ void discardInotifyEvent();
+ void waitOnNanohubLock();
+
+ void initNanohubLock();
+
+ void restoreSensorState();
+ void sendCalibrationOffsets();
+
+#ifdef LID_STATE_REPORTING_ENABLED
+ int mUinputFd;
+
+ status_t initializeUinputNode();
+ void sendFolioEvent(int32_t data);
+#endif // LID_STATE_REPORTING_ENABLED
+
+#ifdef USB_MAG_BIAS_REPORTING_ENABLED
+ int mMagBiasPollIndex;
+ float mUsbMagBias;
+
+ void queueUsbMagBias();
+#endif // USB_MAG_BIAS_REPORTING_ENABLED
+
+ DISALLOW_EVIL_CONSTRUCTORS(HubConnection);
+};
+
+} // namespace android
+
+#endif // HUB_CONNECTION_H_
diff --git a/sensorhal/hubdefs.h b/sensorhal/hubdefs.h
new file mode 100644
index 0000000..63492ab
--- /dev/null
+++ b/sensorhal/hubdefs.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef HUB_DEFS_H_
+#define HUB_DEFS_H_
+
+#include <hardware/sensors.h>
+
+#define MAX_SPI_PAYLOAD_SIZE 256
+
+namespace android {
+
+#define CONTEXTHUB_SETTINGS_PATH "/persist/sensorcal.json"
+#define CONTEXTHUB_SAVED_SETTINGS_PATH "/data/misc/sensorcal_saved.json"
+#define MAG_BIAS_FILE_PATH "/sys/class/power_supply/battery/compass_compensation"
+
+static const uint32_t kMinClockRateHz = 960000;
+static const uint32_t kClockRateHz = kMinClockRateHz * 5; // 4.8MHz
+
+enum comms_sensor_t {
+ COMMS_SENSOR_INVALID = 0,
+ COMMS_SENSOR_ACCEL = 1,
+ COMMS_SENSOR_GYRO = 2,
+ COMMS_SENSOR_MAG = 3,
+ COMMS_SENSOR_PRESSURE = 4,
+ COMMS_SENSOR_TEMPERATURE = 5,
+ COMMS_SENSOR_PROXIMITY = 6,
+ COMMS_SENSOR_LIGHT = 7,
+ COMMS_SENSOR_ORIENTATION = 8,
+ COMMS_SENSOR_STEP_DETECTOR = 9,
+ COMMS_SENSOR_ANY_MOTION = 10,
+ COMMS_SENSOR_NO_MOTION = 11,
+ COMMS_SENSOR_SIGNIFICANT_MOTION = 12,
+ COMMS_SENSOR_FLAT = 13,
+ COMMS_SENSOR_ACTIVITY = 14,
+ COMMS_SENSOR_GRAVITY = 15,
+ COMMS_SENSOR_LINEAR_ACCEL = 16,
+ COMMS_SENSOR_ROTATION_VECTOR = 17,
+ COMMS_SENSOR_HALL = 18,
+ COMMS_SENSOR_GEO_MAG = 19,
+ COMMS_SENSOR_GAME_ROTATION_VECTOR = 20,
+ COMMS_SENSOR_GESTURE = 21,
+ COMMS_SENSOR_TILT = 22,
+ COMMS_SENSOR_MAG_BIAS = 23,
+ COMMS_SENSOR_STEP_COUNTER = 24,
+ COMMS_SENSOR_MAG_UNCALIBRATED = 25,
+ COMMS_SENSOR_GYRO_UNCALIBRATED = 26,
+ COMMS_SENSOR_GYRO_BIAS = 27,
+ COMMS_SENSOR_SYNC = 28,
+ COMMS_SENSOR_DOUBLE_TWIST = 29,
+ COMMS_SENSOR_DOUBLE_TAP = 30,
+ COMMS_SENSOR_WINDOW_ORIENTATION = 31,
+
+ NUM_COMMS_SENSORS_PLUS_1,
+
+ COMMS_SENSOR_DEBUG = 0x99,
+};
+
+enum {
+ SPI_COMMS_CMD_SYNC = 0,
+ SPI_COMMS_CMD_SWITCH_SENSOR = 1,
+ SPI_COMMS_CMD_ABSOLUTE_TIME = 2,
+ SPI_COMMS_SENSOR_DATA_SCALAR = 3,
+ SPI_COMMS_SENSOR_DATA_VEC3 = 4,
+ SPI_COMMS_SENSOR_DATA_VEC4 = 5,
+ SPI_COMMS_SENSOR_DATA_FLUSH = 6,
+ SPI_COMMS_CMD_UPDATE_MAG_BIAS = 7,
+ SPI_COMMS_CMD_UPDATE_MAG_ACCURACY = 8,
+ SPI_COMMS_CMD_UPDATE_GYRO_BIAS = 9,
+ SPI_COMMS_CMD_ACK_SUSPEND_STATE = 10,
+ SPI_COMMS_DEBUG_OUTPUT = 0xff,
+};
+
+// Please keep existing values unchanged when adding or removing SENSOR_TYPE
+enum {
+ SENSOR_TYPE_INTERNAL_TEMPERATURE = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 0,
+ SENSOR_TYPE_SYNC = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 1,
+ SENSOR_TYPE_DOUBLE_TWIST = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 2,
+ SENSOR_TYPE_DOUBLE_TAP = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 3,
+};
+
+} // namespace android
+
+#endif // HUB_DEFS_H_
diff --git a/sensorhal/sensorlist.h b/sensorhal/sensorlist.h
new file mode 100644
index 0000000..b26110b
--- /dev/null
+++ b/sensorhal/sensorlist.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SENSORLIST_H_
+
+#define SENSORLIST_H_
+
+#include <hardware/sensors.h>
+
+// A list of sensors provided by a device.
+extern const sensor_t kSensorList[];
+extern const size_t kSensorCount;
+
+// TODO(aarossig): Consider de-duplicating Google sensors.
+
+#endif // SENSORLIST_H_
diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp
new file mode 100644
index 0000000..7d10869
--- /dev/null
+++ b/sensorhal/sensors.cpp
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_TAG "sensors"
+// #defined LOG_NDEBUG 1
+#include <utils/Log.h>
+
+#include "hubconnection.h"
+#include "sensorlist.h"
+#include "sensors.h"
+
+#include <errno.h>
+#include <math.h>
+#include <media/stagefright/foundation/ADebug.h>
+#include <string.h>
+
+using namespace android;
+
+////////////////////////////////////////////////////////////////////////////////
+
+SensorContext::SensorContext(const struct hw_module_t *module)
+ : mHubConnection(HubConnection::getInstance()), mHubAlive(true) {
+ memset(&device, 0, sizeof(device));
+
+ device.common.tag = HARDWARE_DEVICE_TAG;
+ device.common.version = SENSORS_DEVICE_API_VERSION_1_3;
+ device.common.module = const_cast<hw_module_t *>(module);
+ device.common.close = CloseWrapper;
+ device.activate = ActivateWrapper;
+ device.setDelay = SetDelayWrapper;
+ device.poll = PollWrapper;
+ device.batch = BatchWrapper;
+ device.flush = FlushWrapper;
+
+ mHubAlive = (mHubConnection->initCheck() == OK
+ && mHubConnection->getAliveCheck() == OK);
+}
+
+int SensorContext::close() {
+ ALOGI("close");
+
+ delete this;
+
+ return 0;
+}
+
+int SensorContext::activate(int handle, int enabled) {
+ ALOGI("activate");
+
+ mHubConnection->queueActivate(handle, enabled);
+
+ return 0;
+}
+
+int SensorContext::setDelay(int handle, int64_t delayNs) {
+ ALOGI("setDelay");
+
+ // clamp sample rate based on minDelay and maxDelay defined in kSensorList
+ int64_t delayNsClamped = delayNs;
+ for (size_t i = 0; i < kSensorCount; i++) {
+ sensor_t sensor = kSensorList[i];
+ if (sensor.handle != handle) {
+ continue;
+ }
+
+ if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
+ if ((delayNs/1000) < sensor.minDelay) {
+ delayNsClamped = sensor.minDelay * 1000;
+ } else if ((delayNs/1000) > sensor.maxDelay) {
+ delayNsClamped = sensor.maxDelay * 1000;
+ }
+ }
+
+ break;
+ }
+
+ mHubConnection->queueSetDelay(handle, delayNsClamped);
+
+ return 0;
+}
+
+int SensorContext::poll(sensors_event_t *data, int count) {
+ ALOGV("poll");
+
+ ssize_t n = mHubConnection->read(data, count);
+
+ if (n < 0) {
+ return -1;
+ }
+
+ return n;
+}
+
+int SensorContext::batch(
+ int handle,
+ int flags,
+ int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) {
+ ALOGI("batch");
+
+ // clamp sample rate based on minDelay and maxDelay defined in kSensorList
+ int64_t sampling_period_ns_clamped = sampling_period_ns;
+ for (size_t i = 0; i < kSensorCount; i++) {
+ sensor_t sensor = kSensorList[i];
+ if (sensor.handle != handle) {
+ continue;
+ }
+
+ if ((sensor.flags & REPORTING_MODE_MASK) == SENSOR_FLAG_CONTINUOUS_MODE) {
+ if ((sampling_period_ns/1000) < sensor.minDelay) {
+ sampling_period_ns_clamped = sensor.minDelay * 1000;
+ } else if ((sampling_period_ns/1000) > sensor.maxDelay) {
+ sampling_period_ns_clamped = sensor.maxDelay * 1000;
+ }
+ }
+
+ break;
+ }
+
+ mHubConnection->queueBatch(
+ handle, flags, sampling_period_ns_clamped, max_report_latency_ns);
+
+ return 0;
+}
+
+int SensorContext::flush(int handle) {
+ ALOGI("flush");
+
+ mHubConnection->queueFlush(handle);
+ return 0;
+}
+
+// static
+int SensorContext::CloseWrapper(struct hw_device_t *dev) {
+ return reinterpret_cast<SensorContext *>(dev)->close();
+}
+
+// static
+int SensorContext::ActivateWrapper(
+ struct sensors_poll_device_t *dev, int handle, int enabled) {
+ return reinterpret_cast<SensorContext *>(dev)->activate(handle, enabled);
+}
+
+// static
+int SensorContext::SetDelayWrapper(
+ struct sensors_poll_device_t *dev, int handle, int64_t delayNs) {
+ return reinterpret_cast<SensorContext *>(dev)->setDelay(handle, delayNs);
+}
+
+// static
+int SensorContext::PollWrapper(
+ struct sensors_poll_device_t *dev, sensors_event_t *data, int count) {
+ return reinterpret_cast<SensorContext *>(dev)->poll(data, count);
+}
+
+// static
+int SensorContext::BatchWrapper(
+ struct sensors_poll_device_1 *dev,
+ int handle,
+ int flags,
+ int64_t sampling_period_ns,
+ int64_t max_report_latency_ns) {
+ return reinterpret_cast<SensorContext *>(dev)->batch(
+ handle, flags, sampling_period_ns, max_report_latency_ns);
+}
+
+// static
+int SensorContext::FlushWrapper(struct sensors_poll_device_1 *dev, int handle) {
+ return reinterpret_cast<SensorContext *>(dev)->flush(handle);
+}
+
+bool SensorContext::getHubAlive() {
+ return mHubAlive;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+static bool gHubAlive;
+
+static int open_sensors(
+ const struct hw_module_t *module,
+ const char *,
+ struct hw_device_t **dev) {
+ ALOGI("open_sensors");
+
+ SensorContext *ctx = new SensorContext(module);
+
+ gHubAlive = ctx->getHubAlive();
+ *dev = &ctx->device.common;
+
+ return 0;
+}
+
+static struct hw_module_methods_t sensors_module_methods = {
+ .open = open_sensors
+};
+
+static int get_sensors_list(
+ struct sensors_module_t *,
+ struct sensor_t const **list) {
+ ALOGI("get_sensors_list");
+
+ if (gHubAlive) {
+ *list = kSensorList;
+ return kSensorCount;
+ } else {
+ *list = {};
+ return 0;
+ }
+}
+
+static int set_operation_mode(unsigned int mode) {
+ ALOGI("set_operation_mode");
+ return (mode) ? -EINVAL : 0;
+}
+
+struct sensors_module_t HAL_MODULE_INFO_SYM = {
+ .common = {
+ .tag = HARDWARE_MODULE_TAG,
+ .version_major = 1,
+ .version_minor = 0,
+ .id = SENSORS_HARDWARE_MODULE_ID,
+ .name = "Google Sensor module",
+ .author = "Google",
+ .methods = &sensors_module_methods,
+ .dso = NULL,
+ .reserved = {0},
+ },
+ .get_sensors_list = get_sensors_list,
+ .set_operation_mode = set_operation_mode,
+};
diff --git a/sensorhal/sensors.h b/sensorhal/sensors.h
new file mode 100644
index 0000000..fd034fa
--- /dev/null
+++ b/sensorhal/sensors.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SENSORS_H_
+
+#define SENSORS_H_
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+#include <media/stagefright/foundation/ABase.h>
+
+#include "hubconnection.h"
+
+struct SensorContext {
+ struct sensors_poll_device_1 device;
+
+ explicit SensorContext(const struct hw_module_t *module);
+
+ bool getHubAlive();
+
+private:
+ android::sp<android::HubConnection> mHubConnection;
+ bool mHubAlive;
+
+ int close();
+ int activate(int handle, int enabled);
+ int setDelay(int handle, int64_t delayNs);
+ int poll(sensors_event_t *data, int count);
+
+ int batch(
+ int handle,
+ int flags,
+ int64_t sampling_period_ns,
+ int64_t max_report_latency_ns);
+
+ int flush(int handle);
+
+ static int CloseWrapper(struct hw_device_t *dev);
+
+ static int ActivateWrapper(
+ struct sensors_poll_device_t *dev, int handle, int enabled);
+
+ static int SetDelayWrapper(
+ struct sensors_poll_device_t *dev, int handle, int64_t delayNs);
+
+ static int PollWrapper(
+ struct sensors_poll_device_t *dev, sensors_event_t *data, int count);
+
+ static int BatchWrapper(
+ struct sensors_poll_device_1 *dev,
+ int handle,
+ int flags,
+ int64_t sampling_period_ns,
+ int64_t max_report_latency_ns);
+
+ static int FlushWrapper(struct sensors_poll_device_1 *dev, int handle);
+
+ DISALLOW_EVIL_CONSTRUCTORS(SensorContext);
+};
+
+#endif // SENSORS_H_
diff --git a/util/common/JSONObject.cpp b/util/common/JSONObject.cpp
new file mode 100644
index 0000000..83ed14e
--- /dev/null
+++ b/util/common/JSONObject.cpp
@@ -0,0 +1,754 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+//#define LOG_NDEBUG 0
+#define LOG_TAG "JSONObject"
+#include <utils/Log.h>
+
+#include "JSONObject.h"
+
+#include <ctype.h>
+#include <math.h>
+#include <media/stagefright/foundation/ADebug.h>
+#include <media/stagefright/foundation/AString.h>
+#include <media/stagefright/MediaErrors.h>
+
+namespace android {
+
+// Returns ERROR_MALFORMED if the value overflows a signed int, returns
+// 0 otherwise.
+// This method will assert if it is asked to parse a character which is not
+// a digit.
+static ssize_t parseInt32(const char *data, size_t numDigits, int32_t *out) {
+ int32_t x = 0;
+ for (size_t i = 0; i < numDigits; ++i) {
+ int32_t old_x = x;
+ x *= 10;
+ x += data[i] - '0';
+
+ CHECK(isdigit(data[i]));
+
+ if (x < old_x) {
+ // We've overflowed.
+ return ERROR_MALFORMED;
+ }
+ }
+
+ *out = x;
+ return 0;
+}
+
+// static
+ssize_t JSONValue::Parse(const char *data, size_t size, JSONValue *out) {
+ size_t offset = 0;
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == '[') {
+ sp<JSONArray> array = new JSONArray;
+ ++offset;
+
+ for (;;) {
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == ']') {
+ ++offset;
+ break;
+ }
+
+ JSONValue val;
+ ssize_t n = Parse(&data[offset], size - offset, &val);
+
+ if (n < 0) {
+ return n;
+ }
+
+ array->addValue(val);
+
+ offset += n;
+
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == ',') {
+ ++offset;
+ } else if (data[offset] != ']') {
+ return ERROR_MALFORMED;
+ }
+ };
+
+ out->setArray(array);
+
+ return offset;
+ } else if (data[offset] == '{') {
+ sp<JSONObject> obj = new JSONObject;
+ ++offset;
+
+ for (;;) {
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == '}') {
+ ++offset;
+ break;
+ }
+
+ JSONValue key;
+ ssize_t n = Parse(&data[offset], size - offset, &key);
+
+ if (n < 0) {
+ return n;
+ }
+
+ if (key.type() != TYPE_STRING) {
+ return ERROR_MALFORMED;
+ }
+
+ offset += n;
+
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size || data[offset] != ':') {
+ return ERROR_MALFORMED;
+ }
+
+ ++offset;
+
+ JSONValue val;
+ n = Parse(&data[offset], size - offset, &val);
+
+ if (n < 0) {
+ return n;
+ }
+
+ AString keyVal;
+ CHECK(key.getString(&keyVal));
+
+ obj->setValue(keyVal.c_str(), val);
+
+ offset += n;
+
+ while (offset < size && isspace(data[offset])) {
+ ++offset;
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == ',') {
+ ++offset;
+ } else if (data[offset] != '}') {
+ return ERROR_MALFORMED;
+ }
+ };
+
+ out->setObject(obj);
+
+ return offset;
+ } else if (data[offset] == '"') {
+ ++offset;
+
+ AString s;
+ bool escaped = false;
+ while (offset < size) {
+ if (escaped) {
+ char c;
+ switch (data[offset]) {
+ case '\"':
+ case '\\':
+ case '/':
+ c = data[offset];
+ break;
+ case 'b':
+ c = '\x08';
+ break;
+ case 'f':
+ c = '\x0c';
+ break;
+ case 'n':
+ c = '\x0a';
+ break;
+ case 'r':
+ c = '\x0d';
+ break;
+ case 't':
+ c = '\x09';
+ break;
+ default:
+ return ERROR_MALFORMED;
+ }
+
+ s.append(c);
+ ++offset;
+
+ escaped = false;
+ } else if (data[offset] == '\\') {
+ escaped = true;
+ } else if (data[offset] == '"') {
+ break;
+ }
+
+ s.append(data[offset++]);
+ }
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ ++offset;
+ out->setString(s);
+
+ return offset;
+ } else if (isdigit(data[offset]) || data[offset] == '-') {
+ bool negate = false;
+ if (data[offset] == '-') {
+ negate = true;
+ ++offset;
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+ }
+
+ size_t firstDigitOffset = offset;
+ while (offset < size && isdigit(data[offset])) {
+ ++offset;
+ }
+
+ size_t numDigits = offset - firstDigitOffset;
+ if (numDigits > 1 && data[firstDigitOffset] == '0') {
+ // No leading zeros.
+ return ERROR_MALFORMED;
+ }
+
+ size_t firstFracDigitOffset = 0;
+ size_t numFracDigits = 0;
+
+ if (offset < size && data[offset] == '.') {
+ ++offset;
+
+ firstFracDigitOffset = offset;
+ while (offset < size && isdigit(data[offset])) {
+ ++offset;
+ }
+
+ numFracDigits = offset - firstFracDigitOffset;
+ if (numFracDigits == 0) {
+ return ERROR_MALFORMED;
+ }
+ }
+
+ bool negateExponent = false;
+ size_t firstExpDigitOffset = 0;
+ size_t numExpDigits = 0;
+
+ if (offset < size && (data[offset] == 'e' || data[offset] == 'E')) {
+ ++offset;
+
+ if (offset == size) {
+ return ERROR_MALFORMED;
+ }
+
+ if (data[offset] == '+' || data[offset] == '-') {
+ if (data[offset] == '-') {
+ negateExponent = true;
+ }
+
+ ++offset;
+ }
+
+ firstExpDigitOffset = offset;
+ while (offset < size && isdigit(data[offset])) {
+ ++offset;
+ }
+
+ numExpDigits = offset - firstExpDigitOffset;
+ if (numExpDigits == 0) {
+ return ERROR_MALFORMED;
+ }
+ }
+
+ if (numFracDigits == 0 && numExpDigits == 0) {
+ int32_t x;
+ if (parseInt32(&data[firstDigitOffset], numDigits, &x) != 0) {
+ return ERROR_MALFORMED;
+ }
+
+ out->setInt32(negate ? -x : x);
+ } else {
+ int32_t mantissa;
+ if (parseInt32(&data[firstDigitOffset], numDigits, &mantissa) != 0) {
+ return ERROR_MALFORMED;
+ }
+
+ int32_t fraction;
+ if (parseInt32(&data[firstFracDigitOffset], numFracDigits, &fraction) != 0) {
+ return ERROR_MALFORMED;
+ }
+
+ int32_t exponent;
+ if (parseInt32(&data[firstExpDigitOffset], numExpDigits, &exponent) != 0) {
+ return ERROR_MALFORMED;
+ }
+
+ if (negateExponent) {
+ exponent = -exponent;
+ }
+
+ float x = (float)mantissa;
+ x += (float)fraction * powf(10.0f, exponent - (int32_t)numFracDigits);
+
+ out->setFloat(negate ? -x : x);
+ }
+
+ return offset;
+ } else if (offset + 4 <= size && !strncmp("null", &data[offset], 4)) {
+ out->unset();
+ return offset + 4;
+ } else if (offset + 4 <= size && !strncmp("true", &data[offset], 4)) {
+ out->setBoolean(true);
+ return offset + 4;
+ } else if (offset + 5 <= size && !strncmp("false", &data[offset], 5)) {
+ out->setBoolean(false);
+ return offset + 5;
+ }
+
+ return ERROR_MALFORMED;
+}
+
+JSONValue::JSONValue()
+ : mType(TYPE_NULL) {
+}
+
+JSONValue::JSONValue(const JSONValue &other)
+ : mType(TYPE_NULL) {
+ *this = other;
+}
+
+JSONValue &JSONValue::operator=(const JSONValue &other) {
+ if (&other != this) {
+ unset();
+ mType = other.mType;
+ mValue = other.mValue;
+
+ switch (mType) {
+ case TYPE_STRING:
+ mValue.mString = new AString(*other.mValue.mString);
+ break;
+ case TYPE_OBJECT:
+ case TYPE_ARRAY:
+ mValue.mObjectOrArray->incStrong(this /* id */);
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ return *this;
+}
+
+JSONValue::~JSONValue() {
+ unset();
+}
+
+JSONValue::FieldType JSONValue::type() const {
+ return mType;
+}
+
+bool JSONValue::getInt32(int32_t *value) const {
+ if (mType != TYPE_INT32) {
+ return false;
+ }
+
+ *value = mValue.mInt32;
+ return true;
+}
+
+bool JSONValue::getFloat(float *value) const {
+ switch (mType) {
+ case TYPE_INT32:
+ {
+ *value = mValue.mInt32;
+ break;
+ }
+
+ case TYPE_FLOAT:
+ {
+ *value = mValue.mFloat;
+ break;
+ }
+
+ default:
+ return false;
+ }
+
+ return true;
+}
+
+bool JSONValue::getString(AString *value) const {
+ if (mType != TYPE_STRING) {
+ return false;
+ }
+
+ *value = *mValue.mString;
+ return true;
+}
+
+bool JSONValue::getBoolean(bool *value) const {
+ if (mType != TYPE_BOOLEAN) {
+ return false;
+ }
+
+ *value = mValue.mBoolean;
+ return true;
+}
+
+bool JSONValue::getObject(sp<JSONObject> *value) const {
+ if (mType != TYPE_OBJECT) {
+ return false;
+ }
+
+ *value = static_cast<JSONObject *>(mValue.mObjectOrArray);
+ return true;
+}
+
+bool JSONValue::getArray(sp<JSONArray> *value) const {
+ if (mType != TYPE_ARRAY) {
+ return false;
+ }
+
+ *value = static_cast<JSONArray *>(mValue.mObjectOrArray);
+ return true;
+}
+
+void JSONValue::setInt32(int32_t value) {
+ unset();
+
+ mValue.mInt32 = value;
+ mType = TYPE_INT32;
+}
+
+void JSONValue::setFloat(float value) {
+ unset();
+
+ mValue.mFloat = value;
+ mType = TYPE_FLOAT;
+}
+
+void JSONValue::setString(const AString &value) {
+ unset();
+
+ mValue.mString = new AString(value);
+ mType = TYPE_STRING;
+}
+
+void JSONValue::setBoolean(bool value) {
+ unset();
+
+ mValue.mBoolean = value;
+ mType = TYPE_BOOLEAN;
+}
+
+void JSONValue::setObject(const sp<JSONObject> &obj) {
+ unset();
+
+ mValue.mObjectOrArray = obj.get();
+ mValue.mObjectOrArray->incStrong(this /* id */);
+
+ mType = TYPE_OBJECT;
+}
+
+void JSONValue::setArray(const sp<JSONArray> &array) {
+ unset();
+
+ mValue.mObjectOrArray = array.get();
+ mValue.mObjectOrArray->incStrong(this /* id */);
+
+ mType = TYPE_ARRAY;
+}
+
+void JSONValue::unset() {
+ switch (mType) {
+ case TYPE_STRING:
+ delete mValue.mString;
+ break;
+ case TYPE_OBJECT:
+ case TYPE_ARRAY:
+ mValue.mObjectOrArray->decStrong(this /* id */);
+ break;
+
+ default:
+ break;
+ }
+
+ mType = TYPE_NULL;
+}
+
+static void EscapeString(const char *in, size_t inSize, AString *out) {
+ CHECK(in != out->c_str());
+ out->clear();
+
+ for (size_t i = 0; i < inSize; ++i) {
+ char c = in[i];
+ switch (c) {
+ case '\"':
+ out->append("\\\"");
+ break;
+ case '\\':
+ out->append("\\\\");
+ break;
+ case '/':
+ out->append("\\/");
+ break;
+ case '\x08':
+ out->append("\\b");
+ break;
+ case '\x0c':
+ out->append("\\f");
+ break;
+ case '\x0a':
+ out->append("\\n");
+ break;
+ case '\x0d':
+ out->append("\\r");
+ break;
+ case '\x09':
+ out->append("\\t");
+ break;
+ default:
+ out->append(c);
+ break;
+ }
+ }
+}
+
+AString JSONValue::toString(size_t depth, bool indentFirstLine) const {
+ static const char kIndent[] = " ";
+
+ AString out;
+
+ switch (mType) {
+ case TYPE_STRING:
+ {
+ AString escaped;
+ EscapeString(
+ mValue.mString->c_str(), mValue.mString->size(), &escaped);
+
+ out.append("\"");
+ out.append(escaped);
+ out.append("\"");
+ break;
+ }
+
+ case TYPE_INT32:
+ {
+ out = AStringPrintf("%d", mValue.mInt32);
+ break;
+ }
+
+ case TYPE_FLOAT:
+ {
+ out = AStringPrintf("%f", mValue.mFloat);
+ break;
+ }
+
+ case TYPE_BOOLEAN:
+ {
+ out = mValue.mBoolean ? "true" : "false";
+ break;
+ }
+
+ case TYPE_NULL:
+ {
+ out = "null";
+ break;
+ }
+
+ case TYPE_OBJECT:
+ case TYPE_ARRAY:
+ {
+ out = (mType == TYPE_OBJECT) ? "{\n" : "[\n";
+ out.append(mValue.mObjectOrArray->internalToString(depth + 1));
+ out.append("\n");
+ out.append(kIndent, 2 * depth);
+ out.append(mType == TYPE_OBJECT ? "}" : "]");
+ break;
+ }
+
+ default:
+ TRESPASS();
+ }
+
+ if (indentFirstLine) {
+ out.insert(kIndent, 2 * depth, 0);
+ }
+
+ return out;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+// static
+sp<JSONCompound> JSONCompound::Parse(const char *data, size_t size) {
+ JSONValue value;
+ ssize_t result = JSONValue::Parse(data, size, &value);
+
+ if (result < 0) {
+ return NULL;
+ }
+
+ sp<JSONObject> obj;
+ if (value.getObject(&obj)) {
+ return obj;
+ }
+
+ sp<JSONArray> array;
+ if (value.getArray(&array)) {
+ return array;
+ }
+
+ return NULL;
+}
+
+AString JSONCompound::toString(size_t depth, bool indentFirstLine) const {
+ JSONValue val;
+ if (isObject()) {
+ val.setObject((JSONObject *)this);
+ } else {
+ val.setArray((JSONArray *)this);
+ }
+
+ return val.toString(depth, indentFirstLine);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+JSONObject::JSONObject() {}
+JSONObject::~JSONObject() {}
+
+bool JSONObject::isObject() const {
+ return true;
+}
+
+bool JSONObject::getValue(const char *key, JSONValue *value) const {
+ ssize_t index = mValues.indexOfKey(key);
+ if (index < 0) {
+ return false;
+ }
+
+ *value = mValues.valueAt(index);
+
+ return true;
+}
+
+void JSONObject::setValue(const char *key, const JSONValue &value) {
+ mValues.add(AString(key), value);
+}
+
+AString JSONObject::internalToString(size_t depth) const {
+ static const char kIndent[] = " ";
+
+ AString out;
+ for (size_t i = 0; i < mValues.size(); ++i) {
+ AString key = mValues.keyAt(i);
+ AString escapedKey;
+ EscapeString(key.c_str(), key.size(), &escapedKey);
+
+ out.append(kIndent, 2 * depth);
+ out.append("\"");
+ out.append(escapedKey);
+ out.append("\": ");
+
+ out.append(mValues.valueAt(i).toString(depth + 1, false));
+
+ if (i + 1 < mValues.size()) {
+ out.append(",\n");
+ }
+ }
+
+ return out;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+JSONArray::JSONArray() {}
+
+JSONArray::~JSONArray() {}
+
+bool JSONArray::isObject() const {
+ return false;
+}
+
+size_t JSONArray::size() const {
+ return mValues.size();
+}
+
+bool JSONArray::getValue(size_t key, JSONValue *value) const {
+ if (key >= mValues.size()) {
+ return false;
+ }
+
+ *value = mValues.itemAt(key);
+
+ return true;
+}
+
+void JSONArray::addValue(const JSONValue &value) {
+ mValues.push_back(value);
+}
+
+AString JSONArray::internalToString(size_t depth) const {
+ AString out;
+ for (size_t i = 0; i < mValues.size(); ++i) {
+ out.append(mValues.itemAt(i).toString(depth));
+
+ if (i + 1 < mValues.size()) {
+ out.append(",\n");
+ }
+ }
+
+ return out;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+} // namespace android
+
diff --git a/util/common/JSONObject.h b/util/common/JSONObject.h
new file mode 100644
index 0000000..eafc565
--- /dev/null
+++ b/util/common/JSONObject.h
@@ -0,0 +1,272 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef JSON_OBJECT_H_
+
+#define JSON_OBJECT_H_
+
+#include <media/stagefright/foundation/ABase.h>
+#include <media/stagefright/foundation/AString.h>
+#include <utils/Errors.h>
+#include <utils/KeyedVector.h>
+#include <utils/RefBase.h>
+#include <utils/Vector.h>
+
+namespace android {
+
+struct JSONArray;
+struct JSONCompound;
+struct JSONObject;
+
+struct JSONValue {
+ enum FieldType {
+ TYPE_STRING,
+ TYPE_INT32,
+ TYPE_FLOAT,
+ TYPE_BOOLEAN,
+ TYPE_NULL,
+ TYPE_OBJECT,
+ TYPE_ARRAY,
+ };
+
+ // Returns the number of bytes consumed or an error.
+ static ssize_t Parse(const char *data, size_t size, JSONValue *out);
+
+ JSONValue();
+ JSONValue(const JSONValue &);
+ JSONValue &operator=(const JSONValue &);
+ ~JSONValue();
+
+ FieldType type() const;
+ bool getInt32(int32_t *value) const;
+ bool getFloat(float *value) const;
+ bool getString(AString *value) const;
+ bool getBoolean(bool *value) const;
+ bool getObject(sp<JSONObject> *value) const;
+ bool getArray(sp<JSONArray> *value) const;
+
+ void setInt32(int32_t value);
+ void setFloat(float value);
+ void setString(const AString &value);
+ void setBoolean(bool value);
+ void setObject(const sp<JSONObject> &obj);
+ void setArray(const sp<JSONArray> &array);
+ void unset(); // i.e. setNull()
+
+ AString toString(size_t depth = 0, bool indentFirstLine = true) const;
+
+private:
+ FieldType mType;
+
+ union {
+ int32_t mInt32;
+ float mFloat;
+ AString *mString;
+ bool mBoolean;
+ JSONCompound *mObjectOrArray;
+ } mValue;
+};
+
+struct JSONCompound : public RefBase {
+ static sp<JSONCompound> Parse(const char *data, size_t size);
+
+ AString toString(size_t depth = 0, bool indentFirstLine = true) const;
+
+ virtual bool isObject() const = 0;
+
+protected:
+ virtual ~JSONCompound() {}
+
+ virtual AString internalToString(size_t depth) const = 0;
+
+ JSONCompound() {}
+
+private:
+ friend struct JSONValue;
+
+ DISALLOW_EVIL_CONSTRUCTORS(JSONCompound);
+};
+
+template<class KEY>
+struct JSONBase : public JSONCompound {
+ JSONBase() {}
+
+#define PREAMBLE() \
+ JSONValue value; \
+ if (!getValue(key, &value)) { \
+ return false; \
+ }
+
+ bool getFieldType(KEY key, JSONValue::FieldType *type) const {
+ PREAMBLE()
+ *type = value.type();
+ return true;
+ }
+
+ bool getInt32(KEY key, int32_t *out) const {
+ PREAMBLE()
+ return value.getInt32(out);
+ }
+
+ bool getFloat(KEY key, float *out) const {
+ PREAMBLE()
+ return value.getFloat(out);
+ }
+
+ bool getString(KEY key, AString *out) const {
+ PREAMBLE()
+ return value.getString(out);
+ }
+
+ bool getBoolean(KEY key, bool *out) const {
+ PREAMBLE()
+ return value.getBoolean(out);
+ }
+
+ bool getObject(KEY key, sp<JSONObject> *obj) const {
+ PREAMBLE()
+ return value.getObject(obj);
+ }
+
+ bool getArray(KEY key, sp<JSONArray> *obj) const {
+ PREAMBLE()
+ return value.getArray(obj);
+ }
+
+#undef PREAMBLE
+
+protected:
+ virtual ~JSONBase() {}
+
+ virtual bool getValue(KEY key, JSONValue *value) const = 0;
+
+private:
+ DISALLOW_EVIL_CONSTRUCTORS(JSONBase);
+};
+
+struct JSONObject : public JSONBase<const char *> {
+ JSONObject();
+
+ virtual bool isObject() const;
+ void setValue(const char *key, const JSONValue &value);
+
+ void setInt32(const char *key, int32_t in) {
+ JSONValue val;
+ val.setInt32(in);
+ setValue(key, val);
+ }
+
+ void setFloat(const char *key, float in) {
+ JSONValue val;
+ val.setFloat(in);
+ setValue(key, val);
+ }
+
+ void setString(const char *key, AString in) {
+ JSONValue val;
+ val.setString(in);
+ setValue(key, val);
+ }
+
+ void setBoolean(const char *key, bool in) {
+ JSONValue val;
+ val.setBoolean(in);
+ setValue(key, val);
+ }
+
+ void setObject(const char *key, const sp<JSONObject> &obj) {
+ JSONValue val;
+ val.setObject(obj);
+ setValue(key, val);
+ }
+
+ void setArray(const char *key, const sp<JSONArray> &obj) {
+ JSONValue val;
+ val.setArray(obj);
+ setValue(key, val);
+ }
+
+protected:
+ virtual ~JSONObject();
+
+ virtual bool getValue(const char *key, JSONValue *value) const;
+ virtual AString internalToString(size_t depth) const;
+
+private:
+ KeyedVector<AString, JSONValue> mValues;
+
+ DISALLOW_EVIL_CONSTRUCTORS(JSONObject);
+};
+
+struct JSONArray : public JSONBase<size_t> {
+ JSONArray();
+
+ virtual bool isObject() const;
+ size_t size() const;
+ void addValue(const JSONValue &value);
+
+ void addInt32(int32_t in) {
+ JSONValue val;
+ val.setInt32(in);
+ addValue(val);
+ }
+
+ void addFloat(float in) {
+ JSONValue val;
+ val.setFloat(in);
+ addValue(val);
+ }
+
+ void addString(AString in) {
+ JSONValue val;
+ val.setString(in);
+ addValue(val);
+ }
+
+ void addBoolean(bool in) {
+ JSONValue val;
+ val.setBoolean(in);
+ addValue(val);
+ }
+
+ void addObject(const sp<JSONObject> &obj) {
+ JSONValue val;
+ val.setObject(obj);
+ addValue(val);
+ }
+
+ void addArray(const sp<JSONArray> &obj) {
+ JSONValue val;
+ val.setArray(obj);
+ addValue(val);
+ }
+
+protected:
+ virtual ~JSONArray();
+
+ virtual bool getValue(size_t key, JSONValue *value) const;
+ virtual AString internalToString(size_t depth) const;
+
+
+private:
+ Vector<JSONValue> mValues;
+
+ DISALLOW_EVIL_CONSTRUCTORS(JSONArray);
+};
+
+} // namespace android
+
+#endif // JSON_OBJECT_H_
diff --git a/util/common/file.cpp b/util/common/file.cpp
new file mode 100644
index 0000000..1757601
--- /dev/null
+++ b/util/common/file.cpp
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+//#define LOG_NDEBUG 0
+#define LOG_TAG "file"
+#include <utils/Log.h>
+#include "file.h"
+
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+
+namespace android {
+
+File::File()
+ : mInitCheck(NO_INIT),
+ mFd(-1) {
+}
+
+File::File(const char *path, const char *mode)
+ : mInitCheck(NO_INIT),
+ mFd(-1) {
+ mInitCheck = setTo(path, mode);
+}
+
+File::~File() {
+ close();
+}
+
+status_t File::initCheck() const {
+ return mInitCheck;
+}
+
+status_t File::setTo(const char *path, const char *mode) {
+ close();
+
+ int modeval = 0;
+ if (!strcmp("r", mode)) {
+ modeval = O_RDONLY;
+ } else if (!strcmp("w", mode)) {
+ modeval = O_WRONLY | O_CREAT | O_TRUNC;
+ } else if (!strcmp("rw", mode)) {
+ modeval = O_RDWR | O_CREAT;
+ }
+
+ int filemode = 0;
+ if (modeval & O_CREAT) {
+ filemode = S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH;
+ }
+
+ mFd = open(path, modeval, filemode);
+
+ mInitCheck = (mFd >= 0) ? OK : -errno;
+
+ return mInitCheck;
+}
+
+void File::close() {
+ if (mFd >= 0) {
+ ::close(mFd);
+ mFd = -1;
+ }
+
+ mInitCheck = NO_INIT;
+}
+
+ssize_t File::read(void *data, size_t size) {
+ return ::read(mFd, data, size);
+}
+
+ssize_t File::write(const void *data, size_t size) {
+ return ::write(mFd, data, size);
+}
+
+off64_t File::seekTo(off64_t pos, int whence) {
+ off64_t new_pos = lseek64(mFd, pos, whence);
+ return new_pos < 0 ? -errno : new_pos;
+}
+
+} // namespace android
diff --git a/util/common/file.h b/util/common/file.h
new file mode 100644
index 0000000..97d74b0
--- /dev/null
+++ b/util/common/file.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef FILE_H_
+
+#define FILE_H_
+
+#include <media/stagefright/foundation/ABase.h>
+#include <utils/Errors.h>
+
+namespace android {
+
+struct File {
+ File();
+ File(const char *path, const char *mode);
+
+ ~File();
+
+ status_t initCheck() const;
+ status_t setTo(const char *path, const char *mode);
+ void close();
+
+ ssize_t read(void *data, size_t size);
+ ssize_t write(const void *data, size_t size);
+
+ off64_t seekTo(off64_t pos, int whence = SEEK_SET);
+
+private:
+ status_t mInitCheck;
+ int mFd;
+
+ DISALLOW_EVIL_CONSTRUCTORS(File);
+};
+
+} // namespace android
+
+#endif // FILE_H_
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
new file mode 100644
index 0000000..1d23b82
--- /dev/null
+++ b/util/common/ring.cpp
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+//#define LOG_DEBUG 1
+#define LOG_TAG "ring"
+#include <utils/Log.h>
+
+#include "ring.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+namespace android {
+
+RingBuffer::RingBuffer(size_t size)
+ : mSize(size),
+ mData((sensors_event_t *)malloc(sizeof(sensors_event_t) * mSize)),
+ mReadPos(0),
+ mWritePos(0) {
+}
+
+RingBuffer::~RingBuffer() {
+ free(mData);
+ mData = NULL;
+}
+
+ssize_t RingBuffer::write(const sensors_event_t *ev, size_t size) {
+ Mutex::Autolock autoLock(mLock);
+
+ size_t numAvailableToRead = mWritePos - mReadPos;
+ size_t numAvailableToWrite = mSize - numAvailableToRead;
+
+ if (size > numAvailableToWrite) {
+ size = numAvailableToWrite;
+ }
+
+ size_t writePos = (mWritePos % mSize);
+ size_t copy = mSize - writePos;
+
+ if (copy > size) {
+ copy = size;
+ }
+
+ memcpy(&mData[writePos], ev, copy * sizeof(sensors_event_t));
+
+ if (size > copy) {
+ memcpy(mData, &ev[copy], (size - copy) * sizeof(sensors_event_t));
+ }
+
+ mWritePos += size;
+
+ if (numAvailableToRead == 0 && size > 0) {
+ mNotEmptyCondition.broadcast();
+ }
+
+ return size;
+}
+
+ssize_t RingBuffer::read(sensors_event_t *ev, size_t size) {
+ Mutex::Autolock autoLock(mLock);
+
+ size_t numAvailableToRead;
+ for (;;) {
+ numAvailableToRead = mWritePos - mReadPos;
+ if (numAvailableToRead > 0) {
+ break;
+ }
+
+ mNotEmptyCondition.wait(mLock);
+ }
+
+ if (size > numAvailableToRead) {
+ size = numAvailableToRead;
+ }
+
+ size_t readPos = (mReadPos % mSize);
+ size_t copy = mSize - readPos;
+
+ if (copy > size) {
+ copy = size;
+ }
+
+ memcpy(ev, &mData[readPos], copy * sizeof(sensors_event_t));
+
+ if (size > copy) {
+ memcpy(&ev[copy], mData, (size - copy) * sizeof(sensors_event_t));
+ }
+
+ mReadPos += size;
+
+ return size;
+}
+
+} // namespace android
+
diff --git a/util/common/ring.h b/util/common/ring.h
new file mode 100644
index 0000000..c77c3de
--- /dev/null
+++ b/util/common/ring.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2015 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef RING_BUFFER_H_
+
+#define RING_BUFFER_H_
+
+#include <media/stagefright/foundation/ABase.h>
+
+#include <hardware/sensors.h>
+#include <utils/threads.h>
+
+namespace android {
+
+struct RingBuffer {
+ explicit RingBuffer(size_t size);
+ ~RingBuffer();
+
+ ssize_t write(const sensors_event_t *ev, size_t size);
+ ssize_t read(sensors_event_t *ev, size_t size);
+
+private:
+ Mutex mLock;
+ Condition mNotEmptyCondition;
+
+ size_t mSize;
+ sensors_event_t *mData;
+ size_t mReadPos, mWritePos;
+
+ DISALLOW_EVIL_CONSTRUCTORS(RingBuffer);
+};
+
+} // namespace android
+
+#endif // RING_BUFFER_H_
diff --git a/util/common/util.h b/util/common/util.h
new file mode 100644
index 0000000..a1c233c
--- /dev/null
+++ b/util/common/util.h
@@ -0,0 +1,12 @@
+#ifndef COMMON_UTIL_H_
+#define COMMON_UTIL_H_
+
+#include <chrono>
+
+namespace android {
+
+typedef std::chrono::time_point<std::chrono::steady_clock> SteadyClock;
+
+} // namespace android
+
+#endif // COMMON_UTIL_H_
diff --git a/util/nanoapp_cmd/Android.mk b/util/nanoapp_cmd/Android.mk
index 00cac60..b9eb493 100644
--- a/util/nanoapp_cmd/Android.mk
+++ b/util/nanoapp_cmd/Android.mk
@@ -26,7 +26,6 @@
LOCAL_MODULE_TAGS:= optional
LOCAL_MODULE_OWNER := google
-LOCAL_PROPRIETARY_MODULE := true
LOCAL_LDLIBS := \
-L$(SYSROOT)/usr/lib -llog
diff --git a/util/nanoapp_cmd/nanoapp_cmd.c b/util/nanoapp_cmd/nanoapp_cmd.c
index 1cdf9ae..0d36bb8 100644
--- a/util/nanoapp_cmd/nanoapp_cmd.c
+++ b/util/nanoapp_cmd/nanoapp_cmd.c
@@ -29,11 +29,14 @@
#include <sensType.h>
#include <signal.h>
#include <inttypes.h>
+#include <errno.h>
+#define LOG_TAG "nanoapp_cmd"
#define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL
#define SENSOR_RATE_ONESHOT 0xFFFFFF02UL
#define SENSOR_HZ(_hz) ((uint32_t)((_hz) * 1024.0f))
#define MAX_INSTALL_CNT 8
+#define MAX_DOWNLOAD_RETRIES 3
enum ConfigCmds
{
@@ -150,7 +153,6 @@
struct AppInfo apps[32];
uint8_t appCount = 0;
char appsToInstall[MAX_INSTALL_CNT][32];
-uint8_t installCnt = 0;
void sig_handle(__attribute__((unused)) int sig)
{
@@ -159,6 +161,16 @@
stop = true;
}
+FILE *openFile(const char *fname, const char *mode)
+{
+ FILE *f = fopen(fname, mode);
+ if (f == NULL) {
+ __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Failed to open %s: err=%d [%s]", fname, errno, strerror(errno));
+ printf("\nFailed to open %s: err=%d [%s]\n", fname, errno, strerror(errno));
+ }
+ return f;
+}
+
void parseInstalledAppInfo()
{
FILE *fp;
@@ -166,12 +178,9 @@
size_t len;
ssize_t numRead;
- fp = fopen("/sys/class/nanohub/nanohub/app_info", "r");
- if (fp == NULL) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to open app_info!");
- printf("Failed to open app_info!\n");
+ fp = openFile("/sys/class/nanohub/nanohub/app_info", "r");
+ if (!fp)
return;
- }
while ((numRead = getline(&line, &len, fp)) != -1) {
struct AppInfo *currApp = &apps[appCount++];
@@ -197,19 +206,17 @@
return NULL;
}
-void parseConfigAppInfo()
+int parseConfigAppInfo()
{
FILE *fp;
char *line = NULL;
size_t len;
ssize_t numRead;
+ int installCnt;
- fp = fopen("/vendor/firmware/napp_list.cfg", "r");
- if (fp == NULL) {
- __android_log_write(ANDROID_LOG_WARN, "nanoapp_cmd", "Could not find napp_list.cfg!");
- printf("Could not open napp_list.cfg!\n");
- return;
- }
+ fp = openFile("/vendor/firmware/napp_list.cfg", "r");
+ if (!fp)
+ return -1;
parseInstalledAppInfo();
@@ -231,76 +238,70 @@
if (line)
free(line);
+
+ return installCnt;
+}
+
+bool fileWriteData(const char *fname, const void *data, size_t size)
+{
+ int fd;
+ bool result;
+
+ fd = open(fname, O_WRONLY);
+ if (fd < 0) {
+ __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Failed to open %s: err=%d [%s]", fname, errno, strerror(errno));
+ printf("\nFailed to open %s: err=%d [%s]\n", fname, errno, strerror(errno));
+ return false;
+ }
+
+ result = true;
+ if ((size_t)write(fd, data, size) != size) {
+ __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Failed to write to %s; err=%d [%s]", fname, errno, strerror(errno));
+ printf("\nFailed to write %s; err=%d [%s]\n", fname, errno, strerror(errno));
+ result = false;
+ }
+ close(fd);
+
+ return result;
}
void downloadNanohub()
{
- int fd;
char c = '1';
- fd = open("/sys/class/nanohub/nanohub/download_bl", O_WRONLY);
- if (fd < 0) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to open download_bl!");
- printf("Failed to open download_bl!\n");
- return;
- }
-
- if (write(fd, &c, 1) <= 0) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to write download_bl!");
- printf("Failed to write download_bl!\n");
- }
- close(fd);
+ printf("Updating nanohub OS [if required]...");
+ fflush(stdout);
+ if (fileWriteData("/sys/class/nanohub/nanohub/download_bl", &c, sizeof(c)))
+ printf("done\n");
}
-void downloadApps()
+void downloadApps(int updateCnt)
{
- int fd;
- ssize_t ret;
- uint8_t i;
+ int i;
- fd = open("/sys/class/nanohub/nanohub/download_app", O_WRONLY);
- if (fd < 0) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to open download_app!");
- printf("Failed to open download_app!\n");
- return;
+ for (i = 0; i < updateCnt; i++) {
+ printf("Downloading \"%s.napp\"...", appsToInstall[i]);
+ fflush(stdout);
+ if (fileWriteData("/sys/class/nanohub/nanohub/download_app", appsToInstall[i], strlen(appsToInstall[i])))
+ printf("done\n");
}
-
- for (i = 0; i < installCnt; i++) {
- printf("Downloading \"%s.napp\"...\n", appsToInstall[i]);
- ret = write(fd, appsToInstall[i], strlen(appsToInstall[i]));
- if (ret < 0) {
- __android_log_print(ANDROID_LOG_WARN, "nanoapp_cmd", "Failed to download %s.napp!\n", appsToInstall[i]);
- printf("Failed to download %s.napp!\n", appsToInstall[i]);
- }
- }
-
- close(fd);
}
void resetHub()
{
- int fd;
char c = '1';
- fd = open("/sys/class/nanohub/nanohub/reset", O_WRONLY);
- if (fd < 0) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to open reset sysfs file!");
- printf("Failed to open reset sysfs file!\n");
- return;
- }
-
- if (write(fd, &c, 1) <= 0) {
- __android_log_write(ANDROID_LOG_ERROR, "nanoapp_cmd", "Failed to write reset sysfs file!");
- printf("Failed to write reset sysfs file!\n");
- }
- close(fd);
+ printf("Resetting nanohub...");
+ fflush(stdout);
+ if (fileWriteData("/sys/class/nanohub/nanohub/reset", &c, sizeof(c)))
+ printf("done\n");
}
int main(int argc, char *argv[])
{
struct ConfigCmd mConfigCmd;
int fd;
- int ret;
+ int i;
if (argc < 3 && strcmp(argv[1], "download") != 0) {
printf("usage: %s <action> <sensor> <data> -d\n", argv[0]);
@@ -377,22 +378,28 @@
return 1;
}
downloadNanohub();
- parseConfigAppInfo();
- if (installCnt) {
- downloadApps();
- resetHub();
+ for (i = 0; i < MAX_DOWNLOAD_RETRIES; i++) {
+ int updateCnt = parseConfigAppInfo();
+ if (updateCnt > 0) {
+ downloadApps(updateCnt);
+ resetHub();
+ } else if (!updateCnt){
+ return 0;
+ }
}
- return 0;
+
+ if (parseConfigAppInfo() != 0) {
+ __android_log_write(ANDROID_LOG_ERROR, LOG_TAG, "Failed to download all apps!");
+ printf("Failed to download all apps!\n");
+ }
+ return 1;
} else {
printf("Unsupported action: %s\n", argv[1]);
return 1;
}
- fd = open("/dev/nanohub", O_RDWR);
- do {
- ret = write(fd, &mConfigCmd, sizeof(mConfigCmd));
- } while (ret < 0);
- close(fd);
+ while (!fileWriteData("/dev/nanohub", &mConfigCmd, sizeof(mConfigCmd)))
+ continue;
if (drain) {
signal(SIGINT, sig_handle);
diff --git a/util/nanoapp_encr/Android.mk b/util/nanoapp_encr/Android.mk
new file mode 100644
index 0000000..4d64acc
--- /dev/null
+++ b/util/nanoapp_encr/Android.mk
@@ -0,0 +1,45 @@
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+
+LOCAL_SRC_FILES := \
+ ../../lib/nanohub/aes.c \
+ ../../lib/nanohub/sha2.c \
+ ../../lib/nanohub/nanoapp.c \
+ nanoapp_encr.c \
+
+
+LOCAL_CFLAGS := \
+ -Wall \
+ -Werror \
+ -Wextra \
+ -DHOST_BUILD \
+ -DBOOTLOADER= \
+ -DBOOTLOADER_RO= \
+
+
+LOCAL_C_INCLUDES := \
+ device/google/contexthub/lib/include \
+
+LOCAL_MODULE := nanoapp_encr
+
+LOCAL_MODULE_TAGS := optional
+
+include $(BUILD_HOST_EXECUTABLE)
diff --git a/util/nanoapp_encr/Makefile b/util/nanoapp_encr/Makefile
index eb920b8..d55669e 100644
--- a/util/nanoapp_encr/Makefile
+++ b/util/nanoapp_encr/Makefile
@@ -15,12 +15,14 @@
#
APP = nanoapp_encr
-SRC = nanoapp_encr.c ../../firmware/src/aes.c
+SRC = nanoapp_encr.c ../../lib/nanohub/aes.c ../../lib/nanohub/sha2.c
CC ?= gcc
CC_FLAGS = -Wall -Wextra -Werror
$(APP): $(SRC) Makefile
- $(CC) $(CC_FLAGS) -o $(APP) -O2 $(SRC) -I../../firmware/inc -DHOST_BUILD -DBOOTLOADER= -DBOOTLOADER_RO=
+ $(CC) $(CC_FLAGS) -o $(APP) -O2 $(SRC) \
+ -I../../lib/include \
+ -DHOST_BUILD -DBOOTLOADER= -DBOOTLOADER_RO=
clean:
rm -f $(APP)
diff --git a/util/nanoapp_encr/nanoapp_encr.c b/util/nanoapp_encr/nanoapp_encr.c
index b92ece5..231266a 100644
--- a/util/nanoapp_encr/nanoapp_encr.c
+++ b/util/nanoapp_encr/nanoapp_encr.c
@@ -19,33 +19,15 @@
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
-#include <aes.h>
+#include <inttypes.h>
+#include <nanohub/aes.h>
+#include <nanohub/sha2.h>
+#include <nanohub/nanohub.h>
+#include <nanohub/nanoapp.h>
static FILE* urandom = NULL;
-
-static bool readFile(void *dst, uint32_t len, const char *fileName)
-{
- FILE *f = fopen(fileName, "rb");
- bool ret = false;
-
- if (!f)
- return false;
-
- if (len != fread(dst, 1, len, f))
- goto out;
-
- if (fread(&len, 1, 1, f)) //make sure file is actually over
- goto out;
-
- ret = true;
-
-out:
- fclose(f);
- return ret;
-}
-
static void cleanup(void)
{
if (urandom)
@@ -58,7 +40,7 @@
urandom = fopen("/dev/urandom", "rb");
if (!urandom) {
fprintf(stderr, "Failed to open /dev/urandom. Cannot procceed!\n");
- exit(-2);
+ exit(2);
}
//it might not matter, but we still like to try to cleanup after ourselves
@@ -67,163 +49,302 @@
if (len != fread(dst, 1, len, urandom)) {
fprintf(stderr, "Failed to read /dev/urandom. Cannot procceed!\n");
- exit(-3);
+ exit(2);
}
}
-static void appendToBuf(void **inputDataP, uint32_t *inputLenP, uint32_t *inputBufLenP, uint8_t c)
+static int handleEncrypt(uint8_t **pbuf, uint32_t bufUsed, FILE *out, uint64_t keyId, uint32_t *key)
{
- if (*inputLenP == *inputBufLenP) {
- uint32_t newSz = ((uint64_t)*inputBufLenP * 17) / 16 + 1;
- void *ptr = realloc(*inputDataP, newSz);
- if (!ptr) {
- fprintf(stderr, "Failed to realloc a buffer from %lub to %lub\n", (unsigned long)*inputBufLenP, (unsigned long)newSz);
- exit(-5);
+ uint32_t i;
+ struct AesCbcContext ctx;
+ struct ImageHeader *image;
+ uint32_t *data;
+ struct Sha2state shaState;
+ bool err = false;
+ struct AppSecEncrHdr encr;
+ uint32_t padLen = 0;
+ uint8_t *buf = *pbuf;
+
+ encr.keyID = keyId;
+
+//FIXME: compatibility: all the devices has google secret key with id 1, so we
+// can't simply change and enforce new key naming policy;
+// first, key upload mechanism shall start working, and then we can have
+// all the policies we want; for now, disable enforcement
+
+// if (encr.keyID <= 0xFFFF)
+// encr.keyID = AES_KEY_ID(encr.keyID);
+
+ fprintf(stderr, "Using Key ID: %016" PRIX64 "\n", encr.keyID);
+ rand_bytes(encr.IV, sizeof(encr.IV));
+ printHash(stderr, "Using IV", encr.IV, AES_BLOCK_WORDS);
+
+ if (bufUsed <= sizeof(*image)) {
+ fprintf(stderr, "Input file is too small\n");
+ return 2;
+ }
+
+ encr.dataLen = bufUsed;
+
+ if (((bufUsed - sizeof(*image)) % AES_BLOCK_SIZE) != 0)
+ padLen = AES_BLOCK_SIZE - ((bufUsed - sizeof(*image)) % AES_BLOCK_SIZE);
+
+ if (padLen) {
+ reallocOrDie(buf, bufUsed + padLen);
+ rand_bytes(buf + bufUsed, padLen);
+ bufUsed += padLen;
+ fprintf(stderr, "Padded to %" PRIu32 " bytes\n", bufUsed);
+ *pbuf = buf;
+ }
+
+ image = (struct ImageHeader *)buf;
+
+ if (bufUsed >= sizeof(*image) && image->aosp.magic == NANOAPP_AOSP_MAGIC &&
+ image->aosp.header_version == 1 && image->layout.magic == GOOGLE_LAYOUT_MAGIC) {
+ fprintf(stderr, "Found AOSP header\n");
+ } else {
+ fprintf(stderr, "Unknown binary format\n");
+ return 2;
+ }
+
+ if ((image->aosp.flags & NANOAPP_SIGNED_FLAG) != 0) {
+ fprintf(stderr, "data is marked as signed; encryption is not possible for signed data\n");
+ return 2;
+ }
+ if ((image->aosp.flags & NANOAPP_ENCRYPTED_FLAG) != 0) {
+ fprintf(stderr, "data is marked as encrypted; encryption is not possible for encrypted data\n");
+ return 2;
+ }
+
+ image->aosp.flags |= NANOAPP_ENCRYPTED_FLAG;
+ fwrite(image, sizeof(*image), 1, out);
+ data = (uint32_t *)(image + 1);
+ fprintf(stderr, "orig len: %" PRIu32 " bytes\n", encr.dataLen);
+ bufUsed -= sizeof(*image);
+ encr.dataLen -= sizeof(*image);
+ fwrite(&encr, sizeof(encr), 1, out);
+ sha2init(&shaState);
+
+ //encrypt and emit data
+ aesCbcInitForEncr(&ctx, key, encr.IV);
+ uint32_t outBuf[AES_BLOCK_WORDS];
+ for (i = 0; i < bufUsed/sizeof(uint32_t); i += AES_BLOCK_WORDS) {
+ aesCbcEncr(&ctx, data + i, outBuf);
+ int32_t sz = encr.dataLen - (i * sizeof(uint32_t));
+ sz = sz > AES_BLOCK_SIZE ? AES_BLOCK_SIZE : sz;
+ if (sz > 0) {
+ sha2processBytes(&shaState, data + i, sz);
+ fwrite(outBuf, AES_BLOCK_SIZE, 1, out);
}
- *inputDataP = ptr;
- *inputBufLenP = newSz;
}
- ((uint8_t*)*inputDataP)[(*inputLenP)++] = c;;
+ const uint32_t *hash = sha2finish(&shaState);
+
+ printHash(stderr, "HASH", hash, SHA2_HASH_WORDS);
+
+ // finally, encrypt and output SHA2 hash
+ aesCbcEncr(&ctx, hash, outBuf);
+ fwrite(outBuf, AES_BLOCK_SIZE, 1, out);
+ aesCbcEncr(&ctx, hash + AES_BLOCK_WORDS, outBuf);
+ err = fwrite(outBuf, AES_BLOCK_SIZE, 1, out) != 1;
+
+ return err ? 2 : 0;
}
-static void readInput(void **inputDataP, uint32_t *inputLenP, uint32_t *inputBufLenP)
+static int handleDecrypt(uint8_t **pbuf, uint32_t bufUsed, FILE *out, uint32_t *key)
{
- int c;
+ struct AesCbcContext ctx;
+ struct ImageHeader *image;
+ struct Sha2state shaState;
+ struct AppSecEncrHdr *encr;
+ uint32_t *data;
+ bool err = false;
+ uint32_t fileHash[((SHA2_HASH_WORDS + AES_BLOCK_WORDS - 1) / AES_BLOCK_WORDS) * AES_BLOCK_WORDS], fileHashSz;
+ uint32_t outBuf[AES_BLOCK_WORDS];
+ uint32_t i;
+ uint8_t *buf = *pbuf;
- while ((c = getchar()) != EOF)
- appendToBuf(inputDataP, inputLenP, inputBufLenP, c);
+ //parse header
+ image = (struct ImageHeader*)buf;
+ if (bufUsed >= (sizeof(*image) + sizeof(*encr)) &&
+ image->aosp.header_version == 1 && image->aosp.magic == NANOAPP_AOSP_MAGIC &&
+ image->layout.magic == GOOGLE_LAYOUT_MAGIC) {
+ fprintf(stderr, "Found AOSP header\n");
+ if (!(image->aosp.flags & NANOAPP_ENCRYPTED_FLAG)) {
+ fprintf(stderr, "data is not marked as encrypted; can't decrypt\n");
+ return 2;
+ }
+ image->aosp.flags &= ~NANOAPP_ENCRYPTED_FLAG;
+ data = (uint32_t *)(image + 1);
+ encr = (struct AppSecEncrHdr *)data;
+ data = (uint32_t *)(encr + 1);
+ bufUsed -= sizeof(*image) + sizeof(*encr);
+ } else {
+ fprintf(stderr, "Unknown binary format\n");
+ return 2;
+ }
+
+ if (encr->dataLen > bufUsed) {
+ fprintf(stderr, "Claimed output size of %" PRIu32 "b invalid\n", encr->dataLen);
+ return 2;
+ }
+ fprintf(stderr, "Original size %" PRIu32 "b (%" PRIu32 "b of padding present)\n",
+ encr->dataLen, bufUsed - encr->dataLen);
+ if (!encr->keyID) {
+ fprintf(stderr, "Input data has invalid key ID\n");
+ return 2;
+ }
+ fprintf(stderr, "Using Key ID: %016" PRIX64 "\n", encr->keyID);
+ printHash(stderr, "Using IV", encr->IV, AES_BLOCK_WORDS);
+
+ fwrite(image, sizeof(*image), 1, out);
+ //decrypt and emit data
+ aesCbcInitForDecr(&ctx, key, encr->IV);
+ fileHashSz = 0;
+ sha2init(&shaState);
+ for (i = 0; i < bufUsed / sizeof(uint32_t); i += AES_BLOCK_WORDS) {
+ int32_t size = encr->dataLen - i * sizeof(uint32_t);
+ aesCbcDecr(&ctx, data + i, outBuf);
+ if (size > AES_BLOCK_SIZE)
+ size = AES_BLOCK_SIZE;
+ if (size > 0) {
+ sha2processBytes(&shaState, outBuf, size);
+ err = fwrite(outBuf, size, 1, out) != 1;
+ } else if (fileHashSz < sizeof(fileHash)) {
+ memcpy(((uint8_t*)fileHash) + fileHashSz, outBuf, AES_BLOCK_SIZE);
+ fileHashSz += AES_BLOCK_SIZE;
+ } else {
+ fprintf(stderr, "Too much input data\n");
+ return 2;
+ }
+ }
+ const uint32_t *calcHash = sha2finish(&shaState);
+ printHash(stderr, "HASH [calc]", calcHash, SHA2_HASH_WORDS);
+ printHash(stderr, "HASH [file]", fileHash, SHA2_HASH_WORDS);
+
+ bool verify = memcmp(fileHash, calcHash, SHA2_HASH_SIZE) == 0;
+ fprintf(stderr, "hash verification: %s\n", verify ? "passed" : "failed");
+ if (!verify)
+ return 2;
+
+ if (!err)
+ fprintf(stderr, "Done\n");
+
+ return err ? 2 : 0;
+}
+
+static void fatalUsage(const char *name, const char *msg, const char *arg)
+{
+ if (msg && arg)
+ fprintf(stderr, "Error: %s: %s\n\n", msg, arg);
+ else if (msg)
+ fprintf(stderr, "Error: %s\n\n", msg);
+
+ fprintf(stderr, "USAGE: %s [-e] [-d] [-i <key id>] [-k <key file>] <input file> [<output file>]\n"
+ " -i : 64-bit hex number != 0\n"
+ " -e : encrypt post-processed file\n"
+ " -d : decrypt encrypted post-processed file\n"
+ " -k : binary file (32 byte size) containing AES-256 secret key\n"
+ , name);
+ exit(1);
}
int main(int argc, char **argv)
{
- uint32_t *inputData = NULL, inputLen = 0, inputBufLen = 0, origLen;
- uint32_t i, j, iv[AES_BLOCK_WORDS], key[AES_KEY_WORDS];
- const char *selfExeName = argv[0];
- struct AesCbcContext ctx;
- uint64_t keyId;
- uint8_t tmp;
+ uint32_t bufUsed = 0;
+ uint8_t *buf = NULL;
+ uint64_t keyId = 0;
+ int ret = -1;
+ uint32_t *u32Arg = NULL;
+ uint64_t *u64Arg = NULL;
+ const char **strArg = NULL;
+ const char *appName = argv[0];
+ const char *posArg[2] = { NULL };
+ uint32_t posArgCnt = 0;
+ FILE *out = NULL;
+ const char *prev = NULL;
+ bool decrypt = false;
+ bool encrypt = false;
+ const char *keyFile = NULL;
+ int multi = 0;
+ uint32_t key[AES_KEY_WORDS];
- if (argc == 4 && !strcmp(argv[1], "encr")) {
-
- if (argv[2][0] == '0' && argv[2][1] == 'x')
- keyId = strtoull(argv[2] + 2, NULL, 16);
- else
- keyId = strtoull(argv[2], NULL, 10);
-
- if (!keyId) {
- fprintf(stderr, "Key ID cannot be zero (given '%s')\n", argv[2]);
- goto usage;
- }
-
- fprintf(stderr, "Using Key ID of 0x%08lX%08lX\n", (unsigned long)(keyId >> 32), (unsigned long)(keyId & 0xffffffffull));
- rand_bytes(iv, sizeof(iv));
- fprintf(stderr, "Using IV '");
- for (i = 0; i < AES_BLOCK_WORDS; i++)
- fprintf(stderr, "%08lX", (unsigned long)iv[i]);
- fprintf(stderr, "'\n");
-
- //read key
- if (!readFile(key, sizeof(key), argv[3])) {
- fprintf(stderr, "Key file '%s' does not exist or is not %u bytes\n", argv[3], (unsigned int)sizeof(key));
- goto usage;
- }
-
- //read data
- readInput((void**)&inputData, &inputLen, &inputBufLen);
- origLen = inputLen;
- fprintf(stderr, "Read %lu bytes\n", (unsigned long)origLen);
- while (inputLen & 15) { //round to 16 bytes
- rand_bytes(&tmp, 1);
- appendToBuf((void**)&inputData, &inputLen, &inputBufLen, tmp);
- }
- fprintf(stderr, "Padded to %lu bytes\n", (unsigned long)inputLen);
-
- //emit header
- fwrite("Encr", 4, 1, stdout);
- fwrite(&origLen, 4, 1, stdout);
- fwrite(&keyId, 8, 1, stdout);
- for (i = 0; i < AES_BLOCK_WORDS; i++)
- fwrite(iv + i, 4, 1, stdout);
-
- //encrypt and emit data
- aesCbcInitForEncr(&ctx, key, iv); //todo
- for (i = 0; i < inputLen / sizeof(uint32_t); i += AES_BLOCK_WORDS) {
- uint32_t out[AES_BLOCK_WORDS];
- aesCbcEncr(&ctx, inputData + i, out);
- for (j = 0; j < AES_BLOCK_WORDS; j++)
- fwrite(out + j, 4, 1, stdout);
- }
-
- fprintf(stderr, "Done\n");
- }
- else if (argc == 3 && !strcmp(argv[1], "decr")) {
- //read key
- if (!readFile(key, sizeof(key), argv[2])) {
- fprintf(stderr, "Key file '%s' does not exist or is not %u bytes\n", argv[3], (unsigned int)sizeof(key));
- goto usage;
- }
-
- //read data
- readInput((void**)&inputData, &inputLen, &inputBufLen);
- origLen = inputLen;
- fprintf(stderr, "Read %lu bytes\n", (unsigned long)origLen);
-
- if (inputLen < 32) {
- fprintf(stderr, "Implausibly small input\n");
- goto usage;
- }
-
- //parse header
- if (memcmp(inputData, "Encr", 4)) {
- fprintf(stderr, "Input data lacks 'Encr' header\n");
- goto usage;
- }
- origLen = inputData[1];
- if (origLen > inputLen - 32) {
- fprintf(stderr, "Claimed output size of %lub invalid\n", (unsigned long)origLen);
- goto usage;
- }
- fprintf(stderr, "Original size %lub (%lub of padding present)\n", (unsigned long)origLen, (unsigned long)(inputLen - 32 - origLen));
- keyId = *(uint64_t*)(inputData + 2);
- if (!keyId) {
- fprintf(stderr, "Input data has invaid key ID\n");
- goto usage;
- }
- fprintf(stderr, "Using Key ID of 0x%08lX%08lX\n", (unsigned long)(keyId >> 32), (unsigned long)(keyId & 0xffffffffull));
- for (i = 0; i < AES_BLOCK_WORDS; i++)
- iv[i] = inputData[4 + i];
- fprintf(stderr, "Using IV '");
- for (i = 0; i < AES_BLOCK_WORDS; i++)
- fprintf(stderr, "%08lX", (unsigned long)iv[i]);
- fprintf(stderr, "'\n");
-
- //decrypt and emit data
- aesCbcInitForDecr(&ctx, key, iv); //todo
- for (i = 0; i < (inputLen - 32)/ sizeof(uint32_t); i += AES_BLOCK_WORDS) {
- uint32_t out[AES_BLOCK_WORDS];
- aesCbcDecr(&ctx, inputData + i + 8, out);
- for (j = 0; j < AES_BLOCK_WORDS; j++) {
- uint32_t now = origLen >= 4 ? 4 : origLen;
- fwrite(out + j, now, 1, stdout);
- origLen -= now;
+ for (int i = 1; i < argc; i++) {
+ char *end = NULL;
+ if (argv[i][0] == '-') {
+ prev = argv[i];
+ if (!strcmp(argv[i], "-d"))
+ decrypt = true;
+ else if (!strcmp(argv[i], "-e"))
+ encrypt = true;
+ else if (!strcmp(argv[i], "-k"))
+ strArg = &keyFile;
+ else if (!strcmp(argv[i], "-i"))
+ u64Arg = &keyId;
+ else
+ fatalUsage(appName, "unknown argument", argv[i]);
+ } else {
+ if (u64Arg) {
+ uint64_t tmp = strtoull(argv[i], &end, 16);
+ if (*end == '\0')
+ *u64Arg = tmp;
+ u64Arg = NULL;
+ } else if (u32Arg) {
+ uint32_t tmp = strtoul(argv[i], &end, 16);
+ if (*end == '\0')
+ *u32Arg = tmp;
+ u32Arg = NULL;
+ } else if (strArg) {
+ *strArg = argv[i];
+ strArg = NULL;
+ } else {
+ if (posArgCnt < 2)
+ posArg[posArgCnt++] = argv[i];
+ else
+ fatalUsage(appName, "too many positional arguments", argv[i]);
}
+ prev = 0;
}
-
- fprintf(stderr, "Done\n");
}
+ if (prev)
+ fatalUsage(appName, "missing argument after", prev);
+
+ if (!posArgCnt)
+ fatalUsage(appName, "missing input file name", NULL);
+
+ if (encrypt)
+ multi++;
+ if (decrypt)
+ multi++;
+
+ if (multi != 1)
+ fatalUsage(appName, "select either -d or -e", NULL);
+
+ if (!keyFile)
+ fatalUsage(appName, "no key file given", NULL);
+
+ if (encrypt && !keyId)
+ fatalUsage(appName, "Non-zero Key ID must be given to encrypt data", NULL);
+
+ //read key
+ if (!readFile(key, sizeof(key), keyFile))
+ fatalUsage(appName, "Key file does not exist or has incorrect size", keyFile);
+
+ buf = loadFile(posArg[0], &bufUsed);
+ fprintf(stderr, "Read %" PRIu32 " bytes\n", bufUsed);
+
+ if (!posArg[1])
+ out = stdout;
else
- goto usage;
- free(inputData);
- return 0;
+ out = fopen(posArg[1], "w");
+ if (!out)
+ fatalUsage(appName, "failed to create/open output file", posArg[1]);
-usage:
- fprintf(stderr, "USAGE: %s encr <KEY_ID> <KEY_FILE>< data_to_encr > file_out\n"
- " %s decr <KEY_FILE> < data_to_decr > file_out\n",
- selfExeName, selfExeName);
- free(inputData);
- return -1;
+ if (encrypt)
+ ret = handleEncrypt(&buf, bufUsed, out, keyId, key);
+ else if (decrypt)
+ ret = handleDecrypt(&buf, bufUsed, out, key);
+
+ free(buf);
+ fclose(out);
+ return ret;
}
-
-
-
-
diff --git a/util/nanoapp_postprocess/Android.mk b/util/nanoapp_postprocess/Android.mk
index f6a10dc..9ed9c63 100644
--- a/util/nanoapp_postprocess/Android.mk
+++ b/util/nanoapp_postprocess/Android.mk
@@ -19,10 +19,14 @@
include $(CLEAR_VARS)
LOCAL_SRC_FILES := \
- postprocess.c
+ postprocess.c \
+ ../../lib/nanohub/nanoapp.c \
LOCAL_CFLAGS := -Wall -Werror -Wextra
+LOCAL_C_INCLUDES += \
+ device/google/contexthub/lib/include \
+
LOCAL_MODULE := nanoapp_postprocess
LOCAL_MODULE_TAGS := optional
diff --git a/util/nanoapp_postprocess/Makefile b/util/nanoapp_postprocess/Makefile
index ef6e596..1b2e518 100644
--- a/util/nanoapp_postprocess/Makefile
+++ b/util/nanoapp_postprocess/Makefile
@@ -17,10 +17,10 @@
APP = nanoapp_postprocess
SRC = postprocess.c
CC ?= gcc
-CC_FLAGS = -Wall -Wextra -Werror
+CC_FLAGS = -Wall -Wextra -Werror -I../../lib/include
$(APP): $(SRC) Makefile
$(CC) $(CC_FLAGS) -o $(APP) -O2 $(SRC)
clean:
- rm -f $(APP)
+ rm -f $(APP)
\ No newline at end of file
diff --git a/util/nanoapp_postprocess/postprocess.c b/util/nanoapp_postprocess/postprocess.c
index 5d527b1..88dd556 100644
--- a/util/nanoapp_postprocess/postprocess.c
+++ b/util/nanoapp_postprocess/postprocess.c
@@ -14,7 +14,6 @@
* limitations under the License.
*/
-#include "../../firmware/inc/cpu/cortexm4f/appRelocFormat.h"
#include <assert.h>
#include <sys/types.h>
#include <stdbool.h>
@@ -23,15 +22,15 @@
#include <string.h>
#include <stdint.h>
#include <stdio.h>
+#include <stddef.h>
+#include <errno.h>
+
+#include <nanohub/nanohub.h>
+#include <nanohub/nanoapp.h>
+#include <nanohub/appRelocFormat.h>
//This code assumes it is run on a LE CPU with unaligned access abilities. Sorry.
-
-#define APP_MAGIX_0 0x676f6f47
-#define APP_MAGIX_1 0x614e656c
-#define APP_MAGIX_2 0x70416f6e
-#define APP_MAGIX_3 0xffff0070
-
#define FLASH_BASE 0x10000000
#define RAM_BASE 0x80000000
@@ -49,32 +48,6 @@
#define NANO_RELOC_TYPE_FLASH 1
#define NANO_RELOC_LAST 2 //must be <= (RELOC_TYPE_MASK >> RELOC_TYPE_SHIFT)
-
-struct AppHeader {
- uint32_t magic[4];
-
- uint32_t appID[2];
-
- uint32_t __data_start;
- uint32_t __data_end;
- uint32_t __data_data;
-
- uint32_t __bss_start;
- uint32_t __bss_end;
-
- uint32_t __got_start;
- uint32_t __got_end;
- uint32_t __rel_start;
- uint32_t __rel_end;
-
- uint32_t version;
- uint32_t rfu;
-
- uint32_t start_task;
- uint32_t end_task;
- uint32_t handle_event;
-};
-
struct RelocEntry {
uint32_t where;
uint32_t info; //bottom 8 bits is type, top 24 is sym idx
@@ -91,101 +64,87 @@
uint32_t b, c;
};
-
-
struct NanoRelocEntry {
uint32_t ofstInRam;
uint8_t type;
};
+static void fatalUsage(const char *name, const char *msg, const char *arg)
+{
+ if (msg && arg)
+ fprintf(stderr, "Error: %s: %s\n\n", msg, arg);
+ else if (msg)
+ fprintf(stderr, "Error: %s\n\n", msg);
-int main(int argc, char **argv)
+ fprintf(stderr, "USAGE: %s [-v] [-k <key id>] [-a <app id>] [-r] [-n <layout name>] [-i <layout id>] <input file> [<output file>]\n"
+ " -v : be verbose\n"
+ " -n <layout name> : app, os, key\n"
+ " -i <layout id> : 1 (app), 2 (key), 3 (os)\n"
+ " -f <layout flags>: 16-bit hex value, stored as layout-specific flags\n"
+ " -a <app ID> : 64-bit hex number != 0\n"
+ " -k <key ID> : 64-bit hex number != 0\n"
+ " -r : bare (no AOSP header); used only for inner OS image generation\n"
+ " layout ID and layout name control the same parameter, so only one of them needs to be used\n"
+ , name);
+ exit(1);
+}
+
+static int handleApp(uint8_t **pbuf, uint32_t bufUsed, FILE *out, uint32_t layoutFlags, uint64_t appId, bool verbose)
{
uint32_t i, numRelocs, numSyms, outNumRelocs = 0, packedNanoRelocSz, j, k, lastOutType = 0, origin = 0;
struct NanoRelocEntry *nanoRelocs = NULL;
struct RelocEntry *relocs;
struct SymtabEntry *syms;
uint8_t *packedNanoRelocs;
- uint32_t t, bufUsed = 0;
- struct AppHeader *hdr;
- bool verbose = false;
- uint8_t *buf = NULL;
- uint32_t bufSz = 0;
- uint64_t appId = 0;
- int c, ret = -1;
+ uint32_t t;
+ struct BinHdr *bin;
+ int ret = -1;
+ struct SectInfo *sect;
+ struct AppInfo app;
+ uint8_t *buf = *pbuf;
+ uint32_t bufSz = bufUsed * 3 /2;
-
- argc--;
- assert(argc >= 0);
- argv++;
-
- for (i = 0; i < (uint32_t)argc; i++) {
- if (!strcmp(argv[i], "-v")) {
- verbose = true;
- continue;
- }
- appId = strtoul(argv[i], NULL, 16);
- }
-
- if (!appId) {
- fprintf(stderr, "USAGE: %s [-v] 0123456789abcdef < app.bin >app.ap\n\twhere 0123456789abcdef is app ID in hex\n", argv[-1]);
- return -2;
- }
-
- // read file
- while ((c = getchar()) != EOF) {
- if (bufSz == bufUsed) {
- uint8_t *t;
- bufSz = (bufSz * 5) / 4 + 1;
- t = realloc(buf, bufSz);
- if (!t) {
- fprintf(stderr, "Realloc to %u fails - you're SOL\n", (unsigned)bufSz);
- goto out;
- }
- buf = t;
- }
- buf[bufUsed++] = c;
- }
-
- //make buffer bigger by 50% in case relocs grow out of hand
- buf = realloc(buf, 3 * bufUsed / 2);
- if (!buf) {
- fprintf(stderr, "MEMERR\n");
- exit(-7);
- }
+ //make buffer 50% bigger than bufUsed in case relocs grow out of hand
+ buf = reallocOrDie(buf, bufSz);
+ *pbuf = buf;
//sanity checks
- hdr = (struct AppHeader*)buf;
- if (bufUsed < sizeof(struct AppHeader)) {
+ bin = (struct BinHdr*)buf;
+ if (bufUsed < sizeof(*bin)) {
fprintf(stderr, "File size too small\n");
goto out;
}
- if (hdr->magic[0] != APP_MAGIX_0 || hdr->magic[1] != APP_MAGIX_1 || hdr->magic[2] != APP_MAGIX_2 || hdr->magic[3] != APP_MAGIX_3) {
- fprintf(stderr, "Magic value wrong\n");
+ if (bin->hdr.magic != NANOAPP_FW_MAGIC) {
+ fprintf(stderr, "Magic value is wrong: found %08" PRIX32
+ "; expected %08" PRIX32 "\n",
+ bin->hdr.magic, NANOAPP_FW_MAGIC);
goto out;
}
+
+ sect = &bin->sect;
+
//do some math
- relocs = (struct RelocEntry*)(buf + hdr->__rel_start - FLASH_BASE);
- syms = (struct SymtabEntry*)(buf + hdr->__rel_end - FLASH_BASE);
- numRelocs = (hdr->__rel_end - hdr->__rel_start) / sizeof(struct RelocEntry);
- numSyms = (bufUsed + FLASH_BASE - hdr->__rel_end) / sizeof(struct SymtabEntry);
+ relocs = (struct RelocEntry*)(buf + sect->rel_start - FLASH_BASE);
+ syms = (struct SymtabEntry*)(buf + sect->rel_end - FLASH_BASE);
+ numRelocs = (sect->rel_end - sect->rel_start) / sizeof(struct RelocEntry);
+ numSyms = (bufUsed + FLASH_BASE - sect->rel_end) / sizeof(struct SymtabEntry);
//sanity
- if (numRelocs * sizeof(struct RelocEntry) + hdr->__rel_start != hdr->__rel_end) {
+ if (numRelocs * sizeof(struct RelocEntry) + sect->rel_start != sect->rel_end) {
fprintf(stderr, "Relocs of nonstandard size\n");
goto out;
}
- if (numSyms * sizeof(struct SymtabEntry) + hdr->__rel_end != bufUsed + FLASH_BASE) {
+ if (numSyms * sizeof(struct SymtabEntry) + sect->rel_end != bufUsed + FLASH_BASE) {
fprintf(stderr, "Syms of nonstandard size\n");
goto out;
}
//show some info
- fprintf(stderr, "\nRead %u bytes of binary.\n", (unsigned)bufUsed);
+ fprintf(stderr, "\nRead %" PRIu32 " bytes of binary.\n", bufUsed);
if (verbose)
- fprintf(stderr, "Found %u relocs and a %u-entry symbol table\n", numRelocs, numSyms);
+ fprintf(stderr, "Found %" PRIu32 " relocs and a %" PRIu32 "-entry symbol table\n", numRelocs, numSyms);
//handle relocs
nanoRelocs = malloc(sizeof(struct NanoRelocEntry[numRelocs]));
@@ -200,44 +159,60 @@
uint32_t *valThereP;
if (whichSym >= numSyms) {
- fprintf(stderr, "Reloc %u references a nonexistent symbol!\nINFO:\n\tWhere: 0x%08X\n\ttype: %u\n\tsym: %u\n",
+ fprintf(stderr, "Reloc %" PRIu32 " references a nonexistent symbol!\n"
+ "INFO:\n"
+ " Where: 0x%08" PRIX32 "\n"
+ " type: %" PRIu32 "\n"
+ " sym: %" PRIu32 "\n",
i, relocs[i].where, relocs[i].info & 0xff, whichSym);
goto out;
}
if (verbose) {
+ const char *seg;
- fprintf(stderr, "Reloc[%3u]:\n {@0x%08X, type %3d, -> sym[%3u]: {@0x%08x}, ",
+ fprintf(stderr, "Reloc[%3" PRIu32 "]:\n {@0x%08" PRIX32 ", type %3" PRIu32 ", -> sym[%3" PRIu32 "]: {@0x%08" PRIX32 "}, ",
i, relocs[i].where, relocs[i].info & 0xff, whichSym, syms[whichSym].addr);
- if (IS_IN_RANGE_E(relocs[i].where, hdr->__bss_start, hdr->__bss_end))
- fprintf(stderr, "in .bss}\n");
- else if (IS_IN_RANGE_E(relocs[i].where, hdr->__data_start, hdr->__data_end))
- fprintf(stderr, "in .data}\n");
- else if (IS_IN_RANGE_E(relocs[i].where, hdr->__got_start, hdr->__got_end))
- fprintf(stderr, "in .got}\n");
- else if (IS_IN_RANGE_E(relocs[i].where, FLASH_BASE, FLASH_BASE + sizeof(struct AppHeader)))
- fprintf(stderr, "in APPHDR}\n");
+ if (IS_IN_RANGE_E(relocs[i].where, sect->bss_start, sect->bss_end))
+ seg = ".bss";
+ else if (IS_IN_RANGE_E(relocs[i].where, sect->data_start, sect->data_end))
+ seg = ".data";
+ else if (IS_IN_RANGE_E(relocs[i].where, sect->got_start, sect->got_end))
+ seg = ".got";
+ else if (IS_IN_RANGE_E(relocs[i].where, FLASH_BASE, FLASH_BASE + sizeof(struct BinHdr)))
+ seg = "APPHDR";
else
- fprintf(stderr, "in ???}\n");
+ seg = "???";
+ fprintf(stderr, "in %s}\n", seg);
}
/* handle relocs inside the header */
- if (IS_IN_FLASH(relocs[i].where) && relocs[i].where - FLASH_BASE < sizeof(struct AppHeader) && relocType == RELOC_TYPE_SECT) {
+ if (IS_IN_FLASH(relocs[i].where) && relocs[i].where - FLASH_BASE < sizeof(struct BinHdr) && relocType == RELOC_TYPE_SECT) {
/* relocs in header are special - runtime corrects for them */
if (syms[whichSym].addr) {
- fprintf(stderr, "Weird in-header sect reloc %u to symbol %u with nonzero addr 0x%08x\n", i, whichSym, syms[whichSym].addr);
+ fprintf(stderr, "Weird in-header sect reloc %" PRIu32 " to symbol %" PRIu32 " with nonzero addr 0x%08" PRIX32 "\n",
+ i, whichSym, syms[whichSym].addr);
goto out;
}
valThereP = (uint32_t*)(buf + relocs[i].where - FLASH_BASE);
if (!IS_IN_FLASH(*valThereP)) {
- fprintf(stderr, "In-header reloc %u of location 0x%08X is outside of FLASH!\nINFO:\n\ttype: %u\n\tsym: %u\n\tSym Addr: 0x%08X\n",
- i, relocs[i].where, relocType, whichSym, syms[whichSym].addr);
+ fprintf(stderr, "In-header reloc %" PRIu32 " of location 0x%08" PRIX32 " is outside of FLASH!\n"
+ "INFO:\n"
+ " type: %" PRIu32 "\n"
+ " sym: %" PRIu32 "\n"
+ " Sym Addr: 0x%08" PRIX32 "\n",
+ i, relocs[i].where, relocType, whichSym, syms[whichSym].addr);
goto out;
}
- *valThereP -= FLASH_BASE;
+ // binary header generated by objcopy, .napp header and final FW header in flash are of different size.
+ // we subtract binary header offset here, so all the entry points are relative to beginning of "sect".
+ // FW will use § as a base to call these vectors; no more problems with different header sizes;
+ // Assumption: offsets between sect & vec, vec & code are the same in all images (or, in a simpler words, { sect, vec, code }
+ // must go together). this is enforced by linker script, and maintained by all tools and FW download code in the OS.
+ *valThereP -= FLASH_BASE + BINARY_RELOC_OFFSET;
if (verbose)
fprintf(stderr, " -> Nano reloc skipped for in-header reloc\n");
@@ -246,12 +221,16 @@
}
if (!IS_IN_RAM(relocs[i].where)) {
- fprintf(stderr, "Reloc %u of location 0x%08X is outside of RAM!\nINFO:\n\ttype: %u\n\tsym: %u\n\tSym Addr: 0x%08X\n",
- i, relocs[i].where, relocType, whichSym, syms[whichSym].addr);
+ fprintf(stderr, "In-header reloc %" PRIu32 " of location 0x%08" PRIX32 " is outside of RAM!\n"
+ "INFO:\n"
+ " type: %" PRIu32 "\n"
+ " sym: %" PRIu32 "\n"
+ " Sym Addr: 0x%08" PRIX32 "\n",
+ i, relocs[i].where, relocType, whichSym, syms[whichSym].addr);
goto out;
}
- valThereP = (uint32_t*)(buf + relocs[i].where + hdr-> __data_data - RAM_BASE - FLASH_BASE);
+ valThereP = (uint32_t*)(buf + relocs[i].where + sect->data_data - RAM_BASE - FLASH_BASE);
nanoRelocs[outNumRelocs].ofstInRam = relocs[i].where - RAM_BASE;
@@ -263,7 +242,7 @@
(*valThereP) += syms[whichSym].addr;
if (IS_IN_FLASH(syms[whichSym].addr)) {
- (*valThereP) -= FLASH_BASE;
+ (*valThereP) -= FLASH_BASE + BINARY_RELOC_OFFSET;
nanoRelocs[outNumRelocs].type = NANO_RELOC_TYPE_FLASH;
}
else if (IS_IN_RAM(syms[whichSym].addr)) {
@@ -271,16 +250,18 @@
nanoRelocs[outNumRelocs].type = NANO_RELOC_TYPE_RAM;
}
else {
- fprintf(stderr, "Weird reloc %u to symbol %u in unknown memory space (addr 0x%08x)\n", i, whichSym, syms[whichSym].addr);
+ fprintf(stderr, "Weird reloc %" PRIu32 " to symbol %" PRIu32 " in unknown memory space (addr 0x%08" PRIX32 ")\n",
+ i, whichSym, syms[whichSym].addr);
goto out;
}
if (verbose)
- fprintf(stderr, " -> Abs reference fixed up 0x%08X -> 0x%08X\n", t, *valThereP);
+ fprintf(stderr, " -> Abs reference fixed up 0x%08" PRIX32 " -> 0x%08" PRIX32 "\n", t, *valThereP);
break;
case RELOC_TYPE_SECT:
if (syms[whichSym].addr) {
- fprintf(stderr, "Weird sect reloc %u to symbol %u with nonzero addr 0x%08x\n", i, whichSym, syms[whichSym].addr);
+ fprintf(stderr, "Weird sect reloc %" PRIu32 " to symbol %" PRIu32 " with nonzero addr 0x%08" PRIX32 "\n",
+ i, whichSym, syms[whichSym].addr);
goto out;
}
@@ -288,32 +269,34 @@
if (IS_IN_FLASH(*valThereP)) {
nanoRelocs[outNumRelocs].type = NANO_RELOC_TYPE_FLASH;
- *valThereP -= FLASH_BASE;
+ *valThereP -= FLASH_BASE + BINARY_RELOC_OFFSET;
}
else if (IS_IN_RAM(*valThereP)) {
nanoRelocs[outNumRelocs].type = NANO_RELOC_TYPE_RAM;
*valThereP -= RAM_BASE;
}
else {
- fprintf(stderr, "Weird sec reloc %u to symbol %u in unknown memory space (addr 0x%08x)\n", i, whichSym, *valThereP);
+ fprintf(stderr, "Weird sec reloc %" PRIu32 " to symbol %" PRIu32
+ " in unknown memory space (addr 0x%08" PRIX32 ")\n",
+ i, whichSym, *valThereP);
goto out;
}
if (verbose)
- fprintf(stderr, " -> Sect reference fixed up 0x%08X -> 0x%08X\n", t, *valThereP);
+ fprintf(stderr, " -> Sect reference fixed up 0x%08" PRIX32 " -> 0x%08" PRIX32 "\n", t, *valThereP);
break;
default:
- fprintf(stderr, "Weird reloc %u type %u to symbol %u\n", i, relocType, whichSym);
+ fprintf(stderr, "Weird reloc %" PRIX32 " type %" PRIX32 " to symbol %" PRIX32 "\n", i, relocType, whichSym);
goto out;
}
if (verbose)
- fprintf(stderr, " -> Nano reloc calculated as 0x%08X,0x%02x\n", nanoRelocs[i].ofstInRam, nanoRelocs[i].type);
+ fprintf(stderr, " -> Nano reloc calculated as 0x%08" PRIX32 ",0x%02" PRIX8 "\n", nanoRelocs[i].ofstInRam, nanoRelocs[i].type);
outNumRelocs++;
}
//sort by type and then offset
- for (i = 0; i < outNumRelocs; i++) {
+ for (i = 0; i < outNumRelocs; i++) {
struct NanoRelocEntry t;
for (k = i, j = k + 1; j < outNumRelocs; j++) {
@@ -327,27 +310,26 @@
memcpy(nanoRelocs + k, &t, sizeof(struct NanoRelocEntry));
if (verbose)
- fprintf(stderr, "SortedReloc[%3u] = {0x%08X,0x%02X}\n", i, nanoRelocs[i].ofstInRam, nanoRelocs[i].type);
+ fprintf(stderr, "SortedReloc[%3" PRIu32 "] = {0x%08" PRIX32 ",0x%02" PRIX8 "}\n", i, nanoRelocs[i].ofstInRam, nanoRelocs[i].type);
}
//produce output nanorelocs in packed format
packedNanoRelocs = malloc(outNumRelocs * 6); //definitely big enough
packedNanoRelocSz = 0;
- for (i = 0; i < outNumRelocs; i++) {
-
+ for (i = 0; i < outNumRelocs; i++) {
uint32_t displacement;
if (lastOutType != nanoRelocs[i].type) { //output type if ti changed
if (nanoRelocs[i].type - lastOutType == 1) {
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_RELOC_TYPE_NEXT;
if (verbose)
- fprintf(stderr, "Out: RelocTC (1) // to 0x%02X\n", nanoRelocs[i].type);
+ fprintf(stderr, "Out: RelocTC (1) // to 0x%02" PRIX8 "\n", nanoRelocs[i].type);
}
else {
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_RELOC_TYPE_CHG;
packedNanoRelocs[packedNanoRelocSz++] = nanoRelocs[i].type - lastOutType - 1;
if (verbose)
- fprintf(stderr, "Out: RelocTC (0x%02X) // to 0x%02X\n", nanoRelocs[i].type - lastOutType - 1, nanoRelocs[i].type);
+ fprintf(stderr, "Out: RelocTC (0x%02" PRIX8 ") // to 0x%02" PRIX8 "\n", (uint8_t)(nanoRelocs[i].type - lastOutType - 1), nanoRelocs[i].type);
}
lastOutType = nanoRelocs[i].type;
origin = 0;
@@ -365,7 +347,7 @@
for (j = 1; j + i < outNumRelocs && j < MAX_RUN_LEN && nanoRelocs[j + i].type == lastOutType && nanoRelocs[j + i].ofstInRam - nanoRelocs[j + i - 1].ofstInRam == 4; j++);
if (j >= MIN_RUN_LEN) {
if (verbose)
- fprintf(stderr, "Out: Reloc0 x%u\n", j);
+ fprintf(stderr, "Out: Reloc0 x%" PRIX32 "\n", j);
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_CONSECUTIVE;
packedNanoRelocs[packedNanoRelocSz++] = j - MIN_RUN_LEN;
origin = nanoRelocs[j + i - 1].ofstInRam + 4; //reset origin to last one
@@ -377,12 +359,12 @@
//produce output
if (displacement <= MAX_8_BIT_NUM) {
if (verbose)
- fprintf(stderr, "Out: Reloc8 0x%02X\n", displacement);
+ fprintf(stderr, "Out: Reloc8 0x%02" PRIX32 "\n", displacement);
packedNanoRelocs[packedNanoRelocSz++] = displacement;
}
else if (displacement <= MAX_16_BIT_NUM) {
if (verbose)
- fprintf(stderr, "Out: Reloc16 0x%06X\n", displacement);
+ fprintf(stderr, "Out: Reloc16 0x%06" PRIX32 "\n", displacement);
displacement -= MAX_8_BIT_NUM;
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_16BIT_OFST;
packedNanoRelocs[packedNanoRelocSz++] = displacement;
@@ -390,7 +372,7 @@
}
else if (displacement <= MAX_24_BIT_NUM) {
if (verbose)
- fprintf(stderr, "Out: Reloc24 0x%08X\n", displacement);
+ fprintf(stderr, "Out: Reloc24 0x%08" PRIX32 "\n", displacement);
displacement -= MAX_16_BIT_NUM;
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_24BIT_OFST;
packedNanoRelocs[packedNanoRelocSz++] = displacement;
@@ -399,7 +381,7 @@
}
else {
if (verbose)
- fprintf(stderr, "Out: Reloc32 0x%08X\n", displacement);
+ fprintf(stderr, "Out: Reloc32 0x%08" PRIX32 "\n", displacement);
packedNanoRelocs[packedNanoRelocSz++] = TOKEN_32BIT_OFST;
packedNanoRelocs[packedNanoRelocSz++] = displacement;
packedNanoRelocs[packedNanoRelocSz++] = displacement >> 8;
@@ -408,74 +390,269 @@
}
}
- //put in app id
- hdr->appID[0] = appId;
- hdr->appID[1] = appId >> 32;
-
//overwrite original relocs and symtab with nanorelocs and adjust sizes
memcpy(relocs, packedNanoRelocs, packedNanoRelocSz);
bufUsed -= sizeof(struct RelocEntry[numRelocs]);
bufUsed -= sizeof(struct SymtabEntry[numSyms]);
bufUsed += packedNanoRelocSz;
- hdr->__rel_end = hdr->__rel_start + packedNanoRelocSz;
+ assertMem(bufUsed, bufSz);
+ sect->rel_end = sect->rel_start + packedNanoRelocSz;
//sanity
- if (hdr->__rel_end - FLASH_BASE != bufUsed) {
+ if (sect->rel_end - FLASH_BASE != bufUsed) {
fprintf(stderr, "Relocs end and file end not coincident\n");
goto out;
}
//adjust headers for easy access (RAM)
- if (!IS_IN_RAM(hdr->__data_start) || !IS_IN_RAM(hdr->__data_end) || !IS_IN_RAM(hdr->__bss_start) || !IS_IN_RAM(hdr->__bss_end) || !IS_IN_RAM(hdr->__got_start) || !IS_IN_RAM(hdr->__got_end)) {
+ if (!IS_IN_RAM(sect->data_start) || !IS_IN_RAM(sect->data_end) || !IS_IN_RAM(sect->bss_start) ||
+ !IS_IN_RAM(sect->bss_end) || !IS_IN_RAM(sect->got_start) || !IS_IN_RAM(sect->got_end)) {
fprintf(stderr, "data, bss, or got not in ram\n");
goto out;
}
- hdr->__data_start -= RAM_BASE;
- hdr->__data_end -= RAM_BASE;
- hdr->__bss_start -= RAM_BASE;
- hdr->__bss_end -= RAM_BASE;
- hdr->__got_start -= RAM_BASE;
- hdr->__got_end -= RAM_BASE;
+ sect->data_start -= RAM_BASE;
+ sect->data_end -= RAM_BASE;
+ sect->bss_start -= RAM_BASE;
+ sect->bss_end -= RAM_BASE;
+ sect->got_start -= RAM_BASE;
+ sect->got_end -= RAM_BASE;
//adjust headers for easy access (FLASH)
- if (!IS_IN_FLASH(hdr->__data_data) || !IS_IN_FLASH(hdr->__rel_start) || !IS_IN_FLASH(hdr->__rel_end)) {
- fprintf(stderr, "data.data, or rel not in ram\n");
+ if (!IS_IN_FLASH(sect->data_data) || !IS_IN_FLASH(sect->rel_start) || !IS_IN_FLASH(sect->rel_end)) {
+ fprintf(stderr, "data.data, or rel not in flash\n");
goto out;
}
- hdr->__data_data -= FLASH_BASE;
- hdr->__rel_start -= FLASH_BASE;
- hdr->__rel_end -= FLASH_BASE;
+ sect->data_data -= FLASH_BASE + BINARY_RELOC_OFFSET;
+ sect->rel_start -= FLASH_BASE + BINARY_RELOC_OFFSET;
+ sect->rel_end -= FLASH_BASE + BINARY_RELOC_OFFSET;
+
+ struct ImageHeader outHeader = {
+ .aosp = (struct nano_app_binary_t) {
+ .header_version = 1,
+ .magic = NANOAPP_AOSP_MAGIC,
+ .app_id = appId,
+ .app_version = bin->hdr.appVer,
+ .flags = 0, // encrypted (1), signed (2) (will be set by other tools)
+ },
+ .layout = (struct ImageLayout) {
+ .magic = GOOGLE_LAYOUT_MAGIC,
+ .version = 1,
+ .payload = LAYOUT_APP,
+ .flags = layoutFlags,
+ },
+ };
+ uint32_t dataOffset = sizeof(outHeader) + sizeof(app);
+ uint32_t hdrDiff = dataOffset - sizeof(*bin);
+ app.sect = bin->sect;
+ app.vec = bin->vec;
+
+ assertMem(bufUsed + hdrDiff, bufSz);
+
+ memmove(buf + dataOffset, buf + sizeof(*bin), bufUsed - sizeof(*bin));
+ bufUsed += hdrDiff;
+ memcpy(buf, &outHeader, sizeof(outHeader));
+ memcpy(buf + sizeof(outHeader), &app, sizeof(app));
+ sect = &app.sect;
//if we have any bytes to output, show stats
if (bufUsed) {
- uint32_t codeAndRoDataSz = hdr->__data_data;
- uint32_t relocsSz = hdr->__rel_end - hdr->__rel_start;
- uint32_t gotSz = hdr->__got_end - hdr->__data_start;
- uint32_t bssSz = hdr->__bss_end - hdr->__bss_start;
+ uint32_t codeAndRoDataSz = sect->data_data;
+ uint32_t relocsSz = sect->rel_end - sect->rel_start;
+ uint32_t gotSz = sect->got_end - sect->data_start;
+ uint32_t bssSz = sect->bss_end - sect->bss_start;
- fprintf(stderr,"Final binary size %u bytes\n", bufUsed);
+ fprintf(stderr,"Final binary size %" PRIu32 " bytes\n", bufUsed);
fprintf(stderr, "\n");
- fprintf(stderr, "\tCode + RO data (flash): %6u bytes\n", (unsigned)codeAndRoDataSz);
- fprintf(stderr, "\tRelocs (flash): %6u bytes\n", (unsigned)relocsSz);
- fprintf(stderr, "\tGOT + RW data (flash & RAM): %6u bytes\n", (unsigned)gotSz);
- fprintf(stderr, "\tBSS (RAM): %6u bytes\n", (unsigned)bssSz);
+ fprintf(stderr, " FW header size (flash): %6zu bytes\n", FLASH_RELOC_OFFSET);
+ fprintf(stderr, " Code + RO data (flash): %6" PRIu32 " bytes\n", codeAndRoDataSz);
+ fprintf(stderr, " Relocs (flash): %6" PRIu32 " bytes\n", relocsSz);
+ fprintf(stderr, " GOT + RW data (flash & RAM): %6" PRIu32 " bytes\n", gotSz);
+ fprintf(stderr, " BSS (RAM): %6" PRIu32 " bytes\n", bssSz);
fprintf(stderr, "\n");
- fprintf(stderr,"Runtime flash use: %u bytes\n", codeAndRoDataSz + relocsSz + gotSz);
- fprintf(stderr,"Runtime RAM use: %u bytes\n", gotSz + bssSz);
+ fprintf(stderr,"Runtime flash use: %" PRIu32 " bytes\n", (uint32_t)(codeAndRoDataSz + relocsSz + gotSz + FLASH_RELOC_OFFSET));
+ fprintf(stderr,"Runtime RAM use: %" PRIu32 " bytes\n", gotSz + bssSz);
}
- //output the data
- for (i = 0; i < bufUsed; i++)
- putchar(buf[i]);
-
- //success!
- ret = 0;
+ ret = fwrite(buf, bufUsed, 1, out) == 1 ? 0 : 2;
+ if (ret)
+ fprintf(stderr, "Failed to write output file: %s\n", strerror(errno));
out:
free(nanoRelocs);
- free(buf);
return ret;
}
+static int handleKey(uint8_t **pbuf, uint32_t bufUsed, FILE *out, uint32_t layoutFlags, uint64_t appId, uint64_t keyId)
+{
+ uint8_t *buf = *pbuf;
+ struct KeyInfo ki = { .data = keyId };
+ bool good = true;
+ struct ImageHeader outHeader = {
+ .aosp = (struct nano_app_binary_t) {
+ .header_version = 1,
+ .magic = NANOAPP_AOSP_MAGIC,
+ .app_id = appId,
+ },
+ .layout = (struct ImageLayout) {
+ .magic = GOOGLE_LAYOUT_MAGIC,
+ .version = 1,
+ .payload = LAYOUT_KEY,
+ .flags = layoutFlags,
+ },
+ };
+ good = good && fwrite(&outHeader, sizeof(outHeader), 1, out) == 1;
+ good = good && fwrite(&ki, sizeof(ki), 1, out) == 1;
+ good = good && fwrite(buf, bufUsed, 1, out) == 1;
+
+ return good ? 0 : 2;
+}
+
+static int handleOs(uint8_t **pbuf, uint32_t bufUsed, FILE *out, uint32_t layoutFlags, bool bare)
+{
+ uint8_t *buf = *pbuf;
+ bool good;
+
+ struct OsUpdateHdr os = {
+ .magic = OS_UPDT_MAGIC,
+ .marker = OS_UPDT_MARKER_INPROGRESS,
+ .size = bufUsed
+ };
+
+ struct ImageHeader outHeader = {
+ .aosp = (struct nano_app_binary_t) {
+ .header_version = 1,
+ .magic = NANOAPP_AOSP_MAGIC,
+ },
+ .layout = (struct ImageLayout) {
+ .magic = GOOGLE_LAYOUT_MAGIC,
+ .version = 1,
+ .payload = LAYOUT_OS,
+ .flags = layoutFlags,
+ },
+ };
+
+ if (!bare)
+ good = fwrite(&outHeader, sizeof(outHeader), 1, out) == 1;
+ else
+ good = fwrite(&os, sizeof(os), 1, out) == 1;
+ good = good && fwrite(buf, bufUsed, 1, out) == 1;
+
+ return good ? 0 : 2;
+}
+
+int main(int argc, char **argv)
+{
+ uint32_t bufUsed = 0;
+ bool verbose = false;
+ uint8_t *buf = NULL;
+ uint64_t appId = 0;
+ uint64_t keyId = 0;
+ uint32_t layoutId = 0;
+ uint32_t layoutFlags = 0;
+ int ret = -1;
+ uint32_t *u32Arg = NULL;
+ uint64_t *u64Arg = NULL;
+ const char **strArg = NULL;
+ const char *appName = argv[0];
+ int posArgCnt = 0;
+ const char *posArg[2] = { NULL };
+ FILE *out = NULL;
+ const char *layoutName = "app";
+ const char *prev = NULL;
+ bool bareData = false;
+
+ for (int i = 1; i < argc; i++) {
+ char *end = NULL;
+ if (argv[i][0] == '-') {
+ prev = argv[i];
+ if (!strcmp(argv[i], "-v"))
+ verbose = true;
+ else if (!strcmp(argv[i], "-r"))
+ bareData = true;
+ else if (!strcmp(argv[i], "-a"))
+ u64Arg = &appId;
+ else if (!strcmp(argv[i], "-k"))
+ u64Arg = &keyId;
+ else if (!strcmp(argv[i], "-n"))
+ strArg = &layoutName;
+ else if (!strcmp(argv[i], "-i"))
+ u32Arg = &layoutId;
+ else if (!strcmp(argv[i], "-f"))
+ u32Arg = &layoutFlags;
+ else
+ fatalUsage(appName, "unknown argument", argv[i]);
+ } else {
+ if (u64Arg) {
+ uint64_t tmp = strtoull(argv[i], &end, 16);
+ if (*end == '\0')
+ *u64Arg = tmp;
+ u64Arg = NULL;
+ } else if (u32Arg) {
+ uint32_t tmp = strtoul(argv[i], &end, 16);
+ if (*end == '\0')
+ *u32Arg = tmp;
+ u32Arg = NULL;
+ } else if (strArg) {
+ *strArg = argv[i];
+ strArg = NULL;
+ } else {
+ if (posArgCnt < 2)
+ posArg[posArgCnt++] = argv[i];
+ else
+ fatalUsage(appName, "too many positional arguments", argv[i]);
+ }
+ prev = NULL;
+ }
+ }
+ if (prev)
+ fatalUsage(appName, "missing argument after", prev);
+
+ if (!posArgCnt)
+ fatalUsage(appName, "missing input file name", NULL);
+
+ if (!layoutId) {
+ if (strcmp(layoutName, "app") == 0)
+ layoutId = LAYOUT_APP;
+ else if (strcmp(layoutName, "os") == 0)
+ layoutId = LAYOUT_OS;
+ else if (strcmp(layoutName, "key") == 0)
+ layoutId = LAYOUT_KEY;
+ else
+ fatalUsage(appName, "Invalid layout name", layoutName);
+ }
+
+ if (layoutId == LAYOUT_APP && !appId)
+ fatalUsage(appName, "App layout requires app ID", NULL);
+ if (layoutId == LAYOUT_KEY && !keyId)
+ fatalUsage(appName, "Key layout requires key ID", NULL);
+ if (layoutId == LAYOUT_OS && (keyId || appId))
+ fatalUsage(appName, "OS layout does not need any ID", NULL);
+
+ buf = loadFile(posArg[0], &bufUsed);
+ fprintf(stderr, "Read %" PRIu32 " bytes\n", bufUsed);
+
+ if (!posArg[1])
+ out = stdout;
+ else
+ out = fopen(posArg[1], "w");
+ if (!out)
+ fatalUsage(appName, "failed to create/open output file", posArg[1]);
+
+ switch(layoutId) {
+ case LAYOUT_APP:
+ ret = handleApp(&buf, bufUsed, out, layoutFlags, appId, verbose);
+ break;
+ case LAYOUT_KEY:
+ ret = handleKey(&buf, bufUsed, out, layoutFlags, appId, keyId);
+ break;
+ case LAYOUT_OS:
+ ret = handleOs(&buf, bufUsed, out, layoutFlags, bareData);
+ break;
+ }
+
+ free(buf);
+ fclose(out);
+ return ret;
+}
diff --git a/util/nanoapp_prepare.sh b/util/nanoapp_prepare.sh
index cb8a535..546b698 100755
--- a/util/nanoapp_prepare.sh
+++ b/util/nanoapp_prepare.sh
@@ -19,60 +19,20 @@
# Exit in error if we use an undefined variable (i.e. commit a typo).
set -u
-terminate() { #cleanup and exit
- rm -rf $stage
- exit $1
-}
-
usage () { #show usage and bail out
echo "USAGE:" >&2
- echo " $1 [-e <ENCR_KEY_NUM> <ENCR_KEY_FILE>] [-s <PRIV_KEY_FILE> <PUB_KEY_FILE> [<SIG_TO_CHAIN_1> [<SIG_TO_CHAIN_2> [...]]]] < app.napp > app.final.napp" >&2
- terminate -1
+ echo " $1 <app.napp> [-e <ENCR_KEY_NUM> <ENCR_KEY_FILE>] [-s <PRIV_KEY_FILE> <PUB_KEY_FILE> [<SIG_TO_CHAIN_1> [<SIG_TO_CHAIN_2> [...]]]]" >&2
+ exit 1
}
-putchar() {
- hexch="0123456789abcdef"
- h=$[$1/16]
- l=$[$1%16]
- h=${hexch:$h:1}
- l=${hexch:$l:1}
- e="\x"$h$l
- echo -ne $e
-}
-
-printhex() {
- w3=$[$1/16777216]
- t=$[$w3*16777216]
- a=$[$1-$t]
-
- w2=$[$a/65536]
- t=$[$w2*65536]
- a=$[$a-$t]
-
- w1=$[$a/256]
- w0=$[$a%256]
-
- putchar $w0
- putchar $w1
- putchar $w2
- putchar $w3
-}
-
-#save args and create temp dir
-stage=$(mktemp -dt "$(basename $0).XXXXXXXXXX")
-args=( "$@" )
-
-#sanity checks (on the user)
-if [ -t 1 ]
-then
- usage $0
+if [ $# -ge 1 ] ; then
+app=${1%.napp}
+shift
+else
+usage $0
fi
-if [ -t 0 ]
-then
- usage $0
-fi
-
+args=( $@ )
#get encryption key if it exists & encrypt app
encr_key_num=""
@@ -92,16 +52,11 @@
usage $0
fi
- nanoapp_encr encr "$encr_key_num" "$encr_key_file" > "$stage/postencr"
+ nanoapp_encr -e -i "$encr_key_num" -k "$encr_key_file" "${app}.napp" "${app}.encr.napp"
+ app="${app}.encr"
fi
fi
-
-#if app is not encrypted, just copy it to staging area
-if [ ! -f "$stage/postencr" ]; then
- cat > "$stage/postencr"
-fi
-
#handle signing
if [ ${#args[@]} -ge 1 ]
then
@@ -124,50 +79,16 @@
i=$[$i+1]
done
- #get and save file size
- signed_sz=$(du -b "$stage/postencr" | cut -f1)
-
- nanoapp_sign sign "$priv1" "$pub1" < "$stage/postencr" > "$stage/sig"
-
- #pad data to 16 bytes
- t=$signed_sz
- while [ $[$t%16] -ne 0 ]
- do
- echo -ne "\0" >> "$stage/postencr"
- t=$(du -b "$stage/postencr" | cut -f1)
- done
-
- #produce signed output
- cat "$stage/postencr" "$stage/sig" "$pub1" > "$stage/signed"
+ nanoapp_sign -s -e "$priv1" -m "$pub1" "${app}.napp" "${app}.sign.napp"
#append remaining chunks
i=3
while [ $i -lt ${#args[@]} ]
do
- cat "${args[$i]}" >> "$stage/signed"
+ cat "${args[$i]}" >> "${app}.sign.napp"
i=$[$i+1]
done
-
- #create header
- num_sigs=$[${#args[@]}-2]
-
- echo -n SigndApp > "$stage/finished"
- printhex $signed_sz >> "$stage/finished"
- printhex $num_sigs >> "$stage/finished"
- echo -ne "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0" >> "$stage/finished"
- cat "$stage/signed" >> "$stage/finished"
else
usage $0
fi
fi
-
-#if app is not signed, just copy it to staging area
-if [ ! -f "$stage/finished" ]; then
- mv "$stage/postencr" "$stage/finished"
-fi
-
-#produce output
-cat "$stage/finished"
-
-terminate 0
-
diff --git a/util/nanoapp_sign/Android.mk b/util/nanoapp_sign/Android.mk
new file mode 100644
index 0000000..4a9cd96
--- /dev/null
+++ b/util/nanoapp_sign/Android.mk
@@ -0,0 +1,46 @@
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+ ../../lib/nanohub/rsa.c \
+ ../../lib/nanohub/sha2.c \
+ ../../lib/nanohub/nanoapp.c \
+ nanoapp_sign.c \
+
+
+LOCAL_CFLAGS := \
+ -Wall \
+ -Werror \
+ -Wextra \
+ -DRSA_SUPPORT_PRIV_OP_BIGRAM \
+ -DHOST_BUILD \
+ -DBOOTLOADER= \
+ -DBOOTLOADER_RO= \
+
+
+LOCAL_C_INCLUDES := \
+ device/google/contexthub/lib/include \
+
+
+LOCAL_MODULE := nanoapp_sign
+
+LOCAL_MODULE_TAGS := optional
+
+include $(BUILD_HOST_EXECUTABLE)
diff --git a/util/nanoapp_sign/Makefile b/util/nanoapp_sign/Makefile
index 480c57f..d3310c3 100644
--- a/util/nanoapp_sign/Makefile
+++ b/util/nanoapp_sign/Makefile
@@ -15,12 +15,14 @@
#
APP = nanoapp_sign
-SRC = nanoapp_sign.c ../../firmware/src/rsa.c ../../firmware/src/sha2.c
+SRC = nanoapp_sign.c ../../lib/nanohub/rsa.c ../../lib/nanohub/sha2.c
CC ?= gcc
CC_FLAGS = -Wall -Werror -Wextra
$(APP): $(SRC) Makefile
- $(CC) $(CC_FLAGS) -o $(APP) -O2 $(SRC) -I../../firmware/inc -DRSA_SUPPORT_PRIV_OP_BIGRAM -DHOST_BUILD -DBOOTLOADER= -DBOOTLOADER_RO=
+ $(CC) $(CC_FLAGS) -o $(APP) -O2 $(SRC) \
+ -I../../lib/include \
+ -DRSA_SUPPORT_PRIV_OP_BIGRAM -DHOST_BUILD -DBOOTLOADER= -DBOOTLOADER_RO=
clean:
rm -f $(APP)
diff --git a/util/nanoapp_sign/nanoapp_sign.c b/util/nanoapp_sign/nanoapp_sign.c
index 65f7627..f4304c2 100644
--- a/util/nanoapp_sign/nanoapp_sign.c
+++ b/util/nanoapp_sign/nanoapp_sign.c
@@ -19,25 +19,45 @@
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
-#include <sha2.h>
-#include <rsa.h>
+#include <nanohub/nanohub.h>
+#include <nanohub/nanoapp.h>
+#include <nanohub/sha2.h>
+#include <nanohub/rsa.h>
static FILE* urandom = NULL;
+#if defined(__APPLE__) || defined(_WIN32)
+inline uint32_t bswap32 (uint32_t x) {
+ uint32_t out = 0;
+ for (int i=0; i < 4; ++i, x >>= 8)
+ out = (out << 8) | (x & 0xFF);
+ return out;
+}
+
+#define htobe32(x) bswap32((x))
+#define htole32(x) ((uint32_t)(x))
+#define be32toh(x) bswap32((x))
+#define le32toh(x) ((uint32_t)(x))
+#else
+#include <endian.h>
+#endif
+
//read exactly one hex-encoded byte from a file, skipping all the fluff
-static int getHexEncodedByte(void)
+static int getHexEncodedByte(uint8_t *buf, uint32_t *ppos, uint32_t size)
{
int c, i;
+ uint32_t pos = *ppos;
uint8_t val = 0;
//for first byte
for (i = 0; i < 2; i++) {
val <<= 4;
while(1) {
- c = getchar();
- if (c == EOF)
+ if (pos == size)
return -1;
+ c = buf[pos++];
+ *ppos = pos;
if (c >= '0' && c <= '9')
val += c - '0';
@@ -60,31 +80,6 @@
return val;
}
-static bool readRsaDataFile(const char *fileName, uint32_t *buf)
-{
- uint32_t sz = sizeof(uint32_t[RSA_LIMBS]);
- FILE *f = fopen(fileName, "rb");
- bool ret = false;
-
- if (!f)
- goto out_noclose;
-
- if (sz != fread(buf, 1, sz, f))
- goto out;
-
- //verify file is empty
- if (fread(&sz, 1, 1, f))
- goto out;
-
- ret = true;
-
-out:
- fclose(f);
-
-out_noclose:
- return ret;
-}
-
//provide a random number for which the following property is true ((ret & 0xFF000000) && (ret & 0xFF0000) && (ret & 0xFF00) && (ret & 0xFF))
static uint32_t rand32_no_zero_bytes(void)
{
@@ -119,251 +114,430 @@
fclose(urandom);
}
-int main(int argc, char **argv)
-{
- const char *selfExeName = argv[0];
- const uint32_t *hash, *rsaResult;
- uint32_t rsanum[RSA_LIMBS];
+struct RsaData {
+ uint32_t num[RSA_LIMBS];
uint32_t exponent[RSA_LIMBS];
uint32_t modulus[RSA_LIMBS];
- struct Sha2state shaState;
- struct RsaState rsaState;
- uint8_t buf[RSA_BYTES];
- uint64_t fileSz = 0;
- bool verbose = false;
- int c, ret = -1;
- unsigned int i;
+ struct RsaState state;
+};
- argc--;
- argv++;
+static bool validateSignature(uint8_t *sigPack, struct RsaData *rsa, bool verbose, uint32_t *refHash, bool preset)
+{
+ int i;
+ const uint32_t *rsaResult;
+ const uint32_t *le32SigPack = (const uint32_t*)sigPack;
+ //convert to native uint32_t; ignore possible alignment issues
+ for (i = 0; i < RSA_LIMBS; i++)
+ rsa->num[i] = le32toh(le32SigPack[i]);
+ //update the user
+ if (verbose)
+ printHashRev(stderr, "RSA cyphertext", rsa->num, RSA_LIMBS);
+ if (!preset)
+ memcpy(rsa->modulus, sigPack + RSA_BYTES, RSA_BYTES);
- if (argc >= 1 && !strcmp(argv[0], "-v")) {
- verbose = true;
- argc--;
- argv++;
+ //do rsa op
+ rsaResult = rsaPubOp(&rsa->state, rsa->num, rsa->modulus);
+
+ //update the user
+ if (verbose)
+ printHashRev(stderr, "RSA plaintext", rsaResult, RSA_LIMBS);
+
+ //verify padding is appropriate and valid
+ if ((rsaResult[RSA_LIMBS - 1] & 0xffff0000) != 0x00020000) {
+ fprintf(stderr, "Padding header is invalid\n");
+ return false;
}
- if (argc < 1)
- goto usage;
-
- if (!strcmp(argv[0], "txt2bin")) {
-
- bool haveNonzero = false;
- uint32_t v;
-
- for (i = 0; i < RSA_BYTES; i++) {
-
- //get a byte, skipping all zeroes (openssl likes to prepend one at times)
- do {
- c = getHexEncodedByte();
- } while (c == 0 && !haveNonzero);
- haveNonzero = true;
- if (c < 0) {
- fprintf(stderr, "Invalid text RSA input data\n");
- goto usage;
- }
-
- buf[i] = c;
- }
-
- for (i = 0; i < RSA_LIMBS; i++) {
- for (v = 0, c = 0; c < 4; c++)
- v = (v << 8) | buf[i * 4 + c];
- rsanum[RSA_LIMBS - i - 1] = v;
- }
-
- //output in our binary format (little-endina)
- fwrite(rsanum, 1, sizeof(uint32_t[RSA_LIMBS]), stdout);
- }
- else if (!strcmp(argv[0], "sigdecode")) {
- if (argc < 2)
- goto usage;
-
- //get modulus
- if (!readRsaDataFile(argv[1], modulus)) {
- fprintf(stderr, "failed to read RSA modulus\n");
- goto usage;
- }
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA modulus: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)modulus[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //read input data as bytes
- i = 0;
- while ((c = getchar()) != EOF) {
- if (i == RSA_BYTES) {
- fprintf(stderr, "too many signature bytes\n");
- goto usage;
- }
- buf[i++] = c;
- }
- if (i != RSA_BYTES) {
- fprintf(stderr, "too few signature bytes\n");
- goto usage;
- }
-
- //convert into uint32_ts (just read as little endian and pack)
- for (i = 0; i < RSA_LIMBS; i++) {
- uint32_t v, j;
-
- for (v = 0, j = 0; j < 4; j++)
- v = (v << 8) | buf[i * 4 + 3 - j];
-
- rsanum[i] = v;
- }
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA cyphertext: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)rsanum[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //do rsa op
- rsaResult = rsaPubOp(&rsaState, rsanum, modulus);
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA plaintext: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)rsaResult[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //verify padding is appropriate and valid
- if ((rsaResult[RSA_LIMBS - 1] & 0xffff0000) != 0x00020000) {
- fprintf(stderr, "Padding header is invalid\n");
- goto out;
- }
-
- //verify first two bytes of padding
- if (!(rsaResult[RSA_LIMBS - 1] & 0xff00) || !(rsaResult[RSA_LIMBS - 1] & 0xff)) {
- fprintf(stderr, "Padding bytes 0..1 are invalid\n");
- goto out;
- }
-
- //verify last 3 bytes of padding and the zero terminator
- if (!(rsaResult[8] & 0xff000000) || !(rsaResult[8] & 0xff0000) || !(rsaResult[8] & 0xff00) || (rsaResult[8] & 0xff)) {
- fprintf(stderr, "Padding last bytes & terminator invalid\n");
- goto out;
- }
-
- //verify middle padding bytes
- for (i = 9; i < RSA_LIMBS - 1; i++) {
- if (!(rsaResult[i] & 0xff000000) || !(rsaResult[i] & 0xff0000) || !(rsaResult[i] & 0xff00) || !(rsaResult[i] & 0xff)) {
- fprintf(stderr, "Padding word %d invalid\n", i);
- goto out;
- }
- }
-
- //show the hash
- for (i = 0; i < 8; i++)
- printf("%08lx", (unsigned long)rsaResult[i]);
- printf("\n");
- ret = 0;
- }
- else if (!strcmp(argv[0], "sign")) {
- if (argc != 3)
- goto usage;
-
- //it might not matter, but we still like to try to cleanup after ourselves
- (void)atexit(cleanup);
-
- //prepare & get RSA info
- if (!readRsaDataFile(argv[1], exponent)) {
- fprintf(stderr, "failed to read RSA private exponent\n");
- goto usage;
- }
-
- if (!readRsaDataFile(argv[2], modulus)) {
- fprintf(stderr, "failed to read RSA modulus\n");
- goto usage;
- }
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA modulus: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)modulus[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //hash input
- sha2init(&shaState);
- fprintf(stderr, "Reading data to sign...");
- while ((c = getchar()) != EOF) { //this is slow but our data is small, so deal with it!
- uint8_t byte = c;
- sha2processBytes(&shaState, &byte, 1);
- fileSz++;
- }
- fprintf(stderr, " read %llu bytes\n", (unsigned long long)fileSz);
-
- //update the user on the progress
- hash = sha2finish(&shaState);
- if (verbose) {
- fprintf(stderr, "SHA2 hash: 0x");
- for (i = 0; i < 8; i++)
- fprintf(stderr, "%08lx", (unsigned long)hash[i]);
- fprintf(stderr, "\n");
- }
-
- //write into our "data to sign" area
- for (i = 0; i < 8; i++)
- rsanum[i] = hash[i];
-
- //write padding
- rsanum[i++] = rand32_no_zero_bytes() << 8; //low byte here must be zero as per padding spec
- for (;i < RSA_LIMBS - 1; i++)
- rsanum[i] = rand32_no_zero_bytes();
- rsanum[i] = (rand32_no_zero_bytes() >> 16) | 0x00020000; //as per padding spec
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA plaintext: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)rsanum[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //do the RSA thing
- fprintf(stderr, "Retriculating splines...");
- rsaResult = rsaPrivOp(&rsaState, rsanum, exponent, modulus);
- fprintf(stderr, "DONE\n");
-
- //update the user
- if (verbose) {
- fprintf(stderr, "RSA cyphertext: 0x");
- for (i = 0; i < RSA_LIMBS; i++)
- fprintf(stderr, "%08lx", (unsigned long)rsaResult[RSA_LIMBS - i - 1]);
- fprintf(stderr, "\n");
- }
-
- //output in a format that our microcontroller will be able to digest easily & directly (an array of bytes representing little-endian 32-bit words)
- fwrite(rsaResult, 1, sizeof(uint32_t[RSA_LIMBS]), stdout);
-
- fprintf(stderr, "success\n");
- ret = 0;
+ //verify first two bytes of padding
+ if (!(rsaResult[RSA_LIMBS - 1] & 0xff00) || !(rsaResult[RSA_LIMBS - 1] & 0xff)) {
+ fprintf(stderr, "Padding bytes 0..1 are invalid\n");
+ return false;
}
-out:
- return ret;
+ //verify last 3 bytes of padding and the zero terminator
+ if (!(rsaResult[8] & 0xff000000) || !(rsaResult[8] & 0xff0000) || !(rsaResult[8] & 0xff00) || (rsaResult[8] & 0xff)) {
+ fprintf(stderr, "Padding last bytes & terminator invalid\n");
+ return false;
+ }
-usage:
- fprintf(stderr, "USAGE: %s [-v] sign <BINARY_PRIVATE_EXPONENT_FILE> <BINARY_MODULUS_FILE> < data_to_sign > signature_out\n"
- " %s [-v] sigdecode <BINARY_MODULUS_FILE> < signature_out > expected_sha2\n"
- " %s [-v] txt2bin < TEXT_RSA_FILE > BINARY_RSA_FILE\n"
- "\t<BINARY_PRIVATE_EXPONENT> and <BINARY_MODULUS> files contain RSA numbers, binary little endian format\n"
- "\t<TEXT_RSA_FILE> file contain RSA numbers, text, big-endian format as exported by 'openssl rsa -in YOURKEY -text -noout'\n",
- selfExeName, selfExeName, selfExeName);
- return -1;
+ //verify middle padding bytes
+ for (i = 9; i < RSA_LIMBS - 1; i++) {
+ if (!(rsaResult[i] & 0xff000000) || !(rsaResult[i] & 0xff0000) || !(rsaResult[i] & 0xff00) || !(rsaResult[i] & 0xff)) {
+ fprintf(stderr, "Padding word %d invalid\n", i);
+ return false;
+ }
+ }
+ if (verbose) {
+ printHash(stderr, "Recovered hash ", rsaResult, SHA2_HASH_WORDS);
+ printHash(stderr, "Calculated hash", refHash, SHA2_HASH_WORDS);
+ }
+
+ if (!preset) {
+ // we're doing full verification, with key extracted from signature pack
+ if (memcmp(rsaResult, refHash, SHA2_HASH_SIZE)) {
+ fprintf(stderr, "hash mismatch\n");
+ return false;
+ }
+ } else {
+ // we just decode the signature with key passed as an argument
+ // in this case we return recovered hash
+ memcpy(refHash, rsaResult, SHA2_HASH_SIZE);
+ }
+ return true;
}
+#define SIGNATURE_BLOCK_SIZE (2 * RSA_BYTES)
+static int handleConvertKey(uint8_t **pbuf, uint32_t bufUsed, FILE *out, struct RsaData *rsa)
+{
+ bool haveNonzero = false;
+ uint8_t *buf = *pbuf;
+ int i, c;
+ uint32_t pos = 0;
+ int ret;
+ for (i = 0; i < (int)RSA_BYTES; i++) {
+ //get a byte, skipping all zeroes (openssl likes to prepend one at times)
+ do {
+ c = getHexEncodedByte(buf, &pos, bufUsed);
+ } while (c == 0 && !haveNonzero);
+ haveNonzero = true;
+ if (c < 0) {
+ fprintf(stderr, "Invalid text RSA input data\n");
+ return 2;
+ }
+
+ buf[i] = c;
+ }
+
+ // change form BE to native; ignore alignment
+ uint32_t *be32Buf = (uint32_t*)buf;
+ for (i = 0; i < RSA_LIMBS; i++)
+ rsa->num[RSA_LIMBS - i - 1] = be32toh(be32Buf[i]);
+
+ //output in our binary format (little-endian)
+ ret = fwrite(rsa->num, 1, RSA_BYTES, out) == RSA_BYTES ? 0 : 2;
+ fprintf(stderr, "Conversion status: %d\n", ret);
+
+ return ret;
+}
+
+static int handleVerify(uint8_t **pbuf, uint32_t bufUsed, struct RsaData *rsa, bool verbose, bool bareData)
+{
+ struct Sha2state shaState;
+ uint8_t *buf = *pbuf;
+ uint32_t masterPubKey[RSA_LIMBS];
+
+ memcpy(masterPubKey, rsa->modulus, RSA_BYTES);
+ if (!bareData) {
+ struct ImageHeader *image = (struct ImageHeader *)buf;
+ struct AppSecSignHdr *secHdr = (struct AppSecSignHdr *)&image[1];
+ int block = 0;
+ uint8_t *sigPack;
+ bool trusted = false;
+ bool lastTrusted = false;
+ int sigData;
+
+ if (verbose)
+ fprintf(stderr, "Original Data len=%" PRIu32 " b; file size=%" PRIu32 " b; diff=%" PRIu32 " b\n",
+ secHdr->appDataLen, bufUsed, bufUsed - secHdr->appDataLen);
+
+ if (!(image->aosp.flags & NANOAPP_SIGNED_FLAG)) {
+ fprintf(stderr, "image is not marked as signed, can not verify\n");
+ return 2;
+ }
+ sigData = bufUsed - (secHdr->appDataLen + sizeof(*image) + sizeof(*secHdr));
+ if (sigData <= 0 || (sigData % SIGNATURE_BLOCK_SIZE) != 0) {
+ fprintf(stderr, "Invalid signature header: data size mismatch\n");
+ return 2;
+ }
+
+ sha2init(&shaState);
+ sha2processBytes(&shaState, buf, bufUsed - sigData);
+ int nSig = sigData / SIGNATURE_BLOCK_SIZE;
+ sigPack = buf + bufUsed - sigData;
+ for (block = 0; block < nSig; ++block) {
+ if (!validateSignature(sigPack, rsa, verbose, (uint32_t*)sha2finish(&shaState), false)) {
+ fprintf(stderr, "Signature verification failed: signature block #%d\n", block);
+ return 2;
+ }
+ if (memcmp(masterPubKey, rsa->modulus, RSA_BYTES) == 0) {
+ fprintf(stderr, "Key in block %d is trusted\n", block);
+ trusted = true;
+ lastTrusted = true;
+ } else {
+ lastTrusted = false;
+ }
+ sha2init(&shaState);
+ sha2processBytes(&shaState, sigPack+RSA_BYTES, RSA_BYTES);
+ sigPack += SIGNATURE_BLOCK_SIZE;
+ }
+ if (trusted && !lastTrusted) {
+ fprintf(stderr, "Trusted key is not the last in key sequence\n");
+ }
+ return trusted ? 0 : 2;
+ } else {
+ uint8_t *sigPack = buf + bufUsed - SIGNATURE_BLOCK_SIZE;
+ uint32_t *hash;
+ // can not do signature chains in bare mode
+ if (bufUsed > SIGNATURE_BLOCK_SIZE) {
+ sha2init(&shaState);
+ sha2processBytes(&shaState, buf, bufUsed - SIGNATURE_BLOCK_SIZE);
+ hash = (uint32_t*)sha2finish(&shaState);
+ printHash(stderr, "File hash", hash, SHA2_HASH_WORDS);
+ if (verbose)
+ printHashRev(stderr, "File PubKey", (uint32_t *)(sigPack + RSA_BYTES), RSA_LIMBS);
+ if (!validateSignature(sigPack, rsa, verbose, hash, false)) {
+ fprintf(stderr, "Signature verification failed on raw data\n");
+ return 2;
+ }
+ if (memcmp(masterPubKey, sigPack + RSA_BYTES, RSA_BYTES) == 0) {
+ fprintf(stderr, "Signature verification passed and the key is trusted\n");
+ return 0;
+ } else {
+ fprintf(stderr, "Signature verification passed but the key is not trusted\n");
+ return 2;
+ }
+ } else {
+ fprintf(stderr, "Not enough raw data to extract signature from\n");
+ return 2;
+ }
+ }
+
+ return 0;
+}
+
+static int handleSign(uint8_t **pbuf, uint32_t bufUsed, FILE *out, struct RsaData *rsa, bool verbose, bool bareData)
+{
+ struct Sha2state shaState;
+ uint8_t *buf = *pbuf;
+ uint32_t i;
+ const uint32_t *hash;
+ const uint32_t *rsaResult;
+ int ret;
+
+ if (!bareData) {
+ struct ImageHeader *image = (struct ImageHeader *)buf;
+ struct AppSecSignHdr *secHdr = (struct AppSecSignHdr *)&image[1];
+ uint32_t grow = sizeof(*secHdr);
+ if (!(image->aosp.flags & NANOAPP_SIGNED_FLAG)) {
+ // this is the 1st signature in the chain; inject header, set flag
+ buf = reallocOrDie(buf, bufUsed + grow);
+ *pbuf = buf;
+ image = (struct ImageHeader *)buf;
+ secHdr = (struct AppSecSignHdr *)&image[1];
+
+ fprintf(stderr, "Generating signature header\n");
+ image->aosp.flags |= NANOAPP_SIGNED_FLAG;
+ memmove((uint8_t*)&image[1] + grow, &image[1], bufUsed - sizeof(*image));
+ secHdr->appDataLen = bufUsed - sizeof(*image);
+ bufUsed += grow;
+ fprintf(stderr, "Rehashing file\n");
+ sha2init(&shaState);
+ sha2processBytes(&shaState, buf, bufUsed);
+ } else {
+ int sigSz = bufUsed - sizeof(*image) - sizeof(*secHdr) - secHdr->appDataLen;
+ int numSigs = sigSz / SIGNATURE_BLOCK_SIZE;
+ if ((numSigs * SIGNATURE_BLOCK_SIZE) != sigSz) {
+ fprintf(stderr, "Invalid signature block(s) detected\n");
+ return 2;
+ } else {
+ fprintf(stderr, "Found %d appended signature(s)\n", numSigs);
+ // generating SHA256 of the last PubKey in chain
+ fprintf(stderr, "Hashing last signature's PubKey\n");
+ sha2init(&shaState);
+ sha2processBytes(&shaState, buf + bufUsed- RSA_BYTES, RSA_BYTES);
+ }
+ }
+ } else {
+ fprintf(stderr, "Signing raw data\n");
+ sha2init(&shaState);
+ sha2processBytes(&shaState, buf, bufUsed);
+ }
+
+ //update the user on the progress
+ hash = sha2finish(&shaState);
+ if (verbose)
+ printHash(stderr, "SHA2 hash", hash, SHA2_HASH_WORDS);
+
+ memcpy(rsa->num, hash, SHA2_HASH_SIZE);
+
+ i = SHA2_HASH_WORDS;
+ //write padding
+ rsa->num[i++] = rand32_no_zero_bytes() << 8; //low byte here must be zero as per padding spec
+ for (;i < RSA_LIMBS - 1; i++)
+ rsa->num[i] = rand32_no_zero_bytes();
+ rsa->num[i] = (rand32_no_zero_bytes() >> 16) | 0x00020000; //as per padding spec
+
+ //update the user
+ if (verbose)
+ printHashRev(stderr, "RSA plaintext", rsa->num, RSA_LIMBS);
+
+ //do the RSA thing
+ fprintf(stderr, "Retriculating splines...");
+ rsaResult = rsaPrivOp(&rsa->state, rsa->num, rsa->exponent, rsa->modulus);
+ fprintf(stderr, "DONE\n");
+
+ //update the user
+ if (verbose)
+ printHashRev(stderr, "RSA cyphertext", rsaResult, RSA_LIMBS);
+
+ // output in a format that our microcontroller will be able to digest easily & directly
+ // (an array of bytes representing little-endian 32-bit words)
+ fwrite(buf, 1, bufUsed, out);
+ fwrite(rsaResult, 1, sizeof(uint32_t[RSA_LIMBS]), out);
+ ret = (fwrite(rsa->modulus, 1, RSA_BYTES, out) == RSA_BYTES) ? 0 : 2;
+
+ fprintf(stderr, "Status: %s (%d)\n", ret == 0 ? "success" : "failed", ret);
+ return ret;
+
+}
+
+static void fatalUsage(const char *name, const char *msg, const char *arg)
+{
+ if (msg && arg)
+ fprintf(stderr, "Error: %s: %s\n\n", msg, arg);
+ else if (msg)
+ fprintf(stderr, "Error: %s\n\n", msg);
+
+ fprintf(stderr, "USAGE: %s [-v] [-e <pvt key>] [-m <pub key>] [-t] [-s] [-b] <input file> [<output file>]\n"
+ " -v : be verbose\n"
+ " -b : generate binary key from text file created by OpenSSL\n"
+ " -s : sign post-processed file\n"
+ " -t : verify signature of signed post-processed file\n"
+ " -e : RSA binary private key\n"
+ " -m : RSA binary public key\n"
+ " -r : do not parse headers, do not generate headers (with -t, -s)\n"
+ , name);
+ exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ uint32_t bufUsed = 0;
+ uint8_t *buf = NULL;
+ int ret = -1;
+ const char **strArg = NULL;
+ const char *appName = argv[0];
+ const char *posArg[2] = { NULL };
+ uint32_t posArgCnt = 0;
+ FILE *out = NULL;
+ const char *prev = NULL;
+ bool verbose = false;
+ bool sign = false;
+ bool verify = false;
+ bool txt2bin = false;
+ bool bareData = false;
+ const char *keyPvtFile = NULL;
+ const char *keyPubFile = NULL;
+ int multi = 0;
+ struct RsaData rsa;
+ struct ImageHeader *image;
+
+ //it might not matter, but we still like to try to cleanup after ourselves
+ (void)atexit(cleanup);
+
+ for (int i = 1; i < argc; i++) {
+ if (argv[i][0] == '-') {
+ prev = argv[i];
+ if (!strcmp(argv[i], "-v"))
+ verbose = true;
+ else if (!strcmp(argv[i], "-s"))
+ sign = true;
+ else if (!strcmp(argv[i], "-t"))
+ verify = true;
+ else if (!strcmp(argv[i], "-b"))
+ txt2bin = true;
+ else if (!strcmp(argv[i], "-e"))
+ strArg = &keyPvtFile;
+ else if (!strcmp(argv[i], "-m"))
+ strArg = &keyPubFile;
+ else if (!strcmp(argv[i], "-r"))
+ bareData = true;
+ else
+ fatalUsage(appName, "unknown argument", argv[i]);
+ } else {
+ if (strArg) {
+ *strArg = argv[i];
+ strArg = NULL;
+ } else {
+ if (posArgCnt < 2)
+ posArg[posArgCnt++] = argv[i];
+ else
+ fatalUsage(appName, "too many positional arguments", argv[i]);
+ }
+ prev = 0;
+ }
+ }
+ if (prev)
+ fatalUsage(appName, "missing argument after", prev);
+
+ if (!posArgCnt)
+ fatalUsage(appName, "missing input file name", NULL);
+
+ if (sign)
+ multi++;
+ if (verify)
+ multi++;
+ if (txt2bin)
+ multi++;
+
+ if (multi != 1)
+ fatalUsage(appName, "select either -s, -t, or -b", NULL);
+
+ memset(&rsa, 0, sizeof(rsa));
+
+ if (sign && !(keyPvtFile && keyPubFile))
+ fatalUsage(appName, "We need both PUB (-m) and PVT (-e) keys for signing", NULL);
+
+ if (verify && (!keyPubFile || keyPvtFile))
+ fatalUsage(appName, "We only need PUB (-m) key for signature checking", NULL);
+
+ if (keyPvtFile) {
+ if (!readFile(rsa.exponent, sizeof(rsa.exponent), keyPvtFile))
+ fatalUsage(appName, "Can't read PVT key from", keyPvtFile);
+ else if (verbose)
+ printHashRev(stderr, "RSA exponent", rsa.exponent, RSA_LIMBS);
+ }
+
+ if (keyPubFile) {
+ if (!readFile(rsa.modulus, sizeof(rsa.modulus), keyPubFile))
+ fatalUsage(appName, "Can't read PUB key from", keyPubFile);
+ else if (verbose)
+ printHashRev(stderr, "RSA modulus", rsa.modulus, RSA_LIMBS);
+ }
+
+ buf = loadFile(posArg[0], &bufUsed);
+ fprintf(stderr, "Read %" PRIu32 " bytes\n", bufUsed);
+
+ image = (struct ImageHeader *)buf;
+ if (!bareData && !txt2bin) {
+ if (image->aosp.header_version == 1 &&
+ image->aosp.magic == NANOAPP_AOSP_MAGIC &&
+ image->layout.magic == GOOGLE_LAYOUT_MAGIC) {
+ fprintf(stderr, "Found AOSP header\n");
+ } else {
+ fprintf(stderr, "Unknown binary format\n");
+ return 2;
+ }
+ }
+
+ if (!posArg[1])
+ out = stdout;
+ else
+ out = fopen(posArg[1], "w");
+ if (!out)
+ fatalUsage(appName, "failed to create/open output file", posArg[1]);
+
+ if (sign)
+ ret = handleSign(&buf, bufUsed, out, &rsa, verbose, bareData);
+ else if (verify)
+ ret = handleVerify(&buf, bufUsed, &rsa, verbose, bareData);
+ else if (txt2bin)
+ ret = handleConvertKey(&buf, bufUsed, out, &rsa);
+
+ free(buf);
+ fclose(out);
+ return ret;
+}
diff --git a/util/nanohub_os_update_prepare.sh b/util/nanohub_os_update_prepare.sh
index a18c532..0227e43 100755
--- a/util/nanohub_os_update_prepare.sh
+++ b/util/nanohub_os_update_prepare.sh
@@ -19,98 +19,25 @@
# Exit in error if we use an undefined variable (i.e. commit a typo).
set -u
-terminate() { #cleanup and exit
- rm -rf $stage
- exit $1
-}
-
usage () { #show usage and bail out
echo "USAGE:" >&2
- echo " $1 <PRIV_KEY_FILE> <PUB_KEY_FILE> < nanohub.update.bin > nanohub.update.signed.bin" >&2
- terminate -1
+ echo " $1 <PRIV_KEY_FILE> <PUB_KEY_FILE> nanohub.update.bin" >&2
+ exit 1
}
-putchar() {
- hexch="0123456789abcdef"
- h=$[$1/16]
- l=$[$1%16]
- h=${hexch:$h:1}
- l=${hexch:$l:1}
- e="\x"$h$l
- echo -ne $e
-}
-
-printhex() {
- w3=$[$1/16777216]
- t=$[$w3*16777216]
- a=$[$1-$t]
-
- w2=$[$a/65536]
- t=$[$w2*65536]
- a=$[$a-$t]
-
- w1=$[$a/256]
- w0=$[$a%256]
-
- putchar $w0
- putchar $w1
- putchar $w2
- putchar $w3
-}
-
-#create temp dir
-stage=$(mktemp -dt "$(basename $0).XXXXXXXXXX")
-
-
-#sanity checks (on the user)
-if [ -t 1 ]
-then
- usage $0
+if [ $# != 3 ] ; then
+usage $0
fi
-if [ -t 0 ]
-then
- usage $0
-fi
+priv=$1
+pub=$2
+raw_image=$3
-#handle signing
-if [ $# -ne 2 ]
-then
- usage $0
-fi
-priv1="$1"
-pub1="$2"
+# make signed image with header; suitable for BL
+# to be consumed by BL it has to be named nanohub.kernel.signed
+nanoapp_postprocess -n os -r ${raw_image} ${raw_image}.oshdr
+nanoapp_sign -s -e ${priv} -m ${pub} -r ${raw_image}.oshdr nanohub.kernel.signed
+# embed this image inside nanoapp container
-#save update to file in dir
-cat > "$stage/raw"
-
-#pad update to 4 byte boundary
-t=$(du -b "$stage/raw" | cut -f1)
-while [ $[$t%4] -ne 0 ]
-do
- echo -ne "\0" >> "$stage/raw"
- t=$(du -b "$stage/raw" | cut -f1)
-done
-
-#get and save the file size
-signed_sz=$(du -b "$stage/raw" | cut -f1)
-
-#create the header (with the marker set for signing
-echo -ne "Nanohub OS\x00\xFE" > "$stage/hdr"
-printhex $signed_sz >> "$stage/hdr"
-
-#concat the data to header
-cat "$stage/hdr" "$stage/raw" > "$stage/with_hdr"
-
-#create the signature
-nanoapp_sign sign "$priv1" "$pub1" < "$stage/with_hdr" > "$stage/sig"
-
-#insert proper upload marker
-echo -ne "\xff" | dd bs=1 seek=11 count=1 conv=notrunc of="$stage/with_hdr" 2>/dev/null
-
-#produce signed output
-cat "$stage/with_hdr" "$stage/sig" "$pub1"
-
-terminate 0
-
+nanoapp_postprocess -n os nanohub.kernel.signed ${raw_image}.napp
diff --git a/util/nanotool/Android.mk b/util/nanotool/Android.mk
new file mode 100644
index 0000000..7410b34
--- /dev/null
+++ b/util/nanotool/Android.mk
@@ -0,0 +1,57 @@
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+LOCAL_PATH := $(call my-dir)
+
+NANOTOOL_VERSION := 1.1.0
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+ androidcontexthub.cpp \
+ apptohostevent.cpp \
+ calibrationfile.cpp \
+ contexthub.cpp \
+ log.cpp \
+ nanomessage.cpp \
+ nanotool.cpp \
+ resetreasonevent.cpp \
+ sensorevent.cpp
+
+# JSON file handling from chinook
+COMMON_UTILS_DIR := ../common
+LOCAL_SRC_FILES += \
+ $(COMMON_UTILS_DIR)/file.cpp \
+ $(COMMON_UTILS_DIR)/JSONObject.cpp
+
+LOCAL_C_INCLUDES := \
+ $(LOCAL_PATH)/$(COMMON_UTILS_DIR)
+
+LOCAL_SHARED_LIBRARIES := \
+ liblog \
+ libstagefright_foundation \
+ libutils
+
+LOCAL_CFLAGS += -Wall -Werror -Wextra
+LOCAL_CFLAGS += -std=c++11
+LOCAL_CFLAGS += -DNANOTOOL_VERSION_STR='"version $(NANOTOOL_VERSION)"'
+
+LOCAL_MODULE := nanotool
+
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := google
+
+include $(BUILD_EXECUTABLE)
diff --git a/util/nanotool/androidcontexthub.cpp b/util/nanotool/androidcontexthub.cpp
new file mode 100644
index 0000000..f477977
--- /dev/null
+++ b/util/nanotool/androidcontexthub.cpp
@@ -0,0 +1,395 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "androidcontexthub.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <time.h>
+#include <unistd.h>
+#include <sys/stat.h>
+
+#include <chrono>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <thread>
+#include <vector>
+
+#include "calibrationfile.h"
+#include "log.h"
+
+namespace android {
+
+constexpr char kSensorDeviceFile[] = "/dev/nanohub";
+constexpr char kCommsDeviceFile[] = "/dev/nanohub_comms";
+constexpr char kLockDirectory[] = "/data/system/nanohub_lock";
+constexpr char kLockFile[] = "/data/system/nanohub_lock/lock";
+
+constexpr mode_t kLockDirPermissions = (S_IRUSR | S_IWUSR | S_IXUSR);
+
+constexpr auto kLockDelay = std::chrono::milliseconds(100);
+
+constexpr int kDeviceFileCount = 2;
+constexpr int kPollNoTimeout = -1;
+
+static const std::vector<std::tuple<const char *, SensorType>> kCalibrationKeys = {
+ std::make_tuple("accel", SensorType::Accel),
+ std::make_tuple("gyro", SensorType::Gyro),
+ std::make_tuple("proximity", SensorType::Proximity),
+ std::make_tuple("barometer", SensorType::Barometer),
+ std::make_tuple("light", SensorType::AmbientLightSensor),
+};
+
+static void AppendBytes(const void *data, size_t length, std::vector<uint8_t>& buffer) {
+ const uint8_t *bytes = (const uint8_t *) data;
+ for (size_t i = 0; i < length; i++) {
+ buffer.push_back(bytes[i]);
+ }
+}
+
+static bool CopyInt32Array(const char *key,
+ sp<JSONObject> json, std::vector<uint8_t>& bytes) {
+ sp<JSONArray> array;
+ if (json->getArray(key, &array)) {
+ for (size_t i = 0; i < array->size(); i++) {
+ int32_t val = 0;
+ array->getInt32(i, &val);
+ AppendBytes(&val, sizeof(uint32_t), bytes);
+ }
+
+ return true;
+ }
+ return false;
+}
+
+static bool GetCalibrationBytes(const char *key, SensorType sensor_type,
+ std::vector<uint8_t>& bytes) {
+ bool success = true;
+ auto json = CalibrationFile::Instance()->GetJSONObject();
+
+ switch (sensor_type) {
+ case SensorType::Accel:
+ case SensorType::Gyro:
+ success = CopyInt32Array(key, json, bytes);
+ break;
+
+ case SensorType::AmbientLightSensor:
+ case SensorType::Barometer: {
+ float value = 0;
+ success = json->getFloat(key, &value);
+ if (success) {
+ AppendBytes(&value, sizeof(float), bytes);
+ }
+ break;
+ }
+
+ case SensorType::Proximity: {
+ // Proximity might be an int32 array with 4 values (CRGB) or a single
+ // int32 value - try both
+ success = CopyInt32Array(key, json, bytes);
+ if (!success) {
+ int32_t value = 0;
+ success = json->getInt32(key, &value);
+ if (success) {
+ AppendBytes(&value, sizeof(int32_t), bytes);
+ }
+ }
+ break;
+ }
+
+ default:
+ // If this log message gets printed, code needs to be added in this
+ // switch statement
+ LOGE("Missing sensor type to calibration data mapping sensor %d",
+ static_cast<int>(sensor_type));
+ success = false;
+ }
+
+ return success;
+}
+
+AndroidContextHub::~AndroidContextHub() {
+ if (unlink(kLockFile) < 0) {
+ LOGE("Couldn't remove lock file: %s", strerror(errno));
+ }
+ if (sensor_fd_ >= 0) {
+ DisableActiveSensors();
+ (void) close(sensor_fd_);
+ }
+ if (comms_fd_ >= 0) {
+ (void) close(comms_fd_);
+ }
+}
+
+void AndroidContextHub::TerminateHandler() {
+ (void) unlink(kLockFile);
+}
+
+bool AndroidContextHub::Initialize() {
+ // Acquire a lock on nanohub, so the HAL read threads won't take our events.
+ // We need to delay after creating the file to have good confidence that
+ // the HALs noticed the lock file creation.
+ if (access(kLockDirectory, F_OK) < 0) {
+ if (mkdir(kLockDirectory, kLockDirPermissions) < 0 && errno != EEXIST) {
+ LOGE("Couldn't create lock directory: %s", strerror(errno));
+ }
+ }
+ int lock_fd = open(kLockFile, O_CREAT | O_EXCL, S_IRUSR | S_IWUSR);
+ if (lock_fd < 0) {
+ LOGE("Couldn't create lock file: %s", strerror(errno));
+ if (errno != EEXIST) {
+ return false;
+ }
+ } else {
+ close(lock_fd);
+ std::this_thread::sleep_for(kLockDelay);
+ LOGD("Lock sleep complete");
+ }
+
+ // Sensor device file is used for sensor requests, e.g. configure, etc., and
+ // returns sensor events
+ sensor_fd_ = open(kSensorDeviceFile, O_RDWR);
+ if (sensor_fd_ < 0) {
+ LOGE("Couldn't open device file: %s", strerror(errno));
+ return false;
+ }
+
+ // The comms device file is used for more generic communication with
+ // nanoapps. Calibration results are returned through this channel.
+ comms_fd_ = open(kCommsDeviceFile, O_RDONLY);
+ if (comms_fd_ < 0) {
+ // TODO(bduddie): Currently informational only, as the kernel change
+ // that adds this device file is not available/propagated yet.
+ // Eventually this should be an error.
+ LOGI("Couldn't open comms device file: %s", strerror(errno));
+ }
+
+ return true;
+}
+
+void AndroidContextHub::SetLoggingEnabled(bool logging_enabled) {
+ if (logging_enabled) {
+ LOGE("Logging is not supported on this platform");
+ }
+}
+
+ContextHub::TransportResult AndroidContextHub::WriteEvent(
+ const std::vector<uint8_t>& message) {
+ ContextHub::TransportResult result;
+
+ LOGD("Writing %zu bytes", message.size());
+ LOGD_BUF(message.data(), message.size());
+ int ret = write(sensor_fd_, message.data(), message.size());
+ if (ret == -1) {
+ LOGE("Couldn't write %zu bytes to device file: %s", message.size(),
+ strerror(errno));
+ result = TransportResult::GeneralFailure;
+ } else if (ret != (int) message.size()) {
+ LOGW("Write returned %d, expected %zu", ret, message.size());
+ result = TransportResult::GeneralFailure;
+ } else {
+ LOGD("Successfully sent event");
+ result = TransportResult::Success;
+ }
+
+ return result;
+}
+
+ContextHub::TransportResult AndroidContextHub::ReadEvent(
+ std::vector<uint8_t>& message, int timeout_ms) {
+ ContextHub::TransportResult result = TransportResult::GeneralFailure;
+
+ struct pollfd pollfds[kDeviceFileCount];
+ int fd_count = ResetPollFds(pollfds, kDeviceFileCount);
+
+ int timeout = timeout_ms > 0 ? timeout_ms : kPollNoTimeout;
+ int ret = poll(pollfds, fd_count, timeout);
+ if (ret < 0) {
+ LOGE("Polling failed: %s", strerror(errno));
+ if (errno == EINTR) {
+ result = TransportResult::Canceled;
+ }
+ } else if (ret == 0) {
+ LOGD("Poll timed out");
+ result = TransportResult::Timeout;
+ } else {
+ int read_fd = -1;
+ for (int i = 0; i < kDeviceFileCount; i++) {
+ if (pollfds[i].revents & POLLIN) {
+ read_fd = pollfds[i].fd;
+ break;
+ }
+ }
+
+ if (read_fd == sensor_fd_) {
+ LOGD("Data ready on sensors device file");
+ } else if (read_fd == comms_fd_) {
+ LOGD("Data ready on comms device file");
+ }
+
+ if (read_fd >= 0) {
+ result = ReadEventFromFd(read_fd, message);
+ } else {
+ LOGE("Poll returned but none of expected files are ready");
+ }
+ }
+
+ return result;
+}
+
+bool AndroidContextHub::FlashSensorHub(const std::vector<uint8_t>& bytes) {
+ (void)bytes;
+ LOGE("Flashing is not supported on this platform");
+ return false;
+}
+
+bool AndroidContextHub::LoadCalibration() {
+ std::vector<uint8_t> cal_data;
+ bool success = true;
+
+ for (size_t i = 0; success && i < kCalibrationKeys.size(); i++) {
+ std::string key;
+ SensorType sensor_type;
+
+ std::tie(key, sensor_type) = kCalibrationKeys[i];
+ if (GetCalibrationBytes(key.c_str(), sensor_type, cal_data)) {
+ success = SendCalibrationData(sensor_type, cal_data);
+ }
+
+ cal_data.clear();
+ }
+
+ return success;
+}
+
+bool AndroidContextHub::SetCalibration(SensorType sensor_type, int32_t data) {
+ LOGI("Setting calibration for sensor %d (%s) to %d",
+ static_cast<int>(sensor_type),
+ ContextHub::SensorTypeToAbbrevName(sensor_type).c_str(), data);
+ auto cal_file = CalibrationFile::Instance();
+ const char *key = AndroidContextHub::SensorTypeToCalibrationKey(sensor_type);
+ if (cal_file && key) {
+ return cal_file->SetSingleAxis(key, data);
+ }
+ return false;
+}
+
+bool AndroidContextHub::SetCalibration(SensorType sensor_type, float data) {
+ LOGI("Setting calibration for sensor %d (%s) to %f",
+ static_cast<int>(sensor_type),
+ ContextHub::SensorTypeToAbbrevName(sensor_type).c_str(), data);
+ auto cal_file = CalibrationFile::Instance();
+ const char *key = AndroidContextHub::SensorTypeToCalibrationKey(sensor_type);
+ if (cal_file && key) {
+ return cal_file->SetSingleAxis(key, data);
+ }
+ return false;
+}
+
+bool AndroidContextHub::SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z) {
+ LOGI("Setting calibration for %d to %d %d %d", static_cast<int>(sensor_type),
+ x, y, z);
+ auto cal_file = CalibrationFile::Instance();
+ const char *key = AndroidContextHub::SensorTypeToCalibrationKey(sensor_type);
+ if (cal_file && key) {
+ return cal_file->SetTripleAxis(key, x, y, z);
+ }
+ return false;
+}
+
+bool AndroidContextHub::SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z, int32_t w) {
+ LOGI("Setting calibration for %d to %d %d %d %d", static_cast<int>(sensor_type),
+ x, y, z, w);
+ auto cal_file = CalibrationFile::Instance();
+ const char *key = AndroidContextHub::SensorTypeToCalibrationKey(sensor_type);
+ if (cal_file && key) {
+ return cal_file->SetFourAxis(key, x, y, z, w);
+ }
+ return false;
+}
+
+bool AndroidContextHub::SaveCalibration() {
+ LOGI("Saving calibration data");
+ auto cal_file = CalibrationFile::Instance();
+ if (cal_file) {
+ return cal_file->Save();
+ }
+ return false;
+}
+
+ContextHub::TransportResult AndroidContextHub::ReadEventFromFd(
+ int fd, std::vector<uint8_t>& message) {
+ ContextHub::TransportResult result = TransportResult::GeneralFailure;
+
+ // Set the size to the maximum, so when we resize later, it's always a
+ // shrink (otherwise it will end up clearing the bytes)
+ message.resize(message.capacity());
+
+ LOGD("Calling into read()");
+ int ret = read(fd, message.data(), message.capacity());
+ if (ret < 0) {
+ LOGE("Couldn't read from device file: %s", strerror(errno));
+ if (errno == EINTR) {
+ result = TransportResult::Canceled;
+ }
+ } else if (ret == 0) {
+ // We might need to handle this specially, if the driver implements this
+ // to mean something specific
+ LOGE("Read unexpectedly returned 0 bytes");
+ } else {
+ message.resize(ret);
+ LOGD_VEC(message);
+ result = TransportResult::Success;
+ }
+
+ return result;
+}
+
+int AndroidContextHub::ResetPollFds(struct pollfd *pfds, size_t count) {
+ memset(pfds, 0, sizeof(struct pollfd) * count);
+ pfds[0].fd = sensor_fd_;
+ pfds[0].events = POLLIN;
+
+ int nfds = 1;
+ if (count > 1 && comms_fd_ >= 0) {
+ pfds[1].fd = comms_fd_;
+ pfds[1].events = POLLIN;
+ nfds++;
+ }
+ return nfds;
+}
+
+const char *AndroidContextHub::SensorTypeToCalibrationKey(SensorType sensor_type) {
+ for (size_t i = 0; i < kCalibrationKeys.size(); i++) {
+ const char *key;
+ SensorType sensor_type_for_key;
+
+ std::tie(key, sensor_type_for_key) = kCalibrationKeys[i];
+ if (sensor_type == sensor_type_for_key) {
+ return key;
+ }
+ }
+
+ LOGE("No calibration key mapping for sensor type %d",
+ static_cast<int>(sensor_type));
+ return nullptr;
+}
+
+} // namespace android
diff --git a/util/nanotool/androidcontexthub.h b/util/nanotool/androidcontexthub.h
new file mode 100644
index 0000000..720561d
--- /dev/null
+++ b/util/nanotool/androidcontexthub.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ANDROIDCONTEXTHUB_H_
+#define ANDROIDCONTEXTHUB_H_
+
+#include "contexthub.h"
+
+#include <poll.h>
+#include <unistd.h>
+
+namespace android {
+
+/*
+ * Communicates with a context hub via the /dev/nanohub interface
+ */
+class AndroidContextHub : public ContextHub {
+ public:
+ ~AndroidContextHub();
+
+ // Performs system resource cleanup in the event that the program is
+ // terminated abnormally (via std::terminate)
+ static void TerminateHandler();
+
+ bool Initialize() override;
+ bool LoadCalibration() override;
+ void SetLoggingEnabled(bool logging_enabled) override;
+
+ protected:
+ ContextHub::TransportResult WriteEvent(
+ const std::vector<uint8_t>& request) override;
+ ContextHub::TransportResult ReadEvent(std::vector<uint8_t>& response,
+ int timeout_ms) override;
+ bool FlashSensorHub(const std::vector<uint8_t>& bytes) override;
+
+ bool SetCalibration(SensorType sensor_type, int32_t data) override;
+ bool SetCalibration(SensorType sensor_type, float data) override;
+ bool SetCalibration(SensorType sensor_type, int32_t x, int32_t y, int32_t z)
+ override;
+ bool SetCalibration(SensorType sensor_type, int32_t x, int32_t y,
+ int32_t z, int32_t w) override;
+ bool SaveCalibration() override;
+
+ private:
+ int sensor_fd_ = -1;
+ int comms_fd_ = -1;
+
+ ContextHub::TransportResult ReadEventFromFd(int fd,
+ std::vector<uint8_t>& message);
+ int ResetPollFds(struct pollfd *pfds, size_t count);
+ static const char *SensorTypeToCalibrationKey(SensorType sensor_type);
+};
+
+} // namespace android
+
+#endif // ANDROIDCONTEXTHUB_H_
diff --git a/util/nanotool/apptohostevent.cpp b/util/nanotool/apptohostevent.cpp
new file mode 100644
index 0000000..9ab4fb3
--- /dev/null
+++ b/util/nanotool/apptohostevent.cpp
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "apptohostevent.h"
+
+#include "contexthub.h"
+#include "log.h"
+
+namespace android {
+
+/* AppToHostEvent *************************************************************/
+
+std::unique_ptr<AppToHostEvent> AppToHostEvent::FromBytes(
+ const std::vector<uint8_t>& buffer) {
+ auto event = std::unique_ptr<AppToHostEvent>(new AppToHostEvent());
+ event->Populate(buffer);
+ if (!event->IsValid()) {
+ return nullptr;
+ }
+
+ return event;
+}
+
+uint64_t AppToHostEvent::GetAppId() const {
+ return GetTypedData()->appId;
+}
+
+uint8_t AppToHostEvent::GetDataLen() const {
+ return GetTypedData()->dataLen;
+}
+
+const uint8_t *AppToHostEvent::GetDataPtr() const {
+ return (reinterpret_cast<const uint8_t*>(GetTypedData())
+ + sizeof(struct HostHubRawPacket));
+}
+
+bool AppToHostEvent::IsCalibrationEventForSensor(SensorType sensor_type) const {
+ if (GetDataLen() < sizeof(struct SensorAppEventHeader)) {
+ return false;
+ }
+
+ // Make sure the app ID matches what we expect for the sensor type, bail out
+ // early if it doesn't
+ switch (sensor_type) {
+ case SensorType::Accel:
+ case SensorType::Gyro:
+ if (GetAppId() != kAppIdBoschBmi160Bmm150) {
+ return false;
+ }
+ break;
+
+ case SensorType::Proximity:
+ if (GetAppId() != kAppIdAmsTmd2772 && GetAppId() != kAppIdRohmRpr0521 &&
+ GetAppId() != kAppIdAmsTmd4903) {
+ return false;
+ }
+ break;
+
+ case SensorType::Barometer:
+ if (GetAppId() != kAppIdBoschBmp280) {
+ return false;
+ }
+ break;
+
+ case SensorType::AmbientLightSensor:
+ if (GetAppId() != kAppIdAmsTmd4903) {
+ return false;
+ }
+ break;
+
+ default:
+ return false;
+ }
+
+ // If we made it this far, we only need to confirm the message ID
+ auto header = reinterpret_cast<const struct SensorAppEventHeader *>(
+ GetDataPtr());
+ return (header->msgId == SENSOR_APP_MSG_CALIBRATION_RESULT);
+}
+
+bool AppToHostEvent::IsValid() const {
+ const HostHubRawPacket *packet = GetTypedData();
+ if (!packet) {
+ return false;
+ }
+
+ // dataLen should specify the amount of data that follows the event type
+ // and HostHubRawPacket headers
+ if (event_data.size() < (sizeof(uint32_t) + sizeof(struct HostHubRawPacket)
+ + packet->dataLen)) {
+ LOGW("Invalid/short AppToHost event of size %zu", event_data.size());
+ return false;
+ }
+
+ return true;
+}
+
+const HostHubRawPacket *AppToHostEvent::GetTypedData() const {
+ // After the event type header (uint32_t), we should have struct
+ // HostHubRawPacket
+ if (event_data.size() < sizeof(uint32_t) + sizeof(struct HostHubRawPacket)) {
+ LOGW("Invalid/short AppToHost event of size %zu", event_data.size());
+ return nullptr;
+ }
+ return reinterpret_cast<const HostHubRawPacket *>(
+ event_data.data() + sizeof(uint32_t));
+}
+
+} // namespace android
diff --git a/util/nanotool/apptohostevent.h b/util/nanotool/apptohostevent.h
new file mode 100644
index 0000000..e638062
--- /dev/null
+++ b/util/nanotool/apptohostevent.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef APP_TO_HOST_EVENT_H_
+#define APP_TO_HOST_EVENT_H_
+
+#include "contexthub.h"
+#include "nanomessage.h"
+
+namespace android {
+
+// Copied from nanohub eventnums.h
+struct HostHubRawPacket {
+ uint64_t appId;
+ uint8_t dataLen; //not incl this header, 128 bytes max
+ //raw data in unspecified format here
+} __attribute((packed));
+
+// The u64 appId used in nanohub is 40 bits vendor ID + 24 bits app ID (see seos.h)
+constexpr uint64_t MakeAppId(uint64_t vendorId, uint32_t appId) {
+ return (vendorId << 24) | (appId & 0x00FFFFFF);
+}
+
+constexpr uint64_t kAppIdVendorGoogle = 0x476f6f676cULL; // "Googl"
+
+constexpr uint64_t kAppIdBoschBmi160Bmm150 = MakeAppId(kAppIdVendorGoogle, 2);
+constexpr uint64_t kAppIdBoschBmp280 = MakeAppId(kAppIdVendorGoogle, 5);
+constexpr uint64_t kAppIdAmsTmd2772 = MakeAppId(kAppIdVendorGoogle, 9);
+constexpr uint64_t kAppIdRohmRpr0521 = MakeAppId(kAppIdVendorGoogle, 10);
+constexpr uint64_t kAppIdAmsTmd4903 = MakeAppId(kAppIdVendorGoogle, 12);
+
+/*
+ * These classes represent events sent with event type EVT_APP_TO_HOST. This is
+ * a generic container for arbitrary application-specific data, and is used for
+ * passing back sensor calibration results, implementing app download, etc. The
+ * parser must know the application ID to determine the data format.
+ */
+
+class AppToHostEvent : public ReadEventResponse {
+ public:
+ /*
+ * Constructs and populates an AppToHostEvent instance. Returns nullptr if
+ * the packet is malformed. The rest of the methods in this class are not
+ * guaranteed to be safe unless the object is constructed from this
+ * function.
+ */
+ static std::unique_ptr<AppToHostEvent> FromBytes(
+ const std::vector<uint8_t>& buffer);
+
+ uint64_t GetAppId() const;
+ // Gets the length of the application-specific data segment
+ uint8_t GetDataLen() const;
+ // Returns a pointer to the application-specific data (i.e. past the header)
+ const uint8_t *GetDataPtr() const;
+
+ bool IsCalibrationEventForSensor(SensorType sensor_type) const;
+ virtual bool IsValid() const;
+
+ protected:
+ const HostHubRawPacket *GetTypedData() const;
+};
+
+#define SENSOR_APP_MSG_CALIBRATION_RESULT (0)
+
+struct SensorAppEventHeader {
+ uint8_t msgId;
+ uint8_t sensorType;
+ uint8_t status; // 0 for success
+} __attribute__((packed));
+
+struct SingleAxisCalibrationResult : public SensorAppEventHeader {
+ int32_t bias;
+} __attribute__((packed));
+
+struct TripleAxisCalibrationResult : public SensorAppEventHeader {
+ int32_t xBias;
+ int32_t yBias;
+ int32_t zBias;
+} __attribute__((packed));
+
+struct FloatCalibrationResult : public SensorAppEventHeader {
+ float value;
+} __attribute__((packed));
+
+struct FourAxisCalibrationResult : public SensorAppEventHeader {
+ int32_t xBias;
+ int32_t yBias;
+ int32_t zBias;
+ int32_t wBias;
+} __attribute__((packed));
+
+
+} // namespace android
+
+#endif // APP_TO_HOST_EVENT_H_
diff --git a/util/nanotool/calibrationfile.cpp b/util/nanotool/calibrationfile.cpp
new file mode 100644
index 0000000..3f79397
--- /dev/null
+++ b/util/nanotool/calibrationfile.cpp
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibrationfile.h"
+
+#include "file.h"
+#include "log.h"
+
+namespace android {
+
+constexpr char kCalibrationFile[] = "/persist/sensorcal.json";
+
+std::shared_ptr<CalibrationFile> CalibrationFile::instance_;
+
+std::shared_ptr<CalibrationFile> CalibrationFile::Instance() {
+ if (!CalibrationFile::instance_) {
+ auto inst = std::shared_ptr<CalibrationFile>(new CalibrationFile());
+ if (inst->Initialize()) {
+ CalibrationFile::instance_ = inst;
+ }
+ }
+
+ return CalibrationFile::instance_;
+}
+
+bool CalibrationFile::Initialize() {
+ file_ = std::unique_ptr<File>(new File(kCalibrationFile, "rw"));
+
+ status_t err = file_->initCheck();
+ if (err != OK) {
+ LOGE("Couldn't open calibration file: %d (%s)", err, strerror(-err));
+ return false;
+ }
+
+ off64_t file_size = file_->seekTo(0, SEEK_END);
+ if (file_size > 0) {
+ auto file_data = std::vector<char>(file_size);
+ file_->seekTo(0, SEEK_SET);
+ ssize_t bytes_read = file_->read(file_data.data(), file_size);
+ if (bytes_read != file_size) {
+ LOGE("Read of configuration file returned %zd, expected %" PRIu64,
+ bytes_read, file_size);
+ return false;
+ }
+
+ sp<JSONCompound> json = JSONCompound::Parse(file_data.data(), file_size);
+ if (json == nullptr || !json->isObject()) {
+ // If there's an existing file and we couldn't parse it, or it
+ // parsed to something unexpected, then we don't want to wipe out
+ // the file - the user needs to decide what to do, e.g. they can
+ // manually edit to fix corruption, or delete it, etc.
+ LOGE("Couldn't parse sensor calibration file (requires manual "
+ "resolution)");
+ return false;
+ } else {
+ json_root_ = reinterpret_cast<JSONObject*>(json.get());
+ LOGD("Parsed JSONObject from file:\n%s",
+ json_root_->toString().c_str());
+ }
+ }
+
+ // No errors, but there was no existing calibration data so construct a new
+ // object
+ if (json_root_ == nullptr) {
+ json_root_ = new JSONObject();
+ }
+
+ return true;
+}
+
+const sp<JSONObject> CalibrationFile::GetJSONObject() const {
+ return json_root_;
+}
+
+bool CalibrationFile::SetSingleAxis(const char *key, int32_t value) {
+ json_root_->setInt32(key, value);
+ return true;
+}
+
+bool CalibrationFile::SetSingleAxis(const char *key, float value) {
+ json_root_->setFloat(key, value);
+ return true;
+}
+
+bool CalibrationFile::SetTripleAxis(const char *key, int32_t x, int32_t y,
+ int32_t z) {
+ sp<JSONArray> json_array = new JSONArray();
+ json_array->addInt32(x);
+ json_array->addInt32(y);
+ json_array->addInt32(z);
+ json_root_->setArray(key, json_array);
+ return true;
+}
+
+bool CalibrationFile::SetFourAxis(const char *key, int32_t x, int32_t y,
+ int32_t z, int32_t w) {
+ sp<JSONArray> json_array = new JSONArray();
+ json_array->addInt32(x);
+ json_array->addInt32(y);
+ json_array->addInt32(z);
+ json_array->addInt32(w);
+ json_root_->setArray(key, json_array);
+ return true;
+}
+
+bool CalibrationFile::Save() {
+ AString json_str = json_root_->toString();
+ LOGD("Saving JSONObject to file (%zd bytes):\n%s", json_str.size(),
+ json_str.c_str());
+ file_->seekTo(0, SEEK_SET);
+ ssize_t bytes_written = file_->write(json_str.c_str(), json_str.size());
+ if (bytes_written < 0 || static_cast<size_t>(bytes_written) != json_str.size()) {
+ LOGE("Write returned %zd, expected %zu", bytes_written, json_str.size());
+ return false;
+ }
+ return true;
+}
+
+} // namespace android
diff --git a/util/nanotool/calibrationfile.h b/util/nanotool/calibrationfile.h
new file mode 100644
index 0000000..22b44ce
--- /dev/null
+++ b/util/nanotool/calibrationfile.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CALIBRATION_FILE_H_
+#define CALIBRATION_FILE_H_
+
+#include <inttypes.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "file.h"
+#include "noncopyable.h"
+#include "JSONObject.h"
+#include <utils/RefBase.h>
+
+namespace android {
+
+class CalibrationFile : public NonCopyable {
+ public:
+ // Get a pointer to the singleton instance
+ static std::shared_ptr<CalibrationFile> Instance();
+
+ const sp<JSONObject> GetJSONObject() const;
+
+ bool SetSingleAxis(const char *key, int32_t value);
+ bool SetSingleAxis(const char *key, float value);
+ bool SetTripleAxis(const char *key, int32_t x, int32_t y, int32_t z);
+ bool SetFourAxis(const char *key, int32_t x, int32_t y, int32_t z,
+ int32_t w);
+
+ bool Save();
+
+ private:
+ CalibrationFile() : file_(nullptr), json_root_(nullptr) {}
+
+ static std::shared_ptr<CalibrationFile> instance_;
+ bool Initialize();
+
+ std::unique_ptr<File> file_;
+ sp<JSONObject> json_root_;
+
+ bool Read();
+};
+
+} // namespace android
+
+#endif // CALIBRATION_FILE_H_
diff --git a/util/nanotool/contexthub.cpp b/util/nanotool/contexthub.cpp
new file mode 100644
index 0000000..b734e3a
--- /dev/null
+++ b/util/nanotool/contexthub.cpp
@@ -0,0 +1,546 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "contexthub.h"
+
+#include <cstring>
+#include <errno.h>
+#include <vector>
+
+#include "apptohostevent.h"
+#include "log.h"
+#include "resetreasonevent.h"
+#include "sensorevent.h"
+#include "util.h"
+
+namespace android {
+
+#define UNUSED_PARAM(param) (void) (param)
+
+constexpr int kCalibrationTimeoutMs(10000);
+
+struct SensorTypeNames {
+ SensorType sensor_type;
+ const char *name_abbrev;
+};
+
+static const SensorTypeNames sensor_names_[] = {
+ { SensorType::Accel, "accel" },
+ { SensorType::AnyMotion, "anymo" },
+ { SensorType::NoMotion, "nomo" },
+ { SensorType::SignificantMotion, "sigmo" },
+ { SensorType::Flat, "flat" },
+ { SensorType::Gyro, "gyro" },
+ //{ SensorType::GyroUncal, "gyro_uncal" },
+ { SensorType::Magnetometer, "mag" },
+ //{ SensorType::MagnetometerUncal, "mag_uncal" },
+ { SensorType::Barometer, "baro" },
+ { SensorType::Temperature, "temp" },
+ { SensorType::AmbientLightSensor, "als" },
+ { SensorType::Proximity, "prox" },
+ { SensorType::Orientation, "orien" },
+ //{ SensorType::HeartRateECG, "ecg" },
+ //{ SensorType::HeartRatePPG, "ppg" },
+ { SensorType::Gravity, "gravity" },
+ { SensorType::LinearAccel, "linear_acc" },
+ { SensorType::RotationVector, "rotation" },
+ { SensorType::GeomagneticRotationVector, "geomag" },
+ { SensorType::GameRotationVector, "game" },
+ { SensorType::StepCount, "step_cnt" },
+ { SensorType::StepDetect, "step_det" },
+ { SensorType::Gesture, "gesture" },
+ { SensorType::Tilt, "tilt" },
+ { SensorType::DoubleTwist, "twist" },
+ { SensorType::DoubleTap, "doubletap" },
+ { SensorType::WindowOrientation, "win_orien" },
+ { SensorType::Hall, "hall" },
+ { SensorType::Activity, "activity" },
+ { SensorType::Vsync, "vsync" },
+};
+
+struct SensorTypeAlias {
+ SensorType sensor_type;
+ SensorType sensor_alias;
+ const char *name_abbrev;
+};
+
+static const SensorTypeAlias sensor_aliases_[] = {
+ { SensorType::Accel, SensorType::CompressedAccel, "compressed_accel" },
+};
+
+bool SensorTypeIsAliasOf(SensorType sensor_type, SensorType alias) {
+ for (size_t i = 0; i < ARRAY_LEN(sensor_aliases_); i++) {
+ if (sensor_aliases_[i].sensor_type == sensor_type
+ && sensor_aliases_[i].sensor_alias == alias) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+SensorType ContextHub::SensorAbbrevNameToType(const char *sensor_name_abbrev) {
+ for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
+ if (strcmp(sensor_names_[i].name_abbrev, sensor_name_abbrev) == 0) {
+ return sensor_names_[i].sensor_type;
+ }
+ }
+
+ return SensorType::Invalid_;
+}
+
+SensorType ContextHub::SensorAbbrevNameToType(const std::string& abbrev_name) {
+ return ContextHub::SensorAbbrevNameToType(abbrev_name.c_str());
+}
+
+std::string ContextHub::SensorTypeToAbbrevName(SensorType sensor_type) {
+ for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
+ if (sensor_names_[i].sensor_type == sensor_type) {
+ return std::string(sensor_names_[i].name_abbrev);
+ }
+ }
+
+ for (unsigned int i = 0; i < ARRAY_LEN(sensor_aliases_); i++) {
+ if (sensor_aliases_[i].sensor_alias == sensor_type) {
+ return std::string(sensor_aliases_[i].name_abbrev);
+ }
+ }
+
+ char buffer[24];
+ snprintf(buffer, sizeof(buffer), "unknown (%d)",
+ static_cast<int>(sensor_type));
+ return std::string(buffer);
+}
+
+std::string ContextHub::ListAllSensorAbbrevNames() {
+ std::string sensor_list;
+ for (unsigned int i = 0; i < ARRAY_LEN(sensor_names_); i++) {
+ sensor_list += sensor_names_[i].name_abbrev;
+ if (i < ARRAY_LEN(sensor_names_) - 1) {
+ sensor_list += ", ";
+ }
+ }
+
+ return sensor_list;
+}
+
+bool ContextHub::Flash(const std::string& filename) {
+ FILE *firmware_file = fopen(filename.c_str(), "r");
+ if (!firmware_file) {
+ LOGE("Failed to open firmware image: %d (%s)", errno, strerror(errno));
+ return false;
+ }
+
+ fseek(firmware_file, 0, SEEK_END);
+ long file_size = ftell(firmware_file);
+ fseek(firmware_file, 0, SEEK_SET);
+
+ auto firmware_data = std::vector<uint8_t>(file_size);
+ size_t bytes_read = fread(firmware_data.data(), sizeof(uint8_t),
+ file_size, firmware_file);
+ fclose(firmware_file);
+
+ if (bytes_read != static_cast<size_t>(file_size)) {
+ LOGE("Read of firmware file returned %zu, expected %ld",
+ bytes_read, file_size);
+ return false;
+ }
+ return FlashSensorHub(firmware_data);
+}
+
+bool ContextHub::CalibrateSensors(const std::vector<SensorSpec>& sensors) {
+ bool success = ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
+ return CalibrateSingleSensor(spec);
+ });
+
+ if (success) {
+ success = SaveCalibration();
+ }
+ return success;
+}
+
+bool ContextHub::EnableSensor(const SensorSpec& spec) {
+ ConfigureSensorRequest req;
+
+ req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
+ req.config.sensor_type = static_cast<uint8_t>(spec.sensor_type);
+ req.config.command = static_cast<uint8_t>(
+ ConfigureSensorRequest::CommandType::Enable);
+ if (spec.special_rate != SensorSpecialRate::None) {
+ req.config.rate = static_cast<uint32_t>(spec.special_rate);
+ } else {
+ req.config.rate = ConfigureSensorRequest::FloatRateToFixedPoint(
+ spec.rate_hz);
+ }
+ req.config.latency = spec.latency_ns;
+
+ LOGI("Enabling sensor %d at rate %.0f Hz (special 0x%x) and latency %.2f ms",
+ spec.sensor_type, spec.rate_hz, spec.special_rate,
+ spec.latency_ns / 1000000.0f);
+ auto result = WriteEvent(req);
+ if (result == TransportResult::Success) {
+ sensor_is_active_[static_cast<int>(spec.sensor_type)] = true;
+ return true;
+ }
+
+ LOGE("Could not enable sensor %d", spec.sensor_type);
+ return false;
+}
+
+bool ContextHub::EnableSensors(const std::vector<SensorSpec>& sensors) {
+ return ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
+ return EnableSensor(spec);
+ });
+}
+
+bool ContextHub::DisableSensor(SensorType sensor_type) {
+ ConfigureSensorRequest req;
+
+ req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
+ req.config.sensor_type = static_cast<uint8_t>(sensor_type);
+ req.config.command = static_cast<uint8_t>(
+ ConfigureSensorRequest::CommandType::Disable);
+
+ // Note that nanohub treats us as a single client, so if we call enable
+ // twice then disable once, the sensor will be disabled
+ LOGI("Disabling sensor %d", sensor_type);
+ auto result = WriteEvent(req);
+ if (result == TransportResult::Success) {
+ sensor_is_active_[static_cast<int>(sensor_type)] = false;
+ return true;
+ }
+
+ LOGE("Could not disable sensor %d", sensor_type);
+ return false;
+}
+
+bool ContextHub::DisableSensors(const std::vector<SensorSpec>& sensors) {
+ return ForEachSensor(sensors, [this](const SensorSpec &spec) -> bool {
+ return DisableSensor(spec.sensor_type);
+ });
+}
+
+bool ContextHub::DisableAllSensors() {
+ bool success = true;
+
+ for (int sensor_type = static_cast<int>(SensorType::Invalid_) + 1;
+ sensor_type < static_cast<int>(SensorType::Max_);
+ ++sensor_type) {
+ success &= DisableSensor(static_cast<SensorType>(sensor_type));
+ }
+
+ return success;
+}
+
+bool ContextHub::DisableActiveSensors() {
+ bool success = true;
+
+ LOGD("Disabling all active sensors");
+ for (int sensor_type = static_cast<int>(SensorType::Invalid_) + 1;
+ sensor_type < static_cast<int>(SensorType::Max_);
+ ++sensor_type) {
+ if (sensor_is_active_[sensor_type]) {
+ success &= DisableSensor(static_cast<SensorType>(sensor_type));
+ }
+ }
+
+ return success;
+}
+
+void ContextHub::PrintAllEvents(unsigned int limit) {
+ bool continuous = (limit == 0);
+ auto event_printer = [&limit, continuous](const SensorEvent& event) -> bool {
+ printf("%s", event.ToString().c_str());
+ return (continuous || --limit > 0);
+ };
+ ReadSensorEvents(event_printer);
+}
+
+void ContextHub::PrintSensorEvents(SensorType type, int limit) {
+ bool continuous = (limit == 0);
+ auto event_printer = [type, &limit, continuous](const SensorEvent& event) -> bool {
+ SensorType event_source = event.GetSensorType();
+ if (event_source == type || SensorTypeIsAliasOf(type, event_source)) {
+ printf("%s", event.ToString().c_str());
+ limit -= event.GetNumSamples();
+ }
+ return (continuous || limit > 0);
+ };
+ ReadSensorEvents(event_printer);
+}
+
+void ContextHub::PrintSensorEvents(const std::vector<SensorSpec>& sensors, int limit) {
+ bool continuous = (limit == 0);
+ auto event_printer = [&sensors, &limit, continuous](const SensorEvent& event) -> bool {
+ SensorType event_source = event.GetSensorType();
+ for (unsigned int i = 0; i < sensors.size(); i++) {
+ if (sensors[i].sensor_type == event_source
+ || SensorTypeIsAliasOf(sensors[i].sensor_type, event_source)) {
+ printf("%s", event.ToString().c_str());
+ limit -= event.GetNumSamples();
+ break;
+ }
+ }
+ return (continuous || limit > 0);
+ };
+ ReadSensorEvents(event_printer);
+}
+
+// Protected methods -----------------------------------------------------------
+
+bool ContextHub::CalibrateSingleSensor(const SensorSpec& sensor) {
+ ConfigureSensorRequest req;
+
+ req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
+ req.config.sensor_type = static_cast<uint8_t>(sensor.sensor_type);
+ req.config.command = static_cast<uint8_t>(
+ ConfigureSensorRequest::CommandType::Calibrate);
+
+ LOGI("Issuing calibration request to sensor %d (%s)", sensor.sensor_type,
+ ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str());
+ auto result = WriteEvent(req);
+ if (result != TransportResult::Success) {
+ LOGE("Failed to calibrate sensor %d", sensor.sensor_type);
+ return false;
+ }
+
+ bool success = false;
+ auto calEventHandler = [this, &sensor, &success](const AppToHostEvent &event) -> bool {
+ if (event.IsCalibrationEventForSensor(sensor.sensor_type)) {
+ success = HandleCalibrationResult(sensor, event);
+ return false;
+ }
+ return true;
+ };
+
+ result = ReadAppEvents(calEventHandler, kCalibrationTimeoutMs);
+ if (result != TransportResult::Success) {
+ LOGE("Error reading calibration response %d", static_cast<int>(result));
+ return false;
+ }
+
+ return success;
+}
+
+bool ContextHub::ForEachSensor(const std::vector<SensorSpec>& sensors,
+ std::function<bool(const SensorSpec&)> callback) {
+ bool success = true;
+
+ for (unsigned int i = 0; success && i < sensors.size(); i++) {
+ success &= callback(sensors[i]);
+ }
+
+ return success;
+}
+
+bool ContextHub::HandleCalibrationResult(const SensorSpec& sensor,
+ const AppToHostEvent &event) {
+ auto hdr = reinterpret_cast<const SensorAppEventHeader *>(event.GetDataPtr());
+ if (hdr->status) {
+ LOGE("Calibration of sensor %d (%s) failed with status %u",
+ sensor.sensor_type,
+ ContextHub::SensorTypeToAbbrevName(sensor.sensor_type).c_str(),
+ hdr->status);
+ return false;
+ }
+
+ bool success = false;
+ switch (sensor.sensor_type) {
+ case SensorType::Accel:
+ case SensorType::Gyro: {
+ auto result = reinterpret_cast<const TripleAxisCalibrationResult *>(
+ event.GetDataPtr());
+ success = SetCalibration(sensor.sensor_type, result->xBias,
+ result->yBias, result->zBias);
+ break;
+ }
+
+ case SensorType::Barometer: {
+ auto result = reinterpret_cast<const FloatCalibrationResult *>(
+ event.GetDataPtr());
+ if (sensor.have_cal_ref) {
+ success = SetCalibration(sensor.sensor_type,
+ (sensor.cal_ref - result->value));
+ }
+ break;
+ }
+
+ case SensorType::Proximity: {
+ auto result = reinterpret_cast<const FourAxisCalibrationResult *>(
+ event.GetDataPtr());
+ success = SetCalibration(sensor.sensor_type, result->xBias,
+ result->yBias, result->zBias, result->wBias);
+ break;
+ }
+
+ case SensorType::AmbientLightSensor: {
+ auto result = reinterpret_cast<const FloatCalibrationResult *>(
+ event.GetDataPtr());
+ if (sensor.have_cal_ref && (result->value != 0.0f)) {
+ success = SetCalibration(sensor.sensor_type,
+ (sensor.cal_ref / result->value));
+ }
+ break;
+ }
+
+ default:
+ LOGE("Calibration not supported for sensor type %d",
+ static_cast<int>(sensor.sensor_type));
+ }
+
+ return success;
+}
+
+ContextHub::TransportResult ContextHub::ReadAppEvents(
+ std::function<bool(const AppToHostEvent&)> callback, int timeout_ms) {
+ using Milliseconds = std::chrono::milliseconds;
+
+ TransportResult result;
+ bool timeout_required = timeout_ms > 0;
+ bool keep_going = true;
+
+ while (keep_going) {
+ if (timeout_required && timeout_ms <= 0) {
+ return TransportResult::Timeout;
+ }
+
+ std::unique_ptr<ReadEventResponse> event;
+
+ SteadyClock start_time = std::chrono::steady_clock::now();
+ result = ReadEvent(&event, timeout_ms);
+ SteadyClock end_time = std::chrono::steady_clock::now();
+
+ auto delta = end_time - start_time;
+ timeout_ms -= std::chrono::duration_cast<Milliseconds>(delta).count();
+
+ if (result == TransportResult::Success && event->IsAppToHostEvent()) {
+ AppToHostEvent *app_event = reinterpret_cast<AppToHostEvent*>(
+ event.get());
+ keep_going = callback(*app_event);
+ } else {
+ if (result != TransportResult::Success) {
+ LOGE("Error %d while reading", static_cast<int>(result));
+ if (result != TransportResult::ParseFailure) {
+ return result;
+ }
+ } else {
+ LOGD("Ignoring non-app-to-host event");
+ }
+ }
+ }
+
+ return TransportResult::Success;
+}
+
+void ContextHub::ReadSensorEvents(std::function<bool(const SensorEvent&)> callback) {
+ TransportResult result;
+ bool keep_going = true;
+
+ while (keep_going) {
+ std::unique_ptr<ReadEventResponse> event;
+ result = ReadEvent(&event);
+ if (result == TransportResult::Success && event->IsSensorEvent()) {
+ SensorEvent *sensor_event = reinterpret_cast<SensorEvent*>(
+ event.get());
+ keep_going = callback(*sensor_event);
+ } else {
+ if (result != TransportResult::Success) {
+ LOGE("Error %d while reading", static_cast<int>(result));
+ if (result != TransportResult::ParseFailure) {
+ break;
+ }
+ } else {
+ LOGD("Ignoring non-sensor event");
+ }
+ }
+ }
+}
+
+bool ContextHub::SendCalibrationData(SensorType sensor_type,
+ const std::vector<uint8_t>& cal_data) {
+ ConfigureSensorRequest req;
+
+ req.config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
+ req.config.sensor_type = static_cast<uint8_t>(sensor_type);
+ req.config.command = static_cast<uint8_t>(
+ ConfigureSensorRequest::CommandType::ConfigData);
+ req.SetAdditionalData(cal_data);
+
+ auto result = WriteEvent(req);
+ return (result == TransportResult::Success);
+}
+
+ContextHub::TransportResult ContextHub::WriteEvent(
+ const WriteEventRequest& request) {
+ return WriteEvent(request.GetBytes());
+}
+
+ContextHub::TransportResult ContextHub::ReadEvent(
+ std::unique_ptr<ReadEventResponse>* response, int timeout_ms) {
+ std::vector<uint8_t> responseBuf(256);
+ ContextHub::TransportResult result = ReadEvent(responseBuf, timeout_ms);
+ if (result == TransportResult::Success) {
+ *response = ReadEventResponse::FromBytes(responseBuf);
+ if (*response == nullptr) {
+ result = TransportResult::ParseFailure;
+ }
+ }
+ return result;
+}
+
+// Stubs for subclasses that don't implement calibration support
+bool ContextHub::LoadCalibration() {
+ LOGE("Loading calibration data not implemented");
+ return false;
+}
+
+bool ContextHub::SetCalibration(SensorType sensor_type, int32_t data) {
+ UNUSED_PARAM(sensor_type);
+ UNUSED_PARAM(data);
+ return false;
+}
+
+bool ContextHub::SetCalibration(SensorType sensor_type, float data) {
+ UNUSED_PARAM(sensor_type);
+ UNUSED_PARAM(data);
+ return false;
+}
+
+bool ContextHub::SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z) {
+ UNUSED_PARAM(sensor_type);
+ UNUSED_PARAM(x);
+ UNUSED_PARAM(y);
+ UNUSED_PARAM(z);
+ return false;
+}
+
+bool ContextHub::SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z, int32_t w) {
+ UNUSED_PARAM(sensor_type);
+ UNUSED_PARAM(x);
+ UNUSED_PARAM(y);
+ UNUSED_PARAM(z);
+ UNUSED_PARAM(w);
+ return false;
+}
+
+bool ContextHub::SaveCalibration() {
+ LOGE("Saving calibration data not implemented");
+ return false;
+}
+
+} // namespace android
diff --git a/util/nanotool/contexthub.h b/util/nanotool/contexthub.h
new file mode 100644
index 0000000..0c6489f
--- /dev/null
+++ b/util/nanotool/contexthub.h
@@ -0,0 +1,264 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CONTEXTHUB_H_
+#define CONTEXTHUB_H_
+
+#include "nanomessage.h"
+#include "noncopyable.h"
+
+#include <bitset>
+#include <functional>
+#include <vector>
+
+namespace android {
+
+class AppToHostEvent;
+class SensorEvent;
+
+// Array length helper macro
+#define ARRAY_LEN(arr) (sizeof(arr) / sizeof(arr[0]))
+
+enum class SensorType {
+ Invalid_ = 0,
+
+ // The order of this enum must correspond to sensor types in nanohub's
+ // sensType.h
+ Accel,
+ AnyMotion,
+ NoMotion,
+ SignificantMotion,
+ Flat,
+ Gyro,
+ GyroUncal,
+ Magnetometer,
+ MagnetometerUncal,
+ Barometer,
+ Temperature,
+ AmbientLightSensor,
+ Proximity,
+ Orientation,
+ HeartRateECG,
+ HeartRatePPG,
+ Gravity,
+ LinearAccel,
+ RotationVector,
+ GeomagneticRotationVector,
+ GameRotationVector,
+ StepCount,
+ StepDetect,
+ Gesture,
+ Tilt,
+ DoubleTwist,
+ DoubleTap,
+ WindowOrientation,
+ Hall,
+ Activity,
+ Vsync,
+ CompressedAccel,
+
+ Max_
+};
+
+// Overloaded values of rate used in sensor enable request (see sensors.h)
+enum class SensorSpecialRate : uint32_t {
+ None = 0,
+ OnDemand = 0xFFFFFF00,
+ OnChange = 0xFFFFFF01,
+ OneShot = 0xFFFFFF02,
+};
+
+struct SensorSpec {
+ SensorType sensor_type = SensorType::Invalid_;
+
+ // When enabling a sensor, rate can be specified in Hz or as one of the
+ // special values
+ SensorSpecialRate special_rate = SensorSpecialRate::None;
+ float rate_hz = -1;
+ uint64_t latency_ns = 0;
+
+ // Reference value (ground truth) used for calibration
+ bool have_cal_ref = false;
+ float cal_ref;
+};
+
+/*
+ * An interface for communicating with a ContextHub.
+ */
+class ContextHub : public NonCopyable {
+ public:
+ virtual ~ContextHub() {};
+
+ static std::string SensorTypeToAbbrevName(SensorType sensor_type);
+ static SensorType SensorAbbrevNameToType(const char *abbrev_name);
+ static SensorType SensorAbbrevNameToType(const std::string& abbrev_name);
+ static std::string ListAllSensorAbbrevNames();
+
+ /*
+ * Performs initialization to allow commands to be sent to the context hub.
+ * Must be called before any other functions that send commands. Returns
+ * true on success, false on failure.
+ */
+ virtual bool Initialize() = 0;
+
+ /*
+ * Configures the ContextHub to allow logs to be printed to stdout.
+ */
+ virtual void SetLoggingEnabled(bool logging_enabled) = 0;
+
+ /*
+ * Loads a new firmware image to the ContextHub. The firmware image is
+ * specified by filename. Returns false if an error occurs.
+ */
+ bool Flash(const std::string& filename);
+
+ /*
+ * Performs the sensor calibration routine and writes the resulting data to
+ * a file.
+ */
+ bool CalibrateSensors(const std::vector<SensorSpec>& sensors);
+
+ /*
+ * Sends a sensor enable request to the context hub.
+ */
+ bool EnableSensor(const SensorSpec& sensor);
+ bool EnableSensors(const std::vector<SensorSpec>& sensors);
+
+ /*
+ * Sends a disable sensor request to context hub. Note that this always
+ * results in sending a request, i.e. this does not check whether the sensor
+ * is currently enabled or not.
+ */
+ bool DisableSensor(SensorType sensor_type);
+ bool DisableSensors(const std::vector<SensorSpec>& sensors);
+
+ /*
+ * Sends a disable sensor request for every sensor type we know about.
+ */
+ bool DisableAllSensors();
+
+ /*
+ * Calls DisableSensor() on all active sensors (i.e. those which have been
+ * enabled but not yet disabled). This should be called from the destructor
+ * of derived classes before tearing down communications to ensure we don't
+ * leave sensors enabled after exiting.
+ */
+ bool DisableActiveSensors();
+
+ /*
+ * Sends all data stored in the calibration file to the context hub.
+ */
+ virtual bool LoadCalibration();
+
+ /*
+ * Prints up to <limit> incoming events. If limit is 0, then continues
+ * indefinitely.
+ */
+ void PrintAllEvents(unsigned int limit);
+
+ /*
+ * Prints up to <sample_limit> incoming sensor samples corresponding to the
+ * given SensorType, ignoring other events. If sample_limit is 0, then
+ * continues indefinitely.
+ */
+ void PrintSensorEvents(SensorType sensor_type, int sample_limit);
+ void PrintSensorEvents(const std::vector<SensorSpec>& sensors,
+ int sample_limit);
+
+ protected:
+ enum class TransportResult {
+ Success,
+ GeneralFailure,
+ Timeout,
+ ParseFailure,
+ Canceled,
+ // Add more specific error reasons as needed
+ };
+
+ // Performs the calibration routine, but does not call SaveCalibration()
+ bool CalibrateSingleSensor(const SensorSpec& sensor);
+
+ /*
+ * Iterates over sensors, invoking the given callback on each element.
+ * Returns true if all callbacks returned true. Exits early on failure.
+ */
+ bool ForEachSensor(const std::vector<SensorSpec>& sensors,
+ std::function<bool(const SensorSpec&)> callback);
+
+ /*
+ * Parses a calibration result event and invokes the appropriate
+ * SetCalibration function with the calibration data.
+ */
+ bool HandleCalibrationResult(const SensorSpec& sensor,
+ const AppToHostEvent &event);
+
+ /*
+ * Same as ReadSensorEvents, but filters on AppToHostEvent instead of
+ * SensorEvent.
+ */
+ TransportResult ReadAppEvents(std::function<bool(const AppToHostEvent&)> callback,
+ int timeout_ms = 0);
+
+ /*
+ * Calls ReadEvent in a loop, handling errors and ignoring events that
+ * didn't originate from a sensor. Valid SensorEvents are passed to the
+ * callback for further processing. The callback should return a boolean
+ * indicating whether to continue (true) or exit the read loop (false).
+ */
+ void ReadSensorEvents(std::function<bool(const SensorEvent&)> callback);
+
+ /*
+ * Sends the given calibration data down to the hub
+ */
+ bool SendCalibrationData(SensorType sensor_type,
+ const std::vector<uint8_t>& cal_data);
+
+ /*
+ * Read an event from the sensor hub. Block until a event is successfully
+ * read, no event traffic is generated for the timeout period, or an error
+ * occurs, such as a CRC check failure.
+ */
+ virtual TransportResult ReadEvent(std::vector<uint8_t>& response,
+ int timeout_ms) = 0;
+ virtual TransportResult WriteEvent(const std::vector<uint8_t>& request) = 0;
+
+ // Implements the firmware loading functionality for the sensor hub. Returns
+ // false if an error occurs while writing the firmware to the device.
+ virtual bool FlashSensorHub(const std::vector<uint8_t>& bytes) = 0;
+
+ // Convenience functions that build on top of the more generic byte-level
+ // interface
+ TransportResult ReadEvent(std::unique_ptr<ReadEventResponse>* response,
+ int timeout_ms = 0);
+ TransportResult WriteEvent(const WriteEventRequest& request);
+
+ // Override these if saving calibration data to persistent storage is
+ // supported on the platform
+ virtual bool SetCalibration(SensorType sensor_type, int32_t data);
+ virtual bool SetCalibration(SensorType sensor_type, float data);
+ virtual bool SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z);
+ virtual bool SetCalibration(SensorType sensor_type, int32_t x,
+ int32_t y, int32_t z, int32_t w);
+ virtual bool SaveCalibration();
+
+private:
+ std::bitset<static_cast<int>(SensorType::Max_)> sensor_is_active_;
+};
+
+} // namespace android
+
+#endif // CONTEXTHUB_H_
diff --git a/util/nanotool/log.cpp b/util/nanotool/log.cpp
new file mode 100644
index 0000000..eec303b
--- /dev/null
+++ b/util/nanotool/log.cpp
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "log.h"
+
+#include <cctype>
+#include <chrono>
+#include <cstdio>
+#include <string>
+
+namespace android {
+
+Log::LogLevel Log::level_;
+Logger* Log::logger_;
+std::chrono::time_point<std::chrono::steady_clock> Log::init_time_;
+
+void Log::Initialize(Logger *logger, LogLevel level) {
+ if (Log::logger_) {
+ Log::Warn("Re-initializing logger");
+ }
+ Log::init_time_ = std::chrono::steady_clock::now();
+ Log::logger_ = logger;
+ Log::SetLevel(level);
+}
+
+void Log::SetLevel(LogLevel level) {
+ Log::level_ = level;
+}
+
+#define LOG_EX_VARARGS(level, format) \
+ do { \
+ va_list arg_list; \
+ va_start(arg_list, format); \
+ Log::LogEx(level, format, arg_list); \
+ va_end(arg_list); \
+ } while (0)
+
+void Log::Error(const char *format, ...) {
+ LOG_EX_VARARGS(LogLevel::Error, format);
+}
+
+void Log::Warn(const char *format, ...) {
+ LOG_EX_VARARGS(LogLevel::Warn, format);
+}
+
+void Log::Info(const char *format, ...) {
+ LOG_EX_VARARGS(LogLevel::Info, format);
+}
+
+void Log::Debug(const char *format, ...) {
+ LOG_EX_VARARGS(LogLevel::Debug, format);
+}
+
+void Log::DebugBuf(std::vector<uint8_t> vec) {
+ Log::DebugBuf(vec.data(), vec.size());
+}
+
+void Log::DebugBuf(const uint8_t *buffer, size_t size) {
+ if (Log::level_ < LogLevel::Debug) {
+ return;
+ }
+
+ char line[32];
+ int offset = 0;
+ char line_chars[32];
+ int offset_chars = 0;
+
+ Log::Debug("Dumping buffer of size %zu bytes", size);
+ for (size_t i = 1; i <= size; ++i) {
+ offset += snprintf(&line[offset], sizeof(line) - offset, "%02x ",
+ buffer[i - 1]);
+ offset_chars += snprintf(
+ &line_chars[offset_chars], sizeof(line_chars) - offset_chars,
+ "%c", (isprint(buffer[i - 1])) ? buffer[i - 1] : '.');
+ if ((i % 8) == 0) {
+ Log::Debug(" %s\t%s", line, line_chars);
+ offset = 0;
+ offset_chars = 0;
+ } else if ((i % 4) == 0) {
+ offset += snprintf(&line[offset], sizeof(line) - offset, " ");
+ }
+ }
+
+ if (offset > 0) {
+ std::string tabs;
+ while (offset < 28) {
+ tabs += "\t";
+ offset += 8;
+ }
+ Log::Debug(" %s%s%s", line, tabs.c_str(), line_chars);
+ }
+}
+
+char Log::LevelAbbrev(LogLevel level) {
+ switch (level) {
+ case LogLevel::Error:
+ return 'E';
+ case LogLevel::Warn:
+ return 'W';
+ case LogLevel::Info:
+ return 'I';
+ case LogLevel::Debug:
+ return 'D';
+ default:
+ return '?';
+ }
+}
+
+void Log::LogEx(LogLevel level, const char *format, va_list arg_list) {
+ if (Log::level_ < level) {
+ return;
+ }
+
+ std::chrono::duration<float> log_time =
+ (std::chrono::steady_clock::now() - Log::init_time_);
+
+ // Can add colorization here if desired (should be configurable)
+ char prefix[20];
+ snprintf(prefix, sizeof(prefix), "%c %6.03f: ", Log::LevelAbbrev(level),
+ log_time.count());
+
+ Log::logger_->Output(prefix);
+ Log::logger_->Output(format, arg_list);
+ Log::logger_->Output("\n");
+}
+
+void PrintfLogger::Output(const char *str) {
+ printf("%s", str);
+}
+
+void PrintfLogger::Output(const char *format, va_list arg_list) {
+ vprintf(format, arg_list);
+}
+
+} // namespace android
diff --git a/util/nanotool/log.h b/util/nanotool/log.h
new file mode 100644
index 0000000..2a168f2
--- /dev/null
+++ b/util/nanotool/log.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOG_H_
+#define LOG_H_
+
+#include <stdarg.h>
+
+#include <chrono>
+#include <vector>
+
+namespace android {
+
+/*
+ * Prefer to use these macros instead of calling Log::Error, etc. directly, in
+ * case we want to add tracing of the source file and line number, or compile
+ * out logging completely, etc.
+ */
+#define LOGE(fmt, ...) Log::Error(fmt, ##__VA_ARGS__)
+#define LOGW(fmt, ...) Log::Warn(fmt, ##__VA_ARGS__)
+#define LOGI(fmt, ...) Log::Info(fmt, ##__VA_ARGS__)
+#define LOGD(fmt, ...) Log::Debug(fmt, ##__VA_ARGS__)
+
+#define LOGD_BUF(buf, len) Log::DebugBuf((const uint8_t *) buf, len)
+#define LOGD_VEC(vec) Log::DebugBuf(vec)
+
+// Interface for a log output method
+class Logger {
+ public:
+ virtual ~Logger() {};
+ virtual void Output(const char *str) = 0;
+ virtual void Output(const char *format, va_list arg_list) = 0;
+};
+
+// Singleton used to log messages to an arbitrary output
+class Log {
+ public:
+ enum class LogLevel {
+ // Use with SetLevel to disable logging
+ Disable,
+ Error,
+ Warn,
+ Info,
+ Debug,
+ };
+
+ // Define the logging mechanism and minimum log level that will be printed
+ static void Initialize(Logger *logger, LogLevel level);
+
+ __attribute__((__format__ (printf, 1, 2)))
+ static void Error(const char *format, ...);
+
+ __attribute__((__format__ (printf, 1, 2)))
+ static void Warn(const char *format, ...);
+
+ __attribute__((__format__ (printf, 1, 2)))
+ static void Info(const char *format, ...);
+
+ __attribute__((__format__ (printf, 1, 2)))
+ static void Debug(const char *format, ...);
+
+ static void DebugBuf(std::vector<uint8_t> vec);
+ static void DebugBuf(const uint8_t *buffer, size_t size);
+
+ // Allows for updating the logging level after initialization
+ static void SetLevel(LogLevel level);
+
+ private:
+ static char LevelAbbrev(LogLevel level);
+ static void LogEx(LogLevel level, const char *format, va_list arg_list);
+
+ static Logger* logger_;
+ static LogLevel level_;
+ static std::chrono::time_point<std::chrono::steady_clock> init_time_;
+};
+
+class PrintfLogger : public Logger {
+ public:
+ void Output(const char *str);
+ void Output(const char *format, va_list arg_list);
+};
+
+} // namespace android
+
+#endif // LOG_H_
diff --git a/util/nanotool/nanomessage.cpp b/util/nanotool/nanomessage.cpp
new file mode 100644
index 0000000..340980f
--- /dev/null
+++ b/util/nanotool/nanomessage.cpp
@@ -0,0 +1,227 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "nanomessage.h"
+
+#include <inttypes.h>
+#include <stdio.h>
+
+#include "apptohostevent.h"
+#include "log.h"
+#include "resetreasonevent.h"
+#include "sensorevent.h"
+
+namespace android {
+
+/* HardwareVersionInfo ********************************************************/
+
+bool HardwareVersionInfo::Populate(const std::vector<uint8_t>& buffer) {
+ if (buffer.size() != sizeof(VersionInfo)) {
+ return false;
+ }
+
+ const uint8_t *data = buffer.data();
+ const VersionInfo *source = reinterpret_cast<const VersionInfo *>(data);
+ info = *source;
+ return true;
+}
+
+std::string HardwareVersionInfo::ToString() const {
+ const char format_string[] = "Hardware version info:\n"
+ " Hardware type: %04x\n"
+ " Hardware version: %04x\n"
+ " Bootloader version: %04x\n"
+ " Operating system version: %04x\n"
+ " Variant version: %08x\n";
+
+ char buffer[1024];
+ snprintf(buffer, sizeof(buffer), format_string,
+ info.hardware_type,
+ info.hardware_version,
+ info.bootloader_version,
+ info.operating_system_version,
+ info.variant_version);
+ return std::string(buffer);
+}
+
+/* WriteEventResponse *********************************************************/
+
+std::string WriteEventResponse::ToString() const {
+ const char format_string[] = "Write event accepted: %s\n";
+
+ char buffer[128];
+ snprintf(buffer, sizeof(buffer), format_string,
+ response.accepted ? "true" : "false");
+ return std::string(buffer);
+}
+
+bool WriteEventResponse::Populate(const std::vector<uint8_t>& buffer) {
+ if (buffer.size() != sizeof(Response)) {
+ return false;
+ }
+
+ const uint8_t *data = buffer.data();
+ const Response *source = reinterpret_cast<const Response *>(data);
+ response = *source;
+ return true;
+
+}
+
+/* ReadEventRequest ***********************************************************/
+
+std::vector<uint8_t> ReadEventRequest::GetBytes() const {
+ std::vector<uint8_t> buffer(sizeof(Request));
+
+ uint8_t *data = buffer.data();
+ Request *req = reinterpret_cast<Request *>(data);
+ *req = request;
+ return buffer;
+}
+
+std::string ReadEventRequest::ToString() const {
+ const char format_string[] = "Read event at time: %" PRIx64 "\n";
+
+ char buffer[128];
+ snprintf(buffer, sizeof(buffer), format_string,
+ request.boot_time);
+ return std::string(buffer);
+}
+
+/* ReadEventResponse **********************************************************/
+
+std::string ReadEventResponse::ToString() const {
+ char buffer[32];
+ snprintf(buffer, sizeof(buffer), "ReadEventResponse %u\n", GetEventType());
+ return std::string(buffer);
+}
+
+std::unique_ptr<ReadEventResponse> ReadEventResponse::FromBytes(
+ const std::vector<uint8_t>& buffer) {
+ // The first 4 bytes of any event must be the event type - use it to figure
+ // out which class to construct
+ uint32_t event_type = ReadEventResponse::EventTypeFromBuffer(buffer);
+ if (ReadEventResponse::IsSensorEvent(event_type)) {
+ return SensorEvent::FromBytes(buffer);
+ } else if (ReadEventResponse::IsAppToHostEvent(event_type)) {
+ return AppToHostEvent::FromBytes(buffer);
+ } else if (ReadEventResponse::IsResetReasonEvent(event_type)) {
+ return ResetReasonEvent::FromBytes(buffer);
+ } else {
+ LOGW("Received unexpected/unsupported event type %u", event_type);
+ return nullptr;
+ }
+}
+
+bool ReadEventResponse::Populate(const std::vector<uint8_t>& buffer) {
+ if (buffer.size() < sizeof(Event)) {
+ return false;
+ }
+
+ event_data.resize(buffer.size());
+ std::copy(buffer.begin(), buffer.end(), event_data.begin());
+ return true;
+}
+
+bool ReadEventResponse::IsAppToHostEvent() const {
+ return ReadEventResponse::IsAppToHostEvent(GetEventType());
+}
+
+bool ReadEventResponse::IsSensorEvent() const {
+ return ReadEventResponse::IsSensorEvent(GetEventType());
+}
+
+bool ReadEventResponse::IsResetReasonEvent() const {
+ return ReadEventResponse::IsResetReasonEvent(GetEventType());
+}
+
+uint32_t ReadEventResponse::GetEventType() const {
+ return ReadEventResponse::EventTypeFromBuffer(event_data);
+}
+
+bool ReadEventResponse::IsSensorEvent(uint32_t event_type) {
+ return (event_type >= static_cast<uint32_t>(EventType::FirstSensorEvent) &&
+ event_type <= static_cast<uint32_t>(EventType::LastSensorEvent));
+}
+
+bool ReadEventResponse::IsAppToHostEvent(uint32_t event_type) {
+ return (event_type == static_cast<uint32_t>(EventType::AppToHostEvent));
+}
+
+bool ReadEventResponse::IsResetReasonEvent(uint32_t event_type) {
+ return (event_type == static_cast<uint32_t>(EventType::ResetReasonEvent));
+}
+
+uint32_t ReadEventResponse::EventTypeFromBuffer(const std::vector<uint8_t>& buffer) {
+ if (buffer.size() < sizeof(uint32_t)) {
+ LOGW("Invalid/short event of size %zu", buffer.size());
+ return 0;
+ }
+ return *reinterpret_cast<const uint32_t *>(buffer.data());
+}
+
+/* ConfigureSensorRequest *****************************************************/
+
+ConfigureSensorRequest::ConfigureSensorRequest() {
+ config.event_type = static_cast<uint32_t>(EventType::ConfigureSensor);
+}
+
+uint32_t ConfigureSensorRequest::FloatRateToFixedPoint(float rate) {
+ return rate * 1024.0f;
+}
+
+float ConfigureSensorRequest::FixedPointRateToFloat(uint32_t rate) {
+ return rate / 1024.0f;
+}
+
+// TODO(aarossig): Consider writing a template function for this.
+std::vector<uint8_t> ConfigureSensorRequest::GetBytes() const {
+ std::vector<uint8_t> buffer(sizeof(Configuration));
+
+ uint8_t *data = buffer.data();
+ Configuration *configuration = reinterpret_cast<Configuration *>(data);
+ *configuration = config;
+ buffer.insert(buffer.end(), extra_data_.begin(), extra_data_.end());
+
+ return buffer;
+}
+
+void ConfigureSensorRequest::SetAdditionalData(const std::vector<uint8_t>& data) {
+ extra_data_ = data;
+}
+
+std::string ConfigureSensorRequest::ToString() const {
+ const char format_string[] = "Sensor configuration:\n"
+ " latency: %" PRIx64 "\n"
+ " rate (fixed point): %08x\n"
+ " sensor_type: %02x\n"
+ " command: %02x\n"
+ " flags: %04x\n";
+
+ char buffer[1024];
+ snprintf(buffer, sizeof(buffer), format_string,
+ config.latency,
+ config.rate,
+ config.sensor_type,
+ config.command,
+ config.flags);
+ return std::string(buffer);
+}
+
+EventType ConfigureSensorRequest::GetEventType() const {
+ return static_cast<EventType>(config.event_type);
+}
+
+} // namespace android
diff --git a/util/nanotool/nanomessage.h b/util/nanotool/nanomessage.h
new file mode 100644
index 0000000..8495233
--- /dev/null
+++ b/util/nanotool/nanomessage.h
@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef NANOMESSAGE_H_
+#define NANOMESSAGE_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "noncopyable.h"
+
+namespace android {
+
+/*
+ * Events types that can be pushed back and forth between the ContextHub and
+ * host software.
+ */
+enum class EventType {
+ FirstSensorEvent = 0x00000200,
+ LastSensorEvent = 0x000002FF,
+ ConfigureSensor = 0x00000300,
+ AppToHostEvent = 0x00000401,
+ ResetReasonEvent = 0x00000403,
+};
+
+/*
+ * An interface for all messages passed to and from the ContextHub.
+ */
+class NanoMessage : public NonCopyable {
+ public:
+ virtual ~NanoMessage() {};
+
+ // Generates a string intended to be printed to a console or saved to logs.
+ // This interface requires that the string be terminated with a newline.
+ virtual std::string ToString() const = 0;
+};
+
+/*
+ * An interface for requests sent to the ContextHub.
+ */
+class NanoRequest : public NanoMessage {
+ public:
+ // Returns a payload of bytes to be packaged into a NanoPacket.
+ virtual std::vector<uint8_t> GetBytes() const = 0;
+};
+
+/*
+ * An interface for responses from the ContextHub.
+ */
+class NanoResponse : public NanoMessage {
+ public:
+ // Populates the fields of the NanoMessage given a NanoPacket. Returns
+ // false if the packet is incomplete or incorrect message.
+ virtual bool Populate(const std::vector<uint8_t>& buffer) = 0;
+};
+
+/*
+ * Version information for a ContextHub.
+ */
+class HardwareVersionInfo : public NanoResponse {
+ public:
+ bool Populate(const std::vector<uint8_t>& buffer) override;
+ std::string ToString() const override;
+
+ struct VersionInfo {
+ uint16_t hardware_type;
+ uint16_t hardware_version;
+ uint16_t bootloader_version;
+ uint16_t operating_system_version;
+ uint32_t variant_version;
+ } __attribute__((packed)) info;
+};
+
+/*
+ * The base event for all event data.
+ */
+struct Event {
+ uint32_t event_type;
+} __attribute__((packed));
+
+/*
+ * A request to write an event to the ContextHub.
+ */
+class WriteEventRequest : public NanoRequest {
+ public:
+ virtual EventType GetEventType() const = 0;
+};
+
+/*
+ * A response to writing an event to the ContextHub.
+ */
+class WriteEventResponse : public NanoResponse {
+ public:
+ std::string ToString() const override;
+ bool Populate(const std::vector<uint8_t>& buffer) override;
+
+ struct Response {
+ bool accepted;
+ } __attribute__((packed)) response;
+};
+
+/*
+ * A response to reading an event from the ContextHub.
+ */
+class ReadEventRequest : public NanoRequest {
+ public:
+ std::vector<uint8_t> GetBytes() const override;
+ std::string ToString() const override;
+
+ struct Request {
+ uint64_t boot_time;
+ } __attribute__((packed)) request;
+};
+
+class ReadEventResponse : public NanoResponse {
+ public:
+ virtual std::string ToString() const override;
+
+ // Construct and populate a concrete ReadEventResponse from the given buffer
+ static std::unique_ptr<ReadEventResponse> FromBytes(
+ const std::vector<uint8_t>& buffer);
+
+ bool Populate(const std::vector<uint8_t>& buffer) override;
+
+ bool IsAppToHostEvent() const;
+ bool IsSensorEvent() const;
+ bool IsResetReasonEvent() const;
+ uint32_t GetEventType() const;
+
+ // Event data associated with this response.
+ std::vector<uint8_t> event_data;
+
+ protected:
+ static uint32_t EventTypeFromBuffer(const std::vector<uint8_t>& buffer);
+ static bool IsAppToHostEvent(uint32_t event_type);
+ static bool IsSensorEvent(uint32_t event_type);
+ static bool IsResetReasonEvent(uint32_t event_type);
+};
+
+/*
+ * An event used to configure a sensor with specific attributes.
+ */
+class ConfigureSensorRequest : public WriteEventRequest {
+ public:
+ enum class CommandType {
+ Disable,
+ Enable,
+ Flush,
+ ConfigData,
+ Calibrate
+ };
+
+ ConfigureSensorRequest();
+
+ static uint32_t FloatRateToFixedPoint(float rate);
+ static float FixedPointRateToFloat(uint32_t rate);
+
+ std::vector<uint8_t> GetBytes() const override;
+ std::string ToString() const override;
+ EventType GetEventType() const override;
+
+ // Appends some data to the configuration request, e.g. for the ConfigData
+ // command
+ void SetAdditionalData(const std::vector<uint8_t>& data);
+
+ struct Configuration : public Event {
+ uint64_t latency;
+ uint32_t rate;
+ uint8_t sensor_type;
+ uint8_t command;
+ uint16_t flags;
+ } __attribute__((packed)) config = {};
+
+ private:
+ std::vector<uint8_t> extra_data_;
+};
+
+} // namespace android
+
+#endif // NANOMESSAGE_H_
diff --git a/util/nanotool/nanopacket.cpp b/util/nanotool/nanopacket.cpp
new file mode 100644
index 0000000..b0b1703
--- /dev/null
+++ b/util/nanotool/nanopacket.cpp
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "nanopacket.h"
+
+namespace android {
+
+constexpr uint8_t kSyncByte(0x31);
+
+// CRC constants.
+constexpr uint32_t kInitialCrc(0xffffffff);
+constexpr uint32_t kCrcTable[] = {
+ 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9,
+ 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
+ 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61,
+ 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD
+};
+
+// Computes the CRC of one word.
+uint32_t Crc32Word(uint32_t crc, uint32_t data, int cnt) {
+ crc = crc ^ data;
+
+ for (int i = 0; i < cnt; i++) {
+ crc = (crc << 4) ^ kCrcTable[crc >> 28];
+ }
+
+ return crc;
+}
+
+// Computes the CRC32 of a buffer given a starting CRC.
+uint32_t Crc32(const uint8_t *buffer, int length) {
+ int i;
+ uint32_t crc = kInitialCrc;
+
+ // Word by word crc32
+ for (i = 0; i < (length >> 2); i++) {
+ crc = Crc32Word(crc, ((uint32_t *)buffer)[i], 8);
+ }
+
+ // Zero pad last word if required.
+ if (length & 0x3) {
+ uint32_t word = 0;
+
+ for (i*=4; i<length; i++) {
+ word |= buffer[i] << ((i & 0x3) * 8);
+ }
+
+ crc = Crc32Word(crc, word, 8);
+ }
+
+ return crc;
+}
+
+NanoPacket::NanoPacket(uint32_t sequence_number, PacketReason reason,
+ const std::vector<uint8_t> *data) {
+ Reset();
+ parsing_state_ = ParsingState::Complete;
+ sequence_number_ = sequence_number;
+ reason_ = static_cast<uint32_t>(reason);
+
+ // Resize the buffer to accomodate header, footer and data content.
+ size_t data_size = data ? data->size() : 0;
+ size_t required_buffer_size = 14 + data_size;
+ if (packet_buffer_.size() < required_buffer_size) {
+ packet_buffer_.resize(required_buffer_size);
+ }
+
+ if (packet_content_.size() < required_buffer_size) {
+ packet_content_.resize(required_buffer_size);
+ }
+
+ // Format the header of the packet.
+ packet_buffer_[0] = kSyncByte;
+ packet_buffer_[1] = sequence_number;
+ packet_buffer_[2] = sequence_number >> 8;
+ packet_buffer_[3] = sequence_number >> 16;
+ packet_buffer_[4] = sequence_number >> 24;
+ packet_buffer_[5] = static_cast<uint32_t>(reason);
+ packet_buffer_[6] = static_cast<uint32_t>(reason) >> 8;
+ packet_buffer_[7] = static_cast<uint32_t>(reason) >> 16;
+ packet_buffer_[8] = static_cast<uint32_t>(reason) >> 24;
+ packet_buffer_[9] = data_size;
+
+ // Insert the data content of the packet.
+ if (data) {
+ std::copy(data->begin(), data->end(), packet_buffer_.begin() + 10);
+ std::copy(data->begin(), data->end(), packet_content_.begin());
+ }
+
+ // Format the CRC footer.
+ uint32_t crc = Crc32(packet_buffer_.data(), required_buffer_size - 4);
+ packet_buffer_[data_size + 10] = crc;
+ packet_buffer_[data_size + 11] = crc >> 8;
+ packet_buffer_[data_size + 12] = crc >> 16;
+ packet_buffer_[data_size + 13] = crc >> 24;
+}
+
+NanoPacket::NanoPacket() {
+ Reset();
+}
+
+void NanoPacket::Reset() {
+ packet_buffer_.clear();
+ parsing_state_ = ParsingState::Idle;
+ parsing_progress_ = 0;
+ sequence_number_ = 0;
+ reason_ = 0;
+ packet_content_.clear();
+ crc_ = 0;
+}
+
+bool NanoPacket::ParsingIsComplete() const {
+ return parsing_state_ == ParsingState::Complete;
+}
+
+const std::vector<uint8_t>& NanoPacket::packet_buffer() const {
+ return packet_buffer_;
+}
+
+uint32_t NanoPacket::reason() const {
+ return reason_;
+}
+
+PacketReason NanoPacket::TypedReason() const {
+ return static_cast<PacketReason>(reason_);
+}
+
+const std::vector<uint8_t>& NanoPacket::packet_content() const {
+ return packet_content_;
+}
+
+NanoPacket::ParseResult NanoPacket::Parse(uint8_t *buffer, size_t length,
+ size_t *bytes_parsed) {
+ for (size_t i = 0; i < length; i++) {
+ // Once the state machine is not idle, save all bytes to the current
+ // packet to allow CRC to be computed at the end.
+ if (parsing_state_ != ParsingState::Idle) {
+ packet_buffer_.push_back(buffer[i]);
+ }
+
+ // Proceed through the various states of protocol parsing.
+ if (parsing_state_ == ParsingState::Idle && buffer[i] == kSyncByte) {
+ packet_buffer_.push_back(buffer[i]);
+ parsing_state_ = ParsingState::ParsingSequenceNumber;
+ } else if (parsing_state_ == ParsingState::ParsingSequenceNumber
+ && DeserializeWord(&sequence_number_, buffer[i])) {
+ parsing_state_ = ParsingState::ParsingReason;
+ } else if (parsing_state_ == ParsingState::ParsingReason
+ && DeserializeWord(&reason_, buffer[i])) {
+ parsing_state_ = ParsingState::ParsingLength;
+ } else if (parsing_state_ == ParsingState::ParsingLength) {
+ uint8_t length = buffer[i];
+ if (length > 0) {
+ packet_content_.resize(buffer[i]);
+ parsing_state_ = ParsingState::ParsingContent;
+ } else {
+ parsing_state_ = ParsingState::ParsingCrc;
+ }
+ } else if (parsing_state_ == ParsingState::ParsingContent) {
+ packet_content_[parsing_progress_++] = buffer[i];
+
+ if (parsing_progress_ == packet_content_.size()) {
+ parsing_progress_ = 0;
+ parsing_state_ = ParsingState::ParsingCrc;
+ }
+ } else if (parsing_state_ == ParsingState::ParsingCrc
+ && DeserializeWord(&crc_, buffer[i])) {
+ *bytes_parsed = i + 1;
+ if (ValidateCrc()) {
+ parsing_state_ = ParsingState::Complete;
+ return ParseResult::Success;
+ } else {
+ return ParseResult::CrcMismatch;
+ }
+ }
+ }
+
+ *bytes_parsed = length;
+ return ParseResult::Incomplete;
+}
+
+bool NanoPacket::ValidateCrc() {
+ size_t crc_length = packet_buffer_.size() - 4;
+ uint32_t computed_crc = Crc32(packet_buffer_.data(), crc_length);
+
+ if (computed_crc != crc_) {
+ Reset();
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace android
diff --git a/util/nanotool/nanopacket.h b/util/nanotool/nanopacket.h
new file mode 100644
index 0000000..c7f02dc
--- /dev/null
+++ b/util/nanotool/nanopacket.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef NANOPACKET_H_
+#define NANOPACKET_H_
+
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+
+#include "noncopyable.h"
+
+namespace android {
+
+/*
+ * The various reasons for a NanoPacket to be sent.
+ */
+enum class PacketReason : uint32_t {
+ Acknowledge = 0x00000000,
+ NAcknowledge = 0x00000001,
+ NAcknowledgeBusy = 0x00000002,
+ GetHardwareVersion = 0x00001000,
+ ReadEventRequest = 0x00001090,
+ WriteEventRequest = 0x00001091,
+};
+
+/*
+ * A NanoPacket parsing engine. Used to take a stream of bytes and convert them
+ * into an object that can more easily be worked with.
+ */
+class NanoPacket : public NonCopyable {
+ public:
+ /*
+ * The result of parsing a buffer into the packet.
+ */
+ enum class ParseResult {
+ Success,
+ Incomplete,
+ CrcMismatch,
+ };
+
+ // Formats data into NanoPacket format in the provided buffer.
+ NanoPacket(uint32_t sequence_number, PacketReason reason,
+ const std::vector<uint8_t> *data = nullptr);
+
+ // Creates an empty NanoPacket for data to be parsed into.
+ NanoPacket();
+
+ // Resets the parsing engine to the idle state and clears parsed content.
+ void Reset();
+
+ // Parses content from a buffer. Returns true if a packet has been entirely
+ // parsed.
+ ParseResult Parse(uint8_t *buffer, size_t length, size_t *bytes_parsed);
+
+ // Indicated that parsing of the packet has completed.
+ bool ParsingIsComplete() const;
+
+ // The entire content of the message.
+ const std::vector<uint8_t>& packet_buffer() const;
+
+ // Obtains the reason for the packet.
+ uint32_t reason() const;
+
+ // Obtains the reason as a PacketReason.
+ PacketReason TypedReason() const;
+
+ // Obtains the data content of the packet.
+ const std::vector<uint8_t>& packet_content() const;
+
+ private:
+ /*
+ * The current state of the parser.
+ */
+ enum class ParsingState {
+ Idle,
+ ParsingSequenceNumber,
+ ParsingReason,
+ ParsingLength,
+ ParsingContent,
+ ParsingCrc,
+ Complete,
+ };
+
+ // Parsing engine state.
+ std::vector<uint8_t> packet_buffer_;
+ ParsingState parsing_state_;
+ uint32_t parsing_progress_;
+
+ // Parsed protocol fields.
+ uint32_t sequence_number_;
+ uint32_t reason_;
+ std::vector<uint8_t> packet_content_;
+ uint32_t crc_;
+
+ // Validates that the received packet has a CRC that matches a generated
+ // CRC.
+ bool ValidateCrc();
+
+ // Deserializes a little-endian word using the parsing_progress_ member to
+ // maintain state.
+ template<typename T>
+ bool DeserializeWord(T *destination, uint8_t byte);
+};
+
+} // namespace android
+
+#include "nanopacket_impl.h"
+
+#endif // NANOPACKET_H_
diff --git a/util/nanotool/nanopacket_impl.h b/util/nanotool/nanopacket_impl.h
new file mode 100644
index 0000000..2a7d9a3
--- /dev/null
+++ b/util/nanotool/nanopacket_impl.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "nanopacket.h"
+
+#include <stdio.h>
+
+namespace android {
+
+template<typename T>
+bool NanoPacket::DeserializeWord(T *destination, uint8_t byte) {
+ *destination |= byte << (8 * parsing_progress_);
+ parsing_progress_++;
+
+ if (parsing_progress_ == sizeof(T)) {
+ parsing_progress_ = 0;
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace android
diff --git a/util/nanotool/nanotool.cpp b/util/nanotool/nanotool.cpp
new file mode 100644
index 0000000..21b388d
--- /dev/null
+++ b/util/nanotool/nanotool.cpp
@@ -0,0 +1,478 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <getopt.h>
+#include <signal.h>
+
+#include <cstdlib>
+#include <cstring>
+#include <memory>
+#include <sstream>
+#include <tuple>
+#include <vector>
+
+#include "contexthub.h"
+#include "log.h"
+
+#ifdef __ANDROID__
+#include "androidcontexthub.h"
+#else
+#include "cp2130.h"
+#include "usbcontext.h"
+#include "usbcontexthub.h"
+#endif
+
+using namespace android;
+
+enum class NanotoolCommand {
+ Invalid,
+ Disable,
+ DisableAll,
+ Calibrate,
+ Read,
+ Poll,
+ LoadCalibration,
+ Flash,
+};
+
+struct ParsedArgs {
+ NanotoolCommand command = NanotoolCommand::Poll;
+ std::vector<SensorSpec> sensors;
+ int count = 0;
+ bool logging_enabled = false;
+ std::string filename;
+ int device_index = 0;
+};
+
+static NanotoolCommand StrToCommand(const char *command_name) {
+ static const std::vector<std::tuple<std::string, NanotoolCommand>> cmds = {
+ std::make_tuple("disable", NanotoolCommand::Disable),
+ std::make_tuple("disable_all", NanotoolCommand::DisableAll),
+ std::make_tuple("calibrate", NanotoolCommand::Calibrate),
+ std::make_tuple("cal", NanotoolCommand::Calibrate),
+ std::make_tuple("read", NanotoolCommand::Read),
+ std::make_tuple("poll", NanotoolCommand::Poll),
+ std::make_tuple("load_cal", NanotoolCommand::LoadCalibration),
+ std::make_tuple("flash", NanotoolCommand::Flash),
+ };
+
+ if (!command_name) {
+ return NanotoolCommand::Invalid;
+ }
+
+ for (size_t i = 0; i < cmds.size(); i++) {
+ std::string name;
+ NanotoolCommand cmd;
+
+ std::tie(name, cmd) = cmds[i];
+ if (name.compare(command_name) == 0) {
+ return cmd;
+ }
+ }
+
+ return NanotoolCommand::Invalid;
+}
+
+static void PrintUsage(const char *name) {
+ const char *help_text =
+ "options:\n"
+ " -x, --cmd Argument must be one of:\n"
+ " disable: send a disable request for one sensor\n"
+ " disable_all: send a disable request for all sensors\n"
+ " calibrate: disable the sensor, then perform the sensor\n"
+ " calibration routine\n"
+ " load_cal: send data from calibration file to hub\n"
+ " read: output events for the given sensor, or all events\n"
+ " if no sensor specified\n"
+ " poll (default): enable the sensor, output received\n"
+ " events, then disable the sensor before exiting\n"
+ " flash: Load a new firmware image to the hub\n"
+ "\n"
+ " -s, --sensor Specify sensor type, and parameters for the command.\n"
+ " Format is sensor_type[:rate[:latency_ms]][=cal_ref].\n"
+ " See below for a complete list sensor types. A rate is\n"
+ " required when enabling a sensor, but latency is optional\n"
+ " and defaults to 0. Rate can be specified in Hz, or as one\n"
+ " of the special values \"onchange\", \"ondemand\", or\n"
+ " \"oneshot\".\n"
+ " Some sensors require a ground truth value for calibration.\n"
+ " Use the cal_ref parameter for this purpose (it's parsed as\n"
+ " a float).\n"
+ " This argument can be repeated to perform a command on\n"
+ " multiple sensors.\n"
+ "\n"
+ " -c, --count Number of samples to read before exiting, or set to 0 to\n"
+ " read indefinitely (the default behavior)\n"
+ "\n"
+ " -f, --file\n"
+ " Specifies the file to be used with flash.\n"
+ "\n"
+ " -l, --log Outputs logs from the sensor hub as they become available.\n"
+ " The logs will be printed inline with sensor samples.\n"
+ " The default is for log messages to be ignored.\n"
+#ifndef __ANDROID__
+ // This option is only applicable when connecting over USB
+ "\n"
+ " -i, --index Selects the device to work with by specifying the index\n"
+ " into the device list (default: 0)\n"
+#endif
+ "\n"
+ " -v, -vv Output verbose/extra verbose debugging information\n";
+
+ fprintf(stderr, "%s %s\n\n", name, NANOTOOL_VERSION_STR);
+ fprintf(stderr, "Usage: %s [options]\n\n%s\n", name, help_text);
+ fprintf(stderr, "Supported sensors: %s\n\n",
+ ContextHub::ListAllSensorAbbrevNames().c_str());
+ fprintf(stderr, "Examples:\n"
+ " %s -s accel:50\n"
+ " %s -s accel:50:1000 -s gyro:50:1000\n"
+ " %s -s prox:onchange\n"
+ " %s -x calibrate -s baro=1000\n",
+ name, name, name, name);
+}
+
+/*
+ * Performs higher-level argument validation beyond just parsing the parameters,
+ * for example check whether a required argument is present when the command is
+ * set to a specific value.
+ */
+static bool ValidateArgs(std::unique_ptr<ParsedArgs>& args, const char *name) {
+ if (!args->sensors.size()
+ && (args->command == NanotoolCommand::Disable
+ || args->command == NanotoolCommand::Calibrate
+ || args->command == NanotoolCommand::Poll)) {
+ fprintf(stderr, "%s: At least 1 sensor must be specified for this "
+ "command (use -s)\n",
+ name);
+ return false;
+ }
+
+ if (args->command == NanotoolCommand::Flash
+ && args->filename.empty()) {
+ fprintf(stderr, "%s: A filename must be specified for this command "
+ "(use -f)\n",
+ name);
+ return false;
+ }
+
+ if (args->command == NanotoolCommand::Poll) {
+ for (unsigned int i = 0; i < args->sensors.size(); i++) {
+ if (args->sensors[i].special_rate == SensorSpecialRate::None
+ && args->sensors[i].rate_hz < 0) {
+ fprintf(stderr, "%s: Sample rate must be specified for sensor "
+ "%s\n", name,
+ ContextHub::SensorTypeToAbbrevName(
+ args->sensors[i].sensor_type).c_str());
+ return false;
+ }
+ }
+ }
+
+ if (args->command == NanotoolCommand::Calibrate) {
+ for (unsigned int i = 0; i < args->sensors.size(); i++) {
+ if (!args->sensors[i].have_cal_ref
+ && (args->sensors[i].sensor_type == SensorType::Barometer
+ || args->sensors[i].sensor_type ==
+ SensorType::AmbientLightSensor)) {
+ fprintf(stderr, "%s: Calibration reference required for sensor "
+ "%s (for example: -s baro=1000)\n", name,
+ ContextHub::SensorTypeToAbbrevName(
+ args->sensors[i].sensor_type).c_str());
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+static bool ParseRate(const std::string& param, SensorSpec& spec) {
+ static const std::vector<std::tuple<std::string, SensorSpecialRate>> rates = {
+ std::make_tuple("ondemand", SensorSpecialRate::OnDemand),
+ std::make_tuple("onchange", SensorSpecialRate::OnChange),
+ std::make_tuple("oneshot", SensorSpecialRate::OneShot),
+ };
+
+ for (size_t i = 0; i < rates.size(); i++) {
+ std::string name;
+ SensorSpecialRate rate;
+
+ std::tie(name, rate) = rates[i];
+ if (param == name) {
+ spec.special_rate = rate;
+ return true;
+ }
+ }
+
+ spec.rate_hz = std::stof(param);
+ if (spec.rate_hz < 0) {
+ return false;
+ }
+
+ return true;
+}
+
+// Parse a sensor argument in the form of "sensor_name[:rate[:latency]][=cal_ref]"
+// into a SensorSpec, and add it to ParsedArgs.
+static bool ParseSensorArg(std::vector<SensorSpec>& sensors, const char *arg_str,
+ const char *name) {
+ SensorSpec spec;
+ std::string param;
+ std::string pre_cal_ref;
+ std::stringstream full_arg_ss(arg_str);
+ unsigned int index = 0;
+
+ while (std::getline(full_arg_ss, param, '=')) {
+ if (index == 0) {
+ pre_cal_ref = param;
+ } else if (index == 1) {
+ spec.cal_ref = std::stof(param);
+ spec.have_cal_ref = true;
+ } else {
+ fprintf(stderr, "%s: Only one calibration reference may be "
+ "supplied\n", name);
+ return false;
+ }
+ index++;
+ }
+
+ index = 0;
+ std::stringstream pre_cal_ref_ss(pre_cal_ref);
+ while (std::getline(pre_cal_ref_ss, param, ':')) {
+ if (index == 0) { // Parse sensor type
+ spec.sensor_type = ContextHub::SensorAbbrevNameToType(param);
+ if (spec.sensor_type == SensorType::Invalid_) {
+ fprintf(stderr, "%s: Invalid sensor name '%s'\n",
+ name, param.c_str());
+ return false;
+ }
+ } else if (index == 1) { // Parse sample rate
+ if (!ParseRate(param, spec)) {
+ fprintf(stderr, "%s: Invalid sample rate %s\n", name,
+ param.c_str());
+ return false;
+ }
+ } else if (index == 2) { // Parse latency
+ long long latency_ms = std::stoll(param);
+ if (latency_ms < 0) {
+ fprintf(stderr, "%s: Invalid latency %lld\n", name, latency_ms);
+ return false;
+ }
+ spec.latency_ns = static_cast<uint64_t>(latency_ms) * 1000000;
+ } else {
+ fprintf(stderr, "%s: Too many arguments in -s", name);
+ return false;
+ }
+ index++;
+ }
+
+ sensors.push_back(spec);
+ return true;
+}
+
+static std::unique_ptr<ParsedArgs> ParseArgs(int argc, char **argv) {
+ static const struct option long_opts[] = {
+ {"cmd", required_argument, nullptr, 'x'},
+ {"sensor", required_argument, nullptr, 's'},
+ {"count", required_argument, nullptr, 'c'},
+ {"flash", required_argument, nullptr, 'f'},
+ {"log", no_argument, nullptr, 'l'},
+ {"index", required_argument, nullptr, 'i'},
+ };
+
+ auto args = std::unique_ptr<ParsedArgs>(new ParsedArgs());
+ int index = 0;
+ while (42) {
+ int c = getopt_long(argc, argv, "x:s:c:f:v::li:", long_opts, &index);
+ if (c == -1) {
+ break;
+ }
+
+ switch (c) {
+ case 'x': {
+ args->command = StrToCommand(optarg);
+ if (args->command == NanotoolCommand::Invalid) {
+ fprintf(stderr, "%s: Invalid command '%s'\n", argv[0], optarg);
+ return nullptr;
+ }
+ break;
+ }
+ case 's': {
+ if (!ParseSensorArg(args->sensors, optarg, argv[0])) {
+ return nullptr;
+ }
+ break;
+ }
+ case 'c': {
+ args->count = atoi(optarg);
+ if (args->count < 0) {
+ fprintf(stderr, "%s: Invalid sample count %d\n",
+ argv[0], args->count);
+ return nullptr;
+ }
+ break;
+ }
+ case 'v': {
+ if (optarg && optarg[0] == 'v') {
+ Log::SetLevel(Log::LogLevel::Debug);
+ } else {
+ Log::SetLevel(Log::LogLevel::Info);
+ }
+ break;
+ }
+ case 'l': {
+ args->logging_enabled = true;
+ break;
+ }
+ case 'f': {
+ if (optarg) {
+ args->filename = std::string(optarg);
+ } else {
+ fprintf(stderr, "File requires a filename\n");
+ return nullptr;
+ }
+ break;
+ }
+ case 'i': {
+ args->device_index = atoi(optarg);
+ if (args->device_index < 0) {
+ fprintf(stderr, "%s: Invalid device index %d\n", argv[0],
+ args->device_index);
+ return nullptr;
+ }
+ break;
+ }
+ default:
+ return nullptr;
+ }
+ }
+
+ if (!ValidateArgs(args, argv[0])) {
+ return nullptr;
+ }
+ return args;
+}
+
+static std::unique_ptr<ContextHub> GetContextHub(std::unique_ptr<ParsedArgs>& args) {
+#ifdef __ANDROID__
+ (void) args;
+ return std::unique_ptr<AndroidContextHub>(new AndroidContextHub());
+#else
+ return std::unique_ptr<UsbContextHub>(new UsbContextHub(args->device_index));
+#endif
+}
+
+#ifdef __ANDROID__
+static void SignalHandler(int sig) {
+ // Catches a signal and does nothing, to allow any pending syscalls to be
+ // exited with SIGINT and normal cleanup to occur. If SIGINT is sent a
+ // second time, the system will invoke the standard handler.
+ (void) sig;
+}
+
+static void TerminateHandler() {
+ AndroidContextHub::TerminateHandler();
+ std::abort();
+}
+
+static void SetHandlers() {
+ struct sigaction sa;
+ memset(&sa, 0, sizeof(sa));
+ sa.sa_handler = SignalHandler;
+ sigaction(SIGINT, &sa, NULL);
+
+ std::set_terminate(TerminateHandler);
+}
+#endif
+
+int main(int argc, char **argv) {
+ Log::Initialize(new PrintfLogger(), Log::LogLevel::Warn);
+
+ // If no arguments given, print usage without any error messages
+ if (argc == 1) {
+ PrintUsage(argv[0]);
+ return 1;
+ }
+
+ std::unique_ptr<ParsedArgs> args = ParseArgs(argc, argv);
+ if (!args) {
+ PrintUsage(argv[0]);
+ return 1;
+ }
+
+#ifdef __ANDROID__
+ SetHandlers();
+#endif
+
+ std::unique_ptr<ContextHub> hub = GetContextHub(args);
+ if (!hub || !hub->Initialize()) {
+ LOGE("Error initializing ContextHub");
+ return -1;
+ }
+
+ hub->SetLoggingEnabled(args->logging_enabled);
+
+ bool success = true;
+ switch (args->command) {
+ case NanotoolCommand::Disable:
+ success = hub->DisableSensors(args->sensors);
+ break;
+ case NanotoolCommand::DisableAll:
+ success = hub->DisableAllSensors();
+ break;
+ case NanotoolCommand::Read: {
+ if (!args->sensors.size()) {
+ hub->PrintAllEvents(args->count);
+ } else {
+ hub->PrintSensorEvents(args->sensors, args->count);
+ }
+ break;
+ }
+ case NanotoolCommand::Poll: {
+ success = hub->EnableSensors(args->sensors);
+ if (success) {
+ hub->PrintSensorEvents(args->sensors, args->count);
+ }
+ break;
+ }
+ case NanotoolCommand::Calibrate: {
+ hub->DisableSensors(args->sensors);
+ success = hub->CalibrateSensors(args->sensors);
+ break;
+ }
+ case NanotoolCommand::LoadCalibration: {
+ success = hub->LoadCalibration();
+ break;
+ }
+ case NanotoolCommand::Flash: {
+ success = hub->Flash(args->filename);
+ break;
+ }
+ default:
+ LOGE("Command not implemented");
+ return 1;
+ }
+
+ if (!success) {
+ LOGE("Command failed");
+ return -1;
+ } else if (args->command != NanotoolCommand::Read
+ && args->command != NanotoolCommand::Poll) {
+ printf("Operation completed successfully\n");
+ }
+
+ return 0;
+}
diff --git a/util/nanotool/noncopyable.h b/util/nanotool/noncopyable.h
new file mode 100644
index 0000000..4d8476c
--- /dev/null
+++ b/util/nanotool/noncopyable.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef NONCOPYABLE_H_
+#define NONCOPYABLE_H_
+
+namespace android {
+
+class NonCopyable {
+ public:
+ NonCopyable() = default;
+ NonCopyable(const NonCopyable&) = delete;
+ NonCopyable& operator=(const NonCopyable&) = delete;
+};
+
+} // namespace android
+
+#endif // NONCOPYABLE_H_
diff --git a/util/nanotool/resetreasonevent.cpp b/util/nanotool/resetreasonevent.cpp
new file mode 100644
index 0000000..19e7a37
--- /dev/null
+++ b/util/nanotool/resetreasonevent.cpp
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "resetreasonevent.h"
+
+#include "contexthub.h"
+#include "log.h"
+
+namespace android {
+
+/* ResetReasonEvent *************************************************************/
+
+std::unique_ptr<ResetReasonEvent> ResetReasonEvent::FromBytes(
+ const std::vector<uint8_t>& buffer) {
+ auto event = std::unique_ptr<ResetReasonEvent>(new ResetReasonEvent());
+ event->Populate(buffer);
+
+ return event;
+}
+
+uint32_t ResetReasonEvent::GetReason() const {
+ // After the event type header (uint32_t), we should have the reset reason,
+ // which is of type uint32_t
+ if (event_data.size() < (sizeof(uint32_t) + sizeof(uint32_t))) {
+ LOGW("Invalid/short ResetReason event of size %zu", event_data.size());
+ return 0;
+ } else {
+ return *(uint32_t*)reinterpret_cast<const uint32_t*>(
+ event_data.data() + sizeof(uint32_t));
+ }
+}
+
+} // namespace android
diff --git a/util/nanotool/resetreasonevent.h b/util/nanotool/resetreasonevent.h
new file mode 100644
index 0000000..48c9e67
--- /dev/null
+++ b/util/nanotool/resetreasonevent.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef RESET_REASON_EVENT_H_
+#define RESET_REASON_EVENT_H_
+
+#include "contexthub.h"
+#include "nanomessage.h"
+
+namespace android {
+
+/*
+ * These classes represent events sent with event type EVT_RESET_REASON. The
+ * platform-specific reset reason is sent at each boot of the sensor hub.
+ */
+
+class ResetReasonEvent : public ReadEventResponse {
+ public:
+ /*
+ * Constructs and populates a ResetReasonEvent instance. Returns nullptr if
+ * the packet is malformed. The rest of the methods in this class are not
+ * guaranteed to be safe unless the object is constructed from this
+ * function.
+ */
+ static std::unique_ptr<ResetReasonEvent> FromBytes(
+ const std::vector<uint8_t>& buffer);
+
+ // Returns the 32-bit field that contains the platform-specific reset reason
+ uint32_t GetReason() const;
+};
+
+} // namespace android
+
+#endif // RESET_REASON_EVENT_H_
diff --git a/util/nanotool/sensorevent.cpp b/util/nanotool/sensorevent.cpp
new file mode 100644
index 0000000..2d64eb1
--- /dev/null
+++ b/util/nanotool/sensorevent.cpp
@@ -0,0 +1,286 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "sensorevent.h"
+
+#include <inttypes.h>
+#include <string.h>
+
+#include "contexthub.h"
+#include "log.h"
+
+namespace android {
+
+constexpr float kCompressedSampleRatio(8.0f * 9.81f / 32768.0f);
+
+/* SensorEvent ****************************************************************/
+
+std::unique_ptr<SensorEvent> SensorEvent::FromBytes(
+ const std::vector<uint8_t>& buffer) {
+ SensorEvent *sensor_event = nullptr;
+
+ SensorType sensor_type = static_cast<SensorType>(
+ ReadEventResponse::EventTypeFromBuffer(buffer) -
+ static_cast<uint32_t>(EventType::FirstSensorEvent));
+
+ switch (sensor_type) {
+ case SensorType::Accel:
+ case SensorType::Gyro:
+ case SensorType::GyroUncal:
+ case SensorType::Magnetometer:
+ case SensorType::MagnetometerUncal:
+ case SensorType::Orientation:
+ case SensorType::Gravity:
+ case SensorType::LinearAccel:
+ case SensorType::RotationVector:
+ case SensorType::GeomagneticRotationVector:
+ case SensorType::GameRotationVector:
+ sensor_event = new TripleAxisSensorEvent();
+ break;
+
+ case SensorType::Barometer:
+ case SensorType::Temperature:
+ case SensorType::AmbientLightSensor:
+ case SensorType::Proximity:
+ sensor_event = new SingleAxisSensorEvent();
+ break;
+
+ // TODO: Activity uses a special struct, it should have its own class
+ case SensorType::Activity:
+ case SensorType::AnyMotion:
+ case SensorType::NoMotion:
+ case SensorType::SignificantMotion:
+ case SensorType::Flat:
+ case SensorType::WindowOrientation:
+ case SensorType::Tilt:
+ case SensorType::Hall:
+ case SensorType::HeartRateECG: // Heart rates not implemented, guessing
+ case SensorType::HeartRatePPG: // data type here...
+ case SensorType::StepCount:
+ case SensorType::StepDetect:
+ case SensorType::Gesture:
+ case SensorType::DoubleTwist:
+ case SensorType::DoubleTap:
+ case SensorType::Vsync:
+ sensor_event = new SingleAxisIntSensorEvent();
+ break;
+
+ case SensorType::CompressedAccel:
+ sensor_event = new CompressedTripleAxisSensorEvent();
+ break;
+
+ default:
+ LOGW("Can't create SensorEvent for unknown/invalid sensor type %d",
+ static_cast<int>(sensor_type));
+ }
+
+ if (sensor_event &&
+ (!sensor_event->Populate(buffer) || !sensor_event->SizeIsValid())) {
+ LOGW("Couldn't populate sensor event, or invalid size");
+ delete sensor_event;
+ sensor_event = nullptr;
+ }
+
+ return std::unique_ptr<SensorEvent>(sensor_event);
+}
+
+SensorType SensorEvent::GetSensorType() const {
+ return static_cast<SensorType>(
+ GetEventType() - static_cast<uint32_t>(EventType::FirstSensorEvent));
+}
+
+/* TimestampedSensorEvent *****************************************************/
+
+uint8_t TimestampedSensorEvent::GetNumSamples() const {
+ // Perform size check, but don't depend on SizeIsValid since it will call us
+ if (event_data.size() < (sizeof(struct SensorEventHeader) +
+ sizeof(struct SensorFirstSample))) {
+ LOGW("Short/invalid timestamped sensor event; length %zu",
+ event_data.size());
+ return 0;
+ }
+
+ const struct SensorFirstSample *first_sample_header =
+ reinterpret_cast<const struct SensorFirstSample *>(
+ event_data.data() + sizeof(struct SensorEventHeader));
+
+ return first_sample_header->numSamples;
+}
+
+uint64_t TimestampedSensorEvent::GetReferenceTime() const {
+ if (!SizeIsValid()) {
+ return 0;
+ }
+ const struct SensorEventHeader *header =
+ reinterpret_cast<const struct SensorEventHeader *>(event_data.data());
+ return header->reference_time;
+}
+
+uint64_t TimestampedSensorEvent::GetSampleTime(uint8_t index) const {
+ const SensorSampleHeader *sample;
+ uint64_t sample_time = GetReferenceTime();
+
+ // For index 0, the sample time is the reference time. For each subsequent
+ // sample, sum the delta to the previous sample to get the sample time.
+ for (uint8_t i = 1; i <= index; i++) {
+ sample = GetSampleAtIndex(index);
+ sample_time += sample->delta_time;
+ }
+
+ return sample_time;
+}
+
+std::string TimestampedSensorEvent::GetSampleTimeStr(uint8_t index) const {
+ uint64_t sample_time = GetSampleTime(index);
+
+ char buffer[32];
+ snprintf(buffer, sizeof(buffer), "%" PRIu64 ".%06" PRIu64 " ms",
+ sample_time / 1000000, sample_time % 1000000);
+
+ return std::string(buffer);
+}
+
+const SensorSampleHeader *TimestampedSensorEvent::GetSampleAtIndex(
+ uint8_t index) const {
+ if (index >= GetNumSamples()) {
+ LOGW("Requested sample at invalid index %u", index);
+ return nullptr;
+ }
+
+ unsigned int offset = (sizeof(struct SensorEventHeader) +
+ index * GetSampleDataSize());
+ return reinterpret_cast<const struct SensorSampleHeader *>(
+ event_data.data() + offset);
+}
+
+std::string TimestampedSensorEvent::ToString() const {
+ uint8_t num_samples = GetNumSamples();
+ char buffer[64];
+ snprintf(buffer, sizeof(buffer),
+ "Event from sensor %d (%s) with %d sample%s\n",
+ static_cast<int>(GetSensorType()),
+ ContextHub::SensorTypeToAbbrevName(GetSensorType()).c_str(),
+ num_samples, (num_samples != 1) ? "s" : "");
+
+ return std::string(buffer) + StringForAllSamples();
+}
+
+bool TimestampedSensorEvent::SizeIsValid() const {
+ unsigned int min_size = (sizeof(struct SensorEventHeader) +
+ GetNumSamples() * GetSampleDataSize());
+ if (event_data.size() < min_size) {
+ LOGW("Got short sensor event with %zu bytes, expected >= %u",
+ event_data.size(), min_size);
+ return false;
+ }
+
+ return true;
+}
+
+std::string TimestampedSensorEvent::StringForAllSamples() const {
+ std::string str;
+ for (unsigned int i = 0; i < GetNumSamples(); i++) {
+ str += StringForSample(i);
+ }
+ return str;
+}
+
+/* SingleAxisSensorEvent ******************************************************/
+
+std::string SingleAxisSensorEvent::StringForSample(uint8_t index) const {
+ const SingleAxisDataPoint *sample =
+ reinterpret_cast<const SingleAxisDataPoint *>(GetSampleAtIndex(index));
+
+ char buffer[64];
+ snprintf(buffer, sizeof(buffer), " %f @ %s\n",
+ sample->fdata, GetSampleTimeStr(index).c_str());
+
+ return std::string(buffer);
+}
+
+uint8_t SingleAxisSensorEvent::GetSampleDataSize() const {
+ return sizeof(struct SingleAxisDataPoint);
+}
+
+/* SingleAxisIntSensorEvent ***************************************************/
+
+std::string SingleAxisIntSensorEvent::StringForSample(uint8_t index) const {
+ const SingleAxisDataPoint *sample =
+ reinterpret_cast<const SingleAxisDataPoint *>(GetSampleAtIndex(index));
+
+ char buffer[64];
+ snprintf(buffer, sizeof(buffer), " %d @ %s\n",
+ sample->idata, GetSampleTimeStr(index).c_str());
+
+ return std::string(buffer);
+}
+
+/* TripleAxisSensorEvent ******************************************************/
+
+std::string TripleAxisSensorEvent::StringForSample(uint8_t index) const {
+ const TripleAxisDataPoint *sample =
+ reinterpret_cast<const TripleAxisDataPoint *>(
+ GetSampleAtIndex(index));
+
+ const struct SensorFirstSample *first_sample =
+ reinterpret_cast<const struct SensorFirstSample *>(
+ event_data.data() + sizeof(struct SensorEventHeader));
+ bool is_bias_sample = first_sample->biasPresent
+ && first_sample->biasSample == index;
+
+ char buffer[128];
+ snprintf(buffer, sizeof(buffer), " X:%f Y:%f Z:%f @ %s%s\n",
+ sample->x, sample->y, sample->z, GetSampleTimeStr(index).c_str(),
+ is_bias_sample ? " (Bias Sample)" : "");
+
+ return std::string(buffer);
+}
+
+uint8_t TripleAxisSensorEvent::GetSampleDataSize() const {
+ return sizeof(struct TripleAxisDataPoint);
+}
+
+/* CompressedTripleAxisSensorEvent ********************************************/
+
+std::string CompressedTripleAxisSensorEvent::StringForSample(
+ uint8_t index) const {
+ const CompressedTripleAxisDataPoint *sample =
+ reinterpret_cast<const CompressedTripleAxisDataPoint *>(
+ GetSampleAtIndex(index));
+
+ const struct SensorFirstSample *first_sample =
+ reinterpret_cast<const struct SensorFirstSample *>(
+ event_data.data() + sizeof(struct SensorEventHeader));
+ bool is_bias_sample = first_sample->biasPresent
+ && first_sample->biasSample == index;
+
+ float x = sample->ix * kCompressedSampleRatio;
+ float y = sample->iy * kCompressedSampleRatio;
+ float z = sample->iz * kCompressedSampleRatio;
+
+ char buffer[128];
+ snprintf(buffer, sizeof(buffer), " X:%f Y:%f Z:%f @ %s%s\n",
+ x, y, z, GetSampleTimeStr(index).c_str(),
+ is_bias_sample ? " (Bias Sample)" : "");
+
+ return std::string(buffer);
+}
+
+uint8_t CompressedTripleAxisSensorEvent::GetSampleDataSize() const {
+ return sizeof(CompressedTripleAxisDataPoint);
+}
+
+} // namespace android
diff --git a/util/nanotool/sensorevent.h b/util/nanotool/sensorevent.h
new file mode 100644
index 0000000..f98a160
--- /dev/null
+++ b/util/nanotool/sensorevent.h
@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SENSOREVENT_H_
+#define SENSOREVENT_H_
+
+#include "contexthub.h"
+#include "nanomessage.h"
+
+namespace android {
+
+// Copied from sensors.h in nanohub firmware/inc
+struct SensorFirstSample
+{
+ uint8_t numSamples;
+ uint8_t numFlushes;
+ uint8_t biasCurrent : 1;
+ uint8_t biasPresent : 1;
+ uint8_t biasSample : 6;
+ uint8_t interrupt;
+};
+
+struct SingleAxisDataPoint {
+ union {
+ uint32_t deltaTime; //delta since last sample, for 0th sample this is firstSample
+ struct SensorFirstSample firstSample;
+ };
+ union {
+ float fdata;
+ int32_t idata;
+ };
+} __attribute__((packed));
+
+struct CompressedTripleAxisDataPoint {
+ uint32_t deltaTime;
+ int16_t ix;
+ int16_t iy;
+ int16_t iz;
+} __attribute__((packed));
+
+struct TripleAxisDataPoint {
+ union {
+ uint32_t deltaTime; //delta since last sample, for 0th sample this is firstSample
+ struct SensorFirstSample firstSample;
+ };
+ union {
+ float x;
+ int32_t ix;
+ };
+ union {
+ float y;
+ int32_t iy;
+ };
+ union {
+ float z;
+ int32_t iz;
+ };
+} __attribute__((packed));
+
+/*
+ * Common timestamped sensor event structure is SensorEventHeader followed by
+ * a variable length array of sensor samples, each starting with
+ * SensorSampleHeader.
+ */
+struct SensorEventHeader : public Event {
+ uint64_t reference_time;
+} __attribute__((packed));
+
+struct SensorSampleHeader {
+ union {
+ struct SensorFirstSample first_sample_header;
+ uint32_t delta_time;
+ };
+} __attribute__((packed));
+
+class SensorEvent : public ReadEventResponse {
+ public:
+ /*
+ * Returns a pointer to a ReadEventResponse that will be constructed from
+ * one of the sensor event types and populated from byte stream. If
+ * this function is called, it's assumed that the event type is within the
+ * range [EVT_NO_FIRST_SENSOR_EVENT, EVT_NO_SENSOR_CONFIG_EVENT)
+ */
+ static std::unique_ptr<SensorEvent> FromBytes(
+ const std::vector<uint8_t>& buffer);
+
+ SensorType GetSensorType() const;
+ std::string GetSensorName() const;
+
+ /*
+ * Subclasses should override this function to return the number of samples
+ * contained in the event.
+ */
+ virtual uint8_t GetNumSamples() const = 0;
+
+ protected:
+ virtual bool SizeIsValid() const = 0;
+};
+
+class TimestampedSensorEvent : public SensorEvent {
+ public:
+ uint8_t GetNumSamples() const override;
+ uint64_t GetReferenceTime() const;
+ uint64_t GetSampleTime(uint8_t index) const;
+ std::string GetSampleTimeStr(uint8_t index) const;
+ const SensorSampleHeader *GetSampleAtIndex(uint8_t index) const;
+
+ std::string ToString() const override;
+
+ virtual std::string StringForSample(uint8_t index) const = 0;
+
+ protected:
+ bool SizeIsValid() const override;
+ std::string StringForAllSamples() const;
+
+ /*
+ * Subclasses must implement this to be the size of each data point,
+ * including struct SensorSampleHeader.
+ */
+ virtual uint8_t GetSampleDataSize() const = 0;
+};
+
+class SingleAxisSensorEvent : public TimestampedSensorEvent {
+ public:
+ virtual std::string StringForSample(uint8_t index) const override;
+
+ protected:
+ uint8_t GetSampleDataSize() const override;
+};
+
+// Same as SingleAxisSensorEvent, but data is interpreted as an integer instead
+// of float
+class SingleAxisIntSensorEvent : public SingleAxisSensorEvent {
+ public:
+ std::string StringForSample(uint8_t index) const override;
+};
+
+class TripleAxisSensorEvent : public TimestampedSensorEvent {
+ public:
+ std::string StringForSample(uint8_t index) const override;
+
+ protected:
+ uint8_t GetSampleDataSize() const override;
+};
+
+class CompressedTripleAxisSensorEvent : public TimestampedSensorEvent {
+ public:
+ std::string StringForSample(uint8_t index) const override;
+
+ protected:
+ uint8_t GetSampleDataSize() const override;
+};
+
+} // namespace android
+
+#endif // SENSOREVENT_H_
diff --git a/util/sensortest/sensortest.cpp b/util/sensortest/sensortest.cpp
index c841a9b..c032ca9 100644
--- a/util/sensortest/sensortest.cpp
+++ b/util/sensortest/sensortest.cpp
@@ -26,6 +26,7 @@
int listIndex;
int type;
int32_t rate;
+ int reportLatency;
bool receivedEvent;
};
@@ -39,7 +40,7 @@
void showHelp()
{
- printf("Usage: sensortest [-h] [-l] [-e <type> <rate_usecs>] [-c]\n");
+ printf("Usage: sensortest [-h] [-l] [-e <type> <rate_usecs>] [-b <type> <rate_usecs> <batch_usecs>] [-c]\n");
}
void printSensorList()
@@ -93,6 +94,7 @@
{
int currArgumentIndex = 1;
int sensorIndex;
+ int existingSensorConfigIndex;
mNumSensorConfigs = 0;
@@ -113,14 +115,61 @@
return false;
}
- mSensorConfigList[(mNumSensorConfigs)++] = {
- .listIndex = sensorIndex,
- .type = atoi(argv[currArgumentIndex+1]),
- .rate = atoi(argv[currArgumentIndex+2]),
- .receivedEvent = false
- };
+ existingSensorConfigIndex = findSensorTypeInConfigList(atoi(argv[currArgumentIndex+1]));
+
+ if (existingSensorConfigIndex >= 0) {
+ printf("Replacing previous config for sensor type %d\n", atoi(argv[currArgumentIndex+1]));
+ mSensorConfigList[existingSensorConfigIndex] = {
+ .listIndex = sensorIndex,
+ .type = atoi(argv[currArgumentIndex+1]),
+ .rate = atoi(argv[currArgumentIndex+2]),
+ .reportLatency = 0,
+ .receivedEvent = false
+ };
+ } else {
+ mSensorConfigList[(mNumSensorConfigs)++] = {
+ .listIndex = sensorIndex,
+ .type = atoi(argv[currArgumentIndex+1]),
+ .rate = atoi(argv[currArgumentIndex+2]),
+ .reportLatency = 0,
+ .receivedEvent = false
+ };
+ }
currArgumentIndex += 3;
+ } else if (!strcmp(argv[currArgumentIndex], "-b")) {
+ if (currArgumentIndex + 3 >= argc) {
+ printf ("Not enough arguments for batch option\n");
+ return false;
+ }
+
+ if ((sensorIndex = findSensorTypeInSensorList(atoi(argv[currArgumentIndex+1]))) < 0) {
+ printf ("No sensor found with type \"%d\"\n", atoi(argv[currArgumentIndex+1]));
+ return false;
+ }
+
+ existingSensorConfigIndex = findSensorTypeInConfigList(atoi(argv[currArgumentIndex+1]));
+
+ if (existingSensorConfigIndex >= 0) {
+ printf("Replacing previous config for sensor type %d\n", atoi(argv[currArgumentIndex+1]));
+ mSensorConfigList[existingSensorConfigIndex] = {
+ .listIndex = sensorIndex,
+ .type = atoi(argv[currArgumentIndex+1]),
+ .rate = atoi(argv[currArgumentIndex+2]),
+ .reportLatency = atoi(argv[currArgumentIndex+3]),
+ .receivedEvent = false
+ };
+ } else {
+ mSensorConfigList[(mNumSensorConfigs)++] = {
+ .listIndex = sensorIndex,
+ .type = atoi(argv[currArgumentIndex+1]),
+ .rate = atoi(argv[currArgumentIndex+2]),
+ .reportLatency = atoi(argv[currArgumentIndex+3]),
+ .receivedEvent = false
+ };
+ }
+
+ currArgumentIndex += 4;
} else if (!strcmp(argv[currArgumentIndex], "-c")) {
mContinuousMode = true;
currArgumentIndex++;
@@ -164,12 +213,12 @@
ASensorEventQueue *sensorEventQueue = ASensorManager_createEventQueue(mSensorManager, mLooper, 0, NULL, NULL);
for (int i = 0; i < mNumSensorConfigs; i++) {
- if (ASensorEventQueue_enableSensor(sensorEventQueue, mSensorList[mSensorConfigList[i].listIndex]) < 0) {
- printf("Unable to enable sensor %d\n", mSensorConfigList[i].listIndex);
+ if (ASensorEventQueue_registerSensor(sensorEventQueue, mSensorList[mSensorConfigList[i].listIndex],
+ mSensorConfigList[i].rate, mSensorConfigList[i].reportLatency) < 0) {
+ printf("Unable to register sensor %d with rate %d and report latency %d\n", mSensorConfigList[i].listIndex,
+ mSensorConfigList[i].rate, mSensorConfigList[i].reportLatency);
}
- if (ASensorEventQueue_setEventRate(sensorEventQueue, mSensorList[mSensorConfigList[i].listIndex], mSensorConfigList[i].rate) < 0) {
- printf("Invalid rate \"%d\" for sensor %d\n", mSensorConfigList[i].rate, mSensorConfigList[i].listIndex);
- }
+
}
while (mContinuousMode || !hasReceivedAllEvents()) {
