home *** CD-ROM | disk | FTP | other *** search
/ PC Online 1997 August / PCO0897.ISO / filesbbs / os2 / plnk065.arj / PLNK065.ZIP / pilot-link.0.6.5 / libsock / syspkt.c < prev    next >
Encoding:
C/C++ Source or Header  |  1997-05-23  |  12.9 KB  |  683 lines
  1. /* syspkt.c:  Pilot SysPkt manager
  2.  *
  3.  * (c) 1996, Kenneth Albanowski.
  4.  * Derived from padp.c.
  5.  *
  6.  * This is free software, licensed under the GNU Public License V2.
  7.  * See the file COPYING for details.
  8.  */
  9.  
  10. #include <stdio.h>
  11. #include <stdarg.h>
  12.  
  13. #include "pi-source.h"
  14. #include "pi-socket.h"
  15. #include "pi-syspkt.h"
  16. #include "pi-slp.h"
  17. #include "pi-serial.h"
  18.  
  19. int sys_RPCerror;
  20.  
  21. int syspkt_tx(struct pi_socket *ps, unsigned char *msg, int length)
  22. {
  23.   struct pi_skb *nskb;
  24.  
  25. #ifdef DEBUG
  26.   fprintf(stderr,"-----------------\n");
  27. #endif
  28.  
  29.   ps->laddr.pi_port = msg[0];
  30.   ps->raddr.pi_port = msg[1];
  31.   ps->protocol = msg[2];
  32.   /*ps->xid = msg[3];*/
  33.   
  34.   ps->xid++;
  35.   ps->xid &= 0xff;
  36.   if ((!ps->xid) || (ps->xid==0xff)) ps->xid = 0x11; /* some random # */
  37.                  
  38.   nskb = (struct pi_skb *)malloc(sizeof(struct pi_skb));
  39.   memcpy(&nskb->data[10], msg+4, length-4);
  40.   slp_tx(ps, nskb, length-4);
  41.   
  42.   pi_socket_flush(ps);
  43.  
  44.   return 0;
  45. }
  46.  
  47. int syspkt_rx(struct pi_socket *ps, unsigned char *buf, int len)
  48. {
  49.   struct pi_skb *skb;
  50.   int rlen =0;
  51.   
  52.   if (!ps->rxq)
  53.     pi_socket_read(ps, 1);
  54.   
  55.   if (!ps->rxq)
  56.     return 0;
  57.  
  58.   skb = ps->rxq;
  59.   ps->rxq = skb->next;
  60.   
  61.   rlen = skb->len-12;
  62.   
  63.   buf[0] = ps->laddr.pi_port;
  64.   buf[1] = ps->raddr.pi_port;
  65.   buf[2] = ps->protocol;
  66.   buf[3] = ps->xid;
  67.  
  68.   memcpy(buf+4, &skb->data[10], rlen);
  69.   
  70.   free(skb);
  71.   return rlen+4;
  72.  
  73. }
  74.  
  75.  
  76. int sys_UnpackState(void * buffer, struct Pilot_state * s)
  77. {
  78.   int i;
  79.   unsigned char * data = buffer;
  80.   
  81.   s->reset = get_short(data);
  82.   s->exception = get_short(data+2);
  83.   memcpy(s->func_name, data+152, 32);
  84.   memcpy(s->instructions, data+78, 30);
  85.   s->func_name[32-1] = 0;
  86.   s->func_start = get_long(data+144);
  87.   s->func_end = get_long(data+148);
  88.   sys_UnpackRegisters(data+4, &s->regs);
  89.  
  90.   for (i=0;i<6;i++) {
  91.     s->breakpoint[i].address = get_long(data+108+i*6);
  92.     s->breakpoint[i].enabled = get_byte(data+112+i*6);
  93.   }
  94.   
  95.   s->trap_rev = get_short(data+184);
  96.   
  97.   return 0;
  98. }
  99.  
  100. int sys_UnpackRegisters(void * data, struct Pilot_registers * r)
  101. {
  102.   unsigned char * buffer = data;
  103.   r->D[0] = get_long(buffer+0);
  104.   r->D[1] = get_long(buffer+4);
  105.   r->D[2] = get_long(buffer+8);
  106.   r->D[3] = get_long(buffer+12);
  107.   r->D[4] = get_long(buffer+16);
  108.   r->D[5] = get_long(buffer+20);
  109.   r->D[6] = get_long(buffer+24);
  110.   r->D[7] = get_long(buffer+28);
  111.   r->A[0] = get_long(buffer+32);
  112.   r->A[1] = get_long(buffer+36);
  113.   r->A[2] = get_long(buffer+40);
  114.   r->A[3] = get_long(buffer+44);
  115.   r->A[4] = get_long(buffer+48);
  116.   r->A[5] = get_long(buffer+52);
  117.   r->A[6] = get_long(buffer+56);
  118.   r->USP = get_long(buffer+60);
  119.   r->SSP = get_long(buffer+64);
  120.   r->PC = get_long(buffer+68);
  121.   r->SR = get_short(buffer+72);
  122.   
  123.   return 0;
  124. }
  125.  
  126. int sys_PackRegisters(void * data, struct Pilot_registers * r)
  127. {
  128.   unsigned char * buffer = data;
  129.   int i;
  130.   for (i=0;i<8;i++)
  131.     set_long(buffer+i*4, r->D[i]);
  132.   for (i=0;i<7;i++)
  133.     set_long(buffer+32+i*4, r->A[i]);
  134.   set_long(buffer+60, r->USP);
  135.   set_long(buffer+64, r->SSP);
  136.   set_long(buffer+68, r->PC);
  137.   set_short(buffer+72, r->SR);
  138.   
  139.   return 0;
  140. }
  141.  
  142. int sys_Continue(int sd, struct Pilot_registers * r, struct Pilot_watch * w)
  143. {
  144.   char buf[94];
  145.   
  146.   buf[0] = 0;
  147.   buf[1] = 0;
  148.   buf[2] = 0;
  149.   buf[3] = 0;
  150.   
  151.   buf[4] = 0x07;
  152.   buf[5] = 0; /*gapfil*/
  153.   
  154.   if (!r)
  155.     return pi_write(sd,buf,6);
  156.   
  157.   sys_PackRegisters(buf+6, r);
  158.   set_byte(buf+80, (w != 0) ? 1 : 0);
  159.   set_byte(buf+81, 0);
  160.   set_long(buf+82, w ? w->address : 0);
  161.   set_long(buf+86, w ? w->length : 0);
  162.   set_long(buf+90, w ? w->checksum : 0);
  163.   
  164.   return pi_write(sd, buf, 94);
  165. }
  166.  
  167. int sys_Step(int sd)
  168. {
  169.   char buf[94];
  170.   
  171.   buf[0] = 0;
  172.   buf[1] = 0;
  173.   buf[2] = 0;
  174.   buf[3] = 0;
  175.   
  176.   buf[4] = 0x03;
  177.   buf[5] = 0; /*gapfil*/
  178.   
  179.   return pi_write(sd,buf,6);
  180. }
  181.  
  182. int sys_SetBreakpoints(int sd, struct Pilot_breakpoint * b)
  183. {
  184.   char buf[94];
  185.   int i;
  186.   
  187.   buf[0] = 0;
  188.   buf[1] = 0;
  189.   buf[2] = 0;
  190.   buf[3] = 0;
  191.   
  192.   buf[4] = 0x0c;
  193.   buf[5] = 0; /*gapfil*/
  194.   
  195.   for (i=0;i<6;i++) {
  196.     set_long(buf+6+i*6, b[i].address);
  197.     set_byte(buf+10+i*6, b[i].enabled);
  198.     set_byte(buf+11+i*6, 0);
  199.   }
  200.   
  201.   pi_write(sd, buf, 42);
  202.   
  203.   i = pi_read(sd, buf, 6);
  204.  
  205.   if ((i<=0) || (buf[4] != (char)0x8c))
  206.     return 0;
  207.   else
  208.     return 1;
  209. }
  210.  
  211. int sys_SetTrapBreaks(int sd, int * traps)
  212. {
  213.   char buf[94];
  214.   int i;
  215.   
  216.   buf[0] = 0;
  217.   buf[1] = 0;
  218.   buf[2] = 0;
  219.   buf[3] = 0;
  220.   
  221.   buf[4] = 0x11;
  222.   buf[5] = 0; /*gapfil*/
  223.   
  224.   for (i=0;i<5;i++) {
  225.     set_short(buf+6+i*2, traps[i]);
  226.   }
  227.   
  228.   pi_write(sd, buf, 16);
  229.   
  230.   i = pi_read(sd, buf, 6);
  231.  
  232.   if ((i<=0) || (buf[4] != (char)0x91))
  233.     return 0;
  234.   else
  235.     return 1;
  236. }
  237.  
  238. int sys_GetTrapBreaks(int sd, int * traps)
  239. {
  240.   char buf[94];
  241.   int i;
  242.   
  243.   buf[0] = 0;
  244.   buf[1] = 0;
  245.   buf[2] = 0;
  246.   buf[3] = 0;
  247.   
  248.   buf[4] = 0x10;
  249.   buf[5] = 0; /*gapfil*/
  250.   
  251.   pi_write(sd, buf, 6);
  252.   
  253.   i = pi_read(sd, buf, 16);
  254.  
  255.   if ((i<16) || (buf[4] != (char)0x90))
  256.     return 0;
  257.  
  258.   for (i=0;i<5;i++) {
  259.     traps[i] = get_short(buf+6+i*2);
  260.   }
  261.  
  262.   return 1;
  263. }
  264.  
  265. int sys_ToggleDbgBreaks(int sd)
  266. {
  267.   char buf[94];
  268.   int i;
  269.   
  270.   buf[0] = 0;
  271.   buf[1] = 0;
  272.   buf[2] = 0;
  273.   buf[3] = 0;
  274.   
  275.   buf[4] = 0x0d;
  276.   buf[5] = 0; /*gapfil*/
  277.   
  278.   pi_write(sd, buf, 6);
  279.   
  280.   i = pi_read(sd, buf, 7);
  281.  
  282.   if ((i<7) || (buf[4] != (char)0x8d))
  283.     return 0;
  284.  
  285.   return get_byte(buf+6);
  286. }
  287.  
  288. int sys_QueryState(int sd)
  289. {
  290.   char buf[6];
  291.   
  292.   buf[0] = 0;
  293.   buf[1] = 0;
  294.   buf[2] = 0;
  295.   buf[3] = 0;
  296.   
  297.   buf[4] = 0;
  298.   buf[5] = 0; /*gapfil*/
  299.   
  300.   return pi_write(sd, buf, 6);
  301. }
  302.  
  303. int sys_ReadMemory(int sd, unsigned long addr, unsigned long len, void * dest)
  304. {
  305.   int result;
  306.   unsigned char buf[0xffff];
  307.   unsigned long todo,done;
  308.   
  309.   
  310.   done = 0;
  311.   do {
  312.     todo = len;
  313.     if (todo > 256)
  314.       todo = 256;
  315.  
  316.     buf[0] = 0;
  317.     buf[1] = 0;
  318.     buf[2] = 0;
  319.     buf[3] = 0;
  320.   
  321.     buf[4] = 0x01;
  322.     buf[5] = 0; /*gapfil*/
  323.   
  324.     set_long(buf+6, addr+done);
  325.     set_short(buf+10, todo);
  326.   
  327.     pi_write(sd, buf, 12);
  328.   
  329.     result = pi_read(sd, buf, todo+6);
  330.   
  331.     if (result<0)
  332.       return done;
  333.   
  334.     if ((buf[4] == 0x81) && (result == todo+6)) {
  335.       memcpy(((char *)dest) + done, buf+6, todo);
  336.       done += todo;
  337.     } else {
  338.       return done;
  339.     }
  340.   } while (done < len);
  341.   return done;
  342. }
  343.  
  344. int sys_WriteMemory(int sd, unsigned long addr, unsigned long len, void * src)
  345. {
  346.   int result;
  347.   unsigned char buf[0xffff];
  348.   unsigned long todo, done;
  349.   
  350.   
  351.   done = 0;
  352.   do {
  353.     todo = len;
  354.     if (todo>256)
  355.       todo = 256;
  356.  
  357.     buf[0] = 0;
  358.     buf[1] = 0;
  359.     buf[2] = 0;
  360.     buf[3] = 0;
  361.   
  362.     buf[4] = 0x02;
  363.     buf[5] = 0; /*gapfil*/
  364.  
  365.   
  366.     set_long(buf+6, addr);
  367.     set_short(buf+10, len);
  368.     memcpy(buf+12, ((char *)src)+done, todo);
  369.   
  370.     pi_write(sd, buf, len+12);
  371.   
  372.     result = pi_read(sd, buf, 6);
  373.   
  374.     if (result<0)
  375.       return done;
  376.    
  377.     if ((buf[4] == 0x82) && (result == todo+6)) {
  378.       ;
  379.     } else {
  380.       return done;
  381.     }
  382.   } while (done < len);
  383.   return done;
  384. } 
  385.  
  386. int sys_Find(int sd, unsigned long startaddr, unsigned long stopaddr, int len, int caseinsensitive,
  387.              void * data, unsigned long * found)
  388. {
  389.   int result;
  390.   unsigned char buf[0xffff];
  391.   
  392.   buf[0] = 0;
  393.   buf[1] = 0;
  394.   buf[2] = 0;
  395.   buf[3] = 0;
  396.   
  397.   buf[4] = 0x11;
  398.   buf[5] = 0; /*gapfil*/
  399.   
  400.   set_long(buf+6, startaddr);
  401.   set_long(buf+10, stopaddr);
  402.   set_short(buf+14, len);
  403.   set_byte(buf+16, caseinsensitive);
  404.   memcpy(buf+17, data, len);
  405.   
  406.   pi_write(sd, buf, len+17);
  407.   
  408.   result = pi_read(sd, buf, 12);
  409.   
  410.   if (result<0)
  411.     return result;
  412.     
  413.   if (found)
  414.     *found = get_long(buf+6);
  415.   
  416.   return get_byte(buf+10);
  417. }
  418.  
  419.  
  420. int sys_RemoteEvent(int sd, int penDown, int x, int y, int keypressed, 
  421.                        int keymod, int keyasc, int keycode)
  422. {
  423.   char buf[20];
  424.   
  425.   buf[0] = 2;
  426.   buf[1] = 2;
  427.   buf[2] = 0;
  428.   buf[3] = 0x11;
  429.   
  430.   buf[4] = 0x0d; /*RemoteEvtCommand*/
  431.   buf[5] = 0; /*gapfil*/
  432.   buf[6] = penDown;
  433.   buf[7] = 0; /*gapfil*/
  434.   buf[8] = x >> 8;
  435.   buf[9] = x & 0xff;
  436.   buf[10] = y >> 8;
  437.   buf[11] = y & 0xff;
  438.   buf[12] = keypressed;
  439.   buf[13] = 0; /*gapfil*/
  440.   buf[14] = keymod >> 8;
  441.   buf[15] = keymod & 0xff;
  442.   buf[16] = keyasc >> 8;
  443.   buf[17] = keyasc & 0xff;
  444.   buf[18] = keycode >> 8;
  445.   buf[19] = keycode & 0xff;
  446.   
  447.   return pi_write(sd, buf, 16+4);
  448. }
  449.  
  450. int sys_RPC(int sd, int socket, int trap, long * D0, long * A0, int params, struct RPC_param * param, int reply)
  451. {
  452.   unsigned char buf[4096];
  453.   int i;
  454.   unsigned char * c;
  455.   
  456.   buf[0] = socket; /* 0 for debug, 1 for console */
  457.   buf[1] = socket;
  458.   buf[2] = 0;
  459.   buf[4] = 0x0a;
  460.   buf[5] = 0;
  461.   
  462.   set_short(buf+6, trap);
  463.   set_long(buf+8, *D0);
  464.   set_long(buf+12, *A0);
  465.   set_short(buf+16, params);
  466.   
  467.   c = buf+18;
  468.   for(i=params-1;i>=0;i--) {
  469.     set_byte(c, param[i].byRef); c++;
  470.     set_byte(c, param[i].size); c++;
  471.     if(param[i].data)
  472.       memcpy(c, param[i].data, param[i].size);
  473.     c += param[i].size;
  474.   }
  475.   
  476.   if (socket == 3)
  477.     set_short(buf+4, c-buf - 6);
  478.   
  479.   pi_write(sd, buf, c-buf);
  480.   
  481.   if(reply) {
  482.     int l = pi_read(sd, buf, 4096);
  483.   
  484.     if (l < 0)
  485.       return l;
  486.     if (l < 6)
  487.       return -1;
  488.     if (buf[4] != 0x8a)
  489.       return -2;
  490.     
  491.     *D0 = get_long(buf+8);
  492.     *A0 = get_long(buf+12);
  493.     c = buf+18;
  494.     for(i=params-1;i>=0;i--) {
  495.       c++;
  496.       param[i].size = get_byte(c); c++;
  497.       if(param[i].byRef && param[i].data)
  498.         memcpy(param[i].data, c, param[i].size);
  499.       c += param[i].size;
  500.     }
  501.   }
  502.   return 0;
  503. }
  504.  
  505. /* Deprecated */
  506. int RPC(int sd, int socket, int trap, int reply, ...)
  507. {
  508.   va_list ap;
  509.   struct RPC_param p[20];
  510.   int RPC_arg[20];
  511.   int i=0,j;
  512.   long D0=0,A0=0;
  513.  
  514.   va_start(ap, reply);
  515.   for(;;) {
  516.     int type = va_arg(ap, int);
  517.     if(type == 0)
  518.       break;
  519.     if(type < 0) {
  520.       p[i].byRef = 0;
  521.       p[i].size = -type;
  522.       RPC_arg[i] = va_arg(ap,int);
  523.       p[i].data = &RPC_arg[i];
  524.       p[i].invert = 0;
  525.     } else {
  526.       void * c = va_arg(ap,void*);
  527.       p[i].byRef = 1;
  528.       p[i].size = type;
  529.       p[i].data = c;
  530.       p[i].invert = va_arg(ap,int);
  531.       if(p[i].invert) {
  532.         if(p[i].size == 2) {
  533.           int * s = c;
  534.           *s = htons(*s);
  535.         } else {
  536.           int * l = c;
  537.           *l = htonl(*l);
  538.         }
  539.       }
  540.     }
  541.     i++;
  542.   }
  543.   va_end(ap);
  544.   
  545.   sys_RPCerror = sys_RPC(sd, socket, trap, &D0, &A0, i, p, reply != 2);
  546.   
  547.   for(j=0;j<i;j++) {
  548.       if(p[j].invert) {
  549.         void * c = p[j].data;
  550.         if(p[j].size == 2) {
  551.           int * s = c;
  552.           *s = htons(*s);
  553.         } else {
  554.           int * l = c;
  555.           *l = htonl(*l);
  556.         }
  557.       }
  558.   }
  559.   
  560.   if(reply)
  561.     return A0;
  562.   else
  563.     return D0;
  564. }
  565.  
  566. int PackRPC(struct RPC_params * p, int trap, int reply, ...)
  567. {
  568.   va_list ap;
  569.   int i=0;
  570.   
  571.   p->trap = trap;
  572.   p->reply = reply;
  573.  
  574.   va_start(ap, reply);
  575.   for(;;) {
  576.     int type = (int)va_arg(ap, int);
  577.     if(type == 0)
  578.       break;
  579.     if(type < 0) {
  580.       p->param[i].byRef = 0;
  581.       p->param[i].size = -type;
  582.       p->param[i].arg = (int)va_arg(ap, int);
  583.       p->param[i].data = &p->param[i].arg;
  584.       p->param[i].invert = 0;
  585.     } else {
  586.       void * c = (void*)va_arg(ap,void*);
  587.       p->param[i].byRef = 1;
  588.       p->param[i].size = type;
  589.       p->param[i].data = c;
  590.       p->param[i].invert = (int)va_arg(ap,int);
  591.     }
  592.     i++;
  593.   }
  594.   p->args = i;
  595.   va_end(ap);
  596.   
  597.   return 0;
  598. }
  599.  
  600. void UninvertRPC(struct RPC_params * p)
  601. {
  602.   int j;
  603.   
  604.   for(j=0;j<p->args;j++) {
  605.       if(p->param[j].invert) {
  606.         void * c = p->param[j].data;
  607.         if((p->param[j].invert == 2) && (p->param[j].size == 2)) {
  608.           int * s = c;
  609.           *s = htons(*s)>>8;
  610.         } else if(p->param[j].size == 2) {
  611.           int * s = c;
  612.           *s = htons(*s);
  613.         } else {
  614.           long * l = c;
  615.           *l = htonl(*l);
  616.         }
  617.       }
  618.   }
  619. }
  620.  
  621. void InvertRPC(struct RPC_params * p)
  622. {
  623.   int j;
  624.   
  625.   for(j=0;j<p->args;j++) {
  626.       if(p->param[j].invert) {
  627.         void * c = p->param[j].data;
  628.         if((p->param[j].invert == 2) && (p->param[j].size == 2)) {
  629.           int * s = c;
  630.           *s = ntohs(*s)>>8;
  631.         } else if(p->param[j].size == 2) {
  632.           int * s = c;
  633.           *s = ntohs(*s);
  634.         } else {
  635.           long * l = c;
  636.           *l = ntohl(*l);
  637.         }
  638.       }
  639.   }
  640. }
  641.  
  642.  
  643. unsigned long DoRPC(int sd, int socket, struct RPC_params * p, int * error)
  644. {
  645.   int err;
  646.   long D0=0,A0=0;
  647.   
  648.   InvertRPC(p);
  649.  
  650.   err = sys_RPC(sd, socket, p->trap, &D0, &A0, p->args, &p->param[0], p->reply);
  651.   
  652.   UninvertRPC(p);
  653.   
  654.   if (error)
  655.     *error = err;
  656.   
  657.   if(p->reply==RPC_PtrReply)
  658.     return A0;
  659.   else if (p->reply==RPC_IntReply)
  660.     return D0;
  661.   else
  662.     return err;
  663. }
  664.  
  665. int RPC_Int_Void(int sd, int trap) {
  666.   return RPC(sd, 1, trap, 0, RPC_End);
  667. }
  668.  
  669. int RPC_Ptr_Void(int sd, int trap) {
  670.   return RPC(sd, 1, trap, 1, RPC_End);
  671. }
  672.  
  673. /* Untested complex RPC example
  674. int RPC_MemCardInfo(int sd, int cardno, char * cardname, char * manufname,
  675.                     int * version, long * date, long * romsize, long * ramsize,
  676.                     long * freeram) {
  677.   return RPC(sd, 1, 0xA004, 0, RPC_Short(cardno), RPC_Ptr(cardname, 32), 
  678.                                RPC_Ptr(manufname, 32), RPC_ShortPtr(version),
  679.                                RPC_LongPtr(date), RPC_LongPtr(romsize),
  680.                                RPC_LongPtr(ramsize), RPC_LongPtr(freeram));
  681. }                    
  682. */
  683.