Contiki 2.6

cfs-coffee-arch.c

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Swedish Institute of Computer Science
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions
00007  * are met:
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the Institute nor the names of its contributors
00014  *    may be used to endorse or promote products derived from this software
00015  *    without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
00018  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
00021  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
00023  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00024  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00025  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
00026  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
00027  * SUCH DAMAGE.
00028  *
00029  * This file is part of the Contiki operating system.
00030  *
00031  */
00032 
00033 /**
00034  * \file
00035  *  Coffee architecture-dependent functionality for the AVR-Raven 1284p platform.
00036  * \author
00037  *  Nicolas Tsiftes <nvt@sics.se>
00038  *  Frederic Thepaut <frederic.thepaut@inooi.com>
00039  *  David Kopf <dak664@embarqmail.com>
00040  */
00041 
00042 #include <avr/boot.h>
00043 #include <avr/interrupt.h>
00044 #include <avr/pgmspace.h>
00045 #include <string.h>
00046 
00047 #include "cfs-coffee-arch.h"
00048 
00049 #define DEBUG 0
00050 #if DEBUG
00051 #include <stdio.h>
00052 #define PRINTF(FORMAT,args...) printf_P(PSTR(FORMAT),##args)
00053 #else
00054 #define PRINTF(...)
00055 #endif
00056 
00057 #define TESTCOFFEE 1
00058 #define DEBUG_CFS 1
00059 #if TESTCOFFEE
00060 #if DEBUG_CFS
00061 #include <stdio.h>
00062 #define PRINTF_CFS(FORMAT,args...) printf_P(PSTR(FORMAT),##args)
00063 #else
00064 #define PRINTF_CFS(...)
00065 #endif
00066 
00067 #include "cfs/cfs.h"
00068 #include "cfs/cfs-coffee.h"
00069 #include "lib/crc16.h"
00070 #include "lib/random.h"
00071 #include <stdio.h>
00072 
00073 #define FAIL(x) error = (x); goto end;
00074 
00075 #define FILE_SIZE 512
00076 
00077 int
00078 coffee_file_test(void)
00079 {
00080   int error;
00081   int wfd, rfd, afd;
00082   unsigned char buf[256], buf2[11];
00083   int r, i, j, total_read;
00084   unsigned offset;
00085 
00086   cfs_remove("T1");
00087   cfs_remove("T2");
00088   cfs_remove("T3");
00089   cfs_remove("T4");
00090   cfs_remove("T5");
00091 
00092   wfd = rfd = afd = -1;
00093 
00094   for(r = 0; r < sizeof(buf); r++) {
00095     buf[r] = r;
00096   }
00097 
00098   /* Test 1: Open for writing. */
00099   wfd = cfs_open("T1", CFS_WRITE);
00100   if(wfd < 0) {
00101     FAIL(-1);
00102   }
00103 
00104   /* Test 2: Write buffer. */
00105   r = cfs_write(wfd, buf, sizeof(buf));
00106   if(r < 0) {
00107     FAIL(-2);
00108   } else if(r < sizeof(buf)) {
00109     FAIL(-3);
00110   }
00111 
00112   /* Test 3: Deny reading. */
00113   r = cfs_read(wfd, buf, sizeof(buf));
00114   if(r >= 0) {
00115     FAIL(-4);
00116   }
00117 
00118   /* Test 4: Open for reading. */
00119   rfd = cfs_open("T1", CFS_READ);
00120   if(rfd < 0) {
00121     FAIL(-5);
00122   }
00123 
00124   /* Test 5: Write to read-only file. */
00125   r = cfs_write(rfd, buf, sizeof(buf));
00126   if(r >= 0) {
00127     FAIL(-6);
00128   }
00129 
00130   /* Test 7: Read the buffer written in Test 2. */
00131   memset(buf, 0, sizeof(buf));
00132   r = cfs_read(rfd, buf, sizeof(buf));
00133   if(r < 0) {
00134     FAIL(-8);
00135   } else if(r < sizeof(buf)) {
00136     PRINTF_CFS("r=%d\n", r);
00137     FAIL(-9);
00138   }
00139 
00140   /* Test 8: Verify that the buffer is correct. */
00141   for(r = 0; r < sizeof(buf); r++) {
00142     if(buf[r] != r) {
00143       PRINTF_CFS("r=%d. buf[r]=%d\n", r, buf[r]);
00144       FAIL(-10);
00145     }
00146   }
00147 
00148   /* Test 9: Seek to beginning. */
00149   if(cfs_seek(wfd, 0, CFS_SEEK_SET) != 0) {
00150     FAIL(-11);
00151   }
00152 
00153   /* Test 10: Write to the log. */
00154   r = cfs_write(wfd, buf, sizeof(buf));
00155   if(r < 0) {
00156     FAIL(-12);
00157   } else if(r < sizeof(buf)) {
00158     FAIL(-13);
00159   }
00160 
00161   /* Test 11: Read the data from the log. */
00162   cfs_seek(rfd, 0, CFS_SEEK_SET);
00163   memset(buf, 0, sizeof(buf));
00164   r = cfs_read(rfd, buf, sizeof(buf));
00165   if(r < 0) {
00166     FAIL(-14);
00167   } else if(r < sizeof(buf)) {
00168     FAIL(-15);
00169   }
00170 
00171   /* Test 12: Verify that the data is correct. */
00172   for(r = 0; r < sizeof(buf); r++) {
00173     if(buf[r] != r) {
00174       FAIL(-16);
00175     }
00176   }
00177 
00178   /* Test 13: Write a reversed buffer to the file. */
00179   for(r = 0; r < sizeof(buf); r++) {
00180     buf[r] = sizeof(buf) - r - 1;
00181   }
00182   if(cfs_seek(wfd, 0, CFS_SEEK_SET) != 0) {
00183     FAIL(-17);
00184   }
00185   r = cfs_write(wfd, buf, sizeof(buf));
00186   if(r < 0) {
00187     FAIL(-18);
00188   } else if(r < sizeof(buf)) {
00189     FAIL(-19);
00190   }
00191   if(cfs_seek(rfd, 0, CFS_SEEK_SET) != 0) {
00192     FAIL(-20);
00193   }
00194 
00195   /* Test 14: Read the reversed buffer. */
00196   cfs_seek(rfd, 0, CFS_SEEK_SET);
00197   memset(buf, 0, sizeof(buf));
00198   r = cfs_read(rfd, buf, sizeof(buf));
00199   if(r < 0) {
00200     FAIL(-21);
00201   } else if(r < sizeof(buf)) {
00202     PRINTF_CFS("r = %d\n", r);
00203     FAIL(-22);
00204   }
00205 
00206   /* Test 15: Verify that the data is correct. */
00207   for(r = 0; r < sizeof(buf); r++) {
00208     if(buf[r] != sizeof(buf) - r - 1) {
00209       FAIL(-23);
00210     }
00211   }
00212 
00213   cfs_close(rfd);
00214   cfs_close(wfd);
00215 
00216   if(cfs_coffee_reserve("T2", FILE_SIZE) < 0) {
00217     FAIL(-24);
00218   }
00219 
00220   /* Test 16: Test multiple writes at random offset. */
00221   for(r = 0; r < 100; r++) {
00222     wfd = cfs_open("T2", CFS_WRITE | CFS_READ);
00223     if(wfd < 0) {
00224       FAIL(-25);
00225     }
00226 
00227     offset = random_rand() % FILE_SIZE;
00228 
00229     for(r = 0; r < sizeof(buf); r++) {
00230       buf[r] = r;
00231     }
00232 
00233     if(cfs_seek(wfd, offset, CFS_SEEK_SET) != offset) {
00234       FAIL(-26);
00235     }
00236 
00237     if(cfs_write(wfd, buf, sizeof(buf)) != sizeof(buf)) {
00238       FAIL(-27);
00239     }
00240 
00241     if(cfs_seek(wfd, offset, CFS_SEEK_SET) != offset) {
00242       FAIL(-28);
00243     }
00244 
00245     memset(buf, 0, sizeof(buf));
00246     if(cfs_read(wfd, buf, sizeof(buf)) != sizeof(buf)) {
00247       FAIL(-29);
00248     }
00249 
00250     for(i = 0; i < sizeof(buf); i++) {
00251       if(buf[i] != i) {
00252         PRINTF_CFS("buf[%d] != %d\n", i, buf[i]);
00253         FAIL(-30);
00254       }
00255     }
00256   }
00257   /* Test 17: Append data to the same file many times. */
00258 #define APPEND_BYTES 3000
00259 #define BULK_SIZE 10
00260   for (i = 0; i < APPEND_BYTES; i += BULK_SIZE) {
00261                 afd = cfs_open("T3", CFS_WRITE | CFS_APPEND);
00262                 if (afd < 0) {
00263                         FAIL(-31);
00264                 }
00265                 for (j = 0; j < BULK_SIZE; j++) {
00266                         buf[j] = 1 + ((i + j) & 0x7f);
00267                 }
00268                 if ((r = cfs_write(afd, buf, BULK_SIZE)) != BULK_SIZE) {
00269                         PRINTF_CFS("Count:%d, r=%d\n", i, r);
00270                         FAIL(-32);
00271                 }
00272                 cfs_close(afd);
00273         }
00274 
00275   /* Test 18: Read back the data written in Test 17 and verify that it
00276      is correct. */
00277   afd = cfs_open("T3", CFS_READ);
00278   if(afd < 0) {
00279     FAIL(-33);
00280   }
00281   total_read = 0;
00282   while((r = cfs_read(afd, buf2, sizeof(buf2))) > 0) {
00283     for(j = 0; j < r; j++) {
00284       if(buf2[j] != 1 + ((total_read + j) & 0x7f)) {
00285   FAIL(-34);
00286       }
00287     }
00288     total_read += r;
00289   }
00290   if(r < 0) {
00291           PRINTF_CFS("FAIL:-35 r=%d\n",r);
00292     FAIL(-35);
00293   }
00294   if(total_read != APPEND_BYTES) {
00295           PRINTF_CFS("FAIL:-35 total_read=%d\n",total_read);
00296     FAIL(-35);
00297   }
00298   cfs_close(afd);
00299 
00300 /***************T4********************/
00301 /* file T4 and T5 writing forces to use garbage collector in greedy mode
00302  * this test is designed for 10kb of file system
00303  * */
00304 #define APPEND_BYTES_1 2000
00305 #define BULK_SIZE_1 10
00306   for (i = 0; i < APPEND_BYTES_1; i += BULK_SIZE_1) {
00307                 afd = cfs_open("T4", CFS_WRITE | CFS_APPEND);
00308                 if (afd < 0) {
00309                         FAIL(-36);
00310                 }
00311                 for (j = 0; j < BULK_SIZE_1; j++) {
00312                         buf[j] = 1 + ((i + j) & 0x7f);
00313                 }
00314 
00315 
00316                 if ((r = cfs_write(afd, buf, BULK_SIZE_1)) != BULK_SIZE_1) {
00317                         PRINTF_CFS("Count:%d, r=%d\n", i, r);
00318                         FAIL(-37);
00319                 }
00320                 cfs_close(afd);
00321         }
00322 
00323   afd = cfs_open("T4", CFS_READ);
00324   if(afd < 0) {
00325     FAIL(-38);
00326   }
00327   total_read = 0;
00328   while((r = cfs_read(afd, buf2, sizeof(buf2))) > 0) {
00329     for(j = 0; j < r; j++) {
00330       if(buf2[j] != 1 + ((total_read + j) & 0x7f)) {
00331           PRINTF_CFS("FAIL:-39, total_read=%d r=%d\n",total_read,r);
00332   FAIL(-39);
00333       }
00334     }
00335     total_read += r;
00336   }
00337   if(r < 0) {
00338           PRINTF_CFS("FAIL:-40 r=%d\n",r);
00339     FAIL(-40);
00340   }
00341   if(total_read != APPEND_BYTES_1) {
00342           PRINTF_CFS("FAIL:-41 total_read=%d\n",total_read);
00343     FAIL(-41);
00344   }
00345   cfs_close(afd);
00346   /***************T5********************/
00347 #define APPEND_BYTES_2 1000
00348 #define BULK_SIZE_2 10
00349     for (i = 0; i < APPEND_BYTES_2; i += BULK_SIZE_2) {
00350                 afd = cfs_open("T5", CFS_WRITE | CFS_APPEND);
00351                 if (afd < 0) {
00352                         FAIL(-42);
00353                 }
00354                 for (j = 0; j < BULK_SIZE_2; j++) {
00355                         buf[j] = 1 + ((i + j) & 0x7f);
00356                 }
00357 
00358                 if ((r = cfs_write(afd, buf, BULK_SIZE_2)) != BULK_SIZE_2) {
00359                         PRINTF_CFS("Count:%d, r=%d\n", i, r);
00360                         FAIL(-43);
00361                 }
00362 
00363                 cfs_close(afd);
00364         }
00365 
00366     afd = cfs_open("T5", CFS_READ);
00367     if(afd < 0) {
00368       FAIL(-44);
00369     }
00370     total_read = 0;
00371     while((r = cfs_read(afd, buf2, sizeof(buf2))) > 0) {
00372       for(j = 0; j < r; j++) {
00373         if(buf2[j] != 1 + ((total_read + j) & 0x7f)) {
00374           PRINTF_CFS("FAIL:-45, total_read=%d r=%d\n",total_read,r);
00375     FAIL(-45);
00376         }
00377       }
00378       total_read += r;
00379     }
00380     if(r < 0) {
00381           PRINTF_CFS("FAIL:-46 r=%d\n",r);
00382       FAIL(-46);
00383     }
00384     if(total_read != APPEND_BYTES_2) {
00385           PRINTF_CFS("FAIL:-47 total_read=%d\n",total_read);
00386       FAIL(-47);
00387     }
00388     cfs_close(afd);
00389 
00390   error = 0;
00391 end:
00392   cfs_close(wfd); cfs_close(rfd); cfs_close(afd);
00393   return error;
00394 }
00395 #endif /* TESTCOFFEE */
00396 
00397 /*---------------------------------------------------------------------------*/
00398 /*---------------------------EEPROM ROUTINES---------------------------------*/
00399 /*---------------------------------------------------------------------------*/
00400 #ifdef COFFEE_AVR_EEPROM
00401 
00402 /* Letting .bss initialize nullb to zero saves COFFEE_SECTOR_SIZE of flash */
00403 //static const unsigned char nullb[COFFEE_SECTOR_SIZE] = {0};
00404 static const unsigned char nullb[COFFEE_SECTOR_SIZE];
00405 
00406 /*---------------------------------------------------------------------------*/
00407 /* Erase EEPROM sector
00408  */
00409 void
00410 avr_eeprom_erase(uint16_t sector)
00411 {
00412   eeprom_write(COFFEE_START + sector * COFFEE_SECTOR_SIZE,
00413                  (unsigned char *)nullb, sizeof(nullb));
00414 }
00415 #endif /* COFFEE_AVR_EEPROM */
00416 
00417 #ifdef COFFEE_AVR_FLASH
00418 /*---------------------------------------------------------------------------*/
00419 /*---------------------------FLASH ROUTINES----------------------------------*/
00420 /*---------------------------------------------------------------------------*/
00421 /*
00422  * Read from flash info buf. addr contains starting flash byte address
00423  */
00424 void
00425 avr_flash_read(CFS_CONF_OFFSET_TYPE addr, uint8_t *buf, CFS_CONF_OFFSET_TYPE size)
00426 {
00427   uint32_t addr32=COFFEE_START+addr;
00428   uint16_t isize=size;
00429 #if DEBUG
00430   unsigned char *bufo=(unsigned char *)buf;
00431   uint8_t i;
00432   uint16_t w=addr32>>1;   //Show progmem word address for debug
00433   PRINTF("r0x%04x(%u) ",w,size);
00434 #endif
00435 #ifndef FLASH_WORD_READS
00436   for (;isize>0;isize--) {
00437 #if FLASH_COMPLEMENT_DATA
00438     *buf++=~(uint8_t)pgm_read_byte_far(addr32++);
00439 #else
00440     *buf++=(uint8_t)pgm_read_byte_far(addr32++);
00441 #endif /*FLASH_COMPLEMENT_DATA*/
00442   }
00443 #else
00444 /* 130 bytes more PROGMEM, but faster */
00445   if (isize&0x01) {       //handle first odd byte
00446 #if FLASH_COMPLEMENT_DATA
00447     *buf++=~(uint8_t)pgm_read_byte_far(addr32++);
00448 #else
00449     *buf++=(uint8_t)pgm_read_byte_far(addr32++);
00450 #endif /*FLASH_COMPLEMENT_DATA*/
00451      isize--;
00452   }
00453   for (;isize>1;isize-=2) {//read words from flash
00454 #if FLASH_COMPLEMENT_DATA
00455    *(uint16_t *)buf=~(uint16_t)pgm_read_word_far(addr32);
00456 #else
00457    *(uint16_t *)buf=(uint16_t)pgm_read_word_far(addr32);
00458 #endif /*FLASH_COMPLEMENT_DATA*/
00459     buf+=2;
00460     addr32+=2;
00461   }
00462   if (isize) {            //handle last odd byte
00463 #if FLASH_COMPLEMENT_DATA
00464     *buf++=~(uint8_t)pgm_read_byte_far(addr32);
00465 #else
00466     *buf++=(uint8_t)pgm_read_byte_far(addr32);
00467 #endif /*FLASH_COMPLEMENT_DATA*/
00468   }
00469 #endif /* FLASH_WORD_READS */
00470 
00471 #if DEBUG>1
00472   PRINTF("\nbuf=");
00473 //  PRINTF("%s",bufo);
00474 // for (i=0;i<16;i++) PRINTF("%2x ",*bufo++);
00475 #endif
00476 }
00477 /*---------------------------------------------------------------------------*/
00478 /*
00479  Erase the flash page(s) corresponding to the coffee sector.
00480  This is done by calling the write routine with a null buffer and any address
00481  within each page of the sector (we choose the first byte).
00482  */
00483 BOOTLOADER_SECTION
00484 void avr_flash_erase(coffee_page_t sector) {
00485         coffee_page_t i;
00486 
00487 #if FLASH_COMPLEMENT_DATA
00488         uint32_t addr32;
00489         volatile uint8_t sreg;
00490 
00491         // Disable interrupts.
00492         sreg = SREG;
00493         cli();
00494 
00495         for (i = 0; i < COFFEE_SECTOR_SIZE / COFFEE_PAGE_SIZE; i++) {
00496                 for (addr32 = COFFEE_START + (((sector + i) * COFFEE_PAGE_SIZE)
00497                                 & ~(COFFEE_PAGE_SIZE - 1)); addr32 < (COFFEE_START + (((sector
00498                                 + i + 1) * COFFEE_PAGE_SIZE) & ~(COFFEE_PAGE_SIZE - 1))); addr32
00499                                 += SPM_PAGESIZE) {
00500                         boot_page_erase(addr32);
00501                         boot_spm_busy_wait();
00502 
00503                 }
00504         }
00505         //RE-enable interrupts
00506         boot_rww_enable();
00507         SREG = sreg;
00508 #else
00509         for (i=0;i<COFFEE_SECTOR_SIZE/COFFEE_PAGE_SIZE;i++) {
00510                 avr_flash_write((sector+i)*COFFEE_PAGE_SIZE,0,0);
00511         }
00512 #endif
00513 
00514 #if 0
00515 #if TESTCOFFEE
00516 /* Defining TESTCOFFEE is a convenient way of testing a new configuration.
00517  * It is triggered by an erase of the last sector.
00518  * Note this routine will be reentered during the test!                     */
00519 
00520   if ((sector+i)==COFFEE_PAGES-1) {
00521     int j=(int)(COFFEE_START>>1),k=(int)((COFFEE_START>>1)+(COFFEE_SIZE>>1)),l=(int)(COFFEE_SIZE/1024UL);
00522     printf_P(PSTR("\nTesting coffee filesystem [0x%08x -> 0x%08x (%uKb)] ..."),j,k,l);
00523     int r= coffee_file_test();
00524     if (r<0) {
00525       printf_P(PSTR("\nFailed with return %d! :-(\n"),r);
00526     } else {
00527       printf_P(PSTR("Passed! :-)\n"));
00528     }
00529   }
00530 #endif /* TESTCOFFEE */
00531 #endif
00532 }
00533 
00534 /*httpd-fs routines
00535   getchar is straigtforward.
00536   strcmp only needs to handle file names for fs_open. Note filename in buf will not be zero terminated
00537     if it fills the coffee name field, so a pseudo strcmp is done here.
00538   strchr searches for script starts so must handle arbitrarily large strings
00539  */
00540 char avr_httpd_fs_getchar(char *addr) {
00541   char r;
00542   avr_flash_read((CFS_CONF_OFFSET_TYPE) addr, (uint8_t*) &r, 1);
00543   return r;
00544 }
00545 int avr_httpd_fs_strcmp (char *ram, char *addr) {
00546   uint8_t i,*in,buf[32];
00547   avr_flash_read((CFS_CONF_OFFSET_TYPE)addr, buf, sizeof(buf));
00548 //return strcmp(ram, (char *)buf);
00549   in=(uint8_t *)ram;
00550   for (i=0;i<32;i++) {
00551     if (buf[i]==0) return(0);
00552     if (buf[i]!=*in) break;
00553     in++;
00554   }
00555 /* A proper strcmp would return a + or minus number based on the last comparison*/
00556 //if (buf[i]>*in) return(i); else return(-i);
00557   return(i);
00558 }
00559 char * avr_httpd_fs_strchr (char *addr, int character) {
00560   char buf[129],*pptr;
00561   buf[128]=character;
00562   while (1) {
00563     avr_flash_read((CFS_CONF_OFFSET_TYPE)addr, (uint8_t *) buf, 128);
00564     pptr=strchr(buf, character);
00565     if (pptr!=&buf[128]) {
00566       if (pptr==0) return 0;
00567       return (addr+(pptr-buf));
00568    }
00569     addr+=128;
00570   }
00571 
00572 }
00573 
00574 /*---------------------------------------------------------------------------*/
00575 /*
00576  * Transfer buf[size] from RAM to flash, starting at addr.
00577  * If buf is null, just erase the flash page
00578  * Note this routine has to be in the bootloader NRWW part of program memory,
00579  * and that writing to NRWW (last 32 pages on the 1284p) will halt the CPU.
00580  */
00581 BOOTLOADER_SECTION
00582 void
00583 avr_flash_write(CFS_CONF_OFFSET_TYPE addr, uint8_t *buf, CFS_CONF_OFFSET_TYPE size)
00584 {
00585   uint32_t addr32;
00586   uint16_t w;
00587   uint8_t  bb,ba,sreg;
00588  
00589   /* Disable interrupts, make sure no eeprom write in progress */
00590   sreg = SREG;
00591   cli();
00592   eeprom_busy_wait();
00593 
00594   /* Calculate the starting address of the first flash page being
00595     modified (will be on a page boundary) and the number of
00596     unaltered bytes before and after the data to be written.          */
00597 #if 0    //this is 8 bytes longer
00598   uint16_t startpage=addr/COFFEE_PAGE_SIZE;
00599   addr32=COFFEE_START+startpage*COFFEE_PAGE_SIZE;
00600 #else
00601   addr32=(COFFEE_ADDRESS&~(SPM_PAGESIZE-1))+(addr&~(SPM_PAGESIZE-1));
00602 #endif
00603   bb=addr & (SPM_PAGESIZE-1);
00604   ba=COFFEE_PAGE_SIZE-((addr+size)&0xff);
00605 
00606 #if DEBUG
00607   uint16_t startpage=addr/COFFEE_PAGE_SIZE;
00608   w=addr32>>1;   //Show progmem word address for debug
00609   if (buf) {
00610     PRINTF("w0x%04x %u %u %u",w,size,bb,ba);
00611   } else {
00612     PRINTF("e0x%04x %u ",w,startpage);
00613   }
00614 #endif
00615 
00616   /* If buf not null, modify the page(s) */
00617   if (buf) {
00618     if (size==0) return;            //nothing to write
00619     /*Copy the first part of the existing page into the write buffer */
00620     while (bb>1) {
00621       w=pgm_read_word_far(addr32);
00622       boot_page_fill(addr32,w);
00623       addr32+=2;
00624       bb-=2;
00625     }
00626     /* Transfer the bytes to be modified */
00627     while (size>1) {
00628       if (bb) {                     //handle odd byte boundary
00629         w=pgm_read_word_far(addr32);
00630 #if FLASH_COMPLEMENT_DATA
00631         w  = ~w;
00632 #endif /*FLASH_COMPLEMENT_DATA*/
00633         w &= 0xff;
00634         bb=0;
00635         size++;
00636       } else {
00637         w  = *buf++;
00638       }
00639       w += (*buf++) << 8;
00640 #if FLASH_COMPLEMENT_DATA
00641       w  = ~w;
00642 #endif /*FLASH_COMPLEMENT_DATA*/
00643       boot_page_fill(addr32, w);
00644       size-=2;
00645 /* Below ought to work but writing to 0xnnnnnnfe modifies the NEXT flash page
00646    for some reason, at least in the AVR Studio simulator.
00647       if ((addr32&0x000000ff)==0x000000fe) { //handle page boundary
00648         if (size) {
00649           boot_page_erase(addr32);
00650           boot_spm_busy_wait();
00651           boot_page_write(addr32);
00652           boot_spm_busy_wait();
00653         }
00654       }
00655        addr32+=2;
00656 */
00657        
00658 /* This works...*/
00659       addr32+=2;
00660       if ((addr32&0x000000ff)==0) {    //handle page boundary
00661         if (size) {
00662           addr32-=0x42;                //get an address within the page
00663           boot_page_erase(addr32);
00664           boot_spm_busy_wait();
00665           boot_page_write(addr32);
00666           boot_spm_busy_wait();
00667           addr32+=0x42;
00668         }
00669       }
00670     }
00671     /* Copy the remainder of the existing page */
00672     while (ba>1) {
00673       w=pgm_read_word_far(addr32);
00674       if (size) {                     //handle odd byte boundary
00675         w &= 0xff00;
00676 #if FLASH_COMPLEMENT_DATA
00677         w +=~(*buf);
00678 #else
00679         w +=*buf;
00680 #endif /*FLASH_COMPLEMENT_DATA*/
00681         size=0;
00682       }
00683       boot_page_fill(addr32,w);
00684       addr32+=2;
00685       ba-=2;
00686     }
00687   /* If buf is null, erase the page to zero */
00688   } else {
00689 #if FLASH_COMPLEMENT_DATA
00690     addr32+=2*SPM_PAGESIZE;
00691 #else
00692     for (w=0;w<SPM_PAGESIZE;w++) {
00693       boot_page_fill(addr32, 0);
00694       addr32+=2;
00695     }
00696 #endif /*FLASH_COMPLEMENT_DATA*/
00697   }
00698 /* Write the last (or only) page */
00699   addr32-=0x42; //get an address within the page
00700   boot_page_erase(addr32);
00701   boot_spm_busy_wait();
00702 #if FLASH_COMPLEMENT_DATA
00703   if (buf) {                      //don't write zeroes to erased page
00704     boot_page_write(addr32);
00705     boot_spm_busy_wait();
00706   }
00707 #else
00708   boot_page_write(addr32);
00709   boot_spm_busy_wait();
00710 #endif /*FLASH_COMPLEMENT_DATA*/
00711   /* Reenable RWW-section again. We need this if we want to jump back
00712    * to the application after bootloading. */
00713   boot_rww_enable();
00714 
00715   /* Re-enable interrupts (if they were ever enabled). */
00716   SREG = sreg;
00717 }
00718 
00719 #endif /* COFFEE_AVR_FLASH */