diff options
Diffstat (limited to 'libsensors/mlsdk/platform/linux')
-rw-r--r-- | libsensors/mlsdk/platform/linux/kernel/mpuirq.h | 41 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/kernel/slaveirq.h | 35 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/kernel/timerirq.h | 29 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/log_linux.c | 114 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/log_printf_linux.c | 43 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/mlos_linux.c | 206 | ||||
-rw-r--r-- | libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c | 489 |
7 files changed, 957 insertions, 0 deletions
diff --git a/libsensors/mlsdk/platform/linux/kernel/mpuirq.h b/libsensors/mlsdk/platform/linux/kernel/mpuirq.h new file mode 100644 index 0000000..352170b --- /dev/null +++ b/libsensors/mlsdk/platform/linux/kernel/mpuirq.h @@ -0,0 +1,41 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ + +#ifndef __MPUIRQ__ +#define __MPUIRQ__ + +#ifdef __KERNEL__ +#include <linux/time.h> +#include <linux/ioctl.h> +#include "mldl_cfg.h" +#else +#include <sys/ioctl.h> +#include <sys/time.h> +#endif + +#define MPUIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x40, unsigned long) +#define MPUIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x41, unsigned long) +#define MPUIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x42, struct timeval) +#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long) + +#ifdef __KERNEL__ +void mpuirq_exit(void); +int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg); +#endif + +#endif diff --git a/libsensors/mlsdk/platform/linux/kernel/slaveirq.h b/libsensors/mlsdk/platform/linux/kernel/slaveirq.h new file mode 100644 index 0000000..beb352b --- /dev/null +++ b/libsensors/mlsdk/platform/linux/kernel/slaveirq.h @@ -0,0 +1,35 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ + +#ifndef __SLAVEIRQ__ +#define __SLAVEIRQ__ + +#include <linux/mpu.h> +#include "mpuirq.h" + +#define SLAVEIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x50, unsigned long) +#define SLAVEIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x51, unsigned long) +#define SLAVEIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x52, unsigned long) + + +void slaveirq_exit(struct ext_slave_platform_data *pdata); +int slaveirq_init(struct i2c_adapter *slave_adapter, + struct ext_slave_platform_data *pdata, char *name); + + +#endif diff --git a/libsensors/mlsdk/platform/linux/kernel/timerirq.h b/libsensors/mlsdk/platform/linux/kernel/timerirq.h new file mode 100644 index 0000000..dc3eea2 --- /dev/null +++ b/libsensors/mlsdk/platform/linux/kernel/timerirq.h @@ -0,0 +1,29 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ + +#ifndef __TIMERIRQ__ +#define __TIMERIRQ__ + +#include <linux/mpu.h> + +#define TIMERIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x60, unsigned long) +#define TIMERIRQ_GET_INTERRUPT_CNT _IOW(MPU_IOCTL, 0x61, unsigned long) +#define TIMERIRQ_START _IOW(MPU_IOCTL, 0x62, unsigned long) +#define TIMERIRQ_STOP _IO(MPU_IOCTL, 0x63) + +#endif diff --git a/libsensors/mlsdk/platform/linux/log_linux.c b/libsensors/mlsdk/platform/linux/log_linux.c new file mode 100644 index 0000000..7f7de0e --- /dev/null +++ b/libsensors/mlsdk/platform/linux/log_linux.c @@ -0,0 +1,114 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ +/****************************************************************************** + * $Id: log_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + ******************************************************************************/ + +/** + * @defgroup MPL_LOG + * @brief Logging facility for the MPL + * + * @{ + * @file log.c + * @brief Core logging facility functions. + * + * +**/ + +#include <stdio.h> +#include <string.h> +#include "log.h" +#include "mltypes.h" + +#define LOG_BUFFER_SIZE (256) + +#ifdef WIN32 +#define snprintf _snprintf +#define vsnprintf _vsnprintf +#endif + +int _MLPrintLog (int priority, const char* tag, const char* fmt, ...) +{ + va_list ap; + int result; + + va_start(ap, fmt); + result = _MLPrintVaLog(priority,tag,fmt,ap); + va_end(ap); + + return result; +} + +int _MLPrintVaLog(int priority, const char* tag, const char* fmt, va_list args) +{ + int result; + char buf[LOG_BUFFER_SIZE]; + char new_fmt[LOG_BUFFER_SIZE]; + char priority_char; + + if (NULL == fmt) { + fmt = ""; + } + + switch (priority) { + case MPL_LOG_UNKNOWN: + priority_char = 'U'; + break; + case MPL_LOG_VERBOSE: + priority_char = 'V'; + break; + case MPL_LOG_DEBUG: + priority_char = 'D'; + break; + case MPL_LOG_INFO: + priority_char = 'I'; + break; + case MPL_LOG_WARN: + priority_char = 'W'; + break; + case MPL_LOG_ERROR: + priority_char = 'E'; + break; + case MPL_LOG_SILENT: + priority_char = 'S'; + break; + case MPL_LOG_DEFAULT: + default: + priority_char = 'D'; + break; + }; + + result = snprintf(new_fmt, sizeof(new_fmt), "%c/%s:%s", + priority_char, tag, fmt); + if (result <= 0) { + return INV_ERROR_LOG_MEMORY_ERROR; + } + result = vsnprintf(buf,sizeof(buf),new_fmt, args); + if (result <= 0) { + return INV_ERROR_LOG_OUTPUT_ERROR; + } + + result = _MLWriteLog(buf, strlen(buf)); + return INV_SUCCESS; +} + +/** + * @} +**/ + + diff --git a/libsensors/mlsdk/platform/linux/log_printf_linux.c b/libsensors/mlsdk/platform/linux/log_printf_linux.c new file mode 100644 index 0000000..e6499f3 --- /dev/null +++ b/libsensors/mlsdk/platform/linux/log_printf_linux.c @@ -0,0 +1,43 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ +/******************************************************************************* + * + * $Id: log_printf_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + ******************************************************************************/ + +/** + * @addtogroup MPL_LOG + * + * @{ + * @file log_printf.c + * @brief printf replacement for _MLWriteLog. + */ + +#include <stdio.h> +#include "log.h" + +int _MLWriteLog (const char * buf, int buflen) +{ + return fputs(buf, stdout); +} + +/** + * @} + */ + diff --git a/libsensors/mlsdk/platform/linux/mlos_linux.c b/libsensors/mlsdk/platform/linux/mlos_linux.c new file mode 100644 index 0000000..8704f4b --- /dev/null +++ b/libsensors/mlsdk/platform/linux/mlos_linux.c @@ -0,0 +1,206 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. +**/ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include <sys/time.h> +#include <unistd.h> +#include <pthread.h> +#include <stdlib.h> + +#include "stdint_invensense.h" + +#include "mlos.h" +#include <errno.h> + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param numBytes number of bytes + * @return pointer to allocated space +**/ +void *inv_malloc(unsigned int numBytes) +{ + // Allocate space. + void *allocPtr = malloc(numBytes); + return allocPtr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. +**/ +inv_error_t inv_free(void *ptr) +{ + // Deallocate space. + free(ptr); + + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. +**/ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename,"r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t*)handle; + error = pthread_mutex_destroy(pm); + if (error) { + return errno; + } + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int mSecs) +{ + usleep(mSecs*1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) !=0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + + /**********************/ + /** @} */ /* defgroup */ +/**********************/ + diff --git a/libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c b/libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c new file mode 100644 index 0000000..00d5973 --- /dev/null +++ b/libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c @@ -0,0 +1,489 @@ +/* + $License: + Copyright 2011 InvenSense, 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. + $ + */ + +/****************************************************************************** + * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ + *****************************************************************************/ + +/** + * @defgroup MLSL (Motion Library - Serial Layer) + * @brief The Motion Library System Layer provides the Motion Library the + * interface to the system functions. + * + * @{ + * @file mlsl_linux_mpu.c + * @brief The Motion Library System Layer. + * + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ +#include <stdio.h> +#include <sys/ioctl.h> +#include <stdlib.h> +#include <fcntl.h> +#include <errno.h> +#include <unistd.h> +#include <linux/fs.h> +#include <linux/i2c.h> +#include <string.h> +#include <signal.h> +#include <time.h> + +#include "mpu.h" +#include "mpu3050.h" + +#include "mlsl.h" +#include "mlos.h" +#include "mlmath.h" +#include "mlinclude.h" + +#define MLCAL_ID (0x0A0B0C0DL) +#define MLCAL_FILE "/data/cal.bin" +#define MLCFG_ID (0x01020304L) +#define MLCFG_FILE "/data/cfg.bin" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlsl" + +#ifndef I2CDEV +#define I2CDEV "/dev/mpu" +#endif + +#define SERIAL_FULL_DEBUG (0) + +/* --------------- */ +/* - Prototypes. - */ +/* --------------- */ + +/* ----------------------- */ +/* - Function Pointers. - */ +/* ----------------------- */ + +/* --------------------------- */ +/* - Global and Static vars. - */ +/* --------------------------- */ + +/* ---------------- */ +/* - Definitions. - */ +/* ---------------- */ + +inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len) +{ + FILE *fp; + unsigned int bytesRead; + + fp = fopen(MLCFG_FILE, "rb"); + if (fp == NULL) { + MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesRead = fread(cfg, 1, len, fp); + if (bytesRead != len) { + MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", + bytesRead, len); + return INV_ERROR_FILE_READ; + } + fclose(fp); + + if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3]) + != MLCFG_ID) { + return INV_ERROR_ASSERTION_FAILURE; + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len) +{ + FILE *fp; + unsigned int bytesWritten; + unsigned char cfgId[4]; + + fp = fopen(MLCFG_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE); + return INV_ERROR_FILE_OPEN; + } + + cfgId[0] = (unsigned char)(MLCFG_ID >> 24); + cfgId[1] = (unsigned char)(MLCFG_ID >> 16); + cfgId[2] = (unsigned char)(MLCFG_ID >> 8); + cfgId[3] = (unsigned char)(MLCFG_ID); + bytesWritten = fwrite(cfgId, 1, 4, fp); + if (bytesWritten != 4) { + MPL_LOGE("CFG ID could not be written on file\n"); + return INV_ERROR_FILE_WRITE; + } + + bytesWritten = fwrite(cfg, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes write (%d) don't match requested length (%d)\n", + bytesWritten, len); + return INV_ERROR_FILE_WRITE; + } + + fclose(fp); + + return INV_SUCCESS; +} + +inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len) +{ + FILE *fp; + unsigned int bytesRead; + inv_error_t result = INV_SUCCESS; + + fp = fopen(MLCAL_FILE,"rb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesRead = fread(cal, 1, len, fp); + if (bytesRead != len) { + MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", + bytesRead, len); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + + /* MLCAL_ID not used + if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3]) + != MLCAL_ID) { + result = INV_ERROR_ASSERTION_FAILURE; + goto read_cal_end; + } + */ +read_cal_end: + fclose(fp); + return result; +} + +inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len) +{ + FILE *fp; + unsigned int bytesWritten; + inv_error_t result = INV_SUCCESS; + + fp = fopen(MLCAL_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(cal, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } + fclose(fp); + return result; +} + +inv_error_t inv_serial_get_cal_length(unsigned int *len) +{ + FILE *calFile; + *len = 0; + + calFile = fopen(MLCAL_FILE, "rb"); + if (calFile == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + + *len += (unsigned char)fgetc(calFile) << 24; + *len += (unsigned char)fgetc(calFile) << 16; + *len += (unsigned char)fgetc(calFile) << 8; + *len += (unsigned char)fgetc(calFile); + + fclose(calFile); + + if (*len <= 0) + return INV_ERROR_FILE_READ; + + return INV_SUCCESS; +} + +inv_error_t inv_serial_open(char const *port, void **sl_handle) +{ + INVENSENSE_FUNC_START; + + if (NULL == port) { + port = I2CDEV; + } + *sl_handle = (void*)(uintptr_t) open(port, O_RDWR); + if((intptr_t)*sl_handle < 0) { + /* ERROR HANDLING; you can check errno to see what went wrong */ + MPL_LOGE("inv_serial_open\n"); + MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port); + return INV_ERROR_SERIAL_OPEN_ERROR; + } else { + MPL_LOGI("inv_serial_open: %s\n", port); + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_close(void *sl_handle) +{ + INVENSENSE_FUNC_START; + + close((int)(uintptr_t)sl_handle); + + return INV_SUCCESS; +} + +inv_error_t inv_serial_reset(void *sl_handle __unused) +{ + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +inv_error_t inv_serial_single_write(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned char data) +{ + unsigned char buf[2]; + buf[0] = registerAddr; + buf[1] = data; + return inv_serial_write(sl_handle, slaveAddr, 2, buf); +} + +inv_error_t inv_serial_write(void *sl_handle, + unsigned char slaveAddr __unused, + unsigned short length, + unsigned char const *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + inv_error_t result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = 0; /* not used */ + msg.length = length; + msg.data = (unsigned char*)data; + + if ((result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE, &msg))) { + MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n", + data[0], length, result); + return result; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C Write Success %02x %02x: %s \n", + data[0], length, data_buff); + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_read(void *sl_handle, + unsigned char slaveAddr __unused, + unsigned char registerAddr, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + int result = INV_SUCCESS; + struct mpu_read_write msg; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = registerAddr; + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ, &msg); + + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n", + result, registerAddr, length); + result = INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C Read Success %02x %02x: %s \n", + registerAddr, length, data_buff); + } + + return (inv_error_t) result; +} + +inv_error_t inv_serial_write_mem(void *sl_handle, + unsigned char mpu_addr __unused, + unsigned short memAddr, + unsigned short length, + const unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + msg.address = memAddr; + msg.length = length; + msg.data = (unsigned char *)data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_MEM, &msg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n", + memAddr, length, data_buff); + } + return INV_SUCCESS; +} + +inv_error_t inv_serial_read_mem(void *sl_handle, + unsigned char mpu_addr __unused, + unsigned short memAddr, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = memAddr; + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_MEM, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n", + result, memAddr, length); + return INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n", + memAddr, length, data_buff); + } + return INV_SUCCESS; +} + +inv_error_t inv_serial_write_fifo(void *sl_handle, + unsigned char mpu_addr __unused, + unsigned short length, + const unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = 0; /* Not used */ + msg.length = length; + msg.data = (unsigned char *)data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_FIFO, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n", + MPUREG_FIFO_R_W, length); + return INV_ERROR_SERIAL_WRITE; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C Write Success %02x %02x: %s\n", + MPUREG_FIFO_R_W, length, data_buff); + } + return (inv_error_t) result; +} + +inv_error_t inv_serial_read_fifo(void *sl_handle, + unsigned char mpu_addr __unused, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = MPUREG_FIFO_R_W; /* Not used */ + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_FIFO, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n", + result, MPUREG_FIFO_R_W, length); + return INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, (sizeof(data_buff) - strlen(data_buff) - 1)); + } + MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n", + MPUREG_FIFO_R_W, length, data_buff); + } + return INV_SUCCESS; +} + +/** + * @} + */ + + |