/*********************************************************************
 *
 *  Copyright (C) 2004  Freescale Semiconductors, Inc.
 *
 ********************************************************************/
/*********************************************************************
 *  FILE: rio_discovery.c
 *
 *  DESCRIPTION :
 *    Contains a number of functions used to discover the other procesors
 *    within a flexible, but restricted configuration
 *    rio_discovery : determines ID of adjacent device and calls fabric
 *     or point-to-point version of the code
 *    rio_discovery_fabric : version of the discovery for a system with
 *     a TSI568. Configures switch and discovers devices beyond it
 *    rio_discovery_p2p : point-to-point version of the code, discovers
 *     the adjacent endpoint
 *
 *
 * NOTES :
 *    This software was created and test on a single TSI568 based system running
 *    on the host/boot processor, all other processors agents. Also
 *    tested on a configuration with 2 MPC8548 devices communicating
 *    point-to-point over RapidIO. This software only expects to find
 *    TSI568s (no more than one) and MPC8548 devices (no more than 8 total)
 *
 * HISTORY
 * Version    Date     Auth.,Loc.     Comments
 *   0.1    27-Jul-04  LM & CC, EKB   First released version
 *   0.2    14-Apr-05  LM, EKB        Updated for SRIO on MPC8548
 ********************************************************************/

#include <common.h>
#include <syscall.h>
#include <asm-ppc/immap_85xx.h>
#include <asm-ppc/mmu.h>

//#include "rio_regs.h"
#include "rio_bringup.h"


//extern volatile immap_t *local_imm;
extern volatile ccsr_srio_t *local_srio;
extern u32 system_type;
extern u32 nextdevid;
extern u32 hostdeviceid;

int rio_discovery_fabric (void);
int rio_discovery_p2p (void);

int
rio_discovery (void)
{
  u32 didcaradj;
  int result;
  

  /************************************************************
  STEP 1: Identify the adjacent device
  ************************************************************/
  MAINT_READ (0xFF, 0x00, RIO_DIDCAR_OFFSET, didcaradj);

  /************************************************************
  STEP 2:
  If adjacent device is TSI568, execute the fabric version of the
  descovery process.
  If adjacent device is another PQ3, execute the point-to-point
  version of the discovery process
  If the DIDCAR read back does not identify the adjacent device as a
  TSI568 or another MPC8548 then there is a problem. Exit
  *************************************************************/

  if (didcaradj == TSI568_DIDCAR_VALUE) {
    printf ("Identified adjacent device as Tsi568\n");
    system_type = FABRIC_SYSTEM;
    result = rio_discovery_fabric();
  }
  else if (didcaradj == MPC8548_DIDCAR_VALUE) {
    printf ("Identified adjacent device as MPC8548\n");
    system_type = P2P_SYSTEM;
    result = rio_discovery_p2p ();
  }
  else {
    printf
      ("ERROR : Adjacent device is not TSI568 or MPC8548, Read back didcar of 0x%08x \n",
       didcaradj);
       printf("CARRY ON REGARDLESS AS IF TSI568 !!!!!!! \n"); 
       system_type = FABRIC_SYSTEM;
       result = rio_discovery_fabric();

    //printf ("Exiting\n");
    //return UNKNOWN_DEVICE;
  }
  return result;
}

/********************************************************************
A version of the simplified discovery process for a fabric based system
Makes certain assumptions about the nature of that system i.e. TSI568
plus between 1 and 7 additional MPC8548s
*********************************************************************/
int
rio_discovery_fabric (void)
{
  u32 training;
  u32 hbdidl;
  u32 route;
   u32 bdid;
   u32 swport, numport;
  u32 temp;
   u32 errstatus_offset; 
  u32 ctl_offset; 
  u32 portwidth;
  int i,j, portnum;
  int portstep; 

  printf ("Executing the fabric version of the discovery code \n");
  
   

  
  /************************************************************
  STEP 1: Check switch port
  Read back the port of the TSI568 we are attached to.
  /**************************************************/
  MAINT_READ (0xFF, 0x00, TSI568_SW_PORT, swport);
  printf("read back from SW_PORT = 0x%08x \n", swport); 
  numport = swport >> 8;
  swport = swport & BOTTOM_BYTE_ONLY_MASK;
  printf ("The switch has %d ports\n", numport); 
  printf ("This host device is attached to port %d \n", swport);
   

  /********************************************
  STEP 2: Read back the HBDIDL on the switch
  This lock register determines which
  device initialised the device. 0xFFFF indicates
  that the device has not yet been initialised, if the
  HBDIDL is some other value, issue warning but continue
  ********************************************/
  MAINT_READ (0xFF, 0x00, RIO_HOSTLOCK_OFFSET, hbdidl);
  printf ("Original TSI568 HBDIDL =0x%08x \n", hbdidl);
  if (hbdidl != HBDIDLCSR_NOT_LOCKED) {
    printf ("WARNING : HBDIDL of the TSI568 was 0x%08x \n", hbdidl);
  }

  /********************************************
  STEP 3 : Attempt to lock HBDIDL, write
  the device ID of the switch
  ********************************************/
  MAINT_WRITE (0xFF, 0x00, RIO_HOSTLOCK_OFFSET,
               (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID));

  /********************************************
  STEP 4: check that the lock has been accepted
  If the lock has not been accepted, issue warning, but continue
  (dangerous!!!!)
  /********************************************/
  MAINT_READ (0xFF, 0x00, RIO_HOSTLOCK_OFFSET, hbdidl);
  printf ("Updated HBDIDL on TSI568  =0x%08x \n", hbdidl);
  if (hbdidl != (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID)) {
    printf ("WARNING: HBDIDL not updated, Lock not accepted\n");
    printf ("WARINING : This application continues regardless...");
  }

  /***************************************************************
  STEP 5 : route all packets for hostdeviceid back to this port
  ****************************************************************/
  MAINT_WRITE(0xFF, 0x00, RIO_ROUTE_CFG_DESTID, hostdeviceid);
  asm("sync"); 
  MAINT_WRITE(0xFF, 0x00, RIO_ROUTE_CFG_PORT, swport);
  asm("sync"); 
  printf("Routed all packets for deviceID 0x%02x to port %d \n", hostdeviceid, swport); 
  MAINT_READ(hostdeviceid, 0xFF, 0x00, temp); 
  printf("READ BACK THROUGH THE SWITCH HAS BEEN DISABLED FOR NOW....\n");
  
 /* MAINT_WRITE (0xFF, 0x00, RIO_ROUTE_CFG_DESTID, 0);
  asm("sync");
  MAINT_READ (0xFF, 0x00, RIO_ROUTE_CFG_PORT, temp);
  asm("sync");
  printf("Read from 0x%08x = 0x%08x \n",RIO_ROUTE_CFG_PORT, temp); 
  
  
  MAINT_WRITE (0xFF, 0x00, 0x10070, 0);
  asm("sync");
  MAINT_READ (0xFF, 0x00, 0x10074, temp);
  asm("sync");
  printf("Read from 0x10074 = 0x%08x \n", temp); 
  
   MAINT_WRITE (0xFF, 0x00, 0x11070, 0);
   asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11074, temp);
  asm("sync");
  printf("Read from 0x11074 = 0x%08x \n", temp); 

 MAINT_WRITE (0xFF, 0x00, 0x11170, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11174, temp);
  asm("sync");
  printf("Read from 0x11174 = 0x%08x \n", temp); 

 MAINT_WRITE (0xFF, 0x00, 0x11270, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11274, temp);
  asm("sync");
  printf("Read from 0x11274 = 0x%08x \n", temp); 

 MAINT_WRITE (0xFF, 0x00, 0x11370, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11374, temp);
  asm("sync");
  printf("Read from 0x11374 = 0x%08x \n", temp); 
  
   MAINT_WRITE (0xFF, 0x00, 0x11470, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11474, temp);
  asm("sync");
  printf("Read from 0x11474 = 0x%08x \n", temp); 
  
   MAINT_WRITE (0xFF, 0x00, 0x11570, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11574, temp);
  asm("sync");
  printf("Read from 0x11574 = 0x%08x \n", temp); 
  
   MAINT_WRITE (0xFF, 0x00, 0x11670, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11674, temp);
  asm("sync");
  printf("Read from 0x11674 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11770, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11774, temp);
  asm("sync");
  printf("Read from 0x11774 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11870, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11874, temp);
  asm("sync");
  printf("Read from 0x11874 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11270, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11274, temp);
  asm("sync");
  printf("Read from 0x11274 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11a70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11a74, temp);
  asm("sync");
  printf("Read from 0x11a74 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11b70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11b74, temp);
  asm("sync");
  printf("Read from 0x11b74 = 0x%08x \n", temp);
  
   MAINT_WRITE (0xFF, 0x00, 0x11c70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11c74, temp);
  asm("sync");
  printf("Read from 0x11c74 = 0x%08x \n", temp);

 MAINT_WRITE (0xFF, 0x00, 0x11d70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11d74, temp);
  asm("sync");
  printf("Read from 0x11d74 = 0x%08x \n", temp);
   MAINT_WRITE (0xFF, 0x00, 0x11e70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11e74, temp);
  asm("sync");
  printf("Read from 0x11e74 = 0x%08x \n", temp);
   MAINT_WRITE (0xFF, 0x00, 0x11f70, 0);
 asm("sync");
  MAINT_READ (0xFF, 0x00, 0x11f74, temp);
  asm("sync");
  printf("Read from 0x11f74 = 0x%08x \n", temp);
  
  

  
  MAINT_WRITE (0x00, 0x00, 0x11270, 0);
 asm("sync");
  MAINT_WRITE (0x00, 0x00, 0x11274, swport);
  asm("sync");
  printf("updated the port 2 routing specifically to port 0x%x \n", swport); 
    
    
        
   MAINT_WRITE (0x00, 0x00, 0x11270, 0);
 asm("sync");
  MAINT_READ (0x00, 0x00, 0x11274, temp);
  asm("sync");
  printf("Read from 0x11274 = 0x%08x \n", temp); 
 
  MAINT_WRITE (0x00, 0x00, 0x11670, 0);
 asm("sync");
  MAINT_WRITE (0x00, 0x00, 0x11674, swport);
  asm("sync");
  printf("updated the port 6 routing specifically to port 0x%x \n", swport);
  
     
    
   MAINT_WRITE (0x00, 0x00, 0x11670, 0);
 asm("sync");
  MAINT_READ (0x00, 0x00, 0x11674, temp);
  asm("sync");
  printf("Read from 0x11674 = 0x%08x \n", temp); 
  
  
  
  
   MAINT_WRITE (0x00, 0x00, 0x11270, 2);
 asm("sync");
  MAINT_WRITE (0x00, 0x00, 0x11274, swport);
  asm("sync");
  printf("devid 2 updated the port 2 routing specifically to port 0x%x \n", swport); 
    
    
        
   MAINT_WRITE (0x00, 0x00, 0x11270, 2);
 asm("sync");
  MAINT_READ (0x00, 0x00, 0x11274, temp);
  asm("sync");
  printf("Read from 0x11274 = 0x%08x \n", temp); 
 
  MAINT_WRITE (0x00, 0x00, 0x11670, 2);
 asm("sync");
  MAINT_WRITE (0x00, 0x00, 0x11674, swport);
  asm("sync");
  printf("devid 2 updated the port 6 routing specifically to port 0x%x \n", swport);
  
     
    
   MAINT_WRITE (0x00, 0x00, 0x11670, 2);
 asm("sync");
  MAINT_READ (0x00, 0x00, 0x11674, temp);
  asm("sync");
  printf("Read from 0x11674 = 0x%08x \n", temp); 

*/

 
  
  if(temp == MPC8548_DIDCAR_VALUE)
  {
  	printf("Confirmed route back to host through switch, read bdidcsr\n"); 
  }
  else
  {
	printf("ERROR : Problem with switch routing - read back unexpected didcar!\n");  
  }
    
  /***************************************************************
  The remainder of this function is execute for each of the other ports 
  on the TSI568
  ****************************************************************/
  printf("\nExamine each port on the TSI568, 'discover' any devices\n"); 
  errstatus_offset = P0_ERR_STATUS_OFFSET;
  ctl_offset = P0_CTL_OFFSET;
  for (portnum = 0; portnum < numport; ) {
  	printf ("\n\nExamining port %d  ", portnum);
 	 portstep = 1;

    /************************************************************
    STEP 6 : check training on portX
    Examine the training registers on this TSI568 port to determine
    if it has trained to another device.
    *************************************************************/
    	MAINT_READ (0xFF, 0x00, errstatus_offset, training);
        if (training & TSI568_PORTOK_BIT) {
      		printf ("Trained successfully \n");
		MAINT_READ (0xFF, 0x00, ctl_offset, temp);
		temp &= TSI568_INIT_PWIDTH_MASK;
		if(temp == TSI568_INIT_PWIDTH_1L0)
		{
			printf("Trained 1x on Lane 0\n");
		}
		if(temp == TSI568_INIT_PWIDTH_1L2)
		{
			printf("Trained 1x on Lane 2\n");
		}
		if(temp == TSI568_INIT_PWIDTH_4)
		{
			printf("Trained 4x \n");
			portstep = 2; 	
		}
	
	if(portnum == swport)
	{
		printf("This port number == host port number. Discovered myself! \n"); 
	}

	else
	{
      	/*************************************************************
      	STEP 7: route packets for DeviceID 0xFF to this port
      	**************************************************************/
      	MAINT_WRITE (0xFF, 0x00, RIO_ROUTE_CFG_DESTID, 0xFF);
	asm("sync"); 
	MAINT_WRITE (0xFF, 0x00, RIO_ROUTE_CFG_PORT, portnum);
  	printf ("All packets for DeviceID = 0xFF, now to port %d \n", portnum);

      	/********************************************
      	STEP 8: Read back the DIDCAR of the device connected
      	to this port.
      	********************************************/
	printf ("Examine didcar of device attached to port %d \n", portnum);
      	MAINT_READ (0xFF, 0xFF, RIO_DIDCAR_OFFSET, temp);
            	if (temp != MPC8548_DIDCAR_VALUE) {
       	 printf ("WARNING : Unknown device connected to port %d \n", portnum);
      	}
	else
	{
		printf("Found another MPC8548 \n"); 
	}

	if(nextdevid > MAX_DEVICES)
	{
		printf("Found too many devices. Cannot allocate deviceid  \n");
		return 1; 
	}
      	/********************************************
      	STEP 9: Read back the HBDIDL on the device
      	This is the lock register which determines which
      	device can initialise the agent. 0xFFFF indicates
      	that the device has not yet been initialised, if the
      	HBDIDL is some other value, issue warning but continue
      	********************************************/
      	MAINT_READ (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET, hbdidl);
      	printf ("Original HBDIDL =0x%08x \n", hbdidl);
      	if (hbdidl != HBDIDLCSR_NOT_LOCKED) {
      	  printf ("WARNING : HBDIDL of this device was 0x%08x \n", hbdidl);
      	}

      	/********************************************
      	STEP 10 : Attempt to lock HBDIDL, write
      	the device ID of this device
      	********************************************/
      	MAINT_WRITE (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET,
       	            (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID));

      	/********************************************
      	STEP 11: check that the lock has been accepted
      	If the lock has not been accepted, issue warning, but continue
      	(dangerous!!!!)
      	/********************************************/
      	MAINT_READ (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET, hbdidl);
      	printf ("Updated HBDIDL =0x%08x \n", hbdidl);
      	if (hbdidl != (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID)) {
      	  printf ("WARNING: HBDIDL not updated, Lock not accepted\n");
      	  printf ("WARINING : This application continues regardless...");
      	}

      	/********************************************
      	STEP 12: Update the device ID
      	Regardless of the DeviceID which was read back, we will update
      	it with the device ID we want
      	*********************************************/
      	MAINT_READ (0xFF, 0xFF, RIO_BDIDCSR_OFFSET, bdid);
      	bdid = bdid >> SHIFT_BDIDCSR_TO_DEVICEID;
      	printf ("Original deviceID = 0x%08x \n", bdid);

      	MAINT_WRITE (0xFF, 0xFF, RIO_BDIDCSR_OFFSET,
        	           (nextdevid) << SHIFT_BDIDCSR_TO_DEVICEID);
			   
	printf ("Allocated deviceID = 0x%08x \n", nextdevid);	   

      	/*********************************************
      	STEP 13: Update the routing tables with this new information
      	***********************************************/
       	MAINT_WRITE (0xFF, 0x00, RIO_ROUTE_CFG_DESTID, nextdevid);
	MAINT_WRITE (0xFF, 0x00, RIO_ROUTE_CFG_PORT, portnum);
  	printf ("All packets for DeviceID = 0x%02x, now to port %d \n",nextdevid, portnum);


      	/********************************************
      	STEP 14: Read back the BDIDCAR using the
      	updated device ID
      	********************************************/
      	MAINT_READ (nextdevid, 0xFF, RIO_BDIDCSR_OFFSET, bdid);
      	if((bdid >> SHIFT_BDIDCSR_TO_DEVICEID)  == nextdevid)
	{
     		printf ("Comfirmed read operation using new device id \n");
	}
	else
	{
		printf("ERROR : Unexpected value returned from bdid read = 0x%08x \n", bdid);  
	}
    
      	/*********************************************
      	STEP 15: update the next deviceID that the host can allocate 
      	**************************************************/
      	nextdevid++; 
	}
    	}
    	else {

      	/********************************************
      	If not trained, Report port not trained and fill arrays
      	with values indicating not valid.
      	*********************************************/
      	printf ("This port is not trained \n");
   	 }
	 if(portstep == 2)
	 {
	 	portnum++;
		errstatus_offset += TSI568_PORT_STRIDE; 
		ctl_offset += TSI568_PORT_STRIDE;
		portnum++;
		errstatus_offset += TSI568_PORT_STRIDE; 
		ctl_offset += TSI568_PORT_STRIDE; 
	}
	else
	{
		portnum++;
		errstatus_offset += TSI568_PORT_STRIDE; 
		ctl_offset += TSI568_PORT_STRIDE; 

	}

	 }

    printf ("\n");
  

  return SUCCESS;
}

/********************************************************************
A version of the simplified discovery routine which is used for
point-to-point systems
********************************************************************/
int
rio_discovery_p2p (void)
{
  u32 hbdidl;
  u32 bdid;

  printf ("Executing the point-to-point discovery process \n");
  
  /********************************************
  STEP 1: Read back the HBDIDL on the device
  This is the lock register which determines which
  device can initialise the agent. 0xFFFF indicates
  that the device has not yet been initialised, if the
  HBDIDL is some other value, issue warning but continue
  ********************************************/
  MAINT_READ (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET, hbdidl);
  printf ("Original HBDIDL =0x%08x \n", hbdidl);
  if (hbdidl != 0xFFFF) {
    printf ("WARNING : HBDIDL of this device was  0x%08x \n", hbdidl);
  }

  /********************************************
  STEP 2 : Attempt to lock HBDIDL, write
  the device ID of this device
  ********************************************/
  MAINT_WRITE (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET,
               (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID));

  /********************************************
  STEP 3: check that the lock has been accepted
  If the lock has not been accepted, issue warning, but continue
  (dangerous!!!!)
  /********************************************/
  MAINT_READ (0xFF, 0xFF, RIO_HOSTLOCK_OFFSET, hbdidl);
  printf ("Updated HBDIDL =0x%08x \n", hbdidl);
  if (hbdidl != (local_srio->bdidcsr >> SHIFT_BDIDCSR_TO_DEVICEID)) {
    printf ("WARNING: HBDIDL not updated, Lock not accepted\n");
    printf ("WARINING : This application continues regardless...");
  }

  /********************************************
  STEP 4: Update the device ID
  Regardless of the DeviceID which was read back, we will update
  it with the device ID = 0x02
  *********************************************/
  MAINT_READ (0xFF, 0xFF, RIO_BDIDCSR_OFFSET, bdid);
  bdid = bdid >> SHIFT_BDIDCSR_TO_DEVICEID;
  printf ("Original deviceID = 0x%08x \n", bdid);
  printf ("nextdevid = 0x%08x \n", nextdevid);

  MAINT_WRITE (0xFF, 0xFF, RIO_BDIDCSR_OFFSET,
               (nextdevid << SHIFT_BDIDCSR_TO_DEVICEID));

  /********************************************
  STEP 5: Read back the DIDCAR using the
  updated device ID
  ********************************************/
  /*rapncsg*/
  MAINT_READ (nextdevid, 0xFF, RIO_BDIDCSR_OFFSET, bdid);
  bdid = bdid >> SHIFT_BDIDCSR_TO_DEVICEID;
  printf ("Updated deviceID  = 0x%08x \n", bdid);

  printf ("\n");
  
  nextdevid++;

  return SUCCESS;

}
