arch/m68k-amiga/card/copytuple.c

   1 /*
   2     Copyright © 1995-2014, The AROS Development Team. All rights reserved.
   3     $Id$
   4
   5     Desc: CopyTuple() function.
   6     Lang: english
   7 */
   8
   9 #include <proto/exec.h>
  10
  11 #include "card_intern.h"
  12
  13 #define TUPLEDEBUG(x) do { if (buffer == NULL) x; } while(0);
  14 #define TUPLEDEBUG2(x) x
  15 #define TUPLELOGGING 1
  16
  17 #if TUPLELOGGING
  18 static void byte2ascii(UBYTE **pp, UBYTE ch)
  19 {
  20     UBYTE *p = *pp;
  21     *p++ = ' ';
  22     *p++ = (ch >> 4) > 9 ? (ch >> 4) - 10 + 'A' : (ch >> 4) + '0';
  23     *p++ = (ch & 15) > 9 ? (ch & 15) - 10 + 'A' : (ch & 15) + '0';
  24     *p = 0;
  25     *pp = p;
  26 }
  27 #endif
  28
  29 static BOOL getbyte(ULONG addr, UBYTE *out)
  30 {
  31     volatile UBYTE *p;
  32
  33     *out = 0;
  34     if (addr < GAYLE_RAM || (addr >= GAYLE_RAM + GAYLE_RAMSIZE && addr < GAYLE_ATTRIBUTE) || addr >= GAYLE_ATTRIBUTE + GAYLE_ATTRIBUTESIZE) {
  35         TUPLEDEBUG2(bug("getbyte from invalid address %p\n", addr));
  36         return FALSE;
  37     }
  38     if (!pcmcia_havecard())
  39         return FALSE;
  40     p = (UBYTE*)addr;
  41     *out = *p;
  42     return TRUE;
  43 }
  44 static BOOL getbytes(ULONG addr, UBYTE count, UBYTE nextbyte, UBYTE *out)
  45 {
  46     while (count-- > 0) {
  47         if (!getbyte(addr, out))
  48             return FALSE;
  49         out++;
  50         addr += nextbyte;
  51     }
  52     return TRUE;
  53 }
  54
  55 static BOOL getlelong(ULONG addr, UBYTE offset, UBYTE nextbyte, ULONG *outp)
  56 {
  57     UBYTE v;
  58     ULONG out;
  59
  60     *outp = 0;
  61     if (!getbyte(addr + nextbyte, &v))
  62         return FALSE;
  63     if (v < 4)
  64         return FALSE;
  65     addr += offset * nextbyte;
  66     if (!getbyte(addr + 0 * nextbyte, &v))
  67         return FALSE;
  68     out = v;
  69     if (!getbyte(addr + 1 * nextbyte, &v))
  70         return FALSE;
  71     out |= v << 8;
  72     if (!getbyte(addr + 2 * nextbyte, &v))
  73         return FALSE;
  74     out |= v << 16;
  75     if (!getbyte(addr + 3 * nextbyte, &v))
  76         return FALSE;
  77     out |= v << 24;
  78     *outp = out;
  79     return TRUE;
  80 }
  81
  82 AROS_LH4(ULONG, CopyTuple,
  83         AROS_LHA(struct CardHandle*, handle, A1),
  84         AROS_LHA(UBYTE*, buffer, A0),
  85         AROS_LHA(ULONG, tuplecode, D1),
  86         AROS_LHA(ULONG, size, D0),
  87         struct CardResource*, CardResource, 12, Card)
  88 {
  89     AROS_LIBFUNC_INIT
  90
  91     volatile struct GayleIO *gio = (struct GayleIO*)GAYLE_BASE;
  92     ULONG addr;
  93     UWORD nextbyte, tuplecnt;
  94     ULONG nextjump;
  95     UBYTE oldconfig;
  96     BOOL ret, first;
  97
  98     CARDDEBUG(bug("CopyTuple(%p,%p,%08x,%d)\n", handle, buffer, tuplecode, size));
  99
 100     /* buffer == NULL: output all tuples to debug log */
 101
 102     if (!ISMINE)
 103         return FALSE;
 104
 105     ret = FALSE;
 106
 107     Forbid();
 108
 109     oldconfig = gio->config;
 110     gio->config = GAYLE_CFG_720NS;
 111
 112     tuplecnt = tuplecode >> 16;
 113
 114     nextjump = GAYLE_RAM;
 115     addr = GAYLE_ATTRIBUTE;
 116     nextbyte = 2;
 117     first = TRUE;
 118
 119     for (;;) {
 120         BOOL final = FALSE;
 121         UBYTE v, type;
 122         UBYTE tuplesize;
 123
 124         if (!getbyte(addr, &type))
 125             break;
 126
 127         TUPLEDEBUG(bug("PCMCIA Tuple %02x @%p\n", type, addr));
 128
 129         /* First attribute memory tuple must be CISTPL_DEVICE */
 130         if (first && type != CISTPL_DEVICE)
 131             final = TRUE;
 132         first = FALSE;
 133
 134         switch (type)
 135         {
 136             case CISTPL_NULL:
 137             TUPLEDEBUG(bug("CISTPL_NULL\n"));
 138             break;
 139             case CISTPL_END:
 140             final = TRUE;
 141             TUPLEDEBUG(bug("CISTPL_END\n"));
 142             break;
 143             case CISTPL_NO_LINK:
 144             nextjump = 0xffffffff;
 145             TUPLEDEBUG(bug("CISTPL_NO_LINK\n"));
 146             break;
 147             case CISTPL_LONGLINK_A:
 148             if (!getlelong(addr, 2, nextbyte, &nextjump))
 149                 goto end;
 150             TUPLEDEBUG(bug("CISTPL_LONGLINK_A %08x\n", nextjump));
 151             if (nextjump >= GAYLE_ATTRIBUTESIZE)
 152                 goto end;
 153             nextjump += GAYLE_ATTRIBUTE;
 154             break;
 155             case CISTPL_LONGLINK_C:
 156             if (!getlelong(addr, 2, nextbyte, &nextjump))
 157                 goto end;
 158             TUPLEDEBUG(bug("CISTPL_LONGLINK_C %08x\n", nextjump));
 159             if (nextjump >= GAYLE_RAMSIZE)
 160                 goto end;
 161             nextjump += GAYLE_RAM;
 162             break;
 163         }
 164
 165         if (buffer != NULL && type == (UBYTE)tuplecode) {
 166             if (tuplecnt > 0) {
 167                 tuplecnt--;
 168             } else {
 169                 getbyte(addr, &v);
 170                 *buffer++ = v;
 171                 addr += nextbyte;
 172                 getbyte(addr, &tuplesize);
 173                 *buffer++ = tuplesize;
 174                 addr += nextbyte;
 175                 while (size-- > 0 && tuplesize-- > 0) {
 176                     getbyte(addr, &v);
 177                     *buffer++ = v;
 178                     addr += nextbyte;
 179                 }
 180                 ret = TRUE;
 181                 goto end;
 182             }
 183         }
 184
 185         tuplesize = 0;
 186         if (!final && type != CISTPL_NULL) {
 187             if (!getbyte(addr + nextbyte, &tuplesize))
 188                 goto end;
 189             if (tuplesize == 0xff)
 190                 final = TRUE;
 191         }
 192
 193 #if TUPLELOGGING
 194     if (buffer == NULL) {
 195         UBYTE outbuf[(256 + 2 + 1) * 3];
 196         UWORD ts;
 197         UBYTE *p = outbuf;
 198         for (ts = 0; ts < tuplesize + 2; ts++) {
 199             getbyte(addr + ts * nextbyte, &v);
 200             byte2ascii(&p, v);
 201         }
 202         bug("%s\n", outbuf);
 203     }
 204 #endif
 205
 206         if (final) {
 207             UBYTE buf[5];
 208             TUPLEDEBUG(bug("Next link %08x\n", nextjump));
 209             if (nextjump == 0xffffffff) {
 210                 if (buffer == NULL)
 211                     ret = TRUE;
 212                 break;
 213             }
 214             addr = nextjump;
 215             nextbyte = nextjump < GAYLE_ATTRIBUTE ? 1 : 2;
 216             nextjump = 0xffffffff;
 217             /* valid LINKTARGET? */
 218             if (!getbytes(addr, 5, nextbyte, buf))
 219                 goto end;
 220             if (buf[0] != CISTPL_LINKTARGET || buf[1] < 3 ||
 221                 buf[2] != 'C' || buf[3] != 'I' || buf[4] != 'S') {
 222                  TUPLEDEBUG(bug("Invalid or missing linktarget\n"));
 223                  break;
 224             }
 225             continue;
 226         }
 227
 228         if (type == CISTPL_NULL) {
 229             addr += nextbyte;
 230         } else {
 231             if (!getbyte(addr + nextbyte, &v))
 232                 goto end;
 233             addr += (2 + tuplesize) * nextbyte;
 234         }
 235
 236     }
 237
 238 end:
 239     gio->config = oldconfig;
 240
 241     Permit();
 242
 243     TUPLEDEBUG(bug ("CopyTuple finished\n"));
 244
 245     return ret;
 246
 247     AROS_LIBFUNC_EXIT
 248 }