Merge Android Pie into masterandroid-o-mr1-iot-release-1.0.3

Bug: 112104996
Change-Id: I5786617e5db720dc171326bfbf52920d6b88abf3

100 files changed, 8561 insertions, 2427 deletions

diff --git a/Android.bp b/Android.bp
deleted file mode 100644
index 1e0d8c8f..00000000
--- a/Android.bp
+++ /dev/null
@@ -1,3 +0,0 @@
-subdirs = [
-    "lib",
-]
diff --git a/contexthubhal/Android.bp b/contexthubhal/Android.bp
new file mode 100644
index 00000000..130f7de0
--- /dev/null
+++ b/contexthubhal/Android.bp
@@ -0,0 +1,88 @@
+//
+// Copyright (C) 2018 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.
+cc_defaults {
+    name: "contexthub_libs_default",
+    relative_install_path: "hw",
+    srcs: [
+        "nanohubhal.cpp",
+        "nanohubhal_default.cpp",
+        "system_comms.cpp",
+    ],
+    cflags: ["-Wall", "-Werror", "-Wextra"],
+    shared_libs: [
+        "liblog",
+        "libcutils",
+        "libutils",
+        "libstagefright_foundation",
+    ],
+    static_libs: [
+        "libjsoncpp",
+        "libhubutilcommon",
+    ],
+    header_libs: [
+        "libnanohub_common_headers",
+        "libhardware_headers",
+        "libutils_headers",
+    ],
+    vendor: true,
+}
+
+cc_defaults {
+    name: "contexthub_hidl_libs_default",
+    srcs: [
+        "NanohubHidlAdapter.cpp",
+    ],
+    shared_libs: [
+        "libhidlbase",
+        "libhidltransport",
+        "android.hardware.contexthub@1.0",
+    ],
+}
+
+cc_library {
+    name: "context_hub.default",
+    srcs: [
+        "legacyhal.cpp",
+    ],
+    defaults: [
+        "contexthub_libs_default",
+    ],
+}
+
+cc_library_shared {
+    name: "android.hardware.contexthub@1.0-impl.nanohub",
+    shared_libs: [
+        "libbase",
+    ],
+    defaults: [
+        "contexthub_libs_default",
+        "contexthub_hidl_libs_default",
+    ],
+}
+
+cc_binary {
+    name: "android.hardware.contexthub@1.0-service.nanohub",
+    init_rc: ["android.hardware.contexthub@1.0-service.nanohub.rc"],
+    srcs: [
+        "service.cpp",
+    ],
+    shared_libs: [
+        "libhwbinder",
+    ],
+    defaults: [
+        "contexthub_libs_default",
+        "contexthub_hidl_libs_default",
+    ],
+}
diff --git a/contexthubhal/Android.mk b/contexthubhal/Android.mk
deleted file mode 100644
index be27d470..00000000
--- a/contexthubhal/Android.mk
+++ /dev/null
@@ -1,26 +0,0 @@
-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_HEADER_LIBRARIES := \
-    libhardware_headers \
-    libnanohub_common_headers \
-    libutils_headers
-
-LOCAL_MODULE := context_hub.default
-LOCAL_MODULE_TAGS := optional
-LOCAL_PROPRIETARY_MODULE := true
-
-include $(BUILD_SHARED_LIBRARY)
diff --git a/contexthubhal/NanohubHidlAdapter.cpp b/contexthubhal/NanohubHidlAdapter.cpp
new file mode 100644
index 00000000..9efca2ad
--- /dev/null
+++ b/contexthubhal/NanohubHidlAdapter.cpp
@@ -0,0 +1,591 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+ /*
+  * This file is based on:
+  * hardware/interfaces/contexthub/1.0/default/Contexthub.cpp
+  * with modifications to connect directly to the NanohubHAL and
+  * support endpoints.
+  */
+
+#include "NanohubHidlAdapter.h"
+#include "nanohub_perdevice.h"
+
+#include <inttypes.h>
+
+#include <log/log.h>
+#include <utils/String8.h>
+#include <sys/stat.h>
+
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hardware/context_hub.h>
+#include <sys/endian.h>
+
+#undef LOG_TAG
+#define LOG_TAG "NanohubHidlAdapter"
+
+using namespace android::nanohub;
+
+namespace android {
+namespace hardware {
+namespace contexthub {
+namespace V1_0 {
+namespace implementation {
+
+static constexpr uint64_t ALL_APPS = UINT64_C(0xFFFFFFFFFFFFFFFF);
+
+Contexthub::Contexthub()
+        : mDeathRecipient(new DeathRecipient(this)),
+          mIsTransactionPending(false) {
+}
+
+bool Contexthub::setOsAppAsDestination(hub_message_t *msg, int hubId) {
+    if (!isValidHubId(hubId)) {
+        ALOGW("%s: Hub information is null for hubHandle %d",
+              __FUNCTION__,
+              hubId);
+        return false;
+    } else {
+        msg->app_name = mCachedHubInfo[hubId].osAppName;
+        return true;
+    }
+}
+
+Return<void> Contexthub::getHubs(getHubs_cb _hidl_cb) {
+    std::vector<ContextHub> hubs;
+    const context_hub_t *hub = nanohub::get_hub_info();
+
+    mCachedHubInfo.clear();
+
+    CachedHubInformation info;
+    ContextHub c;
+
+    c.name = hub->name;
+    c.vendor = hub->vendor;
+    c.toolchain = hub->toolchain;
+    c.platformVersion = hub->platform_version;
+    c.toolchainVersion = hub->toolchain_version;
+    c.hubId = hub->hub_id;
+    c.peakMips = hub->peak_mips;
+    c.stoppedPowerDrawMw = hub->stopped_power_draw_mw;
+    c.sleepPowerDrawMw = hub->sleep_power_draw_mw;
+    c.peakPowerDrawMw = hub->peak_power_draw_mw;
+    // c.connectedSensors =
+    c.maxSupportedMsgLen = hub->max_supported_msg_len;
+    // TODO: get this information from nanohub
+    c.chrePlatformId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
+    c.chreApiMajorVersion = 0x01;
+    c.chreApiMinorVersion = 0x02;
+    c.chrePatchVersion = NANOHUB_OS_PATCH_LEVEL;
+
+    info.callback = nullptr;
+    info.osAppName = hub->os_app_name;
+    mCachedHubInfo[hub->hub_id] = info;
+
+    hubs.push_back(c);
+
+    _hidl_cb(hubs);
+    return Void();
+}
+
+Contexthub::DeathRecipient::DeathRecipient(sp<Contexthub> contexthub)
+        : mContexthub(contexthub) {}
+
+void Contexthub::DeathRecipient::serviceDied(
+        uint64_t cookie,
+        const wp<::android::hidl::base::V1_0::IBase>& /*who*/) {
+    uint32_t hubId = static_cast<uint32_t>(cookie);
+    mContexthub->handleServiceDeath(hubId);
+}
+
+bool Contexthub::isValidHubId(uint32_t hubId) {
+    if (!mCachedHubInfo.count(hubId)) {
+        ALOGW("Hub information not found for hubId %" PRIu32, hubId);
+        return false;
+    } else {
+        return true;
+    }
+}
+
+sp<IContexthubCallback> Contexthub::getCallBackForHubId(uint32_t hubId) {
+    if (!isValidHubId(hubId)) {
+        return nullptr;
+    } else {
+        return mCachedHubInfo[hubId].callback;
+    }
+}
+
+Return<Result> Contexthub::sendMessageToHub(uint32_t hubId,
+                                            const ContextHubMsg &msg) {
+    if (!isValidHubId(hubId) || msg.msg.size() > UINT32_MAX) {
+        return Result::BAD_PARAMS;
+    }
+
+    hub_message_t txMsg = {
+        .app_name.id = msg.appName,
+        .message_type = msg.msgType,
+        .message_len = static_cast<uint32_t>(msg.msg.size()), // Note the check above
+        .message = static_cast<const uint8_t *>(msg.msg.data()),
+    };
+
+    // Use a dummy to prevent send_message with empty message from failing prematurely
+    static uint8_t dummy;
+    if (txMsg.message_len == 0 && txMsg.message == nullptr) {
+        txMsg.message = &dummy;
+    }
+
+    ALOGI("Sending msg of type %" PRIu32 ", size %" PRIu32 " to app 0x%" PRIx64,
+          txMsg.message_type,
+          txMsg.message_len,
+          txMsg.app_name.id);
+
+    if(NanoHub::sendToNanohub(hubId, &txMsg, 0, msg.hostEndPoint) != 0) {
+        return Result::TRANSACTION_FAILED;
+    }
+
+    return Result::OK;
+}
+
+Return<Result> Contexthub::registerCallback(uint32_t hubId,
+                                            const sp<IContexthubCallback> &cb) {
+    Return<Result> retVal = Result::BAD_PARAMS;
+
+    if (!isValidHubId(hubId)) {
+        // Initialized, but hubId is  not valid
+        retVal = Result::BAD_PARAMS;
+    } else if (NanoHub::subscribeMessages(hubId,
+                                          contextHubCb,
+                                          this) == 0) {
+        // Initialized && valid hub && subscription successful
+        if (mCachedHubInfo[hubId].callback != nullptr) {
+            ALOGD("Modifying callback for hubId %" PRIu32, hubId);
+            mCachedHubInfo[hubId].callback->unlinkToDeath(mDeathRecipient);
+        }
+
+        mCachedHubInfo[hubId].callback = cb;
+        if (cb != nullptr) {
+            Return<bool> linkResult = cb->linkToDeath(mDeathRecipient, hubId);
+            bool linkSuccess = linkResult.isOk() ?
+                static_cast<bool>(linkResult) : false;
+            if (!linkSuccess) {
+                ALOGW("Couldn't link death recipient for hubId %" PRIu32,
+                      hubId);
+            }
+        }
+        retVal = Result::OK;
+    } else {
+        // Initalized && valid hubId - but subscription unsuccessful
+        // This is likely an internal error in the HAL implementation, but we
+        // cannot add more information.
+        ALOGW("Could not subscribe to the hub for callback");
+        retVal = Result::UNKNOWN_FAILURE;
+    }
+
+    return retVal;
+}
+
+static bool isValidOsStatus(const uint8_t *msg,
+                            size_t msgLen,
+                            status_response_t *rsp) {
+    // Workaround a bug in some HALs
+    if (msgLen == 1) {
+        rsp->result = msg[0];
+        return true;
+    }
+
+    if (msg == nullptr || msgLen != sizeof(*rsp)) {
+        ALOGI("Received invalid response (is null : %d, size %zu)",
+              msg == nullptr ? 1 : 0,
+              msgLen);
+        return false;
+    }
+
+    memcpy(rsp, msg, sizeof(*rsp));
+
+    // No sanity checks on return values
+    return true;
+}
+
+int Contexthub::handleOsMessage(sp<IContexthubCallback> cb,
+                                uint32_t msgType,
+                                const uint8_t *msg,
+                                int msgLen,
+                                uint32_t transactionId) {
+    int retVal = -1;
+
+
+    switch(msgType) {
+        case CONTEXT_HUB_APPS_ENABLE:
+        case CONTEXT_HUB_APPS_DISABLE:
+        case CONTEXT_HUB_LOAD_APP:
+        case CONTEXT_HUB_UNLOAD_APP:
+        {
+            struct status_response_t rsp;
+            TransactionResult result;
+            if (isValidOsStatus(msg, msgLen, &rsp) && rsp.result == 0) {
+                retVal = 0;
+                result = TransactionResult::SUCCESS;
+            } else {
+                result = TransactionResult::FAILURE;
+            }
+
+            mIsTransactionPending = false;
+            if (cb != nullptr) {
+                cb->handleTxnResult(transactionId, result);
+            }
+            retVal = 0;
+            break;
+        }
+
+        case CONTEXT_HUB_QUERY_APPS:
+        {
+            std::vector<HubAppInfo> apps;
+            int numApps = msgLen / sizeof(hub_app_info);
+            const hub_app_info *unalignedInfoAddr = reinterpret_cast<const hub_app_info *>(msg);
+
+            for (int i = 0; i < numApps; i++) {
+                hub_app_info query_info;
+                memcpy(&query_info, &unalignedInfoAddr[i], sizeof(query_info));
+                HubAppInfo app;
+                app.appId = query_info.app_name.id;
+                app.version = query_info.version;
+                // TODO :: Add memory ranges
+
+                apps.push_back(app);
+            }
+
+            if (cb != nullptr) {
+                cb->handleAppsInfo(apps);
+            }
+            retVal = 0;
+            break;
+        }
+
+        case CONTEXT_HUB_QUERY_MEMORY:
+        {
+            // Deferring this use
+            retVal = 0;
+            break;
+        }
+
+        case CONTEXT_HUB_OS_REBOOT:
+        {
+            mIsTransactionPending = false;
+            if (cb != nullptr) {
+                cb->handleHubEvent(AsyncEventType::RESTARTED);
+            }
+            retVal = 0;
+            break;
+        }
+
+        default:
+        {
+            retVal = -1;
+            break;
+        }
+      }
+
+      return retVal;
+}
+
+void Contexthub::handleServiceDeath(uint32_t hubId) {
+    ALOGI("Callback/service died for hubId %" PRIu32, hubId);
+    int ret = NanoHub::subscribeMessages(hubId, nullptr, nullptr);
+    if (ret != 0) {
+        ALOGW("Failed to unregister callback from hubId %" PRIu32 ": %d",
+              hubId, ret);
+    }
+    mCachedHubInfo[hubId].callback.clear();
+}
+
+int Contexthub::contextHubCb(uint32_t hubId,
+                             const nanohub::HubMessage &rxMsg,
+                             void *cookie) {
+    Contexthub *obj = static_cast<Contexthub *>(cookie);
+
+    if (!obj->isValidHubId(hubId)) {
+        ALOGW("Invalid hub Id %" PRIu32, hubId);
+        return -1;
+    }
+
+    sp<IContexthubCallback> cb = obj->getCallBackForHubId(hubId);
+
+    if (cb == nullptr) {
+        // This should not ever happen
+        ALOGW("No callback registered, returning");
+        return -1;
+    }
+
+    if (rxMsg.message_type < CONTEXT_HUB_TYPE_PRIVATE_MSG_BASE) {
+        obj->handleOsMessage(cb,
+                             rxMsg.message_type,
+                             static_cast<const uint8_t *>(rxMsg.message),
+                             rxMsg.message_len,
+                             rxMsg.message_transaction_id);
+    } else {
+        ContextHubMsg msg;
+
+        msg.appName = rxMsg.app_name.id;
+        msg.msgType = rxMsg.message_type;
+        msg.hostEndPoint = rxMsg.message_endpoint;
+        msg.msg = std::vector<uint8_t>(static_cast<const uint8_t *>(rxMsg.message),
+                                       static_cast<const uint8_t *>(rxMsg.message) +
+                                       rxMsg.message_len);
+
+        cb->handleClientMsg(msg);
+    }
+
+    return 0;
+}
+
+Return<Result> Contexthub::unloadNanoApp(uint32_t hubId,
+                                         uint64_t appId,
+                                         uint32_t transactionId) {
+    if (mIsTransactionPending) {
+        return Result::TRANSACTION_PENDING;
+    }
+
+    hub_message_t msg;
+
+    if (setOsAppAsDestination(&msg, hubId) == false) {
+        return Result::BAD_PARAMS;
+    }
+
+    struct apps_disable_request_t req;
+
+    msg.message_type = CONTEXT_HUB_UNLOAD_APP;
+    msg.message_len = sizeof(req);
+    msg.message = &req;
+    req.app_name.id = appId;
+
+    if(NanoHub::sendToNanohub(hubId,
+                              &msg,
+                              transactionId,
+                              static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+        return Result::TRANSACTION_FAILED;
+    } else {
+        mIsTransactionPending = true;
+        return Result::OK;
+    }
+}
+
+Return<Result> Contexthub::loadNanoApp(uint32_t hubId,
+                                       const NanoAppBinary& appBinary,
+                                       uint32_t transactionId) {
+    if (mIsTransactionPending) {
+        return Result::TRANSACTION_PENDING;
+    }
+
+    hub_message_t hubMsg;
+
+    if (setOsAppAsDestination(&hubMsg, hubId) == false) {
+        return Result::BAD_PARAMS;
+    }
+
+    // Data from the nanoapp header is passed through HIDL as explicit fields,
+    // but the legacy HAL expects it prepended to the binary, therefore we must
+    // reconstruct it here prior to passing to the legacy HAL.
+    const struct nano_app_binary_t header = {
+        .header_version = htole32(1),
+        .magic = htole32(NANOAPP_MAGIC),
+        .app_id.id = htole64(appBinary.appId),
+        .app_version = htole32(appBinary.appVersion),
+        .flags = htole32(appBinary.flags),
+        .hw_hub_type = htole64(0),
+        .target_chre_api_major_version = appBinary.targetChreApiMajorVersion,
+        .target_chre_api_minor_version = appBinary.targetChreApiMinorVersion,
+    };
+    const uint8_t *headerBytes = reinterpret_cast<const uint8_t *>(&header);
+
+    std::vector<uint8_t> binaryWithHeader(appBinary.customBinary);
+    binaryWithHeader.insert(binaryWithHeader.begin(),
+                            headerBytes,
+                            headerBytes + sizeof(header));
+
+    hubMsg.message_type = CONTEXT_HUB_LOAD_APP;
+    hubMsg.message_len = binaryWithHeader.size();
+    hubMsg.message = binaryWithHeader.data();
+
+    if(NanoHub::sendToNanohub(hubId,
+                              &hubMsg,
+                              transactionId,
+                              static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+        return Result::TRANSACTION_FAILED;
+    } else {
+        mIsTransactionPending = true;
+        return Result::OK;
+    }
+}
+
+Return<Result> Contexthub::enableNanoApp(uint32_t hubId,
+                                         uint64_t appId,
+                                         uint32_t transactionId) {
+    if (mIsTransactionPending) {
+        return Result::TRANSACTION_PENDING;
+    }
+
+    hub_message_t msg;
+
+    if (setOsAppAsDestination(&msg, hubId) == false) {
+        return Result::BAD_PARAMS;
+    }
+
+    struct apps_enable_request_t req;
+
+    msg.message_type = CONTEXT_HUB_APPS_ENABLE;
+    msg.message_len = sizeof(req);
+    req.app_name.id = appId;
+    msg.message = &req;
+
+    if(NanoHub::sendToNanohub(hubId,
+                              &msg,
+                              transactionId,
+                              static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+        return Result::TRANSACTION_FAILED;
+    } else {
+        mIsTransactionPending = true;
+        return Result::OK;
+    }
+}
+
+Return<Result> Contexthub::disableNanoApp(uint32_t hubId,
+                                          uint64_t appId,
+                                          uint32_t transactionId) {
+    if (mIsTransactionPending) {
+        return Result::TRANSACTION_PENDING;
+    }
+
+    hub_message_t msg;
+
+    if (setOsAppAsDestination(&msg, hubId) == false) {
+        return Result::BAD_PARAMS;
+    }
+
+    struct apps_disable_request_t req;
+
+    msg.message_type = CONTEXT_HUB_APPS_DISABLE;
+    msg.message_len = sizeof(req);
+    req.app_name.id = appId;
+    msg.message = &req;
+
+    if(NanoHub::sendToNanohub(hubId,
+                              &msg,
+                              transactionId,
+                              static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+        return Result::TRANSACTION_FAILED;
+    } else {
+        mIsTransactionPending = true;
+        return Result::OK;
+    }
+}
+
+Return<Result> Contexthub::queryApps(uint32_t hubId) {
+    hub_message_t msg;
+
+    if (setOsAppAsDestination(&msg, hubId) == false) {
+        ALOGW("Could not find hubId %" PRIu32, hubId);
+        return Result::BAD_PARAMS;
+    }
+
+    query_apps_request_t payload;
+    payload.app_name.id = ALL_APPS; // TODO : Pass this in as a parameter
+    msg.message = &payload;
+    msg.message_len = sizeof(payload);
+    msg.message_type = CONTEXT_HUB_QUERY_APPS;
+
+    if(NanoHub::sendToNanohub(hubId,
+                              &msg,
+                              0,
+                              static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+        ALOGW("Query Apps sendMessage failed");
+        return Result::TRANSACTION_FAILED;
+    }
+
+    return Result::OK;
+}
+
+IContexthub *HIDL_FETCH_IContexthub(const char *) {
+    return new Contexthub();
+}
+
+static bool readApp(const char *file, NanoAppBinary *appBinary)
+{
+    bool success = false;
+    int fd = open(file, O_RDONLY);
+
+    if (fd >= 0) {
+        struct stat sb;
+        if (fstat(fd, &sb) == 0) {
+            void *buf = malloc(sb.st_size);
+            if (buf != nullptr && read(fd, buf, sb.st_size) == sb.st_size) {
+                success = true;
+                const struct nano_app_binary_t *header = static_cast<const struct nano_app_binary_t *>(buf);
+                appBinary->appId = header->app_id.id;
+                appBinary->appVersion = header->app_version;
+                appBinary->flags = header->flags;
+                appBinary->targetChreApiMajorVersion = header->target_chre_api_major_version;
+                appBinary->targetChreApiMinorVersion = header->target_chre_api_minor_version;
+                appBinary->customBinary = std::vector<uint8_t>(static_cast<const uint8_t *>(buf) + sizeof(struct nano_app_binary_t), static_cast<const uint8_t *>(buf) + sb.st_size);
+            }
+            free(buf);
+        }
+        close(fd);
+    }
+    return success;
+}
+
+Return<void> Contexthub::debug(const hidl_handle& hh_fd,
+                               const hidl_vec<hidl_string>& hh_data) {
+    if (hh_fd == nullptr || hh_fd->numFds < 1) {
+        return Void();
+    }
+
+    String8 result;
+    int fd = hh_fd.getNativeHandle()->data[0];
+
+    if (hh_data.size() == 0) {
+        result.appendFormat("debug: %d\n", NanoHub::getDebugFlags());
+        std::string appInfo;
+        NanoHub::dumpAppInfo(appInfo);
+        result.append(appInfo.c_str());
+    } else if (hh_data.size() == 1) {
+        NanoHub::setDebugFlags(atoi(hh_data[0].c_str()));
+        result.appendFormat("debug: %d\n", NanoHub::getDebugFlags());
+    } else if (hh_data.size() == 2) {
+        if (strncmp(hh_data[0].c_str(), "load", 4) == 0) {
+            NanoAppBinary appBinary;
+            if (readApp(hh_data[1].c_str(), &appBinary))
+                loadNanoApp(0, appBinary, 0);
+        } else if (strncmp(hh_data[0].c_str(), "unload", 6) == 0) {
+            unloadNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+        } else if (strncmp(hh_data[0].c_str(), "enable", 6) == 0) {
+            enableNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+        } else if (strncmp(hh_data[0].c_str(), "disable", 7) == 0) {
+            disableNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+        }
+    } else {
+        result.appendFormat("unknown debug options");
+    }
+    write(fd, result.string(), result.size());
+
+    return Void();
+}
+
+}  // namespace implementation
+}  // namespace V1_0
+}  // namespace contexthub
+}  // namespace hardware
+}  // namespace android
diff --git a/contexthubhal/NanohubHidlAdapter.h b/contexthubhal/NanohubHidlAdapter.h
new file mode 100644
index 00000000..79d6242f
--- /dev/null
+++ b/contexthubhal/NanohubHidlAdapter.h
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+ /*
+  * This file is based on:
+  * hardware/interfaces/contexthub/1.0/default/Contexthub.h
+  * with modifications to connect directly to the NanohubHAL and
+  * support endpoints.
+  */
+
+#ifndef _NANOHUB_HIDL_ADAPTER_H_
+#define _NANOHUB_HIDL_ADAPTER_H_
+
+#include <unordered_map>
+
+#include <android-base/macros.h>
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hardware/context_hub.h>
+
+#include "nanohubhal.h"
+
+namespace android {
+namespace hardware {
+namespace contexthub {
+namespace V1_0 {
+namespace implementation {
+
+struct Contexthub : public ::android::hardware::contexthub::V1_0::IContexthub {
+    Contexthub();
+
+    Return<void> getHubs(getHubs_cb _hidl_cb) override;
+
+    Return<Result> registerCallback(uint32_t hubId,
+                                    const sp<IContexthubCallback> &cb) override;
+
+    Return<Result> sendMessageToHub(uint32_t hubId,
+                                    const ContextHubMsg &msg) override;
+
+    Return<Result> loadNanoApp(uint32_t hubId,
+                               const NanoAppBinary& appBinary,
+                               uint32_t transactionId) override;
+
+    Return<Result> unloadNanoApp(uint32_t hubId,
+                                 uint64_t appId,
+                                 uint32_t transactionId) override;
+
+    Return<Result> enableNanoApp(uint32_t hubId,
+                                 uint64_t appId,
+                                 uint32_t transactionId) override;
+
+    Return<Result> disableNanoApp(uint32_t hubId,
+                                  uint64_t appId,
+                                  uint32_t transactionId) override;
+
+    Return<Result> queryApps(uint32_t hubId) override;
+
+    Return<Result> reboot(uint32_t hubId);
+
+    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& options) override;
+
+    bool isInitialized();
+
+
+private:
+
+    struct CachedHubInformation{
+        struct hub_app_name_t osAppName;
+        sp<IContexthubCallback> callback;
+    };
+
+    class DeathRecipient : public hidl_death_recipient {
+    public:
+        DeathRecipient(const sp<Contexthub> contexthub);
+
+        void serviceDied(
+                uint64_t cookie,
+                const wp<::android::hidl::base::V1_0::IBase>& who) override;
+
+    private:
+        sp<Contexthub> mContexthub;
+    };
+
+    std::unordered_map<uint32_t, CachedHubInformation> mCachedHubInfo;
+
+    sp<DeathRecipient> mDeathRecipient;
+    bool mIsTransactionPending;
+    uint32_t mTransactionId;
+
+    bool isValidHubId(uint32_t hubId);
+
+    sp<IContexthubCallback> getCallBackForHubId(uint32_t hubId);
+
+    int handleOsMessage(sp<IContexthubCallback> cb,
+                        uint32_t msgType,
+                        const uint8_t *msg,
+                        int msgLen,
+                        uint32_t transactionId);
+
+    // Handle the case where the callback registered for the given hub ID dies
+    void handleServiceDeath(uint32_t hubId);
+
+    static int contextHubCb(uint32_t hubId,
+                            const nanohub::HubMessage &rxMsg,
+                            void *cookie);
+
+    bool setOsAppAsDestination(hub_message_t *msg, int hubId);
+
+    DISALLOW_COPY_AND_ASSIGN(Contexthub);
+};
+
+extern "C" IContexthub *HIDL_FETCH_IContexthub(const char *);
+
+}  // namespace implementation
+}  // namespace V1_0
+}  // namespace contexthub
+}  // namespace hardware
+}  // namespace android
+
+#endif  // _NANOHUB_HIDL_ADAPTER_H_
diff --git a/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc b/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc
new file mode 100644
index 00000000..a60545ef
--- /dev/null
+++ b/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc
@@ -0,0 +1,4 @@
+service contexthub-1-0 /vendor/bin/hw/android.hardware.contexthub@1.0-service.nanohub
+    class hal
+    user system
+    group system
diff --git a/contexthubhal/legacyhal.cpp b/contexthubhal/legacyhal.cpp
new file mode 100644
index 00000000..80ceedb8
--- /dev/null
+++ b/contexthubhal/legacyhal.cpp
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2018 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 "nanohub_perdevice.h"
+#include "nanohubhal.h"
+
+using namespace android::nanohub;
+
+static context_hub_callback *mSavedCbk = nullptr;
+
+static int legacy_cbk(uint32_t hub_id, const HubMessage &rxMsg, void *cookie)
+{
+    return mSavedCbk(hub_id, &rxMsg, cookie);
+}
+
+static int legacy_subscribe_messages(uint32_t hub_id, context_hub_callback *cbk, void *cookie)
+{
+    mSavedCbk = cbk;
+    if (cbk)
+        return NanoHub::subscribeMessages(hub_id, legacy_cbk, cookie);
+    else
+        return NanoHub::subscribeMessages(hub_id, nullptr, nullptr);
+}
+
+static int legacy_send_message(uint32_t hub_id, const hub_message_t *msg)
+{
+    return NanoHub::sendToNanohub(hub_id, msg, 0, ENDPOINT_UNSPECIFIED);
+}
+
+static int legacy_get_hubs(context_hub_module_t*, const context_hub_t ** list)
+{
+    *list = get_hub_info();
+
+    return 1; /* we have one hub */
+}
+
+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 = legacy_get_hubs,
+    .subscribe_messages = legacy_subscribe_messages,
+    .send_message = legacy_send_message,
+};
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp
index 21b63746..2f750a64 100644
--- a/contexthubhal/nanohubhal.cpp
+++ b/contexthubhal/nanohubhal.cpp
@@ -65,13 +65,15 @@ inline std::ostream &operator << (std::ostream &os, const hub_app_name_t &appId)
     return os;
 }
 
-void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status)
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, uint16_t endpoint, 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;
+    if (endpoint)
+        os << "; EPT=" << std::hex << endpoint;
     os << "]:" << std::hex;
     for (size_t i = 0; i < len; ++i) {
         os << " "  << std::setfill('0') << std::setw(2) << (unsigned int)p[i];
@@ -160,7 +162,7 @@ NanoHub::~NanoHub() {
     }
 }
 
-int NanoHub::doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len, uint32_t messageType)
+int NanoHub::doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len, uint32_t messageType, uint16_t endpoint)
 {
     if (len > MAX_RX_PACKET) {
         return -EINVAL;
@@ -173,6 +175,7 @@ int NanoHub::doSendToDevice(const hub_app_name_t name, const void *data, uint32_
             .appId = name.id,
             .len = static_cast<uint8_t>(len),
             .appEventId = messageType,
+            .endpoint = endpoint,
         },
     };
 
@@ -189,6 +192,11 @@ void NanoHub::doSendToApp(HubMessage &&msg)
     mAppTxCond.notify_all();
 }
 
+void NanoHub::doDumpAppInfo(std::string &result)
+{
+    SystemComm::dumpAppInfo(result);
+}
+
 void* NanoHub::runAppTx()
 {
     std::unique_lock<std::mutex> lk(mAppTxLock);
@@ -199,7 +207,7 @@ void* NanoHub::runAppTx()
         }
         HubMessage &m = mAppTxQueue.front();
         lk.unlock();
-        mMsgCbkFunc(0, &m, mMsgCbkData);
+        mMsgCbkFunc(0, m, mMsgCbkData);
         lk.lock();
         mAppTxQueue.pop_front();
     };
@@ -250,34 +258,45 @@ void* NanoHub::runDeviceRx()
                 ALOGE("read failed with %d", ret);
                 break;
             }
-            if (ret < (int)sizeof(msg.hdr)) {
+            if (ret < (int)sizeof(msg.raw.hdr)) {
                 ALOGE("Only read %d bytes", ret);
                 break;
             }
 
-            uint32_t len = msg.hdr.len;
+            uint32_t len = msg.raw.hdr.len;
 
-            if (len > sizeof(msg.data)) {
+            if (len > MAX_RX_PACKET) {
                 ALOGE("malformed packet with len %" PRIu32, len);
                 break;
             }
 
             // receive message from FW in legacy format
-            if (ret != (int)(sizeof(msg.hdr) + len)) {
-                ALOGE("Expected %zu bytes, read %d bytes", sizeof(msg.hdr) + len, ret);
+            if (ret == (int)(sizeof(msg.raw.hdr) + len)) {
+                ret = SystemComm::handleRx(&msg.raw);
+                if (ret > 0) {
+                    hub_app_name_t app_name = { .id = msg.raw.hdr.appId };
+                    if (messageTracingEnabled()) {
+                        dumpBuffer("(RAW) DEV -> APP", app_name, msg.raw.hdr.eventId, 0, &msg.raw.data[0], len);
+                    }
+                    doSendToApp(HubMessage(&app_name, msg.raw.hdr.eventId, ENDPOINT_BROADCAST, &msg.raw.data[0], len));
+                }
+            // receive message from FW in chre format
+            } else if (ret == (int)(sizeof(msg.chre.hdr) + len)) {
+                ret = SystemComm::handleRx(&msg.chre);
+                if (ret > 0) {
+                    hub_app_name_t app_name = { .id = msg.chre.hdr.appId };
+                    if (messageTracingEnabled()) {
+                        dumpBuffer("(CHRE) DEV -> APP", app_name, msg.chre.hdr.appEventId, msg.chre.hdr.endpoint, &msg.chre.data[0], len);
+                    }
+                    doSendToApp(HubMessage(&app_name, msg.chre.hdr.appEventId, msg.chre.hdr.endpoint, &msg.chre.data[0], len));
+                }
+            } else {
+                ALOGE("Expected (%zu|%zu) bytes, read %d bytes", sizeof(msg.raw.hdr) + len, sizeof(msg.chre.hdr) + len, ret);
                 break;
             }
 
-            ret = SystemComm::handleRx(&msg);
-            if (ret < 0) {
+            if (ret < 0)
                 ALOGE("SystemComm::handleRx() returned %d", ret);
-            } else if (ret) {
-                hub_app_name_t app_name = { .id = msg.hdr.appId };
-                if (messageTracingEnabled()) {
-                    dumpBuffer("DEV -> APP", app_name, msg.hdr.eventId, &msg.data[0], msg.hdr.len);
-                }
-                doSendToApp(HubMessage(&app_name, msg.hdr.eventId, &msg.data[0], msg.hdr.len));
-            }
         }
 
         if (myFds[IDX_CLOSE_PIPE].revents & POLLIN) { // we have been asked to die
@@ -354,7 +373,7 @@ int NanoHub::closeHub(void)
     return 0;
 }
 
-int NanoHub::doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie)
+int NanoHub::doSubscribeMessages(uint32_t hub_id, Contexthub_callback *cbk, void *cookie)
 {
     if (hub_id) {
         return -ENODEV;
@@ -386,7 +405,7 @@ int NanoHub::doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, voi
     return ret;
 }
 
-int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg)
+int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg, uint32_t transaction_id, uint16_t endpoint)
 {
     if (hub_id) {
         return -ENODEV;
@@ -405,45 +424,23 @@ int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg)
         } 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);
+                dumpBuffer("APP -> HAL", msg->app_name, msg->message_type, 0, msg->message, msg->message_len);
             }
-            ret = SystemComm::handleTx(msg);
+            ret = SystemComm::handleTx(msg, transaction_id);
         } else if (msg->message_len > MAX_RX_PACKET) {
             ALOGW("not sending invalid message 2");
             ret = -EINVAL;
         } else {
             if (messageTracingEnabled()) {
-                dumpBuffer("APP -> DEV", msg->app_name, msg->message_type, msg->message, msg->message_len);
+                dumpBuffer("APP -> DEV", msg->app_name, msg->message_type, endpoint, msg->message, msg->message_len);
             }
-            ret = doSendToDevice(msg->app_name, msg->message, msg->message_len, msg->message_type);
+            ret = doSendToDevice(msg->app_name, msg->message, msg->message_len, msg->message_type, endpoint);
         }
     }
 
     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
index 6a0432c8..b9cc172c 100644
--- a/contexthubhal/nanohubhal.h
+++ b/contexthubhal/nanohubhal.h
@@ -27,36 +27,50 @@
 
 //as per protocol
 #define MAX_RX_PACKET               128
+#define MAX_TX_PACKET               128
 #define APP_FROM_HOST_EVENT_ID      0x000000F8
 #define APP_FROM_HOST_CHRE_EVENT_ID 0x000000F9
 
+#define ENDPOINT_UNSPECIFIED        0xFFFE
+#define ENDPOINT_BROADCAST          0xFFFF
+
 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);
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, uint16_t endpoint, const void *data, size_t len, int status = 0);
 
 struct nano_message_chre {
-    HostMsgHdrChreV10 hdr;
+    HostMsgHdrChre hdr;
     uint8_t data[MAX_RX_PACKET];
 } __attribute__((packed));
 
-struct nano_message {
+struct nano_message_raw {
     HostMsgHdr hdr;
     uint8_t data[MAX_RX_PACKET];
 } __attribute__((packed));
 
+union nano_message {
+    struct nano_message_chre chre;
+    struct nano_message_raw raw;
+} __attribute__((packed));
+
 class HubMessage : public hub_message_t {
     std::unique_ptr<uint8_t> data_;
 public:
+    uint32_t message_transaction_id;
+    uint16_t message_endpoint;
     HubMessage(const HubMessage &other) = delete;
     HubMessage &operator = (const HubMessage &other) = delete;
 
-    HubMessage(const hub_app_name_t *name, uint32_t typ, const void *data, uint32_t len) {
+    HubMessage(const hub_app_name_t *name, uint32_t typ, uint32_t transaction_id,
+            uint16_t endpoint, const void *data, uint32_t len) {
         app_name = *name;
         message_type = typ;
         message_len = len;
         message = data;
+        message_transaction_id = transaction_id;
+        message_endpoint = endpoint;
         if (len > 0 && data != nullptr) {
             data_ = std::unique_ptr<uint8_t>(new uint8_t[len]);
             memcpy(data_.get(), data, len);
@@ -64,12 +78,31 @@ public:
         }
     }
 
+    HubMessage(const hub_app_name_t *name, uint32_t typ, uint16_t endpoint, const void *data,
+            uint32_t len) : HubMessage(name, typ, 0, endpoint, data, len) { }
+
+    HubMessage(const hub_message_t *msg, uint32_t transaction_id, uint16_t endpoint) {
+        app_name = msg->app_name;
+        message_type = msg->message_type;
+        message_len = msg->message_len;
+        message = msg->message;
+        message_transaction_id = transaction_id;
+        message_endpoint = endpoint;
+        if (msg->message_len > 0 && msg->message != nullptr) {
+            data_ = std::unique_ptr<uint8_t>(new uint8_t[msg->message_len]);
+            memcpy(data_.get(), msg->message, msg->message_len);
+            message = data_.get();
+        }
+    }
+
     HubMessage(HubMessage &&other) {
         *this = (HubMessage &&)other;
     }
 
     HubMessage &operator = (HubMessage &&other) {
         *static_cast<hub_message_t *>(this) = static_cast<hub_message_t>(other);
+        message_transaction_id = other.message_transaction_id;
+        message_endpoint = other.message_endpoint;
         data_ = std::move(other.data_);
         other.message = nullptr;
         other.message_len = 0;
@@ -77,6 +110,8 @@ public:
     }
 };
 
+typedef int Contexthub_callback(uint32_t hub_id, const HubMessage &rxed_msg, void *cookie);
+
 class NanoHub {
     std::mutex mLock;
     bool mAppQuit;
@@ -85,7 +120,7 @@ class NanoHub {
     std::list<HubMessage> mAppTxQueue;
     std::thread mPollThread;
     std::thread mAppThread;
-    context_hub_callback *mMsgCbkFunc;
+    Contexthub_callback *mMsgCbkFunc;
     int mThreadClosingPipe[2];
     int mFd; // [0] is read end
     void * mMsgCbkData;
@@ -113,10 +148,13 @@ class NanoHub {
         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, uint32_t messageType = 0);
+    int doSubscribeMessages(uint32_t hub_id, Contexthub_callback *cbk, void *cookie);
+    int doSendToNanohub(uint32_t hub_id, const hub_message_t *msg,
+            uint32_t transaction_id, uint16_t endpoint);
+    int doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len,
+            uint32_t messageType = 0, uint16_t endpoint = ENDPOINT_UNSPECIFIED);
     void doSendToApp(HubMessage &&msg);
+    void doDumpAppInfo(std::string &result);
 
     static constexpr unsigned int FL_MESSAGE_TRACING = 1;
 
@@ -135,20 +173,25 @@ public:
     static void setDebugFlags(unsigned int flags) {
         hubInstance()->mFlags = flags;
     }
+    static void dumpAppInfo(std::string &result) {
+        hubInstance()->doDumpAppInfo(result);
+    }
 
     // messaging interface
 
     // define callback to invoke for APP messages
-    static int subscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie) {
+    static int subscribeMessages(uint32_t hub_id, Contexthub_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);
+    static int sendToNanohub(uint32_t hub_id, const hub_message_t *msg,
+            uint32_t transaction_id, uint16_t endpoint) {
+        return hubInstance()->doSendToNanohub(hub_id, msg, transaction_id, endpoint);
     }
     // 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);
+    static int sendToDevice(const hub_app_name_t *name, const void *data, uint32_t len,
+            uint32_t transactionId) {
+        return hubInstance()->doSendToDevice(*name, data, len, transactionId, ENDPOINT_UNSPECIFIED);
     }
     // passes message to APP via callback
     static void sendToApp(HubMessage &&msg) {
diff --git a/contexthubhal/service.cpp b/contexthubhal/service.cpp
new file mode 100644
index 00000000..645a269f
--- /dev/null
+++ b/contexthubhal/service.cpp
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2018 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 "android.hardware.contexthub@1.0-service.nanohub"
+
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hidl/HidlSupport.h>
+#include <hidl/HidlTransportSupport.h>
+
+#include "NanohubHidlAdapter.h"
+
+using android::hardware::configureRpcThreadpool;
+using android::hardware::joinRpcThreadpool;
+using android::hardware::contexthub::V1_0::IContexthub;
+using android::hardware::contexthub::V1_0::implementation::Contexthub;
+using namespace android;
+
+status_t registerContexthubService()
+{
+    sp<IContexthub> contexthub = new Contexthub();
+    return contexthub->registerAsService();
+}
+
+int main() {
+    configureRpcThreadpool(1, true);
+    status_t status = registerContexthubService();
+    if (status != OK) {
+        return status;
+    }
+    joinRpcThreadpool();
+}
diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp
index b0e4bc9b..6edd0cbc 100644
--- a/contexthubhal/system_comms.cpp
+++ b/contexthubhal/system_comms.cpp
@@ -16,9 +16,13 @@
 
 #define LOG_TAG "NanohubHAL"
 
+#include "file.h"
+#include <json/json.h>
+
 #include <cassert>
 #include <cerrno>
 #include <cinttypes>
+#include <string>
 
 #include <endian.h>
 
@@ -27,12 +31,20 @@
 #include <log/log.h>
 
 #include <endian.h>
+#include <sys/stat.h>
+
+#include <media/stagefright/foundation/ADebug.h>
 
 #include <hardware/context_hub.h>
 #include "nanohub_perdevice.h"
 #include "system_comms.h"
 #include "nanohubhal.h"
 
+#define CHRE_APP_DIR        "/data/vendor/sensor/chre"
+#define CHRE_APP_DIR_PERMS  (S_IRUSR | S_IWUSR | S_IXUSR)
+#define CHRE_APP_FILE_PERMS (S_IRUSR | S_IWUSR)
+#define CHRE_APP_SETTINGS   CHRE_APP_DIR "/apps.json"
+
 namespace android {
 
 namespace nanohub {
@@ -47,74 +59,134 @@ static void writeAppName(MessageBuf &buf, const hub_app_name_t &name)
     buf.writeU64(name.id);
 }
 
-static void readNanohubAppInfo(MessageBuf &buf, NanohubAppInfo &info)
+static void readNanohubMemInfo(MessageBuf &buf, NanohubMemInfo &mi)
 {
-    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__);
+    uint8_t type, len;
+    uint32_t ramFree = NANOHUB_MEM_SZ_UNKNOWN;
+    uint32_t eeFree = NANOHUB_MEM_SZ_UNKNOWN;
+    uint32_t sharedFree = NANOHUB_MEM_SZ_UNKNOWN;
+    uint32_t osFree = NANOHUB_MEM_SZ_UNKNOWN;
+
+    mi.flashSz = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.blSz = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.osSz = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.sharedSz = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.eeSz = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.ramSz = NANOHUB_MEM_SZ_UNKNOWN;
+
+    mi.blUse = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.osUse = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.sharedUse = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.eeUse = NANOHUB_MEM_SZ_UNKNOWN;
+    mi.ramUse = NANOHUB_MEM_SZ_UNKNOWN;
+
+    while (buf.getRoom() >= 2) {
+        type = buf.readU8();
+        len = buf.readU8();
+        if (buf.getRoom() >= len) {
+            switch(type) {
+            case NANOHUB_HAL_SYS_INFO_HEAP_FREE:
+                if (len == sizeof(ramFree))
+                    ramFree = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_RAM_SIZE:
+                if (len == sizeof(mi.ramSz))
+                    mi.ramSz = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_EEDATA_SIZE:
+                if (len == sizeof(mi.ramSz))
+                    mi.eeSz = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_EEDATA_FREE:
+                if (len == sizeof(eeFree))
+                    eeFree = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_CODE_SIZE:
+                if (len == sizeof(mi.osSz))
+                    mi.osSz = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_CODE_FREE:
+                if (len == sizeof(osFree))
+                    osFree = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_SHARED_SIZE:
+                if (len == sizeof(mi.sharedSz))
+                    mi.sharedSz = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_SHARED_FREE:
+                if (len == sizeof(sharedFree))
+                    sharedFree = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_SYS_INFO_END:
+                if (len != 0 || buf.getRoom() != 0) {
+                    ALOGE("%s: failed to read object", __func__);
+                    return;
+                }
+                break;
+            default:
+                ALOGI("%s: unsupported type: %d", __func__, type);
+                buf.readRaw(len);
+                break;
+            }
+        } else {
+            ALOGE("%s: failed to read object", __func__);
+            return;
+        }
     }
-}
 
-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)) {
+    if (buf.getRoom() != 0) {
         ALOGE("%s: failed to read object", __func__);
+        return;
     }
+
+    if (mi.ramSz != NANOHUB_MEM_SZ_UNKNOWN && ramFree != NANOHUB_MEM_SZ_UNKNOWN)
+        mi.ramUse = mi.ramSz - ramFree;
+    if (mi.eeSz != NANOHUB_MEM_SZ_UNKNOWN && eeFree != NANOHUB_MEM_SZ_UNKNOWN)
+        mi.eeUse = mi.eeSz - eeFree;
+    if (mi.osSz != NANOHUB_MEM_SZ_UNKNOWN && osFree != NANOHUB_MEM_SZ_UNKNOWN)
+        mi.osUse = mi.osSz - osFree;
+    if (mi.sharedSz != NANOHUB_MEM_SZ_UNKNOWN && sharedFree != NANOHUB_MEM_SZ_UNKNOWN)
+        mi.sharedUse = mi.sharedSz - sharedFree;
 }
 
-NanohubRsp::NanohubRsp(MessageBuf &buf, bool no_status)
+NanohubRsp::NanohubRsp(MessageBuf &buf, uint32_t transactionId, bool chre)
 {
-    // all responses start with command
-    // most of them have 4-byte status (result code)
+    // all responses start with command and have a 4-byte status (result code)
     buf.reset();
-    cmd = buf.readU8();
-    if (!buf.getSize()) {
-        status = -EINVAL;
-    } else if (no_status) {
-        status = 0;
+    if (buf.getSize() < 5) {
+        mStatus = -EINVAL;
     } else {
-        if (cmd == NANOHUB_START_UPLOAD || cmd == NANOHUB_CONT_UPLOAD || cmd == NANOHUB_FINISH_UPLOAD)
-            status = buf.readU8();
+        mCmd = buf.readU8();
+        mStatus = buf.readU32();
+        if (chre)
+            mTransactionId = transactionId;
         else
-            status = buf.readU32();
+            mTransactionId = 0;
     }
 }
 
-int SystemComm::sendToSystem(const void *data, size_t len)
+int SystemComm::sendToSystem(const void *data, size_t len, uint32_t transactionId)
 {
     if (NanoHub::messageTracingEnabled()) {
-        dumpBuffer("HAL -> SYS", getSystem()->mHostIfAppName, 0, data, len);
+        dumpBuffer("HAL -> SYS", getSystem()->mHostIfAppName, transactionId, 0, data, len);
     }
-    return NanoHub::sendToDevice(&getSystem()->mHostIfAppName, data, len);
-}
-
-int SystemComm::AppInfoSession::setup(const hub_message_t *)
-{
-    std::lock_guard<std::mutex> _l(mLock);
-    int suggestedSize = mAppInfo.size() ? mAppInfo.size() : 20;
-
-    mAppInfo.clear();
-    mAppInfo.reserve(suggestedSize);
-    setState(SESSION_USER);
-
-    return requestNext();
+    return NanoHub::sendToDevice(&getSystem()->mHostIfAppName, data, len, transactionId);
 }
 
 inline hub_app_name_t deviceAppNameToHost(const hub_app_name_t src)
@@ -129,143 +201,112 @@ inline hub_app_name_t hostAppNameToDevice(const hub_app_name_t src)
     return res;
 }
 
-int SystemComm::AppInfoSession::handleRx(MessageBuf &buf)
+const uint8_t app_info_tags[] =
 {
-    std::lock_guard<std::mutex> _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()
+    NANOHUB_HAL_APP_INFO_APPID,
+    NANOHUB_HAL_APP_INFO_CRC,
+    NANOHUB_HAL_APP_INFO_TID,
+    NANOHUB_HAL_APP_INFO_VERSION,
+    NANOHUB_HAL_APP_INFO_ADDR,
+    NANOHUB_HAL_APP_INFO_SIZE,
+    NANOHUB_HAL_APP_INFO_HEAP,
+    NANOHUB_HAL_APP_INFO_DATA,
+    NANOHUB_HAL_APP_INFO_BSS,
+    NANOHUB_HAL_APP_INFO_CHRE_MAJOR,
+    NANOHUB_HAL_APP_INFO_CHRE_MINOR,
+    NANOHUB_HAL_APP_INFO_END,
+};
+
+const uint8_t sys_info_tags[] =
 {
-    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 *)
+    NANOHUB_HAL_SYS_INFO_HEAP_FREE,
+    NANOHUB_HAL_SYS_INFO_RAM_SIZE,
+    NANOHUB_HAL_SYS_INFO_EEDATA_SIZE,
+    NANOHUB_HAL_SYS_INFO_EEDATA_FREE,
+    NANOHUB_HAL_SYS_INFO_CODE_SIZE,
+    NANOHUB_HAL_SYS_INFO_CODE_FREE,
+    NANOHUB_HAL_SYS_INFO_SHARED_SIZE,
+    NANOHUB_HAL_SYS_INFO_SHARED_FREE,
+    NANOHUB_HAL_SYS_INFO_END,
+};
+
+int SystemComm::MemInfoSession::setup(const hub_message_t *, uint32_t transactionId, AppManager &)
 {
     std::lock_guard<std::mutex> _l(mLock);
     char data[MAX_RX_PACKET];
     MessageBuf buf(data, sizeof(data));
-    buf.writeU8(NANOHUB_QUERY_MEMINFO);
+    buf.writeU8(NANOHUB_HAL_SYS_INFO);
+    buf.writeRaw(sys_info_tags, sizeof(sys_info_tags));
 
     setState(SESSION_USER);
-    return sendToSystem(buf.getData(), buf.getPos());
+    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
 }
 
-int SystemComm::MemInfoSession::handleRx(MessageBuf &buf)
+int SystemComm::MemInfoSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre)
 {
     std::lock_guard<std::mutex> _l(mLock);
-    NanohubRsp rsp(buf, true);
+    NanohubRsp rsp(buf, transactionId, chre);
 
-    if (rsp.cmd != NANOHUB_QUERY_MEMINFO)
+    if (rsp.mCmd != NANOHUB_HAL_SYS_INFO)
         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,
-        });
+    if (len) {
+        NanohubMemInfo mi;
+        readNanohubMemInfo(buf, mi);
+
+        //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,
+    sendToApp(CONTEXT_HUB_QUERY_MEMORY, transactionId,
               static_cast<const void *>(ranges.data()),
               ranges.size() * sizeof(ranges[0]));
 
     complete();
-
     return 0;
 }
 
-int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg)
+int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager)
 {
     std::lock_guard<std::mutex> _l(mLock);
 
@@ -281,203 +322,468 @@ int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg)
 
     switch (mCmd) {
     case  CONTEXT_HUB_APPS_ENABLE:
-        return setupMgmt(appMsg, NANOHUB_EXT_APPS_ON);
+        return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_START, appManager);
     case  CONTEXT_HUB_APPS_DISABLE:
-        return setupMgmt(appMsg, NANOHUB_EXT_APPS_OFF);
+        return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_STOP, appManager);
     case  CONTEXT_HUB_UNLOAD_APP:
-        return setupMgmt(appMsg, NANOHUB_EXT_APP_DELETE);
+        return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_UNLOAD, appManager);
     case  CONTEXT_HUB_LOAD_APP:
     {
-        mData.clear();
-        mData = std::vector<uint8_t>(msgData, msgData + mLen);
         const load_app_request_t *appReq = static_cast<const load_app_request_t*>(appMsg->message);
         if (appReq == nullptr || mLen <= sizeof(*appReq)) {
             ALOGE("%s: Invalid app header: too short\n", __func__);
             return -EINVAL;
         }
         mAppName = appReq->app_binary.app_id;
-        setState(TRANSFER);
-
-        buf.writeU8(NANOHUB_START_UPLOAD);
-        buf.writeU8(0);
-        buf.writeU32(mLen);
-        return sendToSystem(buf.getData(), buf.getPos());
+        if (!appManager.isAppLoaded(mAppName)) {
+            appManager.addNewApp(mAppName, appReq->app_binary.app_version);
+            appManager.writeApp(mAppName, msgData, mLen);
+            mData.clear();
+            mData = std::vector<uint8_t>(msgData, msgData + mLen);
+            setState(TRANSFER);
+
+            buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+            buf.writeU8(0);
+            buf.writeU32(mLen);
+
+            return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+        } else {
+            if (appManager.cmpApp(mAppName, msgData, mLen)) {
+                mFlashAddr = appManager.getFlashAddr(mAppName);
+                if (appManager.isAppRunning(mAppName)) {
+                    setState(STOP_RUN);
+
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT);
+                    writeAppName(buf, mAppName);
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT_STOP);
+
+                    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+                } else {
+                    setState(RUN);
+
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT);
+                    writeAppName(buf, mAppName);
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+
+                    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+                }
+            } else {
+                appManager.setCachedVersion(mAppName, appReq->app_binary.app_version);
+                appManager.writeApp(mAppName, msgData, mLen);
+                mData.clear();
+                mData = std::vector<uint8_t>(msgData, msgData + mLen);
+                if (appManager.isAppRunning(mAppName)) {
+                    setState(STOP_TRANSFER);
+
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT);
+                    writeAppName(buf, mAppName);
+                    buf.writeU8(NANOHUB_HAL_APP_MGMT_STOP);
+
+                    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+                } else {
+                    setState(TRANSFER);
+
+                    buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+                    buf.writeU8(0);
+                    buf.writeU32(mLen);
+
+                    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+                }
+            }
+        }
     }
-
     case  CONTEXT_HUB_OS_REBOOT:
         setState(REBOOT);
-        buf.writeU8(NANOHUB_REBOOT);
-        return sendToSystem(buf.getData(), buf.getPos());
+
+        buf.writeU8(NANOHUB_HAL_SYS_MGMT);
+        buf.writeU8(NANOHUB_HAL_SYS_MGMT_REBOOT);
+
+        return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+
+    case  CONTEXT_HUB_START_APPS:
+        if (mLen == sizeof(mStatus))
+            memcpy(&mStatus, msgData, mLen);
+        appManager.eraseApps();
+        setState(QUERY_START);
+
+        buf.writeU8(NANOHUB_HAL_APP_INFO);
+        buf.writeU32(0);
+        buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+        return sendToSystem(buf.getData(), buf.getPos(), transactionId);
     }
 
     return -EINVAL;
 }
 
-int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t cmd)
+int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t transactionId, uint32_t cmd, AppManager &appManager)
 {
+    int32_t result = 0;
     const hub_app_name_t &appName = *static_cast<const hub_app_name_t*>(appMsg->message);
     if (appMsg->message_len != sizeof(appName)) {
         return -EINVAL;
     }
-
+    mAppName = appName;
+
+    switch (cmd) {
+    case NANOHUB_HAL_APP_MGMT_START:
+        if (appManager.isAppRunning(mAppName)) {
+            appManager.setCachedStart(mAppName, true);
+            sendToApp(mCmd, transactionId, &result, sizeof(result));
+            complete();
+            return 0;
+        }
+        break;
+    case NANOHUB_HAL_APP_MGMT_STOP:
+    case NANOHUB_HAL_APP_MGMT_UNLOAD:
+        appManager.setCachedStart(mAppName, false);
+        if (!appManager.isAppRunning(mAppName)) {
+            sendToApp(mCmd, transactionId, &result, sizeof(result));
+            complete();
+            return 0;
+        }
+        break;
+    }
     char data[MAX_RX_PACKET];
     MessageBuf buf(data, sizeof(data));
-    buf.writeU8(cmd);
+    buf.writeU8(NANOHUB_HAL_APP_MGMT);
     writeAppName(buf, appName);
+    buf.writeU8(cmd);
     setState(MGMT);
 
-    return sendToSystem(buf.getData(), buf.getPos());
+    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
 }
 
-int SystemComm::AppMgmtSession::handleRx(MessageBuf &buf)
+int SystemComm::AppMgmtSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre)
 {
     int ret = 0;
     std::lock_guard<std::mutex> _l(mLock);
-    NanohubRsp rsp(buf);
+    NanohubRsp rsp(buf, transactionId, chre);
 
     switch (getState()) {
     case TRANSFER:
-        ret = handleTransfer(rsp);
+        ret = handleTransfer(rsp, buf, appManager);
+        break;
+    case STOP_TRANSFER:
+        ret = handleStopTransfer(rsp, buf, appManager);
+         break;
+    case QUERY_START:
+        ret = handleQueryStart(rsp, buf, appManager);
+        break;
+    case START:
+        ret = handleStart(rsp, buf, appManager);
         break;
     case FINISH:
-        ret = handleFinish(rsp);
+        ret = handleFinish(rsp, buf, appManager);
         break;
     case RUN:
-        ret = handleRun(rsp);
+        ret = handleRun(rsp, buf, appManager);
         break;
-    case RUN_FAILED:
-        ret = handleRunFailed(rsp);
+    case STOP_RUN:
+        ret = handleStopRun(rsp, buf, appManager);
         break;
     case REBOOT:
-        ret = handleReboot(rsp);
+        ret = handleReboot(rsp, buf, appManager);
+        break;
+    case ERASE_TRANSFER:
+        ret = handleEraseTransfer(rsp, buf, appManager);
         break;
     case MGMT:
-        ret = handleMgmt(rsp);
+        ret = handleMgmt(rsp, buf, appManager);
+        break;
+    case INFO:
+        ret = handleInfo(rsp, buf, appManager);
         break;
     }
 
     return ret;
 }
 
-int SystemComm::AppMgmtSession::handleTransfer(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleTransfer(NanohubRsp &rsp, MessageBuf &, AppManager &appManager)
 {
-    if (rsp.cmd != NANOHUB_CONT_UPLOAD && rsp.cmd != NANOHUB_START_UPLOAD)
+    if (rsp.mCmd != NANOHUB_HAL_CONT_UPLOAD && rsp.mCmd != NANOHUB_HAL_START_UPLOAD)
         return 1;
 
     char data[MAX_RX_PACKET];
     MessageBuf buf(data, sizeof(data));
-    const bool success = rsp.status != 0;
+    int32_t result = 0;
 
     static_assert(NANOHUB_UPLOAD_CHUNK_SZ_MAX <= (MAX_RX_PACKET-5),
                   "Invalid chunk size");
 
-    if (success) {
+    if (rsp.mStatus == NANOHUB_HAL_UPLOAD_ACCEPTED) {
         mPos = mNextPos;
         mErrCnt = 0;
+    } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_RESEND) {
+        mErrCnt ++;
+    } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_RESTART) {
+        mPos = 0;
+        mErrCnt ++;
+    } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_CANCEL ||
+               rsp.mStatus == NANOHUB_HAL_UPLOAD_CANCEL_NO_RETRY) {
+        mPos = mLen;
+        result = NANOHUB_APP_NOT_LOADED;
+    } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_NO_SPACE) {
+        mPos = 0;
+        mErrCnt = 0;
+        setState(ERASE_TRANSFER);
+
+        buf.writeU8(NANOHUB_HAL_SYS_MGMT);
+        buf.writeU8(NANOHUB_HAL_SYS_MGMT_ERASE);
+
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
     } else if (mErrCnt > 5) {
         mPos = mLen;
+        result = NANOHUB_APP_NOT_LOADED;
     } else {
         mErrCnt ++;
     }
 
-    if (mPos < mLen) {
+    if (result != 0) {
+        appManager.clearCachedApp(mAppName);
+
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+        complete();
+        return 0;
+    } else 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.writeU8(NANOHUB_HAL_CONT_UPLOAD);
         buf.writeU32(mPos);
         buf.writeRaw(&mData[mPos], chunkSize);
         mNextPos = mPos + chunkSize;
     } else {
-        buf.writeU8(NANOHUB_FINISH_UPLOAD);
+        buf.writeU8(NANOHUB_HAL_FINISH_UPLOAD);
         setState(FINISH);
     }
 
-    return sendToSystem(buf.getData(), buf.getPos());
+    return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
 }
 
-int SystemComm::AppMgmtSession::handleFinish(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleStopTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
 {
-    if (rsp.cmd != NANOHUB_FINISH_UPLOAD)
+    if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
         return 1;
 
-    int ret = 0;
-    const bool success = rsp.status != 0;
-    mData.clear();
+    uint8_t cmd = buf.readU8();
 
-    if (success) {
+    if (cmd != NANOHUB_HAL_APP_MGMT_STOP)
+        return 1;
+
+    MgmtStatus sts = { .value = buf.readU32() };
+
+    ALOGI("Nanohub NEW APP STOP: %08" PRIX32 "\n", sts.value);
+    if (rsp.mStatus == 0) {
         char data[MAX_RX_PACKET];
         MessageBuf buf(data, sizeof(data));
-        buf.writeU8(NANOHUB_EXT_APPS_ON);
-        writeAppName(buf, mAppName);
-        setState(RUN);
-        ret = sendToSystem(buf.getData(), buf.getPos());
+        setState(TRANSFER);
+
+        buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+        buf.writeU8(0);
+        buf.writeU32(mLen);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
     } else {
         int32_t result = NANOHUB_APP_NOT_LOADED;
 
-        sendToApp(mCmd, &result, sizeof(result));
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
         complete();
+        return 0;
     }
+}
 
-    return ret;
+int SystemComm::AppMgmtSession::handleQueryStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+
+    if (rsp.mCmd != NANOHUB_HAL_APP_INFO)
+        return 1;
+
+    size_t len = buf.getRoom();
+    if (len) {
+        uint32_t nextAddr = appManager.readNanohubAppInfo(buf);
+
+        if (nextAddr) {
+            char data[MAX_RX_PACKET];
+            MessageBuf buf(data, sizeof(data));
+
+            buf.writeU8(NANOHUB_HAL_APP_INFO);
+            buf.writeU32(nextAddr);
+            buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+            return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+        }
+    }
+
+    appManager.getAppsToStart(mAppList);
+    if (mAppList.empty()) {
+        sendToApp(CONTEXT_HUB_OS_REBOOT, 0, &mStatus, sizeof(mStatus));
+        complete();
+        return 0;
+    } else {
+        char data[MAX_RX_PACKET];
+        MessageBuf buf(data, sizeof(data));
+        mAppName = mAppList.back();
+        mAppList.pop_back();
+        setState(START);
+
+        buf.writeU8(NANOHUB_HAL_APP_MGMT);
+        writeAppName(buf, mAppName);
+        buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    }
 }
 
-int SystemComm::AppMgmtSession::handleRun(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
 {
-    if (rsp.cmd != NANOHUB_EXT_APPS_ON)
+    if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
         return 1;
 
-    MgmtStatus sts = { .value = (uint32_t)rsp.status };
+    uint8_t cmd = buf.readU8();
 
-    // op counter returns number of nanoapps that were started as result of the command
-    // for successful start command it must be > 0
-    int32_t result = sts.value > 0 && sts.op > 0 && sts.op <= 0x7F ? 0 : -1;
+    if (cmd != NANOHUB_HAL_APP_MGMT_START)
+        return 1;
 
-    ALOGI("Nanohub NEW APP START: %08" PRIX32 "\n", rsp.status);
-    if (result != 0) {
-        // if nanoapp failed to start we have to unload it
+    MgmtStatus sts = { .value = buf.readU32() };
+
+    ALOGI("Nanohub EXISTING APP START: %08" PRIX32 "\n", sts.value);
+    if (mAppList.empty()) {
+        sendToApp(CONTEXT_HUB_OS_REBOOT, 0, &mStatus, sizeof(mStatus));
+        complete();
+        return 0;
+    } else {
         char data[MAX_RX_PACKET];
         MessageBuf buf(data, sizeof(data));
-        buf.writeU8(NANOHUB_EXT_APP_DELETE);
+        mAppName = mAppList.back();
+        mAppList.pop_back();
+
+        buf.writeU8(NANOHUB_HAL_APP_MGMT);
         writeAppName(buf, mAppName);
-        if (sendToSystem(buf.getData(), buf.getPos()) == 0) {
-            setState(RUN_FAILED);
-            return 0;
-        }
-        ALOGE("%s: failed to send DELETE for failed app\n", __func__);
+        buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    }
+}
+
+int SystemComm::AppMgmtSession::handleFinish(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+    if (rsp.mCmd != NANOHUB_HAL_FINISH_UPLOAD)
+        return 1;
+
+    mFlashAddr = buf.readU32();
+    uint32_t crc = buf.readU32();
+    mData.clear();
+
+    if (rsp.mStatus == 0) {
+        appManager.setCachedCrc(mAppName, crc);
+        char data[MAX_RX_PACKET];
+        MessageBuf buf(data, sizeof(data));
+        setState(RUN);
+
+        buf.writeU8(NANOHUB_HAL_APP_MGMT);
+        writeAppName(buf, mAppName);
+        buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    } else {
+        int32_t result = NANOHUB_APP_NOT_LOADED;
+        appManager.clearCachedApp(mAppName);
+
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+        complete();
+        return 0;
+    }
+}
+
+int SystemComm::AppMgmtSession::handleRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+    if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
+        return 1;
+
+    uint8_t cmd = buf.readU8();
+
+    if (cmd != NANOHUB_HAL_APP_MGMT_START)
+        return 1;
+
+    MgmtStatus sts = { .value = buf.readU32() };
+
+    ALOGI("Nanohub NEW APP START: %08" PRIX32 "\n", sts.value);
+    if (rsp.mStatus == 0) {
+        appManager.setCachedStart(mAppName, true);
+        char data[MAX_RX_PACKET];
+        MessageBuf buf(data, sizeof(data));
+        setState(INFO);
+
+        buf.writeU8(NANOHUB_HAL_APP_INFO);
+        buf.writeU32(mFlashAddr);
+        buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    } else {
+        appManager.setCachedStart(mAppName, false);
+        int32_t result = NANOHUB_APP_NOT_LOADED;
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+        complete();
+        return 0;
     }
+}
 
-    // it is either success, and we report it, or
-    // it is a failure to load, and also failure to send erase command
-    sendToApp(mCmd, &result, sizeof(result));
+int SystemComm::AppMgmtSession::handleInfo(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+    if (rsp.mCmd != NANOHUB_HAL_APP_INFO)
+        return 1;
+    int32_t result = 0;
+    size_t len = buf.getRoom();
+    if (len) {
+        appManager.readNanohubAppInfo(buf);
+        appManager.saveApps();
+    }
+    sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
     complete();
     return 0;
 }
 
-int SystemComm::AppMgmtSession::handleRunFailed(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleStopRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
 {
-    if (rsp.cmd != NANOHUB_EXT_APP_DELETE)
+    if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
         return 1;
 
-    int32_t result = -1;
+    uint8_t cmd = buf.readU8();
+
+    if (cmd != NANOHUB_HAL_APP_MGMT_STOP)
+        return 1;
 
-    ALOGI("%s: APP DELETE [because it failed]: %08" PRIX32 "\n", __func__, rsp.status);
+    MgmtStatus sts = { .value = buf.readU32() };
 
-    sendToApp(mCmd, &result, sizeof(result));
-    complete();
+    ALOGI("Nanohub NEW APP STOP: %08" PRIX32 "\n", sts.value);
+    if (rsp.mStatus == 0) {
+        char data[MAX_RX_PACKET];
+        MessageBuf buf(data, sizeof(data));
+        setState(RUN);
 
-    return 0;
+        buf.writeU8(NANOHUB_HAL_APP_MGMT);
+        writeAppName(buf, mAppName);
+        buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    } else {
+        int32_t result = NANOHUB_APP_NOT_LOADED;
+
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+        complete();
+        return 0;
+    }
 }
 
 /* reboot notification, when triggered by App request */
-int SystemComm::AppMgmtSession::handleReboot(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleReboot(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
 {
-    if (rsp.cmd != NANOHUB_REBOOT)
+    if (rsp.mCmd != NANOHUB_HAL_SYS_MGMT)
         return 1;
-    ALOGI("Nanohub reboot status [USER REQ]: %08" PRIX32 "\n", rsp.status);
+
+    uint8_t cmd = buf.readU8();
+
+    if (cmd == NANOHUB_HAL_SYS_MGMT_REBOOT)
+        ALOGI("Nanohub reboot status [USER REQ]: %08" PRIX32 "\n", rsp.mStatus);
 
     // reboot notification is sent by SessionManager
     complete();
@@ -485,59 +791,102 @@ int SystemComm::AppMgmtSession::handleReboot(NanohubRsp &rsp)
     return 0;
 }
 
-int SystemComm::AppMgmtSession::handleMgmt(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleEraseTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
 {
+    if (rsp.mCmd != NANOHUB_HAL_SYS_MGMT)
+        return 1;
+
+    uint8_t cmd = buf.readU8();
+
+    if (cmd == NANOHUB_HAL_SYS_MGMT_ERASE && rsp.mStatus == 0) {
+        char data[MAX_RX_PACKET];
+        MessageBuf buf(data, sizeof(data));
+        appManager.eraseApps();
+        setState(TRANSFER);
+
+        buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+        buf.writeU8(0);
+        buf.writeU32(mLen);
+        return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+    } else {
+        int32_t result = NANOHUB_APP_NOT_LOADED;
+
+        sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+        complete();
+        return 0;
+    }
+}
+
+int SystemComm::AppMgmtSession::handleMgmt(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+    if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
+        return 1;
+
+    uint8_t cmd = buf.readU8();
+    MgmtStatus sts = { .value = buf.readU32() };
+
     bool valid = false;
 
-    int32_t result = rsp.status;
+    int32_t result = rsp.mStatus;
 
-    // TODO: remove this when context hub service can handle non-zero success status
-    if (result > 0) {
-        // something happened; assume it worked
-        result = 0;
-    } else if (result == 0) {
-        // nothing happened; this is provably an error
+    if (result != 0)
         result = -1;
-    }
 
-    switch (rsp.cmd) {
-    case NANOHUB_EXT_APPS_OFF:
+    switch (cmd) {
+    case NANOHUB_HAL_APP_MGMT_STOP:
         valid = mCmd == CONTEXT_HUB_APPS_DISABLE;
+        if (valid && rsp.mStatus == 0)
+            appManager.clearRunning(mAppName);
         break;
-    case NANOHUB_EXT_APPS_ON:
+    case NANOHUB_HAL_APP_MGMT_START:
         valid = mCmd == CONTEXT_HUB_APPS_ENABLE;
+        if (valid && rsp.mStatus == 0) {
+            appManager.setCachedStart(mAppName, true);
+            char data[MAX_RX_PACKET];
+            MessageBuf buf(data, sizeof(data));
+            setState(INFO);
+
+            buf.writeU8(NANOHUB_HAL_APP_INFO);
+            buf.writeU32(appManager.getFlashAddr(mAppName));
+            buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+            return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+        }
         break;
-    case NANOHUB_EXT_APP_DELETE:
+    case NANOHUB_HAL_APP_MGMT_UNLOAD:
         valid = mCmd == CONTEXT_HUB_UNLOAD_APP;
+        if (valid && rsp.mStatus == 0)
+            appManager.clearRunning(mAppName);
         break;
     default:
         return 1;
     }
 
-    ALOGI("Nanohub MGMT response: CMD=%02X; STATUS=%08" PRIX32, rsp.cmd, rsp.status);
+    ALOGI("Nanohub MGMT response: CMD=%02X; STATUS=%08" PRIX32, rsp.mCmd, sts.value);
     if (!valid) {
         ALOGE("Invalid response for this state: APP CMD=%02X", mCmd);
         return -EINVAL;
     }
 
-    sendToApp(mCmd, &result, sizeof(result));
+    sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
     complete();
-
     return 0;
 }
 
-int SystemComm::KeyInfoSession::setup(const hub_message_t *) {
+int SystemComm::KeyInfoSession::setup(const hub_message_t *, uint32_t transactionId, AppManager &) {
     std::lock_guard<std::mutex> _l(mLock);
+    mKeyNum = 0;
+    mKeyOffset = 0;
     mRsaKeyData.clear();
     setState(SESSION_USER);
     mStatus = -EBUSY;
-    return requestRsaKeys();
+    return requestRsaKeys(transactionId);
 }
 
-int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf)
+int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre)
 {
     std::lock_guard<std::mutex> _l(mLock);
-    NanohubRsp rsp(buf, true);
+    NanohubRsp rsp(buf, transactionId, chre);
 
     if (getState() != SESSION_USER) {
         // invalid state
@@ -545,11 +894,20 @@ int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf)
         return mStatus;
     }
 
-    if (buf.getRoom()) {
+    uint32_t keyLen = buf.readU32();
+    uint32_t dataLen = buf.getRoom();
+
+    if (dataLen) {
         mRsaKeyData.insert(mRsaKeyData.end(),
                            buf.getData() + buf.getPos(),
                            buf.getData() + buf.getSize());
-        return requestRsaKeys();
+        if (mKeyOffset + dataLen >= keyLen) {
+            mKeyNum++;
+            mKeyOffset = 0;
+        } else {
+            mKeyOffset += dataLen;
+        }
+        return requestRsaKeys(transactionId);
     } else {
         mStatus = 0;
         complete();
@@ -557,43 +915,500 @@ int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf)
     }
 }
 
-int SystemComm::KeyInfoSession::requestRsaKeys(void)
+int SystemComm::KeyInfoSession::requestRsaKeys(uint32_t transactionId)
 {
     char data[MAX_RX_PACKET];
     MessageBuf buf(data, sizeof(data));
 
-    buf.writeU8(NANOHUB_QUERY_APPS);
-    buf.writeU32(mRsaKeyData.size());
+    buf.writeU8(NANOHUB_HAL_KEY_INFO);
+    buf.writeU32(mKeyNum);
+    buf.writeU32(mKeyOffset);
+
+    return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+}
+
+void SystemComm::AppManager::dumpAppInfo(std::string &result)
+{
+    char buffer[256];
+
+    for (auto &it : apps_) {
+        uint64_t id = it.first;
+        const auto &app = it.second;
+
+        snprintf(buffer, sizeof(buffer), "App: 0x%016" PRIx64 "\n", id);
+        result.append(buffer);
+        if (app->loaded) {
+            snprintf(buffer, sizeof(buffer),
+                "  Version: 0x%08" PRIx32 "\n"
+                "  flashAddr: 0x%08" PRIx32 "\n"
+                "  Running: %s\n",
+                app->version,
+                app->flashAddr,
+                app->running ? "true" : "false");
+            result.append(buffer);
+
+            if (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+                snprintf(buffer, sizeof(buffer),
+                    "  flashUse: %d\n"
+                    "  CRC: 0x%08" PRIx32 "\n",
+                    app->flashUse,
+                    app->crc);
+                result.append(buffer);
+            }
+
+            if (app->running) {
+                snprintf(buffer, sizeof(buffer),
+                    "  TID: %04x\n"
+                    "  ramUse: %d\n",
+                    app->tid,
+                    app->ramUse);
+                result.append(buffer);
+            }
+
+            if (app->chre) {
+                snprintf(buffer, sizeof(buffer), "  CHRE: %d.%d\n",
+                    app->chre_major, app->chre_minor);
+                result.append(buffer);
+            }
+        }
+
+        if (app->cached_napp) {
+            snprintf(buffer, sizeof(buffer),
+                "  Cached Version: 0x%08" PRIx32 "\n"
+                "  Cached Start: %s\n"
+                "  Cached CRC: 0x%08" PRIx32 "\n",
+                app->cached_version,
+                app->cached_start ? "true" : "false",
+                app->cached_crc);
+            result.append(buffer);
+        }
+    }
+}
+
+bool SystemComm::AppManager::saveApps()
+{
+    mkdir(CHRE_APP_DIR, CHRE_APP_DIR_PERMS);
+    File saved_apps_file(CHRE_APP_SETTINGS, "w");
+    std::shared_ptr<Json::Value> appsObject(new Json::Value);
+    status_t err;
+
+    if ((err = saved_apps_file.initCheck()) != OK) {
+        ALOGW("saved_apps file open (w) failed %d (%s)",
+              err,
+              strerror(-err));
+        return false;
+    }
+
+    for (auto &it : apps_) {
+        uint64_t id = it.first;
+        const auto &app = it.second;
+
+        if (app->cached_napp) {
+            char hexId[17];
+            snprintf(hexId, sizeof(hexId), "%016" PRIX64, id);
+            Json::Value array(Json::arrayValue);
+            array[0] = app->cached_version;
+            array[1] = app->cached_start;
+            array[2] = app->cached_crc;
+            (*appsObject)[hexId] = array;
+        }
+    }
+
+    // Write the JSON string to disk.
+    Json::StyledWriter writer;
+    std::string serializedSettings(writer.write(*appsObject));
+    size_t size = serializedSettings.size();
+    if ((err = saved_apps_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) {
+        ALOGW("saved_apps file write failed %d (%s)",
+              err,
+              strerror(-err));
+        return false;
+    }
+
+    return true;
+}
+
+bool SystemComm::AppManager::restoreApps()
+{
+    File saved_apps_file(CHRE_APP_SETTINGS, "r");
+    std::shared_ptr<Json::Value> appsObject;
+    status_t err;
+
+    if ((err = saved_apps_file.initCheck()) != OK) {
+        ALOGW("saved_apps file open (r) failed %d (%s)",
+              err,
+              strerror(-err));
+        return false;
+    }
+
+    off64_t size = saved_apps_file.seekTo(0, SEEK_END);
+    saved_apps_file.seekTo(0, SEEK_SET);
+
+    if (size > 0) {
+        char *buf = (char *)malloc(size);
+        CHECK_EQ(saved_apps_file.read(buf, size), (ssize_t)size);
+
+        std::string str(buf);
+        std::shared_ptr<Json::Value> in(new Json::Value);
+        Json::Reader reader;
+        bool valid = reader.parse(str, *in);
+        free(buf);
+
+        if (valid && in->isObject()) {
+            appsObject = in;
+        }
+    }
+
+    if (appsObject == nullptr) {
+        appsObject = std::shared_ptr<Json::Value>(new Json::Value(Json::objectValue));
+    }
+
+    Json::Value::Members apps = appsObject->getMemberNames();
+    for (auto &it : apps) {
+        Json::Value &val = (*appsObject)[it];
+        if (val.isArray()) {
+            uint32_t version = val[0].asUInt();
+            uint32_t start = val[1].asUInt();
+            uint32_t crc = val[2].asUInt();
+
+            uint64_t id = strtoull(it.c_str(), nullptr, 16);
+            apps_[id] = std::unique_ptr<AppData>(new AppData);
+            apps_[id]->loaded = false;
+            apps_[id]->running = false;
+            apps_[id]->chre = false;
+            apps_[id]->cached_napp = true;
+            apps_[id]->cached_version = version;
+            apps_[id]->cached_start = start;
+            apps_[id]->cached_crc = crc;
+        }
+    }
+
+    return true;
+}
+
+bool SystemComm::AppManager::eraseApps()
+{
+    for (auto it=apps_.begin(); it != apps_.end();) {
+        if (!it->second->cached_napp)
+            it = apps_.erase(it);
+        else {
+            it->second->loaded = false;
+            it->second->running = false;
+            it->second->chre = false;
+            ++it;
+        }
+    }
+
+    return true;
+}
+
+bool SystemComm::AppManager::writeApp(hub_app_name_t &appName, const uint8_t *data, int32_t len)
+{
+    mkdir(CHRE_APP_DIR, CHRE_APP_DIR_PERMS);
+    char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+    snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
 
-    return sendToSystem(buf.getData(), buf.getPos());
+    int fd = open(file, O_CREAT | O_WRONLY | O_TRUNC, CHRE_APP_FILE_PERMS);
+    if (fd == -1)
+        return false;
+
+    if (write(fd, data, len) == len) {
+        close(fd);
+        return true;
+    } else {
+        close(fd);
+        return false;
+    }
 }
 
-int SystemComm::doHandleRx(const nano_message *msg)
+int32_t SystemComm::AppManager::readApp(hub_app_name_t &appName, void **data)
 {
+    char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+    snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
+
+    int32_t ret = -1;
+    *data = nullptr;
+    int fd = open(file, O_RDONLY);
+
+    if (fd >= 0) {
+        struct stat sb;
+        if (fstat(fd, &sb) == 0) {
+            *data = malloc(sb.st_size);
+            if (*data != nullptr && read(fd, *data, sb.st_size) == sb.st_size)
+                ret = sb.st_size;
+            else {
+                free(*data);
+                *data = nullptr;
+            }
+        }
+        close(fd);
+    }
+    return ret;
+}
+
+bool SystemComm::AppManager::cmpApp(hub_app_name_t &appName, const uint8_t *data, uint32_t len)
+{
+    char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+    snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
+
+    if (!isAppLoaded(appName))
+        return false;
+
+    if ((!apps_[appName.id]->cached_napp) ||
+        (apps_[appName.id]->crc != apps_[appName.id]->cached_crc))
+        return false;
+
+    bool success = false;
+    int fd = open(file, O_RDONLY);
+
+    if (fd >= 0) {
+        struct stat sb;
+        if (fstat(fd, &sb) == 0 && sb.st_size == len) {
+            void *buf = malloc(len);
+            if (buf != nullptr && read(fd, buf, sb.st_size) == sb.st_size && memcmp(data, buf, len) == 0)
+                success = true;
+            free(buf);
+        }
+        close(fd);
+    }
+    return success;
+}
+
+uint32_t SystemComm::AppManager::readNanohubAppInfo(MessageBuf &buf)
+{
+    hub_app_name_t name;
+
+    uint8_t tag, len;
+    uint32_t ramUse = 0;
+    bool ramUseValid = true;
+
+    // first tag must be the appid
+    if (buf.getRoom() < 2 + sizeof(name.id)) {
+        ALOGE("%s: failed to read object", __func__);
+        return 0;
+    }
+
+    tag = buf.readU8();
+    len = buf.readU8();
+    if (tag != NANOHUB_HAL_APP_INFO_APPID || len != sizeof(name.id)) {
+        ALOGE("%s: invalid first tag: %d", __func__, tag);
+        return 0;
+    }
+
+    readAppName(buf, name);
+    if (!isAppPresent(name)) {
+        apps_[name.id] = std::unique_ptr<AppData>(new AppData);
+        apps_[name.id]->loaded = false;
+        apps_[name.id]->chre = false;
+        apps_[name.id]->running = false;
+        apps_[name.id]->cached_napp = false;
+    }
+    const auto &app = apps_[name.id];
+
+    while (buf.getRoom() >= 2) {
+        tag = buf.readU8();
+        len = buf.readU8();
+        if (buf.getRoom() >= len) {
+            switch(tag) {
+            case NANOHUB_HAL_APP_INFO_CRC:
+                if (len == 0)
+                    app->crc = 0;
+                else if (len == sizeof(app->crc))
+                    app->crc = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_TID:
+                if (len == 0) {
+                    app->tid = NANOHUB_TID_UNKNOWN;
+                    ramUseValid = false;
+                    app->loaded = true;
+                    app->running = false;
+                } else if (len  == sizeof(app->tid)) {
+                    app->tid = buf.readU32();
+                    app->loaded = true;
+                    app->running = true;
+                } else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_VERSION:
+                if (len == sizeof(app->version))
+                    app->version = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_ADDR:
+                if (len == sizeof(app->flashAddr))
+                    app->flashAddr = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_SIZE:
+                if (len == 0)
+                    app->flashUse = NANOHUB_MEM_SZ_UNKNOWN;
+                else if (len == sizeof(app->flashUse))
+                    app->flashUse = buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_HEAP:
+            case NANOHUB_HAL_APP_INFO_DATA:
+            case NANOHUB_HAL_APP_INFO_BSS:
+                if (len == 0)
+                    ramUseValid = false;
+                else if (len == sizeof(uint32_t))
+                    ramUse += buf.readU32();
+                else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_CHRE_MAJOR:
+                if (len == 0)
+                    app->chre = false;
+                else if (len == sizeof(app->chre_major)) {
+                    app->chre = true;
+                    app->chre_major = buf.readU8();
+                } else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_CHRE_MINOR:
+                if (len == 0)
+                    app->chre = false;
+                else if (len == sizeof(app->chre_minor)) {
+                    app->chre = true;
+                    app->chre_minor = buf.readU8();
+                } else
+                    buf.readRaw(len);
+                break;
+            case NANOHUB_HAL_APP_INFO_END:
+                if (len != 0 || buf.getRoom() != 0) {
+                    ALOGE("%s: failed to read object", __func__);
+                    return 0;
+                }
+                break;
+            default:
+                ALOGI("%s: unsupported tag: %d", __func__, tag);
+                buf.readRaw(len);
+                break;
+            }
+        } else {
+            ALOGE("%s: failed to read object", __func__);
+            return 0;
+        }
+    }
+
+    if (buf.getRoom() != 0) {
+        ALOGE("%s: failed to read object", __func__);
+        return 0;
+    }
+
+    if (ramUseValid)
+        app->ramUse = ramUse;
+    else
+        app->ramUse = NANOHUB_MEM_SZ_UNKNOWN;
+
+    return app->flashAddr +
+        (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN ? ((app->flashUse+3)&~3) : 4);
+}
+
+void SystemComm::AppManager::sendAppInfoToApp(uint32_t transactionId) {
+    std::vector<hub_app_info> appInfo;
+    for (auto &it : apps_) {
+        uint64_t id = it.first;
+        const auto &app = it.second;
+
+        // TODO: Still have some non-chre apps that need to be reported
+        // if (!app->chre || !app->running || app->flashUse == NANOHUB_MEM_SZ_UNKNOWN)
+        if (!app->running || app->flashUse == NANOHUB_MEM_SZ_UNKNOWN)
+            continue;
+
+        hub_app_info info;
+        info.app_name = { .id = id };
+        info.version = app->version;
+        info.num_mem_ranges = 0;
+        if (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+            mem_range_t &range = info.mem_usage[info.num_mem_ranges++];
+            range.type = HUB_MEM_TYPE_MAIN;
+            range.total_bytes = app->flashUse;
+        }
+        if (app->ramUse != NANOHUB_MEM_SZ_UNKNOWN) {
+            mem_range_t &range = info.mem_usage[info.num_mem_ranges++];
+            range.type = HUB_MEM_TYPE_RAM;
+            range.total_bytes = app->ramUse;
+        }
+
+        appInfo.push_back(info);
+    }
+    sendToApp(CONTEXT_HUB_QUERY_APPS, transactionId,
+              static_cast<const void *>(appInfo.data()),
+              appInfo.size() * sizeof(appInfo[0]));
+}
+
+int SystemComm::AppManager::getAppsToStart(std::vector<hub_app_name_t> &apps)
+{
+    int cnt = 0;
+    apps.clear();
+
+    for (auto &it : apps_) {
+        uint64_t id = it.first;
+        const auto &app = it.second;
+
+        if (app->cached_napp && app->cached_start && app->loaded &&
+            !app->running && app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+            apps.push_back({ .id = id });
+            cnt++;
+        }
+    }
+
+    return cnt;
+}
+
+int SystemComm::doHandleRx(uint64_t appId, uint32_t transactionId, const char *data, int len, bool chre)
+{
+    bool reboot = false;
+    uint32_t rebootStatus;
     //we only care for messages from HostIF
-    if (msg->hdr.appId != mHostIfAppName.id)
+    if (appId != mHostIfAppName.id)
         return 1;
 
     //they must all be at least 1 byte long
-    if (!msg->hdr.len) {
+    if (!len) {
         return -EINVAL;
     }
-    MessageBuf buf(reinterpret_cast<const char*>(msg->data), msg->hdr.len);
+    MessageBuf buf(data, len);
     if (NanoHub::messageTracingEnabled()) {
-        dumpBuffer("SYS -> HAL", mHostIfAppName, 0, buf.getData(), buf.getSize());
+        dumpBuffer("SYS -> HAL", mHostIfAppName, transactionId, 0, buf.getData(), buf.getSize());
     }
-    int status = mSessions.handleRx(buf);
+    int status = mSessions.handleRx(buf, transactionId, mAppManager, chre, reboot, rebootStatus);
     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);
+                   mHostIfAppName, transactionId, 0, buf.getData(), buf.getSize(), status);
         status = status > 0 ? 0 : status;
     }
+    if (reboot) {
+        hub_message_t msg =
+        {
+            .app_name.id = appId,
+            .message_type = CONTEXT_HUB_START_APPS,
+            .message_len = sizeof(rebootStatus),
+            .message = &rebootStatus,
+        };
+
+        status = doHandleTx(&msg, 0xFFFFFFFF);
+    }
 
     return status;
 }
 
-int SystemComm::SessionManager::handleRx(MessageBuf &buf)
+void SystemComm::doDumpAppInfo(std::string &result)
+{
+    mAppManager.dumpAppInfo(result);
+}
+
+int SystemComm::SessionManager::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre, bool &reboot, uint32_t &rebootStatus)
 {
     int status = 1;
     std::unique_lock<std::mutex> lk(lock);
@@ -605,36 +1420,40 @@ int SystemComm::SessionManager::handleRx(MessageBuf &buf)
             continue;
         }
         Session *session = pos->second;
-        status = session->handleRx(buf);
+        status = session->handleRx(buf, transactionId, appManager, chre);
         if (status < 0) {
             session->complete();
         }
     }
 
-    NanohubRsp rsp(buf);
-    if (rsp.cmd == NANOHUB_REBOOT) {
-        // if this is reboot notification, kill all sessions
-        for (auto pos = sessions_.begin(); pos != sessions_.end(); next(pos)) {
-            if (!isActive(pos)) {
-                continue;
+    NanohubRsp rsp(buf, transactionId, chre);
+    if (rsp.mCmd == NANOHUB_HAL_SYS_MGMT) {
+        uint8_t cmd = buf.readU8();
+
+        if (cmd == NANOHUB_HAL_SYS_MGMT_REBOOT) {
+            // if this is reboot notification, kill all sessions
+            for (auto pos = sessions_.begin(); pos != sessions_.end(); next(pos)) {
+                if (!isActive(pos)) {
+                    continue;
+                }
+                Session *session = pos->second;
+                session->abort(-EINTR);
             }
-            Session *session = pos->second;
-            session->abort(-EINTR);
-        }
-        lk.unlock();
-        // log the reboot event, if not handled
-        if (status > 0) {
-            ALOGW("Nanohub reboot status [UNSOLICITED]: %08" PRIX32, rsp.status);
-            status = 0;
+            lk.unlock();
+            // log the reboot event, if not handled
+            if (status > 0) {
+                ALOGW("Nanohub reboot status [UNSOLICITED]: %08" PRIX32, rsp.mStatus);
+                status = 0;
+            }
+            reboot = true;
+            rebootStatus = rsp.mStatus;
         }
-        // report to java apps
-        sendToApp(CONTEXT_HUB_OS_REBOOT, &rsp.status, sizeof(rsp.status));
     }
 
     return status;
 }
 
-int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hub_message_t *appMsg)
+int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager)
 {
     std::lock_guard<std::mutex> _l(lock);
 
@@ -645,7 +1464,7 @@ int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hu
 
     if (sessions_.count(id) == 0 && !session->isRunning()) {
         sessions_[id] = session;
-        int ret = session->setup(appMsg);
+        int ret = session->setup(appMsg, transactionId, appManager);
         if (ret < 0) {
             session->complete();
         }
@@ -654,14 +1473,14 @@ int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hu
     return -EBUSY;
 }
 
-int SystemComm::doHandleTx(const hub_message_t *appMsg)
+int SystemComm::doHandleTx(const hub_message_t *appMsg, uint32_t transactionId)
 {
     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);
+            status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mKeySession, appMsg, transactionId, mAppManager);
             if (status < 0) {
                 break;
             }
@@ -671,21 +1490,25 @@ int SystemComm::doHandleTx(const hub_message_t *appMsg)
                 break;
             }
         }
-        status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg);
+        status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg, transactionId, mAppManager);
         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);
+        status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg, transactionId, mAppManager);
         break;
 
     case CONTEXT_HUB_QUERY_APPS:
-        status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_APPS, &mAppInfoSession, appMsg);
+        mAppManager.sendAppInfoToApp(transactionId);
         break;
 
     case CONTEXT_HUB_QUERY_MEMORY:
-        status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_MEMORY, &mMemInfoSession, appMsg);
+        status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_MEMORY, &mMemInfoSession, appMsg, transactionId, mAppManager);
+        break;
+
+    case CONTEXT_HUB_START_APPS:
+        status = mSessions.setup_and_add(CONTEXT_HUB_START_APPS, &mAppMgmtSession, appMsg, transactionId, mAppManager);
         break;
 
     default:
diff --git a/contexthubhal/system_comms.h b/contexthubhal/system_comms.h
index 6991457e..46ee944e 100644
--- a/contexthubhal/system_comms.h
+++ b/contexthubhal/system_comms.h
@@ -19,6 +19,7 @@
 
 #include <utils/Condition.h>
 
+#include <chrono>
 #include <condition_variable>
 #include <map>
 #include <mutex>
@@ -35,35 +36,74 @@
 #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)
+#define NANOHUB_HAL_APP_MGMT      0x10 // (char cmd, u64 appId, u64 appMsk) -> (int errno, u32 results)
+
+#define NANOHUB_HAL_APP_MGMT_START      0
+#define NANOHUB_HAL_APP_MGMT_STOP       1
+#define NANOHUB_HAL_APP_MGMT_UNLOAD     2
+#define NANOHUB_HAL_APP_MGMT_DELETE     3
+
+#define NANOHUB_HAL_SYS_MGMT      0x11 // (char cmd) -> (int errno)
+
+#define NANOHUB_HAL_SYS_MGMT_ERASE      0
+#define NANOHUB_HAL_SYS_MGMT_REBOOT     1
+
+#define NANOHUB_HAL_APP_INFO      0x12
+
+#define NANOHUB_HAL_APP_INFO_APPID          0x00
+#define NANOHUB_HAL_APP_INFO_CRC            0x01
+#define NANOHUB_HAL_APP_INFO_TID            0x02
+#define NANOHUB_HAL_APP_INFO_VERSION        0x03
+#define NANOHUB_HAL_APP_INFO_ADDR           0x04
+#define NANOHUB_HAL_APP_INFO_SIZE           0x05
+#define NANOHUB_HAL_APP_INFO_HEAP           0x06
+#define NANOHUB_HAL_APP_INFO_DATA           0x07
+#define NANOHUB_HAL_APP_INFO_BSS            0x08
+#define NANOHUB_HAL_APP_INFO_CHRE_MAJOR     0x09
+#define NANOHUB_HAL_APP_INFO_CHRE_MINOR     0x0A
+#define NANOHUB_HAL_APP_INFO_END            0xFF
+
+#define NANOHUB_HAL_SYS_INFO      0x13
+
+#define NANOHUB_HAL_SYS_INFO_HEAP_FREE      0x0F
+#define NANOHUB_HAL_SYS_INFO_RAM_SIZE       0x12
+#define NANOHUB_HAL_SYS_INFO_EEDATA_SIZE    0x13
+#define NANOHUB_HAL_SYS_INFO_EEDATA_FREE    0x14
+#define NANOHUB_HAL_SYS_INFO_CODE_SIZE      0x15
+#define NANOHUB_HAL_SYS_INFO_CODE_FREE      0x16
+#define NANOHUB_HAL_SYS_INFO_SHARED_SIZE    0x17
+#define NANOHUB_HAL_SYS_INFO_SHARED_FREE    0x18
+#define NANOHUB_HAL_SYS_INFO_END            0xFF
+
+#define NANOHUB_HAL_KEY_INFO      0x14
+#define NANOHUB_HAL_START_UPLOAD  0x16
+#define NANOHUB_HAL_CONT_UPLOAD   0x17
+#define NANOHUB_HAL_FINISH_UPLOAD 0x18
+
+#define NANOHUB_HAL_UPLOAD_ACCEPTED         0
+#define NANOHUB_HAL_UPLOAD_WAIT             1
+#define NANOHUB_HAL_UPLOAD_RESEND           2
+#define NANOHUB_HAL_UPLOAD_RESTART          3
+#define NANOHUB_HAL_UPLOAD_CANCEL           4
+#define NANOHUB_HAL_UPLOAD_CANCEL_NO_RETRY  5
+#define NANOHUB_HAL_UPLOAD_NO_SPACE         6
 
 #define NANOHUB_APP_NOT_LOADED  (-1)
 #define NANOHUB_APP_LOADED      (0)
 
 #define NANOHUB_UPLOAD_CHUNK_SZ_MAX 64
 #define NANOHUB_MEM_SZ_UNKNOWN      0xFFFFFFFFUL
+#define NANOHUB_TID_UNKNOWN         0xFFFFFFFFUL
+
+#define CONTEXT_HUB_START_APPS      8
 
 namespace android {
 
 namespace nanohub {
 
-int system_comms_handle_rx(const nano_message *msg);
+int system_comms_handle_rx(const nano_message_raw *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 MgmtStatus {
     union {
         uint32_t value;
@@ -87,9 +127,10 @@ struct NanohubMemInfo {
 } __attribute__((packed));
 
 struct NanohubRsp {
-    uint32_t cmd;
-    int32_t status;
-    explicit NanohubRsp(MessageBuf &buf, bool no_status = false);
+    uint32_t mCmd;
+    uint32_t mTransactionId;
+    int32_t mStatus;
+    explicit NanohubRsp(MessageBuf &buf, uint32_t transactionId, bool chre);
 };
 
 inline bool operator == (const hub_app_name_t &a, const hub_app_name_t &b) {
@@ -103,6 +144,8 @@ inline bool operator != (const hub_app_name_t &a, const hub_app_name_t &b) {
 class SystemComm {
 private:
 
+    class AppManager;
+
     /*
      * Nanohub HAL sessions
      *
@@ -117,8 +160,8 @@ private:
      */
     class ISession {
     public:
-        virtual int setup(const hub_message_t *app_msg) = 0;
-        virtual int handleRx(MessageBuf &buf) = 0;
+        virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &appManager) = 0;
+        virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre) = 0;
         virtual int getState() const = 0; // FSM state
         virtual int getStatus() const = 0; // execution status (result code)
         virtual void abort(int32_t) = 0;
@@ -173,8 +216,13 @@ private:
         }
         int wait() {
             std::unique_lock<std::mutex> lk(mDoneMutex);
-            mDoneCond.wait(lk, [this] { return mState == SESSION_DONE; });
-            return 0;
+            bool success = mDoneCond.wait_for(
+                    lk, std::chrono::seconds(30),
+                    [this] { return mState == SESSION_DONE; });
+            if (!success) {
+                ALOGE("Timed out waiting for response");
+            }
+            return success ? 0 : -1;
         }
         virtual int getState() const override {
             std::lock_guard<std::mutex> _l(mDoneMutex);
@@ -193,11 +241,17 @@ private:
     class AppMgmtSession : public Session {
         enum {
             TRANSFER = SESSION_USER,
+            QUERY_START,
+            START,
+            STOP_TRANSFER,
             FINISH,
             RUN,
+            STOP_RUN,
             RUN_FAILED,
             REBOOT,
+            ERASE_TRANSFER,
             MGMT,
+            INFO,
         };
         uint32_t mCmd; // LOAD_APP, UNLOAD_APP, ENABLE_APP, DISABLE_APP
         uint32_t mResult;
@@ -207,14 +261,21 @@ private:
         uint32_t mNextPos;
         uint32_t mErrCnt;
         hub_app_name_t mAppName;
-
-        int setupMgmt(const hub_message_t *appMsg, uint32_t cmd);
-        int handleTransfer(NanohubRsp &rsp);
-        int handleFinish(NanohubRsp &rsp);
-        int handleRun(NanohubRsp &rsp);
-        int handleRunFailed(NanohubRsp &rsp);
-        int handleReboot(NanohubRsp &rsp);
-        int handleMgmt(NanohubRsp &rsp);
+        uint32_t mFlashAddr;
+        std::vector<hub_app_name_t> mAppList;
+
+        int setupMgmt(const hub_message_t *appMsg, uint32_t transactionId, uint32_t cmd, AppManager &appManager);
+        int handleTransfer(NanohubRsp &rsp, MessageBuf &, AppManager &appManager);
+        int handleStopTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+        int handleQueryStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+        int handleStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+        int handleFinish(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+        int handleRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+        int handleStopRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+        int handleReboot(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+        int handleEraseTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+        int handleMgmt(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+        int handleInfo(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
     public:
         AppMgmtSession() {
             mCmd = 0;
@@ -223,34 +284,133 @@ private:
             mLen = 0;
             memset(&mAppName, 0, sizeof(mAppName));
         }
-        virtual int handleRx(MessageBuf &buf) override;
-        virtual int setup(const hub_message_t *app_msg) override;
+        virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre) override;
+        virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &appManager) override;
     };
 
     class MemInfoSession : public Session {
     public:
-        virtual int setup(const hub_message_t *app_msg) override;
-        virtual int handleRx(MessageBuf &buf) override;
+        virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &) override;
+        virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre) override;
     };
 
     class KeyInfoSession  : public Session {
         std::vector<uint8_t> mRsaKeyData;
-        int requestRsaKeys(void);
+        uint32_t mKeyNum;
+        uint32_t mKeyOffset;
+        int requestRsaKeys(uint32_t transactionId);
     public:
-        virtual int setup(const hub_message_t *) override;
-        virtual int handleRx(MessageBuf &buf) override;
+        virtual int setup(const hub_message_t *, uint32_t, AppManager &) override;
+        virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre) override;
         bool haveKeys() const {
             std::lock_guard<std::mutex> _l(mLock);
             return mRsaKeyData.size() > 0 && !isRunning();
         }
     };
 
-    class AppInfoSession : public Session {
-        std::vector<hub_app_info> mAppInfo;
-        int requestNext();
+    class AppManager {
+        struct AppData {
+            uint32_t version, flashUse, ramUse;
+            uint32_t tid, crc, flashAddr;
+            uint8_t chre_major, chre_minor;
+            bool chre, running, loaded;
+
+            bool cached_start, cached_napp;
+            uint32_t cached_version, cached_crc;
+        };
+
+        typedef std::map<uint64_t, std::unique_ptr<AppData>> AppMap;
+
+        AppMap apps_;
+
     public:
-        virtual int setup(const hub_message_t *) override;
-        virtual int handleRx(MessageBuf &buf) override;
+        AppManager() {
+            restoreApps();
+        }
+        void dumpAppInfo(std::string &result);
+        bool saveApps();
+        bool restoreApps();
+        bool eraseApps();
+        bool writeApp(hub_app_name_t &appName, const uint8_t *data, int32_t len);
+        int32_t readApp(hub_app_name_t &appName, void **data);
+        bool cmpApp(hub_app_name_t &appName, const uint8_t *data, uint32_t len);
+        uint32_t readNanohubAppInfo(MessageBuf &buf);
+        void sendAppInfoToApp(uint32_t transactionId);
+        int getAppsToStart(std::vector<hub_app_name_t> &apps);
+        bool setCachedCrc(hub_app_name_t &appName, uint32_t crc) {
+            if (!isAppPresent(appName))
+                return false;
+            else {
+                apps_[appName.id]->cached_napp = true;
+                apps_[appName.id]->cached_crc = crc;
+                apps_[appName.id]->cached_start = false;
+                saveApps();
+                return true;
+            }
+        }
+        bool clearCachedApp(hub_app_name_t &appName) {
+            if (!isAppPresent(appName))
+                return false;
+            else {
+                apps_[appName.id]->cached_napp = false;
+                apps_[appName.id]->cached_start = false;
+                saveApps();
+                return true;
+            }
+        }
+        bool clearRunning(hub_app_name_t &appName) {
+            if (!isAppLoaded(appName))
+                return false;
+            else {
+                apps_[appName.id]->running = false;
+                return true;
+            }
+        }
+
+        bool setCachedVersion(hub_app_name_t &appName, uint32_t version) {
+            if (!isAppPresent(appName))
+                return false;
+            else {
+                apps_[appName.id]->cached_version = version;
+                return true;
+            }
+        }
+        bool setCachedStart(hub_app_name_t &appName, bool start) {
+            if (!isAppPresent(appName))
+                return false;
+            else {
+                apps_[appName.id]->cached_start = start;
+                saveApps();
+                return true;
+            }
+        }
+        bool addNewApp(hub_app_name_t &appName, uint32_t version) {
+            if (isAppLoaded(appName))
+                return false;
+            else
+                apps_[appName.id] = std::unique_ptr<AppData>(new AppData);
+            apps_[appName.id]->loaded = false;
+            apps_[appName.id]->running = false;
+            apps_[appName.id]->chre = false;
+            apps_[appName.id]->cached_napp = false;
+            apps_[appName.id]->cached_version = version;
+            return true;
+        }
+        bool isAppPresent(hub_app_name_t &appName) {
+            return apps_.count(appName.id) != 0;
+        }
+        bool isAppLoaded(hub_app_name_t &appName) {
+            return apps_.count(appName.id) != 0 && apps_[appName.id]->loaded;
+        }
+        bool isAppRunning(hub_app_name_t &appName) {
+            return apps_.count(appName.id) != 0 && apps_[appName.id]->running;
+        }
+        uint32_t getFlashAddr(hub_app_name_t &appName) {
+            if (isAppPresent(appName))
+                return apps_[appName.id]->flashAddr;
+            else
+                return 0xFFFFFFFF;
+        }
     };
 
     class SessionManager {
@@ -269,8 +429,8 @@ private:
         }
 
     public:
-        int handleRx(MessageBuf &buf);
-        int setup_and_add(int id, Session *session, const hub_message_t *appMsg);
+        int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre, bool &reboot, uint32_t &rebootStatus);
+        int setup_and_add(int id, Session *session, const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager);
     } mSessions;
 
     const hub_app_name_t mHostIfAppName = {
@@ -286,29 +446,44 @@ private:
     SystemComm () = default;
     ~SystemComm() = default;
 
-    int doHandleTx(const hub_message_t *txMsg);
-    int doHandleRx(const nano_message *rxMsg);
+    int doHandleTx(const hub_message_t *txMsg, uint32_t transactionId);
+    int doHandleRx(uint64_t appId, uint32_t transactionId, const char *data, int len, bool chre);
+    void doDumpAppInfo(std::string &result);
+
+    int doHandleRx(const nano_message_raw *rxMsg) {
+        return doHandleRx(rxMsg->hdr.appId, 0, reinterpret_cast<const char*>(rxMsg->data), rxMsg->hdr.len, false);
+    }
+
+    int doHandleRx(const nano_message_chre *rxMsg) {
+        return doHandleRx(rxMsg->hdr.appId, rxMsg->hdr.appEventId, reinterpret_cast<const char*>(rxMsg->data), rxMsg->hdr.len, true);
+    }
 
-    static void sendToApp(uint32_t typ, const void *data, uint32_t len) {
+    static void sendToApp(uint32_t typ, uint32_t transactionId, const void *data, uint32_t len) {
         if (NanoHub::messageTracingEnabled()) {
-            dumpBuffer("HAL -> APP", get_hub_info()->os_app_name, typ, data, len);
+            dumpBuffer("HAL -> APP", get_hub_info()->os_app_name, transactionId, 0, data, len);
         }
-        NanoHub::sendToApp(HubMessage(&get_hub_info()->os_app_name, typ, data, len));
+        NanoHub::sendToApp(HubMessage(&get_hub_info()->os_app_name, typ, transactionId, ENDPOINT_BROADCAST, data, len));
     }
-    static int sendToSystem(const void *data, size_t len);
+    static int sendToSystem(const void *data, size_t len, uint32_t transactionId);
 
     KeyInfoSession mKeySession;
     AppMgmtSession mAppMgmtSession;
-    AppInfoSession mAppInfoSession;
     MemInfoSession mMemInfoSession;
+    AppManager     mAppManager;
 
 public:
-    static int handleTx(const hub_message_t *txMsg) {
-        return getSystem()->doHandleTx(txMsg);
+    static int handleTx(const hub_message_t *txMsg, uint32_t transactionId) {
+        return getSystem()->doHandleTx(txMsg, transactionId);
+    }
+    static int handleRx(const nano_message_raw *rxMsg) {
+        return getSystem()->doHandleRx(rxMsg);
     }
-    static int handleRx(const nano_message *rxMsg) {
+    static int handleRx(const nano_message_chre *rxMsg) {
         return getSystem()->doHandleRx(rxMsg);
     }
+    static void dumpAppInfo(std::string &result) {
+        return getSystem()->doDumpAppInfo(result);
+    }
 };
 
 }; // namespace nanohub
diff --git a/firmware/Android.mk b/firmware/Android.mk
index db2fb860..3f4b9095 100644
--- a/firmware/Android.mk
+++ b/firmware/Android.mk
@@ -88,6 +88,7 @@ LOCAL_WHOLE_STATIC_LIBRARIES := \
     libnanohub_os \
 
 LOCAL_STATIC_LIBRARIES := \
+    libnanohub_common_os \
     libnanomath_os \
     libnanolibc_os \
 
diff --git a/firmware/app/chre/chre.mk b/firmware/app/chre/chre.mk
index aed3ce78..4d4de740 100644
--- a/firmware/app/chre/chre.mk
+++ b/firmware/app/chre/chre.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 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.
@@ -26,8 +26,9 @@ include $(NANOHUB_DIR)/firmware_conf.mk
 
 CFLAGS += $(COMMON_FLAGS)
 
-# CHRE API 1.1
-BIN_POSTPROCESS_ARGS := -c 0x0101
+# CHRE API 1.2
+BIN_POSTPROCESS_ARGS := -c 0x0102
 CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/chre_api/include/chre_api
+CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/util/include
 
 include $(NANOHUB_DIR)/app/app.mk
diff --git a/firmware/app/chre/chre11.mk b/firmware/app/chre/chre11.mk
new file mode 100644
index 00000000..1051e78f
--- /dev/null
+++ b/firmware/app/chre/chre11.mk
@@ -0,0 +1,33 @@
+#
+# Copyright (C) 2017 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.
+#
+################################################################################
+#
+# NanoApp C/C++ Makefile Utils
+#
+################################################################################
+
+SRCS += $(NANOHUB_DIR)/app/chre/common/chre_app.c
+SRCS += $(NANOHUB_DIR)/app/chre/common/chre11_app_syscalls.c
+
+include $(NANOHUB_DIR)/firmware_conf.mk
+
+CFLAGS += $(COMMON_FLAGS)
+
+# CHRE API 1.1
+BIN_POSTPROCESS_ARGS := -c 0x0101
+CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/chre_api/legacy/v1_1
+
+include $(NANOHUB_DIR)/app/app.mk
diff --git a/firmware/app/chre/common/Android.mk b/firmware/app/chre/common/Android.mk
index 0c8302fb..6a996db5 100644
--- a/firmware/app/chre/common/Android.mk
+++ b/firmware/app/chre/common/Android.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 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.
@@ -16,6 +16,46 @@
 
 LOCAL_PATH := $(call my-dir)
 
+########################################################
+# CHRE 1.0 Library
+########################################################
+
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanochre10
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES :=      \
+    chre10_app.c          \
+    chre10_app_syscalls.c \
+
+LOCAL_STATIC_LIBRARIES += libnanobuiltins
+LOCAL_STATIC_LIBRARIES += libnanolibc
+
+include $(BUILD_NANOHUB_APP_STATIC_LIBRARY)
+
+########################################################
+# CHRE 1.1 Library
+########################################################
+
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanochre11
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES :=      \
+    chre_app.c          \
+    chre11_app_syscalls.c \
+
+LOCAL_STATIC_LIBRARIES += libnanobuiltins
+LOCAL_STATIC_LIBRARIES += libnanolibc
+
+include $(BUILD_NANOHUB_APP_STATIC_LIBRARY)
+
+########################################################
+# CHRE 1.2 Library
+########################################################
+
 include $(CLEAR_NANO_VARS)
 
 LOCAL_MODULE := libnanochre
diff --git a/firmware/app/chre/common/chre10_app.c b/firmware/app/chre/common/chre10_app.c
index deda37b5..6da4cc99 100644
--- a/firmware/app/chre/common/chre10_app.c
+++ b/firmware/app/chre/common/chre10_app.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 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.
diff --git a/firmware/app/chre/common/chre10_app_syscalls.c b/firmware/app/chre/common/chre10_app_syscalls.c
index 666bdaed..f72f4d45 100644
--- a/firmware/app/chre/common/chre10_app_syscalls.c
+++ b/firmware/app/chre/common/chre10_app_syscalls.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 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.
@@ -28,6 +28,9 @@
 #include <syscall.h>
 #include <syscall_defs.h>
 
+/* not defined in chre 1.0 */
+#define CHRE_HOST_ENDPOINT_BROADCAST  UINT16_C(0xFFFF)
+
 #define SYSCALL_CHRE_API(name) \
     SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_API, SYSCALL_CHRE_MAIN_API_ ## name)
 
@@ -113,23 +116,12 @@ bool chreSensorConfigure(uint32_t sensorHandle,
                        interval_lo, interval_hi, latency_lo, latency_hi);
 }
 
-bool chreSendEvent(uint16_t eventType, void *eventData,
-                   chreEventCompleteFunction *freeCallback,
-                   uint32_t targetInstanceId)
-{
-    return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
-}
-
-bool chreSendMessageToHost(void *message, uint32_t messageSize,
-                           uint32_t reservedMessageType,
-                           chreMessageFreeFunction *freeCallback)
-{
-    return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, reservedMessageType, freeCallback);
-}
-
 uint32_t chreGetApiVersion(void)
 {
-    return syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+    static uint32_t apiVersion = 0;
+    if (!apiVersion)
+        apiVersion = syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+    return apiVersion;
 }
 
 uint32_t chreGetVersion(void)
@@ -143,3 +135,23 @@ uint64_t chreGetPlatformId(void)
     (void)syscallDo1P(SYSCALL_CHRE_API(GET_PLATFORM_ID), &plat);
     return plat;
 }
+
+bool chreSendEvent(uint16_t eventType, void *eventData,
+                   chreEventCompleteFunction *freeCallback,
+                   uint32_t targetInstanceId)
+{
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+    else
+        return syscallDo4P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+}
+
+bool chreSendMessageToHost(void *message, uint32_t messageSize,
+                           uint32_t reservedMessageType,
+                           chreMessageFreeFunction *freeCallback)
+{
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, reservedMessageType, freeCallback);
+    else
+        return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, reservedMessageType, CHRE_HOST_ENDPOINT_BROADCAST, freeCallback);
+}
diff --git a/firmware/app/chre/common/chre11_app_syscalls.c b/firmware/app/chre/common/chre11_app_syscalls.c
new file mode 100644
index 00000000..4265bfa1
--- /dev/null
+++ b/firmware/app/chre/common/chre11_app_syscalls.c
@@ -0,0 +1,237 @@
+/*
+ * Copyright (C) 2017 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 <stdarg.h>
+
+#include <gpio.h>
+#include <osApi.h>
+#include <sensors.h>
+#include <seos.h>
+#include <util.h>
+
+/* CHRE syscalls */
+#include <chre.h>
+#include <chreApi.h>
+#include <syscall.h>
+#include <syscall_defs.h>
+
+#define SYSCALL_CHRE_API(name) \
+    SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_API, SYSCALL_CHRE_MAIN_API_ ## name)
+
+uint64_t chreGetAppId(void)
+{
+    uint64_t appId = 0;
+    (void)syscallDo1P(SYSCALL_CHRE_API(GET_APP_ID), &appId);
+    return appId;
+}
+
+uint32_t chreGetInstanceId(void)
+{
+    return syscallDo0P(SYSCALL_CHRE_API(GET_INST_ID));
+}
+
+uint64_t chreGetTime(void) {
+    uint64_t time_ns = 0;
+    (void)syscallDo1P(SYSCALL_CHRE_API(GET_TIME), &time_ns);
+    return time_ns;
+}
+
+int64_t chreGetEstimatedHostTimeOffset(void) {
+    int64_t time_ns = 0;
+    (void)syscallDo1P(SYSCALL_CHRE_API(GET_HOST_TIME_OFFSET), &time_ns);
+    return time_ns;
+}
+
+void chreLog(enum chreLogLevel level, const char *str, ...)
+{
+    va_list vl;
+
+    va_start(vl, str);
+    (void)syscallDo3P(SYSCALL_CHRE_API(LOG), level, str, VA_LIST_TO_INTEGER(vl));
+    va_end(vl);
+}
+
+uint32_t chreTimerSet(uint64_t duration, const void* cookie, bool oneShot)
+{
+    uint32_t dur_lo = duration;
+    uint32_t dur_hi = duration >> 32;
+    return syscallDo4P(SYSCALL_CHRE_API(TIMER_SET), dur_lo, dur_hi, cookie, oneShot);
+}
+
+bool chreTimerCancel(uint32_t timerId)
+{
+    return syscallDo1P(SYSCALL_CHRE_API(TIMER_CANCEL), timerId);
+}
+
+void chreAbort(uint32_t abortCode)
+{
+    (void)syscallDo1P(SYSCALL_CHRE_API(ABORT), abortCode);
+}
+
+void* chreHeapAlloc(uint32_t bytes)
+{
+    return (void *)syscallDo1P(SYSCALL_CHRE_API(HEAP_ALLOC), bytes);
+}
+
+void chreHeapFree(void* ptr)
+{
+    (void)syscallDo1P(SYSCALL_CHRE_API(HEAP_FREE), ptr);
+}
+
+bool chreSensorFindDefault(uint8_t sensorType, uint32_t *handle)
+{
+    return syscallDo2P(SYSCALL_CHRE_API(SENSOR_FIND_DEFAULT), sensorType, handle);
+}
+
+bool chreGetSensorInfo(uint32_t sensorHandle, struct chreSensorInfo *info)
+{
+    return syscallDo2P(SYSCALL_CHRE_API(SENSOR_GET_INFO), sensorHandle, info);
+}
+
+bool chreGetSensorSamplingStatus(uint32_t sensorHandle,
+                                 struct chreSensorSamplingStatus *status)
+{
+    return syscallDo2P(SYSCALL_CHRE_API(SENSOR_GET_STATUS), sensorHandle, status);
+}
+
+bool chreSensorConfigure(uint32_t sensorHandle,
+                         enum chreSensorConfigureMode mode,
+                         uint64_t interval, uint64_t latency)
+{
+    uint32_t interval_lo = interval;
+    uint32_t interval_hi = interval >> 32;
+    uint32_t latency_lo = latency;
+    uint32_t latency_hi = latency >> 32;
+    return syscallDo6P(SYSCALL_CHRE_API(SENSOR_CONFIG), sensorHandle, mode,
+                       interval_lo, interval_hi, latency_lo, latency_hi);
+}
+
+uint32_t chreGetApiVersion(void)
+{
+    static uint32_t apiVersion = 0;
+    if (!apiVersion)
+        apiVersion = syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+    return apiVersion;
+}
+
+uint32_t chreGetVersion(void)
+{
+    return syscallDo0P(SYSCALL_CHRE_API(GET_OS_VERSION));
+}
+
+uint64_t chreGetPlatformId(void)
+{
+    uint64_t plat = 0;
+    (void)syscallDo1P(SYSCALL_CHRE_API(GET_PLATFORM_ID), &plat);
+    return plat;
+}
+
+bool chreSendEvent(uint16_t eventType, void *eventData,
+                   chreEventCompleteFunction *freeCallback,
+                   uint32_t targetInstanceId)
+{
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+    else
+        return syscallDo4P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+}
+
+bool chreSendMessageToHost(void *message, uint32_t messageSize,
+                           uint32_t messageType,
+                           chreMessageFreeFunction *freeCallback)
+{
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+    else
+        return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, CHRE_HOST_ENDPOINT_BROADCAST, freeCallback);
+}
+
+bool chreSendMessageToHostEndpoint(void *message, size_t messageSize,
+                                   uint32_t messageType, uint16_t hostEndpoint,
+                                   chreMessageFreeFunction *freeCallback)
+{
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+    else
+        return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
+}
+
+bool chreGetNanoappInfoByAppId(uint64_t appId, struct chreNanoappInfo *info)
+{
+    uint32_t app_lo = appId;
+    uint32_t app_hi = appId >> 32;
+    return syscallDo3P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID), app_lo, app_hi, info);
+}
+
+bool chreGetNanoappInfoByInstanceId(uint32_t instanceId, struct chreNanoappInfo *info)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID), instanceId, info);
+}
+
+void chreConfigureNanoappInfoEvents(bool enable)
+{
+    syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_CFG_INFO), enable);
+}
+
+uint32_t chreGnssGetCapabilities(void)
+{
+    return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_GET_CAP));
+}
+
+bool chreGnssLocationSessionStartAsync(uint32_t minIntervalMs, uint32_t minTimeToNextFixMs, const void *cookie)
+{
+    return syscallDo3P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_LOC_START_ASYNC), minIntervalMs, minTimeToNextFixMs, cookie);
+}
+
+bool chreGnssLocationSessionStopAsync(const void *cookie)
+{
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC), cookie);
+}
+
+bool chreGnssMeasurementSessionStartAsync(uint32_t minIntervalMs, const void *cookie)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC), minIntervalMs, cookie);
+}
+
+bool chreGnssMeasurementSessionStopAsync(const void *cookie)
+{
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC), cookie);
+}
+
+uint32_t chreWifiGetCapabilities(void)
+{
+    return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_GET_CAP));
+}
+
+bool chreWifiConfigureScanMonitorAsync(bool enable, const void *cookie)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_CONF_SCAN_MON_ASYNC), enable, cookie);
+}
+
+bool chreWifiRequestScanAsync(const struct chreWifiScanParams *params, const void *cookie)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_REQ_SCAN_ASYNC), params, cookie);
+}
+
+uint32_t chreWwanGetCapabilities(void)
+{
+    return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CAP));
+}
+
+bool chreWwanGetCellInfoAsync(const void *cookie)
+{
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC), cookie);
+}
diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c
index 0eba2394..04829b61 100644
--- a/firmware/app/chre/common/chre_app.c
+++ b/firmware/app/chre/common/chre_app.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 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.
diff --git a/firmware/app/chre/common/chre_app_syscalls.c b/firmware/app/chre/common/chre_app_syscalls.c
index fc1bc8c3..4335fee3 100644
--- a/firmware/app/chre/common/chre_app_syscalls.c
+++ b/firmware/app/chre/common/chre_app_syscalls.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 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.
@@ -163,7 +163,10 @@ bool chreSendMessageToHostEndpoint(void *message, size_t messageSize,
                                    uint32_t messageType, uint16_t hostEndpoint,
                                    chreMessageFreeFunction *freeCallback)
 {
-    return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
+    if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+        return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+    else
+        return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
 }
 
 bool chreGetNanoappInfoByAppId(uint64_t appId, struct chreNanoappInfo *info)
@@ -183,6 +186,16 @@ void chreConfigureNanoappInfoEvents(bool enable)
     syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_CFG_INFO), enable);
 }
 
+void chreConfigureHostSleepStateEvents(bool enable)
+{
+    syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP), enable);
+}
+
+bool chreIsHostAwake(void)
+{
+    return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE));
+}
+
 uint32_t chreGnssGetCapabilities(void)
 {
     return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_GET_CAP));
@@ -208,6 +221,11 @@ bool chreGnssMeasurementSessionStopAsync(const void *cookie)
     return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC), cookie);
 }
 
+bool chreGnssConfigurePassiveLocationListener(bool enable)
+{
+    return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS), enable);
+}
+
 uint32_t chreWifiGetCapabilities(void)
 {
     return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_GET_CAP));
@@ -232,3 +250,25 @@ bool chreWwanGetCellInfoAsync(const void *cookie)
 {
     return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC), cookie);
 }
+
+bool chreAudioGetSource(uint32_t handle, struct chreAudioSource *audioSource)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_GET_SRC), handle, audioSource);
+}
+
+bool chreAudioConfigureSource(uint32_t handle, bool enable,
+                              uint64_t bufferDuration,
+                              uint64_t deliveryInterval)
+{
+    uint32_t duration_lo = bufferDuration;
+    uint32_t duration_hi = bufferDuration >> 32;
+    uint32_t interval_lo = deliveryInterval;
+    uint32_t interval_hi = deliveryInterval >> 32;
+
+    return syscallDo6P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_CONF_SRC), handle, enable, duration_lo, duration_hi, interval_lo, interval_hi);
+}
+
+bool chreAudioGetStatus(uint32_t handle, struct chreAudioSourceStatus *status)
+{
+    return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_GET_STATUS), handle, status);
+}
diff --git a/firmware/build/app_chre10_executable.mk b/firmware/build/app_chre10_executable.mk
new file mode 100644
index 00000000..43f87767
--- /dev/null
+++ b/firmware/build/app_chre10_executable.mk
@@ -0,0 +1,43 @@
+#
+# Copyright (C) 2017 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_MODULE_SUFFIX := .napp
+include $(NANOHUB_APP_CONFIG)
+
+my_variants := $(LOCAL_NANO_VARIANT_LIST)
+
+ifeq ($(strip $(my_variants)),)
+# default is to use all variants supported by this OS
+my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
+endif
+
+# mark the app as CHRE 1.0 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0100
+
+# add app-side CHRE implementation
+LOCAL_WHOLE_STATIC_LIBRARIES += \
+    libnanochre10               \
+
+# add standard libaries
+LOCAL_STATIC_LIBRARIES +=       \
+    libnanobuiltins             \
+    libnanolibc                 \
+    libnanolibm                 \
+
+LOCAL_C_INCLUDES +=                                     \
+    system/chre/chre_api/legacy/v1_0                    \
+
+$(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/app_chre11_executable.mk b/firmware/build/app_chre11_executable.mk
new file mode 100644
index 00000000..2112418a
--- /dev/null
+++ b/firmware/build/app_chre11_executable.mk
@@ -0,0 +1,43 @@
+#
+# Copyright (C) 2017 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_MODULE_SUFFIX := .napp
+include $(NANOHUB_APP_CONFIG)
+
+my_variants := $(LOCAL_NANO_VARIANT_LIST)
+
+ifeq ($(strip $(my_variants)),)
+# default is to use all variants supported by this OS
+my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
+endif
+
+# mark the app as CHRE 1.1 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0101
+
+# add app-side CHRE implementation
+LOCAL_WHOLE_STATIC_LIBRARIES += \
+    libnanochre11               \
+
+# add standard libaries
+LOCAL_STATIC_LIBRARIES +=       \
+    libnanobuiltins             \
+    libnanolibc                 \
+    libnanolibm                 \
+
+LOCAL_C_INCLUDES +=                                     \
+    system/chre/chre_api/legacy/v1_1                    \
+
+$(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/app_chre_executable.mk b/firmware/build/app_chre_executable.mk
index 009dc15c..da776821 100644
--- a/firmware/build/app_chre_executable.mk
+++ b/firmware/build/app_chre_executable.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 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.
@@ -24,8 +24,8 @@ ifeq ($(strip $(my_variants)),)
 my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
 endif
 
-# mark the app as CHRE 1.1 nanoapp
-LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0101
+# mark the app as CHRE 1.2 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0102
 
 # add app-side CHRE implementation
 LOCAL_WHOLE_STATIC_LIBRARIES += \
@@ -37,4 +37,8 @@ LOCAL_STATIC_LIBRARIES +=       \
     libnanolibc                 \
     libnanolibm                 \
 
+LOCAL_C_INCLUDES +=                                     \
+    system/chre/chre_api/include/chre_api               \
+    system/chre/util/include                            \
+
 $(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/config.mk b/firmware/build/config.mk
index 2776653b..423d9d0d 100644
--- a/firmware/build/config.mk
+++ b/firmware/build/config.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 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.
@@ -37,6 +37,8 @@ BUILD_NANOHUB_BL_EXECUTABLE := $(NANO_BUILD)/bl_executable.mk
 BUILD_NANOHUB_OS_EXECUTABLE := $(NANO_BUILD)/os_executable.mk
 BUILD_NANOHUB_OS_IMAGE := $(NANO_BUILD)/os_image.mk
 BUILD_NANOHUB_APP_EXECUTABLE := $(NANO_BUILD)/app_executable.mk
+BUILD_NANOHUB_APP_CHRE10_EXECUTABLE := $(NANO_BUILD)/app_chre10_executable.mk
+BUILD_NANOHUB_APP_CHRE11_EXECUTABLE := $(NANO_BUILD)/app_chre11_executable.mk
 BUILD_NANOHUB_APP_CHRE_EXECUTABLE := $(NANO_BUILD)/app_chre_executable.mk
 
 NANOAPP_POSTPROCESS := $(HOST_OUT_EXECUTABLES)/nanoapp_postprocess
diff --git a/firmware/firmware.mk b/firmware/firmware.mk
index 9e02634a..221ee0cb 100644
--- a/firmware/firmware.mk
+++ b/firmware/firmware.mk
@@ -85,9 +85,7 @@ SRCS_bl += os/core/bl.c
 #some help for bootloader
 SRCS_bl += os/core/printf.c
 
-ifndef PLATFORM_HAS_HARDWARE_CRC
 SRCS_os += ../lib/nanohub/softcrc.c
-endif
 
 #extra deps
 DEPS += $(wildcard inc/*.h)
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index c7002535..99e96ef7 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -15,13 +15,16 @@
  */
 
 #include "calibration/accelerometer/accel_cal.h"
+
 #include <errno.h>
+#include <inttypes.h>
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
-#include "calibration/magnetometer/mag_cal.h"
+
 #include "calibration/util/cal_log.h"
 
+// clang-format off
 #define KSCALE \
   0.101936799f         // Scaling from m/s^2 to g (0.101 = 1/(9.81 m/s^2)).
 #define KSCALE2 9.81f  // Scaling from g to m/s^2.
@@ -36,8 +39,9 @@
 #define MIN_TEMP 20.0f  // No Data is collected below 20 degree C.
 #define MAX_TEMP 45.0f  // No Data is collected above 45 degree C.
 #define TEMP_CUT 30     // Separation point for temperature buckets 30 degree C.
-#define EIGEN_RATIO 0.35  // EIGEN_RATIO (must be greater than 0.35).
-#define EIGEN_MAG 0.97    // Eigen value magnitude (must be greater than 0.97).
+#define EIGEN_RATIO 0.35f  // EIGEN_RATIO (must be greater than 0.35).
+#define EIGEN_MAG 0.97f    // Eigen value magnitude (must be greater than 0.97).
+#define ACCEL_NEW_BIAS_THRESHOLD (0.0f)  // Bias update detection threshold.
 #ifdef ACCEL_CAL_DBG_ENABLED
 #define TEMP_HIST_LOW \
   16  // Putting all Temp counts in first bucket for temp < 16 degree C.
@@ -47,8 +51,9 @@
 #endif
 #ifdef IMU_TEMP_DBG_ENABLED
 #define IMU_TEMP_DELTA_TIME_NANOS \
-  5000000000 // Printing every 5 seconds IMU temp.
+  5000000000   // Printing every 5 seconds IMU temp.
 #endif
+// clang-format on
 
 /////////// Start Debug //////////////////////
 
@@ -159,19 +164,29 @@ static void accelCalAlgoInit(struct AccelCalAlgo *acc, uint32_t fx,
                              uint32_t fxb, uint32_t fy, uint32_t fyb,
                              uint32_t fz, uint32_t fzb, uint32_t fle) {
   accelGoodDataInit(&acc->agd, fx, fxb, fy, fyb, fz, fzb, fle);
-  initKasa(&acc->akf);
+  kasaInit(&acc->akf);
+}
+
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc) {
+  return fabsf(acc->x_bias - acc->x_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+         fabsf(acc->y_bias - acc->y_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+         fabsf(acc->z_bias - acc->z_bias_new) > ACCEL_NEW_BIAS_THRESHOLD;
 }
 
 // Accel cal init.
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
-                  uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
-                  uint32_t fz, uint32_t fzb, uint32_t fle) {
+void accelCalInit(struct AccelCal *acc,
+                  const struct AccelCalParameters *parameters) {
   // Init core accel data.
-  accelCalAlgoInit(&acc->ac1[0], fx, fxb, fy, fyb, fz, fzb, fle);
-  accelCalAlgoInit(&acc->ac1[1], fx, fxb, fy, fyb, fz, fzb, fle);
+  accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb,
+                   parameters->fy, parameters->fyb, parameters->fz,
+                   parameters->fzb, parameters->fle);
+  accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb,
+                   parameters->fy, parameters->fyb, parameters->fz,
+                   parameters->fzb, parameters->fle);
 
   // Stillness Reset.
-  accelStillInit(&acc->asd, t0, n_s, th);
+  accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
 
 // Debug data init.
 #ifdef ACCEL_CAL_DBG_ENABLED
@@ -267,31 +282,6 @@ static int accelStillnessDetection(struct AccelStillDet *asd,
   return complete;
 }
 
-// Accumulate data for KASA fit.
-static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) {
-  // Run accumulators.
-  float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
-            asd->mean_z * asd->mean_z;
-
-  akf->acc_x += asd->mean_x;
-  akf->acc_y += asd->mean_y;
-  akf->acc_z += asd->mean_z;
-  akf->acc_w += w;
-
-  akf->acc_xx += asd->mean_x * asd->mean_x;
-  akf->acc_xy += asd->mean_x * asd->mean_y;
-  akf->acc_xz += asd->mean_x * asd->mean_z;
-  akf->acc_xw += asd->mean_x * w;
-
-  akf->acc_yy += asd->mean_y * asd->mean_y;
-  akf->acc_yz += asd->mean_y * asd->mean_z;
-  akf->acc_yw += asd->mean_y * w;
-
-  akf->acc_zz += asd->mean_z * asd->mean_z;
-  akf->acc_zw += asd->mean_z * w;
-  akf->nsamples += 1;
-}
-
 // Good data detection, sorting and accumulate the data for Kasa.
 static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
                          float temp) {
@@ -304,42 +294,42 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
     ac1->agd.nx += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Negative x bucket nxb.
   if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) {
     ac1->agd.nxb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Y bucket ny.
   if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) {
     ac1->agd.ny += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Negative y bucket nyb.
   if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) {
     ac1->agd.nyb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Z bucket nz.
   if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) {
     ac1->agd.nz += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Negative z bucket nzb.
   if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) {
     ac1->agd.nzb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // The leftover bucket nle.
   if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
@@ -348,7 +338,7 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
     ac1->agd.nle += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->akf, asd);
+    kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
   }
   // Checking if all buckets are full.
   if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb &&
@@ -357,32 +347,16 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
     //  Check if akf->nsamples is zero.
     if (ac1->akf.nsamples == 0) {
       agdReset(&ac1->agd);
-      magKasaReset(&ac1->akf);
+      kasaReset(&ac1->akf);
       complete = 0;
       return complete;
-    } else {
-      // Normalize the data to the sample numbers.
-      inv = 1.0f / ac1->akf.nsamples;
     }
 
-    ac1->akf.acc_x *= inv;
-    ac1->akf.acc_y *= inv;
-    ac1->akf.acc_z *= inv;
-    ac1->akf.acc_w *= inv;
+    // Normalize the data to the sample numbers.
+    kasaNormalize(&ac1->akf);
 
-    ac1->akf.acc_xx *= inv;
-    ac1->akf.acc_xy *= inv;
-    ac1->akf.acc_xz *= inv;
-    ac1->akf.acc_xw *= inv;
-
-    ac1->akf.acc_yy *= inv;
-    ac1->akf.acc_yz *= inv;
-    ac1->akf.acc_yw *= inv;
-
-    ac1->akf.acc_zz *= inv;
-    ac1->akf.acc_zw *= inv;
-
-    // Calculate the temp VAR and MEA.N
+    // Calculate the temp VAR and MEAN.
+    inv = 1.0f / ac1->akf.nsamples;
     ac1->agd.var_t =
         (ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv;
     ac1->agd.mean_t = ac1->agd.acc_t * inv;
@@ -395,7 +369,7 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
       ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
       ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
     agdReset(&ac1->agd);
-    magKasaReset(&ac1->akf);
+    kasaReset(&ac1->akf);
     complete = 0;
     return complete;
   }
@@ -485,14 +459,15 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
 #ifdef IMU_TEMP_DBG_ENABLED
   if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
     CAL_DEBUG_LOG("IMU Temp Data: ",
-                  ", %s%d.%02d,  %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n",
-                  CAL_ENCODE_FLOAT(temp, 2),
-                  (unsigned long long int)sample_time_nanos,
-                  CAL_ENCODE_FLOAT(acc->x_bias_new,5),
-                  CAL_ENCODE_FLOAT(acc->y_bias_new,5),
-                  CAL_ENCODE_FLOAT(acc->z_bias_new,5));
+                  ", " CAL_FORMAT_3DIGITS ",  %" PRIu64
+                  ", " CAL_FORMAT_6DIGITS_TRIPLET " \n",
+                  CAL_ENCODE_FLOAT(temp, 3),
+                  sample_time_nanos,
+                  CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+                  CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+                  CAL_ENCODE_FLOAT(acc->z_bias_new, 6));
     acc->temp_time_nanos = sample_time_nanos;
-    }
+  }
 #endif
 
   int temp_gate = 0;
@@ -523,7 +498,8 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
         float radius;
 
         // Grabbing the fit from the MAG cal.
-        magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius);
+        kasaFit(&acc->ac1[temp_gate].akf, &bias, &radius, G_NORM_MAX,
+                G_NORM_MIN);
 
         // If offset is too large don't take.
         if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF &&
@@ -567,238 +543,223 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
 
         // Resetting the structs for a new accel cal run.
         agdReset(&acc->ac1[temp_gate].agd);
-        magKasaReset(&acc->ac1[temp_gate].akf);
+        kasaReset(&acc->ac1[temp_gate].akf);
       }
     }
   }
 }
 
 #ifdef ACCEL_CAL_DBG_ENABLED
+
+// Local helper macro for printing log messages.
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_ACCEL_HISTORY                                           \
+  "%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d," \
+  "%s%d.%06d,%s%d.%06d,%s%d.%06d"
+#else
+#define CAL_FORMAT_ACCEL_HISTORY \
+  "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f"
+#endif  // CAL_NO_FLOAT_FORMAT_STRINGS
+
 // Debug Print Output
 void accelCalDebPrint(struct AccelCal *acc, float temp) {
   static int32_t kk = 0;
   if (++kk == 1000) {
     // X offset history last 10 values.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,11,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(x_off history)\n",
-        CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{11," CAL_FORMAT_ACCEL_HISTORY "}(x_off history)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
 
     // Y offset history last 10 values.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,12,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(y_off history)\n",
-        CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{12," CAL_FORMAT_ACCEL_HISTORY "}(y_off history)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
 
     // Z offset history last 10 values.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,13,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(z_off history)\n",
-        CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{13," CAL_FORMAT_ACCEL_HISTORY "}(z_off history)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
 
     // Temp history variation VAR of offset.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,14,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(VAR temp history)\n",
-        CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{14," CAL_FORMAT_ACCEL_HISTORY "}(VAR temp history)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
 
     // Temp mean history of offset.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,15,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(MEAN Temp history)\n",
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{15," CAL_FORMAT_ACCEL_HISTORY "}(MEAN Temp history)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
 
     // KASA radius history.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,16,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(radius)\n",
-        CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]", "{16," CAL_FORMAT_ACCEL_HISTORY "}(radius)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
     kk = 0;
   }
 
   if (kk == 750) {
     // Eigen Vector X.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, "
-        "7,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen x)\n",
-        CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 7," CAL_FORMAT_ACCEL_HISTORY "}(eigen x)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
     // Y.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, "
-        "8,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen y)\n",
-        CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 8," CAL_FORMAT_ACCEL_HISTORY "}(eigen y)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
     // Z.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, "
-        "9,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
-        "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen z)\n",
-        CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
-        CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 9," CAL_FORMAT_ACCEL_HISTORY "}(eigen z)\n",
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
+                  CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
     // Accel Time in ns.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL,10,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,}("
-        "timestamp ns)\n",
-        acc->adf.cal_time[0], acc->adf.cal_time[1], acc->adf.cal_time[2],
-        acc->adf.cal_time[3], acc->adf.cal_time[4], acc->adf.cal_time[5],
-        acc->adf.cal_time[6], acc->adf.cal_time[7], acc->adf.cal_time[8],
-        acc->adf.cal_time[9]);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{10,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+                  ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+                  "}(timestamp ns)\n",
+                  acc->adf.cal_time[0], acc->adf.cal_time[1],
+                  acc->adf.cal_time[2], acc->adf.cal_time[3],
+                  acc->adf.cal_time[4], acc->adf.cal_time[5],
+                  acc->adf.cal_time[6], acc->adf.cal_time[7],
+                  acc->adf.cal_time[8], acc->adf.cal_time[9]);
   }
 
   if (kk == 500) {
     // Total bucket count.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d,}(Total Bucket #)\n",
-        (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, (unsigned)acc->adf.nty,
-        (unsigned)acc->adf.ntyb, (unsigned)acc->adf.ntz,
-        (unsigned)acc->adf.ntzb, (unsigned)acc->adf.ntle);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d}(Total Bucket #)\n",
+                  (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb,
+                  (unsigned)acc->adf.nty, (unsigned)acc->adf.ntyb,
+                  (unsigned)acc->adf.ntz, (unsigned)acc->adf.ntzb,
+                  (unsigned)acc->adf.ntle);
     // Live bucket count lower.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
-        "lower)\n",
-        (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
-        (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
-        (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
-        (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+                  "lower)\n",
+                  (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
+                  (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
+                  (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
+                  (unsigned)acc->ac1[0].agd.nle,
+                  (unsigned)acc->ac1[0].akf.nsamples);
     // Live bucket count hogher.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
-        "higher)\n",
-        (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
-        (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
-        (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
-        (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+                  "higher)\n",
+                  (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
+                  (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
+                  (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
+                  (unsigned)acc->ac1[1].agd.nle,
+                  (unsigned)acc->ac1[1].akf.nsamples);
     // Offset used.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 3,%s%d.%06d, %s%d.%06d, %s%d.%06d, %2d,}(updated offset "
-        "x,y,z, total # of offsets)\n",
-        CAL_ENCODE_FLOAT(acc->x_bias, 6), CAL_ENCODE_FLOAT(acc->y_bias, 6),
-        CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 3,"CAL_FORMAT_6DIGITS_TRIPLET", %2d}(updated offset "
+                  "x,y,z, total # of offsets)\n",
+                  CAL_ENCODE_FLOAT(acc->x_bias, 6),
+                  CAL_ENCODE_FLOAT(acc->y_bias, 6),
+                  CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
     // Offset New.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 4,%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d,}(New offset "
-        "x,y,z, live temp)\n",
-        CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
-        CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
-        CAL_ENCODE_FLOAT(acc->z_bias_new, 6), CAL_ENCODE_FLOAT(temp, 6));
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 4," CAL_FORMAT_6DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS
+                  "}(New offset x,y,z, live temp)\n",
+                  CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+                  CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+                  CAL_ENCODE_FLOAT(acc->z_bias_new, 6),
+                  CAL_ENCODE_FLOAT(temp, 6));
     // Temp Histogram.
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "{MK_ACCEL, 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
-        "%7d, %7d,}(temp histo)\n",
-        (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
-        (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
-        (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
-        (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
-        (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
-        (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
-        (unsigned)acc->adf.t_hist[12]);
-    CAL_DEBUG_LOG(
-        "[BMI160]",
-        "M{K_ACCEL, 6,%7d, %7d, %7d,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
-        "%7d,}(temp histo)\n",
-        (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
-        (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
-        (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
-        (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
-        (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
-        (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+                  "%7d, %7d}(temp histo)\n",
+                  (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
+                  (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
+                  (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
+                  (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
+                  (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
+                  (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
+                  (unsigned)acc->adf.t_hist[12]);
+    CAL_DEBUG_LOG("[ACCEL_CAL]",
+                  "{ 6,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+                  "%7d}(temp histo)\n",
+                  (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
+                  (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
+                  (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
+                  (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
+                  (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
+                  (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
   }
 }
 #endif
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 1cfef614..3324875d 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -27,7 +27,7 @@
 
 #include <stdint.h>
 #include <sys/types.h>
-#include "calibration/magnetometer/mag_cal.h"
+#include "common/math/kasa.h"
 #include "common/math/mat.h"
 
 #ifdef __cplusplus
@@ -127,6 +127,25 @@ struct AccelCalAlgo {
   struct KasaFit akf;
 };
 
+// AccelCal algorithm parameters (see the AccelCal for details).
+struct AccelCalParameters {
+  // t0  -> Sets the time how long the accel has to be still in ns.
+  // n_s -> Defines the minimum number of samples for the stillness.
+  // th  -> Sets the threshold for the stillness VAR in (g rms)^2.
+  // fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+  //                             sphere cap (Bucket) is needed to reach full.
+  uint32_t t0;
+  uint32_t n_s;
+  uint32_t fx;
+  uint32_t fxb;
+  uint32_t fy;
+  uint32_t fyb;
+  uint32_t fz;
+  uint32_t fzb;
+  uint32_t fle;
+  float th;
+};
+
 // Complete accel calibration struct.
 struct AccelCal {
   struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS];
@@ -163,15 +182,15 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
                  float y, float z, float temp);
 
 /* This function initializes the accCalRun data struct.
- * t0     -> Sets the time how long the accel has to be still in ns.
- * n_s    -> Defines the minimum number of samples for the stillness.
- * th     -> Sets the threshold for the stillness VAR in (g rms)^2.
- * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
- *                             sphere cap (Bucket) is needed to reach full.
+ * [parameters]:
+ *   t0     -> Sets the time how long the accel has to be still in ns.
+ *   n_s    -> Defines the minimum number of samples for the stillness.
+ *   th     -> Sets the threshold for the stillness VAR in (g rms)^2.
+ *   fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+ *                               sphere cap (Bucket) is needed to reach full.
  */
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
-                  uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
-                  uint32_t fz, uint32_t fzb, uint32_t fle);
+void accelCalInit(struct AccelCal *acc,
+                  const struct AccelCalParameters *parameters);
 
 void accelCalDestroy(struct AccelCal *acc);
 
@@ -182,6 +201,9 @@ void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z);
 
 void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z);
 
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc);
+
 #ifdef ACCEL_CAL_DBG_ENABLED
 void accelCalDebPrint(struct AccelCal *acc, float temp);
 #endif
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
index d71ad9af..3fab81f8 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
@@ -14,52 +14,46 @@
  * limitations under the License.
  */
 
-#include "calibration/common/diversity_checker.h"
+#include "calibration/diversity_checker/diversity_checker.h"
 
 #include <errno.h>
 #include <stdarg.h>
 #include <stdio.h>
 #include <string.h>
+
 #include "common/math/vec.h"
 
 // Struct initialization.
-void diversityCheckerInit(
-    struct DiversityChecker* diverse_data,
-    size_t min_num_diverse_vectors,
-    size_t max_num_max_distance,
-    float var_threshold,
-    float max_min_threshold,
-    float local_field,
-    float threshold_tuning_param,
-    float max_distance_tuning_param) {
+void diversityCheckerInit(struct DiversityChecker* diverse_data,
+                          const struct DiversityCheckerParameters* parameters) {
   ASSERT_NOT_NULL(diverse_data);
 
   // Initialize parameters.
   diverse_data->threshold_tuning_param_sq =
-      (threshold_tuning_param * threshold_tuning_param);
+      (parameters->threshold_tuning_param * parameters->threshold_tuning_param);
   diverse_data->max_distance_tuning_param_sq =
-      (max_distance_tuning_param * max_distance_tuning_param);
+      (parameters->max_distance_tuning_param *
+       parameters->max_distance_tuning_param);
 
   // Updating the threshold and max_distance using assumed local field.
   // Testing for zero and negative local_field.
-  if (local_field <= 0) {
-    local_field = 1;
-  }
+  const float local_field =
+      (parameters->local_field <= 0.0f) ? 1.0f : parameters->local_field;
   diversityCheckerLocalFieldUpdate(diverse_data, local_field);
-  diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
+  diverse_data->min_num_diverse_vectors = parameters->min_num_diverse_vectors;
 
   // Checking for min_num_diverse_vectors = 0.
-  if (min_num_diverse_vectors < 1) {
+  if (parameters->min_num_diverse_vectors < 1) {
     diverse_data->min_num_diverse_vectors = 1;
   }
-  diverse_data->max_num_max_distance = max_num_max_distance;
-  diverse_data->var_threshold = var_threshold;
-  diverse_data->max_min_threshold = max_min_threshold;
+  diverse_data->max_num_max_distance = parameters->max_num_max_distance;
+  diverse_data->var_threshold = parameters->var_threshold;
+  diverse_data->max_min_threshold = parameters->max_min_threshold;
 
   // Setting the rest to zero.
   diversityCheckerReset(diverse_data);
 
-   // Debug Messages
+  // Debug Messages
 #ifdef DIVERSE_DEBUG_ENABLE
   memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg));
 #endif
@@ -69,8 +63,7 @@ void diversityCheckerInit(
 void diversityCheckerReset(struct DiversityChecker* diverse_data) {
   ASSERT_NOT_NULL(diverse_data);
   // Clear data memory.
-  memset(&diverse_data->diverse_data, 0,
-         sizeof(diverse_data->diverse_data));
+  memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data));
 
   // Resetting counters and data full bit.
   diverse_data->num_points = 0;
@@ -78,74 +71,82 @@ void diversityCheckerReset(struct DiversityChecker* diverse_data) {
   diverse_data->data_full = false;
 }
 
-void diversityCheckerUpdate(
-    struct DiversityChecker* diverse_data, float x, float y, float z) {
-  ASSERT_NOT_NULL(diverse_data);
-
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+                                      float x, float y, float z) {
   // Converting three single inputs to a vector.
-  const float vec[3] = {x, y, z};
+  const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
 
   // Result vector for vector difference.
-  float vec_diff[3];
+  float vec_diff[THREE_AXIS_DATA_DIM];
 
   // normSquared result (k)
   float norm_squared_result;
 
-  // If memory is full, no need to run through the data.
-  if (!diverse_data->data_full) {
-    size_t i;
-    // Running over all existing data points
-    for (i = 0; i < diverse_data->num_points; ++i) {
-      // v = v1 - v2;
-      vecSub(vec_diff,
-             &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
-             vec,
-             THREE_AXIS_DATA_DIM);
-
-      // k = |v|^2
-      norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
-
-      // if k < Threshold then leave the function.
-      if (norm_squared_result < diverse_data->threshold) {
-        return;
-      }
+  size_t i;
 
-      // if k > max_distance, count and leave the function.
-      if (norm_squared_result > diverse_data->max_distance) {
-        diverse_data->num_max_dist_violations++;
-        return;
-      }
+  // Running over all existing data points
+  for (i = 0; i < diverse_data->num_points; ++i) {
+    // v = v1 - v2;
+    vecSub(vec_diff, &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec,
+           THREE_AXIS_DATA_DIM);
+
+    // k = |v|^2
+    norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
+
+    // if k < Threshold then leave the function.
+    if (norm_squared_result < diverse_data->threshold) {
+      return false;
+    }
+
+    // if k > max_distance, count and leave the function.
+    if (norm_squared_result > diverse_data->max_distance) {
+      diverse_data->num_max_dist_violations++;
+      return false;
     }
+  }
+  return true;
+}
+
+void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
+                            float y, float z) {
+  ASSERT_NOT_NULL(diverse_data);
 
-    // If none of the above caused to leave the function, data is diverse.
-    // Notice that the first data vector will be stored no matter what.
-    memcpy(&diverse_data->
-           diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
-           vec,
-           sizeof(float) * THREE_AXIS_DATA_DIM);
-    // Count new data point.
-    diverse_data->num_points++;
-
-    // Setting data_full to 1, if memory is full.
-    if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
-      diverse_data->data_full = true;
+  // If memory is full, no need to run through the data.
+  if (!diverse_data->data_full) {
+    // diversityCheckerDataSet() returns true, if input data is diverse against
+    // the already stored.
+    if (diversityCheckerFindNearestPoint(diverse_data, x, y, z)) {
+      // Converting three single inputs to a vector.
+      const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
+
+      // Notice that the first data vector will be stored no matter what.
+      memcpy(
+          &diverse_data
+               ->diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
+          vec, sizeof(float) * THREE_AXIS_DATA_DIM);
+
+      // Count new data point.
+      diverse_data->num_points++;
+
+      // Setting data_full to true, if memory is full.
+      if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
+        diverse_data->data_full = true;
+      }
     }
   }
 }
 
 bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
-                                 float x_bias,
-                                 float y_bias,
-                                 float z_bias) {
+                                 float x_bias, float y_bias, float z_bias) {
   ASSERT_NOT_NULL(diverse_data);
   // If not enough diverse data points or max distance violations return false.
   if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
       diverse_data->num_max_dist_violations >=
-      diverse_data->max_num_max_distance) {
+          diverse_data->max_num_max_distance) {
     return false;
   }
-  float vec_bias[3] = {x_bias, y_bias, z_bias};
-  float vec_bias_removed[3];
+  float vec_bias[THREE_AXIS_DATA_DIM] = {x_bias, y_bias, z_bias};
+  float vec_bias_removed[THREE_AXIS_DATA_DIM];
   float norm_results;
   float acc_norm = 0.0f;
   float acc_norm_square = 0.0f;
@@ -155,8 +156,7 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
   for (i = 0; i < diverse_data->num_points; ++i) {
     // v = v1 - v_bias;
     vecSub(vec_bias_removed,
-           &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
-           vec_bias,
+           &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec_bias,
            THREE_AXIS_DATA_DIM);
 
     // norm = ||v||
@@ -164,7 +164,7 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
 
     // Accumulate for mean and VAR.
     acc_norm += norm_results;
-    acc_norm_square += norm_results * norm_results ;
+    acc_norm_square += norm_results * norm_results;
 
     if (i == 0) {
       min = norm_results;
@@ -206,11 +206,11 @@ void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
                                       float local_field) {
   if (local_field > 0) {
     // Updating threshold based on the local field information.
-    diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
-        (local_field * local_field);
+    diverse_data->threshold =
+        diverse_data->threshold_tuning_param_sq * (local_field * local_field);
 
     // Updating max distance based on the local field information.
     diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
-        (local_field * local_field);
+                                 (local_field * local_field);
   }
 }
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
index 5a245290..c38549b3 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.h
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
@@ -41,13 +41,18 @@
  * full. This has been done in order to save processing power.
  */
 
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
 
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
 
+#if defined(MAG_CAL_DEBUG_ENABLE) && !defined(DIVERSE_DEBUG_ENABLE)
+// Ensures that diversity messaging is set when mag_cal debugging is enabled.
+#define DIVERSE_DEBUG_ENABLE
+#endif  // MAG_CAL_DEBUG_ENABLE && !DIVERSE_DEBUG_ENABLE
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -68,6 +73,17 @@ struct DiversityDbg {
 };
 #endif
 
+// DiversityChecker parameters container.
+struct DiversityCheckerParameters {
+  float var_threshold;
+  float max_min_threshold;
+  float local_field;
+  float threshold_tuning_param;
+  float max_distance_tuning_param;
+  size_t min_num_diverse_vectors;
+  size_t max_num_max_distance;
+};
+
 // Main data struct.
 struct DiversityChecker {
   // Data memory.
@@ -82,7 +98,7 @@ struct DiversityChecker {
   // Threshold value that is used to check k against.
   float threshold;
 
-  // Threshold tuning paramter used to calculate threshold (k_algo):
+  // Threshold tuning parameter used to calculate threshold (k_algo):
   // threshold = threshold_tuning_param_sq * (local_field)^2.
   float threshold_tuning_param_sq;
 
@@ -97,7 +113,6 @@ struct DiversityChecker {
   bool data_full;
 
   // Setup variables for NormQuality check.
-
   size_t min_num_diverse_vectors;
   size_t max_num_max_distance;
   float var_threshold;
@@ -109,7 +124,7 @@ struct DiversityChecker {
 #endif
 };
 
-// Initialization of the function/struct, input:
+// Initialization of the function/struct, input parameters struct consists of:
 // min_num_diverse_vectors -> sets the gate for a minimum number of data points
 //                           in the memory
 // max_num_max_distance -> sets the value for a max distance violation number
@@ -123,16 +138,18 @@ struct DiversityChecker {
 // max_distance_tuning_param -> Max distance tuning parameter used to calculate
 //                             max_distance.
 void diversityCheckerInit(struct DiversityChecker* diverse_data,
-                          size_t min_num_diverse_vectors,
-                          size_t max_num_max_distance, float var_threshold,
-                          float max_min_threshold, float local_field,
-                          float threshold_tuning_param,
-                          float max_distance_tuning_param);
+                          const struct DiversityCheckerParameters* parameters);
 
 // Resetting the memory and the counters, leaves threshold and max_distance
 // as well as the setup variables for NormQuality check untouched.
 void diversityCheckerReset(struct DiversityChecker* diverse_data);
 
+// Checks if data point (x, y, z) is diverse against the diverse_data set.
+// Returns true when the input point is diverse.
+// Returns false when a maximum distance check is violated.
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+                                      float x, float y, float z);
+
 // Main function. Tests the data (x,y,z) against the memory if diverse and
 // stores it, if so.
 void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
@@ -149,9 +166,7 @@ void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
 // -> norm must be within a MAX/MIN window.
 // Returned value will only be true if all 4 gates are passed.
 bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
-                                 float x_bias,
-                                 float y_bias,
-                                 float z_bias);
+                                 float x_bias, float y_bias, float z_bias);
 
 // This function updates the threshold value and max distance value based on the
 // local field. This ensures a local field independent operation of the
@@ -165,4 +180,4 @@ void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
 }
 #endif
 
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 3179b0eb..90b25446 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -17,6 +17,7 @@
 #include "calibration/gyroscope/gyro_cal.h"
 
 #include <float.h>
+#include <inttypes.h>
 #include <math.h>
 #include <string.h>
 
@@ -40,6 +41,10 @@
 // A debug version label to help with tracking results.
 #define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
 
+// Parameters used for sample rate estimation.
+#define GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS (100)
+#define GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC (1.0f)
+
 // Debug log tag string used to identify debug report output data.
 #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
 #endif  // GYRO_CAL_DBG_ENABLED
@@ -103,26 +108,6 @@ enum DebugPrintData {
   MAG_STATS_TUNING
 };
 
-/*
- * Updates the running calculation of the gyro's mean sampling rate.
- *
- * Behavior:
- *   1)  If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
- *       calculation of the sampling rate is copied, and the function returns.
- *   2)  Else, if 'reset_stats' is 'true' then the local estimate is reset and
- *       the function returns.
- *   3)  Otherwise, the local estimate of the mean sampling rates is updated.
- *
- * INPUTS:
- *   sample_rate_estimator:  Pointer to the estimator data structure.
- *   debug_mean_sampling_rate_hz:  Pointer to the mean sampling rate to update.
- *   timestamp_nanos:  Time stamp (nanoseconds).
- *   reset_stats:  Flag that signals a reset of the sampling rate estimate.
- */
-static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
-                                   float* debug_mean_sampling_rate_hz,
-                                   uint64_t timestamp_nanos, bool reset_stats);
-
 // Updates the information used for debug printouts.
 static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
 
@@ -130,35 +115,13 @@ static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
 static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
                                   char* debug_tag,
                                   enum DebugPrintData print_data);
-
-// This conversion function is necessary for Nanohub firmware compilation (i.e.,
-// can't cast a uint64_t to a float directly). This conversion function was
-// copied from: /third_party/contexthub/firmware/src/floatRt.c
-static float floatFromUint64(uint64_t v)
-{
-    uint32_t hi = v >> 32, lo = v;
-
-    if (!hi) //this is very fast for cases where we fit into a uint32_t
-        return (float)lo;
-    else {
-        return ((float)hi) * 4294967296.0f + (float)lo;
-    }
-}
 #endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
 
 // Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
-                 uint64_t max_still_duration_nanos, float bias_x, float bias_y,
-                 float bias_z, uint64_t calibration_time_nanos,
-                 uint64_t window_time_duration_nanos, float gyro_var_threshold,
-                 float gyro_confidence_delta, float accel_var_threshold,
-                 float accel_confidence_delta, float mag_var_threshold,
-                 float mag_confidence_delta, float stillness_threshold,
-                 float stillness_mean_delta_limit,
-                 float temperature_delta_limit_celsius,
-                 bool gyro_calibration_enable) {
+void gyroCalInit(struct GyroCal* gyro_cal,
+                 const struct GyroCalParameters* parameters) {
   // Clear gyro_cal structure memory.
   memset(gyro_cal, 0, sizeof(struct GyroCal));
 
@@ -166,35 +129,38 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   // Gyro parameter input units are [rad/sec].
   // Accel parameter input units are [m/sec^2].
   // Magnetometer parameter input units are [uT].
-  gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold,
-                   gyro_confidence_delta);
-  gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold,
-                   accel_confidence_delta);
-  gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold,
-                   mag_confidence_delta);
+  gyroStillDetInit(&gyro_cal->gyro_stillness_detect,
+                   parameters->gyro_var_threshold,
+                   parameters->gyro_confidence_delta);
+  gyroStillDetInit(&gyro_cal->accel_stillness_detect,
+                   parameters->accel_var_threshold,
+                   parameters->accel_confidence_delta);
+  gyroStillDetInit(&gyro_cal->mag_stillness_detect,
+                   parameters->mag_var_threshold,
+                   parameters->mag_confidence_delta);
 
   // Reset stillness flag and start timestamp.
   gyro_cal->prev_still = false;
   gyro_cal->start_still_time_nanos = 0;
 
   // Set the min and max window stillness duration.
-  gyro_cal->min_still_duration_nanos = min_still_duration_nanos;
-  gyro_cal->max_still_duration_nanos = max_still_duration_nanos;
+  gyro_cal->min_still_duration_nanos = parameters->min_still_duration_nanos;
+  gyro_cal->max_still_duration_nanos = parameters->max_still_duration_nanos;
 
   // Sets the duration of the stillness processing windows.
-  gyro_cal->window_time_duration_nanos = window_time_duration_nanos;
+  gyro_cal->window_time_duration_nanos = parameters->window_time_duration_nanos;
 
   // Set the watchdog timeout duration.
   gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS;
 
   // Load the last valid cal from system memory.
-  gyro_cal->bias_x = bias_x;  // [rad/sec]
-  gyro_cal->bias_y = bias_y;  // [rad/sec]
-  gyro_cal->bias_z = bias_z;  // [rad/sec]
-  gyro_cal->calibration_time_nanos = calibration_time_nanos;
+  gyro_cal->bias_x = parameters->bias_x;  // [rad/sec]
+  gyro_cal->bias_y = parameters->bias_y;  // [rad/sec]
+  gyro_cal->bias_z = parameters->bias_z;  // [rad/sec]
+  gyro_cal->calibration_time_nanos = parameters->calibration_time_nanos;
 
   // Set the stillness threshold required for gyro bias calibration.
-  gyro_cal->stillness_threshold = stillness_threshold;
+  gyro_cal->stillness_threshold = parameters->stillness_threshold;
 
   // Current window end-time used to assist in keeping sensor data collection in
   // sync. Setting this to zero signals that sensor data will be dropped until a
@@ -202,13 +168,14 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   gyro_cal->stillness_win_endtime_nanos = 0;
 
   // Gyro calibrations will be applied (see, gyroCalRemoveBias()).
-  gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0);
+  gyro_cal->gyro_calibration_enable = (parameters->gyro_calibration_enable > 0);
 
   // Sets the stability limit for the stillness window mean acceptable delta.
-  gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;
+  gyro_cal->stillness_mean_delta_limit = parameters->stillness_mean_delta_limit;
 
   // Sets the min/max temperature delta limit for the stillness period.
-  gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;
+  gyro_cal->temperature_delta_limit_celsius =
+      parameters->temperature_delta_limit_celsius;
 
   // Ensures that the data tracking functionality is reset.
   gyroStillMeanTracker(gyro_cal, DO_RESET);
@@ -221,41 +188,47 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
     CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
   }
 
-  // Ensures that the gyro sampling rate estimate is reset.
-  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
-                         /*reset_stats=*/true);
+  // Initializes the gyro sampling rate estimator.
+  sampleRateEstimatorInit(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+                          GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS,
+                          GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC);
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
 // Void pointer in the gyro calibration data structure (doesn't do anything
 // except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) {
-  (void)gyro_cal;
-}
+void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
 
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
-                    float* bias_z, float* temperature_celsius) {
+                    float* bias_z, float* temperature_celsius,
+                    uint64_t* calibration_time_nanos) {
   *bias_x = gyro_cal->bias_x;
   *bias_y = gyro_cal->bias_y;
   *bias_z = gyro_cal->bias_z;
+  *calibration_time_nanos = gyro_cal->calibration_time_nanos;
   *temperature_celsius = gyro_cal->bias_temperature_celsius;
 }
 
 // Set an initial bias calibration value.
 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
-                    float bias_z, uint64_t calibration_time_nanos) {
+                    float bias_z, float temperature_celsius,
+                    uint64_t calibration_time_nanos) {
   gyro_cal->bias_x = bias_x;
   gyro_cal->bias_y = bias_y;
   gyro_cal->bias_z = bias_z;
   gyro_cal->calibration_time_nanos = calibration_time_nanos;
+  gyro_cal->bias_temperature_celsius = temperature_celsius;
 
 #ifdef GYRO_CAL_DBG_ENABLED
   CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
-                "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
-                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
+                "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+                ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+                CAL_ENCODE_FLOAT(bias_x * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(bias_y * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(bias_z * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(temperature_celsius, 3),
+                calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -298,8 +271,8 @@ void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Update the gyro sampling rate estimate.
-  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
-                         sample_time_nanos, /*reset_stats=*/false);
+  sampleRateEstimatorUpdate(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+                            sample_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
   // Pass gyro data to stillness detector
@@ -396,7 +369,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
 
   // Determines if the device is currently still.
   device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
-      !mean_not_stable && !min_max_temp_exceeded;
+                    !mean_not_stable && !min_max_temp_exceeded;
 
   if (device_is_still) {
     // Device is "still" logic:
@@ -440,12 +413,6 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
       computeGyroCal(gyro_cal,
                      gyro_cal->gyro_stillness_detect.last_sample_time);
 
-#ifdef GYRO_CAL_DBG_ENABLED
-      // Resets the sampling rate estimate.
-      gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
-                             sample_time_nanos, /*reset_stats=*/true);
-#endif  // GYRO_CAL_DBG_ENABLED
-
       // Update stillness flag. Force the start of a new stillness period.
       gyro_cal->prev_still = false;
     } else {
@@ -484,12 +451,6 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
     gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
     gyroStillMeanTracker(gyro_cal, DO_RESET);
 
-#ifdef GYRO_CAL_DBG_ENABLED
-    // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
-                           sample_time_nanos, /*reset_stats=*/true);
-#endif  // GYRO_CAL_DBG_ENABLED
-
     // Update stillness flag.
     gyro_cal->prev_still = false;
   }
@@ -501,17 +462,17 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
 // Calculates a new gyro bias offset calibration value.
 void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
   // Check to see if new calibration values is within acceptable range.
-  if (!(gyro_cal->gyro_stillness_detect.prev_mean_x <  MAX_GYRO_BIAS &&
+  if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS &&
-        gyro_cal->gyro_stillness_detect.prev_mean_y <  MAX_GYRO_BIAS &&
+        gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS &&
-        gyro_cal->gyro_stillness_detect.prev_mean_z <  MAX_GYRO_BIAS &&
+        gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
 #ifdef GYRO_CAL_DBG_ENABLED
     CAL_DEBUG_LOG(
         "[GYRO_CAL:REJECT]",
         "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
-        ", " CAL_FORMAT_3DIGITS ", %llu",
+        ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
         CAL_ENCODE_FLOAT(
             gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
         CAL_ENCODE_FLOAT(
@@ -519,7 +480,7 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
         CAL_ENCODE_FLOAT(
             gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
         CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
-        (unsigned long long int)calibration_time_nanos);
+        calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Outside of range. Ignore, reset, and continue.
@@ -580,21 +541,17 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
 #ifdef GYRO_CAL_DBG_ENABLED
     gyro_cal->debug_watchdog_count++;
     if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) {
-      CAL_DEBUG_LOG(
-          "[GYRO_CAL:WATCHDOG]",
-          "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu",
-          (unsigned long int)gyro_cal->debug_watchdog_count,
-          (unsigned long long int)sample_time_nanos,
-          (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos -
-                                   sample_time_nanos));
+      CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+                    "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64
+                    ", -%" PRIu64,
+                    gyro_cal->debug_watchdog_count, sample_time_nanos,
+                    gyro_cal->gyro_watchdog_start_nanos - sample_time_nanos);
     } else {
-      CAL_DEBUG_LOG(
-          "[GYRO_CAL:WATCHDOG]",
-          "Total#, Timestamp | Delta  [nsec]: %lu, %llu, %llu",
-          (unsigned long int)gyro_cal->debug_watchdog_count,
-          (unsigned long long int)sample_time_nanos,
-          (unsigned long long int)(sample_time_nanos -
-                                   gyro_cal->gyro_watchdog_start_nanos));
+      CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+                    "Total#, Timestamp | Delta  [nsec]: %zu, %" PRIu64
+                    ", %" PRIu64,
+                    gyro_cal->debug_watchdog_count, sample_time_nanos,
+                    sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos);
     }
 #endif  // GYRO_CAL_DBG_ENABLED
 
@@ -607,12 +564,6 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
     gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
     gyroStillMeanTracker(gyro_cal, DO_RESET);
 
-#ifdef GYRO_CAL_DBG_ENABLED
-    // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
-                           sample_time_nanos, /*reset_stats=*/true);
-#endif  // GYRO_CAL_DBG_ENABLED
-
     // Resets the stillness window end-time.
     gyro_cal->stillness_win_endtime_nanos = 0;
 
@@ -631,7 +582,6 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
     }
 
     // Assert watchdog timeout flags.
-    gyro_cal->gyro_watchdog_timeout |= watchdog_timeout;
     gyro_cal->gyro_watchdog_start_nanos = 0;
   }
 }
@@ -833,53 +783,6 @@ bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
 }
 
 #ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
-                            float* debug_mean_sampling_rate_hz,
-                            uint64_t timestamp_nanos, bool reset_stats) {
-  // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
-  // out the estimate of the sampling rate.
-  if (debug_mean_sampling_rate_hz) {
-    if (sample_rate_estimator->num_samples > 1 &&
-        sample_rate_estimator->time_delta_accumulator > 0) {
-      // Computes the final mean calculation.
-      *debug_mean_sampling_rate_hz =
-          sample_rate_estimator->num_samples /
-          (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
-           NANOS_TO_SEC);
-    } else {
-      // Not enough samples to compute a valid sample rate estimate. Indicate
-      // this with a -1 value.
-      *debug_mean_sampling_rate_hz = -1.0f;
-    }
-    reset_stats = true;
-  }
-
-  // Resets the sampling rate mean estimator data.
-  if (reset_stats) {
-    sample_rate_estimator->last_timestamp_nanos = 0;
-    sample_rate_estimator->time_delta_accumulator = 0;
-    sample_rate_estimator->num_samples = 0;
-    return;
-  }
-
-  // Skip adding this data to the accumulator if:
-  //   1. A bad timestamp was received (i.e., time not monotonic).
-  //   2. 'last_timestamp_nanos' is zero.
-  if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
-      sample_rate_estimator->last_timestamp_nanos == 0) {
-    sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
-    return;
-  }
-
-  // Increments the number of samples.
-  sample_rate_estimator->num_samples++;
-
-  // Accumulate the time steps.
-  sample_rate_estimator->time_delta_accumulator +=
-      timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
-  sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
-}
-
 void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
   // Only update this data if debug printing is not currently in progress
   // (i.e., don't want to risk overwriting debug information that is actively
@@ -912,11 +815,6 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
   gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y;
   gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
 
-  // Records the mean gyroscope sampling rate.
-  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
-                         &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
-                         /*reset_stats=*/true);
-
   // Records the min/max gyroscope window stillness mean values.
   memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
          sizeof(gyro_cal->gyro_winmean_min));
@@ -983,8 +881,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
       CAL_DEBUG_LOG(
           debug_tag,
           "Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
-          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
-          (unsigned long int)gyro_cal->debug_calibration_count,
+          "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+          ", %" PRIu64,
+          gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(
               gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
           CAL_ENCODE_FLOAT(
@@ -993,8 +892,7 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
               gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
                            3),
-          (unsigned long long int)
-              gyro_cal->debug_gyro_cal.end_still_time_nanos);
+          gyro_cal->debug_gyro_cal.end_still_time_nanos);
       break;
 
     case STILLNESS_DATA:
@@ -1003,13 +901,11 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
                      : -1.0f;  // Signals that magnetometer was not used.
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Stillness|Confidence [nsec]: %lu, "
-          "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
-          (unsigned long int)gyro_cal->debug_calibration_count,
-          (unsigned long long int)(gyro_cal->debug_gyro_cal
-                                       .end_still_time_nanos -
-                                   gyro_cal->debug_gyro_cal
-                                       .start_still_time_nanos),
+          "Cal#|Stillness|Confidence [nsec]: %zu, "
+          "%" PRIu64 ", " CAL_FORMAT_3DIGITS_TRIPLET,
+          gyro_cal->debug_calibration_count,
+          gyro_cal->debug_gyro_cal.end_still_time_nanos -
+              gyro_cal->debug_gyro_cal.start_still_time_nanos,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
           CAL_ENCODE_FLOAT(mag_data, 3));
@@ -1019,9 +915,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
       CAL_DEBUG_LOG(
           debug_tag,
           "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
-          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+          "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
           ", " CAL_FORMAT_3DIGITS,
-          (unsigned long int)gyro_cal->debug_calibration_count,
+          gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
                            3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3),
@@ -1029,15 +925,17 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
                                gyro_cal->debug_gyro_cal.temperature_min_celsius,
                            3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.sample_rate_estimator
+                               .mean_sampling_rate_estimate_hz,
+                           3));
       break;
 
     case GYRO_MINMAX_STILLNESS_MEAN:
       CAL_DEBUG_LOG(
           debug_tag,
           "Cal#|Gyro Peak Stillness Variation [mDPS]: "
-          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
-          (unsigned long int)gyro_cal->debug_calibration_count,
+          "%zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+          gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
                                RAD_TO_MDEG,
@@ -1055,9 +953,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case ACCEL_STATS:
       CAL_DEBUG_LOG(debug_tag,
                     "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
-                    "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+                    "%zu, " CAL_FORMAT_3DIGITS_TRIPLET
                     ", " CAL_FORMAT_6DIGITS_TRIPLET,
-                    (unsigned long int)gyro_cal->debug_calibration_count,
+                    gyro_cal->debug_calibration_count,
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
@@ -1069,9 +967,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case GYRO_STATS:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
           ", " CAL_FORMAT_3DIGITS_TRIPLET,
-          (unsigned long int)gyro_cal->debug_calibration_count,
+          gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
                            3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
@@ -1093,9 +991,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
       if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
         CAL_DEBUG_LOG(
             debug_tag,
-            "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+            "Cal#|Mag Mean|Var [uT|uT^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
             ", " CAL_FORMAT_6DIGITS_TRIPLET,
-            (unsigned long int)gyro_cal->debug_calibration_count,
+            gyro_cal->debug_calibration_count,
             CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
             CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
             CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
@@ -1104,9 +1002,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
             CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
       } else {
         CAL_DEBUG_LOG(debug_tag,
-                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
+                      "Cal#|Mag Mean|Var [uT|uT^2]: %zu, 0, 0, 0, -1.0, -1.0, "
                       "-1.0",
-                      (unsigned long int)gyro_cal->debug_calibration_count);
+                      gyro_cal->debug_calibration_count);
       }
       break;
 
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 5e7d5eec..1f17254c 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
  *       - Temperature   [Celsius]
  *
  * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * data to assist in tuning the GyroCal parameters.
  */
 
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
@@ -43,6 +42,10 @@
 
 #include "calibration/gyroscope/gyro_stillness_detect.h"
 
+#ifdef GYRO_CAL_DBG_ENABLED
+#include "calibration/sample_rate_estimator/sample_rate_estimator.h"
+#endif  // GYRO_CAL_DBG_ENABLED
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -63,10 +66,10 @@ enum GyroCalDebugState {
 
 // Gyro Cal debug information/data tracking structure.
 struct DebugGyroCal {
+  struct SampleRateEstimator sample_rate_estimator;
   uint64_t start_still_time_nanos;
   uint64_t end_still_time_nanos;
   uint64_t stillness_duration_nanos;
-  float mean_sampling_rate_hz;
   float accel_stillness_conf;
   float gyro_stillness_conf;
   float mag_stillness_conf;
@@ -84,14 +87,28 @@ struct DebugGyroCal {
   float temperature_mean_celsius;
   bool using_mag_sensor;
 };
+#endif  // GYRO_CAL_DBG_ENABLED
 
-// Data structure for sample rate estimation.
-struct SampleRateData {
-  uint64_t last_timestamp_nanos;
-  uint64_t time_delta_accumulator;
-  size_t num_samples;
+// GyroCal algorithm parameters (see GyroCal and GyroStillDet for details).
+struct GyroCalParameters {
+  uint64_t min_still_duration_nanos;
+  uint64_t max_still_duration_nanos;
+  uint64_t calibration_time_nanos;
+  uint64_t window_time_duration_nanos;
+  float bias_x;  // units: radians per second
+  float bias_y;
+  float bias_z;
+  float stillness_threshold;         // units: (radians per second)^2
+  float stillness_mean_delta_limit;  // units: radians per second
+  float gyro_var_threshold;          // units: (radians per second)^2
+  float gyro_confidence_delta;       // units: (radians per second)^2
+  float accel_var_threshold;         // units: (meters per second)^2
+  float accel_confidence_delta;      // units: (meters per second)^2
+  float mag_var_threshold;           // units: micro-tesla^2
+  float mag_confidence_delta;        // units: micro-tesla^2
+  float temperature_delta_limit_celsius;
+  bool gyro_calibration_enable;
 };
-#endif  // GYRO_CAL_DBG_ENABLED
 
 // Data structure for tracking min/max window mean during device stillness.
 struct MinMaxWindowMeanData {
@@ -149,7 +166,6 @@ struct GyroCal {
   // Watchdog timer to reset to a known good state when data capture stalls.
   uint64_t gyro_watchdog_start_nanos;
   uint64_t gyro_watchdog_timeout_duration_nanos;
-  bool gyro_watchdog_timeout;
 
   // Flag is "true" when the magnetometer is used.
   bool using_mag_sensor;
@@ -177,7 +193,7 @@ struct GyroCal {
   float temperature_mean_celsius;
   float temperature_delta_limit_celsius;
 
-//----------------------------------------------------------------
+  //----------------------------------------------------------------
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Debug info.
@@ -185,9 +201,6 @@ struct GyroCal {
   enum GyroCalDebugState debug_state;  // Debug printout state machine.
   enum GyroCalDebugState next_state;   // Debug state machine next state.
   uint64_t wait_timer_nanos;           // Debug message throttle timer.
-
-  struct SampleRateData sample_rate_estimator;  // Debug sample rate estimator.
-
   size_t debug_calibration_count;      // Total number of cals performed.
   size_t debug_watchdog_count;         // Total number of watchdog timeouts.
   bool debug_print_trigger;            // Flag used to trigger data printout.
@@ -197,27 +210,21 @@ struct GyroCal {
 /////// FUNCTION PROTOTYPES //////////////////////////////////////////
 
 // Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
-                 uint64_t max_still_duration_nanos, float bias_x, float bias_y,
-                 float bias_z, uint64_t calibration_time_nanos,
-                 uint64_t window_time_duration_nanos, float gyro_var_threshold,
-                 float gyro_confidence_delta, float accel_var_threshold,
-                 float accel_confidence_delta, float mag_var_threshold,
-                 float mag_confidence_delta, float stillness_threshold,
-                 float stillness_mean_delta_limit,
-                 float temperature_delta_limit_celsius,
-                 bool gyro_calibration_enable);
+void gyroCalInit(struct GyroCal* gyro_cal,
+                 const struct GyroCalParameters* parameters);
 
 // Void all pointers in the gyro calibration data structure.
 void gyroCalDestroy(struct GyroCal* gyro_cal);
 
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
-                    float* bias_z, float* temperature_celsius);
+                    float* bias_z, float* temperature_celsius,
+                    uint64_t* calibration_time_nanos);
 
 // Set an initial bias calibration value.
 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
-                    float bias_z, uint64_t calibration_time_nanos);
+                    float bias_z, float temperature_celsius,
+                    uint64_t calibration_time_nanos);
 
 // Remove gyro bias from the calibration [rad/sec].
 void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index 80f2fa21..bb2063b6 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -15,6 +15,7 @@
  */
 
 #include "calibration/gyroscope/gyro_stillness_detect.h"
+
 #include <string.h>
 
 /////// FORWARD DECLARATIONS /////////////////////////////////////////
@@ -25,8 +26,8 @@ static float gyroStillDetLimit(float value);
 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
 
 // Initialize the GyroStillDet structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
-                      float var_threshold, float confidence_delta) {
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+                      float confidence_delta) {
   // Clear all data structure variables to 0.
   memset(gyro_still_det, 0, sizeof(struct GyroStillDet));
 
@@ -192,12 +193,12 @@ float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
       // Each axis score is limited [0,1].
       tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
       gyro_still_det->stillness_confidence =
-          gyroStillDetLimit(
-              0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
-          gyroStillDetLimit(
-              0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
-          gyroStillDetLimit(
-              0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);
+          gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
+                                       tmp_denom) *
+          gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
+                                       tmp_denom) *
+          gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
+                                       tmp_denom);
     }
   }
 
@@ -207,8 +208,7 @@ float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
 
 // Resets the stillness detector and initiates a new detection window.
 // 'reset_stats' determines whether the stillness statistics are reset.
-void gyroStillDetReset(struct GyroStillDet* gyro_still_det,
-                       bool reset_stats) {
+void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) {
   float tmp_denom = 1.f;
 
   // Reset the stillness data ready flag.
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
index 9a1d876c..51c4bee6 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
@@ -60,7 +60,7 @@ struct GyroStillDet {
   // is used to keep track of the window start time.
   bool start_new_window;
 
-  // Starting time stamp for the the current window.
+  // Starting time stamp for the current window.
   uint64_t window_start_time;
 
   // Accumulator variables for tracking the sample mean during
@@ -93,8 +93,8 @@ struct GyroStillDet {
 /////// FUNCTION PROTOTYPES //////////////////////////////////////////
 
 // Initialize the gyro_still_det_t structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
-                      float var_threshold, float confidence_delta);
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+                      float confidence_delta);
 
 // Update the stillness detector with a new sample.
 void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
deleted file mode 100644
index 7f8e563f..00000000
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ /dev/null
@@ -1,520 +0,0 @@
-/*
- * 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 "calibration/magnetometer/mag_cal.h"
-
-#include <errno.h>
-#include <string.h>
-
-#include "calibration/util/cal_log.h"
-
-#ifdef MAG_CAL_ORIGINAL_TUNING
-#define MAX_EIGEN_RATIO 25.0f
-#define MAX_EIGEN_MAG 80.0f          // uT
-#define MIN_EIGEN_MAG 10.0f          // uT
-#define MAX_FIT_MAG 80.0f
-#define MIN_FIT_MAG 10.0f
-#define MAX_BATCH_WINDOW 15000000UL  // 15 sec
-#define MIN_BATCH_SIZE 25            // samples
-#else
-#define MAX_EIGEN_RATIO 15.0f
-#define MAX_EIGEN_MAG 70.0f          // uT
-#define MIN_EIGEN_MAG 20.0f          // uT
-#define MAX_FIT_MAG 70.0f
-#define MIN_FIT_MAG 20.0f
-#define MAX_BATCH_WINDOW 15000000UL  // 15 sec
-#define MIN_BATCH_SIZE 25            // samples
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
-#define MAX_DISTANCE_VIOLATIONS 2
-#ifdef SPHERE_FIT_ENABLED
-# define MAX_ITERATIONS 30
-# define INITIAL_U_SCALE 1.0e-4f
-# define GRADIENT_THRESHOLD 1.0e-16f
-# define RELATIVE_STEP_THRESHOLD 1.0e-7f
-# define FROM_MICRO_SEC_TO_SEC 1.0e-6f
-#endif
-#endif
-
-// eigen value magnitude and ratio test
-static int moc_eigen_test(struct KasaFit *kasa) {
-  // covariance matrix
-  struct Mat33 S;
-  S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
-  S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
-  S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z;
-  S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y;
-  S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z;
-  S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z;
-
-  struct Vec3 eigenvals;
-  struct Mat33 eigenvecs;
-  mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
-
-  float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
-  evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
-
-  float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
-  evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
-
-  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
-
-  // Testing for negative number.
-  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
-
-  int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
-                   (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
-
-  return eigen_pass;
-}
-
-// Kasa sphere fitting with normal equation
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) {
-  //    A    *   out   =    b
-  // (4 x 4)   (4 x 1)   (4 x 1)
-  struct Mat44 A;
-  A.elem[0][0] = kasa->acc_xx;
-  A.elem[0][1] = kasa->acc_xy;
-  A.elem[0][2] = kasa->acc_xz;
-  A.elem[0][3] = kasa->acc_x;
-  A.elem[1][0] = kasa->acc_xy;
-  A.elem[1][1] = kasa->acc_yy;
-  A.elem[1][2] = kasa->acc_yz;
-  A.elem[1][3] = kasa->acc_y;
-  A.elem[2][0] = kasa->acc_xz;
-  A.elem[2][1] = kasa->acc_yz;
-  A.elem[2][2] = kasa->acc_zz;
-  A.elem[2][3] = kasa->acc_z;
-  A.elem[3][0] = kasa->acc_x;
-  A.elem[3][1] = kasa->acc_y;
-  A.elem[3][2] = kasa->acc_z;
-  A.elem[3][3] = 1.0f;
-
-  struct Vec4 b;
-  initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
-
-  struct Size4 pivot;
-  mat44DecomposeLup(&A, &pivot);
-
-  struct Vec4 out;
-  mat44Solve(&A, &out, &b, &pivot);
-
-  // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
-  //
-  // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
-  // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
-
-  struct Vec3 v;
-  initVec3(&v, out.x, out.y, out.z);
-  vec3ScalarMul(&v, -0.5f);
-
-  float r_square = vec3Dot(&v, &v) - out.w;
-  float r = (r_square > 0) ? sqrtf(r_square) : 0;
-
-  initVec3(bias, v.x, v.y, v.z);
-  *radius = r;
-
-  int success = 0;
-  if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) {
-    success = 1;
-  }
-
-  return success;
-}
-
-void magKasaReset(struct KasaFit *kasa) {
-  kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
-  kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
-  kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
-  kasa->acc_zz = kasa->acc_zw = 0.0f;
-
-  kasa->nsamples = 0;
-}
-
-void magCalReset(struct MagCal *moc) {
-  magKasaReset(&moc->kasa);
-#ifdef DIVERSITY_CHECK_ENABLED
-  diversityCheckerReset(&moc->diversity_checker);
-#endif
-  moc->start_time = 0;
-  moc->kasa_batching = false;
-}
-
-static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
-  int complete = 0;
-
-  if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) &&
-      (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
-    complete = 1;
-
-  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
-    // not enough samples collected in MAX_BATCH_WINDOW or too many
-    // maximum distance violations detected.
-    magCalReset(moc);
-  }
-
-  return complete;
-}
-
-void initKasa(struct KasaFit *kasa) {
-  magKasaReset(kasa);
-}
-
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
-                float c00, float c01, float c02, float c10, float c11,
-                float c12, float c20, float c21, float c22,
-                uint32_t min_batch_window_in_micros
-#ifdef DIVERSITY_CHECK_ENABLED
-                ,size_t min_num_diverse_vectors
-                ,size_t max_num_max_distance
-                ,float var_threshold
-                ,float max_min_threshold
-                ,float local_field
-                ,float threshold_tuning_param
-                ,float max_distance_tuning_param
-#endif
-                ) {
-  magCalReset(moc);
-  moc->update_time = 0;
-  moc->min_batch_window_in_micros = min_batch_window_in_micros;
-  moc->radius = 0.0f;
-
-  moc->x_bias = x_bias;
-  moc->y_bias = y_bias;
-  moc->z_bias = z_bias;
-
-  moc->c00 = c00;
-  moc->c01 = c01;
-  moc->c02 = c02;
-  moc->c10 = c10;
-  moc->c11 = c11;
-  moc->c12 = c12;
-  moc->c20 = c20;
-  moc->c21 = c21;
-  moc->c22 = c22;
-
-#ifdef MAG_CAL_DEBUG_ENABLE
-  moc->mag_dbg.mag_trigger_count = 0;
-  moc->mag_dbg.kasa_count = 0;
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
-  // Diversity Checker
-  diversityCheckerInit(&moc->diversity_checker,
-                       min_num_diverse_vectors,
-                       max_num_max_distance,
-                       var_threshold,
-                       max_min_threshold,
-                       local_field,
-                       threshold_tuning_param,
-                       max_distance_tuning_param);
-#endif
-}
-
-void magCalDestroy(struct MagCal *moc) { (void)moc; }
-
-enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
-                            float x, float y, float z) {
-  enum MagUpdate new_bias = NO_UPDATE;
-
-#ifdef DIVERSITY_CHECK_ENABLED
-  // Diversity Checker Update.
-  diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
-#endif
-
-  // 1. run accumulators
-  float w = x * x + y * y + z * z;
-
-  moc->kasa.acc_x += x;
-  moc->kasa.acc_y += y;
-  moc->kasa.acc_z += z;
-  moc->kasa.acc_w += w;
-
-  moc->kasa.acc_xx += x * x;
-  moc->kasa.acc_xy += x * y;
-  moc->kasa.acc_xz += x * z;
-  moc->kasa.acc_xw += x * w;
-
-  moc->kasa.acc_yy += y * y;
-  moc->kasa.acc_yz += y * z;
-  moc->kasa.acc_yw += y * w;
-
-  moc->kasa.acc_zz += z * z;
-  moc->kasa.acc_zw += z * w;
-
-  if (++moc->kasa.nsamples == 1) {
-    moc->start_time = sample_time_us;
-    moc->kasa_batching = true;
-  }
-
-  // 2. batch has enough samples?
-  if (moc_batch_complete(moc, sample_time_us)) {
-    float inv = 1.0f / moc->kasa.nsamples;
-
-    moc->kasa.acc_x *= inv;
-    moc->kasa.acc_y *= inv;
-    moc->kasa.acc_z *= inv;
-    moc->kasa.acc_w *= inv;
-
-    moc->kasa.acc_xx *= inv;
-    moc->kasa.acc_xy *= inv;
-    moc->kasa.acc_xz *= inv;
-    moc->kasa.acc_xw *= inv;
-
-    moc->kasa.acc_yy *= inv;
-    moc->kasa.acc_yz *= inv;
-    moc->kasa.acc_yw *= inv;
-
-    moc->kasa.acc_zz *= inv;
-    moc->kasa.acc_zw *= inv;
-
-    // 3. eigen test
-    if (moc_eigen_test(&moc->kasa)) {
-      struct Vec3 bias;
-      float radius;
-      // 4. Kasa sphere fitting
-      if (magKasaFit(&moc->kasa, &bias, &radius)) {
-
-#ifdef MAG_CAL_DEBUG_ENABLE
-        moc->mag_dbg.kasa_count++;
-        CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :,",
-                      "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %lu, %lu",
-                      CAL_ENCODE_FLOAT(bias.x, 3),
-                      CAL_ENCODE_FLOAT(bias.y, 3),
-                      CAL_ENCODE_FLOAT(bias.z, 3),
-                      CAL_ENCODE_FLOAT(radius, 3),
-                      (unsigned long int)moc->mag_dbg.kasa_count,
-                      (unsigned long int)moc->mag_dbg.mag_trigger_count);
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
-        // Update the local field.
-        diversityCheckerLocalFieldUpdate(&moc->diversity_checker,
-                                         radius);
-
-        // checking if data is diverse.
-        if (diversityCheckerNormQuality(&moc->diversity_checker,
-                                        bias.x,
-                                        bias.y,
-                                        bias.z) &&
-            moc->diversity_checker.num_max_dist_violations
-            <= MAX_DISTANCE_VIOLATIONS) {
-
-          // DEBUG PRINT OUT.
-#ifdef MAG_CAL_DEBUG_ENABLE
-          moc->mag_dbg.mag_trigger_count++;
-#ifdef DIVERSE_DEBUG_ENABLE
-          moc->diversity_checker.diversity_dbg.new_trigger = 1;
-          CAL_DEBUG_LOG("[MAG_CAL:BIAS UPDATE] :, ",
-                        "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d,"
-                        "%s%d.%03d, %s%d.%03d, %s%d.%03d, %zu, %s%d.%03d, "
-                        "%s%d.%03d, %lu, %lu, %llu, %s%d.%03d, %s%d.%03d, "
-                        "%s%d.%03d, %llu",
-                        CAL_ENCODE_FLOAT(bias.x, 3),
-                        CAL_ENCODE_FLOAT(bias.y, 3),
-                        CAL_ENCODE_FLOAT(bias.z, 3),
-                        CAL_ENCODE_FLOAT(radius, 3),
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.diversity_dbg.var_log, 6),
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.diversity_dbg.mean_log, 3),
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.diversity_dbg.max_log, 3),
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.diversity_dbg.min_log, 3),
-                        moc->diversity_checker.num_points,
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.threshold, 3),
-                        CAL_ENCODE_FLOAT(
-                            moc->diversity_checker.max_distance, 3),
-                        (unsigned long int)moc->mag_dbg.mag_trigger_count,
-                        (unsigned long int)moc->mag_dbg.kasa_count,
-                        (unsigned long long int)sample_time_us,
-                        CAL_ENCODE_FLOAT(moc->x_bias, 3),
-                        CAL_ENCODE_FLOAT(moc->y_bias, 3),
-                        CAL_ENCODE_FLOAT(moc->z_bias, 3),
-                        (unsigned long long int)moc->update_time);
-#endif
-#endif
-#endif
-          moc->x_bias = bias.x;
-          moc->y_bias = bias.y;
-          moc->z_bias = bias.z;
-
-          moc->radius = radius;
-          moc->update_time = sample_time_us;
-
-          new_bias = UPDATE_BIAS;
-
-#ifdef DIVERSITY_CHECK_ENABLED
-        }
-#endif
-      }
-    }
-
-    // 5. reset for next batch
-    magCalReset(moc);
-  }
-
-  return new_bias;
-}
-
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) {
-  *x = moc->x_bias;
-  *y = moc->y_bias;
-  *z = moc->z_bias;
-}
-
-void magCalAddBias(struct MagCal *moc, float x, float y, float z) {
-  moc->x_bias += x;
-  moc->y_bias += y;
-  moc->z_bias += z;
-}
-
-void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
-                      float *xo, float *yo, float *zo) {
-  *xo = xi - moc->x_bias;
-  *yo = yi - moc->y_bias;
-  *zo = zi - moc->z_bias;
-}
-
-void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
-                       float c10, float c11, float c12, float c20, float c21,
-                       float c22) {
-  moc->c00 = c00;
-  moc->c01 = c01;
-  moc->c02 = c02;
-  moc->c10 = c10;
-  moc->c11 = c11;
-  moc->c12 = c12;
-  moc->c20 = c20;
-  moc->c21 = c21;
-  moc->c22 = c22;
-}
-
-void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
-                          float *xo, float *yo, float *zo) {
-  *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi;
-  *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi;
-  *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
-}
-
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSE_DEBUG_ENABLE
-// This function prints every second sample parts of the dbg diverse_data_log,
-// which ensures that all the messages get printed into the log file.
-void magLogPrint(struct DiversityChecker* diverse_data, float temp) {
-  // Sample counter.
-  static size_t sample_counter = 0;
-  const float* data_log_ptr =
-      &diverse_data->diversity_dbg.diverse_data_log[0];
-  if (diverse_data->diversity_dbg.new_trigger == 1) {
-    sample_counter++;
-    if (sample_counter == 2) {
-      CAL_DEBUG_LOG("[MAG_CAL:MEMORY X] :,",
-                    "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
-                    ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d",
-                    (unsigned long int)diverse_data->
-                    diversity_dbg.diversity_count,
-                    CAL_ENCODE_FLOAT(data_log_ptr[0*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[1*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[2*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[3*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[4*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[5*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[6*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[7*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[8*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[9*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[10*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[11*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[12*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[13*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[14*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[15*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[16*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[17*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[18*3], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[19*3], 3),
-                    CAL_ENCODE_FLOAT(temp, 3));
-    }
-
-    if (sample_counter == 4) {
-      CAL_DEBUG_LOG("[MAG_CAL:MEMORY Y] :,",
-                    "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
-                    ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
-                    (unsigned long int)diverse_data->
-                    diversity_dbg.diversity_count,
-                    CAL_ENCODE_FLOAT(data_log_ptr[0*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[1*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[2*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[3*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[4*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[5*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[6*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[7*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[8*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[9*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[10*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[11*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[12*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[13*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[14*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[15*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[16*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[17*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[18*3+1], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[19*3+1], 3));
-    }
-    if (sample_counter == 6) {
-      CAL_DEBUG_LOG("[MAG_CAL:MEMORY Z] :,",
-                    "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
-                    ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
-                    (unsigned long int)diverse_data->
-                    diversity_dbg.diversity_count,
-                    CAL_ENCODE_FLOAT(data_log_ptr[0*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[1*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[2*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[3*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[4*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[5*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[6*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[7*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[8*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[9*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[10*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[11*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[12*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[13*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[14*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[15*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[16*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[17*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[18*3+2], 3),
-                    CAL_ENCODE_FLOAT(data_log_ptr[19*3+2], 3));
-      sample_counter = 0;
-      diverse_data->diversity_dbg.new_trigger = 0;
-    }
-  }
-}
-#endif
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
new file mode 100644
index 00000000..c3f12ae6
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -0,0 +1,358 @@
+/*
+ * 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 "calibration/magnetometer/mag_cal/mag_cal.h"
+
+#include <errno.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+
+// Local helper macro for printing log messages.
+#ifdef MAG_CAL_DEBUG_ENABLE
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_MAG_MEMORY                                          \
+  "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+  "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+  "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+  "%s%d.%03d, %s%d.%03d"
+#else
+#define CAL_FORMAT_MAG_MEMORY                                                \
+  "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, " \
+  "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f"
+#endif  // CAL_NO_FLOAT_FORMAT_STRINGS
+#endif  // MAG_CAL_DEBUG_ENABLE
+
+// clang-format off
+#define MAX_EIGEN_RATIO 15.0f
+#define MAX_EIGEN_MAG 70.0f          // uT
+#define MIN_EIGEN_MAG 20.0f          // uT
+#define MAX_FIT_MAG 70.0f
+#define MIN_FIT_MAG 20.0f
+#define MAX_BATCH_WINDOW 15000000UL  // 15 sec
+#define MIN_BATCH_SIZE 25            // samples
+#define MAX_DISTANCE_VIOLATIONS 2
+// clang-format
+
+// eigen value magnitude and ratio test.
+static int moc_eigen_test(struct KasaFit *kasa) {
+  // covariance matrix.
+  struct Mat33 S;
+  S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
+  S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
+  S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z;
+  S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y;
+  S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z;
+  S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z;
+
+  struct Vec3 eigenvals;
+  struct Mat33 eigenvecs;
+  mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
+
+  float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
+  evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
+
+  float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
+  evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
+
+  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+  // Testing for negative number.
+  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
+
+  int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
+                   (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
+
+  return eigen_pass;
+}
+
+void magCalReset(struct MagCal *moc) {
+  kasaReset(&moc->kasa);
+  diversityCheckerReset(&moc->diversity_checker);
+  moc->start_time = 0;
+  moc->kasa_batching = false;
+}
+
+static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
+  bool complete = false;
+
+  if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) &&
+      (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
+    complete = true;
+
+  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
+    // not enough samples collected in MAX_BATCH_WINDOW or too many
+    // maximum distance violations detected.
+    magCalReset(moc);
+  }
+
+  return complete;
+}
+
+void initMagCal(struct MagCal *moc,
+                const struct MagCalParameters *mag_cal_parameters,
+                const struct DiversityCheckerParameters *diverse_parameters) {
+  magCalReset(moc);
+  moc->update_time = 0;
+  moc->min_batch_window_in_micros =
+      mag_cal_parameters->min_batch_window_in_micros;
+  moc->radius = 0.0f;
+
+  moc->x_bias = mag_cal_parameters->x_bias;
+  moc->y_bias = mag_cal_parameters->y_bias;
+  moc->z_bias = mag_cal_parameters->z_bias;
+
+  moc->c00 = mag_cal_parameters->c00;
+  moc->c01 = mag_cal_parameters->c01;
+  moc->c02 = mag_cal_parameters->c02;
+  moc->c10 = mag_cal_parameters->c10;
+  moc->c11 = mag_cal_parameters->c11;
+  moc->c12 = mag_cal_parameters->c12;
+  moc->c20 = mag_cal_parameters->c20;
+  moc->c21 = mag_cal_parameters->c21;
+  moc->c22 = mag_cal_parameters->c22;
+
+#ifdef MAG_CAL_DEBUG_ENABLE
+  moc->mag_dbg.mag_trigger_count = 0;
+  moc->mag_dbg.kasa_count = 0;
+#endif  // MAG_CAL_DEBUG_ENABLE
+
+  // Diversity Checker
+  diversityCheckerInit(&moc->diversity_checker, diverse_parameters);
+}
+
+void magCalDestroy(struct MagCal *moc) { (void)moc; }
+
+enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
+                            float x, float y, float z) {
+  enum MagUpdate new_bias = NO_UPDATE;
+
+  // Diversity Checker Update.
+  diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
+
+  // 1. run accumulators
+  kasaAccumulate(&moc->kasa, x, y, z);
+
+  if (moc->kasa.nsamples == 1) {
+    moc->start_time = sample_time_us;
+    moc->kasa_batching = true;
+  }
+
+  // 2. batch has enough samples?
+  if (moc_batch_complete(moc, sample_time_us)) {
+    kasaNormalize(&moc->kasa);
+
+    // 3. eigen test
+    if (moc_eigen_test(&moc->kasa)) {
+      struct Vec3 bias;
+      float radius;
+      // 4. Kasa sphere fitting
+      if (kasaFit(&moc->kasa, &bias, &radius, MAX_FIT_MAG, MIN_FIT_MAG)) {
+#ifdef MAG_CAL_DEBUG_ENABLE
+        moc->mag_dbg.kasa_count++;
+        CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET
+                      ", " CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32,
+                      CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
+                      CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
+                      moc->mag_dbg.kasa_count, moc->mag_dbg.mag_trigger_count);
+#endif  // MAG_CAL_DEBUG_ENABLE
+
+        // Update the local field.
+        diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius);
+
+        // checking if data is diverse.
+        if (diversityCheckerNormQuality(&moc->diversity_checker, bias.x, bias.y,
+                                        bias.z) &&
+            moc->diversity_checker.num_max_dist_violations <=
+                MAX_DISTANCE_VIOLATIONS) {
+          // DEBUG PRINT OUT.
+#ifdef MAG_CAL_DEBUG_ENABLE
+          moc->mag_dbg.mag_trigger_count++;
+          moc->diversity_checker.diversity_dbg.new_trigger = 1;
+          CAL_DEBUG_LOG(
+              "[MAG_CAL:BIAS UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET ", "
+              CAL_FORMAT_3DIGITS ", " CAL_FORMAT_6DIGITS ", "
+              CAL_FORMAT_3DIGITS_TRIPLET ", %zu, " CAL_FORMAT_3DIGITS ", "
+              CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32 ", %" PRIu64 ", "
+              CAL_FORMAT_3DIGITS_TRIPLET ", %" PRIu64 "",
+              CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
+              CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
+              CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.var_log, 6),
+              CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.mean_log,
+                               3),
+              CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.max_log, 3),
+              CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.min_log, 3),
+              moc->diversity_checker.num_points,
+              CAL_ENCODE_FLOAT(moc->diversity_checker.threshold, 3),
+              CAL_ENCODE_FLOAT(moc->diversity_checker.max_distance, 3),
+              moc->mag_dbg.mag_trigger_count,
+              moc->mag_dbg.kasa_count,
+              sample_time_us,
+              CAL_ENCODE_FLOAT(moc->x_bias, 3),
+              CAL_ENCODE_FLOAT(moc->y_bias, 3),
+              CAL_ENCODE_FLOAT(moc->z_bias, 3),
+              moc->update_time);
+#endif  // MAG_CAL_DEBUG_ENABLE
+          moc->x_bias = bias.x;
+          moc->y_bias = bias.y;
+          moc->z_bias = bias.z;
+
+          moc->radius = radius;
+          moc->update_time = sample_time_us;
+
+          new_bias = UPDATE_BIAS;
+        }
+      }
+    }
+    // 5. reset for next batch
+    magCalReset(moc);
+  }
+
+  return new_bias;
+}
+
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z) {
+  *x = moc->x_bias;
+  *y = moc->y_bias;
+  *z = moc->z_bias;
+}
+
+void magCalAddBias(struct MagCal *moc, float x, float y, float z) {
+  moc->x_bias += x;
+  moc->y_bias += y;
+  moc->z_bias += z;
+}
+
+void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
+                      float *xo, float *yo, float *zo) {
+  *xo = xi - moc->x_bias;
+  *yo = yi - moc->y_bias;
+  *zo = zi - moc->z_bias;
+}
+
+void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
+                       float c10, float c11, float c12, float c20, float c21,
+                       float c22) {
+  moc->c00 = c00;
+  moc->c01 = c01;
+  moc->c02 = c02;
+  moc->c10 = c10;
+  moc->c11 = c11;
+  moc->c12 = c12;
+  moc->c20 = c20;
+  moc->c21 = c21;
+  moc->c22 = c22;
+}
+
+void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
+                          float *xo, float *yo, float *zo) {
+  *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi;
+  *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi;
+  *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
+}
+
+#if defined MAG_CAL_DEBUG_ENABLE
+// This function prints every second sample parts of the dbg diverse_data_log,
+// which ensures that all the messages get printed into the log file.
+void magLogPrint(struct DiversityChecker *diverse_data, float temp) {
+  // Sample counter.
+  static size_t sample_counter = 0;
+  const float *data_log_ptr = &diverse_data->diversity_dbg.diverse_data_log[0];
+  if (diverse_data->diversity_dbg.new_trigger == 1) {
+    sample_counter++;
+    if (sample_counter == 2) {
+      CAL_DEBUG_LOG(
+          "[MAG_CAL:MEMORY X] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY ", "
+          CAL_FORMAT_3DIGITS,
+          diverse_data->diversity_dbg.diversity_count,
+          CAL_ENCODE_FLOAT(data_log_ptr[0 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[1 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[2 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[3 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[4 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[5 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[6 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[7 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[8 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[9 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[10 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[11 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[12 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[13 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[14 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[15 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[16 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[17 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[18 * 3], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[19 * 3], 3), CAL_ENCODE_FLOAT(temp, 3));
+    }
+
+    if (sample_counter == 4) {
+      CAL_DEBUG_LOG(
+          "[MAG_CAL:MEMORY Y] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+          diverse_data->diversity_dbg.diversity_count,
+          CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 1], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 1], 3));
+    }
+    if (sample_counter == 6) {
+      CAL_DEBUG_LOG(
+          "[MAG_CAL:MEMORY Z] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+          diverse_data->diversity_dbg.diversity_count,
+          CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 2], 3),
+          CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 2], 3));
+      sample_counter = 0;
+      diverse_data->diversity_dbg.new_trigger = 0;
+    }
+  }
+}
+#endif  // MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
index 8c9da6fc..566afa88 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
@@ -13,21 +13,16 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
 
-#ifdef SPHERE_FIT_ENABLED
-#ifndef DIVERSITY_CHECK_ENABLED
-#define DIVERSITY_CHECK_ENABLED
-#endif
-#endif
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
 
 #include <stdbool.h>
 #include <stdint.h>
 #include <sys/types.h>
-#ifdef DIVERSITY_CHECK_ENABLED
-#include "calibration/common/diversity_checker.h"
-#endif
+
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "common/math/kasa.h"
 #include "common/math/mat.h"
 #include "common/math/vec.h"
 
@@ -35,17 +30,13 @@
 extern "C" {
 #endif
 
-struct KasaFit {
-  float acc_x, acc_y, acc_z, acc_w;
-  float acc_xx, acc_xy, acc_xz, acc_xw;
-  float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
-  size_t nsamples;
-};
-
 enum MagUpdate {
   NO_UPDATE = 0x00,
   UPDATE_BIAS = 0x01,
   UPDATE_SPHERE_FIT = 0x02,
+  UPDATE_BIAS_MAGGYRO_MEDIUM = 0x04,
+  UPDATE_BIAS_MAGGYRO_HIGH = 0x08,
+  MAGGYRO_TIMEOUT = 0x10,
 };
 
 #ifdef MAG_CAL_DEBUG_ENABLE
@@ -55,17 +46,32 @@ struct MagDbg {
 };
 #endif
 
+// MagCal algorithm parameters (see MagCal for details).
+struct MagCalParameters {
+  uint32_t min_batch_window_in_micros;
+  float x_bias;  // [micro-Tesla]
+  float y_bias;  // [micro-Tesla]
+  float z_bias;  // [micro-Tesla]
+  float c00;
+  float c01;
+  float c02;
+  float c10;
+  float c11;
+  float c12;
+  float c20;
+  float c21;
+  float c22;
+};
+
 struct MagCal {
-#ifdef DIVERSITY_CHECK_ENABLED
   struct DiversityChecker diversity_checker;
-#endif
   struct KasaFit kasa;
 
-  uint64_t start_time;
-  uint64_t update_time;
+  uint64_t start_time;   // [micro-seconds]
+  uint64_t update_time;  // [micro-seconds]
   uint32_t min_batch_window_in_micros;
   float x_bias, y_bias, z_bias;
-  float radius;
+  float radius;  // [micro-Tesla]
   bool kasa_batching;
   float c00, c01, c02, c10, c11, c12, c20, c21, c22;
 
@@ -74,29 +80,16 @@ struct MagCal {
 #endif
 };
 
-void initKasa(struct KasaFit *kasa);
-
-#ifdef DIVERSITY_CHECK_ENABLED
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
-                float c00, float c01, float c02, float c10, float c11,
-                float c12, float c20, float c21, float c22,
-                uint32_t min_batch_window_in_micros,
-                size_t min_num_diverse_vectors, size_t max_num_max_distance,
-                float var_threshold, float max_min_threshold, float local_field,
-                float threshold_tuning_param, float max_distance_tuning_param);
-#else
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
-                float c00, float c01, float c02, float c10, float c11,
-                float c12, float c20, float c21, float c22,
-                uint32_t min_batch_window_in_micros);
-#endif
+void initMagCal(struct MagCal *moc,
+                const struct MagCalParameters *mag_cal_parameters,
+                const struct DiversityCheckerParameters *diverse_parameters);
 
 void magCalDestroy(struct MagCal *moc);
 
 enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
                             float x, float y, float z);
 
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z);
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z);
 
 void magCalAddBias(struct MagCal *moc, float x, float y, float z);
 
@@ -110,13 +103,9 @@ void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
                           float *xo, float *yo, float *zo);
 
-void magKasaReset(struct KasaFit *kasa);
-
 void magCalReset(struct MagCal *moc);
 
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius);
-
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSITY_CHECK_ENABLED
+#if defined MAG_CAL_DEBUG_ENABLE
 void magLogPrint(struct DiversityChecker *moc, float temp);
 #endif
 
@@ -124,4 +113,4 @@ void magLogPrint(struct DiversityChecker *moc, float temp);
 }
 #endif
 
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
index eb23b876..93c2ac61 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
@@ -14,13 +14,11 @@
  * limitations under the License.
  */
 
-#include "calibration/magnetometer/mag_sphere_fit.h"
+#include "calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h"
 
 #include <errno.h>
 #include <string.h>
 
-#include "calibration/util/cal_log.h"
-
 #define MAX_ITERATIONS 30
 #define INITIAL_U_SCALE 1.0e-4f
 #define GRADIENT_THRESHOLD 1.0e-16f
@@ -33,22 +31,16 @@ void magCalSphereReset(struct MagCalSphere *mocs) {
   memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data));
 }
 
-void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias,
-                      float z_bias, float c00, float c01, float c02, float c10,
-                      float c11, float c12, float c20, float c21, float c22,
-                      uint32_t min_batch_window_in_micros,
-                      size_t min_num_diverse_vectors,
-                      size_t max_num_max_distance, float var_threshold,
-                      float max_min_threshold, float local_field,
-                      float threshold_tuning_param,
-                      float max_distance_tuning_param) {
-  initMagCal(&mocs->moc, x_bias, y_bias, z_bias, c00, c01, c02, c10, c11, c12,
-             c20, c21, c22, min_batch_window_in_micros, min_num_diverse_vectors,
-             max_num_max_distance, var_threshold, max_min_threshold,
-             local_field, threshold_tuning_param, max_distance_tuning_param);
+void initMagCalSphere(
+    struct MagCalSphere *mocs,
+    const struct MagCalParameters *mag_cal_parameters,
+    const struct DiversityCheckerParameters *diverse_parameters,
+    float default_odr_in_hz) {
+  initMagCal(&mocs->moc, mag_cal_parameters, diverse_parameters);
   mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA;
   mocs->batch_time_in_sec =
-      (float)(min_batch_window_in_micros) * FROM_MICRO_SEC_TO_SEC;
+      (float)(mag_cal_parameters->min_batch_window_in_micros) *
+      FROM_MICRO_SEC_TO_SEC;
   // Initialize to take every sample, default setting.
   mocs->sample_drop = 0;
   magCalSphereReset(mocs);
@@ -63,6 +55,9 @@ void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias,
   sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal,
                          &mocs->sphere_fit.lm_data);
   calDataReset(&mocs->sphere_fit.sphere_param);
+
+  // Initializes the starting output data rate estimate.
+  magCalSphereOdrUpdate(mocs, default_odr_in_hz);
 }
 
 void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; }
@@ -91,7 +86,7 @@ void magCalSphereDataUpdate(struct MagCalSphere *mocs, float x, float y,
     if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) {
       memcpy(&mocs->sphere_data[mocs->number_of_data_samples *
                                 THREE_AXIS_DATA_DIM],
-             vec, sizeof(float) * 3);
+             vec, sizeof(float) * THREE_AXIS_DATA_DIM);
       // counting the numbers of samples in the data set.
       mocs->number_of_data_samples++;
     }
@@ -114,8 +109,7 @@ enum MagUpdate magCalSphereFit(struct MagCalSphere *mocs,
 
   // Running the sphere fit and checking if successful.
   if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) {
-    // Updating Sphere parameters. Can use "calDataCorrectData" function to
-    // correct data.
+    // Updating sphere parameters.
     sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal,
                           &mocs->sphere_fit.sphere_param);
 
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
index 85df48fc..3ae4687b 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
@@ -13,11 +13,12 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
 
-#include "calibration/common/sphere_fit_calibration.h"
-#include "calibration/magnetometer/mag_cal.h"
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
+
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#include "calibration/sphere_fit/sphere_fit_calibration.h"
 
 #define NUM_SPHERE_FIT_DATA 50
 
@@ -50,24 +51,17 @@ struct MagCalSphere {
   float sphere_data[THREE_AXIS_DATA_DIM * NUM_SPHERE_FIT_DATA];
 };
 
-void initMagCalSphere(struct MagCalSphere *mocs,
-                      float x_bias, float y_bias, float z_bias,
-                      float c00, float c01, float c02, float c10, float c11,
-                      float c12, float c20, float c21, float c22,
-                      uint32_t min_batch_window_in_micros,
-                      size_t min_num_diverse_vectors,
-                      size_t max_num_max_distance,
-                      float var_threshold,
-                      float max_min_threshold,
-                      float local_field,
-                      float threshold_tuning_param,
-                      float max_distance_tuning_param);
+void initMagCalSphere(
+    struct MagCalSphere *mocs,
+    const struct MagCalParameters *mag_cal_parameters,
+    const struct DiversityCheckerParameters *diverse_parameters,
+    float default_odr_in_hz);
 
 void magCalSphereDestroy(struct MagCalSphere *mocs);
 
 enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
-                                  uint64_t sample_time_us,
-                                  float x, float y, float z);
+                                  uint64_t sample_time_us, float x, float y,
+                                  float z);
 
 void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz);
 
@@ -75,4 +69,4 @@ void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz);
 }
 #endif
 
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
new file mode 100644
index 00000000..12b84701
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+
+#ifdef ACCEL_CAL_ENABLED
+#include "calibration/accelerometer/accel_cal.h"
+#endif  // ACCEL_CAL_ENABLED
+
+#ifdef GYRO_CAL_ENABLED
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/over_temp/over_temp_cal.h"
+#endif  // GYRO_CAL_ENABLED
+
+#ifdef MAG_CAL_ENABLED
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#endif  // MAG_CAL_ENABLED
+
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+//////// ACCELEROMETER CALIBRATION ////////
+#ifdef ACCEL_CAL_ENABLED
+constexpr AccelCalParameters kAccelCalParameters{
+    MSEC_TO_NANOS(800),  // t0
+    5,                   // n_s
+    15,                  // fx
+    15,                  // fxb
+    15,                  // fy
+    15,                  // fyb
+    15,                  // fz
+    15,                  // fzb
+    15,                  // fle
+    0.00025f             // th
+};
+#endif  // ACCEL_CAL_ENABLED
+
+//////// GYROSCOPE CALIBRATION ////////
+#ifdef GYRO_CAL_ENABLED
+constexpr GyroCalParameters kGyroCalParameters{
+    SEC_TO_NANOS(1.4),    // min_still_duration_nanos
+    SEC_TO_NANOS(1.4),    // max_still_duration_nanos [see, NOTE 1]
+    0,                    // calibration_time_nanos
+    MSEC_TO_NANOS(500),   // window_time_duration_nanos
+    0,                    // bias_x
+    0,                    // bias_y
+    0,                    // bias_z
+    0.95f,                // stillness_threshold
+    MDEG_TO_RAD * 60.0f,  // stillness_mean_delta_limit [rad/sec]
+    3.0e-5f,              // gyro_var_threshold [rad/sec]^2
+    3.0e-6f,              // gyro_confidence_delta [rad/sec]^2
+    4.5e-3f,              // accel_var_threshold [m/sec^2]^2
+    9.0e-4f,              // accel_confidence_delta [m/sec^2]^2
+    5.0f,                 // mag_var_threshold [uTesla]^2
+    1.0f,                 // mag_confidence_delta [uTesla]^2
+    1.5f,                 // temperature_delta_limit_celsius
+    true                  // gyro_calibration_enable
+};
+// [NOTE 1]: 'max_still_duration_nanos' is set to 1.4 seconds to achieve a max
+// stillness period of 1.5 seconds and avoid buffer boundary conditions that
+// could push the max stillness to the next multiple of the analysis window
+// length (i.e., 2.0 seconds).
+
+constexpr OverTempCalParameters kGyroOtcParameters{
+    MSEC_TO_NANOS(100),    // min_temp_update_period_nanos
+    DAYS_TO_NANOS(2),      // age_limit_nanos
+    0.75f,                 // delta_temp_per_bin
+    40.0f * MDEG_TO_RAD,   // jump_tolerance
+    100.0f * MDEG_TO_RAD,  // outlier_limit
+    250.0f * MDEG_TO_RAD,  // temp_sensitivity_limit
+    8.0e3f * MDEG_TO_RAD,  // sensor_intercept_limit
+    0.1f * MDEG_TO_RAD,    // significant_offset_change
+    5,                     // min_num_model_pts
+    true                   // over_temp_enable
+};
+#endif  // GYRO_CAL_ENABLED
+
+//////// MAGNETOMETER CALIBRATION ////////
+#ifdef MAG_CAL_ENABLED
+constexpr MagCalParameters kMagCalParameters{
+    3000000,  // min_batch_window_in_micros
+    0.0f,     // x_bias
+    0.0f,     // y_bias
+    0.0f,     // z_bias
+    1.0f,     // c00
+    0.0f,     // c01
+    0.0f,     // c02
+    0.0f,     // c10
+    1.0f,     // c11
+    0.0f,     // c12
+    0.0f,     // c20
+    0.0f,     // c21
+    1.0f      // c22
+};
+
+constexpr DiversityCheckerParameters kMagDiversityParameters{
+    6.0f,    // var_threshold
+    10.0f,   // max_min_threshold
+    48.0f,   // local_field
+    0.5f,    // threshold_tuning_param
+    2.552f,  // max_distance_tuning_param
+    8,       // min_num_diverse_vectors
+    1        // max_num_max_distance
+};
+#endif  // MAG_CAL_ENABLED
+
+}  // namespace nano_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
new file mode 100644
index 00000000..0df2ae6b
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -0,0 +1,416 @@
+/*
+ * Copyright (C) 2017 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 "calibration/nano_calibration/nano_calibration.h"
+
+#include <cstring>
+
+#include "chre/util/nanoapp/log.h"
+
+namespace nano_calibration {
+namespace {
+
+using ::online_calibration::CalibrationDataThreeAxis;
+using ::online_calibration::CalibrationTypeFlags;
+using ::online_calibration::SensorData;
+using ::online_calibration::SensorIndex;
+using ::online_calibration::SensorType;
+
+// NanoSensorCal logging macros.
+#ifdef NANO_SENSOR_CAL_DBG_ENABLED
+#ifndef LOG_TAG
+#define LOG_TAG "[ImuCal]"
+#endif
+#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__)
+#else
+#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#endif  // NANO_SENSOR_CAL_DBG_ENABLED
+
+}  // namespace
+
+void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
+                               OnlineCalibrationThreeAxis *gyro_cal,
+                               OnlineCalibrationThreeAxis *mag_cal) {
+  // Loads stored calibration data and initializes the calibration algorithms.
+  accel_cal_ = accel_cal;
+  if (accel_cal_ != nullptr) {
+    if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
+      LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
+                         &accel_cal_update_flags_, kAccelTag);
+      NANO_CAL_LOGI(kAccelTag,
+                    "Accelerometer runtime calibration initialized.");
+    } else {
+      accel_cal_ = nullptr;
+      NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+
+  gyro_cal_ = gyro_cal;
+  if (gyro_cal_ != nullptr) {
+    if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
+      LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_,
+                         &gyro_cal_update_flags_, kGyroTag);
+      NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
+    } else {
+      gyro_cal_ = nullptr;
+      NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+
+  mag_cal_ = mag_cal;
+  if (mag_cal != nullptr) {
+    if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
+      LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
+                         &mag_cal_update_flags_, kMagTag);
+      NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
+    } else {
+      mag_cal_ = nullptr;
+      NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+}
+
+void NanoSensorCal::HandleSensorSamples(
+    uint16_t event_type, const chreSensorThreeAxisData *event_data) {
+  // Converts CHRE Event -> SensorData::SensorType.
+  SensorData sample;
+  switch (event_type) {
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
+      sample.type = SensorType::kAccelerometerMps2;
+      break;
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
+      sample.type = SensorType::kGyroscopeRps;
+      break;
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
+      sample.type = SensorType::kMagnetometerUt;
+      break;
+    default:
+      // This sensor type is not used.
+      NANO_CAL_LOGW("[NanoSensorCal]",
+                    "Unexpected 3-axis sensor type received.");
+      return;
+  }
+
+  // Sends the sensor payload to the calibration algorithms and checks for
+  // calibration updates.
+  const auto &header = event_data->header;
+  const auto *data = event_data->readings;
+  sample.timestamp_nanos = header.baseTimestamp;
+  for (size_t i = 0; i < header.readingCount; i++) {
+    sample.timestamp_nanos += data[i].timestampDelta;
+    memcpy(sample.data, data[i].v, sizeof(sample.data));
+    ProcessSample(sample);
+  }
+}
+
+void NanoSensorCal::HandleTemperatureSamples(
+    uint16_t event_type, const chreSensorFloatData *event_data) {
+  // Computes the mean of the batched temperature samples and delivers it to the
+  // calibration algorithms. Note, the temperature sensor batch size determines
+  // its minimum update interval.
+  if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
+      event_data->header.readingCount > 0) {
+    const auto header = event_data->header;
+    const auto *data = event_data->readings;
+
+    SensorData sample;
+    sample.type = SensorType::kTemperatureCelsius;
+    sample.timestamp_nanos = header.baseTimestamp;
+
+    float accum_temperature_celsius = 0.0f;
+    for (size_t i = 0; i < header.readingCount; i++) {
+      sample.timestamp_nanos += data[i].timestampDelta;
+      accum_temperature_celsius += data[i].value;
+    }
+    sample.data[SensorIndex::kSingleAxis] =
+        accum_temperature_celsius / header.readingCount;
+    ProcessSample(sample);
+  } else {
+    NANO_CAL_LOGW("[NanoSensorCal]",
+                  "Unexpected single-axis sensor type received.");
+  }
+}
+
+void NanoSensorCal::ProcessSample(const SensorData &sample) {
+  // Sends a new sensor sample to each active calibration algorithm and sends
+  // out notifications for new calibration updates.
+  if (accel_cal_ != nullptr) {
+    const CalibrationTypeFlags new_cal_flags =
+        accel_cal_->SetMeasurement(sample);
+    if (new_cal_flags != CalibrationTypeFlags::NONE) {
+      accel_cal_update_flags_ |= new_cal_flags;
+      NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
+                           accel_cal_->GetSensorCalibration(),
+                           accel_cal_update_flags_, kAccelTag);
+      PrintCalibration(accel_cal_->GetSensorCalibration(),
+                       accel_cal_update_flags_, kAccelTag);
+    }
+  }
+
+  if (gyro_cal_ != nullptr) {
+    const CalibrationTypeFlags new_cal_flags =
+        gyro_cal_->SetMeasurement(sample);
+    if (new_cal_flags != CalibrationTypeFlags::NONE) {
+      gyro_cal_update_flags_ |= new_cal_flags;
+      if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
+                               gyro_cal_->GetSensorCalibration(),
+                               gyro_cal_update_flags_, kGyroTag)) {
+        // Limits the log messaging update rate for the gyro calibrations since
+        // these can occur frequently with rapid temperature changes.
+        if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+                sample.timestamp_nanos, gyro_notification_time_nanos_,
+                kNanoSensorCalMessageIntervalNanos)) {
+          gyro_notification_time_nanos_ = sample.timestamp_nanos;
+          PrintCalibration(gyro_cal_->GetSensorCalibration(),
+                           gyro_cal_update_flags_, kGyroTag);
+        }
+      }
+    }
+  }
+
+  if (mag_cal_ != nullptr) {
+    const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample);
+    if (new_cal_flags != CalibrationTypeFlags::NONE) {
+      mag_cal_update_flags_ |= new_cal_flags;
+      NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
+                           mag_cal_->GetSensorCalibration(),
+                           mag_cal_update_flags_, kMagTag);
+      PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_,
+                       kMagTag);
+    }
+  }
+}
+
+bool NanoSensorCal::NotifyAshCalibration(
+    uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
+    CalibrationTypeFlags flags, const char *sensor_tag) {
+  // Updates the sensor offset calibration using the ASH API.
+  ashCalInfo ash_cal_info;
+  memset(&ash_cal_info, 0, sizeof(ashCalInfo));
+  ash_cal_info.compMatrix[0] = 1.0f;  // Sets diagonal to unity (scale factor).
+  ash_cal_info.compMatrix[4] = 1.0f;
+  ash_cal_info.compMatrix[8] = 1.0f;
+  memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias));
+
+  // Maps CalibrationQualityLevel to ASH calibration accuracy.
+  switch (cal_data.calibration_quality.level) {
+    case online_calibration::CalibrationQualityLevel::HIGH_QUALITY:
+      ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
+      break;
+
+    case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY:
+      ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM;
+      break;
+
+    case online_calibration::CalibrationQualityLevel::LOW_QUALITY:
+      ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW;
+      break;
+
+    default:
+      ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE;
+      break;
+  }
+
+  if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
+    return false;
+  }
+
+  // Uses the ASH API to store all calibration parameters relevant to a given
+  // algorithm as indicated by the input calibration type flags.
+  ashCalParams ash_cal_parameters;
+  memset(&ash_cal_parameters, 0, sizeof(ashCalParams));
+  if (flags & CalibrationTypeFlags::BIAS) {
+    ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius;
+    memcpy(ash_cal_parameters.offset, cal_data.offset,
+           sizeof(ash_cal_parameters.offset));
+    ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+    ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+  }
+
+  if (flags & CalibrationTypeFlags::OVER_TEMP) {
+    memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity,
+           sizeof(ash_cal_parameters.tempSensitivity));
+    memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept,
+           sizeof(ash_cal_parameters.tempIntercept));
+    ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+    ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+  }
+
+  if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
+    return false;
+  }
+
+  return true;
+}
+
+bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
+                                       OnlineCalibrationThreeAxis *online_cal,
+                                       CalibrationTypeFlags* flags,
+                                       const char *sensor_tag) {
+  ashCalParams recalled_ash_cal_parameters;
+  if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
+                               &recalled_ash_cal_parameters)) {
+    // Checks whether a valid set of runtime calibration parameters was received
+    // and can be used for initialization.
+    if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags,
+                                 &recalled_ash_cal_parameters)) {
+      CalibrationDataThreeAxis cal_data;
+      cal_data.type = online_cal->get_sensor_type();
+      cal_data.cal_update_time_nanos = chreGetTime();
+
+      // Analyzes the calibration flags and sets only the runtime calibration
+      // values that were received.
+      if (*flags & CalibrationTypeFlags::BIAS) {
+        cal_data.offset_temp_celsius =
+            recalled_ash_cal_parameters.offsetTempCelsius;
+        memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
+               sizeof(cal_data.offset));
+      }
+
+      if (*flags & CalibrationTypeFlags::OVER_TEMP) {
+        memcpy(cal_data.temp_sensitivity,
+               recalled_ash_cal_parameters.tempSensitivity,
+               sizeof(cal_data.temp_sensitivity));
+        memcpy(cal_data.temp_intercept,
+               recalled_ash_cal_parameters.tempIntercept,
+               sizeof(cal_data.temp_intercept));
+      }
+
+      // Sets the algorithm's initial calibration data and notifies ASH to apply
+      // the recalled calibration data.
+      if (online_cal->SetInitialCalibration(cal_data)) {
+        return NotifyAshCalibration(chreSensorType,
+                                    online_cal->GetSensorCalibration(), *flags,
+                                    sensor_tag);
+      } else {
+        NANO_CAL_LOGE(sensor_tag,
+                      "Calibration data failed to initialize algorithm.");
+      }
+    }
+  } else {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data.");
+  }
+
+  return false;
+}
+
+bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType,
+                                             const char *sensor_tag,
+                                             CalibrationTypeFlags *flags,
+                                             ashCalParams *ash_cal_parameters) {
+  // Analyzes calibration source flags to determine whether runtime
+  // calibration values have been loaded and may be used for initialization. A
+  // valid runtime calibration source will include at least an offset.
+  *flags = CalibrationTypeFlags::NONE;  // Resets the calibration flags.
+
+  // Uses the ASH calibration source flags to set the appropriate
+  // CalibrationTypeFlags. These will be used to determine which values to copy
+  // from 'ash_cal_parameters' and provide to the calibration algorithms for
+  // initialization.
+  bool runtime_cal_detected = false;
+  if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+      ash_cal_parameters->offsetTempCelsiusSource ==
+          ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    runtime_cal_detected = true;
+    *flags = CalibrationTypeFlags::BIAS;
+  }
+
+  if (ash_cal_parameters->tempSensitivitySource ==
+          ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+      ash_cal_parameters->tempInterceptSource ==
+          ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    *flags |= CalibrationTypeFlags::OVER_TEMP;
+  }
+
+  if (runtime_cal_detected) {
+    // Prints the retrieved runtime calibration data.
+    NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected.");
+    PrintAshCalParams(*ash_cal_parameters, sensor_tag);
+  } else {
+    // This is a warning (not an error) since the runtime algorithms will
+    // function correctly with no recalled calibration values. They will
+    // eventually trigger and update the system with valid calibration data.
+    NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
+  }
+
+  return runtime_cal_detected;
+}
+
+// Helper functions for logging calibration information.
+void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params,
+                                      const char *sensor_tag) {
+  if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    NANO_CAL_LOGI(sensor_tag,
+                  "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+                  cal_params.offset[0], cal_params.offset[1],
+                  cal_params.offset[2], cal_params.offsetTempCelsius);
+  }
+
+  if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
+                  cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
+                  cal_params.tempSensitivity[2]);
+  }
+
+  if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
+                  cal_params.tempIntercept[0], cal_params.tempIntercept[1],
+                  cal_params.tempIntercept[2]);
+  }
+
+  if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
+                  cal_params.scaleFactor[0], cal_params.scaleFactor[1],
+                  cal_params.scaleFactor[2]);
+  }
+
+  if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+    NANO_CAL_LOGI(sensor_tag,
+                  "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
+                  cal_params.crossAxis[0], cal_params.crossAxis[1],
+                  cal_params.crossAxis[2]);
+  }
+}
+
+void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
+                                     CalibrationTypeFlags flags,
+                                     const char *sensor_tag) {
+  if (flags & CalibrationTypeFlags::BIAS) {
+    NANO_CAL_LOGI(sensor_tag,
+                  "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+                  cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
+                  cal_data.offset_temp_celsius);
+  }
+
+  if (flags & CalibrationTypeFlags::OVER_TEMP) {
+    NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f",
+                  cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1],
+                  cal_data.temp_sensitivity[2]);
+    NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f",
+                  cal_data.temp_intercept[0], cal_data.temp_intercept[1],
+                  cal_data.temp_intercept[2]);
+  }
+}
+
+}  // namespace nano_calibration
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
new file mode 100644
index 00000000..d56d0347
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2017 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.
+ */
+
+/*
+ * This module provides a containing class (NanoSensorCal) for dynamic runtime
+ * calibration algorithms that affect the following sensors:
+ *       - Accelerometer (offset)
+ *       - Gyroscope (offset, with over-temperature compensation)
+ *       - Magnetometer (offset)
+ *
+ * Sensor Units:
+ *       - Accelerometer [meters/sec^2]
+ *       - Gyroscope     [radian/sec]
+ *       - Magnetometer  [micro Tesla, uT]
+ *       - Temperature   [Celsius].
+ *
+ * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging.
+ *
+ * NOTE2: This module uses pointers to runtime calibration algorithm objects.
+ * These must be constructed and initialized outside of this class. The owner
+ * bares the burden of managing the lifetime of these objects with respect to
+ * the NanoSensorCal class which depends on these objects and handles their
+ * interaction with the Android ASH/CHRE system. This arrangement makes it
+ * convenient to modify the specific algorithm implementations (i.e., choice of
+ * calibration algorithm, parameter tuning, etc.) at the nanoapp level without
+ * the need to specialize the standard functionality implemented here.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <ash.h>
+#include <chre.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+// Common log message sensor-specific identifiers.
+constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
+constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
+constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+
+// Limits NanoSensorCal notifications to once every minute.
+constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
+
+/*
+ * NanoSensorCal is a container class for dynamic runtime calibration sensor
+ * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
+ * is to transfer sensor data to the sensor calibration algorithms and provide
+ * calibration updates to CHRE using the ASH API.
+ */
+class NanoSensorCal {
+ public:
+  // Alias used to reference the three-axis OnlineCalibration baseclass used by
+  // the runtime calibration sensor wrappers. This is for convenience and to
+  // help with code readability.
+  using OnlineCalibrationThreeAxis = online_calibration::OnlineCalibration<
+      online_calibration::CalibrationDataThreeAxis>;
+
+  NanoSensorCal() = default;
+
+  // Sets the sensor calibration object pointers and initializes the algorithms
+  // using runtime values recalled using Android Sensor Hub (ASH). A nullptr may
+  // be passed in to disable a particular sensor calibration.
+  void Initialize(OnlineCalibrationThreeAxis *accel_cal,
+                  OnlineCalibrationThreeAxis *gyro_cal,
+                  OnlineCalibrationThreeAxis *mag_cal);
+
+  // Sends new sensor samples to the calibration algorithms.
+  void HandleSensorSamples(uint16_t event_type,
+                           const chreSensorThreeAxisData *event_data);
+
+  // Provides temperature updates to the calibration algorithms.
+  void HandleTemperatureSamples(uint16_t event_type,
+                                const chreSensorFloatData *event_data);
+
+ private:
+  // Passes sensor data to the runtime calibration algorithms.
+  void ProcessSample(const online_calibration::SensorData &sample);
+
+  // Loads runtime calibration data using the Android Sensor Hub API. Returns
+  // 'true' when runtime calibration values were successfully recalled and used
+  // for algorithm initialization. 'sensor_tag' is a string that identifies a
+  // sensor-specific identifier for log messages. Updates 'flags' to indicate
+  // which runtime calibration parameters were recalled.
+  bool LoadAshCalibration(uint8_t chreSensorType,
+                          OnlineCalibrationThreeAxis *online_cal,
+                          online_calibration::CalibrationTypeFlags* flags,
+                          const char *sensor_tag);
+
+  // Provides sensor calibration updates using the ASH API for the specified
+  // sensor type. 'cal_data' contains the new calibration data. 'flags' is used
+  // to indicate all of the valid calibration values that should be provided
+  // with the update. Returns 'true' with a successful ASH update.
+  bool NotifyAshCalibration(
+      uint8_t chreSensorType,
+      const online_calibration::CalibrationDataThreeAxis &cal_data,
+      online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+  // Checks whether 'ash_cal_parameters' is a valid set of runtime calibration
+  // data and can be used for algorithm initialization. Updates 'flags' to
+  // indicate which runtime calibration parameters were detected.
+  bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag,
+                                online_calibration::CalibrationTypeFlags *flags,
+                                ashCalParams *ash_cal_parameters);
+
+  // Helper functions for logging calibration information.
+  void PrintAshCalParams(const ashCalParams &cal_params,
+                         const char *sensor_tag);
+
+  void PrintCalibration(
+      const online_calibration::CalibrationDataThreeAxis &cal_data,
+      online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+  // Pointer to the accelerometer runtime calibration object.
+  OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
+
+  // Pointer to the gyroscope runtime calibration object.
+  OnlineCalibrationThreeAxis *gyro_cal_ = nullptr;
+
+  // Limits the log messaging update rate for the gyro calibrations since these
+  // can occur frequently with rapid temperature changes.
+  uint64_t gyro_notification_time_nanos_ = 0;
+
+  // Pointer to the magnetometer runtime calibration object.
+  OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
+
+  // Flags that determine which calibration elements are updated with the ASH
+  // API. These are reset during initialization, and latched when a particular
+  // calibration update is detected upon a valid recall of parameters and/or
+  // during runtime. The latching behavior is used to start sending calibration
+  // values of a given type (e.g., bias, over-temp model, etc.) once they are
+  // detected and thereafter.
+  online_calibration::CalibrationTypeFlags accel_cal_update_flags_ =
+      online_calibration::CalibrationTypeFlags::NONE;
+  online_calibration::CalibrationTypeFlags gyro_cal_update_flags_ =
+      online_calibration::CalibrationTypeFlags::NONE;
+  online_calibration::CalibrationTypeFlags mag_cal_update_flags_ =
+      online_calibration::CalibrationTypeFlags::NONE;
+};
+
+}  // namespace nano_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
new file mode 100644
index 00000000..e9def4e6
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2018 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 "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+void AccelOffsetCal::Initialize(
+    const AccelCalParameters& accel_cal_parameters) {
+  accelCalInit(&accel_cal_, &accel_cal_parameters);
+  InitializeCalData();
+}
+
+CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) {
+  // Routes the input sensor sample to the calibration algorithm.
+  switch (sample.type) {
+    case SensorType::kAccelerometerMps2:
+      accelCalRun(&accel_cal_, sample.timestamp_nanos,
+                  sample.data[SensorIndex::kXAxis],
+                  sample.data[SensorIndex::kYAxis],
+                  sample.data[SensorIndex::kZAxis],  // [m/sec^2]
+                  temperature_celsius_);
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+      // Prints debug data report.
+      accelCalDebPrint(&accel_cal_, temperature_celsius_);
+#endif
+      break;
+
+    case SensorType::kTemperatureCelsius:
+      temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+      break;
+
+    default:
+      // This sample is not required.
+      return cal_update_polling_flags_;
+  }
+
+  // Checks for a new offset estimate, and updates the calibration data.
+  if (accelCalNewBiasAvailable(&accel_cal_)) {
+    accelCalUpdateBias(&accel_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+                       &cal_data_.offset[2]);
+
+    cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+    cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality;
+    cal_data_.offset_temp_celsius = temperature_celsius_;
+    cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+    cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+    OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+  }
+
+  return cal_update_polling_flags_;
+}
+
+bool AccelOffsetCal::SetInitialCalibration(
+    const CalibrationDataThreeAxis& input_cal_data) {
+  // Checks that the input calibration type matches the algorithm type.
+  if (input_cal_data.type != get_sensor_type()) {
+    CAL_DEBUG_LOG("[AccelOffsetCal]",
+                  "SetInitialCalibration failed due to wrong sensor type.");
+    return false;
+  }
+
+  // Sets the accelerometer algorithm's calibration data.
+  accelCalBiasSet(&accel_cal_, input_cal_data.offset[0],
+                  input_cal_data.offset[1], input_cal_data.offset[2]);
+
+  // Sync's all initial calibration data.
+  cal_data_ = input_cal_data;
+
+  // Sets the calibration quality.
+  cal_data_.calibration_quality.level = CalibrationQualityLevel::LOW_QUALITY;
+  cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality;
+
+  return true;
+}
+
+}  // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
new file mode 100644
index 00000000..0256495a
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+
+#include "calibration/accelerometer/accel_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for accelerometer offset calibration.
+ *
+ * NOTE: Calibration quality reporting (quantitative metric is not used):
+ *   Initialize             --> CalibrationQualityLevel::UNDETERMINED
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ *   SetInitialCalibration  --> CalibrationQualityLevel::LOW_QUALITY
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ *   New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ */
+class AccelOffsetCal final
+    : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+  // Default constructor.
+  AccelOffsetCal() = default;
+
+  // Creates an AccelOffsetCal with specified algorithm parameters.
+  explicit AccelOffsetCal(const AccelCalParameters& accel_cal_parameters) {
+    Initialize(accel_cal_parameters);
+  }
+
+  // Initializes with specified algorithm parameters, and resets the calibration
+  // data.
+  void Initialize(const AccelCalParameters& accel_cal_parameters);
+
+  // Sends new sensor data to the calibration algorithm, and returns the state
+  // of the calibration update flags, 'cal_update_polling_flags_'.
+  CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+  // Sets the initial calibration data of the calibration algorithm. Returns
+  // true if set successfully.
+  bool SetInitialCalibration(
+      const CalibrationDataThreeAxis& input_cal_data) final;
+
+  // Returns the calibration sensor type.
+  SensorType get_sensor_type() const final {
+    return SensorType::kAccelerometerMps2;
+  };
+
+ private:
+  // Accelerometer offset calibration algorithm data structure.
+  AccelCal accel_cal_;
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
new file mode 100644
index 00000000..62bc443d
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+/*
+ * This module provides the callback functionality employed by the online sensor
+ * calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+
+#include "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+/*
+ * This codebase avoids Standard Template Library (STL) containers to maximize
+ * its usefullness on embedded hardware with basic C++ compiler support. The
+ * following uses type erasure to implement callback functionality to a user's
+ * desired class method. The idea here is to store an object pointer by
+ * instantiating a class template (Callback) which implements a virtual
+ * interface function (CallbackInterface::Call).
+ *
+ * USAGE:
+ * See unit testing for a simple example of how to use this for callback
+ * functionality.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class CallbackInterface {
+ public:
+  // Interface function that is defined to implement the desired callback.
+  virtual void Call(const CalibrationType& cal_data,
+                    CalibrationTypeFlags cal_update_flags) = 0;
+  virtual ~CallbackInterface() {}
+};
+
+// ClassCallerType: Sets the object's class type for the callback.
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class ClassCallerType, class CalibrationType>
+class Callback : public CallbackInterface<CalibrationType> {
+ public:
+  // Constructors.
+  Callback() = delete;
+  Callback(ClassCallerType* obj,
+           void (ClassCallerType::*func)(const CalibrationType& cal_data,
+                                         CalibrationTypeFlags cal_update_flags))
+      : object_ptr_(obj), function_ptr_(func) {}
+
+  // Implements the callback to the desired class-method.
+  void Call(const CalibrationType& cal_data,
+            CalibrationTypeFlags cal_update_flags) final {
+    (object_ptr_->*function_ptr_)(cal_data, cal_update_flags);
+  }
+
+ private:
+  // Pointer to the class that owns the callback method.
+  ClassCallerType* object_ptr_;
+
+  // Templated function pointer with the required function signature.
+  // Calibration callbacks must accept:
+  //   1. Constant reference to the calibration.
+  //   2. Bitmask indicating which calibration components have been updated.
+  void (ClassCallerType::*function_ptr_)(const CalibrationType& cal_data,
+                                         CalibrationTypeFlags cal_update_flags);
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
new file mode 100644
index 00000000..8f1470a8
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2018 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 "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+                               CalibrationTypeFlags rhs) {
+  return static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+                                           static_cast<char>(rhs));
+}
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs) {
+  return static_cast<char>(lhs) & static_cast<char>(rhs);
+}
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+                                 CalibrationTypeFlags rhs) {
+  lhs = static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+                                          static_cast<char>(rhs));
+  return lhs;
+}
+
+}  // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
new file mode 100644
index 00000000..0404936b
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+/*
+ * This module provides the component definitions used to represent sensor
+ * calibration data, labeled flags/enumerators, and the callback functionality
+ * employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "calibration/online_calibration/common_data/calibration_quality.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_model.h"
+
+namespace online_calibration {
+
+/*
+ * Bitmask used to indicate which calibration values have changed for a given
+ * calibration update.
+ *
+ * [Bit Flag]   | [Affected Sensor Calibration Value]
+ * BIAS             - Quasi-static non-zero sensor output (offset), given
+ *                    conditions where the sensor should ideally output zero.
+ *                    Includes corrections for over-temperature compensation.
+ * SCALE_FACTOR     - Sensor axis scaling (ideally unity).
+ * CROSS_AXIS       - Output sensitivity due to variations of perpendicular
+ *                    sensing axes (ideally zero).
+ * OVER_TEMP        - Model parameters that capture variations in sensor
+ *                    behavior with temperature (e.g., linear bias sensitivity
+ *                    model).
+ * QUALITY_DEGRADED - Indicates a degradation in calibration quality.
+ */
+enum class CalibrationTypeFlags : uint8_t {
+  NONE = 0x00,
+  BIAS = 0x01,
+  SCALE_FACTOR = 0x02,
+  CROSS_AXIS = 0x04,
+  OVER_TEMP = 0x08,
+  QUALITY_DEGRADED = 0x10,
+  ALL = 0xFF
+};
+
+// Logic operators to assist with common bitmask setting/checking.
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+                               CalibrationTypeFlags rhs);
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs);
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+                                 CalibrationTypeFlags rhs);
+
+/*
+ * Defines the calibration data specific to a prototypical three-axis sensing
+ * device (e.g., accelerometer, gyroscope, magnetometer).
+ *
+ * Calibration correction may be applied as:
+ *   corrected_data = scale_skew_matrix * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied
+ * when supported by the underlying calibration algorithm).
+ *
+ * The 'scale_skew_matrix' is assumed to be in lower diagonal form where the
+ * sensor frame reference definition is such that cross-axis sensitivities
+ * cross_axis_xy, cross_axis_xz, and cross_axis_yz are set to zero.
+ *
+ *   scale_skew_matrix = [scale_factor_x    0                 0
+ *                        cross_axis_yx     scale_factor_y    0
+ *                        cross_axis_zx     cross_axis_zy     scale_factor_z]
+ *
+ * NOTE1: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ *   temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ *   temp_intercept   - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ *   compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ *
+ * NOTE2: Unless otherwise stated, 3-dimensional array indices: 0=x, 1=y, 2=z.
+ */
+struct CalibrationDataThreeAxis {
+  // Timestamp for the most recent calibration update.
+  uint64_t cal_update_time_nanos = 0;
+
+  // The sensor's offset (i.e., bias) in the x-, y-, z-axes at
+  // 'offset_temp_celsius'.
+  float offset[3];
+
+  // The temperature associated with the sensor offset.
+  float offset_temp_celsius;
+
+  // The temperature sensitivity of the offset.
+  float temp_sensitivity[3];  // [sensor_units/Celsius]
+
+  // The sensor's offset at zero degrees Celsius.
+  float temp_intercept[3];  // [sensor_units]
+
+  // The sensor's scale factor for each axis.
+  float scale_factor[3];
+
+  // The cross-axis factors in order: [0=yx, 1=zx, 2=zy].
+  float cross_axis[3];
+
+  // Metrics used for the reported calibration accuracy. The behavior and
+  // definition depends on the sensor calibration type. See the calibration
+  // algorithm documentation for details.
+  CalibrationQuality calibration_quality;
+
+  // Indicates the type of sensing device being calibrated.
+  SensorType type = SensorType::kUndefined;
+
+  // Optional pointer to an array of over-temperature model data (null when not
+  // used). For initialization, populating a model dataset will take precedence
+  // over the linear model parameters provided in the calibration data.
+  OverTempModelThreeAxis* otc_model_data = nullptr;
+  int16_t num_model_pts = 0;
+
+  // Helper function that resets the calibration data to a set of neutral
+  // reference values where no calibration correction would be applied if used.
+  void reset() {
+    otc_model_data = nullptr;
+    calibration_quality.reset();
+    offset_temp_celsius = 0.0f;
+    scale_factor[0] = 1.0f;
+    scale_factor[1] = 1.0f;
+    scale_factor[2] = 1.0f;
+    memset(offset, 0, sizeof(offset));
+    memset(temp_sensitivity, 0, sizeof(temp_sensitivity));
+    memset(temp_intercept, 0, sizeof(temp_intercept));
+    memset(cross_axis, 0, sizeof(cross_axis));
+  }
+
+  CalibrationDataThreeAxis() { reset(); }
+};
+
+/*
+ * Defines the calibration data for single dimensional sensing device (e.g.,
+ * thermometer, barometer).
+ *
+ * Calibration correction may be applied as:
+ *   corrected_data = scale_factor * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied,
+ * if supported by the underlying calibration algorithm).
+ *
+ * NOTE: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ *   temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ *   temp_intercept   - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ *   compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ */
+struct CalibrationDataSingleAxis {
+  // Timestamp for the most recent calibration update.
+  uint64_t cal_update_time_nanos = 0;
+
+  // The sensor's offset (i.e., bias) at temperature, 'offset_temp_celsius'.
+  float offset;
+
+  // The temperature associated with the sensor offset.
+  float offset_temp_celsius;
+
+  // The temperature sensitivity of the offset.
+  float temp_sensitivity;  // [sensor_units/Celsius]
+
+  // The sensor's offset at zero degrees Celsius.
+  float temp_intercept;  // [sensor_units]
+
+  // The sensor's scale factor.
+  float scale_factor;
+
+  // Metrics used for the reported calibration accuracy. The behavior and
+  // definition depends on the sensor calibration type. See the calibration
+  // algorithm documentation for details.
+  CalibrationQuality calibration_quality;
+
+  // Indicates the type of sensing device being calibrated.
+  SensorType type = SensorType::kUndefined;
+
+  // Optional pointer to an array of over-temperature model data (null when not
+  // used). For initialization, populating a model dataset will take precedence
+  // over the linear model parameters provided in the calibration data.
+  OverTempModelSingleAxis* otc_model_data = nullptr;
+  int16_t num_model_pts = 0;
+
+  // Helper function that resets the calibration data to a set of neutral
+  // reference values where no calibration correction would be applied if used.
+  void reset() {
+    otc_model_data = nullptr;
+    calibration_quality.reset();
+    scale_factor = 1.0f;
+    offset_temp_celsius = 0.0f;
+    offset = 0.0f;
+    temp_sensitivity = 0.0f;
+    temp_intercept = 0.0f;
+  }
+
+  CalibrationDataSingleAxis() { reset(); }
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h
new file mode 100644
index 00000000..d6475c78
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+/*
+ * This module provides quality definitions that represent the accuracy
+ * associated with calibration data. This information may be used to affect how
+ * a system uses available sensor calibration data.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
+
+#include <float.h>
+#include <stdint.h>
+
+namespace online_calibration {
+
+/*
+ * In general, calibration quality values may be thought of in terms of
+ * something akin to an error standard deviation. That is, it's a non-negative
+ * value where lower values imply higher calibration quality, and larger values
+ * indicate poorer quality. However, the units and numerical interpretation is
+ * ultimately dictated by the sensor type and calibration algorithm. Consult the
+ * calibration code implementation for the actual calibration quality metric
+ * details.
+ */
+
+/*
+ * Bitmask used to provide a qualitative ranking of the calibration data's
+ * accuracy. Many systems use this sort of interpretation for calibration
+ * quality (e.g., Android).
+ *
+ * [Bit Flag]   | [Qualitative Calibration Quality]
+ * UNDETERMINED    - Calibration quality has not yet been determined (e.g., a
+ *                   calibration hasn't occurred, or a metric hasn't been
+ *                   established).
+ * LOW_QUALITY     - Sensor calibration is needed. System properties have
+ *                   changed that may have affected the applicability of the
+ *                   current calibration (e.g., calibration data is old, anomaly
+ *                   detected, etc.).
+ * MEDIUM_QUALITY  - The reported sensor calibration has an average level of
+ *                   accuracy, updated calibration may improve the readings.
+ * HIGH_QUALITY    - The reported calibration has maximal accuracy.
+ */
+enum class CalibrationQualityLevel : uint8_t {
+  UNDETERMINED = 0x00,
+  LOW_QUALITY = 0x01,
+  MEDIUM_QUALITY = 0x02,
+  HIGH_QUALITY = 0x04,
+};
+
+// Sets the calibration quality value when this metric is either not
+// implemented, or has not yet been determined (e.g., a calibration hasn't
+// occurred).
+constexpr float kUndeterminedCalibrationQuality = -1.0f;
+
+/*
+ * Calibration quality structure that contains a quantitative (float) and
+ * qualitative (enum) measure of a sensor's calibration accuracy. Both entries
+ * should be co-defined by the algorithm providing the calibration update. The
+ * enum sets the qualitative interpretation of the float value, this is often
+ * used in systems (Android, etc.) to label quality and affect the use of the
+ * calibration data.
+ */
+struct CalibrationQuality {
+  // Provides a qualitative measure for sensor calibration accuracy.
+  CalibrationQualityLevel level = CalibrationQualityLevel::UNDETERMINED;
+
+  // Quantitative metric for the reported calibration accuracy. The behavior and
+  // definition depends on the sensor calibration type. See the calibration
+  // algorithm documentation for details.
+  float value = kUndeterminedCalibrationQuality;
+
+  // Helper function that resets the calibration quality to an initial state.
+  void reset() {
+    level = CalibrationQualityLevel::UNDETERMINED;
+    value = kUndeterminedCalibrationQuality;
+  }
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
new file mode 100644
index 00000000..59e26bae
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+
+#include <string.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This abstract base class provides a set of general interface functions for
+ * calibration algorithms. The data structures used are intended to be lean and
+ * portable to a wide variety of software and hardware systems. Algorithm
+ * wrappers may use this as a basis for providing the following functionality:
+ *
+ *   SetMeasurement - Delivers new sensor data.
+ *   SetInitialCalibration - Initializes the algorithm's calibration data.
+ *   GetSensorCalibration - Retrieves the latest calibration data set.
+ *   new_calibration_ready - Used to poll for new calibration updates.
+ *   SetCalibrationCallback - User provides a pointer its own Callback object.
+ *   UpdateDynamicSystemSettings - Provides feedback to adjust system behavior.
+ *   get_sensor_type - Returns the sensor type which is being calibrated.
+ *
+ * NOTE 1: This class accomodates two methods of providing calibration updates.
+ * Either, or both, may be used depending on system requirements. 1) Polling can
+ * be achieved with new_calibration_ready/GetSensorCalibration functions. 2)
+ * Callback notification of new calibration updates can managed using the
+ * SetCalibrationCallback function.
+ *
+ * NOTE 2: This code implementation specifically avoids using standard template
+ * libraries (STL) and other external API’s since they may not be fully
+ * supported on embedded hardware targets. Only basic C/C++ support will be
+ * assumed.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class OnlineCalibration {
+ public:
+  // Virtual destructor.
+  virtual ~OnlineCalibration() {}
+
+  // Sends new sensor data to the calibration algorithm, and returns the state
+  // of the calibration update flags, 'cal_update_polling_flags_'.
+  virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0;
+
+  // Sets the initial calibration data of the calibration algorithm. Returns
+  // "true" if set successfully.
+  virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0;
+
+  // Polling Updates: New calibration updates are generated during
+  // SetMeasurement and the 'cal_update_polling_flags_' are set according to
+  // which calibration values have changed. To prevent missing updates in
+  // systems that use polling, this bitmask remains latched until the
+  // calibration data is retrieved with this function.
+  const CalibrationType& GetSensorCalibration() const {
+    cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+    return cal_data_;
+  }
+
+  // Polling Updates: This function returns 'cal_update_polling_flags_' to
+  // indicate which calibration components have a pending update. The updated
+  // calibration data may be retrieved with GetSensorCalibration, and the
+  // 'cal_update_polling_flags_' will reset.
+  CalibrationTypeFlags new_calibration_ready() const {
+    return cal_update_polling_flags_;
+  }
+
+  // Sets the pointer to the CallbackInterface object used for notification of
+  // new calibration updates.
+  void SetCalibrationCallback(
+      CallbackInterface<CalibrationType>* calibration_callback) {
+    calibration_callback_ = calibration_callback;
+  }
+
+  // Returns the sensor-type this calibration algorithm provides updates for.
+  virtual SensorType get_sensor_type() const = 0;
+
+ protected:
+  // Helper function that activates the registered callback.
+  void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const {
+    if (calibration_callback_ != nullptr) {
+      calibration_callback_->Call(cal_data_, cal_update_flags);
+    }
+  }
+
+  // Helper function used to initialize the calibration data.
+  void InitializeCalData() {
+    cal_data_.reset();
+    cal_data_.type = get_sensor_type();
+    cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+  }
+
+  // Stores the sensor calibration data.
+  CalibrationType cal_data_;
+
+  // Tracks the most recent sensor temperature value.
+  float temperature_celsius_ = kInvalidTemperatureCelsius;
+
+  // This bitmask indicates which subset of calibration parameters have changed
+  // and is used specifically for polling; the callback notification passes its
+  // own set of update flags which do not need this latching behavior. Marked
+  // mutable so that these flags may be reset when GetSensorCalibration is
+  // called.
+  mutable CalibrationTypeFlags cal_update_polling_flags_ =
+      CalibrationTypeFlags::NONE;
+
+ private:
+  // Pointer to a callback object.
+  CallbackInterface<CalibrationType>* calibration_callback_ = nullptr;
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
new file mode 100644
index 00000000..1331153e
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+/*
+ * This module provides the component definitions used to represent sensor
+ * data employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "common/math/macros.h"
+
+namespace online_calibration {
+
+// Defines an invalid or uninitialized temperature value (referenced from
+// common/math/macros.h).
+constexpr float kInvalidTemperatureCelsius = INVALID_TEMPERATURE_CELSIUS;
+
+// Unit conversion from nanoseconds to microseconds.
+constexpr uint64_t NanoToMicroseconds(uint64_t x) { return x / 1000; }
+
+// Identifies the various sensing devices used by the calibration algorithms.
+enum class SensorType : int8_t {
+  kUndefined = 0,
+  kAccelerometerMps2 = 1,   // 3-axis sensor (units = meter/sec^2).
+  kGyroscopeRps = 2,        // 3-axis sensor (units = radian/sec).
+  kMagnetometerUt = 3,      // 3-axis sensor (units = micro-Tesla).
+  kTemperatureCelsius = 4,  // 1-axis sensor (units = degrees Celsius).
+  kBarometerHpa = 5,        // 1-axis sensor (units = hecto-Pascal).
+  kWifiM = 6                // 3-axis sensor (units = meter).
+};
+
+/*
+ * SensorData is a generalized data structure used to represent sensor samples
+ * produced by either a single- or three-axis device. Usage is implied through
+ * the sensor type (i.e., Gyroscope is a three-axis sensor and would therefore
+ * use all elements of 'data'; a pressure sensor is single-dimensional and would
+ * use 'data[SensorIndex::kSingleAxis]'). This arbitration is determined
+ * at the algorithm wrapper level where knowledge of a sensor's dimensionality
+ * is clearly understood.
+ *
+ * NOTE: The unified dimensional representation makes it convenient to pass
+ * either type of data into the interface functions defined in the
+ * OnlineCalibration.
+ */
+
+// Axis index definitions for SensorData::data.
+enum SensorIndex : int8_t {
+  kSingleAxis = 0,
+  kXAxis = kSingleAxis,
+  kYAxis = 1,
+  kZAxis = 2,
+};
+
+struct SensorData {
+  // Indicates the type of sensor this data originated from.
+  SensorType type;
+
+  // Sensor sample timestamp.
+  uint64_t timestamp_nanos;
+
+  // Generalized sensor sample (represents either single- or three-axis data).
+  float data[3];
+
+  SensorData() : type(SensorType::kUndefined), timestamp_nanos(0) {
+    memset(data, 0, sizeof(data));
+  }
+
+  SensorData(SensorType type, uint64_t ts, float x, float y, float z)
+      : type(type), timestamp_nanos(ts) {
+    data[SensorIndex::kXAxis] = x;
+    data[SensorIndex::kYAxis] = y;
+    data[SensorIndex::kZAxis] = z;
+  }
+
+  SensorData(SensorType type, uint64_t ts, float single_axis_sample)
+      : type(type), timestamp_nanos(ts) {
+    data[SensorIndex::kSingleAxis] = single_axis_sample;
+  }
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
new file mode 100644
index 00000000..26eef573
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2018 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 "calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+// Estimated upper bounds on gyro offset error (i.e., 3-sigma values).
+constexpr float GyroOffsetOtcCal::kMediumQualityRps;
+constexpr float GyroOffsetOtcCal::kHighQualityRps;
+
+void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters,
+                                  const OverTempCalParameters& otc_parameters) {
+  gyroCalInit(&gyro_cal_, &gyro_cal_parameters);
+  overTempCalInit(&over_temp_cal_, &otc_parameters);
+  InitializeCalData();
+}
+
+CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
+    const SensorData& sample) {
+  // Routes the input sensor sample to the calibration algorithm.
+  switch (sample.type) {
+    case SensorType::kAccelerometerMps2:
+      gyroCalUpdateAccel(&gyro_cal_, sample.timestamp_nanos,
+                         sample.data[SensorIndex::kXAxis],
+                         sample.data[SensorIndex::kYAxis],
+                         sample.data[SensorIndex::kZAxis]);  // [m/sec^2]
+      break;
+
+    case SensorType::kGyroscopeRps:
+      gyroCalUpdateGyro(&gyro_cal_, sample.timestamp_nanos,
+                        sample.data[SensorIndex::kXAxis],
+                        sample.data[SensorIndex::kYAxis],
+                        sample.data[SensorIndex::kZAxis],  // [rad/sec]
+                        temperature_celsius_);
+      break;
+
+    case SensorType::kMagnetometerUt:
+      gyroCalUpdateMag(&gyro_cal_, sample.timestamp_nanos,
+                       sample.data[SensorIndex::kXAxis],
+                       sample.data[SensorIndex::kYAxis],
+                       sample.data[SensorIndex::kZAxis]);  // [micro-Tesla]
+      break;
+
+    case SensorType::kTemperatureCelsius:
+      temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+      overTempCalSetTemperature(&over_temp_cal_, sample.timestamp_nanos,
+                                temperature_celsius_);
+      break;
+
+    default:
+      // This sample is not required.
+      return cal_update_polling_flags_;
+  }
+
+  // Checks for a new calibration, and updates the OTC.
+  if (gyroCalNewBiasAvailable(&gyro_cal_)) {
+    float offset[3];
+    float temperature_celsius = kInvalidTemperatureCelsius;
+    uint64_t calibration_time_nanos = 0;
+    gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
+                   &temperature_celsius, &calibration_time_nanos);
+    overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
+                                    offset, temperature_celsius);
+  }
+
+  // Checks the OTC for a new calibration model update.
+  const bool new_otc_model_update =
+      overTempCalNewModelUpdateAvailable(&over_temp_cal_);
+
+  // Checks for a change in the temperature compensated offset estimate.
+  const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_);
+
+  // Sets the new calibration data.
+  CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE;
+  if (new_otc_offset) {
+    overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius,
+                         cal_data_.offset);
+    cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+    cal_update_callback_flags |= CalibrationTypeFlags::BIAS;
+  }
+
+  if (new_otc_model_update) {
+    // Sets the pointer to the OTC model dataset and the number of model points.
+    cal_data_.otc_model_data = over_temp_cal_.model_data;
+    cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+    cal_update_callback_flags |= CalibrationTypeFlags::OVER_TEMP;
+    overTempCalGetModel(&over_temp_cal_, cal_data_.offset,
+                        &cal_data_.offset_temp_celsius,
+                        &cal_data_.cal_update_time_nanos,
+                        cal_data_.temp_sensitivity, cal_data_.temp_intercept);
+  }
+
+  // Sets the new calibration quality, polling flag, and notifies a calibration
+  // callback listener of the new update.
+  if (new_otc_model_update || new_otc_offset) {
+    cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+    cal_data_.calibration_quality.value = kHighQualityRps;
+    cal_update_polling_flags_ |= cal_update_callback_flags;
+    OnNotifyCalibrationUpdate(cal_update_callback_flags);
+  }
+
+  // Print debug data reports.
+#ifdef GYRO_CAL_DBG_ENABLED
+  gyroCalDebugPrint(&gyro_cal_, sample.timestamp_nanos);
+#endif  // GYRO_CAL_DBG_ENABLED
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  overTempCalDebugPrint(&over_temp_cal_, sample.timestamp_nanos);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+  return cal_update_polling_flags_;
+}
+
+bool GyroOffsetOtcCal::SetInitialCalibration(
+    const CalibrationDataThreeAxis& input_cal_data) {
+  // Checks that the input calibration type matches the algorithm type.
+  if (input_cal_data.type != get_sensor_type()) {
+    CAL_DEBUG_LOG("[GyroOffsetOtcCal]",
+                  "SetInitialCalibration failed due to wrong sensor type.");
+    return false;
+  }
+
+  // Sync's all initial calibration data.
+  cal_data_ = input_cal_data;
+
+  // Sets the initial calibration data (offset and OTC model parameters).
+  gyroCalSetBias(&gyro_cal_, cal_data_.offset[0], cal_data_.offset[1],
+                 cal_data_.offset[2], cal_data_.offset_temp_celsius,
+                 cal_data_.cal_update_time_nanos);
+
+  overTempCalSetModel(&over_temp_cal_, cal_data_.offset,
+                      cal_data_.offset_temp_celsius,
+                      cal_data_.cal_update_time_nanos,
+                      cal_data_.temp_sensitivity, cal_data_.temp_intercept,
+                      /*jump_start_model=*/false);
+
+  // Sets the calibration quality.
+  cal_data_.calibration_quality.level = CalibrationQualityLevel::MEDIUM_QUALITY;
+  cal_data_.calibration_quality.value = kMediumQualityRps;
+
+  const bool load_new_model_dataset =
+      (input_cal_data.otc_model_data != nullptr &&
+       input_cal_data.num_model_pts > 0);
+
+  if (load_new_model_dataset) {
+    // Loads the new model dataset and uses it to update the linear model
+    // parameters.
+    overTempCalSetModelData(&over_temp_cal_, input_cal_data.num_model_pts,
+                            cal_data_.cal_update_time_nanos,
+                            input_cal_data.otc_model_data);
+  }
+
+  // Sets the pointer to the OTC model dataset and the number of model points.
+  cal_data_.otc_model_data = over_temp_cal_.model_data;
+  cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+  return true;
+}
+
+}  // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
new file mode 100644
index 00000000..e21007b3
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_cal.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the gyroscope offset calibration with
+ * over-temperature compensation (OTC).
+ *
+ * NOTE: Calibration quality reporting:
+ *   Initialize             --> CalibrationQualityLevel::UNDETERMINED
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ *   SetInitialCalibration  --> CalibrationQualityLevel::MEDIUM_QUALITY
+ *                              CalibrationQuality.value = kMediumQualityRps
+ *   New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ *                              CalibrationQuality.value = kHighQualityRps
+ */
+class GyroOffsetOtcCal final
+    : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+  // Estimated upper bounds on gyro offset error (i.e., 3-sigma values).
+  static constexpr float kMediumQualityRps = 5.23599e-3f;  // 300 mDPS
+  static constexpr float kHighQualityRps   = 1.04720e-3f;  // 60 mDPS
+
+  // Default constructor.
+  GyroOffsetOtcCal() = default;
+
+  // Creates an GyroOffsetOtcCal with specified algorithm parameters.
+  GyroOffsetOtcCal(const GyroCalParameters& gyro_cal_parameters,
+                   const OverTempCalParameters& otc_parameters) {
+    Initialize(gyro_cal_parameters, otc_parameters);
+  }
+
+  // Initializes with specified algorithm parameters, and resets the calibration
+  // data.
+  void Initialize(const GyroCalParameters& gyro_cal_parameters,
+                  const OverTempCalParameters& otc_parameters);
+
+  // Sends new sensor data to the calibration algorithm, and returns the state
+  // of the calibration update flags, 'cal_update_polling_flags_'.
+  CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+  // Sets the initial calibration data of the calibration algorithm. Returns
+  // true if set successfully.
+  bool SetInitialCalibration(
+      const CalibrationDataThreeAxis& input_cal_data) final;
+
+  // Returns the calibration sensor type.
+  SensorType get_sensor_type() const final {
+    return SensorType::kGyroscopeRps;
+  };
+
+ private:
+  // GyroCal algorithm data structure.
+  GyroCal gyro_cal_;
+
+  // Over-temperature offset compensation algorithm data structure.
+  OverTempCal over_temp_cal_;
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
new file mode 100644
index 00000000..fe787b1a
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2018 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 "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+// Empirically estimated upper bounds on offset error.
+constexpr float MagDiverseCal::kLowQualityUt;
+constexpr float MagDiverseCal::kHighQualityUt;
+
+void MagDiverseCal::Initialize(
+    const MagCalParameters& mag_cal_parameters,
+    const DiversityCheckerParameters& diversity_parameters) {
+  initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters);
+  InitializeCalData();
+}
+
+CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) {
+  // Routes the input sensor sample to the calibration algorithm.
+  MagUpdate new_calibration_update = MagUpdate::NO_UPDATE;
+  switch (sample.type) {
+    case SensorType::kMagnetometerUt:
+      new_calibration_update = magCalUpdate(
+          &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos),
+          sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis],
+          sample.data[SensorIndex::kZAxis]);
+      break;
+
+    case SensorType::kTemperatureCelsius:
+      temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+      break;
+
+    default:
+      // This sample is not required.
+      return cal_update_polling_flags_;
+  }
+
+  // Checks for a new offset estimate, and updates the calibration data.
+  if (MagUpdate::UPDATE_BIAS & new_calibration_update) {
+    magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+                  &cal_data_.offset[2]);
+
+    cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+    cal_data_.calibration_quality.value = kHighQualityUt;
+    cal_data_.offset_temp_celsius = temperature_celsius_;
+    cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+    cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+    OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+  }
+
+  return cal_update_polling_flags_;
+}
+
+bool MagDiverseCal::SetInitialCalibration(
+    const CalibrationDataThreeAxis& input_cal_data) {
+  // Checks that the input calibration type matches the algorithm type.
+  if (input_cal_data.type != get_sensor_type()) {
+    CAL_DEBUG_LOG("[MagDiverseCal]",
+                  "SetInitialCalibration failed due to wrong sensor type.");
+    return false;
+  }
+
+  // Sets the magnetometer algorithm's calibration data.
+  magCalReset(&mag_cal_);  // Resets the magnetometer's offset vector.
+  magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1],
+                input_cal_data.offset[2]);
+
+  // Sync's all initial calibration data.
+  cal_data_ = input_cal_data;
+
+  // Sets the calibration quality to undetermined (uncertain magnetic history
+  // makes the usefulness of the input calibration value unknown).
+  cal_data_.calibration_quality.reset();
+
+  return true;
+}
+
+}  // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
new file mode 100644
index 00000000..11ede3d4
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
+
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the magnetometer offset calibration with
+ * diversity checking.
+ *
+ * NOTE: Calibration quality reporting:
+ *   Initialize             --> CalibrationQualityLevel::UNDETERMINED
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ *   SetInitialCalibration  --> CalibrationQualityLevel::UNDETERMINED
+ *                              CalibrationQuality.value =
+ *                                kUndeterminedCalibrationQuality
+ *   New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ *                              CalibrationQuality.value = kHighQualityUt
+ */
+class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+  // Empirically estimated upper bounds on offset error.
+  static constexpr float kLowQualityUt = 1000.0f;  // Units of micro Tesla
+  static constexpr float kHighQualityUt = 5.0f;    // Units of micro Tesla
+
+  MagDiverseCal() = default;
+
+  // Creates an MagDiverseCal with specified algorithm parameters.
+  MagDiverseCal(const MagCalParameters& mag_cal_parameters,
+                const DiversityCheckerParameters& diversity_parameters) {
+    Initialize(mag_cal_parameters, diversity_parameters);
+  }
+
+  // Initializes with specified algorithm parameters.
+  void Initialize(const MagCalParameters& mag_cal_parameters,
+                  const DiversityCheckerParameters& diversity_parameters);
+
+  // Sends new sensor data to the calibration algorithm, and returns the state
+  // of the calibration update flags, 'cal_update_polling_flags_'.
+  CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+  // Sets the initial calibration data of the calibration algorithm. Returns
+  // true if set successfully.
+  bool SetInitialCalibration(
+      const CalibrationDataThreeAxis& input_cal_data) final;
+
+  // Returns the calibration sensor type.
+  SensorType get_sensor_type() const final {
+    return SensorType::kMagnetometerUt;
+  };
+
+ private:
+  // MagCal algorithm data structure.
+  MagCal mag_cal_;
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index d92f1daa..04a9c924 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -17,12 +17,11 @@
 #include "calibration/over_temp/over_temp_cal.h"
 
 #include <float.h>
+#include <inttypes.h>
 #include <math.h>
-#include <stdio.h>
 #include <string.h>
 
 #include "calibration/util/cal_log.h"
-#include "common/math/macros.h"
 #include "util/nano_assert.h"
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,40 +31,39 @@
 
 // Defines the default weighting function for the linear model fit routine.
 // Weighting = 10.0; for offsets newer than 5 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+static const struct OverTempCalWeight kOtcDefaultWeight0 = {
     .offset_age_nanos = MIN_TO_NANOS(5),
     .weight = 10.0f,
 };
 
 // Weighting = 0.1; for offsets newer than 15 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+static const struct OverTempCalWeight kOtcDefaultWeight1 = {
     .offset_age_nanos = MIN_TO_NANOS(15),
     .weight = 0.1f,
 };
 
 // The default weighting used for all older offsets.
-#define OTC_MIN_WEIGHT_VALUE  (0.04f)
+#define OTC_MIN_WEIGHT_VALUE (0.04f)
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // A debug version label to help with tracking results.
-#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
+#define OTC_DEBUG_VERSION_STRING "[Jan 10, 2018]"
 
 // The time interval used to throttle debug messaging (100msec).
 #define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
 
-// The time interval used to throttle temperture print messaging (1 second).
+// The time interval used to throttle temperature print messaging (1 second).
 #define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
 
 // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
-static const char  kDebugAxisLabel[3] = "XYZ";
+static const char kDebugAxisLabel[3] = "XYZ";
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
 /////// FORWARD DECLARATIONS //////////////////////////////////////////////////
 
 // Updates the latest received model estimate data.
 static void setLatestEstimate(struct OverTempCal *over_temp_cal,
-                              const float *offset, float offset_temp_celsius,
-                              uint64_t timestamp_nanos);
+                              const float *offset, float offset_temp_celsius);
 
 /*
  * Determines if a new over-temperature model fit should be performed, and then
@@ -103,11 +101,10 @@ static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
  * Since it may take a while for an empty model to build up enough data to start
  * producing new model parameter updates, the model collection can be
  * jump-started by using the new model parameters to insert "fake" data in place
- * of actual sensor offset data. 'timestamp_nanos' sets the timestamp for the
- * new model data.
+ * of actual sensor offset data. The new model data 'offset_age_nanos' is set to
+ * zero.
  */
-static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
-                               uint64_t timestamp_nanos);
+static bool jumpStartModelData(struct OverTempCal *over_temp_cal);
 
 /*
  * Computes a new model fit and provides updated model parameters for the
@@ -116,7 +113,6 @@ static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
  *
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.
- *   timestamp_nanos:  Current timestamp for the model update.
  * OUTPUTS:
  *   temp_sensitivity: Updated modeled temperature sensitivity (array).
  *   sensor_intercept: Updated model intercept (array).
@@ -127,8 +123,7 @@ static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
  * Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992.
  */
 static void updateModel(const struct OverTempCal *over_temp_cal,
-                        uint64_t timestamp_nanos, float *temp_sensitivity,
-                        float *sensor_intercept);
+                        float *temp_sensitivity, float *sensor_intercept);
 
 /*
  * Computes a new over-temperature compensated offset estimate based on the
@@ -183,7 +178,8 @@ static bool checkAndEnforceTemperatureRange(float *temperature_celsius);
 // Returns "true" if the candidate linear model parameters are within the valid
 // range, and not all zeros.
 static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
-                   float temp_sensitivity, float sensor_intercept);
+                                  float temp_sensitivity,
+                                  float sensor_intercept);
 
 // Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
 static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
@@ -191,8 +187,12 @@ static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
 // Returns the least-squares weight based on the age of a particular offset
 // estimate.
 static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
-                                       uint64_t offset_timestamp_nanos,
-                                       uint64_t current_timestamp_nanos);
+                                       uint64_t offset_age_nanos);
+
+// Computes the age increment, adds it to the age of each OTC model data point,
+// and resets the age update counter.
+static void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+                                  uint64_t timestamp_nanos);
 
 // Updates 'compensated_offset' using the linear OTC model.
 static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -208,9 +208,10 @@ static void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
                                               float delta_temp_celsius);
 
 // Provides an over-temperature compensated offset based on the 'estimate'.
-static void compensateWithEstimate(
-    struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
-    struct OverTempCalDataPt *estimate, float temperature_celsius);
+static void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+                                   uint64_t timestamp_nanos,
+                                   struct OverTempModelThreeAxis *estimate,
+                                   float temperature_celsius);
 
 // Evaluates the nearest-temperature compensation (with linear extrapolation
 // term due to temperature), and compares it with the compensation due to
@@ -233,7 +234,7 @@ static void refreshOtcModel(struct OverTempCal *over_temp_cal,
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // This helper function stores all of the debug tracking information necessary
 // for printing log messages.
-static void updateDebugData(struct OverTempCal* over_temp_cal);
+static void updateDebugData(struct OverTempCal *over_temp_cal);
 
 // Helper function that creates tag strings useful for identifying specific
 // debug output data (embedded system friendly; not all systems have 'sprintf').
@@ -251,12 +252,7 @@ static void createDebugTag(struct OverTempCal *over_temp_cal,
 /////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
 
 void overTempCalInit(struct OverTempCal *over_temp_cal,
-                     size_t min_num_model_pts,
-                     uint64_t min_temp_update_period_nanos,
-                     float delta_temp_per_bin, float jump_tolerance,
-                     float outlier_limit, uint64_t age_limit_nanos,
-                     float temp_sensitivity_limit, float sensor_intercept_limit,
-                     float significant_offset_change, bool over_temp_enable) {
+                     const struct OverTempCalParameters *parameters) {
   ASSERT_NOT_NULL(over_temp_cal);
 
   // Clears OverTempCal memory.
@@ -264,7 +260,7 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
 
   // Initializes the pointers to important sensor offset estimates.
   over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
-  over_temp_cal->latest_offset  = NULL;
+  over_temp_cal->latest_offset = NULL;
 
   // Initializes the OTC linear model parameters.
   resetOtcLinearModel(over_temp_cal);
@@ -272,24 +268,22 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
   // Initializes the model identification parameters.
   over_temp_cal->new_overtemp_model_available = false;
   over_temp_cal->new_overtemp_offset_available = false;
-  over_temp_cal->min_num_model_pts = min_num_model_pts;
-  over_temp_cal->min_temp_update_period_nanos = min_temp_update_period_nanos;
-  over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
-  over_temp_cal->jump_tolerance = jump_tolerance;
-  over_temp_cal->outlier_limit = outlier_limit;
-  over_temp_cal->age_limit_nanos = age_limit_nanos;
-  over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
-  over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
-  over_temp_cal->significant_offset_change = significant_offset_change;
-  over_temp_cal->over_temp_enable = over_temp_enable;
+  over_temp_cal->min_num_model_pts = parameters->min_num_model_pts;
+  over_temp_cal->min_temp_update_period_nanos =
+      parameters->min_temp_update_period_nanos;
+  over_temp_cal->delta_temp_per_bin = parameters->delta_temp_per_bin;
+  over_temp_cal->jump_tolerance = parameters->jump_tolerance;
+  over_temp_cal->outlier_limit = parameters->outlier_limit;
+  over_temp_cal->age_limit_nanos = parameters->age_limit_nanos;
+  over_temp_cal->temp_sensitivity_limit = parameters->temp_sensitivity_limit;
+  over_temp_cal->sensor_intercept_limit = parameters->sensor_intercept_limit;
+  over_temp_cal->significant_offset_change =
+      parameters->significant_offset_change;
+  over_temp_cal->over_temp_enable = parameters->over_temp_enable;
 
   // Initializes the over-temperature compensated offset temperature.
   over_temp_cal->compensated_offset.offset_temp_celsius =
-      OTC_TEMP_INVALID_CELSIUS;
-
-  // Defines the default weighting function for the linear model fit routine.
-  overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
-  overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
+      INVALID_TEMPERATURE_CELSIUS;
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Sets the default sensor descriptors for debugging.
@@ -305,6 +299,10 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
                   "Over-temperature compensation DISABLED.");
   }
 #endif  // OVERTEMPCAL_DBG_ENABLED
+
+  // Defines the default weighting function for the linear model fit routine.
+  overTempValidateAndSetWeight(over_temp_cal, 0, &kOtcDefaultWeight0);
+  overTempValidateAndSetWeight(over_temp_cal, 1, &kOtcDefaultWeight1);
 }
 
 void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
@@ -332,8 +330,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 
   // Model "Jump-Start".
   const bool model_jump_started =
-      (jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos)
-                         : false;
+      jump_start_model ? jumpStartModelData(over_temp_cal) : false;
 
   if (!model_jump_started) {
     // Checks that the new offset data is valid.
@@ -342,7 +339,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
       memcpy(over_temp_cal->model_data[0].offset, offset,
              sizeof(over_temp_cal->model_data[0].offset));
       over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
-      over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
+      over_temp_cal->model_data[0].offset_age_nanos = 0;
       over_temp_cal->num_model_pts = 1;
     } else {
       // No valid offset data to load.
@@ -361,7 +358,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
     memcpy(over_temp_cal->compensated_offset.offset, offset,
            sizeof(over_temp_cal->compensated_offset.offset));
     over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
-    over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
+    over_temp_cal->compensated_offset.offset_age_nanos = 0;
   }
 
   // Resets the latest offset pointer. There are no new offset estimates to
@@ -435,7 +432,7 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
 
 void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
                              size_t data_length, uint64_t timestamp_nanos,
-                             const struct OverTempCalDataPt *model_data) {
+                             const struct OverTempModelThreeAxis *model_data) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(model_data);
 
@@ -446,11 +443,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
     if (isValidOtcOffset(model_data[i].offset,
                          model_data[i].offset_temp_celsius)) {
       memcpy(&over_temp_cal->model_data[i], &model_data[i],
-             sizeof(struct OverTempCalDataPt));
-
-      // Updates the model time stamps to the current load time.
-      over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
-
+             sizeof(struct OverTempModelThreeAxis));
       valid_data_count++;
     }
   }
@@ -491,11 +484,11 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
 
 void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
                              size_t *data_length,
-                             struct OverTempCalDataPt *model_data) {
+                             struct OverTempModelThreeAxis *model_data) {
   ASSERT_NOT_NULL(over_temp_cal);
   *data_length = over_temp_cal->num_model_pts;
   memcpy(model_data, over_temp_cal->model_data,
-         over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
+         over_temp_cal->num_model_pts * sizeof(struct OverTempModelThreeAxis));
 }
 
 void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
@@ -559,6 +552,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
   ASSERT_NOT_NULL(offset);
   ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
+  // Updates the age of each OTC model data point.
+  modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+
   // Checks that the new offset data is valid, returns if bad.
   if (!isValidOtcOffset(offset, temperature_celsius)) {
     return;
@@ -586,14 +582,13 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
       createDebugTag(over_temp_cal, ":OUTLIER]");
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Offset|Temperature|Time [%s|C|nsec]: "
-          CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+          "Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
           over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3),
-          CAL_ENCODE_FLOAT(temperature_celsius, 3),
-          (unsigned long long int)timestamp_nanos);
+          CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
       return;  // Outlier detected: skips adding this offset to the model.
@@ -648,8 +643,8 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
       //    oldest data with the incoming one.
       over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
       for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
-        if (over_temp_cal->latest_offset->timestamp_nanos <
-            over_temp_cal->model_data[i].timestamp_nanos) {
+        if (over_temp_cal->latest_offset->offset_age_nanos <
+            over_temp_cal->model_data[i].offset_age_nanos) {
           over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
         }
       }
@@ -657,8 +652,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
   }
 
   // Updates the latest model estimate data.
-  setLatestEstimate(over_temp_cal, offset, temperature_celsius,
-                    timestamp_nanos);
+  setLatestEstimate(over_temp_cal, offset, temperature_celsius);
 
   // The latest offset estimate is the nearest temperature offset.
   over_temp_cal->nearest_offset = over_temp_cal->latest_offset;
@@ -705,13 +699,20 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
     // Prints out temperature and the current timestamp.
     createDebugTag(over_temp_cal, ":TEMP]");
     CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                  "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
-                  CAL_ENCODE_FLOAT(temperature_celsius, 3),
-                  (unsigned long long int)timestamp_nanos);
+                  "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS
+                  ", %" PRIu64,
+                  CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
   }
 #endif  // OVERTEMPCAL_DBG_LOG_TEMP
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
+  // Updates the age of each OTC model data point.
+  if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+          timestamp_nanos, over_temp_cal->last_age_update_nanos,
+          OTC_MODEL_AGE_UPDATE_NANOS)) {
+    modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+  }
+
   // This check throttles new OTC offset compensation updates so that high data
   // rate temperature samples do not cause excessive computational burden. Note,
   // temperature sensor updates are expected to potentially increase the data
@@ -720,7 +721,7 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
   if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
           timestamp_nanos, over_temp_cal->last_offset_update_nanos,
           over_temp_cal->min_temp_update_period_nanos)) {
-    return; // Time interval too short, skip further data processing.
+    return;  // Time interval too short, skip further data processing.
   }
 
   // Checks that the offset temperature is within a valid range, saturates if
@@ -746,8 +747,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
 }
 
 void overTempGetModelError(const struct OverTempCal *over_temp_cal,
-                   const float *temp_sensitivity, const float *sensor_intercept,
-                   float *max_error) {
+                           const float *temp_sensitivity,
+                           const float *sensor_intercept, float *max_error) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(temp_sensitivity);
   ASSERT_NOT_NULL(sensor_intercept);
@@ -770,14 +771,34 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
   }
 }
 
-// TODO(davejacobs): Refactor to implement a compliance check on the storage of
-// 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
     struct OverTempCal *over_temp_cal, size_t index,
-    const struct OverTempCalWeightPt *new_otc_weight) {
-  if (index < OTC_NUM_WEIGHT_LEVELS) {
+    const struct OverTempCalWeight *new_otc_weight) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(new_otc_weight);
+
+  // The input weighting coefficient must be positive.
+  if (new_otc_weight->weight <= 0.0f) {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+    createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+    CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Invalid weight: Must > 0.");
+#endif  // OVERTEMPCAL_DBG_ENABLED
+    return false;
+  }
+
+  // Ensures that the 'index-1' weight's age is younger.
+  if (index == 0 ||
+      over_temp_cal->weighting_function[index - 1].offset_age_nanos <
+          new_otc_weight->offset_age_nanos) {
     over_temp_cal->weighting_function[index] = *new_otc_weight;
+    return true;
   }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Non monotonic weight age.");
+#endif  // OVERTEMPCAL_DBG_ENABLED
+  return false;
 }
 
 /////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
@@ -827,9 +848,10 @@ void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
   }
 }
 
-void compensateWithEstimate(
-    struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
-    struct OverTempCalDataPt *estimate, float temperature_celsius) {
+void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+                            uint64_t timestamp_nanos,
+                            struct OverTempModelThreeAxis *estimate,
+                            float temperature_celsius) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(estimate);
 
@@ -838,7 +860,7 @@ void compensateWithEstimate(
   memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
 
   // Checks that the offset temperature is valid.
-  if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
+  if (estimate->offset_temp_celsius > INVALID_TEMPERATURE_CELSIUS) {
     const float delta_temp_celsius =
         temperature_celsius - estimate->offset_temp_celsius;
 
@@ -886,7 +908,7 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
         // Adds a delta term to the compensated offset using the temperature
         // difference defined by 'delta_temp_celsius'.
         if (over_temp_cal->compensated_offset.offset_temp_celsius <=
-            OTC_TEMP_INVALID_CELSIUS) {
+            INVALID_TEMPERATURE_CELSIUS) {
           // If temperature is invalid, then skip further processing.
           break;
         }
@@ -918,7 +940,7 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
 
   // If 'temperature_celsius' is invalid, then no changes to the compensated
   // offset are computed.
-  if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+  if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
     return;
   }
 
@@ -927,9 +949,9 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
   // than one estimate in the model (i.e., don't want to remove all data, even
   // if it is very old [something is likely better than nothing]).
   if ((timestamp_nanos >=
-       OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
+       OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer_nanos) &&
       over_temp_cal->num_model_pts > 1) {
-    over_temp_cal->stale_data_timer = timestamp_nanos;  // Resets timer.
+    over_temp_cal->stale_data_timer_nanos = timestamp_nanos;  // Resets timer.
     removeStaleModelData(over_temp_cal, timestamp_nanos);
   }
 
@@ -944,12 +966,32 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
   // not empty.
   const bool model_points_available = (over_temp_cal->num_model_pts > 0);
 
+  // To properly evaluate the logic paths that use the latest and nearest offset
+  // data below, the current age of the nearest and latest offset estimates are
+  // computed.
+  uint64_t latest_offset_age_nanos = 0;
+  if (over_temp_cal->latest_offset != NULL) {
+    latest_offset_age_nanos =
+        (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+            ? over_temp_cal->latest_offset->offset_age_nanos +
+                  timestamp_nanos - over_temp_cal->last_age_update_nanos
+            : over_temp_cal->latest_offset->offset_age_nanos;
+  }
+
+  uint64_t nearest_offset_age_nanos = 0;
+  if (over_temp_cal->nearest_offset != NULL) {
+    nearest_offset_age_nanos =
+        (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+            ? over_temp_cal->nearest_offset->offset_age_nanos +
+                  timestamp_nanos - over_temp_cal->last_age_update_nanos
+            : over_temp_cal->nearest_offset->offset_age_nanos;
+  }
+
   // True when the latest offset estimate will be used to compute a sensor
   // offset calibration estimate.
   const bool use_latest_offset_compensation =
-      over_temp_cal->latest_offset && model_points_available &&
-      timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos +
-                            OTC_USE_RECENT_OFFSET_TIME_NANOS;
+      over_temp_cal->latest_offset != NULL && model_points_available &&
+      latest_offset_age_nanos <= OTC_USE_RECENT_OFFSET_TIME_NANOS;
 
   // True when the conditions are met to use the nearest-temperature offset to
   // compute a sensor offset calibration estimate.
@@ -958,7 +1000,7 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
   //    ii. Offset temperature must be within a small neighborhood of the
   //        current measured temperature (+/- 'delta_temp_per_bin').
   const bool can_compensate_with_nearest =
-      model_points_available && over_temp_cal->nearest_offset &&
+      model_points_available && over_temp_cal->nearest_offset != NULL &&
       NANO_ABS(temperature_celsius -
                over_temp_cal->nearest_offset->offset_temp_celsius) <
           over_temp_cal->delta_temp_per_bin;
@@ -966,18 +1008,14 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
   // True if the last received sensor offset estimate is old or non-existent.
   const bool latest_model_point_not_relevant =
       (over_temp_cal->latest_offset == NULL) ||
-      (over_temp_cal->latest_offset &&
-       NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-           timestamp_nanos, over_temp_cal->latest_offset->timestamp_nanos,
-           OTC_OFFSET_IS_STALE_NANOS));
+      (over_temp_cal->latest_offset != NULL &&
+       latest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
 
   // True if the nearest-temperature offset estimate is old or non-existent.
   const bool nearest_model_point_not_relevant =
       (over_temp_cal->nearest_offset == NULL) ||
-      (over_temp_cal->nearest_offset &&
-       NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-           timestamp_nanos, over_temp_cal->nearest_offset->timestamp_nanos,
-           OTC_OFFSET_IS_STALE_NANOS));
+      (over_temp_cal->nearest_offset != NULL &&
+       nearest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
 
   // ---------------------------------------------------------------------------
   // The following conditional expressions govern new OTC offset updates.
@@ -1051,26 +1089,25 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal,
   over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available;
 
   // If the offset has changed significantly, then the offset compensation
-  // vector and timestamp are updated.
+  // vector is updated.
   if (new_overtemp_offset_available) {
     memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
            sizeof(over_temp_cal->compensated_offset.offset));
-    over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
     over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
   }
 }
 
 void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
-                       float offset_temp_celsius, uint64_t timestamp_nanos) {
+                       float offset_temp_celsius) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(offset);
 
-  if (over_temp_cal->latest_offset) {
+  if (over_temp_cal->latest_offset != NULL) {
     // Sets the latest over-temp calibration estimate.
     memcpy(over_temp_cal->latest_offset->offset, offset,
            sizeof(over_temp_cal->latest_offset->offset));
     over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
-    over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
+    over_temp_cal->latest_offset->offset_age_nanos = 0;
   }
 }
 
@@ -1098,19 +1135,17 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
 
   // Ensures that the minimum number of points required for a model fit has been
   // satisfied.
-  if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts)
-      return;
+  if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) return;
 
   // Updates the linear model fit.
   float temp_sensitivity[3];
   float sensor_intercept[3];
-  updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity,
-              sensor_intercept);
+  updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);
 
   //    2) A new set of model parameters are accepted if:
   //         i. The model fit parameters must be within certain absolute bounds:
-  //              a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
-  //              b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
+  //              a. |temp_sensitivity| < temp_sensitivity_limit
+  //              b. |sensor_intercept| < sensor_intercept_limit
   // NOTE: Model parameter updates are not qualified against model fit error
   // here to protect against the case where there is large change in the
   // temperature characteristic either during runtime (e.g., temperature
@@ -1131,14 +1166,14 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
-          ", " CAL_FORMAT_3DIGITS ", %llu",
+          ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
           kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
           over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(
               temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(
               sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3),
-          (unsigned long long int)timestamp_nanos);
+          timestamp_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
     }
   }
@@ -1161,7 +1196,7 @@ void findNearestEstimate(struct OverTempCal *over_temp_cal,
   ASSERT_NOT_NULL(over_temp_cal);
 
   // If 'temperature_celsius' is invalid, then do not search.
-  if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+  if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
     return;
   }
 
@@ -1186,9 +1221,8 @@ void removeStaleModelData(struct OverTempCal *over_temp_cal,
 
   bool removed_one = false;
   for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
-    if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
-        timestamp_nanos > over_temp_cal->age_limit_nanos +
-                              over_temp_cal->model_data[i].timestamp_nanos) {
+    if (over_temp_cal->model_data[i].offset_age_nanos >=
+        over_temp_cal->age_limit_nanos) {
       // If the latest offset was removed, then indicate this by setting it to
       // NULL.
       if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) {
@@ -1223,8 +1257,8 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
   createDebugTag(over_temp_cal, ":REMOVE]");
   CAL_DEBUG_LOG(
       over_temp_cal->otc_debug_tag,
-      "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
-      ", " CAL_FORMAT_3DIGITS ", %llu",
+      "Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+      ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
       over_temp_cal->otc_unit_tag,
       CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
                            over_temp_cal->otc_unit_conversion,
@@ -1237,22 +1271,20 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
                        3),
       CAL_ENCODE_FLOAT(
           over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
-      (unsigned long long int)over_temp_cal->model_data[model_index]
-          .timestamp_nanos);
+      over_temp_cal->model_data[model_index].offset_age_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
   // Remove the model data at 'model_index'.
   for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
     memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
-           sizeof(struct OverTempCalDataPt));
+           sizeof(struct OverTempModelThreeAxis));
   }
   over_temp_cal->num_model_pts--;
 
   return true;
 }
 
-bool jumpStartModelData(struct OverTempCal *over_temp_cal,
-                        uint64_t timestamp_nanos) {
+bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
@@ -1291,7 +1323,7 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal,
           over_temp_cal->sensor_intercept[j];
     }
     over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius;
-    over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
+    over_temp_cal->model_data[i].offset_age_nanos = 0;
 
     offset_temp_celsius += over_temp_cal->delta_temp_per_bin;
     over_temp_cal->num_model_pts++;
@@ -1301,8 +1333,8 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal,
   createDebugTag(over_temp_cal, ":INIT]");
   if (over_temp_cal->num_model_pts > 0) {
     CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                  "Model Jump-Start:  #Points = %lu.",
-                  (unsigned long int)over_temp_cal->num_model_pts);
+                  "Model Jump-Start:  #Points = %zu.",
+                  over_temp_cal->num_model_pts);
   }
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
@@ -1310,8 +1342,7 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal,
 }
 
 void updateModel(const struct OverTempCal *over_temp_cal,
-                 uint64_t timestamp_nanos, float *temp_sensitivity,
-                 float *sensor_intercept) {
+                 float *temp_sensitivity, float *sensor_intercept) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(temp_sensitivity);
   ASSERT_NOT_NULL(sensor_intercept);
@@ -1328,8 +1359,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   const size_t n = over_temp_cal->num_model_pts;
   for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
-        over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
-        timestamp_nanos);
+        over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
 
     sw += weight;
     st += over_temp_cal->model_data[i].offset_temp_celsius * weight;
@@ -1343,13 +1373,11 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   const float inv_sw = 1.0f / sw;
   for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
-        over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
-        timestamp_nanos);
+        over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
 
     const float t =
-        over_temp_cal->model_data[i].offset_temp_celsius -
-        st * inv_sw;
-    stt +=  weight * t * t;
+        over_temp_cal->model_data[i].offset_temp_celsius - st * inv_sw;
+    stt += weight * t * t;
     stsx += t * over_temp_cal->model_data[i].offset[0] * weight;
     stsy += t * over_temp_cal->model_data[i].offset[1] * weight;
     stsz += t * over_temp_cal->model_data[i].offset[2] * weight;
@@ -1440,13 +1468,11 @@ bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
 }
 
 float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
-                                uint64_t offset_timestamp_nanos,
-                                uint64_t current_timestamp_nanos) {
+                                uint64_t offset_age_nanos) {
   ASSERT_NOT_NULL(over_temp_cal);
   for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
-    if (current_timestamp_nanos <=
-        offset_timestamp_nanos +
-            over_temp_cal->weighting_function[i].offset_age_nanos) {
+    if (offset_age_nanos <=
+        over_temp_cal->weighting_function[i].offset_age_nanos) {
       return over_temp_cal->weighting_function[i].weight;
     }
   }
@@ -1455,6 +1481,26 @@ float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
   return OTC_MIN_WEIGHT_VALUE;
 }
 
+void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+                           uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) {
+    // Age updates must be monotonic.
+    return;
+  }
+
+  uint64_t age_increment_nanos =
+      timestamp_nanos - over_temp_cal->last_age_update_nanos;
+
+  // Resets the age update counter.
+  over_temp_cal->last_age_update_nanos = timestamp_nanos;
+
+  // Updates the model dataset ages.
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+    over_temp_cal->model_data[i].offset_age_nanos += age_increment_nanos;
+  }
+}
+
 /////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
@@ -1468,7 +1514,7 @@ void createDebugTag(struct OverTempCal *over_temp_cal,
       new_debug_tag, strlen(new_debug_tag) + 1);
 }
 
-void updateDebugData(struct OverTempCal* over_temp_cal) {
+void updateDebugData(struct OverTempCal *over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
 
   // Only update this data if debug printing is not currently in progress
@@ -1502,13 +1548,13 @@ void updateDebugData(struct OverTempCal* over_temp_cal) {
 
   // If 'latest_offset' is defined the copy the data for debug printing.
   // Otherwise, the current compensated offset will be printed.
-  if (over_temp_cal->latest_offset) {
+  if (over_temp_cal->latest_offset != NULL) {
     memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
-           over_temp_cal->latest_offset, sizeof(struct OverTempCalDataPt));
+           over_temp_cal->latest_offset, sizeof(struct OverTempModelThreeAxis));
   } else {
     memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
            &over_temp_cal->compensated_offset,
-           sizeof(struct OverTempCalDataPt));
+           sizeof(struct OverTempModelThreeAxis));
   }
 
   // Total number of OTC model data points.
@@ -1516,9 +1562,9 @@ void updateDebugData(struct OverTempCal* over_temp_cal) {
 
   // Computes the maximum error over all of the model data.
   overTempGetModelError(over_temp_cal,
-                over_temp_cal->debug_overtempcal.temp_sensitivity,
-                over_temp_cal->debug_overtempcal.sensor_intercept,
-                over_temp_cal->debug_overtempcal.max_error);
+                        over_temp_cal->debug_overtempcal.temp_sensitivity,
+                        over_temp_cal->debug_overtempcal.sensor_intercept,
+                        over_temp_cal->debug_overtempcal.max_error);
 }
 
 void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
@@ -1554,10 +1600,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       // Prints out the latest offset estimate (input data).
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
-          ", " CAL_FORMAT_3DIGITS ", %llu",
-          over_temp_cal->otc_unit_tag,
-          (unsigned long int)over_temp_cal->debug_num_estimates,
+          "Cal#|Offset|Temp|Age [%s|C|nsec]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+          over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
               over_temp_cal->debug_overtempcal.latest_offset.offset[0] *
                   over_temp_cal->otc_unit_conversion,
@@ -1573,40 +1618,40 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset
                                .offset_temp_celsius,
                            3),
-          (unsigned long long int)
-              over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);
+          over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos);
 
+      // clang-format off
       over_temp_cal->wait_timer_nanos =
           timestamp_nanos;                          // Starts the wait timer.
       over_temp_cal->next_state =
           OTC_PRINT_MODEL_PARAMETERS;               // Sets the next state.
       over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
+      // clang-format on
       break;
 
     case OTC_PRINT_MODEL_PARAMETERS:
       // Prints out the model parameters.
-      CAL_DEBUG_LOG(
-          over_temp_cal->otc_debug_tag,
-          "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
-          over_temp_cal->otc_unit_tag,
-          (unsigned long int)over_temp_cal->debug_num_estimates,
-          CAL_ENCODE_FLOAT(
-              over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
-                  over_temp_cal->otc_unit_conversion,
-              3),
-          CAL_ENCODE_FLOAT(
-              over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
-                  over_temp_cal->otc_unit_conversion,
-              3),
-          CAL_ENCODE_FLOAT(
-              over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
-                  over_temp_cal->otc_unit_conversion,
-              3));
+      CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
+                    "Cal#|Sensitivity [%s/C]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+                    over_temp_cal->otc_unit_tag,
+                    over_temp_cal->debug_num_estimates,
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+                            over_temp_cal->otc_unit_conversion,
+                        3),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+                            over_temp_cal->otc_unit_conversion,
+                        3),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+                            over_temp_cal->otc_unit_conversion,
+                        3));
 
       CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                    "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
+                    "Cal#|Intercept [%s]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
                     over_temp_cal->otc_unit_tag,
-                    (unsigned long int)over_temp_cal->debug_num_estimates,
+                    over_temp_cal->debug_num_estimates,
                     CAL_ENCODE_FLOAT(
                         over_temp_cal->debug_overtempcal.sensor_intercept[0] *
                             over_temp_cal->otc_unit_conversion,
@@ -1621,8 +1666,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
                         3));
 
       over_temp_cal->wait_timer_nanos =
-          timestamp_nanos;                          // Starts the wait timer.
-      over_temp_cal->next_state = OTC_PRINT_MODEL_ERROR;  // Sets the next state.
+          timestamp_nanos;  // Starts the wait timer.
+      over_temp_cal->next_state =
+          OTC_PRINT_MODEL_ERROR;                    // Sets the next state.
       over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
       break;
 
@@ -1630,12 +1676,11 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       // Computes the maximum error over all of the model data.
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
-          "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
-          over_temp_cal->otc_unit_tag,
-          (unsigned long int)over_temp_cal->debug_num_estimates,
-          (unsigned long int)over_temp_cal->debug_num_model_updates,
-          (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
+          "Cal#|#Updates|#ModelPts|Model Error [%s]: %zu, "
+          "%zu, %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+          over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
+          over_temp_cal->debug_num_model_updates,
+          over_temp_cal->debug_overtempcal.num_model_pts,
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
                                over_temp_cal->otc_unit_conversion,
                            3),
@@ -1648,7 +1693,7 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
 
       over_temp_cal->model_counter = 0;  // Resets the model data print counter.
       over_temp_cal->wait_timer_nanos =
-          timestamp_nanos;               // Starts the wait timer.
+          timestamp_nanos;  // Starts the wait timer.
       over_temp_cal->next_state = OTC_PRINT_MODEL_DATA;  // Sets the next state.
       over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
       break;
@@ -1658,10 +1703,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
         CAL_DEBUG_LOG(
             over_temp_cal->otc_debug_tag,
-            "  Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
-            ", " CAL_FORMAT_3DIGITS ", %llu",
-            (unsigned long int)over_temp_cal->model_counter,
-            over_temp_cal->otc_unit_tag,
+            "  Model[%zu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+            ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+            over_temp_cal->model_counter, over_temp_cal->otc_unit_tag,
             CAL_ENCODE_FLOAT(
                 over_temp_cal->model_data[over_temp_cal->model_counter]
                         .offset[0] *
@@ -1681,24 +1725,23 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
                 over_temp_cal->model_data[over_temp_cal->model_counter]
                     .offset_temp_celsius,
                 3),
-            (unsigned long long int)over_temp_cal
-                ->model_data[over_temp_cal->model_counter]
-                .timestamp_nanos);
+            over_temp_cal->model_data[over_temp_cal->model_counter]
+                .offset_age_nanos);
 
         over_temp_cal->model_counter++;
         over_temp_cal->wait_timer_nanos =
-            timestamp_nanos;                        // Starts the wait timer.
+            timestamp_nanos;  // Starts the wait timer.
         over_temp_cal->next_state =
-            OTC_PRINT_MODEL_DATA;                   // Sets the next state.
+            OTC_PRINT_MODEL_DATA;  // Sets the next state.
         over_temp_cal->debug_state =
-            OTC_WAIT_STATE;                         // First, go to wait state.
+            OTC_WAIT_STATE;  // First, go to wait state.
       } else {
         // Sends this state machine to its idle state.
         over_temp_cal->wait_timer_nanos =
-            timestamp_nanos;                        // Starts the wait timer.
-        over_temp_cal->next_state = OTC_IDLE;       // Sets the next state.
+            timestamp_nanos;                   // Starts the wait timer.
+        over_temp_cal->next_state = OTC_IDLE;  // Sets the next state.
         over_temp_cal->debug_state =
-            OTC_WAIT_STATE;                         // First, go to wait state.
+            OTC_WAIT_STATE;  // First, go to wait state.
       }
       break;
 
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 8a404d32..8f6f0a4d 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -100,13 +100,13 @@
 #include <stddef.h>
 #include <stdint.h>
 
+#include "calibration/over_temp/over_temp_model.h"
+#include "common/math/macros.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-// Defines the maximum size of the 'model_data' array.
-#define OTC_MODEL_SIZE (40)
-
 // A common sensor operating temperature at which to begin the model jump-start
 // data.
 #define JUMPSTART_START_TEMP_CELSIUS (30.0f)
@@ -122,13 +122,13 @@ extern "C" {
 #define OTC_TEMP_MIN_CELSIUS (-40.0f)
 #define OTC_TEMP_MAX_CELSIUS (85.0f)
 
-// Invalid sensor temperature.
-#define OTC_TEMP_INVALID_CELSIUS (-274.0f)
-
 // Number of time-interval levels used to define the least-squares weighting
 // function.
 #define OTC_NUM_WEIGHT_LEVELS (2)
 
+// The time interval used to update the model data age.
+#define OTC_MODEL_AGE_UPDATE_NANOS (MIN_TO_NANOS(1))
+
 // Rate-limits the check of old data to every 2 hours.
 #define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
 
@@ -143,7 +143,7 @@ extern "C" {
 #define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
 
 // Defines a weighting function value for the linear model fit routine.
-struct OverTempCalWeightPt {
+struct OverTempCalWeight {
   // The age limit below which an offset will use this weight value.
   uint64_t offset_age_nanos;
 
@@ -151,14 +151,6 @@ struct OverTempCalWeightPt {
   float weight;
 };
 
-// Over-temperature sensor offset estimate structure.
-struct OverTempCalDataPt {
-  // Sensor offset estimate, temperature, and timestamp.
-  uint64_t timestamp_nanos;   // [nanoseconds]
-  float offset_temp_celsius;  // [Celsius]
-  float offset[3];
-};
-
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // Debug printout state enumeration.
 enum OverTempCalDebugState {
@@ -173,7 +165,7 @@ enum OverTempCalDebugState {
 // OverTempCal debug information/data tracking structure.
 struct DebugOverTempCal {
   // The latest received offset estimate data.
-  struct OverTempCalDataPt latest_offset;
+  struct OverTempModelThreeAxis latest_offset;
 
   // The maximum model error over all model_data points.
   float max_error[3];
@@ -184,38 +176,55 @@ struct DebugOverTempCal {
 };
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
+// OverTempCal algorithm parameters (see OverTempCal for details).
+struct OverTempCalParameters {
+  uint64_t min_temp_update_period_nanos;
+  uint64_t age_limit_nanos;
+  float delta_temp_per_bin;         // [Celsius/bin]
+  float jump_tolerance;             // [sensor units]
+  float outlier_limit;              // [sensor units/Celsius]
+  float temp_sensitivity_limit;     // [sensor units/Celsius]
+  float sensor_intercept_limit;     // [sensor units]
+  float significant_offset_change;  // [sensor units]
+  size_t min_num_model_pts;
+  bool over_temp_enable;
+};
+
 // The following data structure contains all of the necessary components for
 // modeling a sensor's temperature dependency and providing over-temperature
 // offset corrections.
 struct OverTempCal {
   // Storage for over-temperature model data.
-  struct OverTempCalDataPt model_data[OTC_MODEL_SIZE];
+  struct OverTempModelThreeAxis model_data[OTC_MODEL_SIZE];
 
   // Implements a weighting function to emphasize fitting a linear model to
   // younger offset estimates.
-  struct OverTempCalWeightPt weighting_function[OTC_NUM_WEIGHT_LEVELS];
+  struct OverTempCalWeight weighting_function[OTC_NUM_WEIGHT_LEVELS];
 
   // The active over-temperature compensated offset estimate data. Contains the
   // current sensor temperature at which offset compensation is performed.
-  struct OverTempCalDataPt compensated_offset;
+  struct OverTempModelThreeAxis compensated_offset;
 
   // Timer used to limit the rate at which old estimates are removed from
   // the 'model_data' collection.
-  uint64_t stale_data_timer;             // [nanoseconds]
+  uint64_t stale_data_timer_nanos;
 
   // Duration beyond which data will be removed to avoid corrupting the model
   // with drift-compromised data.
-  uint64_t age_limit_nanos;              // [nanoseconds]
+  uint64_t age_limit_nanos;
 
   // Timestamp of the last OTC offset compensation update.
-  uint64_t last_offset_update_nanos;     // [nanoseconds]
+  uint64_t last_offset_update_nanos;
 
   // Timestamp of the last OTC model update.
-  uint64_t last_model_update_nanos;      // [nanoseconds]
+  uint64_t last_model_update_nanos;
+
+  // Timestamp of the last OTC model dataset age update.
+  uint64_t last_age_update_nanos;
 
   // Limit on the minimum interval for offset update calculations resulting from
   // an arbitrarily high temperature sampling rate.
-  uint64_t min_temp_update_period_nanos;    // [nanoseconds]
+  uint64_t min_temp_update_period_nanos;
 
   ///// Online Model Identification Parameters ////////////////////////////////
   //
@@ -229,17 +238,17 @@ struct OverTempCal {
   //          model_temp_span >= 'num_model_pts' * delta_temp_per_bin
   //    2) A new set of model parameters are accepted if:
   //         i. The model fit parameters must be within certain absolute bounds:
-  //              a. ABS(temp_sensitivity) < temp_sensitivity_limit
-  //              b. ABS(sensor_intercept) < sensor_intercept_limit
-  float temp_sensitivity_limit;        // [sensor units/Celsius]
-  float sensor_intercept_limit;        // [sensor units]
+  //              a. |temp_sensitivity| < temp_sensitivity_limit
+  //              b. |sensor_intercept| < sensor_intercept_limit
+  float temp_sensitivity_limit;  // [sensor units/Celsius]
+  float sensor_intercept_limit;  // [sensor units]
   size_t min_num_model_pts;
 
   // Pointer to the offset estimate closest to the current sensor temperature.
-  struct OverTempCalDataPt *nearest_offset;
+  struct OverTempModelThreeAxis *nearest_offset;
 
   // Pointer to the most recent offset estimate.
-  struct OverTempCalDataPt *latest_offset;
+  struct OverTempModelThreeAxis *latest_offset;
 
   // Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius].
   float temp_sensitivity[3];
@@ -251,15 +260,15 @@ struct OverTempCal {
   // above which the model fit is preferred for providing offset compensation
   // (also applies to checks between the nearest-temperature and the current
   // compensated estimate).
-  float jump_tolerance;                // [sensor units]
+  float jump_tolerance;  // [sensor units]
 
   // A limit used to reject new offset estimates that deviate from the current
   // model fit.
-  float outlier_limit;                 // [sensor units]
+  float outlier_limit;  // [sensor units]
 
   // This parameter is used to detect offset changes that require updates to
   // system calibration and persistent memory storage.
-  float significant_offset_change;     // [sensor units]
+  float significant_offset_change;  // [sensor units]
 
   // Used to track the previous significant change in temperature.
   float last_temp_check_celsius;
@@ -282,7 +291,7 @@ struct OverTempCal {
   // recent estimates in cases where they arrive frequently near a given
   // temperature, and prevents model oversampling (i.e., dominance of estimates
   // concentrated at a given set of temperatures).
-  float delta_temp_per_bin;            // [Celsius/bin]
+  float delta_temp_per_bin;  // [Celsius/bin]
 
   // Total number of model data points collected.
   size_t num_model_pts;
@@ -299,8 +308,7 @@ struct OverTempCal {
   // overTempCalNewModelUpdateAvailable() is called. This variable indicates
   // that the following should be stored in persistent system memory:
   //    1) 'temp_sensitivity' and 'sensor_intercept'.
-  //    2) The 'compensated_offset' offset data
-  //       (saving timestamp information is not required).
+  //    2) The 'compensated_offset' offset data.
   bool new_overtemp_model_available;
 
   // True when a new offset estimate has been computed and registers as a
@@ -320,15 +328,15 @@ struct OverTempCal {
   uint64_t temperature_print_timer;
 #endif  // OVERTEMPCAL_DBG_LOG_TEMP
 
-  size_t model_counter;                // Model output print counter.
-  float otc_unit_conversion;           // Unit conversion for debug display.
-  char otc_unit_tag[16];               // Unit descriptor (e.g., "mDPS").
-  char otc_sensor_tag[16];             // OTC sensor descriptor (e.g., "GYRO").
-  char otc_debug_tag[32];              // Temporary string descriptor.
-  size_t debug_num_model_updates;      // Total number of model updates.
-  size_t debug_num_estimates;          // Total number of offset estimates.
-  bool debug_print_trigger;            // Flag used to trigger data printout.
-#endif  // OVERTEMPCAL_DBG_ENABLED
+  size_t model_counter;            // Model output print counter.
+  float otc_unit_conversion;       // Unit conversion for debug display.
+  char otc_unit_tag[16];           // Unit descriptor (e.g., "mDPS").
+  char otc_sensor_tag[16];         // OTC sensor descriptor (e.g., "GYRO").
+  char otc_debug_tag[32];          // Temporary string descriptor.
+  size_t debug_num_model_updates;  // Total number of model updates.
+  size_t debug_num_estimates;      // Total number of offset estimates.
+  bool debug_print_trigger;        // Flag used to trigger data printout.
+#endif                             // OVERTEMPCAL_DBG_ENABLED
 };
 
 /////// FUNCTION PROTOTYPES ///////////////////////////////////////////////////
@@ -338,6 +346,9 @@ struct OverTempCal {
  *
  * INPUTS:
  *   over_temp_cal:             Over-temp main data structure.
+ *   parameters:                An algorithm parameters that contains the
+ *                              following initialization variables.
+ * [parameters]:
  *   min_num_model_pts:         Minimum number of model points per model
  *                              calculation update.
  *   min_temp_update_period_nanos: Limits the rate of offset updates due to an
@@ -351,19 +362,14 @@ struct OverTempCal {
  *   temp_sensitivity_limit:    Values that define the upper limits for the
  *   sensor_intercept_limit:    model parameters. The acceptance of new model
  *                              parameters must satisfy:
- *                          i.  ABS(temp_sensitivity) < temp_sensitivity_limit
- *                          ii. ABS(sensor_intercept) < sensor_intercept_limit
+ *                          i.  |temp_sensitivity| < temp_sensitivity_limit
+ *                          ii. |sensor_intercept| < sensor_intercept_limit
  *   significant_offset_change  Minimum limit that triggers offset updates.
  *   over_temp_enable:          Flag that determines whether over-temp sensor
  *                              offset compensation is applied.
  */
 void overTempCalInit(struct OverTempCal *over_temp_cal,
-                     size_t min_num_model_pts,
-                     uint64_t min_temp_update_period_nanos,
-                     float delta_temp_per_bin, float jump_tolerance,
-                     float outlier_limit, uint64_t age_limit_nanos,
-                     float temp_sensitivity_limit, float sensor_intercept_limit,
-                     float significant_offset_change, bool over_temp_enable);
+                     const struct OverTempCalParameters *parameters);
 
 /*
  * Sets the over-temp calibration model parameters.
@@ -417,7 +423,7 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
  */
 void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
                              size_t data_length, uint64_t timestamp_nanos,
-                             const struct OverTempCalDataPt *model_data);
+                             const struct OverTempModelThreeAxis *model_data);
 
 /*
  * Gets the over-temp compensation model data set.
@@ -432,7 +438,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
  */
 void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
                              size_t *data_length,
-                             struct OverTempCalDataPt *model_data);
+                             struct OverTempModelThreeAxis *model_data);
 
 /*
  * Gets the current over-temp compensated offset estimate data.
@@ -472,8 +478,8 @@ bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal);
 bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal);
 
 /*
- * Updates the sensor's offset estimate and conditionally assimilates it into
- * the over-temp model data set, 'model_data'.
+ * Updates the sensor's offset estimate and conditionally incorporates it into
+ * the over-temp model data set, 'model_data'. Updates the model dataset age.
  *
  * INPUTS:
  *   over_temp_cal:       Over-temp data structure.
@@ -491,7 +497,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
 // Updates the temperature at which the offset compensation is performed (i.e.,
 // the current measured temperature value). This function is provided mainly for
 // flexibility since temperature updates may come in from a source other than
-// the sensor itself, and at a different rate.
+// the sensor itself, and at a different rate. Since this function is
+// periodically called, it is also used to update the age of the model
+// estimates.
 void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
                                uint64_t timestamp_nanos,
                                float temperature_celsius);
@@ -527,7 +535,9 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
  * age is less than 'offset_age_nanos'. NOTE: The ordering of the
  * 'offset_age_nanos' values in the weight function array should be
  * monotonically increasing from lowest index to highest so that weighting
- * selection can be conveniently evaluated.
+ * selection can be conveniently evaluated. A simple compliance check is
+ * applied, and 'true' is returned when the input weight is positive and older
+ * than the 'index-1' weight's age.
  *
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.
@@ -536,9 +546,9 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
  *                     value and corresponding age limit below which an offset
  *                     will use the weight.
  */
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
     struct OverTempCal *over_temp_cal, size_t index,
-    const struct OverTempCalWeightPt *new_otc_weight);
+    const struct OverTempCalWeight *new_otc_weight);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_model.h b/firmware/os/algos/calibration/over_temp/over_temp_model.h
new file mode 100644
index 00000000..f359c3b3
--- /dev/null
+++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2018 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 LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Defines the maximum size of the OverTempCal 'model_data' array.
+#define OTC_MODEL_SIZE (40)
+
+/*
+ * Over-temperature data structures that contain a modeled sensor offset
+ * estimate, an associated temperature, and the age of the data point since it
+ * first entered the model.
+ */
+
+struct OverTempModelThreeAxis {
+  // Sensor offset estimate, temperature, and age timestamp.
+  uint64_t offset_age_nanos;  // [nanoseconds]
+  float offset_temp_celsius;  // [Celsius]
+
+  // Three-axis offset estimate (indices: 0=x, 1=y, 2=z).
+  float offset[3];
+};
+
+struct OverTempModelSingleAxis {
+  // Sensor offset estimate, temperature, and age timestamp.
+  uint64_t offset_age_nanos;  // [nanoseconds]
+  float offset_temp_celsius;  // [Celsius]
+
+  // Single-axis offset estimate.
+  float offset;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
new file mode 100644
index 00000000..d1b66195
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2018 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 "calibration/sample_rate_estimator/sample_rate_estimator.h"
+
+#include <string.h>
+
+#include "common/math/macros.h"
+#include "util/nano_assert.h"
+
+// Helper function used to reset the sampling rate estimator accumulator.
+static void sampleRateEstimatorResetAccumulator(
+    struct SampleRateEstimator* sample_rate_estimator) {
+  sample_rate_estimator->last_timestamp_nanos = 0.0f;
+  sample_rate_estimator->interval_accumulator_nanos = 0.0f;
+  sample_rate_estimator->num_intervals_collected = 0;
+}
+
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+                             size_t num_intervals_to_collect,
+                             float max_interval_sec) {
+  ASSERT_NOT_NULL(sample_rate_estimator);
+  memset(sample_rate_estimator, 0, sizeof(struct SampleRateEstimator));
+  sample_rate_estimator->mean_sampling_rate_estimate_hz =
+      SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ;
+  sample_rate_estimator->num_intervals_to_collect = num_intervals_to_collect;
+  sample_rate_estimator->max_interval_nanos =
+      max_interval_sec * SEC_TO_NANOS(1);
+}
+
+float sampleRateEstimatorGetHz(
+    struct SampleRateEstimator* sample_rate_estimator) {
+  sample_rate_estimator->new_sampling_rate_estimate_ready = false;
+  return sample_rate_estimator->mean_sampling_rate_estimate_hz;
+}
+
+void sampleRateEstimatorUpdate(
+    struct SampleRateEstimator* sample_rate_estimator,
+    uint64_t timestamp_nanos) {
+  // Resets the current interval capture and returns if:
+  //   1. A bad timestamp was received (i.e., time not monotonic).
+  //   2. 'last_timestamp_nanos' is zero. NOTE: 'last_timestamp_nanos' = 0
+  //      indicates that the first complete time interval has not been captured
+  //      yet (so, set it and return).
+  if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+      sample_rate_estimator->last_timestamp_nanos == 0) {
+    sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+    return;
+  }
+
+  // Computes the current sampling interval. This conversion will be very fast
+  // for intervals less than ~4.3 seconds (i.e., 2^32 nano-seconds).
+  const float next_interval_nanos = floatFromUint64(
+      timestamp_nanos - sample_rate_estimator->last_timestamp_nanos);
+
+  // Helps prevent corruption of the estimator when there are gaps in the input
+  // sampling intervals greater than 'max_interval_nanos' (i.e., intermittant
+  // periods where there are no input timestamps).
+  if (next_interval_nanos >= sample_rate_estimator->max_interval_nanos) {
+    // Resets the estimator and returns.
+    sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+    return;
+  }
+
+  // Accumulates the next sampling interval.
+  sample_rate_estimator->interval_accumulator_nanos += next_interval_nanos;
+  sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+  sample_rate_estimator->num_intervals_collected++;
+
+  // If the number of collected time intervals exceed the target number, then
+  // this computes a new sample rate estimate.
+  if (sample_rate_estimator->num_intervals_collected >
+      sample_rate_estimator->num_intervals_to_collect) {
+    sample_rate_estimator->mean_sampling_rate_estimate_hz =
+        sample_rate_estimator->num_intervals_collected *
+        (SEC_TO_NANOS(1) / sample_rate_estimator->interval_accumulator_nanos);
+
+    // Sets the polling flag to indicate that a new estimate is ready.
+    sample_rate_estimator->new_sampling_rate_estimate_ready = true;
+
+    // Resets the estimator variables.
+    sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+  }
+}
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
new file mode 100644
index 00000000..f506eea5
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+/*
+ * [Sample Rate Estimator]
+ * This module periodically estimates the mean sampling rate of a uniformly
+ * sampled signal. Note, this estimator uses a floating point accumulator for
+ * the timing intervals. This trades-off some accumulation of finite precision
+ * error to enhance the range of estimated sampling rates and prevent overflow
+ * when an equivalent 32bit integer accumulator is used. In typical use cases
+ * (sample rates: 5Hz - 2kHz, num_intervals_to_collect 10 - 100), the sample
+ * rate accuracy is typically much better than 0.1Hz.
+ *
+ * FUNCTIONALITY:
+ * sampleRateEstimatorInit -- Initializes the estimator. Sets the number of time
+ *   intervals require to compute the sample rate mean, and the upper limit for
+ *   the acceptable time interval.
+ *
+ * new_sampling_rate_estimate_ready -- Check this boolean flag if polling for
+ *   new estimates is desired.
+ *
+ * sampleRateEstimatorGetHz -- Returns the latest sample rate estimate and
+ *   resets the polling flag.
+ *
+ * sampleRateEstimatorUpdate -- Processes new timestamp data and computes new
+ *   sample rate estimates.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Designates the value used to indicate an invalid sample rate estimate.
+#define SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ (-1.0f)
+
+// Sample rate estimator data structure.
+struct SampleRateEstimator {
+  // Used to compute sample intervals to estimate the sampling rate.
+  uint64_t last_timestamp_nanos;
+
+  // Accumulator used for computing the mean sampling interval.
+  float interval_accumulator_nanos;
+
+  // The upper limit on the acceptance of valid time intervals (in nanoseconds).
+  // Larger time deltas result in a reset of the interval accumulator.
+  float max_interval_nanos;
+
+  // The most recent mean sampling rate estimate.
+  float mean_sampling_rate_estimate_hz;
+
+  /*
+   * The targeted number of time intervals required for a new sample rate mean
+   * calculation.
+   *
+   * NOTE: Assuming that the time interval noise is independent and identically
+   * distributed and drawn from a zero-mean normal distribution with variance
+   * 'Sigma^2'. The expected noise reduction is related to the choice of
+   * 'num_intervals_to_collect' as:
+   *
+   *   Output RMS Noise = Sigma / sqrt(num_intervals_to_collect).
+   *
+   * Example, a value of 100 for a 100Hz signal would provide updates once every
+   * second and provide a 90% reduction (i.e., [1 - 1/sqrt(100)] * 100%) in time
+   * interval noise.
+   */
+  size_t num_intervals_to_collect;
+
+  // The number of time intervals currently collected.
+  size_t num_intervals_collected;
+
+  // Polling flag used to signal that a new estimate is ready. This flag is
+  // reset when 'sampleRateEstimatorGetHz' is called.
+  bool new_sampling_rate_estimate_ready;
+};
+
+// Initializes the sample rate estimator, sets the number of time intervals
+// for the mean sample rate calculation, and defines the tolerable gap in timing
+// data (in seconds).
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+                             size_t num_intervals_to_collect,
+                             float max_interval_sec);
+
+// Rather than using a function to poll for new sample rate estimates, which
+// would incur an additional function call, simply check
+// 'new_sampling_rate_estimate_ready' directly.
+
+// Returns the most recent sampling rate estimate, and resets the
+// 'new_sampling_rate_estimate_ready' polling flag. Note, if polling is not
+// used, one may access the sample rate estimate directly from the struct and
+// avoid this function call.
+float sampleRateEstimatorGetHz(
+    struct SampleRateEstimator* sample_rate_estimator);
+
+// Takes in a new timestamp and updates the sampling rate estimator.
+void sampleRateEstimatorUpdate(
+    struct SampleRateEstimator* sample_rate_estimator,
+    uint64_t timestamp_nanos);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
index 9ae72d27..2de4a8c0 100644
--- a/firmware/os/algos/calibration/common/calibration_data.c
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
@@ -1,4 +1,20 @@
-#include "calibration/common/calibration_data.h"
+/*
+ * 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 "calibration/sphere_fit/calibration_data.h"
 
 #include <string.h>
 
@@ -9,7 +25,7 @@
 
 // Set calibration data to identity scale factors, zero skew and
 // zero bias.
-void calDataReset(struct ThreeAxisCalData *calstruct) {
+void calDataReset(struct ThreeAxisCalData* calstruct) {
   memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
   calstruct->scale_factor_x = 1.0f;
   calstruct->scale_factor_y = 1.0f;
@@ -28,9 +44,9 @@ void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
   //                   skew_yx    scale_factor_y   0
   //                   skew_zx       skew_zy   scale_factor_z].
   x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
-  x_corrected[1] = calstruct->skew_yx * x_temp[0] +
-      calstruct->scale_factor_y * x_temp[1];
+  x_corrected[1] =
+      calstruct->skew_yx * x_temp[0] + calstruct->scale_factor_y * x_temp[1];
   x_corrected[2] = calstruct->skew_zx * x_temp[0] +
-      calstruct->skew_zy * x_temp[1] +
-      calstruct->scale_factor_z * x_temp[2];
+                   calstruct->skew_zy * x_temp[1] +
+                   calstruct->scale_factor_z * x_temp[2];
 }
diff --git a/firmware/os/algos/calibration/common/calibration_data.h b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
index b0e6809b..405d0acc 100644
--- a/firmware/os/algos/calibration/common/calibration_data.h
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
@@ -13,7 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-/////////////////////////////////////////////////////////////////////////////
+
 /*
  * This module contains a data structure and corresponding helper functions for
  * a three-axis sensor calibration.  The calibration consists of a bias vector,
@@ -23,8 +23,9 @@
  *
  * corrected_data = scale_skew_mat * (impaired_data - bias).
  */
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
 
 #include <stdint.h>
 
@@ -57,7 +58,7 @@ struct ThreeAxisCalData {
 
 // Set calibration data to identity scale factors, zero skew and
 // zero bias.
-void calDataReset(struct ThreeAxisCalData *calstruct);
+void calDataReset(struct ThreeAxisCalData* calstruct);
 
 // Apply a stored calibration to correct a single sample of impaired sensor
 // data.
@@ -69,4 +70,4 @@ void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
 }
 #endif
 
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
index 2c26af55..853a73d5 100644
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.c
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -1,4 +1,20 @@
-#include "calibration/common/sphere_fit_calibration.h"
+/*
+ * 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 "calibration/sphere_fit/sphere_fit_calibration.h"
 
 #include <errno.h>
 #include <stdarg.h>
@@ -19,7 +35,7 @@ static bool runCalibration(struct SphereFitCal *sphere_cal,
                            const struct SphereFitData *data,
                            uint64_t timestamp_nanos);
 
-#define MIN_VALID_DATA_NORM (1e-4)
+#define MIN_VALID_DATA_NORM (1e-4f)
 
 // FUNCTION IMPLEMENTATIONS
 //////////////////////////////////////////////////////////////////////////////
@@ -103,7 +119,7 @@ void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
   ASSERT_NOT_NULL(f_data);
   ASSERT_NOT_NULL(residual);
 
-  const struct SphereFitData *data = (const struct SphereFitData*)f_data;
+  const struct SphereFitData *data = (const struct SphereFitData *)f_data;
 
   // Verify that expected norm is non-zero, else use default of 1.0.
   float expected_norm = 1.0;
@@ -204,10 +220,9 @@ bool runCalibration(struct SphereFitCal *sphere_cal,
   float x_sol[SF_STATE_DIM];
 
   // Run calibration
-  const enum LmStatus status = lmSolverSolve(&sphere_cal->lm_solver,
-                                             sphere_cal->x0, (void *)data,
-                                             SF_STATE_DIM, data->num_fit_points,
-                                             x_sol);
+  const enum LmStatus status =
+      lmSolverSolve(&sphere_cal->lm_solver, sphere_cal->x0, (void *)data,
+                    SF_STATE_DIM, data->num_fit_points, x_sol);
 
   // Check if solver was successful
   if (status == RELATIVE_STEP_SUFFICIENTLY_SMALL ||
@@ -215,32 +230,34 @@ bool runCalibration(struct SphereFitCal *sphere_cal,
     // TODO(dvitus): Check quality of new fit before using.
     // Store new fit.
 #ifdef SPHERE_FIT_DBG_ENABLED
-    CAL_DEBUG_LOG(
-        "[SPHERE CAL]",
-        "Solution found in %d iterations with status %d.\n",
-        sphere_cal->lm_solver.num_iter, status);
-    CAL_DEBUG_LOG(
-        "[SPHERE CAL]",
-        "Solution:\n {%s%d.%06d [M(1,1)], %s%d.%06d [M(2,1)], "
-        "%s%d.%06d [M(2,2)], \n"
-        "%s%d.%06d [M(3,1)], %s%d.%06d [M(3,2)], %s%d.%06d [M(3,3)], \n"
-        "%s%d.%06d [b(1)], %s%d.%06d [b(2)], %s%d.%06d [b(3)]}.",
-        CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
-        CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
-        CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
-        CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
-        CAL_ENCODE_FLOAT(x_sol[8], 6));
-#endif
+    CAL_DEBUG_LOG("[SPHERE CAL]",
+                  "Solution found in %d iterations with status %d.\n",
+                  sphere_cal->lm_solver.num_iter, status);
+    CAL_DEBUG_LOG("[SPHERE CAL]", "Solution:\n {"
+                  CAL_FORMAT_6DIGITS " [M(1,1)], "
+                  CAL_FORMAT_6DIGITS " [M(2,1)], "
+                  CAL_FORMAT_6DIGITS " [M(2,2)], \n"
+                  CAL_FORMAT_6DIGITS " [M(3,1)], "
+                  CAL_FORMAT_6DIGITS " [M(3,2)], "
+                  CAL_FORMAT_6DIGITS " [M(3,3)], \n"
+                  CAL_FORMAT_6DIGITS " [b(1)], "
+                  CAL_FORMAT_6DIGITS " [b(2)], "
+                  CAL_FORMAT_6DIGITS " [b(3)]}.",
+                  CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
+                  CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
+                  CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
+                  CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
+                  CAL_ENCODE_FLOAT(x_sol[8], 6));
+#endif  // SPHERE_FIT_DBG_ENABLED
     memcpy(sphere_cal->x, x_sol, sizeof(x_sol));
     sphere_cal->estimate_time_nanos = timestamp_nanos;
     return true;
   } else {
 #ifdef SPHERE_FIT_DBG_ENABLED
-     CAL_DEBUG_LOG(
-        "[SPHERE CAL]",
-        "Solution failed in %d iterations with status %d.\n",
-        sphere_cal->lm_solver.num_iter, status);
-#endif
+    CAL_DEBUG_LOG("[SPHERE CAL]",
+                  "Solution failed in %d iterations with status %d.\n",
+                  sphere_cal->lm_solver.num_iter, status);
+#endif  // SPHERE_FIT_DBG_ENABLED
   }
 
   return false;
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
index e49f225a..d3bbf7f1 100644
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.h
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
@@ -13,7 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-/////////////////////////////////////////////////////////////////////////////
+
 /*
  * This module contains an algorithm for performing a sphere fit calibration.
  * A sphere fit calibration solves the following non-linear least squares
@@ -34,13 +34,14 @@
  * M and b.  M is assumed to be a lower diagonal, consisting of 6 parameters.
  *
  */
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
 
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "calibration/common/calibration_data.h"
+#include "calibration/sphere_fit/calibration_data.h"
 #include "common/math/levenberg_marquardt.h"
 
 #ifdef __cplusplus
@@ -140,4 +141,4 @@ void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
 }
 #endif
 
-#endif  //  LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+#endif  //  LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 1bd80fd4..46297dbb 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -16,12 +16,13 @@
 
 ///////////////////////////////////////////////////////////////
 /*
- * Logging macros for printing formatted strings to Nanohub.
+ * Logging macros for printing user-debug messages.
  */
 ///////////////////////////////////////////////////////////////
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
 
+// clang-format off
 #ifdef GCC_DEBUG_LOG
 # include <stdio.h>
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
@@ -35,31 +36,61 @@
    LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
    osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
-#else  // _OS_BUILD_
+#elif NANOHUB_DEBUG_LOG
 # include <chre.h>
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
    chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
+#else
+// CHRE/SLPI Nanoapp Logging.
+# include "chre/util/nanoapp/log.h"
+# ifndef LOG_TAG
+#  define LOG_TAG ""
+# endif  // LOG_TAG
+# define CAL_DEBUG_LOG(tag, format, ...) \
+   LOGI("%s " format, tag, ##__VA_ARGS__)
 #endif  // GCC_DEBUG_LOG
+// clang-format on
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-// Using this macro because floorf() is not currently implemented by the Nanohub
-// firmware.
+// Floor macro implementation for platforms that do not supply the standard
+// floorf() math function.
 #define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x)))  // NOLINT
 
+/*
+ * On some embedded software platforms numerical string formatting is not fully
+ * supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that
+ * prints floating-point values having a specified number of digits using the
+ * CAL_ENCODE_FLOAT macro.
+ *   Examples:
+ *     - Nanohub does not support floating-point format strings.
+ *     - CHRE/SLPI allows %.Xf for printing float values.
+ */
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
 // Macro used to print floating point numbers with a specified number of digits.
-#define CAL_ENCODE_FLOAT(x, num_digits) \
-  ((x < 0) ? "-" : ""),                 \
-  (int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits))  // NOLINT
+# define CAL_ENCODE_FLOAT(x, num_digits)           \
+  ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)),  \
+      (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) *     \
+            powf(10, num_digits))  // NOLINT
 
 // Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
 // desired significant digits.
-#define CAL_FORMAT_3DIGITS "%s%d.%03d"
-#define CAL_FORMAT_6DIGITS "%s%d.%06d"
-#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
-#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+# define CAL_FORMAT_3DIGITS "%s%d.%03d"
+# define CAL_FORMAT_6DIGITS "%s%d.%06d"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+#else
+// Pass-through when float string formatting (e.g., %.6f) is available.
+# define CAL_ENCODE_FLOAT(x, num_digits) (x)
+
+// Float string formatting helpers.
+# define CAL_FORMAT_3DIGITS "%.3f"
+# define CAL_FORMAT_6DIGITS "%.6f"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%.3f, %.3f, %.3f"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%.6f, %.6f, %.6f"
+#endif  // CAL_NO_FLOAT_FORMAT_STRINGS
 
 #ifdef __cplusplus
 }
diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c
new file mode 100644
index 00000000..a24a31bf
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.c
@@ -0,0 +1,120 @@
+#include "common/math/kasa.h"
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "common/math/mat.h"
+
+void kasaReset(struct KasaFit *kasa) {
+  kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
+  kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
+  kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
+  kasa->acc_zz = kasa->acc_zw = 0.0f;
+  kasa->nsamples = 0;
+}
+
+void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
+
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
+  float w = x * x + y * y + z * z;
+
+  kasa->acc_x += x;
+  kasa->acc_y += y;
+  kasa->acc_z += z;
+  kasa->acc_w += w;
+
+  kasa->acc_xx += x * x;
+  kasa->acc_xy += x * y;
+  kasa->acc_xz += x * z;
+  kasa->acc_xw += x * w;
+
+  kasa->acc_yy += y * y;
+  kasa->acc_yz += y * z;
+  kasa->acc_yw += y * w;
+
+  kasa->acc_zz += z * z;
+  kasa->acc_zw += z * w;
+
+  kasa->nsamples += 1;
+}
+
+bool kasaNormalize(struct KasaFit *kasa) {
+  if (kasa->nsamples == 0) {
+    return false;
+  }
+
+  float inv = 1.0f / kasa->nsamples;
+
+  kasa->acc_x *= inv;
+  kasa->acc_y *= inv;
+  kasa->acc_z *= inv;
+  kasa->acc_w *= inv;
+
+  kasa->acc_xx *= inv;
+  kasa->acc_xy *= inv;
+  kasa->acc_xz *= inv;
+  kasa->acc_xw *= inv;
+
+  kasa->acc_yy *= inv;
+  kasa->acc_yz *= inv;
+  kasa->acc_yw *= inv;
+
+  kasa->acc_zz *= inv;
+  kasa->acc_zw *= inv;
+
+  return true;
+}
+
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+            float max_fit, float min_fit) {
+  //    A    *   out   =    b
+  // (4 x 4)   (4 x 1)   (4 x 1)
+  struct Mat44 A;
+  A.elem[0][0] = kasa->acc_xx;
+  A.elem[0][1] = kasa->acc_xy;
+  A.elem[0][2] = kasa->acc_xz;
+  A.elem[0][3] = kasa->acc_x;
+  A.elem[1][0] = kasa->acc_xy;
+  A.elem[1][1] = kasa->acc_yy;
+  A.elem[1][2] = kasa->acc_yz;
+  A.elem[1][3] = kasa->acc_y;
+  A.elem[2][0] = kasa->acc_xz;
+  A.elem[2][1] = kasa->acc_yz;
+  A.elem[2][2] = kasa->acc_zz;
+  A.elem[2][3] = kasa->acc_z;
+  A.elem[3][0] = kasa->acc_x;
+  A.elem[3][1] = kasa->acc_y;
+  A.elem[3][2] = kasa->acc_z;
+  A.elem[3][3] = 1.0f;
+
+  struct Vec4 b;
+  initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
+
+  struct Size4 pivot;
+  mat44DecomposeLup(&A, &pivot);
+
+  struct Vec4 out;
+  mat44Solve(&A, &out, &b, &pivot);
+
+  // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
+  //
+  // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
+  // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
+
+  struct Vec3 v;
+  initVec3(&v, out.x, out.y, out.z);
+  vec3ScalarMul(&v, -0.5f);
+
+  float r_square = vec3Dot(&v, &v) - out.w;
+  float r = (r_square > 0) ? sqrtf(r_square) : 0;
+
+  initVec3(bias, v.x, v.y, v.z);
+  *radius = r;
+
+  int success = 0;
+  if (r > min_fit && r < max_fit) {
+    success = 1;
+  }
+
+  return success;
+}
diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h
new file mode 100644
index 00000000..e9652d60
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.h
@@ -0,0 +1,54 @@
+/*
+ * This module provides a data structure, initialization, and fit
+ * routine for algorithms that use the Kasa method for determining the
+ * 3-dimensional offset vector from a set of points on a sphere.
+ *
+ * Reference: I. Kåsa, "A circle fitting procedure and its error analysis," in
+ * IEEE Transactions on Instrumentation and Measurement, vol. IM-25, no. 1, pp.
+ * 8-14, March 1976.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+
+#include <stdbool.h>
+
+#include "common/math/vec.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct KasaFit {
+  float acc_x, acc_y, acc_z, acc_w;
+  float acc_xx, acc_xy, acc_xz, acc_xw;
+  float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
+  size_t nsamples;
+};
+
+// Resets the KasaFit data structure (sets all variables to zero).
+void kasaReset(struct KasaFit *kasa);
+
+// Initializes the KasaFit data structure.
+void kasaInit(struct KasaFit *kasa);
+
+// Accumulates the Kasa acc_** variables with the input vector [x, y, z], and
+// updates the number of samples.
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z);
+
+// Normalizes the Kasa acc_** variables. Returns 'false' if the number of
+// samples is zero, otherwise 'true'.
+bool kasaNormalize(struct KasaFit *kasa);
+
+// Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for
+// the best-fit sphere using the normal equations, and the sphere's 'radius'.
+// Returns '1' if the radius of the fit sphere is within the bounds
+// (min_fit, max_fit), otherwise '0'.
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+            float max_fit, float min_fit);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
index c01d2eb1..d62308da 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.h
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -109,7 +109,7 @@ struct LmSolver {
 
 // Initializes LM solver with provided parameters and error function.
 void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
-                  ResidualAndJacobianFunction error_func);
+                  ResidualAndJacobianFunction func);
 
 void lmSolverDestroy(struct LmSolver *solver);
 
@@ -133,7 +133,7 @@ void lmSolverSetData(struct LmSolver *solver, struct LmData *data);
  */
 enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
                             void *f_data, size_t state_dim, size_t meas_dim,
-                            float *est_state);
+                            float *state);
 
 ////////////////////////// TEST UTILITIES ////////////////////////////////////
 // This function is exposed here for testing purposes only.
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index 5c06e247..cb75595b 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -14,30 +14,43 @@
  * limitations under the License.
  */
 
-// This file contains helper macros and definitions.
+// This file contains frequently used constants and helper macros.
+
+#include <stdint.h>
 
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
 
-// Mathematical constants.
-#define NANO_PI (3.14159265359f)
+// Constants.
+#define NANO_PI                     (3.14159265359f)
+#define INVALID_TEMPERATURE_CELSIUS (-274.0f)
 
 // Common math operations.
 #define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
 #define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
 #define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+#define SIGMOID(x) (1 / (1 + expf(-x)))
 
 // Timestamp conversion macros.
 #ifdef __cplusplus
-#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000)))
 #else
-#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000)  // NOLINT
+#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000)))  // NOLINT
 #endif
 
-#define SEC_TO_NANOS(x)  MSEC_TO_NANOS(x * 1000)
-#define MIN_TO_NANOS(x)  SEC_TO_NANOS(x * 60)
-#define HRS_TO_NANOS(x)  MIN_TO_NANOS(x * 60)
-#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+#define SEC_TO_NANOS(x)  MSEC_TO_NANOS(x * UINT64_C(1000))
+#define MIN_TO_NANOS(x)  SEC_TO_NANOS (x * UINT64_C(60))
+#define HRS_TO_NANOS(x)  MIN_TO_NANOS (x * UINT64_C(60))
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24))
+
+// Sample rate to period conversion.
+#ifdef __cplusplus
+#define HZ_TO_PERIOD_NANOS(hz) \
+  (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024)))
+#else
+#define HZ_TO_PERIOD_NANOS(hz) \
+  (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024)))  // NOLINT
+#endif
 
 // Unit conversion: nanoseconds to seconds.
 #define NANOS_TO_SEC (1.0e-9f)
@@ -57,4 +70,26 @@
 #define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
   (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// This conversion function may be necessary for embedded hardware that can't
+// cast a uint64_t to a float directly. This conversion function was taken from:
+// [android]//device/google/contexthub/firmware/os/core/floatRt.c
+static inline float floatFromUint64(uint64_t v) {
+  uint32_t hi = v >> 32;
+  uint32_t lo = (uint32_t) v;
+
+  if (!hi) {  // This is very fast for cases where 'v' fits into a uint32_t.
+    return (float)lo;
+  } else {
+    return ((float)hi) * 4294967296.0f + (float)lo;
+  }
+}
+
+#ifdef __cplusplus
+}
+#endif
+
 #endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 34aaa512..8b62cceb 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -32,8 +32,8 @@
 #include <stddef.h>
 #include <string.h>
 
-#define EPSILON 1E-5
-#define CHOLESKY_TOLERANCE 1E-6
+#define EPSILON 1E-5f
+#define CHOLESKY_TOLERANCE 1E-6f
 
 // Forward declarations.
 static void mat33SwapRows(struct Mat33 *A, uint32_t i, uint32_t j);
@@ -248,8 +248,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
       }
     }
     // divide by zero guard.
-    ASSERT(fabs(tmp.elem[i][i]) > 0);
-    if(!(fabs(tmp.elem[i][i]) > 0)) {
+    ASSERT(fabsf(tmp.elem[i][i]) > 0);
+    if(!(fabsf(tmp.elem[i][i]) > 0)) {
       return;
     }
     t = 1.0f / tmp.elem[i][i];
@@ -407,6 +407,16 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
   initVec3(eigenvals, _eigenvals[0], _eigenvals[1], _eigenvals[2]);
 }
 
+float mat33Determinant(const struct Mat33 *A) {
+  ASSERT_NOT_NULL(A);
+  return A->elem[0][0] *
+      (A->elem[1][1] * A->elem[2][2] - A->elem[1][2] * A->elem[2][1])
+      - A->elem[0][1] *
+      (A->elem[1][0] * A->elem[2][2] - A->elem[1][2] * A->elem[2][0])
+      + A->elem[0][2] *
+      (A->elem[1][0] * A->elem[2][1] - A->elem[1][1] * A->elem[2][0]);
+}
+
 // index of largest off-diagonal element in row k
 UNROLLED
 uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h
index 13494f55..9d69405e 100644
--- a/firmware/os/algos/common/math/mat.h
+++ b/firmware/os/algos/common/math/mat.h
@@ -125,6 +125,8 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A);
 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
                         struct Mat33 *eigenvecs);
 
+// Computes the determinant of a 3 by 3 matrix.
+float mat33Determinant(const struct Mat33 *A);
 
 // 4x4 MATRIX MATH /////////////////////////////////////////////////////////////
 // Updates out with the multiplication of A and v, i.e.:
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index 0a4c8b39..e839ad53 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -70,6 +70,17 @@ static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
   v->z += w->z;
 }
 
+// Sets u as the sum of v and w.
+static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v,
+                               const struct Vec3 *w) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  u->x = v->x + w->x;
+  u->y = v->y + w->y;
+  u->z = v->z + w->z;
+}
+
 // Updates v as the subtraction of w from v.
 static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
   ASSERT_NOT_NULL(v);
@@ -79,6 +90,17 @@ static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
   v->z -= w->z;
 }
 
+// Sets u as the difference of v and w.
+static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v,
+                               const struct Vec3 *w) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  u->x = v->x - w->x;
+  u->y = v->y - w->y;
+  u->z = v->z - w->z;
+}
+
 // Scales v by the scalar c, i.e. v = c * v.
 static inline void vec3ScalarMul(struct Vec3 *v, float c) {
   ASSERT_NOT_NULL(v);
diff --git a/firmware/os/algos/util/array.h b/firmware/os/algos/util/array.h
new file mode 100644
index 00000000..9658be44
--- /dev/null
+++ b/firmware/os/algos/util/array.h
@@ -0,0 +1,6 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+
+#define ARRAY_SIZE(a) (sizeof((a)) / sizeof((a)[0]))
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
index da777491..cb3286e2 100644
--- a/firmware/os/algos/util/nano_assert.h
+++ b/firmware/os/algos/util/nano_assert.h
@@ -1,7 +1,7 @@
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
 
-#if defined(__NANOHUB__) || defined(_OS_BUILD_)
+#ifndef GOOGLE3
 
 // For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided
 // by the build system, which has the directory stripped. But allow the
@@ -32,7 +32,7 @@
 #include <assert.h>
 #define ASSERT_IMPL(x) assert(x)
 
-#endif
+#endif  // GOOGLE3
 
 #ifndef ASSERT
 #ifdef NANO_ASSERT_ENABLED
diff --git a/firmware/os/algos/util/nano_log.h b/firmware/os/algos/util/nano_log.h
new file mode 100644
index 00000000..8ec80430
--- /dev/null
+++ b/firmware/os/algos/util/nano_log.h
@@ -0,0 +1,13 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+
+#ifdef GOOGLE3
+#include <stdio.h>
+
+// ignore log level argument
+#define LOG_FUNC(level, ...) printf(__VA_ARGS__)
+#endif
+
+#include "third_party/contexthub/nanoapps/util/log/log.h"
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
diff --git a/firmware/os/core/heap.c b/firmware/os/core/heap.c
index 2f38a0da..f6e50672 100644
--- a/firmware/os/core/heap.c
+++ b/firmware/os/core/heap.c
@@ -229,3 +229,50 @@ int heapFreeAll(uint32_t tid)
 
     return count;
 }
+
+int heapGetFreeSize(int *numChunks, int *largestChunk)
+{
+    struct HeapNode *node;
+    bool haveLock;
+    int bytes = 0;
+    *numChunks = *largestChunk = 0;
+
+    // this can only fail if called from interrupt
+    haveLock = trylockTryTake(&gHeapLock);
+    if (!haveLock)
+        return -1;
+
+    for (node = gHeapHead; node; node = heapPrvGetNext(node)) {
+        if (!node->used) {
+            if (node->size > *largestChunk)
+                *largestChunk = node->size;
+            bytes += node->size + sizeof(struct HeapNode);
+            (*numChunks)++;
+        }
+    }
+    trylockRelease(&gHeapLock);
+
+    return bytes;
+}
+
+int heapGetTaskSize(uint32_t tid)
+{
+    struct HeapNode *node;
+    bool haveLock;
+    int bytes = 0;
+
+    // this can only fail if called from interrupt
+    haveLock = trylockTryTake(&gHeapLock);
+    if (!haveLock)
+        return -1;
+
+    tid &= TIDX_MASK;
+    for (node = gHeapHead; node; node = heapPrvGetNext(node)) {
+        if (node->used && node->tidx == tid) {
+            bytes += node->size + sizeof(struct HeapNode);
+        }
+    }
+    trylockRelease(&gHeapLock);
+
+    return bytes;
+}
diff --git a/firmware/os/core/hostIntf.c b/firmware/os/core/hostIntf.c
index 084e508c..ccd571d8 100644
--- a/firmware/os/core/hostIntf.c
+++ b/firmware/os/core/hostIntf.c
@@ -35,6 +35,7 @@
 #include <nanohubCommand.h>
 #include <nanohubPacket.h>
 #include <seos.h>
+#include <seos_priv.h>
 #include <util.h>
 #include <atomicBitset.h>
 #include <atomic.h>
@@ -1097,18 +1098,36 @@ static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable,
 
 static void hostIntfNotifyReboot(uint32_t reason)
 {
-    struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp));
     __le32 raw_reason = htole32(reason);
 
+    struct NanohubHalSysMgmtTx *resp;
+    resp = heapAlloc(sizeof(*resp));
     if (resp) {
-        resp->hdr = (struct NanohubHalHdr){
+        resp->hdr = (struct NanohubHalHdr) {
             .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
-            .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
-            .msg = NANOHUB_HAL_REBOOT,
+            .len = sizeof(*resp) - sizeof(resp->hdr),
         };
-        memcpy(&resp->reason, &raw_reason, sizeof(resp->reason));
-        osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+        resp->ret = (struct NanohubHalRet) {
+            .msg = NANOHUB_HAL_SYS_MGMT,
+            .status = raw_reason,
+        };
+        resp->cmd = NANOHUB_HAL_SYS_MGMT_REBOOT;
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+    }
+
+#ifdef LEGACY_HAL_ENABLED
+    struct NanohubHalLegacyRebootTx *respLegacy;
+    respLegacy = heapAlloc(sizeof(*respLegacy));
+    if (respLegacy) {
+        respLegacy->hdr = (struct NanohubHalLegacyHdr) {
+            .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+            .len = sizeof(*respLegacy) - sizeof(respLegacy->hdr) + sizeof(respLegacy->hdr.msg),
+            .msg = NANOHUB_HAL_LEGACY_REBOOT,
+        };
+        memcpy(&respLegacy->reason, &raw_reason, sizeof(respLegacy->reason));
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST, respLegacy, heapFree);
     }
+#endif
 }
 
 static void queueFlush(struct ActiveSensor *sensor)
@@ -1151,9 +1170,10 @@ static void onEvtAppStart(const void *evtData)
         struct HostIntfDataBuffer *data;
 
         osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
-        osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
-        osEventSubscribe(mHostIntfTid, EVT_APP_TO_SENSOR_HAL_DATA);
-        osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
+        osEventsSubscribe(4, EVT_NO_SENSOR_CONFIG_EVENT,
+                             EVT_APP_TO_SENSOR_HAL_DATA,
+                             EVT_APP_TO_HOST,
+                             EVT_APP_TO_HOST_CHRE);
 #ifdef DEBUG_LOG_EVT
         osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
         platEarlyLogFlush();
@@ -1186,12 +1206,59 @@ static void onEvtAppToHost(const void *evtData)
     }
 }
 
+static void onEvtAppToHostChre(const void *evtData)
+{
+    const struct HostHubChrePacket *hostMsg = evtData;
+
+    if (hostMsg->messageSize <= HOST_HUB_CHRE_PACKET_MAX_LEN) {
+        struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->messageSize);
+
+        data->sensType = SENS_TYPE_INVALID;
+        data->length = sizeof(*hostMsg) + hostMsg->messageSize;
+        data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
+        data->interrupt = NANOHUB_INT_WAKEUP;
+        memcpy(data->buffer, evtData, data->length);
+        hostIntfAddBlock(data, false, true);
+    }
+}
+
+#ifdef LEGACY_HAL_ENABLED
+static void handleLegacyHalCmd(const uint8_t *halData, uint8_t size)
+{
+    const struct NanohubHalLegacyCommand *halCmd = nanohubHalLegacyFindCommand(halData[0]);
+    if (halCmd)
+        halCmd->handler((void *)&halData[1], size - 1);
+}
+
 static void onEvtAppFromHost(const void *evtData)
 {
     const uint8_t *halMsg = evtData;
-    const struct NanohubHalCommand *halCmd = nanohubHalFindCommand(halMsg[1]);
-    if (halCmd)
-        halCmd->handler((void *)&halMsg[2], halMsg[0] - 1);
+    handleLegacyHalCmd(&halMsg[1], halMsg[0]);
+}
+#endif
+
+static void onEvtAppFromHostChre(const void *evtData)
+{
+    const struct NanohubMsgChreHdr *halMsg = (const struct NanohubMsgChreHdr *)evtData;
+    const struct NanohubHalCommand *halCmd;
+    const uint8_t *halData = (const uint8_t *)(halMsg+1);
+    uint8_t len;
+    uint32_t transactionId;
+
+    memcpy(&transactionId, &halMsg->appEvent, sizeof(halMsg->appEvent));
+
+    if (halMsg->size >= 1) {
+        len = halMsg->size - 1;
+        halCmd = nanohubHalFindCommand(halData[0]);
+        if (halCmd) {
+            if (len >= halCmd->minDataLen && len <= halCmd->maxDataLen)
+                halCmd->handler((void *)&halData[1], len, transactionId);
+            return;
+        }
+    }
+#ifdef LEGACY_HAL_ENABLED
+    handleLegacyHalCmd(halData, halMsg->size);
+#endif
 }
 
 #ifdef DEBUG_LOG_EVT
@@ -1485,16 +1552,24 @@ static void onEvtSensorData(uint32_t evtType, const void* evtData)
 
 static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
 {
-    switch (evtType) {
+    switch (EVENT_GET_EVENT(evtType)) {
     case EVT_APP_START:
         onEvtAppStart(evtData);
         break;
     case EVT_APP_TO_HOST:
         onEvtAppToHost(evtData);
         break;
+    case EVT_APP_TO_HOST_CHRE:
+        onEvtAppToHostChre(evtData);
+        break;
+#ifdef LEGACY_HAL_ENABLED
     case EVT_APP_FROM_HOST:
         onEvtAppFromHost(evtData);
         break;
+#endif
+    case EVT_APP_FROM_HOST_CHRE:
+        onEvtAppFromHostChre(evtData);
+        break;
 #ifdef DEBUG_LOG_EVT
     case EVT_DEBUG_LOG:
         onEvtDebugLog(evtData);
@@ -1510,7 +1585,7 @@ static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
         onEvtAppToSensorHalData(evtData);
         break;
     default:
-        onEvtSensorData(evtType, evtData);
+        onEvtSensorData(EVENT_GET_EVENT(evtType), evtData);
         break;
     }
 }
@@ -1613,4 +1688,4 @@ void hostIntfClearInterruptMask(uint32_t bit)
     cpuIntsRestore(state);
 }
 
-INTERNAL_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
+INTERNAL_CHRE_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
diff --git a/firmware/os/core/nanohubCommand.c b/firmware/os/core/nanohubCommand.c
index 67261188..d679e73d 100644
--- a/firmware/os/core/nanohubCommand.c
+++ b/firmware/os/core/nanohubCommand.c
@@ -39,6 +39,7 @@
 #include <nanohubPacket.h>
 #include <eeData.h>
 #include <seos.h>
+#include <seos_priv.h>
 #include <util.h>
 #include <mpu.h>
 #include <heap.h>
@@ -49,6 +50,7 @@
 #include <cpu.h>
 #include <cpu/cpuMath.h>
 #include <algos/ap_hub_sync.h>
+#include <sensors_priv.h>
 
 #include <chre.h>
 
@@ -56,9 +58,13 @@
         { .reason = _reason, .fastHandler = _fastHandler, .handler = _handler, \
           .minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) }
 
-#define NANOHUB_HAL_COMMAND(_msg, _handler) \
+#define NANOHUB_HAL_LEGACY_COMMAND(_msg, _handler) \
         { .msg = _msg, .handler = _handler }
 
+#define NANOHUB_HAL_COMMAND(_msg, _handler, _minReqType, _maxReqType) \
+        { .msg = _msg, .handler = _handler, \
+          .minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) }
+
 // 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
@@ -91,6 +97,7 @@ struct DownloadState
 
 static struct DownloadState *mDownloadState;
 static AppSecErr mAppSecStatus;
+static struct AppHdr *mApp;
 static struct SlabAllocator *mEventSlab;
 static struct HostIntfDataBuffer mTxCurr, mTxNext;
 static uint8_t mTxCurrLength, mTxNextLength;
@@ -263,7 +270,7 @@ static void freeDownloadState()
     mDownloadState = NULL;
 }
 
-static void resetDownloadState(bool initial)
+static bool resetDownloadState(bool initial, bool erase)
 {
     bool doCreate = true;
 
@@ -280,14 +287,19 @@ static void resetDownloadState(bool initial)
         else
             doCreate = false;
     }
+    mDownloadState->dstOffset = 0;
     if (doCreate)
         mDownloadState->start = osAppSegmentCreate(mDownloadState->size);
-    if (!mDownloadState->start)
-        mDownloadState->erase = true;
-    mDownloadState->dstOffset = 0;
+    if (!mDownloadState->start) {
+        if (erase)
+            mDownloadState->erase = true;
+        else
+            return false;
+    }
+    return true;
 }
 
-static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req)
+static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req, bool erase)
 {
     if (!mDownloadState) {
         mDownloadState = heapAlloc(sizeof(struct DownloadState));
@@ -301,9 +313,7 @@ static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req)
     mDownloadState->size = le32toh(req->size);
     mDownloadState->crc = le32toh(req->crc);
     mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
-    resetDownloadState(true);
-
-    return true;
+    return resetDownloadState(true, erase);
 }
 
 static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
@@ -311,7 +321,7 @@ static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t
     struct NanohubStartFirmwareUploadRequest *req = rx;
     struct NanohubStartFirmwareUploadResponse *resp = tx;
 
-    resp->accepted = doStartFirmwareUpload(req);
+    resp->accepted = doStartFirmwareUpload(req, true);
 
     return sizeof(*resp);
 }
@@ -444,15 +454,18 @@ static uint32_t firmwareFinish(bool valid)
     if (!osAppSegmentClose(app, mDownloadState->dstOffset, segState)) {
         osLog(LOG_INFO, "%s: Failed to close segment\n", __func__);
         valid = false;
+        mApp = NULL;
     } else {
         segState = osAppSegmentGetState(app);
+        mApp = app;
         valid = (segState == SEG_ST_VALID);
     }
     osLog(LOG_INFO, "Loaded %s image type %" PRIu8 ": %" PRIu32
-                    " bytes @ %p; state=%02" PRIX32 "\n",
+                    " bytes @ %p; state=%02" PRIX32 "; crc=%08" PRIX32 "\n",
                     valid ? "valid" : "invalid",
                     app->hdr.payInfoType, mDownloadState->size,
-                    mDownloadState->start, segState);
+                    mDownloadState->start, segState,
+                    mApp ? osAppSegmentGetCrc(mApp) : 0xFFFFFFFF);
 
     freeDownloadState(); // no more access to mDownloadState
 
@@ -502,11 +515,30 @@ static void firmwareErase(void *cookie)
     mDownloadState->eraseScheduled = false;
 }
 
+SET_PACKED_STRUCT_MODE_ON
+struct FirmwareWriteCookie
+{
+    uint32_t evtType;
+    union {
+#ifdef LEGACY_HAL_ENABLED
+        struct NanohubHalLegacyContUploadTx respLegacy;
+#endif
+        struct NanohubHalContUploadTx resp;
+    };
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+static void writeCookieFree(void *ptr)
+{
+    struct FirmwareWriteCookie *buf = container_of(ptr, struct FirmwareWriteCookie, resp);
+    heapFree(buf);
+}
+
 static void firmwareWrite(void *cookie)
 {
     bool valid;
     bool finished = false;
-    struct NanohubHalContUploadTx *resp = cookie;
+    struct FirmwareWriteCookie *resp = cookie;
     // only check crc when cookie is NULL (write came from kernel, not HAL)
     bool checkCrc = !cookie;
 
@@ -545,8 +577,15 @@ static void firmwareWrite(void *cookie)
             valid = false;
     }
     if (resp) {
-        resp->success = valid;
-        osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+        if (resp->evtType == EVT_APP_TO_HOST) {
+#ifdef LEGACY_HAL_ENABLED
+            resp->respLegacy.success = valid;
+            osEnqueueEvtOrFree(EVT_APP_TO_HOST, &resp->respLegacy, writeCookieFree);
+#endif
+        } else {
+            resp->resp.ret.status = !valid;
+            osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &resp->resp, writeCookieFree);
+        }
     }
 }
 
@@ -575,10 +614,10 @@ static uint32_t doFirmwareChunk(uint8_t *data, uint32_t offset, uint32_t len, vo
             firmwareFinish(false);
         } else if (offset != mDownloadState->srcOffset) {
             reply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART;
-            resetDownloadState(false);
+            resetDownloadState(false, true);
         } else {
             if (!cookie)
-                mDownloadState->srcCrc = crc32(data, len, mDownloadState->srcCrc);
+                mDownloadState->srcCrc = soft_crc32(data, len, mDownloadState->srcCrc);
             mDownloadState->srcOffset += len;
             memcpy(mDownloadState->data, data, len);
             mDownloadState->lenLeft = mDownloadState->len = len;
@@ -602,12 +641,24 @@ static uint32_t firmwareChunk(void *rx, uint8_t rx_len, void *tx, uint64_t times
     return sizeof(*resp);
 }
 
-static uint32_t doFinishFirmwareUpload()
+static uint32_t doFinishFirmwareUpload(uint32_t *addr, uint32_t *crc)
 {
     uint32_t reply;
 
     if (!mDownloadState) {
         reply = appSecErrToNanohubReply(mAppSecStatus);
+        if (addr) {
+            if (mApp)
+                *addr = (uint32_t)mApp;
+            else
+                *addr = 0xFFFFFFFF;
+        }
+        if (crc) {
+            if (mApp)
+                *crc = osAppSegmentGetCrc(mApp);
+            else
+                *crc = 0xFFFFFFFF;
+        }
     } else if (mDownloadState->srcOffset == mDownloadState->size) {
         reply = NANOHUB_FIRMWARE_UPLOAD_PROCESSING;
     } else {
@@ -620,7 +671,7 @@ static uint32_t doFinishFirmwareUpload()
 static uint32_t finishFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
 {
     struct NanohubFinishFirmwareUploadResponse *resp = tx;
-    resp->uploadReply = doFinishFirmwareUpload();
+    resp->uploadReply = doFinishFirmwareUpload(NULL, NULL);
     if (resp->uploadReply != NANOHUB_FIRMWARE_UPLOAD_PROCESSING)
         osLog(LOG_INFO, "%s: reply=%" PRIu8 "\n", __func__, resp->uploadReply);
     return sizeof(*resp);
@@ -666,26 +717,12 @@ static uint32_t unmaskInterrupt(void *rx, uint8_t rx_len, void *tx, uint64_t tim
     return sizeof(*resp);
 }
 
-static void nanohubDelayStartApps(void *cookie)
-{
-    uint32_t status = 0;
-    status = osExtAppStartAppsDelayed();
-    osLog(LOG_DEBUG, "Started delayed apps; EXT status: %08" PRIX32 "\n", status);
-}
-
 static void addDelta(struct ApHubSync *sync, uint64_t apTime, uint64_t hubTime)
 {
-    static bool delayStart = false;
-
 #if DEBUG_APHUB_TIME_SYNC
     syncDebugAdd(apTime, hubTime);
 #endif
     apHubSyncAddDelta(sync, apTime, hubTime);
-
-    if (!delayStart) {
-        delayStart = true;
-        osDefer(nanohubDelayStartApps, NULL, false);
-    }
 }
 
 static int64_t getAvgDelta(struct ApHubSync *sync)
@@ -951,7 +988,7 @@ static uint32_t writeEvent(void *rx, uint8_t rx_len, void *tx, uint64_t timestam
         if (rx_len >= sizeof(struct HostMsgHdrChre) &&
             rx_len == sizeof(struct HostMsgHdrChre) + hostPacket->len &&
             osTidById(&hostPacket->appId, &tid)) {
-            if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) {
+            if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) {
                 struct NanohubMsgChreHdr hdr = {
                     .size = hostPacket->len,
                     .endpoint = hostPacket->endpoint,
@@ -976,7 +1013,7 @@ static uint32_t writeEvent(void *rx, uint8_t rx_len, void *tx, uint64_t timestam
         } else if (rx_len >= sizeof(struct HostMsgHdrChreV10) &&
                    rx_len == sizeof(struct HostMsgHdrChreV10) + hostPacketV10->len &&
                    osTidById(&hostPacketV10->appId, &tid)) {
-            if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) {
+            if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) {
                 struct NanohubMsgChreHdr hdr = {
                     .size = hostPacketV10->len,
                     .endpoint = CHRE_HOST_ENDPOINT_UNSPECIFIED,
@@ -1044,7 +1081,7 @@ const static struct NanohubCommand mBuiltinCommands[] = {
     NANOHUB_COMMAND(NANOHUB_REASON_GET_INTERRUPT,
                     getInterrupt,
                     getInterrupt,
-                    0,
+                    struct { },
                     struct NanohubGetInterruptRequest),
     NANOHUB_COMMAND(NANOHUB_REASON_MASK_INTERRUPT,
                     maskInterrupt,
@@ -1080,13 +1117,15 @@ const struct NanohubCommand *nanohubFindCommand(uint32_t packetReason)
     return NULL;
 }
 
-static void halSendMgmtResponse(uint32_t cmd, uint32_t status)
+#ifdef LEGACY_HAL_ENABLED
+
+static void halSendLegacyMgmtResponse(uint32_t cmd, uint32_t status)
 {
-    struct NanohubHalMgmtTx *resp;
+    struct NanohubHalLegacyMgmtTx *resp;
 
     resp = heapAlloc(sizeof(*resp));
     if (resp) {
-        resp->hdr = (struct NanohubHalHdr) {
+        resp->hdr = (struct NanohubHalLegacyHdr) {
             .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
             .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
             .msg = cmd,
@@ -1096,36 +1135,36 @@ static void halSendMgmtResponse(uint32_t cmd, uint32_t status)
     }
 }
 
-static void halExtAppsOn(void *rx, uint8_t rx_len)
+static void halLegacyExtAppsOn(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalMgmtRx *req = rx;
+    struct NanohubHalLegacyMgmtRx *req = rx;
 
-    halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+    halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId))));
 }
 
-static void halExtAppsOff(void *rx, uint8_t rx_len)
+static void halLegacyExtAppsOff(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalMgmtRx *req = rx;
+    struct NanohubHalLegacyMgmtRx *req = rx;
 
-    halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+    halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))));
 }
 
-static void halExtAppDelete(void *rx, uint8_t rx_len)
+static void halLegacyExtAppDelete(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalMgmtRx *req = rx;
+    struct NanohubHalLegacyMgmtRx *req = rx;
 
-    halSendMgmtResponse(NANOHUB_HAL_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+    halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId))));
 }
 
-static void halQueryMemInfo(void *rx, uint8_t rx_len)
+static void halLegacyQueryMemInfo(void *rx, uint8_t rx_len)
 {
 }
 
-static void halQueryApps(void *rx, uint8_t rx_len)
+static void halLegacyQueryApps(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalQueryAppsRx *req = rx;
-    struct NanohubHalQueryAppsTx *resp;
-    struct NanohubHalHdr *hdr;
+    struct NanohubHalLegacyQueryAppsRx *req = rx;
+    struct NanohubHalLegacyQueryAppsTx *resp;
+    struct NanohubHalLegacyHdr *hdr;
     uint64_t appId;
     uint32_t appVer, appSize;
 
@@ -1133,8 +1172,8 @@ static void halQueryApps(void *rx, uint8_t rx_len)
         resp = heapAlloc(sizeof(*resp));
         if (resp) {
             resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-            resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
-            resp->hdr.msg = NANOHUB_HAL_QUERY_APPS;
+            resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+            resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_APPS;
             resp->appId = appId;
             resp->version = appVer;
             resp->flashUse = appSize;
@@ -1146,16 +1185,16 @@ static void halQueryApps(void *rx, uint8_t rx_len)
         if (hdr) {
             hdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
             hdr->len = 1;
-            hdr->msg = NANOHUB_HAL_QUERY_APPS;
+            hdr->msg = NANOHUB_HAL_LEGACY_QUERY_APPS;
             osEnqueueEvtOrFree(EVT_APP_TO_HOST, hdr, heapFree);
         }
     }
 }
 
-static void halQueryRsaKeys(void *rx, uint8_t rx_len)
+static void halLegacyQueryRsaKeys(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalQueryRsaKeysRx *req = rx;
-    struct NanohubHalQueryRsaKeysTx *resp;
+    struct NanohubHalLegacyQueryRsaKeysRx *req = rx;
+    struct NanohubHalLegacyQueryRsaKeysTx *resp;
     int len = 0;
     const uint32_t *ptr;
     uint32_t numKeys;
@@ -1172,75 +1211,77 @@ static void halQueryRsaKeys(void *rx, uint8_t rx_len)
     }
 
     resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1 + len;
-    resp->hdr.msg = NANOHUB_HAL_QUERY_RSA_KEYS;
+    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1 + len;
+    resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS;
 
     osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
 }
 
-static void halStartUpload(void *rx, uint8_t rx_len)
+static void halLegacyStartUpload(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalStartUploadRx *req = rx;
+    struct NanohubHalLegacyStartUploadRx *req = rx;
     struct NanohubStartFirmwareUploadRequest hwReq = {
-        .size= req->length
+        .size = req->length
     };
-    struct NanohubHalStartUploadTx *resp;
+    struct NanohubHalLegacyStartUploadTx *resp;
 
     if (!(resp = heapAlloc(sizeof(*resp))))
         return;
 
     resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
-    resp->hdr.msg = NANOHUB_HAL_START_UPLOAD;
-    resp->success = doStartFirmwareUpload(&hwReq);
+    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+    resp->hdr.msg = NANOHUB_HAL_LEGACY_START_UPLOAD;
+    resp->success = doStartFirmwareUpload(&hwReq, true);
 
     osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
 }
 
-static void halContUpload(void *rx, uint8_t rx_len)
+static void halLegacyContUpload(void *rx, uint8_t rx_len)
 {
     uint32_t offset;
     uint32_t reply;
     uint8_t len;
-    struct NanohubHalContUploadRx *req = rx;
-    struct NanohubHalContUploadTx *resp;
+    struct NanohubHalLegacyContUploadRx *req = rx;
+    struct FirmwareWriteCookie *cookie;
 
-    if (!(resp = heapAlloc(sizeof(*resp))))
+    if (!(cookie = heapAlloc(sizeof(*cookie))))
         return;
 
-    resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
-    resp->hdr.msg = NANOHUB_HAL_CONT_UPLOAD;
+    cookie->evtType = EVT_APP_TO_HOST;
+    cookie->respLegacy.hdr = (struct NanohubHalLegacyHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(cookie->respLegacy) - sizeof(struct NanohubHalLegacyHdr) + 1,
+        .msg = NANOHUB_HAL_LEGACY_CONT_UPLOAD,
+    };
+    cookie->respLegacy.success = false;
 
     if (!mDownloadState) {
         reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
     } else {
         offset = le32toh(req->offset);
         len = rx_len - sizeof(req->offset);
-        reply = doFirmwareChunk(req->data, offset, len, resp);
+        reply = doFirmwareChunk(req->data, offset, len, cookie);
     }
     if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
         osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply);
 
-        resp->success = false;
-
-        osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST, &cookie->respLegacy, writeCookieFree);
     }
 }
 
-static void halFinishUpload(void *rx, uint8_t rx_len)
+static void halLegacyFinishUpload(void *rx, uint8_t rx_len)
 {
-    struct NanohubHalFinishUploadTx *resp;
+    struct NanohubHalLegacyFinishUploadTx *resp;
     uint32_t reply;
 
     if (!(resp = heapAlloc(sizeof(*resp))))
         return;
 
     resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
-    resp->hdr.msg = NANOHUB_HAL_FINISH_UPLOAD;
+    resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+    resp->hdr.msg = NANOHUB_HAL_LEGACY_FINISH_UPLOAD;
 
-    reply = doFinishFirmwareUpload();
+    reply = doFinishFirmwareUpload(NULL, NULL);
 
     osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply);
 
@@ -1249,32 +1290,598 @@ static void halFinishUpload(void *rx, uint8_t rx_len)
     osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
 }
 
-static void halReboot(void *rx, uint8_t rx_len)
+static void halLegacyReboot(void *rx, uint8_t rx_len)
 {
     BL.blReboot();
 }
 
+const static struct NanohubHalLegacyCommand mBuiltinHalLegacyCommands[] = {
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_ON,
+                            halLegacyExtAppsOn),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_OFF,
+                            halLegacyExtAppsOff),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APP_DELETE,
+                            halLegacyExtAppDelete),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_MEMINFO,
+                            halLegacyQueryMemInfo),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_APPS,
+                            halLegacyQueryApps),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS,
+                            halLegacyQueryRsaKeys),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_START_UPLOAD,
+                            halLegacyStartUpload),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_CONT_UPLOAD,
+                            halLegacyContUpload),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_FINISH_UPLOAD,
+                            halLegacyFinishUpload),
+    NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_REBOOT,
+                            halLegacyReboot),
+};
+
+const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg)
+{
+    uint32_t i;
+
+    for (i = 0; i < ARRAY_SIZE(mBuiltinHalLegacyCommands); i++) {
+        const struct NanohubHalLegacyCommand *cmd = &mBuiltinHalLegacyCommands[i];
+        if (cmd->msg == msg)
+            return cmd;
+    }
+    return NULL;
+}
+
+#endif /* LEGACY_HAL_ENABLED */
+
+static void halSendAppMgmtResponse(struct NanohubHalAppMgmtRx *req, uint32_t status, struct MgmtStatus stat, uint32_t transactionId)
+{
+    struct NanohubHalAppMgmtTx *resp;
+
+    resp = heapAlloc(sizeof(*resp));
+    if (resp) {
+        resp->hdr = (struct NanohubHalHdr) {
+            .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+            .len = sizeof(*resp) - sizeof(resp->hdr),
+            .transactionId = transactionId,
+        };
+        resp->ret = (struct NanohubHalRet) {
+            .msg = NANOHUB_HAL_APP_MGMT,
+            .status = htole32(status),
+        };
+        resp->cmd = req->cmd;
+        resp->stat = stat;
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+    }
+}
+
+static void halAppMgmt(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalAppMgmtRx *req = rx;
+    struct MgmtStatus stat;
+    uint32_t ret;
+
+    switch (req->cmd) {
+    case NANOHUB_HAL_APP_MGMT_START:
+        stat.value= osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+        ret = stat.op > 0 ? 0 : -1;
+        break;
+    case NANOHUB_HAL_APP_MGMT_STOP:
+        stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+        ret = stat.op > 0 ? 0 : -1;
+        break;
+    case NANOHUB_HAL_APP_MGMT_UNLOAD:
+        stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+        ret = stat.op > 0 ? 0 : -1;
+        break;
+    case NANOHUB_HAL_APP_MGMT_DELETE:
+        stat.value = osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+        ret = stat.erase > 0 ? 0 : -1;
+        break;
+    default:
+        return;
+    }
+
+    halSendAppMgmtResponse(req, ret, stat, transactionId);
+}
+
+static void deferHalSysMgmtErase(void *cookie)
+{
+    struct NanohubHalSysMgmtTx *resp = cookie;
+
+    bool success = osEraseShared();
+
+    if (success)
+        resp->ret.status = htole32(0);
+    else
+        resp->ret.status = htole32(-1);
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halSysMgmt(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalSysMgmtRx *req = rx;
+    struct NanohubHalSysMgmtTx *resp;
+    uint32_t ret = 0;
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr),
+        .transactionId = transactionId,
+    };
+    resp->ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_SYS_MGMT,
+    };
+    resp->cmd = req->cmd;
+
+    switch (req->cmd) {
+    case NANOHUB_HAL_SYS_MGMT_ERASE:
+        ret = osExtAppStopAppsByAppId(APP_ID_ANY);
+        osLog(LOG_INFO, "%s: unloaded apps, ret=%08lx\n", __func__, ret);
+        // delay to make sure all apps are unloaded before erasing
+        if (osDefer(deferHalSysMgmtErase, resp, false) == false) {
+            resp->ret.status = htole32(-1);
+            osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+        }
+        break;
+    case NANOHUB_HAL_SYS_MGMT_REBOOT:
+        BL.blReboot();
+        break;
+    default:
+        resp->ret.status = htole32(-1);
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+    }
+}
+
+static bool copyTLV64(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint64_t val)
+{
+    if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint64_t) > max_len)
+        return false;
+    buf[(*offset)++] = tag;
+    buf[(*offset)++] = sizeof(uint64_t);
+    memcpy(&buf[*offset], &val, sizeof(uint64_t));
+    *offset += sizeof(uint64_t);
+    return true;
+}
+
+static bool copyTLV32(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint32_t val)
+{
+    if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) > max_len)
+        return false;
+    buf[(*offset)++] = tag;
+    buf[(*offset)++] = sizeof(uint32_t);
+    memcpy(&buf[*offset], &val, sizeof(uint32_t));
+    *offset += sizeof(uint32_t);
+    return true;
+}
+
+static bool copyTLV8(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint8_t val)
+{
+    if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t) > max_len)
+        return false;
+    buf[(*offset)++] = tag;
+    buf[(*offset)++] = sizeof(uint8_t);
+    memcpy(&buf[*offset], &val, sizeof(uint8_t));
+    *offset += sizeof(uint8_t);
+    return true;
+}
+
+static bool copyTLVEmpty(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag)
+{
+    if (*offset + sizeof(uint8_t) + sizeof(uint8_t) > max_len)
+        return false;
+    buf[(*offset)++] = tag;
+    buf[(*offset)++] = 0;
+    return true;
+}
+
+static int processAppTags(const struct AppHdr *app, uint32_t crc, uint32_t size, uint8_t *data, uint8_t *tags, int cnt, bool req_tid)
+{
+    int i;
+    size_t offset = 0;
+    const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet);
+    bool success = true;
+    uint32_t tid;
+    bool tid_valid = false;
+    struct Task *task;
+
+    if (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) {
+        return 0;
+    }
+
+    if (osTidById(&app->hdr.appId, &tid)) {
+        tid_valid = true;
+        task = osTaskFindByTid(tid);
+        if (task) {
+            if (task->app != app)
+                tid_valid = false;
+        } else
+            tid_valid = false;
+    }
+
+    if (!tid_valid && req_tid)
+        return 0;
+
+    for (i=0; i<cnt && success; i++) {
+        switch(tags[i]) {
+        case NANOHUB_HAL_APP_INFO_APPID:
+            success = copyTLV64(data, &offset, max_len, tags[i], app->hdr.appId);
+            break;
+        case NANOHUB_HAL_APP_INFO_CRC:
+            if (size)
+                success = copyTLV32(data, &offset, max_len, tags[i], crc);
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_TID:
+            if (tid_valid)
+                success = copyTLV32(data, &offset, max_len, tags[i], tid);
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_VERSION:
+            success = copyTLV32(data, &offset, max_len, tags[i], app->hdr.appVer);
+            break;
+        case NANOHUB_HAL_APP_INFO_ADDR:
+            success = copyTLV32(data, &offset, max_len, tags[i], (uint32_t)app);
+            break;
+        case NANOHUB_HAL_APP_INFO_SIZE:
+            if (size)
+                success = copyTLV32(data, &offset, max_len, tags[i], size);
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_HEAP:
+            if (tid_valid)
+                success = copyTLV32(data, &offset, max_len, tags[i], heapGetTaskSize(tid));
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_DATA:
+            success = copyTLV32(data, &offset, max_len, tags[i], app->sect.got_end - app->sect.data_start);
+            break;
+        case NANOHUB_HAL_APP_INFO_BSS:
+            success = copyTLV32(data, &offset, max_len, tags[i], app->sect.bss_end - app->sect.bss_start);
+            break;
+        case NANOHUB_HAL_APP_INFO_CHRE_MAJOR:
+            if (app->hdr.fwFlags & FL_APP_HDR_CHRE)
+                success = copyTLV8(data, &offset, max_len, tags[i],
+                    (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x01 :
+                        app->hdr.chreApiMajor);
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_CHRE_MINOR:
+            if (app->hdr.fwFlags & FL_APP_HDR_CHRE)
+                success = copyTLV8(data, &offset, max_len, tags[i],
+                    (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x00 :
+                        app->hdr.chreApiMinor);
+            else
+                success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+            break;
+        case NANOHUB_HAL_APP_INFO_END:
+        default:
+            success = false;
+            copyTLVEmpty(data, &offset, max_len, NANOHUB_HAL_APP_INFO_END);
+            break;
+        }
+    }
+
+    return offset;
+}
+
+static void halAppInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalAppInfoRx *req = rx;
+    struct NanohubHalAppInfoTx *resp;
+    struct SegmentIterator it;
+    uint32_t state;
+    int ret, i;
+    uint32_t sharedSize, numApps;
+    const struct AppHdr *internal;
+    const uint8_t *shared;
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+        .transactionId = transactionId,
+    };
+    resp->ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_APP_INFO,
+    };
+
+    shared = platGetSharedAreaInfo(&sharedSize);
+    internal = platGetInternalAppList(&numApps);
+
+    if ((le32toh(req->addr) >= (uint32_t)shared && le32toh(req->addr) < (uint32_t)shared + sharedSize) ||
+        (le32toh(req->addr) < (uint32_t)shared &&
+            ((uint32_t)shared < (uint32_t)internal ||
+                (numApps > 0 && le32toh(req->addr) > (uint32_t)(internal+numApps-1))))) {
+        osSegmentIteratorInit(&it);
+        while (osSegmentIteratorNext(&it)) {
+            state = osSegmentGetState(it.seg);
+            switch (state) {
+            case SEG_ST_EMPTY:
+            case SEG_ST_RESERVED:
+                 osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+                 return;
+            case SEG_ST_ERASED:
+            case SEG_ST_VALID:
+                if (le32toh(req->addr) <= (uint32_t)osSegmentGetData(it.seg)) {
+                    ret = processAppTags(osSegmentGetData(it.seg), osSegmentGetCrc(it.seg), osSegmentGetSize(it.seg), resp->data, req->tags, rx_len - 4, state == SEG_ST_ERASED);
+                    if (ret > 0) {
+                        resp->hdr.len += ret;
+                        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+                        return;
+                    }
+                }
+                break;
+            }
+        }
+    } else {
+        for (i = 0; i < numApps; i++, internal++) {
+            if (le32toh(req->addr) <= (uint32_t)internal) {
+                ret = processAppTags(internal, 0, 0, resp->data, req->tags, rx_len - 4, false);
+                if (ret > 0) {
+                    resp->hdr.len += ret;
+                    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+                    return;
+                }
+            }
+        }
+    }
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halSysInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    extern uint8_t __code_start[];
+    extern uint8_t __code_end[];
+    extern uint8_t __text_end[];
+    extern uint8_t __ram_start[];
+    extern uint8_t __ram_end[];
+
+    struct NanohubHalSysInfoRx *req = rx;
+    struct NanohubHalSysInfoTx *resp;
+    int i;
+    size_t offset = 0;
+    const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet);
+    bool success = true;
+    int free, chunks, largest;
+    uint32_t shared_size;
+
+    free = heapGetFreeSize(&chunks, &largest);
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+        .transactionId = transactionId,
+    };
+    resp->ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_SYS_INFO,
+    };
+
+    for (i=0; i<rx_len && success; i++) {
+        switch(req->tags[i]) {
+        case NANOHUB_HAL_SYS_INFO_HEAP_FREE:
+            if (free >= 0)
+                success = copyTLV32(resp->data, &offset, max_len, req->tags[i], free);
+            else
+                success = copyTLVEmpty(resp->data, &offset, max_len, req->tags[i]);
+            break;
+        case NANOHUB_HAL_SYS_INFO_RAM_SIZE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __ram_end - __ram_start);
+            break;
+        case NANOHUB_HAL_SYS_INFO_EEDATA_SIZE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetSize());
+            break;
+        case NANOHUB_HAL_SYS_INFO_EEDATA_FREE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetFree());
+            break;
+        case NANOHUB_HAL_SYS_INFO_CODE_SIZE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __code_start);
+            break;
+        case NANOHUB_HAL_SYS_INFO_CODE_FREE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __text_end);
+            break;
+        case NANOHUB_HAL_SYS_INFO_SHARED_SIZE:
+            platGetSharedAreaInfo(&shared_size);
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], shared_size);
+            break;
+        case NANOHUB_HAL_SYS_INFO_SHARED_FREE:
+            success = copyTLV32(resp->data, &offset, max_len, req->tags[i], osSegmentGetFree());
+            break;
+        case NANOHUB_HAL_SYS_INFO_END:
+        default:
+            success = false;
+            copyTLVEmpty(resp->data, &offset, max_len, NANOHUB_HAL_APP_INFO_END);
+            break;
+        }
+    }
+
+    resp->hdr.len += offset;
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halKeyInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalKeyInfoRx *req = rx;
+    struct NanohubHalKeyInfoTx *resp;
+    const uint32_t *ptr;
+    uint32_t numKeys;
+    uint32_t dataLength;
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    ptr = BL.blGetPubKeysInfo(&numKeys);
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+        .transactionId = transactionId,
+    };
+    resp->ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_KEY_INFO,
+    };
+
+    resp->keyLength = 0;
+
+    if (ptr && req->keyNum < numKeys) {
+        if (req->dataOffset < RSA_BYTES) {
+            resp->keyLength = RSA_BYTES;
+            if (RSA_BYTES - req->dataOffset > NANOHUB_RSA_KEY_CHUNK_LEN)
+                dataLength = NANOHUB_RSA_KEY_CHUNK_LEN;
+            else
+                dataLength = RSA_BYTES - req->dataOffset;
+            memcpy(resp->data, (const uint8_t *)ptr + (req->keyNum * RSA_BYTES) + req->dataOffset, dataLength);
+            resp->hdr.len += dataLength;
+        }
+    }
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halStartUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalStartUploadRx *req = rx;
+    struct NanohubStartFirmwareUploadRequest hwReq = {
+        .size = req->length
+    };
+    struct NanohubHalStartUploadTx *resp;
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr),
+        .transactionId = transactionId,
+    };
+
+    resp->ret.msg = NANOHUB_HAL_START_UPLOAD;
+    if (doStartFirmwareUpload(&hwReq, false))
+        resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+    else
+        resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE;
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halContUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    uint32_t offset;
+    uint32_t reply;
+    uint8_t len;
+    struct NanohubHalContUploadRx *req = rx;
+    struct FirmwareWriteCookie *cookie;
+
+    if (!(cookie = heapAlloc(sizeof(*cookie))))
+        return;
+
+    cookie->evtType = EVT_APP_TO_HOST_CHRE;
+    cookie->resp.hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(cookie->resp) - sizeof(cookie->resp.hdr),
+        .transactionId = transactionId,
+    };
+    cookie->resp.ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_CONT_UPLOAD,
+    };
+
+    if (!mDownloadState) {
+        reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
+    } else {
+        offset = le32toh(req->offset);
+        len = rx_len - sizeof(req->offset);
+        reply = doFirmwareChunk(req->data, offset, len, cookie);
+    }
+    if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
+        osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply);
+
+        cookie->resp.ret.status = reply;
+
+        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &cookie->resp, writeCookieFree);
+    }
+}
+
+static void halFinishUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+    struct NanohubHalFinishUploadTx *resp;
+    uint32_t reply;
+    uint32_t addr = 0xFFFFFFFF;
+    uint32_t crc = 0xFFFFFFFF;
+
+    if (!(resp = heapAlloc(sizeof(*resp))))
+        return;
+
+    resp->hdr = (struct NanohubHalHdr) {
+        .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+        .len = sizeof(*resp) - sizeof(resp->hdr),
+        .transactionId = transactionId,
+    };
+
+    reply = doFinishFirmwareUpload(&addr, &crc);
+
+    osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply);
+
+    resp->ret = (struct NanohubHalRet) {
+        .msg = NANOHUB_HAL_FINISH_UPLOAD,
+        .status = reply,
+    };
+
+    resp->addr = addr;
+    resp->crc = crc;
+
+    osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
 const static struct NanohubHalCommand mBuiltinHalCommands[] = {
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_ON,
-                        halExtAppsOn),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_OFF,
-                        halExtAppsOff),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APP_DELETE,
-                        halExtAppDelete),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_MEMINFO,
-                        halQueryMemInfo),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_APPS,
-                        halQueryApps),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_RSA_KEYS,
-                        halQueryRsaKeys),
+    NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_MGMT,
+                            halAppMgmt,
+                            struct NanohubHalAppMgmtRx,
+                            struct NanohubHalAppMgmtRx),
+    NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_MGMT,
+                            halSysMgmt,
+                            struct NanohubHalSysMgmtRx,
+                            struct NanohubHalSysMgmtRx),
+    NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_INFO,
+                            halAppInfo,
+                            __le32,
+                            struct NanohubHalAppInfoRx),
+    NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_INFO,
+                            halSysInfo,
+                            struct { },
+                            struct NanohubHalSysInfoRx),
+    NANOHUB_HAL_COMMAND(NANOHUB_HAL_KEY_INFO,
+                            halKeyInfo,
+                            struct NanohubHalKeyInfoRx,
+                            struct NanohubHalKeyInfoRx),
     NANOHUB_HAL_COMMAND(NANOHUB_HAL_START_UPLOAD,
-                        halStartUpload),
+                            halStartUpload,
+                            struct NanohubHalStartUploadRx,
+                            struct NanohubHalStartUploadRx),
     NANOHUB_HAL_COMMAND(NANOHUB_HAL_CONT_UPLOAD,
-                        halContUpload),
+                            halContUpload,
+                            __le32,
+                            struct NanohubHalContUploadRx),
     NANOHUB_HAL_COMMAND(NANOHUB_HAL_FINISH_UPLOAD,
-                        halFinishUpload),
-    NANOHUB_HAL_COMMAND(NANOHUB_HAL_REBOOT,
-                        halReboot),
+                            halFinishUpload,
+                            struct { },
+                            struct { }),
 };
 
 const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg)
@@ -1289,6 +1896,7 @@ const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg)
     return NULL;
 }
 
+
 int64_t hostGetTimeDelta(void)
 {
     int64_t delta = getAvgDelta(&mTimeSync);
diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c
index 5ed419b6..e6fc9518 100644
--- a/firmware/os/core/nanohub_chre.c
+++ b/firmware/os/core/nanohub_chre.c
@@ -207,7 +207,7 @@ static bool osChreSendMessageToHost(void *message, uint32_t messageSize,
                            chreMessageFreeFunction *freeCallback)
 {
     bool result = false;
-    struct HostHubRawPacket *hostMsg = NULL;
+    struct HostHubChrePacket *hostMsg = NULL;
 
     if (messageSize > CHRE_MESSAGE_TO_HOST_MAX_SIZE || (messageSize && !message))
         goto out;
@@ -220,8 +220,10 @@ static bool osChreSendMessageToHost(void *message, uint32_t messageSize,
         memcpy(hostMsg+1, message, messageSize);
 
     hostMsg->appId = osChreGetAppId();
-    hostMsg->dataLen = messageSize;
-    result = osEnqueueEvtOrFree(EVT_APP_TO_HOST, hostMsg, heapFree);
+    hostMsg->messageSize = messageSize;
+    hostMsg->messageType = messageType;
+    hostMsg->hostEndpoint = hostEndpoint;
+    result = osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, hostMsg, heapFree);
 
 out:
     if (freeCallback)
@@ -538,6 +540,16 @@ static void osChreEventCfgInfo(uintptr_t *retValP, va_list args)
         osEventsUnsubscribe(2, EVT_APP_STARTED, EVT_APP_STOPPED);
 }
 
+static void osChreEventHostSleep(uintptr_t *retValP, va_list args)
+{
+    // bool enable = va_arg(args, int();
+}
+
+static void osChreEventIsHostAwake(uintptr_t *retValP, va_list args)
+{
+    *retValP = true;
+}
+
 static void osChreDrvGnssGetCap(uintptr_t *retValP, va_list args)
 {
     *retValP = CHRE_GNSS_CAPABILITIES_NONE;
@@ -570,6 +582,12 @@ static void osChreDrvGnssMeasStopAsync(uintptr_t *retValP, va_list args)
     *retValP = false;
 }
 
+static void osChreDrvGnssConfLocMon(uintptr_t *retValP, va_list args)
+{
+    // bool enable = va_args(args, bool);
+    *retValP = false;
+}
+
 static void osChreDrvWifiGetCap(uintptr_t *retValP, va_list args)
 {
     *retValP = CHRE_WIFI_CAPABILITIES_NONE;
@@ -600,6 +618,33 @@ static void osChreDrvWwanGetCallInfoAsync(uintptr_t *retValP, va_list args)
     *retValP = false;
 }
 
+static void osChreDrvAudioGetSrc(uintptr_t *retValP, va_list args)
+{
+    // uint32_t handle = va_args(args, uint32_t);
+    // struct chreAudioSource *audioSource = va_args(args, struct chreAudioSource *);
+    *retValP = false;
+}
+
+static void osChreDrvAudioConfSrc(uintptr_t *retValP, va_list args)
+{
+    // uint32_t handle = va_args(args, uint32_t);
+    // bool enable = va_args(args, int);
+    // uint32_t duration_lo = va_arg(args, uint32_t);
+    // uint32_t duration_hi = va_arg(args, uint32_t);
+    // uint64_t bufferDuration = (((uint64_t)dur_hi) << 32) | dur_lo;
+    // uint32_t interval_lo = va_args(args, uint32_t);
+    // uint32_t interval_hi = va_args(args, uint32_t);
+    // uint64_t deliveryInterval = (((uint64_t)del_hi) << 32) | del_lo;
+    *retValP = false;
+}
+
+static void osChreDrvAudioGetStatus(uintptr_t *retValP, va_list args)
+{
+    // uint32_t handle = va_args(args, uint32_t);
+    // struct chreAudioSourceStatus *status = va_args(args, struct chreAudioSourceStatus *);
+    *retValP = false;
+}
+
 static const struct SyscallTable chreMainApiTable = {
     .numEntries = SYSCALL_CHRE_MAIN_API_LAST,
     .entry = {
@@ -618,7 +663,7 @@ static const struct SyscallTable chreMainApiTable = {
         [SYSCALL_CHRE_MAIN_API_SEND_MSG]                = { .func = osChreApiSendMessageToHost },
         [SYSCALL_CHRE_MAIN_API_SENSOR_FIND_DEFAULT]     = { .func = osChreApiSensorFindDefault },
         [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO_OLD]     = { .func = osChreApiSensorGetInfoOld },
-        [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO]     = { .func = osChreApiSensorGetInfo },
+        [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO]         = { .func = osChreApiSensorGetInfo },
         [SYSCALL_CHRE_MAIN_API_SENSOR_GET_STATUS]       = { .func = osChreApiSensorGetStatus },
         [SYSCALL_CHRE_MAIN_API_SENSOR_CONFIG]           = { .func = osChreApiSensorConfig },
         [SYSCALL_CHRE_MAIN_API_GET_OS_API_VERSION]      = { .func = osChreApiChreApiVersion },
@@ -635,6 +680,8 @@ static const struct SyscallTable chreMainEventTable = {
         [SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID]       = { .func = osChreEventInfoByAppId },
         [SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID]      = { .func = osChreEeventInfoByInstId },
         [SYSCALL_CHRE_MAIN_EVENT_CFG_INFO]             = { .func = osChreEventCfgInfo },
+        [SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP]           = { .func = osChreEventHostSleep },
+        [SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE]        = { .func = osChreEventIsHostAwake },
     },
 };
 
@@ -654,6 +701,7 @@ static const struct SyscallTable chreDrvGnssTable = {
         [SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC]          = { .func = osChreDrvGnssLocStopAsync },
         [SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC]        = { .func = osChreDrvGnssMeasStartAsync },
         [SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC]         = { .func = osChreDrvGnssMeasStopAsync },
+        [SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS]       = { .func = osChreDrvGnssConfLocMon },
     },
 };
 
@@ -674,12 +722,22 @@ static const struct SyscallTable chreDrvWwanTable = {
     },
 };
 
+static const struct SyscallTable chreDrvAudioTable = {
+    .numEntries = SYSCALL_CHRE_DRV_AUDIO_LAST,
+    .entry = {
+        [SYSCALL_CHRE_DRV_AUDIO_GET_SRC]                = { .func = osChreDrvAudioGetSrc },
+        [SYSCALL_CHRE_DRV_AUDIO_CONF_SRC]               = { .func = osChreDrvAudioConfSrc },
+        [SYSCALL_CHRE_DRV_AUDIO_GET_STATUS]             = { .func = osChreDrvAudioGetStatus },
+    },
+};
+
 static const struct SyscallTable chreDriversTable = {
     .numEntries = SYSCALL_CHRE_DRV_LAST,
     .entry = {
         [SYSCALL_CHRE_DRV_GNSS]     = { .subtable = (struct SyscallTable*)&chreDrvGnssTable,     },
         [SYSCALL_CHRE_DRV_WIFI]     = { .subtable = (struct SyscallTable*)&chreDrvWifiTable,     },
         [SYSCALL_CHRE_DRV_WWAN]     = { .subtable = (struct SyscallTable*)&chreDrvWwanTable,     },
+        [SYSCALL_CHRE_DRV_AUDIO]    = { .subtable = (struct SyscallTable*)&chreDrvAudioTable     },
     },
 };
 
diff --git a/firmware/os/core/seos.c b/firmware/os/core/seos.c
index ff8aa7d7..9756964f 100644
--- a/firmware/os/core/seos.c
+++ b/firmware/os/core/seos.c
@@ -300,12 +300,25 @@ static inline bool osTaskInit(struct Task *task)
 
 static void osTaskRelease(struct Task *task)
 {
-    uint32_t task_tid = task->tid;
+    uint32_t taskTid = task->tid;
+    uint32_t platErr, sensorErr;
+    int timErr, heapErr;
+    uint64_t appId;
 
-    platFreeResources(task_tid); // HW resources cleanup (IRQ, DMA etc)
-    sensorFreeAll(task_tid);
-    timTimerCancelAll(task_tid);
-    heapFreeAll(task_tid);
+    if (task->app)
+        appId = task->app->hdr.appId;
+    else
+        appId = 0;
+
+    platErr = platFreeResources(taskTid); // HW resources cleanup (IRQ, DMA etc)
+    sensorErr = sensorFreeAll(taskTid);
+    timErr = timTimerCancelAll(taskTid);
+    heapErr = heapFreeAll(taskTid);
+
+    if (platErr || sensorErr || timErr || heapErr)
+        osLog(LOG_WARN, "released app ID 0x%" PRIx64 "; plat:%08" PRIx32 " sensor:%08" PRIx32 " tim:%d heap:%d; TID %04" PRIX32 "\n", appId, platErr, sensorErr, timErr, heapErr, taskTid);
+    else
+        osLog(LOG_INFO, "released app ID 0x%" PRIx64 "; TID %04" PRIX32 "\n", appId, taskTid);
 }
 
 static inline void osTaskEnd(struct Task *task)
@@ -500,6 +513,24 @@ struct Segment *osSegmentGetEnd()
     return (struct Segment *)(start + size);
 }
 
+uint32_t osSegmentGetFree()
+{
+    struct SegmentIterator it;
+    const struct Segment *storageSeg = NULL;
+
+    osSegmentIteratorInit(&it);
+    while (osSegmentIteratorNext(&it)) {
+        if (osSegmentGetState(it.seg) == SEG_ST_EMPTY) {
+            storageSeg = it.seg;
+            break;
+        }
+    }
+    if (!storageSeg || storageSeg > it.sharedEnd)
+        return 0;
+
+    return (uint8_t *)it.sharedEnd - (uint8_t *)storageSeg;
+}
+
 struct Segment *osGetSegment(const struct AppHdr *app)
 {
     uint32_t size;
@@ -601,13 +632,13 @@ bool osAppSegmentClose(struct AppHdr *app, uint32_t segDataSize, uint32_t segSta
     footerLen = (-fullSize) & 3;
     memset(footer, 0x00, footerLen);
 
-#ifdef SEGMENT_CRC_SUPPORT
-    struct SegmentFooter segFooter {
-        .crc = ~crc32(storageSeg, fullSize, ~0),
+    wdtDisableClk();
+    struct SegmentFooter segFooter = {
+        .crc = ~soft_crc32(storageSeg, fullSize, ~0),
     };
+    wdtEnableClk();
     memcpy(&footer[footerLen], &segFooter, sizeof(segFooter));
     footerLen += sizeof(segFooter);
-#endif
 
     if (ret && footerLen)
         ret = osWriteShared((uint8_t*)storageSeg + fullSize, footer, footerLen);
@@ -660,10 +691,10 @@ static inline bool osAppIsValid(const struct AppHdr *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 &&
+            osAppSegmentCalcCrcResidue(app) == CRC_RESIDUE &&
             !(app->hdr.fwFlags & FL_APP_HDR_INTERNAL);
 }
 
@@ -721,7 +752,7 @@ static bool osStartApp(const struct AppHdr *app)
         // print external NanoApp info to facilitate NanoApp debugging
         if (!(task->app->hdr.fwFlags & FL_APP_HDR_INTERNAL))
             osLog(LOG_INFO,
-                  "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04X\n",
+                  "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04" PRIX16 "\n",
                   task->app->hdr.appId, (uintptr_t) task->app, (uintptr_t) task->platInfo.data, task->tid);
 
         done = osTaskInit(task);
@@ -768,20 +799,20 @@ void osTaskAbort(struct Task *task)
     osStopTask(task, true);
 }
 
-static bool matchDelayStart(const void *cookie, const struct AppHdr *app)
+static bool matchAutoStart(const void *cookie, const struct AppHdr *app)
 {
     bool match = (bool)cookie;
 
     if (app->hdr.fwFlags & FL_APP_HDR_CHRE) {
         if (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF)
-            return !match;
+            return match;
         else if ((app->hdr.chreApiMajor < 0x01) ||
                  (app->hdr.chreApiMajor == 0x01 && app->hdr.chreApiMinor < 0x01))
-            return !match;
-        else
             return match;
+        else
+            return !match;
     } else {
-        return !match;
+        return match;
     }
 }
 
@@ -951,11 +982,6 @@ uint32_t osExtAppStartAppsByAppId(uint64_t appId)
     return osExtAppStartApps(matchAppId, &appId);
 }
 
-uint32_t osExtAppStartAppsDelayed()
-{
-    return osExtAppStartApps(matchDelayStart, (void *)true);
-}
-
 static void osStartTasks(void)
 {
     const struct AppHdr *app;
@@ -1006,7 +1032,7 @@ static void osStartTasks(void)
     }
 
     osLog(LOG_DEBUG, "Starting external apps...\n");
-    status = osExtAppStartApps(matchDelayStart, (void *)false);
+    status = osExtAppStartApps(matchAutoStart, (void *)true);
     osLog(LOG_DEBUG, "Started %" PRIu32 " internal apps; EXT status: %08" PRIX32 "\n", taskCnt, status);
 }
 
@@ -1203,13 +1229,22 @@ static void osDeferredActionFreeF(void* event)
 
 static bool osEventsSubscribeUnsubscribeV(bool sub, uint32_t numEvts, va_list ap)
 {
-    union SeosInternalSlabData *act = slabAllocatorAlloc(mMiscInternalThingsSlab);
+    struct Task *task = osGetCurrentTask();
+    union SeosInternalSlabData *act;
     int i;
 
-    if (!act || numEvts > MAX_EVT_SUB_CNT)
+    if (!sub && osTaskTestFlags(task, FL_TASK_STOPPED)) // stopping, so this is a no-op
+        return true;
+
+    if (numEvts > MAX_EVT_SUB_CNT)
+        return false;
+
+    act = slabAllocatorAlloc(mMiscInternalThingsSlab);
+
+    if (!act)
         return false;
 
-    act->evtSub.tid = osGetCurrentTid();
+    act->evtSub.tid = task->tid;
     act->evtSub.numEvts = numEvts;
     for (i = 0; i < numEvts; i++)
         act->evtSub.evts[i] = va_arg(ap, uint32_t);
@@ -1272,12 +1307,8 @@ static bool osEnqueueEvtCommon(uint32_t evt, void *evtData, TaggedPtr evtFreeInf
 
     osTaskAddIoCount(task, 1);
 
-    if (osTaskTestFlags(task, FL_TASK_STOPPED)) {
-        handleEventFreeing(evtType, evtData, evtFreeInfo);
-        return true;
-    }
-
-    if (!evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) {
+    if (osTaskTestFlags(task, FL_TASK_STOPPED) ||
+        !evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) {
         osTaskAddIoCount(task, -1);
         return false;
     }
@@ -1377,7 +1408,7 @@ bool osEnqueuePrivateEvtAsApp(uint32_t evtType, void *evtData, uint32_t toTid)
     return osEnqueuePrivateEvtEx(evtType & EVT_MASK, evtData, taggedPtrMakeFromUint(osGetCurrentTid()), toTid);
 }
 
-bool osTidById(uint64_t *appId, uint32_t *tid)
+bool osTidById(const uint64_t *appId, uint32_t *tid)
 {
     struct Task *task;
 
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 513a9c8c..3f272d6c 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -16,6 +16,7 @@
 
 #include <algos/time_sync.h>
 #include <atomic.h>
+#include <common/math/macros.h>
 #include <cpu/cpuMath.h>
 #include <errno.h>
 #include <gpio.h>
@@ -40,7 +41,7 @@
 #include <variant/variant.h>
 
 #ifdef MAG_SLAVE_PRESENT
-#include <calibration/magnetometer/mag_cal.h>
+#include <calibration/magnetometer/mag_cal/mag_cal.h>
 #endif
 
 #ifdef ACCEL_CAL_ENABLED
@@ -61,13 +62,8 @@
 
 #ifdef GYRO_CAL_ENABLED
 #include <calibration/gyroscope/gyro_cal.h>
-#include <common/math/macros.h>
 #endif  // GYRO_CAL_ENABLED
 
-#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
-#include <calibration/util/cal_log.h>
-#endif  // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
-
 #ifdef OVERTEMPCAL_ENABLED
 #include <calibration/over_temp/over_temp_cal.h>
 #endif  // OVERTEMPCAL_ENABLED
@@ -108,7 +104,7 @@
 #define DBG_WM_CALC               0
 #define TIMESTAMP_DBG             0
 
-#define BMI160_APP_VERSION 17
+#define BMI160_APP_VERSION 20
 
 // fixme: to list required definitions for a slave mag
 #ifdef USE_BMM150
@@ -259,6 +255,18 @@
 
 #define MAX_NUM_COMMS_EVENT_SAMPLES 15
 
+#ifndef BMI160_ACC_SAMPLES
+#define BMI160_ACC_SAMPLES 3000
+#endif
+
+#ifndef BMI160_GYRO_SAMPLES
+#define BMI160_GYRO_SAMPLES 20
+#endif
+
+#ifndef BMI160_MAG_SAMPLES
+#define BMI160_MAG_SAMPLES 600
+#endif
+
 // Default accel range is 8g
 #ifndef BMI160_ACC_RANGE_G
 #define BMI160_ACC_RANGE_G 8
@@ -749,18 +757,19 @@ static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
 {
 #ifdef ACCEL_CAL_ENABLED
     { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
-            NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc,
-            SENS_TYPE_ACCEL_BIAS) },
+            NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW,
+            1.0/kScale_acc, SENS_TYPE_ACCEL_BIAS) },
 #else
     { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
-            NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
+            NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW,
+            1.0/kScale_acc) },
 #endif
     { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
-            NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
+            NANOHUB_INT_NONWAKEUP, BMI160_GYRO_SAMPLES, SENS_TYPE_GYRO_BIAS) },
 #ifdef MAG_SLAVE_PRESENT
     { DEC_INFO_RATE_RAW_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
-            NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_RAW, 1.0/kScale_mag,
-            SENS_TYPE_MAG_BIAS) },
+            NANOHUB_INT_NONWAKEUP, BMI160_MAG_SAMPLES, SENS_TYPE_MAG_RAW,
+            1.0/kScale_mag, SENS_TYPE_MAG_BIAS) },
 #endif
     { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
             NANOHUB_INT_NONWAKEUP, 100) },
@@ -1201,13 +1210,6 @@ static void configFifo(void)
     int i;
     uint8_t val = 0x12;
     bool any_fifo_enabled_prev = anyFifoEnabled();
-#ifdef ACCEL_CAL_ENABLED
-    struct BMI160Sensor *mSensorAcc;
-    bool accelCalNewBiasAvailable;
-    struct TripleAxisDataPoint *sample;
-    float accelCalBiasX, accelCalBiasY, accelCalBiasZ;
-    bool fallThrough;
-#endif
 
     // if ACC is configed, enable ACC bit in fifo_config reg.
     if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) {
@@ -1215,42 +1217,6 @@ static void configFifo(void)
         mTask.fifo_enabled[ACC] = true;
     } else {
         mTask.fifo_enabled[ACC] = false;
-#ifdef ACCEL_CAL_ENABLED
-        // https://source.android.com/devices/sensors/sensor-types.html
-        // "The bias and scale calibration must only be updated while the sensor is deactivated,
-        // so as to avoid causing jumps in values during streaming."
-        accelCalNewBiasAvailable = accelCalUpdateBias(&mTask.acc, &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
-
-        mSensorAcc = &mTask.sensors[ACC];
-        // notify HAL about new accel bias calibration
-        if (accelCalNewBiasAvailable) {
-            fallThrough = true;
-            if (mSensorAcc->data_evt->samples[0].firstSample.numSamples > 0) {
-                // flush existing samples so the bias appears after them
-                flushData(mSensorAcc,
-                        EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType));
-
-                // try to allocate another data event and break if unsuccessful
-                if (!allocateDataEvt(mSensorAcc, sensorGetTime())) {
-                    fallThrough = false;
-                }
-            }
-
-            if (fallThrough) {
-                mSensorAcc->data_evt->samples[0].firstSample.biasCurrent = true;
-                mSensorAcc->data_evt->samples[0].firstSample.biasPresent = 1;
-                mSensorAcc->data_evt->samples[0].firstSample.biasSample =
-                        mSensorAcc->data_evt->samples[0].firstSample.numSamples;
-                sample = &mSensorAcc->data_evt->samples[mSensorAcc->data_evt->samples[0].firstSample.numSamples++];
-                sample->x = accelCalBiasX;
-                sample->y = accelCalBiasY;
-                sample->z = accelCalBiasZ;
-                flushData(mSensorAcc, sensorGetMyEventType(mSensorInfo[ACC].biasType));
-
-                allocateDataEvt(mSensorAcc, sensorGetTime());
-            }
-        }
-#endif
     }
 
     // if GYR is configed, enable GYR bit in fifo_config reg.
@@ -2092,7 +2058,12 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
                       x, y, z, mTask.tempCelsius);
 
           accelCalBiasRemove(&mTask.acc, &x, &y, &z);
-#endif
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+          // Prints debug data report.
+          accelCalDebPrint(&mTask.acc, mTask.tempCelsius);
+#endif  // ACCEL_CAL_DBG_ENABLED
+#endif  // ACCEL_CAL_ENABLED
 
 #ifdef GYRO_CAL_ENABLED
           // Gyro Cal -- Add accelerometer sample.
@@ -2159,12 +2130,54 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
         return;
     }
 
+#ifdef ACCEL_CAL_ENABLED
+    // https://source.android.com/devices/sensors/sensor-types.html
+    // "The bias and scale calibration must only be updated while the sensor is deactivated,
+    // so as to avoid causing jumps in values during streaming." Note, this is now regulated
+    // by the SensorHAL.
+    if (mSensor->idx == ACC) {
+        float accel_offset[3] = {0.0f, 0.0f, 0.0f};
+        bool accelCalNewBiasAvailable = accelCalUpdateBias(
+            &mTask.acc, &accel_offset[0], &accel_offset[1], &accel_offset[2]);
+        if (accelCalNewBiasAvailable) {
+            if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
+                // Flushes existing samples so the bias appears after them.
+                flushData(mSensor,
+                          EVENT_TYPE_BIT_DISCARDABLE |
+                          sensorGetMyEventType(mSensorInfo[ACC].sensorType));
+
+                // Tries to allocate another data event and breaks if unsuccessful.
+                if (!allocateDataEvt(mSensor, rtc_time)) {
+                    return;
+                }
+            }
+            mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
+            mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
+            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++];
+
+            // Updates the accel offset in HAL.
+            sample->x = accel_offset[0];
+            sample->y = accel_offset[1];
+            sample->z = accel_offset[2];
+
+            flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType));
+            if (!allocateDataEvt(mSensor, rtc_time)) {
+                return;
+            }
+        }
+    }
+#endif  // ACCEL_CAL_ENABLED
+
 #ifdef MAG_SLAVE_PRESENT
     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));
+                      EVENT_TYPE_BIT_DISCARDABLE |
+                      sensorGetMyEventType(mSensorInfo[MAG].sensorType));
             if (!allocateDataEvt(mSensor, rtc_time)) {
                 return;
             }
@@ -2176,9 +2189,13 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
         mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
         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++];
+        sample = &mSensor->data_evt->
+            samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+
+        // Updates the mag offset in HAL.
         magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
-        // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias
+
+        // Bias is non-discardable, if we fail to enqueue, don't clear magBiasPosted.
         if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) {
             mTask.magBiasPosted = true;
         }
@@ -2187,17 +2204,20 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
             return;
         }
     }
-#endif
+#endif  // MAG_SLAVE_PRESENT
+
 #ifdef GYRO_CAL_ENABLED
     if (mSensor->idx == GYR) {
       // GyroCal -- Checks for a new offset estimate update.
       float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
       float gyro_offset_temperature_celsius = 0.0f;
+      uint64_t calibration_time_nanos = 0;
       bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
       if (new_gyrocal_offset_update) {
         // GyroCal -- Gets the GyroCal offset estimate.
         gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1],
-                       &gyro_offset[2], &gyro_offset_temperature_celsius);
+                       &gyro_offset[2], &gyro_offset_temperature_celsius,
+                       &calibration_time_nanos);
 
 #ifdef OVERTEMPCAL_ENABLED
         // OTC-Gyro Cal -- Sends a new GyroCal estimate to the OTC-Gyro.
@@ -2243,17 +2263,6 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
         sample->y = gyro_offset[1];
         sample->z = gyro_offset[2];
 
-#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
-        CAL_DEBUG_LOG("[GYRO_OFFSET:STORED]",
-                      "Offset|Temp|Time: %s%d.%06d, %s%d.%06d, %s%d.%06d | "
-                      "%s%d.%06d | %llu",
-                      CAL_ENCODE_FLOAT(sample->x, 6),
-                      CAL_ENCODE_FLOAT(sample->y, 6),
-                      CAL_ENCODE_FLOAT(sample->z, 6),
-                      CAL_ENCODE_FLOAT(gyro_offset_temperature_celsius, 6),
-                      (unsigned long long int)rtc_time);
-#endif  // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
-
         flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
         if (!allocateDataEvt(mSensor, rtc_time)) {
           return;
@@ -2284,7 +2293,7 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
 
     //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.
+    //TODO: This was added to prevent too 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();
@@ -3116,8 +3125,11 @@ static bool gyrCfgData(void *data, void *cookie)
                 bias->hardwareBias[2] & 0xFF);
 
 #ifdef GYRO_CAL_ENABLED
-        gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
-                       bias->softwareBias[2], sensorGetTime());
+        const float dummy_temperature_celsius = 25.0f;
+        gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0],
+                       bias->softwareBias[1], bias->softwareBias[2],
+                       dummy_temperature_celsius,
+                       sensorGetTime());
 #endif  // GYRO_CAL_ENABLED
         if (!saveCalibration()) {
             T(pending_calibration_save) = true;
@@ -3217,9 +3229,8 @@ static bool magCfgData(void *data, void *cookie)
                 (int)(d->inclination * 180 / M_PI + 0.5f));
 
         // Passing local field information to mag calibration routine
-#ifdef DIVERSITY_CHECK_ENABLED
         diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
-#endif
+
         // TODO: pass local field information to rotation vector sensor.
     } else {
         ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size);
@@ -3333,7 +3344,7 @@ static void processPendingEvt(void)
         }
     }
     if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) {
-        T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData();
+        T(pending_step_cnt) = !stepCntFlushGetData() && T(pending_step_cnt);
         return;
     }
     if (mTask.pending_calibration_save) {
@@ -3833,83 +3844,98 @@ static bool startTask(uint32_t task_id)
     osEventSubscribe(mTask.tid, EVT_APP_START);
 
 #ifdef ACCEL_CAL_ENABLED
-    // Init Accel Cal
-    accelCalInit(&mTask.acc,
-                 800000000, /* Stillness Time in ns (0.8s) */
-                 5,         /* Minimum Sample Number */
-                 0.00025,   /* Threshold */
-                 15,        /* nx bucket count */
-                 15,        /* nxb bucket count */
-                 15,        /* ny bucket count */
-                 15,        /* nyb bucket count */
-                 15,        /* nz bucket count */
-                 15,        /* nzb bucket count */
-                 15);       /* nle bucket count */
-#endif
+    // Initializes the accelerometer offset calibration algorithm.
+    const struct AccelCalParameters accel_cal_parameters = {
+        MSEC_TO_NANOS(800),  // t0
+        5,                   // n_s
+        15,                  // fx
+        15,                  // fxb
+        15,                  // fy
+        15,                  // fyb
+        15,                  // fz
+        15,                  // fzb
+        15,                  // fle
+        0.00025f             // th
+    };
+    accelCalInit(&mTask.acc, &accel_cal_parameters);
+#endif  // ACCEL_CAL_ENABLED
 
 #ifdef GYRO_CAL_ENABLED
-    // Gyro Cal -- Initialization.
-    gyroCalInit(&mTask.gyro_cal,
-                SEC_TO_NANOS(5.0f),   // Min stillness period = 5.0 seconds
-                SEC_TO_NANOS(5.9f),   // Max stillness period = 6.0 seconds (NOTE 1)
-                0, 0, 0,              // Initial bias offset calibration
-                0,                    // Time stamp of initial bias calibration
-                SEC_TO_NANOS(1.5f),   // Analysis window length = 1.5 seconds
-                7.5e-5f,              // Gyroscope variance threshold [rad/sec]^2
-                1.5e-5f,              // Gyroscope confidence delta [rad/sec]^2
-                4.5e-3f,              // Accelerometer variance threshold [m/sec^2]^2
-                9.0e-4f,              // Accelerometer confidence delta [m/sec^2]^2
-                5.0f,                 // Magnetometer variance threshold [uT]^2
-                1.0f,                 // Magnetometer confidence delta [uT]^2
-                0.95f,                // Stillness threshold [0,1]
-                40.0f * MDEG_TO_RAD,  // Stillness mean variation limit [rad/sec]
-                1.5f,                 // Max temperature delta during stillness [C]
-                true);                // Gyro calibration enable
-    // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
-    // period of 6.0 seconds and avoid buffer boundary conditions that could push
-    // the max stillness to the next multiple of the analysis window length
-    // (i.e., 7.5 seconds).
+    // Initializes the gyroscope offset calibration algorithm.
+    const struct GyroCalParameters gyro_cal_parameters = {
+        SEC_TO_NANOS(5),      // min_still_duration_nanos
+        SEC_TO_NANOS(5.9f),   // max_still_duration_nanos [see, NOTE 1]
+        0,                    // calibration_time_nanos
+        SEC_TO_NANOS(1.5f),   // window_time_duration_nanos
+        0,                    // bias_x
+        0,                    // bias_y
+        0,                    // bias_z
+        0.95f,                // stillness_threshold
+        MDEG_TO_RAD * 40.0f,  // stillness_mean_delta_limit [rad/sec]
+        7.5e-5f,              // gyro_var_threshold [rad/sec]^2
+        1.5e-5f,              // gyro_confidence_delta [rad/sec]^2
+        4.5e-3f,              // accel_var_threshold [m/sec^2]^2
+        9.0e-4f,              // accel_confidence_delta [m/sec^2]^2
+        5.0f,                 // mag_var_threshold [uTesla]^2
+        1.0f,                 // mag_confidence_delta [uTesla]^2
+        1.5f,                 // temperature_delta_limit_celsius
+        true                  // gyro_calibration_enable
+    };
+    // [NOTE 1]: 'max_still_duration_nanos' is set to 5.9 seconds to achieve a
+    // max stillness period of 6.0 seconds and avoid buffer boundary conditions
+    // that could push the max stillness to the next multiple of the analysis
+    // window length (i.e., 7.5 seconds).
+    gyroCalInit(&mTask.gyro_cal, &gyro_cal_parameters);
 
 #ifdef OVERTEMPCAL_ENABLED
-    // Initialize over-temp calibration.
-    overTempCalInit(&mTask.over_temp_gyro_cal,
-                    5,                     // Min num of points to enable model update
-                    SEC_TO_NANOS(0.5f),    // Min temperature update interval [nsec]
-                    0.75f,                 // Temperature span of bin method [C]
-                    40.0f * MDEG_TO_RAD,   // Jump tolerance [rad/sec]
-                    50.0f * MDEG_TO_RAD,   // Outlier rejection tolerance [rad/sec]
-                    DAYS_TO_NANOS(2),      // Model data point age limit [nsec]
-                    80.0f * MDEG_TO_RAD,   // Limit for temp. sensitivity [rad/sec/C]
-                    3.0e3f * MDEG_TO_RAD,  // Limit for model intercept parameter [rad/sec]
-                    0.1f * MDEG_TO_RAD,    // Significant offset change [rad/sec]
-                    true);                 // Over-temp compensation enable
+    // Initializes the gyroscope over-temperature offset compensation algorithm.
+    const struct OverTempCalParameters gyro_otc_parameters = {
+        MSEC_TO_NANOS(500),    // min_temp_update_period_nanos
+        DAYS_TO_NANOS(2),      // age_limit_nanos
+        0.75f,                 // delta_temp_per_bin
+        40.0f * MDEG_TO_RAD,   // jump_tolerance
+        50.0f * MDEG_TO_RAD,   // outlier_limit
+        80.0f * MDEG_TO_RAD,   // temp_sensitivity_limit
+        3.0e3f * MDEG_TO_RAD,  // sensor_intercept_limit
+        0.1f * MDEG_TO_RAD,    // significant_offset_change
+        5,                     // min_num_model_pts
+        true                   // over_temp_enable
+    };
+    overTempCalInit(&mTask.over_temp_gyro_cal, &gyro_otc_parameters);
+
 #endif  // OVERTEMPCAL_ENABLED
 #endif  // GYRO_CAL_ENABLED
 
 #ifdef MAG_SLAVE_PRESENT
-#ifdef DIVERSITY_CHECK_ENABLED
-    initMagCal(&mTask.moc,
-               0.0f, 0.0f, 0.0f,   // bias x, y, z
-               1.0f, 0.0f, 0.0f,   // c00, c01, c02
-               0.0f, 1.0f, 0.0f,   // c10, c11, c12
-               0.0f, 0.0f, 1.0f,   // c20, c21, c22
-               3000000,            // min_batch_window_in_micros
-               8,                  // min_num_diverse_vectors
-               1,                  // max_num_max_distance
-               6.0f,               // var_threshold
-               10.0f,              // max_min_threshold
-               48.f,               // local_field
-               0.5f,               // threshold_tuning_param
-               2.552f);            // max_distance_tuning_param
-#else
-    initMagCal(&mTask.moc,
-               0.0f, 0.0f, 0.0f,   // bias x, y, z
-               1.0f, 0.0f, 0.0f,   // c00, c01, c02
-               0.0f, 1.0f, 0.0f,   // c10, c11, c12
-               0.0f, 0.0f, 1.0f,   // c20, c21, c22
-               3000000);           // min_batch_window_in_micros
-#endif
-#endif
+    const struct MagCalParameters mag_cal_parameters = {
+        3000000,  // min_batch_window_in_micros
+        0.0f,     // x_bias
+        0.0f,     // y_bias
+        0.0f,     // z_bias
+        1.0f,     // c00
+        0.0f,     // c01
+        0.0f,     // c02
+        0.0f,     // c10
+        1.0f,     // c11
+        0.0f,     // c12
+        0.0f,     // c20
+        0.0f,     // c21
+        1.0f      // c22
+    };
+
+    // Initializes the magnetometer offset calibration algorithm with diversity
+    // checker.
+    const struct DiversityCheckerParameters mag_diversity_parameters = {
+        6.0f,    // var_threshold
+        10.0f,   // max_min_threshold
+        48.0f,   // local_field
+        0.5f,    // threshold_tuning_param
+        2.552f,  // max_distance_tuning_param
+        8,       // min_num_diverse_vectors
+        1        // max_num_max_distance
+    };
+    initMagCal(&mTask.moc, &mag_cal_parameters, &mag_diversity_parameters);
+#endif  // MAG_SLAVE_PRESENT
 
     slabSize = sizeof(struct TripleAxisDataEvent) +
                MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
diff --git a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
index 39b52063..49509732 100644
--- a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
+++ b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
@@ -32,7 +32,7 @@
 
 #define BMP280_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 5)
 
-#define BMP280_APP_VERSION 3
+#define BMP280_APP_VERSION 4
 
 #ifndef BMP280_I2C_BUS_ID
 #define BMP280_I2C_BUS_ID  0
@@ -44,13 +44,15 @@
 
 #define BOSCH_BMP280_ID                 0x58
 
-#define BOSCH_BMP280_REG_RESET          0x60
+#define BOSCH_BMP280_REG_RESET          0xE0
 #define BOSCH_BMP280_REG_DIG_T1         0x88
 #define BOSCH_BMP280_REG_ID             0xd0
 #define BOSCH_BMP280_REG_CTRL_MEAS      0xf4
 #define BOSCH_BMP280_REG_CONFIG         0xf5
 #define BOSCH_BMP280_REG_PRES_MSB       0xf7
 
+#define BOSCH_BMP280_SOFT_RESET_CMD     0xB6
+
 #define BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS   4
 #define BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE      6
 
@@ -62,22 +64,28 @@
 #define CTRL_ON    ((2 << 5) | (5 << 2) | 3)
 // temp: 2x oversampling, baro: 16x oversampling, power: sleep
 #define CTRL_SLEEP ((2 << 5) | (5 << 2))
+// config: standby time: 62.5ms, IIR filter coefficient: 4
+#define CTRL_CFG   ((1 << 5) | (2 << 2))
 
 enum BMP280SensorEvents
 {
     EVT_SENSOR_I2C = EVT_APP_START + 1,
     EVT_SENSOR_BARO_TIMER,
     EVT_SENSOR_TEMP_TIMER,
+    EVT_SENSOR_SOFTRESET_TIMER,
 };
 
 enum BMP280TaskState
 {
     STATE_RESET,
+    STATE_SOFTRESET,
+    STATE_SOFTRESET_MODE,
     STATE_VERIFY_ID,
     STATE_AWAITING_COMP_PARAMS,
     STATE_CONFIG,
     STATE_FINISH_INIT,
     STATE_IDLE,
+    STATE_ENABLING_BARO_TEMP,
     STATE_ENABLING_BARO,
     STATE_ENABLING_TEMP,
     STATE_DISABLING_BARO,
@@ -114,11 +122,14 @@ static struct BMP280Task
     uint32_t tempHandle;
     uint32_t baroTimerHandle;
     uint32_t tempTimerHandle;
+    uint32_t resetHandle;
 
     float offset;
 
     struct I2cTransfer transfers[BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS];
 
+    bool tmpbaroOn;
+    bool tmptempOn;
     bool baroOn;
     bool tempOn;
     bool baroReading;
@@ -262,6 +273,16 @@ static void tempTimerCallback(uint32_t timerId, void *cookie)
     osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, cookie, NULL, mTask.id);
 }
 
+static void softresetCallback(uint32_t timerId, void *cookie)
+{
+    osEnqueuePrivateEvt(EVT_SENSOR_SOFTRESET_TIMER, cookie, NULL, mTask.id);
+}
+
+static void softreset()
+{
+    writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_SOFTRESET);
+}
+
 static void setMode(bool on, uint8_t state)
 {
     writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, (on) ? CTRL_ON : CTRL_SLEEP, state);
@@ -298,12 +319,37 @@ static bool sensorPowerBaro(bool on, void *cookie)
         mTask.baroReading = false;
     }
 
+    if (!on && mTask.tmpbaroOn && mTask.resetHandle)
+    {
+        if (!mTask.tmptempOn) {
+            timTimerCancel(mTask.resetHandle);
+            mTask.resetHandle = 0;
+        }
+        mTask.tmpbaroOn = 0;
+        sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+    }
+
     if (oldMode != newMode)
-        setMode(newMode, (on ? STATE_ENABLING_BARO : STATE_DISABLING_BARO));
+    {
+        if (newMode == 0)
+        {
+            setMode(newMode, STATE_DISABLING_BARO);
+            mTask.baroOn = false;
+        }
+        else
+        {
+            mTask.tmpbaroOn = true;
+            if (!mTask.tmptempOn) {
+                // do soft reset first when newMode is on
+                softreset();
+            }
+        }
+    }
     else
+    {
         sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
-
-    mTask.baroOn = on;
+        mTask.baroOn = on;
+    }
 
     return true;
 }
@@ -362,12 +408,38 @@ static bool sensorPowerTemp(bool on, void *cookie)
         mTask.tempReading = false;
     }
 
+    if (!on && mTask.tmptempOn && mTask.resetHandle)
+    {
+        if(!mTask.tmpbaroOn) {
+            timTimerCancel(mTask.resetHandle);
+            mTask.resetHandle = 0;
+        }
+        mTask.tmptempOn = 0;
+        sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+    }
+
     if (oldMode != newMode)
-        setMode(newMode, (on ? STATE_ENABLING_TEMP : STATE_DISABLING_TEMP));
+    {
+        if (newMode == 0)
+        {
+            setMode(newMode, STATE_DISABLING_TEMP);
+            mTask.tempOn = false;
+        }
+        else
+        {
+            mTask.tmptempOn = true;
+            if (!mTask.tmpbaroOn) {
+                // do soft reset first when newMode is on
+                softreset();
+            }
+        }
+    }
     else
+    {
         sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+        mTask.tempOn = on;
+    }
 
-    mTask.tempOn = on;
 
     return true;
 }
@@ -495,6 +567,31 @@ static void handleI2cEvent(struct I2cTransfer *xfer)
             break;
         }
 
+        case STATE_SOFTRESET: {
+            //create timer for 2ms delay
+            mTask.resetHandle = timTimerSet(2000000ull, 0, 50, softresetCallback, NULL, true);
+            break;
+        }
+
+        case STATE_SOFTRESET_MODE: {
+            if (mTask.tmpbaroOn && mTask.tmptempOn) {
+                setMode(true,STATE_ENABLING_BARO_TEMP);
+                mTask.tmpbaroOn = false;
+                mTask.baroOn = true;
+                mTask.tmptempOn = false;
+                mTask.tempOn = true;
+            } else if (mTask.tmpbaroOn) {
+                setMode(true,STATE_ENABLING_BARO);
+                mTask.tmpbaroOn = false;
+                mTask.baroOn = true;
+            } else if (mTask.tmptempOn) {
+                setMode(true,STATE_ENABLING_TEMP);
+                mTask.tmptempOn = false;
+                mTask.tempOn = true;
+            }
+            break;
+        }
+
         case STATE_VERIFY_ID: {
             /* Check the sensor ID */
             if (xfer->err != 0 || xfer->txrxBuf[0] != BOSCH_BMP280_ID) {
@@ -521,7 +618,13 @@ static void handleI2cEvent(struct I2cTransfer *xfer)
 
         case STATE_CONFIG: {
             // standby time: 62.5ms, IIR filter coefficient: 4
-            writeRegister(BOSCH_BMP280_REG_CONFIG, (1 << 5) | (2 << 2), STATE_FINISH_INIT);
+            writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_FINISH_INIT);
+            break;
+        }
+
+        case STATE_ENABLING_BARO_TEMP: {
+            sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+            sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
             break;
         }
 
@@ -606,7 +709,7 @@ static void handleEvent(uint32_t evtType, const void* evtData)
             i2cMasterRequest(I2C_BUS_ID, I2C_SPEED);
 
             /* Reset chip */
-            writeRegister(BOSCH_BMP280_REG_RESET, 0xB6, STATE_RESET);
+            writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_RESET);
             break;
         }
 
@@ -649,6 +752,12 @@ static void handleEvent(uint32_t evtType, const void* evtData)
             mTask.tempReading = true;
             break;
         }
+
+        case EVT_SENSOR_SOFTRESET_TIMER:
+        {
+            writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_SOFTRESET_MODE);
+            break;
+        }
     }
 }
 
diff --git a/firmware/os/inc/chreApi.h b/firmware/os/inc/chreApi.h
index e355ed5a..2c157010 100644
--- a/firmware/os/inc/chreApi.h
+++ b/firmware/os/inc/chreApi.h
@@ -74,13 +74,16 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t));
 #define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID       2 // (uint64_t, struct chreNanoappInfo *) -> bool
 #define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID      3 // (uint32_t, struct chreNanoappInfo *) -> bool
 #define SYSCALL_CHRE_MAIN_EVENT_CFG_INFO             4 // (bool) -> void
-#define SYSCALL_CHRE_MAIN_EVENT_LAST                 5 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP           5 // (bool) -> void
+#define SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE        6 // (void) -> bool
+#define SYSCALL_CHRE_MAIN_EVENT_LAST                 7 // always last. holes are allowed, but not immediately before this
 
 //level 2 indices in the CHRE.drivers table
 #define SYSCALL_CHRE_DRV_GNSS             0
 #define SYSCALL_CHRE_DRV_WIFI             1
 #define SYSCALL_CHRE_DRV_WWAN             2
-#define SYSCALL_CHRE_DRV_LAST             3 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_DRV_AUDIO            3
+#define SYSCALL_CHRE_DRV_LAST             4 // always last. holes are allowed, but not immediately before this
 
 //level 3 indices in the CHRE.drivers.gnss table
 #define SYSCALL_CHRE_DRV_GNSS_GET_CAP               0 // (void) -> uint32_t
@@ -88,7 +91,8 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t));
 #define SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC        2 // (const void *) -> bool
 #define SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC      3 // (uint32_t, const void *) -> bool
 #define SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC       4 // (const void *) -> bool
-#define SYSCALL_CHRE_DRV_GNSS_LAST                  5 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS     5 // (bool) -> bool
+#define SYSCALL_CHRE_DRV_GNSS_LAST                  6 // always last. holes are allowed, but not immediately before this
 
 //level 3 indices in the CHRE.drivers.wifi table
 #define SYSCALL_CHRE_DRV_WIFI_GET_CAP               0 // (void) -> uint32_t
@@ -98,9 +102,15 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t));
 
 //level 3 indices in the CHRE.drivers.wwan table
 #define SYSCALL_CHRE_DRV_WWAN_GET_CAP               0 // (void) -> uint32_t
-#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC   1 // (const void *cookie) -> bool
+#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC   1 // (const void *) -> bool
 #define SYSCALL_CHRE_DRV_WWAN_LAST                  2 // always last. holes are allowed, but not immediately before this
 
+//level 3 indicies in the CHRE.drivers.audio table
+#define SYSCALL_CHRE_DRV_AUDIO_GET_SRC              0 // (uint32_t, struct chreAudioSource *) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_CONF_SRC             1 // (uint32_t, bool, uint64_t, uint64_t) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_GET_STATUS           2 // (uint32_t, struct chreAudioSourceStatus *) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_LAST                 3 // always last. holes are allowed, but not immediately before this
+
 //called by os entry point to export the api
 void osChreApiExport(void);
 // release CHRE event and optionally call completion callback
diff --git a/firmware/os/inc/eeData.h b/firmware/os/inc/eeData.h
index 9b313eed..0a6eca6a 100644
--- a/firmware/os/inc/eeData.h
+++ b/firmware/os/inc/eeData.h
@@ -45,6 +45,9 @@
 bool eeDataGet(uint32_t name, void *buf, uint32_t *szP);
 bool eeDataSet(uint32_t name, const void *buf, uint32_t len);
 
+uint32_t eeDataGetSize();
+uint32_t eeDataGetFree();
+
 //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
diff --git a/firmware/os/inc/eventnums.h b/firmware/os/inc/eventnums.h
index 5746433e..cda24f31 100644
--- a/firmware/os/inc/eventnums.h
+++ b/firmware/os/inc/eventnums.h
@@ -32,6 +32,7 @@
 #define EVT_APP_TO_SENSOR_HAL_DATA       0x00000404    //sensor driver out of band data update to sensor hal
 #define EVT_APP_STARTED                  0x00000405    //sent when a app has successfully started
 #define EVT_APP_STOPPED                  0x00000406    //sent when a app has stopped
+#define EVT_APP_TO_HOST_CHRE             0x00000407    //app data to host. Type is struct HostHubChrePacket
 #define EVT_DEBUG_LOG                    0x00007F01    //send message payload to Linux kernel log
 #define EVT_MASK                         0x0000FFFF
 
@@ -55,6 +56,18 @@ struct HostHubRawPacket {
 }ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
+#define HOST_HUB_CHRE_PACKET_MAX_LEN     128
+
+SET_PACKED_STRUCT_MODE_ON
+struct HostHubChrePacket {
+    uint64_t appId;
+    uint8_t messageSize; //not incl this header, 128 bytes max
+    uint32_t messageType;
+    uint16_t hostEndpoint;
+    //raw data in unspecified format here
+}ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
 SET_PACKED_STRUCT_MODE_ON
 struct NanohubMsgChreHdrV10 {
     uint8_t size;
diff --git a/firmware/os/inc/heap.h b/firmware/os/inc/heap.h
index c5bea591..cb1ccb2b 100644
--- a/firmware/os/inc/heap.h
+++ b/firmware/os/inc/heap.h
@@ -24,19 +24,16 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 
-
-
-
 bool heapInit(void);
 void* heapAlloc(uint32_t sz);
 void heapFree(void* ptr);
 int heapFreeAll(uint32_t tid);
-
+int heapGetFreeSize(int *numChunks, int *largestChunk);
+int heapGetTaskSize(uint32_t tid);
 
 #ifdef __cplusplus
 }
 #endif
 
-
 #endif
 
diff --git a/firmware/os/inc/nanohubCommand.h b/firmware/os/inc/nanohubCommand.h
index 408cffdc..e1009804 100644
--- a/firmware/os/inc/nanohubCommand.h
+++ b/firmware/os/inc/nanohubCommand.h
@@ -34,11 +34,20 @@ void nanohubInitCommand(void);
 void nanohubPrefetchTx(uint32_t interrupt, uint32_t wakeup, uint32_t nonwakeup);
 const struct NanohubCommand *nanohubFindCommand(uint32_t packetReason);
 
-struct NanohubHalCommand {
+struct NanohubHalLegacyCommand {
     uint8_t msg;
     void (*handler)(void *, uint8_t);
 };
 
+const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg);
+
+struct NanohubHalCommand {
+    uint8_t msg;
+    void (*handler)(void *, uint8_t, uint32_t);
+    uint8_t minDataLen;
+    uint8_t maxDataLen;
+};
+
 const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg);
 uint64_t hostGetTime(void);
 int64_t hostGetTimeDelta(void);
diff --git a/firmware/os/inc/nanohubPacket.h b/firmware/os/inc/nanohubPacket.h
index ad962ea7..df4af664 100644
--- a/firmware/os/inc/nanohubPacket.h
+++ b/firmware/os/inc/nanohubPacket.h
@@ -166,6 +166,7 @@ enum NanohubFirmwareChunkReply {
     NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART,
     NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL,
     NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY,
+    NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE,
 };
 
 SET_PACKED_STRUCT_MODE_ON
@@ -278,18 +279,6 @@ struct NanohubWriteEventResponse {
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalHdr {
-    uint64_t appId;
-    uint8_t len;
-    uint8_t msg;
-} ATTRIBUTE_PACKED;
-SET_PACKED_STRUCT_MODE_OFF
-
-#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
@@ -310,32 +299,46 @@ struct MgmtStatus {
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
+#ifdef LEGACY_HAL_ENABLED
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyHdr {
+    uint64_t appId;
+    uint8_t len;
+    uint8_t msg;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_LEGACY_EXT_APPS_ON      0
+#define NANOHUB_HAL_LEGACY_EXT_APPS_OFF     1
+#define NANOHUB_HAL_LEGACY_EXT_APP_DELETE   2
+
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalMgmtRx {
+struct NanohubHalLegacyMgmtRx {
     __le64 appId;
     struct MgmtStatus stat;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalMgmtTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyMgmtTx {
+    struct NanohubHalLegacyHdr hdr;
     __le32 status;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_QUERY_MEMINFO   3
-#define NANOHUB_HAL_QUERY_APPS      4
+#define NANOHUB_HAL_LEGACY_QUERY_MEMINFO    3
+#define NANOHUB_HAL_LEGACY_QUERY_APPS       4
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryAppsRx {
+struct NanohubHalLegacyQueryAppsRx {
     __le32 idx;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryAppsTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyQueryAppsTx {
+    struct NanohubHalLegacyHdr hdr;
     __le64 appId;
     __le32 version;
     __le32 flashUse;
@@ -343,69 +346,245 @@ struct NanohubHalQueryAppsTx {
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_QUERY_RSA_KEYS  5
+#define NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS   5
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryRsaKeysRx {
+struct NanohubHalLegacyQueryRsaKeysRx {
     __le32 offset;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryRsaKeysTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyQueryRsaKeysTx {
+    struct NanohubHalLegacyHdr hdr;
     uint8_t data[];
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_START_UPLOAD    6
+#define NANOHUB_HAL_LEGACY_START_UPLOAD     6
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalStartUploadRx {
+struct NanohubHalLegacyStartUploadRx {
     uint8_t isOs;
     __le32 length;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalStartUploadTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyStartUploadTx {
+    struct NanohubHalLegacyHdr hdr;
     uint8_t success;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_CONT_UPLOAD     7
+#define NANOHUB_HAL_LEGACY_CONT_UPLOAD      7
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalContUploadRx {
+struct NanohubHalLegacyContUploadRx {
     __le32 offset;
     uint8_t data[];
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalContUploadTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyContUploadTx {
+    struct NanohubHalLegacyHdr hdr;
     uint8_t success;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_FINISH_UPLOAD   8
+#define NANOHUB_HAL_LEGACY_FINISH_UPLOAD    8
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalFinishUploadTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyFinishUploadTx {
+    struct NanohubHalLegacyHdr hdr;
     uint8_t success;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
-#define NANOHUB_HAL_REBOOT          9
+#define NANOHUB_HAL_LEGACY_REBOOT           9
 
 SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalRebootTx {
-    struct NanohubHalHdr hdr;
+struct NanohubHalLegacyRebootTx {
+    struct NanohubHalLegacyHdr hdr;
     __le32 reason;
 } ATTRIBUTE_PACKED;
 SET_PACKED_STRUCT_MODE_OFF
 
+#endif /* LEGACY_HAL_ENABLED */
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalHdr {
+    __le64 appId;
+    uint8_t len;
+    __le32 transactionId;
+    __le16 unused;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalRet {
+    uint8_t msg;
+    __le32 status;
+} ATTRIBUTE_PACKED;
+
+#define NANOHUB_HAL_APP_MGMT            0x10
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppMgmtRx {
+    __le64 appId;
+    uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_MGMT_START      0
+#define NANOHUB_HAL_APP_MGMT_STOP       1
+#define NANOHUB_HAL_APP_MGMT_UNLOAD     2
+#define NANOHUB_HAL_APP_MGMT_DELETE     3
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppMgmtTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    uint8_t cmd;
+    struct MgmtStatus stat;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_MGMT            0x11
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysMgmtRx {
+    uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_MGMT_ERASE      0
+#define NANOHUB_HAL_SYS_MGMT_REBOOT     1
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysMgmtTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_INFO            0x12
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppInfoRx {
+    __le32 addr;
+    uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(__le32)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_INFO_APPID          0x00
+#define NANOHUB_HAL_APP_INFO_CRC            0x01
+#define NANOHUB_HAL_APP_INFO_TID            0x02
+#define NANOHUB_HAL_APP_INFO_VERSION        0x03
+#define NANOHUB_HAL_APP_INFO_ADDR           0x04
+#define NANOHUB_HAL_APP_INFO_SIZE           0x05
+#define NANOHUB_HAL_APP_INFO_HEAP           0x06
+#define NANOHUB_HAL_APP_INFO_DATA           0x07
+#define NANOHUB_HAL_APP_INFO_BSS            0x08
+#define NANOHUB_HAL_APP_INFO_CHRE_MAJOR     0x09
+#define NANOHUB_HAL_APP_INFO_CHRE_MINOR     0x0A
+#define NANOHUB_HAL_APP_INFO_END            0xFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppInfoTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_INFO            0x13
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysInfoRx {
+    uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_INFO_HEAP_FREE      0x0F
+#define NANOHUB_HAL_SYS_INFO_RAM_SIZE       0x12
+#define NANOHUB_HAL_SYS_INFO_EEDATA_SIZE    0x13
+#define NANOHUB_HAL_SYS_INFO_EEDATA_FREE    0x14
+#define NANOHUB_HAL_SYS_INFO_CODE_SIZE      0x15
+#define NANOHUB_HAL_SYS_INFO_CODE_FREE      0x16
+#define NANOHUB_HAL_SYS_INFO_SHARED_SIZE    0x17
+#define NANOHUB_HAL_SYS_INFO_SHARED_FREE    0x18
+#define NANOHUB_HAL_SYS_INFO_END            0xFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysInfoTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_KEY_INFO            0x14
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalKeyInfoRx {
+    uint32_t keyNum;
+    uint32_t dataOffset;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalKeyInfoTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    uint32_t keyLength;
+    uint8_t data[NANOHUB_RSA_KEY_CHUNK_LEN];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_START_UPLOAD        0x16
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalStartUploadRx {
+    uint8_t isOs;
+    __le32 length;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalStartUploadTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_CONT_UPLOAD         0x17
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalContUploadRx {
+    __le32 offset;
+    uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN-sizeof(__le32)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_ON
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalContUploadTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_FINISH_UPLOAD       0x18
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalFinishUploadTx {
+    struct NanohubHalHdr hdr;
+    struct NanohubHalRet ret;
+    __le32 addr;
+    __le32 crc;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
 #endif /* __NANOHUBPACKET_H */
diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h
index c66d8976..c8a3d481 100644
--- a/firmware/os/inc/seos.h
+++ b/firmware/os/inc/seos.h
@@ -22,6 +22,7 @@ extern "C" {
 #endif
 
 #include <plat/taggedPtr.h>
+#include <plat/wdt.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdarg.h>
@@ -30,12 +31,11 @@ extern "C" {
 #include <plat/app.h>
 #include <eventnums.h>
 #include <variant/variant.h>
+#include <crc.h>
 #include "toolchain.h"
 
 #include <nanohub/nanohub.h>
 
-//#define SEGMENT_CRC_SUPPORT
-
 #ifndef MAX_TASKS
 /* Default to 16 tasks, override may come from variant.h */
 #define MAX_TASKS                        16
@@ -177,7 +177,7 @@ void osRemovePendingEvents(bool (*match)(uint32_t evtType, const void *evtData,
 
 bool osDefer(OsDeferCbkF callback, void *cookie, bool urgent);
 
-bool osTidById(uint64_t *appId, uint32_t *tid);
+bool osTidById(const 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);
 bool osExtAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
@@ -189,8 +189,9 @@ 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();
+uint32_t osSegmentGetFree();
+struct Segment *osGetSegment(const struct AppHdr *app);
 
 static inline int32_t osSegmentGetSize(const struct Segment *seg)
 {
@@ -207,17 +208,19 @@ 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 osSegmentGetCrc(const struct Segment *seg)
+{
+    struct SegmentFooter *footer = (struct SegmentFooter *)(((uint8_t*)seg) +
+        ((osSegmentGetSize(seg) + 3) & ~3) + sizeof(*seg));
+    return footer ? footer->crc : 0xFFFFFFFF;
+}
 
 static inline uint32_t osSegmentSizeAlignedWithFooter(uint32_t size)
 {
@@ -243,6 +246,24 @@ static inline uint32_t osAppSegmentGetState(const struct AppHdr *app)
     return osSegmentGetState(osGetSegment(app));
 }
 
+static inline uint32_t osAppSegmentGetCrc(const struct AppHdr *app)
+{
+    return osSegmentGetCrc(osGetSegment(app));
+}
+
+static inline uint32_t osAppSegmentCalcCrcResidue(const struct AppHdr *app)
+{
+    struct Segment *seg = osGetSegment(app);
+    uint32_t size = osSegmentSizeAlignedWithFooter(osSegmentGetSize(seg));
+    uint32_t crc;
+
+    wdtDisableClk();
+    crc = soft_crc32((uint8_t*)seg, size + sizeof(*seg), ~0);
+    wdtEnableClk();
+
+    return crc;
+}
+
 struct SegmentIterator {
     const struct Segment *shared;
     const struct Segment *sharedEnd;
@@ -272,7 +293,6 @@ void osFreeRetainedEvent(uint32_t evtType, void *evtData, TaggedPtr *evtFreeingI
 uint32_t osExtAppStopAppsByAppId(uint64_t appId);
 uint32_t osExtAppEraseAppsByAppId(uint64_t appId);
 uint32_t osExtAppStartAppsByAppId(uint64_t appId);
-uint32_t osExtAppStartAppsDelayed();
 
 bool osAppIsChre(uint16_t tid);
 uint32_t osAppChreVersion(uint16_t tid);
@@ -293,15 +313,33 @@ void osLog(enum LogLevel level, const char *str, ...) PRINTF_ATTRIBUTE(2, 3);
 #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.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)                                                   \
+    .vec.init        = (uint32_t)(_init),                                               \
+    .vec.end         = (uint32_t)(_end),                                                \
+    .vec.handle      = (uint32_t)(_event)                                               \
+}
+#endif
+
+#ifndef INTERNAL_CHRE_APP_INIT
+#define INTERNAL_CHRE_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 | FL_APP_HDR_CHRE, \
+    .hdr.chreApiMajor = 0x01,                                                           \
+    .hdr.chreApiMinor = 0x02,                                                           \
+    .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
 
diff --git a/firmware/os/platform/stm32/eeData.c b/firmware/os/platform/stm32/eeData.c
index 20e859ef..2710ba07 100644
--- a/firmware/os/platform/stm32/eeData.c
+++ b/firmware/os/platform/stm32/eeData.c
@@ -90,6 +90,34 @@ static void *eeDataGetEx(uint32_t name, uint32_t *offsetP, bool first, void *buf
     return (uint32_t*)data - 1;
 }
 
+uint32_t eeDataGetSize()
+{
+    return __eedata_end - __eedata_start;
+}
+
+uint32_t eeDataGetFree()
+{
+    uint32_t *p = __eedata_start;
+
+    //find the last incarnation of "name" in flash area
+    while (p < __eedata_end) {
+        uint32_t info = *p;
+        uint32_t name = info & EE_DATA_NAME_MAX;
+        uint32_t sz = info / (EE_DATA_NAME_MAX + 1);
+
+        //check for ending condition (name == max)
+        if (name == EE_DATA_NAME_MAX)
+            break;
+
+        p++;
+
+        //skip over to next data chunk header
+        p += (sz + 3) / 4;
+    }
+
+    return __eedata_end - p;
+}
+
 bool eeDataGet(uint32_t name, void *buf, uint32_t *szP)
 {
     uint32_t offset = 0;
diff --git a/lib/Android.mk b/lib/Android.mk
index 0caa1234..88707e17 100644
--- a/lib/Android.mk
+++ b/lib/Android.mk
@@ -20,7 +20,6 @@ src_files := \
     nanohub/aes.c \
     nanohub/rsa.c \
     nanohub/sha2.c \
-    nanohub/softcrc.c \
 
 src_includes := \
     $(LOCAL_PATH)/include \
@@ -36,12 +35,24 @@ LOCAL_EXPORT_C_INCLUDE_DIRS := $(src_includes)
 
 include $(BUILD_NANOHUB_BL_STATIC_LIBRARY)
 
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanohub_common_os
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES := nanohub/softcrc.c
+LOCAL_C_INCLUDES := $(src_includes)
+LOCAL_EXPORT_C_INCLUDE_DIRS := $(src_includes)
+
+include $(BUILD_NANOHUB_OS_STATIC_LIBRARY)
+
 include $(CLEAR_VARS)
 
 LOCAL_MODULE := libnanohub_common
 LOCAL_MODULE_TAGS := optional
 LOCAL_SRC_FILES := \
     $(src_files) \
+    nanohub/softcrc.c \
     nanohub/nanoapp.c \
 
 LOCAL_CFLAGS := \
diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk
index 385a7622..04bc80eb 100644
--- a/sensorhal/Android.mk
+++ b/sensorhal/Android.mk
@@ -72,6 +72,9 @@ LOCAL_SRC_FILES := \
 	sensors.cpp \
 	../../../../$(NANOHUB_SENSORHAL_SENSORLIST)
 
+LOCAL_HEADER_LIBRARIES := \
+    libhardware_headers
+
 LOCAL_SHARED_LIBRARIES := \
 	liblog \
 	libcutils \
@@ -114,6 +117,9 @@ LOCAL_C_INCLUDES += \
 LOCAL_SRC_FILES := \
 	activity.cpp
 
+LOCAL_HEADER_LIBRARIES := \
+    libhardware_headers
+
 LOCAL_SHARED_LIBRARIES := \
 	libcutils \
 	libhubconnection \
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 96297036..6bf35125 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -132,6 +132,8 @@ HubConnection::HubConnection()
     mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
     mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
     mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
+    mAccelEnabledBias[0] = mAccelEnabledBias[1] = mAccelEnabledBias[2] = 0.0f;
+    mAccelEnabledBiasStored = true;
     memset(&mGyroOtcData, 0, sizeof(mGyroOtcData));
 
     mLefty.accel = false;
@@ -238,7 +240,6 @@ HubConnection::HubConnection()
     mSensorState[COMMS_SENSOR_DOUBLE_TAP].sensorType = SENS_TYPE_DOUBLE_TAP;
     mSensorState[COMMS_SENSOR_DOUBLE_TAP].rate = SENSOR_RATE_ONCHANGE;
     mSensorState[COMMS_SENSOR_WRIST_TILT].sensorType = SENS_TYPE_WRIST_TILT;
-    mSensorState[COMMS_SENSOR_WRIST_TILT].rate = SENSOR_RATE_ONCHANGE;
     mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].sensorType = SENS_TYPE_DOUBLE_TOUCH;
     mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].rate = SENSOR_RATE_ONESHOT;
     mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START].sensorType = SENS_TYPE_ACTIVITY_IN_VEHICLE_START;
@@ -733,6 +734,22 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
 
         sendDirectReportEvent(&nev[cnt], 1);
         if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
+            if (!mAccelEnabledBiasStored) {
+                // accel is enabled, but no enabled bias. Store latest bias and use
+                // for accel and uncalibrated accel due to:
+                // https://source.android.com/devices/sensors/sensor-types.html
+                // "The bias and scale calibration must only be updated while the sensor is deactivated,
+                // so as to avoid causing jumps in values during streaming."
+                mAccelEnabledBiasStored = true;
+                mAccelEnabledBias[0] = mAccelBias[0];
+                mAccelEnabledBias[1] = mAccelBias[1];
+                mAccelEnabledBias[2] = mAccelBias[2];
+            }
+            // samples arrive using latest bias
+            // adjust for enabled bias being different from lastest bias
+            sv->x += mAccelBias[0] - mAccelEnabledBias[0];
+            sv->y += mAccelBias[1] - mAccelEnabledBias[1];
+            sv->z += mAccelBias[2] - mAccelEnabledBias[2];
             ++cnt;
         }
 
@@ -742,9 +759,17 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
         ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
         ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
-        ue->x_bias = mAccelBias[0];
-        ue->y_bias = mAccelBias[1];
-        ue->z_bias = mAccelBias[2];
+        if (!mAccelEnabledBiasStored) {
+            // No enabled bias (which means accel is disabled). Use latest bias.
+            ue->x_bias = mAccelBias[0];
+            ue->y_bias = mAccelBias[1];
+            ue->z_bias = mAccelBias[2];
+        } else {
+            // enabled bias is valid, so use it
+            ue->x_bias = mAccelEnabledBias[0];
+            ue->y_bias = mAccelEnabledBias[1];
+            ue->z_bias = mAccelEnabledBias[2];
+        }
 
         sendDirectReportEvent(&nev[cnt], 1);
         if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
@@ -1674,6 +1699,11 @@ void HubConnection::queueActivate(int handle, bool enable)
     Mutex::Autolock autoLock(mLock);
 
     if (isValidHandle(handle)) {
+        // disabling accel, so no longer need to use the bias from when
+        // accel was first enabled
+        if (handle == COMMS_SENSOR_ACCEL && !enable)
+            mAccelEnabledBiasStored = false;
+
         mSensorState[handle].enable = enable;
 
         initConfigCmd(&cmd, handle);
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 558ece34..63bd22d3 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -247,7 +247,8 @@ private:
     uint8_t mMagAccuracy;
     uint8_t mMagAccuracyRestore;
 
-    float mGyroBias[3], mAccelBias[3];
+    float mGyroBias[3], mAccelBias[3], mAccelEnabledBias[3];
+    bool mAccelEnabledBiasStored;
     GyroOtcData mGyroOtcData;
 
     float mScaleAccel, mScaleMag;
diff --git a/util/common/Android.bp b/util/common/Android.bp
new file mode 100644
index 00000000..ac6d65d2
--- /dev/null
+++ b/util/common/Android.bp
@@ -0,0 +1,37 @@
+//
+// Copyright (C) 2018 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.
+cc_library_static {
+    name: "libhubutilcommon",
+    srcs: [
+        "file.cpp",
+        "JSONObject.cpp",
+        "ring.cpp",
+    ],
+    cflags: ["-Wall", "-Werror", "-Wextra"],
+    header_libs: [
+        "libhardware_headers",
+        "libstagefright_foundation_headers",
+        "libstagefright_headers",
+        "libutils_headers",
+    ],
+    export_header_lib_headers: [
+        "libhardware_headers",
+        "libutils_headers",
+    ],
+    export_include_dirs: [
+        ".",
+    ],
+    proprietary: true,
+}
diff --git a/util/common/Android.mk b/util/common/Android.mk
deleted file mode 100644
index bd40f5a5..00000000
--- a/util/common/Android.mk
+++ /dev/null
@@ -1,36 +0,0 @@
-# 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.
-
-LOCAL_PATH := $(call my-dir)
-
-COMMON_CFLAGS := -Wall -Werror -Wextra
-
-include $(CLEAR_VARS)
-
-LOCAL_MODULE := libhubutilcommon
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_CFLAGS += $(COMMON_CFLAGS)
-
-LOCAL_C_INCLUDES += \
-        $(LOCAL_PATH)
-
-LOCAL_SRC_FILES := \
-    file.cpp \
-    JSONObject.cpp \
-    ring.cpp
-
-LOCAL_EXPORT_C_INCLUDE_DIRS := $(LOCAL_C_INCLUDES)
-
-include $(BUILD_STATIC_LIBRARY)
diff --git a/util/common/JSONObject.cpp b/util/common/JSONObject.cpp
index 83ed14e9..8679c719 100644
--- a/util/common/JSONObject.cpp
+++ b/util/common/JSONObject.cpp
@@ -14,10 +14,6 @@
  * limitations under the License.
  */
 
-//#define LOG_NDEBUG 0
-#define LOG_TAG "JSONObject"
-#include <utils/Log.h>
-
 #include "JSONObject.h"
 
 #include <ctype.h>
diff --git a/util/common/file.cpp b/util/common/file.cpp
index 17576016..daf9e40c 100644
--- a/util/common/file.cpp
+++ b/util/common/file.cpp
@@ -14,9 +14,6 @@
  * limitations under the License.
  */
 
-//#define LOG_NDEBUG    0
-#define LOG_TAG "file"
-#include <utils/Log.h>
 #include "file.h"
 
 #include <fcntl.h>
diff --git a/util/common/file.h b/util/common/file.h
index 97d74b00..4bbe613c 100644
--- a/util/common/file.h
+++ b/util/common/file.h
@@ -18,6 +18,8 @@
 
 #define FILE_H_
 
+#include <stdio.h> // for SEEK_SET
+
 #include <media/stagefright/foundation/ABase.h>
 #include <utils/Errors.h>
 
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
index 245f3d03..c5ac53d6 100644
--- a/util/common/ring.cpp
+++ b/util/common/ring.cpp
@@ -14,10 +14,6 @@
  * limitations under the License.
  */
 
-//#define LOG_DEBUG     1
-#define LOG_TAG "ring"
-#include <utils/Log.h>
-
 #include "ring.h"
 
 #include <stdlib.h>
diff --git a/util/nanoapp_cmd/nanoapp_cmd.c b/util/nanoapp_cmd/nanoapp_cmd.c
index 56921819..af193860 100644
--- a/util/nanoapp_cmd/nanoapp_cmd.c
+++ b/util/nanoapp_cmd/nanoapp_cmd.c
@@ -45,7 +45,9 @@
 #define MAX_DOWNLOAD_RETRIES    4
 #define UNINSTALL_CMD           "uninstall"
 
-#define NANOHUB_EXT_APP_DELETE  2
+#define NANOHUB_HAL_EXT_APPS_ON     0
+#define NANOHUB_HAL_EXT_APPS_OFF    1
+#define NANOHUB_HAL_EXT_APP_DELETE  2
 
 #define LOGE(fmt, ...) do { \
         __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, fmt, ##__VA_ARGS__); \
@@ -72,6 +74,13 @@ struct ConfigCmd
     uint8_t data[];
 } __attribute__((packed));
 
+struct HalCmd
+{
+    struct HostMsgHdr hdr;
+    uint8_t cmd;
+    uint64_t appId;
+} __attribute__((packed));
+
 struct App
 {
     uint32_t num;
@@ -238,6 +247,38 @@ struct App *findApp(uint64_t appId)
     return NULL;
 }
 
+int findAppIdByName(char *name, uint64_t *appId)
+{
+    FILE *fp;
+    char *line = NULL;
+    size_t len;
+    ssize_t numRead;
+    int ret = 0;
+
+    fp = openFile("/vendor/firmware/napp_list.cfg", "r");
+    if (!fp)
+        return -1;
+
+    while ((numRead = getline(&line, &len, fp)) != -1) {
+        char entry[MAX_APP_NAME_LEN+1];
+        uint32_t appVersion;
+
+        sscanf(line, "%" STRINGIFY(MAX_APP_NAME_LEN) "s %" PRIx64 " %" PRIx32 "\n", entry, appId, &appVersion);
+
+        if (strncmp(entry, name, MAX_APP_NAME_LEN) == 0) {
+            ret = 1;
+            break;
+        }
+    }
+
+    fclose(fp);
+
+    if (line)
+        free(line);
+
+    return ret;
+}
+
 int parseConfigAppInfo(int *installCnt, int *uninstallCnt)
 {
     FILE *fp;
@@ -309,23 +350,62 @@ void downloadNanohub()
         printf("done\n");
 }
 
+bool sendCmd(uint8_t cmd, uint64_t appId)
+{
+    struct HalCmd msg;
+
+    msg.hdr.eventId = EVT_APP_FROM_HOST;
+    msg.hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
+    msg.hdr.len = sizeof(msg) - sizeof(msg.hdr); // payload length
+    msg.cmd = cmd;
+    memcpy(&msg.appId, &appId, sizeof(uint64_t));
+
+    return fileWriteData("/dev/nanohub", &msg, sizeof(msg));
+}
+
+int halCmd(uint8_t cmd, char *arg)
+{
+    uint64_t appId;
+    char *endptr = arg + strlen(arg);
+
+    if (strcmp(arg, UNINSTALL_CMD) == 0) {
+        printf("%s is not a valid app name\n", arg);
+        return 1;
+    }
+
+    if ((findAppIdByName(arg, &appId) == 1) || (appId = strtoull(arg, &endptr, 16)) > 0) {
+        if (*endptr != '\0') {
+            printf("Couldn't find nanoapp '%s' in napp_list.cfg\n", arg);
+            return 1;
+        } else if (cmd == NANOHUB_HAL_EXT_APPS_ON)
+            printf("Loading ");
+        else if (cmd == NANOHUB_HAL_EXT_APPS_OFF)
+            printf("Unloading ");
+        else if (cmd == NANOHUB_HAL_EXT_APP_DELETE)
+            printf("Deleting ");
+        else {
+            printf("Unrecognized cmd: %d\n", cmd);
+            return 1;
+        }
+        printf("\"0x%016" PRIx64 "\"...", appId);
+        fflush(stdout);
+        if (sendCmd(cmd, appId))
+            printf("done\n");
+        return 0;
+    } else {
+        printf("Couldn't find nanoapp '%s' in napp_list.cfg\n", arg);
+        return 1;
+    }
+}
+
 void removeApps(int updateCnt)
 {
-    uint8_t buffer[sizeof(struct HostMsgHdr) + 1 + sizeof(uint64_t)];
-    struct HostMsgHdr *mHostMsgHdr = (struct HostMsgHdr *)(&buffer[0]);
-    uint8_t *cmd = (uint8_t *)(&buffer[sizeof(struct HostMsgHdr)]);
-    uint64_t *appId = (uint64_t *)(&buffer[sizeof(struct HostMsgHdr) + 1]);
     int i;
 
     for (i = 0; i < updateCnt; i++) {
-        mHostMsgHdr->eventId = EVT_APP_FROM_HOST;
-        mHostMsgHdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
-        mHostMsgHdr->len = 1 + sizeof(uint64_t);
-        *cmd = NANOHUB_EXT_APP_DELETE;
-        memcpy(appId, &appsToUninstall[i], sizeof(uint64_t));
-        printf("Deleting \"%016" PRIx64 "\"...", appsToUninstall[i]);
+        printf("Deleting \"0x%016" PRIx64 "\"...", appsToUninstall[i]);
         fflush(stdout);
-        if (fileWriteData("/dev/nanohub", buffer, sizeof(buffer)))
+        if (sendCmd(NANOHUB_HAL_EXT_APP_DELETE, appsToUninstall[i]))
             printf("done\n");
     }
 }
@@ -372,7 +452,7 @@ int main(int argc, char *argv[])
 
     if (argc < 3 && (argc < 2 || strcmp(argv[1], "download") != 0)) {
         printf("usage: %s <action> <sensor> <data> -d\n", argv[0]);
-        printf("       action: config|cfgdata|calibrate|flush|download\n");
+        printf("       action: config|cfgdata|calibrate|flush\n");
         printf("       sensor: (uncal_)accel|(uncal_)gyro|(uncal_)mag|als|prox|baro|temp|orien\n");
         printf("               gravity|geomag|linear_acc|rotation|game\n");
         printf("               win_orien|tilt|step_det|step_cnt|double_tap\n");
@@ -383,6 +463,9 @@ int main(int argc, char *argv[])
         printf("             calibrate: [N.A.]\n");
         printf("             flush: [N.A.]\n");
         printf("       -d: if specified, %s will keep draining /dev/nanohub until cancelled.\n", argv[0]);
+        printf("usage: %s <cmd> [app]\n", argv[0]);
+        printf("       cmd: download|load|unload|delete\n");
+        printf("       app: appId or name from napp_list.cfg\n");
 
         return 1;
     }
@@ -469,6 +552,27 @@ int main(int argc, char *argv[])
             printf("Unsupported sensor: %s For action: %s\n", argv[2], argv[1]);
             return 1;
         }
+    } else if (strcmp(argv[1], "load") == 0) {
+        if (argc != 3) {
+            printf("Wrong arg number\n");
+            return 1;
+        }
+
+        return halCmd(NANOHUB_HAL_EXT_APPS_ON, argv[2]);
+    } else if (strcmp(argv[1], "unload") == 0) {
+        if (argc != 3) {
+            printf("Wrong arg number\n");
+            return 1;
+        }
+
+        return halCmd(NANOHUB_HAL_EXT_APPS_OFF, argv[2]);
+    } else if (strcmp(argv[1], "delete") == 0) {
+        if (argc != 3) {
+            printf("Wrong arg number\n");
+            return 1;
+        }
+
+        return halCmd(NANOHUB_HAL_EXT_APP_DELETE, argv[2]);
     } else if (strcmp(argv[1], "download") == 0) {
         int installCnt, uninstallCnt;
 
@@ -496,8 +600,9 @@ int main(int argc, char *argv[])
 
         if (parseConfigAppInfo(&installCnt, &uninstallCnt) != 0) {
             LOGE("Failed to download all apps!");
+            return 1;
         }
-        return 1;
+        return 0;
     } else {
         printf("Unsupported action: %s\n", argv[1]);
         return 1;