API tools faq
paste
Guest User

AHCI init and IDENTIFY DEVICE

a guest
Jan 9th, 2016
721
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 30.48 KB | None | 0 0
raw download clone embed print report
  1. /* ahci.c : Contains the implementation of the AHCI HBA driver. (GPLv2)
  2.  * */
  3.  
  4. #include "kernel.h"
  5. #include "kdata.h"
  6. #include "pci_regs.h" /* defines from Linux uapi headers */
  7. #include "pci.h"
  8. #include "ahci.h"
  9. #include "module.h"
  10.  
  11. #define AHCIBABBLE
  12. #ifdef AHCIBABBLE
  13. #define abprintf printf
  14. #else
  15. #define abprintf noprintf
  16. #endif
  17.  
  18. struct ale {
  19.         struct list_head devs;
  20.         struct ahci_device* dev;
  21. } ahci_dl = { .dev = NULL, .devs = LIST_HEAD_INIT(ahci_dl.devs) };
  22.  
  23. /* BITFIELDS GENERAL: writing to lower 8/16 bits can cause BYTE/WORD access
  24.  * which is invalid -> use -fstict-voatile-bitfields for gcc -- */
  25.  
  26. #ifndef __clang__
  27. STATIC_ASSERT(sizeof(struct ahci_port)==0x80, ahci_port_not_80h_bytes);
  28. STATIC_ASSERT(sizeof(struct ahci_hba)==0x180, ahci_hba_not_180h_bytes);
  29. STATIC_ASSERT(sizeof(struct ahci_cmdlst)==0x400, ahci_hba_not_400h_bytes);
  30. STATIC_ASSERT(sizeof(struct ahci_cmdtbl)==0x80, ahci_cmdtbl_not_80h_bytes);
  31. STATIC_ASSERT(sizeof(struct ahci_prdt)==0x10, ahci_prdt_not_10h_bytes);
  32. STATIC_ASSERT(sizeof(struct ahci_fis)==0x100, ahci_prdt_not_100h_bytes);
  33. STATIC_ASSERT(sizeof(struct sata_reg_h2d)==0x14, sata_reg_h2d_not_14h_bytes);
  34. #endif
  35.  
  36. /* internal commands */
  37. enum disk_cmd {
  38.         disk_cmd_identify,
  39.         disk_cmd_read,
  40.         disk_cmd_write,
  41.         disk_cmd_trim
  42. };
  43.  
  44. /* this is the concat of three structs pending/proc/ctx */
  45. struct cmd_ctx {
  46.         struct list_head lst;
  47.         enum disk_cmd c;
  48.         /* pending cmd: initial values */
  49.         uint64_t lba;
  50.         uint64_t ct;
  51.         void* ptr;
  52.         uint16_t pmprt;
  53.         /* proc cmd: during execution */
  54.         struct wait_queue_head* wq_wake;
  55.         uint16_t port;
  56.         uint16_t slot;
  57.         void* cur_buf;
  58.         void* cont_ctx; /* specific continuation context */
  59.         uint64_t bts_left;
  60.         /* cmd_ctx: command build context */
  61.         struct {
  62.                 uint16_t prdtl;
  63.                 uint16_t flg; /* PMP R C B R P W A CFL */
  64.         } clh;
  65.         struct ahci_ct_e* entry;
  66.         uint32_t max_prdtl;
  67. }; /* 70bt size -> slab allocator? */
  68.  
  69. struct ahci_ct_pool {
  70.         uint32_t prdt_cap;
  71.         struct ahci_ct_pool* next;
  72.         uint32_t mask; /* not alloc always 1 */
  73.         struct ahci_ct_e {
  74.                 uint16_t port, slot; /* dbg info? */
  75.                 struct ahci_cmdtbl* ptr;
  76.                 uint64_t pmem;
  77.         } tables[32];
  78. };
  79.  
  80. /* The IDENTIFY (PACKET) commands
  81.  * */
  82.  
  83. /* will (almost) always succeed */
  84. int ata_build_device_identify(struct ahci_device* ad, struct cmd_ctx* ctx)
  85. {
  86.         struct ahci_cmdtbl* t = ctx->entry->ptr;
  87.         struct sata_reg_h2d* r = &t->cfis.rh2d;
  88.         void* ptr = ctx->ptr; uint64_t b1, b2 = 0;
  89.         uint16_t bc1 = 512, bc2 = 0;
  90.  
  91.         /* we use exactly one PRD except someone manages
  92.          * to send two pages in ptr for 512 bytes */
  93.         if (ctx->max_prdtl < 1)
  94.                 return -1;
  95.  
  96.         /* build the FIS */
  97.         bzero(t, sizeof(*t) + ctx->max_prdtl * sizeof(*t->prdts));
  98.         r->fis_type = FIS_REG_H2D;
  99.         r->pm_port = ctx->pmprt;
  100.         r->res0 = 0;
  101.         r->c = 1;
  102.         r->command = ATA_CMD_IDENTIFY_DEVICE;
  103.         r->feat_high = r->feat_low = 0; /* N/A */
  104.         r->lba_high = r->lba_low = 0; /* N/A */
  105.         r->count = 0; /* N/A */
  106.         r->device = 0; /* bit4 is transport dependent */
  107.         r->icc = 0;
  108.         r->command = 0;
  109.         r->res1 = 0;
  110.  
  111.         /* get the target memory */
  112.         if (!ptr) {
  113.                 /* we use the device buffers */
  114.                 ptr = ad->identify_data[ctx->entry->port].ptr;
  115.                 b1 = ad->identify_data[ctx->entry->port].pm;
  116.         } else {
  117.                 size_t ofs = (size_t)ptr % 4096;
  118.                 /* we have to get the offset ourself */
  119.                 b1 = mmgr_page_get_pmem(ptr, NULL);
  120.                 if (4096 - ofs < 512) { /* across pages */
  121.                         bc1 = 4096 - ofs;
  122.                         bc2 = 512 - (4096 - ofs);
  123.                         b2 = mmgr_page_get_pmem(ptr + bc1, NULL);
  124.                         /* but contiguous */
  125.                         if (b2 == b1 + 4096) {
  126.                                 bc1 = 512; bc2 = 0;
  127.                                 b2 = 0;
  128.                         }
  129.                 }
  130.                 b1 += ofs;
  131.         }
  132.  
  133.         /* fill the cmdtbl */
  134.         abprintf("IDENTIFY: %016llx %p\n", ctx->entry->pmem, t);
  135.         abprintf("b1: %016llx\n", b1);
  136.         t->prdts[0].dba64 = b1;
  137.         t->prdts[0].dbc = bc1 - 1; /* IDENTIFY returns 512bt */
  138.         t->prdts[0].I = 1; /* interrupt on completion */
  139.         if (b2) {
  140.                 if (ctx->max_prdtl < 2)
  141.                         return -1;
  142.                 t->prdts[1].dba64 = b2;
  143.                 t->prdts[1].dbc = bc2 - 1; /* IDENTIFY returns 512bt */
  144.                 t->prdts[1].I = 1; /* interrupt on completion */
  145.                 t->prdts[0].I = 0; /* not too early */
  146.         }
  147.  
  148.         /* and set the context */
  149.         ctx->clh.prdtl = b2 ? 2 : 1;
  150.         /* PMP; C (clear BSY,CI); 14h bt=5dw length*/
  151.         ctx->clh.flg = 0x0405 | ((ctx->pmprt & 0xf) << 12);
  152.         /* no further actions required, save for cleanup and waking */
  153.         ctx->cont_ctx = NULL;
  154.         ctx->cur_buf = NULL;
  155.         ctx->bts_left = 0;
  156.         return 0;
  157. }
  158.  
  159. /* will (almost) always succeed */
  160. int atapi_build_packet_device_identify(struct ahci_device* ad,
  161.                 struct cmd_ctx* ctx)
  162. {
  163.         printf("ATAPI IDENTIFY PACKET DEVICE %p %p\n", ad, ctx);
  164.         return -1;
  165. }
  166.  
  167. /* The READ_DMA_EXT commands
  168.  * */
  169.  
  170. /* The WRITE_DMA_EXT commands
  171.  * */
  172.  
  173. /* The SATA TRIM command
  174.  * */
  175.  
  176. const char* capstr(int idx, int limit, ...)
  177. {
  178.         const char** rv = (const char**)(&limit + 1);
  179.         return rv[min(idx, limit)];
  180. }
  181.  
  182. void ahci_handle_interrupt(struct ahci_device* d, unsigned int irq)
  183. {
  184.         /* irq is for MSI support /w base */
  185.         printf("intr %p %u\n", d, irq);
  186. }
  187.  
  188. int ahci_irq(unsigned int irq)
  189. {
  190.         struct ale* d;
  191.  
  192.         printf("intr %u\n", irq);
  193.         /*lock_list(ahci_dl.devs);*/
  194.         list_for_each_entry(d, &ahci_dl.devs, devs) {
  195.                 if (d->dev->dev->intr_line == irq) {
  196.                         if (pci_check_interrupt(d->dev->dev)) {
  197.                                 ahci_handle_interrupt(d->dev, irq);
  198.                                 return 1;
  199.                         }
  200.                 }
  201.         }
  202.         /*unlock_list(ahci_dl.devs);*/
  203.         return 0;
  204. }
  205.  
  206. int ahci_find_slot(volatile struct ahci_port* p, int cmd_ct)
  207. {
  208.         uint32_t bitmask = (p->pXci/* NCQ: | p->pXsact*/);
  209.         for (int i = 0; i < cmd_ct; i++) {
  210.                 if (!(bitmask & 1))
  211.                         return i;
  212.                 bitmask >>= 1;
  213.         }
  214.         return -1;
  215. }
  216.  
  217. void ahci_release_cmdtbl(struct ahci_device* ad, struct ahci_ct_e* entry)
  218. {
  219.         struct ahci_ct_pool* ctp = ad->ctbl_pool;
  220.  
  221.         while (ctp) {
  222.                 ptrdiff_t ofs = ctp->tables - entry;
  223.                 if (ofs >= 0 && ofs < 32) {
  224.                         uint16_t p = entry->port, slt = entry->slot;
  225.                         /* we've found the entry */
  226.                         entry->port = entry->slot = 0xffff;
  227.                         if (!((1 << ofs) & __sync_fetch_and_and(&ctp->mask,
  228.                                 ~(1 << ofs)))) {
  229.                                 /* already cleared */
  230.                                 abprintf("attempt to release command table at %p"
  231.                                         " for port %u slot %u which was not "
  232.                                         " acquired. Its offset is %ld.\n",
  233.                                         entry->ptr, p, slt, ofs);
  234.                         }
  235.                         break;
  236.                 }
  237.                 ctp = ctp->next;
  238.         }
  239. }
  240.  
  241. struct ahci_ct_e* ahci_find_cmdtbl(struct ahci_device* ad, uint32_t prdt_ct)
  242. {
  243.         struct ahci_ct_pool* ctp = ad->ctbl_pool;
  244.  
  245.         while (ctp) {
  246.                 if (ctp->prdt_cap >= prdt_ct) {
  247.                         int bit;
  248. Pos1:
  249.                         bit = bsf(~ctp->mask); /* 1 based */
  250.                         if (bit--) {
  251.                                 /* one set (i.e. unset bit) */
  252.                                 if (!((1 << bit) & __sync_fetch_and_or(&ctp->mask, 1 << bit))) {
  253.                                         /* we have a slot */
  254.                                         return &ctp->tables[bit];
  255.                                 } else /* already set by a competitor */
  256.                                         goto Pos1;
  257.                         }
  258.                 }
  259.                 /* advance */
  260.                 ctp = ctp->next;
  261.         }
  262.  
  263.         /* no tables avaiable despite there being slots free */
  264.         /* maybe we should ask for more tables? */
  265.         static uint64_t warn_time = 0;
  266.         if (jiffies - warn_time > HZ * 300) {
  267.                 /* warn every 5 minutes with continuous exhaustion */
  268.                 warn_time = jiffies;
  269.                 cprintf(KFMT_INFO, "Command table pool exhausted.\nI/O performance"
  270.                                 " could suffer from this.\n");
  271.         }
  272.  
  273.         return NULL;
  274. }
  275.  
  276. void ahci_process_command(struct ahci_device* ad,
  277.                 struct ahci_port* p, struct list_head* lh)
  278. {
  279.         struct cmd_ctx* cc; int slt, rv; uint32_t ct;
  280.         struct ahci_cmdlst* cl = ad->command_lists[p - ad->ptr->port];
  281.  
  282.         /* sending a command to a device with CR=0? */
  283.         if (!cl)
  284.                 abprintf("Ignoring command on port %zd as it's command"
  285.                         " engine is not running.\n", p - ad->ptr->port);
  286.  
  287. next_cmd:
  288.         slt = ahci_find_slot(p, ad->num_cmd); /* does not reserve -> no cleanup */
  289.         if (slt == -1) /* still full */
  290.                 return;
  291.  
  292.         abprintf("processing command in slot %d on port %zd\n", slt, p - ad->ptr->port);
  293.         /* we've acquired a slot */
  294.         lock_list(*lh);
  295.         cc = list_first_entry(lh, struct cmd_ctx, lst);    
  296.         unlock_list(*lh);
  297.         list_del(&cc->lst, lh);
  298.         if (!cc) /* the command vanished? */
  299.                 return;
  300.  
  301.         /* get the PRDT count needed */
  302.         if (cc->ptr) {
  303.                 /*void* ptr2; */uint64_t s = cc->ct;
  304.                 /* we'll build a scatter list and then get a
  305.                  * count and throw the results away? insane! */
  306.                 ct = rdiv(s, 4096) + 1; /* first guess */
  307.                 if (ct > 8) {
  308.                         /* now we do this */
  309.                         die("implement this later.\n");
  310.                 }
  311.         } else if (cc->c == disk_cmd_identify) {
  312.                 ct = 1;
  313.         } else
  314.                 ct = 0;
  315.  
  316.         /* now acquire a command table with the matching requirements */
  317.         /* limit is 4MiB per PRDT (BUT! PM!=contiguous) */
  318. find:
  319.         cc->entry = ahci_find_cmdtbl(ad, ct);
  320.         abprintf("find ok\n");
  321.         if (cc->entry) {
  322.                 cc->entry->port = cc->port;
  323.                 cc->entry->slot = slt;
  324.                 cc->slot = slt;
  325.         } else
  326.                 abprintf("no cmdtbl with %u PRDTs capacity found.\n", ct);
  327.  
  328.         if (!cc->entry && ct > 8) { /* divide by twoish */
  329.                 int log = bsr(ct) - 1;
  330.                 uint32_t new_ct = (1 << log);
  331.                 if (new_ct == ct)
  332.                         new_ct >>= 1;
  333.                 ct = new_ct;
  334.                 goto find;
  335.         } else if (!cc->entry) { /* none available */
  336.                 /* skip command processing on this port */
  337.                 abprintf("waiting for cmdtbl to become available.\n");
  338.                 list_add(&cc->lst, lh);
  339.                 return;
  340.         }
  341.  
  342.         abprintf("building command in cmdtbl at %p for port %u slot %u\n",
  343.                 cc->entry->ptr, cc->entry->port, cc->entry->slot);
  344.         /* now build the command */
  345.         cc->max_prdtl = ct;
  346.         if (p->cmd.atapi) { /* ATAPI */
  347.                 switch (cc->c) {
  348.                         case disk_cmd_identify:
  349.                                 rv = atapi_build_packet_device_identify(ad, cc);
  350.                                 break;
  351.                         case disk_cmd_read:
  352.                         case disk_cmd_write:
  353.                         case disk_cmd_trim:
  354.                                 die("not implemented yet!\n");
  355.                                 break;
  356.                 }
  357.         } else {
  358.                 switch (cc->c) { /* ATA */
  359.                         case disk_cmd_identify:
  360.                                 rv = ata_build_device_identify(ad, cc);
  361.                                 break;
  362.                         case disk_cmd_read:
  363.                         case disk_cmd_write:
  364.                         case disk_cmd_trim:
  365.                                 die("not implemented yet!\n");
  366.                                 break;
  367.                 }
  368.         }
  369.  
  370.         /* only issue if rv==0 */
  371.         if (rv) { /* cleanup */
  372.                 abprintf("ignored malformed command on port %u\n", cc->port);
  373.                 ahci_release_cmdtbl(ad, cc->entry);
  374.                 kfree(cc);
  375.         } else if (!(p->pXci & (1 << slt))) {/* issue it on the port p slot slt */
  376.                 struct ahci_cmdhdr* ch = &ad->command_lists[cc->port]->chdrs[slt];
  377.                 bzero(ch, sizeof(*ch));
  378.                 ch->ctba64 = cc->entry->pmem;
  379.                 ch->prdtl = cc->clh.prdtl;
  380.                 ch->flags = cc->clh.flg;
  381.                 /* now enable the slot */
  382.                 p->pXci |= (1 << slt);
  383.                 abprintf("issued the command successfully.\n");
  384.                 /* and tell the IH about it */
  385.                 list_add_tail(&cc->lst, &ad->proc_cmds[cc->port]);
  386.         } else {
  387.                 /* slot became used: parallel access to
  388.                  * inherently single threaded structure? */
  389.                 die("error posting command\n");
  390.         }
  391.  
  392.         /* process next command */
  393.         __sync_fetch_and_sub(&ad->pending_ct, 1);
  394.         if (!list_empty(lh))
  395.                 goto next_cmd;
  396. }
  397.  
  398. void ahci_worker(void* dat)
  399. {
  400.         struct ahci_device* ad = dat;
  401.  
  402.         printf("AHCI worker is running\n");
  403.         while (1) {
  404.                 uint32_t ps = ad->ptr->ports;
  405.                 printf("IS: %x ae: %u ie: %u\n", ad->ptr->intr,
  406.                         ad->ptr->ctrl.ae, ad->ptr->ctrl.ie);
  407.                 for (int i = 0; i < 32; i++)
  408.                         if (ps & (1 << i)) {
  409.                                 printf("P%d det: %u ipm: %u serr: %x\n", i, ad->ptr->port[i].scr0.det,
  410.                                         ad->ptr->port[i].scr0.ipm, ad->ptr[i].port[i].pXserr);
  411.                                 printf("P%d st: %u cr: %u is: %x ie: %x sts: %x err: %x ci: %x sa: %x\n",
  412.                                         i, ad->ptr->port[i].cmd.st, ad->ptr->port[i].cmd.cr,
  413.                                         ad->ptr->port[i].pXis, ad->ptr->port[i].pXie,
  414.                                         ad->ptr->port[i].tfd.sts, ad->ptr->port[i].tfd.err,
  415.                                         ad->ptr->port[i].pXci, ad->ptr->port[i].pXsact);
  416.                                 /* print the cmdlst */
  417.                                 printf("CL: %016llx %016llx %p\n", ad->ptr->port[i].pXclb64,
  418.                                         mmgr_page_get_pmem(ad->command_lists[i], NULL), ad->command_lists[i]);
  419.                                 /* print CH0 */
  420.                                 printf("CH0: %016llx flg: %x prdtl: %u bc: %u\n", ad->command_lists[i]->chdrs[0].ctba64,
  421.                                         ad->command_lists[i]->chdrs[0].flags, ad->command_lists[i]->chdrs[0].prdtl,
  422.                                         ad->command_lists[i]->chdrs[0].prdbc);
  423.                                 /* Identify data */
  424.                                 uint32_t* pti = ad->identify_data[i].ptr;
  425.                                 printf("ID: %08x %08x %08x %08x\n", pti[0], pti[1],
  426.                                         pti[2], pti[3]);
  427.  
  428.                         }
  429.                 sleep(5);
  430.  
  431.                 /* there are pending commands */
  432.                 if (ad->pending_ct) {
  433.                         for (int i = 0; i < 32; i++) {
  434.                                 if (!(ps & (1 << i)))
  435.                                         continue;
  436.                                 if (list_empty(&ad->pending_cmds[i]))
  437.                                         continue;
  438.                                 /* we have commands to process */
  439.                                 ahci_process_command(ad, &ad->ptr->port[i],
  440.                                         &ad->pending_cmds[i]);
  441.                         }
  442.                 }
  443.         }
  444. }
  445.  
  446. void* ahci_allocate_command_table_pool(uint32_t ct, uint32_t prdt_cap, int bit64)
  447. {
  448.         /* 128bt aligned size (8 PRDT/128 bt) */
  449.         struct ahci_ct_pool* rv; uint32_t i = 0;
  450.         size_t byte_ct, page_rest, indiv_ct, pg_pe, pg_ct, epa;
  451.         struct page_range pr; int flg = MMGR_ALLOC_CONTIGUOUS;
  452.         if (!bit64)
  453.                 flg = MMGR_ALLOC_32_BIT;
  454.         if (!(rv = kzalloc(sizeof(*rv))))
  455.                 return NULL;
  456.        
  457.         /* bitmask limit */
  458.         if (ct > 32)
  459.                 ct = 32;
  460.         /* enforce alignment */
  461.         if (prdt_cap % 8)
  462.                 prdt_cap += 8 - prdt_cap % 8;
  463.         indiv_ct = 128 + 16 * prdt_cap;
  464.         byte_ct = ct * indiv_ct;
  465.         pg_pe = rdiv(indiv_ct, 4096);
  466.         page_rest = pg_pe * 4096 - indiv_ct;
  467.  
  468.         /* pg_pe allocs must be contiguous, but if page_rest is low
  469.          * individual tables can be split up to different locations */
  470.         if (!page_rest || (page_rest % indiv_ct == 0)) {
  471.                 epa = pg_pe * 4096 / indiv_ct; /* entries per allocation */
  472.                 pg_ct = rdiv(ct, epa) * pg_pe;
  473.         } else {
  474.                 pg_ct = rdiv(byte_ct, 4096);
  475.                 epa = ct;
  476.                 pg_pe = pg_ct;
  477.         }
  478.  
  479.         /* allocate the physical memory */
  480.         while (pg_ct) {
  481.                 void* ptr; size_t epa2 = epa, rem;
  482.                 mmgr_pmem_allocate(&pr, 1, pg_pe, flg);
  483.                 if (pr.count != pg_pe)
  484.                         die("Out of contiguous memory\n");
  485.                 rem = pg_pe * 4096;
  486.                 /* pg_pe zeroed contiguous memory at pr.base */
  487.                 ptr = mmgr_map_pmem(NULL, &pr, 1, MMGR_MAP_KERNEL |
  488.                         MMGR_MAP_WRITE | MMGR_MAP_NO_CACHING);
  489.                 if (!ptr)
  490.                         die("mmap not working\n");
  491.  
  492.                 /* assign the entries */
  493.                 while (epa2--) {
  494.                         rv->tables[i].pmem = pr.base;
  495.                         rv->tables[i++].ptr = ptr;
  496.                         /* advance */
  497.                         pr.base += indiv_ct;
  498.                         ptr += indiv_ct;
  499.                         rem -= indiv_ct;
  500.                         if (rem < indiv_ct && epa2 > 1)
  501.                                 die("algo error\n");
  502.                 }
  503.                 pg_ct -= pg_pe;
  504.         }
  505.  
  506.         /* assign the other values */
  507.         rv->prdt_cap = prdt_cap;
  508.         rv->next = NULL;
  509.         rv->mask = 0xffffffff << prdt_cap;
  510.  
  511.         /* print it */
  512.         for (i = 0; i < ct; i++)
  513.                 printf("%016llx %p\n", rv->tables[i].pmem, rv->tables[i].ptr);
  514.  
  515.         return rv;
  516. }
  517.  
  518. void ahci_issue_command(struct ahci_device* ad, int port, int pmprt, enum disk_cmd cmd,
  519.                 uint64_t lba, uint64_t ct, void* ptr, struct wait_queue_head* wq_h)
  520. {
  521.         struct cmd_ctx* cc = kzalloc(sizeof(*cc));
  522.         cc->c = cmd;
  523.         cc->lba = lba;
  524.         cc->ct = ct;
  525.         cc->ptr = ptr;
  526.         cc->wq_wake = wq_h;
  527.         cc->pmprt = pmprt;
  528.         cc->port = port;
  529.         list_add_tail(&cc->lst, &ad->pending_cmds[port]);
  530.         if (__sync_fetch_and_add(&ad->pending_ct, 1) == 1) {
  531.                 /* TODO: wake worker */
  532.         }
  533. }
  534.  
  535.  
  536. int ahci_do_bohc(struct ahci_device* ad)
  537. {
  538.         struct ahci_hba* hba = ad->ptr;
  539.         if (hba->cap2.boh) {
  540.                 abprintf("performing AHCI BIOS handover...\n");
  541.                 preempt_disable();
  542.                 hba->bohc |= BOHC_OOS;
  543.                 /* SMI is issued */
  544.                 if (hba->bohc & BOHC_BOS) { /* we have to wait */
  545.                         mdelay(25);
  546.                         if (hba->bohc & BOHC_BB) { /* BIOS does it */
  547.                                 int to = 30;
  548.                                 while (hba->bohc & BOHC_BOS) {
  549.                                         mdelay(100);
  550.                                         if (!to--) /* abort after 3 seconds */
  551.                                                 break;
  552.                                 }
  553.                         } else {;} /* we have to clean up */
  554.                 }
  555.                 /* we're done */
  556.                 preempt_enable();
  557.         }
  558.         /* always acquire (we're only nice to inform the bios) */
  559.         return 0;
  560. }
  561.  
  562. void ahci_reset_port(struct ahci_hba* h, struct ahci_port* p)
  563. {
  564.         /* cap.sclo? */
  565.         die("not impl %p %p\n", h, p);
  566. }
  567.  
  568. void ahci_port_activate_device(struct ahci_hba* h, struct ahci_port* p)
  569. {
  570.         abprintf("activate port %zd:\n", p - h->port);
  571.         /* first check the presence flag */
  572.         if (p->scr0.det == DET_NOT_PRESENT) {
  573.                 abprintf("activate: DET_NOT_PRESENT\n");
  574.                 /* nothing attached? */
  575.                 if (p->cmd.cpd) { /* we rely on CPD */
  576.                         if (!p->cmd.cps) {
  577.                                 abprintf("confirmed by CPD\n");
  578.                                 return;
  579.                         }
  580.                         /* there's something */
  581.                         if (!p->cmd.pod) {
  582.                                 p->cmd.pod = 1; /* power it */
  583.                                 ndelay(20);
  584.                         }
  585.                         if (p->scr0.det != DET_NOT_PRESENT)
  586.                                 goto next_step;
  587.                 }
  588.                 /* spin-up? */
  589.                 if (!p->cmd.sud) { /* always !1 if cap.sss == 0 */
  590.                         abprintf("not spun-up yet?\n");
  591.                         if (p->pXserr & (1 << 26)) /* eXchange bit */
  592.                                 p->pXserr |= (1 << 26); /* RWC */
  593.                         if (p->scr2.det != 0) { /* set to 0 prior sud */
  594.                                 p->scr2.det = 0;
  595.                                 ndelay(20);
  596.                         }
  597.                         p->cmd.sud = 1;
  598.                         udelay(50); /* wait 50 mus */
  599.                         if (p->pXserr & ((1 << 26) | (1 << 18))) { /* received sth. */
  600.                                 /* COMRESET, COMWAKE */
  601.                                 goto next_step;
  602.                         }
  603.                         /* send the reset */
  604.                         p->scr2.det = 1;
  605.                         mdelay(1);
  606.                         p->scr2.det = 0;
  607.                         udelay(50);
  608.                         if (p->scr0.det != DET_NOT_PRESENT)
  609.                                 goto next_step;
  610.                         p->cmd.sud = 0; /* we're done enter listening mode */
  611.                         abprintf("not present\n");
  612.                         goto exit;
  613.                 }
  614.                 /* just try ICC */
  615.                 if (p->scr0.ipm != 1) {
  616.                         abprintf("IPM != ACTIVE\n");
  617.                         int ct = 50;
  618.                         while (p->cmd.icc && ct--)
  619.                                 udelay(10);
  620.                         p->cmd.icc = 1;
  621.                         mdelay(10);
  622.                         if (p->scr0.det != DET_NOT_PRESENT)
  623.                                 goto next_step;
  624.                         abprintf("unable to set to active\n");
  625.                         goto exit;
  626.                 }
  627.         }
  628.  
  629.         /* det != 1*/
  630. next_step:
  631.         if (p->scr0.det == DET_PRESENT) {
  632.                 /* almost done */
  633.                 if (p->scr0.ipm != IPM_ACTIVE) {
  634.                         int ct, wc;
  635.                         abprintf("Present but not active.\n");
  636. activ:
  637.                         ct = 50, wc = 0;
  638. rewait:
  639.                         while (p->cmd.icc && ct--) /* 500ms */
  640.                                 mdelay(10);
  641.                         if (p->scr0.ipm != IPM_ACTIVE && !wc) {
  642.                                 p->cmd.icc = 1;
  643.                                 ndelay(50);
  644.                                 wc++; ct = 50;
  645.                                 goto rewait;
  646.                         }
  647.                         if (wc) {
  648.                                 /* reset */
  649.                                 p->scr2.det = 1;
  650.                                 mdelay(1);
  651.                                 goto next_step;
  652.                         }
  653.                 }
  654.                 abprintf("Device at port %zd is active and present.\n",
  655.                         p - h->port);
  656.                 abprintf("details: %x %x %x %u %u\n", p->pXserr, p->tfd.sts,
  657.                         p->tfd.err, p->scr0.det, p->scr0.ipm);
  658.                 return;
  659.         } else if (p->scr0.det == DET_PHY_OFFLINE) {
  660.                 /* just deactivated */
  661.                 abprintf("PHY offline mode\n");
  662.                 goto activ;
  663.         } else if (p->scr0.det == DET_PRESENT_NO_PHY) {
  664.                 abprintf("PRESENT_NO_PHY mode\n");
  665.                 goto activ; /* try the same */
  666.         }
  667. exit:
  668.                 /* we're really done */
  669.                 abprintf("Not present at port %zd\n", p - h->port);
  670.                 return;
  671. }
  672.  
  673. void ahci_enable_port(struct ahci_device* ad, int pt)
  674. {
  675.         struct ahci_port* p = &ad->ptr->port[pt];
  676.         int reset_ct = 0;
  677.  
  678.         /* skip non-implemented ports */
  679.         if (!(ad->ptr->ports & (1 << pt)))
  680.                 return;
  681.  
  682. Pos1:
  683.         /* clear old data */
  684.         p->pXserr = 0xffffffff; /* 10.1.2 -> 6. */
  685.         p->pXis = 0xffffffff; /* clear all pending interrupts */
  686.  
  687.         /* first allow for the reception of FISes */
  688.         p->cmd.fre = 1;
  689.         ndelay(20); /* wait for BSY to show up */
  690.         while (1) {
  691.                 uint8_t sts = p->tfd.sts;
  692.                 if (sts & TFS_ERR) {
  693.                         /* something went wrong! */
  694.                         if (sts == 0x7f) /* no device */
  695.                                 break;
  696.                         else if (sts == 0x41 && p->tfd.err == 0x20) {
  697.                                 if (p->pXsig == SIG_ATAPI)
  698.                                         break; /* no medium */
  699.                         }
  700.                         cprintf(KFMT_WARN, "port%d indicated task file error %x"
  701.                                 " while starting up.\n", pt, p->tfd.err);
  702.                         abprintf("scr1: %x %x\n", p->scr1.diag, p->scr1.err);
  703.                         abprintf("tfd: %x %x\n", p->tfd.sts, p->tfd.err);
  704.                         p->pXserr = 0xffffffff;
  705.                         if (!reset_ct++) {
  706.                                 ahci_port_activate_device(ad->ptr, p);
  707.                                 goto Pos1;
  708.                         } else if (reset_ct == 1) {
  709.                                 /*ahci_reset_port(ad->ptr, p);*/
  710.                                 goto Pos1;
  711.                         } else
  712.                                 goto defer;
  713.                 }
  714.                 if (!(sts & (TFS_BSY | TFS_DRQ)))
  715.                         break;
  716.                 udelay(50);
  717.         }
  718.  
  719.         /* set ST only if BSY,DRQ and DET=3h or IPM=2,6,8 */
  720.         if (p->scr0.det != 3)
  721.                 ahci_port_activate_device(ad->ptr, p);
  722.         if (p->tfd.sts & (TFS_BSY | TFS_DRQ))
  723.                 goto defer; /* listen */
  724.         if (!((p->scr0.det == 3) || (p->scr0.ipm == 2) ||
  725.                 (p->scr0.ipm == 6) || (p->scr0.ipm == 8)))
  726.                 goto defer;
  727.         /* we're allowed so set it */
  728.         p->cmd.st = 1;
  729.         /* Change in PhyRdy, CPS, TFS.err, PCS, DPS(I=1), UFS, HBFS, HBDS, IFS */
  730.         p->pXie = (1 << 22) | (1 << 6) | (1 << 31) | (1 << 30)
  731.                 | (1 << 5) | (1 << 4) | (1 << 29) | (1 << 28) | (1 << 27);
  732.  
  733.         /* issue the IDENTIFY_(PACKET)_DEVICE command */
  734.         ahci_issue_command(ad, pt, 0, disk_cmd_identify, 0, 0, NULL, NULL /* wq_worker_ad*/);
  735.  
  736.         printf("port %d is now processing commands\n", pt);
  737.         return;
  738.  
  739. defer: /* we're interested in status changes only */
  740.         abprintf("port %d set to listening mode\n", pt);
  741.         p->pXie = (1 << 22) | (1 << 31) | (1 << 6); /* PhyRdy change, CPS, CCS */
  742. }
  743.  
  744. void ahci_stop_port(struct ahci_device* ad, int pt)
  745. {
  746.         struct ahci_port* p = &ad->ptr->port[pt];
  747.         int ct;
  748.  
  749.         /* skip non-implemented ports */
  750.         if (!(ad->ptr->ports & (1 << pt)))
  751.                 return;
  752.  
  753.         /* print the BIOS provided status */
  754.         abprintf("BIOS initial state of port %d:\n", pt);
  755.         abprintf("\t%s%s\n", p->cmd.fre ? "FRE " : "",
  756.                 p->cmd.st ? "ST" : "");
  757.         /* print the pointers */
  758.         abprintf("BIOS command list located at %016llx\n", p->pXclb64);
  759.         abprintf("BIOS FIS received buffer at  %016llx\n", p->pXfb64);
  760.  
  761.         /* check if already off */
  762.         if (!p->cmd.fre && !p->cmd.st)
  763.                 return;
  764.  
  765.         /* Now turn it off (2x500ms max else reset) */
  766.         ct = 50;
  767.         p->cmd.st = 0;
  768.         while (p->cmd.cr && ct--)
  769.                 mdelay(10);
  770.         if (ct == -1)
  771.                 goto reset;
  772.         ct = 50;
  773.         p->cmd.fre = 0;
  774.         while (p->cmd.fr && ct--)
  775.                 mdelay(10);
  776. reset:
  777.         if (ct == -1) {
  778.                 printf("Port %d: reset needed!\n", pt);
  779.                 /* TODO implement this */
  780.                 //ahci_reset_port(ad, pt);
  781.                 while (1);
  782.         }
  783.         /* The port is now off innit? */
  784. }
  785.  
  786. /* we have ad->num_ports active ports needing num_cmd
  787.  * command slots and one FIS receive buffer each.
  788.  * The command lists are held in a shared pool. */
  789. void ahci_assign_buffers(struct ahci_device* ad)
  790. {
  791.         size_t bt_ct, pg_ct; int flg = MMGR_ALLOC_ANY;
  792.         struct page_range pr; int cc, fc, ic;
  793.         /* Total size: 256*np+1024*np+512*np */
  794.         bt_ct = ad->num_ports * (sizeof(struct ahci_fis)
  795.                 + sizeof(struct ahci_cmdlst) + 512);
  796.         pg_ct = rdiv(bt_ct, 4096);
  797.  
  798.         /* allocate and map the physical memory */
  799.         ic = cc = fc = ad->num_ports;
  800.         if (!ad->ptr->cap.is_64bit)
  801.                 flg = MMGR_ALLOC_32_BIT;
  802.         while (pg_ct) {
  803.                 void* ptr; size_t nb;
  804.                 /* we don't need contiguous memory */
  805.                 pg_ct -= mmgr_pmem_allocate(&pr, 1, pg_ct, flg);
  806.                 if (!pr.count)
  807.                         goto quit;
  808.                 nb = 4096 * pr.count;
  809.                 ptr = mmgr_map_pmem(NULL, &pr, 1, /*MMGR_MAP_MMIO | <- 0xe nope */
  810.                         MMGR_MAP_WRITE | MMGR_MAP_KERNEL | MMGR_MAP_NO_CACHING);
  811.                 if (!ptr)
  812.                         goto quit;
  813.  
  814.                 /* now first process the command lists with 1K-alignment */
  815.                 while (cc && nb >= 1024) {
  816.                         int pt = ad->num_ports - cc--;
  817.                         if (ad->ptr->ports & (1 << pt)) {
  818.                                 /* assign for implemented port */
  819.                                 ad->ptr->port[pt].pXclb64 = pr.base;
  820.                                 ad->command_lists[pt] = ptr;
  821.                                 /* advance */
  822.                                 pr.base += 1024;
  823.                                 nb -= 1024;
  824.                                 ptr += 1024;
  825.                         }
  826.                 }
  827.  
  828.                 /* then the FIS buffers (256bt-aligned) */
  829.                 while (fc && nb >= 256) {
  830.                         int pt = ad->num_ports - fc--;
  831.                         if (ad->ptr->ports & (1 << pt)) {
  832.                                 /* as above */
  833.                                 ad->ptr->port[pt].pXfb64 = pr.base;
  834.                                 ad->recv_fis[pt] = ptr;
  835.                                 /* advance */
  836.                                 pr.base += 256;
  837.                                 nb -= 256;
  838.                                 ptr += 256;
  839.                         }
  840.                 }
  841.  
  842.                 while (ic && nb >= 512) {
  843.                         int pt = ad->num_ports - ic--;
  844.                         if (ad->ptr->ports & (1 << pt)) {
  845.                                 /* as above */
  846.                                 ad->identify_data[pt].pm = pr.base;
  847.                                 ad->identify_data[pt].ptr = ptr;
  848.                                 /* advance */
  849.                                 pr.base += 512;
  850.                                 nb -= 512;
  851.                                 ptr += 512;
  852.                         } else
  853.                                 ad->identify_data[pt].sig = 0;
  854.                 }
  855.  
  856.                 /* exit early if we have non-impelented ports */
  857.                 if (!fc && !cc && !ic) {
  858.                         /* also deallocate if possible */
  859.                         if (nb >= 4096) {
  860.                                 ptr += (size_t)ptr % 4096;
  861.                                 mmgr_free_vmem(ptr, nb / 4096);
  862.                         }
  863.                         break;
  864.                 }
  865.         }
  866.         /* Now all implemented buffers are set */
  867.         return;
  868.  
  869. quit:
  870.         die("Out of memory!\n");
  871. }
  872.  
  873. /* also disables interrupts */
  874. void ahci_clear_interrupts(struct ahci_device* ad)
  875. {
  876.         struct ahci_hba* h = ad->ptr;
  877.  
  878.         /* disable interrupts */
  879.         h->ctrl.ie = 0;
  880.  
  881.         /* clear them in every port implemented */
  882.         for (int i = 0; i < ad->num_ports; i++) {
  883.                 struct ahci_port* p;
  884.                 if ((h->ports & (1 << i)) == 0)
  885.                         continue;
  886.                 p = &h->port[i];
  887.                 /* disable and clear all */
  888.                 p->pXie = 0;
  889.                 p->pXis = 0xffffffff;
  890.         }
  891.  
  892.         /* clear the global status */
  893.         h->intr = 0xffffffff; /* W1C */
  894. }
  895.  
  896. int ahci_device_init(struct pci_device* d)
  897. {
  898.         int rv;
  899.  
  900.         /* Check progIf */
  901.         if (d->device_interface != 1) {
  902.                 cprintf(KFMT_WARN, "Unsupported SATA program interface: %u\n",
  903.                         d->device_interface);
  904.                 return -ENOSYS;
  905.         }
  906.  
  907.         /* Setup and map BAR5 if neccessary */
  908.         if (d->ressources[5].type == PCI_BAR_PIO)
  909.                 return -ENOTSUP; /* AHCI requires MMIO at BAR5 */
  910.         if (!d->ressources[5].mmio.base) {
  911.                 int atl = 0; uint64_t m;
  912.                 /* we first need pmem for the device */
  913.                 if (!d->ressources[5].mmio.size) {
  914.                         /* Ask PCI */
  915.                         d->ressources[5].mmio.size = pci_bar_get_size(d, 5);
  916.                         if (!d->ressources[5].mmio.size) {
  917.                                 d->ressources[5].mmio.size = 0x1100; /* 32 ports */
  918.                                 atl = 1; /* about to leave */
  919.                         }
  920.                 }
  921.                 /* Now we want to get some address */
  922.                 m = mmio_allocate_range(d->ressources[5].mmio.size,
  923.                         0x3 & (d->ressources[5].type >> 1));
  924.                 if (!m)
  925.                         return -ENOMEM;
  926.                 /* and assign it */
  927.                 pci_set_bar(d, 5, d->ressources[5].type, m);
  928.                 if (d->ressources[5].mmio.base != m) {
  929.                         /* we can't */
  930.                         mmio_free_range(m, d->ressources[5].mmio.size);
  931.                         if (atl)
  932.                                 d->ressources[5].mmio.size = 0;
  933.                         return -ENOTSUP;
  934.                 }
  935.         }
  936.  
  937.         /* Now we possibly have to map the memory */
  938.         if (!d->ressources[5].mmio.mapping) {
  939.                 void* mem; struct page_range pr;
  940.                 pr.base = d->ressources[5].mmio.base;
  941.                 pr.count = rdiv(d->ressources[5].mmio.size, 4096);
  942.                 mem = mmgr_map_pmem(NULL, &pr, 1, MMGR_MAP_MMIO |
  943.                         MMGR_MAP_WRITE | MMGR_MAP_KERNEL | MMGR_MAP_NO_CACHING);
  944.                 if (!mem)
  945.                         return -ENOMEM;
  946.                 d->ressources[5].mmio.mapping = mem;
  947.         }
  948.  
  949.         /* we now have a working mapping */
  950.         struct ahci_hba* h = d->ressources[5].mmio.mapping;
  951.         struct ahci_device* ad = kzalloc(sizeof(*ad));
  952.         if (!ad)
  953.                 return -ENOMEM;
  954.  
  955.         /* check for AE (access to other registers is disallowed before) */
  956.         if (!(h->control & GHC_CNT_AE)) {
  957.                 abprintf("switching to AHCI mode\n");
  958.                 h->control |= GHC_CNT_AE;
  959.         }
  960.  
  961.         /* fill the descriptors */
  962.         ad->ptr = h;
  963.         ad->num_ports = min(h->cap.num_ports + 1, bsr(h->ports));
  964.         ad->num_cmd = h->cap.num_cmd + 1;
  965.         for (int i = 0; i < 32; i++) {
  966.                 ad->pending_cmds[i] = LIST_HEAD_INIT(ad->pending_cmds[i]);
  967.                 ad->proc_cmds[i] = LIST_HEAD_INIT(ad->proc_cmds[i]);
  968.         }
  969.  
  970. #ifdef AHCIBABBLE /* Tell the world about it */
  971.         printf("AHCI %.0x%x.%x%x conforming HBA detected\n",
  972.                 h->ahci_ver >> 24, 0xff & (h->ahci_ver >> 16),
  973.                 0xff & (h->ahci_ver >> 8), h->ahci_ver & 0xff);
  974.         /* and print the capablilities */
  975.         printf("Features: %s%s%sspeed: %s\n",
  976.                 capstr(h->cap.ncq_supp, 1, "", "NCQ  "),
  977.                 capstr(h->cap.ahci_only, 1, "Legacy support  ", "AHCI only  "),
  978.                 capstr(h->cap.is_64bit, 1, "32-bit  ", "64-bit  "),
  979.                 capstr(h->cap.if_speed, 4, "res", "1.5 Gbps", "3 Gbps", "6Gbps", ">6Gbps"));
  980.         printf("AHCI HBA has %u ports and %u command slots\n",
  981.                 ad->num_ports, ad->num_cmd);
  982. #endif
  983.  
  984.         /* transfer ownership (incl NMI) */
  985.         if (h->cap2.boh) {
  986.                 rv = ahci_do_bohc(ad);
  987.                 if (rv) {
  988.                         kfree(ad);
  989.                         return rv;
  990.                 }
  991.         }
  992.  
  993.         /* disable every possible interrupt */
  994.         if(pci_disable_interrupts(d))
  995.                 die("not possible!\n");
  996.  
  997.         /* Stop the running ports */
  998.         for (int i = 0; i < ad->num_ports; i++)
  999.                 ahci_stop_port(ad, i);
  1000.  
  1001.         /* clear all interrupts and install the handler */
  1002.         ahci_clear_interrupts(ad); /* PCI disabled MSI and we're currently */
  1003.         install_irq_handler(ad->dev->intr_line, ahci_irq); /* not using it */
  1004.  
  1005.         /* assign the command lists and the FIS buffers */
  1006.         ahci_assign_buffers(ad);
  1007.  
  1008.         /* print them */
  1009.         for (int i = 0; i < ad->num_ports; i++) {
  1010.                 printf("port%d: %016llx %016llx %p %p\n", i,
  1011.                         ad->ptr->port[i].pXclb64,
  1012.                         ad->ptr->port[i].pXfb64,
  1013.                         ad->command_lists[i],
  1014.                         ad->recv_fis[i]);
  1015.                 printf("       %016llx %p\n", ad->identify_data[i].pm,
  1016.                         ad->identify_data[i].ptr);
  1017.         }
  1018.  
  1019.         /* allow the device to write to our memory */
  1020.         pci_set_master(d);
  1021.  
  1022.         /* reenable the ports */
  1023.         for (int i = 0; i < ad->num_ports; i++)
  1024.                 ahci_enable_port(ad, i);
  1025.  
  1026.         /* add the device to the list */
  1027.         struct ale* le = kmalloc(sizeof(*le));
  1028.         le->dev = ad;
  1029.         list_add_tail(&le->devs, &ahci_dl.devs);
  1030.  
  1031.         /* allocate the pool */
  1032.         ad->ctbl_pool = ahci_allocate_command_table_pool(
  1033.                 32, 8, h->cap.is_64bit);
  1034.  
  1035.         /* launch the worker */
  1036.         task_spawn(ahci_worker, ad, PRIORITY_ELEVATED);
  1037.  
  1038.         /* reenable interrupts */
  1039.         /* The port_enable function already set the per-port values */
  1040.         h->ctrl.ie = 1;
  1041.         pci_enable_interrupts(d);
  1042.  
  1043.         return 0;
  1044. }
  1045.  
  1046. int MODENTRY module_init()
  1047. {
  1048.         printf("AHCI initialization...\n");
  1049.         struct pci_device* dev; int ct = 0, nc = 0, nf = 0;
  1050.         while ((dev = pci_find_device(1, 6, 1, ct++))) {
  1051.                 int rv;
  1052.                 printf("Found AHCI device at %02x:%02x.%x\n", dev->addr.bus_no,
  1053.                         dev->addr.device_no, dev->addr.func_no);
  1054.                 printf("Attempting initialization...\n");
  1055.                 rv = ahci_device_init(dev);
  1056.                 if (rv) {
  1057.                         cprintf(KFMT_ERROR, "Initialization of this device failed.\n");
  1058.                         nf++;
  1059.                 } else {
  1060.                         cprintf(KFMT_OK, "Initialization of AHCI HBA succeeded.\n");
  1061.                         nc++;
  1062.                 }
  1063.         }
  1064.         /* Summary */
  1065.         cprintf(nf ? KFMT_WARN : KFMT_OK, "Initialized %d AHCI Host Bus Adapters. (%d failed)\n", nc, nf);
  1066.         return 0;
  1067. }
  1068.  
  1069. void MODENTRY module_cleanup()
  1070. {
  1071. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!