leon, spwtdp: Initial driver commit

4 files changed, 1344 insertions, 0 deletions

diff --git a/bsps/sparc/headers.am b/bsps/sparc/headers.am
index 5b4236e1b1..4c6efb86ce 100644
--- a/bsps/sparc/headers.am
+++ b/bsps/sparc/headers.am
@@ -61,6 +61,7 @@ include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/pcif.h
 include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/satcan.h
 include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/spictrl.h
 include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/spwcuc.h
+include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/spwtdp.h
 include_bsp_HEADERS += ../../../../../bsps/sparc/include/bsp/tlib.h
 
 include_drvmgrdir = $(includedir)/drvmgr
diff --git a/bsps/sparc/include/bsp/spwtdp.h b/bsps/sparc/include/bsp/spwtdp.h
new file mode 100644
index 0000000000..b2fa29889f
--- /dev/null
+++ b/bsps/sparc/include/bsp/spwtdp.h
@@ -0,0 +1,320 @@
+/*  SPWTDP - SpaceWire Time Distribution Protocol. The driver provides
+ *  device discovery and interrupt management.
+ *
+ *  COPYRIGHT (c) 2017.
+ *  Cobham Gaisler AB
+ *
+ *  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.
+ *
+ */
+
+#ifndef __SPWTDP_H__
+#define __SPWTDP_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define SPWTDP_ERR_OK		0
+#define SPWTDP_ERR_EINVAL	-1
+#define SPWTDP_ERR_ERROR	-2
+#define SPWTDP_ERR_NOINIT	-3
+
+/* Maximum number of SPWTDP devices supported by driver */
+#define SPWTDP_MAX 2
+
+/* SPWTDP Register layout */
+struct spwtdp_regs {
+	volatile unsigned int conf[4];       /* 0x000-0x00C */
+	volatile unsigned int stat[4];       /* 0x010-0x01C */
+	volatile unsigned int cmd_ctrl;      /* 0x020 */
+	volatile unsigned int cmd_et[5];     /* 0x024-0x034 */
+	volatile unsigned int resv1[2];      /* 0x038-0x03C */
+	volatile unsigned int dat_ctrl;      /* 0x040 */
+	volatile unsigned int dat_et[5];     /* 0x044-0x054 */
+	volatile unsigned int resv2[2];      /* 0x058-0x05C */
+	volatile unsigned int ts_rx_ctrl;    /* 0x060 */
+	volatile unsigned int ts_rx_et[5];   /* 0x064-0x074 */
+	volatile unsigned int resv3[2];      /* 0x078-0x07C */
+	volatile unsigned int ts_tx_ctrl;    /* 0x080 */
+	volatile unsigned int ts_tx_et[5];   /* 0x084-0x094 */
+	volatile unsigned int resv4[2];      /* 0x098 */
+	volatile unsigned int lat_ctrl;      /* 0x0A0 */
+	volatile unsigned int lat_et[5];     /* 0x0A4-0x0B4 */
+	volatile unsigned int resv5[2];      /* 0x0B8-0x0BC */
+	volatile unsigned int ien;           /* 0x0C0 */
+	volatile unsigned int ists;          /* 0x0C4 */
+	volatile unsigned int dlycnt;        /* 0x0C8 */
+	volatile unsigned int dissync;       /* 0x0CC */
+	volatile unsigned int resv6[12];     /* 0x0D0-0x0FC */
+	volatile unsigned int edmask[4];     /* 0x100-0x10C */
+	struct {
+		volatile unsigned int ctrl;          /* 0x110, 0x130, 0x150, 0x170 */
+		volatile unsigned int et[5];         /* 0x114-0x124, 0x134-0x144, 0x154-0x164, 0x174-0x184,  */
+		volatile unsigned int resv0[2];      /* 0x128-0x12C, 0x148-0x14C, 0x168-0x16C, 0x188-0x18C,  */
+	} edat[4];                           /* 0x110-0x18C */
+	volatile unsigned int resv7[4];      /* 0x190-0x19C */
+	volatile unsigned int pulse[8];      /* 0x1A0-0x1BC */
+	volatile unsigned int resv8[16];     /* 0x1C0-0x1FC */
+};
+
+/*
+ * Configuration register definitions 
+ */
+#define CONF0_JE	(0x1 << CONF0_JE_BIT)
+#define CONF0_ST	(0x1 << CONF0_ST_BIT)
+#define CONF0_EP	(0x1 << CONF0_EP_BIT)
+#define CONF0_ET	(0x1 << CONF0_ET_BIT)
+#define CONF0_SP	(0x1 << CONF0_SP_BIT)
+#define CONF0_SE	(0x1 << CONF0_SE_BIT)
+#define CONF0_LE	(0x1 << CONF0_LE_BIT)
+#define CONF0_AE	(0x1 << CONF0_AE_BIT)
+#define CONF0_MAP	(0x1f << CONF0_MAP_BIT)
+#define CONF0_TD	(0x1 << CONF0_TD_BIT)
+#define CONF0_MU	(0x1 << CONF0_MU_BIT)
+#define CONF0_SEL	(0x3 << CONF0_SEL_BIT)
+#define CONF0_ME	(0x1 << CONF0_ME_BIT)
+#define CONF0_RE	(0x1 << CONF0_RE_BIT)
+#define CONF0_TE	(0x1 << CONF0_TE_BIT)
+#define CONF0_RS	(0x1 << CONF0_RS_BIT)
+
+#define CONF0_JE_BIT	24
+#define CONF0_ST_BIT	21
+#define CONF0_EP_BIT	20
+#define CONF0_ET_BIT	19
+#define CONF0_SP_BIT	18
+#define CONF0_SE_BIT	17
+#define CONF0_LE_BIT	16
+#define CONF0_AE_BIT	15
+#define CONF0_MAP_BIT	8
+#define CONF0_TD_BIT	7
+#define CONF0_MU_BIT	6
+#define CONF0_SEL_BIT	4
+#define CONF0_ME_BIT	3
+#define CONF0_RE_BIT	2
+#define CONF0_TE_BIT	1
+#define CONF0_RS_BIT	0
+
+#define CONF1_FSINC	(0x3fffffff << CONF1_FSINC_BIT)
+
+#define CONF1_FSINC_BIT	0
+
+#define CONF2_CV	(0xffffff << CONF2_CV_BIT)
+#define CONF2_ETINC	(0xff << CONF2_ETINC_BIT)
+
+#define CONF2_CV_BIT	8
+#define CONF2_ETINC_BIT	0
+
+#define CONF3_OUTPORT	(0xf << CONF3_OUTPORT_BIT)
+#define CONF3_INPORT	(0xf << CONF3_INPORT_BIT)
+#define CONF3_STM	(0x3f << CONF3_STM_BIT)
+#define CONF3_DI64R	(0x1 << CONF3_DI64R_BIT)
+#define CONF3_DI64T	(0x1 << CONF3_DI64T_BIT)
+#define CONF3_DI64	(0x1 << CONF3_DI64_BIT)
+#define CONF3_DI	(0x1 << CONF3_DI_BIT)
+#define CONF3_INRX	(0x1f << CONF3_INRX_BIT)
+#define CONF3_INTX	(0x1f << CONF3_INTX_BIT)
+
+#define CONF3_OUTPORT_BIT 28
+#define CONF3_INPORT_BIT 24
+#define CONF3_STM_BIT	16
+#define CONF3_DI64R_BIT	13
+#define CONF3_DI64T_BIT 12
+#define CONF3_DI64_BIT	11
+#define CONF3_DI_BIT	10
+#define CONF3_INRX_BIT	5
+#define CONF3_INTX_BIT	0
+
+/*
+ * Control register definitions 
+ */
+#define CTRL_NC		(0x1 << CTRL_NC_BIT)
+#define CTRL_IS		(0x1 << CTRL_IS_BIT)
+#define CTRL_SPWTC	(0xff << CTRL_SPWTC_BIT)
+#define CTRL_CPF	(0xffff << CTRL_CPF_BIT)
+
+#define CTRL_NC_BIT	31
+#define CTRL_IS_BIT	30
+#define CTRL_SPWTC_BIT	16
+#define CTRL_CPF_BIT	0
+
+/* 
+ * Interrupt register definition
+ */
+#define SPWTDP_IRQ_S		(0x1 << SPWTDP_IRQ_S_BIT)
+#define SPWTDP_IRQ_TR		(0x1 << SPWTDP_IRQ_TR_BIT)
+#define SPWTDP_IRQ_TM		(0x1 << SPWTDP_IRQ_TM_BIT)
+#define SPWTDP_IRQ_TT		(0x1 << SPWTDP_IRQ_TT_BIT)
+#define SPWTDP_IRQ_DIR		(0x1 << SPWTDP_IRQ_DIR_BIT)
+#define SPWTDP_IRQ_DIT		(0x1 << SPWTDP_IRQ_DIT_BIT)
+#define SPWTDP_IRQ_EDI0		(0x1 << SPWTDP_IRQ_EDI0_BIT)
+#define SPWTDP_IRQ_EDI1		(0x1 << SPWTDP_IRQ_EDI1_BIT)
+#define SPWTDP_IRQ_EDI2		(0x1 << SPWTDP_IRQ_EDI2_BIT)
+#define SPWTDP_IRQ_EDI3		(0x1 << SPWTDP_IRQ_EDI3_BIT)
+#define SPWTDP_IRQ_SET		(0x1 << SPWTDP_IRQ_SET_BIT)
+#define SPWTDP_IRQ_P0		(0x1 << SPWTDP_IRQ_P0_BIT)
+#define SPWTDP_IRQ_P1		(0x1 << SPWTDP_IRQ_P1_BIT)
+#define SPWTDP_IRQ_P2		(0x1 << SPWTDP_IRQ_P2_BIT)
+#define SPWTDP_IRQ_P3		(0x1 << SPWTDP_IRQ_P3_BIT)
+#define SPWTDP_IRQ_P4		(0x1 << SPWTDP_IRQ_P4_BIT)
+#define SPWTDP_IRQ_P5		(0x1 << SPWTDP_IRQ_P5_BIT)
+#define SPWTDP_IRQ_P6		(0x1 << SPWTDP_IRQ_P6_BIT)
+#define SPWTDP_IRQ_P7		(0x1 << SPWTDP_IRQ_P7_BIT)
+#define SPWTDP_IRQ_NCTC		(0x1 << SPWTDP_IRQ_NCTC_BIT)
+#define SPWTDP_IRQ_WCLEAR	\
+	(SPWTDP_IRQ_S | SPWTDP_IRQ_TR | SPWTDP_IRQ_TM | \
+	 SPWTDP_IRQ_TT | SPWTDP_IRQ_DIR | SPWTDP_IRQ_DIT | SPWTDP_IRQ_EDI0 | \
+	 SPWTDP_IRQ_EDI1 | SPWTDP_IRQ_EDI2 | SPWTDP_IRQ_EDI3 | SPWTDP_IRQ_SET |\
+	 SPWTDP_IRQ_P0 | SPWTDP_IRQ_P1 | SPWTDP_IRQ_P2 | SPWTDP_IRQ_P3 | \
+	 SPWTDP_IRQ_P4 | SPWTDP_IRQ_P5 | SPWTDP_IRQ_P6 | SPWTDP_IRQ_P7 | \
+	 SPWTDP_IRQ_NCTC)
+#define SPWTDP_IRQ_ALL		(SPWTDP_IRQ_WCLEAR)
+
+#define SPWTDP_IRQ_S_BIT 0
+#define SPWTDP_IRQ_TR_BIT 1
+#define SPWTDP_IRQ_TM_BIT 2
+#define SPWTDP_IRQ_TT_BIT 3
+#define SPWTDP_IRQ_DIR_BIT 4
+#define SPWTDP_IRQ_DIT_BIT 5
+#define SPWTDP_IRQ_EDI0_BIT 6
+#define SPWTDP_IRQ_EDI1_BIT 7
+#define SPWTDP_IRQ_EDI2_BIT 8
+#define SPWTDP_IRQ_EDI3_BIT 9
+#define SPWTDP_IRQ_SET_BIT 10
+#define SPWTDP_IRQ_P0_BIT 11
+#define SPWTDP_IRQ_P1_BIT 12
+#define SPWTDP_IRQ_P2_BIT 13
+#define SPWTDP_IRQ_P3_BIT 14
+#define SPWTDP_IRQ_P4_BIT 15
+#define SPWTDP_IRQ_P5_BIT 16
+#define SPWTDP_IRQ_P6_BIT 17
+#define SPWTDP_IRQ_P7_BIT 18
+#define SPWTDP_IRQ_NCTC_BIT 19
+
+/* Register the SPWTDP Driver to the Driver Manager */
+void spwtdp_register_drv(void);
+
+/* Open a SPWTDP device by registration order index. A SPWTDP device can only by
+ * opened once. The handle returned must be used as the input parameter 'spwtdp'
+ * in the rest of the calls in the function interface.
+ */
+extern void *spwtdp_open(int dev_no);
+
+/* Close a previously opened SPWTDP device */
+extern int spwtdp_close(void *spwtdp);
+
+/* Reset SPWTDP Core */
+extern int spwtdp_reset(void *spwtdp);
+
+/* Setup the frequency configuration registers */
+extern int spwtdp_freq_setup(void *spwtdp, uint32_t fsinc, uint32_t cv,
+			     uint8_t etinc);
+
+/* Unmask Interrupts at Interrupt controller */
+extern int spwtdp_interrupt_unmask(void *spwtdp, uint32_t irqmask);
+
+/* Mask Interrupts at Interrupt controller */
+extern int spwtdp_interrupt_mask(void *spwtdp, uint32_t irqmask);
+
+/* Function ISR callback prototype
+ *
+ * ists    - Interrupt STatus register of the SPWTDP core read by ISR
+ * data    - Custom data provided by user
+ */
+typedef void (*spwtdp_isr_t)(unsigned int ists, void *data);
+
+/* Register an Interrupt handler and custom data, the function call is
+ * removed by calling unregister function.
+ */
+extern int spwtdp_isr_register(void *spwtdp, spwtdp_isr_t isr, void *data);
+
+/* Unregister an Interrupt handler */
+extern int spwtdp_isr_unregister(void *spwtdp);
+
+/* Get and clear interrupt status */
+extern int spwtdp_interrupt_status(void *spwtdp, uint32_t *sts,
+				   uint32_t clrmask);
+
+/* Setup Initiator and target */
+#define SPWTDP_TDP_ENABLE CONF0_TD
+#define SPWTDP_TDP_DISABLE 0
+#define SPWTDP_LATENCY_ENABLE CONF0_LE
+#define SPWTDP_LATENCY_DISABLE 0
+#define SPWTDP_EXTET_INC_POLARITY_RISING CONF0_EP
+#define SPWTDP_EXTET_INC_POLARITY_FALLING 0
+#define SPWTDP_EXTET_INC_ENABLE CONF0_ET
+#define SPWTDP_EXTET_INC_DISABLE 0
+#define SPWTDP_EXTET_POLARITY_RISING CONF0_SP
+#define SPWTDP_EXTET_POLARITY_FALLING 0
+#define SPWTDP_EXTET_ENABLE CONF0_SE
+#define SPWTDP_EXTET_DISABLE 0
+#define SPWTDP_TARGET_SPWSYNC_ENABLE CONF0_ST
+#define SPWTDP_TARGET_SPWSYNC_DISABLE 0
+#define SPWTDP_TARGET_JITTERC_ENABLE CONF0_JE
+#define SPWTDP_TARGET_JITTERC_DISABLE 0
+#define SPWTDP_TARGET_MITIGATION_ENABLE CONF0_ME
+#define SPWTDP_TARGET_MITIGATION_DISABLE 0
+extern int spwtdp_initiator_conf(void *spwtdp, uint8_t mapping,
+				 uint32_t options);
+extern int spwtdp_target_conf(void *spwtdp, uint8_t mapping, uint32_t options);
+
+/* Setup Initiator and target dist interrupts */
+extern int spwtdp_initiator_int_conf(void *spwtdp, uint8_t stm, uint8_t inrx,
+				     uint8_t intx);
+#define SPWTDP_TARGET_DISTINT_INTACK CONF3_DI
+#define SPWTDP_TARGET_DISTINT_INT 0
+extern int spwtdp_target_int_conf(void *spwtdp, uint8_t inrx, uint8_t intx,
+				  uint32_t options);
+
+/* Enable Initiator and target */
+extern int spwtdp_initiator_enable(void *spwtdp);
+extern int spwtdp_target_enable(void *spwtdp);
+
+/* Disable Initiator and target */
+extern int spwtdp_initiator_disable(void *spwtdp);
+extern int spwtdp_target_disable(void *spwtdp);
+
+/* Get and clear status */
+extern int spwtdp_status(void *spwtdp, uint32_t *sts, uint32_t clrmask);
+
+/* Define time struct */
+/* Length of the max data sample (136 bits), aligned to the next 32-bit word
+ * (160 bits=20 byte=5 words)
+ */
+#define SPWTDP_TIME_DATA_LENGTH 20
+struct spwtdp_time_t {
+	uint8_t data[SPWTDP_TIME_DATA_LENGTH];
+	uint32_t preamble;
+};
+typedef struct spwtdp_time_t spwtdp_time_t;
+
+/* Get datation elapsed time */
+extern int spwtdp_dat_et_get(void *spwtdp, spwtdp_time_t * val);
+extern int spwtdp_tsrx_et_get(void *spwtdp, spwtdp_time_t * val);
+extern int spwtdp_tstx_et_get(void *spwtdp, spwtdp_time_t * val);
+extern int spwtdp_lat_et_get(void *spwtdp, spwtdp_time_t * val);
+extern int spwtdp_cmd_et_get(void *spwtdp, spwtdp_time_t * val);
+
+/* Configure TSTX */
+extern int spwtdp_initiator_tstx_conf(void *spwtdp, uint8_t tstc);
+
+/* Manage control register */
+extern int spwtdp_initiator_cmd_et_set(void *spwtdp, spwtdp_time_t val);
+extern int spwtdp_initiator_cmd_spwtc_set(void *spwtdp, uint8_t spwtc);
+#define SPWTDP_TARGET_CTRL_NEWCOMMAND_ENABLE CTRL_NC
+#define SPWTDP_TARGET_CTRL_NEWCOMMAND_DISABLE 0
+#define SPWTDP_TARGET_CTRL_INIT CTRL_IS
+#define SPWTDP_TARGET_CTRL_SYNC 0
+extern int spwtdp_target_cmd_conf(void *spwtdp, uint8_t spwtc, uint16_t cpf,
+				  uint32_t options);
+
+/* Get precision */
+extern int spwtdp_precision_get(void *spwtdp, uint8_t *fine, uint8_t *coarse);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/bsps/sparc/include/drvmgr/ambapp_bus.h b/bsps/sparc/include/drvmgr/ambapp_bus.h
index d034aae3a3..94a7fe2216 100644
--- a/bsps/sparc/include/drvmgr/ambapp_bus.h
+++ b/bsps/sparc/include/drvmgr/ambapp_bus.h
@@ -59,6 +59,7 @@ extern "C" {
 #define DRIVER_AMBAPP_GAISLER_L2CACHE_ID	DRIVER_AMBAPP_ID(VENDOR_GAISLER, GAISLER_L2CACHE)
 #define DRIVER_AMBAPP_GAISLER_MEMSCRUB_ID	DRIVER_AMBAPP_ID(VENDOR_GAISLER, GAISLER_MEMSCRUB)
 #define DRIVER_AMBAPP_GAISLER_L4STAT_ID		DRIVER_AMBAPP_ID(VENDOR_GAISLER, GAISLER_L4STAT)
+#define DRIVER_AMBAPP_GAISLER_SPWTDP_ID		DRIVER_AMBAPP_ID(VENDOR_GAISLER, GAISLER_SPWTDP)
 
 /*** ESA Hardware Device Driver IDs ***/
 #define DRIVER_AMBAPP_ESA_MCTRL_ID		DRIVER_AMBAPP_ID(VENDOR_ESA, ESA_MCTRL)
diff --git a/bsps/sparc/shared/spw/spwtdp.c b/bsps/sparc/shared/spw/spwtdp.c
new file mode 100644
index 0000000000..cf5b3af946
--- /dev/null
+++ b/bsps/sparc/shared/spw/spwtdp.c
@@ -0,0 +1,1022 @@
+/*  SPWTDP - SpaceWire Time Distribution Protocol. The driver provides
+ *  device discovery and interrupt management.
+ *
+ *  COPYRIGHT (c) 2017.
+ *  Cobham Gaisler AB
+ *
+ *  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.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stddef.h>
+#include <rtems.h>
+#include <rtems/bspIo.h>
+#include <drvmgr/drvmgr.h>
+#include <ambapp.h>
+#include <drvmgr/ambapp_bus.h>
+#include <bsp.h>
+#include <bsp/spwtdp.h>
+
+/* Use interrupt lock privmitives compatible with SMP defined in
+ * RTEMS 4.11.99 and higher.
+ */
+#if (((__RTEMS_MAJOR__ << 16) | (__RTEMS_MINOR__ << 8) | __RTEMS_REVISION__) >= 0x040b63)
+
+/* map via rtems_interrupt_lock_* API: */
+#define SPIN_DECLARE(lock) RTEMS_INTERRUPT_LOCK_MEMBER(lock)
+#define SPIN_INIT(lock, name) rtems_interrupt_lock_initialize(lock, name)
+#define SPIN_LOCK(lock, level) rtems_interrupt_lock_acquire_isr(lock, &level)
+#define SPIN_LOCK_IRQ(lock, level) rtems_interrupt_lock_acquire(lock, &level)
+#define SPIN_UNLOCK(lock, level) rtems_interrupt_lock_release_isr(lock, &level)
+#define SPIN_UNLOCK_IRQ(lock, level) rtems_interrupt_lock_release(lock, &level)
+#define SPIN_IRQFLAGS(k) rtems_interrupt_lock_context k
+#define SPIN_ISR_IRQFLAGS(k) SPIN_IRQFLAGS(k)
+
+#else
+
+/* maintain single-core compatibility with older versions of RTEMS: */
+#define SPIN_DECLARE(name)
+#define SPIN_INIT(lock, name)
+#define SPIN_LOCK(lock, level)
+#define SPIN_LOCK_IRQ(lock, level) rtems_interrupt_disable(level)
+#define SPIN_UNLOCK(lock, level)
+#define SPIN_UNLOCK_IRQ(lock, level) rtems_interrupt_enable(level)
+#define SPIN_IRQFLAGS(k) rtems_interrupt_level k
+#define SPIN_ISR_IRQFLAGS(k)
+
+#ifdef RTEMS_SMP
+#error SMP mode not compatible with these interrupt lock primitives
+#endif
+
+#endif
+
+/*#define STATIC*/
+#define STATIC static
+
+/*#define INLINE*/
+#define INLINE inline
+
+/*#define UNUSED*/
+#define UNUSED __attribute__((unused))
+
+#define DEBUG 1
+
+#ifdef DEBUG
+#define DBG(x...) printf(x)
+#else
+#define DBG(x...) 
+#endif
+
+/* Memory and HW Registers Access routines. All 32-bit access routines */
+#define REG_WRITE(addr, val) \
+	(*(volatile unsigned int *)(addr) = (unsigned int)(val))
+#define REG_READ(addr) (*(volatile unsigned int *)(addr))
+
+/*
+ * Configuration register definitions
+ * DEFINED in header
+ */
+
+/*
+ * Control register definitions
+ * DEFINED in header
+ */
+
+/*
+ * TSTX Control register definitions
+ */
+#define TSTXCTRL_TSTC (0xff<<TSTXCTRL_TSTC_BIT)
+#define ETCTRL_PF (0xffff<<ETCTRL_PF_BIT)
+
+#define TSTXCTRL_TSTC_BIT 24
+#define ETCTRL_PF_BIT 0
+
+#define DEVNAME_LEN 11
+/* Private structure of SPWTDP driver. */
+struct spwtdp_priv {
+	char devname[DEVNAME_LEN];
+	struct drvmgr_dev *dev; /* Device */
+	struct spwtdp_regs *regs;
+	int open;
+	int index;
+	int initiator; /* Initiator configured */
+	int target; /* Target configured */
+	int freq; /* Frequency configured */
+
+	/* Spin-lock ISR protection */
+	SPIN_DECLARE(devlock);
+
+	/* Driver semaphore */
+	rtems_id sem;
+	spwtdp_isr_t isr;
+	void * isr_arg;
+};
+int spwtdp_count = 0;
+static struct spwtdp_priv *priv_tab[SPWTDP_MAX];
+
+
+STATIC void spwtdp_isr(void *data);
+STATIC int spwtdp_hw_reset(struct spwtdp_priv *priv);
+STATIC int spwtdp_init2(struct drvmgr_dev *dev);
+
+struct drvmgr_drv_ops spwtdp_ops =
+{
+	{NULL, spwtdp_init2, NULL, NULL},
+	NULL,
+	NULL
+};
+
+struct amba_dev_id spwtdp_ids[] =
+{
+	{VENDOR_GAISLER, GAISLER_SPWTDP},
+	{0, 0}	/* Mark end of table */
+};
+
+struct amba_drv_info spwtdp_drv_info =
+{
+	{
+		DRVMGR_OBJ_DRV,			/* Driver */
+		NULL,				/* Next driver */
+		NULL,				/* Device list */
+		DRIVER_AMBAPP_GAISLER_SPWTDP_ID,/* Driver ID */
+		"SPWTDP_DRV",			/* Driver Name */
+		DRVMGR_BUS_TYPE_AMBAPP,		/* Bus Type */
+		&spwtdp_ops,
+		NULL,				/* Funcs */
+		0,				/* No devices yet */
+		sizeof(struct spwtdp_priv),	/* Let DrvMgr allocate priv */
+	},
+	&spwtdp_ids[0]
+};
+
+/* Register the SPWTDP Driver */
+void spwtdp_register_drv(void)
+{
+	DBG("Registering SPWTDP driver\n");
+	drvmgr_drv_register(&spwtdp_drv_info.general);
+}
+
+STATIC int spwtdp_init(struct spwtdp_priv *priv)
+{
+	struct amba_dev_info *ainfo = priv->dev->businfo;
+	struct ambapp_apb_info *apb;
+
+	/* Get device information from AMBA PnP information */
+	if (ainfo == NULL) {
+		return -1;
+	}
+	apb = ainfo->info.apb_slv;
+	priv->regs = (struct spwtdp_regs *)apb->start;
+
+	spwtdp_hw_reset(priv);
+	/* Only support 56 bits counter */
+	if (REG_READ(&priv->regs->dat_ctrl) != 0x2f00) {
+		DBG("SPWTDP only supports 56 bit precission counters.\n");
+		return -1;
+	}
+	DBG("SPWTDP driver initialized\n");
+
+	return 0;
+}
+
+/*** INTERFACE TO DRIVER MANAGER ***/
+STATIC int spwtdp_init2(struct drvmgr_dev *dev)
+{
+	int status;
+	struct spwtdp_priv *priv;
+
+	DBG("SPWTDP[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
+
+	if (spwtdp_count >= SPWTDP_MAX)
+		return DRVMGR_ENORES;
+
+	priv = dev->priv;
+	if (priv == NULL)
+		return DRVMGR_NOMEM;
+
+	/* Register device */
+	priv->dev = dev;
+	priv->index = spwtdp_count;
+	priv_tab[priv->index] = priv;
+	snprintf(priv->devname, DEVNAME_LEN, "spwtdp%01u", priv->index);
+	spwtdp_count++;
+
+	/* Initialize Semaphore */
+	if (rtems_semaphore_create(
+		rtems_build_name('S', 'T', 'P', '0' + priv->index), 1,
+		RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | \
+		RTEMS_NO_INHERIT_PRIORITY | RTEMS_LOCAL | \
+		RTEMS_NO_PRIORITY_CEILING, 0, &priv->sem) != RTEMS_SUCCESSFUL) {
+		priv->sem = RTEMS_ID_NONE;
+		return DRVMGR_FAIL;
+	}
+
+	/* Initialize SPWTDP Hardware */
+	status = spwtdp_init(priv);
+	if (status) {
+		printk("Failed to initialize spwtdp driver %d\n", status);
+		return -1;
+	}
+
+	return DRVMGR_OK;
+}
+
+/* Hardware Reset of SPWTDP */
+STATIC int spwtdp_hw_reset(struct spwtdp_priv *priv)
+{
+	int i = 1000;
+	SPIN_IRQFLAGS(irqflags);
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+
+	/* Clear interrupts */
+	REG_WRITE(&priv->regs->ists, SPWTDP_IRQ_WCLEAR);
+
+	/* Reset the SPWTDP core */
+	REG_WRITE(&priv->regs->conf[0], CONF0_RS);
+
+	/* Wait for reset */
+	while ((REG_READ(&priv->regs->conf[0]) & CONF0_RS) && (i > 0)) {
+		i--;
+	}
+
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	return ((i > 0)? SPWTDP_ERR_OK : SPWTDP_ERR_ERROR);
+}
+
+int spwtdp_reset(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv and unregister isr */
+	int ret = spwtdp_isr_unregister(spwtdp);
+	if (ret != SPWTDP_ERR_OK)
+		return ret;
+
+	priv->initiator=0;
+	priv->target=0;
+	priv->freq=0;
+
+	return spwtdp_hw_reset(priv);
+}
+
+void *spwtdp_open(int dev_no)
+{
+	struct spwtdp_priv *priv;
+
+	if (dev_no >= spwtdp_count)
+		return NULL;
+
+	/* Get Device */
+	priv = priv_tab[dev_no];
+	if ((priv == NULL)||(priv->open == 1)) {
+		return NULL;
+	}
+
+	/* Set initial state of software */
+	priv->open = 1;
+
+	return priv;
+}
+
+int spwtdp_close(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv and reset core */
+	int ret = spwtdp_reset(spwtdp);
+	if (ret != SPWTDP_ERR_OK)
+		return ret;
+
+	priv->open = 0;
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_freq_setup(void *spwtdp, uint32_t fsinc, uint32_t cv, uint8_t etinc)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	REG_WRITE(&priv->regs->conf[1], fsinc & CONF1_FSINC);
+	REG_WRITE(&priv->regs->conf[2],
+		((cv<<CONF2_CV_BIT) & CONF2_CV) |
+		((uint32_t)etinc & CONF2_ETINC));
+
+	rtems_semaphore_release(priv->sem);
+	priv->freq = 1;
+
+	return SPWTDP_ERR_OK;
+}
+
+#define CONF0_INI_MASK (CONF0_EP|CONF0_ET|CONF0_SP|CONF0_SE|CONF0_LE| \
+		CONF0_TD)
+int spwtdp_initiator_conf(void *spwtdp, uint8_t mapping, uint32_t options)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as target */
+	if (priv->target == 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	conf0 &= ~(CONF0_INI_MASK|CONF0_MAP);
+	REG_WRITE(&priv->regs->conf[0],
+			conf0 | (options & CONF0_INI_MASK) |
+			(((uint32_t)mapping << CONF0_MAP_BIT) & CONF0_MAP));
+
+	priv->initiator = 1;
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+#define CONF0_TAR_MASK (CONF0_JE|CONF0_ST|CONF0_EP|CONF0_ET|CONF0_SP| \
+		CONF0_SE|CONF0_LE|CONF0_TD|CONF0_ME)
+int spwtdp_target_conf(void *spwtdp, uint8_t mapping, uint32_t options)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator == 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	conf0 &= ~(CONF0_TAR_MASK|CONF0_MAP);
+	REG_WRITE(&priv->regs->conf[0],
+			conf0 | (options & CONF0_TAR_MASK) |
+			(((uint32_t)mapping << CONF0_MAP_BIT) & CONF0_MAP));
+
+	priv->initiator = 1;
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_int_conf(void *spwtdp, uint8_t stm, uint8_t inrx,
+			      uint8_t intx)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	REG_WRITE(&priv->regs->conf[3],
+			(((uint32_t)stm << CONF3_STM_BIT) & CONF3_STM) |
+			(((uint32_t)inrx << CONF3_INRX_BIT) & CONF3_INRX) |
+			(((uint32_t)intx << CONF3_INTX_BIT) & CONF3_INTX));
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_target_int_conf(void *spwtdp, uint8_t inrx, uint8_t intx,
+			   uint32_t options)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL) {
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as target */
+	if (priv->target != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	REG_WRITE(&priv->regs->conf[3],
+			(options & CONF3_DI) |
+			(((uint32_t)inrx << CONF3_INRX_BIT) & CONF3_INRX) |
+			(((uint32_t)intx << CONF3_INTX_BIT) & CONF3_INTX));
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_enable(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL) {
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if frequency is configured */
+	if (priv->freq != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], conf0 | CONF0_TE);
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_target_enable(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as target */
+	if (priv->target != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if frequency is configured */
+	if (priv->freq != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], conf0 | CONF0_RE);
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_disable(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], conf0 & ~(CONF0_TE));
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_target_disable(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	/* Check priv */
+	if (priv == NULL)
+		return SPWTDP_ERR_NOINIT;
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int conf0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], conf0 & ~(CONF0_RE));
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+/* Get and Clear status */
+int spwtdp_status(void *spwtdp, uint32_t *sts, uint32_t clrmask)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	unsigned int status = REG_READ(&priv->regs->stat[0]);
+	REG_WRITE(&priv->regs->stat[0], status & clrmask);
+
+	if (sts != NULL)
+		*sts = status;
+
+	return SPWTDP_ERR_OK;
+}
+
+/* Get and Clear interrupts */
+int spwtdp_interrupt_status(void *spwtdp, uint32_t *sts, uint32_t clrmask)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+	SPIN_IRQFLAGS(irqflags);
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	SPIN_LOCK_IRQ(&priv->devlock, irqflags);
+	unsigned int status = REG_READ(&priv->regs->ists);
+	REG_WRITE(&priv->regs->ists, status & clrmask);
+	SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+
+	if (sts != NULL)
+		*sts = status;
+
+	return SPWTDP_ERR_OK;
+}
+
+/* Unmask interrupts */
+int spwtdp_interrupt_unmask(void *spwtdp, uint32_t irqmask)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	unsigned int ctrl = REG_READ(&priv->regs->ien);
+	REG_WRITE(&priv->regs->ien, ctrl | irqmask);
+
+	return SPWTDP_ERR_OK;
+}
+
+/* Mask interrupts */
+int spwtdp_interrupt_mask(void *spwtdp, uint32_t irqmask)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	unsigned int ctrl = REG_READ(&priv->regs->ien);
+	REG_WRITE(&priv->regs->ien, ctrl & ~(irqmask));
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_isr_register(void *spwtdp, spwtdp_isr_t func, void *data)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check isr */
+	if (func == NULL) {
+		/* No ISR */
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	priv->isr = func;
+	priv->isr_arg = data;
+
+	/* Register and Enable Interrupt at Interrupt controller */
+	drvmgr_interrupt_register(priv->dev, 0, "spwtdp", spwtdp_isr, priv);
+
+	/* Enable AMBA Interrupts */
+	SPIN_LOCK(&priv->devlock, irqflags);
+	unsigned int cfg0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], cfg0 | CONF0_AE);
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_isr_unregister(void *spwtdp)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	/* Disable IRQS (and check for priv!=NULL) */
+	int ret=spwtdp_interrupt_mask(spwtdp, SPWTDP_IRQ_WCLEAR);
+	if (ret != SPWTDP_ERR_OK)
+		return ret;
+
+	/* Disable AMBA Interrupts */
+	SPIN_LOCK(&priv->devlock, irqflags);
+	unsigned int cfg0 = REG_READ(&priv->regs->conf[0]);
+	REG_WRITE(&priv->regs->conf[0], cfg0 & ~(CONF0_AE));
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* Disable Interrupt at Interrupt controller */
+	drvmgr_interrupt_unregister(priv->dev, 0, spwtdp_isr, priv);
+
+	/* Unregister isr */
+	priv->isr = NULL;
+	priv->isr_arg = NULL;
+
+	return SPWTDP_ERR_OK;
+}
+
+STATIC void spwtdp_isr(void *arg)
+{
+	struct spwtdp_priv *priv = arg;
+	unsigned int ists = REG_READ(&priv->regs->ists);
+	SPIN_ISR_IRQFLAGS(irqflags);
+
+	/* Return if the SPWTDP didn't generate the IRQ */
+	if (ists == 0)
+		return;
+
+	SPIN_LOCK(&priv->devlock, irqflags);
+	REG_WRITE(&priv->regs->ists, ists); /* clear handled interrupt events */
+	SPIN_UNLOCK(&priv->devlock, irqflags);
+
+	/* Let user Handle Interrupt */
+	if (priv->isr!=NULL)
+		priv->isr(ists, priv->isr_arg);
+
+	return;
+}
+
+int spwtdp_dat_et_get(void * spwtdp, spwtdp_time_t * val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check pointer */
+	if (val == NULL) {
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	val->preamble = REG_READ(&priv->regs->dat_ctrl) & ETCTRL_PF;
+	unsigned int * buffer = (unsigned int *) val->data;
+	buffer[0] = REG_READ(&priv->regs->dat_et[0]);
+	buffer[1] = REG_READ(&priv->regs->dat_et[1]);
+	buffer[2] = REG_READ(&priv->regs->dat_et[2]);
+	buffer[3] = REG_READ(&priv->regs->dat_et[3]);
+	buffer[4] = REG_READ(&priv->regs->dat_et[4]);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_tsrx_et_get(void * spwtdp, spwtdp_time_t * val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check pointer */
+	if (val == NULL) {
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	val->preamble = REG_READ(&priv->regs->ts_rx_ctrl) & ETCTRL_PF;
+	unsigned int * buffer = (unsigned int *) val->data;
+	buffer[0] = REG_READ(&priv->regs->ts_rx_et[0]);
+	buffer[1] = REG_READ(&priv->regs->ts_rx_et[1]);
+	buffer[2] = REG_READ(&priv->regs->ts_rx_et[2]);
+	buffer[3] = REG_READ(&priv->regs->ts_rx_et[3]);
+	buffer[4] = REG_READ(&priv->regs->ts_rx_et[4]);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_tstx_et_get(void * spwtdp, spwtdp_time_t * val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check pointer */
+	if (val == NULL) {
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	val->preamble = REG_READ(&priv->regs->ts_tx_ctrl) & ETCTRL_PF;
+	unsigned int * buffer = (unsigned int *) val->data;
+	buffer[0] = REG_READ(&priv->regs->ts_tx_et[0]);
+	buffer[1] = REG_READ(&priv->regs->ts_tx_et[1]);
+	buffer[2] = REG_READ(&priv->regs->ts_tx_et[2]);
+	buffer[3] = REG_READ(&priv->regs->ts_tx_et[3]);
+	buffer[4] = REG_READ(&priv->regs->ts_tx_et[4]);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_lat_et_get(void * spwtdp, spwtdp_time_t * val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check pointer */
+	if (val == NULL) {
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	val->preamble = REG_READ(&priv->regs->lat_ctrl) & ETCTRL_PF;
+	unsigned int * buffer = (unsigned int *) val->data;
+	buffer[0] = REG_READ(&priv->regs->lat_et[0]);
+	buffer[1] = REG_READ(&priv->regs->lat_et[1]);
+	buffer[2] = REG_READ(&priv->regs->lat_et[2]);
+	buffer[3] = REG_READ(&priv->regs->lat_et[3]);
+	buffer[4] = REG_READ(&priv->regs->lat_et[4]);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_cmd_et_get(void * spwtdp, spwtdp_time_t * val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check pointer */
+	if (val == NULL) {
+		return SPWTDP_ERR_EINVAL;
+	}
+
+	val->preamble = REG_READ(&priv->regs->cmd_ctrl) & ETCTRL_PF;
+	unsigned int * buffer = (unsigned int *) val->data;
+	buffer[0] = REG_READ(&priv->regs->cmd_et[0]);
+	buffer[1] = REG_READ(&priv->regs->cmd_et[1]);
+	buffer[2] = REG_READ(&priv->regs->cmd_et[2]);
+	buffer[3] = REG_READ(&priv->regs->cmd_et[3]);
+	buffer[4] = REG_READ(&priv->regs->cmd_et[4]);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_tstx_conf(void * spwtdp, uint8_t tstc)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	REG_WRITE(&priv->regs->ts_tx_ctrl,
+		  (((uint32_t)tstc) << TSTXCTRL_TSTC_BIT) & TSTXCTRL_TSTC);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_cmd_et_set(void *spwtdp, spwtdp_time_t val)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	unsigned int * buffer = (unsigned int *) val.data;
+	REG_WRITE(&priv->regs->lat_et[0], buffer[0]);
+	REG_WRITE(&priv->regs->lat_et[1], buffer[1]);
+	REG_WRITE(&priv->regs->lat_et[2], buffer[2]);
+	REG_WRITE(&priv->regs->lat_et[3], buffer[3]);
+	REG_WRITE(&priv->regs->lat_et[4], buffer[4]);
+
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	/* Signal new command */
+	unsigned int ctrl = REG_READ(&priv->regs->cmd_ctrl);
+	REG_WRITE(&priv->regs->cmd_ctrl, ctrl | CTRL_NC);
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_initiator_cmd_spwtc_set(void *spwtdp, uint8_t spwtc)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as initiator */
+	if (priv->initiator != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	unsigned int ctrl = (REG_READ(&priv->regs->cmd_ctrl) &~ CTRL_SPWTC);
+	REG_WRITE(&priv->regs->cmd_ctrl,
+		  ctrl | (((uint32_t)spwtc << CTRL_SPWTC_BIT) & CTRL_SPWTC));
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+#define CTRL_TAR_MASK (CTRL_NC|CTRL_IS)
+int spwtdp_target_cmd_conf(void *spwtdp, uint8_t spwtc, uint16_t cpf,
+			   uint32_t options)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Check if configured as target */
+	if (priv->target != 1)
+		return SPWTDP_ERR_EINVAL;
+
+	/* Take SPWTDP lock - Wait until we get semaphore */
+	if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
+		!= RTEMS_SUCCESSFUL)
+		return SPWTDP_ERR_ERROR;
+
+	REG_WRITE(&priv->regs->cmd_ctrl, 
+			(options & CTRL_TAR_MASK) |
+			((cpf << CTRL_CPF_BIT) & CTRL_CPF) |
+			(((uint32_t)spwtc << CTRL_SPWTC_BIT) & CTRL_SPWTC));
+
+	rtems_semaphore_release(priv->sem);
+
+	return SPWTDP_ERR_OK;
+}
+
+int spwtdp_precision_get(void *spwtdp, uint8_t *fine, uint8_t *coarse)
+{
+	struct spwtdp_priv *priv = (struct spwtdp_priv *)spwtdp;
+	int coarse_precision, fine_precision;
+
+	if (priv == NULL) {
+		/* SPWTDP not initialized */
+		return SPWTDP_ERR_NOINIT;
+	}
+
+	if (priv->open == 0)
+		return SPWTDP_ERR_EINVAL;
+
+	unsigned int preamble = REG_READ(&priv->regs->dat_ctrl);
+
+	if (preamble & 0x80) {
+		DBG("Pfield second extension set: unknown format");
+		return SPWTDP_ERR_ERROR;
+	}
+	if (!((preamble & 0x7000) == 0x2000 || (preamble & 0x7000) == 0x1000)) {
+		DBG(" PField indicates not unsegmented code: unknown format");
+		return SPWTDP_ERR_ERROR;
+	}
+	/*
+	 * coarse_precision = 32;
+	 * fine_precision = 24;
+	 */
+	coarse_precision = ((preamble >> 10) & 0x3) + 1;
+	if (preamble & 0x80)
+		coarse_precision += (preamble >> 5) & 0x3;
+	fine_precision = (preamble >> 8) & 0x3;
+	if (preamble & 0x80)
+		fine_precision += (preamble >> 2) & 0x7;
+	if (coarse!=NULL)
+		*coarse = coarse_precision;
+	if (fine!=NULL)
+		*fine = fine_precision;
+
+	return SPWTDP_ERR_OK;
+}
+