/*------------------------------------------------------------------------------------------*\
 *   Copyright (C) 2008,2009,2010 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>
#if defined(CONFIG_AVM_POWER)
#include <linux/avm_power.h>
#endif /*--- #if defined(CONFIG_AVM_POWER) ---*/

#if !defined(CONFIG_NETCHIP_AR8216)
#define CONFIG_NETCHIP_AR8216
#endif
#include <asm/mips-boards/prom.h>
#include <asm/mach-ur8/hw_mdio.h>
#include <linux/avm_event.h>
#include <linux/avm_cpmac.h>
#include <linux/ar_reg.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 "cpphy_adm6996.h"
#include "cpphy_ar8216.h"
#include "cpphy_mgmt.h"

/*------------------------------------------------------------------------------------------*\
 * The configuration is defined in cpphy_adm6996.c                                          *
\*------------------------------------------------------------------------------------------*/
extern cpmac_switch_configuration_t switch_configuration[CPMAC_SWITCH_CONF_MAX_PRODUCTS][CPMAC_MODE_MAX_NO];

/*------------------------------------------------------------------------------------------*\
 * die MDIO-Register des AR8216 sind 32bit gross, MDIO kann nur 16bit Zugriffe, d.h.
 * es muessen 2 Zugriffe stattfinden
 * zuerst wird die Registerpage ausgewaehlt, da die Registeradressen des AR8216 groesser als
 * der MDIO-Adressraum ist
\*------------------------------------------------------------------------------------------*/
unsigned int ar8216_mdio_read32(cpphy_mdio_t *mdio, unsigned int regadr) {
    unsigned int tmp0, tmp1, register_address, phy_address;

    register_address = (regadr & 0xFFFFFFFC) >> 1;
    phy_address = 0x10 | ((register_address >> 5) & 0x7);

    cpphy_mdio_user_access_write(mdio, 0, 0x18, (register_address >> 8) & 0x1FF);
    tmp0 = cpphy_mdio_user_access_read(mdio, register_address & 0x1F, phy_address);
    register_address++;
    tmp1 = cpphy_mdio_user_access_read(mdio, register_address & 0x1F, phy_address);
    return (tmp0 | (tmp1 << 16));
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar8216_mdio_read16(cpphy_mdio_t *mdio, unsigned int regadr) {
    unsigned int register_address, phy_address;

    register_address = (regadr & 0xFFFFFFFC) >> 1;
    phy_address = 0x10 | ((register_address >> 5) & 0x7);

    return cpphy_mdio_user_access_read(mdio, register_address & 0x1F, phy_address);
}


/*------------------------------------------------------------------------------------------*\
 * zuerst die oberen 16bit schreiben
\*------------------------------------------------------------------------------------------*/
void ar8216_mdio_write32(cpphy_mdio_t *mdio, unsigned int regadr, unsigned int data) {
    unsigned int register_address, phy_address;

    register_address = (regadr & 0xFFFFFFFC) >> 1;
    phy_address = 0x10 | ((register_address >> 5) & 0x7);

    cpphy_mdio_user_access_write(mdio, 0, 0x18, (register_address >> 8) & 0x1FF);
    register_address++;
    cpphy_mdio_user_access_write(mdio, register_address & 0x1F, phy_address, (data >> 16) & 0xFFFF );
    register_address--;
    cpphy_mdio_user_access_write(mdio, register_address & 0x1F, phy_address, data & 0xFFFF);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar8216_mdio_write16(cpphy_mdio_t *mdio, unsigned int regadr, unsigned int data) {
    unsigned int register_address, phy_address;

    register_address = (regadr & 0xFFFFFFFC) >> 1;
    phy_address = 0x10 | ((register_address >> 5) & 0x7);

    cpphy_mdio_user_access_write(mdio, register_address & 0x1F, phy_address, data & 0xFFFF);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void display_ath_regs(cpphy_mdio_t *mdio, enum ath_display_regs control) {
    int regs[] = {0, 0x10, 0x14, 0x20, 0x24, 0x30, 0x40, 0x44, 0x50, 0x54, 0x58, 0x5C, 0x60, 0x64, 0x68, 0x6C, 0x70, 0x78, 0x80, 0x94, 0x98, -1}; 
    int i = 0, port, control_reg;

    switch(control) {
        case ATH_PORT_CONTROL:
            DEB_TEST("Port control registers\n");
            DEB_TEST("          CPU port    Port 1    Port 2    Port 3    Port 4    Port 5\n");
            for(control_reg = 0; control_reg < 0x14; control_reg += 4) {
                DEB_TEST("  (0x%02x)  %8x  %8x  %8x  %8x  %8x  %8x\n",
                         control_reg,
                         ar8216_mdio_read32(mdio, 0x100 + control_reg),
                         ar8216_mdio_read32(mdio, 0x200 + control_reg),
                         ar8216_mdio_read32(mdio, 0x300 + control_reg),
                         ar8216_mdio_read32(mdio, 0x400 + control_reg),
                         ar8216_mdio_read32(mdio, 0x500 + control_reg),
                         ar8216_mdio_read32(mdio, 0x600 + control_reg));
            }
            break;
        case ATH_CONTROL:
            DEB_TEST("Global control registers\n");
            while (regs[i] != -1) {
                DEB_TEST("  0x%2.x  0x%8.x\n", regs[i], ar8216_mdio_read32(mdio, regs[i]));
                i++;
            }
            break;
        case ATH_MII:
            DEB_TEST("MII registers\n");
            DEB_TEST("                      Port 1    Port 2    Port 3    Port 4    Port 5\n");
            for(i = 0; i <= 0x1D; i++) {
                if(((i > 6) && (i < 0x10)) || ((i > 0x19) && (i < 0x1C)) || (i == 0x17))
                    continue;
                DEB_TEST("  (0x%02x)            %8.x  %8.x  %8.x  %8.x  %8.x\n", 
                         i,
                         cpphy_mdio_user_access_read(mdio, i, 0),
                         cpphy_mdio_user_access_read(mdio, i, 1),
                         cpphy_mdio_user_access_read(mdio, i, 2),
                         cpphy_mdio_user_access_read(mdio, i, 3),
                         cpphy_mdio_user_access_read(mdio, i, 4)
                        );
            }
            DEB_TEST("MDIO Debug registers\n");
            DEB_TEST("                      Port 1    Port 2    Port 3    Port 4    Port 5\n");
            for(i = 0; i < 0x13; i++) {
                if(i == 0x11)
                    continue;
                for(port = 0; port < 5; port++) {
                    cpphy_mdio_user_access_write(mdio, 0x1d, port, i);
                }
                DEB_TEST("  (0x%02x)            %8.x  %8.x  %8.x  %8.x  %8.x\n", 
                         i,
                         cpphy_mdio_user_access_read(mdio, 0x1e, 0),
                         cpphy_mdio_user_access_read(mdio, 0x1e, 1),
                         cpphy_mdio_user_access_read(mdio, 0x1e, 2),
                         cpphy_mdio_user_access_read(mdio, 0x1e, 3),
                         cpphy_mdio_user_access_read(mdio, 0x1e, 4)
                        );
                if(i == 3)
                    i = 0x10 - 1;
            }
              break;
    }
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar8216_softreset(cpphy_mdio_t *mdio) {

    unsigned int tmp = (1<<31);

    DEB_TRC("[ar8216_softreset] softreset switch\n");
    ar8216_mdio_write32(mdio, AR8216_GLOBAL_DEVICE_ID, tmp);
    mdelay(200);

    while (tmp & (1<<31)) {
        tmp = ar8216_mdio_read32(mdio, AR8216_GLOBAL_DEVICE_ID);
    }

}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar8216_read_address_resolution_table(cpphy_mdio_t *mdio) {
    int this;
    int count = 0;
    int status, status1;

    status = ar8216_mdio_read16(mdio, 0x15);
    DEB_TRC("[ar8216_read_address_resolution_table] rxerrors %d\n", status);
    status1 = 0;

    for(  ; ; ) {
        do {
            status = ar8216_mdio_read32(mdio, AR8216_GLOBAL_AR_1) & AR_1_AT_BUSY;
            if(status) printk("[W]");
        } while(status);
        ar8216_mdio_write16(mdio, AR8216_GLOBAL_AR_1, 0
            | AR_1_AT_BUSY 
            | AR_1_AT_FUNC_VALUE(6)
            | AR_1_AT_PORT_NUM_VALUE(0)
            );

        status1 = ar8216_mdio_read32(mdio, AR8216_GLOBAL_AR_1);
        this = ar8216_mdio_read32(mdio, AR8216_GLOBAL_AR_2);
        status = ar8216_mdio_read32(mdio, AR8216_GLOBAL_AR_3);

        if(this == 0) {
            DEB_TRC("[ar8216] index %d: empty\n", count);
            break;
        }
            /*--- break; ---*/

        printk(KERN_ERR "[ar8216] index %d: %02x:%02x:",
                count,
                (this >> 24) & 0xFF,
                (this >> 16) & 0xFF
              );
        printk("%02x:%02x:%02x:%02x ",
                (this >> 8) & 0xFF,
                (this >> 0) & 0xFF,
                (status1 >> AR_1_AT_ADDR_BYTE4_SHIFT) & 0xFF,
                (status1 >> AR_1_AT_ADDR_BYTE5_SHIFT) & 0xFF
                );

        printk("%s%s%s%s%s \n\tStatus: %s DES 0x%x\n" , 
            status & AR_3_AT_PRIORITY_EN ? " AT_PRIORITY_EN" : "",
            status & AR_3_AT_MIRROR_EN ? " AT_MIRROR_EN" : "",
            status & AR_3_SA_DROP_EN ? " SA_DROP_EN" : "",
            status & AR_3_REDIRECT_TO_CPU ? " REDIRECT_TO_CPU" : "",
            status & AR_3_COPY_TO_CPU  ? " COPY_TO_CPU " : "",
            (status & AR_3_AT_STATUS_MASK) == AR_3_AT_STATUS_VALUE(0) ? "entry is empty" :
            (status & AR_3_AT_STATUS_MASK) == AR_3_AT_STATUS_VALUE(1) ? "entry is static und valid" :
            (status & AR_3_AT_STATUS_MASK) == AR_3_AT_STATUS_VALUE(2) ? "entry is valid and has been accessed since the last aging process" :
            (status & AR_3_AT_STATUS_MASK) == AR_3_AT_STATUS_VALUE(3) ? "entry is valid and has not been accessed since the last aging process" : "unknown",
            (status & AR_3_DES_PORT_MASK) >> AR_3_DES_PORT_SHIFT);

    } 
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void init_port(cpphy_mdio_t *mdio, unsigned int port) {
    /*--- unsigned int cpu_port = port == 0 ? 1 : 0; ---*/
    if(port == 0)
        return;
    port++; port *= AR8216_PORT0_CONTROL_BASIS;
    ar8216_mdio_write32(mdio, port + AR8216_PORT_CONTROL_OFFSET,  0
            | ATH_PORT_CTRL_LEARN_EN
            /*--- | ATH_PORT_CTRL_IGMP_MLD_EN ---*/
            | ATH_PORT_CTRL_EG_VLAN_MODE(1)
            /*--- | (cpu_port ? ATH_PORT_CTRL_HEAD_EN : 0) ---*/
            /*--- | (cpu_port ? 0 : ATH_PORT_CTRL_SINGLE_VLAN_EN) ---*/
            /*--- | (cpu_port ? 0 : ATH_PORT_CTRL_ING_MIRROR_EN) ---*/
            | ATH_PORT_CTRL_PORTSTATE_FORWARD
            );
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void add_port_to_vlan_vid(cpphy_mdio_t *mdio,
                          unsigned int port, 
                          unsigned int vid, 
                          unsigned int mem_mask) {
    /*--- unsigned int cpu_port = port == 0 ? 1 : 0; ---*/
    mem_mask &= ~(1 << port);
    port++; port *= AR8216_PORT0_CONTROL_BASIS;

    ar8216_mdio_write32(mdio, port + AR8216_PORT_CONTROL_OFFSET,  0
            | ATH_PORT_CTRL_LEARN_EN
            /*--- | ATH_PORT_CTRL_IGMP_MLD_EN ---*/
            /*--- | (cpu_port ? ATH_PORT_CTRL_HEAD_EN : 0) ---*/
            | ATH_PORT_CTRL_EG_VLAN_MODE(1)
             /*--- | (cpu_port ? ATH_PORT_CTRL_EG_VLAN_MODE(2) : ATH_PORT_CTRL_EG_VLAN_MODE(1)) ---*/
            /*--- | (cpu_port ? 0 : ATH_PORT_CTRL_SINGLE_VLAN_EN) ---*/
            | ATH_PORT_CTRL_PORTSTATE_FORWARD
            );

    ar8216_mdio_write32(mdio, port + AR8216_PORT_VLAN_OFFSET, 0
            | ATH_PORT_VLAN_VID_VALUE(vid)
            | ATH_PORT_VLAN_VID_MEM_VALUE(mem_mask) 
            | ATH_PORT_VLAN_8021_QMODE_VALUE(1)
            );
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void add_vlan_vid(cpphy_mdio_t *mdio, unsigned int vid, unsigned int mem_mask) {
    unsigned int status;

    do {
        status = ar8216_mdio_read32(mdio, AR8216_GLOBAL_VLAN_TABLE_1);
        if(status & VLAN_TABLE_1_VT_FULL_VIO) {
            printk(KERN_ERR "VLAN_TABLE_1_VT_FULL_VIO !\n");
            return;
        }
        if(status & VLAN_TABLE_1_VT_BUSY) {
            printk(KERN_ERR "VLAN_TABLE_1_VT_BUSY !\n");
            schedule();
        }
    } while(status & VLAN_TABLE_1_VT_BUSY);

    ar8216_mdio_write32(mdio, AR8216_GLOBAL_VLAN_TABLE_2, 0 /*--- 0x0000081f); ---*/   
            | VLAN_TABLE_2_VID_MEM_VALUE(mem_mask)
            | VLAN_TABLE_2_VT_VALID           
            );

    ar8216_mdio_write32(mdio, AR8216_GLOBAL_VLAN_TABLE_1, 0 /*--- 0x0001000a); ---*/
            | VLAN_TABLE_1_VT_FUNC_VALUE(0x2)  /*--- load entry ---*/
            | VLAN_TABLE_1_VT_BUSY
            /*--- | VLAN_TABLE_1_VT_FULL_VIO ---*/
            /*--- | VLAN_TABLE_1_VT_PORT_NUM_VALUE(0) ---*/
            | VLAN_TABLE_1_VID_VALUE(vid)
            /*--- | VLAN_TABLE_1_VT_PRI_VALUE(0) ---*/
            /*--- | VLAN_TABLE_1_VT_PRI_EN ---*/
            );
}

extern spinlock_t ar8216_lock;
unsigned int ath8316 = 0; /* FIXME FIXME FIXME */
static unsigned int ar8226 = 0; /* FIXME FIXME FIXME */
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
int ar_init(cpphy_mdio_t *mdio) {
    unsigned int id = ar8216_mdio_read32(mdio, AR8216_GLOBAL_DEVICE_ID);
    unsigned int port, tmp;

    DEB_TEST("[ar_init] Device ID = %#x\n", id & 0xFFFF); /* FIXME */
    switch(id & 0xffff) {
        case AR8316_DEVICE_ID:
            ath8316 = 1;
            break;
        case AR8216_DEVICE_ID:
            break;
        case AR8226_DEVICE_ID:
            ar8226 = 1;
            break;
        default:
            DEB_TRC("[cpphy_ar8216] <no switch found>\n");
            return 1;
    }

    spin_lock_init(&ar8216_lock);

    mdio->cpmac_switch = AVM_CPMAC_SWITCH_AR8216;
    mdio->led_port_offset = 1;
    mdio->Mode = 0 | CPPHY_SWITCH_MODE_WRITE | CPPHY_SWITCH_MODE_READ;
    mdio->switch_ports = 5;
    DEB_TRC("[%s] found switch\n", __FUNCTION__);
    mdio->switch_config = &(switch_configuration[CPMAC_SWITCH_CONF_AR8216][0]);
    mdio->switch_status.cpu_port = 0;
    mdio->run_ar_queue_without_removal_counter = 0;

#if 0
    ar8216_softreset(mdio);

    /*--- standart-Initialiserung ---*/
    for (phy=0;phy<5;phy++) {
        cpphy_mdio_user_access_write(mdio, AR8216_MII_AUTO_NEG_ADV, phy, ATHR_ADVERTISE_ALL);
        cpphy_mdio_user_access_write(mdio, AR8216_MII_CONTROL, phy, 
                AR8216_PHYCTRL_SW_RESET
              | AR8216_PHYCTRL_AUTONEG_EN);
    }
#endif

    mdelay(200);

    if(!ar8226) {
        tmp = ar8216_mdio_read32(mdio, AR8216_GLOBAL_DEVICE_ID);
        tmp |= 0x500000;
        ar8216_mdio_write32(mdio, AR8216_GLOBAL_DEVICE_ID, (1<<24)|tmp);  /*--- change MII-Clockphase by 180 ---*/
    }

    /*--- Port0 wird nicht konfiguriert - hier ist CPU angeschlossen ---*/
    /*--- Port5 wird auch nicht konfiguriert ---*/
    for (port = AR8216_PORT1_CONTROL_BASIS; port < AR8216_PORT5_CONTROL_BASIS; port+=0x100) {
        ar8216_mdio_write32(mdio, port, 
                /*--- ATH_PORT_STATUS_SPEED_100M ---*/
              /*--- | ATH_PORT_STATUS_TXMACEN ---*/
              /*--- | ATH_PORT_STATUS_RXMACEN ---*/
              /*--- | ATH_PORT_STATUS_TXFLOWEN ---*/
              /*--- | ATH_PORT_STATUS_RXFLOWEN ---*/
              /*--- | ATH_PORT_STATUS_FULLDUPLEX ---*/
              /*--- | ATH_PORT_STATUS_LINKEN); ---*/
              ATH_PORT_STATUS_LINKEN);
    }

    ar8216_mdio_write32(mdio, 0x38, 0xc000050e);      /*--- undefined lt. Datenblatt ---*/

    /*--------------------------------------------------------------------------------------*\
    \*--------------------------------------------------------------------------------------*/
    init_port(mdio, 0);
    init_port(mdio, 1);
    init_port(mdio, 2);
    init_port(mdio, 3);
    init_port(mdio, 4);

    ar8216_mdio_write32(mdio, 0x38, 0xc000050e);   /* WAS FUERN REGISTER WELCHE FUNCTION */

    if(ath8316) {
        ar8216_mdio_write32(mdio, 0x78, (0xF<<4));

        tmp = ar8216_mdio_read32(mdio, 0x108);
        tmp |= ((1<<24)|(1<<2));
        ar8216_mdio_write32(mdio, 0x108, tmp);

        ar8216_mdio_write32(mdio, 0x100, 0x7d);
    } else if(ar8226) {
        /* Enable CPU port */
        ar8216_mdio_write32(mdio, 
                            AR8216_PORT0_CONTROL_BASIS,
                            ATH_PORT_STATUS_SPEED_100M
                            | ATH_PORT_STATUS_TXMACEN
                            | ATH_PORT_STATUS_RXMACEN
                            | ATH_PORT_STATUS_TXFLOWEN
                            | ATH_PORT_STATUS_RXFLOWEN
                            | ATH_PORT_STATUS_FULLDUPLEX
                           );

        ar8216_mdio_write32(mdio, 0x0,  0x201);
        ar8216_mdio_write32(mdio, 0x04, 0x80000400);
        ar8216_mdio_write32(mdio, 0x10, 0x80061B20);
        ar8216_mdio_write32(mdio, 0x2c, 0xfe7f007f);
        ar8216_mdio_write32(mdio, AR8216_GLOBAL_CPUPORT, 0x1f0);
    }

    /*--- ar8216_mdio_write32(mdio, 0x104, 0x6004); ---*/
    /*--- ar8216_mdio_write32(mdio, 0x204, 0x6004); ---*/
    /*--- ar8216_mdio_write32(mdio, 0x304, 0x6004); ---*/
    /*--- ar8216_mdio_write32(mdio, 0x404, 0x6004); ---*/
    /*--- ar8216_mdio_write32(mdio, 0x504, 0x6004); ---*/    

    /*--- ar8216_mdio_write32(mdio, 0x60, 0xffffffff); ---*/
    ar8216_mdio_write32(mdio, 0x60, 0x0);
    ar8216_mdio_write32(mdio, 0x64, 0xaaaaaaaa);
    ar8216_mdio_write32(mdio, 0x68, 0x55555555);    
    /*--- ar8216_mdio_write32(mdio, 0x6c, 0x0); ---*/    
    ar8216_mdio_write32(mdio, 0x6c, 0xffffffff);    
    /*--- ar8216_mdio_write32(mdio, 0x70, 0x41af); ---*/
    ar8216_mdio_write32(mdio, 0x70, 0xfa41);

    /*--- ar8216_mdio_write32(mdio, 0x78, (1 << 8)); ---*/
    
    /*--- add_vlan_vid(mdio, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10); ---*/  /*--- VLAN 3 Port 0, 1, 2, 3, 4 ---*/

    /*--- #define AR8216_USE_SINGLE_LAN ---*/
/*--- #   define AR8216_USE_SPLIT_LAN ---*/

#   if defined(AR8216_USE_SINGLE_LAN)
    add_port_to_vlan_vid(mdio, 0, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 1, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 2, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 3, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 4, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
#   endif /*--- #if defined(AR8216_USE_SINGLE_LAN) ---*/
#   if defined(AR8216_USE_SPLIT_LAN)
    add_vlan_vid(mdio, 0x11, 0x01 | 0x02                     );  /*--- VLAN 17 Port 0, 1 ---*/
    add_vlan_vid(mdio, 0x12, 0x01        | 0x04              );  /*--- VLAN 18 Port 0, 2 ---*/
    add_vlan_vid(mdio, 0x13, 0x01               | 0x08       );  /*--- VLAN 19 Port 0, 3 ---*/
    add_vlan_vid(mdio, 0x14, 0x01                      | 0x10);  /*--- VLAN 20 Port 0, 4 ---*/
    add_port_to_vlan_vid(mdio, 0, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 1, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 2, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 3, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    add_port_to_vlan_vid(mdio, 4, 0x01, 0x01 | 0x02 | 0x04 | 0x08 | 0x10);
    for(port = 1; port < 5; port++) {
        unsigned short vid = port + 0x10;
        mdio->cpmac_priv->map_port_out[port] = vid;
#       ifdef CONFIG_IP_MULTICAST_FASTFORWARD
        mcfw_portset_reset(&mdio->cpmac_priv->map_vid_portset[vid]);
        mcfw_portset_port_add(&mdio->cpmac_priv->map_vid_portset[vid], port);
#       endif /*--- #ifdef CONFIG_IP_MULTICAST_FASTFORWARD ---*/
    }
#   endif /*--- #if defined(AR8216_USE_SPLIT_LAN) ---*/

    /* Enable address table entry aging (18 * 17 seconds) */
    ar8216_mdio_write32(mdio, AR8216_GLOBAL_AR_CONTROL, AR_CONTROL_AGE_EN | AR_CONTROL_AGE_TIME(CPMAC_AR_AGE_TIME));

    /* Allow bigger packet size: 1526 bytes for double tagged packets */
    tmp = ar8216_mdio_read32(mdio, AR8216_GLOBAL_CONTROL);
    tmp &= 0xfffffc00;
    tmp |= 0x5f6;
    ar8216_mdio_write32(mdio, AR8216_GLOBAL_CONTROL, tmp);

    ar8216_read_address_resolution_table(mdio);

    cpphy_mgmt_power_init(mdio);

    return 0;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar_check_link_orig(cpphy_mdio_t *mdio) {
    unsigned int link_status = 0;

/*--- #   define USE_MDIO_STATUS_READ ---*/
#   if defined(USE_MDIO_STATUS_READ)
    {
        unsigned int phy, status;
        for(phy = 0; phy < AR8216_MAX_PHYS; phy++) {
            status = cpphy_mdio_user_access_read(mdio, AR8216_MII_PHY_STATUS, phy);
            if(status & ATHR_PHY_STATUS_LINK) {
                link_status |= 1 << (phy + 1);
            }
        }
    }
#   else /*--- #if defined(USE_MDIO_STATUS_READ) ---*/
    /*--- eine Version ohne per MDIO-Read den Linkstatus abzufragen ---*/
    {
        unsigned int alive = MDIO_ALIVE & 0xFFFF;   /*--- die oberen PHY-Adressen nicht abfragen ---*/
        unsigned int link = MDIO_LINK & 0xFFFF;     

        link_status = (alive & link) << 1;
    }
#   endif /*--- #else ---*/ /*--- #if defined(USE_MDIO_STATUS_READ) ---*/

    mdio->linked = link_status;

    return mdio->linked;
}

 
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar_check_link_emv(cpphy_mdio_t *mdio) {
    unsigned int link_status = 0;

    return mdio->linked;

/*--- #   define USE_MDIO_STATUS_READ ---*/
#   if defined(USE_MDIO_STATUS_READ)
    {
        unsigned int phy, status;
        for(phy = 0; phy < AR8216_MAX_PHYS; phy++) {
            status = cpphy_mdio_user_access_read(mdio, AR8216_MII_PHY_STATUS, phy);
            if(status & ATHR_PHY_STATUS_LINK) {
                link_status |= 1 << phy;
            }
        }
    }
#   else /*--- #if defined(USE_MDIO_STATUS_READ) ---*/
    /*--- eine Version ohne per MDIO-Read den Linkstatus abzufragen ---*/
    {
        unsigned int alive = MDIO_ALIVE & 0xFFFF;   /*--- die oberen PHY-Adressen nicht abfragen ---*/
        unsigned int link = MDIO_LINK & 0xFFFF;     

        link_status = alive & link;
    }
#   endif /*--- #else ---*/ /*--- #if defined(USE_MDIO_STATUS_READ) ---*/

    mdio->linked = link_status;

    return mdio->linked;
}

 
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar_switch_port_power_on_boot_orig(cpphy_mdio_t *mdio, unsigned int portset) {
    unsigned int port;

    DEB_WARN("[ar_switch_port_power_on_boot] This will take approx. 13 seconds!\n");
    for(port = 0; port < 5; port++) {
        if((1 << port) & portset) {
            cpphy_mdio_user_access_write(mdio, 0x0,  port, 0x2100);
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x3);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x11);
        }
    }
    ssleep(3);
    for(port = 0; port < 5; port++) {
        if((1 << port) & portset) {
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x2);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x3200);
        }
    }
    ssleep(3);
    for(port = 0; port < 5; port++) {
        if((1 << port) & portset) {
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x1);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x1d0);
        }
    }
    ssleep(3);
    for(port = 0; port < 5; port++) {
        if((1 << port) & portset) {
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x024e);
            cpphy_mdio_user_access_write(mdio, 0x0,  port, 0x2100);
        }
    }
    ssleep(4);
    for(port = 0; port < 5; port++) {
        if((1 << port) & portset) {
            cpphy_mdio_user_access_write(mdio, 0x0,  port, 0xb100);
        }
    }
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar_switch_test_duplex(cpphy_mdio_t *mdio, unsigned int port) {
    unsigned short temp;

    /* Check real time duplex resolved information */
    temp = cpphy_mdio_user_access_read(mdio, 0x11, port);
    return (temp & ATHR_PHY_STATUS_SPEED_DUPLEX);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar_switch_test_link(cpphy_mdio_t *mdio, unsigned int port) {
    unsigned short status, link;

    /* Check real time link information */
    status = cpphy_mdio_user_access_read(mdio, 0x11, port);
    link = (status & ATHR_PHY_STATUS_LINK) ? 1 : 0;
    if(mdio->event_data.port[port].link != link) {
        /* Link changed. Update link status */
        mdio->event_data.port[port].speed100 = (((status & ATHR_PHY_STATUS_SPEED_MASK) >> ATHR_PHY_STATUS_SPEED_SHIFT) == 1) ? 1 : 0;
        mdio->event_data.port[port].fullduplex = (status & ATHR_PHY_STATUS_DUPLEX) ? 1 : 0;
        mdio->event_update_needed++;
    }
    mdio->event_data.port[port].link = link;

    return link;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
unsigned int ar_switch_test_linkstate_machine(cpphy_mdio_t *mdio, unsigned int port) {
    unsigned short temp;

    /* Check real time link state machine information */
    cpphy_mdio_user_access_write(mdio, 0x1d, port, 0xe);
    temp = cpphy_mdio_user_access_read(mdio, 0x1e, port);
    temp &= 0xf;
    return ((temp == 0) || (temp == 2));
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar_switch_port_power_orig(cpphy_mdio_t *mdio, 
                               unsigned int port, 
                               unsigned int power_on) {
    /*--- DEB_TEST("[ar_switch_port_power] %lu Port %u %s\n", jiffies, port, power_on ? "on" : "off"); ---*/ 
    if(power_on) {
        cpphy_mdio_user_access_write(mdio, 0x0,  port, 0xb100);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x3);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x11);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x2);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x3200);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x1);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x1d0);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x024e);
    } else {
        cpphy_mdio_user_access_write(mdio, 0x0,  port, 0x1200);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x124e);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x1);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x10);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x2);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x3000);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x3);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x0000);
    }
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar_boot_power_on_emv(cpphy_mdio_t *mdio) {
    unsigned int port;

    for(port = 0; port < 4; port++) {
        /* Switch on port again */
        DEB_INFOTRC("[ar_boot_power_on] port %u: restart after CPPHY_POWER_MODE_OFF\n", port);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x124e);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x3);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x11);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x2);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x3220);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x1);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x1d0);
        cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
        cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x024e);
        cpphy_mdio_user_access_write(mdio, 0x0,  port, 0x3100);
    }
    ssleep(4);

    for(port = 0; port < 4; port++) {
        /* Force port into CPPHY_POWER_MODE_ON_MDI_MLT3 to prevent initial 10 MBit/s links */
        DEB_INFOTRC("[ar_boot_power_on] port %u: reset as MDI/MLT3\n", port);
        cpphy_mdio_user_access_write(mdio, 0x14, port, 0x2c);
        cpphy_mdio_user_access_write(mdio, 0x10, port, 0x800);
        cpphy_mdio_user_access_write(mdio, 0x0, port, 0xa100);
        mdio->adm_power.port_mode[port] = CPPHY_POWER_MODE_ON_MDI_MLT3;
    }
    ssleep(2);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void ar_switch_port_power_emv(cpphy_mdio_t *mdio, 
                              unsigned int port, 
                              cpphy_switch_powermodes_t mode) {
    unsigned short temp;

    mdio->adm_power.time_to_wait_to[port] = jiffies;

    if(mode == mdio->adm_power.port_mode[port]) {
        DEB_TEST("[ar_switch_port_power] Mode %u already set for port %u\n", mode, port);
        return;
    }

    switch(mode) {
        case CPPHY_POWER_MODE_OFF:
            /* TODO Check, whether this sequence works in all contexts (even in mute mode?) */
            DEB_INFOTRC("[ar_switch_port_power] port %u: CPPHY_POWER_MODE_OFF\n", port);
            cpphy_mdio_user_access_write(mdio, 0x0,  port, 0x1200);
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x0);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x124e);
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x1);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x10);
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x2);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x3000);
            cpphy_mdio_user_access_write(mdio, 0x1d, port, 0x3);
            cpphy_mdio_user_access_write(mdio, 0x1e, port, 0x0000);
            break;

        case CPPHY_POWER_MODE_ON_IDLE_MDI:
            DEB_INFOTRC("[ar_switch_port_power] port %u: CPPHY_POWER_MODE_ON_IDLE_MDI\n", port);
            cpphy_mdio_user_access_write(mdio, 0x10, port, 0x800);
            cpphy_mdio_user_access_write(mdio, 0x00, port, 0xa100);
            break;

        case CPPHY_POWER_MODE_ON:
            temp = cpphy_mdio_user_access_read(mdio, 0x11, port);
            DEB_INFO("[ar_switch_port_power] port %u: CPPHY_POWER_MODE_ON %u MBit/s, %s duplex, MDI%s\n", 
                     port,
                     (temp & ATHR_PHY_STATUS_SPEED_MASK) ? 100 : 10,
                     (temp & ATHR_PHY_STATUS_DUPLEX) ? "full" : "half",
                     (temp & ATHR_PHY_STATUS_MDI_CROSSOVER) ? "X" : ""
                    );
            /* If this is the first linked port, lock port 0 to MDIX */
            if(mdio->linked == 0) {
                if(port != 0) {
                    ar_switch_port_power_emv(mdio, 0, CPPHY_POWER_MODE_ON_MDIX);
                }
            }
            mdio->linked |= (1 << port);
            DEB_TEST("[ar_switch_port_power] ports linked: %#x\n", mdio->linked);
            if((port != 0) || (mdio->linked != (1 << 0))) {
                if(mdio->adm_power.port_mode[port] == CPPHY_POWER_MODE_ON_MDI_MLT3) {
                    cpphy_mdio_user_access_write(mdio, 0x10, port, 0x860);
                    cpphy_mdio_user_access_write(mdio, 0x0, port, 0xffff);
                }
            }
            break;

        case CPPHY_POWER_MODE_ON_MDI_MLT3:
            if(ar_switch_test_link(mdio, port)) {
                ar_switch_port_power_emv(mdio, port, CPPHY_POWER_MODE_ON);
                return;
            }
            DEB_INFOTRC("[ar_switch_port_power] port %u: CPPHY_POWER_MODE_ON_MDI_MLT3\n", port);
            cpphy_mdio_user_access_write(mdio, 0x14, port, 0x2c);
            cpphy_mdio_user_access_write(mdio, 0x10, port, 0x800);
            cpphy_mdio_user_access_write(mdio, 0x0, port, 0xa100);
            break;

        case CPPHY_POWER_MODE_ON_MDIX:
            if(ar_switch_test_link(mdio, port)) {
                ar_switch_port_power_emv(mdio, port, CPPHY_POWER_MODE_ON);
                return;
            }
            DEB_INFOTRC("[ar_switch_port_power] port %u: CPPHY_POWER_MODE_ON_MDIX\n", port);
            cpphy_mdio_user_access_write(mdio, 0x14, port, 0x2c);
            cpphy_mdio_user_access_write(mdio, 0x10, port, 0x820);
            cpphy_mdio_user_access_write(mdio, 0x0, port, 0xffff);
            break;

        case CPPHY_POWER_MODE_LINK_LOST:
            /* Do nothing */
            break;

        default:
            DEB_ERR("[ar_switch_port_power] port %u: Illegal mode (%u)\n", port, mode);
            return;
    }
    mdio->adm_power.port_mode[port] = mode;
}


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

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

    while(VLAN_TABLE_1_VT_BUSY & ar8216_mdio_read32(mdio, AR8216_GLOBAL_VLAN_TABLE_1)) {
        DEB_TRC("[ar_vlan_config] VLAN table busy\n");
        schedule();
    }
    ar8216_mdio_write32(mdio, 
                        AR8216_GLOBAL_VLAN_TABLE_1, 
                          VLAN_TABLE_1_VT_FUNC_VALUE(VLAN_TABLE_FUNC_FLUSH_ALL)
                        | VLAN_TABLE_1_VT_BUSY
                       );

    for(port = 0; port <= 5; port++) {
        unsigned int RegData;
        DEB_INFOTRC("[ar_vlan_config] Configure port %u with port mask %#x, vid %#x, tagging %sabled\n",
                    port,
                    mdio->switch_status.port[port].port_vlan_mask,
                    mdio->switch_status.port[port].vid,
                    mdio->switch_status.port[port].keep_tag_outgoing ? "en" : "dis");
        ar8216_mdio_write32(mdio, 
                            0x100 * (port + 1) + AR8216_PORT_VLAN_OFFSET,
                              ((ATH_PORT_VLAN_8021_QMODE_FALLBACK << ATH_PORT_VLAN_8021_QMODE_SHIFT) & ATH_PORT_VLAN_8021_QMODE_MASK)
                            | ((mdio->switch_status.port[port].port_vlan_mask << ATH_PORT_VLAN_VID_MEM_SHIFT) & ATH_PORT_VLAN_VID_MEM_MASK)
                            | ((mdio->switch_status.port[port].vid << ATH_PORT_VLAN_VID_SHIFT) & ATH_PORT_VLAN_VID_MASK));

        RegData = ar8216_mdio_read32(mdio, (port + 1) * AR8216_PORT0_CONTROL_BASIS + AR8216_PORT_CONTROL_OFFSET);
        RegData &= ~(3u << 8); /* Delete VLAN status */
        if(!mdio->switch_status.port[port].keep_tag_outgoing) {
            RegData |= ATH_PORT_CTRL_EG_VLAN_MODE(1);
        }
        ar8216_mdio_write32(mdio, (port + 1) * AR8216_PORT0_CONTROL_BASIS + AR8216_PORT_CONTROL_OFFSET, RegData);
        RegData = ar8216_mdio_read32(mdio, (port + 1) * AR8216_PORT0_CONTROL_BASIS + AR8216_PORT_CONTROL_OFFSET);
        DEB_TEST("[ar_vlan_config] Control port %u: %#x\n", port, RegData);
    }

    for(group = 0; group < AVM_CPMAC_MAX_VLAN_GROUPS; group++) {
        if(mdio->switch_status.vlan[group].active) {
            DEB_INFOTRC("[ar_vlan_config] Setting VID %#x with portmask %#x\n", 
                        mdio->switch_status.vlan[group].VFL.Bits.VID, 
                        mdio->switch_status.vlan[group].VFH.Bits.M);
            add_vlan_vid(mdio, mdio->switch_status.vlan[group].VFL.Bits.VID, mdio->switch_status.vlan[group].VFH.Bits.M);
        }
        mdio->switch_status.vlan[group].written = 1;
    }
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void ar_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].active = 0;
        mdio->switch_status.vlan[group].VFL.Bits.VV = 0;
        mdio->switch_status.vlan[group].VFL.Bits.VP = 0;
        mdio->switch_status.vlan[group].VFL.Bits.VID = group + 16;
        mdio->switch_status.vlan[group].VFH.Bits.FID = 0;
        mdio->switch_status.vlan[group].VFH.Bits.TM = 0;
        mdio->switch_status.vlan[group].VFH.Bits.M = 0x3F;
    }
    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->switch_status.port[port].port_vlan_mask = 0x3f ^ (1 << port);
        mdio->cpmac_priv->map_port_out[port] = mdio->switch_status.port[port].vid;
    }
}

/*------------------------------------------------------------------------------------------*\
 * Reset the VLAN settings of the switch                                                    *
\*------------------------------------------------------------------------------------------*/
void ar_vlan_fbox_reset(cpphy_mdio_t *mdio) {
    /* switch off VLAN */
    ar_vlan_reset(mdio);
    ar_vlan_config(mdio);
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
/* Find unused VLAN ID group */
#define find_unused_vlan_old() \
    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;

#define adm_get_new_vid() \
    /* Do not use VIDs < 2; ensure it by adding 4, that does not change the lower two bits */ \
    new_vid = (1 << 2); \
    for( ; ; ) { \
        new_vid += (1 << 2); \
        new_vid &= 0xfff; /* To make Klockworks happy */ \
        if(mdio->cpmac_priv->map_in[new_vid].used) \
            continue; \
        DEB_TRC("[adm_get_new_vid] new_vid = %#x\n", new_vid); \
        break; \
    }


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
cpmac_err_t ar_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, vid, devices_old;
    unsigned short new_vid; /*--- = (1 << (4 + ADM_6996_TAGSHIFT)); ---*/
    struct avm_new_switch_struct *status = &(mdio->switch_status);

    if(!(mdio->Mode & CPPHY_SWITCH_MODE_WRITE)) {
        DEB_ERR("[ar_configure_cpmac] Illegal configuration for this hardware!\n");
        return CPMAC_ERR_NO_VLAN_POSSIBLE;
    }

    /* Check plausibility */
    /* For AR8216 allow only normal and ATA mode */
    if(config->cpmac_mode > CPMAC_MODE_ATA) {
        DEB_ERR("[ar_configure_cpmac] Unknown cpmac_configuration %u\n", config->cpmac_mode);
        return CPMAC_ERR_EXCEEDS_LIMIT;
    }
    assert(status->cpu_port < AVM_CPMAC_MAX_PORTS);

    /* Make sure, that nothing works in parallel */
    cpphy_mgmt_work_stop(mdio);
    tasklet_disable(&mdio->cpmac_priv->tasklet);

    /* Backup device entries */
    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;
    }

    devices_old = mdio->cpmac_priv->devices;
    mdio->cpmac_priv->devices = mdio->switch_config[config->cpmac_mode].number_of_devices;
    if(mdio->cpmac_priv->devices > AVM_CPMAC_MAX_DEVS) {
        mdio->cpmac_priv->devices = devices_old;
        DEB_ERR("[ar_configure_cpmac] Too many devices in configuration!\n");
        tasklet_enable(&mdio->cpmac_priv->tasklet);
        cpphy_mgmt_work_start(mdio);
        return CPMAC_ERR_EXCEEDS_LIMIT;
    }
    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);
    }

    switch(config->cpmac_mode) {
        case CPMAC_MODE_NORMAL:
            break;
        case CPMAC_MODE_ATA:
            break;
        default:
            tasklet_enable(&mdio->cpmac_priv->tasklet);
            cpphy_mgmt_work_start(mdio);
            return CPMAC_ERR_ILL_CONTROL;
    }

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

    /* More than four devices might mean problems with port addressing */
    if(mdio->cpmac_priv->devices > 4) {
        DEB_WARN("[ar_configure_cpmac] Warning: Too many devices (%u). This might cause packet loss!\n",
                 mdio->cpmac_priv->devices);
    }

    ar_vlan_reset(mdio);

    /* 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;
    }

    /* 1: Map all VIDs to port 0
     * 2: ATA: Map port 0 to wan, ports 1-4 to eth0
     *    normal: Map port 0-4 to eth0
     * 3: ATA: Define needed VLANs for wan, define default VID for wan
     * 4: Make port VLAN maps: ATA: port 0 -> 0x1e
     *                              port 1 -> 0x01
     *                              port 2 -> 0x1d
     *                              port 3 -> 0x15
     *                              port 4 -> 0x0d
     *                         normal: port 0 -> 0x1e
     *                                 port 1 -> 0x1d
     *                                 port 2 -> 0x1b
     *                                 port 3 -> 0x17
     *                                 port 4 -> 0x0f
     */


    /* Reset VID->Port mapping */
    for(vid = 0; vid < 4096; vid++) {
        mdio->cpmac_priv->map_in[vid].port = status->cpu_port;
        mdio->cpmac_priv->map_in[vid].used = 0;
    }
    /* Normal: Map all ports to device 0 */
    /* ATA   : Map 0, 1 to device 0, 2-5 to device 1 */
    for(port = 0; port < AVM_CPMAC_MAX_PORTS; port++) {
        mdio->cpmac_priv->map_in_port_dev[port].used = (port <= 4) ? 1 : 0;
        mdio->cpmac_priv->map_in_port_dev[port].dev = 0;
        if(   (config->cpmac_mode == CPMAC_MODE_ATA)
           && (port > 1)) {
            mdio->cpmac_priv->map_in_port_dev[port].dev = 1;
        }
    }

    /* Do we have predefined VLAN IDs? */
    if(config->wan_vlan_number) {
        device = 0; /* Hardcode wan device for the moment */
        for(i = 0; i < config->wan_vlan_number; i++) {
            vid = config->wan_vlan_id[i];
            find_unused_vlan_old(); /* sets vid_group */

            status->vlan[vid_group].VFL.Bits.VID = vid;
            mdio->cpmac_priv->map_in[vid].used = 1;
            mdio->cpmac_priv->map_in[vid].port = mdio->switch_config[config->cpmac_mode].wanport;
            status->vlan[vid_group].VFH.Bits.M = mdio->switch_config[config->cpmac_mode].device[device].target_mask;
            status->vlan[vid_group].written = 0;
            status->vlan[vid_group].active = 1;
        }
    }

    /* Tag incoming data with individual tags and deliver it to all interested ports */
    for(port = 0; port <= 4; port++) {
        if(config->cpmac_mode == CPMAC_MODE_ATA) {
            if(port == 1) {
                mdio->switch_status.port[port].port_vlan_mask = 0x3 ^ (1 << port);
            } else {
                mdio->switch_status.port[port].port_vlan_mask = 0x1d ^ (1 << port);
            }
        } else {
            mdio->switch_status.port[port].port_vlan_mask = 0x1f ^ (1 << port);
        }

        if(port < 1) { /* No outgoing tag needed */
            continue;
        }

        /* Create VLANs for port specific outgoing traffic */
        find_unused_vlan_old();
        adm_get_new_vid();
        mdio->cpmac_priv->map_in[new_vid].used = 1; /* To mark VID as used */
        status->port[port].vid = new_vid;
        status->port[port].written = 0;
        status->vlan[vid_group].VFL.Bits.VID = new_vid;
        status->vlan[vid_group].VFH.Bits.M = (1 << port);
        status->vlan[vid_group].active = 1;
        status->vlan[vid_group].written = 0;
        mdio->cpmac_priv->map_port_out[port] = status->vlan[vid_group].VFL.Bits.VID;
        DEB_INFOTRC("Configure outgoing port %u: group %u, VID %#x\n", port, vid_group, new_vid);
    }

    status->port[status->cpu_port].keep_tag_outgoing = 0;
    status->port[status->cpu_port].written = 0;

    mdio->cpmac_priv->wanport = mdio->switch_config[config->cpmac_mode].wanport;
    mdio->cpmac_priv->wanport_keeptags = 0;
    mdio->wan_tagging_enable = 0;
    if(config->wan_vlan_number) {
        assert(mdio->cpmac_priv->wanport < AVM_CPMAC_MAX_PORTS);
        status->port[mdio->cpmac_priv->wanport].keep_tag_outgoing = 1;
        status->port[status->cpu_port].keep_tag_outgoing = 1;
        mdio->cpmac_priv->wanport_keeptags = 1;
        mdio->wan_tagging_enable = 1;
        DEB_INFOTRC("[ar_configure_cpmac] 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].VFH.Bits.M) {
                mcfw_portset_port_add(setp, port);
            }
        }
    }
#   endif

    /* Now register the devices with the corresponding VIDs */
    /* Register eth0 */
    device = (config->cpmac_mode == CPMAC_MODE_ATA) ? 1 : 0;
    mdio->cpmac_priv->devs[device] = find_or_register_device(
            mdio->switch_config[config->cpmac_mode].device[device].name,
            mdio->cpmac_priv,
            0);
    mdio->cpmac_priv->dev_ports[device] = mdio->switch_config[config->cpmac_mode].device[device].target_mask;

    /* Register wan, if necessary */
    if(config->cpmac_mode == CPMAC_MODE_ATA) {
        device = 0;
        find_unused_vlan_old();
        adm_get_new_vid();
        mdio->cpmac_priv->map_in[new_vid].used = 1; /* To mark VID as used */
        status->vlan[vid_group].VFL.Bits.VID = new_vid;
        status->vlan[vid_group].active = 1;
        status->vlan[vid_group].written = 0;
        status->vlan[vid_group].VFH.Bits.M = mdio->switch_config[config->cpmac_mode].device[device].target_mask;
        mdio->cpmac_priv->devs[device] = find_or_register_device(
                mdio->switch_config[config->cpmac_mode].device[device].name,
                mdio->cpmac_priv,
                new_vid);
        mdio->cpmac_priv->wanport_default_vid = new_vid;
        assert(mdio->cpmac_priv->devs[device] != NULL);
        mdio->cpmac_priv->dev_ports[device] = mdio->switch_config[config->cpmac_mode].device[device].target_mask;
    }

    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++) {
        unsigned short vid;
        unsigned char port, devoffset;
        struct net_device *dev;

        if(!status->vlan[vid_group].active) {
            DEB_INFOTRC("Configure: VID group %#x inactive\n", vid_group);
            continue;
        }
        vid = status->vlan[vid_group].VFL.Bits.VID;
        assert(vid < 4096); /* To ease Klocwork checking */
        port = mdio->cpmac_priv->map_in[vid].port;
        assert(port < AVM_CPMAC_MAX_PORTS);
        devoffset = mdio->cpmac_priv->map_in_port_dev[port].dev;
        assert(devoffset < AVM_CPMAC_MAX_DEVS); /* To ease Klocwork checking */
        dev = mdio->cpmac_priv->map_in_port_dev[port].used ? mdio->cpmac_priv->devs[devoffset]
                                                 : NULL;
        dev = dev; /* Avoid warning, if DEB_TRC is defined to an empty command */
        DEB_INFOTRC("Configure: VID group %#x with VID %#x mapped to port %u attached to %s\n",
                    vid_group,
                    vid,
                    mdio->cpmac_priv->map_in[vid].port,
                    dev ? dev->name : "no device");
    }

    ar_vlan_config(mdio);
    /*--- switch_dump(mdio); ---*/
#   if 0
    {
        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);
    }
#   endif /* #if 0 */

    /* Special treatment for fifth to seventh port */ /* TODO Is there a cleaner way for this? */
    assert(mdio->switch_ports <= AVM_CPMAC_MAX_PORTS);
    for(port = 4; port < mdio->switch_ports; port++) {
        if(used_ports & (1 << port)) {
            mdio->adm_power.setup.mode[port] = ADM_PHY_POWER_ON;
        } else {
            mdio->adm_power.setup.mode[port] = ADM_PHY_POWER_OFF;
        }
    }
    tasklet_enable(&mdio->cpmac_priv->tasklet);
    cpphy_mgmt_work_start(mdio);
    mdio->global_power = CPPHY_POWER_GLOBAL_ON;

    return CPMAC_ERR_NOERR;
}