C++ REG_PORT函数代码示例

static int mv88e6xxx_set_port_state(struct dsa_switch *ds, int port, u8 state)
{
	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
	int reg, ret;
	u8 oldstate;

	mutex_lock(&ps->smi_mutex);

	reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_CONTROL);
	if (reg < 0)
		goto abort;

	oldstate = reg & PORT_CONTROL_STATE_MASK;
	if (oldstate != state) {
		/* Flush forwarding database if we're moving a port
		 * from Learning or Forwarding state to Disabled or
		 * Blocking or Listening state.
		 */
		if (oldstate >= PORT_CONTROL_STATE_LEARNING &&
		    state <= PORT_CONTROL_STATE_BLOCKING) {
			ret = _mv88e6xxx_flush_fid(ds, ps->fid[port]);
			if (ret)
				goto abort;
		}
		reg = (reg & ~PORT_CONTROL_STATE_MASK) | state;
		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_CONTROL,
					   reg);
	}

abort:
	mutex_unlock(&ps->smi_mutex);
	return ret;
}

static int mv88e6123_61_65_switch_reset(struct dsa_switch *ds)
{
	int i;
	int ret;

	for (i = 0; i < 8; i++) {
		ret = REG_READ(REG_PORT(i), 0x04);
		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
	}

	msleep(2);

	REG_WRITE(REG_GLOBAL, 0x04, 0xc400);

	for (i = 0; i < 1000; i++) {
		ret = REG_READ(REG_GLOBAL, 0x00);
		if ((ret & 0xc800) == 0xc800)
			break;

		msleep(1);
	}
	if (i == 1000)
		return -ETIMEDOUT;

	return 0;
}

static int mv88e6060_switch_reset(struct dsa_switch *ds)
{
	int i;
	int ret;
	unsigned long timeout;

	/* Set all ports to the disabled state. */
	for (i = 0; i < 6; i++) {
		ret = REG_READ(REG_PORT(i), 0x04);
		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
	}

	/* Wait for transmit queues to drain. */
	usleep_range(2000, 4000);

	/* Reset the switch. */
	REG_WRITE(REG_GLOBAL, 0x0a, 0xa130);

	/* Wait up to one second for reset to complete. */
	timeout = jiffies + 1 * HZ;
	while (time_before(jiffies, timeout)) {
		ret = REG_READ(REG_GLOBAL, 0x00);
		if ((ret & 0x8000) == 0x0000)
			break;

		usleep_range(1000, 2000);
	}
	if (time_after(jiffies, timeout))
		return -ETIMEDOUT;

	return 0;
}

static char *mv88e6063_probe(struct mii_bus *bus, int sw_addr)
{
#else
static char *mv88e6063_probe(struct device *host_dev, int sw_addr)
{
	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
#endif
	int ret;

	ret = mdiobus_read(bus, REG_PORT(0), 0x03);
	if (ret >= 0) {
		ret &= 0xfff0;
		if (ret == 0x1530)
			return "Marvell 88E6063";
	}

	return NULL;
}

static int mv88e6063_switch_reset(struct dsa_switch *ds)
{
	int i;
	int ret;

	/*
	 * Set all ports to the disabled state.
	 */
	for (i = 0; i < NUM_PORTS; i++) {
		ret = REG_READ(REG_PORT(i), 0x04);
		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
	}

	/*
	 * Wait for transmit queues to drain.
	 */
	msleep(2);

	/*
	 * Reset the switch.
	 */
	REG_WRITE(REG_GLOBAL, 0x0a, 0xa130);

	/*
	 * Wait up to one second for reset to complete.
	 */
	for (i = 0; i < 1000; i++) {
		ret = REG_READ(REG_GLOBAL, 0x00);
		if ((ret & 0x8000) == 0x0000)
			break;

		msleep(1);
	}
	if (i == 1000)
		return -ETIMEDOUT;

	return 0;
}

VOID _Lcd_Write_Data( UCHAR data_4bit )
{
	if( data_4bit & 1 ) {
		sbi( REG_PORT(D_LCD_D4_PORT_NAME), BIT_PORT(D_LCD_D4_PORT_NAME, D_LCD_D4_BIT) );
	}
	else {
		cbi( REG_PORT(D_LCD_D4_PORT_NAME), BIT_PORT(D_LCD_D4_PORT_NAME, D_LCD_D4_BIT) );
	}
	data_4bit >>= 1;

	if( data_4bit & 1 ) {
		sbi( REG_PORT(D_LCD_D5_PORT_NAME), BIT_PORT(D_LCD_D5_PORT_NAME, D_LCD_D5_BIT) );
	}
	else {
		cbi( REG_PORT(D_LCD_D5_PORT_NAME), BIT_PORT(D_LCD_D5_PORT_NAME, D_LCD_D5_BIT) );
	}
	data_4bit >>= 1;

	if( data_4bit & 1 ) {
		sbi( REG_PORT(D_LCD_D6_PORT_NAME), BIT_PORT(D_LCD_D6_PORT_NAME, D_LCD_D6_BIT) );
	}
	else {
		cbi( REG_PORT(D_LCD_D6_PORT_NAME), BIT_PORT(D_LCD_D6_PORT_NAME, D_LCD_D6_BIT) );
	}
	data_4bit >>= 1;

	if( data_4bit & 1 ) {
		sbi( REG_PORT(D_LCD_D7_PORT_NAME), BIT_PORT(D_LCD_D7_PORT_NAME, D_LCD_D7_BIT) );
	}
	else {
		cbi( REG_PORT(D_LCD_D7_PORT_NAME), BIT_PORT(D_LCD_D7_PORT_NAME, D_LCD_D7_BIT) );
	}
}

static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
					 int nr_stats,
					 struct mv88e6xxx_hw_stat *stats,
					 int port, uint64_t *data)
{
	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
	int ret;
	int i;

	mutex_lock(&ps->stats_mutex);

	ret = mv88e6xxx_stats_snapshot(ds, port);
	if (ret < 0) {
		mutex_unlock(&ps->stats_mutex);
		return;
	}

	/* Read each of the counters. */
	for (i = 0; i < nr_stats; i++) {
		struct mv88e6xxx_hw_stat *s = stats + i;
		u32 low;
		u32 high = 0;

		if (s->reg >= 0x100) {
			int ret;

			ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
						 s->reg - 0x100);
			if (ret < 0)
				goto error;
			low = ret;
			if (s->sizeof_stat == 4) {
				ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
							 s->reg - 0x100 + 1);
				if (ret < 0)
					goto error;
				high = ret;
			}
			data[i] = (((u64)high) << 16) | low;
			continue;
		}
		mv88e6xxx_stats_read(ds, s->reg, &low);
		if (s->sizeof_stat == 8)
			mv88e6xxx_stats_read(ds, s->reg + 1, &high);

		data[i] = (((u64)high) << 32) | low;
	}
error:
	mutex_unlock(&ps->stats_mutex);
}

static char *mv88e6123_61_65_probe(struct device *host_dev, int sw_addr)
{
	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
	int ret;

	if (bus == NULL)
		return NULL;

	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
	if (ret >= 0) {
		if (ret == PORT_SWITCH_ID_6123_A1)
			return "Marvell 88E6123 (A1)";
		if (ret == PORT_SWITCH_ID_6123_A2)
			return "Marvell 88E6123 (A2)";
		if ((ret & 0xfff0) == PORT_SWITCH_ID_6123)
			return "Marvell 88E6123";

		if (ret == PORT_SWITCH_ID_6161_A1)
			return "Marvell 88E6161 (A1)";
		if (ret == PORT_SWITCH_ID_6161_A2)
			return "Marvell 88E6161 (A2)";
		if ((ret & 0xfff0) == PORT_SWITCH_ID_6161)
			return "Marvell 88E6161";

		if (ret == PORT_SWITCH_ID_6165_A1)
			return "Marvell 88E6165 (A1)";
		if (ret == PORT_SWITCH_ID_6165_A2)
			return "Marvell 88e6165 (A2)";
		if ((ret & 0xfff0) == PORT_SWITCH_ID_6165)
			return "Marvell 88E6165";
	}

	return NULL;
}

void mv88e6xxx_poll_link(struct dsa_switch *ds)
{
	int i;

	for (i = 0; i < DSA_MAX_PORTS; i++) {
		struct net_device *dev;
		int uninitialized_var(port_status);
		int link;
		int speed;
		int duplex;
		int fc;

		dev = ds->ports[i];
		if (dev == NULL)
			continue;

		link = 0;
		if (dev->flags & IFF_UP) {
			port_status = mv88e6xxx_reg_read(ds, REG_PORT(i), 0x00);
			if (port_status < 0)
				continue;

			link = !!(port_status & 0x0800);
		}

		if (!link) {
			if (netif_carrier_ok(dev)) {
				netdev_info(dev, "link down\n");
				netif_carrier_off(dev);
			}
			continue;
		}

		switch (port_status & 0x0300) {
		case 0x0000:
			speed = 10;
			break;
		case 0x0100:
			speed = 100;
			break;
		case 0x0200:
			speed = 1000;
			break;
		default:
			speed = -1;
			break;
		}
		duplex = (port_status & 0x0400) ? 1 : 0;
		fc = (port_status & 0x8000) ? 1 : 0;

		if (!netif_carrier_ok(dev)) {
			netdev_info(dev,
				    "link up, %d Mb/s, %s duplex, flow control %sabled\n",
				    speed,
				    duplex ? "full" : "half",
				    fc ? "en" : "dis");
			netif_carrier_on(dev);
		}
	}
}

/*--------------------------------------------------
 *  LCD & Cursor control (Blinking, ON/OFF,...)
 *--------------------------------------------------*/
VOID Lcd_Control( const UCHAR disonoff, const UCHAR curonoff, const UCHAR curblink )
{
	UCHAR	data;
	// DB=0000_1xxx, RS=0, RW=0

	if( !LCD_IS_OPEN() ) {
		return;
	}

	cbi( REG_PORT(D_LCD_RS_PORT_NAME), BIT_PORT(D_LCD_RS_PORT_NAME, D_LCD_RS_BIT) );
	// 1st
	_Lcd_Write_Data( 0 );	// Hi
	_Lcd_ToggleE();
	// 2nd
	data = 0x8;
	if( disonoff == 1 ) {
		data |= 0x04;	//	1:表示on     / 0:表示off
	}
	if( curonoff == 1 ) {
		data |= 0x02;	//	1:カーソルon / 0:カーソルoff
	}
	if( curblink == 1 ) {
		data |= 0x01;	//	1:ブリンクon / 0:ブリンクoff
	}
	_Lcd_Write_Data( data );
	_Lcd_ToggleE();
	_delay_us( LCD_WAIT_FACTOR(40) );	//wait 40us
}

static int mv88e6131_setup(struct dsa_switch *ds)
{
	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
	int i;
	int ret;

	mutex_init(&ps->smi_mutex);
	mv88e6xxx_ppu_state_init(ds);
	mutex_init(&ps->stats_mutex);

	ps->id = REG_READ(REG_PORT(0), 0x03) & 0xfff0;

	ret = mv88e6131_switch_reset(ds);
	if (ret < 0)
		return ret;

	/* @@@ initialise vtu and atu */

	ret = mv88e6131_setup_global(ds);
	if (ret < 0)
		return ret;

	for (i = 0; i < 11; i++) {
		ret = mv88e6131_setup_port(ds, i);
		if (ret < 0)
			return ret;
	}

	return 0;
}

static char *mv88e6123_61_65_probe(struct mii_bus *bus, int sw_addr)
{
	int ret;

	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
	if (ret >= 0) {
		if (ret == 0x1212)
			return "Marvell 88E6123 (A1)";
		if (ret == 0x1213)
			return "Marvell 88E6123 (A2)";
		if ((ret & 0xfff0) == 0x1210)
			return "Marvell 88E6123";

		if (ret == 0x1612)
			return "Marvell 88E6161 (A1)";
		if (ret == 0x1613)
			return "Marvell 88E6161 (A2)";
		if ((ret & 0xfff0) == 0x1610)
			return "Marvell 88E6161";

		if (ret == 0x1652)
			return "Marvell 88E6165 (A1)";
		if (ret == 0x1653)
			return "Marvell 88e6165 (A2)";
		if ((ret & 0xfff0) == 0x1650)
			return "Marvell 88E6165";
	}

	return NULL;
}

static int mv88e6060_setup_port(struct dsa_switch *ds, int p)
{
	int addr = REG_PORT(p);

	/* Do not force flow control, disable Ingress and Egress
	 * Header tagging, disable VLAN tunneling, and set the port
	 * state to Forwarding.  Additionally, if this is the CPU
	 * port, enable Ingress and Egress Trailer tagging mode.
	 */
	REG_WRITE(addr, 0x04, dsa_is_cpu_port(ds, p) ?  0x4103 : 0x0003);

	/* Port based VLAN map: give each port its own address
	 * database, allow the CPU port to talk to each of the 'real'
	 * ports, and allow each of the 'real' ports to only talk to
	 * the CPU port.
	 */
	REG_WRITE(addr, 0x06,
			((p & 0xf) << 12) |
			 (dsa_is_cpu_port(ds, p) ?
				ds->phys_port_mask :
				(1 << ds->dst->cpu_port)));

	/* Port Association Vector: when learning source addresses
	 * of packets, add the address to the address database using
	 * a port bitmap that has only the bit for this port set and
	 * the other bits clear.
	 */
	REG_WRITE(addr, 0x0b, 1 << p);

	return 0;
}

int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
{
	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
	int reg;

	mutex_lock(&ps->phy_mutex);

	reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
	if (reg < 0)
		goto out;

	e->eee_enabled = !!(reg & 0x0200);
	e->tx_lpi_enabled = !!(reg & 0x0100);

	reg = mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_STATUS);
	if (reg < 0)
		goto out;

	e->eee_active = !!(reg & PORT_STATUS_EEE);
	reg = 0;

out:
	mutex_unlock(&ps->phy_mutex);
	return reg;
}

static int mv88e6060_setup_port(struct dsa_switch *ds, int p)
{
	int addr = REG_PORT(p);

	/* Do not force flow control, disable Ingress and Egress
	 * Header tagging, disable VLAN tunneling, and set the port
	 * state to Forwarding.  Additionally, if this is the CPU
	 * port, enable Ingress and Egress Trailer tagging mode.
	 */
	REG_WRITE(addr, PORT_CONTROL,
		  dsa_is_cpu_port(ds, p) ?
			PORT_CONTROL_TRAILER |
			PORT_CONTROL_INGRESS_MODE |
			PORT_CONTROL_STATE_FORWARDING :
			PORT_CONTROL_STATE_FORWARDING);

	/* Port based VLAN map: give each port its own address
	 * database, allow the CPU port to talk to each of the 'real'
	 * ports, and allow each of the 'real' ports to only talk to
	 * the CPU port.
	 */
	REG_WRITE(addr, PORT_VLAN_MAP,
		  ((p & 0xf) << PORT_VLAN_MAP_DBNUM_SHIFT) |
		   (dsa_is_cpu_port(ds, p) ? dsa_user_ports(ds) :
		    BIT(dsa_to_port(ds, p)->cpu_dp->index)));

	/* Port Association Vector: when learning source addresses
	 * of packets, add the address to the address database using
	 * a port bitmap that has only the bit for this port set and
	 * the other bits clear.
	 */
	REG_WRITE(addr, PORT_ASSOC_VECTOR, BIT(p));

	return 0;
}

static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
{
	struct mv88e6xxx_priv_state *ps = (void *)(ds + 1);
	int addr = REG_PORT(p);
	u16 val;

	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
		if (ps->id == ID_6085)
			REG_WRITE(addr, 0x01, 0x003d); 
		else
			REG_WRITE(addr, 0x01, 0x003e); 
	else
		REG_WRITE(addr, 0x01, 0x0003);

	val = 0x0433;
	if (p == dsa_upstream_port(ds)) {
		val |= 0x0104;
		if (ps->id == ID_6085)
			val |= 0x0008;
	}
	if (ds->dsa_port_mask & (1 << p))
		val |= 0x0100;
	REG_WRITE(addr, 0x04, val);

	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);

	val = (p & 0xf) << 12;
	if (dsa_is_cpu_port(ds, p))
		val |= ds->phys_port_mask;
	else
		val |= 1 << dsa_upstream_port(ds);
	REG_WRITE(addr, 0x06, val);

	REG_WRITE(addr, 0x07, 0x0000);

	if (ps->id == ID_6085)
		REG_WRITE(addr, 0x08, 0x0080);
	else {
		val = 0x0080 | dsa_upstream_port(ds);
		if (p == dsa_upstream_port(ds))
			val |= 0x0040;
		REG_WRITE(addr, 0x08, val);
	}

	REG_WRITE(addr, 0x09, 0x0000);

	REG_WRITE(addr, 0x0a, 0x0000);

	REG_WRITE(addr, 0x0b, 1 << p);

	REG_WRITE(addr, 0x18, 0x3210);

	REG_WRITE(addr, 0x19, 0x7654);

	return 0;
}