Free Electrons

Embedded Linux Experts

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
/*
 * Copyright (C) 2007, 2011 Wolfgang Grandegger <wg@grandegger.com>
 * Copyright (C) 2012 Stephane Grosjean <s.grosjean@peak-system.com>
 *
 * Derived from the PCAN project file driver/src/pcan_pci.c:
 *
 * Copyright (C) 2001-2006  PEAK System-Technik GmbH
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the version 2 of the GNU General Public License
 * as published by the Free Software Foundation
 *
 * 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.
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/netdevice.h>
#include <linux/delay.h>
#include <linux/pci.h>
#include <linux/io.h>
#include <linux/i2c.h>
#include <linux/i2c-algo-bit.h>
#include <linux/can.h>
#include <linux/can/dev.h>

#include "sja1000.h"

MODULE_AUTHOR("Stephane Grosjean <s.grosjean@peak-system.com>");
MODULE_DESCRIPTION("Socket-CAN driver for PEAK PCAN PCI family cards");
MODULE_SUPPORTED_DEVICE("PEAK PCAN PCI/PCIe/PCIeC miniPCI CAN cards");
MODULE_SUPPORTED_DEVICE("PEAK PCAN miniPCIe/cPCI PC/104+ PCI/104e CAN Cards");
MODULE_LICENSE("GPL v2");

#define DRV_NAME  "peak_pci"

struct peak_pciec_card;
struct peak_pci_chan {
	void __iomem *cfg_base;		/* Common for all channels */
	struct net_device *prev_dev;	/* Chain of network devices */
	u16 icr_mask;			/* Interrupt mask for fast ack */
	struct peak_pciec_card *pciec_card;	/* only for PCIeC LEDs */
};

#define PEAK_PCI_CAN_CLOCK	(16000000 / 2)

#define PEAK_PCI_CDR		(CDR_CBP | CDR_CLKOUT_MASK)
#define PEAK_PCI_OCR		OCR_TX0_PUSHPULL

/*
 * Important PITA registers
 */
#define PITA_ICR		0x00	/* Interrupt control register */
#define PITA_GPIOICR		0x18	/* GPIO interface control register */
#define PITA_MISC		0x1C	/* Miscellaneous register */

#define PEAK_PCI_CFG_SIZE	0x1000	/* Size of the config PCI bar */
#define PEAK_PCI_CHAN_SIZE	0x0400	/* Size used by the channel */

#define PEAK_PCI_VENDOR_ID	0x001C	/* The PCI device and vendor IDs */
#define PEAK_PCI_DEVICE_ID	0x0001	/* for PCI/PCIe slot cards */
#define PEAK_PCIEC_DEVICE_ID	0x0002	/* for ExpressCard slot cards */
#define PEAK_PCIE_DEVICE_ID	0x0003	/* for nextgen PCIe slot cards */
#define PEAK_CPCI_DEVICE_ID	0x0004	/* for nextgen cPCI slot cards */
#define PEAK_MPCI_DEVICE_ID	0x0005	/* for nextgen miniPCI slot cards */
#define PEAK_PC_104P_DEVICE_ID	0x0006	/* PCAN-PC/104+ cards */
#define PEAK_PCI_104E_DEVICE_ID	0x0007	/* PCAN-PCI/104 Express cards */
#define PEAK_MPCIE_DEVICE_ID	0x0008	/* The miniPCIe slot cards */

#define PEAK_PCI_CHAN_MAX	4

static const u16 peak_pci_icr_masks[PEAK_PCI_CHAN_MAX] = {
	0x02, 0x01, 0x40, 0x80
};

static const struct pci_device_id peak_pci_tbl[] = {
	{PEAK_PCI_VENDOR_ID, PEAK_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_PCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_MPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_MPCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_PC_104P_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_PCI_104E_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
	{PEAK_PCI_VENDOR_ID, PEAK_CPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
#ifdef CONFIG_CAN_PEAK_PCIEC
	{PEAK_PCI_VENDOR_ID, PEAK_PCIEC_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
#endif
	{0,}
};

MODULE_DEVICE_TABLE(pci, peak_pci_tbl);

#ifdef CONFIG_CAN_PEAK_PCIEC
/*
 * PCAN-ExpressCard needs I2C bit-banging configuration option.
 */

/* GPIOICR byte access offsets */
#define PITA_GPOUT		0x18	/* GPx output value */
#define PITA_GPIN		0x19	/* GPx input value */
#define PITA_GPOEN		0x1A	/* configure GPx as ouput pin */

/* I2C GP bits */
#define PITA_GPIN_SCL		0x01	/* Serial Clock Line */
#define PITA_GPIN_SDA		0x04	/* Serial DAta line */

#define PCA9553_1_SLAVEADDR	(0xC4 >> 1)

/* PCA9553 LS0 fields values */
enum {
	PCA9553_LOW,
	PCA9553_HIGHZ,
	PCA9553_PWM0,
	PCA9553_PWM1
};

/* LEDs control */
#define PCA9553_ON		PCA9553_LOW
#define PCA9553_OFF		PCA9553_HIGHZ
#define PCA9553_SLOW		PCA9553_PWM0
#define PCA9553_FAST		PCA9553_PWM1

#define PCA9553_LED(c)		(1 << (c))
#define PCA9553_LED_STATE(s, c)	((s) << ((c) << 1))

#define PCA9553_LED_ON(c)	PCA9553_LED_STATE(PCA9553_ON, c)
#define PCA9553_LED_OFF(c)	PCA9553_LED_STATE(PCA9553_OFF, c)
#define PCA9553_LED_SLOW(c)	PCA9553_LED_STATE(PCA9553_SLOW, c)
#define PCA9553_LED_FAST(c)	PCA9553_LED_STATE(PCA9553_FAST, c)
#define PCA9553_LED_MASK(c)	PCA9553_LED_STATE(0x03, c)

#define PCA9553_LED_OFF_ALL	(PCA9553_LED_OFF(0) | PCA9553_LED_OFF(1))

#define PCA9553_LS0_INIT	0x40 /* initial value (!= from 0x00) */

struct peak_pciec_chan {
	struct net_device *netdev;
	unsigned long prev_rx_bytes;
	unsigned long prev_tx_bytes;
};

struct peak_pciec_card {
	void __iomem *cfg_base;		/* Common for all channels */
	void __iomem *reg_base;		/* first channel base address */
	u8 led_cache;			/* leds state cache */

	/* PCIExpressCard i2c data */
	struct i2c_algo_bit_data i2c_bit;
	struct i2c_adapter led_chip;
	struct delayed_work led_work;	/* led delayed work */
	int chan_count;
	struct peak_pciec_chan channel[PEAK_PCI_CHAN_MAX];
};

/* "normal" pci register write callback is overloaded for leds control */
static void peak_pci_write_reg(const struct sja1000_priv *priv,
			       int port, u8 val);

static inline void pita_set_scl_highz(struct peak_pciec_card *card)
{
	u8 gp_outen = readb(card->cfg_base + PITA_GPOEN) & ~PITA_GPIN_SCL;
	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
}

static inline void pita_set_sda_highz(struct peak_pciec_card *card)
{
	u8 gp_outen = readb(card->cfg_base + PITA_GPOEN) & ~PITA_GPIN_SDA;
	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
}

static void peak_pciec_init_pita_gpio(struct peak_pciec_card *card)
{
	/* raise SCL & SDA GPIOs to high-Z */
	pita_set_scl_highz(card);
	pita_set_sda_highz(card);
}

static void pita_setsda(void *data, int state)
{
	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
	u8 gp_out, gp_outen;

	/* set output sda always to 0 */
	gp_out = readb(card->cfg_base + PITA_GPOUT) & ~PITA_GPIN_SDA;
	writeb(gp_out, card->cfg_base + PITA_GPOUT);

	/* control output sda with GPOEN */
	gp_outen = readb(card->cfg_base + PITA_GPOEN);
	if (state)
		gp_outen &= ~PITA_GPIN_SDA;
	else
		gp_outen |= PITA_GPIN_SDA;

	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
}

static void pita_setscl(void *data, int state)
{
	struct peak_pciec_card *card = (struct peak_pciec_card *)data;
	u8 gp_out, gp_outen;

	/* set output scl always to 0 */
	gp_out = readb(card->cfg_base + PITA_GPOUT) & ~PITA_GPIN_SCL;
	writeb(gp_out, card->cfg_base + PITA_GPOUT);

	/* control output scl with GPOEN */
	gp_outen = readb(card->cfg_base + PITA_GPOEN);
	if (state)
		gp_outen &= ~PITA_GPIN_SCL;
	else
		gp_outen |= PITA_GPIN_SCL;

	writeb(gp_outen, card->cfg_base + PITA_GPOEN);
}

static int pita_getsda(void *data)
{
	struct peak_pciec_card *card = (struct peak_pciec_card *)data;

	/* set tristate */
	pita_set_sda_highz(card);

	return (readb(card->cfg_base + PITA_GPIN) & PITA_GPIN_SDA) ? 1 : 0;
}

static int pita_getscl(void *data)
{
	struct peak_pciec_card *card = (struct peak_pciec_card *)data;

	/* set tristate */
	pita_set_scl_highz(card);

	return (readb(card->cfg_base + PITA_GPIN) & PITA_GPIN_SCL) ? 1 : 0;
}

/*
 * write commands to the LED chip though the I2C-bus of the PCAN-PCIeC
 */
static int peak_pciec_write_pca9553(struct peak_pciec_card *card,
				    u8 offset, u8 data)
{
	u8 buffer[2] = {
		offset,
		data
	};
	struct i2c_msg msg = {
		.addr = PCA9553_1_SLAVEADDR,
		.len = 2,
		.buf = buffer,
	};
	int ret;

	/* cache led mask */
	if ((offset == 5) && (data == card->led_cache))
		return 0;

	ret = i2c_transfer(&card->led_chip, &msg, 1);
	if (ret < 0)
		return ret;

	if (offset == 5)
		card->led_cache = data;

	return 0;
}

/*
 * delayed work callback used to control the LEDs
 */
static void peak_pciec_led_work(struct work_struct *work)
{
	struct peak_pciec_card *card =
		container_of(work, struct peak_pciec_card, led_work.work);
	struct net_device *netdev;
	u8 new_led = card->led_cache;
	int i, up_count = 0;

	/* first check what is to do */
	for (i = 0; i < card->chan_count; i++) {
		/* default is: not configured */
		new_led &= ~PCA9553_LED_MASK(i);
		new_led |= PCA9553_LED_ON(i);

		netdev = card->channel[i].netdev;
		if (!netdev || !(netdev->flags & IFF_UP))
			continue;

		up_count++;

		/* no activity (but configured) */
		new_led &= ~PCA9553_LED_MASK(i);
		new_led |= PCA9553_LED_SLOW(i);

		/* if bytes counters changed, set fast blinking led */
		if (netdev->stats.rx_bytes != card->channel[i].prev_rx_bytes) {
			card->channel[i].prev_rx_bytes = netdev->stats.rx_bytes;
			new_led &= ~PCA9553_LED_MASK(i);
			new_led |= PCA9553_LED_FAST(i);
		}
		if (netdev->stats.tx_bytes != card->channel[i].prev_tx_bytes) {
			card->channel[i].prev_tx_bytes = netdev->stats.tx_bytes;
			new_led &= ~PCA9553_LED_MASK(i);
			new_led |= PCA9553_LED_FAST(i);
		}
	}

	/* check if LS0 settings changed, only update i2c if so */
	peak_pciec_write_pca9553(card, 5, new_led);

	/* restart timer (except if no more configured channels) */
	if (up_count)
		schedule_delayed_work(&card->led_work, HZ);
}

/*
 * set LEDs blinking state
 */
static void peak_pciec_set_leds(struct peak_pciec_card *card, u8 led_mask, u8 s)
{
	u8 new_led = card->led_cache;
	int i;

	/* first check what is to do */
	for (i = 0; i < card->chan_count; i++)
		if (led_mask & PCA9553_LED(i)) {
			new_led &= ~PCA9553_LED_MASK(i);
			new_led |= PCA9553_LED_STATE(s, i);
		}

	/* check if LS0 settings changed, only update i2c if so */
	peak_pciec_write_pca9553(card, 5, new_led);
}

/*
 * start one second delayed work to control LEDs
 */
static void peak_pciec_start_led_work(struct peak_pciec_card *card)
{
	schedule_delayed_work(&card->led_work, HZ);
}

/*
 * stop LEDs delayed work
 */
static void peak_pciec_stop_led_work(struct peak_pciec_card *card)
{
	cancel_delayed_work_sync(&card->led_work);
}

/*
 * initialize the PCA9553 4-bit I2C-bus LED chip
 */
static int peak_pciec_init_leds(struct peak_pciec_card *card)
{
	int err;

	/* prescaler for frequency 0: "SLOW" = 1 Hz = "44" */
	err = peak_pciec_write_pca9553(card, 1, 44 / 1);
	if (err)
		return err;

	/* duty cycle 0: 50% */
	err = peak_pciec_write_pca9553(card, 2, 0x80);
	if (err)
		return err;

	/* prescaler for frequency 1: "FAST" = 5 Hz */
	err = peak_pciec_write_pca9553(card, 3, 44 / 5);
	if (err)
		return err;

	/* duty cycle 1: 50% */
	err = peak_pciec_write_pca9553(card, 4, 0x80);
	if (err)
		return err;

	/* switch LEDs to initial state */
	return peak_pciec_write_pca9553(card, 5, PCA9553_LS0_INIT);
}

/*
 * restore LEDs state to off peak_pciec_leds_exit
 */
static void peak_pciec_leds_exit(struct peak_pciec_card *card)
{
	/* switch LEDs to off */
	peak_pciec_write_pca9553(card, 5, PCA9553_LED_OFF_ALL);
}

/*
 * normal write sja1000 register method overloaded to catch when controller
 * is started or stopped, to control leds
 */
static void peak_pciec_write_reg(const struct sja1000_priv *priv,
				 int port, u8 val)
{
	struct peak_pci_chan *chan = priv->priv;
	struct peak_pciec_card *card = chan->pciec_card;
	int c = (priv->reg_base - card->reg_base) / PEAK_PCI_CHAN_SIZE;

	/* sja1000 register changes control the leds state */
	if (port == SJA1000_MOD)
		switch (val) {
		case MOD_RM:
			/* Reset Mode: set led on */
			peak_pciec_set_leds(card, PCA9553_LED(c), PCA9553_ON);
			break;
		case 0x00:
			/* Normal Mode: led slow blinking and start led timer */
			peak_pciec_set_leds(card, PCA9553_LED(c), PCA9553_SLOW);
			peak_pciec_start_led_work(card);
			break;
		default:
			break;
		}

	/* call base function */
	peak_pci_write_reg(priv, port, val);
}

static struct i2c_algo_bit_data peak_pciec_i2c_bit_ops = {
	.setsda	= pita_setsda,
	.setscl	= pita_setscl,
	.getsda	= pita_getsda,
	.getscl	= pita_getscl,
	.udelay	= 10,
	.timeout = HZ,
};

static int peak_pciec_probe(struct pci_dev *pdev, struct net_device *dev)
{
	struct sja1000_priv *priv = netdev_priv(dev);
	struct peak_pci_chan *chan = priv->priv;
	struct peak_pciec_card *card;
	int err;

	/* copy i2c object address from 1st channel */
	if (chan->prev_dev) {
		struct sja1000_priv *prev_priv = netdev_priv(chan->prev_dev);
		struct peak_pci_chan *prev_chan = prev_priv->priv;

		card = prev_chan->pciec_card;
		if (!card)
			return -ENODEV;

	/* channel is the first one: do the init part */
	} else {
		/* create the bit banging I2C adapter structure */
		card = kzalloc(sizeof(struct peak_pciec_card), GFP_KERNEL);
		if (!card)
			return -ENOMEM;

		card->cfg_base = chan->cfg_base;
		card->reg_base = priv->reg_base;

		card->led_chip.owner = THIS_MODULE;
		card->led_chip.dev.parent = &pdev->dev;
		card->led_chip.algo_data = &card->i2c_bit;
		strncpy(card->led_chip.name, "peak_i2c",
			sizeof(card->led_chip.name));

		card->i2c_bit = peak_pciec_i2c_bit_ops;
		card->i2c_bit.udelay = 10;
		card->i2c_bit.timeout = HZ;
		card->i2c_bit.data = card;

		peak_pciec_init_pita_gpio(card);

		err = i2c_bit_add_bus(&card->led_chip);
		if (err) {
			dev_err(&pdev->dev, "i2c init failed\n");
			goto pciec_init_err_1;
		}

		err = peak_pciec_init_leds(card);
		if (err) {
			dev_err(&pdev->dev, "leds hardware init failed\n");
			goto pciec_init_err_2;
		}

		INIT_DELAYED_WORK(&card->led_work, peak_pciec_led_work);
		/* PCAN-ExpressCard needs its own callback for leds */
		priv->write_reg = peak_pciec_write_reg;
	}

	chan->pciec_card = card;
	card->channel[card->chan_count++].netdev = dev;

	return 0;

pciec_init_err_2:
	i2c_del_adapter(&card->led_chip);

pciec_init_err_1:
	peak_pciec_init_pita_gpio(card);
	kfree(card);

	return err;
}

static void peak_pciec_remove(struct peak_pciec_card *card)
{
	peak_pciec_stop_led_work(card);
	peak_pciec_leds_exit(card);
	i2c_del_adapter(&card->led_chip);
	peak_pciec_init_pita_gpio(card);
	kfree(card);
}

#else /* CONFIG_CAN_PEAK_PCIEC */

/*
 * Placebo functions when PCAN-ExpressCard support is not selected
 */
static inline int peak_pciec_probe(struct pci_dev *pdev, struct net_device *dev)
{
	return -ENODEV;
}

static inline void peak_pciec_remove(struct peak_pciec_card *card)
{
}
#endif /* CONFIG_CAN_PEAK_PCIEC */

static u8 peak_pci_read_reg(const struct sja1000_priv *priv, int port)
{
	return readb(priv->reg_base + (port << 2));
}

static void peak_pci_write_reg(const struct sja1000_priv *priv,
			       int port, u8 val)
{
	writeb(val, priv->reg_base + (port << 2));
}

static void peak_pci_post_irq(const struct sja1000_priv *priv)
{
	struct peak_pci_chan *chan = priv->priv;
	u16 icr;

	/* Select and clear in PITA stored interrupt */
	icr = readw(chan->cfg_base + PITA_ICR);
	if (icr & chan->icr_mask)
		writew(chan->icr_mask, chan->cfg_base + PITA_ICR);
}

static int peak_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
{
	struct sja1000_priv *priv;
	struct peak_pci_chan *chan;
	struct net_device *dev, *prev_dev;
	void __iomem *cfg_base, *reg_base;
	u16 sub_sys_id, icr;
	int i, err, channels;

	err = pci_enable_device(pdev);
	if (err)
		return err;

	err = pci_request_regions(pdev, DRV_NAME);
	if (err)
		goto failure_disable_pci;

	err = pci_read_config_word(pdev, 0x2e, &sub_sys_id);
	if (err)
		goto failure_release_regions;

	dev_dbg(&pdev->dev, "probing device %04x:%04x:%04x\n",
		pdev->vendor, pdev->device, sub_sys_id);

	err = pci_write_config_word(pdev, 0x44, 0);
	if (err)
		goto failure_release_regions;

	if (sub_sys_id >= 12)
		channels = 4;
	else if (sub_sys_id >= 10)
		channels = 3;
	else if (sub_sys_id >= 4)
		channels = 2;
	else
		channels = 1;

	cfg_base = pci_iomap(pdev, 0, PEAK_PCI_CFG_SIZE);
	if (!cfg_base) {
		dev_err(&pdev->dev, "failed to map PCI resource #0\n");
		err = -ENOMEM;
		goto failure_release_regions;
	}

	reg_base = pci_iomap(pdev, 1, PEAK_PCI_CHAN_SIZE * channels);
	if (!reg_base) {
		dev_err(&pdev->dev, "failed to map PCI resource #1\n");
		err = -ENOMEM;
		goto failure_unmap_cfg_base;
	}

	/* Set GPIO control register */
	writew(0x0005, cfg_base + PITA_GPIOICR + 2);
	/* Enable all channels of this card */
	writeb(0x00, cfg_base + PITA_GPIOICR);
	/* Toggle reset */
	writeb(0x05, cfg_base + PITA_MISC + 3);
	mdelay(5);
	/* Leave parport mux mode */
	writeb(0x04, cfg_base + PITA_MISC + 3);

	icr = readw(cfg_base + PITA_ICR + 2);

	for (i = 0; i < channels; i++) {
		dev = alloc_sja1000dev(sizeof(struct peak_pci_chan));
		if (!dev) {
			err = -ENOMEM;
			goto failure_remove_channels;
		}

		priv = netdev_priv(dev);
		chan = priv->priv;

		chan->cfg_base = cfg_base;
		priv->reg_base = reg_base + i * PEAK_PCI_CHAN_SIZE;

		priv->read_reg = peak_pci_read_reg;
		priv->write_reg = peak_pci_write_reg;
		priv->post_irq = peak_pci_post_irq;

		priv->can.clock.freq = PEAK_PCI_CAN_CLOCK;
		priv->ocr = PEAK_PCI_OCR;
		priv->cdr = PEAK_PCI_CDR;
		/* Neither a slave nor a single device distributes the clock */
		if (channels == 1 || i > 0)
			priv->cdr |= CDR_CLK_OFF;

		/* Setup interrupt handling */
		priv->irq_flags = IRQF_SHARED;
		dev->irq = pdev->irq;

		chan->icr_mask = peak_pci_icr_masks[i];
		icr |= chan->icr_mask;

		SET_NETDEV_DEV(dev, &pdev->dev);
		dev->dev_id = i;

		/* Create chain of SJA1000 devices */
		chan->prev_dev = pci_get_drvdata(pdev);
		pci_set_drvdata(pdev, dev);

		/*
		 * PCAN-ExpressCard needs some additional i2c init.
		 * This must be done *before* register_sja1000dev() but
		 * *after* devices linkage
		 */
		if (pdev->device == PEAK_PCIEC_DEVICE_ID) {
			err = peak_pciec_probe(pdev, dev);
			if (err) {
				dev_err(&pdev->dev,
					"failed to probe device (err %d)\n",
					err);
				goto failure_free_dev;
			}
		}

		err = register_sja1000dev(dev);
		if (err) {
			dev_err(&pdev->dev, "failed to register device\n");
			goto failure_free_dev;
		}

		dev_info(&pdev->dev,
			 "%s at reg_base=0x%p cfg_base=0x%p irq=%d\n",
			 dev->name, priv->reg_base, chan->cfg_base, dev->irq);
	}

	/* Enable interrupts */
	writew(icr, cfg_base + PITA_ICR + 2);

	return 0;

failure_free_dev:
	pci_set_drvdata(pdev, chan->prev_dev);
	free_sja1000dev(dev);

failure_remove_channels:
	/* Disable interrupts */
	writew(0x0, cfg_base + PITA_ICR + 2);

	chan = NULL;
	for (dev = pci_get_drvdata(pdev); dev; dev = prev_dev) {
		priv = netdev_priv(dev);
		chan = priv->priv;
		prev_dev = chan->prev_dev;

		unregister_sja1000dev(dev);
		free_sja1000dev(dev);
	}

	/* free any PCIeC resources too */
	if (chan && chan->pciec_card)
		peak_pciec_remove(chan->pciec_card);

	pci_iounmap(pdev, reg_base);

failure_unmap_cfg_base:
	pci_iounmap(pdev, cfg_base);

failure_release_regions:
	pci_release_regions(pdev);

failure_disable_pci:
	pci_disable_device(pdev);

	return err;
}

static void peak_pci_remove(struct pci_dev *pdev)
{
	struct net_device *dev = pci_get_drvdata(pdev); /* Last device */
	struct sja1000_priv *priv = netdev_priv(dev);
	struct peak_pci_chan *chan = priv->priv;
	void __iomem *cfg_base = chan->cfg_base;
	void __iomem *reg_base = priv->reg_base;

	/* Disable interrupts */
	writew(0x0, cfg_base + PITA_ICR + 2);

	/* Loop over all registered devices */
	while (1) {
		struct net_device *prev_dev = chan->prev_dev;

		dev_info(&pdev->dev, "removing device %s\n", dev->name);
		unregister_sja1000dev(dev);
		free_sja1000dev(dev);
		dev = prev_dev;

		if (!dev) {
			/* do that only for first channel */
			if (chan->pciec_card)
				peak_pciec_remove(chan->pciec_card);
			break;
		}
		priv = netdev_priv(dev);
		chan = priv->priv;
	}

	pci_iounmap(pdev, reg_base);
	pci_iounmap(pdev, cfg_base);
	pci_release_regions(pdev);
	pci_disable_device(pdev);
}

static struct pci_driver peak_pci_driver = {
	.name = DRV_NAME,
	.id_table = peak_pci_tbl,
	.probe = peak_pci_probe,
	.remove = peak_pci_remove,
};

module_pci_driver(peak_pci_driver);