API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 18th, 2022
68
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 18.52 KB | None | 0 0
raw download clone embed print report
  1. #include <linux/module.h>
  2. #include <linux/kernel.h>
  3. #include <linux/init.h>
  4. #include <linux/slab.h>
  5. #include <linux/scatterlist.h>
  6. #include <linux/delay.h>
  7. #include <linux/mmc/core.h>
  8. #include <linux/mmc/card.h>
  9. #include <linux/mmc/host.h>
  10. #include <linux/mmc/mmc.h>
  11. #include <linux/mmc/rawdatatable.h>
  12.  
  13. //#define   RAWACS_DEBUG
  14.  
  15. #define RAWTABLE_DEV        "mmcblk%d"
  16. #define RAWTABLE_ID         0x44524244
  17. #define RAWTABLE_SIG        0x72617774
  18. #define RAWTABLE_RAW        0
  19. #define RAWTABLE_PART       1
  20. #define RAWTABLE_SIZE       (128 * 1024)
  21.  
  22. typedef struct tagRAWTABLEHEADER {
  23.     unsigned long   id;
  24.     unsigned long   count;
  25.     unsigned char   reserved1[24];
  26.     unsigned char   reserved2[28];
  27.     unsigned long   sig;
  28. }   RAWTABLEHEADER;
  29.  
  30. typedef struct tagRAWDATAENTRY {
  31.     unsigned long   type; // 0 : RAW / 1 : Partition
  32.     unsigned long   start;
  33.     unsigned long   size;
  34.     unsigned char   reserved[20];
  35.     char            label[32];
  36. }   RAWDATAENTRY;
  37.  
  38. #ifdef RAWACS_DEBUG
  39. static bool debug_flg = false;
  40. #endif /* RAWACS_DEBUG */
  41.  
  42. static  int                 mmc_dev_no = -1;
  43. static  char                mmc_dev_name[16];
  44. static  bool                rawdata_init    = false;
  45. static  unsigned long       rawdata_addr    = 0;
  46. static  char*               rawdata_table   = NULL;
  47. static  char*               rawdata_param   = NULL;
  48. static  char*               partition_param = NULL;
  49. static  struct mmc_card*    rawdata_card    = NULL;
  50.  
  51. module_param(mmc_dev_no, int, S_IRUGO);
  52. module_param(rawdata_param, charp, S_IRUGO);
  53. module_param(partition_param, charp, S_IRUGO);
  54.  
  55. static void prepare_mrq(struct mmc_card *card, struct mmc_request *mrq, struct scatterlist *sg, unsigned sg_len, unsigned dev_addr, unsigned blocks, unsigned blksz, int write)
  56. {
  57.     BUG_ON(!mrq || !mrq->cmd || !mrq->data || !mrq->stop);
  58.  
  59.     if (blocks > 1) {
  60.         mrq->cmd->opcode = write ?
  61.             MMC_WRITE_MULTIPLE_BLOCK : MMC_READ_MULTIPLE_BLOCK;
  62.     } else {
  63.         mrq->cmd->opcode = write ?
  64.             MMC_WRITE_BLOCK : MMC_READ_SINGLE_BLOCK;
  65.     }
  66.  
  67.     mrq->cmd->arg = dev_addr;
  68.     if (!mmc_card_blockaddr(card))
  69.         mrq->cmd->arg <<= 9;
  70.  
  71.     mrq->cmd->flags = MMC_RSP_R1 | MMC_CMD_ADTC;
  72.  
  73.     if (blocks == 1)
  74.         mrq->stop = NULL;
  75.     else {
  76.         mrq->stop->opcode = MMC_STOP_TRANSMISSION;
  77.         mrq->stop->arg = 0;
  78.         mrq->stop->flags = MMC_RSP_R1B | MMC_CMD_AC;
  79.     }
  80.  
  81.     mrq->data->blksz = blksz;
  82.     mrq->data->blocks = blocks;
  83.     mrq->data->flags = write ? MMC_DATA_WRITE : MMC_DATA_READ;
  84.     mrq->data->sg = sg;
  85.     mrq->data->sg_len = sg_len;
  86.  
  87.     mmc_set_data_timeout(mrq->data, card);
  88. }
  89.  
  90. static int wait_busy(struct mmc_card *card)
  91. {
  92.     int ret, busy;
  93.     struct mmc_command cmd;
  94.  
  95.     busy = 0;
  96.     do {
  97.         memset(&cmd, 0, sizeof(struct mmc_command));
  98.  
  99.         cmd.opcode = MMC_SEND_STATUS;
  100.         cmd.arg = card->rca << 16;
  101.         cmd.flags = MMC_RSP_R1 | MMC_CMD_AC;
  102.  
  103.         ret = mmc_wait_for_cmd(card->host, &cmd, 0);
  104.         if (ret)
  105.             break;
  106.  
  107.         if (!busy && !(cmd.resp[0] & R1_READY_FOR_DATA)) {
  108.             busy = 1;
  109.             mdelay(1);
  110. //          printk(KERN_INFO "%s: Warning: Host did not "
  111. //              "wait for busy state to end.\n",
  112. //              mmc_hostname(card->host));
  113.         }
  114.     } while (!(cmd.resp[0] & R1_READY_FOR_DATA));
  115.  
  116.     return ret;
  117. }
  118.  
  119. extern int mmc_resume_bus(struct mmc_host *host);
  120. static int transfer(struct mmc_card *card, char *buffer, unsigned addr, unsigned num, int write)
  121. {
  122.     int ret;
  123.  
  124.     struct mmc_request mrq;
  125.     struct mmc_command cmd;
  126.     struct mmc_command stop;
  127.     struct mmc_data data;
  128.  
  129.     struct scatterlist sg;
  130. #if 1 /* E_BOOK *//* for stop sleeping 2011/06/10 */
  131.     extern int dont_suspend_by_IO;
  132.     int flag;
  133. #endif
  134.  
  135. #if 1 /* E_BOOK *//* for stop sleeping 2011/06/10 */
  136.     local_irq_save(flag);
  137.     dont_suspend_by_IO++;
  138.     local_irq_restore(flag);
  139. #endif
  140. #ifdef CONFIG_MMC_BLOCK_DEFERRED_RESUME
  141.     mmc_resume_bus(card->host);
  142. #endif
  143.  
  144. /* 2011/2/7 FY11 : Fixed the bug exclusion control does not work. */
  145.     mmc_claim_host(card->host);
  146.  
  147.     memset(&mrq, 0, sizeof(struct mmc_request));
  148.     memset(&cmd, 0, sizeof(struct mmc_command));
  149.     memset(&data, 0, sizeof(struct mmc_data));
  150.     memset(&stop, 0, sizeof(struct mmc_command));
  151.  
  152.     mrq.cmd = &cmd;
  153.     mrq.data = &data;
  154.     mrq.stop = &stop;
  155.  
  156. #if 1 /* E_BOOK */
  157.     sg_init_one(&sg, buffer, 512*num);
  158. #else
  159.     sg_init_one(&sg, buffer, 512);
  160. #endif
  161.  
  162.     prepare_mrq(card, &mrq, &sg, 1, addr, num, 512, write);
  163.  
  164.     mmc_wait_for_req(card->host, &mrq);
  165.  
  166.     if (cmd.error) {
  167.         mmc_release_host(card->host);
  168. #if 1 /* E_BOOK *//* for stop sleeping 2011/06/10 */
  169.         local_irq_save(flag);
  170.         dont_suspend_by_IO--;
  171.         local_irq_restore(flag);
  172. #endif
  173.         return cmd.error;
  174.     }
  175.     if (data.error) {
  176.         mmc_release_host(card->host);
  177. #if 1 /* E_BOOK *//* for stop sleeping 2011/06/10 */
  178.         local_irq_save(flag);
  179.         dont_suspend_by_IO--;
  180.         local_irq_restore(flag);
  181. #endif
  182.         return data.error;
  183.     }
  184.  
  185.     ret = wait_busy(card);
  186.     mmc_release_host(card->host);
  187. #if 1 /* E_BOOK *//* for stop sleeping 2011/06/10 */
  188.     local_irq_save(flag);
  189.     dont_suspend_by_IO--;
  190.     local_irq_restore(flag);
  191. #endif
  192.     if (ret)
  193.         return ret;
  194.  
  195.     return 0;
  196. }
  197.  
  198. static void cmdline_parse_rawdata(void)
  199. {
  200.     char*   p = NULL;
  201.     char    buf[32];
  202.  
  203.     if(mmc_dev_no != -1 &&
  204.        rawdata_addr != 0)
  205.         return;
  206.  
  207.     p = strstr(saved_command_line, "bootdev=");
  208.     if(p) {
  209.         int i = 0;
  210.         int no = -1;
  211.         p += 8;
  212.         memset(buf, 0, 32);
  213.         for(; i < 31 && *p && *p != 0x20; i++, p++)
  214.             buf[i] = *p;
  215.  
  216.         if(sscanf(buf, "%d", &no) == 1 &&
  217.            (no == 0 || no ==2))
  218.             mmc_dev_no = no;
  219.     }
  220.  
  221.     p = strstr(saved_command_line, "rawtable=");
  222.     if(p) {
  223.         int i = 0;
  224.         unsigned long   addr = (unsigned long)-1;
  225.         p += 9;
  226.         if(strncmp(p, "0x", 2) == 0)
  227.             p += 2;
  228.  
  229.         memset(buf, 0, 32);
  230.         for(; i < 31 && *p && *p != 0x20; i++, p++)
  231.             buf[i] = *p;
  232.  
  233.         if(sscanf(buf, "%lx", &addr) == 1 &&
  234.            addr >= 0x100000 &&
  235.            addr < 0x01800000 &&
  236.            (addr % 512) == 0)
  237.             rawdata_addr = addr;
  238.     }
  239.  
  240.     if(mmc_dev_no == -1 &&
  241.        rawdata_addr == 0) {
  242.         p = strstr(saved_command_line, "nfsroot=");
  243.         if(p) {
  244.             mmc_dev_no = 2;
  245.             rawdata_addr = 0x00F40000;
  246.         }
  247.     }
  248. }
  249.  
  250. void rawdatatable_load(struct mmc_card *card, const char* devname)
  251. {
  252.     char    name[16];
  253.     int     len;
  254.  
  255.     if(rawdata_table)
  256.         return;
  257.  
  258.     cmdline_parse_rawdata();
  259.  
  260.     if(mmc_dev_no == -1 ||
  261.        rawdata_addr == 0)
  262.         return;
  263.  
  264.     snprintf(name, sizeof(name), RAWTABLE_DEV, mmc_dev_no);
  265.     len = strlen(name);
  266.  
  267.     if(strncmp(devname, name, len) == 0 &&
  268.        card->host &&
  269.        card->host->ops &&
  270.        card->host->ops->request) {
  271.         RAWTABLEHEADER* header;
  272.         RAWDATAENTRY*   entry;
  273.         char            buf[128];
  274.         char*           raw_p;
  275.         char*           part_p;
  276.         unsigned long   i;
  277.         unsigned long   raw_size, part_size;
  278.         unsigned long   raw_count, part_count;
  279.  
  280.         printk(KERN_INFO "Raw data table load\n");
  281.  
  282.         rawdata_table = (char*)kmalloc(RAWTABLE_SIZE, GFP_KERNEL);
  283.         if(!rawdata_table) {
  284.             printk(KERN_ERR "  out of memory(%dkB)\n", RAWTABLE_SIZE / 1024);
  285.             return;
  286.         }
  287.  
  288.         if(transfer(card, rawdata_table, rawdata_addr / 512, RAWTABLE_SIZE / 512, 0) != 0) {
  289.             printk(KERN_ERR "  read error\n");
  290.             kfree(rawdata_table);
  291.             rawdata_table = NULL;
  292.             return;
  293.         }
  294.  
  295.         header = (RAWTABLEHEADER*)rawdata_table;
  296.         if(header->id != RAWTABLE_ID ||
  297.            header->count >= ((RAWTABLE_SIZE - sizeof(RAWTABLEHEADER)) / sizeof(RAWDATAENTRY)) ||
  298.            header->sig != RAWTABLE_SIG) {
  299.             printk(KERN_ERR "  header error\n");
  300.             kfree(rawdata_table);
  301.             rawdata_table = NULL;
  302.             return;
  303.         }
  304.  
  305.         raw_count  =
  306.         part_count = 0;
  307.         entry      = (RAWDATAENTRY*)&rawdata_table[sizeof(RAWTABLEHEADER)];
  308.         for(i = 0; i < header->count; i++, entry++) {
  309.             if(entry->type == 0)
  310.                 raw_count++;
  311.             else if(entry->type == 1)
  312.                 part_count++;
  313.         }
  314.  
  315.         raw_size  = raw_count  * 64;
  316.         part_size = part_count * 64;
  317.         rawdata_param   = (char*)kmalloc(raw_size, GFP_KERNEL);
  318.         if(!rawdata_param) {
  319.             printk(KERN_ERR "  out of memory (%lu)\n", raw_size);
  320.             kfree(rawdata_table);
  321.             rawdata_table = NULL;
  322.             return;
  323.         }
  324.         if(part_count > 0) {
  325.             partition_param = (char*)kmalloc(part_size, GFP_KERNEL);
  326.             if(!partition_param) {
  327.                 printk(KERN_ERR "  out of memory (%lu)\n", part_size);
  328.                 kfree(rawdata_param);
  329.                 kfree(rawdata_table);
  330.                 rawdata_param = NULL;
  331.                 rawdata_table = NULL;
  332.                 return;
  333.             }
  334.         }
  335.  
  336.         entry  = (RAWDATAENTRY*)&rawdata_table[sizeof(RAWTABLEHEADER)];
  337.         raw_p  = rawdata_param;
  338.         part_p = partition_param;
  339.         for(i = 0; i < header->count; i++, entry++) {
  340.             int             len;
  341.             unsigned long   size;
  342.  
  343.             if(entry->type == 0) {
  344.                 snprintf(buf, sizeof(buf), "%-32s:0x%08lx:0x%08lx\n", entry->label, entry->start, entry->size);
  345.                 size = raw_size;
  346.             } else if(entry->type == 1) {
  347.                 snprintf(buf, sizeof(buf), "%-32s:%lu:0x%08lx\n", entry->label, entry->start, entry->size);
  348.                 size = part_size;
  349.             } else
  350.                 continue;
  351.  
  352.             len = strlen(buf);
  353.             if(size < len + 1) {
  354.                 printk(KERN_ERR "  code error\n");
  355.                 kfree(partition_param);
  356.                 kfree(rawdata_param);
  357.                 kfree(rawdata_table);
  358.                 partition_param = NULL;
  359.                 rawdata_param   = NULL;
  360.                 rawdata_table   = NULL;
  361.                 return;
  362.             }
  363.  
  364.             if(entry->type == 0) {
  365.                 strncpy(raw_p, buf, size);
  366.                 raw_p    += len;
  367.                 raw_size -= len;
  368.             } else {
  369.                 strncpy(part_p, buf, size);
  370.                 part_p    += len;
  371.                 part_size -= len;
  372.             }
  373.         }
  374.  
  375.         rawdata_card = card;
  376.         strncpy(mmc_dev_name, devname, sizeof(mmc_dev_name));
  377.         rawdata_init = true;
  378.     }
  379.  
  380. #ifdef RAWACS_DEBUG
  381.     {
  382.         char*   buffer = (char*)kmalloc(8192, GFP_KERNEL);
  383.         if(!buffer) {
  384.             printk("#### TEST : out of memory\n");
  385.             return;
  386.         }
  387.         memset(buffer, 0, 8192);
  388.  
  389.         while(1) {
  390.             unsigned long   next_start = 0;
  391.             unsigned long   count, *p;
  392.             unsigned long   size  = 0;
  393.             unsigned long   index = rawdata_index(RAWLABEL_LOG, &size);
  394.             if(index == (unsigned long)-1) {
  395.                 printk("#### TEST : open error\n");
  396.                 break;
  397.             }
  398.  
  399.             debug_flg = true;
  400.  
  401.             printk("#### TEST : short... %08lx\n", next_start);
  402.             p = (unsigned long*)buffer;
  403.             *p = (unsigned long)buffer;
  404.             for(count = 0; count < 512; count += sizeof(long)) {
  405.                 unsigned long offset    = next_start + count * 512 + count;
  406.                 unsigned long ret       = (unsigned long)rawdata_write(index, offset, buffer, sizeof(long));
  407.                 if(ret != sizeof(long)) {
  408.                     printk("  write error (%lu) : offset=%lx 4 : ret=%lu\n", count, offset, ret);
  409.                 }
  410.             }
  411.             p = (unsigned long*)&buffer[512];
  412.             memset(p, 0, 512);
  413.             for(count = 0; count < 512; count += sizeof(long)) {
  414.                 unsigned long offset    = next_start + count * 512 + count;
  415.                 unsigned long ret       = (unsigned long)rawdata_read(index, offset, (char*)p, sizeof(long));
  416.                 if(ret != sizeof(long)) {
  417.                     printk("  read error (%lu) : offset=%lx 4 : ret=%lu\n", count, offset, ret);
  418.                 } else if(*p != (unsigned long)buffer) {
  419.                     printk("  verify error (%lu) : offset=%lx 4 : %lx != %lx\n", count, offset, *p, (unsigned long)buffer);
  420.                 }
  421.                 memset(p, 0, 512);
  422.             }
  423.             next_start += (512 * 512 + 1024);
  424.  
  425.             printk("#### TEST : between sector 512byte... %08lx\n", next_start);
  426.             memcpy(buffer, rawdata_table, 512);
  427.             for(count = 0; count < 512; count++) {
  428.                 unsigned long offset    = next_start + count * 1024 + count;
  429.                 unsigned long ret       = (unsigned long)rawdata_write(index, offset, buffer, 512);
  430.                 if(ret != 512) {
  431.                     printk("  write error (%lu) : offset=%lx 512 : ret=%lu\n", count, offset, ret);
  432.                 }
  433.             }
  434.             p = (unsigned long*)&buffer[512];
  435.             memset(p, 0, 512);
  436.             for(count = 0; count < 512; count++) {
  437.                 unsigned long offset    = next_start + count * 1024 + count;
  438.                 unsigned long ret       = (unsigned long)rawdata_read(index, offset, (char*)p, 512);
  439.                 if(ret != 512) {
  440.                     printk("  read error (%lu) : offset=%lx 512 : ret=%lu\n", count, offset, ret);
  441.                 } else if(memcmp(p, rawdata_table, 512) != 0) {
  442.                     printk("  verify error (%lu) : offset=%lx 512\n", count, offset);
  443.                 }
  444.                 memset(p, 0, 512);
  445.             }
  446.             next_start += (512 * 1024 + 1024);
  447.  
  448.             printk("#### TEST : between sector 1024byte... %08lx\n", next_start);
  449.             memcpy(buffer, rawdata_table, 1024);
  450.             for(count = 0; count < 512; count++) {
  451.                 char    c = buffer[count];
  452.                 buffer[count] = buffer[1024 - count - 1];
  453.                 buffer[1024 - count - 1] = c;
  454.             }
  455.             memcpy(&buffer[1024], buffer, 1024);
  456.             for(count = 0; count < 512; count++) {
  457.                 unsigned long offset    = next_start + count * 2048 + count;
  458.                 unsigned long ret;
  459.  
  460.                 printk("Write (%lu) : offset=%lx 1024\n", count, offset);
  461.                 ret = (unsigned long)rawdata_write(index, offset, buffer, 1024);
  462.                 if(ret != 1024) {
  463.                     printk("  error : ret=%lu\n", ret);
  464.                 }
  465.  
  466.                 ret = (unsigned long)rawdata_read(index, (offset / 512) * 512, &buffer[4096], 2048);
  467.                 if(memcmp(&buffer[4096 + (offset % 512)], &buffer[1024], 1024) != 0) {
  468.                     printk("  verify error\n");
  469.                 }
  470.             }
  471.             p = (unsigned long*)buffer;
  472.             memset(p, 0, 1024);
  473.             for(count = 0; count < 512; count++) {
  474.                 unsigned long offset    = next_start + count * 2048 + count;
  475.                 unsigned long ret       = (unsigned long)rawdata_read(index, offset, (char*)p, 1024);
  476.                 if(ret != 1024) {
  477.                     printk("  read error (%lu) : offset=%lx 1024 : ret=%lu\n", count, offset, ret);
  478.                 } else if(memcmp(p, &buffer[1024], 1024) != 0) {
  479.                     printk("  verify error (%lu) : offset=%lx 1024\n", count, offset);
  480.                 }
  481.                 memset(p, 0, 1024);
  482.             }
  483.  
  484.             break;
  485.         }
  486.         debug_flg = false;
  487.         kfree(buffer);
  488.     }
  489. #endif /* RAWACS_DEBUG */
  490. }
  491.  
  492. bool is_rawinit(void)
  493. {
  494.     return rawdata_init;
  495. }
  496. EXPORT_SYMBOL(is_rawinit);
  497.  
  498. bool is_rawdatadev(const char* devname)
  499. {
  500.     return  (strncmp(mmc_dev_name, devname, strlen(devname)) == 0);
  501. }
  502. EXPORT_SYMBOL(is_rawdatadev);
  503.  
  504. unsigned long   rawdata_index(const char* label, unsigned long* size)
  505. {
  506.     unsigned long   index = -1;
  507.  
  508.     if(rawdata_table) {
  509.         RAWTABLEHEADER* header = (RAWTABLEHEADER*)rawdata_table;
  510.         RAWDATAENTRY*   entry  = (RAWDATAENTRY*)&rawdata_table[sizeof(RAWTABLEHEADER)];
  511.         int             len    = strlen(label);
  512.         unsigned long   i;
  513.  
  514.         for(i = 0; i < header->count; i++, entry++) {
  515.             if(strncmp(label, entry->label, len) == 0) {
  516.                 index = i;
  517.                 *size = entry->size;
  518.                 break;
  519.             }
  520.         }
  521.     }
  522.  
  523.     return  index;
  524. }
  525. EXPORT_SYMBOL(rawdata_index);
  526.  
  527. int rawdata_read(unsigned long index, unsigned long offset, char* buffer, unsigned long size)
  528. {
  529.     RAWTABLEHEADER* header = (RAWTABLEHEADER*)rawdata_table;
  530.     RAWDATAENTRY*   entry;
  531.     unsigned        off;
  532.     unsigned        num;
  533.     unsigned        block;
  534.     unsigned        max_block;
  535.     int             ret = -1;
  536.     char            buf[512];
  537.     char*           p = buffer;
  538.  
  539.     if(!rawdata_table ||
  540.        index >= header->count) {
  541.         return ret;
  542.     }
  543.  
  544.     entry  = (RAWDATAENTRY*)&rawdata_table[sizeof(RAWTABLEHEADER) + sizeof(RAWDATAENTRY) * index];
  545.     offset += entry->start;
  546.     if(offset < entry->start ||
  547.        offset >= (entry->start + entry->size)) {
  548.         return ret;
  549.     }
  550.  
  551.     if((offset + size) > (entry->start + entry->size)) {
  552.         size = (entry->start + entry->size) - offset;
  553.     }
  554.  
  555.     if(offset % 512) {
  556.         unsigned in = (unsigned)(offset % 512);
  557.         unsigned re = (unsigned)(512 - in);
  558.         if(re > size)
  559.             re = size;
  560.  
  561.         off = (unsigned)(offset / 512);
  562.         num = 1;
  563.         if(transfer(rawdata_card, buf, off, num, 0) != 0) {
  564.             return ret;
  565.         }
  566.  
  567.         if(ret < 0)
  568.             ret = 0;
  569.         memcpy(p, &buf[in], re);
  570.         p      += re;
  571.         offset += (unsigned long)re;
  572.         size   -= (unsigned long)re;
  573.         ret    += (int)re;
  574.         if(size <= 0)
  575.             return ret;
  576.     }
  577.  
  578.     max_block = rawdata_card->host->max_req_size / 512;
  579.     if(max_block > rawdata_card->host->max_blk_count)
  580.         max_block = rawdata_card->host->max_blk_count;
  581.  
  582.     num = (unsigned)(size / 512);
  583.     for(block = 0; block < num; block += max_block) {
  584.         unsigned    acs_block;
  585.         acs_block = num - block;
  586.         if(acs_block > max_block)
  587.             acs_block = max_block;
  588.  
  589.         off = (unsigned)(offset / 512);
  590.         if(transfer(rawdata_card, p, off, acs_block, 0) != 0) {
  591.             return ret;
  592.         }
  593.  
  594.         if(ret < 0)
  595.             ret = 0;
  596.         acs_block *= 512;
  597.         p         += acs_block;
  598.         offset    += (unsigned long)acs_block;
  599.         size      -= (unsigned long)acs_block;
  600.         ret       += (int)acs_block;
  601.         if(size <= 0)
  602.             return ret;
  603.     }
  604.  
  605.     if(size > 0) {
  606.         off = (unsigned)(offset / 512);
  607.         num = 1;
  608.         if(transfer(rawdata_card, buf, off, num, 0) != 0) {
  609.             return ret;
  610.         }
  611.  
  612.         memcpy(p, buf, size);
  613.  
  614.         if(ret < 0)
  615.             ret = 0;
  616.         ret += (int)size;
  617.     }
  618.  
  619.     return ret;
  620. }
  621. EXPORT_SYMBOL(rawdata_read);
  622.  
  623. int rawdata_write(unsigned long index, unsigned long offset, char* buffer, unsigned long size)
  624. {
  625.     RAWTABLEHEADER* header = (RAWTABLEHEADER*)rawdata_table;
  626.     RAWDATAENTRY*   entry;
  627.     unsigned        off;
  628.     unsigned        num;
  629.     unsigned        block;
  630.     unsigned        max_block;
  631.     int             ret = -1;
  632.     char            buf[512];
  633.     char*           p = buffer;
  634.  
  635.     if(!rawdata_table ||
  636.        index >= header->count) {
  637.         return ret;
  638.     }
  639.  
  640.     entry  = (RAWDATAENTRY*)&rawdata_table[sizeof(RAWTABLEHEADER) + sizeof(RAWDATAENTRY) * index];
  641.     offset += entry->start;
  642.     if(offset < entry->start ||
  643.        offset >= (entry->start + entry->size)) {
  644.         return ret;
  645.     }
  646.  
  647.     if((offset + size) > (entry->start + entry->size)) {
  648.         size = (entry->start + entry->size) - offset;
  649.     }
  650.  
  651.     if(offset % 512) {
  652.         unsigned in = (unsigned)(offset % 512);
  653.         unsigned re = (unsigned)(512 - in);
  654.         if(re > size)
  655.             re = size;
  656.  
  657.         off = (unsigned)(offset / 512);
  658.         num = 1;
  659. #ifdef  RAWACS_DEBUG
  660.         if(debug_flg) {
  661.             printk("    %s #1 : offset=%x pos=%u size=%u p=%p\n", __FUNCTION__, off, in, re, p);
  662.         }
  663. #endif /* RAWACS_DEBUG */
  664.         if(transfer(rawdata_card, buf, off, num, 0) != 0) {
  665.             return ret;
  666.         }
  667.  
  668.         memcpy(&buf[in], p, re);
  669.         if(transfer(rawdata_card, buf, off, num, 1) != 0) {
  670.             return ret;
  671.         }
  672.  
  673.         if(ret < 0)
  674.             ret = 0;
  675.         p      += re;
  676.         offset += (unsigned long)re;
  677.         size   -= (unsigned long)re;
  678.         ret    += (int)re;
  679.         if(size <= 0)
  680.             return ret;
  681.     }
  682.  
  683.     max_block = rawdata_card->host->max_req_size / 512;
  684.     if(max_block > rawdata_card->host->max_blk_count)
  685.         max_block = rawdata_card->host->max_blk_count;
  686.  
  687.     num = (unsigned)(size / 512);
  688.     for(block = 0; block < num; block += max_block) {
  689.         unsigned    acs_block;
  690.         acs_block = num - block;
  691.         if(acs_block > max_block)
  692.             acs_block = max_block;
  693.  
  694.         off = (unsigned)(offset / 512);
  695. #ifdef  RAWACS_DEBUG
  696.         if(debug_flg) {
  697.             printk("    %s #2 : offset=%x size=%u p=%p\n", __FUNCTION__, off, acs_block * 512, p);
  698.         }
  699. #endif /* RAWACS_DEBUG */
  700.         if(transfer(rawdata_card, p, off, acs_block, 1) != 0) {
  701.             return ret;
  702.         }
  703.  
  704.         if(ret < 0)
  705.             ret = 0;
  706.         acs_block *= 512;
  707.         p         += acs_block;
  708.         offset    += (unsigned long)acs_block;
  709.         size      -= (unsigned long)acs_block;
  710.         ret       += (int)acs_block;
  711.         if(size <= 0)
  712.             return ret;
  713.     }
  714.  
  715.     if(size > 0) {
  716.         off = (unsigned)(offset / 512);
  717.         num = 1;
  718. #ifdef  RAWACS_DEBUG
  719.         if(debug_flg) {
  720.             printk("    %s #3 : offset=%x size=%u p=%p\n", __FUNCTION__, off, size, p);
  721.         }
  722. #endif /* RAWACS_DEBUG */
  723.         if(transfer(rawdata_card, buf, off, num, 0) != 0) {
  724.             return ret;
  725.         }
  726.  
  727.         memcpy(buf, p, size);
  728.  
  729.         if(transfer(rawdata_card, buf, off, num, 1) != 0) {
  730.             return ret;
  731.         }
  732.  
  733.         if(ret < 0)
  734.             ret = 0;
  735.         ret += (int)size;
  736.     }
  737.  
  738.     return ret;
  739. }
  740.  
  741.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!