slave.c

/** @file slave.c
 *  @brief Process (and reply to) an Etherbone request.
 *
 *  Copyright (C) 2011-2012 GSI Helmholtz Centre for Heavy Ion Research GmbH 
 *
 *  Processing of read/write operations is deffered to glue/readwrite.c.
 *  Unlink a hardware implementation, responses to writes are not padded.
 *
 *  @author Wesley W. Terpstra <w.terpstra@gsi.de>
 *
 *  @bug None!
 *
 *******************************************************************************
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 3 of the License, or (at your option) any later version.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *  
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library. If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************
 */

#define ETHERBONE_IMPL

#include <string.h>

#include "../transport/transport.h"
#include "../glue/socket.h"
#include "../glue/device.h"
#include "../glue/widths.h"
#include "../memory/memory.h"
#include "bigendian.h"
#include "format.h"

static inline eb_data_t EB_LOAD(uint8_t* rptr, int alignment) {
  switch (alignment) {
  case 2: return be16toh(*(uint16_t*)rptr);
  case 4: return be32toh(*(uint32_t*)rptr);
  case 8: return be64toh(*(uint64_t*)rptr);
  }
  return 0; /* unreachable */
}

static inline void EB_sWRITE(uint8_t* wptr, eb_data_t val, int alignment) {
  switch (alignment) {
  case 2: *(uint16_t*)wptr = htobe16(val); break;
  case 4: *(uint32_t*)wptr = htobe32(val); break;
  case 8: *(uint64_t*)wptr = htobe64(val); break;
  }
}

/* Find the offset */
static uint8_t eb_log2_table[8] = { 0, 1, 2, 4, 7, 3, 6, 5 };
static inline uint8_t eb_log2(uint8_t x) { return eb_log2_table[(uint8_t)(x * 0x17) >> 5]; }

void eb_device_slave(eb_socket_t socketp, eb_transport_t transportp, eb_device_t devicep) {
  struct eb_socket* socket;
  struct eb_transport* transport;
  struct eb_device* device;
  struct eb_link* link;
  eb_link_t linkp;
  int len, keep;
  uint8_t buffer[sizeof(eb_max_align_t)*(255+255+1+1)+8]; /* big enough for worst-case record */
  uint8_t* wptr, * rptr, * eos;
  uint64_t error;
  eb_width_t widths, biggest, data, addr;
  eb_address_t address_filter_bits;
  int alignment, record_alignment, header_alignment, stride, cycle;
  int reply, header, passive, active;
  
  transport = EB_TRANSPORT(transportp);
  socket = EB_SOCKET(socketp);
  
  if (devicep != EB_NULL) {
    device = EB_DEVICE(devicep);
    
    linkp = device->link;
    if (linkp == EB_NULL) return; /* Busted link? */
    link = EB_LINK(linkp);
    
    widths = device->widths;
    header = widths  == 0;
    
    passive = device->passive == devicep;
    active = !passive;
  } else {
    linkp = EB_NULL;
    link = 0;
    
    widths = 0;
    header = 1;
    
    active = 0;
    passive = 0;
  }
  
  /* Cases:
   *   passive device
   *     needing probe request
   *   active device
   *     needing probe response
   *   transport
   *     always needs EB header
   */

  reply = 0;
  len = eb_transports[transport->link_type].poll(transport, link, buffer, sizeof(buffer));
  if (len == 0) return; /* no data ready */
  if (len < 2) goto kill; /* EB is always 2 byte aligned */
  
  /* Expect and require an EB header */
  if (header) {
    /* On a stream, EB header may not be fragmented */
    if (buffer[0] != 0x4E || buffer[1] != 0x6F || len < 4) goto kill;
    
    /* Is this a probe? */
    if ((buffer[2] & EB_HEADER_PF) != 0) { /* probe flag */
      if (len != 8) goto kill; /* > 8: requestor couldn't send data before we respond! */
                               /* < 8: protocol violation! */
      if (active) goto kill; /* active link not probed! */
      
      buffer[2] = 0x12; /* V1 probe response */
      buffer[3] = socket->widths; /* passive and transport both use socket widths */
      
      /* Bytes 4-7 are echoed back */
      eb_transports[transport->link_type].send(transport, link, buffer, 8);
      
      return;
    }
    
    /* Is this a probe response? */
    if ((buffer[2] & EB_HEADER_PR) != 0) { /* probe response */
      eb_device_t devp;
      struct eb_device* dev;
      
      if (len != 8) goto kill; /* > 8: haven't sent requests, passive should not send data */
                               /* < 8: protocol violation! */
      if (passive) goto kill; /* passive link not responded! */
      
      /* Find device by probe id */
      eb_address_t tag;
      tag = be32toh(*(uint32_t*)&buffer[4]);
      
      for (devp = socket->first_device; devp != EB_NULL; devp = dev->next) {
        dev = EB_DEVICE(devp);
        if (((uint32_t)devp) == tag) break;
      }
      if (devp == EB_NULL) goto kill;
      
      dev->widths = buffer[3];
      
      return;
    } 
    
    /* Neither probe nor response, yet multiple widths? fail */
    widths = buffer[3];
    if (!eb_width_refined(widths)) goto kill;
    
    /* Unsupported widths? fail */
    widths &= socket->widths;
    if (!eb_width_possible(widths)) goto kill;
  }
  
  /* Alignment is either 2, 4, or 8. */
  data = widths & EB_DATAX;
  addr = widths >> 4;
  biggest = addr | data;
  alignment = 2;
  alignment += (biggest >= EB_DATA32)*2;
  alignment += (biggest >= EB_DATA64)*4;
  record_alignment = 4;
  record_alignment += (biggest >= EB_DATA64)*4;
  header_alignment = record_alignment;
  /* FIFO stride size */
  stride = data;
  
  /* Only these bits of incoming addresses are processed */
  address_filter_bits = ~(eb_address_t)0;
  address_filter_bits >>= (sizeof(eb_address_t) - addr) << 3;
  address_filter_bits -= (data-1);
  
  /* Setup the initial pointers */
  wptr = &buffer[0];
  if (header) wptr += header_alignment;
  rptr = wptr;
  eos = &buffer[len];
  
  /* Session-limited error shift */
  error = 0;
  cycle = 1;

resume_cycle:
  /* Below this point, assume no dereferenced pointer is valid */

  /* Start processing the payload */
  while (rptr <= eos - record_alignment) {
    int total, wconfig, wfifo, rconfig, rfifo, bconfig, sel_ok;
    eb_address_t bwa, bra, ra;
    eb_data_t wv, data_mask;
    eb_width_t op_width, op_widths;
    uint8_t op_shift, addr_low, bits, bits1;
    uint8_t flags  = rptr[0];
    uint8_t select = rptr[1];
    uint8_t wcount = rptr[2];
    uint8_t rcount = rptr[3];
    
    rptr += record_alignment;
    
    /* Decode the intended width from the select lines */
    
    /* Step 1. How many bytes shifted are the operations? 
     * op_shift = position of first set bit
     */
    op_shift = eb_log2(select & -select);

    /* Step 2. How many ones are there in a row? */
    bits = select >> op_shift;
    bits1 = (bits>>1)+1;
    op_widths = eb_log2(bits1);
    op_width = op_widths+1;
    
    /* Step 3. Check that the bitmask is valid */
    sel_ok = select != 0                 /* select is not 0 */
          && (bits & (bits+1)) == 0      /* One bit set => was an unbroken sequence of bits */
          && (op_width & op_widths) == 0 /* One bit set => operation length was a power of two */
          && (op_shift & op_widths) == 0 /* The operation is aligned to the width */
          && op_width <= data            /* The width must be supported by the port */
          && op_shift < data;            /* The shift must be supported by the port */
    
    /* Determine the low address bits of the operation */
    addr_low = data - (op_shift+op_width); /* Big endian */
    
    /* Create a mask for filtering out the important write data */
    data_mask = ~(eb_data_t)0;
    data_mask >>= (sizeof(eb_data_t) - op_width) << 3;

    /* Is the cycle flag high? */
    cycle = flags & EB_RECORD_CYC;
    
    total = wcount;
    total += rcount;
    total += (wcount>0);
    total += (rcount>0);
    
    /* Test if record overflows packet */
    while (total*alignment > eos-rptr) {
      transport = EB_TRANSPORT(transportp);
      if (linkp != EB_NULL) link = EB_LINK(linkp);
      
      /* If not a streaming socket, this is a critical error */
      if (eb_transports[transport->link_type].mtu != 0) goto kill;
      
      /* Streaming beyond this point */
      
      if (reply) {
        eb_transports[transport->link_type].send(transport, link, buffer, wptr - &buffer[0]);
      }
      
      keep = eos-rptr;
      if (rptr != &buffer[0]) memmove(&buffer[0], rptr, keep);
      
      len = eb_transports[transport->link_type].recv(transport, link, buffer+keep, sizeof(buffer)-keep);
      if (len <= 0) goto kill;
      len += keep;
      
      wptr = rptr = &buffer[0];
      eos = rptr + len;
    }
    
    if (wcount > 0) {
      wfifo = flags & EB_RECORD_WFF;
      wconfig = flags & EB_RECORD_WCA;
      
      bwa = EB_LOAD(rptr, alignment);
      rptr += alignment;
      
      if (wconfig) {
        /* Our config space uses all bits of the address for WBA */
        /* If it ever supports register write access, this would need to change */
      } else {
        /* Wishbone devices ignore the low address bits and use the select lines */
        bwa &= address_filter_bits;
        bwa |= addr_low; 
      }
        
      while (wcount--) {
        wv = EB_LOAD(rptr, alignment);
        rptr += alignment;
        
        wv >>= (op_shift<<3);
        wv &= data_mask;
        
        if (wconfig) {
          if (sel_ok)
            eb_socket_write_config(socketp, op_width, bwa, wv);
        } else {
          if (sel_ok)
            eb_socket_write(socketp, op_width, bwa, wv, &error);
          else
            error = (error<<1) | 1;
        }
        
        if (wfifo == 0) bwa += stride;
      }
    }
    
    if (rcount > 0) {
      reply = 1;
      rfifo = flags & EB_RECORD_RFF;
      bconfig = flags & EB_RECORD_BCA;
      rconfig = flags & EB_RECORD_RCA;
      
      /* Impossible to run out of space; sizeof(request) >= sizeof(reply) */
      
      /* Prepare new header */
      memset(wptr, 0, record_alignment);
      wptr[0] = cycle | 
                (bconfig ? EB_RECORD_WCA : 0) | 
                (rfifo   ? EB_RECORD_WFF : 0);
      wptr[1] = select;
      wptr[2] = rcount;
      wptr[3] = 0;
      
      wptr += record_alignment;
      
      bra = EB_LOAD(rptr, alignment);
      rptr += alignment;
      
      /* Echo back the base return address */
      EB_sWRITE(wptr, bra, alignment);
      wptr += alignment;
      
      while (rcount--) {
        ra = EB_LOAD(rptr, alignment);
        rptr += alignment;
        
        /* Wishbone devices ignore the low address bits and use the select lines */
        ra &= address_filter_bits;
        ra |= addr_low;
        
        if (rconfig) {
          if (sel_ok) {
            wv = eb_socket_read_config(socketp, op_width, ra, error);
          } else {
            wv = 0;
          }
        } else {
          if (sel_ok) {
            wv = eb_socket_read(socketp, op_width, ra, &error);
          } else {
            wv = 0;
            error = (error<<1) | 1;
          }
        }
        
        wv &= data_mask;
        wv <<= (op_shift<<3);
        
        EB_sWRITE(wptr, wv, alignment);
        wptr += alignment;
      }
    }
  }
  
  /* Reply if needed */
  if (reply) {
    eb_transports[transport->link_type].send(transport, link, buffer, wptr - &buffer[0]);
  }
  
  /* Is the cycle line still high? */
  if (cycle == 0) {
    /* Only streaming sockets may keep cycle line high */
    if (eb_transports[transport->link_type].mtu != 0) goto kill;
    
    keep = eos-rptr;
    if (rptr != &buffer[0]) memmove(&buffer[0], rptr, keep);
    
    len = eb_transports[transport->link_type].recv(transport, link, buffer+keep, sizeof(buffer)-keep);
    if (len <= 0) goto kill;
    len += keep;
    
    wptr = rptr = &buffer[0];
    eos = rptr + len;
    goto resume_cycle;
  }
  
  /* Improperly terminated message? */
  if (rptr != eos) goto kill;
  
  return;
  
kill:
  /* Destroy the connection */
  if (devicep == EB_NULL) return;
  
  if (passive) {
    eb_device_close(devicep);
  } else {
    device = EB_DEVICE(devicep);
    transport = EB_TRANSPORT(transportp);
    link = EB_LINK(linkp);
    
    eb_transports[transport->link_type].disconnect(transport, link);
    eb_free_link(device->link);
    device->link = EB_NULL;
  }
}