garmin_gps.c

LINUX 2.6.17.4的源码

		__FUNCTION__, dleSeen, skip, size, count);

	if (size == 0) {
		size = GSP_INITIAL_OFFSET;
	}

	while (offs < count) {

		data = *(buf+offs);
		offs ++;

		if (data == DLE) {
			if (skip) { /* start of a new pkt */
				skip = 0;
				size = GSP_INITIAL_OFFSET;
				dleSeen = 1;
			} else if (dleSeen) {
				dest[size++] = data;
				dleSeen = 0;
			} else {
				dleSeen = 1;
			}
		} else if (data == ETX) {
			if (dleSeen) {
				/* packet complete */

				data = dest[GSP_INITIAL_OFFSET];

				if (data == ACK) {
					ack_or_nak_seen = ACK;
					dbg("ACK packet complete.");
				} else if (data == NAK) {
					ack_or_nak_seen = NAK;
					dbg("NAK packet complete.");
				} else {
					dbg("packet complete "
						        "- id=0x%X.",
						        0xFF & data);
					gsp_rec_packet(garmin_data_p, size);
				}

				skip = 1;
				size = GSP_INITIAL_OFFSET;
				dleSeen = 0;
			} else {
				dest[size++] = data;
			}
		} else if (!skip) {

			if (dleSeen) {
				dbg("non-masked DLE at %d - restarting", i);
				size = GSP_INITIAL_OFFSET;
				dleSeen = 0;
			}

			dest[size++] = data;
		}

		if (size >= GPS_IN_BUFSIZ) {
			dbg("%s - packet too large.", __FUNCTION__);
			skip = 1;
			size = GSP_INITIAL_OFFSET;
			dleSeen = 0;
		}
	}

	garmin_data_p->insize = size;

	// copy flags back to structure
	if (skip)
		garmin_data_p->flags |= FLAGS_GSP_SKIP;
	else
		garmin_data_p->flags &= ~FLAGS_GSP_SKIP;

	if (dleSeen)
		garmin_data_p->flags |= FLAGS_GSP_DLESEEN;
	else
		garmin_data_p->flags &= ~FLAGS_GSP_DLESEEN;

	if (ack_or_nak_seen) {
		garmin_data_p->state = STATE_GSP_WAIT_DATA;
		gsp_next_packet(garmin_data_p);
	}

	return count;
}




/*
 * Sends a usb packet to the tty
 *
 * Assumes, that all packages and at an usb-packet boundary.
 *
 * return <0 on error, 0 if packet is incomplete or > 0 if packet was sent
 */
static int gsp_send(struct garmin_data * garmin_data_p,
		    const unsigned char *buf, int count)
{
	const unsigned char *src;
	unsigned char *dst;
	int pktid = 0;
	int datalen = 0;
	int cksum = 0;
	int i=0;
	int k;

	dbg("%s - state %d - %d bytes.", __FUNCTION__,
	         garmin_data_p->state, count);

	k = garmin_data_p->outsize;
	if ((k+count) > GPS_OUT_BUFSIZ) {
		dbg("packet too large");
		garmin_data_p->outsize = 0;
		return -4;
	}

	memcpy(garmin_data_p->outbuffer+k, buf, count);
	k += count;
	garmin_data_p->outsize = k;

	if (k >= GARMIN_PKTHDR_LENGTH) {
		pktid  = getPacketId(garmin_data_p->outbuffer);
		datalen= getDataLength(garmin_data_p->outbuffer);
		i = GARMIN_PKTHDR_LENGTH + datalen;
		if (k < i)
			return 0;
	} else {
		return 0;
	}

	dbg("%s - %d bytes in buffer, %d bytes in pkt.", __FUNCTION__,
	         k, i);

	/* garmin_data_p->outbuffer now contains a complete packet */

	usb_serial_debug_data(debug, &garmin_data_p->port->dev,
		                   __FUNCTION__, k, garmin_data_p->outbuffer);

	garmin_data_p->outsize = 0;

	if (GARMIN_LAYERID_APPL != getLayerId(garmin_data_p->outbuffer)) {
		dbg("not an application packet (%d)", 
		        getLayerId(garmin_data_p->outbuffer));
		return -1;
	}

	if (pktid > 255) {
		dbg("packet-id %d too large", pktid);
		return -2;
	}

	if (datalen > 255) {
		dbg("packet-size %d too large", datalen);
		return -3;
	}

	/* the serial protocol should be able to handle this packet */

	k = 0;
	src = garmin_data_p->outbuffer+GARMIN_PKTHDR_LENGTH;
	for (i=0; i<datalen; i++) {
		if (*src++ == DLE)
			k++;
	}

	src = garmin_data_p->outbuffer+GARMIN_PKTHDR_LENGTH;
	if (k > (GARMIN_PKTHDR_LENGTH-2)) {
		/* can't add stuffing DLEs in place, move data to end 
                   of buffer ... */
		dst = garmin_data_p->outbuffer+GPS_OUT_BUFSIZ-datalen;
		memcpy(dst, src, datalen);
		src = dst;
	}

	dst = garmin_data_p->outbuffer;

	*dst++ = DLE;
	*dst++ = pktid;
	cksum += pktid;
	*dst++ = datalen;
	cksum += datalen;
	if (datalen == DLE)
		*dst++ = DLE;

	for (i=0; i<datalen; i++) {
		__u8 c = *src++;
		*dst++ = c;
		cksum += c;
		if (c == DLE)
			*dst++ = DLE;
	}
		
	cksum = 0xFF & -cksum;
	*dst++ = cksum;
	if (cksum == DLE)
		*dst++ = DLE;
	*dst++ = DLE;
	*dst++ = ETX;

	i = dst-garmin_data_p->outbuffer;

	send_to_tty(garmin_data_p->port, garmin_data_p->outbuffer, i);

	garmin_data_p->pkt_id = pktid;
	garmin_data_p->state  = STATE_WAIT_TTY_ACK;

	return i;
}





/*
 * Process the next pending data packet - if there is one
 */
static void gsp_next_packet(struct garmin_data * garmin_data_p)
{
	struct garmin_packet *pkt = NULL;

	while ((pkt = pkt_pop(garmin_data_p)) != NULL) {
		dbg("%s - next pkt: %d", __FUNCTION__, pkt->seq);
		if (gsp_send(garmin_data_p, pkt->data, pkt->size) > 0) {
			kfree(pkt);
			return;
		}
		kfree(pkt);
	}
}




/******************************************************************************
 * garmin native mode
 ******************************************************************************/


/*
 * Called for data received from tty
 *
 * The input data is expected to be in garmin usb-packet format.
 *
 * buf contains the data read, it may span more than one packet
 * or even incomplete packets
 */
static int nat_receive(struct garmin_data * garmin_data_p,
                       const unsigned char *buf, int count)
{
	__u8 * dest;
	int offs = 0;
	int result = count;
	int len;

	while (offs < count) {
		// if buffer contains header, copy rest of data
		if (garmin_data_p->insize >= GARMIN_PKTHDR_LENGTH)
			len = GARMIN_PKTHDR_LENGTH
			      +getDataLength(garmin_data_p->inbuffer);
		else
			len = GARMIN_PKTHDR_LENGTH;

		if (len >= GPS_IN_BUFSIZ) {
			/* seem to be an invalid packet, ignore rest of input */
			dbg("%s - packet size too large: %d",
			        __FUNCTION__, len);
			garmin_data_p->insize = 0;
			count = 0;
			result = -EINVPKT;
		} else {
			len -= garmin_data_p->insize;
			if (len > (count-offs))
				len = (count-offs);
			if (len > 0) {
				dest = garmin_data_p->inbuffer
				       	+garmin_data_p->insize;
				memcpy(dest, buf+offs, len);
				garmin_data_p->insize += len;
				offs += len;
			}
		}

		/* do we have a complete packet ? */
		if (garmin_data_p->insize >= GARMIN_PKTHDR_LENGTH) {
			len = GARMIN_PKTHDR_LENGTH+
			   getDataLength(garmin_data_p->inbuffer);
			if (garmin_data_p->insize >= len) {
				garmin_write_bulk (garmin_data_p->port,
				                   garmin_data_p->inbuffer,
				                   len);
				garmin_data_p->insize = 0;

				/* if this was an abort-transfer command,
				   flush all queued data. */
				if (isAbortTrfCmnd(garmin_data_p->inbuffer)) {
					garmin_data_p->flags |= FLAGS_DROP_DATA;
					pkt_clear(garmin_data_p);
				}
			}
		}
	}
	return result;
}


/******************************************************************************
 * private packets
 ******************************************************************************/

static void priv_status_resp(struct usb_serial_port *port)
{
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);
	__le32 *pkt = (__le32 *)garmin_data_p->privpkt;

	pkt[0] = __cpu_to_le32(GARMIN_LAYERID_PRIVATE);
	pkt[1] = __cpu_to_le32(PRIV_PKTID_INFO_RESP);
	pkt[2] = __cpu_to_le32(12);
	pkt[3] = __cpu_to_le32(VERSION_MAJOR << 16 | VERSION_MINOR);
	pkt[4] = __cpu_to_le32(garmin_data_p->mode);
	pkt[5] = __cpu_to_le32(garmin_data_p->serial_num);

	send_to_tty(port, (__u8*)pkt, 6*4);
}


/******************************************************************************
 * Garmin specific driver functions
 ******************************************************************************/

static int process_resetdev_request(struct usb_serial_port *port)
{
	int status;
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);

	garmin_data_p->flags &= ~(CLEAR_HALT_REQUIRED);
	garmin_data_p->state = STATE_RESET;
	garmin_data_p->serial_num = 0;

	usb_kill_urb (port->interrupt_in_urb);
	dbg("%s - usb_reset_device", __FUNCTION__ );
	status = usb_reset_device(port->serial->dev);
	if (status)
		dbg("%s - usb_reset_device failed: %d",
			__FUNCTION__, status);
	return status;
}



/*
 * clear all cached data
 */
static int garmin_clear(struct garmin_data * garmin_data_p)
{
	int status = 0;

	struct usb_serial_port *port = garmin_data_p->port;

	if (port != NULL && garmin_data_p->flags & FLAGS_APP_RESP_SEEN) {
		/* send a terminate command */
		status = garmin_write_bulk(port, GARMIN_STOP_TRANSFER_REQ,
		                           sizeof(GARMIN_STOP_TRANSFER_REQ));
	}

	/* flush all queued data */
	pkt_clear(garmin_data_p);

	garmin_data_p->insize = 0;
	garmin_data_p->outsize = 0;

	return status;
}






static int garmin_init_session(struct usb_serial_port *port)
{
	struct usb_serial *serial = port->serial;
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);
	int status = 0;

	if (status == 0) {
		usb_kill_urb (port->interrupt_in_urb);

		dbg("%s - adding interrupt input", __FUNCTION__);
		port->interrupt_in_urb->dev = serial->dev;
		status = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
		if (status)
			dev_err(&serial->dev->dev,
			        "%s - failed submitting interrupt urb,"
				" error %d\n",
			        __FUNCTION__, status);
	}

	if (status == 0) {
		dbg("%s - starting session ...", __FUNCTION__);
		garmin_data_p->state = STATE_ACTIVE;
		status = garmin_write_bulk(port, GARMIN_START_SESSION_REQ,
		                           sizeof(GARMIN_START_SESSION_REQ));

		if (status >= 0) {

			garmin_data_p->ignorePkts++;

			/* not needed, but the win32 driver does it too ... */
			status = garmin_write_bulk(port,
						   GARMIN_START_SESSION_REQ2,
			                           sizeof(GARMIN_START_SESSION_REQ2));
			if (status >= 0) {
				status = 0;
				garmin_data_p->ignorePkts++;
			}
		}
	}

	return status;
}





static int garmin_open (struct usb_serial_port *port, struct file *filp)
{
	int status = 0;
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);

	dbg("%s - port %d", __FUNCTION__, port->number);

	/*
	 * Force low_latency on so that our tty_push actually forces the data
	 * through, otherwise it is scheduled, and with high data rates (like
	 * with OHCI) data can get lost.
	 */
	if (port->tty)
		port->tty->low_latency = 1;

	garmin_data_p->mode  = initial_mode;
	garmin_data_p->count = 0;
	garmin_data_p->flags = 0;

	/* shutdown any bulk reads that might be going on */
	usb_kill_urb (port->write_urb);
	usb_kill_urb (port->read_urb);

	if (garmin_data_p->state == STATE_RESET) {
		status = garmin_init_session(port);
	}

	garmin_data_p->state = STATE_ACTIVE;

	return status;
}


static void garmin_close (struct usb_serial_port *port, struct file * filp)
{
	struct usb_serial *serial = port->serial;
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);

	dbg("%s - port %d - mode=%d state=%d flags=0x%X", __FUNCTION__,
		port->number, garmin_data_p->mode,
		garmin_data_p->state, garmin_data_p->flags);

	if (!serial)
		return;

	garmin_clear(garmin_data_p);

	/* shutdown our urbs */
	usb_kill_urb (port->read_urb);
	usb_kill_urb (port->write_urb);

	if (noResponseFromAppLayer(garmin_data_p) ||
	    ((garmin_data_p->flags & CLEAR_HALT_REQUIRED) != 0)) {
		process_resetdev_request(port);
		garmin_data_p->state = STATE_RESET;
	} else {
		garmin_data_p->state = STATE_DISCONNECTED;
	}
}


static void garmin_write_bulk_callback (struct urb *urb, struct pt_regs *regs)
{
	struct usb_serial_port *port = (struct usb_serial_port *)urb->context;
	struct garmin_data * garmin_data_p = usb_get_serial_port_data(port);

	/* free up the transfer buffer, as usb_free_urb() does not do this */
	kfree (urb->transfer_buffer);

	dbg("%s - port %d", __FUNCTION__, port->number);

	if (urb->status) {
		dbg("%s - nonzero write bulk status received: %d",
			__FUNCTION__, urb->status);
		garmin_data_p->flags |= CLEAR_HALT_REQUIRED;
	}

	schedule_work(&port->work);
}


static int garmin_write_bulk (struct usb_serial_port *port,
                              const unsigned char *buf, int count)