/*
 $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"
#if defined CONFIG_MPU_SENSORS_MPU6050A2
#    include "mpu6050a2.h"
#elif defined CONFIG_MPU_SENSORS_MPU6050B1
#    include "mpu6050b1.h"
#elif defined CONFIG_MPU_SENSORS_MPU3050
#  include "mpu3050.h"
#else
#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
#endif

#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;
    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;
    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;
    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;
    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*) open(port, O_RDWR);
    if(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)sl_handle);

    return INV_SUCCESS;
}

inv_error_t inv_serial_reset(void *sl_handle)
{
    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,
                         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)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));
        }
        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,
                        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)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));
        }
        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,
                            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)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));
        }
        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,
                           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)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));
        }
        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,
                             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)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));
        }
        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,
                            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)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));
        }
        MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
                 MPUREG_FIFO_R_W, length, data_buff);
    }
    return INV_SUCCESS;
}

/**
 *  @}
 */