cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

peak_pci.c (21026B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 * Copyright (C) 2007, 2011 Wolfgang Grandegger <wg@grandegger.com>
      4 * Copyright (C) 2012 Stephane Grosjean <s.grosjean@peak-system.com>
      5 *
      6 * Derived from the PCAN project file driver/src/pcan_pci.c:
      7 *
      8 * Copyright (C) 2001-2006  PEAK System-Technik GmbH
      9 */
     10
     11#include <linux/kernel.h>
     12#include <linux/module.h>
     13#include <linux/interrupt.h>
     14#include <linux/netdevice.h>
     15#include <linux/delay.h>
     16#include <linux/pci.h>
     17#include <linux/io.h>
     18#include <linux/i2c.h>
     19#include <linux/i2c-algo-bit.h>
     20#include <linux/can.h>
     21#include <linux/can/dev.h>
     22
     23#include "sja1000.h"
     24
     25MODULE_AUTHOR("Stephane Grosjean <s.grosjean@peak-system.com>");
     26MODULE_DESCRIPTION("Socket-CAN driver for PEAK PCAN PCI family cards");
     27MODULE_LICENSE("GPL v2");
     28
     29#define DRV_NAME  "peak_pci"
     30
     31/* FPGA cards FW version registers */
     32#define PEAK_VER_REG1		0x40
     33#define PEAK_VER_REG2		0x44
     34
     35struct peak_pciec_card;
     36struct peak_pci_chan {
     37	void __iomem *cfg_base;		/* Common for all channels */
     38	struct net_device *prev_dev;	/* Chain of network devices */
     39	u16 icr_mask;			/* Interrupt mask for fast ack */
     40	struct peak_pciec_card *pciec_card;	/* only for PCIeC LEDs */
     41};
     42
     43#define PEAK_PCI_CAN_CLOCK	(16000000 / 2)
     44
     45#define PEAK_PCI_CDR		(CDR_CBP | CDR_CLKOUT_MASK)
     46#define PEAK_PCI_OCR		OCR_TX0_PUSHPULL
     47
     48/* Important PITA registers */
     49#define PITA_ICR		0x00	/* Interrupt control register */
     50#define PITA_GPIOICR		0x18	/* GPIO interface control register */
     51#define PITA_MISC		0x1C	/* Miscellaneous register */
     52
     53#define PEAK_PCI_CFG_SIZE	0x1000	/* Size of the config PCI bar */
     54#define PEAK_PCI_CHAN_SIZE	0x0400	/* Size used by the channel */
     55
     56#define PEAK_PCI_VENDOR_ID	0x001C	/* The PCI device and vendor IDs */
     57#define PEAK_PCI_DEVICE_ID	0x0001	/* for PCI/PCIe slot cards */
     58#define PEAK_PCIEC_DEVICE_ID	0x0002	/* for ExpressCard slot cards */
     59#define PEAK_PCIE_DEVICE_ID	0x0003	/* for nextgen PCIe slot cards */
     60#define PEAK_CPCI_DEVICE_ID	0x0004	/* for nextgen cPCI slot cards */
     61#define PEAK_MPCI_DEVICE_ID	0x0005	/* for nextgen miniPCI slot cards */
     62#define PEAK_PC_104P_DEVICE_ID	0x0006	/* PCAN-PC/104+ cards */
     63#define PEAK_PCI_104E_DEVICE_ID	0x0007	/* PCAN-PCI/104 Express cards */
     64#define PEAK_MPCIE_DEVICE_ID	0x0008	/* The miniPCIe slot cards */
     65#define PEAK_PCIE_OEM_ID	0x0009	/* PCAN-PCI Express OEM */
     66#define PEAK_PCIEC34_DEVICE_ID	0x000A	/* PCAN-PCI Express 34 (one channel) */
     67
     68#define PEAK_PCI_CHAN_MAX	4
     69
     70static const u16 peak_pci_icr_masks[PEAK_PCI_CHAN_MAX] = {
     71	0x02, 0x01, 0x40, 0x80
     72};
     73
     74static const struct pci_device_id peak_pci_tbl[] = {
     75	{
     76		PEAK_PCI_VENDOR_ID, PEAK_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     77		.driver_data = (kernel_ulong_t)"PCAN-PCI",
     78	}, {
     79		PEAK_PCI_VENDOR_ID, PEAK_PCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     80		.driver_data = (kernel_ulong_t)"PCAN-PCI Express",
     81	}, {
     82		PEAK_PCI_VENDOR_ID, PEAK_MPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     83		.driver_data = (kernel_ulong_t)"PCAN-miniPCI",
     84	}, {
     85		PEAK_PCI_VENDOR_ID, PEAK_MPCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     86		.driver_data = (kernel_ulong_t)"PCAN-miniPCIe",
     87	}, {
     88		PEAK_PCI_VENDOR_ID, PEAK_PC_104P_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     89		.driver_data = (kernel_ulong_t)"PCAN-PC/104-Plus Quad",
     90	}, {
     91		PEAK_PCI_VENDOR_ID, PEAK_PCI_104E_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     92		.driver_data = (kernel_ulong_t)"PCAN-PCI/104-Express",
     93	}, {
     94		PEAK_PCI_VENDOR_ID, PEAK_CPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
     95		.driver_data = (kernel_ulong_t)"PCAN-cPCI",
     96	}, {
     97		PEAK_PCI_VENDOR_ID, PEAK_PCIE_OEM_ID, PCI_ANY_ID, PCI_ANY_ID,
     98		.driver_data = (kernel_ulong_t)"PCAN-Chip PCIe",
     99	},
    100#ifdef CONFIG_CAN_PEAK_PCIEC
    101	{
    102		PEAK_PCI_VENDOR_ID, PEAK_PCIEC_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
    103		.driver_data = (kernel_ulong_t)"PCAN-ExpressCard",
    104	}, {
    105		PEAK_PCI_VENDOR_ID, PEAK_PCIEC34_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
    106		.driver_data = (kernel_ulong_t)"PCAN-ExpressCard 34",
    107	},
    108#endif
    109	{ /* sentinel */ }
    110};
    111
    112MODULE_DEVICE_TABLE(pci, peak_pci_tbl);
    113
    114#ifdef CONFIG_CAN_PEAK_PCIEC
    115/* PCAN-ExpressCard needs I2C bit-banging configuration option. */
    116
    117/* GPIOICR byte access offsets */
    118#define PITA_GPOUT		0x18	/* GPx output value */
    119#define PITA_GPIN		0x19	/* GPx input value */
    120#define PITA_GPOEN		0x1A	/* configure GPx as output pin */
    121
    122/* I2C GP bits */
    123#define PITA_GPIN_SCL		0x01	/* Serial Clock Line */
    124#define PITA_GPIN_SDA		0x04	/* Serial DAta line */
    125
    126#define PCA9553_1_SLAVEADDR	(0xC4 >> 1)
    127
    128/* PCA9553 LS0 fields values */
    129enum {
    130	PCA9553_LOW,
    131	PCA9553_HIGHZ,
    132	PCA9553_PWM0,
    133	PCA9553_PWM1
    134};
    135
    136/* LEDs control */
    137#define PCA9553_ON		PCA9553_LOW
    138#define PCA9553_OFF		PCA9553_HIGHZ
    139#define PCA9553_SLOW		PCA9553_PWM0
    140#define PCA9553_FAST		PCA9553_PWM1
    141
    142#define PCA9553_LED(c)		(1 << (c))
    143#define PCA9553_LED_STATE(s, c)	((s) << ((c) << 1))
    144
    145#define PCA9553_LED_ON(c)	PCA9553_LED_STATE(PCA9553_ON, c)
    146#define PCA9553_LED_OFF(c)	PCA9553_LED_STATE(PCA9553_OFF, c)
    147#define PCA9553_LED_SLOW(c)	PCA9553_LED_STATE(PCA9553_SLOW, c)
    148#define PCA9553_LED_FAST(c)	PCA9553_LED_STATE(PCA9553_FAST, c)
    149#define PCA9553_LED_MASK(c)	PCA9553_LED_STATE(0x03, c)
    150
    151#define PCA9553_LED_OFF_ALL	(PCA9553_LED_OFF(0) | PCA9553_LED_OFF(1))
    152
    153#define PCA9553_LS0_INIT	0x40 /* initial value (!= from 0x00) */
    154
    155struct peak_pciec_chan {
    156	struct net_device *netdev;
    157	unsigned long prev_rx_bytes;
    158	unsigned long prev_tx_bytes;
    159};
    160
    161struct peak_pciec_card {
    162	void __iomem *cfg_base;		/* Common for all channels */
    163	void __iomem *reg_base;		/* first channel base address */
    164	u8 led_cache;			/* leds state cache */
    165
    166	/* PCIExpressCard i2c data */
    167	struct i2c_algo_bit_data i2c_bit;
    168	struct i2c_adapter led_chip;
    169	struct delayed_work led_work;	/* led delayed work */
    170	int chan_count;
    171	struct peak_pciec_chan channel[PEAK_PCI_CHAN_MAX];
    172};
    173
    174/* "normal" pci register write callback is overloaded for leds control */
    175static void peak_pci_write_reg(const struct sja1000_priv *priv,
    176			       int port, u8 val);
    177
    178static inline void pita_set_scl_highz(struct peak_pciec_card *card)
    179{
    180	u8 gp_outen = readb(card->cfg_base + PITA_GPOEN) & ~PITA_GPIN_SCL;
    181
    182	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
    183}
    184
    185static inline void pita_set_sda_highz(struct peak_pciec_card *card)
    186{
    187	u8 gp_outen = readb(card->cfg_base + PITA_GPOEN) & ~PITA_GPIN_SDA;
    188
    189	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
    190}
    191
    192static void peak_pciec_init_pita_gpio(struct peak_pciec_card *card)
    193{
    194	/* raise SCL & SDA GPIOs to high-Z */
    195	pita_set_scl_highz(card);
    196	pita_set_sda_highz(card);
    197}
    198
    199static void pita_setsda(void *data, int state)
    200{
    201	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
    202	u8 gp_out, gp_outen;
    203
    204	/* set output sda always to 0 */
    205	gp_out = readb(card->cfg_base + PITA_GPOUT) & ~PITA_GPIN_SDA;
    206	writeb(gp_out, card->cfg_base + PITA_GPOUT);
    207
    208	/* control output sda with GPOEN */
    209	gp_outen = readb(card->cfg_base + PITA_GPOEN);
    210	if (state)
    211		gp_outen &= ~PITA_GPIN_SDA;
    212	else
    213		gp_outen |= PITA_GPIN_SDA;
    214
    215	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
    216}
    217
    218static void pita_setscl(void *data, int state)
    219{
    220	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
    221	u8 gp_out, gp_outen;
    222
    223	/* set output scl always to 0 */
    224	gp_out = readb(card->cfg_base + PITA_GPOUT) & ~PITA_GPIN_SCL;
    225	writeb(gp_out, card->cfg_base + PITA_GPOUT);
    226
    227	/* control output scl with GPOEN */
    228	gp_outen = readb(card->cfg_base + PITA_GPOEN);
    229	if (state)
    230		gp_outen &= ~PITA_GPIN_SCL;
    231	else
    232		gp_outen |= PITA_GPIN_SCL;
    233
    234	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
    235}
    236
    237static int pita_getsda(void *data)
    238{
    239	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
    240
    241	/* set tristate */
    242	pita_set_sda_highz(card);
    243
    244	return (readb(card->cfg_base + PITA_GPIN) & PITA_GPIN_SDA) ? 1 : 0;
    245}
    246
    247static int pita_getscl(void *data)
    248{
    249	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
    250
    251	/* set tristate */
    252	pita_set_scl_highz(card);
    253
    254	return (readb(card->cfg_base + PITA_GPIN) & PITA_GPIN_SCL) ? 1 : 0;
    255}
    256
    257/* write commands to the LED chip though the I2C-bus of the PCAN-PCIeC */
    258static int peak_pciec_write_pca9553(struct peak_pciec_card *card,
    259				    u8 offset, u8 data)
    260{
    261	u8 buffer[2] = {
    262		offset,
    263		data
    264	};
    265	struct i2c_msg msg = {
    266		.addr = PCA9553_1_SLAVEADDR,
    267		.len = 2,
    268		.buf = buffer,
    269	};
    270	int ret;
    271
    272	/* cache led mask */
    273	if (offset == 5 && data == card->led_cache)
    274		return 0;
    275
    276	ret = i2c_transfer(&card->led_chip, &msg, 1);
    277	if (ret < 0)
    278		return ret;
    279
    280	if (offset == 5)
    281		card->led_cache = data;
    282
    283	return 0;
    284}
    285
    286/* delayed work callback used to control the LEDs */
    287static void peak_pciec_led_work(struct work_struct *work)
    288{
    289	struct peak_pciec_card *card =
    290		container_of(work, struct peak_pciec_card, led_work.work);
    291	struct net_device *netdev;
    292	u8 new_led = card->led_cache;
    293	int i, up_count = 0;
    294
    295	/* first check what is to do */
    296	for (i = 0; i < card->chan_count; i++) {
    297		/* default is: not configured */
    298		new_led &= ~PCA9553_LED_MASK(i);
    299		new_led |= PCA9553_LED_ON(i);
    300
    301		netdev = card->channel[i].netdev;
    302		if (!netdev || !(netdev->flags & IFF_UP))
    303			continue;
    304
    305		up_count++;
    306
    307		/* no activity (but configured) */
    308		new_led &= ~PCA9553_LED_MASK(i);
    309		new_led |= PCA9553_LED_SLOW(i);
    310
    311		/* if bytes counters changed, set fast blinking led */
    312		if (netdev->stats.rx_bytes != card->channel[i].prev_rx_bytes) {
    313			card->channel[i].prev_rx_bytes = netdev->stats.rx_bytes;
    314			new_led &= ~PCA9553_LED_MASK(i);
    315			new_led |= PCA9553_LED_FAST(i);
    316		}
    317		if (netdev->stats.tx_bytes != card->channel[i].prev_tx_bytes) {
    318			card->channel[i].prev_tx_bytes = netdev->stats.tx_bytes;
    319			new_led &= ~PCA9553_LED_MASK(i);
    320			new_led |= PCA9553_LED_FAST(i);
    321		}
    322	}
    323
    324	/* check if LS0 settings changed, only update i2c if so */
    325	peak_pciec_write_pca9553(card, 5, new_led);
    326
    327	/* restart timer (except if no more configured channels) */
    328	if (up_count)
    329		schedule_delayed_work(&card->led_work, HZ);
    330}
    331
    332/* set LEDs blinking state */
    333static void peak_pciec_set_leds(struct peak_pciec_card *card, u8 led_mask, u8 s)
    334{
    335	u8 new_led = card->led_cache;
    336	int i;
    337
    338	/* first check what is to do */
    339	for (i = 0; i < card->chan_count; i++)
    340		if (led_mask & PCA9553_LED(i)) {
    341			new_led &= ~PCA9553_LED_MASK(i);
    342			new_led |= PCA9553_LED_STATE(s, i);
    343		}
    344
    345	/* check if LS0 settings changed, only update i2c if so */
    346	peak_pciec_write_pca9553(card, 5, new_led);
    347}
    348
    349/* start one second delayed work to control LEDs */
    350static void peak_pciec_start_led_work(struct peak_pciec_card *card)
    351{
    352	schedule_delayed_work(&card->led_work, HZ);
    353}
    354
    355/* stop LEDs delayed work */
    356static void peak_pciec_stop_led_work(struct peak_pciec_card *card)
    357{
    358	cancel_delayed_work_sync(&card->led_work);
    359}
    360
    361/* initialize the PCA9553 4-bit I2C-bus LED chip */
    362static int peak_pciec_init_leds(struct peak_pciec_card *card)
    363{
    364	int err;
    365
    366	/* prescaler for frequency 0: "SLOW" = 1 Hz = "44" */
    367	err = peak_pciec_write_pca9553(card, 1, 44 / 1);
    368	if (err)
    369		return err;
    370
    371	/* duty cycle 0: 50% */
    372	err = peak_pciec_write_pca9553(card, 2, 0x80);
    373	if (err)
    374		return err;
    375
    376	/* prescaler for frequency 1: "FAST" = 5 Hz */
    377	err = peak_pciec_write_pca9553(card, 3, 44 / 5);
    378	if (err)
    379		return err;
    380
    381	/* duty cycle 1: 50% */
    382	err = peak_pciec_write_pca9553(card, 4, 0x80);
    383	if (err)
    384		return err;
    385
    386	/* switch LEDs to initial state */
    387	return peak_pciec_write_pca9553(card, 5, PCA9553_LS0_INIT);
    388}
    389
    390/* restore LEDs state to off peak_pciec_leds_exit */
    391static void peak_pciec_leds_exit(struct peak_pciec_card *card)
    392{
    393	/* switch LEDs to off */
    394	peak_pciec_write_pca9553(card, 5, PCA9553_LED_OFF_ALL);
    395}
    396
    397/* normal write sja1000 register method overloaded to catch when controller
    398 * is started or stopped, to control leds
    399 */
    400static void peak_pciec_write_reg(const struct sja1000_priv *priv,
    401				 int port, u8 val)
    402{
    403	struct peak_pci_chan *chan = priv->priv;
    404	struct peak_pciec_card *card = chan->pciec_card;
    405	int c = (priv->reg_base - card->reg_base) / PEAK_PCI_CHAN_SIZE;
    406
    407	/* sja1000 register changes control the leds state */
    408	if (port == SJA1000_MOD)
    409		switch (val) {
    410		case MOD_RM:
    411			/* Reset Mode: set led on */
    412			peak_pciec_set_leds(card, PCA9553_LED(c), PCA9553_ON);
    413			break;
    414		case 0x00:
    415			/* Normal Mode: led slow blinking and start led timer */
    416			peak_pciec_set_leds(card, PCA9553_LED(c), PCA9553_SLOW);
    417			peak_pciec_start_led_work(card);
    418			break;
    419		default:
    420			break;
    421		}
    422
    423	/* call base function */
    424	peak_pci_write_reg(priv, port, val);
    425}
    426
    427static const struct i2c_algo_bit_data peak_pciec_i2c_bit_ops = {
    428	.setsda	= pita_setsda,
    429	.setscl	= pita_setscl,
    430	.getsda	= pita_getsda,
    431	.getscl	= pita_getscl,
    432	.udelay	= 10,
    433	.timeout = HZ,
    434};
    435
    436static int peak_pciec_probe(struct pci_dev *pdev, struct net_device *dev)
    437{
    438	struct sja1000_priv *priv = netdev_priv(dev);
    439	struct peak_pci_chan *chan = priv->priv;
    440	struct peak_pciec_card *card;
    441	int err;
    442
    443	/* copy i2c object address from 1st channel */
    444	if (chan->prev_dev) {
    445		struct sja1000_priv *prev_priv = netdev_priv(chan->prev_dev);
    446		struct peak_pci_chan *prev_chan = prev_priv->priv;
    447
    448		card = prev_chan->pciec_card;
    449		if (!card)
    450			return -ENODEV;
    451
    452	/* channel is the first one: do the init part */
    453	} else {
    454		/* create the bit banging I2C adapter structure */
    455		card = kzalloc(sizeof(*card), GFP_KERNEL);
    456		if (!card)
    457			return -ENOMEM;
    458
    459		card->cfg_base = chan->cfg_base;
    460		card->reg_base = priv->reg_base;
    461
    462		card->led_chip.owner = THIS_MODULE;
    463		card->led_chip.dev.parent = &pdev->dev;
    464		card->led_chip.algo_data = &card->i2c_bit;
    465		strncpy(card->led_chip.name, "peak_i2c",
    466			sizeof(card->led_chip.name));
    467
    468		card->i2c_bit = peak_pciec_i2c_bit_ops;
    469		card->i2c_bit.udelay = 10;
    470		card->i2c_bit.timeout = HZ;
    471		card->i2c_bit.data = card;
    472
    473		peak_pciec_init_pita_gpio(card);
    474
    475		err = i2c_bit_add_bus(&card->led_chip);
    476		if (err) {
    477			dev_err(&pdev->dev, "i2c init failed\n");
    478			goto pciec_init_err_1;
    479		}
    480
    481		err = peak_pciec_init_leds(card);
    482		if (err) {
    483			dev_err(&pdev->dev, "leds hardware init failed\n");
    484			goto pciec_init_err_2;
    485		}
    486
    487		INIT_DELAYED_WORK(&card->led_work, peak_pciec_led_work);
    488		/* PCAN-ExpressCard needs its own callback for leds */
    489		priv->write_reg = peak_pciec_write_reg;
    490	}
    491
    492	chan->pciec_card = card;
    493	card->channel[card->chan_count++].netdev = dev;
    494
    495	return 0;
    496
    497pciec_init_err_2:
    498	i2c_del_adapter(&card->led_chip);
    499
    500pciec_init_err_1:
    501	peak_pciec_init_pita_gpio(card);
    502	kfree(card);
    503
    504	return err;
    505}
    506
    507static void peak_pciec_remove(struct peak_pciec_card *card)
    508{
    509	peak_pciec_stop_led_work(card);
    510	peak_pciec_leds_exit(card);
    511	i2c_del_adapter(&card->led_chip);
    512	peak_pciec_init_pita_gpio(card);
    513	kfree(card);
    514}
    515
    516#else /* CONFIG_CAN_PEAK_PCIEC */
    517
    518/* Placebo functions when PCAN-ExpressCard support is not selected */
    519static inline int peak_pciec_probe(struct pci_dev *pdev, struct net_device *dev)
    520{
    521	return -ENODEV;
    522}
    523
    524static inline void peak_pciec_remove(struct peak_pciec_card *card)
    525{
    526}
    527#endif /* CONFIG_CAN_PEAK_PCIEC */
    528
    529static u8 peak_pci_read_reg(const struct sja1000_priv *priv, int port)
    530{
    531	return readb(priv->reg_base + (port << 2));
    532}
    533
    534static void peak_pci_write_reg(const struct sja1000_priv *priv,
    535			       int port, u8 val)
    536{
    537	writeb(val, priv->reg_base + (port << 2));
    538}
    539
    540static void peak_pci_post_irq(const struct sja1000_priv *priv)
    541{
    542	struct peak_pci_chan *chan = priv->priv;
    543	u16 icr;
    544
    545	/* Select and clear in PITA stored interrupt */
    546	icr = readw(chan->cfg_base + PITA_ICR);
    547	if (icr & chan->icr_mask)
    548		writew(chan->icr_mask, chan->cfg_base + PITA_ICR);
    549}
    550
    551static int peak_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
    552{
    553	struct sja1000_priv *priv;
    554	struct peak_pci_chan *chan;
    555	struct net_device *dev, *prev_dev;
    556	void __iomem *cfg_base, *reg_base;
    557	u16 sub_sys_id, icr;
    558	int i, err, channels;
    559	char fw_str[14] = "";
    560
    561	err = pci_enable_device(pdev);
    562	if (err)
    563		return err;
    564
    565	err = pci_request_regions(pdev, DRV_NAME);
    566	if (err)
    567		goto failure_disable_pci;
    568
    569	err = pci_read_config_word(pdev, 0x2e, &sub_sys_id);
    570	if (err)
    571		goto failure_release_regions;
    572
    573	dev_dbg(&pdev->dev, "probing device %04x:%04x:%04x\n",
    574		pdev->vendor, pdev->device, sub_sys_id);
    575
    576	err = pci_write_config_word(pdev, 0x44, 0);
    577	if (err)
    578		goto failure_release_regions;
    579
    580	if (sub_sys_id >= 12)
    581		channels = 4;
    582	else if (sub_sys_id >= 10)
    583		channels = 3;
    584	else if (sub_sys_id >= 4)
    585		channels = 2;
    586	else
    587		channels = 1;
    588
    589	cfg_base = pci_iomap(pdev, 0, PEAK_PCI_CFG_SIZE);
    590	if (!cfg_base) {
    591		dev_err(&pdev->dev, "failed to map PCI resource #0\n");
    592		err = -ENOMEM;
    593		goto failure_release_regions;
    594	}
    595
    596	reg_base = pci_iomap(pdev, 1, PEAK_PCI_CHAN_SIZE * channels);
    597	if (!reg_base) {
    598		dev_err(&pdev->dev, "failed to map PCI resource #1\n");
    599		err = -ENOMEM;
    600		goto failure_unmap_cfg_base;
    601	}
    602
    603	/* Set GPIO control register */
    604	writew(0x0005, cfg_base + PITA_GPIOICR + 2);
    605	/* Enable all channels of this card */
    606	writeb(0x00, cfg_base + PITA_GPIOICR);
    607	/* Toggle reset */
    608	writeb(0x05, cfg_base + PITA_MISC + 3);
    609	usleep_range(5000, 6000);
    610	/* Leave parport mux mode */
    611	writeb(0x04, cfg_base + PITA_MISC + 3);
    612
    613	/* FPGA equipped card if not 0 */
    614	if (readl(cfg_base + PEAK_VER_REG1)) {
    615		/* FPGA card: display version of the running firmware */
    616		u32 fw_ver = readl(cfg_base + PEAK_VER_REG2);
    617
    618		snprintf(fw_str, sizeof(fw_str), " FW v%u.%u.%u",
    619			 (fw_ver >> 12) & 0xf,
    620			 (fw_ver >> 8) & 0xf,
    621			 (fw_ver >> 4) & 0xf);
    622	}
    623
    624	/* Display commercial name (and, eventually, FW version) of the card */
    625	dev_info(&pdev->dev, "%ux CAN %s%s\n",
    626		 channels, (const char *)ent->driver_data, fw_str);
    627
    628	icr = readw(cfg_base + PITA_ICR + 2);
    629
    630	for (i = 0; i < channels; i++) {
    631		dev = alloc_sja1000dev(sizeof(struct peak_pci_chan));
    632		if (!dev) {
    633			err = -ENOMEM;
    634			goto failure_remove_channels;
    635		}
    636
    637		priv = netdev_priv(dev);
    638		chan = priv->priv;
    639
    640		chan->cfg_base = cfg_base;
    641		priv->reg_base = reg_base + i * PEAK_PCI_CHAN_SIZE;
    642
    643		priv->read_reg = peak_pci_read_reg;
    644		priv->write_reg = peak_pci_write_reg;
    645		priv->post_irq = peak_pci_post_irq;
    646
    647		priv->can.clock.freq = PEAK_PCI_CAN_CLOCK;
    648		priv->ocr = PEAK_PCI_OCR;
    649		priv->cdr = PEAK_PCI_CDR;
    650		/* Neither a slave nor a single device distributes the clock */
    651		if (channels == 1 || i > 0)
    652			priv->cdr |= CDR_CLK_OFF;
    653
    654		/* Setup interrupt handling */
    655		priv->irq_flags = IRQF_SHARED;
    656		dev->irq = pdev->irq;
    657
    658		chan->icr_mask = peak_pci_icr_masks[i];
    659		icr |= chan->icr_mask;
    660
    661		SET_NETDEV_DEV(dev, &pdev->dev);
    662		dev->dev_id = i;
    663
    664		/* Create chain of SJA1000 devices */
    665		chan->prev_dev = pci_get_drvdata(pdev);
    666		pci_set_drvdata(pdev, dev);
    667
    668		/* PCAN-ExpressCard needs some additional i2c init.
    669		 * This must be done *before* register_sja1000dev() but
    670		 * *after* devices linkage
    671		 */
    672		if (pdev->device == PEAK_PCIEC_DEVICE_ID ||
    673		    pdev->device == PEAK_PCIEC34_DEVICE_ID) {
    674			err = peak_pciec_probe(pdev, dev);
    675			if (err) {
    676				dev_err(&pdev->dev,
    677					"failed to probe device (err %d)\n",
    678					err);
    679				goto failure_free_dev;
    680			}
    681		}
    682
    683		err = register_sja1000dev(dev);
    684		if (err) {
    685			dev_err(&pdev->dev, "failed to register device\n");
    686			goto failure_free_dev;
    687		}
    688
    689		dev_info(&pdev->dev,
    690			 "%s at reg_base=0x%p cfg_base=0x%p irq=%d\n",
    691			 dev->name, priv->reg_base, chan->cfg_base, dev->irq);
    692	}
    693
    694	/* Enable interrupts */
    695	writew(icr, cfg_base + PITA_ICR + 2);
    696
    697	return 0;
    698
    699failure_free_dev:
    700	pci_set_drvdata(pdev, chan->prev_dev);
    701	free_sja1000dev(dev);
    702
    703failure_remove_channels:
    704	/* Disable interrupts */
    705	writew(0x0, cfg_base + PITA_ICR + 2);
    706
    707	chan = NULL;
    708	for (dev = pci_get_drvdata(pdev); dev; dev = prev_dev) {
    709		priv = netdev_priv(dev);
    710		chan = priv->priv;
    711		prev_dev = chan->prev_dev;
    712
    713		unregister_sja1000dev(dev);
    714		free_sja1000dev(dev);
    715	}
    716
    717	/* free any PCIeC resources too */
    718	if (chan && chan->pciec_card)
    719		peak_pciec_remove(chan->pciec_card);
    720
    721	pci_iounmap(pdev, reg_base);
    722
    723failure_unmap_cfg_base:
    724	pci_iounmap(pdev, cfg_base);
    725
    726failure_release_regions:
    727	pci_release_regions(pdev);
    728
    729failure_disable_pci:
    730	pci_disable_device(pdev);
    731
    732	/* pci_xxx_config_word() return positive PCIBIOS_xxx error codes while
    733	 * the probe() function must return a negative errno in case of failure
    734	 * (err is unchanged if negative)
    735	 */
    736	return pcibios_err_to_errno(err);
    737}
    738
    739static void peak_pci_remove(struct pci_dev *pdev)
    740{
    741	struct net_device *dev = pci_get_drvdata(pdev); /* Last device */
    742	struct sja1000_priv *priv = netdev_priv(dev);
    743	struct peak_pci_chan *chan = priv->priv;
    744	void __iomem *cfg_base = chan->cfg_base;
    745	void __iomem *reg_base = priv->reg_base;
    746
    747	/* Disable interrupts */
    748	writew(0x0, cfg_base + PITA_ICR + 2);
    749
    750	/* Loop over all registered devices */
    751	while (1) {
    752		struct net_device *prev_dev = chan->prev_dev;
    753
    754		dev_info(&pdev->dev, "removing device %s\n", dev->name);
    755		/* do that only for first channel */
    756		if (!prev_dev && chan->pciec_card)
    757			peak_pciec_remove(chan->pciec_card);
    758		unregister_sja1000dev(dev);
    759		free_sja1000dev(dev);
    760		dev = prev_dev;
    761
    762		if (!dev)
    763			break;
    764		priv = netdev_priv(dev);
    765		chan = priv->priv;
    766	}
    767
    768	pci_iounmap(pdev, reg_base);
    769	pci_iounmap(pdev, cfg_base);
    770	pci_release_regions(pdev);
    771	pci_disable_device(pdev);
    772}
    773
    774static struct pci_driver peak_pci_driver = {
    775	.name = DRV_NAME,
    776	.id_table = peak_pci_tbl,
    777	.probe = peak_pci_probe,
    778	.remove = peak_pci_remove,
    779};
    780
    781module_pci_driver(peak_pci_driver);