Index: trunk/src/os2ahci/trace.c
===================================================================
--- trunk/src/os2ahci/trace.c	(revision 114)
+++ trunk/src/os2ahci/trace.c	(revision 114)
@@ -0,0 +1,280 @@
+/******************************************************************************
+ * 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);
+}
