path: root/c/src/lib/libbsp/sparc/shared/drvmgr/spw_bus.c

/*  SpaceWire bus driver interface.
 *
 *  COPYRIGHT (c) 2008.
 *  Aeroflex Gaisler.
 *
 *  The license and distribution terms for this file may be
 *  found in the file LICENSE in this distribution or at
 *  http://www.rtems.com/license/LICENSE. 
 *
 *  2009-11-20, Daniel Hellstrom <daniel@gaisler.com>
 *    Created
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>

#include <drvmgr/drvmgr.h>
#include <drvmgr/spw_bus.h>
#include <genirq.h>
#include <gpiolib.h>
#include <rmap.h>

#include <bsp.h>
#include <stdint.h>

#undef DEBUG

#ifdef DEBUG
 #define DBG(args...) printk(args)
#else
 #define DBG(args...)
#endif

struct virq_entry {
	char *gpio_fsname;
	int fd;
};

struct spw_bus_priv {
	/* SpW-Bus driver handle */
	struct drvmgr_bus *bus;

	/* User configuration */
	struct spw_bus_config	*config;		/* User configuration */

	/* Device prefix */
	int			spwbus_id;
	char			prefix[16];		/* Device name prefix */

	/* IRQ Handling */
	volatile unsigned int	irq_mask;		/* Bit mask of which IRQs has been received, and to handle */
	genirq_t		genirq;			/* Shared IRQ, ISR handling */
	rtems_id		irqlock;		/* Protect IRQ handling (register,enable,disable etc.) */
	rtems_id		isr_task;		/* TASK used to execute registered ISRs on SpaceWire Bus */
	rtems_id		isr_execute_sem;	/* Used to signal to ISR TASK that one or more IRQs has been received */
	volatile int		task_terminate;		/* Used to signal to ISR TASK to stop it's execution, and delete itself */
	char			virqs[4];		/* Virtual IRQ table, used to separate IRQ from different GPIO pins */
};

struct drvmgr_drv spw_bus_drv;
static int spw_bus_cnt = 0;

int spw_bus_init1(struct drvmgr_bus *bus);
int spw_bus_unite(struct drvmgr_drv *drv, struct drvmgr_dev *dev);
int spw_bus_int_register(struct drvmgr_dev *dev, int index, const char *info, drvmgr_isr handler, void *arg);
int spw_bus_int_unregister(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg);
int spw_bus_int_mask(struct drvmgr_dev *dev, int index);
int spw_bus_int_unmask(struct drvmgr_dev *dev, int index);
int spw_bus_int_clear(struct drvmgr_dev *dev, int index);

int spw_bus_freq_get(
	struct drvmgr_dev *dev,
	int options,
	unsigned int *freq_hz);
/* READ/WRITE access to SpaceWire target over RMAP */
int spw_bus_memcpy(void *dest, const void *src, int n, struct drvmgr_rw_arg *a);
int spw_bus_write_mem(void *dest, const void *src, int n, struct drvmgr_rw_arg *a);
int spw_bus_memset(void *dest, int c, int n, struct drvmgr_rw_arg *a);
uint8_t spw_bus_r8(uint8_t *srcadr, struct drvmgr_rw_arg *a);
uint16_t spw_bus_r16(uint16_t *srcadr, struct drvmgr_rw_arg *a);
uint32_t spw_bus_r32(uint32_t *srcadr, struct drvmgr_rw_arg *a);
uint64_t spw_bus_r64(uint64_t *srcadr, struct drvmgr_rw_arg *a);
void spw_bus_w8(uint8_t *dstadr, uint8_t data, struct drvmgr_rw_arg *a);
void spw_bus_w16(uint16_t *dstadr, uint16_t data, struct drvmgr_rw_arg *a);
void spw_bus_w32(uint32_t *dstadr, uint32_t data, struct drvmgr_rw_arg *a);
void spw_bus_w64(uint64_t *dstadr, uint64_t data, struct drvmgr_rw_arg *a);
int spw_bus_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params);
void *spw_bus_rw_arg(struct drvmgr_dev *dev);

/* SPW RMAP bus operations */
struct drvmgr_bus_ops spw_bus_ops =
{
	.init		= 
			{
				spw_bus_init1,
				NULL,
				NULL,
				NULL
			},
	.remove		= NULL,
	.unite		= spw_bus_unite,
	.int_register	= spw_bus_int_register,
	.int_unregister	= spw_bus_int_unregister,
	.int_mask	= spw_bus_int_mask,
	.int_unmask	= spw_bus_int_unmask,
	.int_clear	= spw_bus_int_clear,
	.get_params	= spw_bus_get_params,
	.freq_get	= spw_bus_freq_get,
};

struct drvmgr_func spw_bus_funcs[] =
{
	DRVMGR_FUNC(SPWBUS_RW_ARG, spw_bus_rw_arg),

	DRVMGR_FUNC(SPWBUS_R8, spw_bus_r8),
	DRVMGR_FUNC(SPWBUS_R16, spw_bus_r16),
	DRVMGR_FUNC(SPWBUS_R32, spw_bus_r32),
	DRVMGR_FUNC(SPWBUS_R64, spw_bus_r64),

	DRVMGR_FUNC(SPWBUS_W8, spw_bus_w8),
	DRVMGR_FUNC(SPWBUS_W16, spw_bus_w16),
	DRVMGR_FUNC(SPWBUS_W32, spw_bus_w32),
	DRVMGR_FUNC(SPWBUS_W64, spw_bus_w64),

	DRVMGR_FUNC(SPWBUS_RMEM, spw_bus_memcpy),
	DRVMGR_FUNC(SPWBUS_WMEM, spw_bus_write_mem),
	DRVMGR_FUNC(SPWBUS_MEMSET, spw_bus_memset),

	DRVMGR_FUNC_END,
};

int spw_bus_dev_register(struct drvmgr_bus *bus, struct spw_node *node, int index)
{
	struct drvmgr_dev *newdev;
	struct spw_bus_dev_info *info;
	union drvmgr_key_value *value;
	
	int virq;
	char virq_name[6];

	/* Allocate new device and bus information */
	drvmgr_alloc_dev(&newdev, sizeof(struct spw_bus_dev_info));
	info = (struct spw_bus_dev_info *)(newdev + 1);

	/* Set Node ID */
	info->spwid = node->id.spwid;

	/* Get information from bus configuration */
	value = drvmgr_key_val_get(node->keys, "DST_ADR", KEY_TYPE_INT);
	if ( !value ) {
		printk("spw_bus_dev_register: Failed getting resource DST_ADR\n");
		info->dstadr = 0xfe;
	} else {
		DBG("spw_bus_dev_register: DST_ADR: 0x%02x\n", value->i);
		info->dstadr = value->i;
	}
	value = drvmgr_key_val_get(node->keys, "DST_KEY", KEY_TYPE_INT);
	if ( !value ) {
		printk("spw_bus_dev_register: Failed getting resource DST_KEY\n");
		info->dstkey = 0;
	} else {
		DBG("spw_bus_dev_register: DST_KEY: 0x%02x\n", value->i);
		info->dstkey = value->i;
	}
	/* Get the Virtual IRQ numbers, that will be looked up in VIRQ->GPIO table */
	strcpy(virq_name, "VIRQX");
	for (virq=1; virq<5; virq++) {
		virq_name[4] = '0' + virq;
		value = drvmgr_key_val_get(node->keys, virq_name, KEY_TYPE_INT);
		if ( !value ) {
			/* IRQ is optional, this device does not support VIRQ[X] */
			info->virqs[virq-1] = -1;
		} else {
			DBG("spw_bus_dev_register: %s: %d\n", virq_name, value->i);
			info->virqs[virq-1] = value->i;
		}
	}

	/* Init new device */
	newdev->next = NULL;
	newdev->parent = bus; /* Ourselfs */
	newdev->minor_drv = 0;
	newdev->minor_bus = 0;
	newdev->businfo = (void *)info;
	newdev->priv = NULL;
	newdev->drv = NULL;
	newdev->name = node->name;
	newdev->next_in_drv = NULL;
	newdev->bus = NULL;

	/* Register new device */
	drvmgr_dev_register(newdev);

	return 0;
}

/* Interrupt Service Routine, executes in interrupt context. This ISR:
 *  1. Disable/Mask IRQ on IRQ controller, this disables further interrupts on
       this IRQ number
 *  2. Mark in the private struct that the IRQ has happened
 *  3. Wake ISR TASK that will handle each marked IRQ
 *
 * The TASK will for every IRQ that is marked:
 *  1. Call the ISRs that the SpW Node drivers have registered for this specific IRQ
 *  2. unmask IRQ again.
 *
 *  wakes */
void spw_bus_isr(void *arg)
{
	struct spw_bus_priv *priv;
	unsigned int old_irq_mask;
	char *pvirq = (char *)arg;
	int virq = *pvirq;

	priv = (struct spw_bus_priv *)(pvirq - offsetof(struct spw_bus_priv, virqs) - (virq-1));

	gpiolib_irq_mask(priv->config->virq_table[virq-1].handle);

	/* Mark IRQ was received */
	old_irq_mask = priv->irq_mask;
	priv->irq_mask = old_irq_mask | (1 << virq);

	/* Wake ISR execution TASK only if not woken before */
	if ( old_irq_mask == 0 ) {
		rtems_semaphore_release(priv->isr_execute_sem);
	}
}

void spwbus_task(rtems_task_argument argument)
{
	int virq;
	rtems_interrupt_level level;
	unsigned int mask;
	struct spw_bus_priv *priv = (struct spw_bus_priv *)argument;

	DBG("SpW-Bus: ISR Task is started\n");

	while ( priv->task_terminate == 0 ) {
		while ( (mask = priv->irq_mask) != 0 ) {

			/* Mark the IRQs handled */
			rtems_interrupt_disable(level);
			priv->irq_mask &= ~mask;
			rtems_interrupt_enable(level);

			mask = mask >> 1;
			virq = 1;
			while ( mask ) {
				if ( mask & 1 )  {
					/* execute all ISRs on this IRQ */
					DBG("SpW-Bus: ISR Task is executing VIRQ %d\n", virq);
					genirq_doirq(priv->genirq, virq);
				}

				/* Reenable the handled IRQ */
				gpiolib_irq_unmask(priv->config->virq_table[virq-1].handle);

				virq++;
				mask = mask >> 1;
			}
		}

		DBG("SpW-Bus: ISR Task going to sleep\n");

		rtems_task_wake_after(1);

		/* Wait for new IRQs to handle */
		rtems_semaphore_obtain(priv->isr_execute_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

		DBG("SpW-Bus: ISR Task woke up\n");
	}

	DBG("SpW-Bus: ISR Task is deleted\n");

	rtems_task_delete(RTEMS_SELF);
}

int spw_bus_init1(struct drvmgr_bus *bus)
{
	struct spw_node *node;
	int i;
	struct spw_bus_priv *priv = (struct spw_bus_priv *)bus->priv;
	int status;
	struct spwbus_virq_config *vcfg;
	int virq;
	struct gpiolib_config gpiocfg;

	DBG("SpW-BUS: init\n");

	priv->spwbus_id = spw_bus_cnt++;
	priv->irq_mask = 0;
	priv->task_terminate = 0;

	priv->genirq = genirq_init(32);
	if ( priv->genirq == NULL ) {
		return RTEMS_UNSATISFIED;
	}

	if ( priv->config->resources )
		drvmgr_bus_res_add(bus, priv->config->resources);

	/* Create Semaphore used when doing IRQ/ISR registering/enabling etc. */
	status = rtems_semaphore_create(
		rtems_build_name('S', 'P', 'C', '0' + bus->dev->minor_drv),
		1,
		RTEMS_FIFO | RTEMS_COUNTING_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
		RTEMS_LOCAL | RTEMS_NO_PRIORITY_CEILING,
		0,
		&priv->irqlock);
	if ( status != RTEMS_SUCCESSFUL ) {
		DBG("SpW-BUS: Failed to create irqlock semaphore: %d\n", status);
		return -1;
	}

	/* Create Semaphore used to Signal to ISR TASK that a Interrupt has happened */
	status = rtems_semaphore_create(
		rtems_build_name('S', 'P', 'D', '0' + bus->dev->minor_drv),
		0,
		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
		RTEMS_LOCAL | RTEMS_NO_PRIORITY_CEILING,
		0,
		&priv->isr_execute_sem);
	if ( status != RTEMS_SUCCESSFUL ) {
		DBG("SpW-BUS: Failed to create irqlock semaphore: %d\n", status);
		return -1;
	}

	/* Create ISR task */
	status = rtems_task_create(
		rtems_build_name( 'I', 'S', 'T', '0' + bus->dev->minor_drv),
		1,
		RTEMS_MINIMUM_STACK_SIZE,
		RTEMS_NO_PREEMPT,
		RTEMS_LOCAL | RTEMS_NO_FLOATING_POINT,
		&priv->isr_task);
	if (status != RTEMS_SUCCESSFUL) {
		DBG ("SpW-BUS: Can't create task: %d\n", status);
		return -1;
	}

	/* Initialize VIRQs and open the GPIO drivers if not already done */
	vcfg = &priv->config->virq_table[0];
	for (virq=1; virq<5; virq++) {
		priv->virqs[virq-1] = -1;
		if ( vcfg->handle == NULL ) {
			/* Open GPIO PIN and diable IRQ for now */
			if ( vcfg->gpio_fsname ) {
				vcfg->handle = gpiolib_open_by_name(vcfg->gpio_fsname);
				if ( vcfg->handle != NULL ) {
					priv->virqs[virq-1] = virq;
					gpiocfg.mask = 0;
					gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
					gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
					gpiolib_set_config(vcfg->handle, &gpiocfg);
					if ( gpiolib_set(vcfg->handle, 0, 0) ) {
						DBG("SpW-BUS: Failed to configure GPIO as input for VIRQ%d\n", virq);
					}
					if ( gpiolib_irq_register(vcfg->handle, spw_bus_isr, &priv->virqs[virq-1]) ) {
						DBG("SpW-BUS: Failed to register GPIO ISR for VIRQ%d\n", virq);
					}
				} else {
					DBG("SpW-BUS: Failed to open GPIO (%s) for VIRQ%d\n",vcfg->gpio_fsname, virq);
				}
			}
		} else {
			/* Already opened for us */
			priv->virqs[virq-1] = virq;
		}
		vcfg++;
	}

	/* Start ISR Task, there is no work to do, so the task will wait for isr_execute_sem semaphore released by real ISR */
	status = rtems_task_start(priv->isr_task, spwbus_task, (int)priv);
	if ( status != RTEMS_SUCCESSFUL ) {
		DBG("SpW-BUS: Failed to start ISR task: %d\n", status);
		return -1;
	}

	/* Create Device name */
	strcpy(priv->prefix, "/dev/spwbus0");
	priv->prefix[11] = '0' + priv->spwbus_id;
	mkdir(priv->prefix, S_IRWXU | S_IRWXG | S_IRWXO);
	priv->prefix[12] = '/';
	priv->prefix[13] = '\0';

	/**** REGISTER NEW DEVICES ****/
	i=0;
	node = priv->config->nodes;
	if ( node ) {
		while ( node->id.spwid ) {
			DBG("SpW-BUS: register node %d (%p)\n", i, node);
			if ( spw_bus_dev_register(bus, node, i) ) {
				return RTEMS_UNSATISFIED;
			}
			i++;
			node++;
		}
	}

	return DRVMGR_OK;
}

int spw_bus_unite(struct drvmgr_drv *drv, struct drvmgr_dev *dev)
{
	struct spw_bus_dev_info *info;
	struct spw_bus_drv_info *spwdrv;
	struct spw_id *id;

	if ( !drv || !dev || !dev->parent )
		return 0;

	if ( (drv->bus_type!=DRVMGR_BUS_TYPE_SPW_RMAP) || (dev->parent->bus_type != DRVMGR_BUS_TYPE_SPW_RMAP) ) {
		return 0;
	}

	info = (struct spw_bus_dev_info *)dev->businfo;
	if ( !info ) 
		return 0;

	/* Get SPW RMAP driver info */
	spwdrv = (struct spw_bus_drv_info *)drv;
	id = spwdrv->ids;
	if ( !id )
		return 0;

	while ( id->spwid ) {
		if ( id->spwid == info->spwid ) {
			/* Driver is suitable for device, Unite them */
			return 1;
		}
		id++;
	}

	return 0;
}

int spw_bus_int_get(struct drvmgr_dev *dev, int index)
{
	int virq;

	/* Relative (positive) or absolute (negative) IRQ number */
	if ( index >= 0 ) {
		/* IRQ Index relative to Cores base IRQ */

		/* Get Base IRQ */
		virq = ((struct spw_bus_dev_info *)dev->businfo)->virqs[index];
		if ( virq <= 0 )
			return -1;
		virq += index;
	} else {
		/* Absolute IRQ number */
		virq = -index;
	}
	return virq;
}

int spw_bus_int_register(
	struct drvmgr_dev *dev,
	int index,
	const char *info,
	drvmgr_isr handler,
	void *arg)
{
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;
	int status, virq;
	void *handle;

	/* Get IRQ number from index and device information */
	virq = spw_bus_int_get(dev, index);
	if ( virq <= 0 )
		return DRVMGR_FAIL;

	bus = dev->parent;
	priv = bus->priv;

	DBG("SpW-BUS: Register ISR for VIRQ%d\n", virq);

	handle = priv->config->virq_table[virq-1].handle;
	if ( handle == NULL )
		return DRVMGR_FAIL;

	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

	status = genirq_register(priv->genirq, virq, handler, arg);
	if ( status >= 0 ) {
		status = genirq_enable(priv->genirq, virq, handler, arg);
		if ( status == 0 ) {
			/* Enable IRQ for first enabled handler only */

			/* Unmask the GPIO IRQ at the source (at the GPIO core),
			 * and at the IRQ controller
			 */
			struct gpiolib_config gpiocfg;
			gpiocfg.mask = 1;
			gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
			gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
			gpiolib_set_config(handle, &gpiocfg);

			gpiolib_irq_enable(handle);
		}
	}

	rtems_semaphore_release(priv->irqlock);

	if (status < 0)
		return DRVMGR_FAIL;
	else
		return DRVMGR_OK;
}

int spw_bus_int_unregister(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg)
{
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;
	int virq, status;
	void *handle;

	/* Get IRQ number from index and device information */
	virq = spw_bus_int_get(dev, index);
	if ( virq <= 0 )
		return DRVMGR_FAIL;

	DBG("SpW-BUS: unregister ISR for VIRQ%d\n", virq);

	bus = dev->parent;
	priv = bus->priv;

	handle = priv->config->virq_table[virq-1].handle;
	if ( handle == NULL )
		return DRVMGR_FAIL;

	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

	status = genirq_disable(priv->genirq, virq, isr, arg);
	if ( status >= 0 ) {
		if ( status == 0 ) {
			/* Disable IRQ only when no other handler is enabled */

			/* Mask the GPIO IRQ at the source (at the GPIO core),
			 * disable the IRQ at the interrupt controller.
			 */
			struct gpiolib_config gpiocfg;
			gpiocfg.mask = 0;
			gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
			gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
			gpiolib_set_config(handle, &gpiocfg);

			gpiolib_irq_disable(handle);
		}
		status = genirq_unregister(priv->genirq, virq, isr, arg);
	}

	rtems_semaphore_release(priv->irqlock);

	if (status < 0)
		return DRVMGR_FAIL;
	else
		return DRVMGR_OK;
}

/* Unmask interrupt */
int spw_bus_int_unmask(struct drvmgr_dev *dev, int index)
{
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;
	int virq;
	void *handle;

	/* Get IRQ number from index and device information */
	virq = spw_bus_int_get(dev, index);
	if ( virq <= 0 )
		return DRVMGR_FAIL;

	bus = dev->parent;
	priv = bus->priv;

	DBG("SpW-BUS: unmask IRQ for VIRQ%d\n", virq);

	handle = priv->config->virq_table[virq-1].handle;
	if ( handle == NULL )
		return DRVMGR_FAIL;

	if ( genirq_check(priv->genirq, virq) )
		return DRVMGR_FAIL;

	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

	gpiolib_irq_unmask(handle);

	rtems_semaphore_release(priv->irqlock);

	return DRVMGR_OK;
}

/* mask interrupt */
int spw_bus_int_mask(struct drvmgr_dev *dev, int index)
{
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;
	int virq;
	void *handle;

	/* Get IRQ number from index and device information */
	virq = spw_bus_int_get(dev, index);
	if ( virq <= 0 )
		return DRVMGR_FAIL;

	bus = dev->parent;
	priv = bus->priv;

	handle = priv->config->virq_table[virq-1].handle;
	if ( handle == NULL )
		return DRVMGR_FAIL;

	if ( genirq_check(priv->genirq, virq) )
		return DRVMGR_FAIL;

	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

	/* Register a ISR for the first registered handler */
	gpiolib_irq_mask(handle);

	rtems_semaphore_release(priv->irqlock);

	return DRVMGR_OK;
}

int spw_bus_int_clear(struct drvmgr_dev *dev, int index)
{
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;
	int virq;
	void *handle;

	/* Get IRQ number from index and device information */
	virq = spw_bus_int_get(dev, index);
	if ( virq < 0 )
		return DRVMGR_FAIL;

	bus = dev->parent;
	priv = bus->priv;

	handle = priv->config->virq_table[virq-1].handle;
	if ( handle == NULL )
		return DRVMGR_FAIL;

	/* Register a ISR for the first registered handler */
	/*drvmgr_interrupt_clear(bus->dev, -irq, spw_bus_isr, priv);*/
	if ( gpiolib_irq_clear(handle)) {
		DBG("SpW-BUS: Failed to Clear IRQ for VIRQ%d\n", virq);
	}

	return DRVMGR_OK;
}

void *spw_bus_rw_arg(struct drvmgr_dev *dev)
{
	if (dev == NULL)
		return (void *)DRVMGR_FAIL;
	return dev;
}

/* Copy */
int spw_bus_memcpy(void *dest, const void *src, int n, struct drvmgr_rw_arg *a)
{
	struct rmap_command_read readcmd;
	int status;
	struct drvmgr_dev *dev = (struct drvmgr_dev *)a->arg;
	struct spw_bus_dev_info *info = (struct spw_bus_dev_info *)dev->businfo;
	struct spw_bus_priv *priv = (struct spw_bus_priv *)dev->parent->priv;
	int max_pkt_size = 128; /* We assume that RMAP can do at least 128 bytes data per packet */

	unsigned int source, destination, left;

	if (  n > max_pkt_size ) {
		struct rmap_config stack_cfg;
		rmap_ioctl(priv->config->rmap, RMAP_IOCTL_GET_CONFIG, &stack_cfg);
		max_pkt_size = stack_cfg.max_rx_len;
	}

	source = (unsigned int)src;
	destination = (unsigned int )dest;
	left = n;
	while ( left > 0 ) {
		readcmd.type = RMAP_CMD_RI;
		readcmd.dstadr = info->dstadr;
		readcmd.dstkey = info->dstkey;
		readcmd.address = source;
		readcmd.length = (left > max_pkt_size) ? max_pkt_size : left;
		readcmd.datalength = 0;
		readcmd.data = (void *)destination;

		DBG("RMAP READ1: 0x%08x - 0x%08x\n", source, (source + (readcmd.length - 1)));

		/* Send Command */
		status = rmap_send(priv->config->rmap, (struct rmap_command *)&readcmd);

		if ( status ) {
			printf("RMAP_MEMCPY READ: Failed to send/receive command %d\n", status);
			memset(dest, 0, n);
			/* Should we remove device? */
			return -1;
		}

		/* Read Data */
		if ( readcmd.status != 0 ) {
			printf("RMAP_MEMCPY READ: Status non-zero 0x%x, dlen: 0x%x\n", readcmd.status, readcmd.datalength);
			memset(dest, 0, n);
			/* Should we remove device? */
			return -1;
		}

		source += readcmd.length;
		destination += readcmd.length;
		left -= readcmd.length;
	}
	
#ifdef DEBUG
	if ( n == 4 ) {
		printf("RMAP READ2: 0x%08x(4): 0x%08x\n", src, *(unsigned int *)dest);
	} else {
		printf("RMAP READ2: 0x%08x - 0x%08x\n", src, ((unsigned int)src)+(n-1));
	}
#endif

	/* Return sucessful */
	return 0;
}

/* Note that ((unsigned char *)src)[n] will be overwitten with the RMAP DATA CRC */
int spw_bus_write_mem(void *dest, const void *src, int n, struct drvmgr_rw_arg *a)
{
	struct rmap_command_write writecmd;
	int status;
	struct drvmgr_dev *dev = (struct drvmgr_dev *)a->arg;
	struct spw_bus_dev_info *info = (struct spw_bus_dev_info *)dev->businfo;
	struct spw_bus_priv *priv = (struct spw_bus_priv *)dev->parent->priv;

	/* Use Verify Write when accessing registers (length 1,2,4). */
	if ( n <= 4 ) {
		writecmd.type = RMAP_CMD_WIV;
	} else {
		writecmd.type = RMAP_CMD_WI;
	}
	writecmd.dstadr = info->dstadr;
	writecmd.dstkey = info->dstkey;
	writecmd.address = (unsigned int)dest;
	writecmd.length = n;
	writecmd.data = (unsigned char *)src;
#ifdef DEBUG
	if ( n == 4 ) {
		printf("RMAP WRITE: 0x%08x(4): 0x%08x\n",
			dest, *(unsigned int *)src);
	} else {
		printf("RMAP WRITE: 0x%08x - 0x%08x\n",
			dest, ((unsigned int)dest)+(n-1));
	}
#endif
	/* Send Command */
	status = rmap_send(priv->config->rmap,
				(struct rmap_command *)&writecmd);

	if ( status ) {
		printf("RMAP_MEMCPY WRITE: Failed to send/receive command %d\n",
			status);
		return -1;
	}

	/* Read Data */
	if ( writecmd.status != 0 ) {
		printf("RMAP_MEMCPY WRITE: Status non-zero 0x%x, adr: 0x%x\n",
			(unsigned char)writecmd.status,
			(unsigned char)writecmd.address);
		return -1;
	}

	/* Return sucessful */
	return 0;
}

/* Use standard Driver manager skeleton for this implementation */
int spw_bus_memset(void *dest, int c, int n, struct drvmgr_rw_arg *a)
{
	drvmgr_rw_memset(dest, c, n, a, (drvmgr_wmem_arg)spw_bus_write_mem);
	return 0;
}

int spw_bus_freq_get(
	struct drvmgr_dev *dev,
	int options,
	unsigned int *freq_hz)
{
	/* Link Frequency does not translate so good here, since it is
	 * a SpaceWire network, different parts may have different
	 * transfer rates.
	 */
	*freq_hz = 0;

	return -1;
}

uint8_t spw_bus_r8(uint8_t *srcadr, struct drvmgr_rw_arg *a)
{
	uint8_t result;
	if ( spw_bus_memcpy((void *)&result, (const void *)srcadr, 1, a) ) {
		return 0xff;
	}
	return result;
}

uint16_t  spw_bus_r16(uint16_t *srcadr, struct drvmgr_rw_arg *a)
{
	uint16_t result;
	if ( spw_bus_memcpy((void *)&result, (const void *)srcadr, 2, a) ) {
		return 0xff;
	}
	return result;
}

uint32_t  spw_bus_r32(uint32_t *srcadr, struct drvmgr_rw_arg *a)
{
	uint32_t result;
	if ( spw_bus_memcpy((void *)&result, (const void *)srcadr, 4, a) ) {
		return 0xff;
	}
	return result;
}

uint64_t  spw_bus_r64(uint64_t *srcadr, struct drvmgr_rw_arg *a)
{
	uint64_t result;
	if ( spw_bus_memcpy((void *)&result, (const void *)srcadr, 8, a) ) {
		return 0xff;
	}
	return result;
}

void spw_bus_w8(uint8_t *dstadr, uint8_t data, struct drvmgr_rw_arg *a)
{
	uint8_t buf[2]; /* One byte extra room for RMAP DATA CRC */

	buf[0] = data;
	if ( spw_bus_write_mem((void *)dstadr, (const void *)&buf[0], 1, a) ) {
		/* Handle Error */
	}
}

void spw_bus_w16(uint16_t *dstadr, uint16_t data, struct drvmgr_rw_arg *a)
{
	uint16_t buf[2]; /* One byte extra room for RMAP DATA CRC */

	buf[0] = data;
	if ( spw_bus_write_mem((void *)dstadr, (const void *)&buf[0], 2, a) ) {
		/* Handle Error */
	}
}

void spw_bus_w32(uint32_t *dstadr, uint32_t data, struct drvmgr_rw_arg *a)
{
	uint32_t buf[2]; /* One byte extra room for RMAP DATA CRC */

	buf[0] = data;
	if ( spw_bus_write_mem((void *)dstadr, (const void *)&buf[0], 4, a) ) {
		/* Handle Error */
	}
}

void spw_bus_w64(uint64_t *dstadr, uint64_t data, struct drvmgr_rw_arg *a)
{
	uint64_t buf[2]; /* One byte extra room for RMAP DATA CRC */

	buf[0] = data;
	if ( spw_bus_write_mem((void *)dstadr, (const void *)&buf[0], 8, a) ) {
		/* Handle Error */
	}
}

int spw_bus_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
{
	struct spw_bus_priv *priv = dev->parent->priv;

	params->dev_prefix = &priv->prefix[5];

	return 0;
}

/************* USER INTERFACE *************/

/*** START: THIS SHOULD BE MOVED TO GRSPW DRIVER ***/
struct grspw_priv {
   /* configuration parameters */ 
   struct drvmgr_dev *dev; /* Driver manager device */
   char devName[32]; /* Device Name */
   void *regs;
};
extern struct drvmgr_drv grspw_drv_info;

/* Find GRSPW device from device name */
static struct drvmgr_dev *grspw_find_dev(char *devName)
{
	struct drvmgr_drv *drv = &grspw_drv_info;
	struct drvmgr_dev *dev;
	struct grspw_priv *grspw_priv;

	dev = drv->dev;
	while(dev) {
		grspw_priv = dev->priv;
		if ( strcmp(devName, grspw_priv->devName) == 0 )
			return dev;
		dev = dev->next_in_drv;
	}
	return NULL;
}
/*** STOP: THIS SHOULD BE MOVED TO GRSPW DRIVER ***/


/* Called from USER to attach bus */
int spw_bus_register(struct spw_bus_config *config)
{
	struct drvmgr_dev *grspw_dev;
	struct drvmgr_bus *bus;
	struct spw_bus_priv *priv;

	DBG("SpW-BUS: finding GRSPW device\n");

	/* Find GRSPW Driver to attach bus to */
	grspw_dev = grspw_find_dev(config->devName);
	if ( !grspw_dev ) {
		DBG("SpW-BUS: Failed to find GRSPW device\n");
		return -1;
	}
	if ( grspw_dev->bus ) {
		DBG("SpW-BUS: GRSPW already has a bus attached to it, aborting\n");
		return -1;
	}

	/* Allocate Bus and private structures */
	drvmgr_alloc_bus(&bus, sizeof(*priv));
	priv = (struct spw_bus_priv *)(bus + 1);

	/* Save the configuration for later */
	priv->config = config;

	/* Init the bus */
	grspw_dev->bus = bus;
	bus->bus_type = DRVMGR_BUS_TYPE_SPW_RMAP;
	bus->dev = grspw_dev;
	bus->priv = priv;
	bus->ops = (struct drvmgr_bus_ops *)&spw_bus_ops;
	bus->funcs = spw_bus_funcs;
	priv->bus = bus;

	DBG("SpW-BUS: registering bus\n");
	drvmgr_bus_register(bus); /* this will call spw_bus_init to register the devices */

	return 0;
}