/*------------------------------------------------------------------------------------------*\
 *   
 *   Copyright (C) 2006 AVM GmbH <fritzbox_info@avm.de>
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation version 2 of the License.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
\*------------------------------------------------------------------------------------------*/
#include <linux/interrupt.h>
#include <linux/timer.h>
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/module.h>
#include <linux/string.h>
#include <linux/proc_fs.h>
#include <linux/delay.h>
#include <asm/uaccess.h>
#include <linux/init.h>
#include <linux/ioctl.h>
#include <linux/skbuff.h>
#include <asm/atomic.h>
#include <asm/mach_avm.h>
#include <linux/avm_led.h>
#if defined(CONFIG_AVM_POWER)
#include <linux/avm_power.h>
#endif /*--- #if defined(CONFIG_AVM_POWER) ---*/

#if !defined(CONFIG_NETCHIP_ADM69961)
#define CONFIG_NETCHIP_ADM69961
#endif
#include <asm/mips-boards/prom.h>
#include <linux/avm_event.h>
#include <linux/avm_cpmac.h>
#include <linux/adm_reg.h>
#include "cpmac_if.h"
#include "cpmac_const.h"
#include "cpmac_debug.h"
#include "cpmac_eth.h"
#include "cpphy_const.h"
#include "cpphy_types.h"
#include "cpphy_mdio.h"
#include "adm6996.h"
#include "cpphy_adm6996.h"
#include "cpphy_mdio.h"

/*------------------------------------------------------------------------------------------*\
 * Register cache
\*------------------------------------------------------------------------------------------*/
#define PRINT_REGISTER(x) DEB_TRC("Register 0x%x = 0x%x\n", x, ADM_GET_EEPROM_REG(mdio, (x)));
#define NUMBER_OF_SERIAL_REGISTERS (0x3c + 1)
/* alle 200ms Register aktualisieren */
#define MAX_SERIAL_REGISTER_AGE (HZ / 4)
struct adm_struct adm_serial_register;
static unsigned int *serial_register = (unsigned int *) &adm_serial_register;
static unsigned long serial_register_timestamp[NUMBER_OF_SERIAL_REGISTERS];
static int adm_serial_registers[] = {
    REG_ADM_LC_ID, REG_ADM_LC_STATUS0, REG_ADM_LC_STATUS2,
    REG_ADM_LC_RXPKTCNT0, REG_ADM_LC_RXPKTCNT1, REG_ADM_LC_RXPKTCNT2, REG_ADM_LC_RXPKTCNT3, REG_ADM_LC_RXPKTCNT4, REG_ADM_LC_RXPKTCNT5,
    REG_ADM_LC_RXPKTBYTECNT0, REG_ADM_LC_RXPKTBYTECNT1, REG_ADM_LC_RXPKTBYTECNT2, REG_ADM_LC_RXPKTBYTECNT3, REG_ADM_LC_RXPKTBYTECNT4, REG_ADM_LC_RXPKTBYTECNT5,
    REG_ADM_LC_TXPKTCNT0, REG_ADM_LC_TXPKTCNT1, REG_ADM_LC_TXPKTCNT2, REG_ADM_LC_TXPKTCNT3, REG_ADM_LC_TXPKTCNT4, REG_ADM_LC_TXPKTCNT5,
    REG_ADM_LC_TXPKTBYTECNT0, REG_ADM_LC_TXPKTBYTECNT1, REG_ADM_LC_TXPKTBYTECNT2, REG_ADM_LC_TXPKTBYTECNT3, REG_ADM_LC_TXPKTBYTECNT4, REG_ADM_LC_TXPKTBYTECNT5,
    REG_ADM_LC_COLLISIONCNT0, REG_ADM_LC_COLLISIONCNT1, REG_ADM_LC_COLLISIONCNT2, REG_ADM_LC_COLLISIONCNT3, REG_ADM_LC_COLLISIONCNT4, REG_ADM_LC_COLLISIONCNT5,
    REG_ADM_LC_ERRCNT0, REG_ADM_LC_ERRCNT1, REG_ADM_LC_ERRCNT2, REG_ADM_LC_ERRCNT3, REG_ADM_LC_ERRCNT4, REG_ADM_LC_ERRCNT5,
    REG_ADM_LC_OVERFLOWFLAG0, REG_ADM_LC_OVERFLOWFLAG2, REG_ADM_LC_OVERFLOWFLAG4,
    -1
};

static const unsigned int adm_port_registers[] = {
    REG_ADM_LC_PORT0_CONF, REG_ADM_LC_PORT1_CONF, REG_ADM_LC_PORT2_CONF,
    REG_ADM_LC_PORT3_CONF, REG_ADM_LC_PORT4_CONF, REG_ADM_LC_PORT5_CONF
};

static const unsigned int adm_port_link_bits[] = {
    ADM_STATUS0_PORT0_LINKUP, ADM_STATUS0_PORT1_LINKUP, ADM_STATUS0_PORT2_LINKUP,
    ADM_STATUS0_PORT3_LINKUP, ADM_STATUS0_PORT4_LINKUP
};

static const unsigned int adm_port_speed_bits[] = {
    ADM_STATUS0_PORT0_SPEED, ADM_STATUS0_PORT1_SPEED, ADM_STATUS0_PORT2_SPEED,
    ADM_STATUS0_PORT3_SPEED, ADM_STATUS0_PORT4_SPEED
};

static const unsigned int adm_vlan_port_map[] = {
    (1 << 0), (1 << 2), (1 << 4), (1 << 6), (1 << 7), (1 << 8)
};

static cpphy_mdio_t *power_mdio;

#define AVM_CPMAC_7170 0
#define AVM_CPMAC_VDSL 1
#define AVM_CPMAC_PROFIVOIP2 2
#define AVM_CPMAC_PROFIVOIP1 3
#define AVM_CPMAC_MAX_PRODUCTS 4


cpmac_switch_configuration_t switch_configuration[AVM_CPMAC_MAX_PRODUCTS][CPMAC_MODE_MAX_NO] = {
    { /* 7170 */
        /* No legal mode        */ { 0, 0xff,
                                        { {"", 0x0}
                                        } 
                                   },
        /* CPMAC_MODE_NORMAL    */ { 1, 0xff,
                                        { {"eth0", 0x2f}  
                                        } 
                                   },
        /* CPMAC_MODE_ATA       */ { 2, 0,
                                        { {"eth0", 0x2e},
                                          {"wan",  0x21}   
                                        } 
                                   },
        /* CPMAC_MODE_SPLIT     */ { 4, 0xff,
                                        { {"eth0", 0x21},
                                          {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28} 
                                        }
                                   },
        /* CPMAC_MODE_SPLIT_ATA */ { 4, 0,
                                        { {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28},
                                          {"wan",  0x21} 
                                        }
                                   },
        /* CPMAC_MODE_ALL_PORTS */ { 1, 0xff,
                                        { {"eth0", 0x3f}  
                                        } 
                                   }
    }, { /* VDSL */
        /* No legal mode        */ { 0, 0xff,
                                        { {"", 0x0}
                                        } 
                                   },
        /* CPMAC_MODE_NORMAL    */ { 2, 4,
                                        { {"eth0", 0x2f},
                                          {"wan",  0x30}   
                                        } 
                                   },
        /* CPMAC_MODE_ATA       */ { 2, 0,
                                        { {"eth0", 0x2e},
                                          {"wan",  0x21}   
                                        } 
                                   },
        /* CPMAC_MODE_SPLIT     */ { 5, 4,
                                        { {"eth0", 0x21},
                                          {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28},
                                          {"wan",  0x30}   
                                        }
                                   },
        /* CPMAC_MODE_SPLIT_ATA */ { 4, 0,
                                        { {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28},
                                          {"wan",  0x21} 
                                        }
                                   },
        /* CPMAC_MODE_ALL_PORTS */ { 1, 0xff,
                                        { {"eth0", 0x3f}  
                                        } 
                                   }
    }, { /* ProfiVoIP 2 CPUs */
        /* No legal mode        */ { 0, 0xff,
                                        { {"", 0x0}
                                        } 
                                   },
        /* CPMAC_MODE_NORMAL    */ { 2, 0xff,
                                        { {"eth0", 0x2f}, 
                                          {"cpu",  0x30}   
                                        } 
                                   },
        /* CPMAC_MODE_ATA       */ { 3, 0,
                                        { {"eth0", 0x2e},
                                          {"wan",  0x21},  
                                          {"cpu",  0x30}   
                                        } 
                                   },
        /* CPMAC_MODE_SPLIT     */ { 5, 0xff,
                                        { {"eth0", 0x21},
                                          {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28},
                                          {"cpu",  0x30}   
                                        }
                                   },
        /* CPMAC_MODE_SPLIT_ATA */ { 5, 0,
                                        { {"eth1", 0x22},
                                          {"eth2", 0x24},
                                          {"eth3", 0x28},
                                          {"wan",  0x21},
                                          {"cpu",  0x30}   
                                        }
                                   },
        /* CPMAC_MODE_ALL_PORTS */ { 1, 0xff,
                                        { {"eth0", 0x3f}  
                                        } 
                                   }
    }, { /* ProfiVoIP 1 CPU */
        /* No legal mode        */ { 0, 0xff,
                                        { {"", 0x0}
                                        } 
                                   },
        /* CPMAC_MODE_NORMAL    */ { 1, 0xff,
                                        { {"eth0", 0x1f}  
                                        } 
                                   },
        /* CPMAC_MODE_ATA       */ { 2, 0,
                                        { {"eth0", 0x1e},
                                          {"wan",  0x11}   
                                        } 
                                   },
        /* CPMAC_MODE_SPLIT     */ { 4, 0xff,
                                        { {"eth0", 0x11},
                                          {"eth1", 0x12},
                                          {"eth2", 0x14},
                                          {"eth3", 0x18} 
                                        }
                                   },
        /* CPMAC_MODE_SPLIT_ATA */ { 4, 0,
                                        { {"eth1", 0x12},
                                          {"eth2", 0x14},
                                          {"eth3", 0x18},
                                          {"wan",  0x11} 
                                        }
                                   },
        /* CPMAC_MODE_ALL_PORTS */ { 1, 0xff,
                                        { {"eth0", 0x3f}  
                                        } 
                                   }
    }
};



/*------------------------------------------------------------------------------------------*\
 * initialize GPIO pins.  output mode, low 
\*------------------------------------------------------------------------------------------*/
static void adm_gpio_init(void) {
    /*--- set pins GPIO, OUTPUT, LOW ---*/
    avm_gpio_ctrl(GPIO_BIT_FSER_CLK, GPIO_PIN, GPIO_OUTPUT_PIN);
    avm_gpio_ctrl(GPIO_BIT_MII_DIO, GPIO_PIN, GPIO_OUTPUT_PIN);
    avm_gpio_ctrl(GPIO_BIT_MII_DCLK, GPIO_PIN, GPIO_OUTPUT_PIN);
    avm_gpio_set_bitmask(GPIO_MASK_MII_DCLK | GPIO_MASK_MII_DIO | GPIO_MASK_FSER_CLK, 0);
}

static void adm_gpio_finit(void) {
    avm_gpio_ctrl(GPIO_BIT_FSER_CLK, FUNCTION_PIN, GPIO_OUTPUT_PIN);
    avm_gpio_ctrl(GPIO_BIT_MII_DIO, FUNCTION_PIN, GPIO_OUTPUT_PIN);
    avm_gpio_ctrl(GPIO_BIT_MII_DCLK, FUNCTION_PIN, GPIO_OUTPUT_PIN);
}

/*------------------------------------------------------------------------------------------*\
 * read one bit from mdio port 
\*------------------------------------------------------------------------------------------*/
static int adm_mdio_readbit(void) {
    return avm_gpio_in_bit(GPIO_BIT_MII_DIO);
}

/*------------------------------------------------------------------------------------------*\
  MDIO mode selection
  0 -> output
  1 -> input
  switch input/output mode of GPIO 0
\*------------------------------------------------------------------------------------------*/
static void adm_mdio_mode(int mode) {
    avm_gpio_ctrl(GPIO_BIT_MII_DIO, GPIO_PIN, (GPIO_DIR)mode);
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void adm_mdc_hi(void) {
    avm_gpio_out_bit(GPIO_BIT_MII_DCLK, 1);
}

static void adm_mdio_hi(void) {
    avm_gpio_out_bit(GPIO_BIT_MII_DIO, 1);
}

static void adm_mdcs_hi(void) {
    avm_gpio_out_bit(GPIO_BIT_FSER_CLK, 1);
}

static void adm_mdc_lo(void) {
    avm_gpio_out_bit(GPIO_BIT_MII_DCLK, 0);
}

static void adm_mdio_lo(void) {
    avm_gpio_out_bit(GPIO_BIT_MII_DIO, 0);
}

static void adm_mdcs_lo(void) {
    avm_gpio_out_bit(GPIO_BIT_FSER_CLK, 0);
}

/*------------------------------------------------------------------------------------------*\
 * mdc pulse
 * 0 -> 1 -> 0
\*------------------------------------------------------------------------------------------*/
static void adm_mdc_pulse(void) {
    adm_mdc_lo();
    udelay(ADM_SW_MDC_DOWN_DELAY);
    adm_mdc_hi();
    udelay(ADM_SW_MDC_UP_DELAY);
    adm_mdc_lo();
}

/*------------------------------------------------------------------------------------------*\
 * mdc toggle
 * 1 -> 0
\*------------------------------------------------------------------------------------------*/
static void adm_mdc_toggle(void) {
    adm_mdc_hi();
    udelay(ADM_SW_MDC_UP_DELAY);
    adm_mdc_lo();
    udelay(ADM_SW_MDC_DOWN_DELAY);
}

/*------------------------------------------------------------------------------------------*\
 * enable eeprom write
 * For ATC 93C66 type EEPROM; accessing ADM6996 internal EEPROM type registers
\*------------------------------------------------------------------------------------------*/
static void adm_eeprom_write_enable(void) {
    unsigned int op;

    adm_mdcs_lo();
    adm_mdc_lo();
    adm_mdio_hi();
    udelay(ADM_SW_CS_DELAY);
    /* enable chip select */
    adm_mdcs_hi();
    udelay(ADM_SW_CS_DELAY);
    /* start bit */
    adm_mdio_hi();
    adm_mdc_pulse();

    /* eeprom write enable */
    op = ADM_SW_BIT_MASK_4;
    while(op) {
        if(op & ADM_SW_EEPROM_WRITE_ENABLE)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    op = ADM_SW_BIT_MASK_1 << (EEPROM_TYPE - 3);
    while(op) {
        adm_mdio_lo();
        adm_mdc_pulse();
        op >>= 1;
    }
    /* disable chip select */
    adm_mdcs_lo();
    udelay(ADM_SW_CS_DELAY);
    adm_mdc_pulse();
}

/*------------------------------------------------------------------------------------------*\
 * disable eeprom write
\*------------------------------------------------------------------------------------------*/
static void adm_eeprom_write_disable(void) {
    unsigned int op;

    adm_mdcs_lo();
    adm_mdc_lo();
    adm_mdio_hi();
    udelay(ADM_SW_CS_DELAY);
    /* enable chip select */
    adm_mdcs_hi();
    udelay(ADM_SW_CS_DELAY);

    /* start bit */
    adm_mdio_hi();
    adm_mdc_pulse();
    /* eeprom write disable */
    op = ADM_SW_BIT_MASK_4;
    while(op) {
        if(op & ADM_SW_EEPROM_WRITE_DISABLE)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    op = ADM_SW_BIT_MASK_1 << (EEPROM_TYPE - 3);
    while(op) {
        adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }
    /* disable chip select */
    adm_mdcs_lo();
    udelay(ADM_SW_CS_DELAY);
    adm_mdc_pulse();
}

/*------------------------------------------------------------------------------------------*\
  read registers from ADM6996
  32 Bit Mode:
    serial registers start at 0x200 (addr bit 9 = 1b)
    EEPROM registers -> shifted to 16bits; Serial registers -> 32bits
  Autodetect with addr == 0x0: (checks for EEPROM Signature)
\*------------------------------------------------------------------------------------------*/
static unsigned int adm_read_adml(unsigned int addr) {
    unsigned int op, dat;

    adm_gpio_init();

    adm_mdcs_hi();
    udelay(ADM_SW_CS_DELAY);

    adm_mdcs_lo();
    adm_mdc_lo();
    adm_mdio_lo();

    udelay(ADM_SW_CS_DELAY);

    /* preamble, 32 bit 1 */
    adm_mdio_hi();
    op = ADM_SW_BIT_MASK_32;
    while(op) {
        adm_mdc_pulse();
        op >>= 1;
    }

    /* command start (01b) */
    /*--- adm_write_bits_pulsed(ADM_SW_SMI_START, 2); ---*/
    op = ADM_SW_BIT_MASK_2;
    while(op) {
        if(op & ADM_SW_SMI_START)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    /* read command (10b) */
    /*--- adm_write_bits_pulsed(ADM_SW_SMI_READ, 2); ---*/
    op = ADM_SW_BIT_MASK_2;
    while(op) {
        if(op & ADM_SW_SMI_READ)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    /* send address A9 ~ A0 */
    /*--- adm_write_bits_pulsed(addr, 10); ---*/
    op = ADM_SW_BIT_MASK_10;
    while(op) {
        if(op & addr)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    /* set MDIO pin to input mode */
    adm_mdio_mode(ADM_SW_MDIO_INPUT);

    /* turnaround bits */
    op = ADM_SW_BIT_MASK_2;
    adm_mdio_hi();
    while(op) {
        adm_mdc_pulse();
        op >>= 1;
    }

    udelay(ADM_SW_MDC_DOWN_DELAY);

    /* start read data */
    dat = 0;
    if(!addr) {
        /* Autodetect: erstmal nur 16 Bit lesen, weil Mode unbekannt ist */
        op = ADM_SW_BIT_MASK_16;
    } else {
        op = ADM_SW_BIT_MASK_32;
    }
    while(op) {
        dat <<= 1;
        if(adm_mdio_readbit()) 
            dat |= 1;
        adm_mdc_toggle();
        op >>= 1;
    }
    if(!addr && (dat != 0x4154)) {
        /* Autodetect: im 32 Bit Mode die restlichen 16 Bit lesen */
        op = ADM_SW_BIT_MASK_16;
        while(op) {
            dat <<= 1;
            if(adm_mdio_readbit())
                dat |= 1;
            adm_mdc_toggle();
            op >>= 1;
        }
    }

    /* set MDIO to output mode */
    adm_mdio_mode(ADM_SW_MDIO_OUTPUT);

    /* dummy clock */
    op = ADM_SW_BIT_MASK_4;
    adm_mdio_lo();
    while(op) {
        adm_mdc_pulse();
        op >>= 1;
    }

    adm_mdc_lo();
    adm_mdio_lo();
    adm_mdcs_hi();

    /* EEPROM registers */
    if(!(addr & ADM_SERIAL_OFFSET))
    {
        if(addr & 1)
            dat >>= 16;
        else
        dat &= 0xffff;
    }
    return dat;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int adm_read_32Bit_cached(cpphy_mdio_t *mdio, unsigned int addr) {
    unsigned int address, data;

    if(!(mdio->Mode & CPPHY_SWITCH_MODE_READ))
        return 0;

    /* Counter Register lesen */
    address = (addr - 0xa0) >> 1;
    if(   (mdio->Mode & CPPHY_SWITCH_MODE_16BIT) /* No caching for 16 bit hardware mdio access */
       || (unsigned int)(jiffies - serial_register_timestamp[address]) > MAX_SERIAL_REGISTER_AGE) {
        /* Invalid cache value of register */
        serial_register_timestamp[address] = jiffies;
        if(mdio->Mode & CPPHY_SWITCH_MODE_16BIT) {
            data = cpphy_mdio_user_access_read(mdio, addr & 0x1f, addr >> 5) +
                  (cpphy_mdio_user_access_read(mdio, (addr + 1) & 0x1f, (addr + 1) >> 5) << 16);
        } else {
            data = adm_read_adml(address + ADM_DEVICE_ADDRESS + ADM_SERIAL_OFFSET);
        }
        serial_register[address] = data;
        return data;
    }
    return serial_register[address];
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int adm_read(cpphy_mdio_t *mdio, unsigned int addr) {
    if(!(mdio->Mode & CPPHY_SWITCH_MODE_READ)) {
        DEB_INFO("adm_read called without being allowed to read!\n");
        return 0;
    }

    if(mdio->Mode & CPPHY_SWITCH_MODE_16BIT) {
        return cpphy_mdio_user_access_read(mdio, addr & 0x1f, addr >> 5);
    } else {
        return adm_read_adml(addr + ADM_DEVICE_ADDRESS);
    }
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int adm_safe_read(cpphy_mdio_t *mdio, unsigned int addr) {
    unsigned int count = 0, first, second;

    first = adm_read(mdio, addr);
    do {
        count++;
        second = adm_read(mdio, addr);
        if(first == second) {
            if(count > 1) {
                DEB_TEST("Value correct for register %#x on read number %u\n", addr, count);
            }
            return first;
        } else {
            DEB_TEST("Addr %#x: %#x != %#x\n", addr, first, second);
        }
        first = second;
    } while(count < 5);
    DEB_ERR("Could not read correct value for register %#x\n", addr);
    return 0;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void adm_update_error_status(cpphy_mdio_t *mdio, unsigned int port) {
    if(!(mdio->Mode & CPPHY_SWITCH_MODE_READ))
        return;

    switch(port) {
        case 0:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT0);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT0);
            break;
        case 1:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT1);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT1);
            break;
        case 2:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT2);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT2);
            break;
        case 3:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT3);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT3);
            break;
        case 4:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT4);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT4);
            break;
        case 5:
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_COLLISIONCNT5);
            ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_ERRCNT5);
            break;
        default:
            DEB_ERR("adm_update_error_status, unhandled port %u\n", port);
            break;
    }
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void adm_update_cache(cpphy_mdio_t *mdio) {
    unsigned int i = 0;

    if(!(mdio->Mode & CPPHY_SWITCH_MODE_READ))
        return;

    for(i = 0; adm_serial_registers[i] != -1; i++) {
        ADM_GET_SERIAL_REG(mdio, adm_serial_registers[i]);
        PRINT_REGISTER(adm_serial_registers[i]);
    }
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static int adm_write_adml(unsigned int addr, unsigned int dat) {
    unsigned int op;

    adm_gpio_init();

    /* enable write */
    adm_eeprom_write_enable();

    /* chip select */
    adm_mdcs_hi();
    udelay(ADM_SW_CS_DELAY);

    /* issue write command */
    /* start bit */
    adm_mdio_hi();
    adm_mdc_pulse();

    /* EEPROM write command */
    op = ADM_SW_BIT_MASK_2;
    while(op) {
        if(op & ADM_SW_EEPROM_WRITE)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_pulse();
        op >>= 1;
    }

    /* send address A7 ~ A0 */
    op = ADM_SW_BIT_MASK_1 << (EEPROM_TYPE - 1);

    while(op) {
        if(op & addr)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_toggle();
        op >>= 1;
    }

    /* start write data */
    op = ADM_SW_BIT_MASK_16;
    while(op) {
        if(op & dat)
            adm_mdio_hi();
        else
            adm_mdio_lo();

        adm_mdc_toggle();
        op >>= 1;
    }

    /* disable cs & wait 1 clock */
    adm_mdcs_lo();
    udelay(ADM_SW_CS_DELAY);
    adm_mdc_toggle();

    adm_eeprom_write_disable();

    return 0;
}


/*------------------------------------------------------------------------------------------*\
 * write register to ADM6996 eeprom registers
\*------------------------------------------------------------------------------------------*/
int adm_write(cpphy_mdio_t *mdio, unsigned int addr, unsigned int dat) {
    if(!(mdio->Mode & CPPHY_SWITCH_MODE_WRITE)) {
        return -1;
    }

    if(mdio->Mode & CPPHY_SWITCH_MODE_16BIT) {
        cpphy_mdio_user_access_write(mdio, addr & 0x1f, addr >> 5, dat);
    } else {
        adm_write_adml(addr + 0x80, dat);
    }
    return 0;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void adm_prepare_reboot(cpphy_mdio_t *mdio) {
    unsigned int i;

    adm_vlan_fbox_reset(mdio);
    ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE, ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE) & 0xffcf);
    mdio->cpmac_priv->enable_vlan = 0;
    mdio->adm_power.roundrobin = 0;
    for(i = 0; i < 5; i++) {
        mdio->adm_power.setup.mode[i] = ADM_PHY_POWER_ON;
    }
    adm_switch_port_power(mdio);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
#if defined(CONFIG_MIPS_OHIO)
static irqreturn_t adm_interrupt(int irq, void *p_param, struct pt_regs *regs) {
    cpphy_mdio_t *mdio = (cpphy_mdio_t *) p_param;

    DEB_TRC("adm_interrupt: %#x\n", ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_IS));
    /* Reset time stamp to ensure that the just changed value is requested */
    serial_register_timestamp[(REG_ADM_LC_STATUS0 - 0xa0) >> 1] = 0;
    adm_check_link(mdio);

    return IRQ_HANDLED;
}
#endif /*--- #if defined(CONFIG_MIPS_OHIO) ---*/


/*------------------------------------------------------------------------------------------*\
 * default VLAN setting
 * port 0~3 as untag port and PVID = 1
 * VLAN1: port 0~3 and port 5 (MII)
\*------------------------------------------------------------------------------------------*/
void adm_init(cpphy_mdio_t *mdio) {
    unsigned int i;
    char *hwrev, *cpu_nr;

    for(i = 0; i < NUMBER_OF_SERIAL_REGISTERS; i++) {
        serial_register[i] = 0;
        serial_register_timestamp[i] = 0;
    }

    mdio->Mode = 0 | CPPHY_SWITCH_MODE_WRITE | CPPHY_SWITCH_MODE_READ; /* 32 Bit, read, write */
    
    if(mdio->is_vdsl) {
        mdio->switch_config = &(switch_configuration[AVM_CPMAC_VDSL][0]);
    } else {
        mdio->switch_config = &(switch_configuration[AVM_CPMAC_7170][0]);
    }
    hwrev = prom_getenv("HWRevision");
    mdio->switch_status.cpu_port = 5;
    if(hwrev && (strstr("103 ", hwrev) || strstr("104 ", hwrev))) {
        char *cmdline = prom_getcmdline();
        cpu_nr = strstr( cmdline, "CPU_NR=" );
        if(cpu_nr == NULL) {
            DEB_WARN("ProfiVoIP: running on first CPU\n");
            mdio->switch_config = switch_configuration[AVM_CPMAC_PROFIVOIP2];
            mdio->switch_status.cpu_port = 5;
        } else {
            DEB_WARN("ProfiVoIP: running on second CPU\n");
            mdio->Mode = 0; /* No access at all! */
            mdio->linked = 1;
            return;
        }
    } else if(hwrev && (   (!(strncmp( "94", hwrev, 2)) && (strlen(hwrev) < 4)) /* First 7170 revision */
                        || (!(strncmp( "95", hwrev, 2)) && (strlen(hwrev) < 4)) /* First 7140 revision */
                        || (!(strncmp("107", hwrev, 3)) && (strlen(hwrev) < 5)) /* First 7140 Annex A revision */
             )         ) {
        DEB_INFO("switch works in read only 32 bit mode\n");
        mdio->Mode &= ~CPPHY_SWITCH_MODE_WRITE;
        adm_gpio_init();
        adm_check_link(mdio);
        return;
    } 
    
    if(hwrev && (   !(strncmp("79", hwrev, 2)) /* 3070 with ADM6996L */
                 || !(strncmp("84", hwrev, 2)) /* 2070 with ADM6996L */
                )
      ) {
        DEB_INFO("switch works in read/write 32 bit mode\n");
        adm_gpio_init();
    } else {
        /* "Normal" switch with 16 bit access and hardware mdio */
        adm_gpio_finit();
        mdio->Mode |= CPPHY_SWITCH_MODE_16BIT;
        DEB_INFO("adm6996 switch is in 16bit Mode\n");
    }

    /* Initialize the power management for the PHYs */
    mdio->adm_power.setup.time_on  = CPMAC_ADM_PHY_PUP_TO;
    mdio->adm_power.setup.time_off = CPMAC_ADM_PHY_PDN_TO;
    for(i = 0; i < 5; i++) {
        mdio->adm_power.setup.mode[i] = ADM_PHY_POWER_ON;
        mdio->adm_power.port_on[i] = 0;
    }
    adm_switch_port_power_config(mdio, &mdio->adm_power.setup, 1);
    /* There is no handle from the power management, therefor use it like this. */
    power_mdio = mdio;
#   if defined(CONFIG_AVM_POWER)
    mdio->power_handle = PowerManagmentRegister("ethernet", adm_power_config);
#   endif /*--- #if defined(CONFIG_AVM_POWER) ---*/

    /* Check, if we can use the interrupt */
    if(hwrev && !(strncmp("94", hwrev, 2)) && (strlen(hwrev) > 2)) {
        /* We need write access to configure the switch */
        if(mdio->Mode | CPPHY_SWITCH_MODE_WRITE) {
#           if defined(CONFIG_MIPS_OHIO)
            if(request_irq(OHIOINT_EXT_1, adm_interrupt, SA_INTERRUPT | SA_SHIRQ, "ADM6996 Driver", mdio)) {
                DEB_ERR("adm_init, failed to register the irq %u\n", OHIOINT_EXT_1);
                return;
            }
#           elif defined(CONFIG_MIPS_AR7)
#           warning todo: cpphy_adm6996 for sangam ???
#           else
#           error unknown CONFIG_...
#           endif /*--- #if defined(CONFIG_MIPS_OHIO) ---*/
            ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_IE, ADM_IE_PStatIE);
            ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE, ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE) | 0x0004);
            /* Make sure, that all values are zero by reading the register */
            ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_IS);
            mdio->cpmac_irq = 1;
        }
    }
    adm_check_link(mdio);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int adm_check_link(cpphy_mdio_t *mdio) {
    unsigned int device, port;
#   if defined(CONFIG_AVM_POWERMETER)
    unsigned int portset = 0, powervalue = 100;
#   endif /*--- #if defined(CONFIG_AVM_POWERMETER) ---*/
   
    if(!(mdio->Mode & CPPHY_SWITCH_MODE_READ))
        return 1;

    mdio->adm_status = ADM_GET_SERIAL_REG(mdio, REG_ADM_LC_STATUS0);
    mdio->linked = mdio->adm_status & (  ADM_STATUS0_PORT0_LINKUP | ADM_STATUS0_PORT1_LINKUP 
                                       | ADM_STATUS0_PORT2_LINKUP | ADM_STATUS0_PORT3_LINKUP 
                                       | ADM_STATUS0_PORT4_LINKUP);
    
    for(port = 0; port < 4; port++) {
        if(mdio->cpmac_priv->led_handle[port] != 0) {
            /*--- DEB_TRC("Set LAN LED %u to '%s'\n", port, (mdio->adm_status & adm_port_link_bits[port]) ? "on" ---*/
                                                                                                     /*--- : "off"); ---*/
            avm_led_action_with_handle(mdio->cpmac_priv->led_handle[port], 
                                       (mdio->adm_status & adm_port_link_bits[port]) ? avm_led_on 
                                                                                     : avm_led_off);
        /*--- } else { ---*/
            /*--- DEB_TRC("Tried to set LAN LED %u, but there is no handle!\n", port); ---*/
        }
    }
#   if defined(CONFIG_AVM_POWERMETER)
    for(port = 0; port < 5; port++) {
        if(mdio->adm_power.port_on) {
            powervalue += 10; /* Port is powered */
            if(mdio->adm_status & adm_port_link_bits[port]) {
                /* Port is linked */
                portset |= (1 << port);
                if(adm_port_speed_bits[port]) /* 100 MBit/s? */
                    powervalue += 57;
                else
                    powervalue += 90;
            }
        }
    }    
    PowerManagmentRessourceInfo(powerdevice_ethernet, PM_ETHERNET_PARAM(portset, powervalue));
#   endif /*--- #if defined(CONFIG_AVM_POWERMETER) ---*/

    for(device = 0; device < mdio->cpmac_priv->devices; device++) {
        unsigned int carrier = netif_carrier_ok(mdio->cpmac_priv->devs[device]);

        if(mdio->adm_status & mdio->cpmac_priv->dev_ports[device]) {
            if(!carrier) {
                netif_carrier_on(mdio->cpmac_priv->devs[device]);
            }
        } else if(carrier) {
            netif_carrier_off(mdio->cpmac_priv->devs[device]);
        }
    }
    cpphy_mdio_event_dataupdate(mdio); /* Linked ports could have changed */

    return mdio->linked;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void adm_switch_port_power(cpphy_mdio_t *mdio) {
    unsigned int port, value, from = 0, to = 4;
    
    /* There is no power saving without write access */
    if(!(mdio->Mode & CPPHY_SWITCH_MODE_WRITE))
        return;

    /* Invalidate link status cache and update it */
    serial_register_timestamp[REG_ADM_LC_STATUS0] = 0;
    adm_check_link(mdio);

    mdio->adm_power.current_on = !mdio->adm_power.current_on;
    if(mdio->adm_power.current_on) {
        mdio->adm_power.timeout = mdio->adm_power.setup.time_on;
    } else {
        mdio->adm_power.timeout = mdio->adm_power.setup.time_off;
    }
    if(mdio->adm_power.roundrobin) {
        if(mdio->adm_power.current_on) {
            mdio->adm_power.current_port =   (mdio->adm_power.current_port == 4) 
                                           ? 0 
                                           : (mdio->adm_power.current_port + 1);
        }
        from = to = mdio->adm_power.current_port;
    }

    for(port = from; port <= to; port++) {
        if(!mdio->adm_power.port_on[port] || !mdio->event_data.port[port].link) {
            unsigned int turn_on = 0;
            value = ADM_PHY_CONTROL_DPLX | ADM_PHY_CONTROL_ANEN | ADM_PHY_CONTROL_SPEED_LSB;
            if(   ((mdio->adm_power.setup.mode[port] == ADM_PHY_POWER_SAVE) && mdio->adm_power.current_on)
               || (mdio->adm_power.setup.mode[port] == ADM_PHY_POWER_ON)) {
                value |= ADM_PHY_CONTROL_RST;
                turn_on = 1;
            } else { /*--- (mdio->adm_power.setup.mode[port] == ADM_PHY_POWER_OFF) ---*/
                value |= ADM_PHY_CONTROL_PDN;
                turn_on = 0;
            }
            if(mdio->adm_power.port_on[port] != turn_on) {
                DEB_TRC("Power %s port %u\n", (value & ADM_PHY_CONTROL_PDN) ? "down" : "up", port);
                mdio->adm_power.port_on[port] = turn_on;
                ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_PHY_CONTROL_PORT0 + (0x20 * port), value);
            }
        }
    }
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t adm_switch_port_power_config(cpphy_mdio_t          *mdio, 
                                         adm_phy_power_setup_t *setup, 
                                         unsigned char          set) {
    unsigned int i;

    if(!set) {
        setup->time_on  = mdio->adm_power.setup.time_on;
        setup->time_off = mdio->adm_power.roundrobin ? (4 * (  mdio->adm_power.setup.time_on
                                                             + mdio->adm_power.setup.time_off))
                                                     : mdio->adm_power.setup.time_off;
        for(i = 0; i < 5; i++) {
            setup->mode[i] = mdio->adm_power.setup.mode[i];
        }
    } else {
        mdio->adm_power.timeout = 0;
        mdio->adm_power.current_on = 0;
        mdio->adm_power.current_port = 0;
    
        mdio->adm_power.roundrobin = ((setup->time_on * 4) < setup->time_off);
        mdio->adm_power.setup.time_on  = setup->time_on;
        mdio->adm_power.setup.time_off =   mdio->adm_power.roundrobin 
                                         ? ((setup->time_off - (4 * setup->time_on)) / 4)
                                         : setup->time_off;
        DEB_TRC("Power setup: %s with %u on and %u off\n", mdio->adm_power.roundrobin ? "roundrobin" : "normal",
                                                           mdio->adm_power.setup.time_on,
                                                           mdio->adm_power.setup.time_off);
        for(i = 0; i < 5; i++) {
            mdio->adm_power.setup.mode[i] = setup->mode[i];
        }
        adm_switch_port_power(mdio);
    }
    return CPMAC_ERR_NOERR;
}


#if defined(CONFIG_AVM_POWER)
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
int adm_power_config(int state) {
    union _powermanagment_ethernet_state pstate;

    pstate.Register = state;
    if((pstate.Bits.port < 0) || (pstate.Bits.port > 4)) {
        DEB_ERR("adm_power_config tried to set mode for port %u\n", pstate.Bits.port);
        return -1;
    }
    power_mdio->adm_power.setup.mode[pstate.Bits.port] = pstate.Bits.status;
    DEB_TRC("adm_power_config sets mode %u for port %u\n", pstate.Bits.status, pstate.Bits.port);
    return 0;
}
#endif /*--- #if defined(CONFIG_AVM_POWER) ---*/


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void adm_vlan_reset(cpphy_mdio_t *mdio) {
    unsigned int group, port;

    for(group = 0; group < AVM_CPMAC_MAX_VLAN_GROUPS; group++) {
        mdio->switch_status.vlan[group].written = 0;
        mdio->switch_status.vlan[group].vid = group + 16;
        mdio->switch_status.vlan[group].target_mask = 0x3F;
        mdio->cpmac_priv->map_vid_in[group] = group;
    }
    for(port = 0; port <= 5; port++) {
        mdio->switch_status.port[port].written = 0;
        mdio->switch_status.port[port].vid = port + 16;
        mdio->switch_status.port[port].keep_tag_outgoing = 0;
        mdio->cpmac_priv->map_port_out[port] = port + 16;
    }
};


/*------------------------------------------------------------------------------------------*\
 * Configure VLAN on the switch                                                             *
\*------------------------------------------------------------------------------------------*/
static void adm_new_vlan_config(cpphy_mdio_t *mdio) {
    unsigned int vlan_mapping, group, port, RegData;

    if(!(mdio->Mode & CPPHY_SWITCH_MODE_WRITE))
        return; /*--- CPMAC_ERR_NO_VLAN_POSSIBLE; ---*/

    for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
        DEB_TRC("Configure port %u with VLAN group %u %stagged %s\n", 
                port,
                mdio->switch_status.port[port].vid,
                mdio->switch_status.port[port].keep_tag_outgoing ? "" : "un",
                mdio->switch_status.port[port].written ? "" : "(new)");

        if(mdio->switch_status.port[port].written)
            continue;
        mdio->switch_status.port[port].written = 1;
        RegData = ADM_GET_EEPROM_REG(mdio, adm_port_registers[port]);
        RegData &= ~(ADM_CONF_OUTPUT_PKT_TAG | ADM_CONF_PORT_VLAN_ID);
        if(mdio->switch_status.port[port].keep_tag_outgoing)
            RegData |= ADM_SW_PORT_TAG;
        RegData |= (mdio->switch_status.port[port].vid & 0xf) << ADM_SW_PORT_PVID_SHIFT;
        ADM_PUT_EEPROM_REG(mdio, adm_port_registers[port], RegData);
        if(port == 5) {
            RegData  = ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_PORT5_PVID) & 0xff00;
            RegData |= (mdio->switch_status.port[port].vid & 0xff0) >> 4;
            ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_PORT5_PVID, RegData);
        } else if(port == 4) {
            RegData  = ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_PORT34_PVID) & 0x00ff;
            RegData |= (mdio->switch_status.port[port].vid & 0xff0) << 4;
            ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_PORT34_PVID, RegData);
        } else {
            RegData  = ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_PORT0_PVID + port) & 0xff00;
            RegData |= (mdio->switch_status.port[port].vid & 0xff0) >> 4;
            ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_PORT0_PVID + port, RegData);
        }
    }

    for(group = 0; group < AVM_CPMAC_MAX_VLAN_GROUPS; group++) {
        DEB_TRC("Configure VLAN group %u: target 0x%x %s\n", 
                mdio->switch_status.vlan[group].vid,
                mdio->switch_status.vlan[group].target_mask,
                mdio->switch_status.vlan[group].written ? "" : "(new)");
              
        if(mdio->switch_status.vlan[group].written)
            continue;
        mdio->switch_status.vlan[group].written = 1;

        vlan_mapping  = mdio->switch_status.vlan[group].target_mask;
        vlan_mapping |= (mdio->switch_status.vlan[group].vid & 0xf) << 12;
        ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_FILTER0_LOW + 2 * (mdio->switch_status.vlan[group].vid & 0xf), vlan_mapping);
        vlan_mapping = 0x8000 | mdio->switch_status.vlan[group].vid;
        ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_FILTER0_HIGH + 2 * (mdio->switch_status.vlan[group].vid & 0xf), vlan_mapping);
    }
}


/*------------------------------------------------------------------------------------------*\
 * Reset the VLAN settings of the switch                                                    *
\*------------------------------------------------------------------------------------------*/
void adm_vlan_fbox_reset(cpphy_mdio_t *mdio) {
    /* switch off VLAN */
    adm_vlan_reset(mdio);
    adm_new_vlan_config(mdio);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
struct net_device *find_or_register_device(char *name,
                                           cpmac_priv_t *cpmac_priv,
                                           unsigned short VID) {
    struct net_device *dev;
    int i;

    for(i = 0; i < AVM_CPMAC_MAX_DEVS; i++) {
        dev = cpmac_priv->olddevs[i];
        if(dev && strcmp(dev->name, name) == 0) {
            DEB_TRC("Device configuration: Update '%s' with VID %u\n", name, VID);
            cpmac_update_device(dev, VID);
            cpmac_priv->olddevs[i] = 0;
            return dev;
        }
    }
    if(strcmp(name, "wan") != 0) {
        DEB_TRC("Device configuration: Connect '%s' with VID %u\n",
                        name, VID);
        return cpmac_register_eth_device(name, cpmac_priv->owner, VID);
    }
    DEB_TRC("Device configuration: Connect '%s' with VID %u (WAN)\n",
                     name, VID);
    return cpmac_register_wan_device(name, cpmac_priv->owner, VID);
} 


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
/* Find unused VLAN ID group */
#define find_unused_vlan() \
    for(vid_group = 0; vid_group < AVM_CPMAC_MAX_VLAN_GROUPS; vid_group++) { \
        if(!(used_vid_mask & (1 << vid_group))) { \
            break; \
        } \
    } \
    if(vid_group == AVM_CPMAC_MAX_VLAN_GROUPS) { \
        DEB_ERR("Error! I need more VLAN groups!\n"); \
        return CPMAC_ERR_NO_VLAN_POSSIBLE; \
    } \
    used_vid_mask |= 1 << vid_group;


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t adm_configure_cpmac(cpphy_mdio_t *mdio, struct avm_cpmac_config_struct *config) {
    unsigned short used_vid_mask = 0;
    unsigned char port, device, vid_group, used_ports = 0;
    unsigned int i;
    struct avm_new_switch_struct *status = &(mdio->switch_status);

    /* Check plausibility */
    if(config->cpmac_mode >= CPMAC_MODE_MAX_NO) {
        DEB_ERR("Unknown cpmac_configuration %u\n", config->cpmac_mode);
        return CPMAC_ERR_NOERR;
    }

    tasklet_disable(&mdio->cpmac_priv->tasklet);

    for(i = 0; i < AVM_CPMAC_MAX_DEVS; i++) {
        mdio->cpmac_priv->olddevs[i] = mdio->cpmac_priv->devs[i];
        mdio->cpmac_priv->devs[i] = 0;
    }

    mdio->cpmac_priv->devices = mdio->switch_config[config->cpmac_mode].number_of_devices;
    DEB_INFO("Configure cpmac to mode %u with %u devices:\n", 
            config->cpmac_mode, mdio->cpmac_priv->devices);
    for(i = 0; i < mdio->cpmac_priv->devices; i++) {
        DEB_INFO("Configure cpmac device '%s' with target_mask 0x%x\n",
                mdio->switch_config[config->cpmac_mode].device[i].name,
                mdio->switch_config[config->cpmac_mode].device[i].target_mask);
    }

    memcpy(&mdio->cpmac_config, config, sizeof(struct avm_cpmac_config_struct));

    adm_vlan_reset(mdio);
    
    /* Reset VID->Port mapping */
    for(vid_group = 0; vid_group < AVM_CPMAC_MAX_VLAN_GROUPS; vid_group++) {
        mdio->cpmac_priv->map_port_in[vid_group] = status->cpu_port;
        mdio->cpmac_priv->map_devoffset_in[vid_group] = 0;
    }

    /* Do we have predefined VLAN IDs? */
    if(config->wan_vlan_number) {
        for(device = 0; device < mdio->cpmac_priv->devices; device++) {
            if(strncmp(mdio->switch_config[config->cpmac_mode].device[device].name, "wan", 3) == 0) {
                for(i = 0; i < config->wan_vlan_number; i++) {
                    unsigned short vid = config->wan_vlan_id[i];

                    used_vid_mask |= 1 << (vid % 0xf);

                    /* Make all ports that belong to the same VLAN group receive the data of the port */
                    for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
                        if(mdio->switch_config[config->cpmac_mode].device[device].target_mask & (1 << port)) {
                            status->vlan[vid & 0xf].vid = vid;
                            status->vlan[vid & 0xf].target_mask = mdio->switch_config[config->cpmac_mode].device[device].target_mask;
                            status->vlan[vid & 0xf].written = 0;
                            if(port != status->cpu_port)
                                mdio->cpmac_priv->map_port_in[vid & 0xf] = port;
                        }
                    }
                }
            }
        }
    }

    /* Determine which ports are used and therefor need a VLAN ID */
    for(device = 0; device < mdio->cpmac_priv->devices; device++) {
        used_ports |= mdio->switch_config[config->cpmac_mode].device[device].target_mask;
    }
    /* Special treatment for fifth port */ /* FIXME */
    if(used_ports & (1 << 4)) {
        mdio->adm_power.setup.mode[4] = ADM_PHY_POWER_ON;
    } else {
        mdio->adm_power.setup.mode[4] = ADM_PHY_POWER_OFF;
    }

    /* Tag incoming data with individual tags and deliver it to all interested ports */
    for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
        /* Do not tag incoming data from the CPU port */
        if(port == status->cpu_port)
            continue;

        /* If port is unused, it needs no VLAN tag */
        if(!(used_ports & (1 << port))) {
            continue;
        }

        /* Find unused VLAN ID group */
        find_unused_vlan();
        status->port[port].vid = 16 + vid_group;
        status->port[port].written = 0;
        mdio->cpmac_priv->map_port_in[vid_group] = port;
        DEB_TRC("Configure: Setting VLAN tagging on port %u with tag %u\n", port, 16 + vid_group);
        /* Make all ports that belong to the same VLAN group receive the data of the port */
        for(device = 0; device < mdio->cpmac_priv->devices; device++) {
            if(mdio->switch_config[config->cpmac_mode].device[device].target_mask & (1 << port)) {
                status->vlan[vid_group].vid = 16 + vid_group;
                status->vlan[vid_group].target_mask = mdio->switch_config[config->cpmac_mode].device[device].target_mask;
                status->vlan[vid_group].written = 0;
            }
        }

        /* Create VLANs for port specific outgoing traffic */
        find_unused_vlan();
        status->vlan[vid_group].vid = 16 + vid_group;
        status->vlan[vid_group].target_mask = (1 << status->cpu_port) | (1 << port);
        status->vlan[vid_group].written = 0;
        mdio->cpmac_priv->map_port_out[port] = status->vlan[vid_group].vid;
        DEB_TRC("Configure: Setting outgoing VLAN ID for port %u to %u\n", port, 16 + vid_group);
    }
    status->port[status->cpu_port].keep_tag_outgoing = 1;
    status->port[status->cpu_port].written = 0;

    /* Map the ports for one device to one common VID on the system side */
    for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
        unsigned short mask = status->vlan[status->port[port].vid & 0xf].target_mask;

        /* Skip unused ports */
        if(!((1 << port) & used_ports))
            continue;
        for(i = 0; i < AVM_CPMAC_MAX_PORTS; i++) {
            if(mask & (1 << i)) { /* First port in this group is used as reference */
                mdio->cpmac_priv->map_vid_in[status->port[port].vid & 0xf] = status->port[i].vid;
                DEB_TRC("Configure: Map incoming VID %u on port %u to VID %u\n",
                        status->port[port].vid, port, mdio->cpmac_priv->map_vid_in[status->port[port].vid & 0xf]);
                break;
            }
        }
    }

    mdio->cpmac_priv->wanport = mdio->switch_config[config->cpmac_mode].wanport;
    mdio->cpmac_priv->wanport_keeptags = 0;
    if(config->wan_vlan_number) {
        status->port[mdio->cpmac_priv->wanport].keep_tag_outgoing = 1;
        mdio->cpmac_priv->wanport_keeptags = 1;
        DEB_TRC("Configure: Keep tags on wanport\n"); 
    }
#   ifdef CONFIG_IP_MULTICAST_FASTFORWARD
    for(vid_group = 0; vid_group < AVM_CPMAC_MAX_VLAN_GROUPS; vid_group++) {
        mcfw_portset *setp = &mdio->cpmac_priv->map_vid_portset[vid_group];
        mcfw_portset_reset(setp);
        for(port = 0; port < 4; port++) {
            if(port == mdio->cpmac_priv->wanport)
                continue;
            if((1 << port) & status->vlan[vid_group].target_mask)
                mcfw_portset_port_add(setp, port);
        }
    }
#   endif
    
    /* Now register the devices with the corresponding VIDs */
    for(device = 0; device < mdio->cpmac_priv->devices; device++) {
        for(vid_group = 0; vid_group < AVM_CPMAC_MAX_VLAN_GROUPS; vid_group++) {
            if(mdio->switch_config[config->cpmac_mode].device[device].target_mask == status->vlan[vid_group].target_mask) {
                for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
                    if(port == status->cpu_port)
                        continue;
                    if((1 << port) & status->vlan[vid_group].target_mask)
                        mdio->cpmac_priv->map_devoffset_in[vid_group] = device;
                }
                if(mdio->cpmac_priv->devs[device])
                    continue;

                mdio->cpmac_priv->devs[device] = find_or_register_device(
                        mdio->switch_config[config->cpmac_mode].device[device].name,
                        mdio->cpmac_priv,
                        status->vlan[vid_group].vid);
                /* Calculate mask for LINKUP bits in status register */
                mdio->cpmac_priv->dev_ports[device] = 0;
                for(port = 0; port < status->cpu_port; port++) {
                    if((1 << port) & mdio->switch_config[config->cpmac_mode].device[device].target_mask) {
                        mdio->cpmac_priv->dev_ports[device] |= adm_port_link_bits[port];
                    }
                }
            }
        }
    }
    for(i = 0; i < AVM_CPMAC_MAX_DEVS; i++) {
        struct net_device *dev = mdio->cpmac_priv->olddevs[i];
        if(dev) {
            DEB_INFO("Unregister %s\n", dev->name);
            mdio->cpmac_priv->olddevs[i] = 0;
            cpmac_unregister_device(dev);
        }
    }
    DEB_TRC("Possible unregister done\n");

    for(vid_group = 0; vid_group < AVM_CPMAC_MAX_VLAN_GROUPS; vid_group++) {
        int devoffset = mdio->cpmac_priv->map_devoffset_in[vid_group];
        struct net_device *dev = mdio->cpmac_priv->devs[devoffset];
        dev = dev; /* Avoid warning, if DEB_TRC is defined to an empty command */
        DEB_TRC("Configure: VID group %u mapped to port %u attached to %s\n",
                vid_group,
                mdio->cpmac_priv->map_port_in[vid_group],
                dev ? dev->name : "????");
    }

    adm_new_vlan_config(mdio);
    /*--- { ---*/
        /*--- unsigned int i; ---*/
        /*--- for(i = 0x01; i < 0x01 + 9; i++) PRINT_REGISTER(i); ---*/
        /*--- for(i = 0x13; i < 0x13 + 16; i++) PRINT_REGISTER(i); ---*/
        /*--- for(i = 0x28; i <= 0x2c; i++) PRINT_REGISTER(i); ---*/
        /*--- for(i = 0x40; i < 0x40 + 32; i++) PRINT_REGISTER(i); ---*/
    /*--- } ---*/

    if(mdio->cpmac_config.cpmac_mode == CPMAC_MODE_ALL_PORTS) {
        /* MAC clone, Port based by-pass mode */
        DEB_TRC("Configure: Disable VLAN\n");
        ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE, ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE) & 0xffcf);
	} else {
        /* MAC clone, 802.1q based VLAN */
        DEB_TRC("Configure: Enable VLAN\n");
        ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE, ADM_GET_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE) | 0x0030);
        /* MAC clone, 802.1q based VLAN, address aging timer 1 second */
        /*--- ADM_PUT_EEPROM_REG(mdio, REG_ADM_LC_VLAN_MODE, 0xff33); ---*/
	}
    mdio->cpmac_priv->enable_vlan = 1;

    adm_check_link(mdio); /* Make sure that the sub devices have the correct link status */
    tasklet_enable(&mdio->cpmac_priv->tasklet);

    return CPMAC_ERR_NOERR;
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t adm_configure_special(cpphy_mdio_t *mdio, cpmac_switch_configuration_t *config) {
    DEB_TRC("adm_configure_special called}\n");
    memcpy((unsigned char *) &mdio->switch_config[CPMAC_MODE_SPECIAL], config, sizeof(cpmac_switch_configuration_t));
    return CPMAC_ERR_NOERR;
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t adm_get_configured(cpphy_mdio_t *mdio, cpmac_switch_configuration_t *config) {
    memcpy(config, &mdio->switch_config[mdio->cpmac_config.cpmac_mode],
				   sizeof(cpmac_switch_configuration_t));
    return CPMAC_ERR_NOERR;
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t adm_get_config_cpmac(cpphy_mdio_t *mdio, struct avm_cpmac_config_struct *config) {
    memcpy(config, &mdio->cpmac_config, sizeof(struct avm_cpmac_config_struct));
    return CPMAC_ERR_NOERR;
}

