/*
 *       phy_driver.c
 *
 *       Copyright (C) 2018, 2019 Mind Chasers 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.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include "phy_driver.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_i2c.h"
#include "ecp5_driver.h"
#include "cmds.h"
#include "utils.h"
#include "main.h"


/*
 *	Test 0 resets the PHY
 */

void phy_compliance_test(i2c_master_transfer_t* pXfer, uint8_t test) {

	PRINTF("PHY TEST %d\n\r", test);
	if (test == 0) {
		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
	}
	else if (test == 1) {
		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
		write_mdio(pXfer, 0x09, 0x3B00);	// Test Mode 1
		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
		write_mdio(pXfer, 0x1D5, 0xF508);
	}
	else if (test == 2) {
		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
		write_mdio(pXfer, 0x09, 0x5B00);	// Test Mode 2
		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
	}
	else if (test == 3) {
		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
		write_mdio(pXfer, 0x09, 0x7B00);	// Test Mode 3
		write_mdio(pXfer, 0x25, 0x0480);	// Output test mode to all channels
	}
	else if (test == 4) {
		write_mdio(pXfer, 0x1F, 0x8000);	// reset PHY
		write_mdio(pXfer, 0x00, 0x0140);	// 1000 Base-T mode
		write_mdio(pXfer, 0x10, 0x5008);	// forced MDI mode
		write_mdio(pXfer, 0x09, 0x9B00);	// Test Mode 3
		write_mdio(pXfer, 0x25, 0x0400);	// Channel A
	}
	else {
		PRINTF("not supported\n\r");
	}

}


/*
 * 	Write using Extended Addressing
 */
void write_mdio(i2c_master_transfer_t* pXfer, uint16_t addr, uint16_t data) {
	/* set the write address first with a read */
	read_mdio(pXfer, addr);

	uint8_t buf[16];
	buf[0]='z';
	buf[1] = (data & 0xf000) >> 12;
	buf[2] = (data & 0x0f00) >> 8;
	buf[3] = (data & 0x00f0) >> 4;
	buf[4] = data & 0x000f;
	buf[5]=0xd;

	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 6);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);
}
/*
 * Read using Extended Addressing.
 */
uint16_t read_mdio(i2c_master_transfer_t* pXfer, uint16_t addr) {
	uint16_t temp;
	uint8_t buf[16];

	buf[0]='y';
	buf[1] = (addr & 0xf000) >> 12;
	buf[2] = (addr & 0x0f00) >> 8;
	buf[3] = (addr & 0x00f0) >> 4;
	buf[4] = addr & 0x000f;
	buf[5]=0xd;

	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 6);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	temp = (buf[0] & 0xf) << 12 | (buf[1] & 0xf) << 8 | (buf[2] & 0xf) << 4 | (buf[3] & 0xf);
	return temp;
}

int invert_sgmii_polarity(i2c_master_transfer_t* pXfer, uint8_t phy) {
	uint8_t buf[16];

	// set the right PHY mux_sel
	buf[0]='m';
	buf[1]=phy;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 2);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Extended Write of 0x01D5

	// Read 0xD
	buf[0]='r';
	buf[1]=0x0;
	buf[2]=0xd;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Write 0x001f to 0xD
	buf[0]='w';
	buf[1]=0x0;
	buf[2]=0x0;
	buf[3]=0x1;
	buf[4]=0xf;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Read 0xE to set address
	buf[0]='r';
	buf[1]=0x0;
	buf[2]=0xe;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Write the extended address 0x01D0 to 0xe
	buf[0]='w';
	buf[1]=0x0;
	buf[2]=0x1;
	buf[3]=0xD;
	buf[4]=0x5;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Read 0xD to set address
	buf[0]='r';
	buf[1]=0x0;
	buf[2]=0xd;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Write 0x401f to 0xD
	buf[0]='w';
	buf[1]=0x4;
	buf[2]=0x0;
	buf[3]=0x1;
	buf[4]=0xf;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Read 0xE to set address
	buf[0]='r';
	buf[1]=0x0;
	buf[2]=0xe;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Write inversion bits to 0xE
	buf[0]='w';
	buf[1]=0x0;
	buf[2]=0x0;
	buf[3]=0x0;
	buf[4]=0x3;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Perform a soft reset: bit 15 at 0x1f (CTRL Reg)

	// Read 0x1F to set address
	buf[0]='r';
	buf[1]=0x1;
	buf[2]=0xf;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 3);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	// Write bit 14 (0x8000) (SW reset, not including registers)
	buf[0]='w';
	buf[1]=0x4;
	buf[2]=0x0;
	buf[3]=0x0;
	buf[4]=0x0;
	i2c_write(pXfer, I2C_ECP5_ADDR, buf, 5);
	i2c_read(pXfer,I2C_ECP5_ADDR, buf, 6);

	return(0);

}



/* The assumption is that the PHY is brought up before the FPGA / Switch */
//TODO: Should read the other values, so we don't change them / affect other parts of the register */
int phy_init(i2c_master_transfer_t* pXfer, uint8_t* pBuff) {

	PRINTF("Take PHYs out of reset (x111) \r\n");
	i2c_write(pXfer, I2C_ECP5_ADDR, (uint8_t*) &x_init0, sizeof(x_init0));
	i2c_read(pXfer,  I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);

	/*
	invert_sgmii_polarity(0);
	invert_sgmii_polarity(1);
	*/

	return(0);

}