+ struct sflash *sfl;
+ int ret = 0;
+ bool is4712b0;
+ uint32 page, byte, mask;
+ osl_t *osh;
+
+ ASSERT (sbh);
+
+ osh = sb_osh (sbh);
+
+ if (!len)
+ return 0;
+
+ if ((offset + len) > sflash.size)
+ return -22;
+
+ sfl = &sflash;
+ switch (sfl->type)
+ {
+ case SFLASH_ST:
+ is4712b0 = (sbh->chip == BCM4712_CHIP_ID) && (sbh->chiprev == 3);
+ /* Enable writes */
+ sflash_cmd (osh, cc, SFLASH_ST_WREN);
+ if (is4712b0)
+ {
+ mask = 1 << 14;
+ W_REG (osh, &cc->flashaddress, offset);
+ W_REG (osh, &cc->flashdata, *buf++);
+ /* Set chip select */
+ OR_REG (osh, &cc->gpioout, mask);
+ /* Issue a page program with the first byte */
+ sflash_cmd (osh, cc, SFLASH_ST_PP);
+ ret = 1;
+ offset++;
+ len--;
+ while (len > 0)
+ {
+ if ((offset & 255) == 0)
+ {
+ /* Page boundary, drop cs and return */
+ AND_REG (osh, &cc->gpioout, ~mask);
+ if (!sflash_poll (sbh, cc, offset))
+ {
+ /* Flash rejected command */
+ return -11;
+ }
+ return ret;
+ }
+ else
+ {
+ /* Write single byte */
+ sflash_cmd (osh, cc, *buf++);
+ }
+ ret++;
+ offset++;
+ len--;
+ }
+ /* All done, drop cs if needed */
+ if ((offset & 255) != 1)
+ {
+ /* Drop cs */
+ AND_REG (osh, &cc->gpioout, ~mask);
+ if (!sflash_poll (sbh, cc, offset))
+ {
+ /* Flash rejected command */
+ return -12;
+ }
+ }
+ }
+ else if (sbh->ccrev >= 20)
+ {
+ W_REG (NULL, &cc->flashaddress, offset);
+ W_REG (NULL, &cc->flashdata, *buf++);
+ /* Issue a page program with CSA bit set */
+ sflash_cmd (osh, cc, SFLASH_ST_CSA | SFLASH_ST_PP);
+ ret = 1;
+ offset++;
+ len--;
+ while (len > 0)
+ {
+ if ((offset & 255) == 0)
+ {
+ /* Page boundary, poll droping cs and return */
+ W_REG (NULL, &cc->flashcontrol, 0);
+ if (!sflash_poll (sbh, cc, offset))
+ {
+ /* Flash rejected command */
+ return -11;
+ }
+ return ret;
+ }
+ else
+ {
+ /* Write single byte */
+ sflash_cmd (osh, cc, SFLASH_ST_CSA | *buf++);
+ }
+ ret++;
+ offset++;
+ len--;
+ }
+ /* All done, drop cs if needed */
+ if ((offset & 255) != 1)
+ {
+ /* Drop cs, poll */
+ W_REG (NULL, &cc->flashcontrol, 0);
+ if (!sflash_poll (sbh, cc, offset))
+ {
+ /* Flash rejected command */
+ return -12;
+ }
+ }
+ }
+ else
+ {
+ ret = 1;
+ W_REG (osh, &cc->flashaddress, offset);
+ W_REG (osh, &cc->flashdata, *buf);
+ /* Page program */
+ sflash_cmd (osh, cc, SFLASH_ST_PP);
+ }
+ break;
+ case SFLASH_AT:
+ mask = sfl->blocksize - 1;
+ page = (offset & ~mask) << 1;
+ byte = offset & mask;
+ /* Read main memory page into buffer 1 */
+ if (byte || (len < sfl->blocksize))
+ {
+ W_REG (osh, &cc->flashaddress, page);
+ sflash_cmd (osh, cc, SFLASH_AT_BUF1_LOAD);
+ /* 250 us for AT45DB321B */
+ SPINWAIT (sflash_poll (sbh, cc, offset), 1000);
+ ASSERT (!sflash_poll (sbh, cc, offset));