/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
 *
 * This software, related documentation and any modifications thereto (collectively "Software") is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
 * and other intellectual property rights laws.
 *
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
 * and any use, reproduction, disclosure or distribution of the Software without an express license agreement
 * from InvenSense is strictly prohibited.
 *
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
 * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
 * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
 * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
 * OF THE SOFTWARE.
 * ________________________________________________________________________________________________________
 */

#include "inv_imu_extfunc.h"
#include "inv_imu_transport.h"
#include "inv_imu_regmap.h"

#include "InvError.h"


/* Function definition */
static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg);
static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf);
static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf);
static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf);
static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf);


int inv_imu_init_transport(struct inv_imu_device *s)
{
	int status = 0;
	struct inv_imu_transport *t = (struct inv_imu_transport *)s;

	status |= read_sreg(s, (uint8_t)PWR_MGMT0,     1, &(t->register_cache.pwr_mgmt0_reg));
	status |= read_sreg(s, (uint8_t)GYRO_CONFIG0,  1, &(t->register_cache.gyro_config0_reg));
	status |= read_sreg(s, (uint8_t)ACCEL_CONFIG0, 1, &(t->register_cache.accel_config0_reg));
	
	status |= read_mclk_reg(s, (TMST_CONFIG1_MREG1 & 0xFFFF), 1, &(t->register_cache.tmst_config1_reg));

	t->need_mclk_cnt = 0;

	return status;
}

int inv_imu_read_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, uint8_t *buf)
{
	uint32_t i;
	int rc = 0;
	
	for (i = 0; i < len; i++) {
		uint8_t *cache_addr  = get_register_cache_addr(s, reg + i);

		if (cache_addr) {
			buf[i] = *cache_addr;
		} else {
			if (!(reg & 0x10000)) {
				rc |= read_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
			} else {
				rc |= read_sreg(s, (uint8_t)reg + i, len - i, &buf[i]);
				break;
			}
		}
	}

	return rc;
}

int inv_imu_write_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, const uint8_t *buf)
{
	uint32_t i;
	int rc = 0;
	
	for (i = 0; i < len; i++) {
		uint8_t *cache_addr  = get_register_cache_addr(s, reg + i);

		if (cache_addr)
			*cache_addr = buf[i];

		if (!(reg & 0x10000))
			rc |= write_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
	}

	if (reg & 0x10000)
		rc |= write_sreg(s, (uint8_t)reg, len, buf);

	return rc;
}

int inv_imu_switch_on_mclk(struct inv_imu_device *s)
{
	int status = 0;
	uint8_t data;
	struct inv_imu_transport *t = (struct inv_imu_transport *)s;

	/* set IDLE bit only if it is not set yet */
	if (t->need_mclk_cnt == 0) {

		status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
		data |= PWR_MGMT0_IDLE_MASK; 
		status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
		
		if (status)
			return status;

		/* Check if MCLK is ready */
		do {
			status = inv_imu_read_reg(s, MCLK_RDY, 1, &data); 
		} while ((status != 0) || !(data & MCLK_RDY_MCLK_RDY_MASK));
	} else {

		/* Make sure it is already on */
		status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
		if (0 == (data &= PWR_MGMT0_IDLE_MASK))
			status |= INV_ERROR;
	}

	/* Increment the counter to keep track of number of MCLK requesters */
	t->need_mclk_cnt++;
	
	return status;
}

int inv_imu_switch_off_mclk(struct inv_imu_device *s)
{
	int status = 0;
	uint8_t data;
	struct inv_imu_transport *t = (struct inv_imu_transport *)s;

	/* Reset the IDLE but only if there is one requester left */
	if (t->need_mclk_cnt == 1) {
		status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
		data &= ~PWR_MGMT0_IDLE_MASK;
		status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
	} else {
		/* Make sure it is still on */
		status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
		if (0 == (data &= PWR_MGMT0_IDLE_MASK))
			status |= INV_ERROR;
	}

	/* Decrement the counter */
	t->need_mclk_cnt--;
	
	return status;
}


/* Static function */

static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg)
{
	struct inv_imu_transport *t = (struct inv_imu_transport *)s;

	switch(reg) {
	case PWR_MGMT0:          return &(t->register_cache.pwr_mgmt0_reg);
	case GYRO_CONFIG0:       return &(t->register_cache.gyro_config0_reg);
	case ACCEL_CONFIG0:      return &(t->register_cache.accel_config0_reg);
	case TMST_CONFIG1_MREG1: return &(t->register_cache.tmst_config1_reg);
	default:                 return (uint8_t *)0; // Not found
	}
}

static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf)
{
	struct inv_imu_serif *serif = (struct inv_imu_serif *)s;
	
	if (len > serif->max_read)
		return INV_ERROR_SIZE;

	if (serif->read_reg(serif, reg, buf, len) != 0)
		return INV_ERROR_TRANSPORT;

	return 0;
}

static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf)
{
	struct inv_imu_serif *serif = (struct inv_imu_serif *)s;

	if (len > serif->max_write)
		return INV_ERROR_SIZE;

	if (serif->write_reg(serif, reg, buf, len) != 0)
			return INV_ERROR_TRANSPORT;

	return 0;
}

static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf)
{
	uint8_t data;
	uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
	int status = 0;

	// Have IMU not in IDLE mode to access MCLK domain
	status |= inv_imu_switch_on_mclk(s);

	// optimize by changing BLK_SEL only if not NULL
	if (blk_sel)
		status |= write_sreg(s, (uint8_t)BLK_SEL_R & 0xff, 1, &blk_sel);

	data = (regaddr & 0x00FF);
	status |= write_sreg(s, (uint8_t)MADDR_R, 1, &data);
	inv_imu_sleep_us(10);
	status |= read_sreg(s, (uint8_t)M_R, rd_cnt, buf);
	inv_imu_sleep_us(10);
	
	if (blk_sel) {
		data = 0;
		status |= write_sreg(s, (uint8_t)BLK_SEL_R, 1, &data);
	}

	// switch OFF MCLK if needed
	status |= inv_imu_switch_off_mclk(s);

	return status;
}

static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf)
{
	uint8_t data;
	uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
	int status = 0;

	// Have IMU not in IDLE mode to access MCLK domain
	status |= inv_imu_switch_on_mclk(s);

	// optimize by changing BLK_SEL only if not NULL
	if (blk_sel)
		status |= write_sreg(s, (uint8_t)BLK_SEL_W, 1, &blk_sel);
	
	data = (regaddr & 0x00FF);
	status |= write_sreg(s, (uint8_t)MADDR_W, 1, &data);
	for (uint8_t i = 0; i < wr_cnt; i++) {
		status |= write_sreg(s, (uint8_t)M_W, 1, &buf[i]);
		inv_imu_sleep_us(10);
	}
	
	if (blk_sel) {
		data = 0;
		status = write_sreg(s, (uint8_t)BLK_SEL_W, 1, &data);
	}

	status |= inv_imu_switch_off_mclk(s);

	return status;
}
