+ * Write status Register and configuration register with 2 bytes
+ * The first byte will be written to the status register, while the
+ * second byte will be written to the configuration register.
+ * Return negative if error occured.
+ */
+static int write_sr_cr(struct m25p *flash, u16 val)
+{
+ flash->command[0] = OPCODE_WRSR;
+ flash->command[1] = val & 0xff;
+ flash->command[2] = (val >> 8);
+
+ return spi_write(flash->spi, flash->command, 3);
+}
+
+static int macronix_quad_enable(struct m25p *flash)
+{
+ int ret, val;
+ u8 cmd[2];
+ cmd[0] = OPCODE_WRSR;
+
+ val = read_sr(flash);
+ cmd[1] = val | SR_QUAD_EN_MX;
+ write_enable(flash);
+
+ spi_write(flash->spi, &cmd, 2);
+
+ if (wait_till_ready(flash))
+ return 1;
+
+ ret = read_sr(flash);
+ if (!(ret > 0 && (ret & SR_QUAD_EN_MX))) {
+ dev_err(&flash->spi->dev, "Macronix Quad bit not set\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int spansion_quad_enable(struct m25p *flash)
+{
+ int ret;
+ int quad_en = CR_QUAD_EN_SPAN << 8;
+
+ write_enable(flash);
+
+ ret = write_sr_cr(flash, quad_en);
+ if (ret < 0) {
+ dev_err(&flash->spi->dev,
+ "error while writing configuration register\n");
+ return -EINVAL;
+ }
+
+ /* read back and check it */
+ ret = read_cr(flash);
+ if (!(ret > 0 && (ret & CR_QUAD_EN_SPAN))) {
+ dev_err(&flash->spi->dev, "Spansion Quad bit not set\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int set_quad_mode(struct m25p *flash, u32 jedec_id)
+{
+ int status;
+
+ switch (JEDEC_MFR(jedec_id)) {
+ case CFI_MFR_MACRONIX:
+ status = macronix_quad_enable(flash);
+ if (status) {
+ dev_err(&flash->spi->dev,
+ "Macronix quad-read not enabled\n");
+ return -EINVAL;
+ }
+ return status;
+ default:
+ status = spansion_quad_enable(flash);
+ if (status) {
+ dev_err(&flash->spi->dev,
+ "Spansion quad-read not enabled\n");
+ return -EINVAL;
+ }
+ return status;
+ }
+}
+
+/*