gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T-Kernel 2.0 Software Package
    4:  *
    5:  *    Copyright 2011 by Ken Sakamura.
    6:  *    This software is distributed under the latest version of T-License 2.x.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by T-Engine Forum(http://www.t-engine.org/) at 2011/05/17.
   10:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   11:  *
   12:  *----------------------------------------------------------------------
   13:  */
   14: 
   15: /*
   16:  *      memory.c
   17:  *
   18:  *       memory & I/O access processing
   19:  */
   20: 
   21: #include "cmdsvc.h"
   22: 
   23: /* multi-type memory pointer */
   24: typedef union {
   25:         UW     a;
   26:         UW     *w;
   27:         UH     *h;
   28:         UB     *b;
   29: } MP;
   30: 
   31: /*
   32:         read memory
   33: */
   34: EXPORT  W        readMem(UW addr, void *buf, W len, W unit)
   35: {
   36:         W      i, n, alen;
   37:         MP     pa, bp;
   38: 
   39:         // address misalignment is reported as error
   40:         if (addr & (unit - 1)) return 0;
   41: 
   42:         bp.w = buf;
   43:         for (alen = 0; alen < len; alen += i) {
   44: 
   45:                 // memory address check & conversion to physical address
   46:                 n = chkMemAddr(addr + alen, &pa.a, len - alen, 0);
   47:                 if (n < unit) break;  // illegal address
   48: 
   49:                 i = 0;
   50:                 switch(unit) {
   51:                 case 4:
   52:                         for (; i < n; i += 4) *bp.w++ = rd_w(pa.w++);
   53:                         break;
   54:                 case 2:
   55:                         for (; i < n; i += 2) *bp.h++ = rd_h(pa.h++);
   56:                         break;
   57:                 default:
   58:                         for (; i < n; i++) *bp.b++ = rd_b(pa.b++);
   59:                 }
   60:         }
   61:         return alen;
   62: }
   63: /*
   64:         write memory
   65:                 unit & 0x10 : fill
   66: */
   67: EXPORT  W        writeMem(UW addr, void *buf, W len, W unit)
   68: {
   69:         W      i, n, sz, alen;
   70:         MP     pa, bp;
   71: 
   72:         // address misalignment is reported as error
   73:         sz = unit & 0x0f;
   74:         if (addr & (sz - 1)) return 0;
   75: 
   76:         bp.w = buf;
   77:         for (alen = 0; alen < len; alen += i) {
   78: 
   79:                 // memory address check & conversion to physical address
   80:                 n = chkMemAddr(addr + alen, &pa.a, len - alen, 1);
   81:                 if (n < sz) break;    // illegal address
   82: 
   83:                 i = 0;
   84:                 switch(sz) {
   85:                 case 4:
   86:                         if (unit & 0x10) {
   87:                                 for (; i < n; i += 4) wr_w(pa.w++, *bp.w);
   88:                         } else {
   89:                                 for (; i < n; i += 4) wr_w(pa.w++, *bp.w++);
   90:                         }
   91:                         break;
   92:                 case 2:
   93:                         if (unit & 0x10) {
   94:                                 for (; i < n; i += 2) wr_h(pa.h++, *bp.h);
   95:                          } else {
   96:                                 for (; i < n; i += 2) wr_h(pa.h++, *bp.h++);
   97:                         }
   98:                         break;
   99:                 default:
  100:                         if (unit & 0x10) {
  101:                                 for (; i < n; i++) wr_b(pa.b++, *bp.b);
  102:                         } else {
  103:                                 for (; i < n; i++) wr_b(pa.b++, *bp.b++);
  104:                         }
  105:                 }
  106:         }
  107:         return alen;
  108: }
  109: /*
  110:         read character string
  111: */
  112: EXPORT  W        readMemStr(UW addr, void *buf, W len)
  113: {
  114:         W      i, n, alen;
  115:         UW     pa;
  116: 
  117:         for (alen = 0; alen < len; alen += i) {
  118:                 // memory address check & conversion to physical address
  119:                 n = chkMemAddr(addr + alen, &pa, len - alen, 0);
  120:                 if (n == 0) break;    // illegal address
  121:                 for (i = 0; i < n; i++, buf++, pa++) {
  122:                         if ((*(UB*)buf = rd_b((UB*)pa)) == 0) return alen + i;
  123:                 }
  124:         }
  125:         return -1;
  126: }
  127: /*
  128:         I/O read
  129: */
  130: EXPORT  W        readIO(UW addr, UW *data, W unit)
  131: {
  132:         W      n;
  133:         UW     pa;
  134: 
  135:         // address misalignment is reported as error
  136:         if (addr & (unit - 1)) return 0;
  137: 
  138:         // I/O address check & conversion to physical address
  139:         n = chkIOAddr(addr, &pa, unit);
  140:         if (n < unit) return 0;
  141: 
  142:         switch(unit) {
  143:         case 4:                *data = in_w(pa);     break;
  144:         case 2:                *data = in_h(pa);     break;
  145:         default:       *data = in_b(pa);
  146:         }
  147:         return unit;
  148: }
  149: /*
  150:         I/O write
  151: */
  152: EXPORT  W        writeIO(UW addr, UW data, W unit)
  153: {
  154:         W      n;
  155:         UW     pa;
  156: 
  157:         // address misalignment is reported as error
  158:         if (addr & (unit - 1)) return 0;
  159: 
  160:         // I/O address check & conversion to physical address
  161:         n = chkIOAddr(addr, &pa, unit);
  162:         if (n < unit) return 0;
  163: 
  164:         switch(unit) {
  165:         case 4:                out_w(pa, data);      break;
  166:         case 2:                out_h(pa, data);      break;
  167:         default:       out_b(pa, data);
  168:         }
  169:         return unit;
  170: }