C++ ASSERT_ALWAYS函数代码示例

static void
check_inf_nan (void)
{
  /* only if nans and infs are available */
#if _GMP_IEEE_FLOATS && !defined(MPFR_ERRDIVZERO)
  mpfr_t  x;
  double  d;

  mpfr_init2 (x, 123);

  mpfr_set_inf (x, 1);
  d = mpfr_get_d (x, MPFR_RNDZ);
  ASSERT_ALWAYS (d > 0);
  ASSERT_ALWAYS (DOUBLE_ISINF (d));

  mpfr_set_inf (x, -1);
  d = mpfr_get_d (x, MPFR_RNDZ);
  ASSERT_ALWAYS (d < 0);
  ASSERT_ALWAYS (DOUBLE_ISINF (d));

  mpfr_set_nan (x);
  d = mpfr_get_d (x, MPFR_RNDZ);
  ASSERT_ALWAYS (DOUBLE_ISNAN (d));

  mpfr_clear (x);
#endif
}

static void
check_inf_nan (void)
{
  /* only if nans and infs are available */
#if _GMP_IEEE_FLOATS
  mpfr_t  x;
  double  d;
  long    exp;

  mpfr_init2 (x, 123);

  mpfr_set_inf (x, 1);
  d = mpfr_get_d_2exp (&exp, x, MPFR_RNDZ);
  ASSERT_ALWAYS (d > 0);
  ASSERT_ALWAYS (DOUBLE_ISINF (d));

  mpfr_set_inf (x, -1);
  d = mpfr_get_d_2exp (&exp, x, MPFR_RNDZ);
  ASSERT_ALWAYS (d < 0);
  ASSERT_ALWAYS (DOUBLE_ISINF (d));

  mpfr_set_nan (x);
  d = mpfr_get_d_2exp (&exp, x, MPFR_RNDZ);
  ASSERT_ALWAYS (DOUBLE_ISNAN (d));

  mpfr_clear (x);
#endif
}

int
fun_fscanf (const char *input, const char *fmt, void *a1, void *a2)
{
  FILE  *fp;
  int   ret;

  fp = fopen (TEMPFILE, "w+");
  ASSERT_ALWAYS (fp != NULL);
  ASSERT_ALWAYS (fputs (input, fp) != EOF);
  ASSERT_ALWAYS (fflush (fp) == 0);
  rewind (fp);

  if (a2 == NULL)
    ret = fscanf (fp, fmt, a1);
  else
    ret = fscanf (fp, fmt, a1, a2);

  got_ftell = ftell (fp);
  ASSERT_ALWAYS (got_ftell != -1L);

  fromstring_next_c = getc (fp);

  ASSERT_ALWAYS (fclose (fp) == 0);
  return ret;
}

/*
max_freq = 10000000
for prescaler in xrange(int(TIMEVT_INPUT_CLOCK / float(max_freq) + 0.5), 65535 + 1):
    if TIMEVT_INPUT_CLOCK % prescaler:
            continue
    if int(1e9) % (TIMEVT_INPUT_CLOCK / prescaler):
            continue
    print prescaler, TIMEVT_INPUT_CLOCK / prescaler, int(1e9) / (TIMEVT_INPUT_CLOCK / prescaler)
 */
void motor_timer_init(void)
{
	chSysDisable();

	// Power-on and reset
	TIMEVT_RCC_ENR |= TIMEVT_RCC_ENR_MASK;
	TIMEVT_RCC_RSTR |=  TIMEVT_RCC_RSTR_MASK;
	TIMEVT_RCC_RSTR &= ~TIMEVT_RCC_RSTR_MASK;

	TIMSTP_RCC_ENR |= TIMSTP_RCC_ENR_MASK;
	TIMSTP_RCC_RSTR |=  TIMSTP_RCC_RSTR_MASK;
	TIMSTP_RCC_RSTR &= ~TIMSTP_RCC_RSTR_MASK;

	chSysEnable();

	// Find the optimal prescaler value
	uint32_t prescaler = (uint32_t)(TIMEVT_INPUT_CLOCK / ((float)MAX_FREQUENCY)); // Initial value
	if (prescaler < 1)
		prescaler = 1;

	for (;; prescaler++) {
		ASSERT_ALWAYS(prescaler < 0xFFFF);

		if (TIMEVT_INPUT_CLOCK % prescaler) {
			continue;
		}
		const uint32_t prescaled_clock = TIMEVT_INPUT_CLOCK / prescaler;
		if (INT_1E9 % prescaled_clock) {
			continue;
		}
		break; // Ok, current prescaler value can divide the timer frequency with no remainder
	}
	_nanosec_per_tick = INT_1E9 / (TIMEVT_INPUT_CLOCK / prescaler);
	ASSERT_ALWAYS(_nanosec_per_tick < 1000);      // Make sure it is sane

	printf("Motor: Timer resolution: %u nanosec\n", (unsigned)_nanosec_per_tick);

	// Enable IRQ
	nvicEnableVector(TIMEVT_IRQn,  MOTOR_IRQ_PRIORITY_MASK);
	nvicEnableVector(TIMSTP_IRQn,  MOTOR_IRQ_PRIORITY_MASK);

	// Start the event timer
	TIMEVT->ARR = 0xFFFF;
	TIMEVT->PSC = (uint16_t)(prescaler - 1);
	TIMEVT->CR1 = TIM_CR1_URS;
	TIMEVT->SR  = 0;
	TIMEVT->EGR = TIM_EGR_UG;     // Reload immediately
	TIMEVT->CR1 = TIM_CR1_CEN;    // Start

	// Start the timestamping timer
	TIMSTP->ARR = 0xFFFF;
	TIMSTP->PSC = (uint16_t)(prescaler - 1);
	TIMSTP->CR1 = TIM_CR1_URS;
	TIMSTP->SR  = 0;
	TIMSTP->EGR = TIM_EGR_UG;     // Reload immediately
	TIMSTP->DIER = TIM_DIER_UIE;
	TIMSTP->CR1 = TIM_CR1_CEN;    // Start
}

static void adc_calibrate(ADC_TypeDef* const adc)
{
	// RSTCAL
	ASSERT_ALWAYS(!(adc->CR2 & ADC_CR2_RSTCAL));
	adc->CR2 |= ADC_CR2_RSTCAL;
	while (adc->CR2 & ADC_CR2_RSTCAL) { }

	// CAL
	ASSERT_ALWAYS(!(adc->CR2 & ADC_CR2_CAL));
	adc->CR2 |= ADC_CR2_CAL;
	while (adc->CR2 & ADC_CR2_CAL) { }
}

void
hubii_eint_init(cnodeid_t cnode)
{
    int			bit, rv;
    ii_iidsr_u_t    	hubio_eint;
    hubinfo_t		hinfo; 
    cpuid_t		intr_cpu;
    devfs_handle_t 	hub_v;
    ii_ilcsr_u_t	ilcsr;
    int bit_pos_to_irq(int bit);
    int synergy_intr_connect(int bit, int cpuid);


    hub_v = (devfs_handle_t)cnodeid_to_vertex(cnode);
    ASSERT_ALWAYS(hub_v);
    hubinfo_get(hub_v, &hinfo);

    ASSERT(hinfo);
    ASSERT(hinfo->h_cnodeid == cnode);

    ilcsr.ii_ilcsr_regval = REMOTE_HUB_L(hinfo->h_nasid, IIO_ILCSR);

    if ((ilcsr.ii_ilcsr_fld_s.i_llp_stat & 0x2) == 0) {
	/* 
	 * HUB II link is not up. 
	 * Just disable LLP, and don't connect any interrupts.
	 */
	ilcsr.ii_ilcsr_fld_s.i_llp_en = 0;
	REMOTE_HUB_S(hinfo->h_nasid, IIO_ILCSR, ilcsr.ii_ilcsr_regval);
	return;
    }
    /* Select a possible interrupt target where there is a free interrupt
     * bit and also reserve the interrupt bit for this IO error interrupt
     */
    intr_cpu = intr_heuristic(hub_v,0,INTRCONNECT_ANYBIT,II_ERRORINT,hub_v,
			      "HUB IO error interrupt",&bit);
    if (intr_cpu == CPU_NONE) {
	printk("hubii_eint_init: intr_reserve_level failed, cnode %d", cnode);
	return;
    }
	
    rv = intr_connect_level(intr_cpu, bit, 0, NULL);
    synergy_intr_connect(bit, intr_cpu);
    request_irq(bit_pos_to_irq(bit) + (intr_cpu << 8), hubii_eint_handler, 0, "SN hub error", (void *)hub_v);
    ASSERT_ALWAYS(rv >= 0);
    hubio_eint.ii_iidsr_regval = 0;
    hubio_eint.ii_iidsr_fld_s.i_enable = 1;
    hubio_eint.ii_iidsr_fld_s.i_level = bit;/* Take the least significant bits*/
    hubio_eint.ii_iidsr_fld_s.i_node = COMPACT_TO_NASID_NODEID(cnode);
    hubio_eint.ii_iidsr_fld_s.i_pi_id = cpuid_to_subnode(intr_cpu);
    REMOTE_HUB_S(hinfo->h_nasid, IIO_IIDSR, hubio_eint.ii_iidsr_regval);

}

void
hubii_eint_init(cnodeid_t cnode)
{
    int			bit, rv;
    ii_iidsr_u_t    	hubio_eint;
    hubinfo_t		hinfo; 
    cpuid_t		intr_cpu;
    vertex_hdl_t 	hub_v;
    int bit_pos_to_irq(int bit);
    ii_ilcsr_u_t	ilcsr;


    hub_v = (vertex_hdl_t)cnodeid_to_vertex(cnode);
    ASSERT_ALWAYS(hub_v);
    hubinfo_get(hub_v, &hinfo);

    ASSERT(hinfo);
    ASSERT(hinfo->h_cnodeid == cnode);

    ilcsr.ii_ilcsr_regval = REMOTE_HUB_L(hinfo->h_nasid, IIO_ILCSR);
    if ((ilcsr.ii_ilcsr_fld_s.i_llp_stat & 0x2) == 0) {
	/*
	 * HUB II link is not up.  Disable LLP. Clear old errors.
	 * Enable interrupts to handle BTE errors.
	 */
	ilcsr.ii_ilcsr_fld_s.i_llp_en = 0;
	REMOTE_HUB_S(hinfo->h_nasid, IIO_ILCSR, ilcsr.ii_ilcsr_regval);
    }

    /* Select a possible interrupt target where there is a free interrupt
     * bit and also reserve the interrupt bit for this IO error interrupt
     */
    intr_cpu = intr_heuristic(hub_v,0,SGI_II_ERROR,0,hub_v,
			      "HUB IO error interrupt",&bit);
    if (intr_cpu == CPU_NONE) {
	printk("hubii_eint_init: intr_reserve_level failed, cnode %d", cnode);
	return;
    }
	
    rv = intr_connect_level(intr_cpu, SGI_II_ERROR, 0, NULL);
    request_irq(SGI_II_ERROR, hubii_eint_handler, SA_SHIRQ, "SN_hub_error", (void *)hub_v);
    irq_desc(bit)->status |= SN2_IRQ_PER_HUB;
    ASSERT_ALWAYS(rv >= 0);
    hubio_eint.ii_iidsr_regval = 0;
    hubio_eint.ii_iidsr_fld_s.i_enable = 1;
    hubio_eint.ii_iidsr_fld_s.i_level = bit;/* Take the least significant bits*/
    hubio_eint.ii_iidsr_fld_s.i_node = COMPACT_TO_NASID_NODEID(cnode);
    hubio_eint.ii_iidsr_fld_s.i_pi_id = cpuid_to_subnode(intr_cpu);
    REMOTE_HUB_S(hinfo->h_nasid, IIO_IIDSR, hubio_eint.ii_iidsr_regval);

}

void
check_one (mpf_srcptr src, mpf_srcptr trunc, mpf_srcptr ceil, mpf_srcptr floor)
{
  mpf_t  got;

  mpf_init2 (got, mpf_get_prec (trunc));
  ASSERT_ALWAYS (PREC(got) == PREC(trunc));
  ASSERT_ALWAYS (PREC(got) == PREC(ceil));
  ASSERT_ALWAYS (PREC(got) == PREC(floor));

#define CHECK_SEP(name, fun, want)              \
  mpf_set_ui (got, 54321L); /* initial junk */  \
  fun (got, src);                               \
  MPF_CHECK_FORMAT (got);                       \
  if (mpf_cmp (got, want) != 0)                 \
    {                                           \
	printf ("%s wrong\n", name);            \
	check_print (src, got, want);           \
	abort ();                               \
    }

  CHECK_SEP ("mpf_trunc", mpf_trunc, trunc);
  CHECK_SEP ("mpf_ceil",  mpf_ceil,  ceil);
  CHECK_SEP ("mpf_floor", mpf_floor, floor);

#define CHECK_INPLACE(name, fun, want)  \
  mpf_set (got, src);                   \
  fun (got, got);                       \
  MPF_CHECK_FORMAT (got);               \
  if (mpf_cmp (got, want) != 0)         \
    {                                   \
	printf ("%s wrong\n", name);    \
	check_print (src, got, want);   \
	abort ();                       \
    }

  CHECK_INPLACE ("mpf_trunc", mpf_trunc, trunc);

  /* Can't do these unconditionally in case truncation by mpf_set strips
     some low non-zero limbs which would have rounded the result.  */
  if (ABSIZ(src) <= PREC(trunc)+1)
    {
      CHECK_INPLACE ("mpf_ceil",  mpf_ceil,  ceil);
      CHECK_INPLACE ("mpf_floor", mpf_floor, floor);
    }

  mpf_clear (got);
}

void
check_fib_table (void)
{
  int  i;
  for (i = 1; i <= FIB_TABLE_LIMIT; i++)
    ASSERT_ALWAYS (FIB_TABLE(i) != 0);
}

void
mlreset(int slave)
{
	if (!slave) {
		/*
		 * We are the master cpu and node.
		 */ 
		master_nasid = get_nasid();
		set_master_bridge_base();

		/* We're the master processor */
		master_procid = smp_processor_id();
		master_nasid = cpuid_to_nasid(master_procid);

		/*
		 * master_nasid we get back better be same as one from
		 * get_nasid()
		 */
		ASSERT_ALWAYS(master_nasid == get_nasid());

		/* early initialization of iograph */
		iograph_early_init();

		/* Initialize Hub Pseudodriver Management */
		hubdev_init();

	} else { /* slave != 0 */
		/*
		 * This code is performed ONLY by slave processors.
		 */

	}
}

void
mpn_redc_n (mp_ptr rp, mp_ptr up, mp_srcptr mp, mp_size_t n, mp_srcptr ip)
{
  mp_ptr xp, yp, scratch;
  mp_limb_t cy;
  mp_size_t rn;
  TMP_DECL;
  TMP_MARK;

  ASSERT (n > 8);

  rn = mpn_mulmod_bnm1_next_size (n);

  scratch = TMP_ALLOC_LIMBS (n + rn + mpn_mulmod_bnm1_itch (rn, n, n));

  xp = scratch;
  mpn_mullo_n (xp, up, ip, n);

  yp = scratch + n;
  mpn_mulmod_bnm1 (yp, rn, xp, n, mp, n, scratch + n + rn);

  ASSERT_ALWAYS (2 * n > rn);				/* could handle this */

  cy = mpn_sub_n (yp + rn, yp, up, 2*n - rn);		/* undo wrap around */
  MPN_DECR_U (yp + 2*n - rn, rn, cy);

  cy = mpn_sub_n (rp, up + n, yp + n, n);
  if (cy != 0)
    mpn_add_n (rp, rp, mp, n);

  TMP_FREE;
}

void
check_various (void)
{
  mpf_t got;
  mpq_t q;

  mpf_init (got);
  mpq_init (q);

  /* 1/1 == 1 */
  mpf_set_prec (got, 20L);
  mpq_set_ui (q, 1L, 1L);
  mpf_set_q (got, q);
  MPF_CHECK_FORMAT (got);
  ASSERT_ALWAYS (mpf_cmp_ui (got, 1L) == 0);

  /* 1/(2^n+1), a case where truncating the divisor would be wrong */
  mpf_set_prec (got, 500L);
  mpq_set_ui (q, 1L, 1L);
  mpz_mul_2exp (mpq_denref(q), mpq_denref(q), 800L);
  mpz_add_ui (mpq_denref(q), mpq_denref(q), 1L);
  check_one (got, q);

  mpf_clear (got);
  mpq_clear (q);
}

void
check_various (void)
{
  mpf_t got, u, v;

  mpf_init (got);
  mpf_init (u);
  mpf_init (v);

  /* 100/4 == 25 */
  mpf_set_prec (got, 20L);
  mpf_set_ui (u, 100L);
  mpf_set_ui (v, 4L);
  mpf_div (got, u, v);
  MPF_CHECK_FORMAT (got);
  ASSERT_ALWAYS (mpf_cmp_ui (got, 25L) == 0);

  /* 1/(2^n+1), a case where truncating the divisor would be wrong */
  mpf_set_prec (got, 500L);
  mpf_set_prec (v, 900L);
  mpf_set_ui (v, 1L);
  mpf_mul_2exp (v, v, 800L);
  mpf_add_ui (v, v, 1L);
  mpf_div (got, u, v);
  check_one ("1/2^n+1, separate", got, u, v);

  mpf_clear (got);
  mpf_clear (u);
  mpf_clear (v);
}

void _vic_set_interrupt_vector(OS_InterruptVector isr, UINT32 index)
{
	// VIC0
	if(index < 32)			
	{
		*VIC0VECTADDR(index) = (UINT32) isr;
	}
	// VIC1
	else if(index < 64) 		
	{
		*VIC1VECTADDR(index - 32) = (UINT32) isr;
	}
	// VIC2
	else if(index < 96) 
	{
		*VIC2VECTADDR(index - 64) = (UINT32) isr;
	}
	// VIC3
	else if(index < 128) 
	{
		*VIC3VECTADDR(index - 96) = (UINT32) isr;
	}
	else
	{
		ASSERT_ALWAYS("Invalid interrupt index");
	}
	
	return;
}

void
check_one (const char *desc, mpf_ptr got, mpf_srcptr u, mpir_ui v)
{
  mp_size_t  usize, usign;
  mp_ptr     wp;
  mpf_t      want;

  MPF_CHECK_FORMAT (got);

  /* this code not nailified yet */
  ASSERT_ALWAYS (BITS_PER_UI <= GMP_NUMB_BITS);
  usign = SIZ (u);
  usize = ABS (usign);
  wp = refmpn_malloc_limbs (usize + 1);
  wp[usize] = mpn_mul_1 (wp, PTR(u), usize, (mp_limb_t) v);

  PTR(want) = wp;
  SIZ(want) = (usign >= 0 ? usize+1 : -(usize+1));
  EXP(want) = EXP(u) + 1;
  refmpf_normalize (want);

  if (! refmpf_validate ("mpf_mul_ui", got, want))
    {
      mp_trace_base = -16;
      printf    ("  %s\n", desc);
      mpf_trace ("  u", u);
      printf    ("  v %ld  0x%lX\n", v, v);
      abort ();
    }

  free (wp);
}