ambassador.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 2,331 行 · 第 1/5 页

  }
  
  PRINTD (DBG_IRQ|DBG_FLOW, "interrupt_handler done: %p", dev_id);
  return IRQ_HANDLED;
}

/********** make rate (not quite as much fun as Horizon) **********/

static unsigned int make_rate (unsigned int rate, rounding r,
			       u16 * bits, unsigned int * actual) {
  unsigned char exp = -1; // hush gcc
  unsigned int man = -1;  // hush gcc
  
  PRINTD (DBG_FLOW|DBG_QOS, "make_rate %u", rate);
  
  // rates in cells per second, ITU format (nasty 16-bit floating-point)
  // given 5-bit e and 9-bit m:
  // rate = EITHER (1+m/2^9)*2^e    OR 0
  // bits = EITHER 1<<14 | e<<9 | m OR 0
  // (bit 15 is "reserved", bit 14 "non-zero")
  // smallest rate is 0 (special representation)
  // largest rate is (1+511/512)*2^31 = 4290772992 (< 2^32-1)
  // smallest non-zero rate is (1+0/512)*2^0 = 1 (> 0)
  // simple algorithm:
  // find position of top bit, this gives e
  // remove top bit and shift (rounding if feeling clever) by 9-e
  
  // ucode bug: please don't set bit 14! so 0 rate not representable
  
  if (rate > 0xffc00000U) {
    // larger than largest representable rate
    
    if (r == round_up) {
	return -EINVAL;
    } else {
      exp = 31;
      man = 511;
    }
    
  } else if (rate) {
    // representable rate
    
    exp = 31;
    man = rate;
    
    // invariant: rate = man*2^(exp-31)
    while (!(man & (1<<31))) {
      exp = exp - 1;
      man = man<<1;
    }
    
    // man has top bit set
    // rate = (2^31+(man-2^31))*2^(exp-31)
    // rate = (1+(man-2^31)/2^31)*2^exp
    man = man<<1;
    man &= 0xffffffffU; // a nop on 32-bit systems
    // rate = (1+man/2^32)*2^exp
    
    // exp is in the range 0 to 31, man is in the range 0 to 2^32-1
    // time to lose significance... we want m in the range 0 to 2^9-1
    // rounding presents a minor problem... we first decide which way
    // we are rounding (based on given rounding direction and possibly
    // the bits of the mantissa that are to be discarded).
    
    switch (r) {
      case round_down: {
	// just truncate
	man = man>>(32-9);
	break;
      }
      case round_up: {
	// check all bits that we are discarding
	if (man & (-1>>9)) {
	  man = (man>>(32-9)) + 1;
	  if (man == (1<<9)) {
	    // no need to check for round up outside of range
	    man = 0;
	    exp += 1;
	  }
	} else {
	  man = (man>>(32-9));
	}
	break;
      }
      case round_nearest: {
	// check msb that we are discarding
	if (man & (1<<(32-9-1))) {
	  man = (man>>(32-9)) + 1;
	  if (man == (1<<9)) {
	    // no need to check for round up outside of range
	    man = 0;
	    exp += 1;
	  }
	} else {
	  man = (man>>(32-9));
	}
	break;
      }
    }
    
  } else {
    // zero rate - not representable
    
    if (r == round_down) {
      return -EINVAL;
    } else {
      exp = 0;
      man = 0;
    }
    
  }
  
  PRINTD (DBG_QOS, "rate: man=%u, exp=%hu", man, exp);
  
  if (bits)
    *bits = /* (1<<14) | */ (exp<<9) | man;
  
  if (actual)
    *actual = (exp >= 9)
      ? (1 << exp) + (man << (exp-9))
      : (1 << exp) + ((man + (1<<(9-exp-1))) >> (9-exp));
  
  return 0;
}

/********** Linux ATM Operations **********/

// some are not yet implemented while others do not make sense for
// this device

/********** Open a VC **********/

static int amb_open (struct atm_vcc * atm_vcc)
{
  int error;
  
  struct atm_qos * qos;
  struct atm_trafprm * txtp;
  struct atm_trafprm * rxtp;
  u16 tx_rate_bits;
  u16 tx_vc_bits = -1; // hush gcc
  u16 tx_frame_bits = -1; // hush gcc
  
  amb_dev * dev = AMB_DEV(atm_vcc->dev);
  amb_vcc * vcc;
  unsigned char pool = -1; // hush gcc
  short vpi = atm_vcc->vpi;
  int vci = atm_vcc->vci;
  
  PRINTD (DBG_FLOW|DBG_VCC, "amb_open %x %x", vpi, vci);
  
#ifdef ATM_VPI_UNSPEC
  // UNSPEC is deprecated, remove this code eventually
  if (vpi == ATM_VPI_UNSPEC || vci == ATM_VCI_UNSPEC) {
    PRINTK (KERN_WARNING, "rejecting open with unspecified VPI/VCI (deprecated)");
    return -EINVAL;
  }
#endif
  
  if (!(0 <= vpi && vpi < (1<<NUM_VPI_BITS) &&
	0 <= vci && vci < (1<<NUM_VCI_BITS))) {
    PRINTD (DBG_WARN|DBG_VCC, "VPI/VCI out of range: %hd/%d", vpi, vci);
    return -EINVAL;
  }
  
  qos = &atm_vcc->qos;
  
  if (qos->aal != ATM_AAL5) {
    PRINTD (DBG_QOS, "AAL not supported");
    return -EINVAL;
  }
  
  // traffic parameters
  
  PRINTD (DBG_QOS, "TX:");
  txtp = &qos->txtp;
  if (txtp->traffic_class != ATM_NONE) {
    switch (txtp->traffic_class) {
      case ATM_UBR: {
	// we take "the PCR" as a rate-cap
	int pcr = atm_pcr_goal (txtp);
	if (!pcr) {
	  // no rate cap
	  tx_rate_bits = 0;
	  tx_vc_bits = TX_UBR;
	  tx_frame_bits = TX_FRAME_NOTCAP;
	} else {
	  rounding r;
	  if (pcr < 0) {
	    r = round_down;
	    pcr = -pcr;
	  } else {
	    r = round_up;
	  }
	  error = make_rate (pcr, r, &tx_rate_bits, NULL);
	  tx_vc_bits = TX_UBR_CAPPED;
	  tx_frame_bits = TX_FRAME_CAPPED;
	}
	break;
      }
#if 0
      case ATM_ABR: {
	pcr = atm_pcr_goal (txtp);
	PRINTD (DBG_QOS, "pcr goal = %d", pcr);
	break;
      }
#endif
      default: {
	// PRINTD (DBG_QOS, "request for non-UBR/ABR denied");
	PRINTD (DBG_QOS, "request for non-UBR denied");
	return -EINVAL;
      }
    }
    PRINTD (DBG_QOS, "tx_rate_bits=%hx, tx_vc_bits=%hx",
	    tx_rate_bits, tx_vc_bits);
  }
  
  PRINTD (DBG_QOS, "RX:");
  rxtp = &qos->rxtp;
  if (rxtp->traffic_class == ATM_NONE) {
    // do nothing
  } else {
    // choose an RX pool (arranged in increasing size)
    for (pool = 0; pool < NUM_RX_POOLS; ++pool)
      if ((unsigned int) rxtp->max_sdu <= dev->rxq[pool].buffer_size) {
	PRINTD (DBG_VCC|DBG_QOS|DBG_POOL, "chose pool %hu (max_sdu %u <= %u)",
		pool, rxtp->max_sdu, dev->rxq[pool].buffer_size);
	break;
      }
    if (pool == NUM_RX_POOLS) {
      PRINTD (DBG_WARN|DBG_VCC|DBG_QOS|DBG_POOL,
	      "no pool suitable for VC (RX max_sdu %d is too large)",
	      rxtp->max_sdu);
      return -EINVAL;
    }
    
    switch (rxtp->traffic_class) {
      case ATM_UBR: {
	break;
      }
#if 0
      case ATM_ABR: {
	pcr = atm_pcr_goal (rxtp);
	PRINTD (DBG_QOS, "pcr goal = %d", pcr);
	break;
      }
#endif
      default: {
	// PRINTD (DBG_QOS, "request for non-UBR/ABR denied");
	PRINTD (DBG_QOS, "request for non-UBR denied");
	return -EINVAL;
      }
    }
  }
  
  // get space for our vcc stuff
  vcc = kmalloc (sizeof(amb_vcc), GFP_KERNEL);
  if (!vcc) {
    PRINTK (KERN_ERR, "out of memory!");
    return -ENOMEM;
  }
  atm_vcc->dev_data = (void *) vcc;
  
  // no failures beyond this point
  
  // we are not really "immediately before allocating the connection
  // identifier in hardware", but it will just have to do!
  set_bit(ATM_VF_ADDR,&atm_vcc->flags);
  
  if (txtp->traffic_class != ATM_NONE) {
    command cmd;
    
    vcc->tx_frame_bits = tx_frame_bits;
    
    down (&dev->vcc_sf);
    if (dev->rxer[vci]) {
      // RXer on the channel already, just modify rate...
      cmd.request = cpu_to_be32 (SRB_MODIFY_VC_RATE);
      cmd.args.modify_rate.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.modify_rate.rate = cpu_to_be32 (tx_rate_bits << SRB_RATE_SHIFT);
      while (command_do (dev, &cmd))
	schedule();
      // ... and TX flags, preserving the RX pool
      cmd.request = cpu_to_be32 (SRB_MODIFY_VC_FLAGS);
      cmd.args.modify_flags.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.modify_flags.flags = cpu_to_be32
	( (AMB_VCC(dev->rxer[vci])->rx_info.pool << SRB_POOL_SHIFT)
	  | (tx_vc_bits << SRB_FLAGS_SHIFT) );
      while (command_do (dev, &cmd))
	schedule();
    } else {
      // no RXer on the channel, just open (with pool zero)
      cmd.request = cpu_to_be32 (SRB_OPEN_VC);
      cmd.args.open.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.open.flags = cpu_to_be32 (tx_vc_bits << SRB_FLAGS_SHIFT);
      cmd.args.open.rate = cpu_to_be32 (tx_rate_bits << SRB_RATE_SHIFT);
      while (command_do (dev, &cmd))
	schedule();
    }
    dev->txer[vci].tx_present = 1;
    up (&dev->vcc_sf);
  }
  
  if (rxtp->traffic_class != ATM_NONE) {
    command cmd;
    
    vcc->rx_info.pool = pool;
    
    down (&dev->vcc_sf); 
    /* grow RX buffer pool */
    if (!dev->rxq[pool].buffers_wanted)
      dev->rxq[pool].buffers_wanted = rx_lats;
    dev->rxq[pool].buffers_wanted += 1;
    fill_rx_pool (dev, pool, GFP_KERNEL);
    
    if (dev->txer[vci].tx_present) {
      // TXer on the channel already
      // switch (from pool zero) to this pool, preserving the TX bits
      cmd.request = cpu_to_be32 (SRB_MODIFY_VC_FLAGS);
      cmd.args.modify_flags.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.modify_flags.flags = cpu_to_be32
	( (pool << SRB_POOL_SHIFT)
	  | (dev->txer[vci].tx_vc_bits << SRB_FLAGS_SHIFT) );
    } else {
      // no TXer on the channel, open the VC (with no rate info)
      cmd.request = cpu_to_be32 (SRB_OPEN_VC);
      cmd.args.open.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.open.flags = cpu_to_be32 (pool << SRB_POOL_SHIFT);
      cmd.args.open.rate = cpu_to_be32 (0);
    }
    while (command_do (dev, &cmd))
      schedule();
    // this link allows RX frames through
    dev->rxer[vci] = atm_vcc;
    up (&dev->vcc_sf);
  }
  
  // indicate readiness
  set_bit(ATM_VF_READY,&atm_vcc->flags);
  
  return 0;
}

/********** Close a VC **********/

static void amb_close (struct atm_vcc * atm_vcc) {
  amb_dev * dev = AMB_DEV (atm_vcc->dev);
  amb_vcc * vcc = AMB_VCC (atm_vcc);
  u16 vci = atm_vcc->vci;
  
  PRINTD (DBG_VCC|DBG_FLOW, "amb_close");
  
  // indicate unreadiness
  clear_bit(ATM_VF_READY,&atm_vcc->flags);
  
  // disable TXing
  if (atm_vcc->qos.txtp.traffic_class != ATM_NONE) {
    command cmd;
    
    down (&dev->vcc_sf);
    if (dev->rxer[vci]) {
      // RXer still on the channel, just modify rate... XXX not really needed
      cmd.request = cpu_to_be32 (SRB_MODIFY_VC_RATE);
      cmd.args.modify_rate.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.modify_rate.rate = cpu_to_be32 (0);
      // ... and clear TX rate flags (XXX to stop RM cell output?), preserving RX pool
    } else {
      // no RXer on the channel, close channel
      cmd.request = cpu_to_be32 (SRB_CLOSE_VC);
      cmd.args.close.vc = cpu_to_be32 (vci); // vpi 0
    }
    dev->txer[vci].tx_present = 0;
    while (command_do (dev, &cmd))
      schedule();
    up (&dev->vcc_sf);
  }
  
  // disable RXing
  if (atm_vcc->qos.rxtp.traffic_class != ATM_NONE) {
    command cmd;
    
    // this is (the?) one reason why we need the amb_vcc struct
    unsigned char pool = vcc->rx_info.pool;
    
    down (&dev->vcc_sf);
    if (dev->txer[vci].tx_present) {
      // TXer still on the channel, just go to pool zero XXX not really needed
      cmd.request = cpu_to_be32 (SRB_MODIFY_VC_FLAGS);
      cmd.args.modify_flags.vc = cpu_to_be32 (vci);  // vpi 0
      cmd.args.modify_flags.flags = cpu_to_be32
	(dev->txer[vci].tx_vc_bits << SRB_FLAGS_SHIFT);
    } else {
      // no TXer on the channel, close the VC
      cmd.request = cpu_to_be32 (SRB_CLOSE_VC);
      cmd.args.close.vc = cpu_to_be32 (vci); // vpi 0
    }
    // forget the rxer - no more skbs will be pushed
    if (atm_vcc != dev->rxer[vci])
      PRINTK (KERN_ERR, "%s vcc=%p rxer[vci]=%p",
	      "arghhh! we're going to die!",
	      vcc, dev->rxer[vci]);
    dev->rxer[vci] = NULL;
    while (command_do (dev, &cmd))
      schedule();
    
    /* shrink RX buffer pool */
    dev->rxq[pool].buffers_wanted -= 1;
    if (dev->rxq[pool].buffers_wanted == rx_lats) {
      dev->rxq[pool].buffers_wanted = 0;
      drain_rx_pool (dev, pool);
    }
    up (&dev->vcc_sf);
  }
  
  // free our structure
  kfree (vcc);
  
  // say the VPI/VCI is free again
  clear_bit(ATM_VF_ADDR,&atm_vcc->flags);

  return;
}

/********** Set socket options for a VC **********/

// int amb_getsockopt (struct atm_vcc * atm_vcc, int level, int optname, void * optval, int optlen);

/********** Set socket options for a VC **********/

// int amb_setsockopt (struct atm_vcc * atm_vcc, int level, int optname, void * optval, int optlen);

/********** Send **********/

static int amb_send (struct atm_vcc * atm_vcc, struct sk_buff * skb) {
  amb_dev * dev = AMB_DEV(atm_vcc->dev);
  amb_vcc * vcc = AMB_VCC(atm_vcc);
  u16 vc = atm_vcc->vci;
  unsigned int tx_len = skb->len;
  unsigned char * tx_data = skb->data;
  tx_simple * tx_descr;
  tx_in tx;
  
  if (test_bit (dead, &dev->flags))
    return -EIO;
  
  PRINTD (DBG_FLOW|DBG_TX, "amb_send vc %x data %p len %u",
	  vc, tx_data, tx_len);
  
  dump_skb (">>>", vc, skb);
  
  if (!dev->txer[vc].tx_present) {
    PRINTK (KERN_ERR, "attempt to send on RX-only VC %x", vc);
    return -EBADFD;
  }
  
  // this is a driver private field so we have to set it ourselves,
  // despite the fact that we are _required_ to use it to check for a
  // pop function
  ATM_SKB(skb)->vcc = atm_vcc;
  
  if (skb->len > (size_t) atm_vcc->qos.txtp.max_sdu) {
    PRINTK (KERN_ERR, "sk_buff length greater than agreed max_sdu, dropping...");
    return -EIO;
  }
  
  if (check_area (skb->data, skb->len)) {
    atomic_inc(&atm_vcc->stats->tx_err);