C++ diag_iseterr函数代码示例

// diag_tty_break #1 : use Set / ClearCommBreak
// and return as soon as break is cleared.
// ret 0 if ok
int diag_tty_break(struct diag_l0_device *dl0d, const unsigned int ms) {
	LARGE_INTEGER qpc1, qpc2;	//for timing verification
	long real_t;	//"real" duration
	struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
	int errval=0;

	if (wti->fd == INVALID_HANDLE_VALUE) {
		fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
		return diag_iseterr(DIAG_ERR_GENERAL);
	}

	if (ms <= 1)
		return diag_iseterr(DIAG_ERR_GENERAL);

	QueryPerformanceCounter(&qpc1);
	errval = !SetCommBreak(wti->fd);
	QueryPerformanceCounter(&qpc2);
	//that call can take quite a while (6ms !!) on some setups (win7 + CH340 USB-Serial).
	//It's still impossible to know (from here) when exactly TXD goes low (beginning or end of the call)
	real_t=(long) (pf_conv * (qpc2.QuadPart-qpc1.QuadPart)) / 1000L;
	real_t = (long) ms - real_t;	//time remaining
	if (real_t <= 0) real_t = 0;
	diag_os_millisleep((unsigned int ) real_t);

	errval |= !ClearCommBreak(wti->fd);

	if (errval) {
		//if either of the calls failed
		fprintf(stderr, FLFMT "tty_break could not set/clear break!\n", FL);
		return diag_iseterr(DIAG_ERR_GENERAL);
	}

	return 0;
}

/*
 * Routine to read a whole BR1 message
 * length of which depends on the first value received.
 * This also handles "error" messages (top bit of first value set)
 *
 * Returns length of received message, or TIMEOUT error, or BUSERROR
 * if the BR interface tells us theres a congested bus
 */
static int
br_getmsg(struct diag_l0_device *dl0d, uint8_t *dp, unsigned int timeout) {
	uint8_t firstbyte;
	size_t readlen;
	int rv;
	struct br_device *dev = dl0d->l0_int;

	DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
			FLFMT "link %p getmsg timeout %u\n", FL, (void *)dl0d, timeout);

	/*
	 * First read the 1st byte, using the supplied timeout
	 */
	rv = diag_tty_read(dev->tty_int, &firstbyte, 1, timeout);
	if (rv != 1) {
		DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
			FLFMT "link %p getmsg 1st byte timed out\n", FL, (void *)dl0d);

		return diag_ifwderr(rv);
	}

	/*
	 * Now read data. Maximum is 15 bytes.
	 */

	readlen = firstbyte & 0x0f;

	/*
	 * Reasonable timeout here as the interface told us how
	 * much data to expect, so it should arrive
	 */
	rv = diag_tty_read(dev->tty_int, dp, readlen, 100);
	if (rv != (int)readlen) {
		fprintf(stderr, FLFMT "br_getmsg error\n", FL);
		return diag_iseterr(DIAG_ERR_GENERAL);
	}

	DIAG_DBGMDATA(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V, dp, readlen,
			FLFMT "link %p getmsg read ctl 0x%X data:",
			FL, (void *)dl0d, firstbyte & 0xff);

	/*
	 * Message read complete, check error flag
	 * Top bit set means error, Bit 6 = VPW/PWM bus
	 * congestion (i.e retry).
	 */
	if (firstbyte & 0x80) { /* Error indicator */
		return diag_iseterr(DIAG_ERR_TIMEOUT);
	}

	if (firstbyte & 0x40) { /* VPW/PWM bus conflict, need to retry */
		return diag_iseterr(DIAG_ERR_BUSERROR);
	}

	if (readlen == 0) { /* Should never happen */
		return diag_iseterr(DIAG_ERR_TIMEOUT);
	}

	return (int) readlen;
}

/*
 * Safe write routine; return 0 on success
 */
static int
diag_l0_muleng_write(struct diag_l0_device *dl0d, const void *dp, size_t txlen)
{
	if (txlen <=0)
		return diag_iseterr(DIAG_ERR_BADLEN);

	if ( (diag_l0_debug & (DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA)) ==
			(DIAG_DEBUG_WRITE|DIAG_DEBUG_DATA) )
	{
		fprintf(stderr, FLFMT "device link %p sending to ME device: ",
			FL, (void *)dl0d);
		diag_data_dump(stderr, dp, txlen);
		fprintf(stderr, "\n");
	}

	/*
	 * And send it to the interface
	 */
	if (diag_tty_write(dl0d, dp, txlen) != (int) txlen) {
		fprintf(stderr, FLFMT "muleng_write error!!\n", FL);
		return diag_iseterr(DIAG_ERR_GENERAL);
	}

	return 0;
}

/*
 * Set/Clear DTR and RTS lines, as specified
 * on Win32 this can easily be done by calling EscapeCommFunction twice.
 * This takes around ~15-20us to do; probably with ~10 us skew between setting RTS and DTR.
 * If it proves to be a problem, it's possible to change both RTS and DTR at once by updating
 * the DCB and calling SetCommState. That call would take around 30-40us.
 * Note : passing 1 in dtr or rts means "set DTR/RTS", i.e. positive voltage.
 * ret 0 if ok
 */
int
diag_tty_control(struct diag_l0_device *dl0d,  unsigned int dtr, unsigned int rts)
{
    unsigned int escapefunc;
    struct tty_int *wti = (struct tty_int *)dl0d->tty_int;


    if (wti->fd == INVALID_HANDLE_VALUE) {
        fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
        return diag_iseterr(DIAG_ERR_GENERAL);
    }

    if (dtr)
        escapefunc=SETDTR;
    else
        escapefunc=CLRDTR;

    if (! EscapeCommFunction(wti->fd,escapefunc)) {
        fprintf(stderr, FLFMT "Could not change DTR: %s\n", FL, diag_os_geterr(0));
        return diag_iseterr(DIAG_ERR_GENERAL);
    }

    if (rts)
        escapefunc=SETRTS;
    else
        escapefunc=CLRRTS;

    if (! EscapeCommFunction(wti->fd,escapefunc)) {
        fprintf(stderr, FLFMT "Could not change RTS: %s\n", FL, diag_os_geterr(0));
        return diag_iseterr(DIAG_ERR_GENERAL);
    }

    return 0;
} //diag_tty_control

/*
 * Insert the L3 layer on top of the layer 2 connection
 *
 */
static int
diag_l3_vag_start(struct diag_l3_conn *d_l3_conn) {
	struct diag_l2_data l2data;
	struct diag_l2_conn *d_l2_conn;

	/* 5 baud init has been done, make sure we got the correct keybytes */
	d_l2_conn = d_l3_conn->d_l3l2_conn;

	(void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_GET_L2_DATA, (void *)&l2data);

	DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
		FLFMT "start L3 KB 0x%X 0x%X need 0x01 0x8A\n",
		FL, l2data.kb1, l2data.kb2);

	if (l2data.kb1 != 0x01) {
		return diag_iseterr(DIAG_ERR_WRONGKB);
	}
	if (l2data.kb2 != 0x8A) {
		return diag_iseterr(DIAG_ERR_WRONGKB);
	}

	/* OK, ISO 9141 keybytes are correct ! */

	/* Get the initial stuff the ECU tells us */

	return 0;
}

static int
diag_l0_br_write(struct diag_l0_device *dl0d, const void *dp, size_t txlen)
{
	if (txlen <=0)
		return diag_iseterr(DIAG_ERR_BADLEN);

	if (diag_tty_write(dl0d, dp, txlen) != (int) txlen) {
		fprintf(stderr, FLFMT "br_write error\n", FL);
		return diag_iseterr(DIAG_ERR_GENERAL);
	}

	return 0;
}

int
diag_l2_proto_raw_startcomms( struct diag_l2_conn *d_l2_conn,
UNUSED(flag_type flags),
unsigned int bitrate,
target_type target,
source_type source)
{
	int rv;
	struct diag_serial_settings set;

	set.speed = bitrate;
	set.databits = diag_databits_8;
	set.stopbits = diag_stopbits_1;
	set.parflag = diag_par_n;

	/* Set the speed as shown */
	rv=diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, &set);

	if (!rv)
		return diag_iseterr(DIAG_ERR_GENERAL);

	//set tgt and src address in d_l2_conn
	d_l2_conn->diag_l2_destaddr=target;
	d_l2_conn->diag_l2_srcaddr=source;

	return 0;
}

static int
diag_l0_br_write(struct diag_l0_device *dl0d, const void *dp, size_t txlen)
{
	ssize_t xferd;

	while ((size_t)(xferd = diag_tty_write(dl0d, dp, txlen)) != txlen) {
		if (xferd < 0) {
			/* error */
			if (errno != EINTR) {
				fprintf(stderr, FLFMT "write returned error %s.\n",
					FL, strerror(errno));
				return diag_iseterr(DIAG_ERR_GENERAL);
			}
			xferd = 0; /* Interrupted read, nothing transferred. */
			errno = 0;
		}
		/*
		 * Successfully wrote xferd bytes (or 0 bytes if EINTR),
		 * so increment the pointers and continue
		 */
		txlen -= (size_t) xferd;
		dp = (const void *)((const char *)dp + xferd);
	}
	return 0;
}

/*
 * Send the data
 *
 * - with VAG protocol this will sleep as the message is sent as each byte
 * is ack'ed by the far end
 *
 * - after each byte reset the send timer or the timeout routine may try
 * and send something
 *
 * 1st byte of message is command, followed by data
 */
static int
diag_l2_proto_vag_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg)
{
    int rv = 0;
    /*	int i;*/
    /*	int csum;*/
    /*	int len;*/
    /*	uint8_t buf[MAXRBUF];*/
    /*	int offset;*/
    //struct diag_l2_vag *dp;

    if (diag_l2_debug & DIAG_DEBUG_WRITE)
        fprintf(stderr,
                FLFMT "diag_l2_vag_send %p msg %p len %d called\n",
                FL, (void *)d_l2_conn, (void *)msg, msg->len);

    //dp = (struct diag_l2_vag *)d_l2_conn->diag_l2_proto_data;	//not used ?

#if xx
    rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, 0,
                       buf, len, d_l2_conn->diag_l2_p4min);
#endif

    return rv? diag_iseterr(rv):0 ;
}

/*
 * Do wakeup on the bus
 *
 * We do this by noting a wakeup needs to be done for the next packet for
 * fastinit, and doing slowinit now
 */
static int
diag_l0_muleng_initbus(struct diag_l0_device *dl0d, struct diag_l1_initbus_args *in)
{
	int rv = 0;
	struct diag_l0_muleng_device *dev;

	dev = (struct diag_l0_muleng_device *)dl0d->l0_int;

	if (!dev)
		return diag_iseterr(DIAG_ERR_GENERAL);

	if (diag_l0_debug & DIAG_DEBUG_IOCTL)
		fprintf(stderr, FLFMT "device link %p info %p initbus type %d proto %d\n",
			FL, (void *)dl0d, (void *)dev, in->type, dev->protocol);

	diag_tty_iflush(dl0d); /* Empty the receive buffer, wait for idle bus */

	if (in->type == DIAG_L1_INITBUS_5BAUD)
		rv = diag_l0_muleng_slowinit(dl0d, in, dev);
	else
	{
		/* Do wakeup on first TX */
		dev->dev_wakeup = in->type;
		dev->dev_state = MULENG_STATE_FASTSTART;
	}

	return rv;
}

/*
 * Get data (blocking, unless timeout is 0)
 * returns # of bytes read, or <0 if error.
 * XXX currently nothing handles the case of L0 returning 0 bytes read. Logically that could
 * only happen when requesting n bytes with a timeout of 0; otherwise DIAG_ERR_TIMEOUT will
 * generated.
 */
int
diag_l1_recv(struct diag_l0_device *dl0d,
	const char *subinterface, void *data, size_t len, unsigned int timeout) {
	int rv;
	if (!len) {
		return diag_iseterr(DIAG_ERR_BADLEN);
	}

	DIAG_DBGM(diag_l1_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
		FLFMT "_recv request len=%d, timeout=%u;",
		FL, (int)len, timeout);

	if (timeout == 0) {
		fprintf(stderr,
			FLFMT
			"Interesting : L1 read with timeout=0. Report this !\n",
			FL);
	}

	rv=diag_l0_recv(dl0d, subinterface, data, len, timeout);
	if (!rv) {
		fprintf(stderr, FLFMT "L0 returns with 0 bytes; returning TIMEOUT instead. Report this !\n", FL);
		return DIAG_ERR_TIMEOUT;
	}

	if (rv>0) {
		DIAG_DBGMDATA(diag_l1_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
			data, (size_t) rv, "got %d bytes; ",rv);
	}

	return rv;
}

/*
 * XXX All commands should probably have optional "init" hooks.
 */
int set_init(void)
{
	/* Reset parameters to defaults. */

	set_speed = 10400;	/* Comms speed; ECUs will probably send at 10416 bps (96us per bit) */
	set_testerid = 0xf1;	/* Our tester ID */
	set_addrtype = 1;	/* Use virtual addressing */
	set_destaddr = 0x33;	/* Dest ECU address */
	store_set_destaddr = set_destaddr;
	set_L1protocol = DIAG_L1_ISO9141;	/* L1 protocol type */
	set_L2protocol = DIAG_L2_PROT_ISO9141;	/* Protocol type */
	set_initmode = DIAG_L2_TYPE_FASTINIT ;

	set_display = 0;		/* English (1), or Metric (0) */

	set_vehicle = "ODBII";	/* Vehicle */
	set_ecu = "ODBII";	/* ECU name */

	set_interface_idx= DEFAULT_INTERFACE;
	set_interface = l0_names[DEFAULT_INTERFACE].code;	/* Default H/w interface to use */

	strncpy(set_subinterface,"/dev/null",sizeof(set_subinterface));
	printf( "%s: Interface set to default: %s on %s\n", progname, l0_names[set_interface_idx].longname, set_subinterface);

	if (diag_calloc(&set_simfile, strlen(DB_FILE)+1))
		return diag_iseterr(DIAG_ERR_GENERAL);	
	strcpy(set_simfile, DB_FILE);			//default simfile for use with CARSIM
	diag_l0_sim_setfile(set_simfile);
	
	return 0;
}

int
diag_l1_add_l0dev(const struct diag_l0 *l0dev) {
	int rv;

	struct diag_l0_node *last_node, *new_node;

	if (l0dev_list == NULL) {
		/*
		 * No devices yet, create the root.
		 */
		if ( (rv = diag_calloc(&l0dev_list, 1)) ) 
			return rv;

		l0dev_list->l0dev = l0dev;
		return 0;
	}

	for (last_node = l0dev_list; last_node != NULL; last_node = last_node->next)
		if (last_node->l0dev == l0dev)
			return diag_iseterr(DIAG_ERR_GENERAL);	/* Already there. */

	if ( (rv = diag_calloc(&new_node, 1)) ) 
		return rv;

	for (last_node = l0dev_list; last_node->next != NULL; last_node = last_node->next)
		/* Search for the next-to-last node */;

	new_node->l0dev = l0dev;
	last_node->next = new_node;

	return 0;
}

int diag_tty_break(struct diag_l0_device *dl0d, const int ms)
{
	HANDLE hd;
	long h;

	/*
	 * I'm going through this convoluted two-step conversion
	 * to avoid compiler warnings:
	 */

	h = get_osfhandle(dl0d->fd);
	hd = (HANDLE)h;

	if (tcdrain(dl0d->fd)) {
			fprintf(stderr, FLFMT "tcdrain returned %s.\n",
				FL, strerror(errno));
			return diag_iseterr(DIAG_ERR_GENERAL);
		}

	SetCommBreak(hd);
	diag_os_millisleep(ms);
	ClearCommBreak(hd);

	return 0;
}

/*
 * Send a load of data
 *
 * Returns 0 on success, -1 on failure
 *
 * This routine will do a fastinit if needed, but all 5 baud inits
 * will have been done by the slowinit() code
 */
static int
br_send(struct diag_l0_device *dl0d,
UNUSED(const char *subinterface),
const void *data, size_t len) {
	int rv = 0;

	struct br_device *dev;

	dev = (struct br_device *)dl0d->l0_int;

	if (len <= 0) {
		return diag_iseterr(DIAG_ERR_BADLEN);
	}

	DIAG_DBGMDATA(diag_l0_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V, data, len,
			FLFMT "device link %p send %ld bytes protocol %d state %d: ",
			FL, (void *)dl0d, (long)len, dev->protocol, dev->dev_state);

	/*
	 * Special handling for fastinit, we need to collect up the
	 * bytes of the StartComms message sent by the upper layer
	 * when we have a whole message we can then send the request
	 * as part of a special initialisation type
	 */
	if (dev->dev_state == BR_STATE_KWP_FASTINIT) {
		uint8_t outbuf[6];
		if (dev->dev_txlen < 5) {
			memcpy(&dev->dev_txbuf[dev->dev_txlen], data, len);
			dev->dev_txlen += len;
			rv = 0;
		}

		if (dev->dev_txlen >= 5) {
			/*
			 * Startcomms request is 5 bytes long - we have
			 * 5 bytes, so now we should send the initialisation
			 */
			outbuf[0] = 0x03;
			memcpy(&outbuf[1], dev->dev_txbuf, 5);
			rv = br_writemsg(dl0d, BR_WRTYPE_INIT,
				outbuf, 6);
			/* Stays in FASTINIT state until first read */
		}
	} else {
		/*
		 * Now, keep a copy of the data, and set the framenr to 1
		 * This means the receive code will resend the request if it
		 * wants to get a frame number 2 or 3 or whatever
		 */
		memcpy(dev->dev_rxbuf, data, len);
		dev->dev_txlen = len;
		dev->dev_framenr = 1;

		/* And now encapsulate and send the data */
		rv = br_writemsg(dl0d, BR_WRTYPE_DATA, data, len);
	}

	return rv;
}