RMAP: added new asynchonous interface

7 files changed, 563 insertions, 267 deletions

diff --git a/c/src/lib/libbsp/sparc/Makefile.am b/c/src/lib/libbsp/sparc/Makefile.am
index cbf7a08ddc..9e51927ba2 100644
--- a/c/src/lib/libbsp/sparc/Makefile.am
+++ b/c/src/lib/libbsp/sparc/Makefile.am
@@ -72,6 +72,8 @@ EXTRA_DIST += shared/include/debug_defs.h
 EXTRA_DIST += shared/spw/grspw.c
 EXTRA_DIST += shared/spw/grspw_router.c
 EXTRA_DIST += shared/spw/rmap.c
+EXTRA_DIST += shared/spw/rmap_async.c
+EXTRA_DIST += shared/spw/rmap_common.c
 EXTRA_DIST += shared/spw/rmap_drv_grspw.c
 EXTRA_DIST += shared/include/grspw.h
 EXTRA_DIST += shared/include/grspw_router.h
diff --git a/c/src/lib/libbsp/sparc/leon2/Makefile.am b/c/src/lib/libbsp/sparc/leon2/Makefile.am
index 2fedddc83e..fc9683f915 100644
--- a/c/src/lib/libbsp/sparc/leon2/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon2/Makefile.am
@@ -139,6 +139,8 @@ include_HEADERS += ../../sparc/shared/include/rmap_drv_grspw.h
 libbsp_a_SOURCES += ../../sparc/shared/spw/grspw.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/grspw_router.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/rmap.c
+libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_async.c
+libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_common.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_drv_grspw.c
 
 # UART (RAW)
diff --git a/c/src/lib/libbsp/sparc/leon3/Makefile.am b/c/src/lib/libbsp/sparc/leon3/Makefile.am
index d71200f738..cb18132989 100644
--- a/c/src/lib/libbsp/sparc/leon3/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon3/Makefile.am
@@ -155,6 +155,8 @@ libbsp_a_SOURCES += ../../sparc/shared/spw/grspw.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/grspw_pkt.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/grspw_router.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/rmap.c
+libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_async.c
+libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_common.c
 libbsp_a_SOURCES += ../../sparc/shared/spw/rmap_drv_grspw.c
 
 # UART
diff --git a/c/src/lib/libbsp/sparc/shared/include/rmap.h b/c/src/lib/libbsp/sparc/shared/include/rmap.h
index c18dafed5e..6a34ae7c5e 100644
--- a/c/src/lib/libbsp/sparc/shared/include/rmap.h
+++ b/c/src/lib/libbsp/sparc/shared/include/rmap.h
@@ -12,6 +12,8 @@
 #ifndef __RMAP_H__
 #define __RMAP_H__
 
+#include <rtems.h>
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -85,17 +87,18 @@ typedef int (*rmap_route_t)(void *cookie, int dir, int srcadr, int dstadr, void
 
 struct rmap_config {
 	rmap_route_t	route_func;	/* Function that will generate path addressing */
-	int		tid_msb;	/* 8 most significant bits in TID.
+	unsigned short	spw_adr;	/* The SpW Address of the SpW interface used */
+	unsigned char	tid_msb;	/* 8 most significant bits in TID.
 					 * Set to -1 for normal operation using
 					 * all bits in TID for sequence counting.
 					 */
-	int		spw_adr;	/* The SpW Address of the SpW interface used */
+	unsigned char	drv_cap;	/* Driver capabilities */
+	char		thread_safe;	/* Set this to non-zero to enable the RMAP stack to create a
+					 * semaphore used to protect from multiple tasks entering the
+					 * transfer function(s) of the stack at the same time */
 	struct rmap_drv	*drv;		/* Driver used for transmission */
 	int		max_rx_len;	/* Maximum data length of received packets */
 	int		max_tx_len;	/* Maximum data length of transmitted packets */
-	int		thread_safe;	/* Set this to non-zero to enable the RMAP stack to create a
-					 * semaphore used to protect from multiple tasks entering the
-					 * transfer function(s) of the stack at the same time */
 };
 
 struct rmap_command {
@@ -274,8 +277,6 @@ extern int rmap_resp_poll(void *cookie, struct rmap_command *resp, unsigned int
 
 /*** RMAP Stack help interface ***/
 
-/* Parse a Write command */
-
 /* Return the CRC of a RMAP data buffer of length len */
 extern unsigned char rmap_crc_calc(unsigned char *data, unsigned int len);
 
@@ -285,6 +286,37 @@ extern int rmap_write(void *cookie, void *dst, void *buf, int length, int dstadr
 /* A handy read function using the RMAP stack */
 extern int rmap_read(void *cookie, void *src, void *buf, int length, int dstadr, int dstkey);
 
+/*** Asynchronous interface ***/
+
+/* Initilaize Asynchonous RMAP Stack */
+void *rmap_async_init(struct rmap_config *config, int response_array_length);
+
+/* Reset internal states of the asynchonous RMAP stack */
+void rmap_async_reset(void *cookie);
+
+/* Initilize packet buffer according to cmd. If cmd is a cmd will be  */
+int rmap_send_async(void *cookie,
+		    struct rmap_command *cmd,
+		    struct rmap_spw_pkt *pkt);
+
+/* Process received RMAP response packet buffer and find request cmd */
+int rmap_recv_async(void *cookie, struct rmap_spw_pkt *pkt, struct rmap_command **pcmd);
+
+/* If a RMAP command is canceled */
+int rmap_cancel_async(void *cookie, struct rmap_command *cmd);
+
+/*** below here is only used internally to implement RMAP stack ***/
+struct rmap_common_priv {
+	struct rmap_config	*config;		/* Configuration */
+	unsigned short		tid;			/* Current TID */
+	unsigned char		drv_cap;		/* Driver capabilities */
+	rtems_id		lock;			/* Optional Semaphore protection against multiple threads (thread-safe) */
+};
+
+extern int rmap_stack_count;
+
+extern int rmap_build(struct rmap_common_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *txpkt);
+extern int rmap_parse(struct rmap_common_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *rxpkt);
 #ifdef __cplusplus
 }
 #endif
diff --git a/c/src/lib/libbsp/sparc/shared/spw/rmap.c b/c/src/lib/libbsp/sparc/shared/spw/rmap.c
index a3fe60f8bc..ee622fa1bb 100644
--- a/c/src/lib/libbsp/sparc/shared/spw/rmap.c
+++ b/c/src/lib/libbsp/sparc/shared/spw/rmap.c
@@ -23,59 +23,15 @@
  #define DBG(args...)
 #endif
 
-static int rmap_stack_count = 0;
-
-static unsigned char RMAP_CRCTable[256] = {
-    0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
-    0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
-    0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
-    0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
-    0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
-    0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
-    0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
-    0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
-    0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
-    0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
-    0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
-    0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
-    0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
-    0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
-    0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
-    0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
-    0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
-    0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
-    0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
-    0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
-    0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
-    0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
-    0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
-    0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
-    0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
-    0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
-    0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
-    0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
-    0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
-    0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
-    0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
-    0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf,
-};
-
-unsigned char rmap_crc_calc(unsigned char *data, unsigned int len)
-{
-	unsigned char crc = 0;
-	unsigned int i;
-	for (i = 0; i < len; i++) {
-		crc = RMAP_CRCTable[(crc ^ data[i]) & 0xff];
-	}
-	return crc;
-}
-
 struct rmap_priv {
+	/* Common structure */
 	struct rmap_config	*config;		/* Configuration */
+	unsigned short		tid;			/* Current TID */
+	unsigned char		drv_cap;		/* Driver capabilities */
+	rtems_id		lock;			/* Optional Semaphore protection against multiple threads (thread-safe) */
+
 	void			*drv_cookie;		/* Driver private structure */
 	struct rmap_drv_timeout timeout;		/* Driver timeout */
-	unsigned short		tid;			/* Current TID */
-	char			drv_cap;		/* Driver capabiliteis */
 	char			running;		/* 1 is started, 0 if stopped */
 	unsigned char		blocking;		/* Blocking mode */
 	unsigned char		tx_pkt_hdr[256+9];	/* Packet header used for transmission, last 9 bytes is for RMW commands */
@@ -84,7 +40,7 @@ struct rmap_priv {
 	unsigned int		rx_pkt_buf_len;		/* RX packet Buffer length */
 	struct rmap_spw_pkt	rxpkt;			/* RX packet */
 	struct rmap_spw_pkt	txpkt;			/* TX packet */
-	rtems_id		lock;			/* Optional Semaphore protection against multiple threads (thread-safe) */
+
 };
 
 void *rmap_init(struct rmap_config *config)
@@ -160,7 +116,9 @@ int rmap_start(struct rmap_priv *priv)
 
 		if ( priv->drv_cap == 0 ) {
 			unsigned int capabilities;
-			if ( priv->config->drv->ops.ioctl(priv->drv_cookie, RMAP_DRV_IOCTL_GET_CAP, &capabilities) ){
+			if (priv->config->drv_cap != 0) {
+				priv->drv_cap = priv->config->drv_cap;
+			} else if ( priv->config->drv->ops.ioctl(priv->drv_cookie, RMAP_DRV_IOCTL_GET_CAP, &capabilities) ){
 				/* Failed to get capabilities, assume none */
 				priv->drv_cap = 0;
 			} else {
@@ -248,211 +206,6 @@ int rmap_ioctl(void *cookie, int command, void *arg)
 	}
 }
 
-int rmap_build(struct rmap_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *txpkt)
-{
-	int len1, len2;
-	unsigned char *pos, *pkt = (unsigned char *)txpkt->hdr;
-	unsigned int length, src_path_len;
-	int options;
-
-	/* Begin with address translation */
-	if ( priv->config->route_func ) {
-		/* Custom translation */
-
-		/* Generate path addressing to destination */
-		len1 = 120;
-		priv->config->route_func(priv, 0, priv->config->spw_adr, cmd->dstadr, pkt, &len1);
-
-		/* Generate path addressing from destination */
-		len2 = 13;
-		priv->config->route_func(priv, 1, priv->config->spw_adr, cmd->dstadr, &pkt[len1+3], &len2);
-
-	} else {
-		pkt[0] = cmd->dstadr & 0xff;
-		pkt[4] = priv->config->spw_adr;
-		len1 = 1;
-		len2 = 1;
-	}
-	/* Protocol Identifier */
-	if ( len2 == 1 ) {
-		src_path_len = 0;
-	} else {
-		src_path_len = ((len2-1)+3) / 4;
-	}
-	pos = &pkt[len1];
-	*pos++ = 0x01;
-	*pos++ = 0x40 | (cmd->type << 2) | src_path_len;
-	*pos++ = cmd->dstkey;
-	pos += len2; /* Jump over address,  len1 + 3 + len2 */
-	*pos++ = (priv->tid >> 8);
-	*pos++ = priv->tid & 0xff;
-	cmd->tid = priv->tid; /* Remember TID */
-	/* Increment TID */
-	if ( priv->config->tid_msb < 0 ) {
-		priv->tid++;
-	} else {
-		if ( (priv->tid & 0xff) == 0xff) {
-			priv->tid = priv->tid & 0xff00;
-		} else {
-			priv->tid++;
-		}
-	}
-
-	/* Address */
-	*pos++ = (cmd->address >> 32) & 0xff;	/* Extended Address */
-	*pos++ = (cmd->address >> 24) & 0xff;
-	*pos++ = (cmd->address >> 16) & 0xff;
-	*pos++ = (cmd->address >> 8) & 0xff;
-	*pos++ = cmd->address & 0xff;
-	/* pos = pkt[len1+3+len2+7] */
-	if ( cmd->type & RMAP_CMD_WRITE ) {		/* WRITE */
-		length = cmd->data.write.length;
-	} else if ( cmd->type == RMAP_CMD_RMWI ) {	/* READ-MODIFY_WRITE */
-		length = cmd->data.read_m_write.length * 2;
-	} else {					/* READ */
-		length = cmd->data.read.length;
-	}
-	*pos++ = (length >> 16) & 0xff;
-	*pos++ = (length >> 8) & 0xff;
-	*pos++ = length & 0xff;
-	txpkt->hlen = pos - pkt; /* HDR CRC is fixed later */
-
-	if ( cmd->type & RMAP_CMD_WRITE ) {		/* WRITE */
-		txpkt->dlen = length;
-		if ( cmd->data.write.data > 0 ) {
-			txpkt->data = cmd->data.write.data; /* CRC will be added later */
-		} else {
-			txpkt->data = ((char *)&txpkt->dlen + 1); /* Temporary storage of CRC when no data */
-		}
-	} else if ( cmd->type == RMAP_CMD_RMWI ) {	/* READ-MODIFY_WRITE */
-		txpkt->dlen = length; /* CRC will be added later */
-		txpkt->data = &txpkt->hdr[256];
-		if ( cmd->data.read_m_write.length == 4 ) {
-			/* 4byte */
-			txpkt->hdr[256] = (cmd->data.read_m_write.data >> 24) & 0xff;
-			txpkt->hdr[257] = (cmd->data.read_m_write.data >> 16) & 0xff;
-			txpkt->hdr[258] = (cmd->data.read_m_write.data >> 8) & 0xff;
-			txpkt->hdr[259] = cmd->data.read_m_write.data & 0xff;
-			
-			txpkt->hdr[260] = (cmd->data.read_m_write.mask >> 24) & 0xff;
-			txpkt->hdr[261] = (cmd->data.read_m_write.mask >> 16) & 0xff;
-			txpkt->hdr[262] = (cmd->data.read_m_write.mask >> 8) & 0xff;
-			txpkt->hdr[263] = cmd->data.read_m_write.mask & 0xff;
-		} else if ( cmd->data.read_m_write.length == 1 ) {
-			/* 1byte */
-			txpkt->hdr[256] = cmd->data.read_m_write.data & 0xff;
-			txpkt->hdr[257] = cmd->data.read_m_write.mask & 0xff;
-		} else if ( cmd->data.read_m_write.length == 2 ) {
-			/* 2byte */
-			txpkt->hdr[256] = (cmd->data.read_m_write.data >> 8) & 0xff;
-			txpkt->hdr[257] = cmd->data.read_m_write.data & 0xff;
-			
-			txpkt->hdr[258] = (cmd->data.read_m_write.mask >> 8) & 0xff;
-			txpkt->hdr[259] = cmd->data.read_m_write.mask & 0xff;
-		} else {
-			/* Wrong input */
-			return -1;
-		}
-	} else {					/* READ */
-		txpkt->dlen = 0;
-		txpkt->data = NULL;
-	}
-
-	/*** CRC ***/
-	if ( len1 > 1 ) /* PATH target address not part of Header CRC */
-		options = PKT_OPTION_HDR_CRC_SKIPLEN(len1-1);
-	else
-		options = 0;
-
-	/* HEADER CRC */
-	if ( priv->drv_cap & DRV_CAP_HDR_CRC ) {
-		options |= PKT_OPTION_HDR_CRC;
-	} else {
-		/* Generate CRC and put it at the end of data buffer, Don't calculate
-		 * CRC on Destination Path address. 
-		 */
-		unsigned char crc;
-		crc = rmap_crc_calc(&txpkt->hdr[len1-1], txpkt->hlen - (len1-1));
-		((unsigned char *)txpkt->hdr)[txpkt->hlen] = crc;
-		txpkt->hlen++;
-	}
-	/* DATA CRC */
-	if ( txpkt->data ) {
-		if ( txpkt->dlen == 0 ) {
-			/* Need to put CRC even when no data available,
-			 * CRC will always be 0x00 
-			 */
-			((unsigned char *)txpkt->data)[0] = 0x00;
-			txpkt->dlen = 1;
-		} else if ( priv->drv_cap & DRV_CAP_DATA_CRC ) {
-			options |= PKT_OPTION_DATA_CRC;
-		} else {
-			/* Generate CRC and put it at the end of data buffer */
-			unsigned char crc;
-			crc = rmap_crc_calc(txpkt->data, txpkt->dlen);
-			((unsigned char *)txpkt->data)[txpkt->dlen] = crc;
-			txpkt->dlen++;
-		}
-	}
-
-	txpkt->options = options;
-
-	return 0;
-}
-
-int rmap_parse(struct rmap_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *rxpkt)
-{
-	unsigned char *pkt;
-	unsigned int length;
-
-	pkt = rxpkt->data;
-
-	/* Check that the protocol ID, Replay and TID matches before proceeding */
-	if (	(pkt[1] != 0x01) || ((pkt[2] & 0x40) != 0) || 
-		(pkt[5] != (cmd->tid>>8)) || (pkt[6] != (cmd->tid & 0xff))) {
-		return -1;
-	}
-
-	cmd->status = pkt[3];
-	if ( cmd->type & RMAP_CMD_WRITE ){
-		return 0;
-	}
-
-	if ( cmd->type == RMAP_CMD_RMWI ) {
-		/* Copy data content */
-		cmd->data.read_m_write.oldlength = length = pkt[10];
-		if ( length == 4 ) {
-			cmd->data.read_m_write.olddata = ntohl(*(unsigned int *)&pkt[12]);
-		} else if ( length == 1 ){
-			cmd->data.read_m_write.olddata = pkt[12];
-		} else if ( length == 2 ) {
-			cmd->data.read_m_write.olddata = ntohs(*(unsigned short *)&pkt[12]);
-		} else {
-			cmd->data.read_m_write.olddata = 0;
-		}
-		return 0;
-	}
-
-	/* READ COMMAND RESPONSE - copy data */
-	if ( cmd->status != 0 ) {
-		/* Error status, skip other bytes */
-		return 0;
-	}
-
-	/* Length may be equal or less than request nembuer of bytes */
-	cmd->data.read.datalength = length = (pkt[8]<<16) | (pkt[9]<<8) | pkt[10];
-	if ( length > cmd->data.read.length ) {
-		printf("Response DATA Length != Command Data Length [%p, %p]\n",
-			cmd, pkt);
-		return -1;
-	}
-	if ( length > 0 ) {
-		memcpy(cmd->data.read.data, &pkt[12], length);
-	}
-
-	return 0;
-}
-
 int rmap_send(void *cookie, struct rmap_command *cmd)
 {
 	struct rmap_priv *priv = cookie;
@@ -492,8 +245,8 @@ int rmap_send(void *cookie, struct rmap_command *cmd)
 	if ( priv->lock ) {
 		status = rtems_semaphore_obtain(priv->lock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 		if ( status != RTEMS_SUCCESSFUL ) {
-			printf("RMAP-STACK: FAILED TO OBTAIN LOCK: %d (%p)\n",
-				status, priv->lock);
+			printf("RMAP-STACK: FAILED TO OBTAIN LOCK: %d (%x)\n",
+				status, (unsigned int)priv->lock);
 			return -1;
 		}
 	}
@@ -508,7 +261,7 @@ int rmap_send(void *cookie, struct rmap_command *cmd)
 	txpkt->data = NULL;
 	txpkt->dlen = 0;
 	txpkt->options = 0;
-	if ( rmap_build(priv, cmd, txpkt) ){
+	if ( rmap_build((struct rmap_common_priv *)priv, cmd, txpkt) ){
 		ret = -1;
 		goto out_sem;
 	}
@@ -544,7 +297,8 @@ retry:
 		/*** ONE RECEIVED PACKET ***/
 
 		/* Parse input packet */
-		if ( rmap_parse(priv, cmd, rxpkt) == -1 ) {
+		if (rmap_parse((struct rmap_common_priv *)priv, cmd, rxpkt)
+		    == -1) {
 			/* Got wrong packet, for example wrong TID */
 			DBG("RMAP: wrong packet\n");
 			goto retry;
diff --git a/c/src/lib/libbsp/sparc/shared/spw/rmap_async.c b/c/src/lib/libbsp/sparc/shared/spw/rmap_async.c
new file mode 100644
index 0000000000..69187e1d72
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/spw/rmap_async.c
@@ -0,0 +1,225 @@
+/*  RMAP stack and RMAP driver interface implementation
+ *
+ *  COPYRIGHT (c) 2009
+ *  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-17, Daniel Hellstrom <daniel@gaisler.com>
+ *    Created
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rmap.h"
+
+#ifdef DEBUG
+ #define DBG(args...) printf(args);
+#else
+ #define DBG(args...)
+#endif
+
+struct rmap_async_priv {
+	struct rmap_config	*config;	/* Configuration */
+	unsigned short		tid;		/* Current TID */
+	char			drv_cap;	/* Driver capabiliteis */
+	rtems_id		lock;		/* Optional Semaphore protection against multiple threads (thread-safe) */
+
+	int			resp_array_len;	/* Length of cmds array */
+	struct rmap_command	*cmds[];	/* RMAP commands waiting for response */
+};
+
+void *rmap_async_init(struct rmap_config *config, int response_array_length)
+{
+	struct rmap_async_priv *priv;
+	int status;
+
+	priv = (struct rmap_async_priv *)malloc(sizeof(struct rmap_async_priv));
+	if ( !priv )
+		return NULL;
+
+	memset(priv, 0, sizeof(struct rmap_async_priv) +
+	       (response_array_length * sizeof(struct rmap_command *)));
+	priv->config = config;
+	if (config->tid_msb >= 0) {
+		priv->tid = config->tid_msb << 8;
+	}
+	priv->drv_cap = config->drv_cap;
+	priv->resp_array_len = response_array_length;
+
+	/* Create Semaphore used to make RMAP layer Thread-safe */
+	if (config->thread_safe) {
+		status = rtems_semaphore_create(
+			rtems_build_name('R', 'M', 'A', 'P' + rmap_stack_count++),
+			1,
+			RTEMS_FIFO | RTEMS_COUNTING_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \
+			RTEMS_LOCAL | RTEMS_NO_PRIORITY_CEILING,
+			0,
+			&priv->lock);
+		if ( status != RTEMS_SUCCESSFUL ) {
+			printf("RMAP_INIT: Failed to create semaphore: %d\n", status);
+			free(priv);
+			return NULL;
+		}
+		DBG("RMAP_INIT: semaphore ID: 0x%x (%p)\n", priv->lock, &priv->lock);
+	}
+
+	return priv;
+}
+
+void rmap_async_reset(void *cookie)
+{
+	struct rmap_async_priv *priv = cookie;
+
+	memset(priv->cmds, 0, 
+	       priv->resp_array_len * sizeof(struct rmap_command *));
+}
+
+static struct rmap_command *rmap_find_async(void *cookie, unsigned short tid)
+{
+	struct rmap_async_priv *priv = cookie;
+	struct rmap_command *cmd;
+	int i;
+
+	for (i = 0; i<priv->resp_array_len; i++) {
+		cmd = priv->cmds[i];
+		if ((cmd != NULL) && (cmd->tid == tid)) {
+		    	priv->cmds[i] = NULL;
+			return cmd;
+		}
+	}
+
+	return NULL;
+}
+
+static int rmap_add_async(void *cookie, struct rmap_command *cmd)
+{
+	struct rmap_async_priv *priv = cookie;
+	int i, status, ret;
+
+	if (priv->lock) {
+		status = rtems_semaphore_obtain(priv->lock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		if (status != RTEMS_SUCCESSFUL) {
+			printf("RMAP-STACK: FAILED TO OBTAIN LOCK: %d (%x)\n",
+				status, (unsigned int)priv->lock);
+			return -2;
+		}
+	}
+
+	for (i = 0; i<priv->resp_array_len; i++) {
+		if (priv->cmds[i] == NULL) {
+			priv->cmds[i] = cmd;
+			ret = 0;
+			goto out_sem;
+		}
+	}
+	ret = -1;
+
+out_sem:
+	if (priv->lock)
+		rtems_semaphore_release(priv->lock);
+
+	return ret;
+}
+
+int rmap_send_async(void *cookie, struct rmap_command *cmd, struct rmap_spw_pkt *pkt)
+{
+	struct rmap_async_priv *priv = cookie;
+	int wait_response;
+
+	if ( !priv || !cmd || !pkt )
+		return -1;
+
+	/*** Check invalid command type ***/
+	if (	(cmd->type > RMAP_CMD_WIVA) ||
+		((cmd->type < RMAP_CMD_RMWI) && (cmd->type != RMAP_CMD_RS) && (cmd->type != RMAP_CMD_RI))
+	   )
+		return -1;
+	if (	(cmd->type == RMAP_CMD_RMWI) && (cmd->data.read_m_write.length != 1) && 
+		(cmd->data.read_m_write.length != 2) && (cmd->data.read_m_write.length != 4)
+	   )
+		return -1;
+
+	/*** Determine if stack must wait for response to arrive after transmission ***/
+	wait_response = 0;
+	if (	((cmd->type & RMAP_CMD_WRITE) == 0) ||	/* READ ALWAYS RETURNS DATA */
+		(cmd->type & RMAP_CMD_ACKNOWLEDGE)	/* WAIT FOR ACK */
+	   )
+		wait_response = 1;
+
+	/*** Check that READ/WRITE length is within boundaries ***/
+	if ( (cmd->type & RMAP_CMD_WRITE) && (cmd->data.write.length > (priv->config->max_tx_len-4)) ) {
+		return -1;
+	}
+	if ( ((cmd->type & RMAP_CMD_WRITE) == 0) && (cmd->data.read.length > (priv->config->max_rx_len)) ) {
+		return -1;
+	}
+
+	/*** Build command ***/
+	pkt->hlen = 0;
+	pkt->dlen = 0;
+	pkt->options = 0;
+	if ( rmap_build((struct rmap_common_priv *)priv, cmd, pkt) ) {
+		return -2;
+	}
+
+	/* Add packet to response list */
+	if (wait_response) {
+		cmd->status = 0xff; /* indicate to be updated by response */
+		if (rmap_add_async(priv, cmd))
+			return -3;
+	} else {
+		cmd->status = 0; /* no response expected. Ok status. */
+	}
+
+	return wait_response;
+}
+
+int rmap_recv_async(void *cookie, struct rmap_spw_pkt *pkt, struct rmap_command **pcmd)
+{
+	struct rmap_async_priv *priv = cookie;
+	struct rmap_command *cmd;
+	unsigned char *data = pkt->data;
+	unsigned short tid;
+
+	if (!priv || !pkt || !data)
+		return -1;
+
+	/*** From TID find Command structure ***/
+	tid = (data[5] << 8) |data[6];
+
+	/* if *pcmd is not NULL, user wants to find a specific command */
+	if (pcmd && (*pcmd != NULL) && (tid != (*pcmd)->tid))
+		return 1;
+
+	/* Fuind in table of old requests */
+	cmd = rmap_find_async(priv, tid);
+	if (!cmd)
+		return -2;
+	if (pcmd && (*pcmd == NULL))
+		*pcmd = cmd;
+
+	/* Parse input packet */
+	if (rmap_parse((struct rmap_common_priv *)priv, cmd, pkt) != 0)
+		return -3; /* Got wrong packet, for example wrong TID */
+
+	return 0;
+}
+
+int rmap_cancel_async(void *cookie, struct rmap_command *cmd)
+{
+	struct rmap_async_priv *priv = cookie;
+	int i;
+
+	for (i = 0; i<priv->resp_array_len; i++) {
+		if (priv->cmds[i] == cmd) {
+		    	priv->cmds[i] = NULL;
+			return 0;
+		}
+	}
+
+	return -1; /* didn't find command in table */
+}
diff --git a/c/src/lib/libbsp/sparc/shared/spw/rmap_common.c b/c/src/lib/libbsp/sparc/shared/spw/rmap_common.c
new file mode 100644
index 0000000000..ec3755fa2e
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/shared/spw/rmap_common.c
@@ -0,0 +1,279 @@
+/*  RMAP stack and RMAP driver interface implementation
+ *
+ *  COPYRIGHT (c) 2009
+ *  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-17, Daniel Hellstrom <daniel@gaisler.com>
+ *    Created
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <netinet/in.h>
+#include "rmap.h"
+
+#ifdef DEBUG
+ #define DBG(args...) printf(args);
+#else
+ #define DBG(args...)
+#endif
+
+int rmap_stack_count = 0;
+
+static unsigned char RMAP_CRCTable[256] = {
+    0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
+    0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
+    0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
+    0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
+    0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
+    0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
+    0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
+    0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
+    0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
+    0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
+    0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
+    0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
+    0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
+    0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
+    0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
+    0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
+    0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
+    0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
+    0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
+    0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
+    0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
+    0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
+    0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
+    0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
+    0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
+    0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
+    0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
+    0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
+    0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
+    0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
+    0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
+    0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf,
+};
+
+unsigned char rmap_crc_calc(unsigned char *data, unsigned int len)
+{
+	unsigned char crc = 0;
+	unsigned int i;
+	for (i = 0; i < len; i++) {
+		crc = RMAP_CRCTable[(crc ^ data[i]) & 0xff];
+	}
+	return crc;
+}
+
+int rmap_build(struct rmap_common_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *txpkt)
+{
+	int len1, len2;
+	unsigned char *pos, *pkt = (unsigned char *)txpkt->hdr;
+	unsigned int length, src_path_len;
+	int options;
+
+	/* Begin with address translation */
+	if ( priv->config->route_func ) {
+		/* Custom translation */
+
+		/* Generate path addressing to destination */
+		len1 = 120;
+		priv->config->route_func(priv, 0, priv->config->spw_adr, cmd->dstadr, pkt, &len1);
+
+		/* Generate path addressing from destination */
+		len2 = 13;
+		priv->config->route_func(priv, 1, priv->config->spw_adr, cmd->dstadr, &pkt[len1+3], &len2);
+
+	} else {
+		pkt[0] = cmd->dstadr & 0xff;
+		pkt[4] = priv->config->spw_adr;
+		len1 = 1;
+		len2 = 1;
+	}
+	/* Protocol Identifier */
+	if ( len2 == 1 ) {
+		src_path_len = 0;
+	} else {
+		src_path_len = ((len2-1)+3) / 4;
+	}
+	pos = &pkt[len1];
+	*pos++ = 0x01;
+	*pos++ = 0x40 | (cmd->type << 2) | src_path_len;
+	*pos++ = cmd->dstkey;
+	pos += len2; /* Jump over address,  len1 + 3 + len2 */
+	*pos++ = (priv->tid >> 8);
+	*pos++ = priv->tid & 0xff;
+	cmd->tid = priv->tid; /* Remember TID */
+	/* Increment TID */
+	if ( priv->config->tid_msb < 0 ) {
+		priv->tid++;
+	} else {
+		if ( (priv->tid & 0xff) == 0xff) {
+			priv->tid = priv->tid & 0xff00;
+		} else {
+			priv->tid++;
+		}
+	}
+
+	/* Address */
+	*pos++ = (cmd->address >> 32) & 0xff;	/* Extended Address */
+	*pos++ = (cmd->address >> 24) & 0xff;
+	*pos++ = (cmd->address >> 16) & 0xff;
+	*pos++ = (cmd->address >> 8) & 0xff;
+	*pos++ = cmd->address & 0xff;
+	/* pos = pkt[len1+3+len2+7] */
+	if ( cmd->type & RMAP_CMD_WRITE ) {		/* WRITE */
+		length = cmd->data.write.length;
+	} else if ( cmd->type == RMAP_CMD_RMWI ) {	/* READ-MODIFY_WRITE */
+		length = cmd->data.read_m_write.length * 2;
+	} else {					/* READ */
+		length = cmd->data.read.length;
+	}
+	*pos++ = (length >> 16) & 0xff;
+	*pos++ = (length >> 8) & 0xff;
+	*pos++ = length & 0xff;
+	txpkt->hlen = pos - pkt; /* HDR CRC is fixed later */
+
+	if ( cmd->type & RMAP_CMD_WRITE ) {		/* WRITE */
+		txpkt->dlen = length;
+		if ( cmd->data.write.data != 0 ) {
+			txpkt->data = cmd->data.write.data; /* CRC will be added later */
+		} else {
+			txpkt->data = ((unsigned char *)&txpkt->dlen + 1); /* Temporary storage of CRC when no data */
+		}
+	} else if ( cmd->type == RMAP_CMD_RMWI ) {	/* READ-MODIFY_WRITE */
+		txpkt->dlen = length; /* CRC will be added later */
+		txpkt->data = &txpkt->hdr[256];
+		if ( cmd->data.read_m_write.length == 4 ) {
+			/* 4byte */
+			txpkt->hdr[256] = (cmd->data.read_m_write.data >> 24) & 0xff;
+			txpkt->hdr[257] = (cmd->data.read_m_write.data >> 16) & 0xff;
+			txpkt->hdr[258] = (cmd->data.read_m_write.data >> 8) & 0xff;
+			txpkt->hdr[259] = cmd->data.read_m_write.data & 0xff;
+			
+			txpkt->hdr[260] = (cmd->data.read_m_write.mask >> 24) & 0xff;
+			txpkt->hdr[261] = (cmd->data.read_m_write.mask >> 16) & 0xff;
+			txpkt->hdr[262] = (cmd->data.read_m_write.mask >> 8) & 0xff;
+			txpkt->hdr[263] = cmd->data.read_m_write.mask & 0xff;
+		} else if ( cmd->data.read_m_write.length == 1 ) {
+			/* 1byte */
+			txpkt->hdr[256] = cmd->data.read_m_write.data & 0xff;
+			txpkt->hdr[257] = cmd->data.read_m_write.mask & 0xff;
+		} else if ( cmd->data.read_m_write.length == 2 ) {
+			/* 2byte */
+			txpkt->hdr[256] = (cmd->data.read_m_write.data >> 8) & 0xff;
+			txpkt->hdr[257] = cmd->data.read_m_write.data & 0xff;
+			
+			txpkt->hdr[258] = (cmd->data.read_m_write.mask >> 8) & 0xff;
+			txpkt->hdr[259] = cmd->data.read_m_write.mask & 0xff;
+		} else {
+			/* Wrong input */
+			return -1;
+		}
+	} else {					/* READ */
+		txpkt->dlen = 0;
+		txpkt->data = NULL;
+	}
+
+	/*** CRC ***/
+	if ( len1 > 1 ) /* PATH target address not part of Header CRC */
+		options = PKT_OPTION_HDR_CRC_SKIPLEN(len1-1);
+	else
+		options = 0;
+
+	/* HEADER CRC */
+	if ( priv->drv_cap & DRV_CAP_HDR_CRC ) {
+		options |= PKT_OPTION_HDR_CRC;
+	} else {
+		/* Generate CRC and put it at the end of data buffer, Don't calculate
+		 * CRC on Destination Path address. 
+		 */
+		unsigned char crc;
+		crc = rmap_crc_calc(&txpkt->hdr[len1-1], txpkt->hlen - (len1-1));
+		((unsigned char *)txpkt->hdr)[txpkt->hlen] = crc;
+		txpkt->hlen++;
+	}
+	/* DATA CRC */
+	if ( txpkt->data ) {
+		if ( txpkt->dlen == 0 ) {
+			/* Need to put CRC even when no data available,
+			 * CRC will always be 0x00 
+			 */
+			((unsigned char *)txpkt->data)[0] = 0x00;
+			txpkt->dlen = 1;
+		} else if ( priv->drv_cap & DRV_CAP_DATA_CRC ) {
+			options |= PKT_OPTION_DATA_CRC;
+		} else {
+			/* Generate CRC and put it at the end of data buffer */
+			unsigned char crc;
+			crc = rmap_crc_calc(txpkt->data, txpkt->dlen);
+			((unsigned char *)txpkt->data)[txpkt->dlen] = crc;
+			txpkt->dlen++;
+		}
+	}
+
+	txpkt->options = options;
+
+	return 0;
+}
+
+int rmap_parse(struct rmap_common_priv *priv, struct rmap_command *cmd, struct rmap_spw_pkt *rxpkt)
+{
+	unsigned char *pkt;
+	unsigned int length;
+
+	pkt = rxpkt->data;
+
+	/* Check that the protocol ID, Reply and TID matches before proceeding */
+	if (	(pkt[1] != 0x01) || ((pkt[2] & 0x40) != 0) || 
+		(pkt[5] != (cmd->tid>>8)) || (pkt[6] != (cmd->tid & 0xff))) {
+		return -1;
+	}
+
+	cmd->status = pkt[3];
+	if ( cmd->type & RMAP_CMD_WRITE ){
+		return 0;
+	}
+
+	if ( cmd->type == RMAP_CMD_RMWI ) {
+		/* Copy data content */
+		cmd->data.read_m_write.oldlength = length = pkt[10];
+		if ( length == 4 ) {
+			cmd->data.read_m_write.olddata = ntohl(*(unsigned int *)&pkt[12]);
+		} else if ( length == 1 ){
+			cmd->data.read_m_write.olddata = pkt[12];
+		} else if ( length == 2 ) {
+			cmd->data.read_m_write.olddata = ntohs(*(unsigned short *)&pkt[12]);
+		} else {
+			cmd->data.read_m_write.olddata = 0;
+		}
+		return 0;
+	}
+
+	/* READ COMMAND RESPONSE - copy data */
+	if ( cmd->status != 0 ) {
+		/* Error status, skip other bytes */
+		return 0;
+	}
+
+	/* Length may be equal or less than request nembuer of bytes */
+	cmd->data.read.datalength = length = (pkt[8]<<16) | (pkt[9]<<8) | pkt[10];
+	if ( length > cmd->data.read.length ) {
+		printf("Response DATA Length != Command Data Length [%p, %p]\n",
+			cmd, pkt);
+		return -1;
+	}
+	if ( length > 0 ) {
+		if (cmd->data.read.data == NULL)
+			cmd->data.read.data = (unsigned int)&pkt[12];
+		else
+			memcpy(cmd->data.read.data, &pkt[12], length);
+	}
+
+	return 0;
+}