parport: parport_pc: Introduce intel_bug_present function.

Put the code to check present of the Intel bug from parport_EPP_supported
into new intel_bug_present function. The later also return ECR register
to the state it has before function call.

Signed-off-by: Matwey V. Kornilov <matwey@sai.msu.ru>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

1 files changed, 26 insertions, 12 deletions

diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index 76ee7750bc5e..fedc06bed18d 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -1702,6 +1702,30 @@ static int parport_ECP_supported(struct parport *pb)
 }
 #endif
 
+static int intel_bug_present(struct parport *pb)
+{
+        const struct parport_pc_private *priv = pb->private_data;
+        int bug_present = 0;
+
+        if (priv->ecr) {
+                /* store value of ECR */
+                unsigned char ecr = inb(ECONTROL(pb));
+                unsigned char i;
+                for (i = 0x00; i < 0x80; i += 0x20) {
+                        ECR_WRITE(pb, i);
+                        if (clear_epp_timeout(pb)) {
+                                /* Phony EPP in ECP. */
+                                bug_present = 1;
+                                break;
+                        }
+                }
+                /* return ECR into the inital state */
+                ECR_WRITE(pb, ecr);
+        }
+
+        return bug_present;
+}
+
 static int parport_ECPPS2_supported(struct parport *pb)
 {
         const struct parport_pc_private *priv = pb->private_data;
@@ -1722,8 +1746,6 @@ static int parport_ECPPS2_supported(struct parport *pb)
 
 static int parport_EPP_supported(struct parport *pb)
 {
-        const struct parport_pc_private *priv = pb->private_data;
-
         /*
          * Theory:
          *      Bit 0 of STR is the EPP timeout bit, this bit is 0
@@ -1742,16 +1764,8 @@ static int parport_EPP_supported(struct parport *pb)
                 return 0;  /* No way to clear timeout */
 
         /* Check for Intel bug. */
-        if (priv->ecr) {
-                unsigned char i;
-                for (i = 0x00; i < 0x80; i += 0x20) {
-                        ECR_WRITE(pb, i);
-                        if (clear_epp_timeout(pb)) {
-                                /* Phony EPP in ECP. */
-                                return 0;
-                        }
-                }
-        }
+        if (intel_bug_present(pb))
+                return 0;
 
         pb->modes |= PARPORT_MODE_EPP;