1 /*****************************************************************************
2  *
3  *     Author: Xilinx, Inc.
4  *
5  *     This program is free software; you can redistribute it and/or modify it
6  *     under the terms of the GNU General Public License as published by the
7  *     Free Software Foundation; either version 2 of the License, or (at your
8  *     option) any later version.
9  *
10  *     XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS"
11  *     AS A COURTESY TO YOU, SOLELY FOR USE IN DEVELOPING PROGRAMS AND
12  *     SOLUTIONS FOR XILINX DEVICES.  BY PROVIDING THIS DESIGN, CODE,
13  *     OR INFORMATION AS ONE POSSIBLE IMPLEMENTATION OF THIS FEATURE,
14  *     APPLICATION OR STANDARD, XILINX IS MAKING NO REPRESENTATION
15  *     THAT THIS IMPLEMENTATION IS FREE FROM ANY CLAIMS OF INFRINGEMENT,
16  *     AND YOU ARE RESPONSIBLE FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE
17  *     FOR YOUR IMPLEMENTATION.  XILINX EXPRESSLY DISCLAIMS ANY
18  *     WARRANTY WHATSOEVER WITH RESPECT TO THE ADEQUACY OF THE
19  *     IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OR
20  *     REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE FROM CLAIMS OF
21  *     INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  *     FOR A PARTICULAR PURPOSE.
23  *
24  *     (c) Copyright 2002 Xilinx Inc., Systems Engineering Group
25  *     (c) Copyright 2004 Xilinx Inc., Systems Engineering Group
26  *     (c) Copyright 2007-2008 Xilinx Inc.
27  *     All rights reserved.
28  *
29  *     You should have received a copy of the GNU General Public License along
30  *     with this program; if not, write to the Free Software Foundation, Inc.,
31  *     675 Mass Ave, Cambridge, MA 02139, USA.
32  *
33  *****************************************************************************/
34 
35 /*
36  * This is the code behind /dev/icap* -- it allows a user-space
37  * application to use the Xilinx ICAP subsystem.
38  *
39  * The following operations are possible:
40  *
41  * open         open the port and initialize for access.
42  * release      release port
43  * write        Write a bitstream to the configuration processor.
44  * read         Read a data stream from the configuration processor.
45  *
46  * After being opened, the port is initialized and accessed to avoid a
47  * corrupted first read which may occur with some hardware.  The port
48  * is left in a desynched state, requiring that a synch sequence be
49  * transmitted before any valid configuration data.  A user will have
50  * exclusive access to the device while it remains open, and the state
51  * of the ICAP cannot be guaranteed after the device is closed.  Note
52  * that a complete reset of the core and the state of the ICAP cannot
53  * be performed on many versions of the cores, hence users of this
54  * device should avoid making inconsistent accesses to the device.  In
55  * particular, accessing the read interface, without first generating
56  * a write containing a readback packet can leave the ICAP in an
57  * inaccessible state.
58  *
59  * Note that in order to use the read interface, it is first necessary
60  * to write a request packet to the write interface.  i.e., it is not
61  * possible to simply readback the bitstream (or any configuration
62  * bits) from a device without specifically requesting them first.
63  * The code to craft such packets is intended to be part of the
64  * user-space application code that uses this device.  The simplest
65  * way to use this interface is simply:
66  *
67  * cp foo.bit /dev/icap0
68  *
69  * Note that unless foo.bit is an appropriately constructed partial
70  * bitstream, this has a high likelihood of overwriting the design
71  * currently programmed in the FPGA.
72  */
73 
74 #include <linux/module.h>
75 #include <linux/kernel.h>
76 #include <linux/types.h>
77 #include <linux/ioport.h>
78 #include <linux/interrupt.h>
79 #include <linux/fcntl.h>
80 #include <linux/init.h>
81 #include <linux/poll.h>
82 #include <linux/proc_fs.h>
83 #include <linux/mutex.h>
84 #include <linux/sysctl.h>
85 #include <linux/fs.h>
86 #include <linux/cdev.h>
87 #include <linux/platform_device.h>
88 #include <linux/slab.h>
89 #include <linux/io.h>
90 #include <linux/uaccess.h>
91 
92 #ifdef CONFIG_OF
93 /* For open firmware. */
94 #include <linux/of_address.h>
95 #include <linux/of_device.h>
96 #include <linux/of_platform.h>
97 #endif
98 
99 #include "xilinx_hwicap.h"
100 #include "buffer_icap.h"
101 #include "fifo_icap.h"
102 
103 #define DRIVER_NAME "icap"
104 
105 #define HWICAP_REGS   (0x10000)
106 
107 #define XHWICAP_MAJOR 259
108 #define XHWICAP_MINOR 0
109 #define HWICAP_DEVICES 1
110 
111 /* An array, which is set to true when the device is registered. */
112 static DEFINE_MUTEX(hwicap_mutex);
113 static bool probed_devices[HWICAP_DEVICES];
114 static struct mutex icap_sem;
115 
116 static const struct class icap_class = {
117 	.name = "xilinx_config",
118 };
119 
120 #define UNIMPLEMENTED 0xFFFF
121 
122 static const struct config_registers v2_config_registers = {
123 	.CRC = 0,
124 	.FAR = 1,
125 	.FDRI = 2,
126 	.FDRO = 3,
127 	.CMD = 4,
128 	.CTL = 5,
129 	.MASK = 6,
130 	.STAT = 7,
131 	.LOUT = 8,
132 	.COR = 9,
133 	.MFWR = 10,
134 	.FLR = 11,
135 	.KEY = 12,
136 	.CBC = 13,
137 	.IDCODE = 14,
138 	.AXSS = UNIMPLEMENTED,
139 	.C0R_1 = UNIMPLEMENTED,
140 	.CSOB = UNIMPLEMENTED,
141 	.WBSTAR = UNIMPLEMENTED,
142 	.TIMER = UNIMPLEMENTED,
143 	.BOOTSTS = UNIMPLEMENTED,
144 	.CTL_1 = UNIMPLEMENTED,
145 };
146 
147 static const struct config_registers v4_config_registers = {
148 	.CRC = 0,
149 	.FAR = 1,
150 	.FDRI = 2,
151 	.FDRO = 3,
152 	.CMD = 4,
153 	.CTL = 5,
154 	.MASK = 6,
155 	.STAT = 7,
156 	.LOUT = 8,
157 	.COR = 9,
158 	.MFWR = 10,
159 	.FLR = UNIMPLEMENTED,
160 	.KEY = UNIMPLEMENTED,
161 	.CBC = 11,
162 	.IDCODE = 12,
163 	.AXSS = 13,
164 	.C0R_1 = UNIMPLEMENTED,
165 	.CSOB = UNIMPLEMENTED,
166 	.WBSTAR = UNIMPLEMENTED,
167 	.TIMER = UNIMPLEMENTED,
168 	.BOOTSTS = UNIMPLEMENTED,
169 	.CTL_1 = UNIMPLEMENTED,
170 };
171 
172 static const struct config_registers v5_config_registers = {
173 	.CRC = 0,
174 	.FAR = 1,
175 	.FDRI = 2,
176 	.FDRO = 3,
177 	.CMD = 4,
178 	.CTL = 5,
179 	.MASK = 6,
180 	.STAT = 7,
181 	.LOUT = 8,
182 	.COR = 9,
183 	.MFWR = 10,
184 	.FLR = UNIMPLEMENTED,
185 	.KEY = UNIMPLEMENTED,
186 	.CBC = 11,
187 	.IDCODE = 12,
188 	.AXSS = 13,
189 	.C0R_1 = 14,
190 	.CSOB = 15,
191 	.WBSTAR = 16,
192 	.TIMER = 17,
193 	.BOOTSTS = 18,
194 	.CTL_1 = 19,
195 };
196 
197 static const struct config_registers v6_config_registers = {
198 	.CRC = 0,
199 	.FAR = 1,
200 	.FDRI = 2,
201 	.FDRO = 3,
202 	.CMD = 4,
203 	.CTL = 5,
204 	.MASK = 6,
205 	.STAT = 7,
206 	.LOUT = 8,
207 	.COR = 9,
208 	.MFWR = 10,
209 	.FLR = UNIMPLEMENTED,
210 	.KEY = UNIMPLEMENTED,
211 	.CBC = 11,
212 	.IDCODE = 12,
213 	.AXSS = 13,
214 	.C0R_1 = 14,
215 	.CSOB = 15,
216 	.WBSTAR = 16,
217 	.TIMER = 17,
218 	.BOOTSTS = 22,
219 	.CTL_1 = 24,
220 };
221 
222 /**
223  * hwicap_command_desync - Send a DESYNC command to the ICAP port.
224  * @drvdata: a pointer to the drvdata.
225  *
226  * Returns: '0' on success and failure value on error
227  *
228  * This command desynchronizes the ICAP After this command, a
229  * bitstream containing a NULL packet, followed by a SYNCH packet is
230  * required before the ICAP will recognize commands.
231  */
232 static int hwicap_command_desync(struct hwicap_drvdata *drvdata)
233 {
234 	u32 buffer[4];
235 	u32 index = 0;
236 
237 	/*
238 	 * Create the data to be written to the ICAP.
239 	 */
240 	buffer[index++] = hwicap_type_1_write(drvdata->config_regs->CMD) | 1;
241 	buffer[index++] = XHI_CMD_DESYNCH;
242 	buffer[index++] = XHI_NOOP_PACKET;
243 	buffer[index++] = XHI_NOOP_PACKET;
244 
245 	/*
246 	 * Write the data to the FIFO and initiate the transfer of data present
247 	 * in the FIFO to the ICAP device.
248 	 */
249 	return drvdata->config->set_configuration(drvdata,
250 			&buffer[0], index);
251 }
252 
253 /**
254  * hwicap_get_configuration_register - Query a configuration register.
255  * @drvdata: a pointer to the drvdata.
256  * @reg: a constant which represents the configuration
257  * register value to be returned.
258  * Examples: XHI_IDCODE, XHI_FLR.
259  * @reg_data: returns the value of the register.
260  *
261  * Returns: '0' on success and failure value on error
262  *
263  * Sends a query packet to the ICAP and then receives the response.
264  * The icap is left in Synched state.
265  */
266 static int hwicap_get_configuration_register(struct hwicap_drvdata *drvdata,
267 		u32 reg, u32 *reg_data)
268 {
269 	int status;
270 	u32 buffer[6];
271 	u32 index = 0;
272 
273 	/*
274 	 * Create the data to be written to the ICAP.
275 	 */
276 	buffer[index++] = XHI_DUMMY_PACKET;
277 	buffer[index++] = XHI_NOOP_PACKET;
278 	buffer[index++] = XHI_SYNC_PACKET;
279 	buffer[index++] = XHI_NOOP_PACKET;
280 	buffer[index++] = XHI_NOOP_PACKET;
281 
282 	/*
283 	 * Write the data to the FIFO and initiate the transfer of data present
284 	 * in the FIFO to the ICAP device.
285 	 */
286 	status = drvdata->config->set_configuration(drvdata,
287 						    &buffer[0], index);
288 	if (status)
289 		return status;
290 
291 	/* If the syncword was not found, then we need to start over. */
292 	status = drvdata->config->get_status(drvdata);
293 	if ((status & XHI_SR_DALIGN_MASK) != XHI_SR_DALIGN_MASK)
294 		return -EIO;
295 
296 	index = 0;
297 	buffer[index++] = hwicap_type_1_read(reg) | 1;
298 	buffer[index++] = XHI_NOOP_PACKET;
299 	buffer[index++] = XHI_NOOP_PACKET;
300 
301 	/*
302 	 * Write the data to the FIFO and initiate the transfer of data present
303 	 * in the FIFO to the ICAP device.
304 	 */
305 	status = drvdata->config->set_configuration(drvdata,
306 			&buffer[0], index);
307 	if (status)
308 		return status;
309 
310 	/*
311 	 * Read the configuration register
312 	 */
313 	status = drvdata->config->get_configuration(drvdata, reg_data, 1);
314 	if (status)
315 		return status;
316 
317 	return 0;
318 }
319 
320 static int hwicap_initialize_hwicap(struct hwicap_drvdata *drvdata)
321 {
322 	int status;
323 	u32 idcode;
324 
325 	dev_dbg(drvdata->dev, "initializing\n");
326 
327 	/* Abort any current transaction, to make sure we have the
328 	 * ICAP in a good state.
329 	 */
330 	dev_dbg(drvdata->dev, "Reset...\n");
331 	drvdata->config->reset(drvdata);
332 
333 	dev_dbg(drvdata->dev, "Desync...\n");
334 	status = hwicap_command_desync(drvdata);
335 	if (status)
336 		return status;
337 
338 	/* Attempt to read the IDCODE from ICAP.  This
339 	 * may not be returned correctly, due to the design of the
340 	 * hardware.
341 	 */
342 	dev_dbg(drvdata->dev, "Reading IDCODE...\n");
343 	status = hwicap_get_configuration_register(
344 			drvdata, drvdata->config_regs->IDCODE, &idcode);
345 	dev_dbg(drvdata->dev, "IDCODE = %x\n", idcode);
346 	if (status)
347 		return status;
348 
349 	dev_dbg(drvdata->dev, "Desync...\n");
350 	status = hwicap_command_desync(drvdata);
351 	if (status)
352 		return status;
353 
354 	return 0;
355 }
356 
357 static ssize_t
358 hwicap_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
359 {
360 	struct hwicap_drvdata *drvdata = file->private_data;
361 	ssize_t bytes_to_read = 0;
362 	u32 *kbuf;
363 	u32 words;
364 	u32 bytes_remaining;
365 	int status;
366 
367 	status = mutex_lock_interruptible(&drvdata->sem);
368 	if (status)
369 		return status;
370 
371 	if (drvdata->read_buffer_in_use) {
372 		/* If there are leftover bytes in the buffer, just */
373 		/* return them and don't try to read more from the */
374 		/* ICAP device. */
375 		bytes_to_read =
376 			(count < drvdata->read_buffer_in_use) ? count :
377 			drvdata->read_buffer_in_use;
378 
379 		/* Return the data currently in the read buffer. */
380 		if (copy_to_user(buf, drvdata->read_buffer, bytes_to_read)) {
381 			status = -EFAULT;
382 			goto error;
383 		}
384 		drvdata->read_buffer_in_use -= bytes_to_read;
385 		memmove(drvdata->read_buffer,
386 		       drvdata->read_buffer + bytes_to_read,
387 		       4 - bytes_to_read);
388 	} else {
389 		/* Get new data from the ICAP, and return what was requested. */
390 		kbuf = (u32 *) get_zeroed_page(GFP_KERNEL);
391 		if (!kbuf) {
392 			status = -ENOMEM;
393 			goto error;
394 		}
395 
396 		/* The ICAP device is only able to read complete */
397 		/* words.  If a number of bytes that do not correspond */
398 		/* to complete words is requested, then we read enough */
399 		/* words to get the required number of bytes, and then */
400 		/* save the remaining bytes for the next read. */
401 
402 		/* Determine the number of words to read, rounding up */
403 		/* if necessary. */
404 		words = ((count + 3) >> 2);
405 		bytes_to_read = words << 2;
406 
407 		if (bytes_to_read > PAGE_SIZE)
408 			bytes_to_read = PAGE_SIZE;
409 
410 		/* Ensure we only read a complete number of words. */
411 		bytes_remaining = bytes_to_read & 3;
412 		bytes_to_read &= ~3;
413 		words = bytes_to_read >> 2;
414 
415 		status = drvdata->config->get_configuration(drvdata,
416 				kbuf, words);
417 
418 		/* If we didn't read correctly, then bail out. */
419 		if (status) {
420 			free_page((unsigned long)kbuf);
421 			goto error;
422 		}
423 
424 		/* If we fail to return the data to the user, then bail out. */
425 		if (copy_to_user(buf, kbuf, bytes_to_read)) {
426 			free_page((unsigned long)kbuf);
427 			status = -EFAULT;
428 			goto error;
429 		}
430 		memcpy(drvdata->read_buffer,
431 		       kbuf,
432 		       bytes_remaining);
433 		drvdata->read_buffer_in_use = bytes_remaining;
434 		free_page((unsigned long)kbuf);
435 	}
436 	status = bytes_to_read;
437  error:
438 	mutex_unlock(&drvdata->sem);
439 	return status;
440 }
441 
442 static ssize_t
443 hwicap_write(struct file *file, const char __user *buf,
444 		size_t count, loff_t *ppos)
445 {
446 	struct hwicap_drvdata *drvdata = file->private_data;
447 	ssize_t written = 0;
448 	ssize_t left = count;
449 	u32 *kbuf;
450 	ssize_t len;
451 	ssize_t status;
452 
453 	status = mutex_lock_interruptible(&drvdata->sem);
454 	if (status)
455 		return status;
456 
457 	left += drvdata->write_buffer_in_use;
458 
459 	/* Only write multiples of 4 bytes. */
460 	if (left < 4) {
461 		status = 0;
462 		goto error;
463 	}
464 
465 	kbuf = (u32 *) __get_free_page(GFP_KERNEL);
466 	if (!kbuf) {
467 		status = -ENOMEM;
468 		goto error;
469 	}
470 
471 	while (left > 3) {
472 		/* only write multiples of 4 bytes, so there might */
473 		/* be as many as 3 bytes left (at the end). */
474 		len = left;
475 
476 		if (len > PAGE_SIZE)
477 			len = PAGE_SIZE;
478 		len &= ~3;
479 
480 		if (drvdata->write_buffer_in_use) {
481 			memcpy(kbuf, drvdata->write_buffer,
482 					drvdata->write_buffer_in_use);
483 			if (copy_from_user(
484 			    (((char *)kbuf) + drvdata->write_buffer_in_use),
485 			    buf + written,
486 			    len - (drvdata->write_buffer_in_use))) {
487 				free_page((unsigned long)kbuf);
488 				status = -EFAULT;
489 				goto error;
490 			}
491 		} else {
492 			if (copy_from_user(kbuf, buf + written, len)) {
493 				free_page((unsigned long)kbuf);
494 				status = -EFAULT;
495 				goto error;
496 			}
497 		}
498 
499 		status = drvdata->config->set_configuration(drvdata,
500 				kbuf, len >> 2);
501 
502 		if (status) {
503 			free_page((unsigned long)kbuf);
504 			status = -EFAULT;
505 			goto error;
506 		}
507 		if (drvdata->write_buffer_in_use) {
508 			len -= drvdata->write_buffer_in_use;
509 			left -= drvdata->write_buffer_in_use;
510 			drvdata->write_buffer_in_use = 0;
511 		}
512 		written += len;
513 		left -= len;
514 	}
515 	if ((left > 0) && (left < 4)) {
516 		if (!copy_from_user(drvdata->write_buffer,
517 						buf + written, left)) {
518 			drvdata->write_buffer_in_use = left;
519 			written += left;
520 			left = 0;
521 		}
522 	}
523 
524 	free_page((unsigned long)kbuf);
525 	status = written;
526  error:
527 	mutex_unlock(&drvdata->sem);
528 	return status;
529 }
530 
531 static int hwicap_open(struct inode *inode, struct file *file)
532 {
533 	struct hwicap_drvdata *drvdata;
534 	int status;
535 
536 	mutex_lock(&hwicap_mutex);
537 	drvdata = container_of(inode->i_cdev, struct hwicap_drvdata, cdev);
538 
539 	status = mutex_lock_interruptible(&drvdata->sem);
540 	if (status)
541 		goto out;
542 
543 	if (drvdata->is_open) {
544 		status = -EBUSY;
545 		goto error;
546 	}
547 
548 	status = hwicap_initialize_hwicap(drvdata);
549 	if (status) {
550 		dev_err(drvdata->dev, "Failed to open file");
551 		goto error;
552 	}
553 
554 	file->private_data = drvdata;
555 	drvdata->write_buffer_in_use = 0;
556 	drvdata->read_buffer_in_use = 0;
557 	drvdata->is_open = 1;
558 
559  error:
560 	mutex_unlock(&drvdata->sem);
561  out:
562 	mutex_unlock(&hwicap_mutex);
563 	return status;
564 }
565 
566 static int hwicap_release(struct inode *inode, struct file *file)
567 {
568 	struct hwicap_drvdata *drvdata = file->private_data;
569 	int i;
570 	int status = 0;
571 
572 	mutex_lock(&drvdata->sem);
573 
574 	if (drvdata->write_buffer_in_use) {
575 		/* Flush write buffer. */
576 		for (i = drvdata->write_buffer_in_use; i < 4; i++)
577 			drvdata->write_buffer[i] = 0;
578 
579 		status = drvdata->config->set_configuration(drvdata,
580 				(u32 *) drvdata->write_buffer, 1);
581 		if (status)
582 			goto error;
583 	}
584 
585 	status = hwicap_command_desync(drvdata);
586 	if (status)
587 		goto error;
588 
589  error:
590 	drvdata->is_open = 0;
591 	mutex_unlock(&drvdata->sem);
592 	return status;
593 }
594 
595 static const struct file_operations hwicap_fops = {
596 	.owner = THIS_MODULE,
597 	.write = hwicap_write,
598 	.read = hwicap_read,
599 	.open = hwicap_open,
600 	.release = hwicap_release,
601 	.llseek = noop_llseek,
602 };
603 
604 static int hwicap_setup(struct device *dev, int id,
605 		const struct resource *regs_res,
606 		const struct hwicap_driver_config *config,
607 		const struct config_registers *config_regs)
608 {
609 	dev_t devt;
610 	struct hwicap_drvdata *drvdata = NULL;
611 	int retval = 0;
612 
613 	dev_info(dev, "Xilinx icap port driver\n");
614 
615 	mutex_lock(&icap_sem);
616 
617 	if (id < 0) {
618 		for (id = 0; id < HWICAP_DEVICES; id++)
619 			if (!probed_devices[id])
620 				break;
621 	}
622 	if (id < 0 || id >= HWICAP_DEVICES) {
623 		mutex_unlock(&icap_sem);
624 		dev_err(dev, "%s%i too large\n", DRIVER_NAME, id);
625 		return -EINVAL;
626 	}
627 	if (probed_devices[id]) {
628 		mutex_unlock(&icap_sem);
629 		dev_err(dev, "cannot assign to %s%i; it is already in use\n",
630 			DRIVER_NAME, id);
631 		return -EBUSY;
632 	}
633 
634 	probed_devices[id] = 1;
635 	mutex_unlock(&icap_sem);
636 
637 	devt = MKDEV(XHWICAP_MAJOR, XHWICAP_MINOR + id);
638 
639 	drvdata = kzalloc(sizeof(struct hwicap_drvdata), GFP_KERNEL);
640 	if (!drvdata) {
641 		retval = -ENOMEM;
642 		goto failed0;
643 	}
644 	dev_set_drvdata(dev, (void *)drvdata);
645 
646 	if (!regs_res) {
647 		dev_err(dev, "Couldn't get registers resource\n");
648 		retval = -EFAULT;
649 		goto failed1;
650 	}
651 
652 	drvdata->mem_start = regs_res->start;
653 	drvdata->mem_end = regs_res->end;
654 	drvdata->mem_size = resource_size(regs_res);
655 
656 	if (!request_mem_region(drvdata->mem_start,
657 					drvdata->mem_size, DRIVER_NAME)) {
658 		dev_err(dev, "Couldn't lock memory region at %Lx\n",
659 			(unsigned long long) regs_res->start);
660 		retval = -EBUSY;
661 		goto failed1;
662 	}
663 
664 	drvdata->devt = devt;
665 	drvdata->dev = dev;
666 	drvdata->base_address = ioremap(drvdata->mem_start, drvdata->mem_size);
667 	if (!drvdata->base_address) {
668 		dev_err(dev, "ioremap() failed\n");
669 		retval = -ENOMEM;
670 		goto failed2;
671 	}
672 
673 	drvdata->config = config;
674 	drvdata->config_regs = config_regs;
675 
676 	mutex_init(&drvdata->sem);
677 	drvdata->is_open = 0;
678 
679 	dev_info(dev, "ioremap %llx to %p with size %llx\n",
680 		 (unsigned long long) drvdata->mem_start,
681 		 drvdata->base_address,
682 		 (unsigned long long) drvdata->mem_size);
683 
684 	cdev_init(&drvdata->cdev, &hwicap_fops);
685 	drvdata->cdev.owner = THIS_MODULE;
686 	retval = cdev_add(&drvdata->cdev, devt, 1);
687 	if (retval) {
688 		dev_err(dev, "cdev_add() failed\n");
689 		goto failed3;
690 	}
691 
692 	device_create(&icap_class, dev, devt, NULL, "%s%d", DRIVER_NAME, id);
693 	return 0;		/* success */
694 
695  failed3:
696 	iounmap(drvdata->base_address);
697 
698  failed2:
699 	release_mem_region(regs_res->start, drvdata->mem_size);
700 
701  failed1:
702 	kfree(drvdata);
703 
704  failed0:
705 	mutex_lock(&icap_sem);
706 	probed_devices[id] = 0;
707 	mutex_unlock(&icap_sem);
708 
709 	return retval;
710 }
711 
712 static struct hwicap_driver_config buffer_icap_config = {
713 	.get_configuration = buffer_icap_get_configuration,
714 	.set_configuration = buffer_icap_set_configuration,
715 	.get_status = buffer_icap_get_status,
716 	.reset = buffer_icap_reset,
717 };
718 
719 static struct hwicap_driver_config fifo_icap_config = {
720 	.get_configuration = fifo_icap_get_configuration,
721 	.set_configuration = fifo_icap_set_configuration,
722 	.get_status = fifo_icap_get_status,
723 	.reset = fifo_icap_reset,
724 };
725 
726 #ifdef CONFIG_OF
727 static int hwicap_of_probe(struct platform_device *op,
728 				     const struct hwicap_driver_config *config)
729 {
730 	struct resource res;
731 	const unsigned int *id;
732 	const char *family;
733 	int rc;
734 	const struct config_registers *regs;
735 
736 
737 	rc = of_address_to_resource(op->dev.of_node, 0, &res);
738 	if (rc) {
739 		dev_err(&op->dev, "invalid address\n");
740 		return rc;
741 	}
742 
743 	id = of_get_property(op->dev.of_node, "port-number", NULL);
744 
745 	/* It's most likely that we're using V4, if the family is not
746 	 * specified
747 	 */
748 	regs = &v4_config_registers;
749 	family = of_get_property(op->dev.of_node, "xlnx,family", NULL);
750 
751 	if (family) {
752 		if (!strcmp(family, "virtex2p"))
753 			regs = &v2_config_registers;
754 		else if (!strcmp(family, "virtex4"))
755 			regs = &v4_config_registers;
756 		else if (!strcmp(family, "virtex5"))
757 			regs = &v5_config_registers;
758 		else if (!strcmp(family, "virtex6"))
759 			regs = &v6_config_registers;
760 	}
761 	return hwicap_setup(&op->dev, id ? *id : -1, &res, config,
762 			regs);
763 }
764 #else
765 static inline int hwicap_of_probe(struct platform_device *op,
766 				  const struct hwicap_driver_config *config)
767 {
768 	return -EINVAL;
769 }
770 #endif /* CONFIG_OF */
771 
772 static const struct of_device_id hwicap_of_match[];
773 static int hwicap_drv_probe(struct platform_device *pdev)
774 {
775 	const struct of_device_id *match;
776 	struct resource *res;
777 	const struct config_registers *regs;
778 	const char *family;
779 
780 	match = of_match_device(hwicap_of_match, &pdev->dev);
781 	if (match)
782 		return hwicap_of_probe(pdev, match->data);
783 
784 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
785 	if (!res)
786 		return -ENODEV;
787 
788 	/* It's most likely that we're using V4, if the family is not
789 	 * specified
790 	 */
791 	regs = &v4_config_registers;
792 	family = pdev->dev.platform_data;
793 
794 	if (family) {
795 		if (!strcmp(family, "virtex2p"))
796 			regs = &v2_config_registers;
797 		else if (!strcmp(family, "virtex4"))
798 			regs = &v4_config_registers;
799 		else if (!strcmp(family, "virtex5"))
800 			regs = &v5_config_registers;
801 		else if (!strcmp(family, "virtex6"))
802 			regs = &v6_config_registers;
803 	}
804 
805 	return hwicap_setup(&pdev->dev, pdev->id, res,
806 			&buffer_icap_config, regs);
807 }
808 
809 static void hwicap_drv_remove(struct platform_device *pdev)
810 {
811 	struct device *dev = &pdev->dev;
812 	struct hwicap_drvdata *drvdata;
813 
814 	drvdata = dev_get_drvdata(dev);
815 
816 	device_destroy(&icap_class, drvdata->devt);
817 	cdev_del(&drvdata->cdev);
818 	iounmap(drvdata->base_address);
819 	release_mem_region(drvdata->mem_start, drvdata->mem_size);
820 	kfree(drvdata);
821 
822 	mutex_lock(&icap_sem);
823 	probed_devices[MINOR(dev->devt)-XHWICAP_MINOR] = 0;
824 	mutex_unlock(&icap_sem);
825 }
826 
827 #ifdef CONFIG_OF
828 /* Match table for device tree binding */
829 static const struct of_device_id hwicap_of_match[] = {
830 	{ .compatible = "xlnx,opb-hwicap-1.00.b", .data = &buffer_icap_config},
831 	{ .compatible = "xlnx,xps-hwicap-1.00.a", .data = &fifo_icap_config},
832 	{},
833 };
834 MODULE_DEVICE_TABLE(of, hwicap_of_match);
835 #else
836 #define hwicap_of_match NULL
837 #endif
838 
839 static struct platform_driver hwicap_platform_driver = {
840 	.probe = hwicap_drv_probe,
841 	.remove_new = hwicap_drv_remove,
842 	.driver = {
843 		.name = DRIVER_NAME,
844 		.of_match_table = hwicap_of_match,
845 	},
846 };
847 
848 static int __init hwicap_module_init(void)
849 {
850 	dev_t devt;
851 	int retval;
852 
853 	retval = class_register(&icap_class);
854 	if (retval)
855 		return retval;
856 	mutex_init(&icap_sem);
857 
858 	devt = MKDEV(XHWICAP_MAJOR, XHWICAP_MINOR);
859 	retval = register_chrdev_region(devt,
860 					HWICAP_DEVICES,
861 					DRIVER_NAME);
862 	if (retval < 0)
863 		return retval;
864 
865 	retval = platform_driver_register(&hwicap_platform_driver);
866 	if (retval)
867 		goto failed;
868 
869 	return retval;
870 
871  failed:
872 	unregister_chrdev_region(devt, HWICAP_DEVICES);
873 
874 	return retval;
875 }
876 
877 static void __exit hwicap_module_cleanup(void)
878 {
879 	dev_t devt = MKDEV(XHWICAP_MAJOR, XHWICAP_MINOR);
880 
881 	class_unregister(&icap_class);
882 
883 	platform_driver_unregister(&hwicap_platform_driver);
884 
885 	unregister_chrdev_region(devt, HWICAP_DEVICES);
886 }
887 
888 module_init(hwicap_module_init);
889 module_exit(hwicap_module_cleanup);
890 
891 MODULE_AUTHOR("Xilinx, Inc; Xilinx Research Labs Group");
892 MODULE_DESCRIPTION("Xilinx ICAP Port Driver");
893 MODULE_LICENSE("GPL");
894