#define SPEED_100  15
#define SPEED_200  0
#define SPEED_400  2
#define SPEED_600  4
#define SPEED_800  5
#define SPEED_1000 6
#define SUPPORTS(x) & (1<<x)

uint8_t get_maximum_cht_speed(uint16_t mask)
{
	if(mask SUPPORTS(SPEED_1000))
		return SPEED_1000;
	if(mask SUPPORTS(SPEED_800))
		return SPEED_800;
	if(mask SUPPORTS(SPEED_600))
		return SPEED_600;
	if(mask SUPPORTS(SPEED_400))
		return SPEED_400;
	if(mask SUPPORTS(SPEED_200))
		return SPEED_200;
	if(mask SUPPORTS(SPEED_100))
		return SPEED_100;
}

#if 1
void print_speed(uint8_t speed)
{
	const static char *speeds[]={ "200", "n/a", "400", "n/a", "600", "800", 
				     "1000", "n/a", "n/a", "n/a", "n/a", "n/a",
				     "n/a", "n/a", "n/a", "100" };
	print_debug("Link speed: ");
	print_debug(speeds[speed]);
	print_debug("MHz\r\n");

}
#endif

/*
 * dev_a, dev_b   :   device that share a link
 * link_a, link_b :   link output on device a/b (0,1,2)
 */

void setup_cht_linkspeed(device_t dev_a, device_t dev_b, 
				uint8_t link_a, uint8_t link_b)
{
	uint8_t reg_a, reg_b, speed_max;
	uint16_t speed_mask
	uint32_t tmp;

	reg_a = (0x20*link_a)+0x88;
	reg_b = (0x20*link_b)+0x88;

	/* 
	 * get link speed mask of both devices and AND them
	 * to get the speeds supported by both devices.
	 */

	speed_mask = (pci_read_config32(dev_a, reg_a) >> 16) & \
	               (pci_read_config32(dev_b, reg_b) >> 16);
	
	speed_max=get_maximum_cht_speed(speed_mask);
	
#if 1
	print_speed(speed_max);
#endif

	/* write link width on device a */
	tmp =  pci_read_config32(dev_a, reg_a);
	tmp &= ~(15<<8);
	tmp |= (speed_max<<8);
	pci_write_config32(dev_a, reg_a, tmp);
	
	/* write link width on device b */
	tmp =  pci_read_config32(dev_b, reg_b);
	tmp &= ~(15<<8);
	tmp |= (speed_max<<8);
	pci_write_config32(dev_b, reg_b, tmp);

}