#include <stdbool.h>
#include <stdio.h>
#include <errno.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>

#include <sys/ioctl.h>
#include <bits/types/struct_iovec.h>

#define MAX_IOCTL_SGE 16

#define MEGASAS_MAGIC          'M'
#define MEGASAS_IOC_FIRMWARE   _IOWR(MEGASAS_MAGIC, 1, struct megasas_iocpacket)

#define MFI_FRAME_DIR_NONE        0x0000
#define MFI_FRAME_DIR_WRITE       0x0008
#define MFI_FRAME_DIR_READ        0x0010
#define MFI_FRAME_DIR_BOTH        0x0018

#define MFI_CMD_PD_SCSI_IO        0x04

#define SAT_ATA_PASSTHROUGH_12    0xa1
#define SAT_ATA_PASSTHROUGH_16    0x85

#define ATA_SMART_CMD              0xb0
#define ATA_SMART_WRITE_LOG_SECTOR 0xd6

struct megasas_sge32 {
  uint32_t phys_addr;
  uint32_t length;
} __attribute__ ((packed));

union megasas_sgl {
  struct megasas_sge32 sge32[1];
} __attribute__ ((packed));

struct megasas_pthru_frame {
  
  uint8_t cmd;            /*00h */
  uint8_t sense_len;      /*01h */
  uint8_t cmd_status;     /*02h */
  uint8_t scsi_status;    /*03h */
  
  uint8_t target_id;      /*04h */
  uint8_t lun;            /*05h */
  uint8_t cdb_len;        /*06h */
  uint8_t sge_count;      /*07h */
  
  uint32_t context;       /*08h */
  uint32_t pad_0;         /*0Ch */
  
  uint16_t flags;         /*10h */
  uint16_t timeout;       /*12h */
  uint32_t data_xfer_len; /*14h */
  
  uint32_t sense_buf_phys_addr_lo; /*18h */
  uint32_t sense_buf_phys_addr_hi; /*1Ch */
  
  uint8_t cdb[16];            /*20h */
  union megasas_sgl sgl; /*30h */
  
} __attribute__ ((packed));

struct megasas_iocpacket {
  uint16_t host_no;
  uint16_t __pad1;
  uint32_t sgl_off;
  uint32_t sge_count;
  uint32_t sense_off;
  uint32_t sense_len;
  union {
    uint8_t raw[128];
    struct megasas_pthru_frame pthru;
  } frame;
  
  struct iovec sgl[MAX_IOCTL_SGE];
} __attribute__ ((packed));

#define DXFER_NONE        0
#define DXFER_FROM_DEVICE 1
#define DXFER_TO_DEVICE   2

/* scsi_rsoc_elem and scsi_device is defined in dev_interface.h */

struct scsi_cmnd_io
{
    uint8_t * cmnd;     /* [in]: ptr to SCSI command block (cdb) */
    size_t  cmnd_len;   /* [in]: number of bytes in SCSI command */
    int dxfer_dir;      /* [in]: DXFER_NONE, DXFER_FROM_DEVICE, or
                                 DXFER_TO_DEVICE */
    uint8_t * dxferp;   /* [in]: ptr to outgoing or incoming data buffer */
    size_t dxfer_len;   /* [in]: bytes to be transferred to/from dxferp */
    uint8_t * sensep;   /* [in]: ptr to sense buffer, filled when
                                 CHECK CONDITION status occurs */
    size_t max_sense_len;  /* [in]:  max number of bytes to write to sensep */
    unsigned timeout;      /* [in]:  seconds, 0-> default timeout (60 seconds?) */
    size_t resp_sense_len; /* [out]: sense buffer length written */
    uint8_t scsi_status;   /* [out]: 0->ok, 2->CHECK CONDITION, etc ... */
    int resid;             /* [out]: Number of bytes requested to be transferred
                                     less actual number transferred (0 if not
                                     supported) */
};

bool prepare_dev(char* dev_name, uint32_t* out_m_hba) {
  // dev_name should be in /dev/bus/%u
  if (sscanf(dev_name, "/dev/bus/%u", out_m_hba) == 0) {
    fprintf(stderr, "prepare_dev: bad dev_name\n");
    return false;
  }
  return true;
}


bool megasas_cmd(unsigned int m_hba, int m_fd, unsigned int m_disknum, 
    int cdbLen, void* cdb, int dataLen, void* data, 
    int /*senseLen*/, void * /*sense*/, int /*report*/, 
    int dxfer_dir) {
  struct megasas_pthru_frame    *pthru;
  struct megasas_iocpacket      uio;

  memset(&uio, 0, sizeof(uio));
  pthru = &uio.frame.pthru;
  pthru->cmd = MFI_CMD_PD_SCSI_IO;
  pthru->cmd_status = 0xFF;
  pthru->scsi_status = 0x0;
  pthru->target_id = m_disknum;
  pthru->lun = 0;
  pthru->cdb_len = cdbLen;
  pthru->timeout = 0;
  switch (dxfer_dir) {
    case DXFER_NONE:
      pthru->flags = MFI_FRAME_DIR_NONE;
      break;
    case DXFER_FROM_DEVICE:
      pthru->flags = MFI_FRAME_DIR_READ;
      break;
    case DXFER_TO_DEVICE:
      pthru->flags = MFI_FRAME_DIR_WRITE;
      break;
    default:
      fprintf(stderr, "megasas_cmd: bad dxfer_dir\n");
      return false;
  }

  if (dataLen > 0) {
    pthru->sge_count = 1;
    pthru->data_xfer_len = dataLen;
    pthru->sgl.sge32[0].phys_addr = (intptr_t)data;
    pthru->sgl.sge32[0].length = (uint32_t)dataLen;
  }
  memcpy(pthru->cdb, cdb, cdbLen);

  uio.host_no = m_hba;
  if (dataLen > 0) {
    printf("1322 branch taken\n");
    uio.sge_count = 1;
    uio.sgl_off = offsetof(struct megasas_pthru_frame, sgl);
    uio.sgl[0].iov_base = data;
    uio.sgl[0].iov_len = dataLen;
  }

  errno = 0;
  int rc = ioctl(m_fd, MEGASAS_IOC_FIRMWARE, &uio);
}


bool scsi_pass_through(unsigned int m_hba, int m_fd, unsigned int m_disknum, struct scsi_cmnd_io *iop)
{
  // Controller rejects Test Unit Ready
  if (iop->cmnd[0] == 0x00)
    return true;

  if (iop->cmnd[0] == SAT_ATA_PASSTHROUGH_12 || iop->cmnd[0] == SAT_ATA_PASSTHROUGH_16) { 
    // Controller does not return ATA output registers in SAT sense data
    if (iop->cmnd[2] & (1 << 5)) { // chk_cond
      fprintf(stderr, "scsi_pass_through: ATA return descriptor not supported by controller firmware\n");
      return false;
    }
  }
  // SMART WRITE LOG SECTOR causing media errors
  if ((iop->cmnd[0] == SAT_ATA_PASSTHROUGH_16 // SAT16 WRITE LOG
      && iop->cmnd[14] == ATA_SMART_CMD && iop->cmnd[3]==0 && iop->cmnd[4] == ATA_SMART_WRITE_LOG_SECTOR) ||
      (iop->cmnd[0] == SAT_ATA_PASSTHROUGH_12 // SAT12 WRITE LOG
       && iop->cmnd[9] == ATA_SMART_CMD && iop->cmnd[3] == ATA_SMART_WRITE_LOG_SECTOR)) 
  {
      fprintf(stderr, "<UNIMPLEMENTED>: SMART WRITE LOG SECTOR may cause problems\n");
      return false;
  }

  return megasas_cmd(m_hba, m_fd, m_disknum, iop->cmnd_len, iop->cmnd,
    iop->dxfer_len, iop->dxferp,
    iop->max_sense_len, iop->sensep, 0, iop->dxfer_dir);
}

int main(int argc, char* argv[]) {
  int m_fd = open("/dev/megaraid_sas_ioctl_node", O_RDWR);
  char* dev_name = "/dev/bus/0";
  uint32_t m_hba = 0; 
  prepare_dev(dev_name, &m_hba);
  uint32_t m_disknum = 0; // megaraid,0
  struct scsi_cmnd_io iop = {
    // TODO fill this up
  };
  scsi_pass_through(m_hba, m_fd, m_disknum, &iop);
}