#include <linux/mii.h>

#define DEVELOPE_ENV

#ifdef DEVELOPE_ENV
#define	ET_5325E(args)	printk args	
#endif

extern char *nvram_get(const char *name);

void qos_ioctl(etc_info_t *etc, struct ifreq *ifr);
void bcm53xx_qos(robo_info_t *robo, uint8 cid);

struct port_qos_t{
	short addr;
	uint16 content_mask;
	int len;
	uint16 *content_set; 
};

#define PAGE_QOS	0x30
#define REG_QOS_CONTROL	0x0
#define REG_QOS_PAUSE	0x13

#define PORT_OFFSET_1	1
#define PORT_OFFSET_2	2
#define PORT_OFFSET_3	3
#define PORT_OFFSET_4	4
#define QOS_ENABLE_BIT	15
#define QOS_TXQ_BIT	10
//#define QOS_TXQ_MODE	0x1	//Two-Queue Mode	
#define QOS_TXQ_MODE	0x3	//Four-Queue Mode	

#define PORT_CONTENT_1	1 << PORT_OFFSET_1	//0x02
#define PORT_CONTENT_2	1 << PORT_OFFSET_2	//0x04
#define PORT_CONTENT_3	1 << PORT_OFFSET_3	//0x08
#define PORT_CONTENT_4	1 << PORT_OFFSET_4	//0x10
#define QOS_ENABLE_CONTENT	((1 << QOS_ENABLE_BIT) | (QOS_TXQ_MODE << QOS_TXQ_BIT))	//0x8400 //0x8C00

#define PORT_MASK_1	0xffff^PORT_CONTENT_1
#define PORT_MASK_2	0xffff^PORT_CONTENT_2
#define PORT_MASK_3	0xffff^PORT_CONTENT_3
#define PORT_MASK_4	0xffff^PORT_CONTENT_4
#define QOS_ENABLE_MASK	0xffff^QOS_ENABLE_CONTENT

static uint16 port_content_1[] = {0x0, PORT_CONTENT_1};
static uint16 port_content_2[] = {0x0, PORT_CONTENT_2};
static uint16 port_content_3[] = {0x0, PORT_CONTENT_3};
static uint16 port_content_4[] = {0x0, PORT_CONTENT_4};
static uint16 qos_enable_content[] = {0x0, QOS_ENABLE_CONTENT};

static struct port_qos_t priority_1[] = {
	{ REG_QOS_CONTROL, PORT_MASK_1, 2, port_content_1},//port_priority_1
	{ -1}
};
static struct port_qos_t flow_control_1[] = {
	{ REG_QOS_PAUSE, PORT_MASK_1, 2, port_content_1},//port_flow_control_1
	{ -1}
};
static struct port_qos_t rate_limit_1[] = {
	{ -1}
};
static struct port_qos_t priority_2[] = {
	{ REG_QOS_CONTROL, PORT_MASK_2, 2, port_content_2},//port_priority_2
	{ -1}
};
static struct port_qos_t flow_control_2[] = {
	{ REG_QOS_PAUSE, PORT_MASK_2, 2, port_content_2},//port_flow_control_2
	{ -1}
};
static struct port_qos_t rate_limit_2[] = {
	{ -1}
};
static struct port_qos_t priority_3[] = {
	{ REG_QOS_CONTROL, PORT_MASK_3, 2, port_content_3},//port_priority_3
	{ -1}
};
static struct port_qos_t flow_control_3[] = {
	{ REG_QOS_PAUSE, PORT_MASK_3, 2, port_content_3},//port_flow_control_3
	{ -1}
};
static struct port_qos_t rate_limit_3[] = {
	{ -1}
};
static struct port_qos_t priority_4[] = {
	{ REG_QOS_CONTROL, PORT_MASK_4, 2, port_content_4},//port_priority_4
	{ -1}
};
static struct port_qos_t flow_control_4[] = {
	{ REG_QOS_PAUSE, PORT_MASK_4, 2, port_content_4},//port_flow_control_4
	{ -1}
};
static struct port_qos_t rate_limit_4[] = {
	{ -1}
};
static struct port_qos_t qos_enable[] = {
	{ REG_QOS_CONTROL, QOS_ENABLE_MASK, 2, qos_enable_content},
	{ -1}
};

static struct port_qos_t *port_mii_addr[] = {
	priority_1,
	flow_control_1,
	rate_limit_1,
	priority_2,
	flow_control_2,
	rate_limit_2,
	priority_3,
	flow_control_3,
	rate_limit_3,
	priority_4,
	flow_control_4,
	rate_limit_4,
	qos_enable,
	NULL
};

static void WriteDataToRegister_(robo_info_t *robo, unsigned short reg_idx, unsigned short content_idx)
{
    uint8 RegNumber;
    uint8 val8;
    uint16 val16;
    uint32 val32;
    void *data;
    int i, len;
    struct port_qos_t *port_qos = port_mii_addr[reg_idx];
    
    for (i=0; port_qos[i].addr != -1; i++)
    {
    	RegNumber = port_qos[i].addr;
    	len =  port_qos[i].len;
	
	switch (len){
	case 1:
		(uint8 *)data = &val8;
		break;
	case 2:
		(uint16 *)data = &val16;
		break;
	case 4:
		(uint32 *)data = &val32;
		break;
	default :
		(uint16 *)data = &val16;
		break;
	}
	
	mii_rreg(robo, 0, PAGE_QOS, RegNumber, data, len);
	ET_5325E(( "1:(0x%02x)\t value=0x%04x\n", RegNumber, *(uint16 *)data));

        *(uint16 *)data = (*(uint16 *)data & port_qos[i].content_mask) | (port_qos[i].content_set[content_idx]);
	
	mii_wreg(robo, 0, PAGE_QOS, RegNumber, data, len);
	mii_rreg(robo, 0, PAGE_QOS, RegNumber, data, len);
	ET_5325E(( "2:(0x%02x)\t value=0x%04x\n", RegNumber, *(uint16 *)data));
    }
}

void
qos_ioctl(etc_info_t *etc, struct ifreq *ifr)
{
	struct bcm4xxx *ch = (struct bcm4xxx *)etc->ch;
	struct mii_ioctl_data *data = (struct mii_ioctl_data *)ifr->ifr_data;
	
	ET_5325E(("phy_id=%x\n", data->phy_id));
	ET_5325E(("val_in=%x\n", data->val_in));
   	
	WriteDataToRegister_(ch->robo, data->phy_id, data->val_in); 
}

static char *port_option_name[] = {
 	"port_priority_1",
	"port_flow_control_1",
	"port_rate_limit_1",
	"port_priority_2",
	"port_flow_control_2",
	"port_rate_limit_2",
	"port_priority_3",
	"port_flow_control_3",
	"port_rate_limit_3",
	"port_priority_4",
	"port_flow_control_4",
	"port_rate_limit_4",
	"QoS",
	NULL
};

void
bcm53xx_qos(robo_info_t *robo, uint8 cid)
{
	unsigned short i;
	char *value = NULL;
	
	for (i = 0; port_option_name[i]; i++)
	{
		if((value = nvram_get(port_option_name[i])))
			WriteDataToRegister_(robo, i, bcm_atoi(value)); 
	}
	return;
}
