C++ BAD函数代码示例

static void BUS_ERROR_fix(const struct parsedname *pn)
{
	LEVEL_DEBUG("DS9490_Reset: ERROR -- will attempt a fix");
	/* FIXME: FIXED. In ow_usb_msg.c an USB-reset was was issued:
	* USB_Control_Msg(CONTROL_CMD, CTL_RESET_DEVICE, 0x0000, pn) ;
	* So we need to setup the Adapter again 
	* Though here is probably the wrong place, duplicated code from _setup_adaptor
	*/

	// enable both program and strong pulses
	if ( BAD( USB_Control_Msg(MODE_CMD, MOD_PULSE_EN, ENABLE_PROGRAM_AND_PULSE, pn)) ) {
		LEVEL_DATA("EnableProgram error");
	}
	// enable speed changes
	if ( BAD( USB_Control_Msg(MODE_CMD, MOD_SPEED_CHANGE_EN, 1, pn)) ) {
		LEVEL_DATA("RESET_Error: SpeedEnable error");
	}
	// set the strong pullup duration to infinite
	if ( BAD( USB_Control_Msg(COMM_CMD, COMM_SET_DURATION | COMM_IM, 0x0000, pn)) ) {
		LEVEL_DATA("StrongPullup error");
	}
	
	// Don't set speed right now, it calls reset for infinite recursion. Just set for next pass
	++ 	pn->selected_connection->changed_bus_settings ;
}

// Switch to overdrive speed -- 3 tries
static GOOD_OR_BAD DS9490_overdrive(const struct parsedname *pn)
{
	BYTE sp = _1W_OVERDRIVE_SKIP_ROM;
	BYTE resp;
	int i;

	// we need to change speed to overdrive
	for (i = 0; i < 3; i++) {
		LEVEL_DATA("set overdrive speed. Attempt %d",i);
		if ( BAD( gbRESET(BUS_reset(pn)) ) ) {
			continue;
		}
		if ( BAD( DS9490_sendback_data(&sp, &resp, 1, pn) ) || (_1W_OVERDRIVE_SKIP_ROM != resp) ) {
			LEVEL_DEBUG("error setting overdrive %.2X/0x%02X", _1W_OVERDRIVE_SKIP_ROM, resp);
			continue;
		}
		if ( GOOD( USB_Control_Msg(MODE_CMD, MOD_1WIRE_SPEED, ONEWIREBUSSPEED_OVERDRIVE, pn)) ) {
			LEVEL_DEBUG("speed is now set to overdrive");
			return gbGOOD;
		}
	}

	LEVEL_DEBUG("Error setting overdrive after 3 retries");
	return gbBAD;
}

static ZERO_OR_ERROR FS_w_vib_mode(struct one_wire_query *owq)
{
	struct parsedname *pn = PN(owq);
	UINT vib_request = OWQ_U(owq) ;
	BYTE vib_stored ;

	switch (vib_request) {
		case e_mVM001_peak:
		case e_mVM001_rms:
		case e_mVM001_multi:
			break ;
		default:
			return -EINVAL ;
	}
	
	if ( BAD( Cache_Get_SlaveSpecific(&vib_stored, sizeof(vib_stored), SlaveSpecificTag(VIB), pn)) ) {
		if ( vib_stored == vib_request ) {
			return 0 ;
		}
	}
	
	if ( BAD( OW_set_vib_mode( vib_request, pn ) ) ) {
		return -EINVAL ;
	}

	Cache_Add_SlaveSpecific(&vib_request, sizeof(vib_request), SlaveSpecificTag(VIB), pn);
	
	return 0 ;
}

static RESET_TYPE LINK_reset_in(struct connection_in * in)
{
	BYTE resp[1+in->CRLF_size];

	if (in->changed_bus_settings > 0) {
		--in->changed_bus_settings ;
		LINK_set_baud(in);	// reset paramters
	} else {
		LINK_flush(in);
	}

	if ( BAD(LINK_write(LINK_string("r"), 1, in) || BAD( LINK_read(resp, 1, in))) ) {
		LEVEL_DEBUG("Error resetting LINK device");
		LINK_slurp(in);
		return BUS_RESET_ERROR;
	}

	switch (resp[0]) {

	case 'P':
		in->AnyDevices = anydevices_yes;
		return BUS_RESET_OK;
	case 'N':
		in->AnyDevices = anydevices_no;
		return BUS_RESET_OK;
	case 'S':
		return BUS_RESET_SHORT;
	default:
		LEVEL_DEBUG("bad, Unknown LINK response %c", resp[0]);
		LINK_slurp(in);
		return BUS_RESET_ERROR;
	}
}

// return 1 shorted, 0 ok, <0 error
static RESET_TYPE DS2482_reset(const struct parsedname *pn)
{
    BYTE status_byte;
    struct connection_in * in = pn->selected_connection ;
    FILE_DESCRIPTOR_OR_ERROR file_descriptor = in->pown->file_descriptor;

    /* Make sure we're using the correct channel */
    if ( BAD(DS2482_channel_select(in)) ) {
        return BUS_RESET_ERROR;
    }

    /* write the RESET code */
    if (i2c_smbus_write_byte(file_descriptor, DS2482_CMD_1WIRE_RESET)) {
        return BUS_RESET_ERROR;
    }

    /* wait */
    // rstl+rsth+.25 usec

    /* read status */
    if ( BAD( DS2482_readstatus(&status_byte, file_descriptor, DS2482_1wire_reset_usec) ) ) {
        return BUS_RESET_ERROR;			// 8 * Tslot
    }

    in->AnyDevices = (status_byte & DS2482_REG_STS_PPD) ? anydevices_yes : anydevices_no ;
    LEVEL_DEBUG("DS2482 "I2Cformat" Any devices found on reset? %s",I2Cvar(in),in->AnyDevices==anydevices_yes?"Yes":"No");
    return (status_byte & DS2482_REG_STS_SD) ? BUS_RESET_SHORT : BUS_RESET_OK;
}

/* Puts in 9600 baud, sends 11110000 then reads response */
static RESET_TYPE DS9097_reset_in( struct connection_in * in )
{
	BYTE resetbyte = RESET_BYTE;
	BYTE responsebyte;

	if ( BAD( DS9097_pre_reset( in ) ) ) {
		return BUS_RESET_ERROR ;
	}

	if ( BAD( DS9097_send_and_get(&resetbyte, &responsebyte, 1, in )) ) {
		DS9097_post_reset( in) ;
		return BUS_RESET_ERROR ;
	}

	DS9097_post_reset(in) ;
	
	switch (responsebyte) {
	case 0x00:
		return BUS_RESET_SHORT;
	case RESET_BYTE:
		// no presence
		in->AnyDevices = anydevices_no ;
		return BUS_RESET_OK;
	default:
		in->AnyDevices = anydevices_yes ;
		return BUS_RESET_OK;
	}
}

/* 2450 A/D */
static ZERO_OR_ERROR FS_volts(struct one_wire_query *owq)
{
	struct parsedname *pn = PN(owq);
	int resolution ;
	int range ;
	_FLOAT V[4] ;

	switch ( pn->selected_filetype->data.i ) {
		case r_2V_8bit:
			range = 2 ;
			resolution = 8 ;
			break ;
		case r_5V_8bit:
			range = 5 ;
			resolution = 8 ;
			break ;
		case r_2V_16bit:
			range = 2 ;
			resolution = 16 ;
			break ;
		case r_5V_16bit:
		default:
			range = 5 ;
			resolution = 16 ;
			break ;
	}
	if ( BAD( OW_set_resolution( resolution, pn ) ) ) {
		return -EINVAL ;
	}	
	if ( BAD( OW_set_range( range, pn ) ) ) {
		return -EINVAL ;
	}	
	// Start A/D process if needed
	if ( BAD( OW_convert( OWQ_SIMUL_TEST(owq), (int)(.5+.16+4.*resolution*.08), pn) )) {
		return -EINVAL ;
	}	
	
	if ( BAD( OW_volts( V, pn ) )) {
		return -EINVAL ;
	}
	switch ( pn->selected_filetype->data.i ) {
		case r_2V_8bit:
		case r_2V_16bit:
			OWQ_array_F(owq, 0) = V[0]*.5;
			OWQ_array_F(owq, 1) = V[1]*.5;
			OWQ_array_F(owq, 2) = V[2]*.5;
			OWQ_array_F(owq, 3) = V[3]*.5;
			break ;
		case r_5V_8bit:
		case r_5V_16bit:
		default:
			OWQ_array_F(owq, 0) = V[0];
			OWQ_array_F(owq, 1) = V[1];
			OWQ_array_F(owq, 2) = V[2];
			OWQ_array_F(owq, 3) = V[3];
			break ;
	}
	return 0 ;
}

/* USB is a special case, in gets reenumerated, so we look for similar DS2401 chip */
GOOD_OR_BAD TestConnection(const struct parsedname *pn)
{
	GOOD_OR_BAD ret = 0;
	struct connection_in *in ;

	if ( pn == NO_PARSEDNAME ) {
		return gbGOOD ;
	}

	in = pn->selected_connection ;
	if ( in == NO_CONNECTION ) {
		return gbGOOD ;
	}
	
	// Test without a lock -- efficient
	if ( in->reconnect_state < reconnect_error ) {
		return gbGOOD;
	}
	
	// Lock the bus
	BUSLOCK(pn);
	// Test again
	if (in->reconnect_state >= reconnect_error) {
		// Add Statistics
		STAT_ADD1_BUS(e_bus_reconnects, in);

		// Close the bus (should leave enough reconnection information available)
		BUS_close(in);	// already locked

		// Call reconnection
		in->AnyDevices = anydevices_unknown ;
		if ( in->iroutines.reconnect != NO_RECONNECT_ROUTINE ) {
			// reconnect method exists
			ret = (in->iroutines.reconnect) (pn) ;	// call bus-specific reconnect
		} else {
			ret = BUS_detect(in->pown) ;	// call initial opener
		}
		if ( BAD( ret ) ) {
			in->reconnect_state = reconnect_ok + 1 ;
			// delay to slow thrashing
			UT_delay(200);
		} else {
			in->reconnect_state = reconnect_ok;
		}
	}
	BUSUNLOCK(pn);

	if ( BAD( ret ) ) {
		LEVEL_DEFAULT("Failed to reconnect %s bus master!", in->adapter_name);
	} else {
		LEVEL_DEFAULT("%s bus master reconnected", in->adapter_name);
	}

	return ret;
}

int c3db_dump( C3HDL *h, FILE *to, int show_empty, int ts_fmt )
{
	if( !h )
	  	return C3E_BAD_HANDLE;

	if( h->state == C3DB_ST_DIRTY )
	  	BAD( C3E_BAD_STATE );

	if( ts_fmt < C3DB_TS_SEC || ts_fmt >= C3DB_TS_MAX )
		BAD( C3E_BAD_FORMAT );

	return (h->f_dump)( h, to, show_empty, ts_fmt );
}

/* Open a DS9490  -- low level code (to allow for repeats)  */
static GOOD_OR_BAD DS9490_detect_all_adapters(struct port_in * pin_first)
{
	// discover devices
	struct port_in * pin = pin_first ;
	libusb_device **device_list;
	int n_devices = libusb_get_device_list( Globals.luc, &device_list) ;
	int i_device ;
	
	if ( n_devices < 1 ) {
		LEVEL_CONNECT("Could not find a list of USB devices");
		if ( n_devices<0 ) {
			LEVEL_DEBUG("<%s>",libusb_error_name(n_devices));
		}
		return gbBAD ;
	}

	for ( i_device = 0 ; i_device < n_devices ; ++i_device ) {
		libusb_device * current = device_list[i_device] ;
		if ( GOOD( USB_match( current ) ) ) {
			struct connection_in * in = pin->first ;
			if ( BAD(DS9490_open_and_name( current, in)) ) {
				LEVEL_DEBUG("Cannot open USB device %.d:%.d", libusb_get_device_address(current), libusb_get_bus_number(current) );
				continue ;
			} else if ( BAD(DS9490_ID_this_master(in)) ) {
				DS9490_close(in) ;
				LEVEL_DEBUG("Cannot access USB device %.d:%.d", libusb_get_device_address(current), libusb_get_bus_number(current) );
				continue;
			} else{
				pin = NewPort(NULL) ; // no reason to copy anything
				if ( pin == NULL ) {
					return gbGOOD ;
				}
				// set up the new connection for the next adapter
				DS9490_setroutines(in);
			}
		}
	}
	
	libusb_free_device_list(device_list, 1);

	if ( pin == pin_first ) {
		LEVEL_CONNECT("No USB DS9490 bus masters used");
		return gbBAD;
	}
	// Remove the extra connection
	RemovePort(pin);
	return gbGOOD ;
}

/* Open a DS9490  -- low level code (to allow for repeats)  */
static GOOD_OR_BAD DS9490_detect_specific_adapter(int bus_nr, int dev_nr, struct connection_in * in)
{
	// discover devices
	libusb_device **device_list;
	int n_devices = libusb_get_device_list( Globals.luc, &device_list) ;
	int i_device ;
	
	if ( n_devices < 1 ) {
		LEVEL_CONNECT("Could not find a list of USB devices");
		if ( n_devices<0 ) {
			LEVEL_DEBUG("<%s>",libusb_error_name(n_devices));
		}
		return gbBAD ;
	}

	// Mark this connection as taking only this address pair. Important for reconnections.
	in->master.usb.specific_usb_address = 1 ;

	for ( i_device = 0 ; i_device < n_devices ; ++i_device ) {
		libusb_device * current = device_list[i_device] ;
		if ( GOOD( USB_match( current ) ) ) {
			if ( libusb_get_bus_number(current) != bus_nr ) {
				continue ;
			}
			
			if ( libusb_get_device_address(current) != dev_nr ) {
				continue ;
			}
			
			if ( BAD(DS9490_open_and_name( current, in)) ) {
				LEVEL_DEBUG("Cannot open USB device %.d:%.d", libusb_get_device_address(current), libusb_get_bus_number(current) );
				break ;
			} else if ( BAD(DS9490_ID_this_master(in)) ) {
				DS9490_close(in) ;
				LEVEL_DEBUG("Cannot access USB device %.d:%.d", libusb_get_device_address(current), libusb_get_bus_number(current) );
				break ;
			} else{
				libusb_free_device_list(device_list, 1);
				return gbGOOD ;
			}
		}
	}
	
	libusb_free_device_list(device_list, 1);

	LEVEL_CONNECT("No USB DS9490 bus master found matching %d:%d", bus_nr,dev_nr);
	return gbBAD;
}

/* Write now that connection is set */
static ZERO_OR_ERROR FS_w_given_bus(struct one_wire_query *owq)
{
	struct parsedname *pn = PN(owq);

	if ( BAD(TestConnection(pn)) ) {
		return -ECONNABORTED;
	} else if (KnownBus(pn) && BusIsServer(pn->selected_connection)) {
		return ServerWrite(owq);
	} else if (OWQ_pn(owq).type == ePN_real) {
		ZERO_OR_ERROR write_or_error = DeviceLockGet(pn);
		if (write_or_error == 0) {
			write_or_error = FS_w_local(owq);
			DeviceLockRelease(pn);
		} else {
			LEVEL_DEBUG("Cannot lock device for writing") ;
		}
		return write_or_error ;
	} else if ( IsInterfaceDir(pn) ) {
		ZERO_OR_ERROR write_or_error;
		BUSLOCK(pn);
		write_or_error = FS_w_local(owq);
		BUSUNLOCK(pn);
		return write_or_error ;
	} else {
		return FS_w_local(owq);
	}
}

float EDIT_scale_misfit( int nxyz , float fac , short *sar , float *far )
{
    float sf , ff , sum=0.0f , df ;
    int ii , nf=0 ;

    ENTRY("EDIT_scale_misfit") ;

    if( nxyz <= 0 || sar == NULL || far == NULL ) RETURN(0.0f) ;

    if( fac == 0.0f ) fac = 1.0f ;
    df = 1.0f / fac ;

    for( ii=0 ; ii < nxyz ; ii++ ) {
        if( BAD(ii) ) continue ;
        ff = far[ii] ;
        if( ff == 0.0f ) continue ;
        sf = (short)rintf(fac*sar[ii]) ;
        if( sf == 0.0f ) {
            if( fabsf(ff) < df ) sum += fabsf(ff)*fac ;
            else                 sum += 1.0f ;
        } else {
            sf = fabsf((sf-ff)/ff) ;
            if( sf > 1.0f ) sf = 1.0f ;
            sum += sf ;
        }
        nf++ ;
    }

    if( nf > 0 ) sum /= nf ;
    RETURN(sum) ;
}

// range is 5=5V or 2=2.5V
static GOOD_OR_BAD OW_set_range( int range, struct parsedname * pn )
{
	int stored_range ;
	/* Range */
	if ( BAD( Cache_Get_SlaveSpecific(&stored_range, sizeof(stored_range), SlaveSpecificTag(RAN), pn))
		|| stored_range != range) {
			// need to set resolution
		BYTE p[_1W_2450_PAGESIZE];
		RETURN_BAD_IF_BAD( OW_r_mem(p, _1W_2450_PAGESIZE, _ADDRESS_CONTROL_PAGE, pn) ) ;
		switch ( range ) {
			case 2:
				p[_1W_2450_REG_A+1] &= ~_1W_2450_IR ;
				p[_1W_2450_REG_B+1] &= ~_1W_2450_IR ;
				p[_1W_2450_REG_C+1] &= ~_1W_2450_IR ;
				p[_1W_2450_REG_D+1] &= ~_1W_2450_IR ;
				break ;
			case 5:
			default:
				p[_1W_2450_REG_A+1] |= _1W_2450_IR ;
				p[_1W_2450_REG_B+1] |= _1W_2450_IR ;
				p[_1W_2450_REG_C+1] |= _1W_2450_IR ;
				p[_1W_2450_REG_D+1] |= _1W_2450_IR ;
				break ;
		}
		RETURN_BAD_IF_BAD( OW_w_mem(p, _1W_2450_PAGESIZE, _ADDRESS_CONTROL_PAGE, pn) );
		return Cache_Add_SlaveSpecific(&range, sizeof(int), SlaveSpecificTag(RAN), pn);
	}
	return gbGOOD ;
}

static ZERO_OR_ERROR FS_Mclear(struct one_wire_query *owq)
{
	struct parsedname *pn = PN(owq);
	int init = 1;

	if ( BAD( Cache_Get_SlaveSpecific(&init, sizeof(init), SlaveSpecificTag(INI), pn)) ) {
		OWQ_Y(owq) = 1;
		if ( FS_r_strobe(owq) != 0 ) {	// set reset pin to strobe mode
			return -EINVAL;
		}
		RETURN_ERROR_IF_BAD( OW_w_pio(0x30, pn) );

		UT_delay(100);
		// init
		RETURN_ERROR_IF_BAD( OW_w_pio(0x38, pn) ) ;
		UT_delay(10);
		// Enable Display, Cursor, and Blinking
		// Entry-mode: auto-increment, no shift
		RETURN_ERROR_IF_BAD( OW_w_pio(0x0F, pn) ) ;
		RETURN_ERROR_IF_BAD( OW_w_pio(0x06, pn) ) ;
		Cache_Add_SlaveSpecific(&init, sizeof(init), SlaveSpecificTag(INI), pn);
	}
	// clear
	RETURN_ERROR_IF_BAD( OW_w_pio(0x01, pn) );
	UT_delay(2);
	return FS_Mhome(owq);
}