+/* Read up to count-1 bytes of device id. Terminate buffer with
+ * '\0'. Buffer begins with two Device ID length bytes as given by
+ * device. */
+static ssize_t parport_read_device_id (struct parport *port, char *buffer,
+ size_t count)
+{
+ unsigned char length[2];
+ unsigned lelen, belen;
+ size_t idlens[4];
+ unsigned numidlens;
+ unsigned current_idlen;
+ ssize_t retval;
+ size_t len;
+
+ /* First two bytes are MSB,LSB of inclusive length. */
+ retval = parport_read (port, length, 2);
+
+ if (retval < 0)
+ return retval;
+ if (retval != 2)
+ return -EIO;
+
+ if (count < 2)
+ return 0;
+ memcpy(buffer, length, 2);
+ len = 2;
+
+ /* Some devices wrongly send LE length, and some send it two
+ * bytes short. Construct a sorted array of lengths to try. */
+ belen = (length[0] << 8) + length[1];
+ lelen = (length[1] << 8) + length[0];
+ idlens[0] = min(belen, lelen);
+ idlens[1] = idlens[0]+2;
+ if (belen != lelen) {
+ int off = 2;
+ /* Don't try lenghts of 0x100 and 0x200 as 1 and 2 */
+ if (idlens[0] <= 2)
+ off = 0;
+ idlens[off] = max(belen, lelen);
+ idlens[off+1] = idlens[off]+2;
+ numidlens = off+2;
+ }
+ else {
+ /* Some devices don't truly implement Device ID, but
+ * just return constant nibble forever. This catches
+ * also those cases. */
+ if (idlens[0] == 0 || idlens[0] > 0xFFF) {
+ printk (KERN_DEBUG "%s: reported broken Device ID"
+ " length of %#zX bytes\n",
+ port->name, idlens[0]);
+ return -EIO;
+ }
+ numidlens = 2;
+ }
+
+ /* Try to respect the given ID length despite all the bugs in
+ * the ID length. Read according to shortest possible ID
+ * first. */
+ for (current_idlen = 0; current_idlen < numidlens; ++current_idlen) {
+ size_t idlen = idlens[current_idlen];
+ if (idlen+1 >= count)
+ break;
+
+ retval = parport_read (port, buffer+len, idlen-len);
+
+ if (retval < 0)
+ return retval;
+ len += retval;
+
+ if (port->physport->ieee1284.phase != IEEE1284_PH_HBUSY_DAVAIL) {
+ if (belen != len) {
+ printk (KERN_DEBUG "%s: Device ID was %zd bytes"
+ " while device told it would be %d"
+ " bytes\n",
+ port->name, len, belen);
+ }
+ goto done;
+ }
+
+ /* This might end reading the Device ID too
+ * soon. Hopefully the needed fields were already in
+ * the first 256 bytes or so that we must have read so
+ * far. */
+ if (buffer[len-1] == ';') {
+ printk (KERN_DEBUG "%s: Device ID reading stopped"
+ " before device told data not available. "
+ "Current idlen %u of %u, len bytes %02X %02X\n",
+ port->name, current_idlen, numidlens,
+ length[0], length[1]);
+ goto done;
+ }
+ }
+ if (current_idlen < numidlens) {
+ /* Buffer not large enough, read to end of buffer. */
+ size_t idlen, len2;
+ if (len+1 < count) {
+ retval = parport_read (port, buffer+len, count-len-1);
+ if (retval < 0)
+ return retval;
+ len += retval;
+ }
+ /* Read the whole ID since some devices would not
+ * otherwise give back the Device ID from beginning
+ * next time when asked. */
+ idlen = idlens[current_idlen];
+ len2 = len;
+ while(len2 < idlen && retval > 0) {
+ char tmp[4];
+ retval = parport_read (port, tmp,
+ min(sizeof tmp, idlen-len2));
+ if (retval < 0)
+ return retval;
+ len2 += retval;
+ }
+ }
+ /* In addition, there are broken devices out there that don't
+ even finish off with a semi-colon. We do not need to care
+ about those at this time. */
+ done:
+ buffer[len] = '\0';
+ return len;
+}
+