/*
 * __________________________________________________________________
 *
 * Copyright (C) [2023] by InvenSense, Inc.
 * 
 * Permission to use, copy, modify, and/or distribute this software
 * for any purpose with or without fee is hereby granted.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
 * WARRANTIES  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
 * WARRANTIES OF MERCHANTABILITY  AND FITNESS. IN NO EVENT SHALL
 * THE AUTHOR BE LIABLE  FOR ANY SPECIAL, DIRECT, INDIRECT, 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 THIS SOFTWARE.
 * __________________________________________________________________
 */

#include "Invn/Drivers/Ixm42xxx/Ixm42xxxTransport.h"
#include "Invn/Drivers/Ixm42xxx/Ixm42xxxDefs.h"

#include "Invn/InvError.h"


/* Function definition */
static uint8_t * get_register_cache_addr(struct inv_ixm42xxx * s, uint8_t reg);
static uint8_t is_aux_interface(struct inv_ixm42xxx_transport *t);


int inv_ixm42xxx_init_transport(struct inv_ixm42xxx * s)
{
	// Registers in cache must be in bank 0
	int status = 0;
	struct inv_ixm42xxx_transport *t = (struct inv_ixm42xxx_transport *)s;

	if(!is_aux_interface(t)){
		status |= t->serif.read_reg(&(t->serif), MPUREG_INTF_CONFIG1,  &(t->register_cache.intf_cfg_1_reg), 1);
		status |= t->serif.read_reg(&(t->serif), MPUREG_PWR_MGMT_0,    &(t->register_cache.pwr_mngt_0_reg), 1);
		status |= t->serif.read_reg(&(t->serif), MPUREG_GYRO_CONFIG0,  &(t->register_cache.gyro_cfg_0_reg), 1);
		status |= t->serif.read_reg(&(t->serif), MPUREG_ACCEL_CONFIG0, &(t->register_cache.accel_cfg_0_reg), 1);
		status |= t->serif.read_reg(&(t->serif), MPUREG_TMST_CONFIG,   &(t->register_cache.tmst_cfg_reg), 1);
	}
	
	status |= t->serif.read_reg(&(t->serif), MPUREG_REG_BANK_SEL,  &(t->register_cache.bank_sel_reg), 1);

	return status;
}

int inv_ixm42xxx_read_reg(struct inv_ixm42xxx * s, uint8_t reg, uint32_t len, uint8_t * buf)
{
	// First field of struct inv_ixm42xxx is assumed to be a struct inv_ixm42xxx_transport object.
	// So let's cast s to struct inv_ixm42xxx_transport and ignore the rest of struct inv_ixm42xxx.
	struct inv_ixm42xxx_transport *t = (struct inv_ixm42xxx_transport *)s;
	uint32_t i=0;
	
	// Registers in cache are only in bank 0
	// Check if bank0 is used because of duplicate register addresses between banks
	// For AUX interface, register cache must not be used
	if((t->register_cache.bank_sel_reg == 0) && (is_aux_interface(t) == 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
				break; // If one register isn't in cache, exit the loop and proceed a physical access
		}
		
		if(i==len) // If the end of loop is reached, then all registers have been read in cache so return
			return 0;
	}
	
	// Physical access to read registers
	if((len-i) > t->serif.max_read)
		return INV_ERROR_SIZE;
	if(t->serif.read_reg(&(t->serif), reg+i, &buf[i], len-i) != 0)
		return INV_ERROR_TRANSPORT;
	
	return 0;
}

int inv_ixm42xxx_write_reg(struct inv_ixm42xxx * s, uint8_t reg, uint32_t len, const uint8_t * buf)
{
	// First field of struct inv_ixm42xxx is assumed to be a struct inv_ixm42xxx_transport object.
	// So let's cast s to struct inv_ixm42xxx_transport and ignore the rest of struct inv_ixm42xxx.
	struct inv_ixm42xxx_transport *t = (struct inv_ixm42xxx_transport *)s;
	uint32_t i;
	
	if(len > t->serif.max_write)
		return INV_ERROR_SIZE;
	
	
	for(i=0; i<len; i++) {
		// Update bank_sel_reg in the cache
		if((reg+i) == MPUREG_REG_BANK_SEL){
			t->register_cache.bank_sel_reg = buf[i];
		}
		else {
			uint8_t * cache_addr  = get_register_cache_addr(s, reg+i);
			if(cache_addr){
				if (t->register_cache.bank_sel_reg == 0){
					if(is_aux_interface(t) == 1)
						return INV_ERROR_BAD_ARG; // Cached registers must not be written from AUX interface
					else
						*cache_addr = buf[i]; // Update register cache if the current bank is 0
				}
			}
		}
	}
	
	// Physical access to write registers
	if(t->serif.write_reg(&(t->serif), reg, buf, len) != 0)
		return INV_ERROR_TRANSPORT;

	return 0;
}

/* Static function */

/* MPUREG_REG_BANK_SEL shall never be added to the function get_register_cache_addr() */
static uint8_t * get_register_cache_addr(struct inv_ixm42xxx * s, uint8_t reg)
{
	struct inv_ixm42xxx_transport *t = (struct inv_ixm42xxx_transport *)s;

	switch(reg) {
		case MPUREG_INTF_CONFIG1:     return &(t->register_cache.intf_cfg_1_reg);
		case MPUREG_PWR_MGMT_0:       return &(t->register_cache.pwr_mngt_0_reg);
		case MPUREG_GYRO_CONFIG0:     return &(t->register_cache.gyro_cfg_0_reg);
		case MPUREG_ACCEL_CONFIG0:    return &(t->register_cache.accel_cfg_0_reg);
		case MPUREG_TMST_CONFIG:      return &(t->register_cache.tmst_cfg_reg);
		default:                      return (uint8_t *)0; // Not found
	}
}

static uint8_t is_aux_interface(struct inv_ixm42xxx_transport *t)
{
	if ((t->serif.serif_type == IXM42XXX_AUX1_SPI3) || (t->serif.serif_type == IXM42XXX_AUX2_SPI3))
		return 1;
	else
		return 0;
}
