}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);  }