xref: /openbmc/linux/arch/powerpc/sysdev/fsl_rio.c (revision 726ccdba)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Freescale MPC85xx/MPC86xx RapidIO support
4  *
5  * Copyright 2009 Sysgo AG
6  * Thomas Moll <thomas.moll@sysgo.com>
7  * - fixed maintenance access routines, check for aligned access
8  *
9  * Copyright 2009 Integrated Device Technology, Inc.
10  * Alex Bounine <alexandre.bounine@idt.com>
11  * - Added Port-Write message handling
12  * - Added Machine Check exception handling
13  *
14  * Copyright (C) 2007, 2008, 2010, 2011 Freescale Semiconductor, Inc.
15  * Zhang Wei <wei.zhang@freescale.com>
16  *
17  * Copyright 2005 MontaVista Software, Inc.
18  * Matt Porter <mporter@kernel.crashing.org>
19  */
20 
21 #include <linux/init.h>
22 #include <linux/extable.h>
23 #include <linux/types.h>
24 #include <linux/dma-mapping.h>
25 #include <linux/interrupt.h>
26 #include <linux/device.h>
27 #include <linux/of_address.h>
28 #include <linux/of_irq.h>
29 #include <linux/of_platform.h>
30 #include <linux/delay.h>
31 #include <linux/slab.h>
32 
33 #include <linux/io.h>
34 #include <linux/uaccess.h>
35 #include <asm/machdep.h>
36 
37 #include "fsl_rio.h"
38 
39 #undef DEBUG_PW	/* Port-Write debugging */
40 
41 #define RIO_PORT1_EDCSR		0x0640
42 #define RIO_PORT2_EDCSR		0x0680
43 #define RIO_PORT1_IECSR		0x10130
44 #define RIO_PORT2_IECSR		0x101B0
45 
46 #define RIO_GCCSR		0x13c
47 #define RIO_ESCSR		0x158
48 #define ESCSR_CLEAR		0x07120204
49 #define RIO_PORT2_ESCSR		0x178
50 #define RIO_CCSR		0x15c
51 #define RIO_LTLEDCSR_IER	0x80000000
52 #define RIO_LTLEDCSR_PRT	0x01000000
53 #define IECSR_CLEAR		0x80000000
54 #define RIO_ISR_AACR		0x10120
55 #define RIO_ISR_AACR_AA		0x1	/* Accept All ID */
56 
57 #define RIWTAR_TRAD_VAL_SHIFT	12
58 #define RIWTAR_TRAD_MASK	0x00FFFFFF
59 #define RIWBAR_BADD_VAL_SHIFT	12
60 #define RIWBAR_BADD_MASK	0x003FFFFF
61 #define RIWAR_ENABLE		0x80000000
62 #define RIWAR_TGINT_LOCAL	0x00F00000
63 #define RIWAR_RDTYP_NO_SNOOP	0x00040000
64 #define RIWAR_RDTYP_SNOOP	0x00050000
65 #define RIWAR_WRTYP_NO_SNOOP	0x00004000
66 #define RIWAR_WRTYP_SNOOP	0x00005000
67 #define RIWAR_WRTYP_ALLOC	0x00006000
68 #define RIWAR_SIZE_MASK		0x0000003F
69 
70 static DEFINE_SPINLOCK(fsl_rio_config_lock);
71 
72 #define ___fsl_read_rio_config(x, addr, err, op, barrier)	\
73 	__asm__ __volatile__(				\
74 		"1:	"op" %1,0(%2)\n"		\
75 		"	"barrier"\n"			\
76 		"2:\n"					\
77 		".section .fixup,\"ax\"\n"		\
78 		"3:	li %1,-1\n"			\
79 		"	li %0,%3\n"			\
80 		"	b 2b\n"				\
81 		".previous\n"				\
82 		EX_TABLE(1b, 3b)			\
83 		: "=r" (err), "=r" (x)			\
84 		: "b" (addr), "i" (-EFAULT), "0" (err))
85 
86 #ifdef CONFIG_BOOKE
87 #define __fsl_read_rio_config(x, addr, err, op)	\
88 	___fsl_read_rio_config(x, addr, err, op, "mbar")
89 #else
90 #define __fsl_read_rio_config(x, addr, err, op)	\
91 	___fsl_read_rio_config(x, addr, err, op, "eieio")
92 #endif
93 
94 void __iomem *rio_regs_win;
95 void __iomem *rmu_regs_win;
96 resource_size_t rio_law_start;
97 
98 struct fsl_rio_dbell *dbell;
99 struct fsl_rio_pw *pw;
100 
101 #ifdef CONFIG_PPC_E500
102 int fsl_rio_mcheck_exception(struct pt_regs *regs)
103 {
104 	const struct exception_table_entry *entry;
105 	unsigned long reason;
106 
107 	if (!rio_regs_win)
108 		return 0;
109 
110 	reason = in_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR));
111 	if (reason & (RIO_LTLEDCSR_IER | RIO_LTLEDCSR_PRT)) {
112 		/* Check if we are prepared to handle this fault */
113 		entry = search_exception_tables(regs->nip);
114 		if (entry) {
115 			pr_debug("RIO: %s - MC Exception handled\n",
116 				 __func__);
117 			out_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR),
118 				 0);
119 			regs_set_recoverable(regs);
120 			regs_set_return_ip(regs, extable_fixup(entry));
121 			return 1;
122 		}
123 	}
124 
125 	return 0;
126 }
127 EXPORT_SYMBOL_GPL(fsl_rio_mcheck_exception);
128 #endif
129 
130 /**
131  * fsl_local_config_read - Generate a MPC85xx local config space read
132  * @mport: RapidIO master port info
133  * @index: ID of RapdiIO interface
134  * @offset: Offset into configuration space
135  * @len: Length (in bytes) of the maintenance transaction
136  * @data: Value to be read into
137  *
138  * Generates a MPC85xx local configuration space read. Returns %0 on
139  * success or %-EINVAL on failure.
140  */
141 static int fsl_local_config_read(struct rio_mport *mport,
142 				int index, u32 offset, int len, u32 *data)
143 {
144 	struct rio_priv *priv = mport->priv;
145 	pr_debug("fsl_local_config_read: index %d offset %8.8x\n", index,
146 		 offset);
147 	*data = in_be32(priv->regs_win + offset);
148 
149 	return 0;
150 }
151 
152 /**
153  * fsl_local_config_write - Generate a MPC85xx local config space write
154  * @mport: RapidIO master port info
155  * @index: ID of RapdiIO interface
156  * @offset: Offset into configuration space
157  * @len: Length (in bytes) of the maintenance transaction
158  * @data: Value to be written
159  *
160  * Generates a MPC85xx local configuration space write. Returns %0 on
161  * success or %-EINVAL on failure.
162  */
163 static int fsl_local_config_write(struct rio_mport *mport,
164 				int index, u32 offset, int len, u32 data)
165 {
166 	struct rio_priv *priv = mport->priv;
167 	pr_debug
168 		("fsl_local_config_write: index %d offset %8.8x data %8.8x\n",
169 		index, offset, data);
170 	out_be32(priv->regs_win + offset, data);
171 
172 	return 0;
173 }
174 
175 /**
176  * fsl_rio_config_read - Generate a MPC85xx read maintenance transaction
177  * @mport: RapidIO master port info
178  * @index: ID of RapdiIO interface
179  * @destid: Destination ID of transaction
180  * @hopcount: Number of hops to target device
181  * @offset: Offset into configuration space
182  * @len: Length (in bytes) of the maintenance transaction
183  * @val: Location to be read into
184  *
185  * Generates a MPC85xx read maintenance transaction. Returns %0 on
186  * success or %-EINVAL on failure.
187  */
188 static int
189 fsl_rio_config_read(struct rio_mport *mport, int index, u16 destid,
190 			u8 hopcount, u32 offset, int len, u32 *val)
191 {
192 	struct rio_priv *priv = mport->priv;
193 	unsigned long flags;
194 	u8 *data;
195 	u32 rval, err = 0;
196 
197 	pr_debug
198 		("fsl_rio_config_read:"
199 		" index %d destid %d hopcount %d offset %8.8x len %d\n",
200 		index, destid, hopcount, offset, len);
201 
202 	/* 16MB maintenance window possible */
203 	/* allow only aligned access to maintenance registers */
204 	if (offset > (0x1000000 - len) || !IS_ALIGNED(offset, len))
205 		return -EINVAL;
206 
207 	spin_lock_irqsave(&fsl_rio_config_lock, flags);
208 
209 	out_be32(&priv->maint_atmu_regs->rowtar,
210 		 (destid << 22) | (hopcount << 12) | (offset >> 12));
211 	out_be32(&priv->maint_atmu_regs->rowtear, (destid >> 10));
212 
213 	data = (u8 *) priv->maint_win + (offset & (RIO_MAINT_WIN_SIZE - 1));
214 	switch (len) {
215 	case 1:
216 		__fsl_read_rio_config(rval, data, err, "lbz");
217 		break;
218 	case 2:
219 		__fsl_read_rio_config(rval, data, err, "lhz");
220 		break;
221 	case 4:
222 		__fsl_read_rio_config(rval, data, err, "lwz");
223 		break;
224 	default:
225 		spin_unlock_irqrestore(&fsl_rio_config_lock, flags);
226 		return -EINVAL;
227 	}
228 
229 	if (err) {
230 		pr_debug("RIO: cfg_read error %d for %x:%x:%x\n",
231 			 err, destid, hopcount, offset);
232 	}
233 
234 	spin_unlock_irqrestore(&fsl_rio_config_lock, flags);
235 	*val = rval;
236 
237 	return err;
238 }
239 
240 /**
241  * fsl_rio_config_write - Generate a MPC85xx write maintenance transaction
242  * @mport: RapidIO master port info
243  * @index: ID of RapdiIO interface
244  * @destid: Destination ID of transaction
245  * @hopcount: Number of hops to target device
246  * @offset: Offset into configuration space
247  * @len: Length (in bytes) of the maintenance transaction
248  * @val: Value to be written
249  *
250  * Generates an MPC85xx write maintenance transaction. Returns %0 on
251  * success or %-EINVAL on failure.
252  */
253 static int
254 fsl_rio_config_write(struct rio_mport *mport, int index, u16 destid,
255 			u8 hopcount, u32 offset, int len, u32 val)
256 {
257 	struct rio_priv *priv = mport->priv;
258 	unsigned long flags;
259 	u8 *data;
260 	int ret = 0;
261 
262 	pr_debug
263 		("fsl_rio_config_write:"
264 		" index %d destid %d hopcount %d offset %8.8x len %d val %8.8x\n",
265 		index, destid, hopcount, offset, len, val);
266 
267 	/* 16MB maintenance windows possible */
268 	/* allow only aligned access to maintenance registers */
269 	if (offset > (0x1000000 - len) || !IS_ALIGNED(offset, len))
270 		return -EINVAL;
271 
272 	spin_lock_irqsave(&fsl_rio_config_lock, flags);
273 
274 	out_be32(&priv->maint_atmu_regs->rowtar,
275 		 (destid << 22) | (hopcount << 12) | (offset >> 12));
276 	out_be32(&priv->maint_atmu_regs->rowtear, (destid >> 10));
277 
278 	data = (u8 *) priv->maint_win + (offset & (RIO_MAINT_WIN_SIZE - 1));
279 	switch (len) {
280 	case 1:
281 		out_8((u8 *) data, val);
282 		break;
283 	case 2:
284 		out_be16((u16 *) data, val);
285 		break;
286 	case 4:
287 		out_be32((u32 *) data, val);
288 		break;
289 	default:
290 		ret = -EINVAL;
291 	}
292 	spin_unlock_irqrestore(&fsl_rio_config_lock, flags);
293 
294 	return ret;
295 }
296 
297 static void fsl_rio_inbound_mem_init(struct rio_priv *priv)
298 {
299 	int i;
300 
301 	/* close inbound windows */
302 	for (i = 0; i < RIO_INB_ATMU_COUNT; i++)
303 		out_be32(&priv->inb_atmu_regs[i].riwar, 0);
304 }
305 
306 int fsl_map_inb_mem(struct rio_mport *mport, dma_addr_t lstart,
307 	u64 rstart, u64 size, u32 flags)
308 {
309 	struct rio_priv *priv = mport->priv;
310 	u32 base_size;
311 	unsigned int base_size_log;
312 	u64 win_start, win_end;
313 	u32 riwar;
314 	int i;
315 
316 	if ((size & (size - 1)) != 0 || size > 0x400000000ULL)
317 		return -EINVAL;
318 
319 	base_size_log = ilog2(size);
320 	base_size = 1 << base_size_log;
321 
322 	/* check if addresses are aligned with the window size */
323 	if (lstart & (base_size - 1))
324 		return -EINVAL;
325 	if (rstart & (base_size - 1))
326 		return -EINVAL;
327 
328 	/* check for conflicting ranges */
329 	for (i = 0; i < RIO_INB_ATMU_COUNT; i++) {
330 		riwar = in_be32(&priv->inb_atmu_regs[i].riwar);
331 		if ((riwar & RIWAR_ENABLE) == 0)
332 			continue;
333 		win_start = ((u64)(in_be32(&priv->inb_atmu_regs[i].riwbar) & RIWBAR_BADD_MASK))
334 			<< RIWBAR_BADD_VAL_SHIFT;
335 		win_end = win_start + ((1 << ((riwar & RIWAR_SIZE_MASK) + 1)) - 1);
336 		if (rstart < win_end && (rstart + size) > win_start)
337 			return -EINVAL;
338 	}
339 
340 	/* find unused atmu */
341 	for (i = 0; i < RIO_INB_ATMU_COUNT; i++) {
342 		riwar = in_be32(&priv->inb_atmu_regs[i].riwar);
343 		if ((riwar & RIWAR_ENABLE) == 0)
344 			break;
345 	}
346 	if (i >= RIO_INB_ATMU_COUNT)
347 		return -ENOMEM;
348 
349 	out_be32(&priv->inb_atmu_regs[i].riwtar, lstart >> RIWTAR_TRAD_VAL_SHIFT);
350 	out_be32(&priv->inb_atmu_regs[i].riwbar, rstart >> RIWBAR_BADD_VAL_SHIFT);
351 	out_be32(&priv->inb_atmu_regs[i].riwar, RIWAR_ENABLE | RIWAR_TGINT_LOCAL |
352 		RIWAR_RDTYP_SNOOP | RIWAR_WRTYP_SNOOP | (base_size_log - 1));
353 
354 	return 0;
355 }
356 
357 void fsl_unmap_inb_mem(struct rio_mport *mport, dma_addr_t lstart)
358 {
359 	u32 win_start_shift, base_start_shift;
360 	struct rio_priv *priv = mport->priv;
361 	u32 riwar, riwtar;
362 	int i;
363 
364 	/* skip default window */
365 	base_start_shift = lstart >> RIWTAR_TRAD_VAL_SHIFT;
366 	for (i = 0; i < RIO_INB_ATMU_COUNT; i++) {
367 		riwar = in_be32(&priv->inb_atmu_regs[i].riwar);
368 		if ((riwar & RIWAR_ENABLE) == 0)
369 			continue;
370 
371 		riwtar = in_be32(&priv->inb_atmu_regs[i].riwtar);
372 		win_start_shift = riwtar & RIWTAR_TRAD_MASK;
373 		if (win_start_shift == base_start_shift) {
374 			out_be32(&priv->inb_atmu_regs[i].riwar, riwar & ~RIWAR_ENABLE);
375 			return;
376 		}
377 	}
378 }
379 
380 void fsl_rio_port_error_handler(int offset)
381 {
382 	/*XXX: Error recovery is not implemented, we just clear errors */
383 	out_be32((u32 *)(rio_regs_win + RIO_LTLEDCSR), 0);
384 
385 	if (offset == 0) {
386 		out_be32((u32 *)(rio_regs_win + RIO_PORT1_EDCSR), 0);
387 		out_be32((u32 *)(rio_regs_win + RIO_PORT1_IECSR), IECSR_CLEAR);
388 		out_be32((u32 *)(rio_regs_win + RIO_ESCSR), ESCSR_CLEAR);
389 	} else {
390 		out_be32((u32 *)(rio_regs_win + RIO_PORT2_EDCSR), 0);
391 		out_be32((u32 *)(rio_regs_win + RIO_PORT2_IECSR), IECSR_CLEAR);
392 		out_be32((u32 *)(rio_regs_win + RIO_PORT2_ESCSR), ESCSR_CLEAR);
393 	}
394 }
395 static inline void fsl_rio_info(struct device *dev, u32 ccsr)
396 {
397 	const char *str;
398 	if (ccsr & 1) {
399 		/* Serial phy */
400 		switch (ccsr >> 30) {
401 		case 0:
402 			str = "1";
403 			break;
404 		case 1:
405 			str = "4";
406 			break;
407 		default:
408 			str = "Unknown";
409 			break;
410 		}
411 		dev_info(dev, "Hardware port width: %s\n", str);
412 
413 		switch ((ccsr >> 27) & 7) {
414 		case 0:
415 			str = "Single-lane 0";
416 			break;
417 		case 1:
418 			str = "Single-lane 2";
419 			break;
420 		case 2:
421 			str = "Four-lane";
422 			break;
423 		default:
424 			str = "Unknown";
425 			break;
426 		}
427 		dev_info(dev, "Training connection status: %s\n", str);
428 	} else {
429 		/* Parallel phy */
430 		if (!(ccsr & 0x80000000))
431 			dev_info(dev, "Output port operating in 8-bit mode\n");
432 		if (!(ccsr & 0x08000000))
433 			dev_info(dev, "Input port operating in 8-bit mode\n");
434 	}
435 }
436 
437 /**
438  * fsl_rio_setup - Setup Freescale PowerPC RapidIO interface
439  * @dev: platform_device pointer
440  *
441  * Initializes MPC85xx RapidIO hardware interface, configures
442  * master port with system-specific info, and registers the
443  * master port with the RapidIO subsystem.
444  */
445 int fsl_rio_setup(struct platform_device *dev)
446 {
447 	struct rio_ops *ops;
448 	struct rio_mport *port;
449 	struct rio_priv *priv;
450 	int rc = 0;
451 	const u32 *dt_range, *cell, *port_index;
452 	u32 active_ports = 0;
453 	struct device_node *np, *rmu_node;
454 	int rlen;
455 	u32 ccsr;
456 	u64 range_start, range_size;
457 	int paw, aw, sw;
458 	u32 i;
459 	static int tmp;
460 	struct device_node *rmu_np[MAX_MSG_UNIT_NUM] = {NULL};
461 
462 	if (!dev->dev.of_node) {
463 		dev_err(&dev->dev, "Device OF-Node is NULL");
464 		return -ENODEV;
465 	}
466 
467 	rio_regs_win = of_iomap(dev->dev.of_node, 0);
468 	if (!rio_regs_win) {
469 		dev_err(&dev->dev, "Unable to map rio register window\n");
470 		rc = -ENOMEM;
471 		goto err_rio_regs;
472 	}
473 
474 	ops = kzalloc(sizeof(struct rio_ops), GFP_KERNEL);
475 	if (!ops) {
476 		rc = -ENOMEM;
477 		goto err_ops;
478 	}
479 	ops->lcread = fsl_local_config_read;
480 	ops->lcwrite = fsl_local_config_write;
481 	ops->cread = fsl_rio_config_read;
482 	ops->cwrite = fsl_rio_config_write;
483 	ops->dsend = fsl_rio_doorbell_send;
484 	ops->pwenable = fsl_rio_pw_enable;
485 	ops->open_outb_mbox = fsl_open_outb_mbox;
486 	ops->open_inb_mbox = fsl_open_inb_mbox;
487 	ops->close_outb_mbox = fsl_close_outb_mbox;
488 	ops->close_inb_mbox = fsl_close_inb_mbox;
489 	ops->add_outb_message = fsl_add_outb_message;
490 	ops->add_inb_buffer = fsl_add_inb_buffer;
491 	ops->get_inb_message = fsl_get_inb_message;
492 	ops->map_inb = fsl_map_inb_mem;
493 	ops->unmap_inb = fsl_unmap_inb_mem;
494 
495 	rmu_node = of_parse_phandle(dev->dev.of_node, "fsl,srio-rmu-handle", 0);
496 	if (!rmu_node) {
497 		dev_err(&dev->dev, "No valid fsl,srio-rmu-handle property\n");
498 		rc = -ENOENT;
499 		goto err_rmu;
500 	}
501 	rmu_regs_win = of_iomap(rmu_node, 0);
502 
503 	of_node_put(rmu_node);
504 	if (!rmu_regs_win) {
505 		dev_err(&dev->dev, "Unable to map rmu register window\n");
506 		rc = -ENOMEM;
507 		goto err_rmu;
508 	}
509 	for_each_compatible_node(np, NULL, "fsl,srio-msg-unit") {
510 		rmu_np[tmp] = np;
511 		tmp++;
512 	}
513 
514 	/*set up doobell node*/
515 	np = of_find_compatible_node(NULL, NULL, "fsl,srio-dbell-unit");
516 	if (!np) {
517 		dev_err(&dev->dev, "No fsl,srio-dbell-unit node\n");
518 		rc = -ENODEV;
519 		goto err_dbell;
520 	}
521 	dbell = kzalloc(sizeof(struct fsl_rio_dbell), GFP_KERNEL);
522 	if (!(dbell)) {
523 		dev_err(&dev->dev, "Can't alloc memory for 'fsl_rio_dbell'\n");
524 		rc = -ENOMEM;
525 		goto err_dbell;
526 	}
527 	dbell->dev = &dev->dev;
528 	dbell->bellirq = irq_of_parse_and_map(np, 1);
529 	dev_info(&dev->dev, "bellirq: %d\n", dbell->bellirq);
530 
531 	aw = of_n_addr_cells(np);
532 	dt_range = of_get_property(np, "reg", &rlen);
533 	if (!dt_range) {
534 		pr_err("%pOF: unable to find 'reg' property\n",
535 			np);
536 		rc = -ENOMEM;
537 		goto err_pw;
538 	}
539 	range_start = of_read_number(dt_range, aw);
540 	dbell->dbell_regs = (struct rio_dbell_regs *)(rmu_regs_win +
541 				(u32)range_start);
542 
543 	/*set up port write node*/
544 	np = of_find_compatible_node(NULL, NULL, "fsl,srio-port-write-unit");
545 	if (!np) {
546 		dev_err(&dev->dev, "No fsl,srio-port-write-unit node\n");
547 		rc = -ENODEV;
548 		goto err_pw;
549 	}
550 	pw = kzalloc(sizeof(struct fsl_rio_pw), GFP_KERNEL);
551 	if (!(pw)) {
552 		dev_err(&dev->dev, "Can't alloc memory for 'fsl_rio_pw'\n");
553 		rc = -ENOMEM;
554 		goto err_pw;
555 	}
556 	pw->dev = &dev->dev;
557 	pw->pwirq = irq_of_parse_and_map(np, 0);
558 	dev_info(&dev->dev, "pwirq: %d\n", pw->pwirq);
559 	aw = of_n_addr_cells(np);
560 	dt_range = of_get_property(np, "reg", &rlen);
561 	if (!dt_range) {
562 		pr_err("%pOF: unable to find 'reg' property\n",
563 			np);
564 		rc = -ENOMEM;
565 		goto err;
566 	}
567 	range_start = of_read_number(dt_range, aw);
568 	pw->pw_regs = (struct rio_pw_regs *)(rmu_regs_win + (u32)range_start);
569 
570 	/*set up ports node*/
571 	for_each_child_of_node(dev->dev.of_node, np) {
572 		port_index = of_get_property(np, "cell-index", NULL);
573 		if (!port_index) {
574 			dev_err(&dev->dev, "Can't get %pOF property 'cell-index'\n",
575 					np);
576 			continue;
577 		}
578 
579 		dt_range = of_get_property(np, "ranges", &rlen);
580 		if (!dt_range) {
581 			dev_err(&dev->dev, "Can't get %pOF property 'ranges'\n",
582 					np);
583 			continue;
584 		}
585 
586 		/* Get node address wide */
587 		cell = of_get_property(np, "#address-cells", NULL);
588 		if (cell)
589 			aw = *cell;
590 		else
591 			aw = of_n_addr_cells(np);
592 		/* Get node size wide */
593 		cell = of_get_property(np, "#size-cells", NULL);
594 		if (cell)
595 			sw = *cell;
596 		else
597 			sw = of_n_size_cells(np);
598 		/* Get parent address wide wide */
599 		paw = of_n_addr_cells(np);
600 		range_start = of_read_number(dt_range + aw, paw);
601 		range_size = of_read_number(dt_range + aw + paw, sw);
602 
603 		dev_info(&dev->dev, "%pOF: LAW start 0x%016llx, size 0x%016llx.\n",
604 				np, range_start, range_size);
605 
606 		port = kzalloc(sizeof(struct rio_mport), GFP_KERNEL);
607 		if (!port)
608 			continue;
609 
610 		rc = rio_mport_initialize(port);
611 		if (rc) {
612 			kfree(port);
613 			continue;
614 		}
615 
616 		i = *port_index - 1;
617 		port->index = (unsigned char)i;
618 
619 		priv = kzalloc(sizeof(struct rio_priv), GFP_KERNEL);
620 		if (!priv) {
621 			dev_err(&dev->dev, "Can't alloc memory for 'priv'\n");
622 			kfree(port);
623 			continue;
624 		}
625 
626 		INIT_LIST_HEAD(&port->dbells);
627 		port->iores.start = range_start;
628 		port->iores.end = port->iores.start + range_size - 1;
629 		port->iores.flags = IORESOURCE_MEM;
630 		port->iores.name = "rio_io_win";
631 
632 		if (request_resource(&iomem_resource, &port->iores) < 0) {
633 			dev_err(&dev->dev, "RIO: Error requesting master port region"
634 				" 0x%016llx-0x%016llx\n",
635 				(u64)port->iores.start, (u64)port->iores.end);
636 				kfree(priv);
637 				kfree(port);
638 				continue;
639 		}
640 		sprintf(port->name, "RIO mport %d", i);
641 
642 		priv->dev = &dev->dev;
643 		port->dev.parent = &dev->dev;
644 		port->ops = ops;
645 		port->priv = priv;
646 		port->phys_efptr = 0x100;
647 		port->phys_rmap = 1;
648 		priv->regs_win = rio_regs_win;
649 
650 		ccsr = in_be32(priv->regs_win + RIO_CCSR + i*0x20);
651 
652 		/* Checking the port training status */
653 		if (in_be32((priv->regs_win + RIO_ESCSR + i*0x20)) & 1) {
654 			dev_err(&dev->dev, "Port %d is not ready. "
655 			"Try to restart connection...\n", i);
656 			/* Disable ports */
657 			out_be32(priv->regs_win
658 				+ RIO_CCSR + i*0x20, 0);
659 			/* Set 1x lane */
660 			setbits32(priv->regs_win
661 				+ RIO_CCSR + i*0x20, 0x02000000);
662 			/* Enable ports */
663 			setbits32(priv->regs_win
664 				+ RIO_CCSR + i*0x20, 0x00600000);
665 			msleep(100);
666 			if (in_be32((priv->regs_win
667 					+ RIO_ESCSR + i*0x20)) & 1) {
668 				dev_err(&dev->dev,
669 					"Port %d restart failed.\n", i);
670 				release_resource(&port->iores);
671 				kfree(priv);
672 				kfree(port);
673 				continue;
674 			}
675 			dev_info(&dev->dev, "Port %d restart success!\n", i);
676 		}
677 		fsl_rio_info(&dev->dev, ccsr);
678 
679 		port->sys_size = (in_be32((priv->regs_win + RIO_PEF_CAR))
680 					& RIO_PEF_CTLS) >> 4;
681 		dev_info(&dev->dev, "RapidIO Common Transport System size: %d\n",
682 				port->sys_size ? 65536 : 256);
683 
684 		if (port->host_deviceid >= 0)
685 			out_be32(priv->regs_win + RIO_GCCSR, RIO_PORT_GEN_HOST |
686 				RIO_PORT_GEN_MASTER | RIO_PORT_GEN_DISCOVERED);
687 		else
688 			out_be32(priv->regs_win + RIO_GCCSR,
689 				RIO_PORT_GEN_MASTER);
690 
691 		priv->atmu_regs = (struct rio_atmu_regs *)(priv->regs_win
692 			+ ((i == 0) ? RIO_ATMU_REGS_PORT1_OFFSET :
693 			RIO_ATMU_REGS_PORT2_OFFSET));
694 
695 		priv->maint_atmu_regs = priv->atmu_regs + 1;
696 		priv->inb_atmu_regs = (struct rio_inb_atmu_regs __iomem *)
697 			(priv->regs_win +
698 			((i == 0) ? RIO_INB_ATMU_REGS_PORT1_OFFSET :
699 			RIO_INB_ATMU_REGS_PORT2_OFFSET));
700 
701 		/* Set to receive packets with any dest ID */
702 		out_be32((priv->regs_win + RIO_ISR_AACR + i*0x80),
703 			 RIO_ISR_AACR_AA);
704 
705 		/* Configure maintenance transaction window */
706 		out_be32(&priv->maint_atmu_regs->rowbar,
707 			port->iores.start >> 12);
708 		out_be32(&priv->maint_atmu_regs->rowar,
709 			 0x80077000 | (ilog2(RIO_MAINT_WIN_SIZE) - 1));
710 
711 		priv->maint_win = ioremap(port->iores.start,
712 				RIO_MAINT_WIN_SIZE);
713 
714 		rio_law_start = range_start;
715 
716 		fsl_rio_setup_rmu(port, rmu_np[i]);
717 		fsl_rio_inbound_mem_init(priv);
718 
719 		dbell->mport[i] = port;
720 		pw->mport[i] = port;
721 
722 		if (rio_register_mport(port)) {
723 			release_resource(&port->iores);
724 			kfree(priv);
725 			kfree(port);
726 			continue;
727 		}
728 		active_ports++;
729 	}
730 
731 	if (!active_ports) {
732 		rc = -ENOLINK;
733 		goto err;
734 	}
735 
736 	fsl_rio_doorbell_init(dbell);
737 	fsl_rio_port_write_init(pw);
738 
739 	return 0;
740 err:
741 	kfree(pw);
742 	pw = NULL;
743 err_pw:
744 	kfree(dbell);
745 	dbell = NULL;
746 err_dbell:
747 	iounmap(rmu_regs_win);
748 	rmu_regs_win = NULL;
749 err_rmu:
750 	kfree(ops);
751 err_ops:
752 	iounmap(rio_regs_win);
753 	rio_regs_win = NULL;
754 err_rio_regs:
755 	return rc;
756 }
757 
758 /* The probe function for RapidIO peer-to-peer network.
759  */
760 static int fsl_of_rio_rpn_probe(struct platform_device *dev)
761 {
762 	printk(KERN_INFO "Setting up RapidIO peer-to-peer network %pOF\n",
763 			dev->dev.of_node);
764 
765 	return fsl_rio_setup(dev);
766 };
767 
768 static const struct of_device_id fsl_of_rio_rpn_ids[] = {
769 	{
770 		.compatible = "fsl,srio",
771 	},
772 	{},
773 };
774 
775 static struct platform_driver fsl_of_rio_rpn_driver = {
776 	.driver = {
777 		.name = "fsl-of-rio",
778 		.of_match_table = fsl_of_rio_rpn_ids,
779 	},
780 	.probe = fsl_of_rio_rpn_probe,
781 };
782 
783 static __init int fsl_of_rio_rpn_init(void)
784 {
785 	return platform_driver_register(&fsl_of_rio_rpn_driver);
786 }
787 
788 subsys_initcall(fsl_of_rio_rpn_init);
789