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); } }