PACKET_ptr findNextFramePacketId (FRAME_ptr frame_ptr, PACKET_ptr previousPacket_ptr, UINT packetId)
{
/*
** BEWARE !!! No check on valid frame once we're past the first packet.
*/
/*
** Skip the "previous" packet
*/
PACKET_ptr testPacket_ptr = findNextFramePacket(frame_ptr, previousPacket_ptr);
/*
** Now iterate until we fail to get enxt packet or find the one we want
*/
while ( (testPacket_ptr != ptrFailure) && (getPacketId(testPacket_ptr) != packetId) )
testPacket_ptr = findNextFramePacket (frame_ptr, testPacket_ptr);
return testPacket_ptr;
}
/*
** findFramePacketID
**
** Find a packet with a specified packet ID in a frame starting at the
** packet in the frame. If no packet with the ID is found ptrFailure is returned.
**
*/
PACKET_ptr findFramePacketId (FRAME_ptr frame_ptr, UINT packetId)
{
PACKET_ptr testPacket_ptr;
/*
** Check to make sure we have a good frame
*/
if (!validFrameHdr (frame_ptr)) {
return ptrFailure;
}
/*
** Point to first packet
*/
testPacket_ptr = (PACKET_ptr) findFrameDataStart (frame_ptr);
/*
** Now iterate until we fail to get enxt packet or find the one we want
*/
while ( (testPacket_ptr != ptrFailure) && (getPacketId(testPacket_ptr) != packetId) )
testPacket_ptr = findNextFramePacket (frame_ptr, testPacket_ptr);
return testPacket_ptr;
}
/*
* 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)
{
struct device *dev = &garmin_data_p->port->dev;
const unsigned char *src;
unsigned char *dst;
int pktid = 0;
int datalen = 0;
int cksum = 0;
int i = 0;
int k;
dev_dbg(dev, "%s - state %d - %d bytes.\n", __func__,
garmin_data_p->state, count);
k = garmin_data_p->outsize;
if ((k+count) > GPS_OUT_BUFSIZ) {
dev_dbg(dev, "packet too large\n");
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;
}
dev_dbg(dev, "%s - %d bytes in buffer, %d bytes in pkt.\n", __func__, k, i);
/* garmin_data_p->outbuffer now contains a complete packet */
usb_serial_debug_data(&garmin_data_p->port->dev, __func__, k,
garmin_data_p->outbuffer);
garmin_data_p->outsize = 0;
if (GARMIN_LAYERID_APPL != getLayerId(garmin_data_p->outbuffer)) {
dev_dbg(dev, "not an application packet (%d)\n",
getLayerId(garmin_data_p->outbuffer));
return -1;
}
if (pktid > 255) {
dev_dbg(dev, "packet-id %d too large\n", pktid);
return -2;
}
if (datalen > 255) {
dev_dbg(dev, "packet-size %d too large\n", 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;
}
static int garmin_write(struct tty_struct *tty, struct usb_serial_port *port,
const unsigned char *buf, int count)
{
struct device *dev = &port->dev;
int pktid, pktsiz, len;
struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
__le32 *privpkt = (__le32 *)garmin_data_p->privpkt;
usb_serial_debug_data(dev, __func__, count, buf);
if (garmin_data_p->state == STATE_RESET)
return -EIO;
/* check for our private packets */
if (count >= GARMIN_PKTHDR_LENGTH) {
len = PRIVPKTSIZ;
if (count < len)
len = count;
memcpy(garmin_data_p->privpkt, buf, len);
pktsiz = getDataLength(garmin_data_p->privpkt);
pktid = getPacketId(garmin_data_p->privpkt);
if (count == (GARMIN_PKTHDR_LENGTH+pktsiz)
&& GARMIN_LAYERID_PRIVATE ==
getLayerId(garmin_data_p->privpkt)) {
dev_dbg(dev, "%s - processing private request %d\n",
__func__, pktid);
/* drop all unfinished transfers */
garmin_clear(garmin_data_p);
switch (pktid) {
case PRIV_PKTID_SET_MODE:
if (pktsiz != 4)
return -EINVPKT;
garmin_data_p->mode = __le32_to_cpu(privpkt[3]);
dev_dbg(dev, "%s - mode set to %d\n",
__func__, garmin_data_p->mode);
break;
case PRIV_PKTID_INFO_REQ:
priv_status_resp(port);
break;
case PRIV_PKTID_RESET_REQ:
process_resetdev_request(port);
break;
case PRIV_PKTID_SET_DEF_MODE:
if (pktsiz != 4)
return -EINVPKT;
initial_mode = __le32_to_cpu(privpkt[3]);
dev_dbg(dev, "%s - initial_mode set to %d\n",
__func__,
garmin_data_p->mode);
break;
}
return count;
}
}
if (garmin_data_p->mode == MODE_GARMIN_SERIAL) {
return gsp_receive(garmin_data_p, buf, count);
} else { /* MODE_NATIVE */
return nat_receive(garmin_data_p, buf, count);
}
}
