diff --git a/freebsd/ata.c b/freebsd/ata.c index c8089f6..4124a4b 100644 --- a/freebsd/ata.c +++ b/freebsd/ata.c @@ -269,5 +269,105 @@ int32_t SendAtaLba48Command(void* device_ctx, uint32_t* sense, uint32_t* buf_len) { - return -1; + DeviceContext* ctx = device_ctx; + *duration = 0; + *sense = false; + union ccb* camccb; + int error; + int clock_error; + struct timespec start_tp; + struct timespec end_tp; + double start, end; + + if(!ctx) return -1; + if(!ctx->device) return -1; + + camccb = cam_getccb(ctx->device); + + if(!camccb) return -1; + + // TODO CHECK: 48-bit ATA CAM commands can crash FreeBSD < 9.2-RELEASE + + camccb->ccb_h.func_code = XPT_ATA_IO; + camccb->ccb_h.flags = AtaProtocolToCamFlags(protocol); + camccb->ccb_h.xflags = 0; + camccb->ccb_h.retry_count = 1; + camccb->ccb_h.cbfcnp = NULL; + camccb->ccb_h.timeout = timeout; + camccb->ataio.data_ptr = (u_int8_t *)buffer; + camccb->ataio.dxfer_len = *buf_len; + camccb->ccb_h.flags |= CAM_DEV_QFRZDIS; + camccb->ataio.cmd.flags = CAM_ATAIO_NEEDRESULT | CAM_ATAIO_48BIT; + + switch(protocol) + { + case AARUREMOTE_ATA_PROTOCOL_DMA: + case AARUREMOTE_ATA_PROTOCOL_DMA_QUEUED: + case AARUREMOTE_ATA_PROTOCOL_UDMA_IN: + case AARUREMOTE_ATA_PROTOCOL_UDMA_OUT: + camccb->ataio.cmd.flags |= CAM_ATAIO_DMA; + break; + case AARUREMOTE_ATA_PROTOCOL_FPDMA: + camccb->ataio.cmd.flags |= CAM_ATAIO_FPDMA; + break; + } + + camccb->ataio.cmd.command = registers.command; + camccb->ataio.cmd.device = 0x40 |registers.device_head; + camccb->ataio.cmd.lba_high_exp = (registers.lba_high & 0xFF00) >> 8; + camccb->ataio.cmd.lba_high = registers.lba_high & 0xFF; + camccb->ataio.cmd.lba_mid_exp =(registers.lba_mid & 0xFF00) >> 8; + camccb->ataio.cmd.lba_mid =registers.lba_mid & 0xFF; + camccb->ataio.cmd.lba_low_exp= (registers.lba_low & 0xFF00) >> 8; + camccb->ataio.cmd.lba_low= registers.lba_low & 0xFF; + camccb->ataio.cmd.features_exp = (registers.feature & 0xFF00) >> 8; + camccb->ataio.cmd.features = registers.feature & 0xFF; + camccb->ataio.cmd.sector_count_exp= (registers.sector_count & 0xFF00) >> 8; + camccb->ataio.cmd.sector_count= registers.sector_count & 0xFF; + + clock_error = clock_gettime(CLOCK_REALTIME_PRECISE, &start_tp); + + error = cam_send_ccb(ctx->device, camccb); + + if(!clock_error) + clock_error = clock_gettime(CLOCK_REALTIME_PRECISE, &end_tp); + + if(!clock_error) + { + start = (double)start_tp.tv_sec * 1000.0; + start += (double)start_tp.tv_nsec / 1000000.0; + end = (double)end_tp.tv_sec * 1000.0; + end += (double)end_tp.tv_nsec / 1000000.0; + + *duration = (uint32_t)(end-start); + } + + if(error < 0) + error = errno; + + if((camccb->ccb_h.status & CAM_STATUS_MASK) != CAM_REQ_CMP && + (camccb->ccb_h.status & CAM_STATUS_MASK) != CAM_ATA_STATUS_ERROR) + { + error = errno; + *sense = true; + } + + if((camccb->ccb_h.status & CAM_STATUS_MASK) == CAM_ATA_STATUS_ERROR) + *sense = true; + + error_registers->lba_high = (camccb->ataio.res.lba_high_exp << 8) + camccb->ataio.res.lba_high; + error_registers->lba_mid = (camccb->ataio.res.lba_mid_exp << 8) + camccb->ataio.res.lba_mid; + error_registers->lba_low = (camccb->ataio.res.lba_low_exp << 8) + camccb->ataio.res.lba_low; + error_registers->sector_count = (camccb->ataio.res.sector_count_exp << 8) + camccb->ataio.res.sector_count; + error_registers->device_head = camccb->ataio.res.device; + error_registers->error = camccb->ataio.res.error; + error_registers->status = camccb->ataio.res.status; + + cam_freeccb(camccb); + + *buf_len = camccb->ataio.dxfer_len; + + *sense |= error_registers->error || (error_registers->status & 0xA5) || error; + + return error; } \ No newline at end of file