+/*
+ * TODO: the two write funcs (_p2/_at45) should get unified ...
+ */
+static int dataflash_write_p2(struct spi_flash *flash,
+ u32 offset, size_t len, const void *buf)
+{
+ struct atmel_spi_flash *asf = to_atmel_spi_flash(flash);
+ unsigned long page_size;
+ u32 addr = offset;
+ size_t chunk_len;
+ size_t actual;
+ int ret;
+ u8 cmd[4];
+
+ /*
+ * TODO: This function currently uses only page buffer #1. We can
+ * speed this up by using both buffers and loading one buffer while
+ * the other is being programmed into main memory.
+ */
+
+ page_size = (1 << asf->params->l2_page_size);
+
+ ret = spi_claim_bus(flash->spi);
+ if (ret) {
+ debug("SF: Unable to claim SPI bus\n");
+ return ret;
+ }
+
+ for (actual = 0; actual < len; actual += chunk_len) {
+ chunk_len = min(len - actual, page_size - (addr % page_size));
+
+ /* Use the same address bits for both commands */
+ cmd[0] = CMD_AT45_LOAD_BUF1;
+ cmd[1] = addr >> 16;
+ cmd[2] = addr >> 8;
+ cmd[3] = addr;
+
+ ret = spi_flash_cmd_write(flash->spi, cmd, 4,
+ buf + actual, chunk_len);
+ if (ret < 0) {
+ debug("SF: Loading AT45 buffer failed\n");
+ goto out;
+ }
+
+ cmd[0] = CMD_AT45_PROG_BUF1;
+ ret = spi_flash_cmd_write(flash->spi, cmd, 4, NULL, 0);
+ if (ret < 0) {
+ debug("SF: AT45 page programming failed\n");
+ goto out;
+ }
+
+ ret = at45_wait_ready(flash, SPI_FLASH_PROG_TIMEOUT);
+ if (ret < 0) {
+ debug("SF: AT45 page programming timed out\n");
+ goto out;
+ }
+
+ addr += chunk_len;
+ }
+
+ debug("SF: AT45: Successfully programmed %zu bytes @ 0x%x\n",
+ len, offset);
+ ret = 0;
+
+out:
+ spi_release_bus(flash->spi);
+ return ret;
+}
+