Changeset 57e8a0d in rtems


Ignore:
Timestamp:
Oct 13, 2014, 6:18:55 PM (5 years ago)
Author:
Joel Sherrill <joel.sherrill@…>
Branches:
4.11, master
Children:
3c743901
Parents:
21021feb
git-author:
Joel Sherrill <joel.sherrill@…> (10/13/14 18:18:55)
git-committer:
Joel Sherrill <joel.sherrill@…> (10/16/14 13:58:45)
Message:

arm/gp32/smc/smc.c: Fix warnings and clean up

File:
1 edited

Legend:

Unmodified
Added
Removed
  • c/src/lib/libbsp/arm/gp32/smc/smc.c

    r21021feb r57e8a0d  
    1 /* smc.c -- s3c2400 smc disk block device implementation
    2 
    3  Squidge's SMC Low-level access routines.
    4  Inspired and derived from routines provided by Samsung Electronics M/M R&D Center & FireFly.
    5 
    6 */
     1/*
     2 * s3c2400 smc disk block device implementation
     3 *
     4 * Squidge's SMC Low-level access routines.
     5 * Inspired and derived from routines provided by Samsung Electronics
     6 *  M/M R&D Center & FireFly.
     7 */
    78
    89#include <rtems.h>
     
    2122
    2223#define SMC_DEVICE_NAME "/dev/smc"
    23 #define SMC_SAMSUNG_ID          0xEC
    24 #define SMC_TOSHIBA_ID          0x98
    25 
    26 #define SMC_16MB        0x73
    27 #define SMC_32MB        0x75
    28 #define SMC_64MB        0x76
    29 #define SMC_128MB       0x79
    30 
    31 #define LBA_UNUSED              0x80000000
    32 #define LBA_RESERVED            0x80000001
    33 
    34 #define BLOCK_UNUSED            0x80000000
    35 #define BLOCK_RESERVED          0x80000001
     24#define SMC_SAMSUNG_ID    0xEC
     25#define SMC_TOSHIBA_ID    0x98
     26
     27#define SMC_16MB  0x73
     28#define SMC_32MB  0x75
     29#define SMC_64MB  0x76
     30#define SMC_128MB  0x79
     31
     32#define LBA_UNUSED      0x80000000
     33#define LBA_RESERVED    0x80000001
     34
     35#define BLOCK_UNUSED    0x80000000
     36#define BLOCK_RESERVED  0x80000001
    3637
    3738/* SmartMedia Command */
    38 #define SEQ_DATA_INPUT_CMD      0x80
    39 #define READ1_CMD               0x00
    40 #define READ1_1_CMD             0x01
    41 #define READ2_CMD               0x50
    42 #define READ_ID_CMD             0x90
    43 #define RESET_CMD               0xFF
    44 #define PAGE_PROGRAM_CMD        0x10
    45 #define BLOCK_ERASE_CMD         0x60
    46 #define BLOCK_ERASE_CFM_CMD     0xD0
    47 #define READ_STATUS_CMD         0x70
    48 #define RESET_PTR_CMD           0x00
     39#define SEQ_DATA_INPUT_CMD   0x80
     40#define READ1_CMD            0x00
     41#define READ1_1_CMD          0x01
     42#define READ2_CMD            0x50
     43#define READ_ID_CMD          0x90
     44#define RESET_CMD            0xFF
     45#define PAGE_PROGRAM_CMD     0x10
     46#define BLOCK_ERASE_CMD      0x60
     47#define BLOCK_ERASE_CFM_CMD  0xD0
     48#define READ_STATUS_CMD      0x70
     49#define RESET_PTR_CMD        0x00
    4950
    5051
    5152/* Internal SMC disk descriptor */
    52 struct SMC_INFO
    53 {
     53struct SMC_INFO {
    5454  uint8_t  id[3];
    5555  uint32_t bytes_per_page;
     
    113113*/
    114114#if UNUSED
    115 static uint8_t sm_status()
     115static static uint8_t sm_status()
    116116{
    117117  uint8_t status;
     
    134134#endif
    135135
    136 void smc_read_id( uint8_t* buf, uint32_t length)
    137 {
    138 
     136static void smc_read_id( uint8_t* buf, uint32_t length)
     137{
    139138  uint32_t i;
    140139
     
    161160
    162161/* read an entire logical page of 512 bytes.*/
    163 uint8_t smc_read_page (uint32_t lpage, uint8_t* buf)
    164 {
    165         uint32_t block, page, i;
    166 
    167         /* convert logical block to physical block
    168            and then convert into page suitable for read1 command...
    169         */
    170         block = lpage >> 5;
    171         if (smc_l2p[block] < LBA_UNUSED) {
    172                 page = smc_l2p[block] << 5;
    173                 page += (lpage & 0x1F);
    174         }
    175         else
    176                 return 0;
    177 
    178         sm_chip_en();
    179 
    180         sm_cle_en();
    181         sm_write_en();
    182         sm_write(READ1_CMD);
    183         sm_write_dis();
    184         sm_cle_dis();
    185 
    186                         sm_ale_en();
    187                         sm_write_en();
    188                         sm_write( 0x00);
    189                         sm_write( (uint8_t)(page >> 0));
    190                         sm_write( (uint8_t)(page >> 8));
    191                         if (smc_info.mb >= 64) sm_write( (uint8_t)(page >> 16));
    192                         sm_write_dis();
    193                         sm_ale_dis();
    194 
    195                         sm_busy();
    196 
    197                         sm_read_en();
    198                         for (i = 0; i < 512; i++)
    199                         {
    200                                 *buf = sm_read();
    201                                 buf++;
    202                         }
    203                         sm_read_dis();
    204                         sm_chip_dis();
    205 
    206                         sm_busy();
    207         return 1;
    208 }
    209 
    210 void smc_read_spare( uint32_t page, uint8_t* buf, uint8_t length)
     162static uint8_t smc_read_page (uint32_t lpage, uint8_t* buf)
     163{
     164  uint32_t block, page, i;
     165
     166  /* convert logical block to physical block
     167     and then convert into page suitable for read1 command...
     168  */
     169  block = lpage >> 5;
     170  if (smc_l2p[block] < LBA_UNUSED) {
     171    page = smc_l2p[block] << 5;
     172    page += (lpage & 0x1F);
     173  }
     174  else
     175    return 0;
     176
     177  sm_chip_en();
     178
     179  sm_cle_en();
     180  sm_write_en();
     181  sm_write(READ1_CMD);
     182  sm_write_dis();
     183  sm_cle_dis();
     184
     185  sm_ale_en();
     186  sm_write_en();
     187  sm_write( 0x00);
     188  sm_write( (uint8_t)(page >> 0));
     189  sm_write( (uint8_t)(page >> 8));
     190  if (smc_info.mb >= 64)
     191    sm_write( (uint8_t)(page >> 16));
     192  sm_write_dis();
     193  sm_ale_dis();
     194
     195  sm_busy();
     196
     197  sm_read_en();
     198  for (i = 0; i < 512; i++) {
     199    *buf = sm_read();
     200    buf++;
     201  }
     202  sm_read_dis();
     203  sm_chip_dis();
     204
     205  sm_busy();
     206  return 1;
     207}
     208
     209static void smc_read_spare( uint32_t page, uint8_t* buf, uint8_t length)
    211210{
    212211  uint32_t i;
    213 
    214212
    215213  sm_chip_en();
     
    226224  sm_write( (uint8_t)(page >> 0));
    227225  sm_write( (uint8_t)(page >> 8));
    228   if (smc_info.mb >= 64) sm_write( (uint8_t)(page >> 16));
     226  if (smc_info.mb >= 64)
     227    sm_write( (uint8_t)(page >> 16));
    229228  sm_read_en();
    230229  sm_ale_dis();
     
    233232
    234233  sm_read_en();
    235   for (i=0;i<length;i++) *(buf+i) = sm_read();
     234  for (i=0;i<length;i++)
     235    *(buf+i) = sm_read();
    236236  sm_read_dis();
    237237
     
    240240}
    241241
    242 void smc_make_l2p(void)
     242static void smc_make_l2p(void)
    243243{
    244244  uint32_t pblock, i, j, lblock, zone, count, cnt1, cnt2, cnt3;
     
    249249  cnt3 = 0;
    250250
    251   for (i=0;i<0x2000;i++)
    252   {
     251  for (i=0;i<0x2000;i++) {
    253252    smc_l2p[i] = LBA_RESERVED;
    254253    smc_p2l[i] = BLOCK_RESERVED;
    255254  }
    256   for (pblock=0;pblock<smc_info.blocks;pblock++)
    257   {
     255
     256  for (pblock=0;pblock<smc_info.blocks;pblock++) {
    258257    /* read physical block - first page */
    259258    smc_read_spare( pblock*smc_info.pages_per_block, (uint8_t*)&data, 16);
    260259
    261260    zone = pblock >> 10; /* divide by 1024 to get zone */
    262     if ((data[5] == 0xFF) && ((data[6]&0xF8) == 0x10))
    263     {
     261    if ((data[5] == 0xFF) && ((data[6]&0xF8) == 0x10)) {
    264262      lblock = ((((data[6]<<8)|(data[7]<<0)) >> 1) & 0x03FF) + (zone * 1000);
    265263      smc_l2p[lblock] = pblock;
    266264      smc_p2l[pblock] = lblock;
    267265      cnt1++;
    268     }
    269     else
    270     {
     266    } else {
    271267      count = 0;
    272       for (j=0;j<16;j++)
    273       {
     268      for (j=0;j<16;j++) {
    274269        if (data[j] == 0xFF) count++;
    275270      }
    276       if (count == 16)
    277       {
    278         smc_p2l[pblock] = BLOCK_UNUSED;
    279         cnt2++;
     271      if (count == 16) {
     272        smc_p2l[pblock] = BLOCK_UNUSED;
     273        cnt2++;
     274      } else {
     275        smc_p2l[pblock] = BLOCK_RESERVED;
     276        cnt3++;
    280277      }
    281       else
    282       {
    283         smc_p2l[pblock] = BLOCK_RESERVED;
    284         cnt3++;
    285       }
    286     }
    287   }
    288 }
    289 
    290 
    291 void smc_detect( uint8_t id1, uint8_t id2, uint8_t id3)
     278    }
     279  }
     280}
     281
     282
     283static void smc_detect( uint8_t id1, uint8_t id2, uint8_t id3)
    292284{
    293285  smc_info.id[0] = id1;
     
    299291  smc_info.blocks          = 0;
    300292
    301   switch (id1)
    302   {
     293  switch (id1) {
    303294    case SMC_SAMSUNG_ID:
    304     case SMC_TOSHIBA_ID:
    305     {
    306       switch (id2)
    307       {
     295    case SMC_TOSHIBA_ID: {
     296      switch (id2) {
    308297        case SMC_16MB  : smc_info.mb = 16; break;
    309298        case SMC_32MB  : smc_info.mb = 32; break;
     
    315304  }
    316305
    317   switch (smc_info.mb)
    318   {
     306  switch (smc_info.mb) {
    319307    case 16  : smc_info.bytes_per_page = 512; smc_info.pages_per_block = 32; smc_info.blocks = 0x0400; break;
    320308    case 32  : smc_info.bytes_per_page = 512; smc_info.pages_per_block = 32; smc_info.blocks = 0x0800; break;
     
    324312}
    325313
    326 void smc_init( void)
    327 {
    328         unsigned char buf[32];
    329         int i;
    330 
    331         /* reset smc */
    332         sm_chip_en();
    333         sm_cle_en();
    334         sm_write_en();
    335         sm_write(0xFF);
    336         sm_write_dis();
    337         sm_cle_dis();
    338         for(i=0;i<10;i++);
    339         sm_busy();
    340         sm_chip_dis();
    341 
    342         smc_read_id (buf, 4);
    343         smc_detect (buf[0], buf[1], buf[2]);
    344         printk ("SMC: [%02X-%02X-%02X-%02X]\n", buf[0], buf[1], buf[2], buf[3]);
    345         printk ("SMC size: %dMB detected\n",smc_info.mb);
    346         smc_make_l2p();
     314static void smc_init( void)
     315{
     316  unsigned char buf[32];
     317  int i;
     318
     319  /* reset smc */
     320  sm_chip_en();
     321  sm_cle_en();
     322  sm_write_en();
     323  sm_write(0xFF);
     324  sm_write_dis();
     325  sm_cle_dis();
     326  for(i=0;i<10;i++);
     327  sm_busy();
     328  sm_chip_dis();
     329
     330  smc_read_id (buf, 4);
     331  smc_detect (buf[0], buf[1], buf[2]);
     332  printk ("SMC: [%02X-%02X-%02X-%02X]\n", buf[0], buf[1], buf[2], buf[3]);
     333  printk ("SMC size: %dMB detected\n",smc_info.mb);
     334  smc_make_l2p();
    347335}
    348336
     
    350338 * Function: sm_ECCEncode (completely ripped, unaltered, from the samsung routines)
    351339 * Remark:
    352  *      - adopted from "ECC Algorithm for SmartMedia V3.0"
    353  *              by Memory Product & Technology, Samsung Electronics Co. (ecc30.pdf)
     340 *  - adopted from "ECC Algorithm for SmartMedia V3.0"
     341 *    by Memory Product & Technology, Samsung Electronics Co. (ecc30.pdf)
    354342 **********/
    355343int sm_ECCEncode(const uint8_t * p_buf, uint8_t * p_ecc)
    356344{
    357         uint32_t i, j;
    358         uint8_t paritr[256], tmp = 0, tmp2 = 0;
    359         uint8_t data_table0[16] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 };
    360         uint8_t data_table1[16] = { 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1 };
    361         uint8_t sum = 0, paritc = 0;
    362         uint8_t parit0c = 0, parit1c = 0, parit2c = 0, parit3c = 0;
    363         uint8_t parit4c = 0, parit5c = 0, parit6c = 0, parit7c = 0;
    364         uint8_t parit1_1, parit1_2, parit2_1, parit2_2, parit4_1, parit4_2;
    365         uint8_t parit8_1 = 0, parit8_2 = 0, parit16_1 = 0, parit16_2 = 0, parit32_1 = 0, parit32_2 = 0;
    366         uint8_t parit64_1 = 0, parit64_2 = 0, parit128_1 = 0, parit128_2 = 0, parit256_1 = 0, parit256_2 = 0;
    367         uint8_t parit512_1 = 0, parit512_2 = 0, parit1024_1 = 0, parit1024_2 = 0;
    368         uint8_t* paritr_ptr;
    369 
    370         paritr_ptr = paritr;
    371         for (i = 0; i < 256; ++i, ++paritr_ptr, ++p_buf)
    372         {
    373                 paritc ^= *p_buf;
    374                 tmp = (*p_buf & 0xf0) >> 4;
    375                 tmp2 = *p_buf & 0x0f;
    376 
    377                 switch (tmp)
    378                 {
    379                         case 0:
    380                         case 3:
    381                         case 5:
    382                         case 6:
    383                         case 9:
    384                         case 10:
    385                         case 12:
    386                         case 15:
    387                                 *paritr_ptr = *(data_table0 + tmp2);
    388                                 break;
    389 
    390                         case 1:
    391                         case 2:
    392                         case 4:
    393                         case 7:
    394                         case 8:
    395                         case 11:
    396                         case 13:
    397                         case 14:
    398                                 *paritr_ptr = *(data_table1 + tmp2);
    399                                 break;
    400                 }
    401         }
    402 
    403         parit0c = (paritc & 0x01) ? 1 : 0;
    404         parit1c = (paritc & 0x02) ? 1 : 0;
    405         parit2c = (paritc & 0x04) ? 1 : 0;
    406         parit3c = (paritc & 0x08) ? 1 : 0;
    407         parit4c = (paritc & 0x10) ? 1 : 0;
    408         parit5c = (paritc & 0x20) ? 1 : 0;
    409         parit6c = (paritc & 0x40) ? 1 : 0;
    410         parit7c = (paritc & 0x80) ? 1 : 0;
    411         parit1_2 = parit6c ^ parit4c ^ parit2c ^ parit0c;
    412         parit1_1 = parit7c ^ parit5c ^ parit3c ^ parit1c;
    413         parit2_2 = parit5c ^ parit4c ^ parit1c ^ parit0c;
    414         parit2_1 = parit7c ^ parit6c ^ parit3c ^ parit2c;
    415         parit4_2 = parit3c ^ parit2c ^ parit1c ^ parit0c;
    416         parit4_1 = parit7c ^ parit6c ^ parit5c ^ parit4c;
    417 
    418         paritr_ptr = paritr;
    419         for (i = 0; i < 256; ++i, ++paritr_ptr)
    420         {
    421                 sum ^= *paritr_ptr;
    422         }
    423 
    424         paritr_ptr = paritr;
    425         for (i = 0; i < 256; i += 2, paritr_ptr += 2)
    426         {
    427                 parit8_2 ^= *paritr_ptr;
    428         }
    429 
    430         paritr_ptr = paritr;
    431         for (i = 0; i < 256; i += 4, paritr_ptr += 4)
    432         {
    433                 parit16_2 ^= *paritr_ptr;
    434                 parit16_2 ^= *(paritr_ptr + 1);
    435         }
    436 
    437         paritr_ptr = paritr;
    438         for (i = 0; i < 256; i += 8, paritr_ptr += 8)
    439         {
    440                 for (j = 0; j <= 3; ++j)
    441                 {
    442                         parit32_2 ^= *(paritr_ptr + j);
    443                 }
    444         }
    445 
    446         paritr_ptr = paritr;
    447         for (i = 0; i < 256; i += 16, paritr_ptr += 16)
    448         {
    449                 for (j = 0; j <= 7; ++j)
    450                 {
    451                         parit64_2 ^= *(paritr_ptr + j);
    452                 }
    453         }
    454 
    455         paritr_ptr = paritr;
    456         for (i = 0; i < 256; i += 32, paritr_ptr += 32)
    457         {
    458                 for (j = 0; j <= 15; ++j)
    459                 {
    460                         parit128_2 ^= *(paritr_ptr + j);
    461                 }
    462         }
    463 
    464         paritr_ptr = paritr;
    465         for (i = 0; i < 256; i += 64, paritr_ptr += 64)
    466         {
    467                 for (j = 0; j <= 31; ++j)
    468                 {
    469                         parit256_2 ^= *(paritr_ptr + j);
    470                 }
    471         }
    472 
    473         paritr_ptr = paritr;
    474         for (i = 0; i < 256; i += 128, paritr_ptr += 128)
    475         {
    476                 for (j = 0; j <= 63; ++j)
    477                 {
    478                         parit512_2 ^= *(paritr_ptr + j);
    479                 }
    480         }
    481 
    482         paritr_ptr = paritr;
    483         for (i = 0; i < 256; i += 256, paritr_ptr += 256)
    484         {
    485                 for (j = 0; j <= 127; ++j)
    486                 {
    487                         parit1024_2 ^= *(paritr_ptr + j);
    488                 }
    489         }
    490 
    491         if (sum==0)
    492         {
    493                 parit1024_1 = parit1024_2;
    494                 parit512_1 = parit512_2;
    495                 parit256_1 = parit256_2;
    496                 parit128_1 = parit128_2;
    497                 parit64_1 = parit64_2;
    498                 parit32_1 = parit32_2;
    499                 parit16_1 = parit16_2;
    500                 parit8_1 = parit8_2;
    501         }
    502         else
    503         {
    504                 parit1024_1 = parit1024_2 ? 0 : 1;
    505                 parit512_1 = parit512_2 ? 0 : 1;
    506                 parit256_1 = parit256_2 ? 0 : 1;
    507                 parit128_1 = parit128_2 ? 0 : 1;
    508                 parit64_1 = parit64_2 ? 0 : 1;
    509                 parit32_1 = parit32_2 ? 0 : 1;
    510                 parit16_1 = parit16_2 ? 0 : 1;
    511                 parit8_1 = parit8_2 ? 0 : 1;
    512         }
    513 
    514         parit1_2 <<= 2;
    515         parit1_1 <<= 3;
    516         parit2_2 <<= 4;
    517         parit2_1 <<= 5;
    518         parit4_2 <<= 6;
    519         parit4_1 <<= 7;
    520         parit128_1 <<= 1;
    521         parit256_2 <<= 2;
    522         parit256_1 <<= 3;
    523         parit512_2 <<= 4;
    524         parit512_1 <<= 5;
    525         parit1024_2 <<= 6;
    526         parit1024_1 <<= 7;
    527         parit8_1 <<= 1;
    528         parit16_2 <<= 2;
    529         parit16_1 <<= 3;
    530         parit32_2 <<= 4;
    531         parit32_1 <<= 5;
    532         parit64_2 <<= 6;
    533         parit64_1 <<= 7;
    534 
    535         p_ecc[0] = ~(parit64_1 | parit64_2 | parit32_1 | parit32_2 | parit16_1 | parit16_2 | parit8_1 | parit8_2);
    536         p_ecc[1] = ~(parit1024_1 |parit1024_2 | parit512_1 | parit512_2 | parit256_1 | parit256_2 | parit128_1 | parit128_2);
    537         p_ecc[2] = ~(parit4_1 | parit4_2 | parit2_1 | parit2_2 | parit1_1 | parit1_2);
    538 
    539         return 0;
     345  uint32_t i, j;
     346  uint8_t paritr[256], tmp = 0, tmp2 = 0;
     347  uint8_t data_table0[16] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 };
     348  uint8_t data_table1[16] = { 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1 };
     349  uint8_t sum = 0, paritc = 0;
     350  uint8_t parit0c = 0, parit1c = 0, parit2c = 0, parit3c = 0;
     351  uint8_t parit4c = 0, parit5c = 0, parit6c = 0, parit7c = 0;
     352  uint8_t parit1_1, parit1_2, parit2_1, parit2_2, parit4_1, parit4_2;
     353  uint8_t parit8_1 = 0, parit8_2 = 0, parit16_1 = 0, parit16_2 = 0, parit32_1 = 0, parit32_2 = 0;
     354  uint8_t parit64_1 = 0, parit64_2 = 0, parit128_1 = 0, parit128_2 = 0, parit256_1 = 0, parit256_2 = 0;
     355  uint8_t parit512_1 = 0, parit512_2 = 0, parit1024_1 = 0, parit1024_2 = 0;
     356  uint8_t* paritr_ptr;
     357
     358  paritr_ptr = paritr;
     359  for (i = 0; i < 256; ++i, ++paritr_ptr, ++p_buf) {
     360    paritc ^= *p_buf;
     361    tmp = (*p_buf & 0xf0) >> 4;
     362    tmp2 = *p_buf & 0x0f;
     363
     364    switch (tmp) {
     365      case 0:
     366      case 3:
     367      case 5:
     368      case 6:
     369      case 9:
     370      case 10:
     371      case 12:
     372      case 15:
     373        *paritr_ptr = *(data_table0 + tmp2);
     374        break;
     375
     376      case 1:
     377      case 2:
     378      case 4:
     379      case 7:
     380      case 8:
     381      case 11:
     382      case 13:
     383      case 14:
     384        *paritr_ptr = *(data_table1 + tmp2);
     385        break;
     386    }
     387  }
     388
     389  parit0c = (paritc & 0x01) ? 1 : 0;
     390  parit1c = (paritc & 0x02) ? 1 : 0;
     391  parit2c = (paritc & 0x04) ? 1 : 0;
     392  parit3c = (paritc & 0x08) ? 1 : 0;
     393  parit4c = (paritc & 0x10) ? 1 : 0;
     394  parit5c = (paritc & 0x20) ? 1 : 0;
     395  parit6c = (paritc & 0x40) ? 1 : 0;
     396  parit7c = (paritc & 0x80) ? 1 : 0;
     397  parit1_2 = parit6c ^ parit4c ^ parit2c ^ parit0c;
     398  parit1_1 = parit7c ^ parit5c ^ parit3c ^ parit1c;
     399  parit2_2 = parit5c ^ parit4c ^ parit1c ^ parit0c;
     400  parit2_1 = parit7c ^ parit6c ^ parit3c ^ parit2c;
     401  parit4_2 = parit3c ^ parit2c ^ parit1c ^ parit0c;
     402  parit4_1 = parit7c ^ parit6c ^ parit5c ^ parit4c;
     403
     404  paritr_ptr = paritr;
     405  for (i = 0; i < 256; ++i, ++paritr_ptr) {
     406    sum ^= *paritr_ptr;
     407  }
     408
     409  paritr_ptr = paritr;
     410  for (i = 0; i < 256; i += 2, paritr_ptr += 2) {
     411    parit8_2 ^= *paritr_ptr;
     412  }
     413
     414  paritr_ptr = paritr;
     415  for (i = 0; i < 256; i += 4, paritr_ptr += 4) {
     416    parit16_2 ^= *paritr_ptr;
     417    parit16_2 ^= *(paritr_ptr + 1);
     418  }
     419
     420  paritr_ptr = paritr;
     421  for (i = 0; i < 256; i += 8, paritr_ptr += 8) {
     422    for (j = 0; j <= 3; ++j) {
     423      parit32_2 ^= *(paritr_ptr + j);
     424    }
     425  }
     426
     427  paritr_ptr = paritr;
     428  for (i = 0; i < 256; i += 16, paritr_ptr += 16) {
     429    for (j = 0; j <= 7; ++j) {
     430      parit64_2 ^= *(paritr_ptr + j);
     431    }
     432  }
     433
     434  paritr_ptr = paritr;
     435  for (i = 0; i < 256; i += 32, paritr_ptr += 32) {
     436    for (j = 0; j <= 15; ++j) {
     437      parit128_2 ^= *(paritr_ptr + j);
     438    }
     439  }
     440
     441  paritr_ptr = paritr;
     442  for (i = 0; i < 256; i += 64, paritr_ptr += 64) {
     443    for (j = 0; j <= 31; ++j) {
     444      parit256_2 ^= *(paritr_ptr + j);
     445    }
     446  }
     447
     448  paritr_ptr = paritr;
     449  for (i = 0; i < 256; i += 128, paritr_ptr += 128) {
     450    for (j = 0; j <= 63; ++j) {
     451      parit512_2 ^= *(paritr_ptr + j);
     452    }
     453  }
     454
     455  paritr_ptr = paritr;
     456  for (i = 0; i < 256; i += 256, paritr_ptr += 256) {
     457    for (j = 0; j <= 127; ++j) {
     458      parit1024_2 ^= *(paritr_ptr + j);
     459    }
     460  }
     461
     462  if (sum==0) {
     463    parit1024_1 = parit1024_2;
     464    parit512_1 = parit512_2;
     465    parit256_1 = parit256_2;
     466    parit128_1 = parit128_2;
     467    parit64_1 = parit64_2;
     468    parit32_1 = parit32_2;
     469    parit16_1 = parit16_2;
     470    parit8_1 = parit8_2;
     471  } else {
     472    parit1024_1 = parit1024_2 ? 0 : 1;
     473    parit512_1 = parit512_2 ? 0 : 1;
     474    parit256_1 = parit256_2 ? 0 : 1;
     475    parit128_1 = parit128_2 ? 0 : 1;
     476    parit64_1 = parit64_2 ? 0 : 1;
     477    parit32_1 = parit32_2 ? 0 : 1;
     478    parit16_1 = parit16_2 ? 0 : 1;
     479    parit8_1 = parit8_2 ? 0 : 1;
     480  }
     481
     482  parit1_2 <<= 2;
     483  parit1_1 <<= 3;
     484  parit2_2 <<= 4;
     485  parit2_1 <<= 5;
     486  parit4_2 <<= 6;
     487  parit4_1 <<= 7;
     488  parit128_1 <<= 1;
     489  parit256_2 <<= 2;
     490  parit256_1 <<= 3;
     491  parit512_2 <<= 4;
     492  parit512_1 <<= 5;
     493  parit1024_2 <<= 6;
     494  parit1024_1 <<= 7;
     495  parit8_1 <<= 1;
     496  parit16_2 <<= 2;
     497  parit16_1 <<= 3;
     498  parit32_2 <<= 4;
     499  parit32_1 <<= 5;
     500  parit64_2 <<= 6;
     501  parit64_1 <<= 7;
     502
     503  p_ecc[0] = ~(parit64_1 | parit64_2 | parit32_1 | parit32_2 | parit16_1 | parit16_2 | parit8_1 | parit8_2);
     504  p_ecc[1] = ~(parit1024_1 |parit1024_2 | parit512_1 | parit512_2 | parit256_1 | parit256_2 | parit128_1 | parit128_2);
     505  p_ecc[2] = ~(parit4_1 | parit4_2 | parit2_1 | parit2_2 | parit1_1 | parit1_2);
     506
     507  return 0;
    540508}
    541509
     
    545513static int smc_write(rtems_blkdev_request *req)
    546514{
    547         rtems_blkdev_request_done(req, RTEMS_SUCCESSFUL);
    548         return 0;
     515  rtems_blkdev_request_done(req, RTEMS_SUCCESSFUL);
     516  return 0;
    549517}
    550518
     
    571539        if (count > remains)
    572540            count = remains;
    573         smc_read_page(sg->block,sg->buffer);
     541        smc_read_page(sg->block,sg->buffer);
    574542        remains -= count;
    575543    }
     
    602570                    return smc_read(r);
    603571                case RTEMS_BLKDEV_REQ_WRITE:
    604                     return smc_write(r);
     572                    return smc_write(r);
    605573                default:
    606574                    errno = EINVAL;
Note: See TracChangeset for help on using the changeset viewer.