diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Atan.c linux-broadcom/drivers/net/port_based_qos/Atan.c --- linux-mips-cvs/drivers/net/port_based_qos/Atan.c 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/Atan.c 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,177 @@ +#include "Atan.h" +#include "c47xx.h" + +extern void conf_gpio(int x); + +/*---------------------------------------------------------------------------- +Write specified data to eeprom +entry: *src = pointer to specified data to write + len = number of short(2 bytes) to be written +*/ +void write_eeprom( short RegNumber, unsigned short *data, int len ) +{ + int i2; + unsigned short s_addr, s_data; + unsigned short *src; + + src = data; + SetEEpromToSendState(); +// the write enable(WEN) instruction must be executed before any device +// programming can be done + + s_data = 0x04c0; + SendAddrToEEprom(s_data); //00000001 0011000000B + SetCSToLowForEEprom(); + + s_addr = 0x0500 | (RegNumber & 0x0ff); //00000001 01dddddddd + s_data = *src; + + for (i2 = len; i2 > 0 ; i2 --) + { + SendAddrToEEprom(s_addr); //00000001 01dddddd + SendDataToEEprom(s_data); //dddddddd dddddddd + SetCSToLowForEEprom(); + SetCSToLowForEEprom(); + //WriteWait(); + s_addr ++; + src ++; + s_data = *src; + } +// after all data has been written to EEprom , the write disable(WDS) +// instruction must be executed + SetCSToHighForEEprom(); + s_data = 0x0400; + SendAddrToEEprom(s_data); //00000001 00000000B + SetCSToLowForEEprom(); + SetCSToLowForEEprom(); +} + +void SetEEpromToSendState() +{ + conf_gpio(0x0); + conf_gpio(0x0); + conf_gpio(B_ECS); + conf_gpio(B_ECS); + +// ;cs __-- ,bit 2 +// ;sck ____ ,bit 3 +// ;di ____ ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +void ResetEEpromToSendState() +{ + conf_gpio(0x0); + conf_gpio(0x0); + conf_gpio(0x0); + conf_gpio(0x0); + +// ;cs ____ ,bit 2 +// ;sck ____ ,bit 3 +// ;di ____ ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +void SetCSToLowForEEprom() +{ + conf_gpio(0x0); + conf_gpio(B_ECK); + conf_gpio(B_ECK); + conf_gpio(0x0); + +// ;cs ____ ,bit 2 +// ;sck _--_ ,bit 3 +// ;di ____ ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +void SetCSToHighForEEprom() +{ + conf_gpio(B_ECS); + conf_gpio(B_ECS|B_ECK); + conf_gpio(B_ECS|B_ECK); + conf_gpio(B_ECS); + +// ;cs ---- ,bit 2 +// ;sck _--_ ,bit 3 +// ;di ____ ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +void send_1ToEEprom() +{ + conf_gpio(B_ECS|B_EDI); + conf_gpio(B_ECS|B_ECK|B_EDI); + conf_gpio(B_ECS|B_ECK|B_EDI); + conf_gpio(B_ECS|B_EDI); + +// ;cs ---- ,bit 2 +// ;sck _--_ ,bit 3 +// ;di ---- ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +void send_0ToEEprom() +{ + conf_gpio(B_ECS); + conf_gpio(B_ECS|B_ECK); + conf_gpio(B_ECS|B_ECK); + conf_gpio(B_ECS); + +// ;cs ---- ,bit 2 +// ;sck _--_ ,bit 3 +// ;di ____ ,bit 5 +// ;do ____ ,bit 4 +// ; +} + +#if 0 +void WriteWait() +{ + unsigned int status; + + SetCSToLowForEEprom(); + SetCSToHighForEEprom(); + do { + SetCSToHighForEEprom(); + //status = ReadGPIOData(EDO); + status = gpio & B_EDO; // read EDO bit + } + while (!status); // wait for write - ready + SetCSToLowForEEprom(); +} +#endif + +void SendDataToEEprom(short s_data) +{ + int data_mask; + + for (data_mask = 0x8000; data_mask != 0; ) + { + if (s_data & data_mask) + send_1ToEEprom(); + else + send_0ToEEprom(); + data_mask = data_mask >> 1; + } +} + +void SendAddrToEEprom(short s_data) +{ + int data_mask; + + for (data_mask = 0x0400 ;data_mask != 0; ) + { + if (s_data & data_mask) + send_1ToEEprom(); + else + send_0ToEEprom(); + data_mask = data_mask >> 1; + } +} + diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Atan.h linux-broadcom/drivers/net/port_based_qos/Atan.h --- linux-mips-cvs/drivers/net/port_based_qos/Atan.h 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/Atan.h 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,18 @@ +#ifndef _ATAN_H_ +#define _ATAN_H_ + +#define SINGLECOLOUR 0x1 +#define DUALCOLOUR 0x2 + +void write_eeprom(short,unsigned short *,int); +void SetEEpromToSendState(void); +void ResetEEpromToSendState(void); +void SetCSToLowForEEprom(void); +void SetCSToHighForEEprom(void); +void send_1ToEEprom(void); +void send_0ToEEprom(void); +//void WriteWait(void); +void SendAddrToEEprom(short); +void SendDataToEEprom(short); + +#endif diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Makefile linux-broadcom/drivers/net/port_based_qos/Makefile --- linux-mips-cvs/drivers/net/port_based_qos/Makefile 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/Makefile 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,27 @@ +# Copyright 2001, Cybertan Corporation +# All Rights Reserved. +# +# This is UNPUBLISHED PROPRIETARY SOURCE CODE of Cybertan Corporation; +# the contents of this file may not be disclosed to third parties, copied or +# duplicated in any form, in whole or in part, without the prior written +# permission of Cybertan Corporation. +# +# +# $Id$ +# + +O_TARGET := port_based_qos_mod.o + +PORT_BASED_QOS_MOD_OBJS := port_based_qos.o Atan.o c47xx.o eeprom.o + +export-objs := +obj-y := $(PORT_BASED_QOS_MOD_OBJS) +obj-m := $(O_TARGET) + +SRCBASE := $(TOPDIR)/../.. +EXTRA_CFLAGS += -I$(SRCBASE)/include -Wall -I$(SRCBASE)/ + +vpath %.c $(SRCBASE)/shared + +include $(TOPDIR)/Rules.make + diff -Nur linux-mips-cvs/drivers/net/port_based_qos/c47xx.c linux-broadcom/drivers/net/port_based_qos/c47xx.c --- linux-mips-cvs/drivers/net/port_based_qos/c47xx.c 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/c47xx.c 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,85 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "c47xx.h" +extern uint32 sb_gpioouten(void *sbh, uint32 mask, uint32 val); +extern uint32 sb_gpioout(void *sbh, uint32 mask, uint32 val); +extern uint32 sb_gpioin(void *sbh); +extern uint32 sb_gpiointmask(void *sbh, uint32 mask, uint32 val); + +#define OUTENMASK B_RESET|B_ECS|B_ECK|B_EDI +#define CFGMASK B_ECS|B_ECK|B_EDI +#define BIT(x) (1 << (x)) +#define ASSERT(exp) do {} while (0) + +void +conf_gpio(int x) +{ + ASSERT(sbh); + + /* Enable all of output pins */ + sb_gpioouten(sbh, OUTENMASK, OUTENMASK); + + /* We don't want the B_RESET pin changed, unless + * it tries to set the B_RESET pin. + */ + if (x & B_RESET) + sb_gpioout(sbh, OUTENMASK, x); + else + sb_gpioout(sbh, CFGMASK, x); + +} + +void +gpio_line_set(int x, unsigned int value) +{ + ASSERT(sbh); + + if (value == 1) + sb_gpioout(sbh, BIT(x), BIT(x)); + else if (value == 0) + sb_gpioout(sbh, BIT(x), 0); +} + +void +gpio_line_get(int x, int *value) +{ + ASSERT(sbh); + + *value = (sb_gpioin(sbh) >> x) & 0x1; +} + +void +gpio_line_config_in(int x) +{ + ASSERT(sbh); + + sb_gpioouten(sbh, BIT(x), 0); + sb_gpiointmask(sbh, BIT(x), BIT(x)); +} + +void +gpio_line_config_out(int x) +{ + ASSERT(sbh); + + sb_gpiointmask(sbh, BIT(x), 0); + sb_gpioouten(sbh, BIT(x), BIT(x)); +} + +void +gpio_line_config_out_all(int x) +{ + ASSERT(sbh); + + sb_gpioouten(sbh, OUTENMASK, OUTENMASK); +} diff -Nur linux-mips-cvs/drivers/net/port_based_qos/c47xx.h linux-broadcom/drivers/net/port_based_qos/c47xx.h --- linux-mips-cvs/drivers/net/port_based_qos/c47xx.h 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/c47xx.h 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,23 @@ +#ifndef _C47XX_H_ +#define _C47XX_H_ + +extern void *bcm947xx_sbh; +#define sbh bcm947xx_sbh + +#define GPIO0 0 +#define GPIO1 1 +#define GPIO2 2 +#define GPIO3 3 +#define GPIO4 4 +#define GPIO5 5 +#define GPIO6 6 +#define GPIO7 7 +#define GPIO8 8 + +#define B_RESET 1< + +#define EEDO_PIN 4 +#define EECS_PIN 2 +#define EECK_PIN 3 +#define EEDI_PIN 5 +#define RESET_PIN 0 + +static void SetCSToLowForEEprom(void); +//static void SetCSToHighForEEprom(void); +static void send1ToEEprom(void); +static void send0ToEEprom(void); +static void InitSerialInterface(void); +static void SerialPulse(void); +static void WriteDataToRegister(unsigned short RegNumber, unsigned short data); +void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata,int select_count); +static void WriteDataToEEprom(unsigned short addr, unsigned short data); +static void WriteCmdToEEprom(unsigned short cmd); +extern void gpio_line_set(int x, unsigned int value); +extern void gpio_line_get(int x, int *value); +extern void gpio_line_config_out(int x); +extern void gpio_line_config_in(int x); +extern void gpio_line_config_out_all(void); +// ;cs __-- ,bit 3 +// ;sck ____ ,bit 4 +// ;di ____ ,bit 5 +// ;do ____ ,bit 6 +// ; + +static void SetEEpromToSendState(void) +{ + gpio_line_set(EECS_PIN, 0); + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EEDI_PIN, 1); +// gpio_line_set(EEDO_PIN, 1); /* high impedance */ + + mdelay(1); + gpio_line_set(EECS_PIN, 1); +// gpio_line_set(EEDO_PIN, 1); /* high impedance */ +} + +#if 0 +static void EEpromInit(void) +{ + gpio_line_set(EECS_PIN, 0); + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EEDI_PIN, 1); + gpio_line_set(EEDO_PIN, 1); /* high impedance */ + + mdelay(1); +} + +// ;cs ____ ,bit 3 +// ;sck ____ ,bit 4 +// ;di ____ ,bit 5 +// ;do ____ ,bit 6 +// ; +static void ResetEEpromToSendState(void) +{ + gpio_line_set(EECS_PIN, 0); + gpio_line_set(EEDI_PIN, 0); + //gpio_line_set(EEDO_PIN, 0); + gpio_line_set(EECK_PIN, 0); +} +#endif /* 0 */ + +// ;cs ____ ,bit 3 +// ;sck _--_ ,bit 4 +// ;di ____ ,bit 5 +// ;do ____ ,bit 6 +// ; +static void SetCSToLowForEEprom(void) +{ + /* minimum tcs is 1us */ + gpio_line_set(EECS_PIN, 0); + gpio_line_set(EECS_PIN, 0); + + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 0); + + gpio_line_set(EECS_PIN, 1); + + udelay(10); +} + +#if 0 +// ;cs ---- ,bit 3 +// ;sck _--_ ,bit 4 +// ;di ____ ,bit 5 +// ;do ____ ,bit 6 +// ; +static void SetCSToHighForEEprom(void) +{ + gpio_line_set(EECS_PIN, 1); + + /* min tskh and tskl is 1us */ + gpio_line_set(EECK_PIN, 1); + udelay(2); + gpio_line_set(EECK_PIN, 0); +} +#endif /* 0 */ + +// ;cs ---- ,bit 3 +// ;sck _--_ ,bit 4 +// ;di ---- ,bit 5 +// ;do ____ ,bit 6 +// ; +static void send1ToEEprom(void) +{ +//printf("send1ToEEprom(1)..."); + gpio_line_set(EEDI_PIN, 1); + + gpio_line_set(EECK_PIN, 0); + udelay(1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + udelay(1); + gpio_line_set(EECK_PIN, 0); +} + +// ;cs ---- ,bit 3 +// ;sck _--_ ,bit 4 +// ;di ____ ,bit 5 +// ;do ____ ,bit 6 +// ; +static void send0ToEEprom(void) +{ +//printf("send0ToEEprom(0)..."); + gpio_line_set(EEDI_PIN, 0); + + gpio_line_set(EECK_PIN, 0); + udelay(1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 1); + udelay(1); + gpio_line_set(EECK_PIN, 0); +} + +static void WriteDataToEEprom(unsigned short addr, unsigned short data) +{ + unsigned short addr_mask, data_mask; + + SetEEpromToSendState(); + for (addr_mask = 0x400; addr_mask != 0; ) + { + if (addr & addr_mask) + send1ToEEprom(); + else + send0ToEEprom(); + addr_mask = addr_mask >> 1; + } + for (data_mask = 0x8000; data_mask != 0; ) + { + if (data & data_mask) + send1ToEEprom(); + else + send0ToEEprom(); + data_mask = data_mask >> 1; + } + SetCSToLowForEEprom(); +} + +static void WriteCmdToEEprom(unsigned short cmd) +{ + unsigned short cmd_mask; + + SetEEpromToSendState(); + for (cmd_mask = 0x0400 ;cmd_mask != 0; ) + { + if (cmd & cmd_mask) + send1ToEEprom(); + else + send0ToEEprom(); + cmd_mask = cmd_mask >> 1; + } + SetCSToLowForEEprom(); +} + +/* + * Write data to configure registers through EEPROM interface, even we do not have + * an external EEPROM connectted, ADM6996 got a virtual AT39C66 inside + */ +static void WriteDataToRegister(unsigned short RegNumber, unsigned short data) +{ + unsigned short cmd, addr; + + printk("WriteDataToRegister(RegNumber=0x%x, data=0x%x)\n", RegNumber, data); + +// the write enable(WEN) instruction must be executed before any device +// programming can be done + cmd = 0x04c0; + WriteCmdToEEprom(cmd); //00000001 0011000000B + + addr = 0x0500 | (RegNumber & 0x0ff); //00000001 01dddddddd + WriteDataToEEprom(addr, data); //00000001 01dddddd + + +// after all data has been written to EEprom , the write disable(WDS) +// instruction must be executed + cmd = 0x0400; + WriteCmdToEEprom(cmd); //00000001 00000000B +} + +static void SerialDelay(int count) +{ + udelay(count); +} + +static void InitSerialInterface(void) +{ + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EEDI_PIN, 0); +} + +static void SerialPulse(void) +{ + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EECK_PIN, 1); + SerialDelay(10); + gpio_line_set(EECK_PIN, 1); + gpio_line_set(EECK_PIN, 0); +} +/* + * Since there is no EEPROM is our board, read from EEPROM need to obey the timing alike + * MII interface, EECK = MDC, EEDI = MDIO, please refer to section 4.3 of ADM6996 datasheet + */ +void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata, int select_count) +{ + unsigned short addr_mask, data_mask; + int value, i; + unsigned char StartBits, Opcode, TAbits; + + gpio_line_config_out_all(); + mdelay(1); + /* initialize serial interface */ + gpio_line_set(EECS_PIN, 0); + InitSerialInterface(); + + /* Preamble, 35 bits */ + gpio_line_set(EECK_PIN, 0); + gpio_line_set(EEDI_PIN, 1); + for (i = 0; i < 35; i++) + { + gpio_line_set(EECK_PIN, 1); + SerialDelay(10); + gpio_line_set(EECK_PIN, 0); + SerialDelay(10); + } + + /* Start bits, 2-bit(01b) */ + InitSerialInterface(); + StartBits = 0x01; + for (i = 0; i < 2; i++) + { + value = (StartBits & 2) ? 1 : 0; + gpio_line_set(EEDI_PIN, value); + SerialDelay(1); + SerialPulse(); + StartBits <<= 1; + } + + /* Opcode, read = 10b */ + InitSerialInterface(); + Opcode = 0x02; + for (i = 0; i < 2; i++) + { + value = (Opcode & 0x02) ? 1 : 0; + gpio_line_set(EEDI_PIN, value); + SerialDelay(1); + SerialPulse(); + Opcode <<= 1; + } + + /* 10 bits register address */ + /* 1-bit Table Select, 2-bit Device Address, 7-bit Register Address */ + InitSerialInterface(); + if (select_count) + addr = (addr & 0x7f) | 0x200; + else + addr = addr & 0x7f ; + for (addr_mask = 0x200; addr_mask != 0; addr_mask >>= 1) + { + value = (addr & addr_mask) ? 1 : 0; + gpio_line_set(EEDI_PIN, value); + SerialDelay(1); + SerialPulse(); + } + + /* TA, turnaround 2-bit */ + InitSerialInterface(); + TAbits = 0x02; + gpio_line_config_in(EEDI_PIN); + for (i = 0; i < 2; i++) + { + gpio_line_set(EECK_PIN, 1); + SerialDelay(4); + gpio_line_get(EEDI_PIN, &value); + SerialDelay(4); + TAbits <<= 1; + gpio_line_set(EECK_PIN, 1); + } + + + /* Latch data from serial management EEDI pin */ + *hidata = 0; + gpio_line_set(EECK_PIN, 0); + for (data_mask = 0x8000; data_mask != 0; data_mask >>= 1) + { + SerialDelay(4); + gpio_line_set(EECK_PIN, 1); + gpio_line_get(EEDI_PIN, &value); + if (value) + { + *hidata |= data_mask; + } + gpio_line_set(EECK_PIN, 0); + SerialDelay(4); + } + *lodata = 0; + gpio_line_set(EECK_PIN, 0); + for (data_mask = 0x8000; data_mask != 0; data_mask >>= 1) + { + SerialDelay(4); + gpio_line_set(EECK_PIN, 1); + gpio_line_get(EEDI_PIN, &value); + if (value) + { + *lodata |= data_mask; + } + gpio_line_set(EECK_PIN, 0); + SerialDelay(4); + } + + SerialDelay(2); + + /* Idle, EECK must send at least one clock at idle time */ + SerialPulse(); + gpio_line_set(EECK_PIN, 0); + SerialDelay(10); + gpio_line_set(EECK_PIN, 1); + SerialDelay(10); + gpio_line_set(EECK_PIN, 0); + SerialPulse(); + + gpio_line_config_out(EEDI_PIN); + gpio_line_set(EECS_PIN, 1); + + printk("ReadDataFromRegister(addr=0x%x, hidata=0x%x, lodata=0x%x)\n", addr, *hidata, *lodata); +} diff -Nur linux-mips-cvs/drivers/net/port_based_qos/port_based_qos.c linux-broadcom/drivers/net/port_based_qos/port_based_qos.c --- linux-mips-cvs/drivers/net/port_based_qos/port_based_qos.c 1970-01-01 01:00:00.000000000 +0100 +++ linux-broadcom/drivers/net/port_based_qos/port_based_qos.c 2005-01-31 13:13:14.000000000 +0100 @@ -0,0 +1,460 @@ + /* + * Remaining issues: + * + stats support + * + multicast support + * + media sense + * + half/full duplex + * - random MAC addr. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define MODULE_NAME "port_based_qos_mod" +#define DEVICE_NAME "qos" +#define MODULE_VERSION "0.0.1" + +extern void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata,int select_count); + +#ifdef PERFORMANCE_SUPPORT +static struct ctl_table_header *qos_sysctl_header; +static unsigned long qos[28]; + +static ctl_table mytable[] = { + { 2000, "qos", + qos, sizeof(qos), + 0644, NULL, + proc_dointvec }, + { 0 } +}; + +static unsigned short statis_addr_map[7][6] ={ + {0x04, 0x06, 0x08, 0x0a, 0x0b, 0x0c}, + {0x16, 0x18, 0x1a, 0x1c, 0x1d, 0x1e}, + {0x0d, 0x0f, 0x11, 0x13, 0x14, 0x15}, + {0x1f, 0x21, 0x23, 0x25, 0x26, 0x27}, + {0x31, 0x33, 0x35, 0x37, 0x38, 0x39}, + {0x28, 0x2a, 0x2c, 0x2e, 0x2f, 0x30}, + {0x01, 0x01, 0x01, 0x01, 0x01, 0x01} +}; + +unsigned long get_statistic_from_serial(unsigned short port, unsigned short item) +{ + unsigned short hidata, lodata; + + ReadDataFromRegister(statis_addr_map[item][port], &hidata, &lodata, 1); + return ((hidata << 16) | lodata); +} +#endif + +#ifdef HW_QOS_SUPPORT +struct port_qos_t{ + int addr; + int content_mask; + int *content_set; +}; + +void WriteDataToRegister_(unsigned short reg_idx, unsigned short content_idx); +extern void write_eeprom(short,short *,int); + +#define BANDWIDTH_1_BIT 2 +#define BANDWIDTH_2_BIT 4 +#define BANDWIDTH_3_BIT 6 +#define BANDWIDTH_4_BIT 7 + +#define PORT_CONFIG_1 0x3 +#define PORT_CONFIG_2 0x5 +#define PORT_CONFIG_3 0x7 +#define PORT_CONFIG_4 0x8 +#define BANDWIDTH_CTL_123 0x31 +#define BANDWIDTH_CTL_4 0x32 +#define BANDWIDTH_CTL_ENABLE 0x33 +#define DISCARD_MODE 0x10 +#define TOS_PRIO_MAP 0xf + +#define PRIORITY_MASK 0xfc7f +#define PRIORITY_DISABLE_MASK 0xfc7e +#define FLOW_CTL_MASK 0xfffe +#define RATE_LIMIT_MASK_1 0xff8f +#define RATE_LIMIT_MASK_2 0xf8ff +#define RATE_LIMIT_MASK_3 0x8fff +#define RATE_LIMIT_MASK_4 0xfff8 +#define BANDWIDTH_CTL_MASK 0xff2b +#define DISCARD_MASK 0x0fff + +#define BANDWIDTH_ENABLE_1 1 << BANDWIDTH_1_BIT//04 +#define BANDWIDTH_ENABLE_2 1 << BANDWIDTH_2_BIT//10 +#define BANDWIDTH_ENABLE_3 1 << BANDWIDTH_3_BIT//40 +#define BANDWIDTH_ENABLE_4 1 << BANDWIDTH_4_BIT//80 +#define BANDWIDTH_CTL_MASK_1 0xffff^BANDWIDTH_ENABLE_1//0xfffb +#define BANDWIDTH_CTL_MASK_2 0xffff^BANDWIDTH_ENABLE_2//0xffef +#define BANDWIDTH_CTL_MASK_3 0xffff^BANDWIDTH_ENABLE_3//0xffbf +#define BANDWIDTH_CTL_MASK_4 0xffff^BANDWIDTH_ENABLE_4//0xff7f + +/*static int disable_content[] = {0x0}; +//static int enable_content[] = {0xd4, 0x0cff};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%)*/ +//static int sw_content[] = {0x0,0x0c00};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%) +static int sw_content[] = {0x0,0xc000};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%) +static int port_priority_content[] = {0x080,0x380};//Q0,Q3 +//static int port_priority_content[] = {0x300,0x0};//Q1,Q0 +static int port_flow_ctl_content[] = {0x0,0x1}; +static int port_rate_limit_content_1[] = {0x0,0x00,0x10,0x20,0x30,0x40,0x50,0x60,0x70}; +static int port_rate_limit_content_2[] = {0x0,0x000,0x100,0x200,0x300,0x400,0x500,0x600,0x700}; +static int port_rate_limit_content_3[] = {0x0,0x0000,0x1000,0x2000,0x3000,0x4000,0x5000,0x6000,0x7000}; +static int port_rate_limit_content_4[] = {0x0,0x0,0x1,0x2,0x3,0x4,0x5,0x6,0x7}; +static int port_rate_limit_enable_1[] = {0x0, BANDWIDTH_ENABLE_1}; +static int port_rate_limit_enable_2[] = {0x0, BANDWIDTH_ENABLE_2}; +static int port_rate_limit_enable_3[] = {0x0, BANDWIDTH_ENABLE_3}; +static int port_rate_limit_enable_4[] = {0x0, BANDWIDTH_ENABLE_4}; + +static struct port_qos_t port_mii_disable[] = { + { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK, sw_content}, + //{ DISCARD_MODE, DISCARD_MASK, sw_content}, + { PORT_CONFIG_1, PRIORITY_MASK, sw_content},//port_priority_1 + { PORT_CONFIG_2, PRIORITY_MASK, sw_content},//port_priority_2 + { PORT_CONFIG_3, PRIORITY_MASK, sw_content},//port_priority_3 + { PORT_CONFIG_4, PRIORITY_MASK, sw_content},//port_priority_4 + { PORT_CONFIG_1, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_1 + { PORT_CONFIG_2, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_2 + { PORT_CONFIG_3, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_3 + { PORT_CONFIG_4, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_4 + { -1} +}; + +static struct port_qos_t port_mii_enable[] = { + //{ BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK, enable_content}, + //{ DISCARD_MODE, DISCARD_MASK, sw_content}, + { -1} +}; + +struct port_qos_t *port_mii_sw_array[] = {port_mii_disable, port_mii_enable}; + +/*static struct port_qos_t port_mii_addr[] = { + { PORT_CONFIG_1, PRIORITY_MASK, port_priority_content},//port_priority_1 + { PORT_CONFIG_1, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_1 + //{ "port_frame_type_1", 0x3}, + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_14, port_rate_limit_content_14},//port_rate_limit_1 + { PORT_CONFIG_2, PRIORITY_MASK, port_priority_content},//port_priority_2 + { PORT_CONFIG_2, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_2 + //{ "port_frame_type_2", 0x5}, + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_2, port_rate_limit_content_2},//port_rate_limit_2 + { PORT_CONFIG_3, PRIORITY_MASK, port_priority_content},//port_priority_3 + { PORT_CONFIG_3, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_3 + //{ "port_frame_type_3", 0x7}, + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_3, port_rate_limit_content_3},//port_rate_limit_3 + //{ "port_priority_4", 0x8, 0x380}, + { PORT_CONFIG_4, PRIORITY_MASK, port_priority_content},//port_priority_4 + { PORT_CONFIG_4, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_4 + //{ "port_frame_type_4", 0x8}, + { BANDWIDTH_CTL_4, RATE_LIMIT_MASK_14, port_rate_limit_content_14},//port_rate_limit_4 + { -1} +};*/ + +static struct port_qos_t priority_1[] = { + { PORT_CONFIG_1, PRIORITY_MASK, port_priority_content},//port_priority_1 + { -1} +}; +static struct port_qos_t flow_control_1[] = { + { PORT_CONFIG_1, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_1 + { -1} +}; +static struct port_qos_t rate_limit_1[] = { + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_1, port_rate_limit_content_1},//port_rate_limit_1 + { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_1, port_rate_limit_enable_1},//port_rate_limit_4 + { -1} +}; +static struct port_qos_t priority_2[] = { + { PORT_CONFIG_2, PRIORITY_MASK, port_priority_content},//port_priority_2 + { -1} +}; +static struct port_qos_t flow_control_2[] = { + { PORT_CONFIG_2, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_2 + { -1} +}; +static struct port_qos_t rate_limit_2[] = { + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_2, port_rate_limit_content_2},//port_rate_limit_2 + { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_2, port_rate_limit_enable_2},//port_rate_limit_4 + { -1} +}; +static struct port_qos_t priority_3[] = { + { PORT_CONFIG_3, PRIORITY_MASK, port_priority_content},//port_priority_3 + { -1} +}; +static struct port_qos_t flow_control_3[] = { + { PORT_CONFIG_3, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_3 + { -1} +}; +static struct port_qos_t rate_limit_3[] = { + { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_3, port_rate_limit_content_3},//port_rate_limit_3 + { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_3, port_rate_limit_enable_3},//port_rate_limit_4 + { -1} +}; +static struct port_qos_t priority_4[] = { + { PORT_CONFIG_4, PRIORITY_MASK, port_priority_content},//port_priority_4 + { -1} +}; +static struct port_qos_t flow_control_4[] = { + { PORT_CONFIG_4, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_4 + { -1} +}; +static struct port_qos_t rate_limit_4[] = { + { BANDWIDTH_CTL_4, RATE_LIMIT_MASK_4, port_rate_limit_content_4},//port_rate_limit_4 + { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_4, port_rate_limit_enable_4},//port_rate_limit_4 + { -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, + NULL +}; + +void WriteDataToRegister_(unsigned short reg_idx, unsigned short content_idx) +{ + short RegNumber; + unsigned short data, hidata=0x0, lodata=0x0; + int i; + struct port_qos_t *port_qos = port_mii_addr[reg_idx]; + + //printk("\nWriteDataToRegister_:reg_idx=%d content_idx=%d\n", reg_idx, content_idx); + if (!port_qos) + port_qos = port_mii_sw_array[content_idx]; + + for (i=0; port_qos[i].addr != -1; i++) + { + RegNumber = port_qos[i].addr; + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + + if (!(RegNumber % 2)) /* even port number use lower word */ + hidata = lodata; + + data = (hidata & port_qos[i].content_mask) | (((i > 0) && (content_idx > 1))? port_qos[i].content_set[1] : port_qos[i].content_set[content_idx]); + + write_eeprom(RegNumber, &data, 1); + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + } + ReadDataFromRegister(0xf, &hidata, &lodata, 0); + + /*RegNumber = port_mii_addr[reg_idx].addr; + if (RegNumber == -1)//Disable or Enable + { + struct port_qos_t *port_mii_sw = port_mii_sw_array[content_idx]; + + printk("\nWriteDataToRegister_:reg_idx=%d content_idx=%d\n", reg_idx, content_idx); + for (i=0; port_mii_sw[i].addr != -1; i++) + { + RegNumber = port_mii_sw[i].addr; + + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + + if (!(RegNumber % 2)) + hidata = lodata; + + data = (hidata & port_mii_sw[i].content_mask) | port_mii_sw[i].content_set[i]; + + write_eeprom(RegNumber, &data, 1); + + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + printk("\n============== %s===============\n", (content_idx==0)?"disable":"enable"); + } + } + else + { + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + + if (!(RegNumber % 2)) + hidata = lodata; + + data = (hidata & port_mii_addr[reg_idx].content_mask) | port_mii_addr[reg_idx].content_set[content_idx]; + + write_eeprom(RegNumber, &data, 1); + ReadDataFromRegister(RegNumber, &hidata, &lodata, 0); + }*/ +} +#endif + +static int dev_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd) +{ + struct mii_ioctl_data *data = (struct mii_ioctl_data *)req->ifr_data; +#ifdef PERFORMANCE_SUPPORT + int item, port; + + unsigned long status_item; +#endif + + switch (cmd) + { + case SIOCGMIIPHY: /* Get address of MII PHY in use. */ + case SIOCDEVPRIVATE: /* for binary compat, remove in 2.5 */ + + /* Fall through to the SIOCGMIIREG, taken from eepro100 and rtl + * drivers */ +#ifdef PERFORMANCE_SUPPORT + case SIOCGMIIREG: /* Read MII PHY register. */ + for (item=0; item<6; item++) + for (port=1; port<5; port++){ + qos[(item * 4) + (port-1)] = get_statistic_from_serial(port, item); + } + + status_item = get_statistic_from_serial(0, 6); + + qos[24] = (0x1 & (status_item >> 8)); + qos[25] = (0x1 & (status_item >> 16)); + qos[26] = (0x1 & (status_item >> 24)); + qos[27] = (0x1 & (status_item >> 28)); + + return 0; +#endif + + case SIOCDEVPRIVATE+1: /* for binary compat, remove in 2.5 */ +#ifdef HW_QOS_SUPPORT + case SIOCSMIIREG: /* Write MII PHY register. */ + { + printk("\n x phy_id=%x\n", data->phy_id); + printk("\n x reg_num=%x\n", data->reg_num); + printk("\n x val_in=%x\n", data->val_in); + printk("\n x val_out=%x\n", data->val_out); + + WriteDataToRegister_(data->phy_id, data->val_in); + return 0; + } +#endif + case SIOCDEVPRIVATE+2: /* for binary compat, remove in 2.5 */ + default: + return -EOPNOTSUPP; + } +} + +static int __devinit qos_eth_probe(struct net_device *dev) +{ + + SET_MODULE_OWNER(dev); + + ether_setup(dev); + + strcpy(dev->name, DEVICE_NAME "0"); + + dev->do_ioctl = dev_do_ioctl; + + return 0; +} + +#ifdef HW_QOS_SUPPORT +static char *port_option_name[] = { + "port_priority_1", + "port_flow_control_1", + //{ "port_frame_type_1", + "port_rate_limit_1", + "port_priority_2", + "port_flow_control_2", + //{ "port_frame_type_2", + "port_rate_limit_2", + "port_priority_3", + "port_flow_control_3", + //{ "port_frame_type_3", + "port_rate_limit_3", + "port_priority_4", + //{ "port_priority_4", PORT_CONFIG_4, PRIORITY_MASK, port_priority_content}, + "port_flow_control_4", + //{ "port_frame_type_4", + "port_rate_limit_4", + "QoS", + NULL +}; + +extern char *nvram_get(const char *name); +extern uint bcm_atoi(char *s); + +static int set_port_option(struct net_device *dev, unsigned short port_addr, char *option_content) +{ + struct ifreq ifr; + struct mii_ioctl_data stats; + + stats.phy_id=port_addr; + stats.val_in=bcm_atoi(option_content); + + ifr.ifr_data = (void *)&stats; + + return dev_do_ioctl(dev, &ifr, SIOCSMIIREG); +} + + +void +restore_default_from_NV(struct net_device *dev) +{ + unsigned short i; + char *value = NULL; + + for (i = 0; port_option_name[i]; i++) + { + if((value = nvram_get(port_option_name[i]))) + set_port_option(dev, i, value); + } + return; +} +#endif + +static struct net_device qos_devices; + +/* Module initialization and cleanup */ +int init_module(void) +{ + int res; + struct net_device *dev; + + printk("Initializing " MODULE_NAME " driver " MODULE_VERSION "\n"); + + dev = &qos_devices; + + dev->init = qos_eth_probe; + + if ((res = register_netdev(dev))) + printk("Failed to register netdev. res = %d\n", res); + +#ifdef PERFORMANCE_SUPPORT + qos_sysctl_header = register_sysctl_table(mytable, 0); +#endif +#ifdef HW_QOS_SUPPORT + restore_default_from_NV(dev); + write_eeprom(TOS_PRIO_MAP, &sw_content[0], 1);/* disable TOS priority map*/ +#endif + return 0; +} + +void cleanup_module(void) +{ + struct net_device *dev = &qos_devices; + if (dev->priv != NULL) + { + unregister_netdev(dev); + kfree(dev->priv); + dev->priv = NULL; + } + +#ifdef PERFORMANCE_SUPPORT + unregister_sysctl_table(qos_sysctl_header); +#endif +} +