/*
 *       cmds.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.
 *
 *   function: Initialize ECP PCS block
 *
 */

#include <stdio.h>
#include <stdlib.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"


/* set PCS/SERDES block to reset */
void ecp5_pcs_reset(i2c_master_transfer_t* pXfer) {
	// reset our sgmii_cont modules, but leave the PHYs out of reset
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &x_init0, sizeof(x_init0));

	// assert all channel resets
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_reset, sizeof(c_reset));

	// assert all DCU resets
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &u_reset, sizeof(u_reset));
}


int ecp5_pcs_init(i2c_master_transfer_t* pXfer, uint8_t* pBuff) {
	uint8_t errors = 0;


	// clear the I2C buffer of anything in the read FIFO
	i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);

	/*
	 * Check RX and TX status for health
	 * If PLL isn't locked, then we need a reset.
	 */
	PRINTF("Check PCS Status (p) \r\n");
	i2c_write(pXfer,I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
	i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);
	errors = ascii_to_bin(pBuff[2]) << 4 | ascii_to_bin(pBuff[3]);

	if (errors == 0 ) {
		return 0;
	}

	PRINTF("Channel 1, DCU1 Init (u00) \r\n");
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &u_init, sizeof(u_init));
	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);

	PRINTF("Remove SERDES Channel Resets (c555) :\r\n");
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init0, sizeof(c_init0));
	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);

    for (int i=0; i<20; i++) {
            PRINTF("Check PCS Status (p) \r\n");
            i2c_write(pXfer, I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
            i2c_read(pXfer, I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
            dump_data((uint8_t*) pBuff, 8, true);

            if ( pBuff[3] & PLL_LOL_0 ) {
                    PRINTF("Keep Testing Until PLL Lock\r\n");
                    if (i==19) {
                            // give up
                            ecp5_pcs_reset(pXfer);
                            return -1;
                    }
            }
            else {
                    break;
            }
    }


	PRINTF("Remove PCS Channel Resets (c000) :\r\n");
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &c_init1, sizeof(c_init1));
	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);

	PRINTF("Remove SGMII Controller Resets (x000) :\r\n");
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &x_init1, sizeof(x_init1));
	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);

	PRINTF("Check PCS Status (p) \r\n");
	i2c_write(pXfer,  I2C_ECP5_ADDR, (uint8_t*) &pcs_s, sizeof(pcs_s));
	i2c_read(pXfer,   I2C_ECP5_ADDR, (uint8_t*) pBuff, 8);
	dump_data((uint8_t*) pBuff, 8, true);
	errors = ascii_to_bin(pBuff[2]) << 4 | ascii_to_bin(pBuff[3]);

	if ( errors == 0) {
		return 0;
	}
	else {
		// configure resets back to default, so we can try again
		ecp5_pcs_reset(pXfer);
		return -1;
	}

	return 0;
}