diff options
author | Daniel Hellstrom <daniel@gaisler.com> | 2014-01-23 09:43:53 +0100 |
---|---|---|
committer | Daniel Hellstrom <daniel@gaisler.com> | 2014-03-07 12:23:21 +0100 |
commit | 86122cf4a7f8716a7781273041fea0524b73678e (patch) | |
tree | 5856aa0ada918c1d42f5a6bc133f667c14ca908a | |
parent | d4a919c3a795db922dc30dd7f49772d15b87e15c (diff) |
RMAP: added new asynchonous interface
-rw-r--r-- | c/src/lib/libbsp/sparc/Makefile.am | 2 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/leon2/Makefile.am | 2 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/leon3/Makefile.am | 2 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/shared/include/rmap.h | 46 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/shared/spw/rmap.c | 274 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/shared/spw/rmap_async.c | 225 | ||||
-rw-r--r-- | c/src/lib/libbsp/sparc/shared/spw/rmap_common.c | 279 |
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; +} |