spacewire.c

RTEMS (Real-Time Executive for Multiprocessor Systems) is a free open source real-time operating sys

}

rtems_device_driver spacewire_console_control(
  rtems_device_major_number major,
  rtems_device_minor_number minor,
  void                    * arg
)
{
  rtems_libio_ioctl_args_t	*ioarg = (rtems_libio_ioctl_args_t *)arg;
  SPACEWIRE_DBGC(DBGSPW_IOCALLS,"ctrl [%i,%i]\n",major, minor);
  
  if (!ioarg)
    return RTEMS_INVALID_ADDRESS;

  switch(ioarg->command) {
  case SPACEWIRE_IOCTRL_SET_NODEADDR:
    SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_NODEADDR %i\n",
          ioarg->buffer);
    SPW_PARAM(minor).nodeaddr = (unsigned int) ioarg->buffer;
    /*set node address*/
    SPW_WRITE(&SPW_REG(minor,nodeaddr),SPW_PARAM(minor).nodeaddr);
    break;
  case SPACEWIRE_IOCTRL_SET_PROTOCOL:
    SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_PROTOCOL %i\n",
      ioarg->buffer);
    SPW_PARAM(minor).proto = (unsigned int)  ioarg->buffer;
    break;
  case SPACEWIRE_IOCTRL_SET_DESTNODEADDR:
    SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_DESTNODEADDR %i\n",
      ioarg->buffer);
    SPW_PARAM(minor).destnodeaddr = (unsigned int) ioarg->buffer;
    break;
  case SPACEWIRE_IOCTRL_GET_COREBASEADDR:
    SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_BASEADDR=%i\n",
      (unsigned int)SPW_PARAM(minor).regs);
    *(unsigned int *)ioarg->buffer = (unsigned int )SPW_PARAM(minor).regs;
    break;
  case SPACEWIRE_IOCTRL_GET_COREIRQ:
    SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_GET_COREIRQ=%i\n",
      (unsigned int)SPW_PARAM(minor).irq);
    *(unsigned int *)ioarg->buffer = (unsigned int )SPW_PARAM(minor).irq;
    break;
#ifdef SPW_BUFMALLOC
  case SPACEWIRE_IOCTRL_SET_PACKETSIZE:
    {
      spw_ioctl_packetsize *ps = (spw_ioctl_packetsize*)ioarg->buffer;
      SPACEWIRE_DBGC(DBGSPW_IOCTRL,"SPACEWIRE_IOCTRL_SET_RXPACKETSIZE \n",
          ioarg->buffer);
      spacewire_hw_stop(minor);
      SPW_PARAM(minor).rxbufsize = ps->rxsize;
      SPW_PARAM(minor).txbufsize = ps->txsize;
      SPW_PARAM(minor).rxmaxlen = SPW_PARAM(minor).rxbufsize;
      spacewire_buffer_alloc(minor);
      spacewire_hw_startup(minor);
    }
    break;
#endif
  case SPACEWIRE_IOCTRL_GETPACKET: {
      spw_ioctl_packet *p = (spw_ioctl_packet*)ioarg->buffer;
      p->ret_size = spacewire_hw_receive(minor,p->buf,p->buf_size);
    }
    break;
  case SPACEWIRE_IOCTRL_PUTPACKET:{
      spw_ioctl_packet *p = (spw_ioctl_packet*)ioarg->buffer;
      spacewire_hw_send(minor,p->buf,p->buf_size);
    }
    break;
  default:
    return RTEMS_NOT_DEFINED;
  }
  return RTEMS_SUCCESSFUL;
}


/* ========================================================================== */

int spacewire_set_rxmaxlen(int minor) {
  unsigned int rxmax;
  /*set rx maxlength*/
  SPW_WRITE(&SPW_REG(minor,dma0rxmax),SPW_PARAM(minor).rxmaxlen);
  rxmax = SPW_READ(&SPW_REG(minor,dma0rxmax));
  if (rxmax != SPW_PARAM(minor).rxmaxlen) {
    printf("spacewire: error initializing rx max len (0x%x, read: 0x%x)\n",
        SPW_PARAM(minor).rxmaxlen,rxmax);
    return 0;
  }
  return 1;
}

int spacewire_hw_init(int minor) {
  unsigned int dma0ctrl;
  int ctrl = SPW_CTRL_READ(minor);

  SPW_PARAM(minor).rx = (SPACEWIRE_RXBD *)
    SPW_ALIGN(&SPW_PARAM(minor)._rxtable,SPACEWIRE_BDTABLE_SIZE);
  SPW_PARAM(minor).tx = (SPACEWIRE_TXBD *)
    SPW_ALIGN(&SPW_PARAM(minor)._txtable,SPACEWIRE_BDTABLE_SIZE);
  
  SPACEWIRE_DBG("hw_init [minor %i]\n", minor);

  SPW_PARAM(minor).is_rmap = ctrl & SPACEWIRE_CTRL_RA;
  SPW_PARAM(minor).is_rxunaligned = ctrl & SPACEWIRE_CTRL_RX;
  SPW_PARAM(minor).is_rmapcrc = ctrl & SPACEWIRE_CTRL_RC;
  
  /*set node address*/
  SPW_WRITE(&SPW_REG(minor,nodeaddr),SPW_PARAM(minor).nodeaddr);
  
  /*set clock divisor*/
  SPW_WRITE(&SPW_REG(minor,clockdiv),SPW_PARAM(minor).clkdiv);
  if (SPW_PARAM(minor).is_rmap) {
    SPW_WRITE(&SPW_REG(minor,destkey),SPW_PARAM(minor).destkey);  
  }
  SPW_WRITE(
    &SPW_REG(minor,dma0ctrl),
    /*clear status, set ctrl for dma chan*/
    SPACEWIRE_DMACTRL_PS|SPACEWIRE_DMACTRL_PR|
      SPACEWIRE_DMACTRL_TA|SPACEWIRE_DMACTRL_RA
  );
  if ((dma0ctrl = SPW_READ(&SPW_REG(minor,dma0ctrl))) != 0) {
    printf("spacewire: error initializing dma ctrl (0x%x)\n",dma0ctrl);
  }
  
  SPW_CTRL_WRITE(minor, SPACEWIRE_CTRL_RESET ); /*set ctrl*/
  SPW_CTRL_WRITE(minor,
     SPACEWIRE_CTRL_LINKSTART | (SPW_PARAM(minor).maxfreq << 5)); /*set ctrl*/
  return 0;
}

void spacewire_hw_waitlink (int minor) {
  int j;
  while (SPW_LINKSTATE(SPW_STATUS_READ(minor)) != 5) {
    rtems_task_wake_after(100);
    sched_yield();
    printf("timeout loop dev %i\n",minor);
    j++;
  }
}

int spacewire_hw_startup (int minor) {
  int i,j;
  unsigned int dmactrl;
  SPW_STATUS_WRITE(minor,0xFF); /*clear status*/
  SPW_CTRL_WRITE(minor, SPACEWIRE_CTRL_RESET ); /*set ctrl*/
  SPW_CTRL_WRITE(minor,
     SPACEWIRE_CTRL_LINKSTART | (SPW_PARAM(minor).maxfreq << 5)); /*set ctrl*/
  sched_yield();
  j = 0; i = 0;
  spacewire_hw_waitlink(minor);
  
  /* prepare transmit buffers */
  for (i = 0; i < SPW_PARAM(minor).txbufcnt;i++) {
    if (i+1 == SPW_PARAM(minor).txbufcnt) {
      SPW_PARAM(minor).tx[i].ctrl = 0|SPACEWIRE_TXBD_WR;
    } else {
      SPW_PARAM(minor).tx[i].ctrl = 0;
    }
    SPW_PARAM(minor).tx[i].addr_data =
     ((unsigned int)&SPW_PARAM(minor).ptr_txbuf0[0]) +
         (i * SPW_PARAM(minor).txbufsize);
  }
  SPW_PARAM(minor).txcur = 0;
  
  /* prepare receive buffers */
  for (i = 0; i < SPW_PARAM(minor).rxbufcnt;i++) {
    if (i+1 == SPW_PARAM(minor).rxbufcnt) {
      SPW_PARAM(minor).rx[i].ctrl = SPACEWIRE_RXBD_EN|SPACEWIRE_RXBD_WR;
    } else {
      SPW_PARAM(minor).rx[i].ctrl = SPACEWIRE_RXBD_EN;
    }
    SPW_PARAM(minor).rx[i].addr =
       ((unsigned int)&SPW_PARAM(minor).ptr_rxbuf0[0]) +
       (i * SPW_PARAM(minor).rxbufsize);
  }
  SPW_PARAM(minor).rxcur = 0;
  SPW_PARAM(minor).rxbufcur = -1;
  spacewire_set_rxmaxlen(minor);
  
  SPW_WRITE(&SPW_REG(minor,dma0txdesc),(unsigned int)SPW_PARAM(minor).tx); 
  SPW_WRITE(&SPW_REG(minor,dma0rxdesc),(unsigned int)SPW_PARAM(minor).rx); 
  
  /* start RX */
  dmactrl = SPW_READ(&SPW_REG(minor,dma0ctrl));
  SPW_WRITE(&SPW_REG(minor,dma0ctrl),
	    (dmactrl & SPACEWIRE_PREPAREMASK_RX) |
               SPACEWIRE_DMACTRL_RD | SPACEWIRE_DMACTRL_RXEN);

  SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: setup complete\n",SPW_PARAM(minor).regs);
  return RTEMS_SUCCESSFUL;
}

int spacewire_hw_stop (int minor) {
  unsigned int dmactrl;
  
  /* stop RX */
  dmactrl = SPW_READ(&SPW_REG(minor,dma0ctrl));
  SPW_WRITE(&SPW_REG(minor,dma0ctrl),
	    (dmactrl & SPACEWIRE_PREPAREMASK_RX) &
               ~(SPACEWIRE_DMACTRL_RD |SPACEWIRE_DMACTRL_RXEN));

  /* stop link */
  SPW_CTRL_WRITE(minor,
      (SPW_CTRL_READ(minor) & 0x3FD) | SPACEWIRE_CTRL_LINKDISABLE);
  
  return RTEMS_SUCCESSFUL;
}

void spacewire_hw_send(int minor,unsigned char *b,int c) {

  unsigned int dmactrl, ctrl;
  unsigned int cur = SPW_PARAM(minor).txcur;
  char *txb = SPW_PARAM(minor).ptr_txbuf0 + (cur * SPW_PARAM(minor).txbufsize);

  sched_yield();
  memcpy(&txb[2],b,c);
  
  txb[0] = SPW_PARAM(minor).destnodeaddr;
  txb[1] = SPW_PARAM(minor).proto;

#ifdef DEBUG_SPACEWIRE_ONOFF  
  if (DEBUG_SPACEWIRE_FLAGS & DBGSPW_DUMP) {
    int k;
    for (k = 0;k < c+2;k++){
      if (k % 16 == 0) {
	printf ("\n");
      }
      printf ("%.2x(%c) ",txb[k] & 0xff,
        isprint(txb[k] & 0xff) ? txb[k] & 0xff : ' ');
    }
    printf ("\n");
  }
#endif
  
  do {
    SPW_PARAM(minor).tx[0].addr_header = 0;
    SPW_PARAM(minor).tx[0].len = c+2;
    SPW_PARAM(minor).tx[0].addr_data = (unsigned int)txb;
    SPW_PARAM(minor).tx[0].ctrl = SPACEWIRE_TXBD_WR|SPACEWIRE_TXBD_EN;
    
    dmactrl = SPW_READ(&SPW_REG(minor,dma0ctrl));
    SPW_WRITE(&SPW_REG(minor,dma0ctrl),
	      (dmactrl & SPACEWIRE_PREPAREMASK_TX) | SPACEWIRE_DMACTRL_TXEN);
    
    while((ctrl = SPW_READ((volatile void *)&SPW_PARAM(minor).tx[0].ctrl)) &
               SPACEWIRE_TXBD_EN) {
      sched_yield();
    }
    if (ctrl & SPACEWIRE_TXBD_LE) {
      printf("tx error: SPACEWIRE_TXBD_LE, link error\n");
    }
  } while(ctrl & SPACEWIRE_TXBD_ERROR);
  
  SPACEWIRE_DBGC(DBGSPW_TX,"0x%x: transmitted <%i> bytes\n",
    SPW_PARAM(minor).regs,c+2);
}

int spacewire_hw_receive(int minor,unsigned char *b,int c) {
  unsigned int count = 0, len, rxlen, ctrl;
  unsigned int cur = SPW_PARAM(minor).rxcur;
  char *rxb = SPW_PARAM(minor).ptr_rxbuf0 + (cur * SPW_PARAM(minor).rxbufsize);

  SPACEWIRE_DBGC(DBGSPW_RX,"0x%x: waitin packet at pos %i\n",
    SPW_PARAM(minor).regs,cur);
  
  sched_yield();
  
  while(1) {
    ctrl = SPW_READ((volatile void *)&SPW_PARAM(minor).rx[cur].ctrl);
    if (!(ctrl & SPACEWIRE_RXBD_EN)) {
      break;
    }
    if (SPW_LINKSTATE(SPW_STATUS_READ(minor)) != 5) {
      return 0;
    }
    sched_yield();
  }
  SPACEWIRE_DBGC(DBGSPW_RX,"checking packet\n",0);
  
  len = SPACEWIRE_RXBD_LENGTH(ctrl);
  if (!(ctrl & (SPACEWIRE_RXBD_ERROR & ~SPACEWIRE_RXBD_RMAPERROR))) {
  
    if (SPW_PARAM(minor).rxbufcur == -1) {
      SPACEWIRE_DBGC(DBGSPW_RX,"incoming packet len %i\n",len);
      SPW_PARAM(minor).rxbufcur = 2;
    }
    rxlen = len - SPW_PARAM(minor).rxbufcur;
    if (rxlen > c) {
      rxlen = c;
    }
    memcpy(b,rxb+SPW_PARAM(minor).rxbufcur,rxlen);
    count += rxlen;
    b += rxlen;
    c -= rxlen;
    SPW_PARAM(minor).rxbufcur += rxlen;
  } else {
    if (ctrl & SPACEWIRE_RXBD_EEP) {
      printf("rx error: SPACEWIRE_RXBD_EEP, error end of packet\n");
    }
    if (ctrl & SPACEWIRE_RXBD_EHC) {
      printf("rx error: SPACEWIRE_RXBD_EHC, header crc error\n");
    }  
    if (ctrl & SPACEWIRE_RXBD_EDC) {
      printf("rx error: SPACEWIRE_RXBD_EDC, crc error\n");
    }
    if (ctrl & SPACEWIRE_RXBD_ETR) {
      printf("rx error: SPACEWIRE_RXBD_ETR, truncated\n");
    }
  }
  
  if (SPW_PARAM(minor).rxbufcur == len ||
      (ctrl & (SPACEWIRE_RXBD_ERROR & ~SPACEWIRE_RXBD_RMAPERROR))) {
    spacewire_rxnext(minor);
  }
  return count;
}

void spacewire_rxnext(int minor) {
  
  unsigned int dmactrl;
  unsigned int cur = SPW_PARAM(minor).rxcur;
  unsigned int ctrl = SPW_READ((volatile void *)&SPW_PARAM(minor).rx[cur].ctrl);
  int cur2 = cur;
  cur++;
  if (cur >= SPACEWIRE_RXBUFS_NR) {
    cur = 0;
    ctrl |= SPACEWIRE_RXBD_WR;
  }
  SPW_PARAM(minor).rx[cur2].ctrl = ctrl | SPACEWIRE_RXBD_EN;
  
  SPW_PARAM(minor).rxcur = cur;
  SPW_PARAM(minor).rxbufcur = -1;
  
  /* start RX */
  dmactrl = SPW_READ(&SPW_REG(minor,dma0ctrl));
  SPW_WRITE(&SPW_REG(minor,dma0ctrl),
	    (dmactrl & SPACEWIRE_PREPAREMASK_RX) | SPACEWIRE_DMACTRL_RD |
               SPACEWIRE_DMACTRL_RXEN);
  
}