+++ /dev/null
-/*
- Messaging Over Raw Ethernet communication library
-
- Copyrigth Thales 20250319
-*/
-
-#include <assert.h>
-#include <errno.h>
-#include <fcntl.h>
-#if WIN32
-#include <winsock2.h>
-#include <ws2tcpip.h>
-#define OPT_TYPE char
-#define SETBLOCK(fd) do { u_long _m = 0; ioctlsocket (fd, FIONBIO, &_m); } while (0)
-#define SETNOBLK(fd) do { u_long _m = 1; ioctlsocket (fd, FIONBIO, &_m); } while (0)
-#else /* Linux */
-#include <netdb.h>
-#include <netinet/in.h>
-#include <netinet/tcp.h>
-#include <sys/socket.h>
-#define OPT_TYPE int
-#define SETBLOCK(fd) fcntl (fd, F_SETFL, 0)
-#define SETNOBLK(fd) fcntl (fd, F_SETFL, 0_NONBLOCK)
-#endif
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "debug.h"
-
-#include "more.h"
-
-/**
- MORE address structure
-*/
-typedef struct {
- enum { MORE_error, MORE_tx, MORE_rx } mode;
- char mac_address[17];
-} MORE_addr_t;
-
-#define MAX_SAP_NUMBER 256
-
-/**
- Type of private descriptor associated to each connected SAP.
-*/
-typedef struct {
- uint8_t mac[6];
- int sock;
- struct sockaddr_ll sock_addr;
- uint16_t short tx_seqnum;
- uint16_t short rx_seqnum;
- unsigned char tx_buffer[9014]; // jumbo frame (1514 standard frame)
- unsigned char rx_buffer[9014]; // jumbo frame (1514 standard frame)
- int is_blocking;
-} MORE_descriptor_t ;
-
-/**
- List of private descriptor associated to each connected MORE.
-*/
-MORE_descriptor_t *MORE_list[MAX_MORE_NUMBER] = { 0 };
-
-/**
- Init flag
-*/
-static int __init = 1;
-
-void free_all_mores (void)
-{
-
- VERBOSE (more, 2, FPRINTF (stdout, "free_all_mores\n"));
-
- int i;
- for (i = 0; i < MAX_SAP_NUMBER; i++) {
- free (MORE_list[i]);
- MORE_list[i] = NULL;
- }
-}
-
-static inline int parse_url (const char *ascii_url, SAP_addr_t *addr)
-{
- struct in_addr **addr_list;
- char url_to_parse[256] = { 0 };
-
- strncpy (url_to_parse, ascii_url, sizeof (url_to_parse) - 1);
- if (strncmp (url_to_parse, "udp://", 6) == 0)
- addr->type = SAP_udp;
- else if (strncmp (url_to_parse, "tcp://", 6) == 0)
- addr->type = SAP_tcp;
- else
- return -1;
-
- char *hostname = strtok (url_to_parse + 6, ":");
- char *port = strtok (NULL, ":");
- if ((hostname == NULL) || (port == NULL)) {
- errno = EINVAL;
- return -1;
- }
-
- addr->sockaddr.sin_family = AF_INET;
- addr->sockaddr.sin_port = htons (atoi (port));
-
- VERBOSE (sap, 1, PRINTF ("parse URL %s %s\n", hostname, port));
-
- if (hostname[0] != '*') {
- struct hostent *he = gethostbyname (hostname);
- if (he == NULL) {
- errno = EHOSTUNREACH;
- return -1;
- }
-
- addr_list = (struct in_addr **) he->h_addr_list;
- addr->sockaddr.sin_addr.s_addr = addr_list[0]->s_addr;
- } else {
- addr->sockaddr.sin_addr.s_addr = INADDR_ANY;
- }
-
- return 0;
-}
-
-static inline int sap_create_and_listen (char *local_address, int doListen,
- int port_offset)
-{
- SAP_addr_t addr;
- int sap;
-
- VERBOSE (sap, 1, PRINTF ("SAP local addr %s\n", local_address));
- int rc = parse_url (local_address, &addr);
-
- addr.sockaddr.sin_port =
- htons (ntohs (addr.sockaddr.sin_port) + port_offset);
- VERBOSE (sap, 1, PRINTF ("local port: %d\n", ntohs (addr.sockaddr.sin_port)));
-
- if (rc < 0) {
- errno = EINVAL;
- return rc;
- }
-
- switch (addr.type) {
- case SAP_tcp:
- sap = socket (PF_INET, SOCK_STREAM, 0);
- if (sap >= 0) {
- int val = 1;
- setsockopt (sap, SOL_SOCKET, SO_REUSEADDR, (OPT_TYPE *)&val, sizeof (val));
- rc = bind (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
- if (rc < 0)
- return rc;
- if (doListen) {
- rc = listen (sap, 5);
- if (rc < 0) return rc;
- }
- }
- break;
- case SAP_udp:
- sap = socket (PF_INET, SOCK_DGRAM, 0);
- if (sap >= 0) {
- int val = 1;
- setsockopt (sap, SOL_SOCKET, SO_REUSEADDR, (OPT_TYPE *)&val, sizeof (val));
- rc = bind (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
- if (rc < 0)
- return rc;
- }
- break;
- default:
- errno = EINVAL;
- return -1;
- break;
- }
-
- SETNOBLK (sap);
-
- return sap;
-}
-
-int SAP_Create (char *local_address)
-{
- return sap_create_and_listen (local_address, 1, 0);
-}
-
-int SAP_Connect (char *local_address, char * remote_address)
-{
- return SAP_Connect_Offset (local_address, remote_address, 0);
-}
-
-int SAP_Connect_Offset (char *local_address, char *remote_address, int port_offset)
-{
- SAP_addr_t addr;
- int sap;
- int rc = parse_url (remote_address, &addr);
-
- addr.sockaddr.sin_port =
- htons (ntohs (addr.sockaddr.sin_port) + port_offset);
- VERBOSE (sap, 1, PRINTF ("remote port: %d\n", ntohs (addr.sockaddr.sin_port)));
-
- if (rc < 0) {
- errno = EINVAL;
- return rc;
- }
-
- if (local_address) {
- sap = sap_create_and_listen (local_address, 0, port_offset);
- } else {
- switch (addr.type) {
- case SAP_tcp:
- sap = socket (PF_INET, SOCK_STREAM, 0);
- break;
- case SAP_udp:
- sap = socket (PF_INET, SOCK_DGRAM, 0);
- break;
- default:
- errno = EINVAL;
- return -1;
- }
- }
-
- if (sap < 0)
- return sap;
- if (sap >= MAX_SAP_NUMBER) {
- errno = ENOMEM;
- return -1;
- }
-
- SETNOBLK (sap);
- rc = connect (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
-
- if (addr.type == SAP_tcp) {
- int val = 1;
- rc = setsockopt (sap, IPPROTO_TCP, TCP_NODELAY, (OPT_TYPE *)&val, sizeof (val));
- if (rc < 0)
- perror ("setsockopt/TCP_NODELAY");
- }
-
- assert (SAP_list[sap] == NULL);
- SAP_list[sap] = (SAP_descriptor_t *) calloc (1, sizeof (SAP_descriptor_t));
- assert (SAP_list[sap]);
- if (__sync_bool_compare_and_swap (&__init, 1, 0))
- atexit (free_saps);
-
- return sap;
-}
-
-int SAP_IsConnected (int sap)
-{
- int sockerr;
- socklen_t rlen = sizeof (sockerr);
-
- int rc = getsockopt (sap, SOL_SOCKET, SO_ERROR, (OPT_TYPE *)&sockerr, &rlen);
- if (rc < 0)
- return -1;
-
- switch (sockerr) {
- case 0:
- return 1;
- break;
- case EINPROGRESS:
- return 0;
- break;
- }
- return -1;
-}
-
-int SAP_Accept (int sap)
-{
-
- int newsap = accept (sap, NULL, 0);
- if (newsap < 0)
- return newsap;
- if (newsap >= MAX_SAP_NUMBER) {
- errno = ENOMEM;
- return -1;
- }
- assert (SAP_list[newsap] == NULL);
-
- SAP_list[newsap] = (SAP_descriptor_t *)calloc (1, sizeof (SAP_descriptor_t));
- assert (SAP_list[newsap]);
- if (__sync_bool_compare_and_swap (&__init, 1, 0))
- atexit (free_saps);
-
- SETNOBLK (newsap);
-
- int val = 1;
- int rc = setsockopt (newsap, IPPROTO_TCP, TCP_NODELAY, (OPT_TYPE *)&val, sizeof (val));
- if (rc < 0)
- perror ("setsockopt/TCP_NODELAY");
-
- return newsap;
-}
-
-void SAP_Reject (int sap)
-{
- int newsap = accept (sap, NULL, 0);
- close (newsap);
-}
-
-void SAP_Close (int sap)
-{
- if ((sap < 0) || (sap >= MAX_SAP_NUMBER))
- return;
- close (sap);
- if (SAP_list[sap]) {
- free (SAP_list[sap]);
- SAP_list[sap] = NULL;
- }
-}
-
-void SAP_Set_Blocking (int sap)
-{
- SETBLOCK (sap);
- if (SAP_list[sap])
- SAP_list[sap]->is_blocking = 0;
-}
-
-int SAP_Send (int sap, unsigned long msgtype, unsigned char *data, int len)
-{
-
- /* Message format is
- * bytes 0-1 : len
- * bytes 2-3 : seqnumber
- * bytes 4-7 : msgtype
- * bytes 8- : data
- */
-
- assert (((data != NULL) && (len > 0)) || ((data == NULL) && (len == 0)));
-
- int msglen = len + 8;
-
- if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
- errno = EBADF;
- return -1;
- }
-
- unsigned char *buffer = calloc (1, msglen);
- if (buffer == NULL) {
- errno = ENOBUFS;
- return -1;
- }
-
- *(unsigned short *)buffer = htons (len);
- *(unsigned short *)(buffer + 2) = SAP_list[sap]->tx_seqnum++;
- *(unsigned long *)(buffer + 4) = htonl (msgtype);
- if (len > 0)
- memcpy (buffer + 8, data, len);
-
- int rc = write (sap, buffer, msglen);
- free (buffer);
-
- return (rc >= 0) ? rc - 8 : rc;
-}
-
-int SAP_Send_Raw (int sap, unsigned long msgtype, unsigned char *data, int len)
-{
-
- /* Message format is
- * bytes 0-1 : len
- * bytes 2-3 : seqnumber
- * bytes 4-7 : msgtype
- * bytes 8- : data
- */
-
- int msglen = len + 8;
-
- if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
- errno = EBADF;
- return -1;
- }
-
- unsigned char *buffer = calloc (1, msglen);
- if (buffer == NULL) {
- errno = ENOBUFS;
- return -1;
- }
-
- *(unsigned short *)buffer = htons (len);
- *(unsigned short *)(buffer + 2) = SAP_list[sap]->tx_seqnum++;
- *(unsigned long *)(buffer + 4) = htonl (msgtype);
- memcpy (buffer + 8, data, len);
-
- int rc = write (sap, buffer, msglen);
- free (buffer);
-
- return (rc >= 0) ? rc - 8 : rc;
-}
-
-int SAP_Receive_timeout (int sap, unsigned long *msgtype, unsigned char *buffer,
- int buflen, int microseconds)
-{
-
- fd_set rfds;
- struct timeval tv = { 0, microseconds };
- FD_ZERO (&rfds);
- FD_SET (sap, &rfds);
-
- int retval = select (sap + 1, &rfds, NULL, NULL, &tv);
- if (retval != 1)
- return retval;
-
- return SAP_Receive (sap, msgtype, buffer, buflen);
-}
-
-int SAP_Receive (int sap, unsigned long *msgtype, unsigned char *buffer, int buflen)
-{
- int len;
- int closed = 0;
-
- if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
- errno = EBADF;
- return -1;
- }
-
- /* Read has much has possible in our internal buffer */
- /* put received data after remaining data from previous read */
-
- uint8_t *value = SAP_list[sap]->buffer;
- if ((SAP_list[sap]->remain <= 8) ||
- (ntohs (*(uint16_t *)value) > SAP_list[sap]->remain + 8)) {
-
- int rc = read (sap, SAP_list[sap]->buffer + SAP_list[sap]->remain,
- sizeof (SAP_list[sap]->buffer) - SAP_list[sap]->remain);
-
- if (rc == 0)
- closed = -1;
-
- if ((rc < 0) && (errno != EAGAIN))
- return -1;
-
- if (rc > 0)
- SAP_list[sap]->remain += rc;
- }
-
- /* Message header not fully received */
- if (SAP_list[sap]->remain <= 8)
- return closed;
-
- /* Payload of message not fully received */
- len = ntohs (*(unsigned short *)value);
- if (len > SAP_list[sap]->remain + 8)
- return closed;
-
- if (buflen < len) {
- VERBOSE (sap, 0, FPRINTF (stderr, "buflen %d < len %d (0x%X)\n", buflen, len, len));
- return -ENOBUFS;
- }
-
- unsigned short seqnumber =
- ntohs (*(unsigned short *)(SAP_list[sap]->buffer + 2));
- if ((SAP_list[sap]->is_blocking) &&
- (seqnumber != ((SAP_list[sap]->rx_seqnum + 1) & 0xffff)))
- VERBOSE (sap, 0, FPRINTF (stderr, "missing sequence number\n"));
- SAP_list[sap]->rx_seqnum = seqnumber;
-
- *msgtype = ntohl (*(unsigned long *)(SAP_list[sap]->buffer + 4));
- memcpy (buffer, SAP_list[sap]->buffer + 8, len);
-
- /* Move remaining data to begin of private buffer */
- SAP_list[sap]->remain -= len + 8;
- memmove (SAP_list[sap]->buffer, SAP_list[sap]->buffer + 8 + len, SAP_list[sap]->remain);
-
- return len;
-}
-
-/* vi:set tabstop=4 expandtab shiftwidth=4: this line set vi mode*/
--- /dev/null
+/*
+ Messaging Over Raw Ethernet Packets communication library
+
+ Copyrigth Thales 20250319
+*/
+
+#include <assert.h>
+#include <errno.h>
+#include <fcntl.h>
+#if WIN32
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#define OPT_TYPE char
+#define SETBLOCK(fd) do { u_long _m = 0; ioctlsocket (fd, FIONBIO, &_m); } while (0)
+#define SETNOBLK(fd) do { u_long _m = 1; ioctlsocket (fd, FIONBIO, &_m); } while (0)
+#else /* Linux */
+#include <netdb.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <sys/socket.h>
+#define OPT_TYPE int
+#define SETBLOCK(fd) fcntl (fd, F_SETFL, 0)
+#define SETNOBLK(fd) fcntl (fd, F_SETFL, 0_NONBLOCK)
+#endif
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "debug.h"
+
+#include "morep.h"
+
+/**
+ MOREP address structure
+*/
+typedef struct {
+ enum { MOREP_error, MOREP_tx, MOREP_rx } mode;
+ char mac_address[17];
+ uint8_t mac[6];
+} MOREP_addr_t;
+
+#define MAX_MOREP_NUMBER 256
+
+/**
+ Type of private descriptor associated to each connected SAP.
+*/
+typedef struct {
+ int sock;
+ struct sockaddr_ll sock_addr;
+ uint16_t short tx_seqnum;
+ uint16_t short rx_seqnum;
+ unsigned char tx_buffer[9014]; // jumbo frame (1514 standard frame)
+ unsigned char rx_buffer[9014]; // jumbo frame (1514 standard frame)
+ int is_blocking;
+} MOREP_descriptor_t ;
+
+/**
+ List of private descriptor associated to each connected MORE.
+*/
+MOREP_descriptor_t *MOREP_list[MAX_MOREP_NUMBER] = { 0 };
+
+/**
+ Init flag
+*/
+static int __init = 1;
+
+void free_all_moreps (void)
+{
+
+ VERBOSE (morep, 2, FPRINTF (stdout, "free_all_moreps\n"));
+
+ int i;
+ for (i = 0; i < MAX_MOREP_NUMBER; i++) {
+ free (MOREP_list[i]);
+ MOREP_list[i] = NULL;
+ }
+}
+
+static int _parse_mac (const char *hexa_str, MOREP_addr_t *addr)
+{
+ /* MAC address format: XX:XX:XX:XX:XX:XX */
+ if (strlen (hexa_str) != 17) {
+ return -1;
+ }
+
+ int i;
+ for (int i = 0; i < 6; i++) {
+ char one_byte[3] = "xx";
+ char ptr = NULL;
+ memcpy (one_byte, hexa + 3 * i, 2);
+ mac[i] = strtoul(one_byte, NULL, 16, &ptr);
+ if (ptr != NULL) {
+ return -1;
+ }
+ if (i < 5) {
+ if (hexa[3 * i + 2] != ':') {
+ return -1;
+ }
+ }
+ }
+ return 0;
+}
+ struct in_addr **addr_list;
+ char url_to_parse[256] = { 0 };
+
+ strncpy (url_to_parse, ascii_url, sizeof (url_to_parse) - 1);
+ if (strncmp (url_to_parse, "udp://", 6) == 0)
+ addr->type = SAP_udp;
+ else if (strncmp (url_to_parse, "tcp://", 6) == 0)
+ addr->type = SAP_tcp;
+ else
+ return -1;
+
+ char *hostname = strtok (url_to_parse + 6, ":");
+ char *port = strtok (NULL, ":");
+ if ((hostname == NULL) || (port == NULL)) {
+ errno = EINVAL;
+ return -1;
+ }
+
+ addr->sockaddr.sin_family = AF_INET;
+ addr->sockaddr.sin_port = htons (atoi (port));
+
+ VERBOSE (sap, 1, PRINTF ("parse URL %s %s\n", hostname, port));
+
+ if (hostname[0] != '*') {
+ struct hostent *he = gethostbyname (hostname);
+ if (he == NULL) {
+ errno = EHOSTUNREACH;
+ return -1;
+ }
+
+ addr_list = (struct in_addr **) he->h_addr_list;
+ addr->sockaddr.sin_addr.s_addr = addr_list[0]->s_addr;
+ } else {
+ addr->sockaddr.sin_addr.s_addr = INADDR_ANY;
+ }
+
+ return 0;
+}
+
+static inline int sap_create_and_listen (char *local_address, int doListen,
+ int port_offset)
+{
+ SAP_addr_t addr;
+ int sap;
+
+ VERBOSE (sap, 1, PRINTF ("SAP local addr %s\n", local_address));
+ int rc = parse_url (local_address, &addr);
+
+ addr.sockaddr.sin_port =
+ htons (ntohs (addr.sockaddr.sin_port) + port_offset);
+ VERBOSE (sap, 1, PRINTF ("local port: %d\n", ntohs (addr.sockaddr.sin_port)));
+
+ if (rc < 0) {
+ errno = EINVAL;
+ return rc;
+ }
+
+ switch (addr.type) {
+ case SAP_tcp:
+ sap = socket (PF_INET, SOCK_STREAM, 0);
+ if (sap >= 0) {
+ int val = 1;
+ setsockopt (sap, SOL_SOCKET, SO_REUSEADDR, (OPT_TYPE *)&val, sizeof (val));
+ rc = bind (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
+ if (rc < 0)
+ return rc;
+ if (doListen) {
+ rc = listen (sap, 5);
+ if (rc < 0) return rc;
+ }
+ }
+ break;
+ case SAP_udp:
+ sap = socket (PF_INET, SOCK_DGRAM, 0);
+ if (sap >= 0) {
+ int val = 1;
+ setsockopt (sap, SOL_SOCKET, SO_REUSEADDR, (OPT_TYPE *)&val, sizeof (val));
+ rc = bind (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
+ if (rc < 0)
+ return rc;
+ }
+ break;
+ default:
+ errno = EINVAL;
+ return -1;
+ break;
+ }
+
+ SETNOBLK (sap);
+
+ return sap;
+}
+
+int SAP_Create (char *local_address)
+{
+ return sap_create_and_listen (local_address, 1, 0);
+}
+
+int SAP_Connect (char *local_address, char * remote_address)
+{
+ return SAP_Connect_Offset (local_address, remote_address, 0);
+}
+
+int SAP_Connect_Offset (char *local_address, char *remote_address, int port_offset)
+{
+ SAP_addr_t addr;
+ int sap;
+ int rc = parse_url (remote_address, &addr);
+
+ addr.sockaddr.sin_port =
+ htons (ntohs (addr.sockaddr.sin_port) + port_offset);
+ VERBOSE (sap, 1, PRINTF ("remote port: %d\n", ntohs (addr.sockaddr.sin_port)));
+
+ if (rc < 0) {
+ errno = EINVAL;
+ return rc;
+ }
+
+ if (local_address) {
+ sap = sap_create_and_listen (local_address, 0, port_offset);
+ } else {
+ switch (addr.type) {
+ case SAP_tcp:
+ sap = socket (PF_INET, SOCK_STREAM, 0);
+ break;
+ case SAP_udp:
+ sap = socket (PF_INET, SOCK_DGRAM, 0);
+ break;
+ default:
+ errno = EINVAL;
+ return -1;
+ }
+ }
+
+ if (sap < 0)
+ return sap;
+ if (sap >= MAX_SAP_NUMBER) {
+ errno = ENOMEM;
+ return -1;
+ }
+
+ SETNOBLK (sap);
+ rc = connect (sap, (struct sockaddr *)&addr.sockaddr, sizeof (addr.sockaddr));
+
+ if (addr.type == SAP_tcp) {
+ int val = 1;
+ rc = setsockopt (sap, IPPROTO_TCP, TCP_NODELAY, (OPT_TYPE *)&val, sizeof (val));
+ if (rc < 0)
+ perror ("setsockopt/TCP_NODELAY");
+ }
+
+ assert (SAP_list[sap] == NULL);
+ SAP_list[sap] = (SAP_descriptor_t *) calloc (1, sizeof (SAP_descriptor_t));
+ assert (SAP_list[sap]);
+ if (__sync_bool_compare_and_swap (&__init, 1, 0))
+ atexit (free_saps);
+
+ return sap;
+}
+
+int SAP_IsConnected (int sap)
+{
+ int sockerr;
+ socklen_t rlen = sizeof (sockerr);
+
+ int rc = getsockopt (sap, SOL_SOCKET, SO_ERROR, (OPT_TYPE *)&sockerr, &rlen);
+ if (rc < 0)
+ return -1;
+
+ switch (sockerr) {
+ case 0:
+ return 1;
+ break;
+ case EINPROGRESS:
+ return 0;
+ break;
+ }
+ return -1;
+}
+
+int SAP_Accept (int sap)
+{
+
+ int newsap = accept (sap, NULL, 0);
+ if (newsap < 0)
+ return newsap;
+ if (newsap >= MAX_SAP_NUMBER) {
+ errno = ENOMEM;
+ return -1;
+ }
+ assert (SAP_list[newsap] == NULL);
+
+ SAP_list[newsap] = (SAP_descriptor_t *)calloc (1, sizeof (SAP_descriptor_t));
+ assert (SAP_list[newsap]);
+ if (__sync_bool_compare_and_swap (&__init, 1, 0))
+ atexit (free_saps);
+
+ SETNOBLK (newsap);
+
+ int val = 1;
+ int rc = setsockopt (newsap, IPPROTO_TCP, TCP_NODELAY, (OPT_TYPE *)&val, sizeof (val));
+ if (rc < 0)
+ perror ("setsockopt/TCP_NODELAY");
+
+ return newsap;
+}
+
+void SAP_Reject (int sap)
+{
+ int newsap = accept (sap, NULL, 0);
+ close (newsap);
+}
+
+void SAP_Close (int sap)
+{
+ if ((sap < 0) || (sap >= MAX_SAP_NUMBER))
+ return;
+ close (sap);
+ if (SAP_list[sap]) {
+ free (SAP_list[sap]);
+ SAP_list[sap] = NULL;
+ }
+}
+
+void SAP_Set_Blocking (int sap)
+{
+ SETBLOCK (sap);
+ if (SAP_list[sap])
+ SAP_list[sap]->is_blocking = 0;
+}
+
+int SAP_Send (int sap, unsigned long msgtype, unsigned char *data, int len)
+{
+
+ /* Message format is
+ * bytes 0-1 : len
+ * bytes 2-3 : seqnumber
+ * bytes 4-7 : msgtype
+ * bytes 8- : data
+ */
+
+ assert (((data != NULL) && (len > 0)) || ((data == NULL) && (len == 0)));
+
+ int msglen = len + 8;
+
+ if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
+ errno = EBADF;
+ return -1;
+ }
+
+ unsigned char *buffer = calloc (1, msglen);
+ if (buffer == NULL) {
+ errno = ENOBUFS;
+ return -1;
+ }
+
+ *(unsigned short *)buffer = htons (len);
+ *(unsigned short *)(buffer + 2) = SAP_list[sap]->tx_seqnum++;
+ *(unsigned long *)(buffer + 4) = htonl (msgtype);
+ if (len > 0)
+ memcpy (buffer + 8, data, len);
+
+ int rc = write (sap, buffer, msglen);
+ free (buffer);
+
+ return (rc >= 0) ? rc - 8 : rc;
+}
+
+int SAP_Send_Raw (int sap, unsigned long msgtype, unsigned char *data, int len)
+{
+
+ /* Message format is
+ * bytes 0-1 : len
+ * bytes 2-3 : seqnumber
+ * bytes 4-7 : msgtype
+ * bytes 8- : data
+ */
+
+ int msglen = len + 8;
+
+ if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
+ errno = EBADF;
+ return -1;
+ }
+
+ unsigned char *buffer = calloc (1, msglen);
+ if (buffer == NULL) {
+ errno = ENOBUFS;
+ return -1;
+ }
+
+ *(unsigned short *)buffer = htons (len);
+ *(unsigned short *)(buffer + 2) = SAP_list[sap]->tx_seqnum++;
+ *(unsigned long *)(buffer + 4) = htonl (msgtype);
+ memcpy (buffer + 8, data, len);
+
+ int rc = write (sap, buffer, msglen);
+ free (buffer);
+
+ return (rc >= 0) ? rc - 8 : rc;
+}
+
+int SAP_Receive_timeout (int sap, unsigned long *msgtype, unsigned char *buffer,
+ int buflen, int microseconds)
+{
+
+ fd_set rfds;
+ struct timeval tv = { 0, microseconds };
+ FD_ZERO (&rfds);
+ FD_SET (sap, &rfds);
+
+ int retval = select (sap + 1, &rfds, NULL, NULL, &tv);
+ if (retval != 1)
+ return retval;
+
+ return SAP_Receive (sap, msgtype, buffer, buflen);
+}
+
+int SAP_Receive (int sap, unsigned long *msgtype, unsigned char *buffer, int buflen)
+{
+ int len;
+ int closed = 0;
+
+ if ((sap < 0) || (sap >= MAX_SAP_NUMBER) || (SAP_list[sap] == NULL)) {
+ errno = EBADF;
+ return -1;
+ }
+
+ /* Read has much has possible in our internal buffer */
+ /* put received data after remaining data from previous read */
+
+ uint8_t *value = SAP_list[sap]->buffer;
+ if ((SAP_list[sap]->remain <= 8) ||
+ (ntohs (*(uint16_t *)value) > SAP_list[sap]->remain + 8)) {
+
+ int rc = read (sap, SAP_list[sap]->buffer + SAP_list[sap]->remain,
+ sizeof (SAP_list[sap]->buffer) - SAP_list[sap]->remain);
+
+ if (rc == 0)
+ closed = -1;
+
+ if ((rc < 0) && (errno != EAGAIN))
+ return -1;
+
+ if (rc > 0)
+ SAP_list[sap]->remain += rc;
+ }
+
+ /* Message header not fully received */
+ if (SAP_list[sap]->remain <= 8)
+ return closed;
+
+ /* Payload of message not fully received */
+ len = ntohs (*(unsigned short *)value);
+ if (len > SAP_list[sap]->remain + 8)
+ return closed;
+
+ if (buflen < len) {
+ VERBOSE (sap, 0, FPRINTF (stderr, "buflen %d < len %d (0x%X)\n", buflen, len, len));
+ return -ENOBUFS;
+ }
+
+ unsigned short seqnumber =
+ ntohs (*(unsigned short *)(SAP_list[sap]->buffer + 2));
+ if ((SAP_list[sap]->is_blocking) &&
+ (seqnumber != ((SAP_list[sap]->rx_seqnum + 1) & 0xffff)))
+ VERBOSE (sap, 0, FPRINTF (stderr, "missing sequence number\n"));
+ SAP_list[sap]->rx_seqnum = seqnumber;
+
+ *msgtype = ntohl (*(unsigned long *)(SAP_list[sap]->buffer + 4));
+ memcpy (buffer, SAP_list[sap]->buffer + 8, len);
+
+ /* Move remaining data to begin of private buffer */
+ SAP_list[sap]->remain -= len + 8;
+ memmove (SAP_list[sap]->buffer, SAP_list[sap]->buffer + 8 + len, SAP_list[sap]->remain);
+
+ return len;
+}
+
+/* vi:set tabstop=4 expandtab shiftwidth=4: this line set vi mode*/
Soft'N'Design Software / projects / morep.git / commitdiff
