vmc-client.c 20.5 KB
Newer Older
1
/*
2
 * Copyright (c) 2004, 2005 University of Utah and the Flux Group.
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
 * 
 * {{{EMULAB-LICENSE
 * 
 * This file is part of the Emulab network testbed software.
 * 
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public
 * License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * }}}
22 23 24
 */

#include "config.h"
Timothy Stack's avatar
Timothy Stack committed
25 26 27

#include <stdio.h>
#include <errno.h>
28
#include <fcntl.h>
Timothy Stack's avatar
Timothy Stack committed
29 30 31 32 33 34 35
#include <assert.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/time.h>
36
#include <sys/mman.h>
Timothy Stack's avatar
Timothy Stack committed
37 38
#include <sys/socket.h>
#include <netinet/in.h>
39 40
#include <netinet/tcp.h>
#include <arpa/inet.h>
Timothy Stack's avatar
Timothy Stack committed
41

42 43
#include <zlib.h>

Timothy Stack's avatar
Timothy Stack committed
44 45
#include "log.h"
#include "mtp.h"
David Johnson's avatar
David Johnson committed
46
#include <math.h>
Timothy Stack's avatar
Timothy Stack committed
47

48 49
#define RECORD_FRAME_COUNT 30

50 51
#define MAX_CLIENT_COUNT 10

Timothy Stack's avatar
Timothy Stack committed
52
#if defined(HAVE_MEZZANINE)
53
#  include "mezz.h"
Timothy Stack's avatar
Timothy Stack committed
54 55
#else

56 57 58
#  warning "Mezzanine not available, simulating instead."

#  define MEZZ_MAX_OBJECTS 100
Timothy Stack's avatar
Timothy Stack committed
59 60 61

typedef struct {
    int class[2];       // Color class for the two blobs
Timothy Stack's avatar
Timothy Stack committed
62
    int valid;
Timothy Stack's avatar
Timothy Stack committed
63 64 65 66 67 68 69 70
    double max_disp;    // Maximum inter-frame displacement
    double max_sep;     // Maximum blob separation
    int max_missed;     // Max frames before looking for a new match
    int missed;         // Number of missed frames (object not seen)
    double px, py, pa;  // Object pose (world cs).
} mezz_object_t;

typedef struct {
David Johnson's avatar
David Johnson committed
71 72
    int count;
    mezz_object_t objects[MEZZ_MAX_OBJECTS];
Timothy Stack's avatar
Timothy Stack committed
73 74 75 76
} mezz_objectlist_t;

typedef struct {
    double time;
77
    int calibrate;
Timothy Stack's avatar
Timothy Stack committed
78 79 80
    mezz_objectlist_t objectlist;
} mezz_mmap_t;

81 82 83 84 85
/**
 * File to read simulated positions from.
 */
static FILE *posfile = NULL;

Timothy Stack's avatar
Timothy Stack committed
86 87
#endif

88 89 90
/**
 * Default port to listen for client connections.
 */
Timothy Stack's avatar
Timothy Stack committed
91 92
#define VMCCLIENT_DEFAULT_PORT 6969

93 94 95
/**
 * Debugging flag.
 */
Timothy Stack's avatar
Timothy Stack committed
96 97
static int debug = 0;

98 99 100 101
/**
 * Flag that indicates whether or not to continue in the main loop.
 */
static volatile int looping = 1;
Timothy Stack's avatar
Timothy Stack committed
102

103 104 105 106
static volatile int do_snapshot = 0;
static int snapshot_start;
static int snapshot_frame = 0;

107 108 109 110
/**
 * Counter used to track when a new frame was received from mezzanine.
 */
static volatile unsigned int mezz_frame_count = 0;
Timothy Stack's avatar
Timothy Stack committed
111

112 113 114 115
/**
 * X offset from the real world X == 0 to our local camera X == 0.
 * Y offset from the real world Y == 0 to our local camera Y == 0.
 */
116
static struct robot_position offsets;
David Johnson's avatar
David Johnson committed
117

118 119
static double z_offset = 0.0;

120 121 122
/* This is a fiducial offset, perpendicular to robot axis. */
static double fiducial_offset = 0.0;

123 124 125 126
static mezz_mmap_t *recorded_frames[RECORD_FRAME_COUNT];

static char *recordpath = "/var/log/mezzframe";

127 128
static int maxfd;

129 130 131 132
static XDR xdr;
static char packet_buffer[2048];
static char *cursor;

133 134 135
/**
 * Print the usage statement to standard error.
 */
Timothy Stack's avatar
Timothy Stack committed
136 137 138
static void usage(void)
{
    fprintf(stderr,
139
            "Usage: vmc-client [-hd] [-p port] [-x off] [-y off] mezzfile\n"
David Johnson's avatar
David Johnson committed
140 141 142 143 144 145
            "Required arguments:\n"
            "  mezzfile\tThe name of the mezzanine shared file\n"
            "Options:\n"
            "  -h\t\tPrint this message\n"
            "  -d\t\tTurn on debugging messages and do not daemonize\n"
            "  -l logfile\tSpecify the log file\n"
David Johnson's avatar
David Johnson committed
146
            "  -p port\tSpecify the port number to listen on. (Default: %d)\n"
147 148
            "  -x offset\tx offset from real world x = 0 to our local x = 0\n"
            "  -y offset\ty offset from real world y = 0 to our local y = 0\n"
149
	    "  -z offset\tz offset from\n"
150
	    "  -o orientation\tcamera orientation\n"
151
	    "  -H offset\tFiducial offset, perpendicular to robot axis\n"
152 153 154 155
#if !defined(HAVE_MEZZANINE)
	    "  -f file\tFile to read simulated positions from.\n"
#endif
	    ,
David Johnson's avatar
David Johnson committed
156
            VMCCLIENT_DEFAULT_PORT);
Timothy Stack's avatar
Timothy Stack committed
157 158
}

159 160 161 162 163 164 165 166 167 168 169 170 171
static int mem_write(char *handle, char *data, int len)
{
    int retval = len;

    assert(handle == NULL);
    assert(data != NULL);

    memcpy(cursor, data, len);
    cursor += len;
    
    return retval;
}

172 173 174 175 176 177
/**
 * Signal handler for SIGINT, SIGQUIT, and SIGTERM that just stops the main
 * loop.
 *
 * @param signal The actual signal received.
 */
Timothy Stack's avatar
Timothy Stack committed
178 179
static void sigquit(int signal)
{
180 181
    assert((signal == SIGINT) || (signal == SIGQUIT) || (signal == SIGTERM));
    
Timothy Stack's avatar
Timothy Stack committed
182 183 184
    looping = 0;
}

185 186 187 188 189 190
/**
 * Signal handler for SIGUSR1 that updates the mezzanine frame count so the
 * main loop knows there is a new frame available.
 *
 * @param signal The actual signal received.
 */
Timothy Stack's avatar
Timothy Stack committed
191 192
static void sigusr1(int signal)
{
193 194 195 196 197
    assert(signal == SIGUSR1);
    
    mezz_frame_count += 1;
}

198 199 200 201 202 203 204 205 206 207 208 209 210
#define ROBOT_HEIGHT 0.16f

void fiducial_offset_trans(struct robot_position *pio) {
/*      float hyp, theta; */
/*      struct robot_position rp; */

/*      rp = *pio; */

/*      mtp_polar(NULL,&rp,&hyp,&theta); */

    pio->x = pio->x + fiducial_offset*cosf(pio->theta);
    pio->y = pio->y - fiducial_offset*sinf(pio->theta);
}
211 212 213 214 215 216 217 218 219 220 221 222 223 224

void radial_trans(struct robot_position *p_inout)
{
    float distance_from_center, theta, vtheta, offset;
    struct robot_position rp;
    
    rp = *p_inout;

    mtp_polar(NULL, &rp, &distance_from_center, &theta);
    vtheta = atan2f(z_offset, distance_from_center);
    offset = ROBOT_HEIGHT / tanf(vtheta);
    mtp_cartesian(NULL, distance_from_center - offset, theta, p_inout);
}

225 226 227 228 229
/**
 * Transform a local camera coordinate to a real world coordinate.
 *
 * @param p The position to transform.
 */
230
void local2global_posit_trans(struct robot_position *p_inout)
231
{
232 233
    struct robot_position rp;
    float ct, st;
234 235
    
    assert(p_inout != NULL);
236 237 238 239 240 241 242

    ct = cosf(offsets.theta);
    st = sinf(offsets.theta);
    rp.x = ct * p_inout->x + st * -p_inout->y + offsets.x;
    rp.y = ct * -p_inout->y + st * -p_inout->x + offsets.y;
    rp.theta = mtp_theta(p_inout->theta + offsets.theta + M_PI_2);
    *p_inout = rp;
Timothy Stack's avatar
Timothy Stack committed
243 244
}

245 246 247 248 249 250
static void record_frame(mezz_mmap_t *mm)
{
    int frame_number;
    
    assert(mm != NULL);

251 252 253 254 255
    if (mm->calibrate != 0) {
	frame_number = (mezz_frame_count - 1) % RECORD_FRAME_COUNT;
	memcpy(recorded_frames[frame_number], mm, sizeof(mezz_mmap_t));
	recorded_frames[frame_number]->calibrate = -1;
    }
256 257 258 259 260 261 262 263
}

static void sighup(int sig)
{
    snapshot_start = mezz_frame_count;
    do_snapshot = 1;
}

264 265 266 267 268 269 270
/**
 * Encode any object identified by mezzanine as mtp packets in the given
 * buffer.
 *
 * @param mm The mezzanine memory-mapped file.
 * @return The length, in bytes, of the packets encoded in the buffer.
 */
271
static int encode_packets(mezz_mmap_t *mm)
Timothy Stack's avatar
Timothy Stack committed
272
{
273
    struct mtp_update_position *mup;
Timothy Stack's avatar
Timothy Stack committed
274 275
    mezz_objectlist_t *mol;
    struct mtp_packet mp;
276
    int lpc, retval = 0;
Timothy Stack's avatar
Timothy Stack committed
277
    int last_idx_set;
David Johnson's avatar
David Johnson committed
278
    
Timothy Stack's avatar
Timothy Stack committed
279
    assert(mm != NULL);
280

Timothy Stack's avatar
Timothy Stack committed
281
    mol = &mm->objectlist;
282 283

    cursor = packet_buffer;
David Johnson's avatar
David Johnson committed
284
    
285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302
#if !defined(HAVE_MEZZANINE)
    if (posfile) {
	if (fscanf(posfile, "%d", &mol->count) != 1) {
	    error("bad position file, expecting object count\n");
	}
	else {
	    for (lpc = 0; lpc < mol->count; lpc++) {
		if (fscanf(posfile,
			   "%lf %lf %lf",
			   &mol->objects[lpc].px,
			   &mol->objects[lpc].py,
			   &mol->objects[lpc].pa) != 3) {
		    error("bad position file, expecting coords\n");
		}
	    }
	}
    }
#endif
303 304 305 306 307

    /*
     * Initialize one packet and then overwrite whatever is different between
     * any located objects.
     */
308 309
    mp.data.opcode = MTP_UPDATE_POSITION;
    mp.vers = MTP_VERSION;
Timothy Stack's avatar
Timothy Stack committed
310
    mp.role = MTP_ROLE_VMC;
311
    mup = &mp.data.mtp_payload_u.update_position;
312 313 314 315 316

    /*
     * XXX Find the index of the last valid objct so we know when to put
     * MTP_POSITION_STATUS_CYCLE_COMPLETE in the status field.
     */
Timothy Stack's avatar
Timothy Stack committed
317
    last_idx_set = 0;
David Johnson's avatar
David Johnson committed
318 319 320 321 322 323 324
    for (lpc = 0; lpc < mol->count; ++lpc) {
        if (mol->objects[lpc].valid) {
            last_idx_set = lpc;
        }
    }

    for (lpc = 0; lpc < mol->count; ++lpc) {
325 326 327 328 329 330 331 332 333
        if (mol->objects[lpc].valid) { /* Don't send meaningless data. */
	    if (debug > 1) {
		info("vmc-client: object[%d] - %f %f %f\n",
		     lpc,
		     mol->objects[lpc].px,
		     mol->objects[lpc].py,
		     mol->objects[lpc].pa);
	    }
	    
334
            mup->robot_id = -1;
335 336

	    /* Copy the camera-coordinates, then */
337 338 339
            mup->position.x = mol->objects[lpc].px;
            mup->position.y = mol->objects[lpc].py;
            mup->position.theta = mol->objects[lpc].pa;
David Johnson's avatar
David Johnson committed
340

341 342
	    radial_trans(&(mup->position));

343
	    /* ... transform them into global coordinates. */
344
            local2global_posit_trans(&(mup->position));
345 346 347 348 349
	    
	    if (fiducial_offset != 0.0f) {
		fiducial_offset_trans(&(mup->position));
	    }
	    
David Johnson's avatar
David Johnson committed
350
            if (lpc == last_idx_set) {
351 352 353 354
                /*
		 * Mark the end of updates for this frame so that vmcd can
		 * delete stale tracks.
		 */
355
                mup->status = MTP_POSITION_STATUS_CYCLE_COMPLETE;
David Johnson's avatar
David Johnson committed
356 357
            }
            else {
358
                mup->status = MTP_POSITION_STATUS_UNKNOWN;
David Johnson's avatar
David Johnson committed
359
            }
360 361 362 363 364 365

	    /*
	     * XXX We could probably just look at the timestamp to figure out
	     * when an update is for a different frame, instead of setting the
	     * MTP_POSITION_STATUS_CYCLE_COMPLETE flag.
	     */
366 367 368 369 370
            mup->position.timestamp = mm->time;

	    if (debug > 1) {
		mtp_print_packet(stdout, &mp);
	    }
371 372

	    /* Finally, encode the packet. */
373 374 375 376
            if (!xdr_mtp_packet(&xdr, &mp))
		assert(0);

	    xdrrec_endofrecord(&xdr, 1);
David Johnson's avatar
David Johnson committed
377
        }
Timothy Stack's avatar
Timothy Stack committed
378
    }
379 380 381 382 383

#if !defined(HAVE_MEZZANINE)
    mm->time += 1.0;
#endif

384 385
    retval = cursor - packet_buffer;

386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401
    if (retval == 0) {
	mtp_init_packet(&mp,
			MA_Opcode, MTP_CONTROL_ERROR,
			MA_Role, MTP_ROLE_VMC,
			MA_Code, MTP_POSITION_STATUS_CYCLE_COMPLETE,
			MA_Message, "",
			MA_TAG_DONE);
	
	if (!xdr_mtp_packet(&xdr, &mp))
	    assert(0);

	xdrrec_endofrecord(&xdr, 1);

	retval = cursor - packet_buffer;
    }

402 403 404 405
    if (debug > 1) {
	info("vmc-client: packet length %d\n", retval);
    }
    
Timothy Stack's avatar
Timothy Stack committed
406 407 408 409 410 411 412 413 414 415 416
    return retval;
}

int main(int argc, char *argv[])
{
    int c, port = VMCCLIENT_DEFAULT_PORT, serv_sock, on_off = 1;
    char *mezzfile, *logfile = NULL, *pidfile = NULL;
    mezz_mmap_t *mezzmap = NULL;
    int retval = EXIT_FAILURE;
    struct sockaddr_in sin;
    struct sigaction sa;
417 418

    xdrrec_create(&xdr, 0, 0, NULL, NULL, mem_write);
David Johnson's avatar
David Johnson committed
419
    
420
    while ((c = getopt(argc, argv, "hdr:p:l:i:f:x:y:z:o:H:")) != -1) {
David Johnson's avatar
David Johnson committed
421 422 423 424 425 426 427 428 429 430 431 432 433 434
        switch (c) {
        case 'h':
            usage();
            exit(0);
            break;
        case 'd':
            debug += 1;
            break;
        case 'l':
            logfile = optarg;
            break;
        case 'i':
            pidfile = optarg;
            break;
435 436 437
	case 'r':
	    recordpath = optarg;
	    break;
David Johnson's avatar
David Johnson committed
438 439
        case 'p':
            if (sscanf(optarg, "%d", &port) != 1) {
440
                error("error: -p option is not a number: %s\n", optarg);
David Johnson's avatar
David Johnson committed
441 442 443 444
                usage();
                exit(1);
            }
            break;
David Johnson's avatar
David Johnson committed
445
        case 'f':
446
#if !defined(HAVE_MEZZANINE)
David Johnson's avatar
David Johnson committed
447
            if ((posfile = fopen(optarg, "r")) == NULL) {
448 449 450
                error("error: cannot open %s\n", optarg);
                usage();
                exit(1);
David Johnson's avatar
David Johnson committed
451
            }
452
#endif
David Johnson's avatar
David Johnson committed
453 454
            break;
        case 'x':
455
	    if (sscanf(optarg, "%f", &offsets.x) != 1) {
456 457 458 459
                error("error: -x option is not a number: %s\n", optarg);
                usage();
                exit(1);
	    }
David Johnson's avatar
David Johnson committed
460 461
            break;
        case 'y':
462
	    if (sscanf(optarg, "%f", &offsets.y) != 1) {
463 464 465 466
                error("error: -y option is not a number: %s\n", optarg);
                usage();
                exit(1);
	    }
David Johnson's avatar
David Johnson committed
467
            break;
468 469 470 471 472 473 474
	case 'z':
	    if (sscanf(optarg, "%lf", &z_offset) != 1) {
                error("error: -z option is not a number: %s\n", optarg);
                usage();
                exit(1);
	    }
	    break;
475 476 477 478 479 480 481
	case 'o':
	    if (sscanf(optarg, "%f", &offsets.theta) != 1) {
                error("error: -o option is not a number: %s\n", optarg);
                usage();
                exit(1);
	    }
	    break;
482 483 484 485 486 487 488
	case 'H':
	    if (sscanf(optarg,"%lf",&fiducial_offset) != 1) {
		error("error: -H option is not a number: %s\n", optarg);
                usage();
                exit(1);
	    }
	    break;
David Johnson's avatar
David Johnson committed
489 490 491
        default:
            break;
        }
Timothy Stack's avatar
Timothy Stack committed
492
    }
David Johnson's avatar
David Johnson committed
493
    
Timothy Stack's avatar
Timothy Stack committed
494 495
    argv += optind;
    argc -= optind;
David Johnson's avatar
David Johnson committed
496
    
Timothy Stack's avatar
Timothy Stack committed
497
    if (argc == 0) {
498
        error("error: missing mezzanine file argument\n");
David Johnson's avatar
David Johnson committed
499 500 501
        usage();
        exit(1);
    }
David Johnson's avatar
David Johnson committed
502
    
Timothy Stack's avatar
Timothy Stack committed
503 504 505
    signal(SIGQUIT, sigquit);
    signal(SIGTERM, sigquit);
    signal(SIGINT, sigquit);
506 507
    
    signal(SIGHUP, sighup);
508 509

    signal(SIGPIPE, SIG_IGN);
David Johnson's avatar
David Johnson committed
510
    
Timothy Stack's avatar
Timothy Stack committed
511
    mezzfile = argv[0];
David Johnson's avatar
David Johnson committed
512
    
Timothy Stack's avatar
Timothy Stack committed
513
    if (debug) {
David Johnson's avatar
David Johnson committed
514
        loginit(0, logfile);
Timothy Stack's avatar
Timothy Stack committed
515 516
    }
    else {
David Johnson's avatar
David Johnson committed
517 518 519 520 521 522 523
        /* Become a daemon */
        daemon(0, 0);
        
        if (logfile)
            loginit(0, logfile);
        else
            loginit(1, "vmcclient");
Timothy Stack's avatar
Timothy Stack committed
524
    }
David Johnson's avatar
David Johnson committed
525
    
Timothy Stack's avatar
Timothy Stack committed
526 527
#if defined(HAVE_MEZZANINE)
    {
David Johnson's avatar
David Johnson committed
528 529 530 531 532
        if (mezz_init(0, mezzfile) == -1) {
            errorc("unable to initialize mezzanine\n");
            exit(2);
        }
        mezzmap = mezz_mmap();
533 534
	if (debug > 1)
	    mezzmap->calibrate += 1;
Timothy Stack's avatar
Timothy Stack committed
535 536 537
    }
#else
    {
David Johnson's avatar
David Johnson committed
538 539
        mezzmap = calloc(1, sizeof(mezz_mmap_t));
        
540
        mezzmap->time = 0.0;
David Johnson's avatar
David Johnson committed
541 542
        mezzmap->objectlist.count = 2;
        
Timothy Stack's avatar
Timothy Stack committed
543
        mezzmap->objectlist.objects[0].valid = 1;
David Johnson's avatar
David Johnson committed
544 545 546 547
        mezzmap->objectlist.objects[0].px = 2.5;
        mezzmap->objectlist.objects[0].py = 5.5;
        mezzmap->objectlist.objects[0].pa = 0.48;
        
Timothy Stack's avatar
Timothy Stack committed
548
        mezzmap->objectlist.objects[1].valid = 1;
David Johnson's avatar
David Johnson committed
549 550 551
        mezzmap->objectlist.objects[1].px = 4.5;
        mezzmap->objectlist.objects[1].py = 6.5;
        mezzmap->objectlist.objects[1].pa = 0.54;
Timothy Stack's avatar
Timothy Stack committed
552 553
    }
#endif
554 555 556 557 558 559

    /*
     * Install our own SIGUSR1 handler _over_ the one installed by mezz_init,
     * so we can increment the local frame count and really be able to tell
     * when a frame was received.
     */
Timothy Stack's avatar
Timothy Stack committed
560 561 562 563 564
    sa.sa_handler = sigusr1;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = 0;
    sigaction(SIGUSR1, &sa, NULL);
    
Timothy Stack's avatar
Timothy Stack committed
565
    if (pidfile) {
David Johnson's avatar
David Johnson committed
566 567 568 569 570 571
        FILE *fp;
        
        if ((fp = fopen(pidfile, "w")) != NULL) {
            fprintf(fp, "%d\n", getpid());
            (void) fclose(fp);
        }
Timothy Stack's avatar
Timothy Stack committed
572 573 574
    }
    
    memset(&sin, 0, sizeof(sin));
575
#ifndef linux
Timothy Stack's avatar
Timothy Stack committed
576
    sin.sin_len = sizeof(sin);
577
#endif
Timothy Stack's avatar
Timothy Stack committed
578 579 580
    sin.sin_family = AF_INET;
    sin.sin_port = htons(port);
    sin.sin_addr.s_addr = INADDR_ANY;
David Johnson's avatar
David Johnson committed
581
    
Timothy Stack's avatar
Timothy Stack committed
582
    if ((serv_sock = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
David Johnson's avatar
David Johnson committed
583
        error("cannot create socket\n");
Timothy Stack's avatar
Timothy Stack committed
584 585
    }
    else if (setsockopt(serv_sock,
586 587
			SOL_SOCKET,
			SO_REUSEADDR,
David Johnson's avatar
David Johnson committed
588 589
                        &on_off,
                        sizeof(on_off)) == -1) {
590
        errorc("setsockopt(SO_REUSEADDR)");
Timothy Stack's avatar
Timothy Stack committed
591 592
    }
    else if (bind(serv_sock, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
David Johnson's avatar
David Johnson committed
593
        errorc("bind");
Timothy Stack's avatar
Timothy Stack committed
594 595
    }
    else if (listen(serv_sock, 5) == -1) {
David Johnson's avatar
David Johnson committed
596
        errorc("listen");
Timothy Stack's avatar
Timothy Stack committed
597 598
    }
    else {
599
	mtp_handle_t mh[MAX_CLIENT_COUNT];
600
        int lpc, last_mezz_frame = 0;
David Johnson's avatar
David Johnson committed
601 602 603
        fd_set readfds, clientfds;
        
        FD_ZERO(&readfds);
604
        FD_ZERO(&clientfds); /* We'll track clients using this fd_set. */
David Johnson's avatar
David Johnson committed
605 606
        
        FD_SET(serv_sock, &readfds);
607

608 609 610
	if (serv_sock > maxfd)
	    maxfd = serv_sock;

611 612 613 614 615 616 617
	for (lpc = 0; lpc < RECORD_FRAME_COUNT; lpc++) {
	    if ((recorded_frames[lpc] = malloc(sizeof(mezz_mmap_t))) == NULL) {
		fatal("cannot allocate space for recorded frames\n");
	    }
	    mlock(recorded_frames[lpc], sizeof(mezz_mmap_t));
	}
	
618
        while (looping) { /* The main loop. */
David Johnson's avatar
David Johnson committed
619 620 621 622
            fd_set rreadyfds;
            int rc;
            
            rreadyfds = readfds;
623 624 625 626 627 628

	    /*
	     * Block waiting for a SIGUSR1 signal or a file-descriptor to
	     * become read-ready.  Technically, we could miss a signal and a
	     * frame, but I'm too lazy to fix it at the moment.
	     */
629
            rc = select(maxfd + 1, &rreadyfds, NULL, NULL, NULL);
630
	    
David Johnson's avatar
David Johnson committed
631 632
            if (rc > 0) {
                int lpc;
633 634

		/* First, check for a client connection. */
David Johnson's avatar
David Johnson committed
635 636 637 638 639 640 641 642 643 644 645
                if (FD_ISSET(serv_sock, &rreadyfds)) {
                    struct sockaddr_in peer_sin;
                    socklen_t slen;
                    int fd;
                    
                    slen = sizeof(peer_sin);
                    if ((fd = accept(serv_sock,
                                     (struct sockaddr *)&peer_sin,
                                     &slen)) == -1) {
                        errorc("accept");
                    }
646 647 648 649 650 651 652
                    else if (fd >= MAX_CLIENT_COUNT) {
			error("too many clients connected\n");
		    }
		    else if ((mh[fd] = mtp_create_handle(fd)) == NULL) {
			error("unable to allocate handle\n");
		    }
		    else {
653
			if (debug) {
654 655 656 657
			    info("vmc-client: connect from %s:%d (fd=%d)\n",
				 inet_ntoa(peer_sin.sin_addr),
				 ntohs(peer_sin.sin_port),
				 fd);
658 659
			}
			
660
			// We want mezzanine to copy the image data over.
661 662
			if (debug > 1)
			    mezzmap->calibrate += 1;
663 664 665 666 667 668 669
			if (setsockopt(fd,
				       IPPROTO_TCP,
				       TCP_NODELAY,
				       &on_off,
				       sizeof(on_off)) == -1) {
			    errorc("setsockopt(TCP_NODELAY)");
			}
David Johnson's avatar
David Johnson committed
670 671
                        FD_SET(fd, &readfds);
                        FD_SET(fd, &clientfds);
672

673 674 675
			if (fd > maxfd)
			    maxfd = fd;
			
676
			fd = -1;
David Johnson's avatar
David Johnson committed
677
                    }
678 679 680

		    if (fd != -1)
			close(fd);
David Johnson's avatar
David Johnson committed
681 682
                }
                
683 684 685
                /*
		 * We assume that if somebody connected to us tries to write
                 * us some data, they're screwing up; we're just a data source.
David Johnson's avatar
David Johnson committed
686
                 */
687
                for (lpc = 0; lpc < MAX_CLIENT_COUNT; lpc++) {
David Johnson's avatar
David Johnson committed
688
                    if ((lpc != serv_sock) && FD_ISSET(lpc, &rreadyfds)) {
689 690 691 692 693 694 695 696 697
			struct mtp_packet mp;
			int good = 0;
			
			if (((rc = mtp_receive_packet(mh[lpc], &mp)) !=
			     MTP_PP_SUCCESS) ||
			    (mp.vers != MTP_VERSION)) {
			    error("invalid client %d %p\n", rc, mp);
			}
			else {
698 699
			    switch (mp.data.opcode) {
			    case MTP_CONFIG_VMC_CLIENT:
700 701 702 703
				offsets.x = mp.data.mtp_payload_u.
				    config_vmc_client.fixed_x;
				offsets.y = mp.data.mtp_payload_u.
				    config_vmc_client.fixed_y;
704 705
				break;
			    case MTP_SNAPSHOT:
706 707 708 709 710
				if (debug > 1) {
				    info("doing snapshot\n");
				    snapshot_start = mezz_frame_count;
				    do_snapshot = 1;
				}
711 712 713
				break;
			    default:
				break;
714
			    }
715
			    good = 1;
716
			}
717 718

			if (!good) {
719 720
			    if (debug > 1)
				mezzmap->calibrate -= 1;
721 722 723 724 725
			    close(lpc);
			    FD_CLR(lpc, &readfds);
			    FD_CLR(lpc, &clientfds);
			}
		    }
David Johnson's avatar
David Johnson committed
726 727 728 729 730
                }
            }
            else if (rc == -1) {
                switch (errno) {
                case EINTR:
731 732 733
                    /*
		     * Check the current frame count against the last one so we
		     * can tell if there really is a new frame ready.
David Johnson's avatar
David Johnson committed
734
                     */
735 736
		    do {
			if (mezz_frame_count != last_mezz_frame) {
737 738
			    int plen;
			    
739 740 741 742 743
			    if (debug > 1) {
				info("vmc-client: new frame\n");
			    }
			    
			    record_frame(mezzmap);
744 745
			    
			    if ((plen = encode_packets(mezzmap)) == -1) {
746 747
				errorc("error: unable to encode packets");
			    }
748
			    else if (plen == 0) {
749 750 751 752
				if (debug) {
				    info("vmc-client: nothing to send %d\n",
					 mezz_frame_count);
				}
753
			    }
754 755 756
			    else {
				int lpc;
				
757
				for (lpc = 0; lpc <= maxfd; lpc++) {
758
				    if (FD_ISSET(lpc, &clientfds)) {
759 760 761 762 763 764 765 766 767
					int rc;
					
					do {
					    rc = write(lpc,
						       packet_buffer,
						       plen);
					} while (rc == -1 && errno == EINTR);
					if (rc == -1)
					    errorc("error: network write\n");
768 769 770 771 772
				    }
				}
			    }
			    
			    last_mezz_frame = mezz_frame_count;
773
			}
774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807

			if (snapshot_frame >= RECORD_FRAME_COUNT) {
			    snapshot_frame = 0;
			    do_snapshot = 0;
			}
			else if (do_snapshot) {
			    char filename[128];
			    int fd;
			    
			    snprintf(filename, sizeof(filename),
				     "%s.%03d",
				     recordpath, snapshot_frame);
			    if ((fd = open(filename,
					   O_WRONLY|O_CREAT|O_TRUNC,
					   0644)) == -1) {
				error("cannot open snapshot file %s\n",
				      filename);
			    }
			    else {
				int frame_number;
				
				frame_number =
				    (snapshot_start + snapshot_frame) %
				    RECORD_FRAME_COUNT;
				write(fd,
				      recorded_frames[frame_number],
				      sizeof(mezz_mmap_t));
				close(fd);
				fd = -1;
			    }
			    
			    snapshot_frame += 1;
			}
		    } while (do_snapshot);
David Johnson's avatar
David Johnson committed
808 809 810 811 812 813 814
                    break;
                default:
                    errorc("unhandled select error\n");
                    break;
                }
            }
        }
Timothy Stack's avatar
Timothy Stack committed
815
    }
David Johnson's avatar
David Johnson committed
816
    
Timothy Stack's avatar
Timothy Stack committed
817
#if defined(HAVE_MEZZANINE)
818 819
    if (debug > 1)
	mezzmap->calibrate -= 1;
Timothy Stack's avatar
Timothy Stack committed
820 821
    mezz_term(0);
#endif
822 823

    xdr_destroy(&xdr);
Timothy Stack's avatar
Timothy Stack committed
824 825 826
    
    return retval;
}