path: root/bsps/shared/grlib/1553/gr1553rt.c

/* SPDX-License-Identifier: BSD-2-Clause */

/*  GR1553B RT driver
 *
 *  COPYRIGHT (c) 2010.
 *  Cobham Gaisler AB.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <rtems.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <grlib/gr1553b.h>
#include <grlib/gr1553rt.h>

#include <drvmgr/drvmgr.h>
#include <grlib/ambapp_bus.h>

#include <grlib/grlib_impl.h>

#define GR1553RT_WRITE_MEM(adr, val) *(volatile uint32_t *)(adr) = (uint32_t)(val)
#define GR1553RT_READ_MEM(adr) (*(volatile uint32_t *)(adr))

#define GR1553RT_WRITE_REG(adr, val) *(volatile uint32_t *)(adr) = (uint32_t)(val)
#define GR1553RT_READ_REG(adr) (*(volatile uint32_t *)(adr))

/* Software representation of one hardware descriptor */
struct gr1553rt_sw_bd {
	unsigned short this_next;/* Next entry or this entry. 0xffff: no next */
	unsigned char listid;	/* ListID of List the descriptor is attached */
	char unused;
} __attribute__((packed));

/* Software description of a subaddress */
struct gr1553rt_subadr {
	/* RX LIST */
	unsigned char rxlistid;
	/* TX LIST */
	unsigned char txlistid;
};

struct gr1553rt_irqerr {
	gr1553rt_irqerr_t func;
	void *data;
};

struct gr1553rt_irqmc {
	gr1553rt_irqmc_t func;
	void *data;
};

struct gr1553rt_irq {
	gr1553rt_irq_t func;
	void *data;
};

struct gr1553rt_priv {
	/* Pointer to Hardware registers */
	struct gr1553b_regs *regs;

	/* Software State */
	int started;
	struct gr1553rt_cfg cfg;
	SPIN_DECLARE(devlock);

	/* Handle to GR1553B RT device layer */
	struct drvmgr_dev **pdev;

	/* Each Index represents one RT Subaddress. 31 = Broadcast */
	struct gr1553rt_subadr subadrs[32];

	/* Pointer to array of Software's description of a hardware
	 * descriptor.
	 */
#if (RTBD_MAX == 0)
	struct gr1553rt_sw_bd *swbds;
#else
	struct gr1553rt_sw_bd swbds[RTBD_MAX];
#endif

	/* List of Free descriptors */
	unsigned short swbd_free;
	int swbd_free_cnt;

	/* Hardware SubAddress descriptors given for CPU and Hardware */
	void *satab_buffer;
	struct gr1553rt_sa *sas_cpu;	/* Translated for CPU */
	struct gr1553rt_sa *sas_hw;	/* Translated for Hardware */

	/* Hardware descriptors address given for CPU and hardware */
	void *bd_buffer;
	int bds_cnt;			/* Number of descriptors */
	struct gr1553rt_bd *bds_cpu;	/* Translated for CPU */
	struct gr1553rt_bd *bds_hw;	/* Translated for Hardware */


	/* Event Log buffer in */
	void *evlog_buffer;
	unsigned int *evlog_cpu_next;	/* Next LOG entry to be handled */
	unsigned int *evlog_cpu_base;	/* First Entry in LOG */
	unsigned int *evlog_cpu_end;	/* Last+1 Entry in LOG */
	unsigned int *evlog_hw_base;	/* Translated for Hardware */

	/* Each Index represents a LIST ID */
	struct gr1553rt_list *lists[RTLISTID_MAX];

	/* IRQ handlers, one per SUBADDRESS */
	struct gr1553rt_irq irq_rx[32];
	struct gr1553rt_irq irq_tx[32];

	/* ISR called when an ERROR IRQ is received */
	struct gr1553rt_irqerr irq_err;

	/* ISR called when an Mode Code is received */
	struct gr1553rt_irqmc irq_mc;
};

void gr1553rt_sw_init(struct gr1553rt_priv *priv);
void gr1553rt_sw_free(struct gr1553rt_priv *priv);
void gr1553rt_isr(void *data);

/* Assign and ID to the list. An LIST ID is needed before scheduling list
 * on an RT subaddress.
 *
 * Only 64 lists can be registered at a time on the same device.
 */
static int gr1553rt_list_reg(struct gr1553rt_list *list)
{
	struct gr1553rt_priv *priv = list->rt;
	int i;

	/* Find first free list ID */
	for ( i=0; i<RTLISTID_MAX; i++) {
		if ( priv->lists[i] == NULL ) {
			priv->lists[i] = list;
			list->listid = i;
			return i;
		}
	}

	/* No available LIST IDs */
	list->listid = -1;

	return -1;
}

/* Unregister List from device */
static void gr1553rt_list_unreg(struct gr1553rt_list *list)
{
	struct gr1553rt_priv *priv = list->rt;

	priv->lists[list->listid] = NULL;
	list->listid = -1;
}

static int gr1553rt_bdid(void *rt, struct gr1553rt_sw_bd *bd)
{
	struct gr1553rt_priv *priv = rt;

	unsigned short index;

	/* Get Index of Software BD */
	index = ((unsigned int)bd - (unsigned int)&priv->swbds[0]) /
		sizeof(struct gr1553rt_sw_bd);

	return index;
}

static void gr1553rt_bd_alloc_init(void *rt, int count)
{
	struct gr1553rt_priv *priv = rt;
	int i;

	for (i=0; i<count-1; i++) {
		priv->swbds[i].this_next = i+1;
	}
	priv->swbds[count-1].this_next = 0xffff;
	priv->swbd_free = 0;
	priv->swbd_free_cnt = count;
}

/* Allocate a Chain of descriptors */
static int gr1553rt_bd_alloc(void *rt, struct gr1553rt_sw_bd **bd, int cnt)
{
	struct gr1553rt_priv *priv = rt;
	struct gr1553rt_sw_bd *curr;
	int i;

	if ((priv->swbd_free_cnt < cnt) || (cnt <= 0)) {
		*bd = NULL;
		return -1;
	}

	*bd = &priv->swbds[priv->swbd_free];
	curr = &priv->swbds[priv->swbd_free];
	for (i=0; i<cnt; i++) {
		if ( i != 0) {
			curr = &priv->swbds[curr->this_next];
		}
		if ( curr->this_next == 0xffff ) {
			*bd = NULL;
			return -1;
		}
	}
	priv->swbd_free = curr->this_next;
	priv->swbd_free_cnt -= cnt;
	curr->this_next = 0xffff; /* Mark end of chain on last entry */

	return 0;
}

#if 0 /* unused for now */
static void gr1553rt_bd_free(void *rt, struct gr1553rt_sw_bd *bd)
{
	struct gr1553rt_priv *priv = rt;
	unsigned short index;

	/* Get Index of Software BD */
	index = gr1553rt_bdid(priv, bd);

	/* Insert first in list */
	bd->this_next = priv->swbd_free;
	priv->swbd_free = index;
	priv->swbd_free_cnt++;
}
#endif

int gr1553rt_list_init
	(
	void *rt,
	struct gr1553rt_list **plist,
	struct gr1553rt_list_cfg *cfg
	)
{
	struct gr1553rt_priv *priv = rt;
	size_t size;
	int i, malloc_used;
	struct gr1553rt_sw_bd *swbd;
	unsigned short index;
	struct gr1553rt_list *list;

	/* The user may provide a pre allocated LIST, or
	 * let the driver handle allocation by using malloc()
	 *
	 * If the IN/OUT plist argument points to NULL a list
	 * dynamically allocated here.
	 */
	malloc_used = 0;
	list = *plist;
	if ( list == NULL ) {
		/* Dynamically allocate LIST */
		size = sizeof(*list) +
			(cfg->bd_cnt * sizeof(list->bds[0]));
		list = grlib_malloc(size);
		if ( list == NULL )
			return -1; /* Out of Memory */
		*plist = list;
		malloc_used = 1;
	}

	list->rt = rt;
	list->subadr = -1;
	list->listid = gr1553rt_list_reg(list);
	if ( list->listid == -1 ) {
		if (malloc_used)
			free(list);
		return -2; /* Too many lists */
	}
	list->cfg = cfg;
	list->bd_cnt = cfg->bd_cnt;

	/* Allocate all BDs needed by list */
	if ( gr1553rt_bd_alloc(rt, &swbd, list->bd_cnt) ) {
		gr1553rt_list_unreg(list);
		if (malloc_used)
			free(list);
		return -3; /* Too few descriptors */
	}

	/* Get ID/INDEX of Software BDs */
	index = gr1553rt_bdid(rt, swbd);
	list->bds[0] = index;
	for (i=1; i<list->bd_cnt; i++) {
		list->bds[i] = priv->swbds[list->bds[i-1]].this_next;
	}

	/* Now that the next pointer has fullfilled it's job and not
	 * needed anymore, we use it as list entry pointer instead.
	 * The this_next pointer is a list entry number.
	 */
	for (i=0; i<list->bd_cnt; i++) {
		priv->swbds[list->bds[i]].this_next = i;
	}

	return 0;
}

int gr1553rt_bd_init(
	struct gr1553rt_list *list,
	unsigned short entry_no,
	unsigned int flags,
	uint16_t *dptr,
	unsigned short next
	)
{
	struct gr1553rt_priv *priv;
	unsigned short bdid;
	struct gr1553rt_bd *bd;
	unsigned int nextbd, dataptr;
	SPIN_IRQFLAGS(irqflags);

	if ( entry_no >= list->bd_cnt )
		return -1;

	/* Find Descriptor */
	bdid = list->bds[entry_no];
	priv = list->rt;
	bd = &priv->bds_cpu[bdid];

	if ( next == 0xfffe ) {
		next = entry_no + 1;
		if ( next >= list->bd_cnt )
			next = 0;
	}

	/* Find next descriptor in address space that the
	 * Hardware understand.
	 */
	if ( next >= 0xffff ) {
		nextbd = 0x3; /* End of list */
	} else if ( next >= list->bd_cnt ) {
		return -1;
	} else {
		bdid = list->bds[next];
		nextbd = (unsigned int)&priv->bds_hw[bdid];
	}

	dataptr = (unsigned int)dptr;
	if ( dataptr & 1 ) {
		/* Translate address from CPU-local into remote */
		dataptr &= ~1;
		drvmgr_translate(
			*priv->pdev,
			CPUMEM_TO_DMA,
			(void *)dataptr,
			(void **)&dataptr
			);
	}

	/* Init BD */
	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	bd->ctrl = flags & GR1553RT_BD_FLAGS_IRQEN;
	bd->dptr = (unsigned int)dptr;
	bd->next = nextbd;
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

int gr1553rt_bd_update(
	struct gr1553rt_list *list,
	int entry_no,
	unsigned int *status,
	uint16_t **dptr
	)
{
	struct gr1553rt_priv *priv;
	unsigned short bdid;
	struct gr1553rt_bd *bd;
	unsigned int tmp, dataptr;
	SPIN_IRQFLAGS(irqflags);

	if ( entry_no >= list->bd_cnt )
		return -1;

	/* Find Descriptor */
	bdid = list->bds[entry_no];
	priv = list->rt;
	bd = &priv->bds_cpu[bdid];

	/* Prepare translation if needed */
	if ( dptr && (dataptr=(unsigned int)*dptr) ) {
		if ( dataptr & 1 ) {
			/* Translate address from CPU-local into remote. May
			 * be used when RT core is accessed over the PCI bus.
			 */
			dataptr &= ~1;
			drvmgr_translate(
				*priv->pdev,
				CPUMEM_TO_DMA,
				(void *)dataptr,
				(void **)&dataptr
				);
		}
	}

	/* Get current values and then set new values in BD */
	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	/* READ/WRITE Status/Control word */
	if ( status ) {
		tmp = bd->ctrl;
		if ( *status ) {
			bd->ctrl = *status;
		}
		*status = tmp;
	}
	/* READ/WRITE Data-Pointer word */
	if ( dptr ) {
		tmp = bd->dptr;
		if ( dataptr ) {
			bd->dptr = dataptr;
		}
		*dptr = (uint16_t *)tmp;
	}
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

int gr1553rt_irq_err
	(
	void *rt,
	gr1553rt_irqerr_t func,
	void *data
	)
{
	struct gr1553rt_priv *priv = rt;
	SPIN_IRQFLAGS(irqflags);

	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	priv->irq_err.func = func;
	priv->irq_err.data = data;
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

int gr1553rt_irq_mc
	(
	void *rt,
	gr1553rt_irqmc_t func,
	void *data
	)
{
	struct gr1553rt_priv *priv = rt;
	SPIN_IRQFLAGS(irqflags);

	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	priv->irq_mc.func = func;
	priv->irq_mc.data = data;
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

int gr1553rt_irq_sa
	(
	void *rt,
	int subadr,
	int tx,
	gr1553rt_irq_t func,
	void *data
	)
{
	struct gr1553rt_priv *priv = rt;
	SPIN_IRQFLAGS(irqflags);

	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	if ( tx ) {
		priv->irq_tx[subadr].func = func;
		priv->irq_tx[subadr].data = data;
	} else {
		priv->irq_rx[subadr].func = func;
		priv->irq_rx[subadr].data = data;
	}
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

/* GR1553-RT Interrupt Service Routine */
void gr1553rt_isr(void *data)
{
	struct gr1553rt_priv *priv = data;
	unsigned int firstirq, lastpos;
	int index;
	unsigned int *last, *curr, entry, hwbd;
	int type, samc, mcode, subadr;
	int listid;
	struct gr1553rt_irq *pisr, isr;
	struct gr1553rt_irqerr isrerr;
	struct gr1553rt_irqmc isrmc;
	unsigned int irq;
	SPIN_ISR_IRQFLAGS(irqflags);

	/* Ack IRQ before reading current write pointer, but after
	 * reading current IRQ pointer. This is because RT_EVIRQ
	 * may be updated after we ACK the IRQ source.
	 */
	irq = priv->regs->irq &
		(GR1553B_IRQ_RTTE|GR1553B_IRQ_RTD|GR1553B_IRQ_RTEV);
	if ( irq == 0 )
		return;

	firstirq = priv->regs->rt_evirq;
	priv->regs->irq = irq;
	lastpos = priv->regs->rt_evlog;

	/* Quit if nothing has been added to the log */
	if ( lastpos == firstirq )
		return;

	if ( irq & (GR1553B_IRQ_RTTE|GR1553B_IRQ_RTD) ) {
		/* copy func and arg while owning lock */
		SPIN_LOCK(&priv->devlock, irqflags);
		isrerr = priv->irq_err;
		SPIN_UNLOCK(&priv->devlock, irqflags);
		if ( isrerr.func ) {
			isrerr.func(irq, isrerr.data);
		}

		/* Stop Hardware and enter non-started mode. This will
		 * make all future calls to driver result in an error.
		 */
		gr1553rt_stop(priv);
	}

	/* Step between first log entry causing an IRQ to last
	 * entry. Each entry that has caused an IRQ will be handled
	 * by calling user-defined function.
	 *
	 * We convert hardware addresses into CPU accessable addresses
	 * first.
	 */
	index = (firstirq - (unsigned int)priv->evlog_hw_base) /
		sizeof(unsigned int);
	curr = priv->evlog_cpu_base + index;
	index = (lastpos - (unsigned int)priv->evlog_hw_base) /
		sizeof(unsigned int);
	last = priv->evlog_cpu_base + index;

	do {
		/* Process one entry */
		entry = *curr;

		if ( entry & 0x80000000 ) {
			/* Entry caused IRQ */
			type = (entry >> 29) & 0x3;
			samc = (entry >> 24) & 0x1f;
			if ( (type & 0x2) == 0 ) {
				/* Transmit/Receive Data */
				subadr = samc;
				if ( type ) {
					/* Receive */
					listid = priv->subadrs[subadr].rxlistid;
					hwbd = priv->sas_cpu[subadr].rxptr;
					pisr = &priv->irq_rx[subadr];
				} else {
					/* Transmit */
					listid = priv->subadrs[subadr].txlistid;
					hwbd = priv->sas_cpu[subadr].txptr;
					pisr = &priv->irq_tx[subadr];
				}

				index = ((unsigned int)hwbd - (unsigned int)
					priv->bds_hw)/sizeof(struct gr1553rt_bd);

				/* copy func and arg while owning lock */
				SPIN_LOCK(&priv->devlock, irqflags);
				isr = *pisr;
				SPIN_UNLOCK(&priv->devlock, irqflags);

				/* Call user ISR of RX/TX transfer */
				if ( isr.func ) {
					isr.func(
						priv->lists[listid],
						entry,
						priv->swbds[index].this_next,
						isr.data
						);
				}
			} else if ( type == 0x2) {
				/* Modecode */
				mcode = samc;

				/* copy func and arg while owning lock */
				SPIN_LOCK(&priv->devlock, irqflags);
				isrmc = priv->irq_mc;
				SPIN_UNLOCK(&priv->devlock, irqflags);

				/* Call user ISR of ModeCodes RX/TX */
				if ( isrmc.func ) {
					isrmc.func(
						mcode,
						entry,
						isrmc.data
						);
				}
			} else {
				/* ERROR OF SOME KIND, EVLOG OVERWRITTEN? */
				rtems_fatal_error_occurred(RTEMS_IO_ERROR);
			}
		}

		/* Calc next entry posistion */
		curr++;
		if ( curr == priv->evlog_cpu_end )
			curr = priv->evlog_cpu_base;

	} while ( curr != last );
}

int gr1553rt_indication(void *rt, int subadr, int *txeno, int *rxeno)
{
	struct gr1553rt_priv *priv = rt;
	struct gr1553rt_sa *sa;
	unsigned int bd, index;

	/*  Sub address valid */
	if ( (subadr < 0) || (subadr > 31) )
		return -1;

	/* Get SubAddress Descriptor address as accessed from CPU */
	sa = &priv->sas_cpu[subadr];

	/* Indication of TX descriptor? */
	if ( txeno ) {
		bd = sa->txptr;
		/* Get Index of Hardware BD */
		index = ((unsigned int)bd - (unsigned int)&priv->bds_hw[0]) /
			sizeof(struct gr1553rt_bd);
		*txeno = priv->swbds[index].this_next;
	}

	/* Indication of RX descriptor? */
	if ( rxeno ) {
		bd = sa->rxptr;
		/* Get Index of Hardware BD */
		index = ((unsigned int)bd - (unsigned int)&priv->bds_hw[0]) /
			sizeof(struct gr1553rt_bd);
		*rxeno = priv->swbds[index].this_next;
	}

	return 0;
}

void gr1553rt_hw_stop(struct gr1553rt_priv *priv);

void gr1553rt_register(void)
{
	/* The RT driver rely on the GR1553B Driver */
	gr1553_register();
}

void *gr1553rt_open(int minor)
{
	struct drvmgr_dev **pdev = NULL;
	struct gr1553rt_priv *priv = NULL;
	struct amba_dev_info *ambadev;
	struct ambapp_core *pnpinfo;

	/* Allocate requested device */
	pdev = gr1553_rt_open(minor);
	if ( pdev == NULL )
		goto fail;

	priv = grlib_calloc(1, sizeof(*priv));
	if ( priv == NULL )
		goto fail;

	/* Assign a device private to RT device */
	priv->pdev = pdev;
	(*pdev)->priv = priv;

	/* Get device information from AMBA PnP information */
	ambadev = (struct amba_dev_info *)(*pdev)->businfo;
	pnpinfo = &ambadev->info;
	priv->regs = (struct gr1553b_regs *)pnpinfo->apb_slv->start;

	SPIN_INIT(&priv->devlock, "gr1553rt");

	/* Start with default configuration */
	/*priv->cfg = gr1553rt_default_config;*/

	/* Unmask IRQs and so */
	gr1553rt_hw_stop(priv);

	/* Register ISR handler. hardware mask IRQ, so it is safe to unmask
	 * at IRQ controller.
	 */
	if (drvmgr_interrupt_register(*priv->pdev, 0, "gr1553rt", gr1553rt_isr, priv))
		goto fail;

	return priv;

fail:
	if ( pdev )
		gr1553_rt_close(pdev);
	if ( priv )
		free(priv);
	return NULL;
}

void gr1553rt_close(void *rt)
{
	struct gr1553rt_priv *priv = rt;

	if ( priv->started ) {
		gr1553rt_stop(priv);
	}

	/* Remove ISR handler */
	drvmgr_interrupt_unregister(*priv->pdev, 0, gr1553rt_isr, priv);

	/* Free dynamically allocated buffers if any */
	gr1553rt_sw_free(priv);
	SPIN_FREE(&priv->devlock);

	/* Return RT/BC device */
	gr1553_rt_close(priv->pdev);
}

/* Stop Hardware and disable IRQ */
void gr1553rt_hw_stop(struct gr1553rt_priv *priv)
{
	uint32_t irqmask;

	/* Disable RT */
	GR1553RT_WRITE_REG(&priv->regs->rt_cfg, GR1553RT_KEY);

	/* Stop BC if not already stopped: BC can not be used simultaneously
	 * as the RT anyway
	 */
	GR1553RT_WRITE_REG(&priv->regs->bc_ctrl, GR1553BC_KEY | 0x0204);

	/* Turn off RT IRQ generation */
	irqmask=GR1553RT_READ_REG(&priv->regs->imask);
	irqmask&=~(GR1553B_IRQEN_RTEVE|GR1553B_IRQEN_RTDE);
	GR1553RT_WRITE_REG(&priv->regs->irq, irqmask);
}

/* Free dynamically allocated buffers, if any */
void gr1553rt_sw_free(struct gr1553rt_priv *priv)
{
	/* Event log */
	if ( (priv->cfg.evlog_buffer == NULL) && priv->evlog_buffer ) {
		free(priv->evlog_buffer);
		priv->evlog_buffer = NULL;
	}

	/* RX/TX Descriptors */
	if ( (priv->cfg.bd_buffer == NULL) && priv->bd_buffer ) {
		free(priv->bd_buffer);
		priv->bd_buffer = NULL;
	}

#if (RTBD_MAX == 0)
	if ( priv->swbds ) {
		free(priv->swbds);
		priv->swbds = NULL;
	}
#endif

	/* Sub address table */
	if ( (priv->cfg.satab_buffer == NULL) && priv->satab_buffer ) {
		free(priv->satab_buffer);
		priv->satab_buffer = NULL;
	}
}

static int gr1553rt_sw_alloc(struct gr1553rt_priv *priv)
{
	int size;
	int retval = 0;

	/* Allocate Event log */
	if ((unsigned int)priv->cfg.evlog_buffer & 1) {
		/* Translate Address from HARDWARE (REMOTE) to CPU-LOCAL */
		priv->evlog_buffer = (void *)
			((unsigned int)priv->cfg.evlog_buffer & ~0x1);
		priv->evlog_hw_base = (unsigned int*)priv->evlog_buffer;
		drvmgr_translate_check(
			*priv->pdev,
			DMAMEM_TO_CPU,
			(void *)priv->evlog_hw_base,
			(void **)&priv->evlog_cpu_base,
			priv->cfg.evlog_size
			);
	} else {
		if (priv->cfg.evlog_buffer == NULL) {
			priv->evlog_buffer = grlib_malloc(
				priv->cfg.evlog_size * 2);
			if (priv->evlog_buffer == NULL) {
				retval = -1;
				goto err;
			}
			/* Align to SIZE bytes boundary */
			priv->evlog_cpu_base = (unsigned int *)
				(((unsigned int)priv->evlog_buffer +
				(priv->cfg.evlog_size-1)) & ~(priv->cfg.evlog_size-1));
		} else {
			/* Addess already CPU-LOCAL */
			priv->evlog_buffer = priv->cfg.evlog_buffer;
			priv->evlog_cpu_base  = (unsigned int *)priv->evlog_buffer;
		}

		drvmgr_translate_check(
			*priv->pdev,
			CPUMEM_TO_DMA,
			(void *)priv->evlog_cpu_base,
			(void **)&priv->evlog_hw_base,
			priv->cfg.evlog_size
			);
	}
	/* Verify alignment */
	if ((unsigned int)priv->evlog_hw_base & (priv->cfg.evlog_size-1)) {
		retval = -2;
		goto err;
	}
	priv->evlog_cpu_end = priv->evlog_cpu_base +
				priv->cfg.evlog_size/sizeof(unsigned int *);

	/* Allocate Transfer Descriptors */
	priv->bds_cnt = priv->cfg.bd_count;
	size = priv->bds_cnt * sizeof(struct gr1553rt_bd);
	if ((unsigned int)priv->cfg.bd_buffer & 1) {
		/* Translate Address from HARDWARE (REMOTE) to CPU-LOCAL */
		priv->bd_buffer = (void *)
			((unsigned int)priv->cfg.bd_buffer & ~0x1);
		priv->bds_hw = (struct gr1553rt_bd *)priv->bd_buffer;
		drvmgr_translate_check(
			*priv->pdev,
			DMAMEM_TO_CPU,
			(void *)priv->bds_hw,
			(void **)&priv->bds_cpu,
			size
			);
	} else {
		if ( priv->cfg.bd_buffer == NULL ) {
			priv->bd_buffer = grlib_malloc(size + 0xf);
			if (priv->bd_buffer == NULL) {
				retval = -1;
				goto err;
			}
			/* Align to 16 bytes boundary */
			priv->bds_cpu = (struct gr1553rt_bd *)
				(((unsigned int)priv->bd_buffer + 0xf) & ~0xf);
		} else {
			/* Addess already CPU-LOCAL */
			priv->bd_buffer	= priv->cfg.bd_buffer;
			priv->bds_cpu = (struct gr1553rt_bd *)priv->bd_buffer;
		}

		/* Translate from CPU address to hardware address */
		drvmgr_translate_check(
			*priv->pdev,
			CPUMEM_TO_DMA,
			(void *)priv->bds_cpu,
			(void **)&priv->bds_hw,
			size
			);
	}
	/* Verify alignment */
	if ((unsigned int)priv->bds_hw & (0xf)) {
		retval = -2;
		goto err;
	}

#if (RTBD_MAX == 0)
	/* Allocate software description of */
	priv->swbds = grlib_malloc(priv->cfg.bd_count * sizeof(*priv->swbds));
	if ( priv->swbds == NULL ) {
		retval = -1;
		goto err;
	}
#endif

	/* Allocate Sub address table */
	if ((unsigned int)priv->cfg.satab_buffer & 1) {
		/* Translate Address from HARDWARE (REMOTE) to CPU-LOCAL */
		priv->satab_buffer = (void *)
			((unsigned int)priv->cfg.satab_buffer & ~0x1);
		priv->sas_hw = (struct gr1553rt_sa *)priv->satab_buffer;

		drvmgr_translate_check(
			*priv->pdev,
			DMAMEM_TO_CPU,
			(void *)priv->sas_hw,
			(void **)&priv->sas_cpu,
			16 * 32);
	} else {
		if (priv->cfg.satab_buffer == NULL) {
			priv->satab_buffer = grlib_malloc((16 * 32) * 2);
			if (priv->satab_buffer == NULL) {
				retval = -1;
				goto err;
			}
			/* Align to 512 bytes boundary */
			priv->sas_cpu = (struct gr1553rt_sa *)
				(((unsigned int)priv->satab_buffer + 0x1ff) & ~0x1ff);
		} else {
			/* Addess already CPU-LOCAL */
			priv->satab_buffer = priv->cfg.satab_buffer;
			priv->sas_cpu = (struct gr1553rt_sa *)priv->satab_buffer;
		}

		/* Translate Address from CPU-LOCAL to HARDWARE (REMOTE) */
		drvmgr_translate_check(
			*priv->pdev,
			CPUMEM_TO_DMA,
			(void *)priv->sas_cpu,
			(void **)&priv->sas_hw,
			16 * 32);
	}
	/* Verify alignment */
	if ((unsigned int)priv->sas_hw & (0x1ff)) {
		retval = -2;
		goto err;
	}

err:
	if (retval) {
		gr1553rt_sw_free(priv);
	}
	return retval;
}

void gr1553rt_sw_init(struct gr1553rt_priv *priv)
{
	int i;

	/* Clear Sub Address table */
	memset(priv->sas_cpu, 0, 512);

	/* Clear Transfer descriptors */
	memset(priv->bds_cpu, 0, priv->bds_cnt * 16);

	/* Clear the Event log */
	memset(priv->evlog_cpu_base, 0, priv->cfg.evlog_size);

	/* Init descriptor allocation algorithm */
	gr1553rt_bd_alloc_init(priv, priv->bds_cnt);

	/* Init table used to convert from sub address to list.
	 * Currently non assigned.
	 */
	for (i=0; i<32; i++) {
		priv->subadrs[i].rxlistid = 0xff;
		priv->subadrs[i].txlistid = 0xff;
	}

	/* Clear all previous IRQ handlers */
	for (i=0; i<32; i++) {
		priv->irq_rx[i].func = NULL;
		priv->irq_tx[i].data = NULL;
	}
	priv->irq_err.func = NULL;
	priv->irq_err.data = NULL;
	priv->irq_mc.func = NULL;
	priv->irq_mc.data = NULL;

	/* Clear LIST to LISTID table */
	for (i=0; i<RTLISTID_MAX; i++) {
		priv->lists[i] = NULL;
	}
}

int gr1553rt_config(void *rt, struct gr1553rt_cfg *cfg)
{
	struct gr1553rt_priv *priv = rt;
	int retval = 0;
	if ( priv->started )
		return -1;

	/*** Free dynamically allocated buffers ***/

	gr1553rt_sw_free(priv);

	/*** Check new config ***/
	if ( cfg->rtaddress > 30 )
		return -1;
	if ( (cfg->evlog_size & (cfg->evlog_size-1)) != 0)
		return -2; /* SIZE: Not aligned to a power of 2 */
	if ( ((unsigned int)priv->cfg.evlog_buffer & (cfg->evlog_size-1)) != 0 )
		return -2; /* Buffer: Not aligned to size */
#if (RTBD_MAX > 0)
	if ( cfg->bd_count > RTBD_MAX )
		return -1;
#endif

	/*** Make new config current ***/
	priv->cfg = *cfg;

	/*** Adapt to new config ***/

	if ( (retval=gr1553rt_sw_alloc(priv)) != 0 ) {
		return retval;
	}

	gr1553rt_sw_init(priv);

	return 0;
}

int gr1553rt_start(void *rt)
{
	struct gr1553rt_priv *priv = rt;
	SPIN_IRQFLAGS(irqflags);

	if ( priv->started )
		return -1;

	/*** Initialize software Pointers and stuff ***/

	if ( !priv->satab_buffer || !priv->bd_buffer || !priv->evlog_buffer )
		return -2;

	priv->evlog_cpu_next = priv->evlog_cpu_base;

	/*** Initialize Registers ***/

	/* Subaddress table base */
	priv->regs->rt_tab = (unsigned int)priv->sas_hw;

	/* Mode code configuration */
	priv->regs->rt_mcctrl = priv->cfg.modecode;

	/* RT Time Tag resolution */
	priv->regs->rt_ttag = priv->cfg.time_res << 16;

	/* Event LOG base and size */
	priv->regs->rt_evsz = ~(priv->cfg.evlog_size - 1);
	priv->regs->rt_evlog = (unsigned int)priv->evlog_hw_base;
	priv->regs->rt_evirq = 0;

	/* Clear and old IRQ flag and Enable IRQ */
	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	priv->regs->irq = GR1553B_IRQ_RTEV|GR1553B_IRQ_RTD|GR1553B_IRQ_RTTE;
	priv->regs->imask |= GR1553B_IRQEN_RTEVE | GR1553B_IRQEN_RTDE |
			GR1553B_IRQEN_RTTEE;

	/* Enable and Set RT address */
	priv->regs->rt_cfg = GR1553RT_KEY |
			(priv->cfg.rtaddress << GR1553B_RT_CFG_RTADDR_BIT) |
			GR1553B_RT_CFG_RTEN;

	/* Tell software RT is started */
	priv->started = 1;
	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);

	return 0;
}

void gr1553rt_stop(void *rt)
{
	struct gr1553rt_priv *priv = rt;
	SPIN_IRQFLAGS(irqflags);

	SPIN_LOCK_IRQ(&priv->devlock, irqflags);

	/* Stop Hardware */
	gr1553rt_hw_stop(priv);

	/* Software state */
	priv->started = 0;

	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
}

void gr1553rt_sa_schedule(
	void *rt,
	int subadr,
	int tx,
	struct gr1553rt_list *list
	)
{
	struct gr1553rt_priv *priv = rt;
	unsigned short bdid;
	struct gr1553rt_bd *bd;

	if ( !list || (list->listid == -1) )
		return;

	/* Get Hardware address of first descriptor in list */
	bdid = list->bds[0];
	if ( bdid == 0xffff )
		return;
	bd = &priv->bds_hw[bdid];

	list->subadr = subadr;

	/* Update Sub address table */
	if ( tx ) {
		list->subadr |= 0x100;
		priv->subadrs[subadr].txlistid = list->listid;
		priv->sas_cpu[subadr].txptr = (unsigned int)bd;
	} else {
		priv->subadrs[subadr].rxlistid = list->listid;
		priv->sas_cpu[subadr].rxptr = (unsigned int)bd;
	}
}

void gr1553rt_sa_setopts(
	void *rt,
	int subadr,
	unsigned int mask,
	unsigned int options
	)
{
	struct gr1553rt_priv *priv = rt;
	unsigned int ctrl;

	if ( (subadr > 31) || (priv->sas_cpu == NULL) )
		return;

	ctrl = priv->sas_cpu[subadr].ctrl;
	priv->sas_cpu[subadr].ctrl = (ctrl & ~mask) | options;
}

void gr1553rt_set_vecword(void *rt, unsigned int mask, unsigned int words)
{
	struct gr1553rt_priv *priv = rt;
	unsigned int vword;

	if ( mask == 0 )
		return;

	vword = priv->regs->rt_statw;

	priv->regs->rt_statw = (vword & ~mask) | (words & mask);
}

void gr1553rt_set_bussts(void *rt, unsigned int mask, unsigned int sts)
{
	struct gr1553rt_priv *priv = rt;
	unsigned int stat;

	stat = priv->regs->rt_stat2;
	priv->regs->rt_stat2 = (stat & ~mask) | (mask & sts);
}

void gr1553rt_status(void *rt, struct gr1553rt_status *status)
{
	struct gr1553rt_priv *priv = rt;
	struct gr1553b_regs *regs = priv->regs;
	unsigned int tmp;
	SPIN_IRQFLAGS(irqflags);

	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
	status->status = regs->rt_stat;
	status->bus_status = regs->rt_stat2;

	tmp = regs->rt_sync;
	status->synctime = tmp >> 16;
	status->syncword = tmp & 0xffff;

	tmp = regs->rt_ttag;
	status->time_res = tmp >> 16;
	status->time = tmp & 0xffff;

	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
}

void gr1553rt_list_sa(struct gr1553rt_list *list, int *subadr, int *tx)
{
	int sa, trt;

	if ( list->subadr == -1 ) {
		sa = -1;
		trt = -1;
	} else {
		sa = list->subadr & 0xff;
		trt = (list->subadr & 0x100) >> 8;
	}

	if ( subadr )
		*subadr = sa;
	if ( tx )
		*tx = trt;
}

int gr1553rt_evlog_read(void *rt, unsigned int *dst, int max)
{
	struct gr1553rt_priv *priv = rt;
	int cnt, top, bot, left;
	unsigned int *hwpos;

	/* Get address of hardware's current working entry */
	hwpos = (unsigned int *)priv->regs->rt_evlog;

	/* Convert into CPU address */
	hwpos = (unsigned int *)
		((unsigned int)hwpos - (unsigned int)priv->evlog_hw_base +
		(unsigned int)priv->evlog_cpu_base);

	if ( priv->evlog_cpu_next == hwpos )
		return 0;

	if ( priv->evlog_cpu_next > hwpos ) {
		top = (unsigned int)priv->evlog_cpu_end -
			(unsigned int)priv->evlog_cpu_next;
		bot = (unsigned int)hwpos - (unsigned int)priv->evlog_cpu_base;
	} else {
		top = (unsigned int)hwpos - (unsigned int)priv->evlog_cpu_next;
		bot = 0;
	}
	top = top / 4;
	bot = bot / 4;

	left = max;
	if ( top > 0 ) {
		if ( top > left ) {
			cnt = left;
		} else {
			cnt = top;
		}
		memcpy(dst, priv->evlog_cpu_next, cnt*4);
		dst += cnt;
		left -= cnt;
	}

	if ( (bot > 0) && (left > 0) ) {
		if ( bot > left ) {
			cnt = left;
		} else {
			cnt = bot;
		}
		memcpy(dst, priv->evlog_cpu_base, cnt*4);
		left -= cnt;
	}

	cnt = max - left;
	priv->evlog_cpu_next += cnt;
	if ( priv->evlog_cpu_next >= priv->evlog_cpu_end ) {
		priv->evlog_cpu_next = (unsigned int *)
			((unsigned int)priv->evlog_cpu_base +
			((unsigned int)priv->evlog_cpu_next -
			 (unsigned int)priv->evlog_cpu_end ));
	}

	return max - left;
}