source: trunk/src/os2ahci/trace.c@ 114

/******************************************************************************
 * trace.c - code for our internal trace ring buffer
 *
 * Copyright (c) 2011 thi.guten Software Development
 * Copyright (c) 2011 Mensys B.V.
 *
 * Authors: Christian Mueller, Markus Thielen
 *
 * Parts copied from/inspired by the Linux AHCI driver;
 * those parts are (c) Linux AHCI/ATA maintainers
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "os2ahci.h"

/* -------------------------- macros and constants ------------------------- */

#define AHCI_TRACE_BUF_SIZE  0x8000U /* 32k */

/* ------------------------ typedefs and structures ------------------------ */

/* -------------------------- function prototypes -------------------------- */

/* ------------------------ global/static variables ------------------------ */

struct {
  u32 phys_addr;   /* physical address of allocated buffer */
  u8 _far *tbuf;   /* mapped address of trace buffer */
  u16 writep;      /* current write offset in buffer */
  u16 readp;       /* current read offset in buffer */
  int empty;       /* != 0 if buffer is empty */
} ahci_trace_buf;

/* ----------------------------- start of code ----------------------------- */

/******************************************************************************
 * initialize AHCI circular trace buffer
 * 
 * NOTE: this func must be called during INIT time since it allocates
 *       a GDT selector for the trace ring buffer
 */
void trace_init(void)
{
  SEL sel = 0;

  /* initialize ring buffer logic */
  ahci_trace_buf.writep = 0;
  ahci_trace_buf.readp = 0;
  ahci_trace_buf.empty = 1;

  if (ahci_trace_buf.phys_addr == 0) {
    /* allocate buffer */
    if (DevHelp_AllocPhys((ULONG) AHCI_TRACE_BUF_SIZE, MEMTYPE_ABOVE_1M,
                         &(ahci_trace_buf.phys_addr))) {
      /* failed above 1MB, try below */
      if (DevHelp_AllocPhys((ULONG) AHCI_TRACE_BUF_SIZE, MEMTYPE_BELOW_1M,
                            &(ahci_trace_buf.phys_addr))) {
        /* failed, too. Give up */
        ahci_trace_buf.phys_addr = 0;
        cprintf("OS2AHCI warning: failed to allocate %dk trace buffer\n",
                AHCI_TRACE_BUF_SIZE / 1024);
        return;
      }
    }

    /* allocate GDT selector and map our physical trace buffer to it */
    if (DevHelp_AllocGDTSelector(&sel, 1) ||
        DevHelp_PhysToGDTSelector(ahci_trace_buf.phys_addr,
                                  AHCI_TRACE_BUF_SIZE, sel)) {
      /* failed; free GDT selector and physical memory we allocated before */
      if (sel) {
        DevHelp_FreeGDTSelector(sel);
        sel = 0;
      }
      DevHelp_FreePhys(ahci_trace_buf.phys_addr);
      ahci_trace_buf.phys_addr = 0;
      return;
    }

    /* create ring buffer address */
    ahci_trace_buf.tbuf = (u8 _far *) ((u32) sel << 16);

  }
  
  
}

/******************************************************************************
 * cleanup trace buffer
 *
 * NOTE: this function is here for completeness; the trace buffer should not
 *       be deallocated and then reallocated.
 */
void trace_exit(void)
{
  /* free physical address */
  if (ahci_trace_buf.phys_addr) {
    DevHelp_FreePhys(ahci_trace_buf.phys_addr);
    ahci_trace_buf.phys_addr = 0;
  }

  /* free GDT selector */
  if (ahci_trace_buf.tbuf) {
    DevHelp_FreeGDTSelector((SEL) ((u32) (ahci_trace_buf.tbuf) >> 16));
    ahci_trace_buf.tbuf = NULL;
  }
}


/******************************************************************************
 * write a string to the circular trace buffer
 *
 * Note: This func wraps the buffer if necessary, so the caller does not
 *       need to call repeatedly until everything is written.
 *       
 */
void trace_write(u8 _far *s, int len)
{
  u16 cbw; /* bytes we can store before buffer wrap occurrs */
  u16 cb;
  USHORT awake_cnt;
  
  if (ahci_trace_buf.phys_addr == 0) {
    /* tracing not active */
    return;
  }
  
  cbw = AHCI_TRACE_BUF_SIZE - ahci_trace_buf.writep;

  while (len > 0) {
    cb = cbw < len ? cbw : len;
    if (cb && ahci_trace_buf.empty) {
      /* if trace buffer was empty, we wake up any evtl. waiting
       * processes now */
      ahci_trace_buf.empty = 0;
      DevHelp_ProcRun(ahci_trace_buf.phys_addr, &awake_cnt);
    }
    memcpy(ahci_trace_buf.tbuf + ahci_trace_buf.writep, s, cb);
    s += cb;
    ahci_trace_buf.writep += cb;
    len -= cb;

    /* wrap? */
    if (ahci_trace_buf.writep >= AHCI_TRACE_BUF_SIZE) {
      ahci_trace_buf.writep = 0;
      cbw = AHCI_TRACE_BUF_SIZE;
    }
  }
       
       
}

/******************************************************************************
 * read data from circular trace buffer
 * returns the number of bytes written to the caller's buffer
 *
 * NOTE: the caller is expected to call this func repeatedly 
 *       (up to two times) until it returns 0
 */
u16 trace_read(void _far *buf, u16 cb_buf)
{
  u16 cb_avail;
  u16 cb_read;

  if (ahci_trace_buf.phys_addr == NULL || ahci_trace_buf.empty) {
    return 0;
  }
  
  /* get number of bytes in buffer */
  if (ahci_trace_buf.readp < ahci_trace_buf.writep) {
    /* read pointer is smaller than write pointer; no wrap in between */
    cb_avail = ahci_trace_buf.writep - ahci_trace_buf.readp;
  } else {
    /* read pointer is larger than (or same as) write pointer; 
     * read up to buffer end and let caller come again (if necessary)
     */
    cb_avail = AHCI_TRACE_BUF_SIZE - ahci_trace_buf.readp;
  }

  /* determine number of bytes we actually can return */
  cb_read = cb_buf > cb_avail ? cb_avail : cb_buf;

  memcpy(buf, ahci_trace_buf.tbuf + ahci_trace_buf.readp, cb_read);
  
  /* update read pointer */
  ahci_trace_buf.readp += cb_read;
  if (ahci_trace_buf.readp >= AHCI_TRACE_BUF_SIZE) {
    ahci_trace_buf.readp = 0;
  }
  
  /* check if buffer is empty now */
  if (ahci_trace_buf.readp == ahci_trace_buf.writep) {
    ahci_trace_buf.empty = 1;
  }
     
  return cb_read;
}

/******************************************************************************
 * return number of bytes we can read from the trace buffer
 */
u16 trace_bytes_avail(void)
{
  
  if (ahci_trace_buf.empty) {
    return 0;
  }

  if (ahci_trace_buf.readp <= ahci_trace_buf.writep) {
    /* read pointer is smaller than write pointer; no wrap in between */
    if (ahci_trace_buf.writep == ahci_trace_buf.readp) {
      /* full buffer available */
      return AHCI_TRACE_BUF_SIZE;
    }
    return ahci_trace_buf.writep - ahci_trace_buf.readp;
  }

  /* read pointer is larger than write pointer; buffer wrapped */
  return AHCI_TRACE_BUF_SIZE - ahci_trace_buf.readp + ahci_trace_buf.writep;
}


/******************************************************************************
 * copy trace buffer content to character device reader (request block buffer)
 */
u16 trace_char_dev(RP_RWV _far *rwrb)
{
  u8 _far *to_buf;
  u16 cb_read = 0;
  u16 cb;
  USHORT mode = 0;

  /* block process until data is available in the trace buffer;
   * see also comment in ioctl.c next to DevHelp_ProcBlock
   * concerning spin_lock and ProcBlock interaction
   */
  while (ahci_trace_buf.empty) {
    spin_lock(com_lock);
#   ifndef OS2AHCI_SMP
      com_lock = 0;
#   endif
      if (DevHelp_ProcBlock((ULONG) ahci_trace_buf.phys_addr, 
                            (ULONG) -1, 0) == WAIT_INTERRUPTED) {
        /* user pressed Ctrl+C or whatever */
        rwrb->NumSectors = 0;
        return STDON;
      }
    spin_lock(com_lock);
  }

  /* get pointer to caller's buffer */
  if (DevHelp_PhysToVirt(rwrb->XferAddr, rwrb->NumSectors, &to_buf, &mode)) {
    spin_unlock(com_lock);
    return (STATUS_DONE | STERR);
  }

  /* loop until caller's buffer is full or no more data in trace buffer */
  do {
    cb = trace_read(to_buf + cb_read, rwrb->NumSectors - cb_read);
    cb_read += cb;
  } while (cb > 0 && cb_read < rwrb->NumSectors);

  spin_unlock(com_lock);
  rwrb->NumSectors = cb_read;

  return(STDON);
}