| Line 40... |
Line 40... |
#include <netinet/tcp.h>
|
#include <netinet/tcp.h>
|
#include <inttypes.h>
|
#include <inttypes.h>
|
|
|
//#include "sim-config.h"
|
//#include "sim-config.h"
|
|
|
unsigned int serverIP = 0;
|
static unsigned int serverIP = 0;
|
|
|
struct {
|
struct {
|
struct {
|
struct {
|
|
int enabled;
|
unsigned int server_port;
|
unsigned int server_port;
|
} vapi;
|
} vapi;
|
|
struct {
|
|
int verbose;
|
|
} sim;
|
} config;
|
} config;
|
|
#define debug printf
|
|
|
|
static unsigned int server_fd = 0;
|
|
static unsigned int nhandlers = 0;
|
|
|
unsigned int server_fd = 0;
|
|
unsigned int vapi_fd = 0;
|
|
void server_request(void);
|
void server_request(void);
|
|
|
static int tcp_level = 0;
|
static int tcp_level = 0;
|
|
|
struct vapi_handler {
|
static struct vapi_handler {
|
|
int fd;
|
unsigned long id;
|
unsigned long id;
|
void (*read_func)(int);
|
void (*read_func)(unsigned long);
|
struct vapi_handler *next;
|
struct vapi_handler *next;
|
|
int temp;
|
} *vapi_handler;
|
} *vapi_handler;
|
|
|
|
|
|
/* Structure for polling, it is cached, that it doesn't have to be rebuilt each time */
|
|
static struct pollfd *fds = NULL;
|
|
static int nfds = 0;
|
|
|
|
/* Rebuilds the fds structures; see fds. */
|
|
void rebuild_fds () {
|
|
struct vapi_handler *t;
|
|
if (fds)
|
|
free (fds);
|
|
fds = (struct pollfd *) malloc (sizeof (struct pollfd) * (nhandlers + 1));
|
|
if (!fds) {
|
|
fprintf (stderr, "FATAL: Out of memory.\n");
|
|
exit (1);
|
|
}
|
|
|
|
nfds = 0;
|
|
fds[nfds].fd = server_fd;
|
|
fds[nfds].events = POLLIN;
|
|
fds[nfds++].revents = 0;
|
|
|
|
for (t = vapi_handler; t; t = t->next) {
|
|
if (t->fd) {
|
|
t->temp = nfds;
|
|
fds[nfds].fd = t->fd;
|
|
fds[nfds].events = POLLIN;
|
|
fds[nfds++].revents = 0;
|
|
} else
|
|
t->temp = -1;
|
|
}
|
|
}
|
|
|
|
/* Finds a handler with given ID, return it, NULL if not found. */
|
|
static struct vapi_handler *find_handler (unsigned long id) {
|
|
struct vapi_handler *t = vapi_handler;
|
|
while (t && t->id != id)
|
|
t = t->next;
|
|
return t;
|
|
}
|
|
|
|
/* Adds a handler with given id and returns it. */
|
|
static struct vapi_handler *add_handler (unsigned long id) {
|
|
struct vapi_handler **t = &vapi_handler;
|
|
struct vapi_handler *tt;
|
|
while ((*t))
|
|
t = &(*t)->next;
|
|
tt = (struct vapi_handler *)malloc (sizeof (struct vapi_handler));
|
|
tt->next = NULL;
|
|
tt->id = id;
|
|
tt->read_func = NULL;
|
|
tt->fd = 0;
|
|
(*t) = tt;
|
|
free (fds);
|
|
fds = NULL;
|
|
nhandlers++;
|
|
rebuild_fds ();
|
|
return tt;
|
|
}
|
|
|
|
static int vapi_write_stream(int fd, void* buf, int len)
|
|
{
|
|
int n;
|
|
char* w_buf = (char*)buf;
|
|
struct pollfd block;
|
|
|
|
while(len) {
|
|
if((n = write(fd, w_buf, len)) < 0) {
|
|
switch(errno) {
|
|
case EWOULDBLOCK: /* or EAGAIN */
|
|
/* We've been called on a descriptor marked
|
|
for nonblocking I/O. We better simulate
|
|
blocking behavior. */
|
|
block.fd = fd;
|
|
block.events = POLLOUT;
|
|
block.revents = 0;
|
|
poll(&block,1,-1);
|
|
continue;
|
|
case EINTR:
|
|
continue;
|
|
case EPIPE:
|
|
close(fd);
|
|
fd = 0;
|
|
return -1;
|
|
default:
|
|
return -1;
|
|
}
|
|
} else {
|
|
len -= n;
|
|
w_buf += n;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int vapi_read_stream(int fd, void* buf, int len)
|
|
{
|
|
int n;
|
|
char* r_buf = (char*)buf;
|
|
struct pollfd block;
|
|
|
|
while(len) {
|
|
if((n = read(fd,r_buf,len)) < 0) {
|
|
switch(errno) {
|
|
case EWOULDBLOCK: /* or EAGAIN */
|
|
/* We've been called on a descriptor marked
|
|
for nonblocking I/O. We better simulate
|
|
blocking behavior. */
|
|
block.fd = fd;
|
|
block.events = POLLIN;
|
|
block.revents = 0;
|
|
poll(&block,1,-1);
|
|
continue;
|
|
case EINTR:
|
|
continue;
|
|
default:
|
|
return -1;
|
|
}
|
|
} else if(n == 0) {
|
|
close(fd);
|
|
fd = 0;
|
|
return -1;
|
|
} else {
|
|
len -= n;
|
|
r_buf += n;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
/* Added by CZ 24/05/01 */
|
/* Added by CZ 24/05/01 */
|
int get_server_socket(const char* name,const char* proto,int port)
|
int get_server_socket(const char* name,const char* proto,int port)
|
{
|
{
|
struct servent *service;
|
struct servent *service;
|
struct protoent *protocol;
|
struct protoent *protocol;
|
| Line 175... |
Line 312... |
initiation before the accept could take place. */
|
initiation before the accept could take place. */
|
if(errno != EWOULDBLOCK && errno != EAGAIN) {
|
if(errno != EWOULDBLOCK && errno != EAGAIN) {
|
perror("accept");
|
perror("accept");
|
close(server_fd);
|
close(server_fd);
|
server_fd = 0;
|
server_fd = 0;
|
config.vapi.server_port = 0;
|
config.vapi.enabled = 0;
|
serverIP = 0;
|
serverIP = 0;
|
}
|
}
|
return;
|
return;
|
}
|
}
|
|
|
if(vapi_fd) {
|
|
close(fd);
|
|
return;
|
|
}
|
|
|
|
if(fcntl(fd,F_GETFL,&flags) < 0) {
|
if(fcntl(fd,F_GETFL,&flags) < 0) {
|
sprintf(sTemp,"Unable to get flags for vapi socket %d",fd);
|
sprintf(sTemp,"Unable to get flags for vapi socket %d",fd);
|
perror(sTemp);
|
perror(sTemp);
|
close(fd);
|
close(fd);
|
return;
|
return;
|
| Line 208... |
Line 340... |
perror(sTemp);
|
perror(sTemp);
|
close(fd);
|
close(fd);
|
return;
|
return;
|
}
|
}
|
|
|
vapi_fd = fd;
|
/* Install new handler */
|
}
|
|
|
|
static int vapi_write_stream(void* buf, int len)
|
|
{
|
{
|
int n;
|
unsigned long id;
|
char* w_buf = (char*)buf;
|
struct vapi_handler *t;
|
struct pollfd block;
|
if (vapi_read_stream (fd, &id, sizeof (id))) {
|
|
perror ("Cannot get id");
|
while(len) {
|
close (fd);
|
if((n = write(vapi_fd,w_buf,len)) < 0) {
|
return;
|
switch(errno) {
|
|
case EWOULDBLOCK: /* or EAGAIN */
|
|
/* We've been called on a descriptor marked
|
|
for nonblocking I/O. We better simulate
|
|
blocking behavior. */
|
|
block.fd = vapi_fd;
|
|
block.events = POLLOUT;
|
|
block.revents = 0;
|
|
poll(&block,1,-1);
|
|
continue;
|
|
case EINTR:
|
|
continue;
|
|
case EPIPE:
|
|
close(vapi_fd);
|
|
vapi_fd = 0;
|
|
return -1;
|
|
default:
|
|
return -1;
|
|
}
|
|
} else {
|
|
len -= n;
|
|
w_buf += n;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int vapi_read_stream(void* buf, int len)
|
|
{
|
|
int n;
|
|
char* r_buf = (char*)buf;
|
|
struct pollfd block;
|
|
|
|
while(len) {
|
|
if((n = read(vapi_fd,r_buf,len)) < 0) {
|
|
switch(errno) {
|
|
case EWOULDBLOCK: /* or EAGAIN */
|
|
/* We've been called on a descriptor marked
|
|
for nonblocking I/O. We better simulate
|
|
blocking behavior. */
|
|
block.fd = vapi_fd;
|
|
block.events = POLLIN;
|
|
block.revents = 0;
|
|
poll(&block,1,-1);
|
|
continue;
|
|
case EINTR:
|
|
continue;
|
|
default:
|
|
return -1;
|
|
}
|
}
|
} else if(n == 0) {
|
t = find_handler (id);
|
close(vapi_fd);
|
if (t)
|
vapi_fd = 0;
|
if (t->fd) {
|
return -1;
|
fprintf (stderr, "WARNING: Test with id %x already connected. Ignoring.\n", id);
|
} else {
|
close (fd);
|
len -= n;
|
return;
|
r_buf += n;
|
} else t->fd = fd;
|
|
else {
|
|
t = add_handler (id);
|
|
t->fd = fd;
|
|
rebuild_fds ();
|
}
|
}
|
|
if(config.sim.verbose)
|
|
printf ("Connection with test (id %x) established.\n", id);
|
}
|
}
|
return 0;
|
|
}
|
}
|
|
|
static int write_packet (unsigned long id, unsigned long data) {
|
static int write_packet (unsigned long id, unsigned long data) {
|
|
struct vapi_handler *t = find_handler (id);
|
|
if (!t || !t->fd)
|
|
return 1;
|
id = htonl (id);
|
id = htonl (id);
|
if (vapi_write_stream(&id, sizeof (id)) < 0)
|
if (vapi_write_stream(t->fd, &id, sizeof (id)) < 0)
|
return 1;
|
return 1;
|
data = htonl (data);
|
data = htonl (data);
|
if (vapi_write_stream(&data, sizeof (data)) < 0)
|
if (vapi_write_stream(t->fd, &data, sizeof (data)) < 0)
|
return 1;
|
return 1;
|
return 0;
|
return 0;
|
}
|
}
|
|
|
static int read_packet (unsigned long *id, unsigned long *data) {
|
static int read_packet (unsigned long id, unsigned long *data) {
|
if (vapi_read_stream(id, sizeof (unsigned long)) < 0)
|
unsigned long t_id;
|
|
struct vapi_handler *t = find_handler (id);
|
|
if (!t || !t->fd)
|
return 1;
|
return 1;
|
*id = htonl (*id);
|
if (vapi_read_stream(t->fd, &t_id, sizeof (unsigned long)) < 0)
|
if (vapi_read_stream(data, sizeof (unsigned long)) < 0)
|
return 1;
|
|
t_id = htonl (t_id);
|
|
if (t_id != id) {
|
|
fprintf (stderr, "IDs not the same (got %x, expected %x)\n", t_id, id);
|
|
return 1;
|
|
}
|
|
if (vapi_read_stream(t->fd, data, sizeof (unsigned long)) < 0)
|
return 1;
|
return 1;
|
*data = htonl (*data);
|
*data = htonl (*data);
|
return 0;
|
return 0;
|
}
|
}
|
|
|
static void vapi_request()
|
static void vapi_request (struct vapi_handler *t) {
|
{
|
unsigned long data;
|
unsigned long id, data;
|
if(read_packet (t->id, &data)) {
|
struct vapi_handler *t;
|
if(t->fd) {
|
if(read_packet (&id, &data)) {
|
|
if(vapi_fd) {
|
|
perror("vapi read");
|
perror("vapi read");
|
close(vapi_fd);
|
close(t->fd);
|
vapi_fd = 0;
|
t->fd = 0;
|
}
|
}
|
return;
|
return;
|
}
|
}
|
|
|
printf ("[%08x, %08x]\n", id, data);
|
debug ("[%08x, %08x]\n", t->id, data);
|
t = vapi_handler;
|
if (!t->read_func)
|
while (t && t->id != id)
|
fprintf (stderr, "WARNING: packet sent to undefined id %x, %x\n", t->id, data);
|
t = t->next;
|
|
if (!t && t->read_func)
|
|
fprintf (stderr, "WARNING: packet sent to undefined id %x, %x\n", id, data);
|
|
else
|
else
|
t->read_func(data);
|
t->read_func(data);
|
write_packet (id, data + 1);
|
write_packet (t->id, data + 1); //REMOVE!!!!!!!!!!
|
}
|
}
|
|
|
void vapi_check ()
|
void vapi_check ()
|
{
|
{
|
struct pollfd fds[2];
|
struct vapi_handler *t;
|
int nfds = 0;
|
|
int o_serv_fd = server_fd;
|
|
|
|
if(!o_serv_fd && !vapi_fd)
|
|
return;
|
|
|
|
if(o_serv_fd) {
|
if (!server_fd || !fds) {
|
fds[nfds].fd = o_serv_fd;
|
fprintf (stderr, "FATAL: Unable to maintain VAPI server.\n");
|
fds[nfds].events = POLLIN;
|
exit (1);
|
fds[nfds++].revents = 0;
|
|
}
|
|
if(vapi_fd) {
|
|
fds[nfds].fd = vapi_fd;
|
|
fds[nfds].events = POLLIN;
|
|
fds[nfds++].revents = 0;
|
|
}
|
}
|
|
|
|
/* Handle everything in queue. */
|
while(1) {
|
while(1) {
|
switch(poll(fds,nfds,0)) {
|
switch(poll(fds,nfds,0)) {
|
case -1:
|
case -1:
|
if(errno == EINTR)
|
if(errno == EINTR)
|
continue;
|
continue;
|
perror("poll");
|
perror("poll");
|
server_fd = 0;
|
if (server_fd)
|
break;
|
close(server_fd);
|
|
config.vapi.enabled = 0;
|
|
serverIP = 0;
|
|
return;
|
case 0: /* Nothing interesting going on */
|
case 0: /* Nothing interesting going on */
|
return;
|
return;
|
default:
|
default:
|
/* Make sure to handle the vapi port first! */
|
/* Handle the vapi ports first. */
|
if((fds[0].revents && (vapi_fd && !o_serv_fd) ||
|
for (t = vapi_handler; t; t = t->next)
|
fds[1].revents && (server_fd && vapi_fd))) {
|
if (t->temp >= 0 && fds[t->temp].revents)
|
int revents = o_serv_fd ? fds[1].revents : fds[0].revents;
|
vapi_request (t);
|
|
|
if(revents & POLLIN)
|
if(fds[0].revents) {
|
vapi_request();
|
|
else { /* Error Occurred */
|
|
fprintf(stderr,"Received flags 0x%08x on vapi socket. Shutting down.\n",revents);
|
|
close(vapi_fd);
|
|
vapi_fd = 0;
|
|
}
|
|
}
|
|
if(fds[0].revents && o_serv_fd) {
|
|
if(fds[0].revents & POLLIN)
|
if(fds[0].revents & POLLIN)
|
server_request();
|
server_request();
|
else { /* Error Occurred */
|
else { /* Error Occurred */
|
fprintf(stderr,"Received flags 0x%08x on server. Shutting down.\n",fds[0].revents);
|
fprintf(stderr,"Received flags 0x%08x on server. Shutting down.\n",fds[0].revents);
|
close(o_serv_fd);
|
if (server_fd)
|
|
close(server_fd);
|
server_fd = 0;
|
server_fd = 0;
|
config.vapi.server_port = 0;
|
config.vapi.enabled = 0;
|
serverIP = 0;
|
serverIP = 0;
|
}
|
}
|
}
|
}
|
break;
|
break;
|
} /* End of switch statement */
|
} /* End of switch statement */
|
} /* End of while statement */
|
} /* End of while statement */
|
}
|
}
|
|
|
int vapi_init () {
|
/* Inits the VAPI, according to sim-config */
|
|
int vapi_init ()
|
{
|
{
|
|
nhandlers = 0;
|
|
vapi_handler = NULL;
|
|
if (!config.vapi.enabled)
|
|
return 0; /* Nothing to do */
|
|
|
|
if (!config.vapi.server_port) {
|
|
fprintf (stderr, "WARNING: server_port = 0, shutting down VAPI\n");
|
|
config.vapi.enabled = 0;
|
|
return 1;
|
|
}
|
if(server_fd = get_server_socket("or1ksim","tcp",config.vapi.server_port))
|
if(server_fd = get_server_socket("or1ksim","tcp",config.vapi.server_port))
|
printf("VAPI Server started on port %d\n",config.vapi.server_port);
|
printf("VAPI Server started on port %d\n",config.vapi.server_port);
|
else
|
else {
|
perror ("Connection");
|
perror ("Connection");
|
|
return 1;
|
|
}
|
|
|
freopen("/dev/fd/0", "w+", stdout);
|
rebuild_fds ();
|
|
return 0;
|
}
|
}
|
|
|
void vapi_done () {}
|
/* Closes the VAPI */
|
|
void vapi_done ()
|
|
{
|
|
int i;
|
|
struct vapi_handler *t = vapi_handler;
|
|
|
|
for (i = 0; i < nfds; i++)
|
|
if (fds[i].fd)
|
|
fclose (fds[i].fd);
|
|
server_fd = 0;
|
|
config.vapi.enabled = 0;
|
|
serverIP = 0;
|
|
free (fds);
|
|
fds = 0;
|
|
while (vapi_handler) {
|
|
t = vapi_handler;
|
|
vapi_handler = vapi_handler->next;
|
|
free (t);
|
|
}
|
|
}
|
|
|
/* Installs a vapi handler to VAPI id */
|
/* Installs a vapi handler to VAPI id */
|
void vapi_install_handler (unsigned long id, void (*read_func) (unsigned long)) {
|
void vapi_install_handler (unsigned long id, void (*read_func) (unsigned long))
|
struct vapi_handler **t = &vapi_handler;
|
{
|
struct vapi_handler *tt;
|
struct vapi_handler *tt;
|
if (readfunc == NULL) {
|
if (read_func == NULL) {
|
|
struct vapi_handler **t = &vapi_handler;
|
while ((*t) && (*t)->id != id)
|
while ((*t) && (*t)->id != id)
|
t = &(*t)->next;
|
t = &(*t)->next;
|
if (!t) {
|
if (!t) {
|
fprintf (stderr, "Cannot uninstall VAPI read handler from id %x\n", id);
|
fprintf (stderr, "Cannot uninstall VAPI read handler from id %x\n", id);
|
exit (1);
|
exit (1);
|
}
|
}
|
tt = *t;
|
tt = *t;
|
(*t) = (*t)->next;
|
(*t) = (*t)->next;
|
free (tt);
|
free (tt);
|
|
nhandlers--;
|
} else {
|
} else {
|
while ((*t))
|
tt = add_handler (id);
|
t = &(*t)->next;
|
|
tt = (struct vapi_handler *)malloc (sizeof (struct vapi_handler));
|
|
tt->next = NULL;
|
|
tt->id = id;
|
|
tt->read_func = read_func;
|
tt->read_func = read_func;
|
(*t) = tt;
|
|
}
|
}
|
}
|
}
|
|
|
int main () {
|
/* Returns number of unconnected handles. */
|
|
int vapi_num_unconnected ()
|
|
{
|
|
struct vapi_handler *t = vapi_handler;
|
|
int numu = 0;
|
|
for (; t; t = t->next)
|
|
if (!t->fd) numu++;
|
|
return numu;
|
|
}
|
|
|
|
int main ()
|
|
{
|
|
config.vapi.enabled = 1;
|
|
config.vapi.server_port = 9999;
|
vapi_init ();
|
vapi_init ();
|
while (1) {
|
while (1) {
|
vapi_check();
|
vapi_check();
|
|
usleep(1);
|
}
|
}
|
vapi_done ();
|
vapi_done ();
|
}
|
}
|
|
|
No newline at end of file
|
No newline at end of file
|
