fifo.c under xillybus_demoapps directory

fifo.c

#include <pthread.h>
#include <semaphore.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <stdint.h>
#include <sys/time.h>

/*********************************************************************
 *                                                                   *
 *                 D E C L A R A T I O N S                           *
 *                                                                   *
 *********************************************************************/

struct xillyfifo {
  unsigned long read_total;
  unsigned long write_total;
  unsigned int bytes_in_fifo;
  unsigned int read_position;
  unsigned int write_position;
  unsigned int size; 
  unsigned int done;
  unsigned char *baseaddr;
  sem_t write_sem;
  sem_t read_sem;   
};

struct xillyinfo {
  int slept;
  int bytes;
  int position;
  void *addr;
};

#define FIFO_BACKOFF 10
static int read_fd = 0;
static int write_fd = 1;

// pointers to two buffers, declared but yet to allocate memory
unsigned char *array_input;
unsigned char *array_hardware;
int i;  // to address the indexes of memory data pointed by 'array_input' and 'array_hardware' for initialization and comparison purposes

struct timeval tv1,tv2;  // for computing execution time

/*********************************************************************
 *                                                                   *
 *                 A P I   F U N C T I O N S                         *
 *                                                                   *
 *********************************************************************/

// IMPORTANT:
// =========
//
// NEITHER of the fifo_* functions is reentrant. Only one thread should have
// access to any set of them. This is pretty straightforward when one thread
// writes and one thread reads from the FIFO.
//
// Also make sure that fifo_drained() and fifo_wrote() are NEVER called with
// req_bytes larger than what their request-counterparts RETURNED, or
// things will go crazy pretty soon.


int fifo_init(struct xillyfifo *fifo,
	      unsigned int size) {

  fifo->baseaddr = NULL;
  fifo->size = 0;
  fifo->bytes_in_fifo = 0;
  fifo->read_position = 0;
  fifo->write_position = 0;
  fifo->read_total = 0;
  fifo->write_total = 0;
  fifo->done = 0;

  if (sem_init(&fifo->read_sem, 0, 0) == -1)
    return -1; // Fail!

  if (sem_init(&fifo->write_sem, 0, 1) == -1)
    return -1;
 
  fifo->baseaddr = malloc(2*size);  // to accomodate both 'array_input' (outgoing DMA buffer/FIFO) and 'array_hardware' (incoming DMA buffer/FIFO)

  if (!fifo->baseaddr)
    return -1;

  if (mlock(fifo->baseaddr, 2*size)) {
    unsigned int i;
    unsigned char *buf = fifo->baseaddr;

    fprintf(stderr, "Warning: Failed to lock RAM, so FIFO's memory may swap to disk.\n"
	    "(You may want to use ulimit -l)\n");

    // Write something every 1024 bytes (4096 should be OK, actually).
    // Hopefully all pages are in real RAM after this. Better than nothing.

    for (i=0; i<(2*size); i+=1024)
      buf[i] = 0;
  }

  fifo->size = 2*size;

  array_input = fifo->baseaddr;   // points the 'array_input' which is the outgoing FIFO to the base starting memory address
  array_hardware = fifo->baseaddr + size;  // points the 'array_hardware' which is the incoming FIFO to memory address after end of 'array_input'

  // generate inputs and initializing outputs
  for(i=0; i<size; i++){
    array_input[i] = i;
    array_hardware[i] = 0;
  }

  return 0; // Success
}

void fifo_done(struct xillyfifo *fifo) {
  fifo->done = 1;
  sem_post(&fifo->read_sem);
  sem_post(&fifo->write_sem);
}

void fifo_destroy(struct xillyfifo *fifo) {
  if (!fifo->baseaddr)
    return; // Better safe than SEGV

  munlock(fifo->baseaddr, fifo->size);
  free(fifo->baseaddr);
  
  sem_destroy(&fifo->read_sem);
  sem_destroy(&fifo->write_sem);

  fifo->baseaddr = NULL;
}

int fifo_request_drain(struct xillyfifo *fifo,
		       struct xillyinfo *info) {
  int taken = 0;
  unsigned int now_bytes, max_bytes;

  info->slept = 0;
  info->addr = NULL;

  now_bytes = __sync_add_and_fetch(&fifo->bytes_in_fifo, 0);

  while (now_bytes == 0) {
    if (fifo->done)
      goto fail; // FIFO will not be used by other side, and is empty

    // fifo_wrote() updates bytes_in_fifo and then increments semaphore,
    // so there's no chance for oversleeping. On the other hand, it's 
    // possible that the data was drained between the bytes_in_fifo
    // update and the semaphore increment, leading to a false wakeup.
    // That's why we're in a while loop ( + other race conditions).
    
    info->slept = 1;

    if (sem_wait(&fifo->read_sem) && (errno != EINTR))
      goto fail;

    now_bytes = __sync_add_and_fetch(&fifo->bytes_in_fifo, 0);
  }

  max_bytes = fifo->size - fifo->read_position;
  taken = (now_bytes < max_bytes) ? now_bytes : max_bytes;
  info->addr = fifo->baseaddr + fifo->read_position;

 fail:
  info->bytes = taken;
  info->position = fifo->read_position;

  return taken;
}

void fifo_drained(struct xillyfifo *fifo,
		 unsigned int req_bytes) {

  int semval;

  if (req_bytes == 0)
    return;

  __sync_sub_and_fetch(&fifo->bytes_in_fifo, req_bytes);
  __sync_add_and_fetch(&fifo->read_total, req_bytes);
  
  fifo->read_position += req_bytes;

  if (fifo->read_position >= fifo->size)
    fifo->read_position -= fifo->size;

  if (sem_getvalue(&fifo->write_sem, &semval))
    semval = 1; // This fallback should never happen

  // Don't increment the semaphore if it's nonzero anyhow. The possible
  // race condition between reading and possibly incrementing has no effect.

  if (semval == 0)
    sem_post(&fifo->write_sem);
}

int fifo_request_write(struct xillyfifo *fifo,
		       struct xillyinfo *info) {
  int taken = 0;
  unsigned int now_bytes, max_bytes;

  info->slept = 0;
  info->addr = NULL;

  now_bytes = __sync_add_and_fetch(&fifo->bytes_in_fifo, 0);

  if (fifo->done)
    goto fail; // No point filling an abandoned FIFO

  while (now_bytes >= (fifo->size - FIFO_BACKOFF)) {
    // fifo_drained() updates bytes_in_fifo and then increments semaphore,
    // so there's no chance for oversleeping. On the other hand, it's 
    // possible that the data was written between the bytes_in_fifo
    // update and the semaphore increment, leading to a false wakeup.
    // That's why we're in a while loop ( + other race conditions).
    // Two wakeup conditions, 1) FIFO is not full   2）read/drain finishes    After drain finishes, write thread will wakeup.  However before wakeup, FIFO is full again，so this is a false wakeup.  This while loop guarantees write thread continues to sleep when false wakeup occurs.
    info->slept = 1;

    if (sem_wait(&fifo->write_sem) && (errno != EINTR))  // with the while loop and read_sem had not incremented semaphore, write thread goes into sem_wait() and continues to sleep (returns 'taken' as 0).
      goto fail;
  
    if (fifo->done)
      goto fail; // No point filling an abandoned FIFO

    now_bytes = __sync_add_and_fetch(&fifo->bytes_in_fifo, 0);
  }

  taken = fifo->size - (now_bytes + FIFO_BACKOFF);

  max_bytes = fifo->size - fifo->write_position;

  if (taken > max_bytes)
    taken = max_bytes;
  info->addr = fifo->baseaddr + fifo->size + fifo->write_position;   // + size because 'array_hardware' starts from (fifo->baseaddr + size)

 fail:
  info->bytes = taken;
  info->position = fifo->write_position;

  return taken;
}

void fifo_wrote(struct xillyfifo *fifo,
		 unsigned int req_bytes) {

  int semval;

  if (req_bytes == 0)
    return;

  __sync_add_and_fetch(&fifo->bytes_in_fifo, req_bytes);
  __sync_add_and_fetch(&fifo->write_total, req_bytes);
  
  fifo->write_position += req_bytes;
  
  if (fifo->write_position >= fifo->size)
    fifo->write_position -= fifo->size;
  
  if (sem_getvalue(&fifo->read_sem, &semval))
    semval = 1; // This fallback should never happen

  // Don't increment the semaphore if it's nonzero anyhow. The possible
  // race condition between reading and possibly incrementing has no effect.

  if (semval == 0)
    sem_post(&fifo->read_sem);
}

/*********************************************************************
 *                                                                   *
 *                 A P P L I C A T I O N   C O D E                   *
 *                                                                   *
 *********************************************************************/

// Read from outgoing DMA buffer/FIFO, write to write_fd (standard output OR /dev/xillybus_write_32)

void *write_thread(void *arg)
{
  struct xillyfifo *fifo = arg;
  int do_bytes, written_bytes;
  struct xillyinfo info;
  //unsigned char* buf;

  printf("Before fifo_request_drain(fifo, &info)\r\n");

  do_bytes = fifo_request_drain(fifo, &info);

  printf("In write_thread , do_bytes = %d\r\n", do_bytes);

  while (do_bytes > 0) {
    do_bytes = fifo_request_drain(fifo, &info);

    printf("In write_thread , do_bytes = %d\r\n", do_bytes);

    if (do_bytes == 0) {
      printf("before returning NULL\r\n");
      return NULL;
    }

    printf("before for (array_input = info.addr; do_bytes > 0;    array_input += written_bytes, do_bytes -= written_bytes)\r\n");

    for (array_input = info.addr; do_bytes > 0;
	 array_input += written_bytes, do_bytes -= written_bytes) {   // here, info.addr refers to read pointer of the FIFO

      printf("written_bytes = write(write_fd, array_input, do_bytes);\r\n");

      written_bytes = write(write_fd, array_input, do_bytes);

      printf("written_bytes = %d\r\n", written_bytes);

      if ((written_bytes < 0) && (errno != EINTR)) {
	perror("write() failed");
	return NULL;
      }

      if (written_bytes == 0) {
	fprintf(stderr, "Reached write EOF (?!)\n");
	fifo_done(fifo);
	return NULL;
      }

      if (written_bytes < 0) { // errno is EINTR
	written_bytes = 0;
	continue;
      }
      
      fifo_drained(fifo, written_bytes);
    }
  }
}

// Write to incoming DMA buffer/FIFO, read from read_fd (standard output OR /dev/xillybus_read_32)

void *read_thread(void *arg)
{
  struct xillyfifo *fifo = arg;
  int do_bytes, read_bytes;
  struct xillyinfo info;
  //unsigned char* buf;

  do_bytes = fifo_request_write(fifo, &info);

  while (do_bytes > 0) {
    do_bytes = fifo_request_write(fifo, &info);

    printf("In read_thread , do_bytes = %d\r\n", do_bytes);

    if (do_bytes == 0) {
      printf("before returning NULL\r\n");
      return NULL;
    }

    printf("before for (array_hardware = info.addr; do_bytes > 0;    array_hardware += read_bytes, do_bytes -= read_bytes)\r\n");

    for (array_hardware = info.addr; do_bytes > 0;
	 array_hardware += read_bytes, do_bytes -= read_bytes) {   // here, info.addr refers to write pointer of the FIFO
      
      printf("before read(read_fd, array_hardware, do_bytes)\r\n");

      read_bytes = read(read_fd, array_hardware, do_bytes);

      printf("read_bytes = %d\r\n", read_bytes);

      if ((read_bytes < 0) && (errno != EINTR)) {
	      perror("read() failed");
	      return NULL;
      }

      if (read_bytes == 0) {
	      // Reached EOF. Quit without complaining.
	      fifo_done(fifo);
	      return NULL;
      }

      if (read_bytes < 0) { // errno is EINTR
	      read_bytes = 0;
	      continue;
      }
      
      fifo_wrote(fifo, read_bytes);
    }
  }
}


void *status_thread(void *arg) {
  struct xillyfifo *fifo = arg;
  
  while (fifo->done < 2)
    fprintf(stderr, "%9d bytes in FIFO, %12ld read, %12ld written\r",
	    __sync_add_and_fetch(&fifo->bytes_in_fifo, 0),
	    __sync_add_and_fetch(&fifo->read_total, 0),
	    __sync_add_and_fetch(&fifo->write_total, 0)	   
	    ); 
  return NULL;
}

int main(int argc, char *argv[]) {
  pthread_t tid[3];
  struct xillyfifo fifo;
  unsigned int fifo_size;

  if ((argc != 2) && (argc != 3) && (argc != 4)) {
    printf("argc = %d\r\n", argc);
    fprintf(stderr, "Usage: %s fifo_size [read-file]\n", argv[0]);
    exit(1);
  }

  fifo_size = atoi(argv[1]);

  if (fifo_size == 0) {
    fprintf(stderr, "Bad fifo_size argument %s\n", argv[1]);
    exit(1);
  }

  if (fifo_init(&fifo, fifo_size)) {
    perror("Failed to init");
    exit(1);
  }

  if (argc > 2) {
    read_fd = open(argv[2], O_RDONLY | O_NONBLOCK);  // for loopback, use /dev/stdout
  }
  else {
    read_fd = open("/dev/xillybus_read_32", O_RDONLY | O_NONBLOCK);
  }

  if (read_fd < 0) {
    perror("Failed to open read file");
    exit(1);
  }

  if (argc > 3) {
    write_fd = open(argv[3], O_WRONLY | O_NONBLOCK); // for loopback, use /dev/stdout
  }
  else {
    write_fd = open("/dev/xillybus_write_32", O_WRONLY | O_NONBLOCK);
  }

  if (write_fd < 0) {
    perror("Failed to open write file");
    exit(1);
  }

  if (pthread_create(&tid[0], NULL, read_thread, &fifo)) {
    perror("Failed to create thread");
    exit(1);
  }

  if (pthread_create(&tid[1], NULL, write_thread, &fifo)) {
    perror("Failed to create thread");
    exit(1);
  }

  if (pthread_create(&tid[2], NULL, status_thread, &fifo)) {
    perror("Failed to create thread");
    exit(1);
  }

  /** Synch of threads in order to exit normally*/
  gettimeofday(&tv1, NULL);

  pthread_join(tid[0], NULL);
  pthread_join(tid[1], NULL);

  gettimeofday(&tv2, NULL);
  printf("%f\n\r", (double)1000000*(tv2.tv_sec-tv1.tv_sec)+(tv2.tv_usec-tv1.tv_usec));

  fifo.done = 2; // This is a hack for the status thread
  pthread_join(tid[2], NULL);

  int success = 1;

  for(i=0;i<fifo_size;i++)
  {
    if( array_input[i] != array_hardware[i] )
    {
      success = 0;
      printf("read ID %d :%d \n\r",i,array_hardware[i]);
      break;
    }
  }

  if (success == 0){
      printf("Test is unsuccessful!\n\r");
      
      /*for(i=0; i<N; i++){
        printf("o/p from p1 is %d\n\r",array_hardware[i]);
      }*/
  }

  fifo_destroy(&fifo);

  pthread_exit(NULL);

  return 0;
}