From eb541ae9006e3b519e3e71fe8d0739ed4c2c8d13 Mon Sep 17 00:00:00 2001
From: Kelvin Sherlock <ksherlock@gmail.com>
Date: Mon, 14 May 2012 21:30:27 -0400
Subject: [PATCH] getpeername

---
 driver.c       | 38 ++++++++++++++++++++++++++++++++++++++
 maccept.c      | 39 ++++++++++++---------------------------
 marignotti.h   | 12 ++++++++++--
 mgetpeername.c | 41 +++++++++++++++++++++++++++++++++++++++++
 mgetsockname.c | 43 ++++++++-----------------------------------
 5 files changed, 109 insertions(+), 64 deletions(-)
 create mode 100644 mgetpeername.c

diff --git a/driver.c b/driver.c
index 4e81a43..5ca55aa 100644
--- a/driver.c
+++ b/driver.c
@@ -11,9 +11,46 @@ int block(int sem)
 {
     int xerrno = 0;
     Kswait(sem, &xerrno);
+
     return xerrno;
 }
 
+
+void copy_addr(xsockaddr_in *src, xsockaddr_in *dest, int *addrlen)
+{
+    int len;
+    
+    // bytswap the port.
+    asm {
+        ldy #2
+        lda [src],y
+        xba
+        sta [src],y
+    }
+
+    len = 8;
+    if (*addrlen < 8)
+        len = *addrlen;
+    else
+        *addrlen = 8;
+    
+    // data is truncated if there isn't enough space.
+    asm {
+        ldx <len
+        beq done
+        ldy #0
+        sep #0x20
+    loop:
+        lda [src],y
+        sta [dest],y
+        iny
+        dex
+        bne loop
+        rep #0x20
+    done:
+    }
+}
+
 int queue_command(Entry *e, Word command, LongWord cookie, LongWord timeout)
 {
     int xerrno;
@@ -117,6 +154,7 @@ int driver(
         break;
         
     case PRU_PEERADDR:
+        return mgetpeername(e, p1, p2, p3, p4, p5);
         break;
         
     case PRU_RCVD:
diff --git a/maccept.c b/maccept.c
index b0882f8..9165dc1 100644
--- a/maccept.c
+++ b/maccept.c
@@ -40,35 +40,20 @@ static int sock_accept(
     child->select_fx = fx;
     
     // address...
-    if (addr && addrlen)
+    if (addr && addrlen && *addrlen)
     {
-        if (*addrlen >= 8)
-        {
-            destRec dr;
-            Word port;
-
-            IncBusy();
-            TCPIPGetDestination(ipid, &dr);
-            DecBusy();
-            
-            port = dr.drDestPort;
-            
-            asm {
-                lda <port
-                xba
-                sta <port
-            }            
-            addr->sin_port = port;
-            addr->sin_addr = dr.drDestIP;
-            addr->sin_family = AF_INET;
-            
-            *addrlen = 8;
-        }
-        else
-        {
-            *addrlen = 0;
-        }
+        destRec dr;
+        xsockaddr_in tmp;
 
+        IncBusy();
+        TCPIPGetDestination(ipid, &dr);
+        DecBusy();
+              
+        tmp.sin_family = AF_INET;
+        tmp.sin_port = port;
+        tmp.sin_addr = dr.drDestIP;
+        
+        copy_address(&tmp, addr, addrlen);
     }
     
     *newfd = ipid;
diff --git a/marignotti.h b/marignotti.h
index 5092327..0e58a29 100644
--- a/marignotti.h
+++ b/marignotti.h
@@ -52,7 +52,7 @@ typedef struct Entry {
     unsigned _OOBINLINE:1;
     unsigned _LINGER:1;
     unsigned _NOSIGPIPE:1;
-    
+        
     Word _TYPE;
     
     LongWord _SNDLOWAT;
@@ -71,7 +71,7 @@ typedef struct xsockaddr_in {
   unsigned short sin_family;
   unsigned short sin_port;
   unsigned long sin_addr;
-  unsigned char sin_zero[8];
+  //unsigned char sin_zero[8];
 } xsockaddr_in;
 
 typedef struct xsockaddr {
@@ -79,6 +79,12 @@ typedef struct xsockaddr {
     unsigned char sa_data[14];
 } xsockaddr;
 
+union xsplit {  
+    LongWord i32;
+    Word i16[2];
+    Byte i8[4];
+};
+
 #define IncBusy() asm { jsl 0xE10064 }
 #define DecBusy() asm { jsl 0xE10068 }
 #define Resched() asm { cop 0x7f }
@@ -98,6 +104,7 @@ void process_table(void);
 Entry *find_entry(Word ipid);
 Entry *create_entry(Word ipid);
 
+void copy_addr(xsockaddr_in *src, xsockaddr_in *dest, int *addrlen);
 
 
 // driver stuff.
@@ -122,6 +129,7 @@ int msetsockopt(Entry *, void *p1, void *p2, void *p3, void *p4, void *p5);
 
 int mioctl(Entry *, void *p1, void *p2, void *p3, void *p4, void *p5);
 int mgetsockname(Entry *, void *p1, void *p2, void *p3, void *p4, void *p5);
+int mgetpeername(Entry *, void *p1, void *p2, void *p3, void *p4, void *p5);
 int mselect(Entry *, void *p1, void *p2, void *p3, void *p4, void *p5);
 
 int maccept(Entry *e, void *p1, void *p2, void *p3, void *p4, void *p5);
diff --git a/mgetpeername.c b/mgetpeername.c
new file mode 100644
index 0000000..912edbb
--- /dev/null
+++ b/mgetpeername.c
@@ -0,0 +1,41 @@
+#include "marignotti.h"
+#include <gno/kerntool.h>
+#include <errno.h>
+#include <string.h>
+
+#include <sys/socket.h>
+
+#include "s16debug.h"
+
+
+#pragma noroot
+#pragma optimize 79
+
+int mgetpeername(Entry *e, void *p1, void *p2, void *p3, void *p4, void *p5)
+{
+    destRec dr;
+    xsockaddr_in tmp;
+
+    xsockaddr_in *addr = (xsockaddr_in *)p3;
+    int *addrlen = (int *)p4;
+
+    if (Debug > 0)
+    {
+        s16_debug_printf("getpeername");
+    }
+    
+    if (!addrlen) return EINVAL;
+    if (!sock_addr) return EINVAL;
+    
+    IncBusy();
+    TCPIPGetDestination(e->ipid, &dr);
+    DecBusy();
+
+    tmp.sin_family = AF_INET;
+    tmp.sin_port = dr.drDestPort;
+    tmp.sin_addr = dr.drDestIP;
+    
+    copy_addr(&tmp, addr, addrlen);
+
+    return 0;
+}
\ No newline at end of file
diff --git a/mgetsockname.c b/mgetsockname.c
index 6d02fa7..6dce1aa 100644
--- a/mgetsockname.c
+++ b/mgetsockname.c
@@ -11,13 +11,13 @@
 #pragma noroot
 #pragma optimize 79
 
+
+
 int mgetsockname(Entry *e, void *p1, void *p2, void *p3, void *p4, void *p5)
 {
-    LongWord addr;
-    Word port;
-    int len;
+    xsockaddr_in tmp;
     
-    xsockaddr_in *sock_addr = (xsockaddr_in *)p3;
+    xsockaddr_in *addr = (xsockaddr_in *)p3;
     int *addrlen = (int *)p4;
 
     if (Debug > 0)
@@ -29,41 +29,14 @@ int mgetsockname(Entry *e, void *p1, void *p2, void *p3, void *p4, void *p5)
     if (!addrlen) return EINVAL;
     if (!sock_addr) return EINVAL;
 
-    len = *addrlen;
-
-    // bsd { has char sin_len; char sin_family; ... }
-    // gno has { short sin_family; ... }
-    
-    
-    // TODO -- is this only true for servers?
-    // should it use TCPIPGetDestination for clients?
-    
+    tmp.sin_family = AF_INET;
     IncBusy();
-    port = TCPIPGetSourcePort(e->ipid);
-    addr = TCPIPGetMyIPAddress();
+    tmp.sin_port = TCPIPGetSourcePort(e->ipid);
+    tmp.sin_addr = TCPIPGetMyIPAddress();
     DecBusy();
     
-    memset(sock_addr, 0, len);
-    
-    // truncate missing data.
-    if (len >= 2)
-        sock_addr->sin_family = AF_INET;
-    
-    if (len >= 4)
-    {
-        asm {
-            lda <port;
-            xba
-            sta <port;
-        }
-        sock_addr->sin_port = port;
-    }
-    
-    if (len >= 8)
-    {
-        sock_addr->sin_addr = addr;
-    }
     
+    copy_addr(&tmp, addr, addrlen);
     
     return 0;
 }
\ No newline at end of file