bool EmcAdapter::connect()
{
int retval = 0;
if (mEmcStatusBuffer == 0)
{
mEmcStatusBuffer = new RCS_STAT_CHANNEL(emcFormat, (char*) "emcStatus", (char*) "adapter", mNmlFile);
if (! mEmcStatusBuffer->valid() || EMC_STAT_TYPE != mEmcStatusBuffer->peek())
{
rcs_print_error("emcStatus buffer not available\n");
delete mEmcStatusBuffer;
mEmcStatusBuffer = 0;
mPower.setValue(PowerState::eOFF);
return false;
}
}
if (mEmcErrorBuffer == 0)
{
mEmcErrorBuffer = new NML(nmlErrorFormat, (char*) "emcError", (char*) "adapter", mNmlFile);
if (!mEmcErrorBuffer->valid())
{
rcs_print_error("emcError buffer not available\n");
delete mEmcErrorBuffer;
mEmcErrorBuffer = 0;
mPower.setValue(PowerState::eOFF);
return false;
}
}
mConnected = true;
return true;
}
/* release semaphore when done */
int
release_access (short int *sem, double timeout)
{
int tst;
ULONG timeout_ticks, start_ticks, current_ticks, elapsed_ticks;
timeout_ticks = (ULONG) (timeout * sysClkRateGet ());
start_ticks = tickGet ();
elapsed_ticks = 0;
while (elapsed_ticks <= timeout_ticks)
{
tst = decrement_read_status (sem);
if (tst == 0)
{
taskUnlock ();
return 0; /* success */
}
else
{
if (tst < 0)
{
taskUnlock ();
rcs_print_error ("invalid semaphore on 0x%x", (int) sem);
return -1; /* invalid semaphore */
}
}
current_ticks = tickGet ();
elapsed_ticks = current_ticks - start_ticks;
}
taskUnlock ();
rcs_print_error ("timed out while releasing access on semphore 0x%x",
(int) sem);
return -1; /* timeout failure */
}
int NML_INTERP_LIST::append(NMLmsg *nml_msg_ptr)
{
/* check for invalid data */
if (NULL == nml_msg_ptr)
{
rcs_print_error("NML_INTERP_LIST::append : attempt to append NULL msg\n");
return -1;
}
if (0 == nml_msg_ptr->type)
{
rcs_print_error("NML_INTERP_LIST::append : attempt to append 0 type\n");
return -1;
}
if(nml_msg_ptr->size > MAX_NML_COMMAND_SIZE -64 )
{
rcs_print_error("NML_INTERP_LIST::append : command size is too large.");
return -1;
}
if(nml_msg_ptr->size < 4 )
{
rcs_print_error("NML_INTERP_LIST::append : command size is invalid.");
return -1;
}
#ifdef DEBUG_INTERPL
if(sizeof(temp_node) < MAX_NML_COMMAND_SIZE+4 ||
sizeof(temp_node) > MAX_NML_COMMAND_SIZE+16 ||
((void *) &temp_node.line_number) > ((void *) &temp_node.command.commandbuf))
{
rcs_print_error("NML_INTERP_LIST::append : assumptions about NML_INTERP_LIST_NODE have been violated.");
return -1;
}
#endif
if (NULL == linked_list_ptr)
{
return -1;
}
// fill in the NML_INTERP_LIST_NODE
temp_node.line_number = next_line_number;
temp_node.file_line=next_file_line;
memcpy(temp_node.command.commandbuf, nml_msg_ptr, nml_msg_ptr->size);
// stick it on the list
linked_list_ptr->store_at_tail(&temp_node, nml_msg_ptr->size+sizeof(temp_node.line_number)+sizeof(temp_node.file_line)+sizeof(temp_node.dummy)+32+(32-nml_msg_ptr->size%32), 1);
#ifdef DEBUG_INTERPL
if(EMC_DEBUG & EMC_DEBUG_INTERP_LIST)
{
rcs_print("NML_INTERP_LIST::append(nml_msg{size=%d,type=%d}) : list_size=%d, line_number = %d\n",
nml_msg_ptr->size,nml_msg_ptr->type,linked_list_ptr->list_size,
temp_node.line_number);
}
#endif
return 0;
}
/* remove semaphore from OS-- this must be done *before* sem_close,
since rcs_sem_close frees the storage allocated for the rcs_sem_t */
int rcs_sem_destroy(rcs_sem_t * sem)
{
/* remove OS semaphore */
if (semctl(*sem, 0, IPC_RMID, 0) == -1) {
rcs_print_error("semctl(%d,0,%d) failed: (errno = %d) %s\n",
*sem, IPC_RMID, errno, strerror(errno));
return -1;
}
return 0;
}
/* number of seconds from some epoch, to clock tick resolution */
double etime()
{
struct timeval tp;
double retval;
if (0 != gettimeofday(&tp, NULL)) {
rcs_print_error("etime: can't get time\n");
return 0.0;
}
retval = ((double) tp.tv_sec) + ((double) tp.tv_usec) / 1000000.0;
return retval;
}
/* acquire access to a semaphored data buffer */
int
acquire_access (short int *sem, double timeout)
{
ULONG timeout_ticks=0;
ULONG start_ticks =0;
ULONG current_ticks=0;
ULONG elapsed_ticks=0;
if (timeout == 0.0)
{
timeout_ticks = 1;
start_ticks = tickGet ();
elapsed_ticks = 0;
}
else if (timeout > 0.0)
{
timeout_ticks = (int) (timeout * sysClkRateGet ());
start_ticks = tickGet ();
elapsed_ticks = 0;
}
/* loop until timeout_count expires */
while (elapsed_ticks <= timeout_ticks && (timeout >= 0.0))
{
taskLock ();
if (!increment_read_status (sem))
{
return 0; /* success */
}
else
{
taskUnlock ();
taskDelay (1);
if (timeout >= 0.0)
{
current_ticks = tickGet ();
elapsed_ticks = current_ticks - start_ticks;
}
}
}
taskUnlock ();
rcs_print_error ("timed out while acquiring access on semaphore 0x%x\n",
(int) sem);
return -1; /* indicate timeout failure */
}
/* Write "n" bytes to a descriptor. */
int sendn(int fd, const void *vptr, int n, int _flags, double _timeout)
{
int nleft;
long nwritten;
int select_ret;
double start_time, current_time, timeleft;
char *ptr;
struct timeval timeout_tv;
struct timeval timeout_tv_copy;
fd_set send_fd_set;
timeout_tv.tv_sec = (long) _timeout;
timeout_tv.tv_usec = (long) (_timeout * 1000000.0);
if (timeout_tv.tv_usec >= 1000000) {
timeout_tv.tv_usec = timeout_tv.tv_usec % 1000000;
}
timeout_tv_copy = timeout_tv;
FD_ZERO(&send_fd_set);
FD_SET(fd, &send_fd_set);
ptr = (char *) vptr; /* can't do pointer arithmetic on void* */
nleft = n;
current_time = start_time = etime();
timeleft = _timeout;
while (nleft > 0) {
if (fabs(_timeout) > 1E-6) {
if (_timeout > 0) {
current_time = etime();
timeleft = start_time + _timeout - current_time;
if (timeleft <= 0.0) {
if (print_sendn_timeout_errors) {
rcs_print_error
("sendn(fd=%d, vptr=%p, int n=%d, int flags=%d, double _timeout=%f) timed out.\n",
fd, vptr, n, _flags, _timeout);
}
sendn_timedout = 1;
return -1;
}
timeout_tv.tv_sec = (long) timeleft;
timeout_tv.tv_usec = (long) (timeleft * 1000000.0);
if (timeout_tv.tv_usec >= 1000000) {
timeout_tv.tv_usec = timeout_tv.tv_usec % 1000000;
}
select_ret =
select(fd + 1, (fd_set *) NULL, &send_fd_set,
(fd_set *) NULL, &timeout_tv);
} else {
select_ret =
select(fd + 1, (fd_set *) NULL, &send_fd_set,
(fd_set *) NULL, NULL);
}
switch (select_ret) {
case -1:
rcs_print_error("Error in select: %d -> %s\n", errno,
strerror(errno));
rcs_print_error
("sendn(fd=%d, vptr=%p, int n=%d, int _flags=%d, double _timeout=%f) failed.\n",
fd, vptr, n, _flags, _timeout);
return -1;
case 0:
rcs_print_error
("sendn(fd=%d, vptr=%p, int n=%d, int _flags=%d, double _timeout=%f) timed out.\n",
fd, vptr, n, _flags, _timeout);
return -1;
default:
break;
}
}
if ((nwritten = send(fd, ptr, nleft, _flags)) == -1) {
rcs_print_error("Send error: %d = %s\n", errno, strerror(errno));
return (-1); /* error */
}
nleft -= nwritten;
ptr += nwritten;
if (nleft > 0 && _timeout > 0.0) {
current_time = etime();
if (current_time - start_time > _timeout) {
rcs_print_error("sendn: timed out after %f seconds.\n",
current_time - start_time);
return (-1);
}
esleep(0.001);
}
}
rcs_print_debug(PRINT_SOCKET_WRITE_SIZE, "wrote %d bytes to %d\n", n, fd);
return (n);
}
int
sendmsgt (int _socket_fd, struct msghdr *_msg_header, int _flags,
double _timeout,
__unused_parameter__ unsigned char *collection_buffer,
__unused_parameter__ long collection_buffer_size
)
{
struct timeval timeout_timeval;
fd_set write_fd_set;
int bytes_sent;
int sockerrno;
const char *sockerrstr;
char sockerrbuf[256];
#ifdef USE_SENDTO
int i=0;
long required_size = 0;
unsigned char *temp_pointer=0;
if(!collection_buffer ||
(collection_buffer_size) <= 0)
{
rcs_print_error("call to sendmst(%d,%p,%d(0x%X),%f,%p,%ld) with bad collecton_buffer.\n",
_socket_fd,_msg_header,_flags,_flags,
_timeout,collection_buffer,collection_buffer_size);
return -1;
}
#endif
sendmsgt_timed_out = 0;
if (_timeout > 1E-6)
{
timeout_timeval.tv_sec = (long) _timeout;
timeout_timeval.tv_usec = (long) (_timeout * 1000000.0);
if (timeout_timeval.tv_usec >= 1000000)
{
timeout_timeval.tv_usec = timeout_timeval.tv_usec % 1000000;
}
FD_ZERO (&write_fd_set);
RCS_FD_SET (_socket_fd, &write_fd_set);
switch (dl_select
(_socket_fd + 1, NULL, &write_fd_set, NULL, &timeout_timeval))
{
case -1:
sockerrno = dl_get_last_socket_error_int( _socket_fd );
sockerrstr = dl_get_last_socket_error_string(_socket_fd,sockerrno,sockerrbuf,sizeof(sockerrbuf));
rcs_print_error ("sendmsgt: select error: %d %s\n",
sockerrno, sockerrstr);
return -1;
case 0:
sendmsgt_timed_out = 1;
rcs_print_error ("sendmgt: select timed out.\n");
return (0);
default:
break;
}
}
else if (_timeout > -1E-6)
{
timeout_timeval.tv_sec = 0L;
timeout_timeval.tv_usec = 0L;
FD_ZERO (&write_fd_set);
RCS_FD_SET (_socket_fd, &write_fd_set);
switch (dl_select
(_socket_fd + 1, NULL, &write_fd_set, NULL, &timeout_timeval))
{
case -1:
rcs_print_error ("sendmsgt: select error: %d %s\n",
errno, strerror (errno));
return -1;
case 0:
sendmsgt_timed_out = 1;
return (0);
default:
break;
}
}
#ifdef USE_SENDTO
if (_msg_header->msg_iovlen > 1)
{
for (i = 0, required_size = 0; i < (int)_msg_header->msg_iovlen; i++)
{
required_size += _msg_header->msg_iov[i].iov_len;
}
if (required_size > (collection_buffer_size))
{
rcs_print_error("sendmsgt collection_buffer_size(%ld) less than required size(%ld)\n",
(collection_buffer_size),
required_size);
return -1;
}
for (i = 0, temp_pointer = collection_buffer;
i < (int) _msg_header->msg_iovlen; i++)
{
memcpy (temp_pointer, _msg_header->msg_iov[i].iov_base,
_msg_header->msg_iov[i].iov_len);
temp_pointer += _msg_header->msg_iov[i].iov_len;
}
bytes_sent =
dl_sendto (_socket_fd,
((char *) collection_buffer),
required_size, _flags,
(struct sockaddr *) _msg_header->msg_name,
_msg_header->msg_namelen);
}
else
{
bytes_sent = dl_sendto (_socket_fd, _msg_header->msg_iov[0].iov_base,
_msg_header->msg_iov[0].iov_len, _flags,
(struct sockaddr *) _msg_header->msg_name,
_msg_header->msg_namelen);
}
if (bytes_sent < 0)
{
sockerrno = dl_get_last_socket_error_int( _socket_fd );
sockerrstr =
dl_get_last_socket_error_string(_socket_fd,
sockerrno,
sockerrbuf,
sizeof(sockerrbuf));
rcs_print_error ("sendto(%d,%p,%ld,0x%X,%s,%d) failed: %d -- %s \n",
_socket_fd, _msg_header->msg_iov[0].iov_base,
(long) _msg_header->msg_iov[0].iov_len, _flags,
dl_inet_ptr_ntoa (&((struct sockaddr_in *)
_msg_header->msg_name)->sin_addr),
_msg_header->msg_namelen, sockerrno,sockerrstr);
}
#else
bytes_sent = sendmsg (_socket_fd, _msg_header, _flags);
if (bytes_sent < 0)
{
sockerrno = dl_get_last_socket_error_int( _socket_fd );
sockerrstr = dl_get_last_socket_error_string(_socket_fd,sockerrno,sockerrbuf,sizeof(sockerrbuf));
rcs_print_error ("sendmsg(%d,%p,%d) error: %d = %s\n",
_socket_fd, _msg_header, _flags,
sockerrno, sockerrstr);
rcs_print_error (" _msg_header->msg_iovlen = %lu\n",
((unsigned long) _msg_header->msg_iovlen));
rcs_print_error ("_msg_header->msg_iov[0].iov_len = %lu\n",
((unsigned long) _msg_header->msg_iov[0].iov_len));
}
#endif
total_bytes_sent += bytes_sent;
rcs_print_debug (PRINT_SOCKET_WRITE_SIZE, "sendmsg %d bytes to %d\n",
bytes_sent, _socket_fd);
return (bytes_sent);
}
