/*
 * fan.c
 *
 * Fan speed servo driver. Monitors temperature of I2C onboard sensors and drives the fan accordingly.
 *
 *  Created on: Jul 29, 2012
 *  Authors:
 *  	- Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
 * 		- Benoit RAT <benoit|AT|sevensols.com>
 *
 * 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...
 */
#include <stdio.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include <trace.h>

#include <pio.h>
#include <fan.h>

#include <at91_softpwm.h>

#include "i2c.h"
#include "i2c_fpga_reg.h"
#include "fpga_io.h"
#include "shw_io.h"
#include "spwm-regs.h"


#define FAN_TEMP_SENSOR_ADDR 0x4c

#define DESIRED_TEMPERATURE 55.0

static int fan_update_timeout = 0;
static int is_cpu_pwn = 0;
static int enable_d0 = 0;

static i2c_fpga_reg_t fpga_sensors_bus_master = {
		.base_address = FPGA_I2C_SENSORS_ADDRESS,
		.prescaler = 500,
};

static struct i2c_bus fpga_sensors_i2c = {
		.name = "fpga_sensors",
		.type = I2C_BUS_TYPE_FPGA_REG,
		.type_specific = &fpga_sensors_bus_master,
};

#define PI_FRACBITS 8

/* PI regulator state */
typedef struct {
	float ki, kp; 			/* integral and proportional gains (1<<PI_FRACBITS == 1.0f) */
	float integrator;		/* current integrator value */
	float bias;					/* DC offset always added to the output */
	int anti_windup;	/* when non-zero, anti-windup is enabled */
	float y_min;				/* min/max output range, used by clapming and antiwindup algorithms */
	float y_max;
	float x, y;         /* Current input (x) and output value (y) */
} pi_controller_t;
static pi_controller_t fan_pi;


//-----------------------------------------
//-- Old CPU PWM system (<3.3)
static int pwm_fd;
//-- New FPGA PWM system
static volatile struct SPWM_WB *spwm_wbr;
//----------------------------------------

void shw_pwm_update_timeout(int tout_100ms)
{
	fan_update_timeout=tout_100ms*100000;
	TRACE(TRACE_INFO,"Fan tick timeout is =%d",fan_update_timeout);
}

/* Processes a single sample (x) with Proportional Integrator control algorithm (pi). Returns the value (y) to
	 drive the actuator. */
static inline float pi_update(pi_controller_t *pi, float x)
{
	float i_new, y;
	pi->x = x;
	i_new = pi->integrator + x;

	y = ((i_new * pi->ki + x * pi->kp)) + pi->bias;

	/* clamping (output has to be in <y_min, y_max>) and anti-windup:
	   stop the integrator if the output is already out of range and the output
	   is going further away from y_min/y_max. */
	if(y < pi->y_min)
	{
		y = pi->y_min;
		if((pi->anti_windup && (i_new > pi->integrator)) || !pi->anti_windup)
			pi->integrator = i_new;
	} else if (y > pi->y_max) {
		y = pi->y_max;
		if((pi->anti_windup && (i_new < pi->integrator)) || !pi->anti_windup)
			pi->integrator = i_new;
	} else /* No antiwindup/clamping? */
			pi->integrator = i_new;

	pi->y = y;
	return y;
}

/* initializes the PI controller state. Currently almost a stub. */
static inline void pi_init(pi_controller_t *pi)
{
	pi->integrator = 0;
}

/* Configures a PWM output on gpio pin (pin). Rate accepts range from 0 (0%) to 1000 (100%) */
void pwm_configure_pin(const pio_pin_t *pin, int enable, int rate)
{
	int index = pin->port * 32 + pin->pin;
	if(pin==0) return;

	if(enable && !enable_d0)
		ioctl(pwm_fd, AT91_SOFTPWM_ENABLE, index);
	else if(!enable && enable_d0)
		ioctl(pwm_fd, AT91_SOFTPWM_DISABLE, index);

	enable_d0 = enable;
	ioctl(pwm_fd, AT91_SOFTPWM_SETPOINT, rate);
}


/* Configures a PWM output on gpio pin (pin). Rate accepts range from 0 (0%) to 1000 (100%) */
void pwm_configure_fpga(int enmask, float rate)
{
	uint8_t u8speed=(uint8_t)((rate>=1)?0xff:(rate*255.0));

	if((enmask & 0x1)>0) spwm_wbr->DR0=u8speed;
	if((enmask & 0x2)>0) spwm_wbr->DR1=u8speed;
}

/* Configures a PWM output. Rate accepts range is from 0 (0%) to 1 (100%) */
void shw_pwm_speed(int enmask, float rate)
{
	//TRACE(TRACE_INFO,"%x %f",enmask,rate);
	if(is_cpu_pwn)
	{
		pwm_configure_pin(get_pio_pin(shw_io_box_fan_en), enmask, rate*1000);
	}
	else
	{
		pwm_configure_fpga(enmask,rate);
	}
}


/* Texas Instruments TMP100 temperature sensor driver */
uint32_t tmp100_read_reg(int dev_addr, uint8_t reg_addr, int n_bytes)
{
	uint8_t data[8];
	uint32_t rv=0, i;

	data[0] = reg_addr;
	i2c_write(&fpga_sensors_i2c, dev_addr, 1, data); 
	i2c_read(&fpga_sensors_i2c, dev_addr, n_bytes, data); 

	for(i=0; i<n_bytes;i++)
	{
		rv <<= 8;
		rv |= data[i];
	}

	return rv;
}



void tmp100_write_reg(int dev_addr, uint8_t reg_addr, uint8_t value)
{
	uint8_t data[2];

	data[0] = reg_addr;
	data[1] = value;
	i2c_write(&fpga_sensors_i2c, dev_addr, 2, data); 
}


float tmp100_read_temp(int dev_addr)
{
	int temp = tmp100_read_reg(dev_addr, 0, 2);
	return ((float) (temp >> 4)) / 16.0;
}

int shw_init_i2c_sensors()
{
	if (i2c_init_bus(&fpga_sensors_i2c) < 0) {
		TRACE(TRACE_FATAL, "can't initialize temperature sensors I2C bus.\n");
		return -1;
	}
}

int shw_init_fans()
{
	uint8_t dev_map[128];
	uint32_t val=0;
	int detect, i;

	//Set the type of PWM
	if(shw_get_hw_ver()<330) is_cpu_pwn=1;
	else is_cpu_pwn=0;

	TRACE(TRACE_INFO, "Configuring %s PWMs for fans (desired temperature = %.1f degC)...",is_cpu_pwn?"CPU":"FPGA",DESIRED_TEMPERATURE);

	if(is_cpu_pwn)
	{
		pwm_fd = open("/dev/at91_softpwm", O_RDWR);
		if(pwm_fd < 0)
		{
			TRACE(TRACE_FATAL, "at91_softpwm driver not installed or device not created.\n");
			return -1;
		}

		fan_pi.ki = 1.0;
		fan_pi.kp = 4.0;
		fan_pi.y_min = 200;
		fan_pi.bias = 200;
		fan_pi.y_max = 800;
	}
	else
	{
		//Point to the corresponding WB direction
		spwm_wbr= (volatile struct SPWM_WB *) (FPGA_BASE_ADDR + FPGA_BASE_SPWM);

		//Configure SPWM register the 30~=(62.5MHz÷(8kHz×2^8))−1
		val= SPWM_CR_PRESC_W(30) | SPWM_CR_PERIOD_W(255);
		spwm_wbr->CR=val;

		fan_pi.ki = 1.0;
		fan_pi.kp = 4.0;
		fan_pi.y_min = 400;
		fan_pi.bias = 200;
		fan_pi.y_max = 1000;
	}

	shw_init_i2c_sensors();

	tmp100_write_reg(FAN_TEMP_SENSOR_ADDR, 1, 0x60); // 12-bit resolution

	pi_init(&fan_pi);

	shw_pwm_update_timeout(SHW_FAN_UPDATETO_DEFAULT);

	return 0;
}


/*
 * Reads out the temperature and drives the fan accordingly
 * note: This call is done by hal_main.c:hal_update()
 */
void shw_update_fans()
{
	static int64_t last_tics = -1;
	int64_t cur_tics = shw_get_tics();

	if(fan_update_timeout>0 && (last_tics < 0 || (cur_tics - last_tics) > fan_update_timeout))
	{
		float t_cur = tmp100_read_temp(FAN_TEMP_SENSOR_ADDR);
		float drive = pi_update(&fan_pi, t_cur - DESIRED_TEMPERATURE);
		//TRACE(TRACE_INFO,"t=%f,pwm=%f",t_cur , drive);
		shw_pwm_speed(0xFF, drive/1000); //enable two and one
		last_tics = cur_tics;
	}
}

