From eee481c1cbf00a7c775dac0def7194da8aaa1b99 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 18 Sep 2015 15:22:25 -0700 Subject: [PATCH 01/21] Add a .gitignore file for .o and .a's Signed-off-by: Hunyue Yau --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e0292b1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.o +*.a From cbc0c61560fd5386cad03bb4ca11e9076a7b40ea Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Tue, 15 Sep 2015 18:30:23 -0700 Subject: [PATCH 02/21] Bring in additional features (open algs and daemon) - Provide a set of basic algs. Currently, it has only been tested with the daemon. It should be useable on the embedded side. The make system needs to be beefed up to support embedded use. - Add a daemon. This supports different modes of operation/translation. Data can be sourced from: - Input event devices. - IIO devices - Files (testing) Data can be sent to: - Another input device - A file. With suitable mods, this can also be linked to part of a the HAL. Signed-off-by: Hunyue Yau --- algorithm/osp/LICENSE | 202 +++++++ algorithm/osp/Makefile | 12 + algorithm/osp/OSP_interface.c | 59 ++ algorithm/osp/SecondOrderLPF.c | 127 +++++ algorithm/osp/ecompass.c | 150 +++++ algorithm/osp/ecompass.h | 12 + algorithm/osp/fp_atan2.c | 82 +++ algorithm/osp/fp_sensor.h | 65 +++ algorithm/osp/fp_sqrt.c | 98 ++++ algorithm/osp/fp_trig.c | 77 +++ algorithm/osp/fpsup.c | 319 +++++++++++ algorithm/osp/fpsup.h | 100 ++++ algorithm/osp/gravity_lin.c | 54 ++ algorithm/osp/gravity_lin.h | 13 + algorithm/osp/include/osp-alg-types.h | 80 +++ algorithm/osp/lpf.h | 35 ++ algorithm/osp/osp.c | 779 ++++++++++++++++++++++++++ algorithm/osp/osp.h | 16 + algorithm/osp/rotvec.c | 154 +++++ algorithm/osp/rotvec.h | 13 + algorithm/osp/rotvec_precise.c | 110 ++++ algorithm/osp/sigmot.c | 28 + algorithm/osp/sigmot.h | 8 + algorithm/osp/step.c | 99 ++++ algorithm/osp/step.h | 6 + algorithm/osp/tilt.c | 112 ++++ algorithm/osp/tilt.h | 9 + algorithm/osp/trig_sin.c | 331 +++++++++++ include/linux/iio/events.h | 105 ++++ include/linux/iio/types.h | 56 ++ include/osp-api.h | 17 +- include/osp-sensors.h | 17 - linux/daemon/Makefile | 17 + linux/daemon/OSPDaemon.c | 601 ++++++++++++++++++++ linux/daemon/OSPDaemon.h | 239 ++++++++ linux/daemon/OSPDaemon_config.c | 632 +++++++++++++++++++++ linux/daemon/OSPDaemon_driver.c | 125 +++++ linux/daemon/OSPDaemon_driver.h | 26 + linux/daemon/OSPDaemon_filecsv.c | 249 ++++++++ linux/daemon/OSPDaemon_filecsv.h | 5 + linux/daemon/OSPDaemon_iio.c | 710 +++++++++++++++++++++++ linux/daemon/OSPDaemon_iio.h | 43 ++ linux/daemon/OSPDaemon_input.c | 173 ++++++ linux/daemon/OSPDaemon_inputreader.c | 132 +++++ linux/daemon/OSPDaemon_inputreader.h | 8 + linux/daemon/OSPDaemon_pm.c | 112 ++++ linux/daemon/OSPDaemon_pm.h | 6 + linux/daemon/OSPDaemon_queue.c | 58 ++ linux/daemon/OSPDaemon_queue.h | 20 + 49 files changed, 6479 insertions(+), 22 deletions(-) create mode 100644 algorithm/osp/LICENSE create mode 100644 algorithm/osp/Makefile create mode 100644 algorithm/osp/OSP_interface.c create mode 100644 algorithm/osp/SecondOrderLPF.c create mode 100644 algorithm/osp/ecompass.c create mode 100644 algorithm/osp/ecompass.h create mode 100644 algorithm/osp/fp_atan2.c create mode 100644 algorithm/osp/fp_sensor.h create mode 100644 algorithm/osp/fp_sqrt.c create mode 100644 algorithm/osp/fp_trig.c create mode 100644 algorithm/osp/fpsup.c create mode 100644 algorithm/osp/fpsup.h create mode 100644 algorithm/osp/gravity_lin.c create mode 100644 algorithm/osp/gravity_lin.h create mode 100755 algorithm/osp/include/osp-alg-types.h create mode 100644 algorithm/osp/lpf.h create mode 100644 algorithm/osp/osp.c create mode 100644 algorithm/osp/osp.h create mode 100644 algorithm/osp/rotvec.c create mode 100644 algorithm/osp/rotvec.h create mode 100644 algorithm/osp/rotvec_precise.c create mode 100644 algorithm/osp/sigmot.c create mode 100644 algorithm/osp/sigmot.h create mode 100644 algorithm/osp/step.c create mode 100644 algorithm/osp/step.h create mode 100644 algorithm/osp/tilt.c create mode 100644 algorithm/osp/tilt.h create mode 100644 algorithm/osp/trig_sin.c create mode 100644 include/linux/iio/events.h create mode 100644 include/linux/iio/types.h create mode 100644 linux/daemon/Makefile create mode 100644 linux/daemon/OSPDaemon.c create mode 100644 linux/daemon/OSPDaemon.h create mode 100644 linux/daemon/OSPDaemon_config.c create mode 100644 linux/daemon/OSPDaemon_driver.c create mode 100644 linux/daemon/OSPDaemon_driver.h create mode 100644 linux/daemon/OSPDaemon_filecsv.c create mode 100644 linux/daemon/OSPDaemon_filecsv.h create mode 100644 linux/daemon/OSPDaemon_iio.c create mode 100644 linux/daemon/OSPDaemon_iio.h create mode 100644 linux/daemon/OSPDaemon_input.c create mode 100644 linux/daemon/OSPDaemon_inputreader.c create mode 100644 linux/daemon/OSPDaemon_inputreader.h create mode 100644 linux/daemon/OSPDaemon_pm.c create mode 100644 linux/daemon/OSPDaemon_pm.h create mode 100644 linux/daemon/OSPDaemon_queue.c create mode 100644 linux/daemon/OSPDaemon_queue.h diff --git a/algorithm/osp/LICENSE b/algorithm/osp/LICENSE new file mode 100644 index 0000000..e06d208 --- /dev/null +++ b/algorithm/osp/LICENSE @@ -0,0 +1,202 @@ +Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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. + diff --git a/algorithm/osp/Makefile b/algorithm/osp/Makefile new file mode 100644 index 0000000..8895093 --- /dev/null +++ b/algorithm/osp/Makefile @@ -0,0 +1,12 @@ +CC=gcc +CFLAGS=-Wall -g -Iinclude -I../../include -DFEAT_STEP + +OSP_OBJS=SecondOrderLPF.o ecompass.o fp_atan2.o fp_sqrt.o fp_trig.o fpsup.o gravity_lin.o osp.o rotvec.o step.o tilt.o sigmot.o + +all: libOSP.a + +libOSP.a: $(OSP_OBJS) + ar r libOSP.a $(OSP_OBJS) + +clean: + rm -f *.o libOSP.a diff --git a/algorithm/osp/OSP_interface.c b/algorithm/osp/OSP_interface.c new file mode 100644 index 0000000..b7e79ae --- /dev/null +++ b/algorithm/osp/OSP_interface.c @@ -0,0 +1,59 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Interface layer for OSP. Implements a subset of the public + * functions as defined by the OSP API. + */ + +#include + +#include "osp-api.h" + +/* Dummy routines for OSP */ + +OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t* pSystemDesc) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_DoForegroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_DoBackgroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_SetInputData(InputSensorHandle_t h, TriAxisSensorRawData_t *d) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *pSensorDescriptor, InputSensorHandle_t *pReturnedHandle) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_SubscribeSensorResult(ResultDescriptor_t *pResultDescriptor, ResultHandle_t *pResultHandle) +{ + return OSP_STATUS_OK; +} + + +static const OSP_Library_Version_t Version = { + .VersionNumber = 0x99889988, + .VersionString = "OSP Dummy stub library", + .buildTime = __DATE__"@"__TIME__, +}; + +OSP_STATUS_t OSP_GetVersion(const OSP_Library_Version_t **ppVersionStruct) +{ + *ppVersionStruct = &Version; + return OSP_STATUS_OK; +} + diff --git a/algorithm/osp/SecondOrderLPF.c b/algorithm/osp/SecondOrderLPF.c new file mode 100644 index 0000000..813a344 --- /dev/null +++ b/algorithm/osp/SecondOrderLPF.c @@ -0,0 +1,127 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Second order IIR BiCubicFilter. based on + * Implementation from Android AOSP. + */ + +#include +#include "string.h" +#include "fpsup.h" +#include "lpf.h" +#if 0 +struct LPF { + Q15_t iQ; + Q15_t fc; + Q15_t K; + Q15_t iD; + Q15_t a0; + Q15_t a1; + Q15_t b1; + Q15_t b2; + + Q15_t x1; + Q15_t x2; + Q15_t y1; + Q15_t y2; +}; + +struct LPF_CBQF { + struct LPF mA; + struct LPF mB; +}; +#endif + +void LPF_init(struct LPF *lpf, Q15_t Q, Q15_t fc) +{ + lpf->iQ = RECIP_Q15(Q); + lpf->fc = fc; +} + +void LPF_setSamplingPeriod(struct LPF *lpf, Q15_t dT) +{ + + Q15_t tmp, k2; +#if 1 + tmp = MUL_Q15(q15_pi, lpf->fc); + tmp = MUL_Q15(tmp, dT); +#else + tmp = MUL_Q15(q15_pi, q15_c1); +#endif + lpf->K = tan_q15(tmp); + k2 = MUL_Q15(lpf->K, lpf->K); + + lpf->iD = RECIP_Q15(k2 + MUL_Q15(lpf->K, lpf->iQ) + q15_c1); + lpf->a0 = MUL_Q15(k2, lpf->iD); + lpf->a1 = MUL_Q15(q15_c2, lpf->a0); + lpf->b1 = MUL_Q15(q15_c2, lpf->iD); + lpf->b1 = MUL_Q15(k2 - q15_c1, lpf->b1); + lpf->b2 = MUL_Q15(lpf->iD, (k2 - MUL_Q15(lpf->K, lpf->iQ) + q15_c1)); +} + +Q15_t LPF_BQF_init(struct LPF *lpf, Q15_t x) +{ + lpf->x1 = x; + lpf->x2 = x; + + lpf->y1 = x; + lpf->y2 = x; + + return x; +} + +Q15_t LPF_BQF_data(struct LPF *lpf, Q15_t x) +{ + Q15_t y; + + y = MUL_Q15((x+lpf->x2), lpf->a0); + y += MUL_Q15(lpf->x1, lpf->a1); + y -= MUL_Q15(lpf->y1, lpf->b1); + y -= MUL_Q15(lpf->y2, lpf->b2); + + lpf->x2 = lpf->x1; + lpf->y2 = lpf->y1; + lpf->x1 = x; + lpf->y1 = y; + + return y; +} + + +void LPF_CBQF_init(struct LPF_CBQF *clpf, struct LPF *lpf, Q15_t x) +{ + memcpy(&clpf->mA, lpf, sizeof(struct LPF)); + memcpy(&clpf->mB, lpf, sizeof(struct LPF)); + + LPF_BQF_init(&clpf->mA, x); + LPF_BQF_init(&clpf->mB, x); +} + + +Q15_t LPF_CBQF_data(struct LPF_CBQF *clpf, Q15_t x) +{ + return LPF_BQF_data(&clpf->mB, LPF_BQF_data(&clpf->mA, x)); +} + +#if 0 +int main(int argc, char **argv) +{ + struct LPF lpf; + struct LPF_CBQF clpf; + Q15_t d; + + int i; + + LPF_init(&lpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_setSamplingPeriod(&lpf, FP_to_Q15(0.020f)); + LPF_CBQF_init(&clpf, &lpf, FP_to_Q15(9.8f)); + + for (i = 0; i < 200; i++) { + d = LPF_CBQF_data(&clpf, FP_to_Q15(9.8f)); + printf("Data: %f\n", Q15_to_FP(d)); + } +} +#endif diff --git a/algorithm/osp/ecompass.c b/algorithm/osp/ecompass.c new file mode 100644 index 0000000..ec23817 --- /dev/null +++ b/algorithm/osp/ecompass.c @@ -0,0 +1,150 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Project magnetometer data onto the X/Y plane using the + * accel. Angles are computed from that using trig functions. + */ + +#include +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "ecompass.h" + +struct Heading { + float roll; + float pitch; + float yaw; +}; + +static struct ThreeAxis HardIron; +static int calstate; +static int samplecount = 0; +static struct ThreeAxis minMag, maxMag; + +static Q15_t deg_atan2(Q15_t y, Q15_t x) +{ + return MUL_Q15(atan2_q15(y, x), FP_to_Q15(180.0/M_PI)); +} + + +static void adj_mag(struct ThreeAxis *mag) +{ + mag->x -= HardIron.x; + mag->y -= HardIron.y; + mag->z -= HardIron.z; +} + + +static void adj_acc(struct ThreeAxis *acc) +{ +} + +void OSP_ecompass_init(void) +{ + calstate = 0; + HardIron.x = 0; HardIron.y = 0; HardIron.z = 0; +} + +void OSP_ecompass_process(struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Euler *result) +{ + Q15_t pMx, pMy; + Q15_t hyp; + + samplecount++; + + if (calstate) { + /* Assume 30Hz, this is about 30 seconds */ + if (samplecount > 900) { + calstate = 0; + do { + if (maxMag.x == MIN_Q15 || + maxMag.y == MIN_Q15 || + maxMag.z == MIN_Q15) + break; + if (minMag.x == MAX_Q15 || + minMag.y == MAX_Q15 || + minMag.z == MAX_Q15) + break; + + HardIron.x = MUL_Q15((maxMag.x - minMag.x), q15_half); + HardIron.x = HardIron.x - maxMag.x; + + + HardIron.y = MUL_Q15((maxMag.y - minMag.y), q15_half); + HardIron.y = HardIron.y - maxMag.y; + + HardIron.z = MUL_Q15((maxMag.z - minMag.z), q15_half); + HardIron.z = HardIron.z - maxMag.z; + + } while (0); + printf("Cal completed - Offsets: %f %f %f\n", + Q15_to_FP(HardIron.x), + Q15_to_FP(HardIron.y), + Q15_to_FP(HardIron.z)); + } else { + if (mag->x > maxMag.x) maxMag.x = mag->x; + if (mag->y > maxMag.y) maxMag.y = mag->y; + if (mag->z > maxMag.z) maxMag.z = mag->z; + + if (mag->x < minMag.x) minMag.x = mag->x; + if (mag->y < minMag.y) minMag.y = mag->y; + if (mag->z < minMag.z) minMag.z = mag->z; + } + } + + adj_mag(mag); + adj_acc(acc); +#if 0 + acc->y = -acc->y; + acc->x = -acc->x; + acc->z = -acc->z; +#endif + result->roll = deg_atan2(acc->x, acc->z); +#if 0 + result->roll = result->roll+FP_to_Q15(180.0f); + if (result->roll > q15_c180) result->roll -= q15_c360; +#endif + + result->pitch = deg_atan2(acc->y, acc->z); +#if 0 + result->pitch = result->roll + q15_c180; + if (result->pitch > q15_c180) result->pitch -= q15_c360; +#endif + result->pitch = -result->pitch; + hyp = sqrt_q15(MUL_Q15(acc->x, acc->x) + MUL_Q15(acc->z, acc->z)); + if (hyp == 0) return; + + pMx = (MUL_Q15(mag->x, DIV_Q15(acc->z,hyp)) + MUL_Q15(mag->z, DIV_Q15(acc->x,hyp))); + + hyp = sqrt_q15(MUL_Q15(acc->y, acc->y) + MUL_Q15(acc->z, acc->z)); + if (hyp == 0) return; + + pMy = (MUL_Q15(mag->y, DIV_Q15(acc->z,hyp)) + MUL_Q15(mag->z, DIV_Q15(acc->y,hyp))); + + result->yaw = -1 * deg_atan2(pMx, pMy); + result->yaw += q15_c180; +#if 0 + printf("%s roll %f pitch %f yaw %f\n", __func__, Q15_to_FP(result->roll), Q15_to_FP(result->pitch), Q15_to_FP(result->yaw)); +#endif +} + +void OSP_ecompass_cal(void) +{ + calstate = 1; + samplecount = 0; + minMag.x = MAX_Q15; + minMag.y = MAX_Q15; + minMag.z = MAX_Q15; + + maxMag.x = MIN_Q15; + maxMag.y = MIN_Q15; + maxMag.z = MIN_Q15; + + printf("Cal requested\n"); +} diff --git a/algorithm/osp/ecompass.h b/algorithm/osp/ecompass.h new file mode 100644 index 0000000..1a09f31 --- /dev/null +++ b/algorithm/osp/ecompass.h @@ -0,0 +1,12 @@ +#ifndef _ECOMPAAS_H_ +#define _ECOMPASS_H_ 1 +#include "fp_sensor.h" + +/* Start a cal sequence */ +void OSP_ecompass_cal(void); +void OSP_ecompass_process(struct ThreeAxis *, + struct ThreeAxis *, + struct Euler *result); +void OSP_ecompass_init(void); + +#endif diff --git a/algorithm/osp/fp_atan2.c b/algorithm/osp/fp_atan2.c new file mode 100644 index 0000000..6d678aa --- /dev/null +++ b/algorithm/osp/fp_atan2.c @@ -0,0 +1,82 @@ +/* + * Approximate atan2 + */ + +#include +#include +#include +#include "fpsup.h" + +/* + * arctan of y/x + */ + +static const Q15_t q15_pi_4 = FP_to_Q15(M_PI/4.0); + +Q15_t atan2_q15(Q15_t y, Q15_t x) +{ + const Q15_t q15_3_4_PI = FP_to_Q15(3.0*M_PI/4.0f); + Q15_t abs_y, r, rt, angle; + + if (y == 0 && x < 0) return q15_pi; + if (y == 0 && x > 0) return 0; + if (y < 0 && x == 0) return -(q15_pi >> 1); /* -PI/2 */ + if (y > 0 && x == 0) return (q15_pi >> 1); /* PI/2 */ + if (y == 0 && x == 0) return q15_pi; + + abs_y = abs_q15(y); + + if (x >= 0) { + rt = RECIP_Q15(x+abs_y); + r = MUL_Q15((x-abs_y), rt); + + angle = q15_pi_4 - MUL_Q15(q15_pi_4, r); + } else { + rt = RECIP_Q15(abs_y-x); + r = MUL_Q15((x+abs_y), rt); + angle = q15_3_4_PI - MUL_Q15(q15_pi_4, r); + } + + if (y < 0) + return -angle; + else + return angle; +} +#ifdef TEST +int main(int argc, char **argv) +{ + float ang; + float ang_rad; + Q15_t qy, qx, qat; + float fy, fx, fat; + float err = 0.0; + + if (argc > 1) { + err = atof(argv[1]); + if (err > 2.0) + err = 0.0; + } + + fprintf(stderr, "Printing errors greater then %f\n", err); + + for (ang = 0; ang < 360.0f; ang+= 0.5) { + ang_rad = ang * M_PI/180.0f; + qx = q15_c1; + fx = 1.0; + fy = tan(ang_rad); + qy = FP_to_Q15(fy); + + fat = atan2(fy, fx); + qat = atan2_q15(qy, qx); + if ((Q15_to_FP(qat)-fat) > err) { + + fprintf(stderr, + "ANG: %f - tan = %f (%f), atan() = %f (%f) err = %f\n", + ang, fy, Q15_to_FP(qy), + fat, Q15_to_FP(qat), + Q15_to_FP(qat)-fat); + } + } +} + +#endif diff --git a/algorithm/osp/fp_sensor.h b/algorithm/osp/fp_sensor.h new file mode 100644 index 0000000..1b11eaf --- /dev/null +++ b/algorithm/osp/fp_sensor.h @@ -0,0 +1,65 @@ +#ifndef FP_SENSOR_H +#define FP_SENSOR_H 1 + +#include "fpsup.h" +#include "osp-sensors.h" + +/* NTEXTENDED is int32_t Q12 */ +/* NTPRECISE is int32_t Q24 */ +#define Q15_to_NTEXTENDED(x) (x >> (Q15_SHIFT-12)) +#define Q15_to_NTPRECISE(x) (x << (24-Q15_SHIFT)) +#define NTEXTENDED_to_Q15(x) (x << (Q15_SHIFT-12)) +#define NTPRECISE_to_Q15(x) (x >> (24-Q15_SHIFT)) + +#define FLAG(x) (1 << x) + +struct ThreeAxis { + Q15_t x; + Q15_t y; + Q15_t z; +}; + +struct Euler { + Q15_t roll; + Q15_t pitch; + Q15_t yaw; +}; + +struct Quat { + Q15_t x; + Q15_t y; + Q15_t z; + Q15_t w; +}; + +#ifdef FEAT_ROTVEC_PRECISE +struct Quat_precise { + Q24_t x; + Q24_t y; + Q24_t z; + Q24_t w; +}; +#endif + +struct StepInfo { + int32_t count; + int32_t detect; +}; + +struct Results { + union { + struct ThreeAxis result; + struct Euler euler; +#ifdef FEAT_ROTVEC_PRECISE + struct Quat_precise quat; +#else + struct Quat quat; +#endif +#ifdef FEAT_STEP + struct StepInfo step; +#endif + } ResType; + uint32_t time; +}; + +#endif diff --git a/algorithm/osp/fp_sqrt.c b/algorithm/osp/fp_sqrt.c new file mode 100644 index 0000000..86473ed --- /dev/null +++ b/algorithm/osp/fp_sqrt.c @@ -0,0 +1,98 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Math support routines for sqrt. + */ +/* Apache Licensed code from PebbleFit */ +#include +#include "fpsup.h" + +#ifdef ENABLE_Q12 +Q12_t sqrt_q12(Q12_t num) +{ + Q12_t a, p, e = FP_to_Q12(0.001f), b; + Q12_t t; + int nb = 0; + + a = num; + + p = MUL_Q12(a, a);; + while ((p - num >= e) && (nb++ < 40)) { + t = RECIP_Q12(a); + b = a + MUL_Q12(num, t); + a = b >> 1; + p = MUL_Q12(a, a); + } + + return a; +} +#endif +Q15_t sqrt_q15(Q15_t num) +{ + Q15_t ans; + LQ15_t a, b; + LQ15_t p, e = FP_to_Q15(0.0001f); + LQ15_t t, numl; + int nb = 0; + + numl = num; + a = numl; + + p = MUL_LQ15(a, a); + while ((abs_lq15(p - num) >= e) && (nb++ < 64)) { + t = RECIP_LQ15(a); + b = a + MUL_LQ15(numl, t); + a = b >> 1; + p = MUL_LQ15(a, a); + nb++; + } + ans = a; +#if 0 + printf("SQRT: %f = %f nb = %i\n", LQ15_to_FP(numl), LQ15_to_FP(a), nb); +#endif + return ans; +} + +#ifdef ENABLE_Q24 +Q24_t sqrt_q24(Q24_t num) +{ + Q24_t a, p, e = FP_to_Q24(0.001f), b; + Q24_t t; + int nb = 0; + + a = num; + + p = MUL_Q24(a, a);; + while ((abs_q24(p - num) >= e) && (nb++ < 40)) { + t = RECIP_Q24(a); + b = a + MUL_Q24(num, t); + a = b >> 1; + p = MUL_Q24(a, a); + nb++; + } + return a; +} +#endif + +#ifdef TEST_SQRT +#include +int main(int argc, char **argv) +{ + float testv[] = { + 37483.949219f, + -1.0f + }; + int i; + + for (i = 0; testv[i] > 0; i++) { + printf("SQRT(%f) = (Q15: %f, float: %f)\n", + testv[i], Q15_to_FP(sqrt_q15(FP_to_Q15(testv[i]))), + sqrt(testv[i])); + } + return 0; +} + +#endif diff --git a/algorithm/osp/fp_trig.c b/algorithm/osp/fp_trig.c new file mode 100644 index 0000000..6c36dd9 --- /dev/null +++ b/algorithm/osp/fp_trig.c @@ -0,0 +1,77 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Table look up tan/sin/cos + */ +#include "fpsup.h" +#include "trig_sin.c" + + +Q15_t sin_q15(Q15_t ang) +{ + int adj = 1; + int i; + + do { + /* Sine is odd */ + if (ang >= q15_pi/2 && ang < q15_pi) { + ang -= q15_pi/2; + break; + } else if (ang >= q15_pi) { + ang -= q15_pi; + adj = -1 * adj; + } else if (ang < 0) { + ang += q15_pi; + adj = -1 * adj; + } else { + /* Between 0 - pi/2 */ + break; + } + } while(1); + + for (i = 0; i < sizeof(trig_sin)/sizeof(struct TRIG_SIN); i++) { + if (ang < trig_sin[i].ang) { + return trig_sin[i].sin * adj; + } + } + return 0; +} + +Q15_t cos_q15(Q15_t ang) +{ + return sin_q15(ang + q15_pi/2); +} + +Q15_t tan_q15(Q15_t ang) +{ + return DIV_Q15(sin_q15(ang),cos_q15(ang)); +} + +Q15_t arcsin_q15(Q15_t v) +{ + Q15_t l, ang; + int i, adj; + + if (v < 0) adj = -1; else adj = 1; + l = v * adj; + + for (i = 0; i < sizeof(trig_sin)/sizeof(struct TRIG_SIN); i++) { + if (l < trig_sin[i].sin) { + ang = trig_sin[i].ang; + break; + } + } + return ang*adj; + +} + +Q15_t arccos_q15(Q15_t v) +{ + Q15_t ang; + + ang = arcsin_q15(v); + return (q15_pi/2 - ang); +} diff --git a/algorithm/osp/fpsup.c b/algorithm/osp/fpsup.c new file mode 100644 index 0000000..2d31663 --- /dev/null +++ b/algorithm/osp/fpsup.c @@ -0,0 +1,319 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + +/* + * Fixed point math support library. + * Q15 - value is scaled by 1<<15 and stored in an int32_t + * Q24 - value is scaled by 1<<24 and stored in an int32_t + */ + +#include +#include +#include +#include "fpsup.h" + +const Q15_t q15_pi = FP_to_Q15(M_PI); +const Q15_t q15_c360 = FP_to_Q15(360.0f); +const Q15_t q15_c180 = FP_to_Q15(180.0f); +const Q15_t q15_c90 = FP_to_Q15(90.0f); +const Q15_t q15_c1 = FP_to_Q15(1.0f); +const Q15_t q15_c2 = FP_to_Q15(2.0f); +const Q15_t q15_half = FP_to_Q15(0.5f); +const Q15_t q15_quarter = FP_to_Q15(0.25f); + +#ifdef ENABLE_Q12 +const Q15_t q12_pi = FP_to_Q12(M_PI); +const Q15_t q12_c360 = FP_to_Q12(360.0f); +const Q15_t q12_c180 = FP_to_Q12(180.0f); +const Q15_t q12_c90 = FP_to_Q12(90.0f); +const Q15_t q12_c1 = FP_to_Q12(1.0f); +const Q15_t q12_c2 = FP_to_Q12(2.0f); +const Q15_t q12_half = FP_to_Q12(0.5f); +const Q15_t q12_quarter = FP_to_Q12(0.25f); +#endif + +#ifdef ENABLE_Q24 +const Q24_t q24_pi = FP_to_Q24(M_PI); +const Q24_t q24_c90 = FP_to_Q24(90.0f); +const Q24_t q24_c1 = FP_to_Q24(1.0f); +const Q24_t q24_c2 = FP_to_Q24(2.0f); +const Q24_t q24_half = FP_to_Q24(0.5f); +const Q24_t q24_quarter = FP_to_Q24(0.25f); +#endif + +/* MUL_LQ15 */ +/* + * LQ15 - 64 bit storage: + * 1 signed bit + * 63 bits of value divided into: + * 48bits of integer + * 15bits of fractional. + * Multiplication of 2 LQ15 produces 126bits of value. + * 15bits needs to be thrown away to convert it to 15 bits of + * fractional. This leaves 111 bits of numbers. + * + * LQ15 is to support intermediates used for sqrt() Q15 guesses. So + * the largest value needed is: + * 1 sign bit + * 2*16 integer bits + * 15 fractional bits + * ------ + * 48bits total. Thus the upper 63 bits is always zero. (except for + * sign extension). + */ +LQ15_t MUL_LQ15(LQ15_t a, LQ15_t b) +{ + /* Long multiplication principle: + * (From: http://www.edaboard.com/thread253439.html) + * + * x,y is the 2 multiplier (signed 64 bit) + * after sign valuation and (if need) sign inversion + * it is split into xh xl yh yl (unsigned 32 bit) + * the operations performed are: + * + * xh xl * + * yh yl = + * ------- + * xl_yl + + * xh_yl + + * xl_yh + + * xh_yh = + * ------------- + * (sign) z3 z1 z2 z0 + * + */ + + int8_t sign = 1; + + /* Intermediates */ + uint64_t al_bl, ah_bl, al_bh, ah_bh; + + uint32_t al, ah; + uint32_t bl, bh; + + uint64_t res0, res1; + + if (a<0) { + a = -a; + sign = -sign; + } + + if (b<0) { + b = -b; + sign = -sign; + } + + al = a & 0xffffffff; ah = a>>32; + bl = b & 0xffffffff; bh = b>>32; + if (ah != 0 || bh != 0) { + fprintf(stderr, "HY-DBG: OVERFLOW in %s\n", __func__); + return 0; + } + + al_bl = (uint64_t)al*(uint64_t)bl; + ah_bl = (uint64_t)ah*(uint64_t)bl; + al_bh = (uint64_t)al*(uint64_t)bh; + ah_bh = (uint64_t)ah*(uint64_t)bh; + + res0 = al_bl & 0xffffffff; + + res1 = (al_bl>>32)+(ah_bl & 0xffffffff)+(al_bh & 0xffffffff); + res0 |= (res1&0xffffffff) << 32; + + res1 >>= 32; // this is 'carry out' of the previous sum + res1 += (ah_bl>>32) + (al_bh>>32) +ah_bh; + + res0 >>= Q15_SHIFT; + + res0 &= ~(((uint64_t)(1<<15)-1) << (64-Q15_SHIFT)); + res0 |= (res1 & ((1<<15)-1)) << (64-Q15_SHIFT); + + return res0; +} + +/* MUL_Q15 */ + +Q15_t MUL_Q15(Q15_t a, Q15_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q15_SHIFT; +} + +#ifdef ENABLE_Q12 +Q12_t MUL_Q12(Q12_t a, Q12_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q12_SHIFT; +} +#endif + +#ifdef ENABLE_Q24 +/* MUL_Q24 */ +Q24_t MUL_Q24(Q24_t a, Q24_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q24_SHIFT; +} +#endif +LQ15_t DIV_LQ15(LQ15_t a, LQ15_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q15_SHIFT; + + return tmp/b; +} +/* DIV_Q15 */ +Q15_t DIV_Q15(Q15_t a, Q15_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q15_SHIFT; + + return tmp/b; +} +#ifdef ENABLE_Q12 +/* DIV_Q12 */ +Q12_t DIV_Q12(Q12_t a, Q12_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q12_SHIFT; + + return tmp/b; +} +#endif + +#ifdef ENABLE_Q24 +/* DIV_Q24 */ +Q24_t DIV_Q24(Q24_t a, Q24_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q24_SHIFT; + + return tmp/b; +} +#endif +/* RECIP_Q15 */ + +LQ15_t RECIP_LQ15(LQ15_t a) +{ + return DIV_LQ15(FP_to_Q15(1.0f), a); +} + + +Q15_t RECIP_Q15(Q15_t a) +{ + return DIV_Q15(q15_c1, a); +} + +#ifdef ENABLE_Q12 +Q12_t RECIP_Q12(Q12_t a) +{ + return DIV_Q12(q12_c1, a); +} +#endif + +#ifdef ENABLE_Q24 +/* RECIP_Q24 */ +Q24_t RECIP_Q24(Q24_t a) +{ + return DIV_Q24(q24_c1, a); +} +#endif + +double Q15_to_FP(Q15_t v) +{ + return ((double)v)/(double)(1< + +#ifndef M_PI +#define M_PI 3.141592f +#endif +typedef int32_t Q15_t; +#define Q15_SHIFT 15 +#define FP_to_Q15(v) ((v)*(1<> (Q15_SHIFT-Q12_SHIFT)) +#define Q12_to_Q15(x) ((x) << (Q15_SHIFT-Q12_SHIFT)) +Q12_t RECIP_Q12(Q12_t); +Q12_t DIV_Q12(Q12_t, Q12_t); +#endif + +#define MAX_Q15 0x7fffffff +#define MIN_Q15 0x80000000 + +Q15_t DIV_Q15(Q15_t, Q15_t); +LQ15_t DIV_LQ15(LQ15_t, LQ15_t); +Q15_t MUL_Q15(Q15_t, Q15_t); +LQ15_t MUL_LQ15(LQ15_t, LQ15_t); +Q15_t RECIP_Q15(Q15_t); +LQ15_t RECIP_LQ15(LQ15_t); + +extern const Q15_t q15_pi; +extern const Q15_t q15_c360; +extern const Q15_t q15_c180; +extern const Q15_t q15_c90; +extern const Q15_t q15_c1; +extern const Q15_t q15_c2; +extern const Q15_t q15_half; +extern const Q15_t q15_quarter; + +#ifdef ENABLE_Q12 +const Q12_t q12_pi; +const Q12_t q12_c360; +const Q12_t q12_c180; +const Q12_t q12_c90; +const Q12_t q12_c1; +const Q12_t q12_c2; +const Q12_t q12_half; +const Q12_t q12_quarter; +#endif + +#ifdef ENABLE_Q24 +Q24_t DIV_Q24(Q24_t, Q24_t); +Q24_t MUL_Q24(Q24_t, Q24_t); +Q24_t RECIP_Q24(Q24_t); + +const Q24_t q24_pi; +const Q24_t q24_c360; +const Q24_t q24_c180; +const Q24_t q24_c90; +const Q24_t q24_c1; +const Q24_t q24_c2; +const Q24_t q24_half; +const Q24_t q24_quarter; + +double Q24_to_FP(Q24_t); +#endif +double Q15_to_FP(Q15_t); +double LQ15_to_FP(LQ15_t); + +#ifdef ENABLE_Q12 +double Q12_to_FP(Q12_t); +#endif + +Q15_t atan2_q15(Q15_t, Q15_t); +Q15_t tan_q15(Q15_t); +Q15_t sqrt_q15(Q15_t); +Q15_t arccos_q15(Q15_t); +Q15_t arcsin_q15(Q15_t); +#ifdef ENABLE_Q24 +Q15_t sqrt_q24(Q15_t); +#endif + +Q15_t pow_q15(Q15_t, Q15_t); +Q15_t abs_q15(Q15_t); +LQ15_t abs_lq15(LQ15_t); +#endif diff --git a/algorithm/osp/gravity_lin.c b/algorithm/osp/gravity_lin.c new file mode 100644 index 0000000..1e20607 --- /dev/null +++ b/algorithm/osp/gravity_lin.c @@ -0,0 +1,54 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Gravity and linear acceleration computation from AOSP. + */ +#include "fpsup.h" +#include "lpf.h" +#include "fp_sensor.h" +#include "gravity_lin.h" + +static struct LPF xlpf, ylpf, zlpf; +static struct LPF_CBQF xclpf, yclpf, zclpf; + +static const Q15_t SAMPLE_PERIOD = FP_to_Q15(0.020f); + +void OSP_gravity_init(void) +{ + LPF_init(&xlpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_init(&ylpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_init(&zlpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_setSamplingPeriod(&xlpf, SAMPLE_PERIOD); + LPF_setSamplingPeriod(&ylpf, SAMPLE_PERIOD); + LPF_setSamplingPeriod(&zlpf, SAMPLE_PERIOD); + LPF_CBQF_init(&xclpf, &xlpf, 0); + LPF_CBQF_init(&yclpf, &ylpf, 0); + LPF_CBQF_init(&zclpf, &zlpf, FP_to_Q15(9.8f)); +} + +void OSP_linear_acc_init(void) +{ +} + +void OSP_gravity_process(struct ThreeAxis *acc, struct ThreeAxis *res) +{ + res->x = LPF_CBQF_data(&xclpf, acc->x); + res->y = LPF_CBQF_data(&yclpf, acc->y); + res->z = LPF_CBQF_data(&zclpf, acc->z); +}; + +void OSP_linear_acc_process(struct ThreeAxis *acc, + struct ThreeAxis *gravity, + struct ThreeAxis *res) +{ + res->x = acc->x - (gravity->x); + res->y = acc->y - (gravity->y); + res->z = acc->z - (gravity->z); +} diff --git a/algorithm/osp/gravity_lin.h b/algorithm/osp/gravity_lin.h new file mode 100644 index 0000000..6718acc --- /dev/null +++ b/algorithm/osp/gravity_lin.h @@ -0,0 +1,13 @@ +#ifndef _GRAVITY_LIN_H_ +#define _GRAVITY_LIN_H_ 1 +#include "fp_sensor.h" + +void OSP_gravity_init(void); +void OSP_gravity_process(struct ThreeAxis *acc, struct ThreeAxis *res); + +void OSP_linear_acc_init(void); +void OSP_linear_acc_process(struct ThreeAxis *acc, + struct ThreeAxis *gravity, + struct ThreeAxis *res); + +#endif diff --git a/algorithm/osp/include/osp-alg-types.h b/algorithm/osp/include/osp-alg-types.h new file mode 100755 index 0000000..aabbeff --- /dev/null +++ b/algorithm/osp/include/osp-alg-types.h @@ -0,0 +1,80 @@ +/* Open Sensor Platform Project + * https://github.com/sensorplatforms/open-sensor-platform + * + * Copyright (C) 2013 Sensor Platforms Inc. + * + * 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. + */ +#if !defined (OSP_ALG_TYPES) +#define OSP_ALG_TYPES + +/*-------------------------------------------------------------------------------------------------*\ + | I N C L U D E F I L E S +\*-------------------------------------------------------------------------------------------------*/ +#include +#include "osp-types.h" +#include "osp-fixedpoint-types.h" + +/*-------------------------------------------------------------------------------------------------*\ + | C O N S T A N T S & M A C R O S +\*-------------------------------------------------------------------------------------------------*/ +#define NUM_ACCEL_AXES (3) + +/*-------------------------------------------------------------------------------------------------*\ + | T Y P E D E F I N I T I O N S +\*-------------------------------------------------------------------------------------------------*/ +//! Enumeration typedef for segment type +typedef enum EStepSegmentType { + firstStep, + midStep, + lastStep +} EStepSegmentType; + +//! Struct definition for a step segment +typedef struct { + //store start/stop times of the last step + NTTIME startTime; + NTTIME stopTime; + EStepSegmentType type; +} StepSegment_t; + +//struct for defining a step +typedef struct StepDataOSP_t{ + NTTIME startTime; + NTTIME stopTime; + osp_float_t stepFrequency; + uint32_t numStepsTotal; + uint32_t numStepsSinceWalking; +} StepDataOSP_t; + +typedef void (*OSP_StepSegmentResultCallback_t)(StepSegment_t * segment); +typedef void (*OSP_StepResultCallback_t)(StepDataOSP_t* stepData); +typedef void (*OSP_EventResultCallback_t)(NTTIME * eventTime); + +/*-------------------------------------------------------------------------------------------------*\ + | E X T E R N A L V A R I A B L E S & F U N C T I O N S +\*-------------------------------------------------------------------------------------------------*/ + +/*-------------------------------------------------------------------------------------------------*\ + | P U B L I C V A R I A B L E S D E F I N I T I O N S +\*-------------------------------------------------------------------------------------------------*/ + +/*-------------------------------------------------------------------------------------------------*\ + | P U B L I C F U N C T I O N D E C L A R A T I O N S +\*-------------------------------------------------------------------------------------------------*/ + + +#endif /* OSP_ALG_TYPES */ +/*-------------------------------------------------------------------------------------------------*\ + | E N D O F F I L E +\*-------------------------------------------------------------------------------------------------*/ diff --git a/algorithm/osp/lpf.h b/algorithm/osp/lpf.h new file mode 100644 index 0000000..7e05da0 --- /dev/null +++ b/algorithm/osp/lpf.h @@ -0,0 +1,35 @@ +#ifndef LPF_H +#define LPF_H 1 + +struct LPF { + Q15_t iQ; + Q15_t fc; + Q15_t K; + Q15_t iD; + Q15_t a0; + Q15_t a1; + Q15_t b1; + Q15_t b2; + + Q15_t x1; + Q15_t x2; + Q15_t y1; + Q15_t y2; +}; + +struct LPF_CBQF { + struct LPF mA; + struct LPF mB; +}; + +void LPF_init(struct LPF *lpf, Q15_t Q, Q15_t fc); +void LPF_setSamplingPeriod(struct LPF *lpf, Q15_t dT); +Q15_t LPF_BQF_init(struct LPF *lpf, Q15_t x); +Q15_t LPF_BQF_data(struct LPF *lpf, Q15_t x); +void LPF_CBQF_init(struct LPF_CBQF *clpf, struct LPF *lpf, Q15_t x); +Q15_t LPF_CBQF_data(struct LPF_CBQF *clpf, Q15_t x); + + + + +#endif diff --git a/algorithm/osp/osp.c b/algorithm/osp/osp.c new file mode 100644 index 0000000..5a9db7a --- /dev/null +++ b/algorithm/osp/osp.c @@ -0,0 +1,779 @@ +/* + * (C) Copyright 2015 HY Research LLC + * + * Author: hy-git@hy-research.com + * + * + * Apache License. + * + * Interface layer for OSP. Implements a subset of the public + * functions as defined by the OSP API. Translates between the + * Fix point names used by OSP to a more generic Q24/Q12/Q15/etc + * naming scheme. + */ + + + + +#include +#include +#include "fp_sensor.h" +#include "fpsup.h" +#include "ecompass.h" +#include "gravity_lin.h" +#include "rotvec.h" +#include "sigmot.h" +#include "osp-sensors.h" +#include "osp-fixedpoint-types.h" +#include "osp-version.h" +#include "osp-api.h" +#include "osp-alg-types.h" +#if 0 +#include "signalgenerator.h" +#include "significantmotiondetector.h" +#include "stepdetector.h" +#endif +#include "step.h" +#include "tilt.h" + +#define CLEAN 0 +#define DIRTY_IN (1<<0) +#define DIRTY_OUT (1<<1) + +#define SEN_ENABLE 1 + +#undef KEIL_WORKAROUND +static struct Results RESULTS[NUM_ANDROID_SENSOR_TYPE]; +static unsigned char dirty[NUM_ANDROID_SENSOR_TYPE]; +static SystemDescriptor_t const *sys; +static SensorDescriptor_t const *InputSensors[NUM_ANDROID_SENSOR_TYPE]; + +static const OSP_Library_Version_t libVersion = { + .VersionNumber = (OSP_VERSION_MAJOR << 16) | (OSP_VERSION_MINOR << 8) | (OSP_VERSION_PATCH), + .VersionString = OSP_VERSION_STRING, + .buildTime = __DATE__" "__TIME__ +}; + +#ifndef KEIL_WORKAROUND +static const uint64_t depend[NUM_ANDROID_SENSOR_TYPE] = +{ + [SENSOR_ACCELEROMETER] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_MAGNETIC_FIELD] = FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_ORIENTATION] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_ROTATION_VECTOR] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_GRAVITY] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_LINEAR_ACCELERATION] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_GYROSCOPE] = FLAG(SENSOR_GYROSCOPE), + [SENSOR_PRESSURE] = FLAG(SENSOR_PRESSURE), + [SENSOR_STEP_DETECTOR] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_STEP_COUNTER] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_SIGNIFICANT_MOTION] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_TILT_DETECTOR] = FLAG(SENSOR_ACCELEROMETER), +}; +#else +static uint64_t depend[NUM_ANDROID_SENSOR_TYPE]; +#endif + + +static ResultDescriptor_t * resHandles[NUM_ANDROID_SENSOR_TYPE]; + +static uint8_t sensor_state[NUM_ANDROID_SENSOR_TYPE]; +static void (*readyCB[NUM_ANDROID_SENSOR_TYPE])(struct Results *, int); + +static void OSP_SetDataMag(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.x = x; + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.y = y; + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.z = z; + RESULTS[SENSOR_MAGNETIC_FIELD].time = time; + dirty[SENSOR_MAGNETIC_FIELD] = DIRTY_IN | DIRTY_OUT; +} + +static void OSP_SetDataAcc(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + osp_float_t measurementFiltered[NUM_ACCEL_AXES]; + osp_float_t measurementFloat[NUM_ACCEL_AXES]; + NTTIME filterTime = time; + + RESULTS[SENSOR_ACCELEROMETER].ResType.result.x = x; + RESULTS[SENSOR_ACCELEROMETER].ResType.result.y = y; + RESULTS[SENSOR_ACCELEROMETER].ResType.result.z = z; + RESULTS[SENSOR_ACCELEROMETER].time = time; + dirty[SENSOR_ACCELEROMETER] = DIRTY_IN | DIRTY_OUT; +#if 0 + measurementFloat[0] = Q15_to_FP(x); + measurementFloat[1] = Q15_to_FP(y); + measurementFloat[2] = Q15_to_FP(z); + + if (SignalGenerator_SetAccelerometerData(measurementFloat, + measurementFiltered)){ + filterTime -= SIGNAL_GENERATOR_DELAY; + + //update significant motion alg + SignificantMotDetector_SetFilteredAccelerometerMeasurement( + filterTime, + measurementFiltered); + + StepDetector_SetFilteredAccelerometerMeasurement(filterTime, + measurementFiltered); + } +#endif +} + +static void OSP_SetDataGyr(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + RESULTS[SENSOR_GYROSCOPE].ResType.result.x = x; + RESULTS[SENSOR_GYROSCOPE].ResType.result.y = y; + RESULTS[SENSOR_GYROSCOPE].ResType.result.z = z; + RESULTS[SENSOR_GYROSCOPE].time = time; + dirty[SENSOR_GYROSCOPE] = DIRTY_IN | DIRTY_OUT; +} + +void OSPalg_SetDataBaro(Q15_t p, Q15_t t, NTTIME time) +{ + RESULTS[SENSOR_PRESSURE].ResType.result.x = p; + RESULTS[SENSOR_PRESSURE].ResType.result.z = t; + RESULTS[SENSOR_PRESSURE].time = time; + dirty[SENSOR_PRESSURE] = DIRTY_IN | DIRTY_OUT; +} + +static void OSPalg_EnableSensor(unsigned int sensor) +{ + int i; + if (sensor_state[sensor] == SEN_ENABLE) + return; + + if (depend[sensor] != FLAG(sensor)) { + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (i == sensor) + continue; + if (depend[sensor] & FLAG(i)) { + if (sensor_state[i] == SEN_ENABLE) + continue; + OSPalg_EnableSensor(i); + } + } + } + sensor_state[sensor] = SEN_ENABLE; + if (sys->SensorsControl) { + SensorControl_t ctrl; + + ctrl.Data = 0; + ctrl.Command = SENSOR_CONTROL_SENSOR_ON; + ctrl.Handle = &InputSensors[sensor]; + sys->SensorsControl(&ctrl); + } +} + +/* Dispatch data in the different esoteric structure */ +static void ResultReadyCB(struct Results *res, int sensor) +{ + union { + Android_TriAxisPreciseData_t calaccel; + Android_UncalibratedTriAxisPreciseData_t uncalaccel; + Android_TriAxisPreciseData_t calgyro; + Android_UncalibratedTriAxisPreciseData_t uncalgyro; + + Android_BooleanResultData_t sigmot; + + Android_UncalibratedTriAxisExtendedData_t uncalmag; + Android_TriAxisExtendedData_t calmag; + Android_OrientationResultData_t orient; + Android_TriAxisPreciseData_t generic; + Android_RotationVectorResultData_t rotvec; + } r; + + if (!resHandles[sensor]) + return; + switch(sensor) { + case SENSOR_ACCELEROMETER: + r.calaccel.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calaccel.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calaccel.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calaccel.TimeStamp = res->time; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + r.uncalmag.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.uncalmag.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.uncalmag.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.uncalmag.TimeStamp = res->time; + break; + case SENSOR_MAGNETIC_FIELD: + r.calmag.X = Q15_to_NTEXTENDED(res->ResType.result.x); + r.calmag.Y = Q15_to_NTEXTENDED(res->ResType.result.y); + r.calmag.Z = Q15_to_NTEXTENDED(res->ResType.result.z); + r.calmag.TimeStamp = res->time; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + r.uncalgyro.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.uncalgyro.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.uncalgyro.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.uncalgyro.TimeStamp = res->time; + break; + case SENSOR_GYROSCOPE: + r.calgyro.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calgyro.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calgyro.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calgyro.TimeStamp = res->time; + break; + case SENSOR_ORIENTATION: + r.orient.Pitch = Q15_to_NTEXTENDED(res->ResType.euler.pitch); + r.orient.Roll = Q15_to_NTEXTENDED(res->ResType.euler.roll); + r.orient.Yaw = Q15_to_NTEXTENDED(res->ResType.euler.yaw); + r.orient.TimeStamp = res->time; + break; + case SENSOR_PRESSURE: + return; + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + r.generic.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.generic.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.generic.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.generic.TimeStamp = res->time; + break; + + case SENSOR_ROTATION_VECTOR: + r.rotvec.X = Q15_to_NTPRECISE(res->ResType.quat.x); + r.rotvec.Y = Q15_to_NTPRECISE(res->ResType.quat.y); + r.rotvec.Z = Q15_to_NTPRECISE(res->ResType.quat.z); + r.rotvec.W = Q15_to_NTPRECISE(res->ResType.quat.w); + r.rotvec.TimeStamp = res->time; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_TILT_DETECTOR: + r.sigmot.data = true; + r.sigmot.TimeStamp = res->time; + break; + default: + return; + } + if (resHandles[sensor]->pResultReadyCallback) { + printf("Calling: %i\n", sensor); + resHandles[sensor]->pResultReadyCallback(resHandles[sensor], + &r); + } +} +#if 0 +static void OnStepResultsReady( StepDataOSP_t* stepData ) +{ + if (resHandles[SENSOR_STEP_COUNTER]) { + Android_StepCounterResultData_t callbackData; + + callbackData.StepCount = stepData->numStepsTotal; + callbackData.TimeStamp = stepData->startTime; //!TODO - Double check if start time or stop time + + resHandles[SENSOR_STEP_COUNTER]->pResultReadyCallback( + resHandles[SENSOR_STEP_COUNTER], + &callbackData); + } +} + +static void OnSignificantMotionResult(NTTIME * eventTime) +{ + if (resHandles[SENSOR_SIGNIFICANT_MOTION]) { + Android_BooleanResultData_t callbackData; + + callbackData.data = true; + callbackData.TimeStamp = *eventTime; + + resHandles[SENSOR_SIGNIFICANT_MOTION]->pResultReadyCallback( + resHandles[SENSOR_SIGNIFICANT_MOTION], + &callbackData); + } +} +#endif +static void OSPalg_EnableSensorCB(unsigned int sensor, + void (*ready)(struct Results *, int)) +{ + readyCB[sensor] = ready; + OSPalg_EnableSensor(sensor); +} + +static void OSPalg_DisableSensor(unsigned int sensor) +{ + int i; + int busy = 0; + + if (sensor_state[sensor] != SEN_ENABLE) + return; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (i == sensor) continue; + + if ((sensor_state[i] == SEN_ENABLE) && + (depend[i] & FLAG(sensor))) { + busy++; + } + } + if (busy == 0) { + sensor_state[sensor] = 0; + if (sys->SensorsControl) { + SensorControl_t ctrl; + + ctrl.Data = 0; + ctrl.Command = SENSOR_CONTROL_SENSOR_OFF; + ctrl.Handle = &InputSensors[sensor]; + sys->SensorsControl(&ctrl); + } + } +} + +void OSPalg_cal(void) +{ + OSP_ecompass_cal(); +} + +OSP_STATUS_t OSP_DoBackgroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_DoForegroundProcessing(void) +{ + int i; + if (sensor_state[SENSOR_GRAVITY] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN) { + OSP_gravity_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result); + dirty[SENSOR_GRAVITY] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_GRAVITY].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_LINEAR_ACCELERATION] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_linear_acc_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_LINEAR_ACCELERATION].ResType.result); + dirty[SENSOR_LINEAR_ACCELERATION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_LINEAR_ACCELERATION].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_SIGNIFICANT_MOTION] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_SIGNIFICANT_MOTION] & DIRTY_IN) { + OSP_sigmot_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_SIGNIFICANT_MOTION].ResType.result); + dirty[SENSOR_SIGNIFICANT_MOTION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_SIGNIFICANT_MOTION].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_ORIENTATION] == SEN_ENABLE) + if (dirty[SENSOR_MAGNETIC_FIELD] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_ecompass_process(&RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_ORIENTATION].ResType.euler); + dirty[SENSOR_ORIENTATION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_ORIENTATION].time = RESULTS[SENSOR_GRAVITY].time; + } + + if (sensor_state[SENSOR_ROTATION_VECTOR] == SEN_ENABLE) + if (dirty[SENSOR_MAGNETIC_FIELD] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_rotvec_process(&RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_ROTATION_VECTOR].ResType.quat); + dirty[SENSOR_ROTATION_VECTOR] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_ROTATION_VECTOR].time = RESULTS[SENSOR_GRAVITY].time; + } + if (sensor_state[SENSOR_TILT_DETECTOR] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_TILT_DETECTOR] & DIRTY_IN) { + OSP_tilt_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_TILT_DETECTOR].ResType.result); + dirty[SENSOR_TILT_DETECTOR] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_TILT_DETECTOR].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + +#ifdef FEAT_STEP + if (sensor_state[SENSOR_STEP_COUNTER] == SEN_ENABLE || + sensor_state[SENSOR_STEP_DETECTOR] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN) { + OSP_step_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_STEP_COUNTER].ResType.step); + if (RESULTS[SENSOR_STEP_DETECTOR].ResType.step.detect) { + dirty[SENSOR_STEP_COUNTER] = DIRTY_IN|DIRTY_OUT; + dirty[SENSOR_STEP_DETECTOR] = DIRTY_IN|DIRTY_OUT; + } + RESULTS[SENSOR_STEP_DETECTOR].time = RESULTS[SENSOR_ACCELEROMETER].time; + } +#endif + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (dirty[i]) { + if (readyCB[i] && sensor_state[i] == SEN_ENABLE) { + (readyCB[i])(&RESULTS[i], i); + } + dirty[i] = CLEAN; + } + } + + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *SenDesc, + InputSensorHandle_t *rHandle) +{ + if (!SenDesc) return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + switch(SenDesc->SensorType) { + case SENSOR_ACCELEROMETER: + case SENSOR_MAGNETIC_FIELD: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_GYROSCOPE: + case SENSOR_GYROSCOPE_UNCALIBRATED: + if (InputSensors[SenDesc->SensorType]) + return OSP_STATUS_SENSOR_ALREADY_REGISTERED; + InputSensors[SenDesc->SensorType] = SenDesc; + *rHandle = &InputSensors[SenDesc->SensorType]; + return OSP_STATUS_OK; + default: + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + } + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; +} + +static void conv2OSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long ts, int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + d->q24data.X = val[0]; + d->q24data.Y = val[1]; + d->q24data.Z = val[2]; + d->q24data.TimeStamp = ts; + break; + + case SENSOR_GEOMAGNETIC_FIELD: + d->q12data.X = val[0]; + d->q12data.Y = val[1]; + d->q12data.Z = val[2]; + d->q12data.TimeStamp = ts; + break; + + case SENSOR_ORIENTATION: + d->orientdata.Yaw = val[0]; + d->orientdata.Pitch = val[1]; + d->orientdata.Roll = val[2]; + d->orientdata.TimeStamp = ts; + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + d->rotvec.X = val[0]; + d->rotvec.Y = val[1]; + d->rotvec.Z = val[2]; + d->rotvec.W = val[3]; + d->rotvec.TimeStamp = ts; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + d->uncal_q24data.X = val[0]; + d->uncal_q24data.Y = val[1]; + d->uncal_q24data.Z = val[2]; + d->uncal_q24data.TimeStamp = ts; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + d->uncal_q12data.X = val[0]; + d->uncal_q12data.Y = val[1]; + d->uncal_q12data.Z = val[2]; + d->uncal_q12data.TimeStamp = ts; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + d->booldata.data = val[0]; + d->booldata.TimeStamp = ts; + break; + case SENSOR_STEP_COUNTER: + d->stepcount.StepCount = val[0]; + d->stepcount.TimeStamp = ts; + break; + default: + break; + } +} +static inline int extractOSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long *ts, int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + val[0] = d->q24data.X; + val[1] = d->q24data.Y; + val[2] = d->q24data.Z; + *ts = d->q24data.TimeStamp; + return 3; + + case SENSOR_GEOMAGNETIC_FIELD: + val[0] = d->q12data.X; + val[1] = d->q12data.Y; + val[2] = d->q12data.Z; + *ts = d->q12data.TimeStamp; + return 3; + + case SENSOR_ORIENTATION: + val[0] = d->orientdata.Yaw; + val[1] = d->orientdata.Pitch; + val[2] = d->orientdata.Roll; + *ts = d->orientdata.TimeStamp; + return 3; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + val[0] = d->rotvec.X; + val[1] = d->rotvec.Y; + val[2] = d->rotvec.Z; + val[3] = d->rotvec.W; + *ts = d->rotvec.TimeStamp; + return 4; + case SENSOR_GYROSCOPE_UNCALIBRATED: + val[0] = d->uncal_q24data.X; + val[1] = d->uncal_q24data.Y; + val[2] = d->uncal_q24data.Z; + *ts = d->uncal_q24data.TimeStamp; + return 3; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + val[0] = d->uncal_q12data.X; + val[1] = d->uncal_q12data.Y; + val[2] = d->uncal_q12data.Z; + *ts = d->uncal_q12data.TimeStamp; + return 3; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + val[0] = d->booldata.data; + *ts = d->booldata.TimeStamp; + return 1; + case SENSOR_STEP_COUNTER: + val[0] = d->stepcount.StepCount; + *ts = d->stepcount.TimeStamp; + return 1; + default: + *ts = 0; + return 0; + } + return 0; +} + +/* BUG: Mag needs to be handled differently. */ +OSP_STATUS_t OSP_SetInputData(InputSensorHandle_t handle, + OSP_InputSensorData_t *data) +{ + SensorDescriptor_t *s, **v; + Q15_t x, y, z; + int32_t rawX, rawY, rawZ; + int32_t xidx, yidx, zidx; + int32_t xs, ys, zs; + int i; + int val[5], vallen; + unsigned long long ts; + + if (!handle) return OSP_STATUS_INVALID_HANDLE; + v = handle; + s = *v; + vallen = extractOSP(s->SensorType, data, &ts, val); + + for (i = 0; i < 3; i++) { + switch (s->AxisMapping[i]) { + case AXIS_MAP_POSITIVE_X: + xidx = i; + xs = 1; + break; + case AXIS_MAP_NEGATIVE_X: + xidx = i; + xs = -1; + break; + case AXIS_MAP_POSITIVE_Y: + yidx = i; + ys = 1; + break; + case AXIS_MAP_NEGATIVE_Y: + yidx = i; + ys = -1; + break; + case AXIS_MAP_POSITIVE_Z: + zidx = i; + zs = 1; + break; + case AXIS_MAP_NEGATIVE_Z: + zidx = i; + zs = -1; + break; + default: + break; + } + } + rawX = xs*val[xidx] - s->ConversionOffset[xidx]; + rawY = ys*val[yidx] - s->ConversionOffset[yidx]; + rawZ = zs*val[zidx] - s->ConversionOffset[zidx]; + x = MUL_Q15(INT_to_Q15(rawX), NTPRECISE_to_Q15(s->ConversionScale[xidx])); + y = MUL_Q15(INT_to_Q15(rawY), NTPRECISE_to_Q15(s->ConversionScale[yidx])); + z = MUL_Q15(INT_to_Q15(rawZ), NTPRECISE_to_Q15(s->ConversionScale[zidx])); + + switch(s->SensorType) { + case SENSOR_ACCELEROMETER: + OSP_SetDataAcc(x, y, z, ts); + break; + case SENSOR_MAGNETIC_FIELD: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + OSP_SetDataMag(x, y, z, ts); + break; + case SENSOR_GYROSCOPE: + case SENSOR_GYROSCOPE_UNCALIBRATED: + OSP_SetDataGyr(x, y, z, ts); + break; + case SENSOR_PRESSURE: + /* Ignore for now */ + break; + default: + break; + } + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_SubscribeSensorResult(ResultDescriptor_t *ResDesc, + ResultHandle_t *ResHandle) +{ + if (!ResDesc) return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + switch (ResDesc->SensorType) { + case SENSOR_ACCELEROMETER: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_MAGNETIC_FIELD: + case SENSOR_GYROSCOPE_UNCALIBRATED: + case SENSOR_GYROSCOPE: + case SENSOR_ORIENTATION: + case SENSOR_PRESSURE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_ROTATION_VECTOR: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; + OSPalg_EnableSensorCB(ResDesc->SensorType, + ResultReadyCB); + *ResHandle = resHandles[ResDesc->SensorType]; + break; + case SENSOR_STEP_DETECTOR: + case SENSOR_STEP_COUNTER: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + StepDetector_Init(OnStepResultsReady, NULL); +#endif + break; + case SENSOR_SIGNIFICANT_MOTION: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + SignificantMotDetector_Init(OnSignificantMotionResult); +#endif + OSP_sigmot_init(); + break; + case SENSOR_TILT_DETECTOR: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + SignificantMotDetector_Init(OnSignificantMotionResult); +#endif + OSP_tilt_init(); + break; + default: + return OSP_STATUS_SENSOR_INVALID_TYPE; + } + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_UnsubscribeSensorResult(ResultHandle_t ResHandle) +{ + ResultDescriptor_t **rd; + + rd = ResHandle; + + if (!rd) return OSP_STATUS_INVALID_HANDLE; + + if (*rd == NULL) + return OSP_STATUS_NOT_SUBSCRIBED; + if ((*rd)->SensorType == SENSOR_SIGNIFICANT_MOTION) + OSP_sigmot_init(); + + if ((*rd)->SensorType == SENSOR_TILT_DETECTOR) + OSP_tilt_init(); +#if 0 + if ((*rd)->SensorType == SENSOR_SIGNIFICANT_MOTION) + SignificantMotDetector_Init(NULL); + if ((*rd)->SensorType == SENSOR_STEP_COUNTER) + StepDetector_Init(NULL, NULL); +#endif + /* Do magic with disabling callbacks */ + OSPalg_DisableSensor((*rd)->SensorType); + *(rd) = NULL; + + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t *sysdesc) +{ + int i; + + /* Work around for broken Keil */ +#ifdef KEIL_WORKAROUND +#error Keil is broken. + depend[SENSOR_ACCELEROMETER] = FLAG(SENSOR_ACCELEROMETER); + depend[SENSOR_MAGNETIC_FIELD] = FLAG(SENSOR_MAGNETIC_FIELD); + depend[SENSOR_ORIENTATION] = FLAG(SENSOR_ACCELEROMETER)|FLAG(SENSOR_MAGNETIC_FIELD); + depend[SENSOR_ROTATION_VECTOR] = FLAG(SENSOR_ACCELEROMETER)|FLAG(SENSOR_MAGNETIC_FIELD); + depend[SENSOR_GRAVITY] = FLAG(SENSOR_ACCELEROMETER); + depend[SENSOR_LINEAR_ACCELERATION] = FLAG(SENSOR_ACCELEROMETER); + depend[SENSOR_GYROSCOPE] = FLAG(SENSOR_GYROSCOPE); + depend[SENSOR_PRESSURE] = FLAG(SENSOR_PRESSURE); + depend[SENSOR_STEP_DETECTOR] = FLAG(SENSOR_ACCELEROMETER); + depend[SENSOR_STEP_COUNTER] = FLAG(SENSOR_ACCELEROMETER); + depend[SENSOR_SIGNIFICANT_MOTION] = FLAG(SENSOR_ACCELEROMETER); +#endif + + sys = sysdesc; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + InputSensors[i] = NULL; + dirty[i] = CLEAN; + sensor_state[i] = 0; + readyCB[i] = NULL; + resHandles[i] = NULL; + memset(&RESULTS[i], 0, sizeof(struct Results)); + } + OSP_gravity_init(); + OSP_ecompass_init(); + OSP_rotvec_init(); + OSP_linear_acc_init(); + OSP_sigmot_init(); +#ifdef FEAT_STEP + OSP_step_init(); +#endif + OSP_tilt_init(); + //Initialize signal generator +#if 0 + SignalGenerator_Init(); + + //Initialize algs + SignificantMotDetector_Init(NULL); + StepDetector_Init(NULL, NULL); +#endif + + + return OSP_STATUS_OK; +} + + +OSP_STATUS_t OSP_GetLibraryVersion(const OSP_Library_Version_t **v) +{ + *v = &libVersion; + return OSP_STATUS_OK; +} diff --git a/algorithm/osp/osp.h b/algorithm/osp/osp.h new file mode 100644 index 0000000..050da9b --- /dev/null +++ b/algorithm/osp/osp.h @@ -0,0 +1,16 @@ +#ifndef OSP_H +#define OSP_H 1 + +#include "osp-fixedpoint-types.h" +#include "fpsup.h" +#if 0 +void OSP_init(void); +void OSPalg_Process(void); +void OSPalg_SetDataAcc(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_SetDataMag(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_SetDataGyr(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_cal(void); +void OSPalg_EnableSensorCB(unsigned int, void (*ready)(struct Results *, int)); +void OSPalg_DisableSensor(unsigned int); +#endif +#endif \ No newline at end of file diff --git a/algorithm/osp/rotvec.c b/algorithm/osp/rotvec.c new file mode 100644 index 0000000..215116f --- /dev/null +++ b/algorithm/osp/rotvec.c @@ -0,0 +1,154 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Compute rotation vector from accel/mag. + * Based on Android implementation. Converted to fixed point. + */ + +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "rotvec.h" + +#include + +static Q15_t copysign_q15(Q15_t v, Q15_t s) +{ + if (s < 0) return -v; + else return v; +} + + +static Q15_t clamp_q15(Q15_t v) +{ + if (v < 0) return 0; + return v; +} + +void OSP_rotvec_init() +{ +} + +void OSP_rotvec_process( + struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Quat *rot) +{ + Q15_t normH; + Q15_t accMag2; + + Q15_t magMag2; + Q15_t Hx, Hy, Hz; + Q15_t Ex, Ey, Ez; + + Q15_t invA; + Q15_t invH; + + Q15_t Ax, Ay, Az; + Q15_t Mx, My, Mz; + + Q15_t qw, qx, qy, qz; + + Ax = acc->x; + Ay = acc->y; + Az = acc->z; + + if (Ax == 0 && Ay == 0 && Az == 0) return; + + Ex = mag->x; + Ey = mag->y; + Ez = mag->z; + +#if 0 + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez)); +#endif + Hx = MUL_Q15(Ey, Az) - MUL_Q15(Ez, Ay); + Hy = MUL_Q15(Ez, Ax) - MUL_Q15(Ex, Az); + Hz = MUL_Q15(Ex, Ay) - MUL_Q15(Ey, Ax); + + + magMag2 = MUL_Q15(Hx,Hx); + magMag2 += MUL_Q15(Hy, Hy); + magMag2 += MUL_Q15(Hz, Hz); + + normH = sqrt_q15(magMag2); + + if (normH < FP_to_Q15(0.1f)) { + printf("Bad mag? %f (from %f)\n", Q15_to_FP(normH), Q15_to_FP(magMag2)); + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f Hx: %f Hy: %f Hz: %f (%f, %f, %f)\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez), + Q15_to_FP(Hx), Q15_to_FP(Hy), Q15_to_FP(Hz), + Q15_to_FP(MUL_Q15(Hx,Hx)), + Q15_to_FP(MUL_Q15(Hy,Hy)), + Q15_to_FP(MUL_Q15(Hz,Hz)) + ); + return; + } + invH = RECIP_Q15(normH); + if (invH <= 0) { + printf("OF/UF?\n"); + } + + accMag2 = MUL_Q15(Ax, Ax); + accMag2 += MUL_Q15(Ay, Ay); + accMag2 += MUL_Q15(Az, Az); + + if (accMag2 < 0) { + printf("OF? - 2\n"); + } + + invA = RECIP_Q15(sqrt_q15(accMag2)); + + Hx = DIV_Q15(Hx, normH); + Hy = DIV_Q15(Hy, normH); + Hz = DIV_Q15(Hz, normH); + + Ax = MUL_Q15(Ax, invA); + Ay = MUL_Q15(Ay, invA); + Az = MUL_Q15(Az, invA); + + Mx = MUL_Q15(Ay, Hz) - MUL_Q15(Az, Hy); + My = MUL_Q15(Az, Hx) - MUL_Q15(Ax, Hz); + Mz = MUL_Q15(Ax, Hy) - MUL_Q15(Ay, Hx); +#if 0 + qw = sqrt_q12(MUL_Q12(clamp_q12( Hx+My+Az+q12_c1), q12_quarter)); + qx = sqrt_q12(MUL_Q12(clamp_q12( Hx-My-Az+q12_c1), q12_quarter)); + qy = sqrt_q12(MUL_Q12(clamp_q12(-Hx+My-Az+q12_c1), q12_quarter)); + qz = sqrt_q12(MUL_Q12(clamp_q12(-Hx-My+Az+q12_c1), q12_quarter)); +#else + { + Q15_t t; + + t = MUL_Q15(clamp_q15( Hx+My+Az+q15_c1), q15_quarter); + qw = sqrt_q15(t); + + t = MUL_Q15(clamp_q15( Hx-My-Az+q15_c1), q15_quarter); + qx = sqrt_q15(t); + + t = MUL_Q15(clamp_q15(-Hx+My-Az+q15_c1), q15_quarter); + qy = sqrt_q15(t); + + t = MUL_Q15(clamp_q15(-Hx-My+Az+q15_c1), q15_quarter); + qz = sqrt_q15(t); + } +#endif + + qx = copysign_q15(qx, Ay - Mz); + qy = copysign_q15(qy, Hz - Ax); + qz = copysign_q15(qz, Mx - Hy); + + rot->x = qx; + rot->y = qy; + rot->z = qz; + rot->w = qw; +} diff --git a/algorithm/osp/rotvec.h b/algorithm/osp/rotvec.h new file mode 100644 index 0000000..ff114de --- /dev/null +++ b/algorithm/osp/rotvec.h @@ -0,0 +1,13 @@ +#ifndef _ROTVEC_H_ +#define _ROTVEC_H_ + +#include "fp_sensor.h" + +void OSP_rotvec_init(void); +#ifdef FEAT_ROTVEC_PRECISE +void OSP_rotvec_process(struct ThreeAxis *, struct ThreeAxis *, struct Quat_precise *); +#else +void OSP_rotvec_process(struct ThreeAxis *, struct ThreeAxis *, struct Quat *); +#endif + +#endif diff --git a/algorithm/osp/rotvec_precise.c b/algorithm/osp/rotvec_precise.c new file mode 100644 index 0000000..a6b8445 --- /dev/null +++ b/algorithm/osp/rotvec_precise.c @@ -0,0 +1,110 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Precise version of rotation vector using Q24's. + * Based on Android implementation. Converted to fixed point. + */ +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "rotvec.h" + +static Q24_t copysign_q24(Q24_t v, Q24_t s) +{ + if (s < 0) return -v; + else return v; +} + + +static Q24_t clamp_q24(Q24_t v) +{ + if (v < 0) return 0; + return v; +} + +void OSP_rotvec_init() +{ +} + +void OSP_rotvec_process( + struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Quat_precise *rot) +{ + Q24_t normH; + Q24_t magMag2, accMag2; + Q24_t invH, invA; + Q24_t Hx, Hy, Hz; + Q24_t Ax, Ay, Az; + Q24_t Mx, My, Mz; + Q24_t Ex, Ey, Ez; + + Q15_t qw, qx, qy, qz; + + Ax = q15_to_q24(acc->x); + Ay = q15_to_q24(acc->y); + Az = q15_to_q24(acc->z); + + Ex = q15_to_q24(mag->x); + Ey = q15_to_q24(mag->y); + Ez = q15_to_q24(mag->z); +#if 0 + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez)); +#endif + Hx = MUL_Q24(Ey, Az) - MUL_Q24(Ez, Ay); + Hy = MUL_Q24(Ez, Ax) - MUL_Q24(Ex, Az); + Hz = MUL_Q24(Ex, Ay) - MUL_Q24(Ey, Ax); + + magMag2 = MUL_Q24(Hx,Hx); + magMag2 += MUL_Q24(Hy, Hy); + magMag2 += MUL_Q24(Hz, Hz); + + normH = sqrt_q24(magMag2); + + if (normH < FP_to_Q24(0.1f)) { + printf("Bad mag? %f\n", Q24_to_FP(normH)); + return; + } + invH = RECIP_Q24(normH); + + accMag2 = MUL_Q24(Ax, Ax); + accMag2 += MUL_Q24(Ay, Ay); + accMag2 += MUL_Q24(Az, Az); + + invA = RECIP_Q24(sqrt_q24(accMag2)); + + Hx = MUL_Q24(Hx, invH); + Hy = MUL_Q24(Hy, invH); + Hz = MUL_Q24(Hz, invH); + + Ax = MUL_Q24(Ax, invA); + Ay = MUL_Q24(Ay, invA); + Az = MUL_Q24(Az, invA); + + Mx = MUL_Q24(Ay, Hz) - MUL_Q15(Az, Hy); + My = MUL_Q24(Az, Hx) - MUL_Q15(Ax, Hz); + Mz = MUL_Q24(Ax, Hy) - MUL_Q15(Ay, Hx); + + qw = sqrt_q24(MUL_Q24(clamp_q24( Hx+My+Az+q24_c1), q24_quarter)); + qx = sqrt_q24(MUL_Q24(clamp_q24( Hx-My-Az+q24_c1), q24_quarter)); + qy = sqrt_q24(MUL_Q24(clamp_q24(-Hx+My-Az+q24_c1), q24_quarter)); + qz = sqrt_q24(MUL_Q24(clamp_q24(-Hx-My+Az+q24_c1), q24_quarter)); + + qx = copysign_q24(qx, Ay - Mz); + qy = copysign_q24(qy, Hz - Ax); + qz = copysign_q24(qz, Mx - Hy); + + rot->x = qx; + rot->y = qy; + rot->z = qz; + rot->w = qw; +} diff --git a/algorithm/osp/sigmot.c b/algorithm/osp/sigmot.c new file mode 100644 index 0000000..8103618 --- /dev/null +++ b/algorithm/osp/sigmot.c @@ -0,0 +1,28 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Initial attempt at the significant motion sensor. + */ + + +#include "sigmot.h" + +enum { + SIGMOT_RESET, + SIGMOT_WAIT, +}; + +int state; + +void OSP_sigmot_init(void) +{ + state = SIGMOT_WAIT; +} + +void OSP_sigmot_process(struct ThreeAxis *acc, struct ThreeAxis *res) +{ + res->x = 0; +} diff --git a/algorithm/osp/sigmot.h b/algorithm/osp/sigmot.h new file mode 100644 index 0000000..b372e57 --- /dev/null +++ b/algorithm/osp/sigmot.h @@ -0,0 +1,8 @@ +#ifndef _SIGMOT_H_ +#define _SIGMOT_H_ +#include "fp_sensor.h" + +void OSP_sigmot_init(void); +void OSP_sigmot_process(struct ThreeAxis *, struct ThreeAxis *); + +#endif diff --git a/algorithm/osp/step.c b/algorithm/osp/step.c new file mode 100644 index 0000000..a0d7a01 --- /dev/null +++ b/algorithm/osp/step.c @@ -0,0 +1,99 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Adopted from alg from PebbleFit, Apache Licensed . + */ + +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "step.h" + +static const Q15_t q15_0_20c = FP_to_Q15(0.20f); +static Q15_t sens_Const = FP_to_Q15(200.0f); +static Q15_t threshold = 0; + +void OSP_step_init(void) +{ +} + +/* + * Det is number of steps detected. + * Count is count accumated count. On return: + * *count += det; + * Assumes a call back at a period of 0.020sec + */ +void OSP_step_process(struct ThreeAxis *acc, struct StepInfo *step) +{ + static int sampcnt = 0; /* Used for avaeraging */ + static Q15_t x = 0, y = 0, z = 0; + static Q15_t dAcc, sampleOld = 0, sampleNew = 0; + Q15_t min = FP_to_Q15(10000.0f), max = FP_to_Q15(-10000.0f); + static int lastStepSamp = 0; + static int sampcnt2 = 0; + static int regulation = 0; + + + /* Process groups of 5 samples */ + x += acc->x; y += acc->y; z+= acc->z; + sampcnt++; + sampcnt2++; + + step->detect = 0; + + if (sampcnt > 5) { + sampcnt = 0; + x = MUL_Q15(x, q15_0_20c); + y = MUL_Q15(y, q15_0_20c); + z = MUL_Q15(z, q15_0_20c); + dAcc = MUL_Q15(x,x) + MUL_Q15(y,y) + MUL_Q15(z,z); + dAcc = sqrt_q15(dAcc); + sampleOld = sampleNew; + if (abs_q15(dAcc - sampleNew) > sens_Const) + sampleNew = dAcc; + if ((sampleNew < threshold && + sampleOld > threshold) && + sampleOld != 0) { + /* Cadence sanity check: + * 0.2sec - 2sec between steps. + */ + if (lastStepSamp && + (sampcnt2 - lastStepSamp) > 9 && + (sampcnt2 - lastStepSamp) < 100) { + /* Require 5 consecutive steps for validity */ + regulation++; + + if (regulation == 5) { + step->detect = 1; + step->count += 5; + } else if (regulation > 5) { + step->detect = 1; + step->count += 1; + } + } else { + regulation = 0; + } + lastStepSamp = sampcnt2; + } + if (dAcc < min) min = dAcc; + if (dAcc > max) max = dAcc; + + threshold = (max+min) >> 1; + } + + /* Avoid wrap */ + if (sampcnt2 > (1 << 30)) { + if (lastStepSamp == 0) { + sampcnt2 -= (1 << 30); + } else if (lastStepSamp > 0 && lastStepSamp > (1<<30)) { + sampcnt2 -= (1 << 30); + lastStepSamp -= (1 << 30); + } else if ((sampcnt2 - lastStepSamp) > 1000) { + sampcnt2 -= (1 << 30); + lastStepSamp = 0; + } + } +} diff --git a/algorithm/osp/step.h b/algorithm/osp/step.h new file mode 100644 index 0000000..13d0558 --- /dev/null +++ b/algorithm/osp/step.h @@ -0,0 +1,6 @@ +#ifndef _STEP_H_ +#define _STEP_H_ + +void OSP_step_process(struct ThreeAxis *, struct StepInfo *); +void OSP_step_init(void); +#endif diff --git a/algorithm/osp/tilt.c b/algorithm/osp/tilt.c new file mode 100644 index 0000000..53a9335 --- /dev/null +++ b/algorithm/osp/tilt.c @@ -0,0 +1,112 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + +/* Compute a tilt sensor as defined by Android */ +#include "fpsup.h" +#include "tilt.h" + +#define NUM_SAMPLE2 50 +#define NUM_SAMPLE1 25 + +static struct ThreeAxis accHist[NUM_SAMPLE2]; +static struct ThreeAxis prevMean; +int sampcount; +int samp_head, samp_tail; + +#define ABS(x) ((x > 0)?x:-x) + +void OSP_tilt_init(void) +{ + sampcount = 0; + samp_head = 49, samp_tail = 0; +} + +static void computeMean(struct ThreeAxis *mean, struct ThreeAxis *buf, int beg, int end) +{ + int i, count; + + count = 0; + mean->x = 0; mean->y = 0; mean->z = 0; + for (i = beg; i != end; i++, i %= NUM_SAMPLE2) { + mean->x += buf[i].x; + mean->y += buf[i].y; + mean->z += buf[i].z; + count++; + } + mean->x = DIV_Q15(mean->x, INT_to_Q15(count)); + mean->y = DIV_Q15(mean->y, INT_to_Q15(count)); + mean->z = DIV_Q15(mean->z, INT_to_Q15(count)); +} + +static Q15_t dotProduct(struct ThreeAxis *v1, struct ThreeAxis *v2) +{ + Q15_t res; + + res = MUL_Q15(v1->x, v2->x); + res += MUL_Q15(v1->y, v2->y); + res += MUL_Q15(v1->z, v2->z); + + return res; +} + +static Q15_t norm(struct ThreeAxis *v1) +{ + Q15_t sum; + + sum = MUL_Q15(v1->x, v1->x); + sum += MUL_Q15(v1->y, v1->y); + sum += MUL_Q15(v1->z, v1->z); + return sqrt_q15(sum); +} + +static Q15_t computeAngle(struct ThreeAxis *v1, struct ThreeAxis *v2) +{ + Q15_t v, n; + + v = dotProduct(v1, v2); + n = MUL_Q15(norm(v1), norm(v2)); + v = DIV_Q15(v, n); + + return arccos_q15(v); +} + +/* Assumes the sensor runs at 50Hz */ +void OSP_tilt_process(struct ThreeAxis *acc, struct ThreeAxis *tilt) +{ + struct ThreeAxis cur; + int ang = 0; + + if (sampcount > 10000) { + tilt->x = 0; + return; + } + + + accHist[samp_head] = *acc; + + if (sampcount < (NUM_SAMPLE2-1)) { + if (sampcount == (NUM_SAMPLE1-1)) { + computeMean(&prevMean, accHist, 0, NUM_SAMPLE1); + } + sampcount++; + samp_head++; + samp_head %= NUM_SAMPLE2; + tilt->x = 0; + return; + } + + computeMean(&cur, accHist, samp_tail, samp_head); + samp_head++; samp_head %= NUM_SAMPLE2; + samp_tail++; samp_tail %= NUM_SAMPLE2; + ang = computeAngle(&cur, &prevMean); + if (ABS(ang) > INT_to_Q15(35)) { + tilt->x = 1; + sampcount = 99999; + } + return; +} diff --git a/algorithm/osp/tilt.h b/algorithm/osp/tilt.h new file mode 100644 index 0000000..36496a6 --- /dev/null +++ b/algorithm/osp/tilt.h @@ -0,0 +1,9 @@ +#ifndef _TILT_H_ +#define _TILT_H_ + +#include "fp_sensor.h" +void OSP_tilt_init(void); + +void OSP_tilt_process(struct ThreeAxis *acc, struct ThreeAxis *tilt); + +#endif diff --git a/algorithm/osp/trig_sin.c b/algorithm/osp/trig_sin.c new file mode 100644 index 0000000..b81a597 --- /dev/null +++ b/algorithm/osp/trig_sin.c @@ -0,0 +1,331 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Trig look up table. + */ + + + +static struct TRIG_SIN { + Q15_t sin; + Q15_t ang; +} trig_sin[] = { +{FP_to_Q15(0.00000), FP_to_Q15(0.00000)}, +{FP_to_Q15(0.00500), FP_to_Q15(0.00500)}, +{FP_to_Q15(0.01000), FP_to_Q15(0.01000)}, +{FP_to_Q15(0.01500), FP_to_Q15(0.01500)}, +{FP_to_Q15(0.02000), FP_to_Q15(0.02000)}, +{FP_to_Q15(0.02500), FP_to_Q15(0.02500)}, +{FP_to_Q15(0.03000), FP_to_Q15(0.03000)}, +{FP_to_Q15(0.03499), FP_to_Q15(0.03500)}, +{FP_to_Q15(0.03999), FP_to_Q15(0.04000)}, +{FP_to_Q15(0.04498), FP_to_Q15(0.04500)}, +{FP_to_Q15(0.04998), FP_to_Q15(0.05000)}, +{FP_to_Q15(0.05497), FP_to_Q15(0.05500)}, +{FP_to_Q15(0.05996), FP_to_Q15(0.06000)}, +{FP_to_Q15(0.06495), FP_to_Q15(0.06500)}, +{FP_to_Q15(0.06994), FP_to_Q15(0.07000)}, +{FP_to_Q15(0.07493), FP_to_Q15(0.07500)}, +{FP_to_Q15(0.07991), FP_to_Q15(0.08000)}, +{FP_to_Q15(0.08490), FP_to_Q15(0.08500)}, +{FP_to_Q15(0.08988), FP_to_Q15(0.09000)}, +{FP_to_Q15(0.09486), FP_to_Q15(0.09500)}, +{FP_to_Q15(0.09983), FP_to_Q15(0.10000)}, +{FP_to_Q15(0.10481), FP_to_Q15(0.10500)}, +{FP_to_Q15(0.10978), FP_to_Q15(0.11000)}, +{FP_to_Q15(0.11475), FP_to_Q15(0.11500)}, +{FP_to_Q15(0.11971), FP_to_Q15(0.12000)}, +{FP_to_Q15(0.12467), FP_to_Q15(0.12500)}, +{FP_to_Q15(0.12963), FP_to_Q15(0.13000)}, +{FP_to_Q15(0.13459), FP_to_Q15(0.13500)}, +{FP_to_Q15(0.13954), FP_to_Q15(0.14000)}, +{FP_to_Q15(0.14449), FP_to_Q15(0.14500)}, +{FP_to_Q15(0.14944), FP_to_Q15(0.15000)}, +{FP_to_Q15(0.15438), FP_to_Q15(0.15500)}, +{FP_to_Q15(0.15932), FP_to_Q15(0.16000)}, +{FP_to_Q15(0.16425), FP_to_Q15(0.16500)}, +{FP_to_Q15(0.16918), FP_to_Q15(0.17000)}, +{FP_to_Q15(0.17411), FP_to_Q15(0.17500)}, +{FP_to_Q15(0.17903), FP_to_Q15(0.18000)}, +{FP_to_Q15(0.18395), FP_to_Q15(0.18500)}, +{FP_to_Q15(0.18886), FP_to_Q15(0.19000)}, +{FP_to_Q15(0.19377), FP_to_Q15(0.19500)}, +{FP_to_Q15(0.19867), FP_to_Q15(0.20000)}, +{FP_to_Q15(0.20357), FP_to_Q15(0.20500)}, +{FP_to_Q15(0.20846), FP_to_Q15(0.21000)}, +{FP_to_Q15(0.21335), FP_to_Q15(0.21500)}, +{FP_to_Q15(0.21823), FP_to_Q15(0.22000)}, +{FP_to_Q15(0.22311), FP_to_Q15(0.22500)}, +{FP_to_Q15(0.22798), FP_to_Q15(0.23000)}, +{FP_to_Q15(0.23284), FP_to_Q15(0.23500)}, +{FP_to_Q15(0.23770), FP_to_Q15(0.24000)}, +{FP_to_Q15(0.24256), FP_to_Q15(0.24500)}, +{FP_to_Q15(0.24740), FP_to_Q15(0.25000)}, +{FP_to_Q15(0.25225), FP_to_Q15(0.25500)}, +{FP_to_Q15(0.25708), FP_to_Q15(0.26000)}, +{FP_to_Q15(0.26191), FP_to_Q15(0.26500)}, +{FP_to_Q15(0.26673), FP_to_Q15(0.27000)}, +{FP_to_Q15(0.27155), FP_to_Q15(0.27500)}, +{FP_to_Q15(0.27636), FP_to_Q15(0.28000)}, +{FP_to_Q15(0.28116), FP_to_Q15(0.28500)}, +{FP_to_Q15(0.28595), FP_to_Q15(0.29000)}, +{FP_to_Q15(0.29074), FP_to_Q15(0.29500)}, +{FP_to_Q15(0.29552), FP_to_Q15(0.30000)}, +{FP_to_Q15(0.30029), FP_to_Q15(0.30500)}, +{FP_to_Q15(0.30506), FP_to_Q15(0.31000)}, +{FP_to_Q15(0.30982), FP_to_Q15(0.31500)}, +{FP_to_Q15(0.31457), FP_to_Q15(0.32000)}, +{FP_to_Q15(0.31931), FP_to_Q15(0.32500)}, +{FP_to_Q15(0.32404), FP_to_Q15(0.33000)}, +{FP_to_Q15(0.32877), FP_to_Q15(0.33500)}, +{FP_to_Q15(0.33349), FP_to_Q15(0.34000)}, +{FP_to_Q15(0.33820), FP_to_Q15(0.34500)}, +{FP_to_Q15(0.34290), FP_to_Q15(0.35000)}, +{FP_to_Q15(0.34759), FP_to_Q15(0.35500)}, +{FP_to_Q15(0.35227), FP_to_Q15(0.36000)}, +{FP_to_Q15(0.35695), FP_to_Q15(0.36500)}, +{FP_to_Q15(0.36162), FP_to_Q15(0.37000)}, +{FP_to_Q15(0.36627), FP_to_Q15(0.37500)}, +{FP_to_Q15(0.37092), FP_to_Q15(0.38000)}, +{FP_to_Q15(0.37556), FP_to_Q15(0.38500)}, +{FP_to_Q15(0.38019), FP_to_Q15(0.39000)}, +{FP_to_Q15(0.38481), FP_to_Q15(0.39500)}, +{FP_to_Q15(0.38942), FP_to_Q15(0.40000)}, +{FP_to_Q15(0.39402), FP_to_Q15(0.40500)}, +{FP_to_Q15(0.39861), FP_to_Q15(0.41000)}, +{FP_to_Q15(0.40319), FP_to_Q15(0.41500)}, +{FP_to_Q15(0.40776), FP_to_Q15(0.42000)}, +{FP_to_Q15(0.41232), FP_to_Q15(0.42500)}, +{FP_to_Q15(0.41687), FP_to_Q15(0.43000)}, +{FP_to_Q15(0.42141), FP_to_Q15(0.43500)}, +{FP_to_Q15(0.42594), FP_to_Q15(0.44000)}, +{FP_to_Q15(0.43046), FP_to_Q15(0.44500)}, +{FP_to_Q15(0.43497), FP_to_Q15(0.45000)}, +{FP_to_Q15(0.43946), FP_to_Q15(0.45500)}, +{FP_to_Q15(0.44395), FP_to_Q15(0.46000)}, +{FP_to_Q15(0.44842), FP_to_Q15(0.46500)}, +{FP_to_Q15(0.45289), FP_to_Q15(0.47000)}, +{FP_to_Q15(0.45734), FP_to_Q15(0.47500)}, +{FP_to_Q15(0.46178), FP_to_Q15(0.48000)}, +{FP_to_Q15(0.46621), FP_to_Q15(0.48500)}, +{FP_to_Q15(0.47063), FP_to_Q15(0.49000)}, +{FP_to_Q15(0.47503), FP_to_Q15(0.49500)}, +{FP_to_Q15(0.47943), FP_to_Q15(0.50000)}, +{FP_to_Q15(0.48381), FP_to_Q15(0.50500)}, +{FP_to_Q15(0.48818), FP_to_Q15(0.51000)}, +{FP_to_Q15(0.49253), FP_to_Q15(0.51500)}, +{FP_to_Q15(0.49688), FP_to_Q15(0.52000)}, +{FP_to_Q15(0.50121), FP_to_Q15(0.52500)}, +{FP_to_Q15(0.50553), FP_to_Q15(0.53000)}, +{FP_to_Q15(0.50984), FP_to_Q15(0.53500)}, +{FP_to_Q15(0.51414), FP_to_Q15(0.54000)}, +{FP_to_Q15(0.51842), FP_to_Q15(0.54500)}, +{FP_to_Q15(0.52269), FP_to_Q15(0.55000)}, +{FP_to_Q15(0.52694), FP_to_Q15(0.55500)}, +{FP_to_Q15(0.53119), FP_to_Q15(0.56000)}, +{FP_to_Q15(0.53542), FP_to_Q15(0.56500)}, +{FP_to_Q15(0.53963), FP_to_Q15(0.57000)}, +{FP_to_Q15(0.54383), FP_to_Q15(0.57500)}, +{FP_to_Q15(0.54802), FP_to_Q15(0.58000)}, +{FP_to_Q15(0.55220), FP_to_Q15(0.58500)}, +{FP_to_Q15(0.55636), FP_to_Q15(0.59000)}, +{FP_to_Q15(0.56051), FP_to_Q15(0.59500)}, +{FP_to_Q15(0.56464), FP_to_Q15(0.60000)}, +{FP_to_Q15(0.56876), FP_to_Q15(0.60500)}, +{FP_to_Q15(0.57287), FP_to_Q15(0.61000)}, +{FP_to_Q15(0.57696), FP_to_Q15(0.61500)}, +{FP_to_Q15(0.58104), FP_to_Q15(0.62000)}, +{FP_to_Q15(0.58510), FP_to_Q15(0.62500)}, +{FP_to_Q15(0.58914), FP_to_Q15(0.63000)}, +{FP_to_Q15(0.59318), FP_to_Q15(0.63500)}, +{FP_to_Q15(0.59720), FP_to_Q15(0.64000)}, +{FP_to_Q15(0.60120), FP_to_Q15(0.64500)}, +{FP_to_Q15(0.60519), FP_to_Q15(0.65000)}, +{FP_to_Q15(0.60916), FP_to_Q15(0.65500)}, +{FP_to_Q15(0.61312), FP_to_Q15(0.66000)}, +{FP_to_Q15(0.61706), FP_to_Q15(0.66500)}, +{FP_to_Q15(0.62099), FP_to_Q15(0.67000)}, +{FP_to_Q15(0.62490), FP_to_Q15(0.67500)}, +{FP_to_Q15(0.62879), FP_to_Q15(0.68000)}, +{FP_to_Q15(0.63267), FP_to_Q15(0.68500)}, +{FP_to_Q15(0.63654), FP_to_Q15(0.69000)}, +{FP_to_Q15(0.64039), FP_to_Q15(0.69500)}, +{FP_to_Q15(0.64422), FP_to_Q15(0.70000)}, +{FP_to_Q15(0.64803), FP_to_Q15(0.70500)}, +{FP_to_Q15(0.65183), FP_to_Q15(0.71000)}, +{FP_to_Q15(0.65562), FP_to_Q15(0.71500)}, +{FP_to_Q15(0.65938), FP_to_Q15(0.72000)}, +{FP_to_Q15(0.66314), FP_to_Q15(0.72500)}, +{FP_to_Q15(0.66687), FP_to_Q15(0.73000)}, +{FP_to_Q15(0.67059), FP_to_Q15(0.73500)}, +{FP_to_Q15(0.67429), FP_to_Q15(0.74000)}, +{FP_to_Q15(0.67797), FP_to_Q15(0.74500)}, +{FP_to_Q15(0.68164), FP_to_Q15(0.75000)}, +{FP_to_Q15(0.68529), FP_to_Q15(0.75500)}, +{FP_to_Q15(0.68892), FP_to_Q15(0.76000)}, +{FP_to_Q15(0.69254), FP_to_Q15(0.76500)}, +{FP_to_Q15(0.69614), FP_to_Q15(0.77000)}, +{FP_to_Q15(0.69972), FP_to_Q15(0.77500)}, +{FP_to_Q15(0.70328), FP_to_Q15(0.78000)}, +{FP_to_Q15(0.70683), FP_to_Q15(0.78500)}, +{FP_to_Q15(0.71035), FP_to_Q15(0.79000)}, +{FP_to_Q15(0.71386), FP_to_Q15(0.79500)}, +{FP_to_Q15(0.71736), FP_to_Q15(0.80000)}, +{FP_to_Q15(0.72083), FP_to_Q15(0.80500)}, +{FP_to_Q15(0.72429), FP_to_Q15(0.81000)}, +{FP_to_Q15(0.72773), FP_to_Q15(0.81500)}, +{FP_to_Q15(0.73115), FP_to_Q15(0.82000)}, +{FP_to_Q15(0.73455), FP_to_Q15(0.82500)}, +{FP_to_Q15(0.73793), FP_to_Q15(0.83000)}, +{FP_to_Q15(0.74130), FP_to_Q15(0.83500)}, +{FP_to_Q15(0.74464), FP_to_Q15(0.84000)}, +{FP_to_Q15(0.74797), FP_to_Q15(0.84500)}, +{FP_to_Q15(0.75128), FP_to_Q15(0.85000)}, +{FP_to_Q15(0.75457), FP_to_Q15(0.85500)}, +{FP_to_Q15(0.75784), FP_to_Q15(0.86000)}, +{FP_to_Q15(0.76110), FP_to_Q15(0.86500)}, +{FP_to_Q15(0.76433), FP_to_Q15(0.87000)}, +{FP_to_Q15(0.76754), FP_to_Q15(0.87500)}, +{FP_to_Q15(0.77074), FP_to_Q15(0.88000)}, +{FP_to_Q15(0.77391), FP_to_Q15(0.88500)}, +{FP_to_Q15(0.77707), FP_to_Q15(0.89000)}, +{FP_to_Q15(0.78021), FP_to_Q15(0.89500)}, +{FP_to_Q15(0.78333), FP_to_Q15(0.90000)}, +{FP_to_Q15(0.78643), FP_to_Q15(0.90500)}, +{FP_to_Q15(0.78950), FP_to_Q15(0.91000)}, +{FP_to_Q15(0.79256), FP_to_Q15(0.91500)}, +{FP_to_Q15(0.79560), FP_to_Q15(0.92000)}, +{FP_to_Q15(0.79862), FP_to_Q15(0.92500)}, +{FP_to_Q15(0.80162), FP_to_Q15(0.93000)}, +{FP_to_Q15(0.80460), FP_to_Q15(0.93500)}, +{FP_to_Q15(0.80756), FP_to_Q15(0.94000)}, +{FP_to_Q15(0.81050), FP_to_Q15(0.94500)}, +{FP_to_Q15(0.81342), FP_to_Q15(0.95000)}, +{FP_to_Q15(0.81631), FP_to_Q15(0.95500)}, +{FP_to_Q15(0.81919), FP_to_Q15(0.96000)}, +{FP_to_Q15(0.82205), FP_to_Q15(0.96500)}, +{FP_to_Q15(0.82489), FP_to_Q15(0.97000)}, +{FP_to_Q15(0.82770), FP_to_Q15(0.97500)}, +{FP_to_Q15(0.83050), FP_to_Q15(0.98000)}, +{FP_to_Q15(0.83327), FP_to_Q15(0.98500)}, +{FP_to_Q15(0.83603), FP_to_Q15(0.99000)}, +{FP_to_Q15(0.83876), FP_to_Q15(0.99500)}, +{FP_to_Q15(0.84147), FP_to_Q15(1.00000)}, +{FP_to_Q15(0.84416), FP_to_Q15(1.00500)}, +{FP_to_Q15(0.84683), FP_to_Q15(1.01000)}, +{FP_to_Q15(0.84948), FP_to_Q15(1.01500)}, +{FP_to_Q15(0.85211), FP_to_Q15(1.02000)}, +{FP_to_Q15(0.85471), FP_to_Q15(1.02500)}, +{FP_to_Q15(0.85730), FP_to_Q15(1.03000)}, +{FP_to_Q15(0.85986), FP_to_Q15(1.03500)}, +{FP_to_Q15(0.86240), FP_to_Q15(1.04000)}, +{FP_to_Q15(0.86492), FP_to_Q15(1.04500)}, +{FP_to_Q15(0.86742), FP_to_Q15(1.05000)}, +{FP_to_Q15(0.86990), FP_to_Q15(1.05500)}, +{FP_to_Q15(0.87236), FP_to_Q15(1.06000)}, +{FP_to_Q15(0.87479), FP_to_Q15(1.06500)}, +{FP_to_Q15(0.87720), FP_to_Q15(1.07000)}, +{FP_to_Q15(0.87959), FP_to_Q15(1.07500)}, +{FP_to_Q15(0.88196), FP_to_Q15(1.08000)}, +{FP_to_Q15(0.88430), FP_to_Q15(1.08500)}, +{FP_to_Q15(0.88663), FP_to_Q15(1.09000)}, +{FP_to_Q15(0.88893), FP_to_Q15(1.09500)}, +{FP_to_Q15(0.89121), FP_to_Q15(1.10000)}, +{FP_to_Q15(0.89346), FP_to_Q15(1.10500)}, +{FP_to_Q15(0.89570), FP_to_Q15(1.11000)}, +{FP_to_Q15(0.89791), FP_to_Q15(1.11500)}, +{FP_to_Q15(0.90010), FP_to_Q15(1.12000)}, +{FP_to_Q15(0.90227), FP_to_Q15(1.12500)}, +{FP_to_Q15(0.90441), FP_to_Q15(1.13000)}, +{FP_to_Q15(0.90653), FP_to_Q15(1.13500)}, +{FP_to_Q15(0.90863), FP_to_Q15(1.14000)}, +{FP_to_Q15(0.91071), FP_to_Q15(1.14500)}, +{FP_to_Q15(0.91276), FP_to_Q15(1.15000)}, +{FP_to_Q15(0.91479), FP_to_Q15(1.15500)}, +{FP_to_Q15(0.91680), FP_to_Q15(1.16000)}, +{FP_to_Q15(0.91879), FP_to_Q15(1.16500)}, +{FP_to_Q15(0.92075), FP_to_Q15(1.17000)}, +{FP_to_Q15(0.92269), FP_to_Q15(1.17500)}, +{FP_to_Q15(0.92461), FP_to_Q15(1.18000)}, +{FP_to_Q15(0.92650), FP_to_Q15(1.18500)}, +{FP_to_Q15(0.92837), FP_to_Q15(1.19000)}, +{FP_to_Q15(0.93022), FP_to_Q15(1.19500)}, +{FP_to_Q15(0.93204), FP_to_Q15(1.20000)}, +{FP_to_Q15(0.93384), FP_to_Q15(1.20500)}, +{FP_to_Q15(0.93562), FP_to_Q15(1.21000)}, +{FP_to_Q15(0.93737), FP_to_Q15(1.21500)}, +{FP_to_Q15(0.93910), FP_to_Q15(1.22000)}, +{FP_to_Q15(0.94081), FP_to_Q15(1.22500)}, +{FP_to_Q15(0.94249), FP_to_Q15(1.23000)}, +{FP_to_Q15(0.94415), FP_to_Q15(1.23500)}, +{FP_to_Q15(0.94578), FP_to_Q15(1.24000)}, +{FP_to_Q15(0.94740), FP_to_Q15(1.24500)}, +{FP_to_Q15(0.94898), FP_to_Q15(1.25000)}, +{FP_to_Q15(0.95055), FP_to_Q15(1.25500)}, +{FP_to_Q15(0.95209), FP_to_Q15(1.26000)}, +{FP_to_Q15(0.95361), FP_to_Q15(1.26500)}, +{FP_to_Q15(0.95510), FP_to_Q15(1.27000)}, +{FP_to_Q15(0.95657), FP_to_Q15(1.27500)}, +{FP_to_Q15(0.95802), FP_to_Q15(1.28000)}, +{FP_to_Q15(0.95944), FP_to_Q15(1.28500)}, +{FP_to_Q15(0.96084), FP_to_Q15(1.29000)}, +{FP_to_Q15(0.96221), FP_to_Q15(1.29500)}, +{FP_to_Q15(0.96356), FP_to_Q15(1.30000)}, +{FP_to_Q15(0.96488), FP_to_Q15(1.30500)}, +{FP_to_Q15(0.96618), FP_to_Q15(1.31000)}, +{FP_to_Q15(0.96746), FP_to_Q15(1.31500)}, +{FP_to_Q15(0.96872), FP_to_Q15(1.32000)}, +{FP_to_Q15(0.96994), FP_to_Q15(1.32500)}, +{FP_to_Q15(0.97115), FP_to_Q15(1.33000)}, +{FP_to_Q15(0.97233), FP_to_Q15(1.33500)}, +{FP_to_Q15(0.97348), FP_to_Q15(1.34000)}, +{FP_to_Q15(0.97462), FP_to_Q15(1.34500)}, +{FP_to_Q15(0.97572), FP_to_Q15(1.35000)}, +{FP_to_Q15(0.97681), FP_to_Q15(1.35500)}, +{FP_to_Q15(0.97786), FP_to_Q15(1.36000)}, +{FP_to_Q15(0.97890), FP_to_Q15(1.36500)}, +{FP_to_Q15(0.97991), FP_to_Q15(1.37000)}, +{FP_to_Q15(0.98089), FP_to_Q15(1.37500)}, +{FP_to_Q15(0.98185), FP_to_Q15(1.38000)}, +{FP_to_Q15(0.98279), FP_to_Q15(1.38500)}, +{FP_to_Q15(0.98370), FP_to_Q15(1.39000)}, +{FP_to_Q15(0.98459), FP_to_Q15(1.39500)}, +{FP_to_Q15(0.98545), FP_to_Q15(1.40000)}, +{FP_to_Q15(0.98629), FP_to_Q15(1.40500)}, +{FP_to_Q15(0.98710), FP_to_Q15(1.41000)}, +{FP_to_Q15(0.98789), FP_to_Q15(1.41500)}, +{FP_to_Q15(0.98865), FP_to_Q15(1.42000)}, +{FP_to_Q15(0.98939), FP_to_Q15(1.42500)}, +{FP_to_Q15(0.99010), FP_to_Q15(1.43000)}, +{FP_to_Q15(0.99079), FP_to_Q15(1.43500)}, +{FP_to_Q15(0.99146), FP_to_Q15(1.44000)}, +{FP_to_Q15(0.99210), FP_to_Q15(1.44500)}, +{FP_to_Q15(0.99271), FP_to_Q15(1.45000)}, +{FP_to_Q15(0.99330), FP_to_Q15(1.45500)}, +{FP_to_Q15(0.99387), FP_to_Q15(1.46000)}, +{FP_to_Q15(0.99441), FP_to_Q15(1.46500)}, +{FP_to_Q15(0.99492), FP_to_Q15(1.47000)}, +{FP_to_Q15(0.99542), FP_to_Q15(1.47500)}, +{FP_to_Q15(0.99588), FP_to_Q15(1.48000)}, +{FP_to_Q15(0.99632), FP_to_Q15(1.48500)}, +{FP_to_Q15(0.99674), FP_to_Q15(1.49000)}, +{FP_to_Q15(0.99713), FP_to_Q15(1.49500)}, +{FP_to_Q15(0.99749), FP_to_Q15(1.50000)}, +{FP_to_Q15(0.99784), FP_to_Q15(1.50500)}, +{FP_to_Q15(0.99815), FP_to_Q15(1.51000)}, +{FP_to_Q15(0.99844), FP_to_Q15(1.51500)}, +{FP_to_Q15(0.99871), FP_to_Q15(1.52000)}, +{FP_to_Q15(0.99895), FP_to_Q15(1.52500)}, +{FP_to_Q15(0.99917), FP_to_Q15(1.53000)}, +{FP_to_Q15(0.99936), FP_to_Q15(1.53500)}, +{FP_to_Q15(0.99953), FP_to_Q15(1.54000)}, +{FP_to_Q15(0.99967), FP_to_Q15(1.54500)}, +{FP_to_Q15(0.99978), FP_to_Q15(1.55000)}, +{FP_to_Q15(0.99988), FP_to_Q15(1.55500)}, +{FP_to_Q15(0.99994), FP_to_Q15(1.56000)}, +{FP_to_Q15(0.99998), FP_to_Q15(1.56500)}, +{FP_to_Q15(1.00000), FP_to_Q15(1.57000)}, +}; diff --git a/include/linux/iio/events.h b/include/linux/iio/events.h new file mode 100644 index 0000000..c25f0e3 --- /dev/null +++ b/include/linux/iio/events.h @@ -0,0 +1,105 @@ +/* The industrial I/O - event passing to userspace + * + * Copyright (c) 2008-2011 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ +#ifndef _IIO_EVENTS_H_ +#define _IIO_EVENTS_H_ + +#include +#include +#include "types.h" + +/** + * struct iio_event_data - The actual event being pushed to userspace + * @id: event identifier + * @timestamp: best estimate of time of event occurrence (often from + * the interrupt handler) + */ +struct iio_event_data { + __u64 id; + __s64 timestamp; +}; + +#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int) + +enum iio_event_type { + IIO_EV_TYPE_THRESH, + IIO_EV_TYPE_MAG, + IIO_EV_TYPE_ROC, + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, +}; + +enum iio_event_direction { + IIO_EV_DIR_EITHER, + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, +}; + +/** + * IIO_EVENT_CODE() - create event identifier + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @diff: Whether the event is for an differential channel or not. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @direction: Direction of the event. One of enum iio_event_direction. + * @type: Type of the event. Should be one enum iio_event_type. + * @chan: Channel number for non-differential channels. + * @chan1: First channel number for differential channels. + * @chan2: Second channel number for differential channels. + */ + +#define IIO_EVENT_CODE(chan_type, diff, modifier, direction, \ + type, chan, chan1, chan2) \ + (((u64)type << 56) | ((u64)diff << 55) | \ + ((u64)direction << 48) | ((u64)modifier << 40) | \ + ((u64)chan_type << 32) | (((u16)chan2) << 16) | ((u16)chan1) | \ + ((u16)chan)) + + +#define IIO_EV_DIR_MAX 4 +#define IIO_EV_BIT(type, direction) \ + (1 << (type*IIO_EV_DIR_MAX + direction)) + +/** + * IIO_MOD_EVENT_CODE() - create event identifier for modified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @type: Type of the event. Should be one enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_MOD_EVENT_CODE(chan_type, number, modifier, \ + type, direction) \ + IIO_EVENT_CODE(chan_type, 0, modifier, direction, type, number, 0, 0) + +/** + * IIO_UNMOD_EVENT_CODE() - create event identifier for unmodified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @type: Type of the event. Should be one enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_UNMOD_EVENT_CODE(chan_type, number, type, direction) \ + IIO_EVENT_CODE(chan_type, 0, 0, direction, type, number, 0, 0) + +#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF) + +#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF) + +#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF) + +/* Event code number extraction depends on which type of event we have. + * Perhaps review this function in the future*/ +#define IIO_EVENT_CODE_EXTRACT_CHAN(mask) ((__s16)(mask & 0xFFFF)) +#define IIO_EVENT_CODE_EXTRACT_CHAN2(mask) ((__s16)(((mask) >> 16) & 0xFFFF)) + +#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF) +#define IIO_EVENT_CODE_EXTRACT_DIFF(mask) (((mask) >> 55) & 0x1) + +#endif diff --git a/include/linux/iio/types.h b/include/linux/iio/types.h new file mode 100644 index 0000000..7873357 --- /dev/null +++ b/include/linux/iio/types.h @@ -0,0 +1,56 @@ +/* industrial I/O data types needed both in and out of kernel + * + * Copyright (c) 2008 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#ifndef _IIO_TYPES_H_ +#define _IIO_TYPES_H_ + +enum iio_chan_type { + /* real channel types */ + IIO_VOLTAGE, + IIO_CURRENT, + IIO_POWER, + IIO_ACCEL, + IIO_ANGL_VEL, + IIO_MAGN, + IIO_LIGHT, + IIO_INTENSITY, + IIO_PROXIMITY, + IIO_TEMP, + IIO_INCLI, + IIO_ROT, + IIO_ANGL, + IIO_TIMESTAMP, + IIO_CAPACITANCE, + IIO_PRESSURE, + IIO_QUATERNION, +}; + +enum iio_modifier { + IIO_NO_MOD, + IIO_MOD_X, + IIO_MOD_Y, + IIO_MOD_Z, + IIO_MOD_X_AND_Y, + IIO_MOD_X_AND_Z, + IIO_MOD_Y_AND_Z, + IIO_MOD_X_AND_Y_AND_Z, + IIO_MOD_X_OR_Y, + IIO_MOD_X_OR_Z, + IIO_MOD_Y_OR_Z, + IIO_MOD_X_OR_Y_OR_Z, + IIO_MOD_LIGHT_BOTH, + IIO_MOD_LIGHT_IR, + IIO_MOD_R, +}; + +#define IIO_VAL_INT 1 +#define IIO_VAL_INT_PLUS_MICRO 2 +#define IIO_VAL_INT_PLUS_NANO 3 + +#endif /* _IIO_TYPES_H_ */ diff --git a/include/osp-api.h b/include/osp-api.h index 228b7f5..fa2ebe7 100644 --- a/include/osp-api.h +++ b/include/osp-api.h @@ -199,8 +199,15 @@ typedef struct { //! Union structure to encapsulate various data types to library algorithm. typedef union { - Android_TriAxisRawSensorData_t data; - // Add any future input data type here as needed. + Android_TriAxisRawSensorData_t rawdata; + Android_TriAxisPreciseData_t q24data; + Android_TriAxisExtendedData_t q12data; + Android_UncalibratedTriAxisPreciseData_t uncal_q24data; + Android_UncalibratedTriAxisExtendedData_t uncal_q12data; + Android_OrientationResultData_t orientdata; + Android_BooleanResultData_t booldata; + Android_StepCounterResultData_t stepcount; + Android_RotationVectorResultData_t rotvec; }OSP_InputSensorData_t; //! callback type used when the library needs to do an atomic operation @@ -293,8 +300,8 @@ typedef void (* OSP_ResultReadyCallback_t)(ResultHandle_t ResultHandle, void* p */ typedef struct { - InputSensor_t SensorType; //!< physical sensor accel, gyro, etc input to fusion algorithm - char* SensorName; //!< short human readable description, Null terminated + ASensorType_t SensorType; //!< sensor type from either the ASensorType_t or PSensorType_t list + char const * SensorName; //!< short human readable description, Null terminated uint32_t DataWidthMask; //!< how much of the data word that is sent significant AxisMapType_t AxisMapping[3]; //!< e.g. use to switch from left handed sensor to right handed system or swap X and Y axes of a two axis sensor int32_t ConversionOffset[3]; //!< an offset for X,Y & Z to compensate for sensor origin (0,0,0) @@ -402,7 +409,7 @@ OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t* pSystemDesc); * * \return status as specified in OSP_Types.h */ -OSP_STATUS_t OSP_RegisterInputSensor(SensorDescriptor_t *pSensorDescriptor, +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *pSensorDescriptor, InputSensorHandle_t *pReturnedHandle); //! use to change a sensors operating mode (output rate, position, etc...) diff --git a/include/osp-sensors.h b/include/osp-sensors.h index d9ea94c..f8e5111 100644 --- a/include/osp-sensors.h +++ b/include/osp-sensors.h @@ -146,23 +146,6 @@ typedef enum _SensorParamId { } SensorParamId_t; -/* Enumeration for the input sensors. */ -typedef enum _InputSensor -{ - ACCEL_INPUT_SENSOR, - MAG_INPUT_SENSOR, - GYRO_INPUT_SENSOR, - LIGHT_INPUT_SENSOR, - PRESSURE_INPUT_SENSOR, - TEMPERATURE_INPUT_SENSOR, - PROXIMITY_INPUT_SENSOR, - RELATIVE_HUMIDITY_INPUT_SENSOR, - AMBIENT_TEMPERATURE_INPUT_SENSOR, - - NUM_INPUT_SENSORS, - UNKNOWN_INPUT_SENSOR = 0xFF -} InputSensor_t; - // TODO: Need more description how these are use. //! Segment detector subtypes typedef enum _SegmentSubType{ diff --git a/linux/daemon/Makefile b/linux/daemon/Makefile new file mode 100644 index 0000000..5b023be --- /dev/null +++ b/linux/daemon/Makefile @@ -0,0 +1,17 @@ +ARMCC=arm-none-linux-gnueabi-gcc +CC=gcc +CFLAGS=-Wall -g -I../../include +LDFLAGS=-Wall -g -static +LIBS=-L../../algorithm/osp -lOSP + + +OBJS=OSPDaemon_queue.o OSPDaemon.o OSPDaemon_iio.o OSPDaemon_config.o \ + OSPDaemon_input.o OSPDaemon_inputreader.o \ + OSPDaemon_filecsv.o \ + OSPDaemon_pm.o OSPDaemon_driver.o + +OSPDaemon: $(OBJS) + $(CC) $(LDFLAGS) $(OBJS) -o OSPDaemon $(LIBS) + +clean: + rm -f *.o OSPDaemon diff --git a/linux/daemon/OSPDaemon.c b/linux/daemon/OSPDaemon.c new file mode 100644 index 0000000..cd39cbc --- /dev/null +++ b/linux/daemon/OSPDaemon.c @@ -0,0 +1,601 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +/* + * TODO: + * - Handle cal data. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_pm.h" +#include "OSPDaemon_driver.h" + +#include "osp-api.h" + + +unsigned int debug_level = 0; +unsigned int mainstatus = 0; +unsigned int disablepm = 0; + +static struct OSPDaemon_SensorDescription *sd; +static struct pollfd pfd[NUM_ANDROID_SENSOR_TYPE+NUM_PRIVATE_SENSOR_TYPE]; +static int nfd = NUM_ANDROID_SENSOR_TYPE+NUM_PRIVATE_SENSOR_TYPE; +static int ignorecal = 0; + +static OSP_STATUS_t OSPDaemon_SensorControl_cb(SensorControl_t *cmd); + +static const SystemDescriptor_t SystemDesc = { + .TstampConversionToSeconds = 1, + .EnterCritical = NULL, + .ExitCritical = NULL, + .SensorsControl = OSPDaemon_SensorControl_cb, +}; + + +static OSP_STATUS_t OSPDaemon_SensorControl_cb(SensorControl_t *cmd) +{ + int i; + + for (i = 0; i sensor_count; i++) { + if (sd->sensor[i].handle == cmd->Handle) + break; + } + + if (i == sd->sensor_count) + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + switch (cmd->Command) { + case SENSOR_CONTROL_SENSOR_OFF: + OSPDaemon_driver_disable_in(&sd->sensor[i]); + return OSP_STATUS_OK; + case SENSOR_CONTROL_SENSOR_ON: + OSPDaemon_driver_enable_in(&sd->sensor[i]); + return OSP_STATUS_OK; + default: + return OSP_STATUS_UNSPECIFIED_ERROR; + } +} + +static unsigned char caldata[MAX_CALSIZE]; + +static void ReadCalData(char *name) +{ + int fd, ret; + int i; + struct OSPDaemon_CalRecord rec; + + if (ignorecal) return; + + fd = open(name, O_RDONLY); + if (fd < 0) return; + + do { + ret = read(fd, &rec, sizeof(rec)); + if (ret < sizeof(rec)) { + close(fd); + return; + } + if (rec.len > 1) { + ret = read(fd, caldata+1, rec.len-1); + if (ret != rec.len-1) { + close(fd); + return; + } + caldata[0] = rec.data[0]; + } + if (rec.len > MAX_CALSIZE) + continue; + /* Dispatch cal data - store in pCalibrationData */ + for (i = 0; i < sd->sensor_count; i++) { + if (sd->sensor[i].sensor.SensorType == rec.sensor) + break; + } + if (i != sd->sensor_count) { + memcpy(sd->sensor[i].caldata, caldata, rec.len); + sd->sensor[i].sensor.pCalibrationData = sd->sensor[i].caldata; + } + + } while(1); +} + +static void updateCalFile(char *calfile, ASensorType_t s, + void *data, uint32_t sz) +{ + char tmpfile[PATH_MAX+1]; + int oldfd, newfd; + int ret; + struct OSPDaemon_CalRecord rec; + unsigned char *cdata; + + snprintf(tmpfile, PATH_MAX, "%s.update", calfile); + tmpfile[PATH_MAX] = '\0'; + unlink(tmpfile); + + oldfd = open(calfile, O_RDONLY); + if (oldfd < 0) return; + + newfd = open(tmpfile, O_WRONLY | O_CREAT, 0777); + if (newfd < 0) { + close(oldfd); + return; + } + + do { + ret = read(oldfd, &rec, sizeof(rec)); + if (ret == 0) { /* EOF */ + break; + } + if (ret < sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + if (rec.len > MAX_CALSIZE) + break; + if (rec.len > 1) { + ret = read(oldfd, caldata+1, rec.len-1); + if (ret != rec.len-1) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + caldata[0] = rec.data[0]; + } + if (rec.sensor == s) { + cdata = data; + rec.len = sz; + rec.data[0] = cdata[0]; + } else { + cdata = caldata; + } + ret = write(newfd, &rec, sizeof(rec)); + if (ret != sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + if (rec.len > 1) { + ret = write(newfd, cdata + 1, rec.len - 1); + if (ret != sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + } + } while(1); + + close(newfd); + close(oldfd); + unlink(calfile); + rename(tmpfile, calfile); +} + +static void OSPDaemon_write_cal_cb(InputSensorHandle_t handle, + void *data, uint32_t sz, NTTIME ts) +{ + int i; + + for (i = 0; i < sd->sensor_count; i++) { + if (handle == sd->sensor[i].handle) { + break; + } + } + if (i == sd->sensor_count) { + fprintf(stderr, "Write call back with unknown handle\n"); + return; + } + if (sz > MAX_CALSIZE) return; + if (sd->CalFile) + updateCalFile(sd->CalFile, sd->sensor[i].sensor.SensorType, + data, sz); +} + +/* + * Generic alg data output call back. + * Looks up the queue assocaited with the sensor and + * places data on the queue. + */ +static void OSPDaemon_callback(ResultHandle_t handle, void *pData) +{ + int i; + struct OSPDaemon_output *out; + + for (i = 0; i < sd->output_count; i++) { + if (handle == sd->output[i].handle) + break; + } + if (i == sd->output_count) return; + out = &sd->output[i]; + + OSPDaemon_queue_put(&out->q, pData); + for (i = 0; i < nfd; i++) { + if (pfd[i].fd == out->fd) { + pfd[i].events = POLLOUT; + break; + } + } +} + +/* Apply modifications to the sensor data. Used with + * config file options to modify copied sensor data. + * Return: 0 - no mod, 1 - mod */ +static int modifyOutput(struct OSPDaemon_output *out, + OSP_InputSensorData_t *orig, + OSP_InputSensorData_t *mod) +{ + int val[5]; + int count; + int i; + unsigned long long ts; + union { + int i; + float f; + + } conv; + + if (out->format == OUT_FORMAT_INTEGER) + return 0; + count = extractOSP(out->ResultDesc.SensorType, orig, &ts, val); + switch (out->format) { + case OUT_FORMAT_FLOAT: + for (i = 0; i < count; i++) { + conv.f = val[i]; + val[i] = conv.i; + } + break; + case OUT_FORMAT_Q24: + for (i = 0; i < count; i++) { + val[i] = val[i] << 24; + } + break; + case OUT_FORMAT_Q16: + for (i = 0; i < count; i++) { + val[i] = val[i] << 16; + } + break; + case OUT_FORMAT_Q15: + for (i = 0; i < count; i++) { + val[i] = val[i] << 15; + } + break; + case OUT_FORMAT_Q12: + for (i = 0; i < count; i++) { + val[i] = val[i] << 12; + } + break; + default: + break; + + } + conv2OSP(out->ResultDesc.SensorType, mod, ts, val); + return 1; +} +/* + * Processes received data: + * If the sensor is configured to be tee'd, queue it in the output sensor. + * Send data to the alg. + */ +static int OSPDaemon_senddata(struct OSPDaemon_SensorDetail *s) +{ + OSP_InputSensorData_t *od; + OSP_InputSensorData_t mod; + OSP_STATUS_t stat; + int ret = 0; + + if (!s->noprocess && s->pending) { + od = &s->pdata; + stat = OSP_SetInputData(s->handle, od); + if (stat != OSP_STATUS_OK) return 1; + s->pending = 0; + } + + do { + od = OSPDaemon_queue_get(&s->q); + if (od == NULL) break; + + if (s->output) { + if (disablepm || s->output->enable) { + if (modifyOutput(s->output, od, &mod) == 1) { + OSPDaemon_queue_put(&s->output->q, &mod); + } else { + OSPDaemon_queue_put(&s->output->q, od); + } + } + } + if (!s->noprocess) { + stat = OSP_SetInputData(s->handle, od); + if (stat != OSP_STATUS_OK) { + s->pending = 1; + memcpy(&s->pdata, od, sizeof(s->pdata)); + ret = 1; + break; + } + } + } while(1); + + return ret; +} + +static int OSPDaemon_process_inbound(struct pollfd *pfd, struct pollfd *list, int nfd, int dirty) +{ + int ret; + int j, k; + + /* Input events */ + for (j = 0; j < sd->sensor_count; j++) + if (sd->sensor[j].fd == pfd->fd) + break; + if (j < sd->sensor_count) { + OSPDaemon_driver_read(&sd->sensor[j]); + dirty++; + ret = OSPDaemon_senddata(&sd->sensor[j]); + if (sd->sensor[j].output) { + for (k = 0; k < nfd; k++) { + if (list[k].fd == sd->sensor[j].output->fd) { + list[k].events = POLLOUT; + break; + } + } + } + if (ret > 0) { + ret++; + } + } + return dirty; +} + +static int OSPDaemon_process_outbound(struct pollfd *pfd, int nfd) +{ + int j; + + for (j = 0; j < sd->output_count; j++) + if (sd->output[j].fd == pfd->fd) + break; + if (j < sd->output_count) { + if (OSPDaemon_driver_send(&sd->output[j]) == 0) { + pfd->events = 0; + } + } + + return 0; +} + +static int OSPDaemon_flush_out(struct OSPDaemon_SensorDescription *d) +{ + int j; + + for (j = 0; j < d->output_count; j++) { + if (!OSPDaemon_queue_isempty(&d->output[j].q)) { + OSPDaemon_driver_send(&d->output[j]); + } + } + + return 0; +} + +static int OSPDaemon_init_outbound(struct pollfd *pfd, int nfd, int start) +{ + int i, nstart; + OSP_STATUS_t oret; + + nstart = start; + for (i = 0; i < sd->output_count; i++) { + nstart++; + pfd[i+start].fd = sd->output[i].fd; + pfd[i+start].events = 0; + OSPDaemon_queue_init(&sd->output[i].q); + + if (sd->output[i].noprocess) continue; + + sd->output[i].ResultDesc.pResultReadyCallback = OSPDaemon_callback; + if (sd->output[i].type != -1) { + oret = OSP_SubscribeSensorResult(&sd->output[i].ResultDesc, &sd->output[i].handle); + if (oret != OSP_STATUS_OK) { + DBG(DEBUG_INIT, "Sensor subscribe failed\n"); + } + } + } + return nstart; +} + +/* + * start - index to start populating the pfd array. + * + * Returns start + number of entries used. + */ +static int OSPDaemon_init_inbound(struct pollfd *pfd, int nfd, int start) +{ + int i; + OSP_STATUS_t oret; + int nstart; + + if (sd->CalFile) + ReadCalData(sd->CalFile); + + for (i = 0; i < sd->sensor_count; i++) { + nstart++; + if (sd->sensor[i].fd < 0) + continue; + + pfd[i+start].fd = sd->sensor[i].fd; + pfd[i+start].events = POLLIN; + OSPDaemon_queue_init(&sd->sensor[i].q); + DBG(DEBUG_INIT, "Input sensor noprocess = %i\n", + sd->sensor[i].noprocess); + if (sd->sensor[i].noprocess) continue; + sd->sensor[i].sensor.pOptionalWriteDataCallback = + OSPDaemon_write_cal_cb; + + oret = OSP_RegisterInputSensor(&sd->sensor[i].sensor, &sd->sensor[i].handle); + if (oret != OSP_STATUS_OK) { + DBG(DEBUG_INIT, "Sensor register failed (%i)\n", oret); + } + } + return nstart; +} + +static void OSPDaemon(char *confname) +{ + int i, r = 0; + int dirty; + + if ((sd = OSPDaemon_config(confname)) == NULL) { + fprintf(stderr, "Bad/invalid config.\n"); + return; + } + + OSP_Initialize(&SystemDesc); + + for (i = 0; i < nfd; i++) { + pfd[i].fd = 0; + pfd[i].events = 0; + } + + OSPDaemon_driver_setup_in(sd->sensor, sd->sensor_count); + r = OSPDaemon_init_inbound(pfd, nfd, r); + OSPDaemon_driver_setup_out(sd->output, sd->output_count); + nfd = OSPDaemon_init_outbound(pfd, nfd, r); + sd->powerfd = OSPDaemon_power_init(sd); + if (sd->powerfd > 0) { + pfd[nfd].fd = sd->powerfd; + pfd[nfd].events |= POLLIN; + nfd++; + } + + for (i = 0; i < nfd; i++) { + DBG(DEBUG_INIT, "%i: fd %i events = %i\n", i, pfd[i].fd, pfd[i].events); + } + + dirty = 0; + while (!mainstatus) { + if (poll(pfd, nfd, -1) <= 0) + continue; + + for (i = 0; i < nfd; i++) { + if (pfd[i].revents & POLLIN) { + DBG(DEBUG_LOOP, "IN event\n"); + if (pfd[i].fd == sd->powerfd) { + OSPDaemon_power_process(sd, + sd->powerfd); + } else { + dirty = OSPDaemon_process_inbound( + &pfd[i], pfd, + nfd, dirty); + } + } + if (pfd[i].revents & POLLOUT) { + DBG(DEBUG_LOOP, "%s:%i OUT event\n", __func__, __LINE__); + OSPDaemon_process_outbound(&pfd[i], nfd); + } + } + + OSPDaemon_flush_out(sd); + + if (dirty) { + DBG(DEBUG_LOOP, "%s:%i PROCESS\n", __func__, __LINE__); + if (OSP_DoForegroundProcessing() == OSP_STATUS_IDLE) + dirty = 0; + OSP_DoBackgroundProcessing(); + } + } +} + +static void OSPDaemon_version(void) +{ + const OSP_Library_Version_t *ver; + OSP_GetLibraryVersion(&ver); + + printf("OSPDaemon compiled "__DATE__"@"__TIME__"\n"); + printf("Library version: %s\n", ver->VersionString); + printf("Library build: %s\n", ver->buildTime); +} + +static void OSPDaemon_help(const char *name) +{ + fprintf(stderr, "Usage %s [-c configfile|-d level|-v|-h|-p]\n", name); + fprintf(stderr, "-c configfile: Use configfile\n"); + fprintf(stderr, "-d level: Sets the debug level\n"); + fprintf(stderr, "-p: Disables power management.\n"); + fprintf(stderr, " All sensors default on\n"); + fprintf(stderr, "-v: Dumps version info and exits.\n"); + fprintf(stderr, "-h: Prints this message and exits.\n"); +} + +int main(int argc, char **argv) +{ + char *confname = NULL; + int ar = 1; + + if (argc > 1) { + do { + if (argv[ar][0] == '-') { + switch (argv[ar][1]) { + case 'c': /* Config file */ + if (ar+1 < argc) { + confname = argv[ar+1]; + ar++; + } + break; + case 'd': /* Debug level */ + if (ar+1 < argc) { + debug_level = strtol(argv[ar+1], NULL, 0); + ar++; + } + break; + case 'h': /* Help */ + OSPDaemon_help(argv[0]); + exit(0); + break; + case 'p': /* Disable PM */ + disablepm = 1; + break; + case 'n': /* Ignore cal file */ + ignorecal = 1; + break; + case 'v': /* Version */ + OSPDaemon_version(); + exit(0); + break; + } + ar++; + } + } while (ar < argc); + } + if (confname == NULL) + confname = "OSPDaemon.conf"; + printf("Reading config from %s\n", confname); + printf("Debug level 0x%08x\n", debug_level); + if (disablepm) { + printf("Disabling Power Management\n"); + } + /* Catch SIGPIPE? */ + /* queue_test(); */ + /* Driver inits. Replace with linker magic? */ + OSPDaemon_iio_init(); + OSPDaemon_inputreader_init(); + OSPDaemon_input_init(); + OSPDaemon_filecsv_init(); + /* End driver inits */ + + OSPDaemon(confname); + return 0; +} diff --git a/linux/daemon/OSPDaemon.h b/linux/daemon/OSPDaemon.h new file mode 100644 index 0000000..e4d1684 --- /dev/null +++ b/linux/daemon/OSPDaemon.h @@ -0,0 +1,239 @@ +#ifndef _OSPDAEMON_H_ +#define _OSPDAEMON_H_ 1 +#include "osp-api.h" +#include "OSPDaemon_queue.h" +#include "osp-api.h" + +/* Per sensor cal data size. Data larger then this is ignored */ +#define MAX_CALSIZE 2048 + +struct OSPDaemon_SensorDetail; + +struct OSPDaemon_output { + ResultDescriptor_t ResultDesc; + ResultHandle_t handle; + struct OSPDaemon_SensorDetail *source; + int type; + char *name; + int fd; + int driver; + int noprocess; + int enable; + int usage; + int format; + struct queue q; +}; + +enum { + OUT_FORMAT_INTEGER, + OUT_FORMAT_FLOAT, + OUT_FORMAT_Q24, + OUT_FORMAT_Q16, + OUT_FORMAT_Q15, + OUT_FORMAT_Q12, +}; + +enum { + IN_FORMAT_INTEGER, + IN_FORMAT_FLOAT, +}; + +enum { + DRIVER_INPUT, + DRIVER_IIO, + DRIVER_IIOEVENT, + DRIVER_FILECSV, + DRIVER_UNKNOWN +}; + +enum { + DRIVER_TYPE_INPUT, + DRIVER_TYPE_OUTPUT, +}; + +static inline void conv2OSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long ts, + int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + d->q24data.X = val[0]; + d->q24data.Y = val[1]; + d->q24data.Z = val[2]; + d->q24data.TimeStamp = ts; + break; + + case SENSOR_GEOMAGNETIC_FIELD: + d->q12data.X = val[0]; + d->q12data.Y = val[1]; + d->q12data.Z = val[2]; + d->q12data.TimeStamp = ts; + break; + + case SENSOR_ORIENTATION: + d->orientdata.Yaw = val[0]; + d->orientdata.Pitch = val[1]; + d->orientdata.Roll = val[2]; + d->orientdata.TimeStamp = ts; + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + d->rotvec.X = val[0]; + d->rotvec.Y = val[1]; + d->rotvec.Z = val[2]; + d->rotvec.W = val[3]; + d->rotvec.TimeStamp = ts; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + d->uncal_q24data.X = val[0]; + d->uncal_q24data.Y = val[1]; + d->uncal_q24data.Z = val[2]; + d->uncal_q24data.TimeStamp = ts; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + d->uncal_q12data.X = val[0]; + d->uncal_q12data.Y = val[1]; + d->uncal_q12data.Z = val[2]; + d->uncal_q12data.TimeStamp = ts; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + d->booldata.data = val[0]; + d->booldata.TimeStamp = ts; + break; + case SENSOR_STEP_COUNTER: + d->stepcount.StepCount = val[0]; + d->stepcount.TimeStamp = ts; + break; + default: + break; + } +} +static inline int extractOSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long *ts, + int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + val[0] = d->q24data.X; + val[1] = d->q24data.Y; + val[2] = d->q24data.Z; + *ts = d->q24data.TimeStamp; + return 3; + + case SENSOR_GEOMAGNETIC_FIELD: + val[0] = d->q12data.X; + val[1] = d->q12data.Y; + val[2] = d->q12data.Z; + *ts = d->q12data.TimeStamp; + return 3; + + case SENSOR_ORIENTATION: + val[0] = d->orientdata.Yaw; + val[1] = d->orientdata.Pitch; + val[2] = d->orientdata.Roll; + *ts = d->orientdata.TimeStamp; + return 3; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + val[0] = d->rotvec.X; + val[1] = d->rotvec.Y; + val[2] = d->rotvec.Z; + val[3] = d->rotvec.W; + *ts = d->rotvec.TimeStamp; + return 4; + case SENSOR_GYROSCOPE_UNCALIBRATED: + val[0] = d->uncal_q24data.X; + val[1] = d->uncal_q24data.Y; + val[2] = d->uncal_q24data.Z; + *ts = d->uncal_q24data.TimeStamp; + return 3; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + val[0] = d->uncal_q12data.X; + val[1] = d->uncal_q12data.Y; + val[2] = d->uncal_q12data.Z; + *ts = d->uncal_q12data.TimeStamp; + return 3; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + val[0] = d->booldata.data; + *ts = d->booldata.TimeStamp; + return 1; + case SENSOR_STEP_COUNTER: + val[0] = d->stepcount.StepCount; + *ts = d->stepcount.TimeStamp; + return 1; + default: + *ts = 0; + return 0; + } + return 0; +} + + +struct OSPDaemon_SensorDetail { + SensorDescriptor_t sensor; + InputSensorHandle_t handle; + char caldata[MAX_CALSIZE]; + char const *name; + int fd; + int driver; + void *private; + struct OSPDaemon_output *output; + struct queue q; + int pending; + int noprocess; + int format; + OSP_InputSensorData_t pdata; +}; + +struct OSPDaemon_SensorDescription { + struct OSPDaemon_SensorDetail *sensor; + int sensor_count; + struct OSPDaemon_output *output; + int output_count; + char *CalFile; + char *powerPath; + int powerfd; +}; + + +/* Cal file format is an array of struct OSPDaemon_CalRecord */ + +struct OSPDaemon_CalRecord { + ASensorType_t sensor; + uint32_t len; + unsigned char data[1]; /* Array length is determined by len */ +}; + +#define DBG(x, ...) if (debug_level & x) {printf(__VA_ARGS__);} + +#define DEBUG_INIT (1<<0) +#define DEBUG_CONF (1<<1) +#define DEBUG_INDRIVER (1<<2) +#define DEBUG_OUTDRIVER (1<<3) +#define DEBUG_LOOP (1<<4) +#define DEBUG_DATA (1<<5) +#define DEBUG_PM (1<<6) +#define DEBUG_DRIVER (1<<7) + +extern unsigned int debug_level; +extern unsigned int mainstatus; +extern unsigned int disablepm; + +struct OSPDaemon_SensorDescription * OSPDaemon_config(char *); + +void OSPDaemon_iio_init(void); +void OSPDaemon_input_init(void); +void OSPDaemon_inputreader_init(void); +void OSPDaemon_filecsv_init(void); + +#endif diff --git a/linux/daemon/OSPDaemon_config.c b/linux/daemon/OSPDaemon_config.c new file mode 100644 index 0000000..6d8268f --- /dev/null +++ b/linux/daemon/OSPDaemon_config.c @@ -0,0 +1,632 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ +/* + * Config file parser. + * + * Does not explicitly use malloc. + */ + +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "osp-api.h" + +#define MAX_TAG 50 +#define NAME_LEN 80 + +#define DBGOUT(x...) DBG(DEBUG_CONF, "CONFIG: "x) +/* Pre-allocate storate to avoid malloc.*/ +static struct OSPDaemon_SensorDetail Sensor[MAX_TAG]; +static char SensorName[MAX_TAG][NAME_LEN]; +static int sensor_count = 0; +static struct OSPDaemon_output Output[MAX_TAG]; +static int output_count = 0; +static char CalFile[PATH_MAX]; +static char powerPath[PATH_MAX]; +char output_name[MAX_TAG][NAME_LEN]; +static struct OSPDaemon_SensorDescription sd; +/* + * File format: + * Lines beginning with '#' are comments. + * + * For source sensors (sensors to read from) + * sensor0.type = + * sensor0.driver = + * sensor0. + * .... + * For output sensor (sensors to output) + * output0.type = + * output0.name = + * output0.driver = + */ + +#define SEN_MAP(x) [x] = #x + +static int proc_sensor_type(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_noise(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_convert(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_swap(int linetype, int tag, char *val, int *taglist); +static int proc_name(int linetype, int tag, char *val, int *taglist); +static int proc_driver(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_copy(int linetype, int tag, char *val, int *taglist); +static int proc_output_noprocess(int linetype, int tag, char *val, int *taglist); +static int proc_output_format(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_format(int linetype, int tag, char *val, int *taglist); +static int proc_system_calfile(int linetype, int tag, char *val, int *taglist); +static int proc_system_pmdir(int linetype, int tag, char *val, int *taglist); + +enum { + LINE_UNKNOWN, + LINE_SENSOR, + LINE_OUTPUT, + LINE_SYSTEM, +}; +static const char *LineName[] = { + [LINE_UNKNOWN] = "Unkown", + [LINE_SENSOR] = "Input", + [LINE_OUTPUT] = "Output", + [LINE_SYSTEM] = "System", +}; + +static const char * SensorTypeMap[] = { + SEN_MAP(SENSOR_ACCELEROMETER), + SEN_MAP(SENSOR_GYROSCOPE), + SEN_MAP(SENSOR_MAGNETIC_FIELD), + SEN_MAP(SENSOR_ORIENTATION), + SEN_MAP(SENSOR_LIGHT), + SEN_MAP(SENSOR_PRESSURE), + SEN_MAP(SENSOR_TEMPERATURE), + SEN_MAP(SENSOR_PROXIMITY), + SEN_MAP(SENSOR_GRAVITY), + SEN_MAP(SENSOR_LINEAR_ACCELERATION), + SEN_MAP(SENSOR_ROTATION_VECTOR), + SEN_MAP(SENSOR_RELATIVE_HUMIDITY), + SEN_MAP(SENSOR_AMBIENT_TEMPERATURE), + SEN_MAP(SENSOR_MAGNETIC_FIELD_UNCALIBRATED), + SEN_MAP(SENSOR_GAME_ROTATION_VECTOR), + SEN_MAP(SENSOR_GYROSCOPE_UNCALIBRATED), + SEN_MAP(SENSOR_SIGNIFICANT_MOTION), + SEN_MAP(SENSOR_STEP_DETECTOR), + SEN_MAP(SENSOR_STEP_COUNTER), + SEN_MAP(SENSOR_GEOMAGNETIC_ROTATION_VECTOR), + [NUM_ANDROID_SENSOR_TYPE] = NULL +}; + +static const char *DriverTypeMap[] = { + [DRIVER_INPUT] = "input", + [DRIVER_IIO] = "iio", + [DRIVER_IIOEVENT] = "iioevent", + [DRIVER_FILECSV] = "file_csv", +}; + +static const struct _keymap { + char *key; + int (*proc)(int linetype, int tag, char *val, int *taglist); +} KeyMap[] = { + {"type", proc_sensor_type}, + {"swap", proc_sensor_swap}, + {"convert", proc_sensor_convert}, + {"noise", proc_sensor_noise}, + {"name", proc_name}, + {"driver", proc_driver}, + {"copy", proc_sensor_copy}, + {"noprocess", proc_output_noprocess}, + {"calfile", proc_system_calfile}, + {"pmdir", proc_system_pmdir}, + {"format", proc_output_format}, + {"iformat", proc_sensor_format}, +}; + +/* Parse a string containing 3 comma seperated values */ +static int parse_3_double(char *val, double out[3]) +{ + char *ptr; + + out[0] = strtod(val, &ptr); + if (ptr && *ptr == ',') { + out[1] = strtod(ptr+1, &ptr); + if (ptr && *ptr == ',') + out[2] = strtod(ptr+1, NULL); + } else { + out[1] = out[0]; + out[2] = out[0]; + } + return 0; +} + +static int allocate_sensor(void) +{ + int ret; + + ret = sensor_count; + sensor_count++; + /* Initialize descriptor to sane values */ + Sensor[ret].sensor.DataWidthMask = 0xffffffff; + Sensor[ret].sensor.AxisMapping[0] = AXIS_MAP_POSITIVE_X; + Sensor[ret].sensor.AxisMapping[1] = AXIS_MAP_POSITIVE_Y; + Sensor[ret].sensor.AxisMapping[2] = AXIS_MAP_POSITIVE_Z; + Sensor[ret].sensor.MaxValue = 0xffffffff; + Sensor[ret].sensor.MinValue = 0xffffffff; + Sensor[ret].sensor.ConversionScale[0] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.ConversionScale[1] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.ConversionScale[2] = TOFIX_PRECISE(1.0f); + + Sensor[ret].sensor.NominalSamplePeriodInSeconds = TOFIX_PRECISE(0.02f); + Sensor[ret].sensor.factoryskr[0][0] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.factoryskr[0][1] = 0; + Sensor[ret].sensor.factoryskr[0][2] = 0; + + Sensor[ret].sensor.factoryskr[1][0] = 0; + Sensor[ret].sensor.factoryskr[1][1] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.factoryskr[1][2] = 0; + + Sensor[ret].sensor.factoryskr[2][0]= 0; + Sensor[ret].sensor.factoryskr[2][1]= 0; + Sensor[ret].sensor.factoryskr[2][2]= TOFIX_PRECISE(1.0f); + + return ret; +} + +static int allocate_output(void) +{ + int ret; + + ret = output_count; + output_count++; + return ret; +} + +static int proc_system_calfile(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return -1; + + strncpy(CalFile, val, PATH_MAX-1); + CalFile[PATH_MAX-1] = '\0'; + DBGOUT("Cal file: %s\n", val); + + return 0; +} +static int proc_system_pmdir(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return -1; + + strncpy(powerPath, val, PATH_MAX-1); + powerPath[PATH_MAX-1] = '\0'; + DBGOUT("power path: %s\n", val); + + return 0; +} + +/* + * sensor0.copy=output0 + */ +static int proc_sensor_copy(int linetype, int tag, char *val, int *taglist) +{ + int out, ret = -1; + + if (linetype != LINE_SENSOR) return -1; + DBGOUT("Line %s", LineName[linetype]); + + if (taglist[tag] < 0) { + taglist[tag] = allocate_sensor(); + } + + if (strncmp("output", val, 6) == 0) { + out = atoi(val+6); + DBGOUT("Linking to out tag %i\n", out); + if (out >= 0 && out < MAX_TAG) { + Sensor[taglist[tag]].output = &Output[out]; + Output[out].source = &Sensor[taglist[tag]]; + ret = 0; + } + } + + return ret; +} +static int proc_output_noprocess(int linetype, int tag, char *val, int *taglist) +{ + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s", LineName[linetype]); + if (linetype == LINE_SENSOR) + Sensor[taglist[tag]].noprocess = 1; + else if (linetype == LINE_OUTPUT) + Output[taglist[tag]].noprocess = 1; + + return 0; +} +static int proc_output_format(int linetype, int tag, char *val, int *taglist) +{ + int shift; + + if (linetype != LINE_OUTPUT) return 0; + + if (taglist[tag] < 0) { + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s scale %s\n", LineName[linetype], val); + + /* scaleNN -> shift by NN */ + if (strncasecmp("shift", val, 5) == 0) { + shift = atoi(val+5); + DBGOUT("Line %s scale %s shift %i\n", LineName[linetype], val, shift); + switch(shift) { + case 24: + Output[taglist[tag]].format = OUT_FORMAT_Q24; + break; + case 16: + Output[taglist[tag]].format = OUT_FORMAT_Q16; + break; + case 12: + Output[taglist[tag]].format = OUT_FORMAT_Q12; + break; + case 15: + Output[taglist[tag]].format = OUT_FORMAT_Q15; + break; + default: + break; + } + + } else if (strcasecmp("integer", val) == 0) { + Output[taglist[tag]].format = OUT_FORMAT_INTEGER; + + } else if (strcasecmp("float", val) == 0) { + Output[taglist[tag]].format = OUT_FORMAT_FLOAT; + } + + return 0; +} + +/* Internal code (algs, etc) assume data is Qnn. If the input is + * not in that format, this config option converts to that. + */ +static int proc_sensor_format(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return 0; + + if (taglist[tag] < 0) { + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s scale %s\n", LineName[linetype], val); + + /* scaleNN -> shift by NN */ + if (strcasecmp("integer", val) == 0) { + Sensor[taglist[tag]].format = IN_FORMAT_INTEGER; + + } else if (strcasecmp("float", val) == 0) { + Sensor[taglist[tag]].format = IN_FORMAT_FLOAT; + } + + return 0; +} +static int proc_driver(int linetype, int tag, char *val, int *taglist) +{ + int i; + + for (i = 0; i < DRIVER_UNKNOWN; i++) { + DBGOUT("Comparing %s to %s\n", DriverTypeMap[i], val); + if (strcmp(DriverTypeMap[i], val) == 0) { + break; + } + } + DBGOUT("Line %s val = %s mapping to %i\n", LineName[linetype], val, i); + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + + if (linetype == LINE_OUTPUT) { + Output[taglist[tag]].driver = i; + } else { + Sensor[taglist[tag]].driver = i; + } + return 0; +} + +static int proc_name(int linetype, int tag, char *val, int *taglist) +{ + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + + if (linetype == LINE_OUTPUT) { + strncpy(output_name[taglist[tag]], val, NAME_LEN-1); + output_name[taglist[tag]][NAME_LEN-1] = '\0'; + Output[taglist[tag]].name = output_name[taglist[tag]]; + DBGOUT("OUTPUT: tag %i - %s\n", tag, val); + } else if (linetype == LINE_SENSOR) { + strncpy(SensorName[taglist[tag]], val, NAME_LEN-1); + SensorName[taglist[tag]][NAME_LEN-1] = '\0'; + Sensor[taglist[tag]].name = SensorName[taglist[tag]]; + DBGOUT("SENSOR: tag %i - %s\n", tag, val); + } else { + return -1; + } + + return 0; +} + +/* Mapping to values used by the descriptor */ +static const int OSPaxisMap[3] = { + [0] = AXIS_MAP_POSITIVE_X, + [1] = AXIS_MAP_POSITIVE_Y, + [2] = AXIS_MAP_POSITIVE_Z, +}; + +/* sensor0.swap=0,1,2 */ +static int proc_sensor_swap(int linetype, int tag, char *val, int *taglist) +{ + int map[3]; + int ret = -1; + int idx; + + if (linetype != LINE_SENSOR) return -1; + + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + idx = 0; + + map[0] = val[idx]-'0'; + if (map[0] < 0 || map[0] > 2) goto error; + + idx++; + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + if (val[idx] != ',') goto error; + idx++; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + map[1] = val[idx]-'0'; + idx++; + if (map[1] < 0 || map[1] > 2) goto error; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + if (val[3] != ',') ret = -1; + idx++; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + map[2] = val[idx]-'0'; + if (map[1] < 0 || map[1] > 2) goto error; + + if (map[0] == map[1] || map[0] == map[2] || + map[1] == map[2]) { + ret = -2; + goto error; + } + ret = 0; + + Sensor[taglist[tag]].sensor.AxisMapping[0] = OSPaxisMap[map[0]]; + Sensor[taglist[tag]].sensor.AxisMapping[1] = OSPaxisMap[map[1]]; + Sensor[taglist[tag]].sensor.AxisMapping[2] = OSPaxisMap[map[2]]; + DBGOUT("MAP: tag %i - %i %i %i\n", tag, map[0], map[1], map[2]); + +error: + if (ret < 0) + printf("INVALID MAP\n"); + return ret; +} + +/* sensor0.convert=-0.12345,-0.12345,-0.12345 */ +/* sensor0.convert=-0.12345 */ +static int proc_sensor_convert(int linetype, int tag, char *val, int *taglist) +{ + double convert[3]; + + if (linetype != LINE_SENSOR) return -1; + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + parse_3_double(val, convert); + Sensor[taglist[tag]].sensor.ConversionScale[0] = convert[0]; + Sensor[taglist[tag]].sensor.ConversionScale[1] = convert[1]; + Sensor[taglist[tag]].sensor.ConversionScale[2] = convert[2]; + + DBGOUT("CONVERT: tag %i %f %f %f\n", tag, convert[0], convert[1], convert[2]); + return 0; + +} +/* sensor0.noise=-0.12345,-0.12345,-0.12345 */ +/* sensor0.noise=-0.12345 */ +static int proc_sensor_noise(int linetype, int tag, char *val, int *taglist) +{ + double noise[3]; + + if (linetype != LINE_SENSOR) return -1; + + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + parse_3_double(val, noise); + Sensor[taglist[tag]].sensor.Noise[0] = noise[0]; + Sensor[taglist[tag]].sensor.Noise[1] = noise[1]; + Sensor[taglist[tag]].sensor.Noise[2] = noise[2]; + + DBGOUT("NOISE: tag %i %f %f %f\n", tag, noise[0], noise[1], noise[2]); + + return 0; +} + +/* sensor0.type=SENSOR_ACCELEROMETER */ +static int proc_sensor_type(int linetype, int tag, char *val, int *taglist) +{ + int i; + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (SensorTypeMap[i] == NULL) continue; + if (strcmp(SensorTypeMap[i], val) == 0) { + DBGOUT("FOUND SENSOR: %i tag %i : %s\n", linetype, tag, SensorTypeMap[i]); + if (linetype == LINE_SENSOR) { + taglist[tag] = sensor_count; + sensor_count++; + Sensor[taglist[tag]].sensor.SensorType = i; + Sensor[taglist[tag]].sensor.SensorName = SensorTypeMap[i]; + } else if (linetype == LINE_OUTPUT) { + taglist[tag] = output_count; + output_count++; + Output[taglist[tag]].type = i; + Output[taglist[tag]].ResultDesc.SensorType = i; + Output[taglist[tag]].ResultDesc.OutputRateInSeconds = 0; + Output[taglist[tag]].ResultDesc.OptionData = &Output[taglist[tag]]; + + } + break; + } + } + if (i == NUM_ANDROID_SENSOR_TYPE) { + fprintf(stderr, "Bad/invalid sensor: %s\n", val); + } + return 0; +} + +static void remove_trail_whitespace(char *p, int crlf) +{ + char *ptr; + ptr = p; + while (*ptr != '\0') { + if (*ptr == '\t') { + *ptr = '\0'; + continue; + } + if (*ptr == ' ') { + *ptr = '\0'; + continue; + } + if (crlf) { + if (*ptr == '\r') { + *ptr = '\0'; + continue; + } + if (*ptr == '\n') { + *ptr = '\0'; + continue; + } + } + ptr++; + } +} +static char * remove_lead_whitespace(char *p) +{ + char *ptr; + ptr = p; + while (*ptr == ' ' || *ptr == '\t') ptr++; + + return ptr; +} + +static void parse_line(int lineno, char *l, int *taglist1, int *taglist2) +{ + int i; + char *eq, *ptr, *key; + int tag; + int linetype = LINE_UNKNOWN; + + if (l[0] == '#' || + l[0] == '\r' || + l[0] == '\n' || + l[0] == ' ' || + l[0] == '\t' || + l[0] == ';') return; /* Comments */ + eq = strchr(l, '='); + if (!eq) return; + if (strncmp(l, "input", 5) == 0) + linetype = LINE_SENSOR; + else if (strncmp(l, "output", 6) == 0) + linetype = LINE_OUTPUT; + else if (strncmp(l, "system", 6) == 0) + linetype = LINE_SYSTEM; + + DBGOUT("Line %i: %s\n", lineno, LineName[linetype]); + if (linetype > 0) { + tag = strtol(l+6, &ptr, 0); + if (tag >= MAX_TAG) return; + + if (ptr > eq) return; + if (*ptr != '.') return; + key=ptr+1; + *eq = '\0'; + + remove_trail_whitespace(key, 0); + ptr = remove_lead_whitespace(eq+1); + remove_trail_whitespace(ptr, 1); + + if (ptr[0] == '\0') return; + + for (i = 0; i < sizeof(KeyMap)/sizeof(struct _keymap); i++) { + if (strcmp(key, KeyMap[i].key) == 0) { + if (KeyMap[i].proc == NULL) + break; + KeyMap[i].proc(linetype, tag, ptr, linetype == LINE_SENSOR?taglist1:(linetype == LINE_OUTPUT?taglist2:NULL)); + } + } + } +} + +/* A nonzero return will abort the daemon */ +struct OSPDaemon_SensorDescription *OSPDaemon_config(char *confname) +{ + FILE *f; + char line[2048], *r; + int i; + int tagmap1[MAX_TAG]; + int tagmap2[MAX_TAG]; + int lineno = 1; + + f = fopen(confname, "r"); + if (!f) return NULL; + for (i = 0; i < MAX_TAG; i++) { + tagmap1[i] = -1; + tagmap2[i] = -1; + memset(&Sensor[i], 0, sizeof(Sensor[i])); + Sensor[i].fd = -1; + memset(&Output[i], 0, sizeof(Sensor[i])); + Output[i].fd = -1; + } + sd.powerPath = NULL; + powerPath[0] = '\0'; + DBGOUT("%s:%i\n", __func__, __LINE__); + do { + r = fgets(line, 2047, f); + if (r) { + line[2047] = '\0'; + parse_line(lineno, line, tagmap1, tagmap2); + } + lineno++; + } while (r != NULL); + DBGOUT("Processed %i lines\n", lineno); + fclose(f); + sd.sensor = Sensor; + sd.sensor_count = sensor_count; + sd.output = Output; + sd.output_count = output_count; + sd.CalFile = CalFile; + if (powerPath[0] != '\0') + sd.powerPath = powerPath; + return &sd; +} diff --git a/linux/daemon/OSPDaemon_driver.c b/linux/daemon/OSPDaemon_driver.c new file mode 100644 index 0000000..2366993 --- /dev/null +++ b/linux/daemon/OSPDaemon_driver.c @@ -0,0 +1,125 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +/* Driver framework */ + +#include +#include +#include "OSPDaemon.h" +#include "OSPDaemon_driver.h" + + +static struct OSPDaemon_driver *inmap[] = { + [DRIVER_INPUT] = NULL, + [DRIVER_IIO] = NULL, + [DRIVER_IIOEVENT] = NULL, + [DRIVER_FILECSV] = NULL, +}, *outmap[] = { + [DRIVER_INPUT] = NULL, + [DRIVER_IIO] = NULL, + [DRIVER_IIOEVENT] = NULL, + [DRIVER_FILECSV] = NULL, +}; + +int OSPDaemon_driver_register(struct OSPDaemon_driver *ops) +{ + if (ops == NULL) return -EINVAL; + + if (ops->drvtype == DRIVER_TYPE_INPUT) { + switch (ops->driver) { + case DRIVER_INPUT: + case DRIVER_IIO: + case DRIVER_IIOEVENT: + case DRIVER_FILECSV: + break; + default: + return -EINVAL; + } + inmap[ops->driver] = ops; + } else if (ops->drvtype == DRIVER_TYPE_OUTPUT) { + switch (ops->driver) { + case DRIVER_INPUT: + case DRIVER_FILECSV: + break; + default: + return -EINVAL; + } + outmap[ops->driver] = ops; + } else { + return -EINVAL; + } + return 0; +} + +int OSPDaemon_driver_read(struct OSPDaemon_SensorDetail *s) +{ + if (s == NULL || inmap[s->driver] == NULL) + return -EINVAL; + + return inmap[s->driver]->read(s); +} + +int OSPDaemon_driver_send(struct OSPDaemon_output *s) +{ + if (s == NULL || outmap[s->driver] == NULL) + return -EINVAL; + DBG(DEBUG_DRIVER, "Sending to driver %i\n", s->driver); + + return outmap[s->driver]->send(s); +} + +int OSPDaemon_driver_setup_in(struct OSPDaemon_SensorDetail *s, int count) +{ + if (inmap[DRIVER_IIO]) + inmap[DRIVER_IIO]->setup_in(s, count); + if (inmap[DRIVER_INPUT]) + inmap[DRIVER_INPUT]->setup_in(s, count); + if (inmap[DRIVER_FILECSV]) + inmap[DRIVER_FILECSV]->setup_in(s, count); + + return 0; +} + +int OSPDaemon_driver_setup_out(struct OSPDaemon_output *s, int count) +{ + if (outmap[DRIVER_INPUT]) + outmap[DRIVER_INPUT]->setup_out(s, count); + if (outmap[DRIVER_FILECSV]) + outmap[DRIVER_FILECSV]->setup_out(s, count); + + return 0; +} + +int OSPDaemon_driver_enable_out(struct OSPDaemon_output *s) +{ + s->enable = 1; + s->usage = 1; + + return 0; +} + +int OSPDaemon_driver_disable_out(struct OSPDaemon_output *s) +{ + s->enable = 0; + s->usage = 0; + + return 0; +} + +int OSPDaemon_driver_enable_in(struct OSPDaemon_SensorDetail *s) +{ + if (!s) return -1; + if (inmap[s->driver]->enable_in) + inmap[s->driver]->enable_in(s); + return 0; +} +int OSPDaemon_driver_disable_in(struct OSPDaemon_SensorDetail *s) +{ + if (!s) return -1; + if (inmap[s->driver]->disable_in) + inmap[s->driver]->disable_in(s); + return 0; +} diff --git a/linux/daemon/OSPDaemon_driver.h b/linux/daemon/OSPDaemon_driver.h new file mode 100644 index 0000000..0db87ec --- /dev/null +++ b/linux/daemon/OSPDaemon_driver.h @@ -0,0 +1,26 @@ +#ifndef _OSPDAEMON_DRIVER_H_ +#define _OSPDAEMON_DRIVER_H_ + +struct OSPDaemon_driver { + int drvtype; + int driver; + int (*setup_in)(struct OSPDaemon_SensorDetail *, int count); + int (*setup_out)(struct OSPDaemon_output *, int count); + int (*send)(struct OSPDaemon_output *); + int (*read)(struct OSPDaemon_SensorDetail *s); + int (*enable_out)(struct OSPDaemon_output *); + int (*enable_in)(struct OSPDaemon_SensorDetail *); + int (*disable_out)(struct OSPDaemon_output *); + int (*disable_in)(struct OSPDaemon_SensorDetail *); +}; + +int OSPDaemon_driver_setup_out(struct OSPDaemon_output *s, int count); +int OSPDaemon_driver_setup_in(struct OSPDaemon_SensorDetail *s, int count); +int OSPDaemon_driver_send(struct OSPDaemon_output *); +int OSPDaemon_driver_read(struct OSPDaemon_SensorDetail *); +int OSPDaemon_driver_register(struct OSPDaemon_driver *); +int OSPDaemon_driver_enable_out(struct OSPDaemon_output *s); +int OSPDaemon_driver_disable_out(struct OSPDaemon_output *s); +int OSPDaemon_driver_enable_in(struct OSPDaemon_SensorDetail *s); +int OSPDaemon_driver_disable_in(struct OSPDaemon_SensorDetail *s); +#endif diff --git a/linux/daemon/OSPDaemon_filecsv.c b/linux/daemon/OSPDaemon_filecsv.c new file mode 100644 index 0000000..da489a6 --- /dev/null +++ b/linux/daemon/OSPDaemon_filecsv.c @@ -0,0 +1,249 @@ +/* + * (C) 2015 HY Research LLC + * This file is under the Apache Public License. + */ + +/* + * Driver to read and write input from a file: + * Since a normal file is always "ready", we need to do a few + * special things like pausing on errors. + * This requires the rest of the system to be muxed with poll + * to insure fairness. Otherwise, this code will monopolize the + * system. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_filecsv.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_driver.h" + +#define BUF_SZ 512 +#define NUM_CSV 10 +#define LINE_LEN 80 + +static int filecsv_count = 0; +static struct FileCSV_Sensor { + int axis_count; + char buf[BUF_SZ]; + int remain; + int start; +} FileCSV[NUM_CSV]; + +static int OSPDaemon_filecsv_create(const char *name, struct FileCSV_Sensor *fs) +{ + int fd; + + fd = open(name, O_RDONLY); + if (fd < 0) return -1; + fs->remain = 0; + fs->start = 0; + + return fd; +} + +static int OSPDaemon_filecsv_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i; + int f_count = 0, f_success = 0; + + for (i = 0; i < count; i++) { + if (s->driver == DRIVER_FILECSV) { + f_count++; + if (filecsv_count >= NUM_CSV) continue; + s->fd = OSPDaemon_filecsv_create(s->name, &FileCSV[filecsv_count]); + if (s->fd > 0) { + f_success++; + s->private = &FileCSV[filecsv_count]; + switch (s->sensor.SensorType) { + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + FileCSV[filecsv_count].axis_count = 4; + break; + default: + FileCSV[filecsv_count].axis_count = 3; + } + filecsv_count++; + } + } + s++; + } + DBG(DEBUG_INDRIVER, "FileCSV: req %i success %i\n", f_count, f_success); + return 0; +} + +static void pause20(void) +{ + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 20000; + select(0, NULL, NULL, NULL, &tv); +} + +static int OSPDaemon_filecsv_read(struct OSPDaemon_SensorDetail *s) +{ + char line[LINE_LEN]; + char *next; + struct FileCSV_Sensor *fs = s->private; + OSP_InputSensorData_t od; + int val[5]; + int i, ret; + int lineloc = 0; + int gotline = 0; + int count; + + do { + if (fs->remain == 0) { + ret = read(s->fd, fs->buf, BUF_SZ); + if (ret <= 0) { + pause20(); + mainstatus = 1; + return 0; + } + fs->remain = ret; + fs->start = 0; + } + count = fs->remain; + for (i = 0; i < count && gotline == 0; i++, fs->start++, fs->remain--) { + if (lineloc > LINE_LEN-2) { + gotline = 1; + line[LINE_LEN-1] = '\0'; + break; + } + switch(fs->buf[fs->start]) { + case '\0': + case '\r': + case '\n': + gotline = 1; + line[lineloc] = '\0'; + break; + default: + line[lineloc] = fs->buf[fs->start]; + lineloc++; + break; + } + } + } while(!gotline); + + /* Assume X,Y,Z in integers */ + switch (line[0]) { + /* Comments */ + case '#': + case ';': + case '/': + case '\t': + case ' ': + return 0; + } + + DBG(DEBUG_INDRIVER, "Data line: %s\n", line); + + val[0] = strtol(line, &next, 10); + if (next == NULL) return 0; + + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[1] = strtol(next, &next, 10); + + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[2] = strtol(next, &next, 10); + + if (fs->axis_count == 4) { + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[3] = strtol(next, &next, 10); + } + conv2OSP(s->sensor.SensorType, &od, 0, val); + DBG(DEBUG_INDRIVER, "Data: %i, %i, %i\n", val[0], val[1], val[2]); + OSPDaemon_queue_put(&s->q, &od); + pause20(); + + return 1; +} + +static void OSPDaemon_writecsv_create(struct OSPDaemon_output *out) +{ + int fd; + DBG(DEBUG_OUTDRIVER, "Init for %s\n", out->name); + fd = open(out->name, O_WRONLY|O_CREAT|O_APPEND, S_IRWXU|S_IRGRP|S_IROTH); + if (fd < 0) return; + out->fd = fd; +} + +static int OSPDaemon_writecsv_setup(struct OSPDaemon_output *out, int count) +{ + int i; + int f_count = 0, f_success = 0; + for (i = 0; i < count; i++) { + if (out[i].driver == DRIVER_FILECSV) { + f_count++; + OSPDaemon_writecsv_create(&out[i]); + if (out[i].fd >= 0) + f_success++; + } + } + DBG(DEBUG_OUTDRIVER, "FileCSV: req %i suc %i\n", f_count, f_success); + return 0; +} + +static int OSPDaemon_writecsv_send(struct OSPDaemon_output *out) +{ + OSP_InputSensorData_t *od; + char line[LINE_LEN]; + int val[5], vallen; + unsigned long long ts; + int c; + int count = 0; + + while ((od = OSPDaemon_queue_get(&out->q)) != NULL) { + count++; + vallen = extractOSP(out->type, od, &ts, val); + DBG(DEBUG_OUTDRIVER,"Sending (FILE): %i %i %i fd = %i\n", + val[0], val[1], val[2], out->fd); + switch(out->ResultDesc.SensorType) { + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + c = snprintf(line, 79, "%i, %i, %i, %i, %lli\n", + val[0], val[1], val[2], val[3], ts); + break; + default: + c = snprintf(line, 79, "%i, %i, %i, %lli\n", + val[0], val[1], val[2], ts); + break; + } + if (out->fd >= 0) + write(out->fd, line, c); + if (count > 5) break; /* Limit writes */ + } + + if (od == NULL) return 0; else return 1; +} + +static struct OSPDaemon_driver ReadCSV = { + .driver = DRIVER_FILECSV, + .drvtype = DRIVER_TYPE_INPUT, + .setup_in= OSPDaemon_filecsv_setup, + .read = OSPDaemon_filecsv_read, +}, WriteCSV = { + .driver = DRIVER_FILECSV, + .drvtype = DRIVER_TYPE_OUTPUT, + .setup_out = OSPDaemon_writecsv_setup, + .send = OSPDaemon_writecsv_send, +}; + +void OSPDaemon_filecsv_init(void) +{ + OSPDaemon_driver_register(&ReadCSV); + OSPDaemon_driver_register(&WriteCSV); +} diff --git a/linux/daemon/OSPDaemon_filecsv.h b/linux/daemon/OSPDaemon_filecsv.h new file mode 100644 index 0000000..eae9e25 --- /dev/null +++ b/linux/daemon/OSPDaemon_filecsv.h @@ -0,0 +1,5 @@ +#ifndef _OSPDAEMON_FILECSV_H_ +#define _OSPDAEMON_FILECSV_H_ +#include "OSPDaemon.h" + +#endif diff --git a/linux/daemon/OSPDaemon_iio.c b/linux/daemon/OSPDaemon_iio.c new file mode 100644 index 0000000..3d82126 --- /dev/null +++ b/linux/daemon/OSPDaemon_iio.c @@ -0,0 +1,710 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * + * Licensed under Apache Public License. + */ + +/* + * Driver to read data from an IIO device. + * For now, it assumes the IIO device done in the form of the + * OSP IIO drivers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_iio.h" +#include "OSPDaemon_driver.h" + +#define IIO_DEVICE_DIR "/sys/bus/iio/devices" + +#define DBGOUT(x...) DBG(DEBUG_INDRIVER, "IIO: "x) + +static struct IIO_Sensor IIOSen[MAX_SENSOR]; + +/* sysfs support */ +static void sysfs_write_val(const char *path, const int val) +{ + FILE *f; + + f = fopen(path, "w"); + if (f) { + fprintf(f, "%i", val); + fclose(f); + } +} +static void sysfs_write_str(const char *path, const char *str) +{ + FILE *f; + + f = fopen(path, "w"); + if (f) { + fprintf(f, "%s", str); + fclose(f); + } +} +/* -------------- */ +/* Parse a IIO type file */ +static void parse_type(const char *path, struct DataDesc *dd) +{ + FILE *f; + char desc[1024]; + char *ptr = NULL, *ptr2; + + DBGOUT("Parsing %s\n", path); + f = fopen(path, "r"); + if (f) { + ptr = fgets(desc, 1024, f); + fclose(f); + } + if (ptr) { + /* le:s64/64>>0 */ + /* Ignore endianness for now */ + if (desc[3] == 's') { + dd->sign = 1; + } else { + dd->sign = 0; + } + + dd->size = strtol(desc+4, &ptr, 0); + dd->store = strtol(ptr+1, &ptr2, 0); + if (*ptr2 == '>') { + dd->shift = strtol(ptr2+2, NULL, 0); + } else { + dd->shift = 0; + } + } +} +/* Expects: + * in_NAME_AXIS_misc + */ +static const char *tsname = "timestamp"; + +static int parse_axis(const char *name) +{ + int axis = axis_invalid; + int i, j; + + if (name[0] == 'i' && name[1] == 'n' && name[2] == '_') { + for (i = 3, j = 0; name[i] != '\0' && i < 1024; i++, j++) { + if (name[i] == '_') break; + if (j < 2048) { + if (name[i] != tsname[j]) + j = 2048; + } + } + if (j < 2048) + axis = axis_timestamp; + else if (name[i] == '_') { + i++; + switch (name[i]) { + case 'x': + axis = axis_x; + break; + case 'y': + axis = axis_y; + break; + case 'z': + axis = axis_z; + break; + case 'r': + axis = axis_r; + break; + } + } + } + + return axis; +} + +/* + * Parse the index file. + */ +static int parse_index(const char *fname) +{ + FILE *f; + char desc[1024]; + char *ptr = NULL; + int idx = -1; + + f = fopen(fname, "r"); + if (f) { + ptr = fgets(desc, 1024, f); + desc[1023] = '\0'; + fclose(f); + } + if (ptr) { + idx = atoi(desc); + } + return idx; +} + +static OSP_InputSensorData_t * parse_iiodata(struct IIO_Sensor *is, char *buf, int len) +{ + int i, ax; + union { + uint8_t b[8]; + uint16_t s[4]; + uint32_t w[2]; + uint64_t ll[1]; + int8_t sb[8]; + int16_t ss[4]; + int32_t sw[2]; + int64_t sll[1]; + } conf; + int val[6]; + unsigned long long ts; + double outval; + + for (i = 0; i < MAX_AXIS; i++) { + ax = is->index2axis[i]; + if (ax < 0) continue; + conf.w[0] = 0; conf.w[1] = 0; + + switch (is->IIOAxis[ax].dd.size) { + case 8: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sb[0]; + else + outval = conf.b[0]; + break; + case 16: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.ss[0]; + else + outval = conf.s[0]; + break; + case 32: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + conf.b[2] = buf[is->IIOAxis[ax].offset+2]; + conf.b[3] = buf[is->IIOAxis[ax].offset+3]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sw[0]; + else + outval = conf.w[0]; + break; + case 64: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + conf.b[2] = buf[is->IIOAxis[ax].offset+2]; + conf.b[3] = buf[is->IIOAxis[ax].offset+3]; + conf.b[4] = buf[is->IIOAxis[ax].offset+4]; + conf.b[5] = buf[is->IIOAxis[ax].offset+5]; + conf.b[6] = buf[is->IIOAxis[ax].offset+6]; + conf.b[7] = buf[is->IIOAxis[ax].offset+7]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sll[0]; + else + outval = conf.ll[0]; + break; + } +#if 0 + if (is->IIOAxis[ax].dd.shift) + outval /= (double)(1<IIOAxis[ax].dd.shift); +#endif +#if 0 + if (ax == axis_timestamp) + printf("%s = 0x%llx | ", axis_name[ax], conf.ll[0]); + else + printf("%s = %08f | ", axis_name[ax], outval); +#else + /* DANGEROUS CODE. ASSUMES LAYOUT DETAILS */ + switch (ax) { + case axis_x: + val[0] = outval; + break; + case axis_y: + val[1] = outval; + break; + case axis_z: + val[2] = outval; + break; + case axis_r: + val[3] = outval; + break; + case axis_timestamp: + ts = conf.ll[0]; + break; + } +#endif + } + conv2OSP(is->type, &is->data, ts, val); + return &is->data; +} + +/* + * Parse contents of scan elements directory. + * Enable all elements. + */ +static void setupiio(struct IIO_Sensor *is) +{ + char dname[PATH_MAX]; + char fname[PATH_MAX]; + DIR *d; + struct dirent *dent; + int len, ax; + + snprintf(dname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements", is->iionum); + + d = opendir(dname); + + while((dent = readdir(d)) != NULL) { + if (dent->d_name[0] == '.') continue; + len = strlen(dent->d_name); + if (len < 6) continue; + + ax = parse_axis(dent->d_name); + if (ax < 0) continue; + + if (strcmp(dent->d_name + len - 5, "_type") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + + parse_type(fname, &is->IIOAxis[ax].dd); + DBGOUT("%s - %i, %i\n", fname, + is->IIOAxis[ax].dd.store, ax); + /* print_Desc(&is->IIOAxis[ax].dd); */ + } else if (strcmp(dent->d_name + len - 3, "_en") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + sysfs_write_val(fname, 1); + } else if (strcmp(dent->d_name + len - 6, "_index") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + is->IIOAxis[ax].index = parse_index(fname); + is->index2axis[is->IIOAxis[ax].index] = ax; + } + } + + closedir(d); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/trigger/current_trigger", is->iionum); + printf("setting trigger name %s to %s\n", is->name, fname); + sysfs_write_str(fname, is->name); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/length", is->iionum); + sysfs_write_val(fname, 100); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/enable", is->iionum); + sysfs_write_val(fname, 1); +} +/* + * Find the IIO device number associate with the named sensor + */ +static int getiionum(const char *sname) +{ + int i, j; + FILE *f; + char fname[PATH_MAX]; + char name[1024]; + + for (i = 0; i < MAX_IIO; i++) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/name", i); + fname[PATH_MAX-1] = '\0'; + + f = fopen(fname, "r"); + if (f == NULL) continue; + if (fgets(name, 1023, f) != NULL) { + name[1023] = '\0'; + for (j = 0; j < 1024 && name[j] != '\0'; j++) { + if (name[j] == '\n' || + name[j] == '\r') { + name[j] = '\0'; + break; + } + } + } + fclose(f); + if (strcmp(name, sname) == 0) + return i; + } + return -1; +} + +static void OSPDaemon_iio_SetState(struct IIO_Sensor *s, int state) +{ + char name[PATH_MAX+1]; + + if (state != 0) state = 1; + name[PATH_MAX] = '\0'; + snprintf(name, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/enable", + s->iionum); + + sysfs_write_val(name, state); +} + +static int OSPDaemon_iio_enable(struct OSPDaemon_SensorDetail *s) +{ + struct IIO_Sensor *is; + + is = s->private; + OSPDaemon_iio_SetState(is, 1); + + return 0; +} + +static int OSPDaemon_iio_disable(struct OSPDaemon_SensorDetail *s) +{ + struct IIO_Sensor *is; + + is = s->private; + OSPDaemon_iio_SetState(is, 0); + + return 0; +} + +static int OSPDaemon_iio_create(const char *name, struct IIO_Sensor *s) +{ + int rec_sz; + int i; + int fd; + char dname[PATH_MAX]; + + s->iionum = getiionum(name); + if (s->iionum < 0) return -1; + DBGOUT("HY-DBG: %s:%i - IIOnum = %i\n", __func__, __LINE__, s->iionum); + s->name = name; + setupiio(s); + + rec_sz = 0; + for (i = 0; i < MAX_AXIS; i++) { + if (s->IIOAxis[i].index >= 0) { + if (s->IIOAxis[i].dd.store == 0) continue; + + /* print_Desc(&s->IIOAxis[i].dd); */ + /* Make sure each group is aligned */ + DBGOUT("1: rec_sz = %i, s->IIOAxis[i].dd.store = %i, %i,\n", rec_sz, s->IIOAxis[i].dd.store, i); + if (rec_sz % (s->IIOAxis[i].dd.store/8) != 0) { + rec_sz += ((s->IIOAxis[i].dd.store/8)-(rec_sz % (s->IIOAxis[i].dd.store/8))); + } + s->IIOAxis[i].offset = rec_sz; + rec_sz += s->IIOAxis[i].dd.store/8; + DBGOUT("2: rec_sz = %i\n", rec_sz); + } + } + s->rec_sz = rec_sz; + + snprintf(dname, PATH_MAX, "/dev/iio:device%i", s->iionum); + + fd = open(dname, O_RDONLY); + if (!disablepm) { + OSPDaemon_iio_SetState(s, 0); + } + return fd; +} + +static int OSPDaemon_iioevent_create(const char *name, struct IIO_Sensor *s) +{ + int fd, evfd; + char dname[PATH_MAX]; + int ret; + + s->iionum = getiionum(name); + if (s->iionum < 0) return -1; + DBGOUT("HY-DBG: %s:%i - IIOnum = %i\n", __func__, __LINE__, s->iionum); + s->name = name; + +#if 0 + setupiio(s); + + rec_sz = 0; + for (i = 0; i < MAX_AXIS; i++) { + if (s->IIOAxis[i].index >= 0) { + if (s->IIOAxis[i].dd.store == 0) continue; + + /* print_Desc(&s->IIOAxis[i].dd); */ + /* Make sure each group is aligned */ + DBGOUT("1: rec_sz = %i, s->IIOAxis[i].dd.store = %i, %i,\n", rec_sz, s->IIOAxis[i].dd.store, i); + if (rec_sz % (s->IIOAxis[i].dd.store/8) != 0) { + rec_sz += ((s->IIOAxis[i].dd.store/8)-(rec_sz % (s->IIOAxis[i].dd.store/8))); + } + s->IIOAxis[i].offset = rec_sz; + rec_sz += s->IIOAxis[i].dd.store/8; + DBGOUT("2: rec_sz = %i\n", rec_sz); + } + } + s->rec_sz = rec_sz; +#endif + snprintf(dname, PATH_MAX, "/dev/iio:device%i", s->iionum); + + fd = open(dname, O_RDONLY); + /* Can the main fd be closed? */ + ret = ioctl(fd, IIO_GET_EVENT_FD_IOCTL, &evfd); + return evfd; +} + +static int OSPDaemon_iio_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i, j; + int iiocount = 0; + for (i = 0; i < count; i++) { + DBGOUT("Driver: %i, count = %i\n", s->driver, count); + if (s->driver == DRIVER_IIO) { + DBGOUT("IIO Driver: %i\n", s->driver); + for (j = 0; j < MAX_AXIS; j++) { + IIOSen[i].IIOAxis[j].index = -1; + IIOSen[i].index2axis[j] = -1; + } + DBGOUT("%s:%i Setting up: %s\n", __func__, __LINE__, s->name); + s->fd = OSPDaemon_iio_create(s->name, &IIOSen[i]); + s->private = &IIOSen[i]; + IIOSen[i].type = s->sensor.SensorType; + if (s->fd < 0) { + fprintf(stderr, "IIO: Failed on %s\n", s->name); + } + iiocount++; + } else if (s->driver == DRIVER_IIOEVENT) { + DBGOUT("IIO Event Driver: %i\n", s->driver); + s->fd = OSPDaemon_iioevent_create(s->name, &IIOSen[i]); + s->private = &IIOSen[i]; + iiocount++; + + } + s++; + } + DBGOUT("IIO: Found %i iio requests\n", iiocount); + + return 0; +} + +static int OSPDaemon_iio_read(struct OSPDaemon_SensorDetail *s) +{ + char readbuf[2048]; + struct IIO_Sensor *is; + int ret, used; + OSP_InputSensorData_t *od; + + is = s->private; + + ret = read(s->fd, readbuf, 2048); + + if (ret > 0) { + used = 0; + do { + od = parse_iiodata(is, readbuf+used, ret-used); + if (od != NULL) { + DBGOUT("Got data for %s\n", is->name); + OSPDaemon_queue_put(&s->q, od); + } + ret -= is->rec_sz; + used += is->rec_sz; + } while (ret > 0); + } + + return 0; +} + +static int OSPDaemon_iioevent_read(struct OSPDaemon_SensorDetail *s) +{ + int ret; + struct iio_event_data event; + struct IIO_Sensor *is; + + is = s->private; + + ret = read(s->fd, &event, sizeof(event)); + if (ret < 0) { + DBGOUT("Error on event channel read %i\n", errno); + } else { + is->data.booldata.TimeStamp = event.timestamp; + is->data.booldata.data = 1; + OSPDaemon_queue_put(&s->q, &is->data); + } + + return 0; +} +static struct OSPDaemon_driver IIODriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_IIO, + .setup_in = OSPDaemon_iio_setup, + .read = OSPDaemon_iio_read, + .enable_in = OSPDaemon_iio_enable, + .disable_in = OSPDaemon_iio_disable, +}, IIOEventDriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_IIOEVENT, + .setup_in = NULL, + .read = OSPDaemon_iioevent_read, +}; + + +void OSPDaemon_iio_init(void) +{ + OSPDaemon_driver_register(&IIODriver); + OSPDaemon_driver_register(&IIOEventDriver); +} +/* --------------------------------------- */ + +#if 0 + +#include +#include +#include + + +#define MAX_AXIS 5 +#define MAX_IIO 99 +#define MAX_SENSOR 10 + + +enum { + axis_x, + axis_y, + axis_z, + axis_r, + axis_timestamp, + axis_invalid +}; + +static const char *axis_name[] = { + [axis_x] = "X Axis", + [axis_y] = "Y Axis", + [axis_z] = "Z Axis", + [axis_r] = "R Axis", + [axis_timestamp] = "Time Axis", + [axis_invalid] = "INVALID", +}; + +struct DataDesc { + int sign; + int size; + int store; + int shift; +}; + +struct IIO_SensorAxis { + struct DataDesc dd; + int index; + int offset; +}; + + +/* + * List all the available IIO devices and the name. + */ +void dumpiio(void) +{ + int i, j; + FILE *f; + char fname[PATH_MAX]; + char name[1024]; + + for (i = 0; i < MAX_IIO; i++) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/name", i); + fname[PATH_MAX-1] = '\0'; + + f = fopen(fname, "r"); + if (f == NULL) continue; + if (fgets(name, 1023, f) != NULL) { + name[1023] = '\0'; + for (j = 0; j < 1024 && name[j] != '\0'; j++) { + if (name[j] == '\n' || + name[j] == '\r') { + name[j] = '\0'; + break; + } + } + } + fclose(f); + fprintf(stderr, "Device: %s\n", name); + } +} + +void print_Desc(const struct DataDesc *dd) +{ + printf("Data: sign = %i, shift = %i, store = %i, size = %i\n", + dd->sign, dd->shift, dd->store, dd->size); +} + + +void unsetup_iio(struct IIO_Sensor *is, int sencount) +{ + int i, len; + char dname[PATH_MAX]; + char fname[PATH_MAX]; + DIR *d; + struct dirent *dent; + + for (i = 0; i < sencount; i++) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/enable", is[i].iionum); + sysfs_write_val(fname, 0); + + + snprintf(dname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements", is[i].iionum); + + d = opendir(dname); + + while((dent = readdir(d)) != NULL) { + if (dent->d_name[0] == '.') continue; + len = strlen(dent->d_name); + if (len < 6) continue; + if (strcmp(dent->d_name + len - 3, "_en") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is[i].iionum, dent->d_name); + sysfs_write_val(fname, 0); + } + } + + closedir(d); + } +} + + +void mainloop(int count, struct IIO_Sensor *is, + const int sencount, const int reclen) +{ + int fd, ret; + char dname[PATH_MAX]; + char *buf; + int i; + struct pollfd pfd[10]; + int nfd = 0; + int used; + + buf = malloc(reclen*5); + if (buf == NULL) return; + + for (i = 0; i < sencount; i++) { + snprintf(dname, PATH_MAX, "/dev/iio:device%i", is[nfd].iionum); + + fd = open(dname, O_RDONLY); + if (fd < 0) return; + + pfd[nfd].fd = fd; + pfd[nfd].events = POLLIN; + nfd++; + } + while((count < 0) || count > 0) { + if (count > 0) + count--; + + if (poll(pfd, nfd, -1) <= 0) + continue; + for (i = 0; i < sencount; i++) { + if (!(pfd[i].revents&POLLIN)) + continue; + ret = read(pfd[i].fd, buf, reclen*5); + if (ret > 0) + while (0) {printf("Got data %i\n", ret);} + else + continue; + used = 0; + do { + parse_iiodata(&is[i], buf+used, ret); + ret -= is[i].rec_sz; + used+= is[i].rec_sz; + } while (ret > 0); + } + } +} + +#endif diff --git a/linux/daemon/OSPDaemon_iio.h b/linux/daemon/OSPDaemon_iio.h new file mode 100644 index 0000000..c9b4208 --- /dev/null +++ b/linux/daemon/OSPDaemon_iio.h @@ -0,0 +1,43 @@ +#ifndef _OSPDAEMON_IIO_H_ +#define _OSPDAEMON_IIO_H_ +#include "OSPDaemon.h" + +#define MAX_AXIS 5 +#define MAX_IIO 99 +#define MAX_SENSOR 10 + +#define IIO_DEVICE_DIR "/sys/bus/iio/devices" + +enum { + axis_x, + axis_y, + axis_z, + axis_r, + axis_timestamp, + axis_invalid +}; + +struct DataDesc { + int sign; + int size; + int store; + int shift; +}; + +struct IIO_SensorAxis { + struct DataDesc dd; + int index; + int offset; +}; + +struct IIO_Sensor { + struct IIO_SensorAxis IIOAxis[MAX_AXIS]; + OSP_InputSensorData_t data; + int index2axis[MAX_AXIS]; + int iionum; + int rec_sz; + char const *name; + int type; +}; + +#endif diff --git a/linux/daemon/OSPDaemon_input.c b/linux/daemon/OSPDaemon_input.c new file mode 100644 index 0000000..d8903b2 --- /dev/null +++ b/linux/daemon/OSPDaemon_input.c @@ -0,0 +1,173 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License + */ + +/* + * Driver for input events OUTPUT. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_driver.h" + +#define DBGOUT(x...) DBG(DEBUG_INDRIVER, "INPUT: "x) + +static int OSPDaemon_input_create(struct OSPDaemon_output *out) +{ + struct uinput_user_dev uindev; + int fd; + int ret; + + DBGOUT("%s:%i\n", __func__, __LINE__); + + fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (fd < 0) { + ret = fd; + goto error; + } + memset(&uindev, 0, sizeof(uindev)); + + strncpy(uindev.name, out->name, UINPUT_MAX_NAME_SIZE); + uindev.id.bustype = BUS_HOST; + uindev.id.vendor = 0; + uindev.id.product = 0; + uindev.id.version = 1; + uindev.absmin[ABS_X] = 0; + uindev.absmax[ABS_X] = 1023; + uindev.absmin[ABS_Y] = 0; + uindev.absmax[ABS_Y] = 1023; + + ret = write(fd, &uindev, sizeof(uindev)); + if (ret < 0) { + perror("uindev write"); + goto error; + } + + + ret = ioctl(fd, UI_SET_EVBIT, EV_ABS); + if (ret < 0) { + perror("ioctl EV_ABS EVBIT"); + goto error; + } + ret = ioctl(fd, UI_SET_EVBIT, EV_SYN); + if (ret < 0) { + perror("ioctl EV_SYN EVBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_X); + if (ret < 0) { + perror("ioctl ABS_X ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_Y); + if (ret < 0) { + perror("ioctl ABS_Y ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_Z); + if (ret < 0) { + perror("ioctl ABS_Y ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_DEV_CREATE); + if (ret < 0) { + perror("ioctl CREATE"); + goto error; + } + out->fd = fd; + + +error: + if (fd < 0) { + fprintf(stderr, "INPUT: Failed on %s\n", out->name); + } + return ret; +} + +static int OSPDaemon_input_setup(struct OSPDaemon_output *out, int count) +{ + int i; + + for (i = 0; i < count; i++) { + if (out[i].driver == DRIVER_INPUT) + OSPDaemon_input_create(&out[i]); + } + + return 0; +} + +static int OSPDaemon_input_send(struct OSPDaemon_output *out) +{ + OSP_InputSensorData_t *od; + struct input_event ev; + int ret; + static int noise=0; + int val[5], vallen; + unsigned long long ts; + + while ((od = OSPDaemon_queue_get(&out->q)) != NULL) { + vallen = extractOSP(out->type, od, &ts, val); + DBGOUT("Sending (INPUT) %i %i %i\n", val[0], val[1], val[2]); + noise++; + noise %= 2; + ev.type = EV_ABS; + ev.code = ABS_X; + ev.value = val[0]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + + ev.type = EV_ABS; + ev.code = ABS_Y; + ev.value = val[1]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + + ev.type = EV_ABS; + ev.code = ABS_Z; + ev.value = val[2]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + if (vallen > 3) { + ev.type = EV_ABS; + ev.code = ABS_THROTTLE; /* ??? */ + ev.value = val[3]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } + + ev.type = EV_SYN; + ev.code = 0; + ev.value = 0; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } + if (od == NULL) return 0; else return 1; + return 0; +} + +static struct OSPDaemon_driver InputDriver = { + .drvtype = DRIVER_TYPE_OUTPUT, + .driver = DRIVER_INPUT, + .setup_out = OSPDaemon_input_setup, + .send = OSPDaemon_input_send, +}; + +void OSPDaemon_input_init(void) +{ + OSPDaemon_driver_register(&InputDriver); +} diff --git a/linux/daemon/OSPDaemon_inputreader.c b/linux/daemon/OSPDaemon_inputreader.c new file mode 100644 index 0000000..f5e71c9 --- /dev/null +++ b/linux/daemon/OSPDaemon_inputreader.c @@ -0,0 +1,132 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License. + */ + +/* + * Driver for reading data from an input event device. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_inputreader.h" +#include "OSPDaemon_driver.h" + +#define INPUT_EVENT_DIR "/dev/input" + +struct RawBuf { + int val[5]; + unsigned long long ts; +} databuf[MAX_SENSOR]; + +static int OSPDaemon_inputreader_create(char const *name) +{ + int i; + int fd = -1; + char dname[PATH_MAX]; + char evname[80]; + int ret; + + for (i = 0; i < 99; i++) { + snprintf(dname, PATH_MAX, INPUT_EVENT_DIR"/event%i", i); + fd = open(dname, O_RDONLY); + if (fd < 0) continue; + ret = ioctl(fd, EVIOCGNAME(sizeof(evname)-1), &evname); + if (ret < 0) { + close(fd); + continue; + } + close(fd); + if (strncmp(evname, name, 80) == 0) { + break; + } + fd = -1; + } + + return fd; +} + +static int OSPDaemon_inputreader_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i; + int ircount = 0, irsuccess = 0; + DBG(DEBUG_INDRIVER, "InputReader %s:%i\n", __func__, __LINE__); + + for (i = 0; i < count; i++) { + if (s->driver == DRIVER_INPUT) { + s->fd = OSPDaemon_inputreader_create(s->name); + ircount++; + if (s->fd > 0) { + irsuccess++; + s->private = &databuf[i]; + } + } + s++; + } + DBG(DEBUG_INDRIVER, "InputReader: %i request, %i success\n", + ircount, irsuccess); + + return 0; +} + +#define NUM_EVENT 10 + +static int OSPDaemon_inputreader_read(struct OSPDaemon_SensorDetail *s) +{ + int ret; + struct input_event iev[NUM_EVENT]; + int i; + OSP_InputSensorData_t od; + struct RawBuf *d; + int dirty = 0; + + ret = read(s->fd, &iev, sizeof(struct input_event)*NUM_EVENT); + if (ret < 0) return -1; + for (i = 0; i < NUM_EVENT; i++) { + if (sizeof(struct input_event)*i > ret) + break; + if (iev[i].type == EV_REL || iev[i].type == EV_ABS) { + d = s->private; + switch (iev[i].code) { + case ABS_X: + d->val[0] = iev[i].value; + break; + case ABS_Y: + d->val[1] = iev[i].value; + break; + case ABS_Z: + d->val[2] = iev[i].value; + break; + /* Missing Quat R case */ + } + } else if (iev[i].type == EV_SYN) { + d = s->private; + d->ts = iev[i].time.tv_usec; + conv2OSP(s->sensor.SensorType, &od, d->ts, d->val); + OSPDaemon_queue_put(&s->q, &od); + dirty++; + } + } + return dirty; +} +static struct OSPDaemon_driver InputReaderDriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_INPUT, + .setup_in = OSPDaemon_inputreader_setup, + .read = OSPDaemon_inputreader_read, +}; + +void OSPDaemon_inputreader_init(void) +{ + OSPDaemon_driver_register(&InputReaderDriver); +} diff --git a/linux/daemon/OSPDaemon_inputreader.h b/linux/daemon/OSPDaemon_inputreader.h new file mode 100644 index 0000000..edfab19 --- /dev/null +++ b/linux/daemon/OSPDaemon_inputreader.h @@ -0,0 +1,8 @@ +#ifndef _OSPDAEMON_INPUTREADER_H_ +#define _OSPDAEMON_INPUTREADER_H_ +#include "OSPDaemon.h" + +#define MAX_AXIS 5 +#define MAX_SENSOR 10 + +#endif diff --git a/linux/daemon/OSPDaemon_pm.c b/linux/daemon/OSPDaemon_pm.c new file mode 100644 index 0000000..cfde971 --- /dev/null +++ b/linux/daemon/OSPDaemon_pm.c @@ -0,0 +1,112 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_driver.h" +#include "OSPDaemon_pm.h" + +#define DBGOUT(x...) DBG(DEBUG_PM, "PM: "x); + +int OSPDaemon_power_init(struct OSPDaemon_SensorDescription *sd) +{ + int fd; + int ret; + int i; + char fname[PATH_MAX+1]; + + if (sd->powerPath == NULL) { + DBGOUT("No power path defined. Disable PM.\n"); + disablepm = 1; + return -1; + } + + /* Clear out the directory */ + for (i = 0; i < sd->output_count; i++) { + snprintf(fname, PATH_MAX, "%s/%s", + sd->powerPath, sd->output[i].name); + unlink(fname); + } + + fd = inotify_init(); + if (fd < 0) { + DBGOUT("inotify_init failed\n"); + return fd; + } + ret = inotify_add_watch(fd, sd->powerPath, IN_CREATE | IN_DELETE); + if (ret < 0) { + mkdir(sd->powerPath, 0777); + ret = inotify_add_watch(fd, sd->powerPath, + IN_CREATE | IN_DELETE); + if (ret < 0) { + DBGOUT("inotify_add_watch failed\n"); + close(fd); + fd = -1; + } + } + DBGOUT("Successfully added inotify watch for PM using %s\n", sd->powerPath); + + return fd; +} + +static void pm_process_output(struct OSPDaemon_output *out, int mask) +{ + if (mask & IN_CREATE) { + /* Enable sensor */ + out->enable = 1; + OSPDaemon_driver_enable_out(out); + if (!out->noprocess) { + OSP_SubscribeSensorResult(&out->ResultDesc, + &out->handle); + DBGOUT("Subscribe"); + } else + OSPDaemon_driver_enable_in(out->source); + DBGOUT("Enabling %s\n", out->name); + } else if (mask & IN_DELETE) { + /* Disable sensor */ + out->enable = 0; + OSPDaemon_driver_disable_out(out); + if (!out->noprocess) { + OSP_UnsubscribeSensorResult(out->handle); + DBGOUT("UnSubscribe"); + } else + OSPDaemon_driver_disable_in(out->source); + DBGOUT("Disabling %s\n", out->name); + } +} + +int OSPDaemon_power_process(struct OSPDaemon_SensorDescription *sd, int fd) +{ + int i; + int ret; + char buf[2048], *ptr; + struct inotify_event *inptr; + + ret = read(fd, buf, 2048); + if (ret <= 0) + return 0; + ptr = buf; + do { + inptr = (struct inotify_event *)ptr; + for (i = 0; i < sd->output_count; i++) { + if (strcmp(sd->output[i].name, inptr->name) == 0) { + printf("Power event found 0x%x\n", inptr->mask); + pm_process_output(&sd->output[i], inptr->mask); + break; + } + } + ptr += (sizeof(struct inotify_event)+inptr->len); + } while(ptr < (buf+ret)); + + return 0; +} diff --git a/linux/daemon/OSPDaemon_pm.h b/linux/daemon/OSPDaemon_pm.h new file mode 100644 index 0000000..5fa73c5 --- /dev/null +++ b/linux/daemon/OSPDaemon_pm.h @@ -0,0 +1,6 @@ +#ifndef _OSPDAEMON_PM_H_ +#define _OSPDAEMON_PM_H_ + +int OSPDaemon_power_init(struct OSPDaemon_SensorDescription *); +int OSPDaemon_power_process(struct OSPDaemon_SensorDescription *, int); +#endif diff --git a/linux/daemon/OSPDaemon_queue.c b/linux/daemon/OSPDaemon_queue.c new file mode 100644 index 0000000..44f825e --- /dev/null +++ b/linux/daemon/OSPDaemon_queue.c @@ -0,0 +1,58 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License + */ + +/* + * Queue management routes. + */ + +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "osp-api.h" +int OSPDaemon_queue_init(struct queue *q) +{ + if (q == NULL) + return -1; + + q->write = 0; q->read = 0; + + return 0; +} + +/* Data valid til next enqueue op */ +OSP_InputSensorData_t *OSPDaemon_queue_get(struct queue *q) +{ + OSP_InputSensorData_t *d; + + if (q == NULL) return NULL; + + if (q->write == q->read) return NULL; + d = &(q->data[q->read]); + q->read++; + q->read %= QUEUE_LEN; + return d; +} +/* d can be destory after this call */ +int OSPDaemon_queue_put(struct queue *q, OSP_InputSensorData_t *d) +{ + if ((q->write+1)%QUEUE_LEN == q->read) return -1; + memcpy(&(q->data[q->write]), d, sizeof(OSP_InputSensorData_t)); + q->write++; + q->write %= QUEUE_LEN; + + return 0; +} + + +/* Checks a queue for emptyness. + * 1 - empty + * 0 - there is data. + */ +int OSPDaemon_queue_isempty(struct queue *q) +{ + return (q->write == q->read)?1:0; +} diff --git a/linux/daemon/OSPDaemon_queue.h b/linux/daemon/OSPDaemon_queue.h new file mode 100644 index 0000000..79cae45 --- /dev/null +++ b/linux/daemon/OSPDaemon_queue.h @@ -0,0 +1,20 @@ +#ifndef _OSPDAEMON_QUEUE_H_ +#define _OSPDAEMON_QUEUE_H_ 1 + +#define QUEUE_LEN 25 +#include "osp-api.h" +struct queue { + int write; + int read; + OSP_InputSensorData_t data[QUEUE_LEN]; +}; + +int OSPDaemon_queue_init(struct queue *q); + +/* Pointer to data is valid til the next queue op */ +OSP_InputSensorData_t *OSPDaemon_queue_get(struct queue *q); +/* Pointer to data only needs to be valid during this call. */ +int OSPDaemon_queue_put(struct queue *q, OSP_InputSensorData_t *d); +int OSPDaemon_queue_isempty(struct queue *q); + +#endif From 7dd5c285ad42ff6f491ec2456ac0fd762ca6e12a Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 18 Sep 2015 16:34:27 -0700 Subject: [PATCH 03/21] Linux kernel driver for an I2C interfaced sensor hub This is not for a particular kernel and can be adapted to most kernels. This driver uses I2C to talk to an OSP sensor hub. Two output options are offered: - input events - IIO See README for installation details. Signed-off-by: Hunyue Yau --- linux/driver/MQ_sensors.h | 85 +-- linux/driver/README | 63 ++ linux/driver/SensorPackets.h | 274 ++++---- linux/driver/linux/osp-sh.h | 33 + linux/driver/osp-iio.c | 1203 ++++++++++++++++++++++++++++++++++ linux/driver/osp-input.c | 305 +++++++++ linux/driver/osp-sh.c | 787 ++++++---------------- 7 files changed, 2006 insertions(+), 744 deletions(-) create mode 100644 linux/driver/README create mode 100644 linux/driver/linux/osp-sh.h create mode 100644 linux/driver/osp-iio.c create mode 100644 linux/driver/osp-input.c diff --git a/linux/driver/MQ_sensors.h b/linux/driver/MQ_sensors.h index c789428..fe7ed7d 100644 --- a/linux/driver/MQ_sensors.h +++ b/linux/driver/MQ_sensors.h @@ -1,4 +1,4 @@ -/**************************************************************************************************** +/***************************************************************** * @file MQ_sensors.h * * Sensors related enumerations @@ -11,60 +11,63 @@ * $Author: rverma $ * $DateTime: 2014/10/16 15:32:24 $ * $Revision: #4 $ - * $Id: //AudEngr/Hardware/DeltaPlus/Software/BoskoApp_D300_SensorHub/include/MQ_sensors.h#4 $ * * Copyright 2012 Audience, Incorporated. All rights reserved. * * This software is the confidential and proprietary information of - * Audience Inc. ("Confidential Information"). You shall not disclose - * such Confidential Information and shall use it only in accordance - * with the Terms of Sale of Audience products and the terms of any + * Audience Inc. ("Confidential Information"). You shall not disclose + * such Confidential Information and shall use it only in accordance + * with the Terms of Sale of Audience products and the terms of any * license agreement you entered into with Audience for such products. - * - * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY - * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, - * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE - * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR - * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE - * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF - * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. * - ***************************************************************************************************/ + * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY + * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, + * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE + * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR + * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE + * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF + * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. + * + ****************************************************************************/ #if !defined (MQ_SENSORS_H) #define MQ_SENSORS_H -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | I N C L U D E F I L E S -\*-------------------------------------------------------------------------------------------------*/ -/* This file is meant to provide a common definition of sensor related enumerations/defines and - * generally should not depend on any other includes +\*--------------------------------------------------------------------------*/ +/* This file is meant to provide a common definition of sensor related + * enumerations/defines and generally should not depend on any other + * includes */ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | C O N S T A N T S & M A C R O S -\*-------------------------------------------------------------------------------------------------*/ -#define SENSOR_SUBTYPE_UNUSED 0 //!< Subtype is not used for the sensor type -#define SENSOR_SUBTYPE_START 1 //!< Subtype enumeration starts with 1 -#define SENSOR_DEVICE_PRIVATE_BASE 0x10000 //!< Android defined private sensor type base - -#define M_PSensorToAndroidBase(type) ((type) | SENSOR_DEVICE_PRIVATE_BASE) +\*--------------------------------------------------------------------------*/ +#define SENSOR_SUBTYPE_UNUSED 0 //!< Subtype is not used for the sensor type +#define SENSOR_SUBTYPE_START 1 //!< Subtype enumeration starts with 1 +#define SENSOR_DEVICE_PRIVATE_BASE 0x10000 //!< Android defined private sensor type base +#define M_PSensorToAndroidBase(type) ((type) | SENSOR_DEVICE_PRIVATE_BASE) -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | T Y P E D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ //! use to specify the kind of sensor input or output /*! * \sa OSP_RegisterInputSensor * \sa OSP_SubscribeOutputSensor * - * Final units of input/outputs are defined by the sensor data convention field of the sensor descriptor. - * Flags in the descriptor specify if sensor is calibrated/uncalibrated and/or used as input - * If a sensor type not is supported by the library implementation, an error will be returned on its usage + * Final units of input/outputs are defined by the sensor data convention + * field of the sensor descriptor. Flags in the descriptor specify if + * sensor is calibrated/uncalibrated and/or used as input. If a sensor + * type not is supported by the library implementation, an error will be + * returned on its usage */ -/* Android sensor types - this should match the defines/enumeration in Android's sensors.h */ -/* Note that these are defined as SENSOR_ instead of SENSOR_TYPE_ to avoid clash with Android defines in - * situations where this header in included for packet processing in Sensor HAL +/* Android sensor types - this should match the defines/enumeration in + * Android's sensors.h */ +/* Note that these are defined as SENSOR_ instead of SENSOR_TYPE_ to + * avoid clash with Android defines in situations where this header in + * included for packet processing in Sensor HAL */ typedef enum _ASensorType { SENSOR_META_DATA = 0, @@ -213,20 +216,20 @@ typedef struct TriAxisRawData_t int32_t Axis[3]; } TriAxisRawData_t; -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | E X T E R N A L V A R I A B L E S & F U N C T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | P U B L I C V A R I A B L E S D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*---------------------------------------------------------------------------*\ | P U B L I C F U N C T I O N D E C L A R A T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*---------------------------------------------------------------------------*/ #endif /* MQ_SENSORS_H */ -/*-------------------------------------------------------------------------------------------------*\ +/*---------------------------------------------------------------------------*\ | E N D O F F I L E -\*-------------------------------------------------------------------------------------------------*/ +\*---------------------------------------------------------------------------*/ diff --git a/linux/driver/README b/linux/driver/README new file mode 100644 index 0000000..bead904 --- /dev/null +++ b/linux/driver/README @@ -0,0 +1,63 @@ +Linux kernel driver for OSP sensor hub +-------------------------------------- +The driver is composed of 2 parts. A core OSP packet parsing driver +and a driver responsible for outputing to userland in the desire fashion. +The 2 parts communicate with a defined API. The userland driver attaches +as a child driver of the main OSP driver. + +The API between the 2 parts is defined by linux/osp-sh.h. The API +consist of a callback registration to claim output for a particular +sensor type along with an enable/disable call. Once registered, the call +back will be called wit the sensor data. + +Provided is 2 userland interface drivers. One provides data using the +input event system. The other driver provides it using the IIO subsystem. + +Installation +------------ +Copy the contents of the linux directory into include/linux on your +kernel tree. + +Place osp-sh.c in an appropriate place and modify the Makefile to build it. +It needs to be able to access the following header files. +MQ_sensors.h +SensorPackets.h +osp_i2c_map.h + +Connect the OSP I2C device with the osp-sh driver. The process will +depend on the version of the kernel. Currently, there is no specific +hooks for device tree. For pre-device tree kernels, the board file +will need to be modified or userland will need to manually instantiate +the device. + +For the IIO option: (only 1 can be used at a time) +Place osp-iio.c in the appropriate place for IIO drivers in your tree. This +is often under drivers/iio or drivers/staging/iio. Modify the Makefile to +pickup the driver. + +For the input event ouption: +Place osp-input.c in the appropriate place. For example, drivers/input/misc. +Modify the Makefile to pickup the driver. + +Notes: + +Input driver needs to be manually enabled using the enable_new attribute +associated with the device. + +IIO driver will automatically enable/disable with the triggers. However, +an additional control is available with the enable attribute. This allows +the HAL to enable/disable the driver without changing the poll descriptor. +[Is there a better way using triggers?] + +The IIO driver provides data in 2 ways. One is a general IIO channel. This +is used for sensors that report actual numeric data. For example, an +accelerometer or a step counter. The second way is using IIO events. +This is used for one shot sensors. The step detector and significant +motion are examples of events. A single IIO device can generate channel +data AND events if they are logically connected. For example, the step +counter driver outputs channel data for the count. It also reports step +detector data using events. Most IIO sensors can also report data +using sysfs attributes. This is for debugging and non buffered output. + +Each sensor is presented as a seperate device. They are identified by +name. diff --git a/linux/driver/SensorPackets.h b/linux/driver/SensorPackets.h index 7a309d6..d5bd61f 100644 --- a/linux/driver/SensorPackets.h +++ b/linux/driver/SensorPackets.h @@ -1,4 +1,4 @@ -/**************************************************************************************************** +/***************************************************************************** * @file SensorPackets.h * * Definitions and macros for host inteface packet format @@ -11,102 +11,101 @@ * $Author: rverma $ * $DateTime: 2014/11/13 17:24:40 $ * $Revision: #8 $ - * $Id: //AudEngr/Hardware/DeltaPlus/Software/BoskoApp_D300_SensorHub/overlay/SensorHub/HostInterface/SensorPackets.h#8 $ * * Copyright 2012 Audience, Incorporated. All rights reserved. * * This software is the confidential and proprietary information of - * Audience Inc. ("Confidential Information"). You shall not disclose - * such Confidential Information and shall use it only in accordance - * with the Terms of Sale of Audience products and the terms of any + * Audience Inc. ("Confidential Information"). You shall not disclose + * such Confidential Information and shall use it only in accordance + * with the Terms of Sale of Audience products and the terms of any * license agreement you entered into with Audience for such products. - * - * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY - * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, - * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE - * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR - * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE - * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF - * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. * - ***************************************************************************************************/ + * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY + * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, + * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE + * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR + * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE + * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF + * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. + * + ****************************************************************************/ #if !defined (SENSOR_PACKETS_H) #define SENSOR_PACKETS_H -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | I N C L U D E F I L E S - \*-------------------------------------------------------------------------------------------------*/ + \*--------------------------------------------------------------------------*/ #if 0 #include #include #endif #include "MQ_sensors.h" -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | C O N S T A N T S & M A C R O S - \*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ /********************************************************/ /* COMMON PACKET DEFINITION */ /********************************************************/ /* Packet Identifier for version 0 packets */ -#define PKID_SENSOR_DATA 0x00 -#define PKID_CONTROL_REQ 0x10 -#define PKID_CONTROL_RESP 0x20 -#define PKID_TEST_DATA 0x30 -#define PKID_MASK_VER0 0xF0 +#define PKID_SENSOR_DATA 0x00 +#define PKID_CONTROL_REQ 0x10 +#define PKID_CONTROL_RESP 0x20 +#define PKID_TEST_DATA 0x30 +#define PKID_MASK_VER0 0xF0 /** =============== CONTROL ID BYTE =============== */ -#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) -#define SENSOR_TYPE_ANDROID 0x0 -#define SENSOR_TYPE_PRIVATE 0x1 +#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) +#define SENSOR_TYPE_ANDROID 0x0 +#define SENSOR_TYPE_PRIVATE 0x1 /** =============== SENSOR ID BYTE =============== */ -#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) -#define M_ParseSensorMetaData(i) ((uint8_t)((i >> 6) & 0x03)) -#define M_SensorType(s) ((uint8_t)(s & 0x3F)) +#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) +#define M_ParseSensorMetaData(i) ((uint8_t)((i >> 6) & 0x03)) +#define M_SensorType(s) ((uint8_t)(s & 0x3F)) /** =============== ATTRIBUTE BYTE =============== */ -#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) -#define M_ParseSensorSubType(st) ((uint8_t)((st >> 4) & 0x0F)) +#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) +#define M_ParseSensorSubType(st) ((uint8_t)((st >> 4) & 0x0F)) /* Checksum option (check sum is always 16-bit CRC if enabled) */ -#define CHECK_SUM_PRESENT 0x01 +#define CHECK_SUM_PRESENT 0x01 /********************************************************/ /* SENSOR DATA PACKET */ /********************************************************/ /** =============== CONTROL BYTE =============== */ /* CRC option */ -#define CRC_ENABLE 0x08 +#define CRC_ENABLE 0x08 /*Enumeration type of sensor*/ -#define SENSOR_ANDROID_TYPE_MASK 0x01 +#define SENSOR_ANDROID_TYPE_MASK 0x01 /* Format for data values */ -#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor values */ -#define DATA_FORMAT_FIXPOINT 0x04 /* Fixed point format in application defined units */ -#define DATA_FORMAT_MASK 0x04 +#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor values */ +#define DATA_FORMAT_FIXPOINT 0x04 /* Fixed point format in application defined units */ +#define DATA_FORMAT_MASK 0x04 /* Time Format option */ -#define TIME_FORMAT_RAW 0x00 /* RAW count values for time stamp base */ -#define TIME_FORMAT_FIXPOINT 0x02 /* Fixed point format specified in seconds */ -#define TIME_FORMAT_MASK 0x02 +#define TIME_FORMAT_RAW 0x00 /* RAW count values for time stamp base */ +#define TIME_FORMAT_FIXPOINT 0x02 /* Fixed point format specified in seconds */ +#define TIME_FORMAT_MASK 0x02 /** ============ SENSOR IDENTIFIER BYTE ========== */ -#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ -#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ +#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ +#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ /** =============== ATTRIBUTE BYTE =============== */ /* Data size option */ -#define DATA_SIZE_8_BIT 0x00 -#define DATA_SIZE_16_BIT 0x02 -#define DATA_SIZE_32_BIT 0x04 -#define DATA_SIZE_64_BIT 0x06 -#define DATA_SIZE_MASK 0x06 +#define DATA_SIZE_8_BIT 0x00 +#define DATA_SIZE_16_BIT 0x02 +#define DATA_SIZE_32_BIT 0x04 +#define DATA_SIZE_64_BIT 0x06 +#define DATA_SIZE_MASK 0x06 /* Time Stamp Size */ -#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ -#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ -#define TIME_STAMP_SIZE_MASK 0x01 +#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ +#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ +#define TIME_STAMP_SIZE_MASK 0x01 /********************************************************/ /* SENSOR CONTROL REQ/RESP PACKET */ @@ -114,77 +113,77 @@ /** =============== CONTROL BYTE =============== */ /* First 5 MS bits are same as Sensor Data Packet */ /* Data Format */ -#define CTRL_PKT_DF_INTEGER 0x00 -#define CTRL_PKT_DF_FIXPOINT 0x02 -#define CTRL_PKT_DF_FLOAT 0x04 -#define CTRL_PKT_DF_DOUBLE 0x06 +#define CTRL_PKT_DF_INTEGER 0x00 +#define CTRL_PKT_DF_FIXPOINT 0x02 +#define CTRL_PKT_DF_FLOAT 0x04 +#define CTRL_PKT_DF_DOUBLE 0x06 /* A/P Definition same as Sensor Data Packet */ /** =============== ATTRIBUTE BYTE 1 =============== */ /* Sequence Number for request/response */ -#define M_SequenceNum(sNum) ((sNum) & 0x0F) +#define M_SequenceNum(sNum) ((sNum) & 0x0F) /** =============== ATTRIBUTE BYTE 2 =============== */ /* Parameter ID */ -#define M_GetParamId(AttrByte2) ((AttrByte2) >> 3) -#define M_SetParamId(id) (((id) & 0x1F) << 3) +#define M_GetParamId(AttrByte2) ((AttrByte2) >> 3) +#define M_SetParamId(id) (((id) & 0x1F) << 3) /* Parameter Size */ -#define M_GetParamSize(AttrByte2) ((AttrByte2) & 0x07) +#define M_GetParamSize(AttrByte2) ((AttrByte2) & 0x07) /* Param data size */ -#define PARAM_DATA_SZ_8_BIT 0x00 -#define PARAM_DATA_SZ_16_BIT 0x01 -#define PARAM_DATA_SZ_32_BIT 0x02 -#define PARAM_DATA_SZ_64_BIT 0x03 -#define PARAM_DATA_SZ_BOOL_FALSE 0x04 -#define PARAM_DATA_SZ_BOOL_TRUE 0x05 -#define PARAM_DATA_SZ_UNKNOWN 0x07 +#define PARAM_DATA_SZ_8_BIT 0x00 +#define PARAM_DATA_SZ_16_BIT 0x01 +#define PARAM_DATA_SZ_32_BIT 0x02 +#define PARAM_DATA_SZ_64_BIT 0x03 +#define PARAM_DATA_SZ_BOOL_FALSE 0x04 +#define PARAM_DATA_SZ_BOOL_TRUE 0x05 +#define PARAM_DATA_SZ_UNKNOWN 0x07 /* Byte extraction macros */ -#define BYTE3(x) ((uint8_t)((x >> 24) & 0xFF)) -#define BYTE2(x) ((uint8_t)((x >> 16) & 0xFF)) -#define BYTE1(x) ((uint8_t)((x >> 8) & 0xFF)) -#define BYTE0(x) ((uint8_t)(x & 0xFF)) +#define BYTE3(x) ((uint8_t)((x >> 24) & 0xFF)) +#define BYTE2(x) ((uint8_t)((x >> 16) & 0xFF)) +#define BYTE1(x) ((uint8_t)((x >> 8) & 0xFF)) +#define BYTE0(x) ((uint8_t)(x & 0xFF)) /* 16-bit & 32-bit data combine macros */ -#define BYTES_TO_SHORT(b1,b0) ((int16_t)(((int16_t)b1 << 8) | b0)) +#define BYTES_TO_SHORT(b1,b0) ((int16_t)(((int16_t)b1 << 8) | b0)) #define BYTES_TO_LONG_ARR(arr,ind) BYTES_TO_LONG(arr[ind],arr[ind+1],arr[ind+2],arr[ind+3]) -#define BYTES_TO_LONG(b3,b2,b1,b0) \ +#define BYTES_TO_LONG(b3,b2,b1,b0) \ ((int32_t)(((int32_t)b3 << 24) | ((uint32_t)b2 << 16) | ((uint32_t)b1 << 8) | b0)) -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | T Y P E D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ /* Definition for quaternion data packets for internal usage */ typedef struct _QuaternionFixP { - TimeStamp_t TimeStamp; - int32_t Quat[4]; //W,X,Y,Z order + TimeStamp_t TimeStamp; + int32_t Quat[4]; //W,X,Y,Z order } QuaternionFixP_t; typedef struct _UncalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z order - int32_t Offset[3]; //XOFF,YOFF,ZOFF order + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z order + int32_t Offset[3]; //XOFF,YOFF,ZOFF order } UncalibratedFixP_t; typedef struct _CalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z, + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z, } CalibratedFixP_t; /* Union of the structures that can be parsed out of Sensor Data packet */ typedef struct _SensorPacketTypes { - union { - TriAxisRawData_t RawSensor; - UncalibratedFixP_t UncalFixP; - CalibratedFixP_t CalFixP; - QuaternionFixP_t QuatFixP; - } P; - ASensorType_t SType; - uint8_t SubType; + union { + TriAxisRawData_t RawSensor; + UncalibratedFixP_t UncalFixP; + CalibratedFixP_t CalFixP; + QuaternionFixP_t QuatFixP; + } P; + ASensorType_t SType; + uint8_t SubType; } SensorPacketTypes_t; /* ========== Host Inteface Packet definitions ========== */ @@ -195,84 +194,97 @@ typedef struct _SensorPacketTypes { /* Sensor Packet Type qualifier */ typedef struct _HifSensorPktQualifier { - uint8_t ControlByte; - uint8_t SensorIdByte; - uint8_t AttributeByte; + uint8_t ControlByte; + uint8_t SensorIdByte; + uint8_t AttributeByte; } HifSnsrPktQualifier_t; /* Basic packet: Raw 16-bit data, 32-bit integer time stamp; No checksum */ typedef struct _HifSensorDataRaw { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[4]; //32-bit Time Stamp - uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[4]; //32-bit Time Stamp + uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data } HifSensorDataRaw_t; -#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) +#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) -/* Uncalibrated data packet: 32-bit Fixedpoint uncalibrated; 64-bit Fixedpoint time stamp; No Checksum */ +/* Uncalibrated data packet: + * 32-bit Fixedpoint uncalibrated; + * 64-bit Fixedpoint time stamp; + * No Checksum */ typedef struct _HifUncalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data - uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data + uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset } HifUncalibratedFixPoint_t; -#define UNCALIB_FIXP_DATA_PKT_SZ (offsetof(HifUncalibratedFixPoint_t, Offset)) -#define UNCALIB_FIXP_DATA_OFFSET_PKT_SZ sizeof(HifUncalibratedFixPoint_t) +#define UNCALIB_FIXP_DATA_PKT_SZ (offsetof(HifUncalibratedFixPoint_t, Offset)) +#define UNCALIB_FIXP_DATA_OFFSET_PKT_SZ sizeof(HifUncalibratedFixPoint_t) /* Calibrated data packet: 32-bit Fixedpoint Calibrated; 64-bit Fixedpoint time stamp; No Checksum */ typedef struct _HifCalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data } HifCalibratedFixPoint_t; -#define CALIBRATED_FIXP_DATA_PKT_SZ sizeof(HifCalibratedFixPoint_t) +#define CALIBRATED_FIXP_DATA_PKT_SZ sizeof(HifCalibratedFixPoint_t) /* Quaternion data packet: 32-bit Fixedpoint quaternion; 64-bit Fixedpoint time stamp; No Checksum */ typedef struct _HifQuaternionFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[16]; //4-Axis Fixed point 32-bit quaternion data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[16]; //4-Axis Fixed point 32-bit quaternion data } HifQuaternionFixPoint_t; -#define QUATERNION_FIXP_DATA_PKT_SZ sizeof(HifQuaternionFixPoint_t) +#define QUATERNION_FIXP_DATA_PKT_SZ sizeof(HifQuaternionFixPoint_t) /* Sensor control packet for enable/disable sensor */ typedef struct _HifSensorEnable { - HifSnsrPktQualifier_t Q; - uint8_t AttrByte2; + HifSnsrPktQualifier_t Q; + uint8_t AttrByte2; } HifSensorEnable_t; -#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorEnable_t) +#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorEnable_t) /* Define union for all the host interface packet types */ typedef union _HostIFPackets { - HifSensorDataRaw_t SensPktRaw; - HifUncalibratedFixPoint_t UncalPktFixP; - HifCalibratedFixPoint_t CalPktFixP; - HifQuaternionFixPoint_t QuatPktFixP; - HifSensorEnable_t Enable; + HifSensorDataRaw_t SensPktRaw; + HifUncalibratedFixPoint_t UncalPktFixP; + HifCalibratedFixPoint_t CalPktFixP; + HifQuaternionFixPoint_t QuatPktFixP; + HifSensorEnable_t Enable; } HostIFPackets_t; -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | E X T E R N A L V A R I A B L E S & F U N C T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | P U B L I C V A R I A B L E S D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | P U B L I C F U N C T I O N D E C L A R A T I O N S -\*-------------------------------------------------------------------------------------------------*/ -int16_t FormatSensorDataPktRaw( uint8_t *pDest, const TriAxisRawData_t *pSensData, uint8_t metaData, ASensorType_t sType, uint8_t subType ); -int16_t FormatQuaternionPktFixP( uint8_t *pDest, const QuaternionFixP_t *pQuatData, ASensorType_t sType ); -int16_t FormatUncalibratedPktFixP( uint8_t *pDest, const UncalibratedFixP_t *pUncalData, uint8_t metaData, ASensorType_t sType ); -int16_t FormatCalibratedPktFixP( uint8_t *pDest, const CalibratedFixP_t *pCalData, ASensorType_t sType ); -int16_t ParseHostIntefacePkt( SensorPacketTypes_t *pOut, uint8_t *pPacket, uint16_t pktSize ); +\*--------------------------------------------------------------------------*/ +int16_t FormatSensorDataPktRaw(uint8_t *pDest, + const TriAxisRawData_t *pSensData, + uint8_t metaData, ASensorType_t sType, + uint8_t subType); +int16_t FormatQuaternionPktFixP(uint8_t *pDest, + const QuaternionFixP_t *pQuatData, + ASensorType_t sType); +int16_t FormatUncalibratedPktFixP(uint8_t *pDest, + const UncalibratedFixP_t *pUncalData, + uint8_t metaData, ASensorType_t sType); +int16_t FormatCalibratedPktFixP(uint8_t *pDest, + const CalibratedFixP_t *pCalData, + ASensorType_t sType); +int16_t ParseHostIntefacePkt(SensorPacketTypes_t *pOut, + uint8_t *pPacket, uint16_t pktSize); #endif /* SENSOR_PACKETS_H */ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | E N D O F F I L E -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ diff --git a/linux/driver/linux/osp-sh.h b/linux/driver/linux/osp-sh.h new file mode 100644 index 0000000..44c8bc1 --- /dev/null +++ b/linux/driver/linux/osp-sh.h @@ -0,0 +1,33 @@ +#ifndef _OSP_SH_H_ +#define _OSP_SH_H_ + +union OSP_SensorData { + struct { + u32 x; + u32 y; + u32 z; + u64 ts; + } xyz; + struct { + u32 x; + u32 y; + u32 z; + u32 r; + u64 ts; + } quat; +}; + +/* Register for sensor data. Only one call back per sensor.*/ +int OSP_Sensor_Register(int sensor, /* Sensor name and space */ + int private, + void (*dataready)(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata), + void *prv /* Private data for callback use */ + ); + +int OSP_Sensor_UnRegister(int sensor, int private); + +int OSP_Sensor_State(int sensor, int private, int state); + +#endif diff --git a/linux/driver/osp-iio.c b/linux/driver/osp-iio.c new file mode 100644 index 0000000..96e5570 --- /dev/null +++ b/linux/driver/osp-iio.c @@ -0,0 +1,1203 @@ +/** + * Copyright (c) 2015 Hunyue Yau for Audience + * + * Based on iio_simple_dummy by: + * Copyright (c) 2011 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + * + * A reference industrial I/O driver to illustrate the functionality available. + * + * There are numerous real drivers to illustrate the finer points. + * The purpose of this driver is to provide a driver with far more comments + * and explanatory notes than any 'real' driver would have. + * Anyone starting out writing an IIO driver should first make sure they + * understand all of this driver except those bits specifically marked + * as being present to allow us to 'fake' the presence of hardware. + + * Simple OSP IIO output layer. + */ + +#include +#include +#include +#include +#include +#include + +#include "iio.h" +#include "sysfs.h" +#include "events.h" +#include "buffer.h" +#include "kfifo_buf.h" +#include "trigger.h" +#include "trigger_consumer.h" +#include + +#include "MQ_sensors.h" + +/* Simulate periodic data */ +static struct timer_list osp_timer; + +enum { + axis_x, + axis_y, + axis_z, + axis_r, +}; + +static ssize_t osp_iio_renable(struct device *dev, + struct device_attribute *attr, char *buf); +static ssize_t osp_iio_wenable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len); + +static IIO_DEVICE_ATTR(enable, S_IWUSR|S_IRUSR|S_IRGRP|S_IWGRP, + &osp_iio_renable, &osp_iio_wenable, 0); + +static struct attribute *osp_iio_attrs[] = { + &iio_dev_attr_enable.dev_attr.attr, + NULL, +}; + +static const struct attribute_group osp_iio_group = { + .attrs = osp_iio_attrs, +}; +/* + * Description of available channels + * + * This array of structures tells the IIO core about what the device + * actually provides for a given channel. + */ +static struct iio_chan_spec accel_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 0, + .address = 0, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 1, + .address = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 2, + .address = 2, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static struct iio_chan_spec gyro_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static struct iio_chan_spec mag_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .scan_type = IIO_ST('s', 32, 32, 12), + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + }, + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec linacc_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec orient_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec rotvec_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_R, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_r, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static struct iio_chan_spec press_channels[] = { + { + .type = IIO_PRESSURE, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; +struct osp_iio_sensor { + struct iio_dev *indio_dev; + struct iio_trigger *trigger; + struct mutex lock; + int sensor; + int private; + int state; + long long ts; + union OSP_SensorData data; +}; + +struct osp_iio_sensor *osp_sensors_and[NUM_ANDROID_SENSOR_TYPE]; +struct osp_iio_sensor *osp_sensors_prv[NUM_PRIVATE_SENSOR_TYPE]; + +static int osp_sensor_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask); +static int osp_sensor_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask); +/* + * Device type specific information. + */ +static const struct iio_info osp_sensor_info = { + .driver_module = THIS_MODULE, + .read_raw = &osp_sensor_read_raw, + .write_raw = &osp_sensor_write_raw, + .attrs = &osp_iio_group, +}; + +static const struct OSP_SensorDesc { + char *name; + char *trigname; + struct iio_chan_spec *channels; + int num_channels; + struct iio_info const *info; + int usebuffer; + int useevent; +} and_sensor[NUM_ANDROID_SENSOR_TYPE] = { + [SENSOR_GYROSCOPE] = { + .name = "osp_gyro", + .trigname = "osp_gyro", + .channels = gyro_channels, + .num_channels = ARRAY_SIZE(gyro_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GYROSCOPE_UNCALIBRATED] = { + .name = "osp_uncal_gyro", + .trigname = "osp_uncal_gyro", + .channels = gyro_channels, + .num_channels = ARRAY_SIZE(gyro_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ACCELEROMETER] = { + .name = "osp_accel", + .trigname = "osp_accel", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GRAVITY] = { + .name = "osp_gravity", + .trigname = "osp_gravity", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_MAGNETIC_FIELD] = { + .name = "osp_mag", + .trigname = "osp_mag", + .channels = mag_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = { + .name = "osp_uncal_mag", + .trigname = "osp_uncal_mag", + .channels = mag_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ORIENTATION] = { + .name = "osp_compass_orientation", + .trigname = "osp_compass_orientation", + .channels = orient_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_LINEAR_ACCELERATION] = { + .name = "osp_linear_acceleration", + .trigname = "osp_linear_acceleration", + .channels = linacc_channels, + .num_channels = ARRAY_SIZE(linacc_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ROTATION_VECTOR] = { + .name = "osp_rotation_vector", + .trigname = "osp_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GAME_ROTATION_VECTOR] = { + .name = "osp_game_rotation_vector", + .trigname = "osp_game_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = { + .name = "osp_geo_rotation_vector", + .trigname = "osp_geo_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_SIGNIFICANT_MOTION] = { + .name = "osp_sigmot", + .trigname = "osp_sigmot", + .channels = NULL, + .num_channels = 0, + .info = &osp_sensor_info, + .usebuffer = 0, + .useevent = 1, + }, + [SENSOR_STEP_COUNTER] = { + .name = "osp_step", + .trigname = "osp_step", + .channels = NULL, + .num_channels = 0, + .info = &osp_sensor_info, + .usebuffer = 0, + .useevent = 1, + }, + [SENSOR_PRESSURE] = { + .name = "osp_pressure", + .trigname = "osp_pressure", + .channels = NULL, + .num_channels = 0, + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, +}, prv_sensor[NUM_PRIVATE_SENSOR_TYPE] = { + [PSENSOR_ACCELEROMETER_RAW] = { + .name = "osp_accel_raw", + .trigname = "osp_accel_raw", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [PSENSOR_ACCELEROMETER_UNCALIBRATED] = { + .name = "osp_uncal_accel", + .trigname = "osp_uncal_accel", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [PSENSOR_CONTEXT_DEVICE_MOTION] = { + .name = "osp_context", + .trigname = "osp_context", + .usebuffer = 0, + }, +}; + +/* ------- sysfs ------- */ + +static ssize_t osp_iio_renable(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + ssize_t len = 0; + + len += sprintf(buf, "%i\n", osp_sensor->state); + + return len; +} + +static ssize_t osp_iio_wenable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + int en; + + if (buf[0] == '1') + en = 1; + else + en = 0; + + OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, en); + osp_sensor->state = en; + + return len; +} + +/* --------------------- */ + +/** + * osp_sensor_read_raw() - data read function. + * @indio_dev: the struct iio_dev associated with this device instance + * @chan: the channel whose data is to be read + * @val: first element of returned value (typically INT) + * @val2: second element of returned value (typically MICRO) + * @mask: what we actually want to read. 0 is the channel, everything else + * is as per the info_mask in iio_chan_spec. + */ +static int osp_sensor_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) +{ + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + struct osp_iio_sensor *st = osp_sensor; + int ret = -EINVAL; + + mutex_lock(&st->lock); + switch (mask) { + case 0: /* magic value - channel value read */ + switch (chan->type) { + case IIO_ACCEL: + case IIO_ANGL_VEL: + case IIO_MAGN: + switch (chan->scan_index) { + case axis_x: + *val = osp_sensor->data.xyz.x; + break; + case axis_y: + *val = osp_sensor->data.xyz.y; + break; + case axis_z: + *val = osp_sensor->data.xyz.z; + break; + default: + break; + } + ret = IIO_VAL_INT; + break; + + default: + break; + } + break; + default: + break; + } + mutex_unlock(&st->lock); + return ret; +} + +/** + * osp_sensor_write_raw() - data write function. + * @indio_dev: the struct iio_dev associated with this device instance + * @chan: the channel whose data is to be read + * @val: first element of returned value (typically INT) + * @val2: second element of returned value (typically MICRO) + * @mask: what we actually want to read. 0 is the channel, everything else + * is as per the info_mask in iio_chan_spec. + * + * Note that all raw writes are assumed IIO_VAL_INT and info mask elements + * are assumed to be IIO_INT_PLUS_MICRO unless the callback write_raw_get_fmt + * in struct iio_info is provided by the driver. + */ +static int osp_sensor_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, + int val2, + long mask) +{ +#if 0 + int i; + int ret = 0; + struct iio_dummy_state *st = iio_priv(indio_dev); + + switch (mask) { + case 0: + if (chan->output == 0) + return -EINVAL; + + /* Locking not required as writing single value */ + mutex_lock(&st->lock); + st->dac_val = val; + mutex_unlock(&st->lock); + return 0; + case IIO_CHAN_INFO_CALIBBIAS: + mutex_lock(&st->lock); + /* Compare against table - hard matching here */ + for (i = 0; i < ARRAY_SIZE(dummy_scales); i++) + if (val == dummy_scales[i].val && + val2 == dummy_scales[i].val2) + break; + if (i == ARRAY_SIZE(dummy_scales)) + ret = -EINVAL; + else + st->accel_calibscale = &dummy_scales[i]; + mutex_unlock(&st->lock); + return ret; + default: + return -EINVAL; + } +#else + return 0; +#endif +} + +/** + * osp_iio_sensor_init() - Sets up sensor specific details. + * @indio_dev: the iio device structure + * + * Most drivers have one of these to set up default values, + * reset the device to known state etc. + * + * Init data to known values for debugging. + */ +static int osp_iio_sensor_init(struct iio_dev *indio_dev, int sensor, int type) +{ + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + osp_sensor->indio_dev = indio_dev; + memset(&osp_sensor->data, 0, sizeof(osp_sensor->data)); + osp_sensor->sensor = sensor; + + if (type == 0) { + switch (sensor) { + case SENSOR_ACCELEROMETER: + osp_sensor->data.xyz.x = 0; + osp_sensor->data.xyz.y = 1; + osp_sensor->data.xyz.z = 2; + break; + case SENSOR_GYROSCOPE: + osp_sensor->data.xyz.x = 2002; + osp_sensor->data.xyz.y = 2003; + osp_sensor->data.xyz.z = 2004; + break; + case SENSOR_MAGNETIC_FIELD: + osp_sensor->data.xyz.x = 4002; + osp_sensor->data.xyz.y = 4003; + osp_sensor->data.xyz.z = 4004; + break; + case SENSOR_ROTATION_VECTOR: + osp_sensor->data.quat.x = 6001; + osp_sensor->data.quat.y = 6002; + osp_sensor->data.quat.z = 6003; + osp_sensor->data.quat.r = 1004; + break; + } + } + + return 0; +} + +static int osp_iio_trigger(struct iio_trigger *trig, bool state) +{ + struct iio_dev *indio_dev = trig->private_data; + struct osp_iio_sensor *osp_sensor; + osp_sensor = iio_priv(indio_dev); + + OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, state); + osp_sensor->state = state; + + return 0; +} + +static irqreturn_t osp_iio_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + iio_push_to_buffer(indio_dev->buffer, (u8 *)&osp_sensor->data, + osp_sensor->ts); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static const struct iio_trigger_ops osp_iio_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = &osp_iio_trigger, +}; + +static const struct iio_buffer_setup_ops osp_iio_buffer_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &iio_triggered_buffer_postenable, + .predisable = &iio_triggered_buffer_predisable, +}; + +/* + * Call back from core OSP code for data ready. + * Store data and trigger if buffers used. + */ +static void dataready(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata) +{ + struct iio_dev *indio_dev = private; + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + osp_sensor->ts = ts; + osp_sensor->data = *sensordata; + if (osp_sensor->private == 0 && and_sensor[sensor].useevent) { + switch (sensor) { + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_COUNTER: + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_INTENSITY, 0, + IIO_MOD_X, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + ts); + break; + } + } + if ((osp_sensor->private == 0 && and_sensor[sensor].usebuffer) || + (osp_sensor->private == 1 && prv_sensor[sensor].usebuffer)) + iio_trigger_poll(osp_sensor->trigger, osp_sensor->ts); +} + +static int osp_iio_destroy(int sensor, int space) +{ + struct iio_dev *indio_dev; + int ret = 0; + struct osp_iio_sensor **osp_sensors; + + if (space == 0) + osp_sensors = osp_sensors_and; + else + osp_sensors = osp_sensors_prv; + + indio_dev = osp_sensors[sensor]->indio_dev; + + /* Disable the sensor */ + OSP_Sensor_State(sensor, space, 0); + osp_sensors[sensor]->state = 0; + + /* Unregister the device */ + iio_device_unregister(indio_dev); + + /* Device specific code to power down etc */ + + /* Buffered capture related cleanup */ + iio_buffer_unregister(indio_dev); +#if 0 + iio_simple_dummy_unconfigure_buffer(indio_dev); +#endif + /* Free all structures */ + iio_free_device(indio_dev); + +error_ret: + return ret; +} + +/** + * osp_iio_create() - Create a IIO sensor instance. + * index - sensor id. enum space is determined by value of space. + * space - enum space for sensors. + * dev - caller used pointer. + */ +static int osp_iio_create(int index, int space, void *dev) +{ + int ret; + struct iio_dev *indio_dev; + struct osp_iio_sensor *st; + struct OSP_SensorDesc *desc; + struct osp_iio_sensor **osp_sensor; + + if (space == 1) { /* Private sensors */ + if (index >= NUM_PRIVATE_SENSOR_TYPE || index < 0) + return -EINVAL; + desc = prv_sensor; + osp_sensor = osp_sensors_prv; + } else if (space == 0) { /* Android sensors */ + if (index >= NUM_ANDROID_SENSOR_TYPE || index < 0) + return -EINVAL; + desc = and_sensor; + osp_sensor = osp_sensors_and; + } else { + return -EINVAL; + } + /* + * Allocate an IIO device. + * + * This structure contains all generic state + * information about the device instance. + * It also has a region (accessed by iio_priv() + * for chip specific state information. + */ + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + ret = -ENOMEM; + goto error_ret; + } + + st = iio_priv(indio_dev); + st->private = space; + st->state = 0; + st->data.xyz.x = 0; + st->data.xyz.y = 0; + st->data.xyz.z = 0; + mutex_init(&st->lock); + osp_iio_sensor_init(indio_dev, index, 0); + + indio_dev->dev.parent = dev; + + osp_sensor[index] = st; + + /* + * Set the device name. + * + * This is typically a part number and obtained from the module + * id table. + * e.g. for i2c and spi: + * indio_dev->name = id->name; + * indio_dev->name = spi_get_device_id(spi)->name; + */ + indio_dev->name = desc[index].name; + + /* Provide description of available channels */ + indio_dev->channels = desc[index].channels; + indio_dev->num_channels = desc[index].num_channels; + + /* + * Provide device type specific interface functions and + * constant data. + */ + indio_dev->info = desc[index].info; + + /* Specify that device provides sysfs type interfaces */ + indio_dev->modes = INDIO_DIRECT_MODE | + INDIO_BUFFER_TRIGGERED; + st->trigger = iio_allocate_trigger(desc[index].trigname); + + if (st->trigger) { + st->trigger->ops = &osp_iio_trigger_ops; + iio_trigger_register(st->trigger); + st->trigger->private_data = indio_dev; + } + + if (desc[index].usebuffer) { +#if 1 + indio_dev->pollfunc = iio_alloc_pollfunc( + &iio_pollfunc_store_time, + &osp_iio_trigger_handler, + IRQF_ONESHOT, indio_dev, + desc[index].trigname); + + indio_dev->buffer = iio_kfifo_allocate(indio_dev); + indio_dev->setup_ops = &osp_iio_buffer_setup_ops; + + ret = iio_buffer_register(indio_dev, + desc[index].channels, + desc[index].num_channels); + + indio_dev->buffer->scan_timestamp = 1; + +#else + ret = iio_triggered_buffer_setup(indio_dev, + &iio_pollfunc_store_time, + &osp_iio_trigger_handler, NULL); + if (ret < 0) + goto error_free_device; +#endif + } + OSP_Sensor_Register(index, space, dataready, indio_dev); + ret = iio_device_register(indio_dev); + if (ret < 0) + goto error_unregister_buffer; + + return 0; +error_unregister_buffer: + iio_buffer_unregister(indio_dev); +#if 0 +error_unconfigure_buffer: + iio_simple_dummy_unconfigure_buffer(indio_dev); +error_unregister_events: + iio_simple_dummy_events_unregister(indio_dev); +#endif +error_free_device: + /* Note free device should only be called, before registration + * has succeeded. */ + iio_free_device(indio_dev); +error_ret: + return ret; +} + +/* Dummy signal generator */ +static void osp_poll_timer(unsigned long unused) +{ +#if 0 + int i; + struct osp_iio_sensor *osp_sensor;; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (osp_sensors_and[i]) { + osp_sensor = osp_sensors_and[i]; + if (i == SENSOR_ROTATION_VECTOR) { + osp_sensor->data.quat.x++; + osp_sensor->data.quat.y++; + osp_sensor->data.quat.z++; + osp_sensor->data.quat.r++; + osp_sensor->data.quat.ts = 0x3fffffff00000000LL+jiffies; + } else { + osp_sensor->data.xyz.x++; + osp_sensor->data.xyz.y++; + osp_sensor->data.xyz.z++; + osp_sensor->data.xyz.ts = 0x7fffffff00000000LL+jiffies; + } + if (and_sensor[i].usebuffer) + iio_trigger_poll(osp_sensor->trigger, osp_sensor->ts); + } + } + mod_timer(&osp_timer, jiffies+msecs_to_jiffies(500)); +#endif +} + +static int osp_iio_probe(struct platform_device *pdev) +{ + int ret, i; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + osp_sensors_and[i] = NULL; + } + + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + osp_sensors_prv[i] = NULL; + } +#if 0 + osp_iio_create(SENSOR_GYROSCOPE, 0, NULL); + osp_iio_create(SENSOR_GYROSCOPE_UNCALIBRATED, 0, NULL); + osp_iio_create(SENSOR_ACCELEROMETER, 0, NULL); + osp_iio_create(SENSOR_GRAVITY, 0, NULL); + osp_iio_create(SENSOR_MAGNETIC_FIELD, 0, NULL); + osp_iio_create(SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 0, NULL); + osp_iio_create(SENSOR_ORIENTATION, 0, NULL); + osp_iio_create(SENSOR_LINEAR_ACCELERATION, 0, NULL); + osp_iio_create(SENSOR_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_GAME_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_GEOMAGNETIC_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_SIGNIFICANT_MOTION, 0, NULL); + osp_iio_create(SENSOR_STEP_COUNTER, 0, NULL); +#endif +#if 1 + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (and_sensor[i].name) { + ret = osp_iio_create(i, 0, NULL); + if (ret < 0) { + printk("Cannot create %s\n", and_sensor[i].name); + } + } + } +#if 0 + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + if (prv_sensor[i].name) { + ret = osp_iio_create(i, 1, NULL); + if (ret < 0) { + printk("Cannot create %s\n", and_sensor[i].name); + } + } + } +#endif +#endif +#if 0 + ret = osp_iio_create(SENSOR_ACCELEROMETER, 0, NULL); + ret = osp_iio_create(SENSOR_GYROSCOPE, 0, NULL); + ret = osp_iio_create(SENSOR_MAGNETIC_FIELD, 0, NULL); + ret = osp_iio_create(SENSOR_ROTATION_VECTOR, 0, NULL); + ret = osp_iio_create(SENSOR_ORIENTATION, 0, NULL); + ret = osp_iio_create(SENSOR_LINEAR_ACCELERATION, 0, NULL); + ret = osp_iio_create(SENSOR_GRAVITY, 0, NULL); + ret = osp_iio_create(SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 0, NULL); +#endif +#if 0 + /* For dummy signal generator */ + if (ret >= 0) { + setup_timer(&osp_timer, osp_poll_timer, 0); + mod_timer(&osp_timer, jiffies + msecs_to_jiffies(1000)); + } +#endif + return 0; +} + +static int osp_iio_remove(struct platform_device *pdev) +{ + return 0; +} +static struct platform_driver osp_iio_driver = { + .probe = osp_iio_probe, + .remove = osp_iio_remove, + .driver = { + .name = "osp-output", + .owner = THIS_MODULE, + }, +}; +static __init int osp_iio_init(void) +{ + return platform_driver_register(&osp_iio_driver); +} +module_init(osp_iio_init); + +/** + * osp_iio_exit() - device driver removal + * + * Varies depending on bus type of the device. + * As there is no device here, call remove directly. + */ +static __exit void osp_iio_exit(void) +{ + int i; + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) + if (osp_sensors_and[i]) { + osp_iio_destroy(i, osp_sensors_and[i]->private); + } + + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) + if (osp_sensors_prv[i]) { + osp_iio_destroy(i, osp_sensors_and[i]->private); + } +} +module_exit(osp_iio_exit); + +MODULE_AUTHOR("Hunyue Yau "); +MODULE_DESCRIPTION("OSP IIO driver"); +MODULE_LICENSE("GPL v2"); diff --git a/linux/driver/osp-input.c b/linux/driver/osp-input.c new file mode 100644 index 0000000..ec93006 --- /dev/null +++ b/linux/driver/osp-input.c @@ -0,0 +1,305 @@ +/* + * Copyright (C) 2041 Audience, Inc. + * Written by Hunyue Yau + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA + * 02111-1307, USA + */ + +#include +#include +#include +#include +#include +#include "MQ_sensors.h" +#include + +/* INPUT_ABS CONSTANTS */ +#define FUZZ 3 +#define FLAT 3 +#define G_MAX 8000 +#if 0 +static char *in_pname[NUM_PRIVATE_SENSOR_TYPE] = { + [PSENSOR_ACCELEROMETER_RAW] = "acc_raw", + [PSENSOR_ACCELEROMETER_UNCALIBRATED] = "acc_uncal", +}; +#endif + +struct osp_input_sensor { + struct input_dev *input_dev; + char *name; +} and_sensor[NUM_ANDROID_SENSOR_TYPE] = { + [SENSOR_ACCELEROMETER] = { + .name = "fm-accelerometer", + }, + [SENSOR_MAGNETIC_FIELD] = { + .name = "fm-magnetometer", + }, + [SENSOR_GYROSCOPE] = { + .name = "fm-gyroscope", + }, + [SENSOR_ROTATION_VECTOR] = { + .name = "fm-rotation-vector", + }, + [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = { + .name = "fm-uncalibrated-magnetometer", + }, + [SENSOR_GYROSCOPE_UNCALIBRATED] = { + .name = "fm-uncalibrated-gyroscope", + }, + [SENSOR_ORIENTATION] = { + .name = "fm-compass-orientation", + }, + [SENSOR_LINEAR_ACCELERATION] = { + .name = "fm-linear-acceleration", + }, + [SENSOR_GRAVITY] = { + .name = "fm-gravity", + }, + [SENSOR_STEP_COUNTER] = { + .name = "fm-step-counter", + }, + [SENSOR_STEP_DETECTOR] = { + .name = "fm-step-detector", + }, + [SENSOR_SIGNIFICANT_MOTION] = { + .name = "fm-significant-motion", + }, + [SENSOR_PRESSURE] = { + .name = "pressure", + }, + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = { + .name = "fm-geomagnetic-rotation-vector", + }, + [SENSOR_GAME_ROTATION_VECTOR] = { + .name = "fm-game-rotation-vector", + }, +}; + +static ssize_t osp_get_enable(struct device *dev, struct device_attribute *attr, char *buf) +{ + return 0; +} + +static ssize_t osp_set_enable(struct device *dev, struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input = to_input_dev(dev); + struct osp_input_sensor *osp_input = input_get_drvdata(input); + int i; + unsigned long v; + + if (strict_strtoul(buf, 0, &v)) + return -EINVAL; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (and_sensor[i].input_dev == input) { + break; + } + } + + if (i == NUM_ANDROID_SENSOR_TYPE) + return -EINVAL; + OSP_Sensor_State(i, 0, v); + + return count; +} +#if 0 +static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR, osp_get_enable, osp_set_enable); +#else +static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR|S_IWUGO, osp_get_enable, osp_set_enable); +#endif + +static struct attribute *osp_attributes[] = { + &dev_attr_enable_new.attr, + NULL +}; + +static struct attribute_group osp_attribute_group = { + .attrs = osp_attributes +}; + +static int osp_input_open(struct input_dev *input) +{ + return 0; +} + +static void osp_input_close(struct input_dev *dev) +{ +} + +static void dataready(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata) +{ + struct osp_input_sensor *sen = private; + struct input_dev *input_dev = sen->input_dev; + static int counter = 0; + int PSensor; + + if (prv != 0) return; + + switch (sensor) { + case SENSOR_STEP_DETECTOR: + case SENSOR_SIGNIFICANT_MOTION: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, 1); + input_report_abs(input_dev, ABS_GAS, counter); + counter++; + input_sync(input_dev); + break; + case SENSOR_STEP_COUNTER: + case SENSOR_PRESSURE: + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_GYROSCOPE_UNCALIBRATED: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case SENSOR_ACCELEROMETER: + case SENSOR_MAGNETIC_FIELD: + case SENSOR_GYROSCOPE: + case SENSOR_ORIENTATION: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + if (!input_dev) break; + input_report_abs(input_dev, ABS_GAS, sensordata->quat.r); + input_report_abs(input_dev, ABS_X, sensordata->quat.x); + input_report_abs(input_dev, ABS_Y, sensordata->quat.y); + input_report_abs(input_dev, ABS_Z, sensordata->quat.z); + input_sync(input_dev); + break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED|SENSOR_DEVICE_PRIVATE_BASE: + PSensor = sensor & ~(SENSOR_DEVICE_PRIVATE_BASE); + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case PSENSOR_ACCELEROMETER_RAW|SENSOR_DEVICE_PRIVATE_BASE: + PSensor = sensor & ~(SENSOR_DEVICE_PRIVATE_BASE); + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + default: + break; + } +} + + +static int osp_input_create(struct device *dev, int sensor, int private) +{ + struct input_dev *input_dev; + struct osp_input_sensor *sen; + int err; + + if (private == 0) { + sen = and_sensor; + } else { + return -EINVAL; + } + + input_dev = input_allocate_device(); + if (!input_dev) { + dev_err(dev, "input device allocate failed\n"); + return -ENOMEM; + } + + sen[sensor].input_dev = input_dev; + + input_dev->open = osp_input_open; + input_dev->close = osp_input_close; + input_set_drvdata(input_dev, &sen[sensor]); + + __set_bit(EV_ABS, input_dev->evbit); + input_set_abs_params(input_dev, ABS_X, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_Y, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_Z, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_THROTTLE, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_GAS, -G_MAX, G_MAX, FUZZ, FLAT); + + input_dev->name = sen[sensor].name; + input_dev->id.bustype = BUS_I2C; + input_dev->dev.parent = dev; + input_dev->relbit[0] |= BIT_MASK(REL_X); + + err = input_register_device(input_dev); + if (err) { + dev_err(dev, + "unable to register input polled device %s: %d\n", + input_dev->name, err); + input_free_device(input_dev); + return err; + } + err = sysfs_create_group(&input_dev->dev.kobj, &osp_attribute_group); + if (err) { + printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); + } + + OSP_Sensor_Register(sensor, private, dataready, &sen[sensor]); + + return 0; +} + +static int osp_input_probe(struct platform_device *pdev) +{ + osp_input_create(&pdev->dev, SENSOR_ACCELEROMETER, 0); + osp_input_create(&pdev->dev, SENSOR_MAGNETIC_FIELD, 0); + osp_input_create(&pdev->dev, SENSOR_GYROSCOPE, 0); + osp_input_create(&pdev->dev, SENSOR_ROTATION_VECTOR, 0); + osp_input_create(&pdev->dev, SENSOR_ORIENTATION, 0); + osp_input_create(&pdev->dev, SENSOR_LINEAR_ACCELERATION, 0); + osp_input_create(&pdev->dev, SENSOR_GRAVITY, 0); + + return 0; +} +static int osp_input_remove(struct platform_device *pdev) +{ + return 0; +} + +static struct platform_driver osp_input_driver = { + .probe = osp_input_probe, + .remove = osp_input_remove, + .driver = { + .name = "osp-output", + .owner = THIS_MODULE, + }, +}; + +static int __init osp_input_init(void) +{ + return platform_driver_register(&osp_input_driver); +} + +module_init(osp_input_init); + + +MODULE_DESCRIPTION("OSP Input driver"); +MODULE_AUTHOR("Hunyue Yau "); +MODULE_LICENSE("GPL"); diff --git a/linux/driver/osp-sh.c b/linux/driver/osp-sh.c index c1d0433..d8baea5 100644 --- a/linux/driver/osp-sh.c +++ b/linux/driver/osp-sh.c @@ -1,6 +1,6 @@ /* * Copyright (C) 2041 Audience, Inc. - * Written by Hunyue Yau + * Written by Hunyue Yau * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -19,44 +19,38 @@ #include #include -#include #include #include #include -#include #include +#include #include "MQ_sensors.h" #include "SensorPackets.h" #include "osp_i2c_map.h" +#include #define NAME "ospsh" -/* INPUT_ABS CONSTANTS */ -#define FUZZ 3 -#define FLAT 3 -#define G_MAX 8000 - -static char *in_name[NUM_ANDROID_SENSOR_TYPE] = { - [SENSOR_ACCELEROMETER] = "fm-accelerometer", - [SENSOR_MAGNETIC_FIELD] = "fm-magnetometer", - [SENSOR_GYROSCOPE] = "fm-gyroscope", - [SENSOR_ROTATION_VECTOR] = "fm-rotation-vector", - [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = "fm-uncalibrated-magnetometer", - [SENSOR_GYROSCOPE_UNCALIBRATED] = "fm-uncalibrated-gyroscope", - [SENSOR_ORIENTATION] = "fm-compass-orientation", - [SENSOR_LINEAR_ACCELERATION] = "fm-linear-acceleration", - [SENSOR_GRAVITY] = "fm-gravity", - [SENSOR_STEP_COUNTER] = "fm-step-counter", - [SENSOR_STEP_DETECTOR] = "fm-step-detector", - [SENSOR_SIGNIFICANT_MOTION] = "fm-significant-motion", - [SENSOR_PRESSURE] = "pressure", - [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = "fm-geomagnetic-rotation-vector", - [SENSOR_GAME_ROTATION_VECTOR] = "fm-game-rotation-vector", -}; -static char *in_pname[NUM_PRIVATE_SENSOR_TYPE] = { - [PSENSOR_ACCELEROMETER_RAW] = "acc_raw", - [PSENSOR_ACCELEROMETER_UNCALIBRATED] = "acc_uncal", +static struct OSP_Sensor { + void (*dataready)(int sensor, int prv, + void *private, long long ts, union OSP_SensorData *sensordata); + void *private; +} and_sensor[NUM_ANDROID_SENSOR_TYPE], prv_sensor[NUM_PRIVATE_SENSOR_TYPE]; + +struct osp_data { + struct i2c_client *client; + struct timer_list osp_timer; + struct work_struct osp_work; + struct mutex lock; }; + +static struct osp_data *gOSP; +static struct work_queue *osp_workq; + +/* Number of packets to parse */ +#define NUM_PACK 2 +static unsigned char *osp_pack[NUM_PACK]; + /* ------------ OSP Packet parsing code -------------------- */ /*********************************************************************** * @fn ParseSensorDataPkt @@ -154,10 +148,8 @@ static int OSP_ParseSensorDataPkt_Private( } break; case SYSTEM_REAL_TIME_CLOCK: - printk("HY-DBG: %s:%i type %i\n", __func__, __LINE__, sensType); break; default: - printk("HY-DBG: %s:%i type %i\n", __func__, __LINE__, sensType); break; } if (errCode == 0) @@ -247,7 +239,6 @@ static int OSP_ParseSensorDataPkt_Android( } break; case SENSOR_SIGNIFICANT_MOTION: - printk("HY-DBG: %s:%i\n", __func__, __LINE__); case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_GYROSCOPE_UNCALIBRATED: case SENSOR_PRESSURE: @@ -294,7 +285,6 @@ static int OSP_ParseSensorDataPkt_Android( { int i; unsigned char *p; - printk("HY-DBG: %s:%i type %i: ", __func__, __LINE__, sensType); p = (unsigned char *)pHif; for (i = 0; i < 8; i++) { printk("%02x ", p[i]); @@ -333,7 +323,7 @@ static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, timeFormat = pHif->SensPktRaw.Q.ControlByte & TIME_FORMAT_MASK; tSize = pHif->SensPktRaw.Q.AttributeByte & TIME_STAMP_SIZE_MASK; - /* Check Sensor enumeration type Android or Private */ + /* Check Sensor enumeration type Android or Private */ if (!isPrivateType) { /*Sensor Enumeration type is Android*/ errCode = OSP_ParseSensorDataPkt_Android(pOut, pHif, @@ -355,75 +345,31 @@ static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, } /* ------------ END OSP Packet parsing code -------------------- */ -struct osp_data { - struct i2c_client *client; - struct input_dev *input_dev; - struct input_polled_dev *poll_dev; - struct input_dev *in_dev[NUM_ANDROID_SENSOR_TYPE]; - struct input_dev *in_pdev[NUM_PRIVATE_SENSOR_TYPE]; - struct timer_list osp_timer; - struct work_struct osp_work; -}; +static int OSP_Sensor_enable(struct osp_data *osp, int sensor, int private) +{ + int retval = -EINVAL; -static struct osp_data *gOSP; + /* Private not supported yet */ + if (private != 0) return -EINVAL; -static struct work_queue *osp_workq; + if (sensor < 0x30) + retval = i2c_smbus_read_byte_data(osp->client, 0x20+sensor); -static ssize_t osp_get_enable(struct device *dev, struct device_attribute *attr, char *buf) -{ - return 0; + return retval; } -static ssize_t osp_set_enable(struct device *dev, struct device_attribute *attr, - const char *buf, size_t count) +static int OSP_Sensor_disable(struct osp_data *osp, int sensor, int private) { - struct input_dev *input = to_input_dev(dev); - struct osp_data *osp = input_get_drvdata(input); - int i; - int retval; - unsigned long v; - - if (strict_strtoul(buf, 0, &v)) - return -EINVAL; - - for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == input) { - break; - } - } + int retval = -EINVAL; - if (i == NUM_ANDROID_SENSOR_TYPE) - return -EINVAL; + /* Private not supported yet */ + if (private != 0) return -EINVAL; - printk("HY-DBG: %s:%i - osp %p\n", __func__, __LINE__, osp); - if (v) { - printk("HY-DBG: %s:%i - enabling %i\n", __func__, __LINE__, i); - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x20+i); - } - } else { - printk("HY-DBG: %s:%i - disabling %i\n", __func__, __LINE__, i); - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x50+i); - } - } + if (sensor < 0x30) + retval = i2c_smbus_read_byte_data(osp->client, 0x50+sensor); - return count; + return retval; } -#if 0 -static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR, osp_get_enable, osp_set_enable); -#else -static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR|S_IWUGO, osp_get_enable, osp_set_enable); -#endif - -static struct attribute *osp_attributes[] = { - &dev_attr_enable_new.attr, - NULL -}; - -static struct attribute_group osp_attribute_group = { - .attrs = osp_attributes -}; static int osp_i2c_read(struct osp_data *osp, u8 addr, u8 *data, int len) { @@ -474,394 +420,224 @@ static int osp_i2c_read(struct osp_data *osp, u8 addr, u8 *data, int len) #endif } -#if 0 -static irqreturn_t osp_isr(int irq, void *dev) -{ - struct osp_data *osp = dev; - int err; - - /* data ready is the only possible interrupt type */ - osp_report_acceleration_data(tj9); - - err = i2c_smbus_read_byte_data(tj9->client, INT_REL); - if (err < 0) - dev_err(&osp->client->dev, - "error clearing interrupt status: %d\n", err); - return IRQ_HANDLED; -} -#endif -static int osp_enable(struct osp_data *osp) -{ - return 0; -} static void osp_disable(struct osp_data *osp) { } -static int osp_input_open(struct input_dev *input) +/* ------------ Call back management code -------------- */ +static void OSP_CB_init(void) { - struct osp_data *osp = input_get_drvdata(input); -#if 0 int i; - int retval; for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == input) { - break; - } + and_sensor[i].dataready = NULL; } - printk("HY-DBG: %s:%i - closing %i\n", __func__, __LINE__, i); - - if (i == NUM_ANDROID_SENSOR_TYPE) - return 0; - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x20+i); + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + prv_sensor[i].dataready = NULL; } -#endif - return osp_enable(osp); } -static void osp_input_close(struct input_dev *dev) +/* API Call: Register a callback for a sensor */ +int OSP_Sensor_Register(int sensor, int private, + void (*dataready)(int sensor, int prv, + void *private, long long ts, union OSP_SensorData *sensordata), + void *prv) { - struct osp_data *osp = input_get_drvdata(dev); -#if 0 - int i; - int retval; + struct OSP_Sensor *sen; - for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == dev) { - break; - } - } + if (private == 1) + sen = prv_sensor; + else if (private == 0) + sen = and_sensor; + else + return -EINVAL; - printk("HY-DBG: %s:%i - closing %i\n", __func__, __LINE__, i); + if (sen[sensor].dataready != NULL) + return -EBUSY; - if (i == NUM_ANDROID_SENSOR_TYPE) - return; - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x50+i); - } -#endif - osp_disable(osp); -} + sen[sensor].dataready = dataready; + sen[sensor].private = prv; -static void osp_init_input_device(struct osp_data *osp, - struct input_dev *input_dev, char *name) -{ - __set_bit(EV_ABS, input_dev->evbit); - input_set_abs_params(input_dev, ABS_X, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_Y, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_Z, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_THROTTLE, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_GAS, -G_MAX, G_MAX, FUZZ, FLAT); - - input_dev->name = name; - input_dev->id.bustype = BUS_I2C; - input_dev->dev.parent = &osp->client->dev; - input_dev->relbit[0] |= BIT_MASK(REL_X); + return 0; } +EXPORT_SYMBOL_GPL(OSP_Sensor_Register); -static int osp_setup_other_input(struct osp_data *osp) +/* API Call: Release a sensor */ +int OSP_Sensor_UnRegister(int sensor, int private) { - int i = 0, j = 0; - struct input_dev *input_dev; - int err; + struct OSP_Sensor *sen; - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - osp->in_dev[i] = NULL; - } - for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { - osp->in_pdev[i] = NULL; - } + if (private == 1) + sen = prv_sensor; + else if (private == 0) + sen = and_sensor; + else + return -EINVAL; - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - err = -ENOMEM; - goto err_input_alloc; - } else { - osp->in_dev[i] = input_dev; - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - osp_init_input_device(osp, input_dev, in_name[i]); - } - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_pname[j] == NULL) - continue; - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - err = -ENOMEM; - goto err_input_alloc; - } else { - osp->in_pdev[j] = input_dev; - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - osp_init_input_device(osp, input_dev, in_pname[j]); - } - } - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - err = input_register_device(osp->in_dev[i]); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->in_dev[i]->name, err); - goto err_input_reg; - } - err = sysfs_create_group(&osp->in_dev[i]->dev.kobj, &osp_attribute_group); - if (err) { - printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); - } - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_pname[j] == NULL) - continue; - err = input_register_device(osp->in_pdev[j]); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->in_pdev[j]->name, err); - goto err_input_reg; - } - } + sen->dataready = NULL; return 0; -err_input_reg: - i--; - for (; i > 1; i--) { - if (in_name[i] == NULL) - continue; - input_unregister_device(osp->in_dev[i]); - } - if (j != 0) { - j--; - for (; j > 1; j--) { - if (in_pname[j] == NULL) - continue; - input_unregister_device(osp->in_dev[j]); - } - } - -err_input_alloc: - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - if (osp->in_dev) - input_free_device(osp->in_dev[i]); - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_name[j] == NULL) - continue; - if (osp->in_dev) - input_free_device(osp->in_pdev[j]); - } - - return err; } -static int osp_setup_input_device(struct osp_data *osp) +EXPORT_SYMBOL_GPL(OSP_Sensor_UnRegister); +/* API Call: Activates/suspends a sensor */ +int OSP_Sensor_State(int sensor, int private, int state) { - struct input_dev *input_dev; - int err; - - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - return -ENOMEM; - } + struct OSP_Sensor *sen; - osp->input_dev = input_dev; - osp->in_dev[1] = input_dev; - - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - - osp_init_input_device(osp, input_dev, in_name[1]); + if (private == 1) { + sen = prv_sensor; + return -EINVAL; /* Private not supported yet */ + } else if (private == 0) + sen = and_sensor; + else + return -EINVAL; - err = input_register_device(osp->input_dev); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->input_dev->name, err); - input_free_device(osp->input_dev); - return err; + if (state == 1) { + OSP_Sensor_enable(gOSP, sensor, private); + } else { + OSP_Sensor_disable(gOSP, sensor, private); } - err = sysfs_create_group(&osp->in_dev[1]->dev.kobj, &osp_attribute_group); - if (err) { - printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); - } - osp_setup_other_input(osp); return 0; } +EXPORT_SYMBOL_GPL(OSP_Sensor_State); -static void OSP_ReportSensor(struct osp_data *osp, +static void OSP_ReportSensor(struct osp_data *osp, SensorPacketTypes_t *spack) { int PSensor; static int counter = 0; + union OSP_SensorData data; - /* printk("HY-DBG:%s:%i(%i)\n", __func__,__LINE__, spack->SType); */ switch(spack->SType) { case SENSOR_STEP_DETECTOR: case SENSOR_SIGNIFICANT_MOTION: - printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); - - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, 1); - input_report_abs(osp->in_dev[spack->SType], ABS_GAS, counter); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = 1; + data.xyz.y = counter; counter++; - input_sync(osp->in_dev[spack->SType]); + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, jiffies, &data); break; case SENSOR_STEP_COUNTER: case SENSOR_PRESSURE: - printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.CalFixP.Axis[0]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, jiffies, &data); + break; case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_GYROSCOPE_UNCALIBRATED: - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.UncalFixP.Axis[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.UncalFixP.Axis[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.UncalFixP.Axis[2]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.UncalFixP.Axis[0]; + data.xyz.y = spack->P.UncalFixP.Axis[1]; + data.xyz.z = spack->P.UncalFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, jiffies, &data); break; case SENSOR_ACCELEROMETER: case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: case SENSOR_ORIENTATION: - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.CalFixP.Axis[0], spack->P.CalFixP.Axis[1], spack->P.CalFixP.Axis[2]); */ - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.CalFixP.Axis[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.CalFixP.Axis[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.CalFixP.Axis[2]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.CalFixP.Axis[0]; + data.xyz.y = spack->P.CalFixP.Axis[1]; + data.xyz.z = spack->P.CalFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, jiffies, &data); break; case SENSOR_ROTATION_VECTOR: case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: case SENSOR_GAME_ROTATION_VECTOR: - /* printk("HY-DBG:%s:%i(%i) %p\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType]); */ - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_GAS, spack->P.QuatFixP.Quat[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.QuatFixP.Quat[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.QuatFixP.Quat[2]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.QuatFixP.Quat[3]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.quat.r = spack->P.QuatFixP.Quat[0]; + data.quat.x = spack->P.QuatFixP.Quat[1]; + data.quat.y = spack->P.QuatFixP.Quat[2]; + data.quat.z = spack->P.QuatFixP.Quat[3]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, jiffies, &data); break; case PSENSOR_ACCELEROMETER_UNCALIBRATED|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_pdev[PSensor], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); */ - if (!osp->in_pdev[PSensor]) break; - input_report_abs(osp->in_pdev[PSensor], ABS_X, spack->P.UncalFixP.Axis[0]); - input_report_abs(osp->in_pdev[PSensor], ABS_Y, spack->P.UncalFixP.Axis[1]); - input_report_abs(osp->in_pdev[PSensor], ABS_Z, spack->P.UncalFixP.Axis[2]); - input_sync(osp->in_pdev[PSensor]); + if (!prv_sensor[PSensor].dataready) break; + + data.xyz.x = spack->P.UncalFixP.Axis[0]; + data.xyz.y = spack->P.UncalFixP.Axis[1]; + data.xyz.z = spack->P.UncalFixP.Axis[2]; + prv_sensor[PSensor].dataready(PSensor, 1, + prv_sensor[PSensor].private, jiffies, &data); break; case PSENSOR_ACCELEROMETER_RAW|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_pdev[PSensor], spack->P.RawSensor.Axis[0], spack->P.RawSensor.Axis[1], spack->P.RawSensor.Axis[2]); */ - if (!osp->in_pdev[PSensor]) break; - input_report_abs(osp->in_pdev[PSensor], ABS_X, spack->P.RawSensor.Axis[0]); - input_report_abs(osp->in_pdev[PSensor], ABS_Y, spack->P.RawSensor.Axis[1]); - input_report_abs(osp->in_pdev[PSensor], ABS_Z, spack->P.RawSensor.Axis[2]); - input_sync(osp->in_pdev[PSensor]); + if (!prv_sensor[PSensor].dataready) break; + data.xyz.x = spack->P.RawSensor.Axis[0]; + data.xyz.y = spack->P.RawSensor.Axis[1]; + data.xyz.z = spack->P.RawSensor.Axis[2]; + prv_sensor[PSensor].dataready(PSensor, 1, + prv_sensor[PSensor].private, jiffies, &data); break; default: break; } - /* printk("HY-DBG:%s:%i(%i)\n", __func__,__LINE__, spack->SType); */ } -static unsigned char *osp_pack; -static unsigned char *osp_pack2; - +/* Attempt to grab 2 packets in a row to maximize through put. + * I2C is a slow bus (can sleep during xfers) and may take longer then + * the time it takes to create the data on the hub if we wait too long. + */ static void osp_work_q(struct work_struct *work) { struct osp_data *osp = container_of(work, struct osp_data, osp_work); - uint16_t plen = 0, plen_old = 0; - uint16_t plen2 = 0, plen2_old = 0; + SensorPacketTypes_t spack; unsigned char *pack_ptr; int pack_count; - SensorPacketTypes_t spack; - int err; int ret; uint32_t intlen; - int gotpack1 = 0; - int gotpack2 = 0; - - ret = osp_i2c_read(osp, OSP_INT_LEN, (unsigned char *)&intlen, 4); - if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { - plen = (intlen >> 4); - plen_old = plen; - if (plen>0 && plen < 8192) { - ret = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack, plen); - if (ret < 0) return; - gotpack1 = 1; + int i; + int gotpack[NUM_PACK] = {0,0}; + uint16_t plen[NUM_PACK] = {0,0}; + + /* Grab buffers as quickly as possible */ + for (i = 0; i < NUM_PACK; i++) { + ret = osp_i2c_read(osp, OSP_INT_LEN, + (unsigned char *)&intlen, 4); + if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { + plen[i] = (intlen >> 4); + if (plen[i] > 0 && plen[i] < 8192) { + ret = osp_i2c_read(osp, OSP_DATA_OUT, + osp_pack[i], plen[i]); + if (ret < 0) return; + gotpack[i] = 1; + } } } - - ret = osp_i2c_read(osp, OSP_INT_LEN, (unsigned char *)&intlen, 4); - if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { - plen2 = (intlen >> 4); - plen2_old = plen2; - if (plen2>0 && plen2 < 8192) { - ret = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack2, plen2); - if (ret >= 0) - gotpack2 = 1; + for (i = 0; i < NUM_PACK; i++) { + if (gotpack[i]) { + pack_count = 0; + pack_ptr = osp_pack[i]; + do { + ret = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen[i]); + if (ret>0) { + OSP_ReportSensor(osp, &spack); + } else { + dev_err(&osp->client->dev, + "OSP packet parsing error = %i, pack_count = %i plen = %i\n", ret, pack_count, plen[i]); + break; + } + plen[i] -= ret; + pack_ptr += ret; + pack_count++; + } while (plen[i] > 0); } } - - if (gotpack1) { - pack_count = 0; - pack_ptr = osp_pack; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen); - if (err>0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("OSP packet parsing error = %i, pack_count = %i plen = %i, plen_old = %i\n", err, pack_count, plen, plen_old); - break; - } - plen -= err; - pack_ptr += err; - pack_count++; - } while (plen > 0 && plen2 < 8192); - } - if (gotpack2) { - pack_count = 0; - pack_ptr = osp_pack2; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen2); - if (err>0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("OSP packet parsing error = %i, pack_count = %i plen2 = %i, plen2_old = %i\n", err, pack_count, plen2, plen2_old); - return; - } - plen2 -= err; - pack_ptr += err; - pack_count++; - } while (plen2 > 0 && plen2 < 8192); - } - /* printk("HY-DBG: pack_count = %i\n", pack_count); */ } static void osp_poll_timer(unsigned long _osp) { struct osp_data *osp = (void *)_osp; queue_work(osp_workq, &osp->osp_work); -#if 0 - mod_timer(&osp->osp_timer, jiffies+msecs_to_jiffies(20)); -#else mod_timer(&osp->osp_timer, jiffies+1); -#endif } static irqreturn_t osp_irq_thread(int irq, void *dev) @@ -869,152 +645,55 @@ static irqreturn_t osp_irq_thread(int irq, void *dev) struct osp_data *osp = dev; queue_work(osp_workq, &osp->osp_work); - return IRQ_HANDLED; } -static void osp_poll(struct input_polled_dev *dev) -{ -#if 0 - struct osp_data *osp = dev->private; - int retval; - int maxcount = 0x1ff; - uint16_t plen, plen_old; - unsigned char *pack_ptr; - int pack_count; - SensorPacketTypes_t spack; - int err; - - do { - retval = i2c_smbus_read_byte_data(osp->client, OSP_INT_REASON); - if (retval&OSP_INT_DRDY) { - osp_i2c_read(osp, OSP_DATA_LEN_L, (unsigned char *)&plen, 2); - pack_count = 0; - if (plen>0 && plen < 8192) { - pack_ptr = osp_pack; - retval = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack, plen); - plen_old = plen; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen); - if (err>=0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("%s:%i: OSP packet parsing error = %i, pack_count = %i plen = %i, plen_old = %i\n", __func__, __LINE__, err, pack_count, plen, plen_old); - break; - } - plen -= err; - pack_ptr += err; - pack_count++; - } while (plen > 0); - } - /* printk("HY-DBG: pack_count = %i\n", pack_count); */ - } else - break; - maxcount--; - } while (maxcount); - if (maxcount < 0x3f) { - printk("WARNING - Maxcount = %i\n", maxcount); - if (!maxcount) - printk("Too busy in poll, maxcount = %i\n", maxcount); - } -#endif -} -static void osp_polled_input_open(struct input_polled_dev *dev) -{ - struct osp_data *osp = dev->private; - - osp_enable(osp); -} - -static void osp_polled_input_close(struct input_polled_dev *dev) -{ - struct osp_data *osp = dev->private; - - osp_disable(osp); -} - - -static int osp_setup_polled_device(struct osp_data *osp) -{ - int err; - struct input_polled_dev *poll_dev; - poll_dev = input_allocate_polled_device(); - - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - if (!poll_dev) { - dev_err(&osp->client->dev, - "Failed to allocate polled device\n"); - return -ENOMEM; - } - - osp->poll_dev = poll_dev; - osp->input_dev = poll_dev->input; - - poll_dev->private = osp; - poll_dev->poll = osp_poll; - poll_dev->open = osp_polled_input_open; - poll_dev->close = osp_polled_input_close; -#if 1 - poll_dev->poll_interval = 20; /* 50Hz */ -#else - poll_dev->poll_interval = 0; /* 50Hz */ -#endif - - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - osp_init_input_device(osp, poll_dev->input, in_name[1]); - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - - osp->in_dev[1] = poll_dev->input; - err = input_register_polled_device(poll_dev); - if (err) { - dev_err(&osp->client->dev, - "Unable to register polled device, err=%d\n", err); - input_free_polled_device(poll_dev); - return err; - } - - osp_setup_other_input(osp); - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - - return 0; -} - -static void osp_teardown_polled_device(struct osp_data *osp) -{ - /* Need to add other input devices */ - input_unregister_polled_device(osp->poll_dev); - input_free_polled_device(osp->poll_dev); -} - +/* HY-DBG: Verify return code with latest firmwrae. */ static int osp_verify(struct osp_data *osp) { int retval; retval = i2c_smbus_read_byte_data(osp->client, OSP_WHOAMI); if (retval < 0) { - dev_err(&osp->client->dev, "read err int source, 0x%x\n", retval); + dev_err(&osp->client->dev, + "read err int source, 0x%x\n", retval); goto out; } - retval = (retval != 0x54) ? -EIO : 0; - retval = i2c_smbus_read_byte_data(osp->client, OSP_CONFIG); + retval = (retval != 0x54) ? -EIO : 0; + retval = 0; out: return retval; } + +static int OSP_add_child(struct osp_data *osp) +{ + struct platform_device *pdev; + + pdev = platform_device_alloc("osp-output", 0); + if (!pdev) { + printk("Cannot allocate dev\n"); + return -ENOMEM; + } + pdev->dev.parent = &osp->client->dev; + + return platform_device_add(pdev); +} + static int osp_probe(struct i2c_client *client, - const struct i2c_device_id *id) + const struct i2c_device_id *id) { struct osp_data *osp; int err; + int i; if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C | I2C_FUNC_SMBUS_BYTE_DATA)) { dev_err(&client->dev, "client is not i2c capable\n"); return -ENXIO; } - osp = kzalloc(sizeof(*osp), GFP_KERNEL); if (!osp) { dev_err(&client->dev, @@ -1022,17 +701,18 @@ static int osp_probe(struct i2c_client *client, return -ENOMEM; } gOSP = osp; - osp_pack = kzalloc(8192, GFP_KERNEL); - if (!osp_pack) { - dev_err(&client->dev, - "failed to allocate memory for packet data\n"); - return -ENOMEM; - } - osp_pack2 = kzalloc(8192, GFP_KERNEL); - if (!osp_pack2) { - dev_err(&client->dev, - "failed to allocate memory for packet data\n"); - return -ENOMEM; + mutex_init(&osp->lock); + for (i = 0; i < NUM_PACK; i++) { + osp_pack[i] = kzalloc(8192, GFP_KERNEL); + if (!osp_pack[i]) { + dev_err(&client->dev, + "failed to allocate memory for packet data\n"); + i--; + for (; i >= 0; i--) { + kfree(osp_pack[i]); + } + return -ENOMEM; + } } osp->client = client; @@ -1050,12 +730,6 @@ static int osp_probe(struct i2c_client *client, INIT_WORK(&osp->osp_work, osp_work_q); } -#if 0 - osp_setup_polled_device(osp); -#else - osp_setup_input_device(osp); -#endif - client->irq = -1; /* Force it for now */ if (client->irq > 0) { @@ -1066,27 +740,18 @@ static int osp_probe(struct i2c_client *client, client->irq, err); } - } else { - err = -EINVAL; } - setup_timer(&osp->osp_timer, osp_poll_timer, (unsigned long)osp); - if (err < 0) { + if (client->irq <= 0 || err < 0) { + setup_timer(&osp->osp_timer, osp_poll_timer, (unsigned long)osp); mod_timer(&osp->osp_timer, jiffies + msecs_to_jiffies(10)); - } + } + + OSP_CB_init(); + /* Create child device */ + OSP_add_child(osp); return 0; -#if 0 -err_free_irq: - free_irq(client->irq, osp); -err_destroy_input: - input_unregister_device(osp->input_dev); -#endif -#if 0 -err_pdata_exit: - if (tj9->pdata.exit) - tj9->pdata.exit(); -#endif err_free_mem: kfree(osp); return err; @@ -1097,13 +762,7 @@ static int osp_remove(struct i2c_client *client) struct osp_data *osp = i2c_get_clientdata(client); if (client->irq) { -#if 0 - sysfs_remove_group(&client->dev.kobj, &kxtj9_attribute_group); -#endif free_irq(client->irq, osp); - input_unregister_device(osp->input_dev); - } else { - osp_teardown_polled_device(osp); } kfree(osp); @@ -1116,31 +775,16 @@ static int osp_suspend(struct device *dev) { struct i2c_client *client = to_i2c_client(dev); struct osp_data *osp = i2c_get_clientdata(client); - struct input_dev *input_dev = osp->input_dev; - - mutex_lock(&input_dev->mutex); - - if (input_dev->users) - osp_disable(osp); - mutex_unlock(&input_dev->mutex); + mutex_lock(&osp->lock); + osp_disable(osp); + mutex_unlock(&osp->lock); return 0; } static int osp_resume(struct device *dev) { - struct i2c_client *client = to_i2c_client(dev); - struct osp_data *osp = i2c_get_clientdata(client); - struct input_dev *input_dev = osp->input_dev; - int retval = 0; - - mutex_lock(&input_dev->mutex); - - if (input_dev->users) - osp_enable(osp); - - mutex_unlock(&input_dev->mutex); - return retval; + return 0; } #endif @@ -1171,10 +815,9 @@ static int __init osp_init(void) return i2c_add_driver(&osp_driver); } module_init(osp_init); - #endif MODULE_DESCRIPTION("OSP driver"); -MODULE_AUTHOR("Hunyue Yau "); +MODULE_AUTHOR("Hunyue Yau "); MODULE_LICENSE("GPL"); From 5f8190434333e5f675f1cfad4e02910d1211f375 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 18 Sep 2015 16:59:31 -0700 Subject: [PATCH 04/21] Clean up code Remove obsolete Keil work around code. Consolidate union members. Signed-off-by: Hunyue Yau --- algorithm/osp/osp.c | 66 ++++++++++++++------------------------------- 1 file changed, 20 insertions(+), 46 deletions(-) diff --git a/algorithm/osp/osp.c b/algorithm/osp/osp.c index 5a9db7a..23afe27 100644 --- a/algorithm/osp/osp.c +++ b/algorithm/osp/osp.c @@ -42,7 +42,6 @@ #define SEN_ENABLE 1 -#undef KEIL_WORKAROUND static struct Results RESULTS[NUM_ANDROID_SENSOR_TYPE]; static unsigned char dirty[NUM_ANDROID_SENSOR_TYPE]; static SystemDescriptor_t const *sys; @@ -54,7 +53,6 @@ static const OSP_Library_Version_t libVersion = { .buildTime = __DATE__" "__TIME__ }; -#ifndef KEIL_WORKAROUND static const uint64_t depend[NUM_ANDROID_SENSOR_TYPE] = { [SENSOR_ACCELEROMETER] = FLAG(SENSOR_ACCELEROMETER), @@ -71,9 +69,6 @@ static const uint64_t depend[NUM_ANDROID_SENSOR_TYPE] = [SENSOR_SIGNIFICANT_MOTION] = FLAG(SENSOR_ACCELEROMETER), [SENSOR_TILT_DETECTOR] = FLAG(SENSOR_ACCELEROMETER), }; -#else -static uint64_t depend[NUM_ANDROID_SENSOR_TYPE]; -#endif static ResultDescriptor_t * resHandles[NUM_ANDROID_SENSOR_TYPE]; @@ -170,17 +165,12 @@ static void OSPalg_EnableSensor(unsigned int sensor) static void ResultReadyCB(struct Results *res, int sensor) { union { - Android_TriAxisPreciseData_t calaccel; - Android_UncalibratedTriAxisPreciseData_t uncalaccel; - Android_TriAxisPreciseData_t calgyro; - Android_UncalibratedTriAxisPreciseData_t uncalgyro; - - Android_BooleanResultData_t sigmot; - - Android_UncalibratedTriAxisExtendedData_t uncalmag; + Android_TriAxisPreciseData_t calresult; Android_TriAxisExtendedData_t calmag; + Android_UncalibratedTriAxisPreciseData_t uncalresult; + Android_UncalibratedTriAxisExtendedData_t uncalmag; + Android_BooleanResultData_t sigmot; Android_OrientationResultData_t orient; - Android_TriAxisPreciseData_t generic; Android_RotationVectorResultData_t rotvec; } r; @@ -188,10 +178,10 @@ static void ResultReadyCB(struct Results *res, int sensor) return; switch(sensor) { case SENSOR_ACCELEROMETER: - r.calaccel.X = Q15_to_NTPRECISE(res->ResType.result.x); - r.calaccel.Y = Q15_to_NTPRECISE(res->ResType.result.y); - r.calaccel.Z = Q15_to_NTPRECISE(res->ResType.result.z); - r.calaccel.TimeStamp = res->time; + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; break; case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: r.uncalmag.X = Q15_to_NTPRECISE(res->ResType.result.x); @@ -206,16 +196,16 @@ static void ResultReadyCB(struct Results *res, int sensor) r.calmag.TimeStamp = res->time; break; case SENSOR_GYROSCOPE_UNCALIBRATED: - r.uncalgyro.X = Q15_to_NTPRECISE(res->ResType.result.x); - r.uncalgyro.Y = Q15_to_NTPRECISE(res->ResType.result.y); - r.uncalgyro.Z = Q15_to_NTPRECISE(res->ResType.result.z); - r.uncalgyro.TimeStamp = res->time; + r.uncalresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.uncalresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.uncalresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.uncalresult.TimeStamp = res->time; break; case SENSOR_GYROSCOPE: - r.calgyro.X = Q15_to_NTPRECISE(res->ResType.result.x); - r.calgyro.Y = Q15_to_NTPRECISE(res->ResType.result.y); - r.calgyro.Z = Q15_to_NTPRECISE(res->ResType.result.z); - r.calgyro.TimeStamp = res->time; + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; break; case SENSOR_ORIENTATION: r.orient.Pitch = Q15_to_NTEXTENDED(res->ResType.euler.pitch); @@ -227,10 +217,10 @@ static void ResultReadyCB(struct Results *res, int sensor) return; case SENSOR_GRAVITY: case SENSOR_LINEAR_ACCELERATION: - r.generic.X = Q15_to_NTPRECISE(res->ResType.result.x); - r.generic.Y = Q15_to_NTPRECISE(res->ResType.result.y); - r.generic.Z = Q15_to_NTPRECISE(res->ResType.result.z); - r.generic.TimeStamp = res->time; + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; break; case SENSOR_ROTATION_VECTOR: @@ -723,22 +713,6 @@ OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t *sysdesc) { int i; - /* Work around for broken Keil */ -#ifdef KEIL_WORKAROUND -#error Keil is broken. - depend[SENSOR_ACCELEROMETER] = FLAG(SENSOR_ACCELEROMETER); - depend[SENSOR_MAGNETIC_FIELD] = FLAG(SENSOR_MAGNETIC_FIELD); - depend[SENSOR_ORIENTATION] = FLAG(SENSOR_ACCELEROMETER)|FLAG(SENSOR_MAGNETIC_FIELD); - depend[SENSOR_ROTATION_VECTOR] = FLAG(SENSOR_ACCELEROMETER)|FLAG(SENSOR_MAGNETIC_FIELD); - depend[SENSOR_GRAVITY] = FLAG(SENSOR_ACCELEROMETER); - depend[SENSOR_LINEAR_ACCELERATION] = FLAG(SENSOR_ACCELEROMETER); - depend[SENSOR_GYROSCOPE] = FLAG(SENSOR_GYROSCOPE); - depend[SENSOR_PRESSURE] = FLAG(SENSOR_PRESSURE); - depend[SENSOR_STEP_DETECTOR] = FLAG(SENSOR_ACCELEROMETER); - depend[SENSOR_STEP_COUNTER] = FLAG(SENSOR_ACCELEROMETER); - depend[SENSOR_SIGNIFICANT_MOTION] = FLAG(SENSOR_ACCELEROMETER); -#endif - sys = sysdesc; for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { From 63a08b5f0f333bd751d78c694ce70870d19887a3 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 18 Sep 2015 17:01:06 -0700 Subject: [PATCH 05/21] Remove comment about cal data. It has been implemented. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/linux/daemon/OSPDaemon.c b/linux/daemon/OSPDaemon.c index cd39cbc..d1375b6 100644 --- a/linux/daemon/OSPDaemon.c +++ b/linux/daemon/OSPDaemon.c @@ -4,12 +4,6 @@ * Apache Licensed. */ -/* - * TODO: - * - Handle cal data. - */ - - #include #include #include From 1930455ce4eb27a9346369751b667b0611319951 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Wed, 23 Sep 2015 16:51:55 -0700 Subject: [PATCH 06/21] Proposed diffs to support private sensors This is a temporary work around to support private sensors prior to debugging inbound control packets. This patch adds 2 registers used to select between private and android sensor spaces. By default, android sensor space is selected to retain compatibility. 0x1f - a read of this register selects the android sensor space 0x1e - a read of this register selects the private sensor space Usage protocol: To enable/disable a public sensor: read(0x1f) read(0x20+sensor number) or read(0x50+sensor number) To enable/disable a private sensor: read(0x1e) read(0x20+private sensor number) or read(0x50+private sensor number) Note the private sensor number is the sensor number without the high private bit set! Signed-off-by: Hunyue Yau --- .../sources/app/i2c_slavecomm_t.c | 131 ++++++++++++------ 1 file changed, 91 insertions(+), 40 deletions(-) diff --git a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c index 6d1ee8c..a5be660 100644 --- a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c +++ b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c @@ -72,63 +72,110 @@ int FastData_add(volatile struct FastData *FD, Buffer_t *pHIFPkt); #define SH_VERSION1 0x23 static uint32_t SensorState[2]; +static uint32_t PSensorState[2]; // TODO: Need to call algorithm module to subscribe the enable sensor. -static void SensorEnable(ASensorType_t sen) +static void SensorEnable(ASensorType_t sen, int space) { int v = (int)sen; int set; - - if (v < 32) { - set = (1<= 0x20 && rx_buf[0] < 0x50) { /* ENABLE */ - SensorEnable((ASensorType_t)(rx_buf[0]-0x20)); + SensorEnable((ASensorType_t)(rx_buf[0]-0x20), space); D0_printf("Enable %i\r\n", rx_buf[0] - 0x20); } else if (rx_buf[0] >= 0x50 && rx_buf[0] < 0x80) { /* DISABLE */ - SensorDisable((ASensorType_t)(rx_buf[0]-0x50)); + SensorDisable((ASensorType_t)(rx_buf[0]-0x50), space); D0_printf("Disable %i\r\n", rx_buf[0] - 0x50); } break; From 9101eafb1a64a366fdb7e451272650e103f15c97 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Wed, 23 Sep 2015 16:57:40 -0700 Subject: [PATCH 07/21] Bump version so to identify private sensor support. Signed-off-by: Hunyue Yau --- embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c index a5be660..c68e642 100644 --- a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c +++ b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c @@ -69,7 +69,7 @@ int FastData_add(volatile struct FastData *FD, Buffer_t *pHIFPkt); \*-------------------------------------------------------------------------*/ #define SH_WHO_AM_I 0x54 #define SH_VERSION0 0x01 -#define SH_VERSION1 0x23 +#define SH_VERSION1 0x24 static uint32_t SensorState[2]; static uint32_t PSensorState[2]; From 85f8ca74cab6e5d0685e7f7bcf1c7b50797209cf Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 25 Sep 2015 16:53:24 -0700 Subject: [PATCH 08/21] Provide sane axis defaults. Signed-off-by: Hunyue Yau --- algorithm/osp/osp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/algorithm/osp/osp.c b/algorithm/osp/osp.c index 23afe27..763f0e7 100644 --- a/algorithm/osp/osp.c +++ b/algorithm/osp/osp.c @@ -557,8 +557,8 @@ OSP_STATUS_t OSP_SetInputData(InputSensorHandle_t handle, SensorDescriptor_t *s, **v; Q15_t x, y, z; int32_t rawX, rawY, rawZ; - int32_t xidx, yidx, zidx; - int32_t xs, ys, zs; + int32_t xidx = 0, yidx = 1, zidx = 2; + int32_t xs = 1, ys = 1, zs = 1; int i; int val[5], vallen; unsigned long long ts; From 34138462881c7012cb8549a528920845298de3bc Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 25 Sep 2015 16:53:51 -0700 Subject: [PATCH 09/21] Default to nonstatic binaries. Non static binaries requires usage of a bionic toolchain. Signed-off-by: Hunyue Yau --- linux/daemon/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/daemon/Makefile b/linux/daemon/Makefile index 5b023be..6a77ede 100644 --- a/linux/daemon/Makefile +++ b/linux/daemon/Makefile @@ -1,7 +1,7 @@ ARMCC=arm-none-linux-gnueabi-gcc CC=gcc CFLAGS=-Wall -g -I../../include -LDFLAGS=-Wall -g -static +LDFLAGS=-Wall -g LIBS=-L../../algorithm/osp -lOSP From 84ad870e29e3fead9b198f1e7a869dc6edbad62d Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 25 Sep 2015 16:55:18 -0700 Subject: [PATCH 10/21] Add missing init of nstart variable so return isn't trash. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/linux/daemon/OSPDaemon.c b/linux/daemon/OSPDaemon.c index d1375b6..362be8b 100644 --- a/linux/daemon/OSPDaemon.c +++ b/linux/daemon/OSPDaemon.c @@ -243,7 +243,6 @@ static int modifyOutput(struct OSPDaemon_output *out, union { int i; float f; - } conv; if (out->format == OUT_FORMAT_INTEGER) @@ -420,7 +419,7 @@ static int OSPDaemon_init_inbound(struct pollfd *pfd, int nfd, int start) { int i; OSP_STATUS_t oret; - int nstart; + int nstart = start; if (sd->CalFile) ReadCalData(sd->CalFile); @@ -582,7 +581,6 @@ int main(int argc, char **argv) printf("Disabling Power Management\n"); } /* Catch SIGPIPE? */ - /* queue_test(); */ /* Driver inits. Replace with linker magic? */ OSPDaemon_iio_init(); OSPDaemon_inputreader_init(); From 989458c7c21c3455564926566c43885e90b0e077 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 25 Sep 2015 16:57:03 -0700 Subject: [PATCH 11/21] Add basis for private sensor support. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon_config.c | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/linux/daemon/OSPDaemon_config.c b/linux/daemon/OSPDaemon_config.c index 6d8268f..17fc80d 100644 --- a/linux/daemon/OSPDaemon_config.c +++ b/linux/daemon/OSPDaemon_config.c @@ -98,6 +98,13 @@ static const char * SensorTypeMap[] = { SEN_MAP(SENSOR_GEOMAGNETIC_ROTATION_VECTOR), [NUM_ANDROID_SENSOR_TYPE] = NULL }; +static const char * SensorTypeMapP[] = { + SEN_MAP(PSENSOR_ACCELEROMETER_RAW), + SEN_MAP(PSENSOR_MAGNETIC_FIELD_RAW), + SEN_MAP(PSENSOR_GYROSCOPE_RAW), + SEN_MAP(PSENSOR_ACCELEROMETER_UNCALIBRATED), + [NUM_PRIVATE_SENSOR_TYPE] = NULL +}; static const char *DriverTypeMap[] = { [DRIVER_INPUT] = "input", @@ -494,12 +501,34 @@ static int proc_sensor_type(int linetype, int tag, char *val, int *taglist) Output[taglist[tag]].ResultDesc.SensorType = i; Output[taglist[tag]].ResultDesc.OutputRateInSeconds = 0; Output[taglist[tag]].ResultDesc.OptionData = &Output[taglist[tag]]; - } break; } } - if (i == NUM_ANDROID_SENSOR_TYPE) { + if (i != NUM_ANDROID_SENSOR_TYPE) { + return 0; + } + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + if (SensorTypeMapP[i] == NULL) continue; + if (strcmp(SensorTypeMapP[i], val) == 0) { + DBGOUT("FOUND SENSOR: %i tag %i : %s\n", linetype, tag, SensorTypeMapP[i]); + if (linetype == LINE_SENSOR) { + taglist[tag] = sensor_count; + sensor_count++; + Sensor[taglist[tag]].sensor.SensorType = M_PSensorToAndroidBase(i); + Sensor[taglist[tag]].sensor.SensorName = SensorTypeMapP[i]; + } else if (linetype == LINE_OUTPUT) { + taglist[tag] = output_count; + output_count++; + Output[taglist[tag]].type = M_PSensorToAndroidBase(i); + Output[taglist[tag]].ResultDesc.SensorType = M_PSensorToAndroidBase(i); + Output[taglist[tag]].ResultDesc.OutputRateInSeconds = 0; + Output[taglist[tag]].ResultDesc.OptionData = &Output[taglist[tag]]; + } + break; + } + } + if (i == NUM_PRIVATE_SENSOR_TYPE) { fprintf(stderr, "Bad/invalid sensor: %s\n", val); } return 0; From aaae0b9ea4f7360b961658a99cf485b6399cf64f Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Fri, 25 Sep 2015 16:57:49 -0700 Subject: [PATCH 12/21] Add debugging info and handle unbuffered IIO devices. Unbuffered iio devices do not have a scanelements directory. Handle the case where that directory is not there. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon_driver.c | 3 +++ linux/daemon/OSPDaemon_iio.c | 3 +++ 2 files changed, 6 insertions(+) diff --git a/linux/daemon/OSPDaemon_driver.c b/linux/daemon/OSPDaemon_driver.c index 2366993..5325b65 100644 --- a/linux/daemon/OSPDaemon_driver.c +++ b/linux/daemon/OSPDaemon_driver.c @@ -85,10 +85,13 @@ int OSPDaemon_driver_setup_in(struct OSPDaemon_SensorDetail *s, int count) int OSPDaemon_driver_setup_out(struct OSPDaemon_output *s, int count) { + DBG(DEBUG_DRIVER, "Calling INPUT outbound setup\n"); if (outmap[DRIVER_INPUT]) outmap[DRIVER_INPUT]->setup_out(s, count); + DBG(DEBUG_DRIVER, "Calling FileCSV outbound setup\n"); if (outmap[DRIVER_FILECSV]) outmap[DRIVER_FILECSV]->setup_out(s, count); + DBG(DEBUG_DRIVER, "Done out setup\n"); return 0; } diff --git a/linux/daemon/OSPDaemon_iio.c b/linux/daemon/OSPDaemon_iio.c index 3d82126..7953cff 100644 --- a/linux/daemon/OSPDaemon_iio.c +++ b/linux/daemon/OSPDaemon_iio.c @@ -263,6 +263,7 @@ static void setupiio(struct IIO_Sensor *is) snprintf(dname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements", is->iionum); d = opendir(dname); + if (d == NULL) return; while((dent = readdir(d)) != NULL) { if (dent->d_name[0] == '.') continue; @@ -495,12 +496,14 @@ static int OSPDaemon_iio_read(struct OSPDaemon_SensorDetail *s) if (od != NULL) { DBGOUT("Got data for %s\n", is->name); OSPDaemon_queue_put(&s->q, od); + DBGOUT("Queued data for %s\n", is->name); } ret -= is->rec_sz; used += is->rec_sz; } while (ret > 0); } + DBGOUT("Finish reading data for %s\n", is->name); return 0; } From 3d14eb8b2efd83c3d6cd66bcec426b2354e66a5d Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:10:09 -0700 Subject: [PATCH 13/21] Fix error checking and implement a quiet option. - Add null pointer check for data coming from algs. - Abort if cal length being read is longer then available buffer space. - Add quiet option to suppress initial banner. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/linux/daemon/OSPDaemon.c b/linux/daemon/OSPDaemon.c index 362be8b..1ecde30 100644 --- a/linux/daemon/OSPDaemon.c +++ b/linux/daemon/OSPDaemon.c @@ -45,6 +45,7 @@ static OSP_STATUS_t OSPDaemon_SensorControl_cb(SensorControl_t *cmd) { int i; + if (cmd == NULL) return OSP_STATUS_UNSPECIFIED_ERROR; for (i = 0; i sensor_count; i++) { if (sd->sensor[i].handle == cmd->Handle) break; @@ -84,6 +85,8 @@ static void ReadCalData(char *name) close(fd); return; } + if (rec.len > MAX_CALSIZE) + continue; if (rec.len > 1) { ret = read(fd, caldata+1, rec.len-1); if (ret != rec.len-1) { @@ -92,8 +95,6 @@ static void ReadCalData(char *name) } caldata[0] = rec.data[0]; } - if (rec.len > MAX_CALSIZE) - continue; /* Dispatch cal data - store in pCalibrationData */ for (i = 0; i < sd->sensor_count; i++) { if (sd->sensor[i].sensor.SensorType == rec.sensor) @@ -103,7 +104,6 @@ static void ReadCalData(char *name) memcpy(sd->sensor[i].caldata, caldata, rec.len); sd->sensor[i].sensor.pCalibrationData = sd->sensor[i].caldata; } - } while(1); } @@ -536,7 +536,8 @@ static void OSPDaemon_help(const char *name) int main(int argc, char **argv) { char *confname = NULL; - int ar = 1; + int ar = 1; + int quiet = 0; if (argc > 1) { do { @@ -564,6 +565,9 @@ int main(int argc, char **argv) case 'n': /* Ignore cal file */ ignorecal = 1; break; + case 'q': + quiet = 1; + break; case 'v': /* Version */ OSPDaemon_version(); exit(0); @@ -575,10 +579,10 @@ int main(int argc, char **argv) } if (confname == NULL) confname = "OSPDaemon.conf"; - printf("Reading config from %s\n", confname); - printf("Debug level 0x%08x\n", debug_level); + if (!quiet) printf("Reading config from %s\n", confname); + if (!quiet) printf("Debug level 0x%08x\n", debug_level); if (disablepm) { - printf("Disabling Power Management\n"); + if (!quiet) printf("Disabling Power Management\n"); } /* Catch SIGPIPE? */ /* Driver inits. Replace with linker magic? */ From 65c98cae10b1c46e52a939f0d1fade5e34276169 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:12:07 -0700 Subject: [PATCH 14/21] Add [inputN|outputN].option parsing [inputN|outputN].option provides a way for a driver to receive configuration/option info. The parameter is a 32bit unsigned value. Values can be specified as decimal or hex by prefixing it with a 0x. The meaning of the value is solely determined by the driver. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon.h | 9 +++++++++ linux/daemon/OSPDaemon_config.c | 24 ++++++++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/linux/daemon/OSPDaemon.h b/linux/daemon/OSPDaemon.h index e4d1684..fe8ba15 100644 --- a/linux/daemon/OSPDaemon.h +++ b/linux/daemon/OSPDaemon.h @@ -22,6 +22,7 @@ struct OSPDaemon_output { int usage; int format; struct queue q; + int option; }; enum { @@ -51,6 +52,13 @@ enum { DRIVER_TYPE_OUTPUT, }; +/* Output embedded time stamps */ +#define INPUT_OPTION_EMBEDTS (1<<0) +/* Send all 4 elements of a quaternion */ +#define INPUT_OPTION_QUAT4 (1<<1) +/* Dither output to defeat input event dup rejection */ +#define INPUT_OPTION_DITHER (1<<2) + static inline void conv2OSP(ASensorType_t sensor, OSP_InputSensorData_t *d, unsigned long long ts, int val[]) @@ -193,6 +201,7 @@ struct OSPDaemon_SensorDetail { int noprocess; int format; OSP_InputSensorData_t pdata; + int option; }; struct OSPDaemon_SensorDescription { diff --git a/linux/daemon/OSPDaemon_config.c b/linux/daemon/OSPDaemon_config.c index 17fc80d..515f10c 100644 --- a/linux/daemon/OSPDaemon_config.c +++ b/linux/daemon/OSPDaemon_config.c @@ -61,6 +61,7 @@ static int proc_output_format(int linetype, int tag, char *val, int *taglist); static int proc_sensor_format(int linetype, int tag, char *val, int *taglist); static int proc_system_calfile(int linetype, int tag, char *val, int *taglist); static int proc_system_pmdir(int linetype, int tag, char *val, int *taglist); +static int proc_option(int linetype, int tag, char *val, int *taglist); enum { LINE_UNKNOWN, @@ -129,6 +130,7 @@ static const struct _keymap { {"pmdir", proc_system_pmdir}, {"format", proc_output_format}, {"iformat", proc_sensor_format}, + {"option", proc_option}, }; /* Parse a string containing 3 comma seperated values */ @@ -253,6 +255,28 @@ static int proc_output_noprocess(int linetype, int tag, char *val, int *taglist) return 0; } +static int proc_option(int linetype, int tag, char *val, int *taglist) +{ + unsigned int opt; + + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s", LineName[linetype]); + + opt = strtol(val, NULL, 0); + if (linetype == LINE_OUTPUT) { + Output[taglist[tag]].option = opt; + } else if (linetype == LINE_SENSOR) { + Sensor[taglist[tag]].option = opt; + } else { + return -1; + } +} + static int proc_output_format(int linetype, int tag, char *val, int *taglist) { int shift; From 86e03a7a638864d466aae052650d254b4d9d696f Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:15:47 -0700 Subject: [PATCH 15/21] Tighten error checking and remove dead code. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon_iio.c | 201 ++--------------------------------- 1 file changed, 8 insertions(+), 193 deletions(-) diff --git a/linux/daemon/OSPDaemon_iio.c b/linux/daemon/OSPDaemon_iio.c index 7953cff..b0ebfbb 100644 --- a/linux/daemon/OSPDaemon_iio.c +++ b/linux/daemon/OSPDaemon_iio.c @@ -60,13 +60,13 @@ static void sysfs_write_str(const char *path, const char *str) static void parse_type(const char *path, struct DataDesc *dd) { FILE *f; - char desc[1024]; + char desc[512]; char *ptr = NULL, *ptr2; DBGOUT("Parsing %s\n", path); f = fopen(path, "r"); if (f) { - ptr = fgets(desc, 1024, f); + ptr = fgets(desc, 512, f); fclose(f); } if (ptr) { @@ -165,7 +165,7 @@ static OSP_InputSensorData_t * parse_iiodata(struct IIO_Sensor *is, char *buf, i int64_t sll[1]; } conf; int val[6]; - unsigned long long ts; + unsigned long long ts = 0; double outval; for (i = 0; i < MAX_AXIS; i++) { @@ -213,6 +213,8 @@ static OSP_InputSensorData_t * parse_iiodata(struct IIO_Sensor *is, char *buf, i else outval = conf.ll[0]; break; + default: + outval = 0.0; } #if 0 if (is->IIOAxis[ax].dd.shift) @@ -279,7 +281,6 @@ static void setupiio(struct IIO_Sensor *is) parse_type(fname, &is->IIOAxis[ax].dd); DBGOUT("%s - %i, %i\n", fname, is->IIOAxis[ax].dd.store, ax); - /* print_Desc(&is->IIOAxis[ax].dd); */ } else if (strcmp(dent->d_name + len - 3, "_en") == 0) { snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); sysfs_write_val(fname, 1); @@ -292,7 +293,7 @@ static void setupiio(struct IIO_Sensor *is) closedir(d); snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/trigger/current_trigger", is->iionum); - printf("setting trigger name %s to %s\n", is->name, fname); + DBGOUT("setting trigger name %s to %s\n", is->name, fname); sysfs_write_str(fname, is->name); snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/length", is->iionum); sysfs_write_val(fname, 100); @@ -382,7 +383,6 @@ static int OSPDaemon_iio_create(const char *name, struct IIO_Sensor *s) if (s->IIOAxis[i].index >= 0) { if (s->IIOAxis[i].dd.store == 0) continue; - /* print_Desc(&s->IIOAxis[i].dd); */ /* Make sure each group is aligned */ DBGOUT("1: rec_sz = %i, s->IIOAxis[i].dd.store = %i, %i,\n", rec_sz, s->IIOAxis[i].dd.store, i); if (rec_sz % (s->IIOAxis[i].dd.store/8) != 0) { @@ -415,27 +415,6 @@ static int OSPDaemon_iioevent_create(const char *name, struct IIO_Sensor *s) DBGOUT("HY-DBG: %s:%i - IIOnum = %i\n", __func__, __LINE__, s->iionum); s->name = name; -#if 0 - setupiio(s); - - rec_sz = 0; - for (i = 0; i < MAX_AXIS; i++) { - if (s->IIOAxis[i].index >= 0) { - if (s->IIOAxis[i].dd.store == 0) continue; - - /* print_Desc(&s->IIOAxis[i].dd); */ - /* Make sure each group is aligned */ - DBGOUT("1: rec_sz = %i, s->IIOAxis[i].dd.store = %i, %i,\n", rec_sz, s->IIOAxis[i].dd.store, i); - if (rec_sz % (s->IIOAxis[i].dd.store/8) != 0) { - rec_sz += ((s->IIOAxis[i].dd.store/8)-(rec_sz % (s->IIOAxis[i].dd.store/8))); - } - s->IIOAxis[i].offset = rec_sz; - rec_sz += s->IIOAxis[i].dd.store/8; - DBGOUT("2: rec_sz = %i\n", rec_sz); - } - } - s->rec_sz = rec_sz; -#endif snprintf(dname, PATH_MAX, "/dev/iio:device%i", s->iionum); fd = open(dname, O_RDONLY); @@ -469,7 +448,6 @@ static int OSPDaemon_iio_setup(struct OSPDaemon_SensorDetail *s, int count) s->fd = OSPDaemon_iioevent_create(s->name, &IIOSen[i]); s->private = &IIOSen[i]; iiocount++; - } s++; } @@ -501,6 +479,8 @@ static int OSPDaemon_iio_read(struct OSPDaemon_SensorDetail *s) ret -= is->rec_sz; used += is->rec_sz; } while (ret > 0); + } else { + DBGOUT("Read error for fd %i (errno = %i)\n", s->fd, errno); } DBGOUT("Finish reading data for %s\n", is->name); @@ -546,168 +526,3 @@ void OSPDaemon_iio_init(void) OSPDaemon_driver_register(&IIODriver); OSPDaemon_driver_register(&IIOEventDriver); } -/* --------------------------------------- */ - -#if 0 - -#include -#include -#include - - -#define MAX_AXIS 5 -#define MAX_IIO 99 -#define MAX_SENSOR 10 - - -enum { - axis_x, - axis_y, - axis_z, - axis_r, - axis_timestamp, - axis_invalid -}; - -static const char *axis_name[] = { - [axis_x] = "X Axis", - [axis_y] = "Y Axis", - [axis_z] = "Z Axis", - [axis_r] = "R Axis", - [axis_timestamp] = "Time Axis", - [axis_invalid] = "INVALID", -}; - -struct DataDesc { - int sign; - int size; - int store; - int shift; -}; - -struct IIO_SensorAxis { - struct DataDesc dd; - int index; - int offset; -}; - - -/* - * List all the available IIO devices and the name. - */ -void dumpiio(void) -{ - int i, j; - FILE *f; - char fname[PATH_MAX]; - char name[1024]; - - for (i = 0; i < MAX_IIO; i++) { - snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/name", i); - fname[PATH_MAX-1] = '\0'; - - f = fopen(fname, "r"); - if (f == NULL) continue; - if (fgets(name, 1023, f) != NULL) { - name[1023] = '\0'; - for (j = 0; j < 1024 && name[j] != '\0'; j++) { - if (name[j] == '\n' || - name[j] == '\r') { - name[j] = '\0'; - break; - } - } - } - fclose(f); - fprintf(stderr, "Device: %s\n", name); - } -} - -void print_Desc(const struct DataDesc *dd) -{ - printf("Data: sign = %i, shift = %i, store = %i, size = %i\n", - dd->sign, dd->shift, dd->store, dd->size); -} - - -void unsetup_iio(struct IIO_Sensor *is, int sencount) -{ - int i, len; - char dname[PATH_MAX]; - char fname[PATH_MAX]; - DIR *d; - struct dirent *dent; - - for (i = 0; i < sencount; i++) { - snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/enable", is[i].iionum); - sysfs_write_val(fname, 0); - - - snprintf(dname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements", is[i].iionum); - - d = opendir(dname); - - while((dent = readdir(d)) != NULL) { - if (dent->d_name[0] == '.') continue; - len = strlen(dent->d_name); - if (len < 6) continue; - if (strcmp(dent->d_name + len - 3, "_en") == 0) { - snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is[i].iionum, dent->d_name); - sysfs_write_val(fname, 0); - } - } - - closedir(d); - } -} - - -void mainloop(int count, struct IIO_Sensor *is, - const int sencount, const int reclen) -{ - int fd, ret; - char dname[PATH_MAX]; - char *buf; - int i; - struct pollfd pfd[10]; - int nfd = 0; - int used; - - buf = malloc(reclen*5); - if (buf == NULL) return; - - for (i = 0; i < sencount; i++) { - snprintf(dname, PATH_MAX, "/dev/iio:device%i", is[nfd].iionum); - - fd = open(dname, O_RDONLY); - if (fd < 0) return; - - pfd[nfd].fd = fd; - pfd[nfd].events = POLLIN; - nfd++; - } - while((count < 0) || count > 0) { - if (count > 0) - count--; - - if (poll(pfd, nfd, -1) <= 0) - continue; - for (i = 0; i < sencount; i++) { - if (!(pfd[i].revents&POLLIN)) - continue; - ret = read(pfd[i].fd, buf, reclen*5); - if (ret > 0) - while (0) {printf("Got data %i\n", ret);} - else - continue; - used = 0; - do { - parse_iiodata(&is[i], buf+used, ret); - ret -= is[i].rec_sz; - used+= is[i].rec_sz; - } while (ret > 0); - } - } -} - -#endif From 218e143656a6955a3a8d27188593d8356d361dda Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:16:08 -0700 Subject: [PATCH 16/21] Implement options support in the input driver. Add support for 3 options: bit 0 - enable embedded time stamp support. 64 bit time stamps are sent as ABS_THROTTLE (upper 32bit) and ABS_RUDDER (lower 32bit) bit 1 - Send the 4th element of a quaternion as ABS_WHEEL. bit 2 - Enable dithering of data. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon_input.c | 54 +++++++++++++++++++++++++++++----- 1 file changed, 46 insertions(+), 8 deletions(-) diff --git a/linux/daemon/OSPDaemon_input.c b/linux/daemon/OSPDaemon_input.c index d8903b2..6156c3a 100644 --- a/linux/daemon/OSPDaemon_input.c +++ b/linux/daemon/OSPDaemon_input.c @@ -47,6 +47,10 @@ static int OSPDaemon_input_create(struct OSPDaemon_output *out) uindev.absmax[ABS_X] = 1023; uindev.absmin[ABS_Y] = 0; uindev.absmax[ABS_Y] = 1023; + uindev.absmin[ABS_Z] = 0; + uindev.absmax[ABS_Z] = 1023; + uindev.absmin[ABS_WHEEL] = 0; + uindev.absmax[ABS_WHEEL] = 1023; ret = write(fd, &uindev, sizeof(uindev)); if (ret < 0) { @@ -54,7 +58,6 @@ static int OSPDaemon_input_create(struct OSPDaemon_output *out) goto error; } - ret = ioctl(fd, UI_SET_EVBIT, EV_ABS); if (ret < 0) { perror("ioctl EV_ABS EVBIT"); @@ -80,9 +83,29 @@ static int OSPDaemon_input_create(struct OSPDaemon_output *out) ret = ioctl(fd, UI_SET_ABSBIT, ABS_Z); if (ret < 0) { - perror("ioctl ABS_Y ABSBIT"); + perror("ioctl ABS_Z ABSBIT"); goto error; } + if (out->option & INPUT_OPTION_EMBEDTS) { + ret = ioctl(fd, UI_SET_ABSBIT, ABS_THROTTLE); + if (ret < 0) { + perror("ioctl ABS_THROTTLE ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_RUDDER); + if (ret < 0) { + perror("ioctl ABS_RUDDER ABSBIT"); + goto error; + } + } + if (out->option & INPUT_OPTION_QUAT4) { + ret = ioctl(fd, UI_SET_ABSBIT, ABS_WHEEL); + if (ret < 0) { + perror("ioctl ABS_WHEEL ABSBIT"); + goto error; + } + } ret = ioctl(fd, UI_DEV_CREATE); if (ret < 0) { @@ -91,7 +114,6 @@ static int OSPDaemon_input_create(struct OSPDaemon_output *out) } out->fd = fd; - error: if (fd < 0) { fprintf(stderr, "INPUT: Failed on %s\n", out->name); @@ -123,8 +145,10 @@ static int OSPDaemon_input_send(struct OSPDaemon_output *out) while ((od = OSPDaemon_queue_get(&out->q)) != NULL) { vallen = extractOSP(out->type, od, &ts, val); DBGOUT("Sending (INPUT) %i %i %i\n", val[0], val[1], val[2]); - noise++; - noise %= 2; + if (out->option & INPUT_OPTION_DITHER) { + noise++; + noise %= 2; + } ev.type = EV_ABS; ev.code = ABS_X; ev.value = val[0]+noise; @@ -141,14 +165,28 @@ static int OSPDaemon_input_send(struct OSPDaemon_output *out) ev.code = ABS_Z; ev.value = val[2]+noise; ret = write(out->fd, &ev, sizeof(ev)); - if ( ret <= 0) return 1; - if (vallen > 3) { + if (ret <= 0) return 1; + if (vallen > 3 && out->option & INPUT_OPTION_QUAT4) { ev.type = EV_ABS; - ev.code = ABS_THROTTLE; /* ??? */ + ev.code = ABS_WHEEL; /* ??? */ ev.value = val[3]+noise; ret = write(out->fd, &ev, sizeof(ev)); if ( ret <= 0) return 1; } + if (out->option & INPUT_OPTION_EMBEDTS) { + /* Upper 32 bit */ + ev.type = EV_ABS; + ev.code = ABS_RUDDER; /* ??? */ + ev.value = (ts >> 32); + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + /* Lower 32 bit */ + ev.type = EV_ABS; + ev.code = ABS_THROTTLE; + ev.value = (ts & ((((unsigned long long)1)<<32)-1)); + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } ev.type = EV_SYN; ev.code = 0; From bcc459048843502dbcfa58e5fafec02739fa4716 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:18:47 -0700 Subject: [PATCH 17/21] Add header files from the Linux kernel. These files are used for defines and datastructures but are not included by the NDK toolchain. For now, save a copy here. Signed-off-by: Hunyue Yau --- include/linux/input.h | 1120 ++++++++++++++++++++++++++++++++++++++++ include/linux/uinput.h | 137 +++++ 2 files changed, 1257 insertions(+) create mode 100644 include/linux/input.h create mode 100644 include/linux/uinput.h diff --git a/include/linux/input.h b/include/linux/input.h new file mode 100644 index 0000000..d723b63 --- /dev/null +++ b/include/linux/input.h @@ -0,0 +1,1120 @@ +#ifndef _INPUT_H +#define _INPUT_H + +/* + * Copyright (c) 1999-2002 Vojtech Pavlik + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#include +#include +#include +#include + +/* + * The event structure itself + */ + +struct input_event { + struct timeval time; + __u16 type; + __u16 code; + __s32 value; +}; + +/* + * Protocol version. + */ + +#define EV_VERSION 0x010001 + +/* + * IOCTLs (0x00 - 0x7f) + */ + +struct input_id { + __u16 bustype; + __u16 vendor; + __u16 product; + __u16 version; +}; + +/** + * struct input_absinfo - used by EVIOCGABS/EVIOCSABS ioctls + * @value: latest reported value for the axis. + * @minimum: specifies minimum value for the axis. + * @maximum: specifies maximum value for the axis. + * @fuzz: specifies fuzz value that is used to filter noise from + * the event stream. + * @flat: values that are within this value will be discarded by + * joydev interface and reported as 0 instead. + * @resolution: specifies resolution for the values reported for + * the axis. + * + * Note that input core does not clamp reported values to the + * [minimum, maximum] limits, such task is left to userspace. + * + * Resolution for main axes (ABS_X, ABS_Y, ABS_Z) is reported in + * units per millimeter (units/mm), resolution for rotational axes + * (ABS_RX, ABS_RY, ABS_RZ) is reported in units per radian. + */ +struct input_absinfo { + __s32 value; + __s32 minimum; + __s32 maximum; + __s32 fuzz; + __s32 flat; + __s32 resolution; +}; + +/** + * struct input_keymap_entry - used by EVIOCGKEYCODE/EVIOCSKEYCODE ioctls + * @scancode: scancode represented in machine-endian form. + * @len: length of the scancode that resides in @scancode buffer. + * @index: index in the keymap, may be used instead of scancode + * @flags: allows to specify how kernel should handle the request. For + * example, setting INPUT_KEYMAP_BY_INDEX flag indicates that kernel + * should perform lookup in keymap by @index instead of @scancode + * @keycode: key code assigned to this scancode + * + * The structure is used to retrieve and modify keymap data. Users have + * option of performing lookup either by @scancode itself or by @index + * in keymap entry. EVIOCGKEYCODE will also return scancode or index + * (depending on which element was used to perform lookup). + */ +struct input_keymap_entry { +#define INPUT_KEYMAP_BY_INDEX (1 << 0) + __u8 flags; + __u8 len; + __u16 index; + __u32 keycode; + __u8 scancode[32]; +}; + +#define EVIOCGVERSION _IOR('E', 0x01, int) /* get driver version */ +#define EVIOCGID _IOR('E', 0x02, struct input_id) /* get device ID */ +#define EVIOCGREP _IOR('E', 0x03, unsigned int[2]) /* get repeat settings */ +#define EVIOCSREP _IOW('E', 0x03, unsigned int[2]) /* set repeat settings */ + +#define EVIOCGKEYCODE _IOR('E', 0x04, unsigned int[2]) /* get keycode */ +#define EVIOCGKEYCODE_V2 _IOR('E', 0x04, struct input_keymap_entry) +#define EVIOCSKEYCODE _IOW('E', 0x04, unsigned int[2]) /* set keycode */ +#define EVIOCSKEYCODE_V2 _IOW('E', 0x04, struct input_keymap_entry) + +#define EVIOCGNAME(len) _IOC(_IOC_READ, 'E', 0x06, len) /* get device name */ +#define EVIOCGPHYS(len) _IOC(_IOC_READ, 'E', 0x07, len) /* get physical location */ +#define EVIOCGUNIQ(len) _IOC(_IOC_READ, 'E', 0x08, len) /* get unique identifier */ +#define EVIOCGPROP(len) _IOC(_IOC_READ, 'E', 0x09, len) /* get device properties */ + +#define EVIOCGKEY(len) _IOC(_IOC_READ, 'E', 0x18, len) /* get global key state */ +#define EVIOCGLED(len) _IOC(_IOC_READ, 'E', 0x19, len) /* get all LEDs */ +#define EVIOCGSND(len) _IOC(_IOC_READ, 'E', 0x1a, len) /* get all sounds status */ +#define EVIOCGSW(len) _IOC(_IOC_READ, 'E', 0x1b, len) /* get all switch states */ + +#define EVIOCGBIT(ev,len) _IOC(_IOC_READ, 'E', 0x20 + (ev), len) /* get event bits */ +#define EVIOCGABS(abs) _IOR('E', 0x40 + (abs), struct input_absinfo) /* get abs value/limits */ +#define EVIOCSABS(abs) _IOW('E', 0xc0 + (abs), struct input_absinfo) /* set abs value/limits */ + +#define EVIOCSFF _IOC(_IOC_WRITE, 'E', 0x80, sizeof(struct ff_effect)) /* send a force effect to a force feedback device */ +#define EVIOCRMFF _IOW('E', 0x81, int) /* Erase a force effect */ +#define EVIOCGEFFECTS _IOR('E', 0x84, int) /* Report number of effects playable at the same time */ + +#define EVIOCGRAB _IOW('E', 0x90, int) /* Grab/Release device */ + +/* + * Device properties and quirks + */ + +#define INPUT_PROP_POINTER 0x00 /* needs a pointer */ +#define INPUT_PROP_DIRECT 0x01 /* direct input devices */ +#define INPUT_PROP_BUTTONPAD 0x02 /* has button(s) under pad */ +#define INPUT_PROP_SEMI_MT 0x03 /* touch rectangle only */ + +#define INPUT_PROP_MAX 0x1f +#define INPUT_PROP_CNT (INPUT_PROP_MAX + 1) + +/* + * Event types + */ + +#define EV_SYN 0x00 +#define EV_KEY 0x01 +#define EV_REL 0x02 +#define EV_ABS 0x03 +#define EV_MSC 0x04 +#define EV_SW 0x05 +#define EV_LED 0x11 +#define EV_SND 0x12 +#define EV_REP 0x14 +#define EV_FF 0x15 +#define EV_PWR 0x16 +#define EV_FF_STATUS 0x17 +#define EV_MAX 0x1f +#define EV_CNT (EV_MAX+1) + +/* + * Synchronization events. + */ + +#define SYN_REPORT 0 +#define SYN_CONFIG 1 +#define SYN_MT_REPORT 2 +#define SYN_DROPPED 3 + +/* + * Keys and buttons + * + * Most of the keys/buttons are modeled after USB HUT 1.12 + * (see http://www.usb.org/developers/hidpage). + * Abbreviations in the comments: + * AC - Application Control + * AL - Application Launch Button + * SC - System Control + */ + +#define KEY_RESERVED 0 +#define KEY_ESC 1 +#define KEY_1 2 +#define KEY_2 3 +#define KEY_3 4 +#define KEY_4 5 +#define KEY_5 6 +#define KEY_6 7 +#define KEY_7 8 +#define KEY_8 9 +#define KEY_9 10 +#define KEY_0 11 +#define KEY_MINUS 12 +#define KEY_EQUAL 13 +#define KEY_BACKSPACE 14 +#define KEY_TAB 15 +#define KEY_Q 16 +#define KEY_W 17 +#define KEY_E 18 +#define KEY_R 19 +#define KEY_T 20 +#define KEY_Y 21 +#define KEY_U 22 +#define KEY_I 23 +#define KEY_O 24 +#define KEY_P 25 +#define KEY_LEFTBRACE 26 +#define KEY_RIGHTBRACE 27 +#define KEY_ENTER 28 +#define KEY_LEFTCTRL 29 +#define KEY_A 30 +#define KEY_S 31 +#define KEY_D 32 +#define KEY_F 33 +#define KEY_G 34 +#define KEY_H 35 +#define KEY_J 36 +#define KEY_K 37 +#define KEY_L 38 +#define KEY_SEMICOLON 39 +#define KEY_APOSTROPHE 40 +#define KEY_GRAVE 41 +#define KEY_LEFTSHIFT 42 +#define KEY_BACKSLASH 43 +#define KEY_Z 44 +#define KEY_X 45 +#define KEY_C 46 +#define KEY_V 47 +#define KEY_B 48 +#define KEY_N 49 +#define KEY_M 50 +#define KEY_COMMA 51 +#define KEY_DOT 52 +#define KEY_SLASH 53 +#define KEY_RIGHTSHIFT 54 +#define KEY_KPASTERISK 55 +#define KEY_LEFTALT 56 +#define KEY_SPACE 57 +#define KEY_CAPSLOCK 58 +#define KEY_F1 59 +#define KEY_F2 60 +#define KEY_F3 61 +#define KEY_F4 62 +#define KEY_F5 63 +#define KEY_F6 64 +#define KEY_F7 65 +#define KEY_F8 66 +#define KEY_F9 67 +#define KEY_F10 68 +#define KEY_NUMLOCK 69 +#define KEY_SCROLLLOCK 70 +#define KEY_KP7 71 +#define KEY_KP8 72 +#define KEY_KP9 73 +#define KEY_KPMINUS 74 +#define KEY_KP4 75 +#define KEY_KP5 76 +#define KEY_KP6 77 +#define KEY_KPPLUS 78 +#define KEY_KP1 79 +#define KEY_KP2 80 +#define KEY_KP3 81 +#define KEY_KP0 82 +#define KEY_KPDOT 83 + +#define KEY_ZENKAKUHANKAKU 85 +#define KEY_102ND 86 +#define KEY_F11 87 +#define KEY_F12 88 +#define KEY_RO 89 +#define KEY_KATAKANA 90 +#define KEY_HIRAGANA 91 +#define KEY_HENKAN 92 +#define KEY_KATAKANAHIRAGANA 93 +#define KEY_MUHENKAN 94 +#define KEY_KPJPCOMMA 95 +#define KEY_KPENTER 96 +#define KEY_RIGHTCTRL 97 +#define KEY_KPSLASH 98 +#define KEY_SYSRQ 99 +#define KEY_RIGHTALT 100 +#define KEY_LINEFEED 101 +#define KEY_HOME 102 +#define KEY_UP 103 +#define KEY_PAGEUP 104 +#define KEY_LEFT 105 +#define KEY_RIGHT 106 +#define KEY_END 107 +#define KEY_DOWN 108 +#define KEY_PAGEDOWN 109 +#define KEY_INSERT 110 +#define KEY_DELETE 111 +#define KEY_MACRO 112 +#define KEY_MUTE 113 +#define KEY_VOLUMEDOWN 114 +#define KEY_VOLUMEUP 115 +#define KEY_POWER 116 /* SC System Power Down */ +#define KEY_KPEQUAL 117 +#define KEY_KPPLUSMINUS 118 +#define KEY_PAUSE 119 +#define KEY_SCALE 120 /* AL Compiz Scale (Expose) */ + +#define KEY_KPCOMMA 121 +#define KEY_HANGEUL 122 +#define KEY_HANGUEL KEY_HANGEUL +#define KEY_HANJA 123 +#define KEY_YEN 124 +#define KEY_LEFTMETA 125 +#define KEY_RIGHTMETA 126 +#define KEY_COMPOSE 127 + +#define KEY_STOP 128 /* AC Stop */ +#define KEY_AGAIN 129 +#define KEY_PROPS 130 /* AC Properties */ +#define KEY_UNDO 131 /* AC Undo */ +#define KEY_FRONT 132 +#define KEY_COPY 133 /* AC Copy */ +#define KEY_OPEN 134 /* AC Open */ +#define KEY_PASTE 135 /* AC Paste */ +#define KEY_FIND 136 /* AC Search */ +#define KEY_CUT 137 /* AC Cut */ +#define KEY_HELP 138 /* AL Integrated Help Center */ +#define KEY_MENU 139 /* Menu (show menu) */ +#define KEY_CALC 140 /* AL Calculator */ +#define KEY_SETUP 141 +#define KEY_SLEEP 142 /* SC System Sleep */ +#define KEY_WAKEUP 143 /* System Wake Up */ +#define KEY_FILE 144 /* AL Local Machine Browser */ +#define KEY_SENDFILE 145 +#define KEY_DELETEFILE 146 +#define KEY_XFER 147 +#define KEY_PROG1 148 +#define KEY_PROG2 149 +#define KEY_WWW 150 /* AL Internet Browser */ +#define KEY_MSDOS 151 +#define KEY_COFFEE 152 /* AL Terminal Lock/Screensaver */ +#define KEY_SCREENLOCK KEY_COFFEE +#define KEY_DIRECTION 153 +#define KEY_CYCLEWINDOWS 154 +#define KEY_MAIL 155 +#define KEY_BOOKMARKS 156 /* AC Bookmarks */ +#define KEY_COMPUTER 157 +#define KEY_BACK 158 /* AC Back */ +#define KEY_FORWARD 159 /* AC Forward */ +#define KEY_CLOSECD 160 +#define KEY_EJECTCD 161 +#define KEY_EJECTCLOSECD 162 +#define KEY_NEXTSONG 163 +#define KEY_PLAYPAUSE 164 +#define KEY_PREVIOUSSONG 165 +#define KEY_STOPCD 166 +#define KEY_RECORD 167 +#define KEY_REWIND 168 +#define KEY_PHONE 169 /* Media Select Telephone */ +#define KEY_ISO 170 +#define KEY_CONFIG 171 /* AL Consumer Control Configuration */ +#define KEY_HOMEPAGE 172 /* AC Home */ +#define KEY_REFRESH 173 /* AC Refresh */ +#define KEY_EXIT 174 /* AC Exit */ +#define KEY_MOVE 175 +#define KEY_EDIT 176 +#define KEY_SCROLLUP 177 +#define KEY_SCROLLDOWN 178 +#define KEY_KPLEFTPAREN 179 +#define KEY_KPRIGHTPAREN 180 +#define KEY_NEW 181 /* AC New */ +#define KEY_REDO 182 /* AC Redo/Repeat */ + +#define KEY_F13 183 +#define KEY_F14 184 +#define KEY_F15 185 +#define KEY_F16 186 +#define KEY_F17 187 +#define KEY_F18 188 +#define KEY_F19 189 +#define KEY_F20 190 +#define KEY_F21 191 +#define KEY_F22 192 +#define KEY_F23 193 +#define KEY_F24 194 + +#define KEY_PLAYCD 200 +#define KEY_PAUSECD 201 +#define KEY_PROG3 202 +#define KEY_PROG4 203 +#define KEY_DASHBOARD 204 /* AL Dashboard */ +#define KEY_SUSPEND 205 +#define KEY_CLOSE 206 /* AC Close */ +#define KEY_PLAY 207 +#define KEY_FASTFORWARD 208 +#define KEY_BASSBOOST 209 +#define KEY_PRINT 210 /* AC Print */ +#define KEY_HP 211 +#define KEY_CAMERA 212 +#define KEY_SOUND 213 +#define KEY_QUESTION 214 +#define KEY_EMAIL 215 +#define KEY_CHAT 216 +#define KEY_SEARCH 217 +#define KEY_CONNECT 218 +#define KEY_FINANCE 219 /* AL Checkbook/Finance */ +#define KEY_SPORT 220 +#define KEY_SHOP 221 +#define KEY_ALTERASE 222 +#define KEY_CANCEL 223 /* AC Cancel */ +#define KEY_BRIGHTNESSDOWN 224 +#define KEY_BRIGHTNESSUP 225 +#define KEY_MEDIA 226 + +#define KEY_SWITCHVIDEOMODE 227 /* Cycle between available video + outputs (Monitor/LCD/TV-out/etc) */ +#define KEY_KBDILLUMTOGGLE 228 +#define KEY_KBDILLUMDOWN 229 +#define KEY_KBDILLUMUP 230 + +#define KEY_SEND 231 /* AC Send */ +#define KEY_REPLY 232 /* AC Reply */ +#define KEY_FORWARDMAIL 233 /* AC Forward Msg */ +#define KEY_SAVE 234 /* AC Save */ +#define KEY_DOCUMENTS 235 + +#define KEY_BATTERY 236 + +#define KEY_BLUETOOTH 237 +#define KEY_WLAN 238 +#define KEY_UWB 239 + +#define KEY_UNKNOWN 240 + +#define KEY_VIDEO_NEXT 241 /* drive next video source */ +#define KEY_VIDEO_PREV 242 /* drive previous video source */ +#define KEY_BRIGHTNESS_CYCLE 243 /* brightness up, after max is min */ +#define KEY_BRIGHTNESS_ZERO 244 /* brightness off, use ambient */ +#define KEY_DISPLAY_OFF 245 /* display device to off state */ + +#define KEY_WIMAX 246 +#define KEY_RFKILL 247 /* Key that controls all radios */ + +#define KEY_MICMUTE 248 /* Mute / unmute the microphone */ + +/* Code 255 is reserved for special needs of AT keyboard driver */ + +#define BTN_MISC 0x100 +#define BTN_0 0x100 +#define BTN_1 0x101 +#define BTN_2 0x102 +#define BTN_3 0x103 +#define BTN_4 0x104 +#define BTN_5 0x105 +#define BTN_6 0x106 +#define BTN_7 0x107 +#define BTN_8 0x108 +#define BTN_9 0x109 + +#define BTN_MOUSE 0x110 +#define BTN_LEFT 0x110 +#define BTN_RIGHT 0x111 +#define BTN_MIDDLE 0x112 +#define BTN_SIDE 0x113 +#define BTN_EXTRA 0x114 +#define BTN_FORWARD 0x115 +#define BTN_BACK 0x116 +#define BTN_TASK 0x117 + +#define BTN_JOYSTICK 0x120 +#define BTN_TRIGGER 0x120 +#define BTN_THUMB 0x121 +#define BTN_THUMB2 0x122 +#define BTN_TOP 0x123 +#define BTN_TOP2 0x124 +#define BTN_PINKIE 0x125 +#define BTN_BASE 0x126 +#define BTN_BASE2 0x127 +#define BTN_BASE3 0x128 +#define BTN_BASE4 0x129 +#define BTN_BASE5 0x12a +#define BTN_BASE6 0x12b +#define BTN_DEAD 0x12f + +#define BTN_GAMEPAD 0x130 +#define BTN_A 0x130 +#define BTN_B 0x131 +#define BTN_C 0x132 +#define BTN_X 0x133 +#define BTN_Y 0x134 +#define BTN_Z 0x135 +#define BTN_TL 0x136 +#define BTN_TR 0x137 +#define BTN_TL2 0x138 +#define BTN_TR2 0x139 +#define BTN_SELECT 0x13a +#define BTN_START 0x13b +#define BTN_MODE 0x13c +#define BTN_THUMBL 0x13d +#define BTN_THUMBR 0x13e + +#define BTN_DIGI 0x140 +#define BTN_TOOL_PEN 0x140 +#define BTN_TOOL_RUBBER 0x141 +#define BTN_TOOL_BRUSH 0x142 +#define BTN_TOOL_PENCIL 0x143 +#define BTN_TOOL_AIRBRUSH 0x144 +#define BTN_TOOL_FINGER 0x145 +#define BTN_TOOL_MOUSE 0x146 +#define BTN_TOOL_LENS 0x147 +#define BTN_TOOL_QUINTTAP 0x148 /* Five fingers on trackpad */ +#define BTN_TOUCH 0x14a +#define BTN_STYLUS 0x14b +#define BTN_STYLUS2 0x14c +#define BTN_TOOL_DOUBLETAP 0x14d +#define BTN_TOOL_TRIPLETAP 0x14e +#define BTN_TOOL_QUADTAP 0x14f /* Four fingers on trackpad */ + +#define BTN_WHEEL 0x150 +#define BTN_GEAR_DOWN 0x150 +#define BTN_GEAR_UP 0x151 + +#define KEY_OK 0x160 +#define KEY_SELECT 0x161 +#define KEY_GOTO 0x162 +#define KEY_CLEAR 0x163 +#define KEY_POWER2 0x164 +#define KEY_OPTION 0x165 +#define KEY_INFO 0x166 /* AL OEM Features/Tips/Tutorial */ +#define KEY_TIME 0x167 +#define KEY_VENDOR 0x168 +#define KEY_ARCHIVE 0x169 +#define KEY_PROGRAM 0x16a /* Media Select Program Guide */ +#define KEY_CHANNEL 0x16b +#define KEY_FAVORITES 0x16c +#define KEY_EPG 0x16d +#define KEY_PVR 0x16e /* Media Select Home */ +#define KEY_MHP 0x16f +#define KEY_LANGUAGE 0x170 +#define KEY_TITLE 0x171 +#define KEY_SUBTITLE 0x172 +#define KEY_ANGLE 0x173 +#define KEY_ZOOM 0x174 +#define KEY_MODE 0x175 +#define KEY_KEYBOARD 0x176 +#define KEY_SCREEN 0x177 +#define KEY_PC 0x178 /* Media Select Computer */ +#define KEY_TV 0x179 /* Media Select TV */ +#define KEY_TV2 0x17a /* Media Select Cable */ +#define KEY_VCR 0x17b /* Media Select VCR */ +#define KEY_VCR2 0x17c /* VCR Plus */ +#define KEY_SAT 0x17d /* Media Select Satellite */ +#define KEY_SAT2 0x17e +#define KEY_CD 0x17f /* Media Select CD */ +#define KEY_TAPE 0x180 /* Media Select Tape */ +#define KEY_RADIO 0x181 +#define KEY_TUNER 0x182 /* Media Select Tuner */ +#define KEY_PLAYER 0x183 +#define KEY_TEXT 0x184 +#define KEY_DVD 0x185 /* Media Select DVD */ +#define KEY_AUX 0x186 +#define KEY_MP3 0x187 +#define KEY_AUDIO 0x188 /* AL Audio Browser */ +#define KEY_VIDEO 0x189 /* AL Movie Browser */ +#define KEY_DIRECTORY 0x18a +#define KEY_LIST 0x18b +#define KEY_MEMO 0x18c /* Media Select Messages */ +#define KEY_CALENDAR 0x18d +#define KEY_RED 0x18e +#define KEY_GREEN 0x18f +#define KEY_YELLOW 0x190 +#define KEY_BLUE 0x191 +#define KEY_CHANNELUP 0x192 /* Channel Increment */ +#define KEY_CHANNELDOWN 0x193 /* Channel Decrement */ +#define KEY_FIRST 0x194 +#define KEY_LAST 0x195 /* Recall Last */ +#define KEY_AB 0x196 +#define KEY_NEXT 0x197 +#define KEY_RESTART 0x198 +#define KEY_SLOW 0x199 +#define KEY_SHUFFLE 0x19a +#define KEY_BREAK 0x19b +#define KEY_PREVIOUS 0x19c +#define KEY_DIGITS 0x19d +#define KEY_TEEN 0x19e +#define KEY_TWEN 0x19f +#define KEY_VIDEOPHONE 0x1a0 /* Media Select Video Phone */ +#define KEY_GAMES 0x1a1 /* Media Select Games */ +#define KEY_ZOOMIN 0x1a2 /* AC Zoom In */ +#define KEY_ZOOMOUT 0x1a3 /* AC Zoom Out */ +#define KEY_ZOOMRESET 0x1a4 /* AC Zoom */ +#define KEY_WORDPROCESSOR 0x1a5 /* AL Word Processor */ +#define KEY_EDITOR 0x1a6 /* AL Text Editor */ +#define KEY_SPREADSHEET 0x1a7 /* AL Spreadsheet */ +#define KEY_GRAPHICSEDITOR 0x1a8 /* AL Graphics Editor */ +#define KEY_PRESENTATION 0x1a9 /* AL Presentation App */ +#define KEY_DATABASE 0x1aa /* AL Database App */ +#define KEY_NEWS 0x1ab /* AL Newsreader */ +#define KEY_VOICEMAIL 0x1ac /* AL Voicemail */ +#define KEY_ADDRESSBOOK 0x1ad /* AL Contacts/Address Book */ +#define KEY_MESSENGER 0x1ae /* AL Instant Messaging */ +#define KEY_DISPLAYTOGGLE 0x1af /* Turn display (LCD) on and off */ +#define KEY_SPELLCHECK 0x1b0 /* AL Spell Check */ +#define KEY_LOGOFF 0x1b1 /* AL Logoff */ + +#define KEY_DOLLAR 0x1b2 +#define KEY_EURO 0x1b3 + +#define KEY_FRAMEBACK 0x1b4 /* Consumer - transport controls */ +#define KEY_FRAMEFORWARD 0x1b5 +#define KEY_CONTEXT_MENU 0x1b6 /* GenDesc - system context menu */ +#define KEY_MEDIA_REPEAT 0x1b7 /* Consumer - transport control */ +#define KEY_10CHANNELSUP 0x1b8 /* 10 channels up (10+) */ +#define KEY_10CHANNELSDOWN 0x1b9 /* 10 channels down (10-) */ +#define KEY_IMAGES 0x1ba /* AL Image Browser */ + +#define KEY_DEL_EOL 0x1c0 +#define KEY_DEL_EOS 0x1c1 +#define KEY_INS_LINE 0x1c2 +#define KEY_DEL_LINE 0x1c3 + +#define KEY_FN 0x1d0 +#define KEY_FN_ESC 0x1d1 +#define KEY_FN_F1 0x1d2 +#define KEY_FN_F2 0x1d3 +#define KEY_FN_F3 0x1d4 +#define KEY_FN_F4 0x1d5 +#define KEY_FN_F5 0x1d6 +#define KEY_FN_F6 0x1d7 +#define KEY_FN_F7 0x1d8 +#define KEY_FN_F8 0x1d9 +#define KEY_FN_F9 0x1da +#define KEY_FN_F10 0x1db +#define KEY_FN_F11 0x1dc +#define KEY_FN_F12 0x1dd +#define KEY_FN_1 0x1de +#define KEY_FN_2 0x1df +#define KEY_FN_D 0x1e0 +#define KEY_FN_E 0x1e1 +#define KEY_FN_F 0x1e2 +#define KEY_FN_S 0x1e3 +#define KEY_FN_B 0x1e4 + +#define KEY_BRL_DOT1 0x1f1 +#define KEY_BRL_DOT2 0x1f2 +#define KEY_BRL_DOT3 0x1f3 +#define KEY_BRL_DOT4 0x1f4 +#define KEY_BRL_DOT5 0x1f5 +#define KEY_BRL_DOT6 0x1f6 +#define KEY_BRL_DOT7 0x1f7 +#define KEY_BRL_DOT8 0x1f8 +#define KEY_BRL_DOT9 0x1f9 +#define KEY_BRL_DOT10 0x1fa + +#define KEY_NUMERIC_0 0x200 /* used by phones, remote controls, */ +#define KEY_NUMERIC_1 0x201 /* and other keypads */ +#define KEY_NUMERIC_2 0x202 +#define KEY_NUMERIC_3 0x203 +#define KEY_NUMERIC_4 0x204 +#define KEY_NUMERIC_5 0x205 +#define KEY_NUMERIC_6 0x206 +#define KEY_NUMERIC_7 0x207 +#define KEY_NUMERIC_8 0x208 +#define KEY_NUMERIC_9 0x209 +#define KEY_NUMERIC_STAR 0x20a +#define KEY_NUMERIC_POUND 0x20b + +#define KEY_CAMERA_FOCUS 0x210 +#define KEY_WPS_BUTTON 0x211 /* WiFi Protected Setup key */ + +#define KEY_TOUCHPAD_TOGGLE 0x212 /* Request switch touchpad on or off */ +#define KEY_TOUCHPAD_ON 0x213 +#define KEY_TOUCHPAD_OFF 0x214 + +#define KEY_CAMERA_ZOOMIN 0x215 +#define KEY_CAMERA_ZOOMOUT 0x216 +#define KEY_CAMERA_UP 0x217 +#define KEY_CAMERA_DOWN 0x218 +#define KEY_CAMERA_LEFT 0x219 +#define KEY_CAMERA_RIGHT 0x21a + +#define BTN_TRIGGER_HAPPY 0x2c0 +#define BTN_TRIGGER_HAPPY1 0x2c0 +#define BTN_TRIGGER_HAPPY2 0x2c1 +#define BTN_TRIGGER_HAPPY3 0x2c2 +#define BTN_TRIGGER_HAPPY4 0x2c3 +#define BTN_TRIGGER_HAPPY5 0x2c4 +#define BTN_TRIGGER_HAPPY6 0x2c5 +#define BTN_TRIGGER_HAPPY7 0x2c6 +#define BTN_TRIGGER_HAPPY8 0x2c7 +#define BTN_TRIGGER_HAPPY9 0x2c8 +#define BTN_TRIGGER_HAPPY10 0x2c9 +#define BTN_TRIGGER_HAPPY11 0x2ca +#define BTN_TRIGGER_HAPPY12 0x2cb +#define BTN_TRIGGER_HAPPY13 0x2cc +#define BTN_TRIGGER_HAPPY14 0x2cd +#define BTN_TRIGGER_HAPPY15 0x2ce +#define BTN_TRIGGER_HAPPY16 0x2cf +#define BTN_TRIGGER_HAPPY17 0x2d0 +#define BTN_TRIGGER_HAPPY18 0x2d1 +#define BTN_TRIGGER_HAPPY19 0x2d2 +#define BTN_TRIGGER_HAPPY20 0x2d3 +#define BTN_TRIGGER_HAPPY21 0x2d4 +#define BTN_TRIGGER_HAPPY22 0x2d5 +#define BTN_TRIGGER_HAPPY23 0x2d6 +#define BTN_TRIGGER_HAPPY24 0x2d7 +#define BTN_TRIGGER_HAPPY25 0x2d8 +#define BTN_TRIGGER_HAPPY26 0x2d9 +#define BTN_TRIGGER_HAPPY27 0x2da +#define BTN_TRIGGER_HAPPY28 0x2db +#define BTN_TRIGGER_HAPPY29 0x2dc +#define BTN_TRIGGER_HAPPY30 0x2dd +#define BTN_TRIGGER_HAPPY31 0x2de +#define BTN_TRIGGER_HAPPY32 0x2df +#define BTN_TRIGGER_HAPPY33 0x2e0 +#define BTN_TRIGGER_HAPPY34 0x2e1 +#define BTN_TRIGGER_HAPPY35 0x2e2 +#define BTN_TRIGGER_HAPPY36 0x2e3 +#define BTN_TRIGGER_HAPPY37 0x2e4 +#define BTN_TRIGGER_HAPPY38 0x2e5 +#define BTN_TRIGGER_HAPPY39 0x2e6 +#define BTN_TRIGGER_HAPPY40 0x2e7 + +/* We avoid low common keys in module aliases so they don't get huge. */ +#define KEY_MIN_INTERESTING KEY_MUTE +#define KEY_MAX 0x2ff +#define KEY_CNT (KEY_MAX+1) + +/* + * Relative axes + */ + +#define REL_X 0x00 +#define REL_Y 0x01 +#define REL_Z 0x02 +#define REL_RX 0x03 +#define REL_RY 0x04 +#define REL_RZ 0x05 +#define REL_HWHEEL 0x06 +#define REL_DIAL 0x07 +#define REL_WHEEL 0x08 +#define REL_MISC 0x09 +#define REL_MAX 0x0f +#define REL_CNT (REL_MAX+1) + +/* + * Absolute axes + */ + +#define ABS_X 0x00 +#define ABS_Y 0x01 +#define ABS_Z 0x02 +#define ABS_RX 0x03 +#define ABS_RY 0x04 +#define ABS_RZ 0x05 +#define ABS_THROTTLE 0x06 +#define ABS_RUDDER 0x07 +#define ABS_WHEEL 0x08 +#define ABS_GAS 0x09 +#define ABS_BRAKE 0x0a +#define ABS_HAT0X 0x10 +#define ABS_HAT0Y 0x11 +#define ABS_HAT1X 0x12 +#define ABS_HAT1Y 0x13 +#define ABS_HAT2X 0x14 +#define ABS_HAT2Y 0x15 +#define ABS_HAT3X 0x16 +#define ABS_HAT3Y 0x17 +#define ABS_PRESSURE 0x18 +#define ABS_DISTANCE 0x19 +#define ABS_TILT_X 0x1a +#define ABS_TILT_Y 0x1b +#define ABS_TOOL_WIDTH 0x1c + +#define ABS_VOLUME 0x20 + +#define ABS_MISC 0x28 + +#define ABS_MT_SLOT 0x2f /* MT slot being modified */ +#define ABS_MT_TOUCH_MAJOR 0x30 /* Major axis of touching ellipse */ +#define ABS_MT_TOUCH_MINOR 0x31 /* Minor axis (omit if circular) */ +#define ABS_MT_WIDTH_MAJOR 0x32 /* Major axis of approaching ellipse */ +#define ABS_MT_WIDTH_MINOR 0x33 /* Minor axis (omit if circular) */ +#define ABS_MT_ORIENTATION 0x34 /* Ellipse orientation */ +#define ABS_MT_POSITION_X 0x35 /* Center X ellipse position */ +#define ABS_MT_POSITION_Y 0x36 /* Center Y ellipse position */ +#define ABS_MT_TOOL_TYPE 0x37 /* Type of touching device */ +#define ABS_MT_BLOB_ID 0x38 /* Group a set of packets as a blob */ +#define ABS_MT_TRACKING_ID 0x39 /* Unique ID of initiated contact */ +#define ABS_MT_PRESSURE 0x3a /* Pressure on contact area */ +#define ABS_MT_DISTANCE 0x3b /* Contact hover distance */ + + +#define ABS_MAX 0x3f +#define ABS_CNT (ABS_MAX+1) + +/* + * Switch events + */ + +#define SW_LID 0x00 /* set = lid shut */ +#define SW_TABLET_MODE 0x01 /* set = tablet mode */ +#define SW_HEADPHONE_INSERT 0x02 /* set = inserted */ +#define SW_RFKILL_ALL 0x03 /* rfkill master switch, type "any" + set = radio enabled */ +#define SW_RADIO SW_RFKILL_ALL /* deprecated */ +#define SW_MICROPHONE_INSERT 0x04 /* set = inserted */ +#define SW_DOCK 0x05 /* set = plugged into dock */ +#define SW_LINEOUT_INSERT 0x06 /* set = inserted */ +#define SW_JACK_PHYSICAL_INSERT 0x07 /* set = mechanical switch set */ +#define SW_VIDEOOUT_INSERT 0x08 /* set = inserted */ +#define SW_CAMERA_LENS_COVER 0x09 /* set = lens covered */ +#define SW_KEYPAD_SLIDE 0x0a /* set = keypad slide out */ +#define SW_FRONT_PROXIMITY 0x0b /* set = front proximity sensor active */ +#define SW_ROTATE_LOCK 0x0c /* set = rotate locked/disabled */ +#define SW_LINEIN_INSERT 0x0d /* set = inserted */ +#define SW_MAX 0x0f +#define SW_CNT (SW_MAX+1) + +/* + * Misc events + */ + +#define MSC_SERIAL 0x00 +#define MSC_PULSELED 0x01 +#define MSC_GESTURE 0x02 +#define MSC_RAW 0x03 +#define MSC_SCAN 0x04 +#define MSC_MAX 0x07 +#define MSC_CNT (MSC_MAX+1) + +/* + * LEDs + */ + +#define LED_NUML 0x00 +#define LED_CAPSL 0x01 +#define LED_SCROLLL 0x02 +#define LED_COMPOSE 0x03 +#define LED_KANA 0x04 +#define LED_SLEEP 0x05 +#define LED_SUSPEND 0x06 +#define LED_MUTE 0x07 +#define LED_MISC 0x08 +#define LED_MAIL 0x09 +#define LED_CHARGING 0x0a +#define LED_MAX 0x0f +#define LED_CNT (LED_MAX+1) + +/* + * Autorepeat values + */ + +#define REP_DELAY 0x00 +#define REP_PERIOD 0x01 +#define REP_MAX 0x01 +#define REP_CNT (REP_MAX+1) + +/* + * Sounds + */ + +#define SND_CLICK 0x00 +#define SND_BELL 0x01 +#define SND_TONE 0x02 +#define SND_MAX 0x07 +#define SND_CNT (SND_MAX+1) + +/* + * IDs. + */ + +#define ID_BUS 0 +#define ID_VENDOR 1 +#define ID_PRODUCT 2 +#define ID_VERSION 3 + +#define BUS_PCI 0x01 +#define BUS_ISAPNP 0x02 +#define BUS_USB 0x03 +#define BUS_HIL 0x04 +#define BUS_BLUETOOTH 0x05 +#define BUS_VIRTUAL 0x06 + +#define BUS_ISA 0x10 +#define BUS_I8042 0x11 +#define BUS_XTKBD 0x12 +#define BUS_RS232 0x13 +#define BUS_GAMEPORT 0x14 +#define BUS_PARPORT 0x15 +#define BUS_AMIGA 0x16 +#define BUS_ADB 0x17 +#define BUS_I2C 0x18 +#define BUS_HOST 0x19 +#define BUS_GSC 0x1A +#define BUS_ATARI 0x1B +#define BUS_SPI 0x1C + +/* + * MT_TOOL types + */ +#define MT_TOOL_FINGER 0 +#define MT_TOOL_PEN 1 +#define MT_TOOL_MAX 1 + +/* + * Values describing the status of a force-feedback effect + */ +#define FF_STATUS_STOPPED 0x00 +#define FF_STATUS_PLAYING 0x01 +#define FF_STATUS_MAX 0x01 + +/* + * Structures used in ioctls to upload effects to a device + * They are pieces of a bigger structure (called ff_effect) + */ + +/* + * All duration values are expressed in ms. Values above 32767 ms (0x7fff) + * should not be used and have unspecified results. + */ + +/** + * struct ff_replay - defines scheduling of the force-feedback effect + * @length: duration of the effect + * @delay: delay before effect should start playing + */ +struct ff_replay { + __u16 length; + __u16 delay; +}; + +/** + * struct ff_trigger - defines what triggers the force-feedback effect + * @button: number of the button triggering the effect + * @interval: controls how soon the effect can be re-triggered + */ +struct ff_trigger { + __u16 button; + __u16 interval; +}; + +/** + * struct ff_envelope - generic force-feedback effect envelope + * @attack_length: duration of the attack (ms) + * @attack_level: level at the beginning of the attack + * @fade_length: duration of fade (ms) + * @fade_level: level at the end of fade + * + * The @attack_level and @fade_level are absolute values; when applying + * envelope force-feedback core will convert to positive/negative + * value based on polarity of the default level of the effect. + * Valid range for the attack and fade levels is 0x0000 - 0x7fff + */ +struct ff_envelope { + __u16 attack_length; + __u16 attack_level; + __u16 fade_length; + __u16 fade_level; +}; + +/** + * struct ff_constant_effect - defines parameters of a constant force-feedback effect + * @level: strength of the effect; may be negative + * @envelope: envelope data + */ +struct ff_constant_effect { + __s16 level; + struct ff_envelope envelope; +}; + +/** + * struct ff_ramp_effect - defines parameters of a ramp force-feedback effect + * @start_level: beginning strength of the effect; may be negative + * @end_level: final strength of the effect; may be negative + * @envelope: envelope data + */ +struct ff_ramp_effect { + __s16 start_level; + __s16 end_level; + struct ff_envelope envelope; +}; + +/** + * struct ff_condition_effect - defines a spring or friction force-feedback effect + * @right_saturation: maximum level when joystick moved all way to the right + * @left_saturation: same for the left side + * @right_coeff: controls how fast the force grows when the joystick moves + * to the right + * @left_coeff: same for the left side + * @deadband: size of the dead zone, where no force is produced + * @center: position of the dead zone + */ +struct ff_condition_effect { + __u16 right_saturation; + __u16 left_saturation; + + __s16 right_coeff; + __s16 left_coeff; + + __u16 deadband; + __s16 center; +}; + +/** + * struct ff_periodic_effect - defines parameters of a periodic force-feedback effect + * @waveform: kind of the effect (wave) + * @period: period of the wave (ms) + * @magnitude: peak value + * @offset: mean value of the wave (roughly) + * @phase: 'horizontal' shift + * @envelope: envelope data + * @custom_len: number of samples (FF_CUSTOM only) + * @custom_data: buffer of samples (FF_CUSTOM only) + * + * Known waveforms - FF_SQUARE, FF_TRIANGLE, FF_SINE, FF_SAW_UP, + * FF_SAW_DOWN, FF_CUSTOM. The exact syntax FF_CUSTOM is undefined + * for the time being as no driver supports it yet. + * + * Note: the data pointed by custom_data is copied by the driver. + * You can therefore dispose of the memory after the upload/update. + */ +struct ff_periodic_effect { + __u16 waveform; + __u16 period; + __s16 magnitude; + __s16 offset; + __u16 phase; + + struct ff_envelope envelope; + + __u32 custom_len; + __s16 *custom_data; +}; + +/** + * struct ff_rumble_effect - defines parameters of a periodic force-feedback effect + * @strong_magnitude: magnitude of the heavy motor + * @weak_magnitude: magnitude of the light one + * + * Some rumble pads have two motors of different weight. Strong_magnitude + * represents the magnitude of the vibration generated by the heavy one. + */ +struct ff_rumble_effect { + __u16 strong_magnitude; + __u16 weak_magnitude; +}; + +/** + * struct ff_effect - defines force feedback effect + * @type: type of the effect (FF_CONSTANT, FF_PERIODIC, FF_RAMP, FF_SPRING, + * FF_FRICTION, FF_DAMPER, FF_RUMBLE, FF_INERTIA, or FF_CUSTOM) + * @id: an unique id assigned to an effect + * @direction: direction of the effect + * @trigger: trigger conditions (struct ff_trigger) + * @replay: scheduling of the effect (struct ff_replay) + * @u: effect-specific structure (one of ff_constant_effect, ff_ramp_effect, + * ff_periodic_effect, ff_condition_effect, ff_rumble_effect) further + * defining effect parameters + * + * This structure is sent through ioctl from the application to the driver. + * To create a new effect application should set its @id to -1; the kernel + * will return assigned @id which can later be used to update or delete + * this effect. + * + * Direction of the effect is encoded as follows: + * 0 deg -> 0x0000 (down) + * 90 deg -> 0x4000 (left) + * 180 deg -> 0x8000 (up) + * 270 deg -> 0xC000 (right) + */ +struct ff_effect { + __u16 type; + __s16 id; + __u16 direction; + struct ff_trigger trigger; + struct ff_replay replay; + + union { + struct ff_constant_effect constant; + struct ff_ramp_effect ramp; + struct ff_periodic_effect periodic; + struct ff_condition_effect condition[2]; /* One for each axis */ + struct ff_rumble_effect rumble; + } u; +}; + +/* + * Force feedback effect types + */ + +#define FF_RUMBLE 0x50 +#define FF_PERIODIC 0x51 +#define FF_CONSTANT 0x52 +#define FF_SPRING 0x53 +#define FF_FRICTION 0x54 +#define FF_DAMPER 0x55 +#define FF_INERTIA 0x56 +#define FF_RAMP 0x57 + +#define FF_EFFECT_MIN FF_RUMBLE +#define FF_EFFECT_MAX FF_RAMP + +/* + * Force feedback periodic effect types + */ + +#define FF_SQUARE 0x58 +#define FF_TRIANGLE 0x59 +#define FF_SINE 0x5a +#define FF_SAW_UP 0x5b +#define FF_SAW_DOWN 0x5c +#define FF_CUSTOM 0x5d + +#define FF_WAVEFORM_MIN FF_SQUARE +#define FF_WAVEFORM_MAX FF_CUSTOM + +/* + * Set ff device properties + */ + +#define FF_GAIN 0x60 +#define FF_AUTOCENTER 0x61 + +#define FF_MAX 0x7f +#define FF_CNT (FF_MAX+1) + +#endif diff --git a/include/linux/uinput.h b/include/linux/uinput.h new file mode 100644 index 0000000..4466595 --- /dev/null +++ b/include/linux/uinput.h @@ -0,0 +1,137 @@ +#ifndef __UINPUT_H_ +#define __UINPUT_H_ +/* + * User level driver support for input subsystem + * + * Heavily based on evdev.c by Vojtech Pavlik + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Author: Aristeu Sergio Rozanski Filho + * + * Changes/Revisions: + * 0.3 24/05/2006 (Anssi Hannula ) + * - update ff support for the changes in kernel interface + * - add UINPUT_VERSION + * 0.2 16/10/2004 (Micah Dowty ) + * - added force feedback support + * - added UI_SET_PHYS + * 0.1 20/06/2002 + * - first public version + */ + +#include + +#define UINPUT_VERSION 3 + + +struct uinput_ff_upload { + int request_id; + int retval; + struct ff_effect effect; + struct ff_effect old; +}; + +struct uinput_ff_erase { + int request_id; + int retval; + int effect_id; +}; + +/* ioctl */ +#define UINPUT_IOCTL_BASE 'U' +#define UI_DEV_CREATE _IO(UINPUT_IOCTL_BASE, 1) +#define UI_DEV_DESTROY _IO(UINPUT_IOCTL_BASE, 2) + +#define UI_SET_EVBIT _IOW(UINPUT_IOCTL_BASE, 100, int) +#define UI_SET_KEYBIT _IOW(UINPUT_IOCTL_BASE, 101, int) +#define UI_SET_RELBIT _IOW(UINPUT_IOCTL_BASE, 102, int) +#define UI_SET_ABSBIT _IOW(UINPUT_IOCTL_BASE, 103, int) +#define UI_SET_MSCBIT _IOW(UINPUT_IOCTL_BASE, 104, int) +#define UI_SET_LEDBIT _IOW(UINPUT_IOCTL_BASE, 105, int) +#define UI_SET_SNDBIT _IOW(UINPUT_IOCTL_BASE, 106, int) +#define UI_SET_FFBIT _IOW(UINPUT_IOCTL_BASE, 107, int) +#define UI_SET_PHYS _IOW(UINPUT_IOCTL_BASE, 108, char*) +#define UI_SET_SWBIT _IOW(UINPUT_IOCTL_BASE, 109, int) +#define UI_SET_PROPBIT _IOW(UINPUT_IOCTL_BASE, 110, int) + +#define UI_BEGIN_FF_UPLOAD _IOWR(UINPUT_IOCTL_BASE, 200, struct uinput_ff_upload) +#define UI_END_FF_UPLOAD _IOW(UINPUT_IOCTL_BASE, 201, struct uinput_ff_upload) +#define UI_BEGIN_FF_ERASE _IOWR(UINPUT_IOCTL_BASE, 202, struct uinput_ff_erase) +#define UI_END_FF_ERASE _IOW(UINPUT_IOCTL_BASE, 203, struct uinput_ff_erase) + +/* + * To write a force-feedback-capable driver, the upload_effect + * and erase_effect callbacks in input_dev must be implemented. + * The uinput driver will generate a fake input event when one of + * these callbacks are invoked. The userspace code then uses + * ioctls to retrieve additional parameters and send the return code. + * The callback blocks until this return code is sent. + * + * The described callback mechanism is only used if ff_effects_max + * is set. + * + * To implement upload_effect(): + * 1. Wait for an event with type == EV_UINPUT and code == UI_FF_UPLOAD. + * A request ID will be given in 'value'. + * 2. Allocate a uinput_ff_upload struct, fill in request_id with + * the 'value' from the EV_UINPUT event. + * 3. Issue a UI_BEGIN_FF_UPLOAD ioctl, giving it the + * uinput_ff_upload struct. It will be filled in with the + * ff_effects passed to upload_effect(). + * 4. Perform the effect upload, and place a return code back into + the uinput_ff_upload struct. + * 5. Issue a UI_END_FF_UPLOAD ioctl, also giving it the + * uinput_ff_upload_effect struct. This will complete execution + * of our upload_effect() handler. + * + * To implement erase_effect(): + * 1. Wait for an event with type == EV_UINPUT and code == UI_FF_ERASE. + * A request ID will be given in 'value'. + * 2. Allocate a uinput_ff_erase struct, fill in request_id with + * the 'value' from the EV_UINPUT event. + * 3. Issue a UI_BEGIN_FF_ERASE ioctl, giving it the + * uinput_ff_erase struct. It will be filled in with the + * effect ID passed to erase_effect(). + * 4. Perform the effect erasure, and place a return code back + * into the uinput_ff_erase struct. + * 5. Issue a UI_END_FF_ERASE ioctl, also giving it the + * uinput_ff_erase_effect struct. This will complete execution + * of our erase_effect() handler. + */ + +/* + * This is the new event type, used only by uinput. + * 'code' is UI_FF_UPLOAD or UI_FF_ERASE, and 'value' + * is the unique request ID. This number was picked + * arbitrarily, above EV_MAX (since the input system + * never sees it) but in the range of a 16-bit int. + */ +#define EV_UINPUT 0x0101 +#define UI_FF_UPLOAD 1 +#define UI_FF_ERASE 2 + +#define UINPUT_MAX_NAME_SIZE 80 +struct uinput_user_dev { + char name[UINPUT_MAX_NAME_SIZE]; + struct input_id id; + int ff_effects_max; + int absmax[ABS_CNT]; + int absmin[ABS_CNT]; + int absfuzz[ABS_CNT]; + int absflat[ABS_CNT]; +}; +#endif /* __UINPUT_H_ */ + From 0c7514f44b9b4629ac0ebc7a0a2c3795206cabcf Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Mon, 28 Sep 2015 18:39:09 -0700 Subject: [PATCH 18/21] Move the first valid driver define to a non-zero value. The data structure is initialized to 0 but DRIVER_INPUT is also 0. Moving DRIVER_INPUT to a non-zero value allows 0'ed but not configured entries to be ignored. Signed-off-by: Hunyue Yau --- linux/daemon/OSPDaemon.h | 1 + linux/daemon/OSPDaemon_config.c | 1 + 2 files changed, 2 insertions(+) diff --git a/linux/daemon/OSPDaemon.h b/linux/daemon/OSPDaemon.h index fe8ba15..6cba63b 100644 --- a/linux/daemon/OSPDaemon.h +++ b/linux/daemon/OSPDaemon.h @@ -40,6 +40,7 @@ enum { }; enum { + DRIVER_INVALID, DRIVER_INPUT, DRIVER_IIO, DRIVER_IIOEVENT, diff --git a/linux/daemon/OSPDaemon_config.c b/linux/daemon/OSPDaemon_config.c index 515f10c..12a9210 100644 --- a/linux/daemon/OSPDaemon_config.c +++ b/linux/daemon/OSPDaemon_config.c @@ -108,6 +108,7 @@ static const char * SensorTypeMapP[] = { }; static const char *DriverTypeMap[] = { + [DRIVER_INVALID] = "invalid", [DRIVER_INPUT] = "input", [DRIVER_IIO] = "iio", [DRIVER_IIOEVENT] = "iioevent", From 43c5cdde69bc40884979053730b9db0083af518e Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Tue, 29 Sep 2015 13:53:27 -0700 Subject: [PATCH 19/21] Documentation for daemon config file format Signed-off-by: Hunyue Yau --- docs/OSP-daemon.html | 132 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100755 docs/OSP-daemon.html diff --git a/docs/OSP-daemon.html b/docs/OSP-daemon.html new file mode 100755 index 0000000..2b30feb --- /dev/null +++ b/docs/OSP-daemon.html @@ -0,0 +1,132 @@ +

OSPDaemon config file structure

+

Comments

+
    +
  • Lines beginning with the following is consider a comment: + Space, tab, #, ; +
  • Blank lines are ignored. +
+

Line format

+ KEYn.tag=value +

+ Where KEY is: +

+
system +
output +
This configures a sensor with data created or passed out by the daemon. It is for outbound data. +
input +
This configures a sensor that feeds data into the daemon. The data +can be consumed and processed by algs and/or it can be merely copied out to an outbound sensor. +
+ n is an integer from 0 to 99. Each KEY has its own number space.
+ This number is used to identify all the attributes related + to that entity. It is also used to reference the entity. + +

Valid lines

+
+
output0.name and input0.name +
This defines the name of the sensor. Exact usage depends on the +driver. +
    +
  • For input event drivers, this is the name of the input +device. For the outbound case, this is the name created. For the inbound case, +this is the name to look for. +
  • For iio drivers, this is the name of the iio device to +look for. +
  • For file oriented drivers, this is the name of the file to +read from or create/append. +
+ +
output0.type and input0.type +
This defines the type of sensor for output and input purposes. It +should be a string like SENSOR_ACCELEROMETER. The name is defined by the +enumerated type in OSP. Outbound sensors will come from either another +inbound sensor or from the algs. + +
output0.driver and input0.driver +
This defines the driver associated with the entity. + +
input0.copy +
Defines an output sensor to send a copy of the data to. Value +is the name of the output entity. For example:

+input0.copy=output1

+This will send a copy of data received on the input0 entity to output1 without +any transformation. + +

input0.swap +
This defines how the 3 axises from the input sensor will be +handled when it is process by the alg. This is specified by providing 3 +comma seperated numbers: 0, 1, and 2 representing X, Y, and Z respectively. +The order is X, Y, Z. Examples:

+

    +
  • No change: 0,1,2 +
  • Swap X and Y: 1,0,2 +
+
input0.convert +
Define a conversion factor for each axis. This number is multiplied +by the data prior being used. The number is a floating point value. The value can be provided in 2 formats. +
    +
  1. A single value. The same value is applied to all 3 axis. +
  2. 3 common seperated values. Each axis can have a unique value. +
+This value is typically used to scale and if needed invert an axis. +
input0.noise +
Define a noise value for each axis. The number is a floating point value. The value can be provided in 2 formats. +
    +
  1. A single value. The same value is applied to all 3 axis. +
  2. 3 common seperated values. Each axis can have a unique value. +
+This value is used by the algs. +
output0.noprocess +
Value is ignored. This attribute prevents the alg version of a +sensor from being used. For example, if there is an inbound data source for +SENSOR_ACCELEROMETER and the alg also provides a SENSOR_ACCELEROMETER (prehaps +as a calibrated or otherwise modified), this attribute will for the usage of +the version directly from the inbound data source. Unless there is a corresponding copy from an inbound sensor, this output entity will preoduce no data. +
input0.noprocess +
Value is ignored. This attribute prevents the inbound data from +being submitted to the algs. Unless there is a copy attibute, the data from +this sensor is ignored. +
output0.format +
Modifies the value to fit a particular output format. This is +used with the noprocess option. Valid options are: +
+
integer +
Data is not modified. +
float +
Data is converted to a float first then the binary contents +of the float is sent. +
shift24 +
Shift data by 24 bits. +
shift16 +
Shift data by 16 bits. +
shift15 +
Shift data by 15 bits. +
shift12 +
Shift data by 12 bits. +
+
input0.option, output0.option +
This defines configuration option for the driver. Parameter is +a value. Values beginning with 0x are considered to be hex. Meaning of the +value depends on the driver. Currently, only the input event output driver +uses this: +
    +
  • bit 0: Enable embedded time stamps. 64 bit time stamps +as passed as ABS_THROTTLE (upper 32 bits)/ABS_RUDDER (lower 32bits) +
  • bit 1: Enable sending the 4th element of a quaternion. +This value is passed as ABS_WHEEL. +
  • bit 2: Enable dittering of the output to defeat the input +subsystem's duplicate value elimination. It adds a value from 0-2 to each +data. +
+
system0.calfile +
This defines the name of the file to write cal data to. The + last one specified will be used. Relative path are relative to the +process working directory. To avoid surprises, use absolute paths. +
system0.pmdir +
Define a directory for enabling/disabling sensors. A path in a +tmpfs is highly desireable to minimize flash wear. +
+
+Updated +Fri Sep 11 18:35:37 PDT 2015 + From 7452edf639e3cfc7e0e1c98e8ae1dd8f2cd322dd Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Tue, 13 Oct 2015 17:55:12 -0700 Subject: [PATCH 20/21] Latest kernel driver to match the latest firmware. Signed-off-by: Hunyue Yau --- linux/driver/MQ_sensors.h | 235 ------------------ linux/driver/SensorPackets.h | 395 ++++++++++++++++++++++--------- linux/driver/osp-iio.c | 83 +++---- linux/driver/osp-input.c | 9 +- linux/driver/osp-sh.c | 446 +++++++++++++++++++++++++---------- 5 files changed, 644 insertions(+), 524 deletions(-) delete mode 100644 linux/driver/MQ_sensors.h diff --git a/linux/driver/MQ_sensors.h b/linux/driver/MQ_sensors.h deleted file mode 100644 index fe7ed7d..0000000 --- a/linux/driver/MQ_sensors.h +++ /dev/null @@ -1,235 +0,0 @@ -/***************************************************************** - * @file MQ_sensors.h - * - * Sensors related enumerations - * - * < Detailed description of subsystem > - * - * @seealso < Links to relevant files, references, etc. > - * - * @info - * $Author: rverma $ - * $DateTime: 2014/10/16 15:32:24 $ - * $Revision: #4 $ - * - * Copyright 2012 Audience, Incorporated. All rights reserved. - * - * This software is the confidential and proprietary information of - * Audience Inc. ("Confidential Information"). You shall not disclose - * such Confidential Information and shall use it only in accordance - * with the Terms of Sale of Audience products and the terms of any - * license agreement you entered into with Audience for such products. - * - * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY - * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, - * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE - * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR - * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE - * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF - * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. - * - ****************************************************************************/ -#if !defined (MQ_SENSORS_H) -#define MQ_SENSORS_H - -/*--------------------------------------------------------------------------*\ - | I N C L U D E F I L E S -\*--------------------------------------------------------------------------*/ -/* This file is meant to provide a common definition of sensor related - * enumerations/defines and generally should not depend on any other - * includes - */ - -/*--------------------------------------------------------------------------*\ - | C O N S T A N T S & M A C R O S -\*--------------------------------------------------------------------------*/ -#define SENSOR_SUBTYPE_UNUSED 0 //!< Subtype is not used for the sensor type -#define SENSOR_SUBTYPE_START 1 //!< Subtype enumeration starts with 1 -#define SENSOR_DEVICE_PRIVATE_BASE 0x10000 //!< Android defined private sensor type base -#define M_PSensorToAndroidBase(type) ((type) | SENSOR_DEVICE_PRIVATE_BASE) - - -/*--------------------------------------------------------------------------*\ - | T Y P E D E F I N I T I O N S -\*--------------------------------------------------------------------------*/ -//! use to specify the kind of sensor input or output -/*! - * \sa OSP_RegisterInputSensor - * \sa OSP_SubscribeOutputSensor - * - * Final units of input/outputs are defined by the sensor data convention - * field of the sensor descriptor. Flags in the descriptor specify if - * sensor is calibrated/uncalibrated and/or used as input. If a sensor - * type not is supported by the library implementation, an error will be - * returned on its usage - */ -/* Android sensor types - this should match the defines/enumeration in - * Android's sensors.h */ -/* Note that these are defined as SENSOR_ instead of SENSOR_TYPE_ to - * avoid clash with Android defines in situations where this header in - * included for packet processing in Sensor HAL - */ -typedef enum _ASensorType { - SENSOR_META_DATA = 0, - SENSOR_ACCELEROMETER = 1, - SENSOR_GEOMAGNETIC_FIELD = 2, - SENSOR_MAGNETIC_FIELD = SENSOR_GEOMAGNETIC_FIELD, - SENSOR_ORIENTATION = 3, - SENSOR_GYROSCOPE = 4, - SENSOR_LIGHT = 5, - SENSOR_PRESSURE = 6, - SENSOR_TEMPERATURE = 7, - SENSOR_PROXIMITY = 8, - SENSOR_GRAVITY = 9, - SENSOR_LINEAR_ACCELERATION = 10, - SENSOR_ROTATION_VECTOR = 11, - SENSOR_RELATIVE_HUMIDITY = 12, - SENSOR_AMBIENT_TEMPERATURE = 13, - SENSOR_MAGNETIC_FIELD_UNCALIBRATED = 14, - SENSOR_GAME_ROTATION_VECTOR = 15, - SENSOR_GYROSCOPE_UNCALIBRATED = 16, - SENSOR_SIGNIFICANT_MOTION = 17, - SENSOR_STEP_DETECTOR = 18, - SENSOR_STEP_COUNTER = 19, - SENSOR_GEOMAGNETIC_ROTATION_VECTOR = 20, - - NUM_ANDROID_SENSOR_TYPE //!< Total number of Android sensor type -} ASensorType_t; - -/* Private Sensor types (translates to Android SENSOR_TYPE_DEVICE_PRIVATE_BASE start) */ -typedef enum _PSensorType { - PSENSOR_ENUM_FIRST_SENSOR = 0, - - PSENSOR_DEBUG_TUNNEL = PSENSOR_ENUM_FIRST_SENSOR, //!< Debug message pipe to host - PSENSOR_ACCELEROMETER_RAW = 1, //!< raw accelerometer data (direct from sensor) - PSENSOR_MAGNETIC_FIELD_RAW = 2, //!< magnetometer data (direct from sensor) - PSENSOR_GYROSCOPE_RAW = 3, //!< calibrated gyroscope data (direct from sensor) - PSENSOR_LIGHT_UV = 4, //!< UV light sensor data (Android Units) - PSENSOR_LIGHT_RGB = 5, //!< RGB light sensor data (Android Units) - PSENSOR_STEP = 6, //!< step data - PSENSOR_ACCELEROMETER_UNCALIBRATED = 7, //!< uncalibrated accelerometer data (Android Units) - PSENSOR_ORIENTATION = 8, //!< yaw, pitch, roll (also use this for Win8 Inclinometer) - PSENSOR_CONTEXT_DEVICE_MOTION = 9, //!< context of device relative to world frame - PSENSOR_CONTEXT_CARRY = 10, //!< context of device relative to user - PSENSOR_CONTEXT_POSTURE = 11, //!< context of user relative to world frame - PSENSOR_CONTEXT_TRANSPORT = 12, //!< context of environment relative to world frame - PSENSOR_GESTURE_EVENT = 13, //!< gesture event such as a double-tap or shake - PSENSOR_HEART_RATE = 15, //!< heart-rate data - SYSTEM_REAL_TIME_CLOCK = 16, //!< Real time clock used for time stamp - - NUM_PRIVATE_SENSOR_TYPE //!< Total number of Private sensor type -} PSensorType_t; - -/* Sensor Parameter Identifiers */ -typedef enum _SensorParamId { - SENSOR_PARAM_ERROR_CODE = 0, //!< Used to convey error code instead of parameter value - SENSOR_PARAM_OFFSET = 1, //!< Offset or bias of a sensor - SENSOR_PARAM_DATA_RATE = 2, //!< Datarate for the sensor - SENSOR_PARAM_BAND_WIDTH = 3, //!< Bandwidth setting for the sensor - SENSOR_PARAM_HP_FILTER = 4, //!< High Pass filter setting for the sensor - SENSOR_PARAM_LP_FILTER = 5, //!< Low Pass filter setting for the sensor - SENSOR_PARAM_ENABLE = 6, //!< Sensor Enable control - - NUM_SENSOR_PARAM -} SensorParamId_t; - - -//! Use these values as a sub-type for STEP result -typedef enum _StepSubType { - CONTEXT_STEP = SENSOR_SUBTYPE_START, //!< only one kind of step now - STEP_SEGMENT_DETECTOR, //!< low compute trigger for analyzing if step may have occured - - NUM_PSENSOR_STEP_SUBTYPE -} StepSubType_t; - -//! Use these values as a sub-type for CONTEXT_DEVICE_MOTION result -typedef enum _ContextDeviceMotionSubType { - CONTEXT_DEVICE_MOTION_STILL = SENSOR_SUBTYPE_START, - CONTEXT_DEVICE_MOTION_ACCELERATING, - CONTEXT_DEVICE_MOTION_ROTATING, - CONTEXT_DEIVCE_MOTION_TRANSLATING, - CONTEXT_DEVICE_MOTION_FREE_FALLING, - CONTEXT_DEVICE_MOTION_SIGNIFICANT_MOTION, //!< significant motion (as specified by Android HAL 1.0) - CONTEXT_DEVICE_MOTION_SIGNIFICANT_STILLNESS, //!< complement to significant motion - CONTEXT_DEVICE_MOTION_CHANGE_DETECTOR, //!< low compute trigger for seeing if context may have changed - - NUM_PSENSOR_CONTEXT_DEVICE_MOTION_SUBTYPE -} ContextDeviceMotionSubType_t; - -//! Use these values as a sub-type for CONTEXT_CARRY result -typedef enum _ContextCarrySubType { - CONTEXT_CARRY_IN_POCKET = SENSOR_SUBTYPE_START, - CONTEXT_CARRY_IN_HAND, - CONTEXT_CARRY_NOT_ON_PERSON, - CONTEXT_CARRY_IN_HAND_FRONT, - CONTEXT_CARRY_IN_HAND_SIDE, - - NUM_PSENSOR_CONTEXT_CARRY_SUBTYPE -} ContextCarrySubType_t; - -//! Use these values as a sub-type for CONTEXT_POSTURE result -typedef enum _ContextPostureSubType { - CONTEXT_POSTURE_WALKING = SENSOR_SUBTYPE_START, - CONTEXT_POSTURE_STANDING, - CONTEXT_POSTURE_SITTING, - CONTEXT_POSTURE_JOGGING, - CONTEXT_POSTURE_RUNNING, - - NUM_PSENSOR_CONTEXT_POSTURE_SUBTYPE -} ContextPostureSubType_t; - - -//! Use these values as a sub-type for CONTEXT_TRANSPORT result -typedef enum _ContextTransportSubType { - CONTEXT_TRANSPORT_VEHICLE = SENSOR_SUBTYPE_START, - CONTEXT_TRANSPORT_CAR, - CONTEXT_TRANSPORT_TRAIN, - CONTEXT_TRANSPORT_UP_STAIRS, - CONTEXT_TRANSPORT_DOWN_STAIRS, - CONTEXT_TRANSPORT_UP_ELEVATOR, - CONTEXT_TRANSPORT_DOWN_ELEVATOR, - CONTEXT_TRANSPORT_ON_BIKE, - - NUM_PSENSOR_CONTEXT_TRANSPORT_SUBTYPE -} ContextTransportSubType_t; - -//! Use these values as a sub-type for GESTURE_EVENT result -typedef enum _GestureSubType { - SENSOR_GESTURE_TAP = SENSOR_SUBTYPE_START, - SENSOR_GESTURE_DOUBLE_TAP, - SENSOR_GESTURE_SHAKE, - - NUM_PSENSOR_GESTURE_SUBTYPE -} GestureSubType_t; - -//! Time Stamp definition for capturing raw time counts -typedef union _TimeStamp { - uint64_t TS64; - uint32_t TS32[2]; - uint8_t TS8[8]; -} TimeStamp_t; - -//! Generic structure definition for a 3-axis sensor raw values in 2's complement format -typedef struct TriAxisRawData_t -{ - TimeStamp_t TStamp; - int32_t Axis[3]; -} TriAxisRawData_t; - -/*--------------------------------------------------------------------------*\ - | E X T E R N A L V A R I A B L E S & F U N C T I O N S -\*--------------------------------------------------------------------------*/ - -/*--------------------------------------------------------------------------*\ - | P U B L I C V A R I A B L E S D E F I N I T I O N S -\*--------------------------------------------------------------------------*/ - -/*---------------------------------------------------------------------------*\ - | P U B L I C F U N C T I O N D E C L A R A T I O N S -\*---------------------------------------------------------------------------*/ - - -#endif /* MQ_SENSORS_H */ -/*---------------------------------------------------------------------------*\ - | E N D O F F I L E -\*---------------------------------------------------------------------------*/ diff --git a/linux/driver/SensorPackets.h b/linux/driver/SensorPackets.h index d5bd61f..9789963 100644 --- a/linux/driver/SensorPackets.h +++ b/linux/driver/SensorPackets.h @@ -1,7 +1,7 @@ /***************************************************************************** * @file SensorPackets.h * - * Definitions and macros for host inteface packet format + * Definitions and macros for host interface packet format * * < Detailed description of subsystem > * @@ -9,8 +9,8 @@ * * @info * $Author: rverma $ - * $DateTime: 2014/11/13 17:24:40 $ - * $Revision: #8 $ + * $DateTime: 2015/06/26 11:03:37 $ + * $Revision: #4 $ * * Copyright 2012 Audience, Incorporated. All rights reserved. * @@ -34,12 +34,15 @@ /*--------------------------------------------------------------------------*\ | I N C L U D E F I L E S - \*--------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ #if 0 #include #include #endif -#include "MQ_sensors.h" +#include "osp-sensors.h" +#if 0 +#include "osp-types.h" +#endif /*--------------------------------------------------------------------------*\ | C O N S T A N T S & M A C R O S @@ -48,64 +51,65 @@ /* COMMON PACKET DEFINITION */ /********************************************************/ /* Packet Identifier for version 0 packets */ -#define PKID_SENSOR_DATA 0x00 -#define PKID_CONTROL_REQ 0x10 -#define PKID_CONTROL_RESP 0x20 -#define PKID_TEST_DATA 0x30 -#define PKID_MASK_VER0 0xF0 +#define PKID_SENSOR_DATA 0x00 +#define PKID_CONTROL_REQ_RD 0x10 +#define PKID_CONTROL_REQ_WR 0x20 +#define PKID_CONTROL_RESP 0x30 +#define PKID_TEST_DATA 0x40 +#define PKID_MASK_VER0 0xF0 /** =============== CONTROL ID BYTE =============== */ -#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) -#define SENSOR_TYPE_ANDROID 0x0 -#define SENSOR_TYPE_PRIVATE 0x1 +#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) +#define SENSOR_TYPE_ANDROID 0x0 +#define SENSOR_TYPE_PRIVATE 0x1 /** =============== SENSOR ID BYTE =============== */ -#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) -#define M_ParseSensorMetaData(i) ((uint8_t)((i >> 6) & 0x03)) -#define M_SensorType(s) ((uint8_t)(s & 0x3F)) +#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) +#define M_ParseSensorMetaData(i) ((uint8_t)((i >> 6) & 0x03)) +#define M_SensorType(s) ((uint8_t)(s & 0x3F)) /** =============== ATTRIBUTE BYTE =============== */ -#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) -#define M_ParseSensorSubType(st) ((uint8_t)((st >> 4) & 0x0F)) +#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) +#define M_ParseSensorSubType(st) ((uint8_t)((st >> 4) & 0x0F)) /* Checksum option (check sum is always 16-bit CRC if enabled) */ -#define CHECK_SUM_PRESENT 0x01 +#define CHECK_SUM_PRESENT 0x01 /********************************************************/ /* SENSOR DATA PACKET */ /********************************************************/ /** =============== CONTROL BYTE =============== */ /* CRC option */ -#define CRC_ENABLE 0x08 +#define CRC_ENABLE 0x08 /*Enumeration type of sensor*/ -#define SENSOR_ANDROID_TYPE_MASK 0x01 +#define SENSOR_ANDROID_TYPE_MASK 0x01 /* Format for data values */ -#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor values */ -#define DATA_FORMAT_FIXPOINT 0x04 /* Fixed point format in application defined units */ -#define DATA_FORMAT_MASK 0x04 +#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor */ +#define DATA_FORMAT_FIXPOINT 0x04 /* FixPoint format in app defined units */ +#define DATA_FORMAT_MASK 0x04 /* Time Format option */ -#define TIME_FORMAT_RAW 0x00 /* RAW count values for time stamp base */ -#define TIME_FORMAT_FIXPOINT 0x02 /* Fixed point format specified in seconds */ -#define TIME_FORMAT_MASK 0x02 +#define TIME_FORMAT_RAW 0x00 /* RAW count values for timestamp base */ +#define TIME_FORMAT_FIXPOINT 0x02 /* FixPoint format in seconds */ +#define TIME_FORMAT_MASK 0x02 /** ============ SENSOR IDENTIFIER BYTE ========== */ -#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ -#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ +#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ +#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ /** =============== ATTRIBUTE BYTE =============== */ /* Data size option */ -#define DATA_SIZE_8_BIT 0x00 -#define DATA_SIZE_16_BIT 0x02 -#define DATA_SIZE_32_BIT 0x04 -#define DATA_SIZE_64_BIT 0x06 -#define DATA_SIZE_MASK 0x06 +#define DATA_SIZE_8_BIT 0x00 +#define DATA_SIZE_16_BIT 0x02 +#define DATA_SIZE_32_BIT 0x04 +#define DATA_SIZE_64_BIT 0x06 +#define DATA_SIZE_MASK 0x06 /* Time Stamp Size */ -#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ -#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ -#define TIME_STAMP_SIZE_MASK 0x01 +#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ +#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ +#define TIME_STAMP_SIZE_MASK 0x01 /********************************************************/ /* SENSOR CONTROL REQ/RESP PACKET */ @@ -113,74 +117,161 @@ /** =============== CONTROL BYTE =============== */ /* First 5 MS bits are same as Sensor Data Packet */ /* Data Format */ -#define CTRL_PKT_DF_INTEGER 0x00 -#define CTRL_PKT_DF_FIXPOINT 0x02 -#define CTRL_PKT_DF_FLOAT 0x04 -#define CTRL_PKT_DF_DOUBLE 0x06 +#define CTRL_PKT_DF_INTEGER 0x00 +#define CTRL_PKT_DF_FIXPOINT 0x02 +#define CTRL_PKT_DF_FLOAT 0x04 +#define CTRL_PKT_DF_DOUBLE 0x06 /* A/P Definition same as Sensor Data Packet */ /** =============== ATTRIBUTE BYTE 1 =============== */ /* Sequence Number for request/response */ -#define M_SequenceNum(sNum) ((sNum) & 0x0F) +#define M_SequenceNum(sNum) ((sNum) & 0x0F) /** =============== ATTRIBUTE BYTE 2 =============== */ /* Parameter ID */ -#define M_GetParamId(AttrByte2) ((AttrByte2) >> 3) -#define M_SetParamId(id) (((id) & 0x1F) << 3) - -/* Parameter Size */ -#define M_GetParamSize(AttrByte2) ((AttrByte2) & 0x07) - -/* Param data size */ -#define PARAM_DATA_SZ_8_BIT 0x00 -#define PARAM_DATA_SZ_16_BIT 0x01 -#define PARAM_DATA_SZ_32_BIT 0x02 -#define PARAM_DATA_SZ_64_BIT 0x03 -#define PARAM_DATA_SZ_BOOL_FALSE 0x04 -#define PARAM_DATA_SZ_BOOL_TRUE 0x05 -#define PARAM_DATA_SZ_UNKNOWN 0x07 +#define M_GetParamId(AttrByte2) (AttrByte2) +#define M_SetParamId(id) (id) + +/* Parameter Identifier*/ +#define PARAM_ID_ENABLE 0x01 +#define PARAM_ID_BATCH 0x02 +#define PARAM_ID_FLUSH 0x03 +#define PARAM_ID_RANGE_RESOLUTION 0x04 +#define PARAM_ID_POWER 0x05 +#define PARAM_ID_MINMAX_DELAY 0x06 +#define PARAM_ID_FIFO_EVT_CNT 0x07 +#define PARAM_ID_AXIS_MAPPING 0x08 +#define PARAM_ID_CONVERSION_OFFSET 0x09 +#define PARAM_ID_CONVERSION_SCALE 0x0A +#define PARAM_ID_SENSOR_NOISE 0x0B +#define PARAM_ID_TIMESTAMP_OFFSET 0x0C +#define PARAM_ID_ONTIME_WAKETIME 0x0D +#define PARAM_ID_HPF_LPF_CUTOFF 0x0E +#define PARAM_ID_SENSOR_NAME 0x0F +#define PARAM_ID_XYZ_OFFSET 0x10 +#define PARAM_ID_F_SKOR_MATRIX 0x11 +#define PARAM_ID_F_CAL_OFFSET 0x12 +#define PARAM_ID_F_NONLINEAR_EFFECTS 0x13 +#define PARAM_ID_BIAS_STABILITY 0x14 +#define PARAM_ID_REPEATABILITY 0x15 +#define PARAM_ID_TEMP_COEFF 0x16 +#define PARAM_ID_SHAKE_SUSCEPTIBILIY 0x17 +#define PARAM_ID_EXPECTED_NORM 0x18 +#define PARAM_ID_VERISON 0x19 +#define PARAM_ID_DYNAMIC_CAL_SCALE 0x1A +#define PARAM_ID_DYNAMIC_CAL_SKEW 0x1B +#define PARAM_ID_DYNAMIC_CAL_OFFSET 0x1C +#define PARAM_ID_DYNAMIC_CAL_ROTATION 0x1D +#define PARAM_ID_DYNAMIC_CAL_QUALITY 0x1E +#define PARAM_ID_DYNAMIC_CAL_SOURCE 0x1F +#define PARAM_ID_CONFIG_DONE 0x20 /* Byte extraction macros */ +#define BYTE7(x) ((uint8_t)((x >> 56) & 0xFF)) +#define BYTE6(x) ((uint8_t)((x >> 48) & 0xFF)) +#define BYTE5(x) ((uint8_t)((x >> 40) & 0xFF)) +#define BYTE4(x) ((uint8_t)((x >> 32) & 0xFF)) #define BYTE3(x) ((uint8_t)((x >> 24) & 0xFF)) #define BYTE2(x) ((uint8_t)((x >> 16) & 0xFF)) #define BYTE1(x) ((uint8_t)((x >> 8) & 0xFF)) #define BYTE0(x) ((uint8_t)(x & 0xFF)) -/* 16-bit & 32-bit data combine macros */ -#define BYTES_TO_SHORT(b1,b0) ((int16_t)(((int16_t)b1 << 8) | b0)) +/* 16-bit, 32-bit & 64-bit data macros */ +#define BYTES_TO_SHORT(b1,b0) ((int16_t)(((int16_t)b1 << 8) | b0)) + #define BYTES_TO_LONG_ARR(arr,ind) BYTES_TO_LONG(arr[ind],arr[ind+1],arr[ind+2],arr[ind+3]) -#define BYTES_TO_LONG(b3,b2,b1,b0) \ - ((int32_t)(((int32_t)b3 << 24) | ((uint32_t)b2 << 16) | ((uint32_t)b1 << 8) | b0)) + +#define BYTES_TO_LONG(b3,b2,b1,b0) \ + ((int32_t)(((int32_t)b3 << 24) | ((uint32_t)b2 << 16) | \ + ((uint32_t)b1 << 8) | b0)) + +#define BYTES_TO_LONGLONG(b7,b6,b5,b4,b3,b2,b1,b0) \ + ((uint64_t)(((uint64_t)b7 << 56) | ((uint64_t)b6 << 48) | \ + ((uint64_t)b5 << 40) | ((uint64_t)b4 << 32) | ((uint64_t)b3 << 24) | \ + ((uint64_t)b2 << 16) | ((uint64_t)b1 << 8) | b0)) /*--------------------------------------------------------------------------*\ | T Y P E D E F I N I T I O N S \*--------------------------------------------------------------------------*/ + +/* To be removed */ +typedef union _TimeStamp { + uint64_t TS64; + uint32_t TS32[2]; + uint8_t TS8[8]; +} TimeStamp_t; +/* End to be removed */ + /* Definition for quaternion data packets for internal usage */ typedef struct _QuaternionFixP { - TimeStamp_t TimeStamp; - int32_t Quat[4]; //W,X,Y,Z order + TimeStamp_t TimeStamp; + int32_t Quat[4]; //W,X,Y,Z order +#if 0 + // Add these two fields to comply with Android spec. + int32_t HeadingError; + int32_t TiltError; +#endif } QuaternionFixP_t; +typedef struct _OrientationFixP { + TimeStamp_t TimeStamp; + int32_t Pitch; + int32_t Roll; + int32_t Yaw; +} OrientationFixP_t; + +typedef struct _ThreeAxisFixP { + TimeStamp_t TimeStamp; + int32_t Axis[3]; + uint8_t Accuracy; +} ThreeAxisFixP_t; + +typedef struct _SignificantMotion { + TimeStamp_t TimeStamp; + uint8_t MotionDetected; +} SignificantMotion_t; + +typedef struct _StepCounter { + TimeStamp_t TimeStamp; + uint64_t NumStepsTotal; +} StepCounter_t; + +typedef struct _StepDetector { + TimeStamp_t TimeStamp; + uint8_t StepDetected; +} StepDetector_t; + typedef struct _UncalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z order - int32_t Offset[3]; //XOFF,YOFF,ZOFF order + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z order + int32_t Offset[3]; //XOFF,YOFF,ZOFF order } UncalibratedFixP_t; typedef struct _CalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z, + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z, } CalibratedFixP_t; +// Possible combine this and the CalibratedFixP_t into a common data type. +typedef struct _TriAxisRawData { + TimeStamp_t TStamp; + int32_t Axis[3]; //X,Y,Z, +} TriAxisRawData_t; + /* Union of the structures that can be parsed out of Sensor Data packet */ typedef struct _SensorPacketTypes { union { - TriAxisRawData_t RawSensor; - UncalibratedFixP_t UncalFixP; - CalibratedFixP_t CalFixP; - QuaternionFixP_t QuatFixP; + TriAxisRawData_t RawSensor; + UncalibratedFixP_t UncalFixP; + CalibratedFixP_t CalFixP; + QuaternionFixP_t QuatFixP; + OrientationFixP_t OrientFixP; + ThreeAxisFixP_t ThreeAxisFixP; + SignificantMotion_t SigMotion; + StepCounter_t StepCount; + StepDetector_t StepDetector; } P; ASensorType_t SType; uint8_t SubType; @@ -194,44 +285,100 @@ typedef struct _SensorPacketTypes { /* Sensor Packet Type qualifier */ typedef struct _HifSensorPktQualifier { - uint8_t ControlByte; - uint8_t SensorIdByte; - uint8_t AttributeByte; + uint8_t ControlByte; + uint8_t SensorIdByte; + uint8_t AttributeByte; } HifSnsrPktQualifier_t; /* Basic packet: Raw 16-bit data, 32-bit integer time stamp; No checksum */ typedef struct _HifSensorDataRaw { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[4]; //32-bit Time Stamp - uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[4]; //32-bit Time Stamp + uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data } HifSensorDataRaw_t; -#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) +#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) /* Uncalibrated data packet: * 32-bit Fixedpoint uncalibrated; * 64-bit Fixedpoint time stamp; * No Checksum */ typedef struct _HifUncalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data - uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data + uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset } HifUncalibratedFixPoint_t; -#define UNCALIB_FIXP_DATA_PKT_SZ (offsetof(HifUncalibratedFixPoint_t, Offset)) -#define UNCALIB_FIXP_DATA_OFFSET_PKT_SZ sizeof(HifUncalibratedFixPoint_t) +#define UNCALIB_FIXP_DATA_PKT_SZ \ + (offsetof(HifUncalibratedFixPoint_t, Offset)) +#define UNCALIB_FIXP_DATA_OFFSET_PKT_SZ sizeof(HifUncalibratedFixPoint_t) -/* Calibrated data packet: 32-bit Fixedpoint Calibrated; 64-bit Fixedpoint time stamp; No Checksum */ +/* Calibrated data packet: 32-bit Fixedpoint Calibrated; 64-bit Fixedpoint + time stamp; No Checksum */ typedef struct _HifCalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data } HifCalibratedFixPoint_t; -#define CALIBRATED_FIXP_DATA_PKT_SZ sizeof(HifCalibratedFixPoint_t) +#define CALIBRATED_FIXP_DATA_PKT_SZ sizeof(HifCalibratedFixPoint_t) + +/* Three Axis Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifThreeAxisPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data +} HifThreeAxisPktFixPoint_t; + +#define THREEAXIS_FIXP_DATA_PKT_SZ sizeof(HifThreeAxisPktFixPoint_t) + +/* SignificantMotion Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifSignificantMotionPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t significantMotionDetected; +} HifSignificantMotionPktFixPoint_t; + +#define SIGNIFICANTMOTION_FIXP_DATA_PKT_SZ \ + sizeof(HifSignificantMotionPktFixPoint_t) + +/* StepCounter data packet: 64-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifStepCounter { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t NumStepsTotal[8]; +} HifStepCounter_t; -/* Quaternion data packet: 32-bit Fixedpoint quaternion; 64-bit Fixedpoint time stamp; No Checksum */ +#define STEPCOUNTER_DATA_PKT_SZ sizeof(HifStepCounter_t) + +/* StepDetector data packet: 8-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifStepDetector { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t stepDetected; +} HifStepDetector_t; + +#define STEPDETECTOR_DATA_PKT_SZ sizeof(HifStepDetector_t) + +/* Orientation Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifOrientationPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //!< Time in seconds + uint8_t Pitch[4]; //!< pitch in degrees + uint8_t Roll[4]; //!< roll in degrees + uint8_t Yaw[4]; //!< yaw in degrees +} HifOrientationPktFixPoint_t; + +#define ORIENTATION_FIXP_DATA_PKT_SZ sizeof(HifOrientationPktFixPoint_t) + +/* Quaternion data packet: 32-bit Fixedpoint quaternion; + 64-bit Fixedpoint time stamp; No Checksum */ typedef struct _HifQuaternionFixPoint { HifSnsrPktQualifier_t Q; uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format @@ -240,21 +387,27 @@ typedef struct _HifQuaternionFixPoint { #define QUATERNION_FIXP_DATA_PKT_SZ sizeof(HifQuaternionFixPoint_t) -/* Sensor control packet for enable/disable sensor */ -typedef struct _HifSensorEnable { - HifSnsrPktQualifier_t Q; - uint8_t AttrByte2; -} HifSensorEnable_t; +/* Sensor control packet for Boolean parameters */ +typedef struct _HifSensorParamBool { + HifSnsrPktQualifier_t Q; + uint8_t AttrByte2; + uint8_t Data; +} HifSensorParamBool_t; -#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorEnable_t) +#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorParamBool_t) /* Define union for all the host interface packet types */ typedef union _HostIFPackets { - HifSensorDataRaw_t SensPktRaw; - HifUncalibratedFixPoint_t UncalPktFixP; - HifCalibratedFixPoint_t CalPktFixP; - HifQuaternionFixPoint_t QuatPktFixP; - HifSensorEnable_t Enable; + HifSensorDataRaw_t SensPktRaw; + HifUncalibratedFixPoint_t UncalPktFixP; + HifCalibratedFixPoint_t CalPktFixP; + HifQuaternionFixPoint_t QuatPktFixP; + HifSensorParamBool_t Enable; + HifOrientationPktFixPoint_t OrientationFixP; + HifThreeAxisPktFixPoint_t ThreeAxisFixp; + HifSignificantMotionPktFixPoint_t SignificantMotion; + HifStepCounter_t StepCounter; + HifStepDetector_t StepDetector; } HostIFPackets_t; /*--------------------------------------------------------------------------*\ @@ -268,21 +421,31 @@ typedef union _HostIFPackets { /*--------------------------------------------------------------------------*\ | P U B L I C F U N C T I O N D E C L A R A T I O N S \*--------------------------------------------------------------------------*/ -int16_t FormatSensorDataPktRaw(uint8_t *pDest, - const TriAxisRawData_t *pSensData, - uint8_t metaData, ASensorType_t sType, - uint8_t subType); -int16_t FormatQuaternionPktFixP(uint8_t *pDest, - const QuaternionFixP_t *pQuatData, - ASensorType_t sType); -int16_t FormatUncalibratedPktFixP(uint8_t *pDest, - const UncalibratedFixP_t *pUncalData, - uint8_t metaData, ASensorType_t sType); -int16_t FormatCalibratedPktFixP(uint8_t *pDest, - const CalibratedFixP_t *pCalData, - ASensorType_t sType); -int16_t ParseHostIntefacePkt(SensorPacketTypes_t *pOut, - uint8_t *pPacket, uint16_t pktSize); +int16_t FormatSensorDataPktRaw( uint8_t *pDest, + const TriAxisRawData_t *pSensData, uint8_t metaData, + ASensorType_t sType, uint8_t subType ); +int16_t FormatQuaternionPktFixP( uint8_t *pDest, + const QuaternionFixP_t *pQuatData, ASensorType_t sType ); +int16_t FormatUncalibratedPktFixP( uint8_t *pDest, + const UncalibratedFixP_t *pUncalData, uint8_t metaData, + ASensorType_t sType ); +int16_t FormatCalibratedPktFixP( uint8_t *pDest, + const CalibratedFixP_t *pCalData, ASensorType_t sType ); +int16_t ParseHostInterfacePkt( SensorPacketTypes_t *pOut, uint8_t *pPacket, + uint16_t pktSize ); +int16_t FormatSensorEnableReq( uint8_t *pDest, uint8_t enable, + ASensorType_t sType, uint8_t subType, uint8_t seqNum ); +int16_t FormatStepDetectorPkt( uint8_t *pDest, + const StepDetector_t *pStepDetectorData, ASensorType_t sType ); +int16_t FormatStepCounterPkt( uint8_t *pDest, + const StepCounter_t *pStepCounterData, ASensorType_t sType ); +int16_t FormatThreeAxisPktFixP( uint8_t *pDest, + const ThreeAxisFixP_t *p3AxisData, ASensorType_t sType ); +int16_t FormatSignificantMotionPktFixP( uint8_t *pDest, + const SignificantMotion_t *pSignificantMotionData, + ASensorType_t sType ); +int16_t FormatOrientationFixP( uint8_t *pDest, + const OrientationFixP_t *pOrientationData, ASensorType_t sType ); #endif /* SENSOR_PACKETS_H */ /*--------------------------------------------------------------------------*\ diff --git a/linux/driver/osp-iio.c b/linux/driver/osp-iio.c index 96e5570..c2a447a 100644 --- a/linux/driver/osp-iio.c +++ b/linux/driver/osp-iio.c @@ -36,7 +36,7 @@ #include "trigger_consumer.h" #include -#include "MQ_sensors.h" +#include "osp-sensors.h" /* Simulate periodic data */ static struct timer_list osp_timer; @@ -71,6 +71,20 @@ static const struct attribute_group osp_iio_group = { * This array of structures tells the IIO core about what the device * actually provides for a given channel. */ +static struct iio_chan_spec step_channels[] = { + { + .type = IIO_ACCEL, /* Nothing better defined */ + .modified = 1, + .channel = 0, + .address = 0, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + IIO_CHAN_SOFT_TIMESTAMP(3), +}; static struct iio_chan_spec accel_channels[] = { /* * 'modified' (i.e. axis specified) acceleration channel @@ -598,10 +612,10 @@ static const struct OSP_SensorDesc { [SENSOR_STEP_COUNTER] = { .name = "osp_step", .trigname = "osp_step", - .channels = NULL, - .num_channels = 0, + .channels = step_channels, + .num_channels = ARRAY_SIZE(step_channels), .info = &osp_sensor_info, - .usebuffer = 0, + .usebuffer = 1, .useevent = 1, }, [SENSOR_PRESSURE] = { @@ -664,7 +678,7 @@ static ssize_t osp_iio_wenable(struct device *dev, if (buf[0] == '1') en = 1; - else + else en = 0; OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, en); @@ -691,10 +705,11 @@ static int osp_sensor_read_raw(struct iio_dev *indio_dev, long mask) { struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); - struct osp_iio_sensor *st = osp_sensor; int ret = -EINVAL; - mutex_lock(&st->lock); + printk("HY-DBG: %s:%i called\n", __func__, __LINE__); + + mutex_lock(&osp_sensor->lock); switch (mask) { case 0: /* magic value - channel value read */ switch (chan->type) { @@ -724,7 +739,7 @@ static int osp_sensor_read_raw(struct iio_dev *indio_dev, default: break; } - mutex_unlock(&st->lock); + mutex_unlock(&osp_sensor->lock); return ret; } @@ -752,6 +767,8 @@ static int osp_sensor_write_raw(struct iio_dev *indio_dev, int ret = 0; struct iio_dummy_state *st = iio_priv(indio_dev); + printk("HY-DBG: %s:%i called\n", __func__, __LINE__); + switch (mask) { case 0: if (chan->output == 0) @@ -831,10 +848,15 @@ static int osp_iio_sensor_init(struct iio_dev *indio_dev, int sensor, int type) static int osp_iio_trigger(struct iio_trigger *trig, bool state) { + /* HY-DBG 0 */ struct iio_dev *indio_dev = trig->private_data; struct osp_iio_sensor *osp_sensor; osp_sensor = iio_priv(indio_dev); + printk("HY-DBG: %s:%i for %s (%i)\n", __func__, __LINE__, + (osp_sensor->private)? + prv_sensor[osp_sensor->sensor].name: + and_sensor[osp_sensor->sensor].name, state); OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, state); osp_sensor->state = state; @@ -847,7 +869,7 @@ static irqreturn_t osp_iio_trigger_handler(int irq, void *p) struct iio_dev *indio_dev = pf->indio_dev; struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); - iio_push_to_buffer(indio_dev->buffer, (u8 *)&osp_sensor->data, + iio_push_to_buffer(indio_dev->buffer, (u8 *)&osp_sensor->data, osp_sensor->ts); iio_trigger_notify_done(indio_dev->trig); @@ -882,9 +904,9 @@ static void dataready(int sensor, int prv, switch (sensor) { case SENSOR_SIGNIFICANT_MOTION: case SENSOR_STEP_COUNTER: - iio_push_event(indio_dev, + iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_INTENSITY, 0, - IIO_MOD_X, + IIO_MOD_X, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), ts); @@ -902,7 +924,7 @@ static int osp_iio_destroy(int sensor, int space) int ret = 0; struct osp_iio_sensor **osp_sensors; - if (space == 0) + if (space == 0) osp_sensors = osp_sensors_and; else osp_sensors = osp_sensors_prv; @@ -1063,36 +1085,6 @@ static int osp_iio_create(int index, int space, void *dev) return ret; } -/* Dummy signal generator */ -static void osp_poll_timer(unsigned long unused) -{ -#if 0 - int i; - struct osp_iio_sensor *osp_sensor;; - - for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp_sensors_and[i]) { - osp_sensor = osp_sensors_and[i]; - if (i == SENSOR_ROTATION_VECTOR) { - osp_sensor->data.quat.x++; - osp_sensor->data.quat.y++; - osp_sensor->data.quat.z++; - osp_sensor->data.quat.r++; - osp_sensor->data.quat.ts = 0x3fffffff00000000LL+jiffies; - } else { - osp_sensor->data.xyz.x++; - osp_sensor->data.xyz.y++; - osp_sensor->data.xyz.z++; - osp_sensor->data.xyz.ts = 0x7fffffff00000000LL+jiffies; - } - if (and_sensor[i].usebuffer) - iio_trigger_poll(osp_sensor->trigger, osp_sensor->ts); - } - } - mod_timer(&osp_timer, jiffies+msecs_to_jiffies(500)); -#endif -} - static int osp_iio_probe(struct platform_device *pdev) { int ret, i; @@ -1148,13 +1140,6 @@ static int osp_iio_probe(struct platform_device *pdev) ret = osp_iio_create(SENSOR_LINEAR_ACCELERATION, 0, NULL); ret = osp_iio_create(SENSOR_GRAVITY, 0, NULL); ret = osp_iio_create(SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 0, NULL); -#endif -#if 0 - /* For dummy signal generator */ - if (ret >= 0) { - setup_timer(&osp_timer, osp_poll_timer, 0); - mod_timer(&osp_timer, jiffies + msecs_to_jiffies(1000)); - } #endif return 0; } diff --git a/linux/driver/osp-input.c b/linux/driver/osp-input.c index ec93006..ec80d8a 100644 --- a/linux/driver/osp-input.c +++ b/linux/driver/osp-input.c @@ -1,6 +1,6 @@ /* * Copyright (C) 2041 Audience, Inc. - * Written by Hunyue Yau + * Written by Hunyue Yau * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -22,7 +22,7 @@ #include #include #include -#include "MQ_sensors.h" +#include "osp-sensors.h" #include /* INPUT_ABS CONSTANTS */ @@ -100,7 +100,7 @@ static ssize_t osp_set_enable(struct device *dev, struct device_attribute *attr, int i; unsigned long v; - if (strict_strtoul(buf, 0, &v)) + if (strict_strtoul(buf, 0, &v)) return -EINVAL; for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { @@ -171,6 +171,7 @@ static void dataready(int sensor, int prv, input_sync(input_dev); break; case SENSOR_ACCELEROMETER: + printk("HY-DBG: %s:%i ACCEL\n", __func__, __LINE__); case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: case SENSOR_ORIENTATION: @@ -301,5 +302,5 @@ module_init(osp_input_init); MODULE_DESCRIPTION("OSP Input driver"); -MODULE_AUTHOR("Hunyue Yau "); +MODULE_AUTHOR("Hunyue Yau "); MODULE_LICENSE("GPL"); diff --git a/linux/driver/osp-sh.c b/linux/driver/osp-sh.c index d8baea5..7574a47 100644 --- a/linux/driver/osp-sh.c +++ b/linux/driver/osp-sh.c @@ -24,16 +24,20 @@ #include #include #include -#include "MQ_sensors.h" +#include "osp-sensors.h" #include "SensorPackets.h" #include "osp_i2c_map.h" #include #define NAME "ospsh" +/* Private sensors */ +#define FEAT_PRIVATE (1<<0) + static struct OSP_Sensor { void (*dataready)(int sensor, int prv, void *private, long long ts, union OSP_SensorData *sensordata); + void *private; } and_sensor[NUM_ANDROID_SENSOR_TYPE], prv_sensor[NUM_PRIVATE_SENSOR_TYPE]; @@ -42,9 +46,11 @@ struct osp_data { struct timer_list osp_timer; struct work_struct osp_work; struct mutex lock; + unsigned int features; }; static struct osp_data *gOSP; + static struct work_queue *osp_workq; /* Number of packets to parse */ @@ -71,9 +77,9 @@ static int OSP_ParseSensorDataPkt_Private( uint8_t dSize, uint8_t dFormat, uint8_t timeFormat, - uint8_t tSize, + uint8_t tSize, uint8_t hasMetaData -) + ) { int errCode = -EPROTONOSUPPORT; int lengthParsed; @@ -84,47 +90,55 @@ static int OSP_ParseSensorDataPkt_Private( case PSENSOR_MAGNETIC_FIELD_RAW: case PSENSOR_GYROSCOPE_RAW: if ((sensSubType == SENSOR_SUBTYPE_UNUSED) && - (dSize == DATA_SIZE_16_BIT) && - (dFormat == DATA_FORMAT_RAW) && - (timeFormat == TIME_FORMAT_RAW)) { + (dSize == DATA_SIZE_16_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_RAW)) { /* Extract Raw sensor data from packet */ - pOut->SType = (ASensorType_t)M_PSensorToAndroidBase(sensType); + pOut->SType = (ASensorType_t)M_PSensorToAndroidBase( + sensType); pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.RawSensor.Axis[0] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[0], - pHif->SensPktRaw.DataRaw[1]); + pHif->SensPktRaw.DataRaw[0], + pHif->SensPktRaw.DataRaw[1]); pOut->P.RawSensor.Axis[1] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[2], - pHif->SensPktRaw.DataRaw[3]); + pHif->SensPktRaw.DataRaw[2], + pHif->SensPktRaw.DataRaw[3]); pOut->P.RawSensor.Axis[2] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[4], - pHif->SensPktRaw.DataRaw[5]); - + pHif->SensPktRaw.DataRaw[4], + pHif->SensPktRaw.DataRaw[5]); + /* Extract time stamp */ - pOut->P.RawSensor.TStamp.TS64 = 0; //helps clear higher 32-bit - pOut->P.RawSensor.TStamp.TS8[3] = pHif->SensPktRaw.TimeStamp[0]; //MSB - pOut->P.RawSensor.TStamp.TS8[2] = pHif->SensPktRaw.TimeStamp[1]; - pOut->P.RawSensor.TStamp.TS8[1] = pHif->SensPktRaw.TimeStamp[2]; - pOut->P.RawSensor.TStamp.TS8[0] = pHif->SensPktRaw.TimeStamp[3]; //LSB + pOut->P.RawSensor.TStamp.TS64 = 0; //helps clear higher + // 32-bit + pOut->P.RawSensor.TStamp.TS8[3] = + pHif->SensPktRaw.TimeStamp[0]; //MSB + pOut->P.RawSensor.TStamp.TS8[2] = + pHif->SensPktRaw.TimeStamp[1]; + pOut->P.RawSensor.TStamp.TS8[1] = + pHif->SensPktRaw.TimeStamp[2]; + pOut->P.RawSensor.TStamp.TS8[0] = + pHif->SensPktRaw.TimeStamp[3]; //LSB //TODO: 64-bit time stamp extension?? errCode = 0; lengthParsed = SENSOR_RAW_DATA_PKT_SZ; } break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract uncalibrated sensor data from packet */ - pOut->SType = (ASensorType_t)M_PSensorToAndroidBase(sensType); + pOut->SType = (ASensorType_t)M_PSensorToAndroidBase( + sensType); pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.UncalFixP.Axis[0] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data,0); + pHif->UncalPktFixP.Data,0); pOut->P.UncalFixP.Axis[1] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 4); + pHif->UncalPktFixP.Data, 4); pOut->P.UncalFixP.Axis[2] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 8); + pHif->UncalPktFixP.Data, 8); /* Check if META_DATA is set to 0x01 then read offset */ if (hasMetaData) { @@ -142,13 +156,17 @@ static int OSP_ParseSensorDataPkt_Private( for (i = 0; i < sizeof(uint64_t); i++) { /* Copy LSB to MSB data - remember * that HIF packets are Big-Endian formatted */ - pOut->P.UncalFixP.TimeStamp.TS8[i] = pHif->UncalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pOut->P.UncalFixP.TimeStamp.TS8[i] = + pHif->UncalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; } break; + case SYSTEM_REAL_TIME_CLOCK: break; + default: break; } @@ -166,9 +184,9 @@ static int OSP_ParseSensorDataPkt_Android( uint8_t dSize, uint8_t dFormat, uint8_t timeFormat, - uint8_t tSize, + uint8_t tSize, uint8_t hasMetaData -) + ) { int errCode = -EPROTONOSUPPORT; int lengthParsed; @@ -178,22 +196,19 @@ static int OSP_ParseSensorDataPkt_Android( case SENSOR_ACCELEROMETER: case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: - case SENSOR_ORIENTATION: - case SENSOR_LINEAR_ACCELERATION: - case SENSOR_GRAVITY: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract sensor data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.CalFixP.Axis[0] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 0); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 0); pOut->P.CalFixP.Axis[1] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 4); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 4); pOut->P.CalFixP.Axis[2] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 8); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 8); /* Extract fixed point time stamp */ for (i = 0; i < sizeof(uint64_t); i++) { @@ -202,61 +217,62 @@ static int OSP_ParseSensorDataPkt_Android( * Big-Endian formatted */ pOut->P.CalFixP.TimeStamp.TS8[i] = - pHif->CalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pHif->CalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; lengthParsed = CALIBRATED_FIXP_DATA_PKT_SZ; } break; - case SENSOR_GAME_ROTATION_VECTOR: - case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract Quaternion data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.QuatFixP.Quat[0] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 0); + pHif->QuatPktFixP.Data, 0); pOut->P.QuatFixP.Quat[1] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 4); + pHif->QuatPktFixP.Data, 4); pOut->P.QuatFixP.Quat[2] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 8); + pHif->QuatPktFixP.Data, 8); pOut->P.QuatFixP.Quat[3] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 12); + pHif->QuatPktFixP.Data, 12); /* Extract fixed point time stamp */ for (i = 0; i < sizeof(uint64_t); i++) { /* Copy LSB to MSB data - remember * that HIF packets are Big-Endian formatted */ - pOut->P.QuatFixP.TimeStamp.TS8[i] = pHif->QuatPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pOut->P.QuatFixP.TimeStamp.TS8[i] = + pHif->QuatPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; lengthParsed = QUATERNION_FIXP_DATA_PKT_SZ; } break; - case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_GYROSCOPE_UNCALIBRATED: case SENSOR_PRESSURE: - case SENSOR_STEP_DETECTOR: - case SENSOR_STEP_COUNTER: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract Quaternion data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.UncalFixP.Axis[0] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 0); + pHif->UncalPktFixP.Data, 0); pOut->P.UncalFixP.Axis[1] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 4); + pHif->UncalPktFixP.Data, 4); pOut->P.UncalFixP.Axis[2] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 8); + pHif->UncalPktFixP.Data, 8); /* Check if META_DATA is set to 0x01 then read offset */ if (hasMetaData) { @@ -275,38 +291,176 @@ static int OSP_ParseSensorDataPkt_Android( /* Copy LSB to MSB data - remember that * HIF packets are Big-Endian formatted */ pOut->P.UncalFixP.TimeStamp.TS8[i] = - pHif->UncalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pHif->UncalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; } break; - default: - { - int i; - unsigned char *p; - p = (unsigned char *)pHif; - for (i = 0; i < 8; i++) { - printk("%02x ", p[i]); + case SENSOR_SIGNIFICANT_MOTION: + if ((dSize == DATA_SIZE_8_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract SignificantMotion data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.SigMotion.MotionDetected = + pHif->SignificantMotion. + significantMotionDetected; + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.SigMotion.TimeStamp.TS8[i] = + pHif->SignificantMotion.TimeStamp[ + sizeof(uint64_t) - i-1]; } - printk("\n"); - + errCode = 0; + lengthParsed = SIGNIFICANTMOTION_FIXP_DATA_PKT_SZ; + } + break; + + case SENSOR_STEP_DETECTOR: + if ((dSize == DATA_SIZE_8_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract StepDetector data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.StepDetector.StepDetected = + pHif->StepDetector.stepDetected; + + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.StepDetector.TimeStamp.TS8[i] = + pHif->StepDetector.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = STEPDETECTOR_DATA_PKT_SZ; } break; + + case SENSOR_STEP_COUNTER: + if ((dSize == DATA_SIZE_64_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract StepCounter data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.StepCount.NumStepsTotal = BYTES_TO_LONGLONG( + pHif->StepCounter.NumStepsTotal[0], + pHif->StepCounter.NumStepsTotal[1], + pHif->StepCounter.NumStepsTotal[2], + pHif->StepCounter.NumStepsTotal[3], + pHif->StepCounter.NumStepsTotal[4], + pHif->StepCounter.NumStepsTotal[5], + pHif->StepCounter.NumStepsTotal[6], + pHif->StepCounter.NumStepsTotal[7]); + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.StepCount.TimeStamp.TS8[i] = + pHif->StepCounter.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = STEPCOUNTER_DATA_PKT_SZ; + } + break; + + case SENSOR_ORIENTATION: + if ((dSize == DATA_SIZE_32_BIT) && + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract OrientationFixP data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.OrientFixP.Pitch = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Pitch, 0); + pOut->P.OrientFixP.Roll = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Roll, 0); + pOut->P.OrientFixP.Yaw = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Yaw, 0); + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.OrientFixP.TimeStamp.TS8[i] = + pHif->OrientationFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = ORIENTATION_FIXP_DATA_PKT_SZ; + } + break; + + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_GRAVITY: + if ((dSize == DATA_SIZE_32_BIT) && + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract ThreeAxisFixp data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.ThreeAxisFixP.Axis[0] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 0); + + pOut->P.ThreeAxisFixP.Axis[1] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 4); + + pOut->P.ThreeAxisFixP.Axis[2] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 8); + //TODO: How to handle the Accuracy here ? Leave it in + // the Metadeta ? + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.ThreeAxisFixP.TimeStamp.TS8[i] = + pHif->ThreeAxisFixp.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = THREEAXIS_FIXP_DATA_PKT_SZ; + } + break; + + default: + { + int i; + unsigned char *p; + p = (unsigned char *)pHif; + for (i = 0; i < 8; i++) { + printk("%02x ", p[i]); + } + printk("\n"); + } + break; } if (errCode == 0) return lengthParsed; else return errCode; } + static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, - uint8_t *pPacket, uint16_t pktSize) + uint8_t *pPacket, uint16_t pktSize) { HostIFPackets_t *pHif = (HostIFPackets_t*)pPacket; int errCode = -EPROTONOSUPPORT; uint8_t sensType; uint8_t sensSubType, dSize, dFormat, timeFormat, - tSize, isPrivateType, hasMetaData; + tSize, isPrivateType, hasMetaData; /* Sanity... */ if ((pOut == NULL) || (pPacket == NULL)) @@ -316,7 +470,7 @@ static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, sensType = M_SensorType(pHif->SensPktRaw.Q.SensorIdByte); sensSubType = M_ParseSensorSubType(pHif->SensPktRaw.Q.AttributeByte); isPrivateType = pHif->SensPktRaw.Q.ControlByte & - SENSOR_ANDROID_TYPE_MASK; + SENSOR_ANDROID_TYPE_MASK; hasMetaData = M_ParseSensorMetaData (pHif->SensPktRaw.Q.SensorIdByte); dSize = pHif->SensPktRaw.Q.AttributeByte & DATA_SIZE_MASK; dFormat = pHif->SensPktRaw.Q.ControlByte & DATA_FORMAT_MASK; @@ -327,30 +481,28 @@ static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, if (!isPrivateType) { /*Sensor Enumeration type is Android*/ errCode = OSP_ParseSensorDataPkt_Android(pOut, pHif, - (ASensorType_t)sensType, - sensSubType, dSize, - dFormat, timeFormat, tSize, - hasMetaData); + (ASensorType_t)sensType, + sensSubType, dSize, + dFormat, timeFormat, tSize, + hasMetaData); } else { /*Sensor Enumeration type is Private*/ errCode = OSP_ParseSensorDataPkt_Private(pOut, pHif, - (ASensorType_t)sensType, - sensSubType, dSize, - dFormat, timeFormat, tSize, - hasMetaData); - - + (ASensorType_t)sensType, + sensSubType, dSize, + dFormat, timeFormat, tSize, + hasMetaData); } return errCode; } + /* ------------ END OSP Packet parsing code -------------------- */ static int OSP_Sensor_enable(struct osp_data *osp, int sensor, int private) { int retval = -EINVAL; - /* Private not supported yet */ - if (private != 0) return -EINVAL; + if (private != 0 && !(osp->features & FEAT_PRIVATE)) return -EINVAL; if (sensor < 0x30) retval = i2c_smbus_read_byte_data(osp->client, 0x20+sensor); @@ -362,8 +514,7 @@ static int OSP_Sensor_disable(struct osp_data *osp, int sensor, int private) { int retval = -EINVAL; - /* Private not supported yet */ - if (private != 0) return -EINVAL; + if (private != 0 && !(osp->features & FEAT_PRIVATE)) return -EINVAL; if (sensor < 0x30) retval = i2c_smbus_read_byte_data(osp->client, 0x50+sensor); @@ -461,6 +612,7 @@ int OSP_Sensor_Register(int sensor, int private, return 0; } + EXPORT_SYMBOL_GPL(OSP_Sensor_Register); /* API Call: Release a sensor */ @@ -479,53 +631,68 @@ int OSP_Sensor_UnRegister(int sensor, int private) return 0; } + EXPORT_SYMBOL_GPL(OSP_Sensor_UnRegister); /* API Call: Activates/suspends a sensor */ int OSP_Sensor_State(int sensor, int private, int state) { - struct OSP_Sensor *sen; - - if (private == 1) { - sen = prv_sensor; - return -EINVAL; /* Private not supported yet */ - } else if (private == 0) - sen = and_sensor; - else + if (private != 1 && private != 0) return -EINVAL; if (state == 1) { - OSP_Sensor_enable(gOSP, sensor, private); + OSP_Sensor_enable(gOSP, sensor, private); } else { - OSP_Sensor_disable(gOSP, sensor, private); + OSP_Sensor_disable(gOSP, sensor, private); } return 0; } + EXPORT_SYMBOL_GPL(OSP_Sensor_State); static void OSP_ReportSensor(struct osp_data *osp, - SensorPacketTypes_t *spack) + SensorPacketTypes_t *spack) { int PSensor; - static int counter = 0; + static int sig_counter = 0, step_counter = 0; union OSP_SensorData data; switch(spack->SType) { case SENSOR_STEP_DETECTOR: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.StepDetector.StepDetected; + data.xyz.y = step_counter; + step_counter++; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.StepDetector.TimeStamp.TS64, &data); + break; + case SENSOR_SIGNIFICANT_MOTION: if (!and_sensor[spack->SType].dataready) break; - data.xyz.x = 1; - data.xyz.y = counter; - counter++; + data.xyz.x = spack->P.SigMotion.MotionDetected; + data.xyz.y = sig_counter; + sig_counter++; and_sensor[spack->SType].dataready(spack->SType, 0, - and_sensor[spack->SType].private, jiffies, &data); + and_sensor[spack->SType].private, + spack->P.SigMotion.TimeStamp.TS64, &data); break; - case SENSOR_STEP_COUNTER: + case SENSOR_PRESSURE: if (!and_sensor[spack->SType].dataready) break; data.xyz.x = spack->P.CalFixP.Axis[0]; and_sensor[spack->SType].dataready(spack->SType, 0, - and_sensor[spack->SType].private, jiffies, &data); + and_sensor[spack->SType].private, + spack->P.CalFixP.TimeStamp.TS64, &data); + break; + + case SENSOR_STEP_COUNTER: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = (uint32_t)spack->P.StepCount.NumStepsTotal; + data.xyz.y = (uint32_t)(spack->P.StepCount.NumStepsTotal >> 32); + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.StepCount.TimeStamp.TS64, &data); break; case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: @@ -535,19 +702,42 @@ static void OSP_ReportSensor(struct osp_data *osp, data.xyz.y = spack->P.UncalFixP.Axis[1]; data.xyz.z = spack->P.UncalFixP.Axis[2]; and_sensor[spack->SType].dataready(spack->SType, 0, - and_sensor[spack->SType].private, jiffies, &data); + and_sensor[spack->SType].private, + spack->P.UncalFixP.TimeStamp.TS64, &data); break; + case SENSOR_ACCELEROMETER: case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: - case SENSOR_ORIENTATION: if (!and_sensor[spack->SType].dataready) break; data.xyz.x = spack->P.CalFixP.Axis[0]; data.xyz.y = spack->P.CalFixP.Axis[1]; data.xyz.z = spack->P.CalFixP.Axis[2]; and_sensor[spack->SType].dataready(spack->SType, 0, - and_sensor[spack->SType].private, jiffies, &data); + and_sensor[spack->SType].private, + spack->P.CalFixP.TimeStamp.TS64, &data); + break; + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_GRAVITY: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.ThreeAxisFixP.Axis[0]; + data.xyz.y = spack->P.ThreeAxisFixP.Axis[1]; + data.xyz.z = spack->P.ThreeAxisFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.ThreeAxisFixP.TimeStamp.TS64, &data); + break; + + case SENSOR_ORIENTATION: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.z = spack->P.OrientFixP.Pitch; + data.xyz.y = spack->P.OrientFixP.Roll; + data.xyz.x = spack->P.OrientFixP.Yaw; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.OrientFixP.TimeStamp.TS64, &data); break; + case SENSOR_ROTATION_VECTOR: case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: case SENSOR_GAME_ROTATION_VECTOR: @@ -557,8 +747,10 @@ static void OSP_ReportSensor(struct osp_data *osp, data.quat.y = spack->P.QuatFixP.Quat[2]; data.quat.z = spack->P.QuatFixP.Quat[3]; and_sensor[spack->SType].dataready(spack->SType, 0, - and_sensor[spack->SType].private, jiffies, &data); + and_sensor[spack->SType].private, + spack->P.QuatFixP.TimeStamp.TS64, &data); break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); if (!prv_sensor[PSensor].dataready) break; @@ -567,8 +759,10 @@ static void OSP_ReportSensor(struct osp_data *osp, data.xyz.y = spack->P.UncalFixP.Axis[1]; data.xyz.z = spack->P.UncalFixP.Axis[2]; prv_sensor[PSensor].dataready(PSensor, 1, - prv_sensor[PSensor].private, jiffies, &data); + prv_sensor[PSensor].private, + spack->P.UncalFixP.TimeStamp.TS64, &data); break; + case PSENSOR_ACCELEROMETER_RAW|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); if (!prv_sensor[PSensor].dataready) break; @@ -576,8 +770,10 @@ static void OSP_ReportSensor(struct osp_data *osp, data.xyz.y = spack->P.RawSensor.Axis[1]; data.xyz.z = spack->P.RawSensor.Axis[2]; prv_sensor[PSensor].dataready(PSensor, 1, - prv_sensor[PSensor].private, jiffies, &data); + prv_sensor[PSensor].private, + spack->P.RawSensor.TStamp.TS64, &data); break; + default: break; } @@ -602,12 +798,12 @@ static void osp_work_q(struct work_struct *work) /* Grab buffers as quickly as possible */ for (i = 0; i < NUM_PACK; i++) { ret = osp_i2c_read(osp, OSP_INT_LEN, - (unsigned char *)&intlen, 4); + (unsigned char *)&intlen, 4); if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { plen[i] = (intlen >> 4); if (plen[i] > 0 && plen[i] < 8192) { ret = osp_i2c_read(osp, OSP_DATA_OUT, - osp_pack[i], plen[i]); + osp_pack[i], plen[i]); if (ret < 0) return; gotpack[i] = 1; } @@ -618,12 +814,15 @@ static void osp_work_q(struct work_struct *work) pack_count = 0; pack_ptr = osp_pack[i]; do { - ret = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen[i]); + ret = OSP_ParseSensorDataPkt(&spack, pack_ptr, + plen[i]); if (ret>0) { OSP_ReportSensor(osp, &spack); } else { - dev_err(&osp->client->dev, - "OSP packet parsing error = %i, pack_count = %i plen = %i\n", ret, pack_count, plen[i]); + dev_err( + &osp->client->dev, + "OSP packet parsing error = %i, pack_count = %i plen = %i\n", + ret, pack_count, plen[i]); break; } plen[i] -= ret; @@ -633,6 +832,7 @@ static void osp_work_q(struct work_struct *work) } } } + static void osp_poll_timer(unsigned long _osp) { struct osp_data *osp = (void *)_osp; @@ -683,14 +883,15 @@ static int OSP_add_child(struct osp_data *osp) } static int osp_probe(struct i2c_client *client, - const struct i2c_device_id *id) + const struct i2c_device_id *id) { struct osp_data *osp; + int err; int i; if (!i2c_check_functionality(client->adapter, - I2C_FUNC_I2C | I2C_FUNC_SMBUS_BYTE_DATA)) { + I2C_FUNC_I2C | I2C_FUNC_SMBUS_BYTE_DATA)) { dev_err(&client->dev, "client is not i2c capable\n"); return -ENXIO; } @@ -709,7 +910,7 @@ static int osp_probe(struct i2c_client *client, "failed to allocate memory for packet data\n"); i--; for (; i >= 0; i--) { - kfree(osp_pack[i]); + kfree(osp_pack[i]); } return -ENOMEM; } @@ -730,16 +931,19 @@ static int osp_probe(struct i2c_client *client, INIT_WORK(&osp->osp_work, osp_work_q); } - client->irq = -1; /* Force it for now */ + client->irq = -1; /* Force it for now */ if (client->irq > 0) { - err = request_threaded_irq(client->irq, NULL, osp_irq_thread, IRQF_TRIGGER_LOW, "osp-sh", osp); + err = + request_threaded_irq(client->irq, NULL, osp_irq_thread, + IRQF_TRIGGER_LOW, + "osp-sh", + osp); if (err < 0) { dev_err(&client->dev, "irq request failed %d, error %d\n", client->irq, err); } - } if (client->irq <= 0 || err < 0) { @@ -786,6 +990,7 @@ static int osp_resume(struct device *dev) { return 0; } + #endif static SIMPLE_DEV_PM_OPS(osp_pm_ops, osp_suspend, osp_resume); @@ -814,6 +1019,7 @@ static int __init osp_init(void) { return i2c_add_driver(&osp_driver); } + module_init(osp_init); #endif From 0993939a9cc4f4354d9304460439ce9094fe3aa9 Mon Sep 17 00:00:00 2001 From: Hunyue Yau Date: Wed, 14 Oct 2015 16:59:31 -0700 Subject: [PATCH 21/21] Fix roll/pitch for the orientation sensor. Signed-off-by: Hunyue Yau --- linux/driver/osp-sh.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/linux/driver/osp-sh.c b/linux/driver/osp-sh.c index 7574a47..5623171 100644 --- a/linux/driver/osp-sh.c +++ b/linux/driver/osp-sh.c @@ -730,8 +730,8 @@ static void OSP_ReportSensor(struct osp_data *osp, case SENSOR_ORIENTATION: if (!and_sensor[spack->SType].dataready) break; - data.xyz.z = spack->P.OrientFixP.Pitch; - data.xyz.y = spack->P.OrientFixP.Roll; + data.xyz.y = spack->P.OrientFixP.Pitch; + data.xyz.z = spack->P.OrientFixP.Roll; data.xyz.x = spack->P.OrientFixP.Yaw; and_sensor[spack->SType].dataready(spack->SType, 0, and_sensor[spack->SType].private,